diff --git a/.gitattributes b/.gitattributes index d523ee98a..25300b4ef 100644 --- a/.gitattributes +++ b/.gitattributes @@ -14,3 +14,22 @@ vendor/wgpu/lib/wgpu-windows-x86_64-msvc-release/lib/wgpu_native.dll filter=lfs vendor/wgpu/lib/wgpu-windows-x86_64-msvc-release/lib/wgpu_native.dll.lib filter=lfs diff=lfs merge=lfs -text vendor/wgpu/lib/wgpu-windows-x86_64-msvc-release/lib/wgpu_native.lib filter=lfs diff=lfs merge=lfs -text vendor/wgpu/lib/wgpu-windows-x86_64-msvc-release/lib/wgpu_native.pdb filter=lfs diff=lfs merge=lfs -text +vendor/raylib/linux/libraygui.so filter=lfs diff=lfs merge=lfs -text +vendor/raylib/linux/libraygui.a filter=lfs diff=lfs merge=lfs -text +vendor/raylib/linux/libraylib.a filter=lfs diff=lfs merge=lfs -text +vendor/raylib/linux/libraylib.so.600 filter=lfs diff=lfs merge=lfs -text +vendor/raylib/linux-arm64/libraylib.a filter=lfs diff=lfs merge=lfs -text +vendor/raylib/linux-arm64/libraylib.so.600 filter=lfs diff=lfs merge=lfs -text +vendor/raylib/macos/libraygui-arm64.dylib filter=lfs diff=lfs merge=lfs -text +vendor/raylib/macos/libraygui.dylib filter=lfs diff=lfs merge=lfs -text +vendor/raylib/macos/libraylib.a filter=lfs diff=lfs merge=lfs -text +vendor/raylib/macos/libraylib.600.dylib filter=lfs diff=lfs merge=lfs -text +vendor/raylib/wasm/libraylib.web.a filter=lfs diff=lfs merge=lfs -text +vendor/raylib/windows/raylib.dll filter=lfs diff=lfs merge=lfs -text +vendor/raylib/windows/raylib.lib filter=lfs diff=lfs merge=lfs -text +vendor/raylib/windows/raylibdll.lib filter=lfs diff=lfs merge=lfs -text +vendor/raylib/windows/raygui.dll filter=lfs diff=lfs merge=lfs -text +vendor/raylib/windows/raygui.lib filter=lfs diff=lfs merge=lfs -text +vendor/raylib/windows/rayguidll.lib filter=lfs diff=lfs merge=lfs -text +vendor/box3d/lib/linux-amd64/libbox3d.a filter=lfs diff=lfs merge=lfs -text +vendor/box3d/lib/darwin/libbox3d.a filter=lfs diff=lfs merge=lfs -text diff --git a/.gitignore b/.gitignore index a12255166..bf0efe2a0 100644 --- a/.gitignore +++ b/.gitignore @@ -277,7 +277,6 @@ odin *.bin demo.bin libLLVM*.so* -*.a # core:rexcode commits its generated encode/decode tables: each arch's tablegen # emits human-readable Odin, serialized to tables/*.bin, which the library diff --git a/base/runtime/core_builtin.odin b/base/runtime/core_builtin.odin index 8c522413f..e65cf56f0 100644 --- a/base/runtime/core_builtin.odin +++ b/base/runtime/core_builtin.odin @@ -1277,7 +1277,7 @@ assign_at_elem_fixed_capacity_dynamic_array :: proc "contextless" (array: ^$T/[d array[index] = arg ok = true } else { - resize(array, index+1, loc) or_return + resize(array, index+1) or_return array[index] = arg ok = true } @@ -1296,7 +1296,7 @@ assign_at_elems_fixed_capacity_dynamic_array :: proc "contextless" (array: ^$T/[ copy(array[index:], args) ok = true } else { - resize(array, new_size, loc) or_return + resize(array, new_size) or_return copy(array[index:], args) ok = true } @@ -1314,7 +1314,7 @@ assign_at_elem_string_fixed_capacity_dynamic_array :: proc "contextless" (array: copy(array[index:], arg) ok = true } else { - resize(array, new_size, loc) or_return + resize(array, new_size) or_return copy(array[index:], arg) ok = true } diff --git a/build.bat b/build.bat index 9cddcc375..cc9d6af05 100644 --- a/build.bat +++ b/build.bat @@ -21,7 +21,7 @@ if "%VSCMD_ARG_TGT_ARCH%" neq "x64" ( where /Q git.exe || goto skip_git_hash if not exist .git\ goto skip_git_hash -for /f "tokens=1,2" %%i IN ('git show "--pretty=%%cd %%h" "--date=format:%%Y-%%m-%%d" --no-patch --no-notes HEAD') do ( +for /f "tokens=1,2" %%i IN ('git -c "log.showSignature=false" show "--pretty=%%cd %%h" "--date=format:%%Y-%%m-%%d" --no-patch --no-notes HEAD') do ( set CURR_DATE_TIME=%%i set GIT_SHA=%%j ) @@ -138,4 +138,4 @@ if %release_mode% EQU 0 echo: & echo Debug compiler built. Note: run "build.bat del *.obj > NUL 2> NUL -:end_of_build \ No newline at end of file +:end_of_build diff --git a/build_odin.sh b/build_odin.sh index 65e74120a..9eab985e9 100755 --- a/build_odin.sh +++ b/build_odin.sh @@ -1,6 +1,10 @@ #!/usr/bin/env sh set -eu +SUPPORTED_LLVM_VERSIONS="22 21 20 19 18 17" +SUGGESTED_LLVM_VERSION="22" +MINIMUM_LLVM_VERSION="17" + : ${CPPFLAGS=} : ${CXXFLAGS=} : ${LDFLAGS=} @@ -13,8 +17,10 @@ OS_ARCH="$(uname -m)" OS_NAME="$(uname -s)" if [ -d ".git" ] && [ -n "$(command -v git)" ]; then - GIT_SHA=$(git show --pretty='%h' --no-patch --no-notes HEAD) - GIT_DATE=$(git show "--pretty=%cd" "--date=format:%Y-%m" --no-patch --no-notes HEAD) + # Counter the user's git config to show the signature in logs. + gitnosig="-c log.showSignature=false" + GIT_SHA=$(git $gitnosig show --pretty='%h' --no-patch --no-notes HEAD) + GIT_DATE=$(git $gitnosig show "--pretty=%cd" "--date=format:%Y-%m" --no-patch --no-notes HEAD) CPPFLAGS="$CPPFLAGS -DGIT_SHA=\"$GIT_SHA\"" else GIT_DATE=$(date +"%Y-%m") @@ -26,8 +32,6 @@ error() { exit 1 } -SUPPORTED_LLVM_VERSIONS="22 21 20 19 18 17 14" - # Brew advises people not to add llvm to their $PATH, so try and use brew to find it. if [ -z "$LLVM_CONFIG" ] && [ -n "$(command -v brew)" ]; then for V in $SUPPORTED_LLVM_VERSIONS; do @@ -61,7 +65,22 @@ if [ -z "$LLVM_CONFIG" ]; then done if [ -z "$LLVM_CONFIG" ]; then - error "No supported llvm-config command found. Set LLVM_CONFIG to proceed." + if [ -f "/etc/os-release" ]; then + . /etc/os-release + case "$ID" in + ubuntu|debian|linuxmint|pop|zorin|kali|elementary|raspbian) + echo "ERROR: No supported llvm-config command found. Set LLVM_CONFIG to proceed." + echo "We suggest installing LLVM $SUGGESTED_LLVM_VERSION from https://apt.llvm.org" + exit 1 + ;; + fedora|centos|rocky|almalinux|rhel|amzn|ol|centos-stream) + echo "ERROR: No supported llvm-config command found. Set LLVM_CONFIG to proceed." + echo "We suggest installing LLVM $SUGGESTED_LLVM_VERSION via COPR, e.g. dnf copr enable fedora-llvm-team/llvm-snapshots" + exit 1 + ;; + esac + fi + error "No supported llvm-config command found. Set LLVM_CONFIG to proceed. We suggest LLVM $SUGGESTED_LLVM_VERSION." fi fi @@ -78,8 +97,8 @@ LLVM_VERSION_MAJOR="$(echo $LLVM_VERSION | awk -F. '{print $1}')" LLVM_VERSION_MINOR="$(echo $LLVM_VERSION | awk -F. '{print $2}')" LLVM_VERSION_PATCH="$(echo $LLVM_VERSION | awk -F. '{print $3}')" -if [ $LLVM_VERSION_MAJOR -lt 14 ] || ([ $LLVM_VERSION_MAJOR -gt 14 ] && [ $LLVM_VERSION_MAJOR -lt 17 ]) || [ $LLVM_VERSION_MAJOR -gt 22 ]; then - error "Invalid LLVM version $LLVM_VERSION: must be 14, 17, 18, 19, 20, 21 or 22" +if [ $LLVM_VERSION_MAJOR -lt $MINIMUM_LLVM_VERSION ]; then + error "Unsupported LLVM version $LLVM_VERSION: must be 17, 18, 19, 20, 21 or 22" fi case "$OS_NAME" in diff --git a/core/container/avl/avl.odin b/core/container/avl/avl.odin index 1208cd213..7c99879a7 100644 --- a/core/container/avl/avl.odin +++ b/core/container/avl/avl.odin @@ -60,13 +60,13 @@ Iterator :: struct($Value: typeid) { _called_next: bool, } -// init initializes a tree. +// `init` initializes a tree. init :: proc { init_ordered, init_cmp, } -// init_cmp initializes a tree. +// `init_cmp` initializes a tree. init_cmp :: proc( t: ^$T/Tree($Value), cmp_fn: proc(a, b: Value) -> Ordering, @@ -78,7 +78,7 @@ init_cmp :: proc( t._size = 0 } -// init_ordered initializes a tree containing ordered items, with +// `init_ordered` initializes a tree containing ordered items, with // a comparison function that results in an ascending order sort. init_ordered :: proc( t: ^$T/Tree($Value), @@ -87,7 +87,7 @@ init_ordered :: proc( init_cmp(t, slice.cmp_proc(Value), node_allocator) } -// destroy de-initializes a tree. +// `destroy` de-initializes a tree. destroy :: proc(t: ^$T/Tree($Value), call_on_remove: bool = true) { iter := iterator(t, Direction.Forward) for _ in iterator_next(&iter) { @@ -95,24 +95,24 @@ destroy :: proc(t: ^$T/Tree($Value), call_on_remove: bool = true) { } } -// len returns the number of elements in the tree. +// `len` returns the number of elements in the tree. len :: proc "contextless" (t: ^$T/Tree($Value)) -> int { return t._size } -// first returns the first node in the tree (in-order) or nil if and only if (⟺) +// `first` returns the first node in the tree (in-order) or nil if and only if (⟺) // the tree is empty. first :: proc "contextless" (t: ^$T/Tree($Value)) -> ^Node(Value) { return tree_first_or_last_in_order(t, Direction.Backward) } -// last returns the last element in the tree (in-order) or nil if and only if (⟺) +// `last` returns the last element in the tree (in-order) or nil if and only if (⟺) // the tree is empty. last :: proc "contextless" (t: ^$T/Tree($Value)) -> ^Node(Value) { return tree_first_or_last_in_order(t, Direction.Forward) } -// find finds the value in the tree, and returns the corresponding +// `find` finds the value in the tree, and returns the corresponding // node or nil if and only if (⟺) the value is not present. find :: proc(t: ^$T/Tree($Value), value: Value) -> ^Node(Value) { cur := t._root @@ -130,7 +130,7 @@ find :: proc(t: ^$T/Tree($Value), value: Value) -> ^Node(Value) { return cur } -// find_or_insert attempts to insert the value into the tree, and returns +// `find_or_insert` attempts to insert the value into the tree, and returns // the node, a boolean indicating if the value was inserted, and the // node allocator error if relevant. If the value is already // present, the existing node is returned un-altered. @@ -168,7 +168,7 @@ find_or_insert :: proc( return } -// remove removes a node or value from the tree, and returns true if and only if (⟺) the +// `remove` removes a node or value from the tree, and returns true if and only if (⟺) the // removal was successful. While the node's value will be left intact, // the node itself will be freed via the tree's node allocator. remove :: proc { @@ -176,7 +176,7 @@ remove :: proc { remove_node, } -// remove_value removes a value from the tree, and returns true if and only if (⟺) the +// `remove_value` removes a value from the tree, and returns true if and only if (⟺) the // removal was successful. While the node's value will be left intact, // the node itself will be freed via the tree's node allocator. remove_value :: proc(t: ^$T/Tree($Value), value: Value, call_on_remove: bool = true) -> bool { @@ -187,7 +187,7 @@ remove_value :: proc(t: ^$T/Tree($Value), value: Value, call_on_remove: bool = t return remove_node(t, n, call_on_remove) } -// remove_node removes a node from the tree, and returns true if and only if (⟺) the +// `remove_node` removes a node from the tree, and returns true if and only if (⟺) the // removal was successful. While the node's value will be left intact, // the node itself will be freed via the tree's node allocator. remove_node :: proc(t: ^$T/Tree($Value), node: ^Node(Value), call_on_remove: bool = true) -> bool { @@ -249,7 +249,7 @@ remove_node :: proc(t: ^$T/Tree($Value), node: ^Node(Value), call_on_remove: boo return true } -// iterator returns a tree iterator in the specified direction. +// `iterator` returns a tree iterator in the specified direction. iterator :: proc "contextless" (t: ^$T/Tree($Value), direction: Direction) -> Iterator(Value) { it: Iterator(Value) it._tree = transmute(^Tree(Value))t @@ -260,7 +260,7 @@ iterator :: proc "contextless" (t: ^$T/Tree($Value), direction: Direction) -> It return it } -// iterator_from_pos returns a tree iterator in the specified direction, +// `iterator_from_pos` returns a tree iterator in the specified direction, // spanning the range [pos, last] (inclusive). iterator_from_pos :: proc "contextless" ( t: ^$T/Tree($Value), @@ -280,14 +280,14 @@ iterator_from_pos :: proc "contextless" ( return it } -// iterator_get returns the node currently pointed to by the iterator, +// `iterator_get` returns the node currently pointed to by the iterator, // or nil if and only if (⟺) the node has been removed, the tree is empty, or the end // of the tree has been reached. iterator_get :: proc "contextless" (it: ^$I/Iterator($Value)) -> ^Node(Value) { return it._cur } -// iterator_remove removes the node currently pointed to by the iterator, +// `iterator_remove` removes the node currently pointed to by the iterator, // and returns true if and only if (⟺) the removal was successful. Semantics are the // same as the Tree remove. iterator_remove :: proc(it: ^$I/Iterator($Value), call_on_remove: bool = true) -> bool { @@ -303,7 +303,7 @@ iterator_remove :: proc(it: ^$I/Iterator($Value), call_on_remove: bool = true) - return ok } -// iterator_next advances the iterator and returns the (node, true) or +// `iterator_next` advances the iterator and returns the (node, true) or // or (nil, false) if and only if (⟺) the end of the tree has been reached. // // Note: The first call to iterator_next will return the first node instead diff --git a/core/container/handle_map/dynamic_handle_map.odin b/core/container/handle_map/dynamic_handle_map.odin index 09ea92f82..952d9daea 100644 --- a/core/container/handle_map/dynamic_handle_map.odin +++ b/core/container/handle_map/dynamic_handle_map.odin @@ -1,7 +1,6 @@ package container_handle_map import "base:runtime" -import "base:builtin" import "base:intrinsics" @(require) import "core:container/xar" @@ -139,4 +138,4 @@ dynamic_iterate :: proc "contextless" (it: ^$DHI/Dynamic_Handle_Map_Iterator($D/ } it.index = 0 return -} \ No newline at end of file +} diff --git a/core/container/queue/queue.odin b/core/container/queue/queue.odin index 6790e825a..4b371f378 100644 --- a/core/container/queue/queue.odin +++ b/core/container/queue/queue.odin @@ -406,7 +406,7 @@ push_back_elems :: proc(q: ^$Q/Queue($T), elems: ..T, loc := #caller_location) - } /* -Consume `n` elements from the back of the queue. +Consume `n` elements from the front of the queue. This will raise a bounds checking error if the queue does not have enough elements. */ diff --git a/core/container/rbtree/rbtree.odin b/core/container/rbtree/rbtree.odin index c138838df..d3b790dcc 100644 --- a/core/container/rbtree/rbtree.odin +++ b/core/container/rbtree/rbtree.odin @@ -63,13 +63,13 @@ Iterator :: struct($Key: typeid, $Value: typeid) { _called_next: bool, } -// init initializes a tree. +// `init` initializes a tree. init :: proc { init_ordered, init_cmp, } -// init_cmp initializes a tree. +// `init_cmp` initializes a tree. init_cmp :: proc(t: ^$T/Tree($Key, $Value), cmp_fn: proc(a, b: Key) -> Ordering, node_allocator := context.allocator) { t._root = nil t._node_allocator = node_allocator @@ -77,13 +77,13 @@ init_cmp :: proc(t: ^$T/Tree($Key, $Value), cmp_fn: proc(a, b: Key) -> Ordering, t._size = 0 } -// init_ordered initializes a tree containing ordered keys, with +// `init_ordered` initializes a tree containing ordered keys, with // a comparison function that results in an ascending order sort. init_ordered :: proc(t: ^$T/Tree($Key, $Value), node_allocator := context.allocator) where intrinsics.type_is_ordered(Key) { init_cmp(t, slice.cmp_proc(Key), node_allocator) } -// destroy de-initializes a tree. +// `destroy` de-initializes a tree. destroy :: proc(t: ^$T/Tree($Key, $Value), call_on_remove: bool = true) { iter := iterator(t, .Forward) for _ in iterator_next(&iter) { @@ -95,19 +95,19 @@ len :: proc "contextless" (t: $T/Tree($Key, $Value)) -> (node_count: int) { return t._size } -// first returns the first node in the tree (in-order) or nil if and only if (⟺) +// `first` returns the first node in the tree (in-order) or nil if and only if (⟺) // the tree is empty. first :: proc "contextless" (t: ^$T/Tree($Key, $Value)) -> ^Node(Key, Value) { return tree_first_or_last_in_order(t, Direction.Backward) } -// last returns the last element in the tree (in-order) or nil if and only if (⟺) +// `last` returns the last element in the tree (in-order) or nil if and only if (⟺) // the tree is empty. last :: proc "contextless" (t: ^$T/Tree($Key, $Value)) -> ^Node(Key, Value) { return tree_first_or_last_in_order(t, Direction.Forward) } -// find finds the key in the tree, and returns the corresponding node, or nil if and only if (⟺) the value is not present. +// `find` finds the key in the tree, and returns the corresponding node, or nil if and only if (⟺) the value is not present. find :: proc(t: $T/Tree($Key, $Value), key: Key) -> (node: ^Node(Key, Value)) { node = t._root for node != nil { @@ -120,7 +120,7 @@ find :: proc(t: $T/Tree($Key, $Value), key: Key) -> (node: ^Node(Key, Value)) { return node } -// find_value finds the key in the tree, and returns the corresponding value, or nil if and only if (⟺) the value is not present. +// `find_value` finds the key in the tree, and returns the corresponding value, or nil if and only if (⟺) the value is not present. find_value :: proc(t: $T/Tree($Key, $Value), key: Key) -> (value: Value, ok: bool) #optional_ok { if n := find(t, key); n != nil { return n.value, true @@ -128,10 +128,36 @@ find_value :: proc(t: $T/Tree($Key, $Value), key: Key) -> (value: Value, ok: boo return } -// find_or_insert attempts to insert the key-value pair into the tree, and returns +// `find_or_insert` attempts to insert the key-value pair into the tree, and returns // the node, a boolean indicating if a new node was inserted, and the -// node allocator error if relevant. If the key is already present, the existing node is updated and returned. +// node allocator error if relevant. If the key is already present, the existing node is returned un-altered. find_or_insert :: proc(t: ^$T/Tree($Key, $Value), key: Key, value: Value) -> (n: ^Node(Key, Value), inserted: bool, err: runtime.Allocator_Error) { + n_ptr := &t._root + for n_ptr^ != nil { + n = n_ptr^ + switch t._cmp_fn(key, n.key) { + case .Less: + n_ptr = &n._left + case .Greater: + n_ptr = &n._right + case .Equal: + return + } + } + _parent := n + + n = new_clone(Node(Key, Value){key=key, value=value, _parent=_parent, _color=.Red}, t._node_allocator) or_return + n_ptr^ = n + insert_case1(t, n) + t._size += 1 + return n, true, nil +} + + +// `upsert` attempts to insert the key-value pair into the tree, and returns +// the node, a boolean indicating if a new node was inserted, and the +// node allocator error if relevant. If the key is already present, the existing node's value is updated. +upsert :: proc(t: ^$T/Tree($Key, $Value), key: Key, value: Value) -> (n: ^Node(Key, Value), inserted: bool, err: runtime.Allocator_Error) { n_ptr := &t._root for n_ptr^ != nil { n = n_ptr^ @@ -154,7 +180,7 @@ find_or_insert :: proc(t: ^$T/Tree($Key, $Value), key: Key, value: Value) -> (n: return n, true, nil } -// remove removes a node or value from the tree, and returns true if and only if (⟺) the +// `remove` removes a node or value from the tree, and returns true if and only if (⟺) the // removal was successful. While the node's value will be left intact, // the node itself will be freed via the tree's node allocator. remove :: proc { @@ -162,7 +188,7 @@ remove :: proc { remove_node, } -// remove_value removes a value from the tree, and returns true if and only if (⟺) the +// `remove_value` removes a value from the tree, and returns true if and only if (⟺) the // removal was successful. While the node's key + value will be left intact, // the node itself will be freed via the tree's node allocator. remove_key :: proc(t: ^$T/Tree($Key, $Value), key: Key, call_on_remove := true) -> bool { @@ -173,7 +199,7 @@ remove_key :: proc(t: ^$T/Tree($Key, $Value), key: Key, call_on_remove := true) return remove_node(t, n, call_on_remove) } -// remove_node removes a node from the tree, and returns true if and only if (⟺) the +// `remove_node` removes a node from the tree, and returns true if and only if (⟺) the // removal was successful. While the node's key + value will be left intact, // the node itself will be freed via the tree's node allocator. remove_node :: proc(t: ^$T/Tree($Key, $Value), node: ^$N/Node(Key, Value), call_on_remove := true) -> (found: bool) { @@ -207,7 +233,7 @@ remove_node :: proc(t: ^$T/Tree($Key, $Value), node: ^$N/Node(Key, Value), call_ return true } -// iterator returns a tree iterator in the specified direction. +// `iterator` returns a tree iterator in the specified direction. iterator :: proc "contextless" (t: ^$T/Tree($Key, $Value), direction: Direction) -> Iterator(Key, Value) { it: Iterator(Key, Value) it._tree = cast(^Tree(Key, Value))t @@ -218,7 +244,7 @@ iterator :: proc "contextless" (t: ^$T/Tree($Key, $Value), direction: Direction) return it } -// iterator_from_pos returns a tree iterator in the specified direction, +// `iterator_from_pos` returns a tree iterator in the specified direction, // spanning the range [pos, last] (inclusive). iterator_from_pos :: proc "contextless" (t: ^$T/Tree($Key, $Value), pos: ^Node(Key, Value), direction: Direction) -> Iterator(Key, Value) { it: Iterator(Key, Value) @@ -234,14 +260,14 @@ iterator_from_pos :: proc "contextless" (t: ^$T/Tree($Key, $Value), pos: ^Node(K return it } -// iterator_get returns the node currently pointed to by the iterator, +// `iterator_get` returns the node currently pointed to by the iterator, // or nil if and only if (⟺) the node has been removed, the tree is empty, or the end // of the tree has been reached. iterator_get :: proc "contextless" (it: ^$I/Iterator($Key, $Value)) -> ^Node(Key, Value) { return it._cur } -// iterator_remove removes the node currently pointed to by the iterator, +// `iterator_remove` removes the node currently pointed to by the iterator, // and returns true if and only if (⟺) the removal was successful. Semantics are the // same as the Tree remove. iterator_remove :: proc(it: ^$I/Iterator($Key, $Value), call_on_remove: bool = true) -> bool { @@ -257,7 +283,7 @@ iterator_remove :: proc(it: ^$I/Iterator($Key, $Value), call_on_remove: bool = t return ok } -// iterator_next advances the iterator and returns the (node, true) or +// `iterator_next` advances the iterator and returns the (node, true) or // or (nil, false) if and only if (⟺) the end of the tree has been reached. // // Note: The first call to iterator_next will return the first node instead diff --git a/core/container/xar/xar.odin b/core/container/xar/xar.odin index 9f0b6e90c..d2a517464 100644 --- a/core/container/xar/xar.odin +++ b/core/container/xar/xar.odin @@ -265,7 +265,7 @@ array_push_back_elem :: proc(x: ^$X/Array($T, $SHIFT), value: T, loc := #caller_ chunk_idx, elem_idx, chunk_cap := _meta_get(SHIFT, uint(x.len)) if x.chunks[chunk_idx] == nil { - x.chunks[chunk_idx] = make([^]T, chunk_cap, x.allocator) or_return + x.chunks[chunk_idx] = make([^]T, chunk_cap, x.allocator, loc) or_return } x.chunks[chunk_idx][elem_idx] = value x.len += 1 @@ -328,7 +328,7 @@ array_push_back_elem_and_get_ptr :: proc(x: ^$X/Array($T, $SHIFT), value: T, loc chunk_idx, elem_idx, chunk_cap := _meta_get(SHIFT, uint(x.len)) if x.chunks[chunk_idx] == nil { - x.chunks[chunk_idx] = make([^]T, chunk_cap, x.allocator) or_return + x.chunks[chunk_idx] = make([^]T, chunk_cap, x.allocator, loc) or_return } x.chunks[chunk_idx][elem_idx] = value x.len += 1 diff --git a/core/crypto/README.md b/core/crypto/README.md index 5065a6d02..0d1a8711b 100644 --- a/core/crypto/README.md +++ b/core/crypto/README.md @@ -13,10 +13,11 @@ constant-time byte comparison. - The crypto packages are not thread-safe. - Best-effort is make to mitigate timing side-channels on reasonable architectures. Architectures that are known to be unreasonable include - but are not limited to i386, i486, and WebAssembly. + but are not limited to i386, i486, VIA Nano 2000, ARM7T/ARM9T/Cortex-M3, + and WASM. - Implementations assume a 64-bit architecture (64-bit integer arithmetic - is fast, and includes add-with-carry, sub-with-borrow, and full-result - multiply). + is fast, and includes contant-time add-with-carry, sub-with-borrow, and + full-result multiply). - Hardware sidechannels are explicitly out of scope for this package. Notable examples include but are not limited to: - Power/RF side-channels etc. @@ -29,4 +30,4 @@ constant-time byte comparison. ## License -This library is made available under the zlib license. \ No newline at end of file +This library is made available under the zlib license. diff --git a/core/crypto/_bigint/i31.odin b/core/crypto/_bigint/i31.odin new file mode 100644 index 000000000..e71a95310 --- /dev/null +++ b/core/crypto/_bigint/i31.odin @@ -0,0 +1,904 @@ +// Constant time Big Integers +package _bigint + +// Copyright (c) 2016 Thomas Pornin +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions +// are met: +// +// 1. Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// THIS SOFTWARE IS PROVIDED BY THE AUTHORS “AS IS” AND ANY EXPRESS OR +// IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHORS BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +// DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE +// GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, +// WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING +// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF +// THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +import "base:intrinsics" +import "core:crypto" +import subtle "core:crypto/_subtle" +import "core:slice" + +// Integers 'i31' +// -------------- +// +// The 'i31' functions implement computations on big integers using +// an internal representation as an array of 32-bit integers. For +// an array `x`: +// -- x[0] encodes the array length and the "announced bit length" +// of the integer: namely, if the announced bit length is k, +// then x[0] = ((k / 31) << 5) + (k % 31). +// -- x[1], x[2]... contain the value in little-endian order, 31 +// bits per word (x[1] contains the least significant 31 bits). +// The upper bit of each word is 0. +// +// Multiplications rely on the elementary 32x32->64 multiplication. +// +// The announced bit length specifies the number of bits that are +// significant in the subsequent 32-bit words. Unused bits in the +// last (most significant) word are set to 0; subsequent words are +// uninitialized and need not exist at all. +// +// The execution time and memory access patterns of all computations +// depend on the announced bit length, but not on the actual word +// values. For modular integers, the announced bit length of any integer +// modulo `n` is equal to the actual bit length of `n`; thus, computations +// on modular integers are "constant-time" (only the modulus length may leak). + +I31_MASK :: 0x7fff_ffff + +// Compute the bit length of a 32-bit integer. +// Returned value is between 0 and 32 (inclusive). +@(require_results) +_u32_bit_length :: proc "contextless" (x: u32) -> (length: u32) { + x := x + k := subtle.neq(x, 0) + c := subtle.gt(x, 0xFFFF); x = subtle.csel(x, x >> 16, c); k += c << 4 + c = subtle.gt(x, 0x00FF); x = subtle.csel(x, x >> 8, c); k += c << 3 + c = subtle.gt(x, 0x000F); x = subtle.csel(x, x >> 4, c); k += c << 2 + c = subtle.gt(x, 0x0003); x = subtle.csel(x, x >> 2, c); k += c << 1 + k += subtle.gt(x, 0x0001) + return k +} + +// Multiply two 31-bit integers, with a 62-bit result. This default +// implementation assumes that the basic multiplication operator +// yields constant-time code. +// +// The mul31_lo() returns only the low 31 bits of the product. +// +// Note/Odin: +// The original BearSSL code provides alternative implemenetations +// of these routines gated behind `BR_CT_MUL31`, however that macro +// is only useful on Intel 80386/80486, VIA Nano 2000, and ARM7T/ARM9T. +@(require_results) +_mul31 :: #force_inline proc "contextless" (x, y: u32) -> (res: u64) { + return u64(x) * u64(y) +} + +@(private="file", require_results) +_mul31_lo :: #force_inline proc "contextless" (x, y: u32) -> (res: u32) { + return (x * y) & I31_MASK +} + +// Wrapper for `div_rem`; the remainder is returned, and the quotient is +// discarded. +@(private, require_results) +_rem_u32 :: #force_inline proc "contextless" (hi: u32, lo: u32, d: u32) -> (res: u32) { + _, rem := div_rem_u32(hi, lo, d) + return rem +} + +// Wrapper for `div_rem`; the quotient is returned, and the remainder is +// discarded. +@(private="file", require_results) +_div_u32 :: #force_inline proc "contextless" (hi: u32, lo: u32, d: u32) -> (quo: u32) { + q, _ := div_rem_u32(hi, lo, d) + return q +} + +// Constant-time division. The dividend `hi:lo` is divided by the divisor `d`; +// the quotient and remainder are returned. +// +// If `hi == d`, then the quotient does not fit on 32 bits; returned value is thus truncated. +// If `hi > d`, returned values are indeterminate. +@(require_results) +div_rem_u32 :: proc "contextless" (hi: u32, lo: u32, d: u32) -> (quo: u32, rem: u32) { + // TODO: optimize this + hi := hi + lo := lo + ch := subtle.eq(hi, d) + hi = subtle.csel(hi, 0, ch) + for k := uint(31); k > 0; k -= 1 { + j := 32 - k + w := (hi << j) | (lo >> k) + ctl := subtle.ge(w, d) | (hi >> k) + hi2 := (w - d) >> j + lo2 := lo - (d << k) + hi = subtle.csel(hi, hi2, ctl) + lo = subtle.csel(lo, lo2, ctl) + quo |= ctl << k + } + cf := subtle.ge(lo, d) | hi + quo |= cf + rem = subtle.csel(lo, lo - d, cf) + return +} + +// i31_rem computes x / y and returns the remainder. +@(require_results) +i31_rem :: proc "contextless" (x: []u32, y: u32) -> u32 { + words := uint(x[0] + 31) >> 5 + x_ := x[1:] + + r: u32 + for i := int(words-1); i >= 0; i -= 1 { + r = _rem_u32(r, x_[i], y) + } + + return r +} + +// Test whether an integer `x` is zero. +@(optimization_mode="none", require_results) +i31_is_zero :: proc "contextless" (x: []u32) -> (res: u32) { + z: u32 + + for u := (x[0] + 31) >> 5; u > 0; u -= 1 { + z |= x[u] + } + return ~(z | -z) >> 31 +} + +// Add `b` to `a` and return the `carry` (`0` or `1`). if `ctl` is `1`. +// If `ctl` is `0`, `a` is left alone but the `carry` will still be computed. +// +// The slices `a` and `b` MUST have the same announced bit length (in subscript `0`) +// +// `a` and `b` MAY be the same array, but partial overlap is not allowed. +@(require_results) +i31_add :: proc "contextless" (a: []u32, b: []u32, ctl: u32) -> (carry: u32) { + words := uint(a[0] + 63) >> 5 + for u in 1..> 31 + a[u] = subtle.csel(aw, naw & I31_MASK, ctl) + } + return +} + +// Subtract `b` from `a` and return the `carry` (`0` or `1`), if `ctl` is `1`. +// If `ctl` is `0`, then `a` is unmodified, but the carry is still computed +// and returned. +// +// The slices `a` and `b` MUST have the same announced bit length (in subscript `0`) +// +// `a` and `b` MAY be the same array, but partial overlap is not allowed. +@(require_results) +i31_sub :: proc "contextless" (a: []u32, b: []u32, ctl: u32) -> (carry: u32) { + words := uint(a[0] + 63) >> 5 + for u in 1..> 31 + a[u] = subtle.csel(aw, naw & I31_MASK, ctl) + } + return +} + +// Compute the ENCODED actual bit length of an integer `x`. +// The argument `x` should point to the first (least significant) +// value word of the integer. +// +// The upper bit of each value word MUST be `0`. +// +// Returned value is `((k / 31) << 5) + (k % 31)` if the bit length is `k`. +// +// CT: value or length of `x` does not leak. +@(require_results) +i31_bit_length :: proc "contextless" (x: []u32) -> (res: u32) { + tw, twk: u32 + + xlen := len(x) + for xlen > 0 { + xlen -= 1 + c := subtle.eq(tw, 0) + w := x[xlen] + + tw = subtle.csel(tw, w, c) + twk = subtle.csel(twk, u32(xlen), c) + } + return (twk << 5) + _u32_bit_length(tw) +} + +// Decode an integer from its big-endian unsigned representation. The +// "true" bit length of the integer is computed and set in the encoded +// announced bit length (`x[0]`), but all words of `x` corresponding to +// the full slice of source bytes. +// +// `x` needs to have a minimum length of: `1 + ((len(src) * 8) + 31) / 31` +// +// CT: value or length of `x` does not leak. +i31_decode :: proc "contextless" (x: []u32, src: []byte) { + u := len(src) - 1 + v := 1 + acc := u32(0) + acc_len := uint(0) + for u >= 0 { + b := u32(src[u]) + acc |= b << acc_len + acc_len += 8 + if acc_len >= 31 { + x[v] = acc & I31_MASK + acc_len -= 31 + acc = b >> (8 - acc_len) + v += 1 + } + u -= 1 + } + if acc_len != 0 { + x[v] = acc + v += 1 + } + x[0] = i31_bit_length(x[1:]) +} + +// Decode an integer from its big-endian unsigned representation. +// The integer MUST be lower than `m`; the (encoded) announced bit length +// written in `x` will be equal to that of `m`. All bytes from the +// `src` slice are read. +// +// Returned value is `1` if the decode value fits within the modulus, `0` +// otherwise. In the latter case, the `x` buffer will be set to `0` (but +// still with the announced bit length of `m`). +// +// CT: value or length of `x` does not leak. Memory access pattern depends +// only `src`'s length and the announced bit length of `m`. Whether `x` fits or +// not does not leak either. +@(require_results) +i31_decode_mod :: proc "contextless" (x: []u32, src: []byte, m: []u32) -> (res: u32) { + // Two-pass algorithm: in the first pass, we determine whether the + // value fits; in the second pass, we do the actual write. + // + // During the first pass, `res` contains the comparison result so far: + // 0x00000000 value is equal to the modulus + // 0x00000001 value is greater than the modulus + // 0xFFFFFFFF value is lower than the modulus + // + // Since we iterate starting with the least significant bytes (at + // the end of `src`), each new comparison overrides the previous + // except when the comparison yields 0 (equal). + // + // During the second pass, `res` is either 0xFFFFFFFF (value fits) 0x00000000 (value does not fit). + // We must iterate over all bytes of the source, _and_ possibly + // some extra virtual bytes (with value 0) so as to cover the + // complete modulus as well. We also add 4 such extra bytes beyond + // the modulus length because it then guarantees that no accumulated + // partial word remains to be processed. + _len := uint(len(src)) + mlen := uint((m[0] + 31) >> 5) + tlen := uint(mlen << 2) + if tlen < _len { + tlen = _len + } + tlen += 4 + + for pass in 0..<2 { + v := uint(1) + acc := u32(0) + acc_len := u32(0) + + for u in uint(0)..= 31 { + xw := acc & I31_MASK + acc_len -= 31 + + acc = b >> (8 - acc_len) + if v <= mlen { + if pass == 1 { + x[v] = res & xw + } else { + cc := u32(subtle.cmp(xw, m[v])) + res = subtle.csel(cc, res, subtle.eq(cc, 0)) + } + } else { + if pass == 0 { + res = subtle.csel(1, res, subtle.eq(xw, 0)) + } + } + v += 1 + } + } + + // When we reach this point at the end of the first pass: + // r is either 0, 1 or -1; we want to set r to 0 if it + // is equal to 0 or 1, and leave it to -1 otherwise. + // + // When we reach this point at the end of the second pass: + // r is either 0 or -1; we want to leave that value + // untouched. This is a subcase of the previous. + res >>= 1 + res |= (res << 1) + } + + x[0] = m[0] + + return res & 1 +} + +// Zeroize integer `x`. The announced bit length is set to the provided value, +// and the corresponding words are set to 0. The ENCODED bit length is expected +//here. +i31_zero :: proc "contextless" (x: []u32, bit_len: u32) { + x[0] = bit_len + intrinsics.mem_zero(raw_data(x[1:]), ((bit_len + 31) >> 5) * size_of(u32)) +} + +// Make a random integer of the provided size. The size is encoded. +// The header word is untouched. +i31_mkrand :: proc(x: []u32, esize: u32) { + _len := (esize + 31) >> 5 + x_ := slice.reinterpret([]byte, x) + crypto.rand_bytes(x_[4:4 + _len * size_of(u32)]) + for u in 1..<_len { + x[u] &= I31_MASK + } + m := _len & 31 + if m == 0 { + x[_len] &= I31_MASK + } else { + x[_len] &= I31_MASK >> (31 - m) + } +} + +// Right-shift an integer. The shift amount must be lower than 31 bits. +i31_rshift :: proc "contextless" (x: []u32, shift_amount: i32) { + _len := uint(x[0] + 31) >> 5 + if _len == 0 { + return + } + + count := uint(shift_amount) + + r := x[1] >> count + for u in 2..= _len { + w := u32(x[u]) + + x[u - 1] = ((w << (31 - count)) | r) & I31_MASK + r = w >> count + } + x[_len] = r +} + +// Reduce integer `a` modulo `m`. The result is written to `x`, +// and its announced bit length is set to be equal to that of `m`. +// +// `x` MUST be distinct from `a` and `m`. +// +// CT: only announced bit lengths leak, not values of `x`, `a` or `m`. +i31_reduce :: proc "contextless" (x: []u32, a: []u32, m: []u32) { + m_bitlen := m[0] + mlen := uint(m_bitlen + 31) >> 5 + + x[0] = m_bitlen + if m_bitlen == 0 { + return + } + + // If the source is shorter, then simply copy all words from a[] + // and zero out the upper words. + a_bitlen := a[0] + alen := uint(a_bitlen + 31) >> 5 + if a_bitlen < m_bitlen { + copy(x[1:], a[1:][:alen]) + for u in alen.. 0; u -= 1 { + i31_muladd_small(x, a[u], m) + } +} + +// Decode an integer from its big-endian unsigned representation, and +// reduce it modulo the provided modulus `m`. The announced bit length +// of the result is set to be equal to that of the modulus. +// +// `x` MUST be distinct from `m`. +i31_decode_reduce :: proc "contextless" (x: []u32, src: []byte, m: []u32) { + // Get the encoded bit length. + m_ebitlen := m[0] + + // Special case for an invalid (null) modulus. + if m_ebitlen == 0 { + x[0] = 0 + return + } + + // Clear the destination. + i31_zero(x, m_ebitlen) + + // First decode directly as many bytes as possible. + // This requires computing the actual bit length. + m_rbitlen := m_ebitlen >> 5 + m_rbitlen = (m_ebitlen & 31) + (m_rbitlen << 5) - m_rbitlen + + mblen := uint(m_rbitlen + 7) >> 3 + k := mblen - 1 + _len := uint(len(src)) + + if k >= _len { + i31_decode(x, src) + x[0] = m_ebitlen + return + } + + i31_decode(x, src[:k]) + x[0] = m_ebitlen + + // Input remaining bytes, using 31-bit words. + acc := u32(0) + acc_len := uint(0) + + for { + v := u32(src[k]) + + if acc_len >= 23 { + acc_len -= 23 + acc <<= (8 - acc_len) + acc |= v >> acc_len + i31_muladd_small(x, acc, m) + acc = v & (0xFF >> (8 - acc_len)) + } else { + acc = (acc << 8) | v + acc_len += 8 + } + + if k += 1; k >= _len { + break + } + } + + // We may have some bits accumulated. We then perform a shift to + // be able to inject these bits as a full 31-bit word. + if acc_len != 0 { + acc = (acc | (x[1] << acc_len)) & I31_MASK + i31_rshift(x, i32(31 - acc_len)) + i31_muladd_small(x, acc, m) + } +} + +// Multiply `x` by 2^31 and then add integer `z`, modulo `m`. +// This function assumes that `x` and `m` have the same announced bit +// length, the announced bit length of `m` matches its true bit length. +// +// `x` and `m` MUST be distinct arrays. +// `z` MUST fit in 31 bits (upper bit set to 0). +// +// CT: only the common announced bit length of `x` and `m` leaks, not +// the values of `x`, `z` or `m`. +i31_muladd_small :: proc "contextless" (x: []u32, z: u32, m: []u32) { + // We can test on the modulus bit length since we accept to leak + // that length. + m_bitlen := m[0] + if m_bitlen == 0 { + return + } + hi: u32 + if m_bitlen <= 31 { + hi = x[1] >> 1 + lo := (x[1] << 31) | z + x[1] = _rem_u32(hi, lo, m[1]) + return + } + mlen := uint(m_bitlen + 31) >> 5 + mblr := uint(m_bitlen) & 31 + + // Principle: we estimate the quotient (x*2^31+z)/m by + // doing a 64/32 division with the high words. + // + // Let: + // w = 2^31 + // a = (w*a0 + a1) * w^N + a2 + // b = b0 * w^N + b2 + // such that: + // 0 <= a0 < w + // 0 <= a1 < w + // 0 <= a2 < w^N + // w/2 <= b0 < w + // 0 <= b2 < w^N + // a < w*b + // I.e. the two top words of a are a0:a1, the top word of b is + // b0, we ensured that b0 is "full" (high bit set), and a is + // such that the quotient q = a/b fits on one word (0 <= q < w). + // + // If a = b*q + r (with 0 <= r < q), we can estimate q by + // doing an Euclidean division on the top words: + // a0*w+a1 = b0*u + v (with 0 <= v < b0) + // Then the following holds: + // 0 <= u <= w + // u-2 <= q <= u + hi = x[mlen] + a0, a1, b0: u32 + if mblr == 0 { + a0 = x[mlen] + intrinsics.mem_copy(raw_data(x[2:]), raw_data(x[1:]), (mlen - 1) * size_of(u32)) + x[1] = z + a1 = x[mlen] + b0 = m[mlen] + } else { + a0 = ((x[mlen] << (31 - mblr)) | (x[mlen - 1] >> mblr)) & I31_MASK + intrinsics.mem_copy(raw_data(x[2:]), raw_data(x[1:]), (mlen - 1) * size_of(u32)) + x[1] = z + a1 = ((x[mlen] << (31 - mblr)) | (x[mlen - 1] >> mblr)) & I31_MASK + b0 = ((m[mlen] << (31 - mblr)) | (m[mlen - 1] >> mblr)) & I31_MASK + } + + // We estimate a divisor q. If the quotient returned by div() + // is g: + // -- If a0 == b0 then g == 0; we want q = 0x7FFFFFFF. + // -- Otherwise: + // -- if g == 0 then we set q = 0; + // -- otherwise, we set q = g - 1. + // The properties described above then ensure that the true + // quotient is q-1, q or q+1. + // + // Take care that a0, a1 and b0 are 31-bit words, not 32-bit. We + // must adjust the parameters to br_div() accordingly. + g := _div_u32(a0 >> 1, a1 | (a0 << 31), b0) + q := subtle.csel(subtle.csel(g - 1, 0, subtle.eq(g, 0)), I31_MASK, subtle.eq(a0, b0)) + + // We subtract q*m from x (with the extra high word of value 'hi'). + // Since q may be off by 1 (in either direction), we may have to + // add or subtract m afterwards. + // + // The 'tb' flag will be true (1) at the end of the loop if the + // result is greater than or equal to the modulus (not counting + // 'hi' or the carry). + cc := u32(0) + tb := u32(1) + for u in 1..= mlen { + mw := m[u] + zl := _mul31(mw, q) + u64(cc) + cc = u32(zl >> 31) + zw := u32(zl) & I31_MASK + xw := x[u] + nxw := xw - zw + cc += nxw >> 31 + nxw &= I31_MASK + x[u] = nxw + tb = subtle.csel(subtle.gt(nxw, mw), tb, subtle.eq(nxw, mw)) + } + + // If we underestimated q, then either cc < hi (one extra bit + // beyond the top array word), or cc == hi and tb is true (no + // extra bit, but the result is not lower than the modulus). In + // these cases we must subtract m once. + // + // Otherwise, we may have overestimated, which will show as + // cc > hi (thus a negative result). Correction is adding m once. + over := subtle.gt(cc, hi) + under := ~over & (tb | subtle.lt(cc, hi)) + _ = i31_add(x, m, over) + _ = i31_sub(x, m, under) +} + +// Encode an integer into its big-endian unsigned representation. The +// output length in bytes is provided (parameter 'len'); if the length +// is too short then the integer is appropriately truncated; if it is +// too long then the extra bytes are set to 0. +i31_encode :: proc "contextless" (dst: []byte, x: []u32) { + xlen := uint(x[0] + 31) >> 5 + if xlen == 0 { + intrinsics.mem_zero(raw_data(dst[:]), len(dst) * size_of(u32)) + return + } + _len := uint(len(dst)) + k := uint(1) + acc := u32(0) + acc_len := uint(0) + for _len != 0 { + w := (k <= xlen) ? x[k] : 0 + k += 1 + if (acc_len == 0) { + acc = w + acc_len = 31 + } else { + z := acc | (w << acc_len) + acc_len -= 1 + acc = w >> (31 - acc_len) + if _len >= 4 { + _len -= 4 + ptr := (^u32be)(raw_data(dst[_len:])) + intrinsics.unaligned_store(ptr, u32be(z)) + } else { + switch _len { + case 3: + dst[_len - 3] = byte(z >> 16) + fallthrough + case 2: + dst[_len - 2] = byte(z >> 8) + fallthrough + case 1: + dst[_len - 1] = byte(z) + } + return + } + } + } +} + +// Compute `-(1/x) % 2^31`. If `x` is even, then this function returns `0`. +i31_ninv31 :: proc "contextless" (x: u32) -> (y: u32) { + y = 2 - x + y *= 2 - y * x + y *= 2 - y * x + y *= 2 - y * x + y *= 2 - y * x + return subtle.csel(0, -y, x & 1) & I31_MASK +} + +// Compute a modular Montgomery multiplication. `d` is filled with the +// value of `x*y/R % m` (where `R` is the Montgomery factor). +// +// The array `d` MUST be distinct from `x`, `y` and `m`[]. +// `x` and `y` MUST be numerically lower than `m`. +// +// `x` and `y` MAY be the same array. +// +// The `m0i` parameter is equal to `-(1/m0) mod 2^31`, where `m0` is the least +// significant value word of `m` (this works only if `m` is an odd integer). +i31_montymul :: proc "contextless" (d: []u32, x: []u32, y: []u32, m: []u32, m0i: u32) { + // Each outer loop iteration computes: + // `d <- (d + xu*y + f*m) / 2^31` + // We have `xu <= 2^31-1` and `f <= 2^31-1`. + // Thus, if `d <= 2*m-1` on input, then: + // `2*m-1 + 2*(2^31-1)*m <= (2^32)*m-1` + // and the new `d` value is less than `2*m`. + // + // We represent `d` over 31-bit words, with an extra word `dh`, + // which can thus be only 0 or 1. + _len := uint((m[0] + 31) >> 5) + len4 := _len & ~uint(3) + i31_zero(d, m[0]) + dh := u32(0) + for u in 0..<_len { + // The carry for each operation fits on 32 bits: + // `d[v+1] <= 2^31-1` + // `xu*y[v+1] <= (2^31-1)*(2^31-1)` + // `f*m[v+1] <= (2^31-1)*(2^31-1)` + // `r <= 2^32-1` + // `(2^31-1) + 2*(2^31-1)*(2^31-1) + (2^32-1) = 2^63 - 2^31` + // + // After division by `2^31`, the new `r` is then at most `2^32-1` + // + // Using a 32-bit carry has performance benefits on 32-bit + // systems; however, on 64-bit architectures, we prefer to + // keep the carry (r) in a 64-bit register, thus avoiding some + // "clear high bits" operations. + xu := x[u + 1] + f := _mul31_lo((d[1] + _mul31_lo(xu, y[1])), m0i) + + r := u64(0) + v := uint(0) + for ; v < len4; v += 4 { + z := u64(d[v + 1]) + _mul31(xu, y[v + 1]) + _mul31(f, m[v + 1]) + r + r = z >> 31 + d[v + 0] = u32(z) & I31_MASK + z = u64(d[v + 2]) + _mul31(xu, y[v + 2]) + _mul31(f, m[v + 2]) + r + r = z >> 31 + d[v + 1] = u32(z) & I31_MASK + z = u64(d[v + 3]) + _mul31(xu, y[v + 3]) + _mul31(f, m[v + 3]) + r + r = z >> 31 + d[v + 2] = u32(z) & I31_MASK + z = u64(d[v + 4]) + _mul31(xu, y[v + 4]) + _mul31(f, m[v + 4]) + r + r = z >> 31 + d[v + 3] = u32(z) & I31_MASK + } + for ; v < _len; v += 1 { + z := u64(d[v + 1]) + _mul31(xu, y[v + 1]) + _mul31(f, m[v + 1]) + r + r = z >> 31 + d[v] = u32(z) & I31_MASK + } + + // Since the new `dh` can only be `0` or `1`, the addition of + // the old dh with the carry MUST fit on 32 bits, and + // thus can be done into dh itself. + dh += u32(r) + d[_len] = dh & I31_MASK + dh >>= 31 + } + + // We must write back the bit length because it was overwritten in + // the loop (not overwriting it would require a test in the loop, + // which would yield bigger and slower code). + d[0] = m[0] + + // `d` may still be greater than `m` at that point; notably, the `dh` + // word may be non-zero. + _ = i31_sub(d, m, subtle.neq(dh, 0) | subtle.not(i31_sub(d, m, 0))) +} + +// Convert a modular integer to Montgomery representation. +// +// The integer `x` MUST be lower than `m`, but with the same announced bit length. +i31_to_monty :: proc "contextless" (x: []u32, m: []u32) { + // uint32_t k; + for k := (m[0] + 31) >> 5; k > 0; k -= 1 { + i31_muladd_small(x, 0, m) + } +} + +// Convert a modular integer back from Montgomery representation. +// +// The integer `x` MUST be lower than `m`[], but with the same announced bit +// length. +// +// The `m0i` parameter is equal to `-(1/m0) mod 2^32`, where `m0` is the least +// significant value word of `m` (this works only if `m` is an odd integer). +i31_from_monty :: proc "contextless" (x: []u32, m: []u32, m0i: u32) { + _len := uint(m[0] + 31) >> 5 + for _ in 0..<_len { + f := _mul31_lo(x[1], m0i) + cc := u64(0) + for v in 0..<_len { + z := u64(x[v + 1]) + _mul31(f, m[v + 1]) + cc + cc = z >> 31 + if v != 0 { + x[v] = u32(z & I31_MASK) + } + } + x[_len] = u32(cc) + } + + // We may have to do an extra subtraction, but only if the value in `x` + // is indeed greater than or equal to that of `m`, which is why we must + // do two calls: + // - First call computes the carry + // - Second call performs the subtraction only if the carry is 0). + _ = i31_sub(x, m, subtle.not(i31_sub(x, m, 0))) +} + +// Compute a modular exponentiation. +// +// `x` MUST be an integer modulo `m` (same announced bit length, lower value). +// `m` MUST be odd. +// +// The exponent `e` is in big-endian unsigned notation. +// +// The `m0i` parameter is equal to `-(1/m0) mod 2^31`, where `m0` is the least +// significant value word of `m` (this works only if `m` is an odd integer). +// +// The `t1` and `t2` parameters must be temporary arrays, each large enough to +// accommodate an integer with the same size as `m`. +i31_modpow :: proc "contextless" (x: []u32, e: []byte, m: []u32, m0i: u32, t1: []u32, t2: []u32) { + // `mlen` is the length of `m` expressed in `u32`'s (including the + // "bit length" first field). + mlen := uint((m[0] + 63) >> 5) + elen := u32(len(e)) + + // Throughout the algorithm: + // -- `t1` is in Montgomery representation; it contains x, x^2, x^4, x^8... + // -- The result is accumulated, in normal representation, in the `x` array. + // -- `t2` is used as destination buffer for each multiplication. + // + // Note that there is no need to call `i32_from_monty()`. + copy(t1[:mlen], x[:mlen]) + i31_to_monty(t1, m) + i31_zero(x, m[0]) + x[1] = 1 + for k := u32(0); k < (elen << 3); k += 1 { + ctl := (e[elen - 1 - (k >> 3)] >> (k & 7)) & 1 + + i31_montymul(t2, x, t1, m, m0i) + + for &d, i in x[:mlen] { + d = subtle.csel(d, t2[i], ctl) + } + + i31_montymul(t2, t1, t1, m, m0i) + copy(t1[:mlen], t2[:mlen]) + } +} + + +// Compute a modular exponentiation. +// +// `x` MUST be an integer modulo `m` (same announced bit length, lower value). +// `m` MUST be odd. +// +// The exponent `e` is in big-endian unsigned notation. +// +// The `m0i` parameter is equal to `-(1/m0) mod 2^31`, where `m0` is the least +// significant value word of `m`[] (this works only if m[] is an odd integer). +// +// The `tmp` array is used for temporaries; it must be large enough to accommodate +// at least two temporary values with the same size as `m` (including the leading +// "bit length" word). +// +// If there is room for more temporaries, then this function may use the extra +// room for window-based optimisation, resulting in faster computations. +// +// Returned value is `true` on success, `false` on error. An error is reported if +// the provided `tmp`array is too short. +i31_modpow_opt :: proc "contextless" (x: []u32, e: []byte, m: []u32, m0i: u32, tmp: []u32) -> u32 { + // NOTE/yawning: This is only used by the rsa_i31 code, with the key + // generation taking a function pointer to either this routine, + // or the i62 variant. + // + // If we ever need to support the i32 version, it is used extensively, + // but non e-waste architecutures will all do the right thing with + // the i62 version, albeit with a perforance hit on 32-bit CPUs. + + unimplemented_contextless() + + // i31_mod_pow(x, e, m, m0i, tmp[:len(m)], tmp[len(m):]) + // return 1 +} + +// Compute `d+a*b`, result in `d`. +// +// The initial announced bit length of `d` MUST match that of `a`[]. +// +// The `d` array MUST be large enough to accommodate the full result, +// plus (possibly) an extra word. The resulting announced bit length +// of `d` will be the sum of the announced bit lengths of `a` and `b` +// (therefore, it may be larger than the actual bit length of the numerical result). +// +// `a` and `b` may be the same array. `d` must be disjoint from both `a` and `b`. +i31_mulacc :: proc "contextless" (d: []u32, a: []u32, b: []u32) { + a_len := uint((a[0] + 31) >> 5) + b_len := uint((b[0] + 31) >> 5) + + // We want to add the two bit lengths, but these are encoded, + // which requires some extra care. + d_l := (a[0] & 31) + (b[0] & 31) + d_h := (a[0] >> 5) + (b[0] >> 5) + d[0] = (d_h << 5) + d_l + (~u32(d_l - 31) >> 31) + + for u in 0..> 31 + d[1 + u + v] = u32(z) & I31_MASK + } + d[1 + u + a_len] = u32(cc) + } +} diff --git a/core/crypto/_bigint/i62.odin b/core/crypto/_bigint/i62.odin new file mode 100644 index 000000000..eb6d99907 --- /dev/null +++ b/core/crypto/_bigint/i62.odin @@ -0,0 +1,361 @@ +package _bigint + +// Copyright (c) 2017 Thomas Pornin +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions +// are met: +// +// 1. Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// THIS SOFTWARE IS PROVIDED BY THE AUTHORS “AS IS” AND ANY EXPRESS OR +// IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHORS BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +// DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE +// GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, +// WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING +// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF +// THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +import "base:intrinsics" +import "core:math/bits" +import subtle "core:crypto/_subtle" +import "core:slice" + +@(private="file") +I62_MASK :: 0x3fff_ffff_ffff_ffff + +// Compute x*y+v1+v2. Operands are 64-bit, and result is 128-bit, with +// high word in "hi" and low word in "lo". +@(private="file", require_results) +_fma1 :: #force_inline proc "contextless" (x, y, v1, v2: u64) -> (hi, lo: u64) { + hi, lo = bits.mul_u64(x, y) + + carry: u64 + lo, carry = bits.add_u64(lo, v1, 0) + hi += carry + + lo, carry = bits.add_u64(lo, v2, 0) + hi += carry + + return +} + +// Compute x1*y1+x2*y2+v1+v2. Operands are 64-bit, and result is 128-bit, +// with high word in "hi" and low word in "lo". +// +// Callers should ensure that the two inner products, and the v1 and v2 +// operands, are multiple of 4 (this is not used by this specific definition +// but may help other implementations). +@(private="file", require_results) +_fma2 :: #force_inline proc "contextless" (x1, y1, x2, y2, v1, v2: u64) -> (hi, lo: u64) { + hi_1, lo_1 := bits.mul_u64(x1, y1) + hi_2, lo_2 := bits.mul_u64(x2, y2) + + carry: u64 + lo, carry = bits.add_u64(lo_1, lo_2, 0) + hi, _ = bits.add_u64(hi_1, hi_2, carry) + + lo, carry = bits.add_u64(lo, v1, 0) + hi += carry + + lo, carry = bits.add_u64(lo, v2, 0) + hi += carry + + return +} + +@(private="file", require_results) +_mul62_lo :: #force_inline proc "contextless" (x, y: u64) -> u64 { + return (x * y) & I62_MASK +} + +// Subtract b from a, and return the final carry. If 'ctl32' is 0, then +// a[] is kept unmodified, but the final carry is still computed and +// returned. +@(private="file", require_results) +_i62_sub :: proc "contextless" (a, b: []u64, num: int, ctl32: u32) -> u32 { + cc: u64 + + ctl := -ctl32 + mask := u64(ctl) | (u64(ctl) << 32) + for u in 0..> 63 + dw &= I62_MASK + a[u] = aw ~ (mask & (dw ~ aw)) + } + + return u32(cc) +} + +// Montgomery multiplication, over arrays of 62-bit values. The +// destination array (d) must be distinct from the other operands +// (x, y and m). All arrays are in little-endian format (least +// significant word comes first) over 'num' words. +@(private="file") +_i62_montymul :: proc "contextless" (d, x, y, m: []u64, num: int, m0i: u64) { + dh: u64 + + num4 := 1 + u64((num - 1) & ~int(3)) + intrinsics.mem_zero(raw_data(d), num * size_of(u64)) + for u in 0..> 62) + d[v - 1] = lo >> 2 + hi, lo = _fma2(xu, y[v + 1], f, m[v + 1], d[v + 1] << 2, r << 2) + r = hi + (r >> 62) + d[v + 0] = lo >> 2 + hi, lo = _fma2(xu, y[v + 2], f, m[v + 2], d[v + 2] << 2, r << 2) + r = hi + (r >> 62) + d[v + 1] = lo >> 2 + hi, lo = _fma2(xu, y[v + 3], f, m[v + 3], d[v + 3] << 2, r << 2) + r = hi + (r >> 62) + d[v + 2] = lo >> 2 + } + for ; v < num; v += 1 { + hi, lo = _fma2(xu, y[v], f, m[v], d[v] << 2, r << 2) + r = hi + (r >> 62) + d[v - 1] = lo >> 2 + } + + zh := dh + r + d[num - 1] = zh & I62_MASK + dh = zh >> 62 + } + _ = _i62_sub(d, m, num, u32(dh) | subtle.not(_i62_sub(d, m, num, 0))) +} + +// Conversion back from Montgomery representation. +@(private="file") +_i62_frommonty :: proc "contextless" (x, m: []u64, num: int, m0i: u64) { + for _ in 0..> 2 + } + } + x[num - 1] = cc >> 2 + } + _ = _i62_sub(x, m, num, subtle.not(_i62_sub(x, m, num, 0))) +} + +// Variant of i31_modpow_opt() that internally uses 64x64->128 +// multiplications. It expects the same parameters as i31_modpow_opt(), +// except that the temporaries should be 64-bit integers, not 32-bit +// integers. +i62_modpow_opt :: proc "contextless" (x31: []u32, e: []byte, m31: []u32, m0i31: u32, tmp: []u64) -> u32 { + twlen := len(tmp) + + // Get modulus size, in words. + mw31num := int((m31[0] + 31) >> 5) + mw62num := int((mw31num + 1) >> 1) + + // In order to apply this function, we must have enough room to + // copy the operand and modulus into the temporary array, along + // with at least two temporaries. If there is not enough room, + // switch to br_i31_modpow(). We also use br_i31_modpow() if the + // modulus length is not at least four words (94 bits or more). + if mw31num < 4 || mw62num << 2 > twlen { + // We assume here that we can split an aligned uint64_t + // into two properly aligned uint32_t. Since both types + // are supposed to have an exact width with no padding, + // then this property must hold. + + txlen := mw31num + 1 + if twlen < txlen { + return 0 + } + + tmp_as_u32s := slice.reinterpret([]u32, tmp) + t1, t2 := tmp_as_u32s[:txlen], tmp_as_u32s[txlen:] + + i31_modpow(x31, e, m31, m0i31, t1, t2) + + return 1 + } + + // Convert x to Montgomery representation: this means that + // we replace x with x*2^z mod m, where z is the smallest multiple + // of the word size such that 2^z >= m. We want to reuse the 31-bit + // functions here (for constant-time operation), but we need z + // for a 62-bit word size. + for _ in 0..> 1 + if u + 1 == mw31num { + m[v] = u64(m31[u + 1]) + x[v] = u64(x31[u + 1]) + } else { + m[v] = u64(m31[u + 1]) + (u64(m31[u + 2]) << 31) + x[v] = u64(x31[u + 1]) + (u64(x31[u + 2]) << 31) + } + } + + // Compute window size. We support windows up to 5 bits; for a + // window of size k bits, we need 2^k+1 temporaries (for k = 1, + // we use special code that uses only 2 temporaries). + win_len: int + for win_len = 5; win_len > 1; win_len -= 1 { + if (1 << uint(win_len) + 1) * mw62num <= twlen { + break + } + } + + t1 := tmp_[:mw62num] + t2 := tmp_[mw62num:] + + // Compute m0i, which is equal to -(1/m0) mod 2^62. We were + // provided with m0i31, which already fulfills this property + // modulo 2^31; the single expression below is then sufficient. + m0i := u64(m0i31) + m0i = _mul62_lo(m0i, 2 + _mul62_lo(m0i, m[0])) + + // Compute window contents. If the window has size one bit only, + // then t2 is set to x; otherwise, t2[0] is left untouched, and + // t2[k] is set to x^k (for k >= 1). + if win_len == 1 { + copy(t2, x) + } else { + copy(t2[mw62num:], x) + + base := t2[mw62num:] + for u := 2; u < 1 << uint(win_len); u += 1 { + _i62_montymul(base[mw62num:], base, x, m, mw62num, m0i) + base = base[mw62num:] + } + } + + // Set x to 1, in Montgomery representation. We again use the + // 31-bit code. + i31_zero(x31, m31[0]) + x31[(m31[0] + 31) >> 5] = 1 + i31_muladd_small(x31, 0, m31) + if mw31num & 1 != 0 { + i31_muladd_small(x31, 0, m31) + } + for u := 0; u < mw31num; u+= 2 { + v := u >> 1 + if u + 1 == mw31num { + x[v] = u64(x31[u + 1]) + } else { + x[v] = u64(x31[u + 1]) + (u64(x31[u + 2]) << 31) + } + } + + e_, e_len := e, len(e) + // We process bits from most to least significant. At each + // loop iteration, we have acc_len bits in acc. + acc: u32 + acc_len: uint + for acc_len > 0 || e_len > 0 { + // Get the next bits. + k := uint(win_len) + if acc_len < uint(win_len) { + if e_len > 0 { + acc = (acc << 8) | u32(e_[0]) + e_ = e_[1:] + e_len -= 1 + acc_len += 8 + } else { + k = acc_len + } + } + bits := (acc >> (acc_len - k)) & ((u32(1) << k) - 1) + acc_len -= k + + // We could get exactly k bits. Compute k squarings. + for _ in 0.. 1 { + intrinsics.mem_zero(raw_data(t2), mw62num * size_of(u64)) + + base := t2[mw62num:] + for u := u32(1); u < u32(1) << k; u += 1 { + mask := -u64(subtle.eq(u, bits)) + for v in 0..> 1]) + x31[u + 1] = u32(zw) & I31_MASK + if u + 1 < mw31num { + x31[u + 2] = u32(zw >> 31) + } + } + + return 1 +} + +// Wrapper for i62_modpow_opt() that uses the same type as +// i31_modpow_opt(); however, it requires its 'tmp' argument to the +// 64-bit aligned. +i62_modpow_opt_as_i31 :: proc "contextless" (x31: []u32, e: []byte, m31: []u32, m0i31: u32, tmp: []u32) -> u32 { + // As documented, this function expects the 'tmp' argument to be + // 64-bit aligned. This is OK since this function is internal (it + // is not part of BearSSL's public API). + ensure_contextless(uintptr(raw_data(tmp)) & 7 == 0) + ensure_contextless(len(tmp) & 1 == 0) // Length MUST be even. + + tmp_as_u64s := slice.reinterpret([]u64, tmp) + + return i62_modpow_opt(x31, e, m31, m0i31, tmp_as_u64s) +} diff --git a/core/crypto/_bigint/i62_primes.odin b/core/crypto/_bigint/i62_primes.odin new file mode 100644 index 000000000..a9606db05 --- /dev/null +++ b/core/crypto/_bigint/i62_primes.odin @@ -0,0 +1,265 @@ +package _bigint + +// Copyright (c) 2017 Thomas Pornin +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions +// are met: +// +// 1. Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// THIS SOFTWARE IS PROVIDED BY THE AUTHORS “AS IS” AND ANY EXPRESS OR +// IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHORS BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +// DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE +// GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, +// WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING +// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF +// THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +import subtle "core:crypto/_subtle" +import "core:math/big" +import "core:slice" + +// Perform trial divisions on a candidate prime. We opt for the simple +// route and "just" compute a series of trial divisions. +// +// Returned value is 1 on success (none of the small primes +// divides x), 0 on error (a non-trivial GCD is obtained). +@(private="file", require_results) +trial_divisions :: proc "contextless" (x: []u32) -> u32 { + for factor in big._private_prime_table { + if factor <= 11 { + continue + } + if i31_rem(x, u32(factor)) == 0 { + return 0 + } + } + + return 1 +} + +// Perform n rounds of Miller-Rabin on the candidate prime x. This +// function assumes that x = 3 mod 4. +// +// WARNING: t MUST be 64-bit aligned, and be large enough such that +// it can hold 4 encoded integers that have the same number of limbs +// as x. +// +// Returned value is 1 on success (all rounds completed successfully), +// 0 otherwise. +@(private="file", require_results) +i62_miller_rabin :: proc(x: []u32, n: int, t: []u32) -> u32 { + // Since x = 3 mod 4, the Miller-Rabin test is simple: + // - get a random base a (such that 1 < a < x-1) + // - compute z = a^((x-1)/2) mod x + // - if z != 1 and z != x-1, the number x is composite + // + // We generate bases 'a' randomly with a size which is + // one bit less than x, which ensures that a < x-1. It + // is not useful to verify that a > 1 because the probability + // that we get a value a equal to 0 or 1 is much smaller + // than the probability of our Miller-Rabin tests not to + // detect a composite, which is already quite smaller than the + // probability of the hardware misbehaving and return a + // composite integer because of some glitch (e.g. bad RAM + // or ill-timed cosmic ray). + + // Compute (x-1)/2 (encoded). + xm1d2 := slice.reinterpret([]byte, t) + xm1d2_len := ((x[0] - (x[0] >> 5)) + 7) >> 3 + i31_encode(xm1d2[:xm1d2_len], x) + cc: u32 + for u in 0..> 1) | cc) + cc = w << 7 + } + + // We used some words of the provided buffer for (x-1)/2. + xm1d2_len_u32 := (xm1d2_len + 3) >> 2 + t_ := t[xm1d2_len_u32:] + tlen := len(t_) + + xlen := (x[0] + 31) >> 5 + asize := x[0] - 1 - subtle.eq0(x[0] & 31) + x0i := i31_ninv31(x[1]) + for _ in 0..> 5 + + for { + // Generate random bits. We force the two top bits and the + // two bottom bits to 1. + i31_mkrand(x, esize) + if (esize & 31) == 0 { + x[_len] |= 0x60000000 + } else if (esize & 31) == 1 { + x[_len] |= 0x00000001 + x[_len - 1] |= 0x40000000 + } else { + x[_len] |= 0x00000003 << ((esize & 31) - 2) + } + x[1] |= 0x00000003 + + // Trial division with low primes (3, 5, 7 and 11). We + // use the following properties: + // + // 2^2 = 1 mod 3 + // 2^4 = 1 mod 5 + // 2^3 = 1 mod 7 + // 2^10 = 1 mod 11 + m3, m5, m7, m11: u32 + s7, s11: uint + for u in 0..<_len { + w := x[1 + u] + w3 := (w & 0xFFFF) + (w >> 16) // max: 98302 + w5 := (w & 0xFFFF) + (w >> 16) // max: 98302 + w7 := (w & 0x7FFF) + (w >> 15) // max: 98302 + w11 := (w & 0xFFFFF) + (w >> 20) // max: 1050622 + + m3 += w3 << (u & 1) + m3 = (m3 & 0xFF) + (m3 >> 8) // max: 1025 + + m5 += w5 << ((4 - u) & 3) + m5 = (m5 & 0xFFF) + (m5 >> 12) // max: 4479 + + m7 += w7 << s7 + m7 = (m7 & 0x1FF) + (m7 >> 9) // max: 1280 + s7 += 1 + if s7 == 3 { + s7 = 0 + } + + m11 += w11 << s11 + s11 += 1 + if s11 == 10 { + s11 = 0 + } + m11 = (m11 & 0x3FF) + (m11 >> 10) // max: 526847 + } + + m3 = (m3 & 0x3F) + (m3 >> 6) // max: 78 + m3 = (m3 & 0x0F) + (m3 >> 4) // max: 18 + m3 = ((m3 * 43) >> 5) & 3 + + m5 = (m5 & 0xFF) + (m5 >> 8) // max: 271 + m5 = (m5 & 0x0F) + (m5 >> 4) // max: 31 + m5 -= 20 & -subtle.gt(m5, 19) + m5 -= 10 & -subtle.gt(m5, 9) + m5 -= 5 & -subtle.gt(m5, 4) + + m7 = (m7 & 0x3F) + (m7 >> 6) // max: 82 + m7 = (m7 & 0x07) + (m7 >> 3) // max: 16 + m7 = ((m7 * 147) >> 7) & 7 + + // 2^5 = 32 = -1 mod 11. + m11 = (m11 & 0x3FF) + (m11 >> 10) // max: 1536 + m11 = (m11 & 0x3FF) + (m11 >> 10) // max: 1023 + m11 = (m11 & 0x1F) + 33 - (m11 >> 5) // max: 64 + m11 -= 44 & -subtle.gt(m11, 43) + m11 -= 22 & -subtle.gt(m11, 21) + m11 -= 11 & -subtle.gt(m11, 10) + + // If any of these modulo is 0, then the candidate is + // not prime. Also, if pubexp is 3, 5, 7 or 11, and the + // corresponding modulus is 1, then the candidate must + // be rejected, because we need e to be invertible + // modulo p-1. We can use simple comparisons here + // because they won't leak information on a candidate + // that we keep, only on one that we reject (and is thus + // not secret). + if m3 == 0 || m5 == 0 || m7 == 0 || m11 == 0 { + continue + } + if (pubexp == 3 && m3 == 1) || (pubexp == 5 && m5 == 1) || (pubexp == 7 && m7 == 1) || (pubexp == 11 && m11 == 1) { + continue + } + + // More trial divisions. + if trial_divisions(x) == 0 { + continue + } + + // Miller-Rabin algorithm. Since we selected a random + // integer, not a maliciously crafted integer, we can use + // relatively few rounds to lower the risk of a false + // positive (i.e. declaring prime a non-prime) under + // 2^(-80). It is not useful to lower the probability much + // below that, since that would be substantially below + // the probability of the hardware misbehaving. Sufficient + // numbers of rounds are extracted from the Handbook of + // Applied Cryptography, note 4.49 (page 149). + // + // Since we work on the encoded size (esize), we need to + // compare with encoded thresholds. + rounds: int + switch { + case esize < 309: + rounds = 12 + case esize < 464: + rounds = 9 + case esize < 670: + rounds = 6 + case esize < 877: + rounds = 4 + case esize < 1341: + rounds = 3 + case: + rounds = 2 + } + + if i62_miller_rabin(x, rounds, t) == 1 { + return + } + } +} diff --git a/core/crypto/_fiat/field_p256r1/field.odin b/core/crypto/_fiat/field_p256r1/field.odin index f7dd978aa..fcb7357a7 100644 --- a/core/crypto/_fiat/field_p256r1/field.odin +++ b/core/crypto/_fiat/field_p256r1/field.odin @@ -71,7 +71,7 @@ fe_equal :: proc "contextless" (arg1, arg2: ^Montgomery_Domain_Field_Element) -> // This will only underflow if and only if (⟺) arg1 == arg2, and we return the borrow, // which will be 1. - is_eq := subtle.u64_is_zero(fe_non_zero(&tmp)) + is_eq := subtle.eq0(fe_non_zero(&tmp)) fe_clear(&tmp) diff --git a/core/crypto/_fiat/field_p384r1/field.odin b/core/crypto/_fiat/field_p384r1/field.odin index 2bddff18c..af371873e 100644 --- a/core/crypto/_fiat/field_p384r1/field.odin +++ b/core/crypto/_fiat/field_p384r1/field.odin @@ -77,7 +77,7 @@ fe_equal :: proc "contextless" (arg1, arg2: ^Montgomery_Domain_Field_Element) -> // This will only underflow if and only if (⟺) arg1 == arg2, and we return the borrow, // which will be 1. - is_eq := subtle.u64_is_zero(fe_non_zero(&tmp)) + is_eq := subtle.eq0(fe_non_zero(&tmp)) fe_clear(&tmp) diff --git a/core/crypto/_fiat/field_scalarp384r1/field.odin b/core/crypto/_fiat/field_scalarp384r1/field.odin index cfc27f322..d465bae44 100644 --- a/core/crypto/_fiat/field_scalarp384r1/field.odin +++ b/core/crypto/_fiat/field_scalarp384r1/field.odin @@ -60,7 +60,7 @@ fe_from_bytes :: proc "contextless" ( reduced[3], borrow = bits.sub_u64(tmp[3], ELL[3], borrow) reduced[4], borrow = bits.sub_u64(tmp[4], ELL[4], borrow) reduced[5], borrow = bits.sub_u64(tmp[5], ELL[5], borrow) - need_reduced := subtle.u64_is_zero(borrow) + need_reduced := subtle.eq0(borrow) fe_cond_select(&tmp, &tmp, &reduced, int(need_reduced)) fe_to_montgomery(out1, &tmp) diff --git a/core/crypto/_mlkem/poly.odin b/core/crypto/_mlkem/poly.odin index 1982f9102..13dd54930 100644 --- a/core/crypto/_mlkem/poly.odin +++ b/core/crypto/_mlkem/poly.odin @@ -130,7 +130,7 @@ poly_frommsg :: proc "contextless" (r: ^Poly, msg: []byte) #no_bounds_check { for i in 0..> uint(j))&1) + r.coeffs[8*i+j] = subtle.csel_i16(0, (Q+1)/2, (msg[i] >> uint(j))&1) } } } diff --git a/core/crypto/_subtle/subtle.odin b/core/crypto/_subtle/subtle.odin index 01c84cf2a..e57d4ca36 100644 --- a/core/crypto/_subtle/subtle.odin +++ b/core/crypto/_subtle/subtle.odin @@ -3,76 +3,245 @@ Various useful bit operations in constant time. */ package _subtle -import "core:crypto/_fiat" -import "core:math/bits" +import "base:intrinsics" -// byte_eq returns 1 if and only if (⟺) a == b, 0 otherwise. -@(optimization_mode="none") -byte_eq :: proc "contextless" (a, b: byte) -> int { +// Copyright (c) 2016 Thomas Pornin +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions +// are met: +// +// 1. Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// THIS SOFTWARE IS PROVIDED BY THE AUTHORS “AS IS” AND ANY EXPRESS OR +// IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHORS BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +// DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE +// GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, +// WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING +// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF +// THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +// Constant-time primitives. These functions manipulate integer values in +// order to provide constant-time comparisons and multiplexers. +// +// Boolean values (the "ctl" bits) MUST have value 0 or 1. +// +// Implementation notes: +// ===================== +// +// The uintN_t types are unsigned and with width exactly N bits; the C +// standard guarantees that computations are performed modulo 2^N, and +// there can be no overflow. Negation (unary '-') works on unsigned types +// as well. +// +// The intN_t types are guaranteed to have width exactly N bits, with no +// padding bit, and using two's complement representation. Casting +// intN_t to uintN_t really is conversion modulo 2^N. Beware that intN_t +// types, being signed, trigger implementation-defined behaviour on +// overflow (including raising some signal): with GCC, while modular +// arithmetics are usually applied, the optimizer may assume that +// overflows don't occur (unless the -fwrapv command-line option is +// added); Clang has the additional -ftrapv option to explicitly trap on +// integer overflow or underflow. + +// This code only works on a two's complement system. +#assert((-1 & 3) == 3) + +// not negates a boolean which MUST be `0` or `1` +@(optimization_mode="none", require_results) +not :: proc "contextless" (ctrl: $T) -> T where intrinsics.type_is_unsigned(T) { + return ctrl ~ 1 +} + +@(optimization_mode="none", require_results) +byte_eq :: proc "contextless" (a, b: byte) -> byte { v := a ~ b // v == 0 if and only if (⟺) a == b. The subtraction will underflow, setting the // sign bit, which will get returned. - return int((u32(v)-1) >> 31) + return byte((u32(v)-1) >> 31) } -// u64_eq returns 1 if and only if (⟺) a == b, 0 otherwise. -@(optimization_mode="none") +@(optimization_mode="none", require_results) +u32_eq :: proc "contextless" (a, b: u32) -> u32 { + q := a ~ b + return ((q | -q) >> 31) ~ 1 +} + +@(optimization_mode="none", require_results) u64_eq :: proc "contextless" (a, b: u64) -> u64 { - _, borrow := bits.sub_u64(0, a ~ b, 0) - return (~borrow) & 1 + q := a ~ b + return ((q | -q) >> 63) ~ 1 } +// eq returns 1 if and only if (⟺) a == b, 0 otherwise. eq :: proc { byte_eq, + u32_eq, u64_eq, } -// u64_is_zero returns 1 if and only if (⟺) a == 0, 0 otherwise. -@(optimization_mode="none") -u64_is_zero :: proc "contextless" (a: u64) -> u64 { - _, borrow := bits.sub_u64(a, 1, 0) - return borrow +@(require_results) +byte_neq :: proc "contextless" (a, b: byte) -> byte { + return #force_inline byte_eq(a, b) ~ 1 } -// u64_is_non_zero returns 1 if and only if (⟺) a != 0, 0 otherwise. -@(optimization_mode="none") -u64_is_non_zero :: proc "contextless" (a: u64) -> u64 { - is_zero := u64_is_zero(a) - return (~is_zero) & 1 +@(optimization_mode="none", require_results) +u32_neq :: proc "contextless" (a, b: u32) -> u32 { + q := a ~ b + return (q | -q) >> 31 } -@(optimization_mode="none") -cmov_bytes :: proc "contextless" (dst, src: []byte, ctrl: int) { +@(optimization_mode="none", require_results) +u64_neq :: proc "contextless" (a, b: u64) -> u64 { + q := a ~ b + return (q | -q) >> 63 +} + +// neq returns 1 if and only if (⟺) a != b, 0 otherwise. +neq :: proc { + byte_neq, + u32_neq, + u64_neq, +} + +@(optimization_mode="none", require_results) +u32_gt :: proc "contextless" (x, y: u32) -> u32 { + /* + * If both x < 2^31 and y < 2^31, then y-x will have its high + * bit set if x > y, cleared otherwise. + * + * If either x >= 2^31 or y >= 2^31 (but not both), then the + * result is the high bit of x. + * + * If both x >= 2^31 and y >= 2^31, then we can virtually + * subtract 2^31 from both, and we are back to the first case. + * Since (y-2^31)-(x-2^31) = y-x, the subtraction is already + * fine. + */ + z := y - x + return (z ~ ((x ~ y) & (x ~ z))) >> 31 +} + +@(optimization_mode="none", require_results) +u64_gt :: proc "contextless" (x, y: u64) -> u64 { + z := y - x + return (z ~ ((x ~ y) & (x ~ z))) >> 63 +} + +// gt returns 1 if x > y, 0 otherwise. +gt :: proc { + u32_gt, + u64_gt, +} + +// gt returns 1 if x >= y, 0 otherwise. +@(require_results) +ge :: proc "contextless" (x, y: $T) -> T where T == u32 || T == u64 { + return #force_inline(gt(y, x)) ~ 1 +} + +// lt returns 1 if x < y, 0 otherwise. +@(require_results) +lt :: proc "contextless" (x, y: $T) -> T where T == u32 || T == u64 { + return #force_inline(gt(y, x)) +} + +// le returns 1 if x <= y, 0 otherwise. +@(require_results) +le :: proc "contextless" (x, y: $T) -> T where T == u32 || T == u64 { + return #force_inline(gt(x, y)) ~ 1 +} + +@(require_results) +u32_cmp :: proc "contextless" (x, y: u32) -> i32 { + return i32(#force_inline gt(x, y)) | -i32(#force_inline gt(y, x)) +} + +@(require_results) +u64_cmp :: proc "contextless" (x, y: u64) -> i64 { + return i64(#force_inline gt(x, y)) | -i64(#force_inline gt(y, x)) +} + +// cmp returns -1, 0, or 1, depending on wheter x is lower than, equal +// to, or greater than y. +cmp :: proc { + u32_cmp, + u64_cmp, +} + +// eq0 returns 1 if and only if (⟺) a == 0, 0 otherwise. +@(require_results) +eq0 :: proc "contextless" (a: $T) -> T where T == u32 || T == u64 { + return #force_inline eq(a, 0) +} + +// neq0 returns 1 if and only if (⟺) a != 0, 0 otherwise. +@(require_results) +neq0 :: proc "contextless" (a: $T) -> T where T == u32 || T == u64 { + return #force_inline eq(a, 0) ~ 1 +} + +cmov_bytes :: proc "contextless" (dst, src: []byte, #any_int ctrl: int) { + ensure_contextless(len(src) == len(dst), "crypto: cmov length mismatch") + + cmov_impl(dst, src, ctrl) +} + +cmov_u32s :: proc "contextless" (dst, src: []u32, #any_int ctrl: int) { + ensure_contextless(len(src) == len(dst), "crypto: cmov length mismatch") + + cmov_impl(dst, src, ctrl) +} + +@(private="file", optimization_mode="none") +cmov_impl :: proc "contextless"(dst, src: []$T, ctrl: int) { s_len := len(src) - ensure_contextless(s_len == len(dst), "crypto: cmov length mismatch") - c := -(byte)(ctrl) + c := -(T)(ctrl) for i in 0.. i16 { - c := -(u16)(ctrl) +// cmov copies `src` into `dst` if and only if (⟺) ctrl == 1. `dst` and +// `src` may overlap completely (but not partially). +cmov :: proc { + cmov_bytes, + cmov_u32s, +} + +@(optimization_mode="none", require_results) +csel_i16 :: proc "contextless" (a, b: i16, #any_int ctrl: u16) -> i16 { + c := -ctrl return a ~ i16(c & u16(a ~ b)) } -@(optimization_mode="none") -csel_u16 :: proc "contextless" (a, b: u16, ctrl: int) -> u16 { - c := -(u16)(ctrl) +@(optimization_mode="none", require_results) +csel_u16 :: proc "contextless" (a, b: u16, #any_int ctrl: u16) -> u16 { + c := -ctrl return a ~ (c & (a ~ b)) } -csel_u32 :: proc "contextless" (a, b: u32, ctrl: int) -> u32 { - return _fiat.cmovznz_u32(_fiat.u1(ctrl), a, b) +@(optimization_mode="none", require_results) +csel_u32 :: proc "contextless" (a, b: u32, #any_int ctrl: u32) -> u32 { + c := -ctrl + return a ~ (c & (a ~ b)) } -csel_u64 :: proc "contextless" (a, b: u64, ctrl: int) -> u64 { - return _fiat.cmovznz_u64(_fiat.u1(ctrl), a, b) +@(optimization_mode="none", require_results) +csel_u64 :: proc "contextless" (a, b: u64, #any_int ctrl: u64) -> u64 { + c := -ctrl + return a ~ (c & (a ~ b)) } +// csel returns `a` if ctl == `0`, `b` if ctl == `1`. csel :: proc { csel_i16, csel_u16, diff --git a/core/crypto/_weierstrass/fe.odin b/core/crypto/_weierstrass/fe.odin index 8ff6fe346..2b93d58a4 100644 --- a/core/crypto/_weierstrass/fe.odin +++ b/core/crypto/_weierstrass/fe.odin @@ -196,10 +196,10 @@ fe_gen_y_p384r1 :: proc "contextless" (fe: ^Field_Element_p384r1) { @(require_results) fe_is_zero_p256r1 :: proc "contextless" (fe: ^Field_Element_p256r1) -> int { - return int(subtle.u64_is_zero(p256r1.fe_non_zero(fe))) + return int(subtle.eq0(p256r1.fe_non_zero(fe))) } @(require_results) fe_is_zero_p384r1 :: proc "contextless" (fe: ^Field_Element_p384r1) -> int { - return int(subtle.u64_is_zero(p384r1.fe_non_zero(fe))) + return int(subtle.eq0(p384r1.fe_non_zero(fe))) } diff --git a/core/crypto/_weierstrass/sc.odin b/core/crypto/_weierstrass/sc.odin index 1ecfea6f9..6a3c7b68c 100644 --- a/core/crypto/_weierstrass/sc.odin +++ b/core/crypto/_weierstrass/sc.odin @@ -133,10 +133,10 @@ sc_is_zero :: proc { @(require_results) sc_is_zero_p256r1 :: proc "contextless" (fe: ^Scalar_p256r1) -> int { - return int(subtle.u64_is_zero(p256r1.fe_non_zero(fe))) + return int(subtle.eq0(p256r1.fe_non_zero(fe))) } @(require_results) sc_is_zero_p384r1 :: proc "contextless" (fe: ^Scalar_p384r1) -> int { - return int(subtle.u64_is_zero(p384r1.fe_non_zero(fe))) + return int(subtle.eq0(p384r1.fe_non_zero(fe))) } diff --git a/core/crypto/_weierstrass/scalar_mul.odin b/core/crypto/_weierstrass/scalar_mul.odin index eb3fa1459..1af3ba989 100644 --- a/core/crypto/_weierstrass/scalar_mul.odin +++ b/core/crypto/_weierstrass/scalar_mul.odin @@ -293,7 +293,7 @@ when crypto.COMPACT_IMPLS == false { // conditionally select the right result. pt_add_mixed(tmp, point, &tmp.x, &tmp.y) - ctrl := subtle.u64_is_non_zero(idx) + ctrl := subtle.neq0(idx) pt_cond_select(point, point, tmp, int(ctrl)) } } diff --git a/core/crypto/crypto.odin b/core/crypto/crypto.odin index 3218b8670..fff66fdd0 100644 --- a/core/crypto/crypto.odin +++ b/core/crypto/crypto.odin @@ -49,7 +49,7 @@ compare_byte_ptrs_constant_time :: proc "contextless" (a, b: ^byte, n: int) -> i // After the loop, v == 0 if and only if (⟺) a == b. The subtraction will underflow // if and only if (⟺) v == 0, setting the sign-bit, which gets returned. - return subtle.eq(0, v) + return int(subtle.eq(0, v)) } // is_zero_constant_time returns 1 if and only if (⟺) b is all 0s, 0 otherwise. @@ -59,7 +59,7 @@ is_zero_constant_time :: proc "contextless" (b: []byte) -> int { v |= b_ } - return subtle.byte_eq(0, v) + return int(subtle.byte_eq(0, v)) } /* diff --git a/core/crypto/ed25519/ed25519.odin b/core/crypto/ed25519/ed25519.odin index 9f2d2b330..cc7f93e07 100644 --- a/core/crypto/ed25519/ed25519.odin +++ b/core/crypto/ed25519/ed25519.odin @@ -50,6 +50,7 @@ Public_Key :: struct { // private_key_generate uses the system entropy source to generate a new // Private_Key. This will only fail if and only if (⟺) the system entropy source is // missing or broken. +@(require_results) private_key_generate :: proc(priv_key: ^Private_Key) -> bool { private_key_clear(priv_key) @@ -61,13 +62,12 @@ private_key_generate :: proc(priv_key: ^Private_Key) -> bool { defer crypto.zero_explicit(&b, size_of(b)) crypto.rand_bytes(b[:]) - private_key_set_bytes(priv_key, b[:]) - - return true + return private_key_set_bytes(priv_key, b[:]) } // private_key_set_bytes decodes a byte-encoded private key, and returns // true if and only if (⟺) the operation was successful. +@(require_results) private_key_set_bytes :: proc(priv_key: ^Private_Key, b: []byte) -> bool { if len(b) != PRIVATE_KEY_SIZE { return false @@ -189,6 +189,7 @@ sign :: proc(priv_key: ^Private_Key, msg, sig: []byte) { // public_key_set_bytes decodes a byte-encoded public key, and returns // true if and only if (⟺) the operation was successful. +@(require_results) public_key_set_bytes :: proc "contextless" (pub_key: ^Public_Key, b: []byte) -> bool { if len(b) != PUBLIC_KEY_SIZE { return false @@ -237,6 +238,7 @@ public_key_bytes :: proc(pub_key: ^Public_Key, dst: []byte) { } // public_key_equal returns true if and only if (⟺) pub_key is equal to other. +@(require_results) public_key_equal :: proc(pub_key, other: ^Public_Key) -> bool { ensure(pub_key._is_initialized && other._is_initialized, "crypto/ed25519: uninitialized public key") @@ -254,6 +256,7 @@ public_key_clear :: proc "contextless" (pub_key: ^Public_Key) { // implementation strictly compatible with FIPS 186-5, at the expense of // SBS-security. Doing so is NOT recommended, and the disallowed // public keys all have a known discrete-log. +@(require_results) verify :: proc(pub_key: ^Public_Key, msg, sig: []byte, allow_small_order_A := false) -> bool { switch { case !pub_key._is_initialized: diff --git a/core/crypto/rsa/doc.odin b/core/crypto/rsa/doc.odin new file mode 100644 index 000000000..537333cfc --- /dev/null +++ b/core/crypto/rsa/doc.odin @@ -0,0 +1,7 @@ +/* +RSA (Rivest–Shamir–Adleman) cryptosystem. + +See: +- [[ https://www.rfc-editor.org/info/rfc8017/ ]] +*/ +package rsa diff --git a/core/crypto/rsa/rsa.odin b/core/crypto/rsa/rsa.odin new file mode 100644 index 000000000..29ef4dac8 --- /dev/null +++ b/core/crypto/rsa/rsa.odin @@ -0,0 +1,444 @@ +package rsa + +// Copyright (c) 2016 Thomas Pornin +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions +// are met: +// +// 1. Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// THIS SOFTWARE IS PROVIDED BY THE AUTHORS “AS IS” AND ANY EXPRESS OR +// IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHORS BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +// DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE +// GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, +// WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING +// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF +// THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +import "core:bytes" +import "core:crypto" +import subtle "core:crypto/_subtle" +import "core:encoding/endian" + +// Minimum size for a RSA modulus (in bits). +// +// Note: 1024-bits is arguably insufficient as of this writing, with +// 2048-bits being a more sensible value, however 1024-bits is likely +// still in frequent enough use. +// +// Note: CA signed TLS certificates have a strict requirement of a modulus +// size that is at least 2048-bits [[ https://cabforum.org/working-groups/server/baseline-requirements/documents/]]. +MODULUS_MIN_SIZE :: 1024 + +// Maximum size for a RSA modulus (in bits). +// +// This value MUST be a multiple of 64. This value MUST NOT exceed 47666 +// (some computations in RSA key generation rely on the factor size being +// no more than 23833 bits). RSA key sizes beyond 3072 bits don't make a +// lot of sense anyway. +MODULUS_MAX_SIZE :: 4096 + +// Maxmimum size for a RSA public exponent (in bits). +// +// Note: This implementation supports arbitrary size exponents, however +// limit it to something sensible (some implementations are known to +// choke on exponents >= 2^32), with the most common choice being +// `65537`. +EXPONENT_MAX_SIZE :: 32 + +// Maximum size for a RSA factor (in bits). This is for RSA private-key +// operations. Default is to support factors up to a bit more than half +// the maximum modulus size. +// +// This value MUST be a multiple of 32. +FACTOR_MAX_SIZE :: (MODULUS_MAX_SIZE + 64) >> 1 + +// Default size for a RSA key (in bits). +DEFAULT_MODULUS_SIZE :: 2048 + +// RSA public exponent used for key generation. This MUST be a prime +// number greater than 2. +@(private) +PUBLIC_EXPONENT :: 65537 + +#assert(EXPONENT_MAX_SIZE <= 32) + +// Private_Key is a RSA private key. +Private_Key :: struct { + _pub_key: Public_Key, + _d: Modulus, // Private exponent has the same size as n. + _p: Factor, + _q: Factor, + + // CRT coefficients. + _dp: Factor, // d % (p - 1) + _dq: Factor, // d % (q - 1) + _iq: Factor, // q^(-1) mod p + + _is_initialized: bool, +} + +// Public_Key is a RSA public key. +Public_Key :: struct { + _n: Modulus, + _e: u32, + _is_initialized: bool, +} + +// private_key_generate uses the system entropy source to generate a new +// Private_Key. The key size is specified in bits, and must be a multiple +// of 8. +@(require_results) +private_key_generate :: proc(priv_key: ^Private_Key, key_size := DEFAULT_MODULUS_SIZE) -> bool { + if !crypto.HAS_RAND_BYTES { + return false + } + if key_size < MODULUS_MIN_SIZE || key_size > MODULUS_MAX_SIZE { + return false + } + if key_size % 8 != 0 { + return false + } + + private_key_clear(priv_key) + defer if !priv_key._is_initialized { + private_key_clear(priv_key) + } + + for { + // The only way this can fail is if we get extremely unlucky + // and we fail to derive `iq` (1/d mod p). + if keygen_inner(priv_key, key_size) == 1 { + break + } + } + priv_key._is_initialized = true + priv_key._pub_key._is_initialized = true + + // Self-test the key. + priv_key._is_initialized = pkcs1_sig_selftest(priv_key) + + return priv_key._is_initialized +} + +// private_key_n copies the private key's public modulus to dst if dst is +// non-nil and of sufficient size, and returns the number of bytes +// copied/would be copied (ie: calling with `dst = nil` gets the required +// size). +@(require_results) +private_key_n :: proc(priv_key: ^Private_Key, dst: []byte) -> (n_len: int) { + ensure(priv_key._is_initialized, "crypto/rsa: uninitialized private key") + + return public_key_n(&priv_key._pub_key, dst) +} + +// private_key_e returns the private key's public exponent as a u32. +@(require_results) +private_key_e :: proc(priv_key: ^Private_Key) -> u32 { + ensure(priv_key._is_initialized, "crypto/rsa: uninitialized private key") + + return public_key_e(&priv_key._pub_key) +} + +// private_key_d copies the private key's private exponent `d` to dst if +// dst is non-nil and of sufficient size, and returns the number of bytes +// copied/would be copied (ie: calling with `dst = nil` gets the required +// size). +// +// Note: The data returned MUST be kept confidential. +@(require_results) +private_key_d :: proc(priv_key: ^Private_Key, dst: []byte) -> (n_len: int) { + ensure(priv_key._is_initialized, "crypto/rsa: uninitialized private key") + + return modulus_copyout(&priv_key._d, dst) +} + +// private_key_p copies the private key's first prime factor `p` to dst +// if dst is non-nil and of sufficient size, and returns the number of +// bytes copied/would be copied (ie: calling with `dst = nil` gets the +// required size). +// +// Note: The data returned MUST be kept confidential. +@(require_results) +private_key_p :: proc(priv_key: ^Private_Key, dst: []byte) -> (n_len: int) { + ensure(priv_key._is_initialized, "crypto/rsa: uninitialized private key") + + return factor_copyout(&priv_key._p, dst) +} + +// private_key_q copies the private key's second prime factor `q` to dst +// if dst is non-nil and of sufficient size, and returns the number of +// bytes copied/would be copied (ie: calling with `dst = nil` gets the +// required size). +// +// Note: The data returned MUST be kept confidential. +@(require_results) +private_key_q :: proc(priv_key: ^Private_Key, dst: []byte) -> (n_len: int) { + ensure(priv_key._is_initialized, "crypto/rsa: uninitialized private key") + + return factor_copyout(&priv_key._q, dst) +} + +// private_key_dp copies the private key's first reduced exponent +// `d % (p-1)` to dst if dst is non-nil and of sufficient size, and +// returns the number of bytes copied/would be copied (ie: calling with +//`dst = nil` gets the required size). +// +// Note: The data returned MUST be kept confidential. +@(require_results) +private_key_dp :: proc(priv_key: ^Private_Key, dst: []byte) -> (n_len: int) { + ensure(priv_key._is_initialized, "crypto/rsa: uninitialized private key") + + return factor_copyout(&priv_key._dp, dst) +} + +// private_key_dq copies the private key's second reduced exponent +// `d % (q-1)` to dst if dst is non-nil and of sufficient size, and +// returns the number of bytes copied/would be copied (ie: calling with +//`dst = nil` gets the required size). +// +// Note: The data returned MUST be kept confidential. +@(require_results) +private_key_dq :: proc(priv_key: ^Private_Key, dst: []byte) -> (n_len: int) { + ensure(priv_key._is_initialized, "crypto/rsa: uninitialized private key") + + return factor_copyout(&priv_key._dq, dst) +} + +// private_key_iq copies the private key's CRT coefficient `iq` to dst if +// dst is non-nil and of sufficient size, and returns the number of bytes +// copied/would be copied (ie: calling with`dst = nil` gets the required +// size). +// +// Note: The data returned MUST be kept confidential. +@(require_results) +private_key_iq :: proc(priv_key: ^Private_Key, dst: []byte) -> (n_len: int) { + ensure(priv_key._is_initialized, "crypto/rsa: uninitialized private key") + + return factor_copyout(&priv_key._iq, dst) +} + +// private_key_size returns the size of the private key's public modulus +// in bytes. All ciphertexts and signatures will also be this size. +@(require_results) +private_key_size :: proc(priv_key: ^Private_Key) -> int { + ensure(priv_key._is_initialized, "crypto/rsa: uninitialized private key") + + return priv_key._pub_key._n.v_len +} + +// private_key_set_bytes sets a private key from byte-encoded components, +// and returns true if and only if (⟺) the operation was successful. +// +// Note: All values are mandatory, and match the values included in the +// PKCS private key format. +// +// WARNING: This routine validates that it is possible to sign/verify with +// the deserialized values, however d is not checked at all, nor is the +// primality of p and q. +@(require_results) +private_key_set_bytes :: proc( + priv_key: ^Private_Key, + n: []byte, + e: []byte, + d: []byte, + p: []byte, + q: []byte, + dp: []byte, + dq: []byte, + iq: []byte, +) -> bool { + private_key_clear(priv_key) + defer if !priv_key._is_initialized { + private_key_clear(priv_key) + } + + if !public_key_set_bytes(&priv_key._pub_key, n, e) { + return false + } + + if !modulus_set_bytes(&priv_key._d, d) { + return false + } + if !factor_set_bytes(&priv_key._p, p) { + return false + } + if !factor_set_bytes(&priv_key._q, q) { + return false + } + if !factor_set_bytes(&priv_key._dp, dp) { + return false + } + if !factor_set_bytes(&priv_key._dq, dq) { + return false + } + if !factor_set_bytes(&priv_key._iq, iq) { + return false + } + + priv_key._is_initialized = true + + // Test the key. + // + // Note: This DOES NOT check that p/q are prime and if d is + // consistent (as it is not used by our implementation). + priv_key._is_initialized = pkcs1_sig_selftest(priv_key) + + return priv_key._is_initialized +} + +// private_key_set sets priv_key to src. +private_key_set :: proc(priv_key, src: ^Private_Key) { + if src == nil || !src._is_initialized { + private_key_clear(priv_key) + return + } + + public_key_set(&priv_key._pub_key, &src._pub_key) + modulus_set(&priv_key._d, &src._d) + factor_set(&priv_key._p, &src._p) + factor_set(&priv_key._q, &src._q) + factor_set(&priv_key._dp, &src._dp) + factor_set(&priv_key._dq, &src._dq) + factor_set(&priv_key._iq, &src._iq) + + priv_key._is_initialized = true +} + +// private_key_equal returns true if and only if (⟺) priv_key is equal to other. +@(require_results) +private_key_equal :: proc(priv_key, other: ^Private_Key) -> bool { + ensure(priv_key._is_initialized && other._is_initialized, "crypto/rsa: uninitialized private key") + + pk_eq := public_key_equal(&priv_key._pub_key, &other._pub_key) + + eq := crypto.compare_constant_time(modulus_bytes(&priv_key._d), modulus_bytes(&other._d)) + eq &= crypto.compare_constant_time(factor_bytes(&priv_key._p), factor_bytes(&other._p)) + eq &= crypto.compare_constant_time(factor_bytes(&priv_key._q), factor_bytes(&other._q)) + eq &= crypto.compare_constant_time(factor_bytes(&priv_key._dp), factor_bytes(&other._dp)) + eq &= crypto.compare_constant_time(factor_bytes(&priv_key._dq), factor_bytes(&other._dq)) + eq &= crypto.compare_constant_time(factor_bytes(&priv_key._iq), factor_bytes(&other._iq)) + + return pk_eq & (eq == 1) +} + +// private_key_clear clears priv_key to the uninitialized state. +private_key_clear :: proc "contextless" (priv_key: ^Private_Key) { + crypto.zero_explicit(priv_key, size_of(Private_Key)) +} + +// public_key_n copies the public key's modulus `n` to dst if dst is +// non-nil and of sufficient size, and returns the number of bytes +// copied/would be copied (ie: calling with `dst = nil` gets the +// required size). +@(require_results) +public_key_n :: proc(pub_key: ^Public_Key, dst: []byte) -> (n_len: int) { + ensure(pub_key._is_initialized, "crypto/rsa: uninitialized public key") + + return modulus_copyout(&pub_key._n, dst) +} + +// public_key_e returns the public key's exponent `e` as a u32. +@(require_results) +public_key_e :: proc(pub_key: ^Public_Key) -> u32 { + ensure(pub_key._is_initialized, "crypto/rsa: uninitialized public key") + + return pub_key._e +} + +// public_key_size returns the size of the public key's modulus in bytes. +// All ciphertexts and signatures will also be this size. +@(require_results) +public_key_size :: proc(pub_key: ^Public_Key) -> int { + ensure(pub_key._is_initialized, "crypto/rsa: uninitialized public key") + + return pub_key._n.v_len +} + +// public_key_set_bytes sets a public key from byte-encoded components, +// and returns true if and only if (⟺) the operation was successful. +@(require_results) +public_key_set_bytes :: proc(pub_key: ^Public_Key, n, e: []byte) -> bool { + public_key_clear(pub_key) + defer if !pub_key._is_initialized { + public_key_clear(pub_key) + } + + ok := modulus_set_bytes(&pub_key._n, n) + if !ok { + return false + } + if modulus_len(&pub_key._n) < MODULUS_MIN_SIZE >> 3 { + return false + } + if !modulus_is_odd(&pub_key._n) { + return false + } + + e_ := bytes.trim_left(e, []byte{0x00}) + e_len := len(e_) + if e_len > EXPONENT_MAX_SIZE >> 3 { + return false + } + e_buf: [4]byte + copy(e_buf[4 - e_len:], e) + e_u32 := endian.unchecked_get_u32be(e_buf[:]) + if e_u32 < 3 || e_u32 & 1 == 0 { + return false + } + pub_key._e = e_u32 + + pub_key._is_initialized = true + + return true +} + +// public_key_set sets pub_key to src. +public_key_set :: proc(pub_key, src: ^Public_Key) { + if src == nil || !src._is_initialized { + public_key_clear(pub_key) + return + } + + modulus_set(&pub_key._n, &src._n) + pub_key._e = src._e + pub_key._is_initialized = true +} + +// public_key_set_priv sets pub_key to the public component of priv_key. +public_key_set_priv :: proc(pub_key: ^Public_Key, priv_key: ^Private_Key) { + ensure(priv_key._is_initialized, "crypto/rsa: uninitialized private key") + pub_key^ = priv_key._pub_key +} + +// public_key_equal returns true if and only if (⟺) pub_key is equal to other. +public_key_equal :: proc(pub_key, other: ^Public_Key) -> bool { + ensure(pub_key._is_initialized && other._is_initialized, "crypto/rsa: uninitialized public key") + + eq := crypto.compare_constant_time(modulus_bytes(&pub_key._n), modulus_bytes(&other._n)) + eq &= int(subtle.eq(pub_key._e, other._e)) + + return eq == 1 +} + +// public_key_clear clears pub_key to the uninitialized state. +public_key_clear :: proc "contextless" (pub_key: ^Public_Key) { + crypto.zero_explicit(pub_key, size_of(Public_Key)) +} + +// size returns the size of the key's public modulus in bytes. +// All ciphertexts and signatures will also be this size. +size :: proc "contextless" (key: ^$T) -> int where T == Private_Key || T == Private_Key { + when T == Private_Key { + return private_key_size(key) + } else { + return public_key_size(key) + } +} diff --git a/core/crypto/rsa/rsa_dec_oaep.odin b/core/crypto/rsa/rsa_dec_oaep.odin new file mode 100644 index 000000000..37f48fea9 --- /dev/null +++ b/core/crypto/rsa/rsa_dec_oaep.odin @@ -0,0 +1,197 @@ +package rsa + +// Copyright (c) 2018 Thomas Pornin +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions +// are met: +// +// 1. Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// THIS SOFTWARE IS PROVIDED BY THE AUTHORS “AS IS” AND ANY EXPRESS OR +// IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHORS BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +// DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE +// GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, +// WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING +// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF +// THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +import "core:crypto" +import subtle "core:crypto/_subtle" +import "core:crypto/hash" + +// decrypt_oaep returns the plaintext and true if and only if (⟺) it +// successfully decrypts the ciphertext with OAEP parameterized by +// label, hash_algo, and mgf1_algo, and writes the plaintext into dst. +// If mgf1_algo is unspecified, hash_algo will be used. +// +// Note: dst MUST be large enough to contain the plaintext. +@(require_results) +decrypt_oaep :: proc( + priv_key: ^Private_Key, + hash_algo: hash.Algorithm, + ciphertext: []byte, + dst: []byte, + label: []byte = nil, + mgf1_algo := hash.Algorithm.Invalid, +) -> (plaintext: []byte, ok: bool) { + if !priv_key._is_initialized { + return + } + ct_len := len(ciphertext) + if ct_len != modulus_len(&priv_key._pub_key._n) { + return + } + if hash_algo == .Invalid { + return + } + mgf1_algo_ := mgf1_algo + if mgf1_algo == .Invalid { + mgf1_algo_ = hash_algo + } + + tmp: [MODULUS_MAX_SIZE >> 3]byte + pt_buf := tmp[:ct_len] + defer crypto.zero_explicit(raw_data(pt_buf), ct_len) + + copy(pt_buf, ciphertext) + r := private_modpow(pt_buf, priv_key) + r_, l := oaep_dec_unpad(hash_algo, mgf1_algo_, label, pt_buf) + + // Conditional branches are ok as we are past the padding + // verification. + if ok = r & r_ == 1; ok { + if l <= len(dst) { + copy(dst, pt_buf[:l]) + plaintext = dst[:l] + } else { + ok = false + } + } + + return +} + +// oaep_max_plaintext_size returns the maximum supported plaintext size +// for a given key, with OAEP parameterized by hash_algo and mgf1_algo. +// If mgf1_algo is unspecified, hash_algo will be used. +@(require_results) +oaep_max_plaintext_size :: proc( + k: ^$T, + hash_algo: hash.Algorithm, + mgf1_algo := hash.Algorithm.Invalid, +) -> int where T == Private_Key || T == Public_Key { + if !k._is_initialized { + return 0 + } + if hash_algo == .Invalid { + return 0 + } + mgf1_algo_ := mgf1_algo + if mgf1_algo == .Invalid { + mgf1_algo_ = hash_algo + } + + overhead := 2 + hash.DIGEST_SIZES[hash_algo] + hash.DIGEST_SIZES[mgf1_algo_] + + pub_key: ^Public_Key + when T == Private_Key { + pub_keyk = &k._pub_key + } else { + pub_key = k + } + return modulus_len(&k._n) - overhead +} + +@(private="file") +xor_hash_data :: proc(hash_algo: hash.Algorithm, dst: []byte, src: []byte) { + tmp: [hash.MAX_DIGEST_SIZE]byte = --- + hash_len := hash.DIGEST_SIZES[hash_algo] + digest := tmp[:hash_len] + defer crypto.zero_explicit(raw_data(digest), hash_len) + + hash.hash_bytes_to_buffer(hash_algo, src, digest) + for v, u in digest { + dst[u] ~= v + } +} + +@(private="file") +oaep_dec_unpad :: proc( + hash_algo: hash.Algorithm, + mgf1_algo: hash.Algorithm, + label: []byte, + data: []byte, +) -> (u32, int) { + hash_len := hash.DIGEST_SIZES[hash_algo] + k := len(data) + buf := data + + // There must be room for the padding. + if k < (hash_len << 1) + 2 { + return 0, 0 + } + + // Unmask the seed, then the DB value. + seed, db := buf[1:1+hash_len], buf[1+hash_len:] + mgf1_xor(seed, mgf1_algo, db) + mgf1_xor(db, mgf1_algo, seed) + + // Hash the label and XOR it with the value in the array; if + // they are equal then these should yield only zeros. + xor_hash_data(hash_algo, db, label) + + // At that point, if the padding was correct, when we should + // have: 0x00 || seed || 0x00 ... 0x00 0x01 || M + // Padding is valid as long as: + // - There is at least hlen+1 leading bytes of value 0x00. + // - There is at least one non-zero byte. + // - The first (leftmost) non-zero byte has value 0x01. + // + // Ultimately, we may leak the resulting message length, i.e. + // the position of the byte of value 0x01, but we must take care + // to do so only if the number of zero bytes has been verified + // to be at least hlen+1. + // + // The loop below counts the number of bytes of value 0x00, and + // checks that the next byte has value 0x01, in constant-time. + // + // - If the initial byte (before the seed) is not 0x00, then + // r and s are set to 0, and stay there. + // - Value r is 1 until the first non-zero byte is reached + // (after the seed); it switches to 0 at that point. + // - Value s is set to 1 if and only if the data encountered + // at the time of the transition of r from 1 to 0 has value + // exactly 0x01. + // - Value zlen counts the number of leading bytes of value zero + // (after the seed). + r := u32(subtle.eq(buf[0], 0)) + s, zlen: u32 + for u in hash_len + 1..> 8) + s |= nz & subtle.eq(w, 0x01) + r &= subtle.not(nz) + zlen += r + } + + // Padding is correct only if s == 1, _and_ zlen >= hlen. + s &= subtle.ge(zlen, u32(hash_len)) + + // At that point, padding was verified, and we are now allowed + // to make conditional jumps. + if s != 0 { + plen := 2 + hash_len + int(zlen) + k -= plen + copy(buf[:k], buf[plen:]) + } + return s, k +} diff --git a/core/crypto/rsa/rsa_dec_tls_pms.odin b/core/crypto/rsa/rsa_dec_tls_pms.odin new file mode 100644 index 000000000..cedd9032d --- /dev/null +++ b/core/crypto/rsa/rsa_dec_tls_pms.odin @@ -0,0 +1,55 @@ +package rsa + +// Copyright (c) 2016 Thomas Pornin +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions +// are met: +// +// 1. Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// THIS SOFTWARE IS PROVIDED BY THE AUTHORS “AS IS” AND ANY EXPRESS OR +// IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHORS BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +// DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE +// GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, +// WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING +// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF +// THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +import subtle "core:crypto/_subtle" + +// unsafe_decrypt_tls_pms decrypts a TLS RSA-Encrypted Premaster Secret +// Message, unconditionally moves the decrypted plaintext to `data[:48]`, +// and returns 1 if and only if (⟺) the operation was successful. +// +// WARNING: This routine MUST only be used when implementing server-side +// support for TLS 1.2's Client Key Exchange message, and extreme care +// MUST be taken when handling failures. This key exchange scheme was +// removed in TLS 1.3, and not implementing support in the first place +// is strongly RECOMMENDED even for TLS 1.2 servers. +@(require_results) +unsafe_decrypt_tls_pms :: proc(priv_key: ^Private_Key, data: []byte) -> u32 { + // A first check on length. Since this test works only on the + // buffer length, it needs not (and cannot) be constant-time. + _len := len(data) + if _len < 59 || _len != priv_key._pub_key._n.v_len { + return 0 + } + x := private_modpow(data, priv_key) + + x &= u32(subtle.eq(data[0], 0x00)) + x &= u32(subtle.eq(data[1], 0x02)) + for u in 2..<(_len-49) { + x &= u32(subtle.neq(data[u], 0)) + } + x &= u32(subtle.eq(data[_len - 49], 0x00)) + copy(data[:48], data[_len - 48:]) + + return x +} diff --git a/core/crypto/rsa/rsa_enc_oaep.odin b/core/crypto/rsa/rsa_enc_oaep.odin new file mode 100644 index 000000000..7013b823b --- /dev/null +++ b/core/crypto/rsa/rsa_enc_oaep.odin @@ -0,0 +1,105 @@ +package rsa + +// Copyright (c) 2018 Thomas Pornin +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions +// are met: +// +// 1. Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// THIS SOFTWARE IS PROVIDED BY THE AUTHORS “AS IS” AND ANY EXPRESS OR +// IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHORS BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +// DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE +// GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, +// WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING +// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF +// THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +import "base:intrinsics" +import "core:crypto" +import "core:crypto/hash" + +// encrypt_oaep returns true if and only if (⟺) it successfully +// encrypts the plaintext with OAEP parameterized by label, hash_algo, +// and mgf1_algo, and writes the cipherttext into dst. If mgf1_algo is +// unspecified, hash_algo will be used. +// +// This routine will fail if the system entropy source is unavailable. +encrypt_oaep :: proc( + pub_key: ^Public_Key, + hash_algo: hash.Algorithm, + plaintext: []byte, + dst: []byte, + label: []byte = nil, + mgf1_algo := hash.Algorithm.Invalid, +) -> bool { + if !pub_key._is_initialized { + return false + } + if hash_algo == .Invalid { + return false + } + mgf1_algo_ := mgf1_algo + if mgf1_algo == .Invalid { + mgf1_algo_ = hash_algo + } + if len(dst) != modulus_len(&pub_key._n) { + return false + } + if len(plaintext) > oaep_max_plaintext_size(pub_key, hash_algo, mgf1_algo_) { + return false + } + + if oaep_enc_pad(hash_algo, mgf1_algo_, label, dst, plaintext) != 1 { + return false + } + + return public_modpow(dst, pub_key) == 1 +} + +@(private="file") +oaep_enc_pad :: proc( + hash_algo: hash.Algorithm, + mgf1_algo: hash.Algorithm, + label: []byte, + dst: []byte, + src: []byte, +) -> u32 { + hash_len := hash.DIGEST_SIZES[hash_algo] + src_len := len(src) + k := len(dst) + + // Note: Length checks are handled by the caller. + + // Apply padding. At this point, things cannot fail. + buf := dst + + // Assemble: DB = lHash || PS || 0x01 || M + // We first place the source message M with copy(), so that + // overlaps between source and destination buffers are supported. + copy(buf[k - src_len:], src) + hash.hash_bytes_to_buffer(hash_algo, label, buf[1+hash_len:1+hash_len << 1]) + intrinsics.mem_zero(raw_data(buf[1 + hash_len << 1:]), k - src_len - (hash_len << 1) - 2) + buf[k - src_len - 1] = 0x01 + + // Make the random seed. + seed, db := buf[1:1+hash_len], buf[1+hash_len:] + crypto.rand_bytes(seed) + + // Mask DB with the mask generated from the seed. + mgf1_xor(db, mgf1_algo, seed) + + // Mask the seed with the mask generated from the masked DB. + mgf1_xor(seed, mgf1_algo, db) + + // Padding result: EM = 0x00 || maskedSeed || maskedDB. + buf[0] = 0x00 + return 1 +} diff --git a/core/crypto/rsa/rsa_int.odin b/core/crypto/rsa/rsa_int.odin new file mode 100644 index 000000000..1e84c1fcb --- /dev/null +++ b/core/crypto/rsa/rsa_int.odin @@ -0,0 +1,110 @@ +#+private +package rsa + +import "core:bytes" + +Big_Int :: struct($N: int) { + v: [N]byte, + v_len: int, +} + +Modulus :: Big_Int(MODULUS_MAX_SIZE >> 3) +Factor :: Big_Int(FACTOR_MAX_SIZE >> 3) + +@(require_results) +modulus_set_bytes :: proc(n: ^Modulus, b: []byte) -> bool { + b_ := bytes.trim_left(b, []byte{0x00}) + b_len := len(b_) + + if b_len > size_of(n.v) || b_len == 0 { + return false + } + + copy(n.v[:], b_) + n.v_len = b_len + + return true +} + +modulus_set :: proc "contextless" (n, other: ^Modulus) { + // Copy the full thing. + copy(n.v[:], other.v[:]) + n.v_len = other.v_len +} + +@(require_results) +modulus_bytes :: #force_inline proc "contextless" (n: ^Modulus) -> []byte { + return n.v[:n.v_len] +} + +@(require_results) +modulus_len :: #force_inline proc "contextless" (n: ^Modulus) -> int { + return n.v_len +} + +@(require_results) +modulus_copyout :: proc(n: ^Modulus, dst: []byte) -> (n_len: int) { + if n_len = modulus_len(n); n_len == 0 { + return + } + + if len(dst) > 0 { + ensure(len(dst) >= n_len, "crypto/rsa: insufficent buffer size") + copy(dst, modulus_bytes(n)) + } + + return +} + +@(require_results) +modulus_is_odd :: proc "contextless" (n: ^Modulus) -> bool { + if n.v_len == 0 || n.v[n.v_len-1] & 1 == 0 { + return false + } + return true +} + +@(require_results) +factor_set_bytes :: proc(n: ^Factor, b: []byte) -> bool { + b_ := bytes.trim_left(b, []byte{0x00}) + b_len := len(b_) + + if b_len > size_of(n.v) || b_len == 0 { + return false + } + + copy(n.v[:], b_) + n.v_len = b_len + + return true +} + +factor_set :: proc "contextless" (n, other: ^Factor) { + // Copy the full thing. + copy(n.v[:], other.v[:]) + n.v_len = other.v_len +} + +@(require_results) +factor_bytes :: #force_inline proc "contextless" (n: ^Factor) -> []byte { + return n.v[:n.v_len] +} + +@(require_results) +factor_len :: #force_inline proc "contextless" (n: ^Factor) -> int { + return n.v_len +} + +@(require_results) +factor_copyout :: proc(n: ^Factor, dst: []byte) -> (n_len: int) { + if n_len = factor_len(n); n_len == 0 { + return + } + + if len(dst) > 0 { + ensure(len(dst) >= n_len, "crypto/rsa: insufficent buffer size") + copy(dst, factor_bytes(n)) + } + + return +} diff --git a/core/crypto/rsa/rsa_keygen.odin b/core/crypto/rsa/rsa_keygen.odin new file mode 100644 index 000000000..c0ca12043 --- /dev/null +++ b/core/crypto/rsa/rsa_keygen.odin @@ -0,0 +1,367 @@ +package rsa + +// Copyright (c) 2018 Thomas Pornin +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions +// are met: +// +// 1. Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// THIS SOFTWARE IS PROVIDED BY THE AUTHORS “AS IS” AND ANY EXPRESS OR +// IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHORS BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +// DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE +// GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, +// WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING +// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF +// THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +import "core:crypto" +import bigint "core:crypto/_bigint" +import subtle "core:crypto/_subtle" +import "core:slice" + +// Swap two buffers in RAM. They must be disjoint. +@(private="file") +bufswap_u32 :: proc "contextless" (b1, b2: []u32) { + l := len(b1) + + for u in 0.. u32 { + // We need temporary values for at least 7 integers of the same size + // as a factor (including header word); more space helps with performance + // (in modular exponentiations), but we much prefer to remain under + // 2 kilobytes in total, to save stack space. The macro TEMPS below + // exceeds 512 (which is a count in 32-bit words) when MODULUS_MAX_SIZE + // is greater than 4464 (default value is 4096, so the 2-kB limit is + // maintained unless MODULUS_MAX_SIZE was modified). + TEMPS :: max(512, ((((7 * ((((MODULUS_MAX_SIZE + 1) >> 1) + 61) / 31))) + 1) >> 1) << 1) + + assert(key_size >= MODULUS_MIN_SIZE && key_size <= MODULUS_MAX_SIZE) + + t64: [TEMPS >> 1]u64 + t32 := slice.reinterpret([]u32, t64[:]) + defer crypto.zero_explicit(&t64, size_of(t64)) + + esize_p := u32(key_size + 1) >> 1 + esize_q := u32(key_size) - esize_p + sk._p.v_len = int((esize_p + 7) >> 3) + sk._q.v_len = int((esize_q + 7) >> 3) + sk._dp.v_len = sk._p.v_len + sk._dq.v_len = sk._q.v_len + sk._iq.v_len = sk._p.v_len + + pk := &sk._pub_key + pk._n.v_len = (key_size + 7) >> 3 + pk._e = PUBLIC_EXPONENT + + sk._d.v_len = pk._n.v_len // Private exponent length is that of the modulus. + + // We now switch to encoded sizes. + // + // floor((x * 16913) / (2^19)) is equal to floor(x/31) for all + // integers x from 0 to 34966; the intermediate product fits on + // 30 bits, thus we can use MUL31(). + esize_p += u32(bigint._mul31(esize_p, 16913) >> 19) + esize_q += u32(bigint._mul31(esize_q, 16913) >> 19) + plen := (esize_p + 31) >> 5 + qlen := (esize_q + 31) >> 5 + p := t32 + q := p[1 + plen:] + t := q[1 + qlen:] + + // Since we use a prime exponent, when searching for candidate primes, + // checking if `GCD(e, prime - 1) = 1` is a simple matter of euclidian + // division. + for { + bigint.i62_mkprime(p, esize_p, PUBLIC_EXPONENT, t) + p[1] -= 1 + if bigint.i31_rem(p, PUBLIC_EXPONENT) != 0 { + p[1] += 1 + break + } + } + + for { + bigint.i62_mkprime(q, esize_q, PUBLIC_EXPONENT, t) + q[1] -= 1 + if bigint.i31_rem(q, PUBLIC_EXPONENT) != 0 { + q[1] += 1 + break + } + } + + // If p and q have the same size, then it is possible that q > p + // (when the target modulus size is odd, we generate p with a + // greater bit length than q). If q > p, we want to swap p and q + // for two reasons: + // - The final step below (inversion of q modulo p) is easier if + // p > q. + // - While BearSSL's RSA code is perfectly happy with RSA keys such + // that p < q, some other implementations have restrictions and + // require p > q. + // + // Note that we can do a simple non-constant-time swap here, + // because the only information we leak here is that we insist on + // returning p and q such that p > q, which is not a secret. + if esize_p == esize_q && bigint.i31_sub(p, q, 0) == 1 { + bufswap_u32(p[:1+plen], q) + } + + sk_p, sk_q := factor_bytes(&sk._p), factor_bytes(&sk._q) + bigint.i31_encode(sk_p, p) + bigint.i31_encode(sk_q, q) + // The odds of this happening are infinitesimally small, however + // checking for it is cheap. + if crypto.compare_constant_time(sk_p, sk_q) == 1 { + return 0 + } + + // Compute the public modulus too. + bigint.i31_zero(t, p[0]) + bigint.i31_mulacc(t, p, q) + bigint.i31_encode(modulus_bytes(&pk._n), t) + + // Compute the private exponent. + // + // Computing p - 1 and q - 1 this way is safe as p and q + // are guaranteed to be odd, thus the LSB will always be + // set. + p[1], q[1] = p[1] - 1, q[1] - 1 // p = p - 1, q = q - 1 + if compute_privexp(sk, p, q, pk._e, t) != 1 { + return 0 + } + + // Compute `d % (p - 1)`. + d_mod := t[:1+plen] + bigint.i31_decode_reduce(d_mod, modulus_bytes(&sk._d), p) + bigint.i31_encode(factor_bytes(&sk._dp), d_mod) + + // Compute `d % (q - 1)`. + bigint.i31_decode_reduce(d_mod, modulus_bytes(&sk._d), q) + bigint.i31_encode(factor_bytes(&sk._dq), d_mod) + + // Compute `q^(-1) mod p`. + p[1], q[1] = p[1] + 1, q[1] + 1 // Restore p, q. + return compute_qinv(sk, p, q, plen, t) +} + +@(private="file") +compute_qinv :: proc "contextless" (sk: ^Private_Key, p, q: []u32, plen: u32, t: []u32) -> u32 { + // Per Fermat's Little Theorem, `q^(-1) mod p = q^(p-2) mod p`. + // + // Note: p is guaranteed to be odd as it is a large prime. + + // Compute and encode `p-2`. + p_minus_two := t[:1+plen] + copy(p_minus_two, p[:1+plen]) + two := t[1+plen:] // Temporarily use this for 2. + bigint.i31_zero(two, p[0]) + bigint.i31_decode(two, []byte{2}) + _ = bigint.i31_sub(p_minus_two, two, 1) + iq := factor_bytes(&sk._iq) // Temporarily use this for p - 2. + bigint.i31_encode(iq, p_minus_two) + + // Enforce 64-bit alignment. + t_ := t + if len(t_) & 1 != 0 { + t_ = t_[1:] + } + + m0i := bigint.i31_ninv31(p[1]) + ret := bigint.i62_modpow_opt_as_i31(q, iq, p, m0i, t) + if ret != 0 { + bigint.i31_encode(iq, q) + } + + return ret +} + +@(private="file") +compute_privexp :: proc "contextless" (sk: ^Private_Key, p_minus_one, q_minus_one: []u32, e: u32, tmp: []u32) -> u32 { + // Compute phi = (p-1)*(q-1). The mulacc function sets the announced + // bit length of t to be the sum of the announced bit lengths of + // p-1 and q-1, which is usually exact but may overshoot by one 1 + // bit in some cases; we readjust it to its true length. + phi := tmp + bigint.i31_zero(phi, p_minus_one[0]) + bigint.i31_mulacc(phi, p_minus_one, q_minus_one) + _len := (phi[0] + 31) >> 5 + phi[0] = bigint.i31_bit_length(phi[1:1+_len]) + _len = (phi[0] + 31) >> 5 + + // Divide phi by public exponent e. The final remainder r must be + // non-zero (otherwise, the key is invalid). The quotient is k, + // which we write over phi, since we don't need phi after that. + r: u32 + for u := _len; u >= 1; u -= 1 { + // Upon entry, r < e, and phi[u] < 2^31; hence, + // hi:lo < e*2^31. Thus, the produced word k[u] + // must be lower than 2^31, and the new remainder r + // is lower than e. + hi := r >> 1 + lo := (r << 31) + phi[u] + phi[u], r = bigint.div_rem_u32(hi, lo, e) + } + if r == 0 { + return 0 + } + k := phi + + // Compute u and v such that u*e - v*r = GCD(e,r). We use + // a binary GCD algorithm, with 6 extra integers a, b, + // u0, u1, v0 and v1. Initial values are: + // a = e u0 = 1 v0 = 0 + // b = r u1 = r v1 = e-1 + // The following invariants are maintained: + // a = u0*e - v0*r + // b = u1*e - v1*r + // 0 < a <= e + // 0 < b <= r + // 0 <= u0 <= r + // 0 <= v0 <= e + // 0 <= u1 <= r + // 0 <= v1 <= e + // + // At each iteration, we reduce either a or b by one bit, and + // adjust u0, u1, v0 and v1 to maintain the invariants: + // - if a is even, then a <- a/2 + // - otherwise, if b is even, then b <- b/2 + // - otherwise, if a > b, then a <- (a-b)/2 + // - otherwise, if b > a, then b <- (b-a)/2 + // Algorithm stops when a = b. At that point, the common value + // is the GCD of e and r; it must be 1 (otherwise, the private + // key or public exponent is not valid). The (u0,v0) or (u1,v1) + // pairs are the solution we are looking for. + // + // Since either a or b is reduced by at least 1 bit at each + // iteration, 62 iterations are enough to reach the end + // condition. + // + // To maintain the invariants, we must compute the same operations + // on the u* and v* values that we do on a and b: + // - When a is divided by 2, u0 and v0 must be divided by 2. + // - When b is divided by 2, u1 and v1 must be divided by 2. + // - When b is subtracted from a, u1 and v1 are subtracted from + // u0 and v0, respectively. + // - When a is subtracted from b, u0 and v0 are subtracted from + // u1 and v1, respectively. + // + // However, we want to keep the u* and v* values in their proper + // ranges. The following remarks apply: + // + // - When a is divided by 2, then a is even. Therefore: + // + // * If r is odd, then u0 and v0 must have the same parity; + // if they are both odd, then adding r to u0 and e to v0 + // makes them both even, and the division by 2 brings them + // back to the proper range. + // + // * If r is even, then u0 must be even; if v0 is odd, then + // adding r to u0 and e to v0 makes them both even, and the + // division by 2 brings them back to the proper range. + // + // Thus, all we need to do is to look at the parity of v0, + // and add (r,e) to (u0,v0) when v0 is odd. In order to avoid + // a 32-bit overflow, we can add ((r+1)/2,(e/2)+1) after the + // division (r+1 does not overflow since r < e; and (e/2)+1 + // is equal to (e+1)/2 since e is odd). + // + // - When we subtract b from a, three cases may occur: + // + // * u1 <= u0 and v1 <= v0: just do the subtractions + // + // * u1 > u0 and v1 > v0: compute: + // (u0, v0) <- (u0 + r - u1, v0 + e - v1) + // + // * u1 <= u0 and v1 > v0: compute: + // (u0, v0) <- (u0 + r - u1, v0 + e - v1) + // + // The fourth case (u1 > u0 and v1 <= v0) is not possible + // because it would contradict "b < a" (which is the reason + // why we subtract b from a). + // + // The tricky case is the third one: from the equations, it + // seems that u0 may go out of range. However, the invariants + // and ranges of other values imply that, in that case, the + // new u0 does not actually exceed the range. + // + // We can thus handle the subtraction by adding (r,e) based + // solely on the comparison between v0 and v1. + a, b: u32 = e, r + u0, v0: u32 = 1, 0 + u1, v1: u32 = r, e - 1 + hr, he := (r + 1) >> 1, (e >> 1) + 1 + for _ in 0..<62 { + oa := a & 1 // 1 if a is odd + ob := b & 1 // 1 if b is odd + agtb := subtle.gt(a, b) // 1 if a > b + bgta := subtle.gt(b, a) // 1 if b > a + + sab := oa & ob & agtb // 1 if a <- a-b + sba := oa & ob & bgta // 1 if b <- b-a + + // a <- a-b, u0 <- u0-u1, v0 <- v0-v1 + ctl := subtle.gt(v1, v0) + a -= b & -sab + u0 -= (u1 - (r & -ctl)) & -sab + v0 -= (v1 - (e & -ctl)) & -sab + + // b <- b-a, u1 <- u1-u0 mod r, v1 <- v1-v0 mod e + ctl = subtle.gt(v0, v1) + b -= a & -sba + u1 -= (u0 - (r & -ctl)) & -sba + v1 -= (v0 - (e & -ctl)) & -sba + + da := subtle.not(oa) | sab // 1 if a <- a/2 + db := (oa & subtle.not(ob)) | sba // 1 if b <- b/2 + + // a <- a/2, u0 <- u0/2, v0 <- v0/2 + ctl = v0 & 1 + a ~= (a ~ (a >> 1)) & -da + u0 ~= (u0 ~ ((u0 >> 1) + (hr & -ctl))) & -da + v0 ~= (v0 ~ ((v0 >> 1) + (he & -ctl))) & -da + + // b <- b/2, u1 <- u1/2 mod r, v1 <- v1/2 mod e + ctl = v1 & 1 + b ~= (b ~ (b >> 1)) & -db + u1 ~= (u1 ~ ((u1 >> 1) + (hr & -ctl))) & -db + v1 ~= (v1 ~ ((v1 >> 1) + (he & -ctl))) & -db + } + + // Check that the GCD is indeed 1. If not, then the key is invalid + // (and there's no harm in leaking that piece of information). + if (a != 1) { + return 0 + } + + // Now we have u0*e - v0*r = 1. Let's compute the result as: + // d = u0 + v0*k + // We still have k in the tmp[] array, and its announced bit + // length is that of phi. + m := k[1+_len:] + m[0] = (1 << 5) + 1 // bit length is 32 bits, encoded + m[1] = v0 & bigint.I31_MASK + m[2] = v0 >> 31 + z := m[3:] + bigint.i31_zero(z, k[0]) + z[1] = u0 & bigint.I31_MASK + z[2] = u0 >> 31 + bigint.i31_mulacc(z, k, m) + + // Encode the result. + bigint.i31_encode(modulus_bytes(&sk._d), z) + + return 1 +} diff --git a/core/crypto/rsa/rsa_mgf1.odin b/core/crypto/rsa/rsa_mgf1.odin new file mode 100644 index 000000000..70500c65a --- /dev/null +++ b/core/crypto/rsa/rsa_mgf1.odin @@ -0,0 +1,49 @@ +package rsa + +// Copyright (c) 2018 Thomas Pornin +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions +// are met: +// +// 1. Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// THIS SOFTWARE IS PROVIDED BY THE AUTHORS “AS IS” AND ANY EXPRESS OR +// IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHORS BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +// DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE +// GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, +// WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING +// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF +// THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +import "core:crypto/hash" +import "core:encoding/endian" + +@(private) +mgf1_xor :: proc(data: []byte, hash_algo: hash.Algorithm, seed: []byte) { + tmp: [hash.MAX_DIGEST_SIZE]byte = --- + ctx: hash.Context = --- + + buf, blen := data, len(data) + hlen := hash.DIGEST_SIZES[hash_algo] + digest := tmp[:hlen] + for u, c := int(0), u32(0); u < blen; u, c = u + hlen, c + 1 { + hash.init(&ctx, hash_algo) + hash.update(&ctx, seed) + endian.unchecked_put_u32be(tmp[:], c) + hash.update(&ctx, tmp[:4]) + hash.final(&ctx, digest) + for v in 0..= blen { + break + } + buf[u + v] ~= digest[v] + } + } +} diff --git a/core/crypto/rsa/rsa_modpow_priv.odin b/core/crypto/rsa/rsa_modpow_priv.odin new file mode 100644 index 000000000..2de4ffb39 --- /dev/null +++ b/core/crypto/rsa/rsa_modpow_priv.odin @@ -0,0 +1,165 @@ +package rsa + +// Copyright (c) 2016 Thomas Pornin +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions +// are met: +// +// 1. Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// THIS SOFTWARE IS PROVIDED BY THE AUTHORS “AS IS” AND ANY EXPRESS OR +// IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHORS BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +// DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE +// GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, +// WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING +// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF +// THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +import "core:crypto" +import bigint "core:crypto/_bigint" +import "core:slice" + +@(private, require_results) +private_modpow :: proc(x: []byte, sk: ^Private_Key) -> u32 { + U :: (2 + ((FACTOR_MAX_SIZE + 30) / 31)) + TLEN :: (4 * U) // TLEN is counted in 64-bit words + + ensure(sk._is_initialized, "crypto/rsa: uninitialized private key") + + // Compute the actual lengths of p and q, in bytes. + // These lengths are not considered secret (we cannot really hide + // them anyway in constant-time code). + // + // Note/yawning: The factors should already be the correct size, + // with leading `0x00`s stripped. + p := factor_bytes(&sk._p) + plen := len(p) + for plen > 0 && p[0] == 0 { + p = p[1:] + plen -= 1 + } + q := factor_bytes(&sk._q) + qlen := len(q) + for qlen > 0 && q[0] == 0 { + q = q[1:] + qlen -= 1 + } + + // Compute the maximum factor length, in 31-bit words. + z := max(plen, qlen) << 3 + fwlen := 1 + for z > 0 { + z -= 31 + fwlen += 1 + } + + // Convert size to 62-bit words. + fwlen = (fwlen + 1) >> 1 + + // We need to fit at least 6 values in the stack buffer. + if 6 * fwlen > TLEN { + return 0 + } + + // Compute signature length (in bytes). + xlen := modulus_len(&sk._pub_key._n) + + tmp_: [TLEN]u64 // WARNING: This must be zeroed out. + defer crypto.zero_explicit(&tmp_, size_of(tmp_)) + tmp := tmp_[:] + + // Decode q. + mq := slice.reinterpret([]u32, tmp) + bigint.i31_decode(mq, q) + + // Decode p. + t1 := slice.reinterpret([]u32, tmp[fwlen:]) + bigint.i31_decode(t1, p) + + // Upstream recomputes the public modulus n, but we can just + // decode it as our key representation stores all PKCS#1 + // private key values, + t2 := slice.reinterpret([]u32, tmp[2*fwlen:]) + bigint.i31_decode(t2, modulus_bytes(&sk._pub_key._n)) + + // We encode the modulus into bytes, to perform the comparison + // with bytes. We know that the product length, in bytes, is + // exactly xlen. + // The comparison actually computes the carry when subtracting + // the modulus from the source value; that carry must be 1 for + // a value in the correct range. We keep it in r, which is our + // accumulator for the error code. + m_buf := slice.reinterpret([]byte, tmp[4*fwlen:]) + bigint.i31_encode(m_buf[:xlen], t2) + u := xlen + r: u32 + for u > 0 { + u -= 1 + wn := u32(m_buf[u]) + wx := u32(x[u]) + r = ((wx - (wn + r)) >> 8) & 1 + } + + // Move the decoded p to another temporary buffer. + mp := t2 + copy(mp, t1[:2*fwlen]) + + // Compute s2 = x^dq mod q. + q0i := bigint.i31_ninv31(mq[1]) + s2 := t1 + bigint.i31_decode_reduce(s2, x, mq) + r &= bigint.i62_modpow_opt(s2, factor_bytes(&sk._dq), mq, q0i, tmp[3*fwlen:]) + + // Compute s1 = x^dp mod p. + p0i := bigint.i31_ninv31(mp[1]) + s1 := slice.reinterpret([]u32, tmp[3*fwlen:]) + bigint.i31_decode_reduce(s1, x, mp) + r &= bigint.i62_modpow_opt(s1, factor_bytes(&sk._dp), mp, p0i, tmp[4*fwlen:]) + + // Compute: + // h = (s1 - s2)*(1/q) mod p + // s1 is an integer modulo p, but s2 is modulo q. PKCS#1 is + // unclear about whether p may be lower than q (some existing, + // widely deployed implementations of RSA don't tolerate p < q), + // but we want to support that occurrence, so we need to use the + // reduction function. + // + // Since we use br_i31_decode_reduce() for iq (purportedly, the + // inverse of q modulo p), we also tolerate improperly large + // values for this parameter. + t1 = slice.reinterpret([]u32, tmp[4*fwlen:]) + t2 = slice.reinterpret([]u32, tmp[5*fwlen:]) + bigint.i31_reduce(t2, s2, mp) + _ = bigint.i31_add(s1, mp, bigint.i31_sub(s1, t2, 1)) + bigint.i31_to_monty(s1, mp) + bigint.i31_decode_reduce(t1, factor_bytes(&sk._iq), mp) + bigint.i31_montymul(t2, s1, t1, mp, p0i) + + // h is now in t2. We compute the final result: + // s = s2 + q*h + // All these operations are non-modular. + // + // We need mq, s2 and t2. We use the t3 buffer as destination. + // The buffers mp, s1 and t1 are no longer needed, so we can + // reuse them for t3. Moreover, the first step of the computation + // is to copy s2 into t3, after which s2 is not needed. Right + // now, mq is in slot 0, s2 is in slot 1, and t2 is in slot 5. + // Therefore, we have ample room for t3 by simply using s2. + t3 := s2 + bigint.i31_mulacc(t3, mq, t2) + + // Encode the result. Since we already checked the value of xlen, + // we can just use it right away. + bigint.i31_encode(x, t3) + + // The only error conditions remaining at that point are invalid + // values for p and q (even integers). + return p0i & q0i & r +} diff --git a/core/crypto/rsa/rsa_modpow_pub.odin b/core/crypto/rsa/rsa_modpow_pub.odin new file mode 100644 index 000000000..f40bddf29 --- /dev/null +++ b/core/crypto/rsa/rsa_modpow_pub.odin @@ -0,0 +1,89 @@ +package rsa + +// Copyright (c) 2016 Thomas Pornin +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions +// are met: +// +// 1. Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// THIS SOFTWARE IS PROVIDED BY THE AUTHORS “AS IS” AND ANY EXPRESS OR +// IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHORS BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +// DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE +// GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, +// WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING +// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF +// THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +import bigint "core:crypto/_bigint" +import "core:encoding/endian" +import "core:slice" + +@(private, require_results) +public_modpow :: proc(x: []byte, pk: ^Public_Key) -> u32 { + TLEN :: (2 * (2 + ((MODULUS_MAX_SIZE + 30) / 31))) + + ensure(pk._is_initialized, "crypto/rsa: uninitialized public key") + + // Get the actual length of the modulus, and see if it fits within + // our stack buffer. We also check that the length of x[] is valid. + // + // Note/yawning: The modulus should already be the correct size, + // with leading `0x00`s stripped. + n := modulus_bytes(&pk._n) + nlen := modulus_len(&pk._n) + for nlen > 0 && n[0] == 0 { + n = n[1:] + nlen -= 1 + } + if nlen == 0 || nlen > (MODULUS_MAX_SIZE >> 3) || len(x) != nlen { + return 0 + } + z := nlen << 3 + fwlen := 1 + for z > 0 { + z -= 31 + fwlen += 1 + } + // Convert fwlen to a count in 62-bit words. + fwlen = (fwlen + 1) >> 1 + + // The modulus gets decoded into m[]. + // The value to exponentiate goes into a[]. + tmp: [TLEN]u64 // WARNING: This must be zeroed out. + m := slice.reinterpret([]u32, tmp[:fwlen]) + a := slice.reinterpret([]u32, tmp[fwlen:2*fwlen]) + + // Decode the modulus. + bigint.i31_decode(m, n) + m0i := bigint.i31_ninv31(m[1]) + + // Note: if m[] is even, then m0i == 0. Otherwise, m0i must be + // an odd integer. + r := m0i & 1 + + // Decode x[] into a[]; we also check that its value is proper. + r &= bigint.i31_decode_mod(a, x, m) + + // Compute the modular exponentiation. + e_: [EXPONENT_MAX_SIZE >> 3]byte + e_off: int + endian.unchecked_put_u32be(e_[:], pk._e) + if e_[0] == 0 { + // `e = 65537` is the most common and sensible value, so this + // is the most sensible value. + e_off = 1 + } + bigint.i62_modpow_opt(a, e_[e_off:], m, m0i, tmp[2*fwlen:]) + + // Encode the result. + bigint.i31_encode(x, a) + return r +} diff --git a/core/crypto/rsa/rsa_sig_pkcs1.odin b/core/crypto/rsa/rsa_sig_pkcs1.odin new file mode 100644 index 000000000..dd01c654b --- /dev/null +++ b/core/crypto/rsa/rsa_sig_pkcs1.odin @@ -0,0 +1,233 @@ +package rsa + +// Copyright (c) 2017 Thomas Pornin +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions +// are met: +// +// 1. Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// THIS SOFTWARE IS PROVIDED BY THE AUTHORS “AS IS” AND ANY EXPRESS OR +// IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHORS BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +// DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE +// GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, +// WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING +// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF +// THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +import "core:bytes" +import "core:crypto" +import "core:crypto/hash" + +// PKCS1_HASH_OIDS maps common hash algorithms to the OIDs for +// use with PKCS#1 signatures. +@(rodata) +PKCS1_HASH_OIDS := #partial [hash.Algorithm][]byte { + // WARNING: Legacy verification ONLY. + .Insecure_SHA1 = []byte{ + 0x05, 0x2B, 0x0E, 0x03, 0x02, 0x1A, + }, + .SHA224 = []byte{ + 0x09, 0x60, 0x86, 0x48, 0x01, 0x65, 0x03, 0x04, 0x02, 0x04, + }, + .SHA256 = []byte{ + 0x09, 0x60, 0x86, 0x48, 0x01, 0x65, 0x03, 0x04, 0x02, 0x01, + }, + .SHA384 = []byte{ + 0x09, 0x60, 0x86, 0x48, 0x01, 0x65, 0x03, 0x04, 0x02, 0x02, + }, + .SHA512 = []byte{ + 0x09, 0x60, 0x86, 0x48, 0x01, 0x65, 0x03, 0x04, 0x02, 0x03, + }, + .SHA512_256 = []byte{ + 0x09, 0x60, 0x86, 0x48, 0x01, 0x65, 0x03, 0x04, 0x02, 0x06, + }, +} + +@(private="file", rodata) +PKCS1_SELFTEST_DIGEST_SHA256 := []byte{ + 0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07, 0x08, + 0x09, 0x0a, 0x0b, 0x0c, 0x0d, 0x0e, 0x0f, 0x10, + 0x11, 0x12, 0x13, 0x14, 0x15, 0x16, 0x17, 0x18, + 0x19, 0x1a, 0x1b, 0x1c, 0x1d, 0x1e, 0x1f, 0x20, +} + +// verify_pkcs1 returns true if and only if (⟺) sig is a valid PKCS#1 +// signature by pub_key over msg, hased using hash_algo. If pre_hashed +// is set to true, it is assumed that msg is already hashed. +@(require_results) +verify_pkcs1 :: proc(pub_key: ^Public_Key, hash_algo: hash.Algorithm, msg, sig: []byte, is_prehashed := false) -> bool { + if !pub_key._is_initialized { + return false + } + if len(sig) != modulus_len(&pub_key._n) { + return false + } + + // Lookup the OID. + oid := PKCS1_HASH_OIDS[hash_algo] + if oid == nil { + return false + } + hash_len := hash.DIGEST_SIZES[hash_algo] + + // Compute the message hash. + msg_hash_buf: [hash.MAX_DIGEST_SIZE]byte = --- + msg_hash: []byte + switch is_prehashed { + case true: + if len(msg) != hash_len { + return false + } + msg_hash = msg + case false: + msg_hash = hash.hash_bytes_to_buffer(hash_algo, msg, msg_hash_buf[:]) + } + + // PKCS #1 V2.2 (RFC 8017) 8.2.2 specifies this as computing + // and comparing the padded hash, with unpadding and extracting + // the hash being an alternative. Upstream BearSSL implements + // the latter, which is not a problem if done correctly (which + // it does), however we will opt to go for implementing this + // as specified as it is more robust against implementation + // errors. + + // Compute the expected hash. + sig_buf, padded_hash_buf: [MODULUS_MAX_SIZE >> 3]byte = ---, --- + if len(sig) > len(sig_buf) { + return false + } + padded_hash_ := padded_hash_buf[:len(sig)] + if pkcs1_sig_pad(oid, msg_hash, padded_hash_) != 1 { + return false + } + + // Compute the signature's padded hash. + sig_ := sig_buf[:len(sig)] + copy(sig_, sig) + if public_modpow(sig_, pub_key) != 1 { + return false + } + + return bytes.equal(sig_, padded_hash_) +} + +// sign_pkcs1 returns true if and only if (⟺) it successfully writes +// the PKCS#1 signature by priv_key over msg, hashed using hash_algo. +// If pre_hashed is set to true, it is assumed that msg is already hashed. +@(require_results) +sign_pkcs1 :: proc(priv_key: ^Private_Key, hash_algo: hash.Algorithm, msg, sig: []byte, is_prehashed := false) -> bool { + if !priv_key._is_initialized { + return false + } + if len(sig) != modulus_len(&priv_key._pub_key._n) { + return false + } + + // Lookup the OID. + oid := PKCS1_HASH_OIDS[hash_algo] + if oid == nil { + return false + } + + // Compute the message hash. + msg_hash_buf: [hash.MAX_DIGEST_SIZE]byte = --- + msg_hash: []byte + switch is_prehashed { + case true: + if len(msg) != hash.DIGEST_SIZES[hash_algo] { + return false + } + msg_hash = msg + case false: + msg_hash = hash.hash_bytes_to_buffer(hash_algo, msg, msg_hash_buf[:]) + } + + if pkcs1_sig_pad(oid, msg_hash, sig) != 1 { + return false + } + + return private_modpow(sig, priv_key) == 1 +} + +@(private="file", require_results) +pkcs1_sig_pad :: proc "contextless" (hash_oid, hash, x: []byte) -> u32 { + // Padded hash value has format: + // 00 01 FF .. FF 00 30 x1 30 x2 06 x3 OID 05 00 04 x4 HASH + // + // with the following rules: + // + // -- Total length is equal to the modulus length (unsigned + // encoding). + // + // -- There must be at least eight bytes of value 0xFF. + // + // -- x4 is equal to the hash length (hash_len). + // + // -- x3 is equal to the encoded OID value length (hash_oid[0]). + // + // -- x2 = x3 + 4. + // + // -- x1 = x2 + x4 + 4 = x3 + x4 + 8. + // + // Note: the "05 00" is optional (signatures with and without + // that sequence exist in practice), but notes in PKCS#1 seem to + // indicate that the presence of that sequence (specifically, + // an ASN.1 NULL value for the hash parameters) may be slightly + // more "standard" than the opposite. + xlen, hash_len := len(x), len(hash) + + // Note/yawning: The hash OID is mandatory, as is the "05 00". + x3 := hash_oid[0] + + // Check that there is enough room for all the elements, + // including at least eight bytes of value 0xFF. + if xlen < int(x3) + hash_len + 21 { + return 0 + } + x[0] = 0x00 + x[1] = 0x01 + u := xlen - int(x3) - hash_len - 11 + for i in 2..< u { + x[i] = 0xff + } + x[u] = 0x00 + x[u + 1] = 0x30 + x[u + 2] = x3 + byte(hash_len) + 8 + x[u + 3] = 0x30 + x[u + 4] = x3 + 4 + x[u + 5] = 0x06 + copy(x[u+6:], hash_oid) + u += int(x3) + 7 + x[u] = 0x05 + u += 1 + x[u] = 0x00 + u += 1 + x[u] = 0x04 + u += 1 + x[u] = byte(hash_len) + u += 1 + copy(x[u:], hash) + + return 1 +} + +@(private) +pkcs1_sig_selftest :: proc(priv_key: ^Private_Key) -> bool { + sig_buf: [MODULUS_MAX_SIZE >> 3]byte = --- + defer crypto.zero_explicit(&sig_buf, size_of(sig_buf)) + + sig := sig_buf[:private_key_size(priv_key)] + if !sign_pkcs1(priv_key, .SHA256, PKCS1_SELFTEST_DIGEST_SHA256, sig, true) { + return false + } + + return verify_pkcs1(&priv_key._pub_key, .SHA256, PKCS1_SELFTEST_DIGEST_SHA256, sig, true) +} diff --git a/core/crypto/rsa/rsa_sig_pss.odin b/core/crypto/rsa/rsa_sig_pss.odin new file mode 100644 index 000000000..a2b19e509 --- /dev/null +++ b/core/crypto/rsa/rsa_sig_pss.odin @@ -0,0 +1,293 @@ +package rsa + +// Copyright (c) 2018 Thomas Pornin +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions +// are met: +// +// 1. Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// THIS SOFTWARE IS PROVIDED BY THE AUTHORS “AS IS” AND ANY EXPRESS OR +// IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHORS BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +// DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE +// GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, +// WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING +// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF +// THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +import "base:intrinsics" +import "core:crypto" +import bigint "core:crypto/_bigint" +import subtle "core:crypto/_subtle" +import "core:crypto/hash" + +// verify_pss returns true if and only if (⟺) sig is a valid PSS +// signature by pub_key over msg, hashed using hash_algo, and MGF1 +// parameterized by mgf1_algo and salt_len. If mgf1_algo is +// unspecified, hash_algo will be used. If pre_hashed is set +// to true, it is assumed that msg is already hashed. +@(require_results) +verify_pss :: proc( + pub_key: ^Public_Key, + hash_algo: hash.Algorithm, + salt_len: int, + msg: []byte, + sig: []byte, + is_prehashed := false, + mgf1_algo := hash.Algorithm.Invalid, +) -> bool { + if !pub_key._is_initialized { + return false + } + if hash_algo == .Invalid { + return false + } + mgf1_algo_ := mgf1_algo + if mgf1_algo == .Invalid { + mgf1_algo_ = hash_algo + } + if len(sig) != modulus_len(&pub_key._n) { + return false + } + + // Compute the message hash. + msg_hash_buf: [hash.MAX_DIGEST_SIZE]byte = --- + hash_len := hash.DIGEST_SIZES[hash_algo] + msg_hash: []byte + switch is_prehashed { + case true: + if len(msg) != hash_len { + return false + } + msg_hash = msg + case false: + msg_hash = hash.hash_bytes_to_buffer(hash_algo, msg, msg_hash_buf[:]) + } + + sig_buf: [MODULUS_MAX_SIZE >> 3]byte = --- + sig_ := sig_buf[:len(sig)] + copy(sig_, sig) + if public_modpow(sig_, pub_key) != 1 { + return false + } + + return pss_sig_unpad(hash_algo, mgf1_algo_, msg_hash, salt_len, pub_key, sig_) == 1 +} + +// sign_pss returns true if and only if (⟺) it successfully writes +// the PKCS#1 signature by priv_key over msg, hashed using hash_algo, and +// MGF1 parameterized by mgf1_algo and salt_len. If mgf1_algo is +// unspecified, hash_algo will be used. If pre_hashed is set to true, +// it is assumed that msg is already hashed. A reasonable choice for +// salt_len is the digest size of hash_algo, and FIPS 140-3 mandates +// that as the maximum permissible size. +// +// This routine will fail if the system entropy source is unavailable. +@(require_results) +sign_pss :: proc( + priv_key: ^Private_Key, + hash_algo: hash.Algorithm, + salt_len: int, + msg: []byte, + sig: []byte, + is_prehashed := false, + mgf1_algo := hash.Algorithm.Invalid, +) -> bool { + if !priv_key._is_initialized { + return false + } + if len(sig) != modulus_len(&priv_key._pub_key._n) { + return false + } + if hash_algo == .Invalid { + return false + } + mgf1_algo_ := mgf1_algo + if mgf1_algo == .Invalid { + mgf1_algo_ = hash_algo + } + if !crypto.HAS_RAND_BYTES && salt_len != 0 { + return false + } + + // Compute the message hash. + msg_hash_buf: [hash.MAX_DIGEST_SIZE]byte = --- + hash_len := hash.DIGEST_SIZES[hash_algo] + msg_hash: []byte + switch is_prehashed { + case true: + if len(msg) != hash_len { + return false + } + msg_hash = msg + case false: + msg_hash = hash.hash_bytes_to_buffer(hash_algo, msg, msg_hash_buf[:]) + } + + // Work out the exact length of n in bits. + n := modulus_bytes(&priv_key._pub_key._n) + assert(len(n) > 0 && n[0] != 0) + n_bitlen := int(bigint._u32_bit_length(u32(n[0]))) + (len(n) - 1) * 8 + + if pss_sig_pad(hash_algo, mgf1_algo_, msg_hash, salt_len, n_bitlen, sig) != 1 { + return false + } + + return private_modpow(sig, priv_key) == 1 +} + +@(private="file", require_results) +pss_sig_unpad :: proc( + data_algo: hash.Algorithm, + mgf1_algo: hash.Algorithm, + digest: []byte, + salt_len: int, + pk: ^Public_Key, + sig: []byte, +) -> u32 { + hash_len := hash.DIGEST_SIZES[data_algo] + x := sig + + // Value r will be set to a non-zero value is any test fails. + r: u32 + + // The value bit length (as an integer) must be strictly less than + // that of the modulus. + // + // Note/yawning: The modulus should already be the correct size, + // with leading `0x00`s stripped. + n := modulus_bytes(&pk._n) + nlen := modulus_len(&pk._n) + u: int + for u = 0; u < nlen; u += 1 { + if n[u] != 0 { + break + } + } + if u == nlen { + return 0 + } + n_bitlen := bigint._u32_bit_length(u32(n[u])) + (u32(nlen - u - 1) << 3) + n_bitlen -= 1 + if (n_bitlen & 7) == 0 { + r |= u32(x[0]) + x = x[1:] + } else { + r |= u32(x[0] & (0xFF << (n_bitlen & 7))) + } + xlen := int((n_bitlen + 7) >> 3) + + // Check that the modulus is large enough for the hash value + // length combined with the intended salt length. + if hash_len > xlen || salt_len > xlen || (hash_len + salt_len + 2) > xlen { + return 0 + } + + // Check value of rightmost byte. + r |= u32(x[xlen - 1] ~ 0xBC) + + // Generate the mask and XOR it into the first bytes to reveal PS; + // we must also mask out the leading bits. + seed := x[xlen - hash_len - 1:] + mgf1_xor(x[:xlen - hash_len - 1], mgf1_algo, seed[:hash_len]) + if (n_bitlen & 7) != 0 { + x[0] &= 0xFF >> (8 - (n_bitlen & 7)) + } + + // Check that all padding bytes have the expected value. + for u = 0; u < (xlen - hash_len - salt_len - 2); u += 1 { + r |= u32(x[u]) + } + r |= u32(x[xlen - hash_len - salt_len - 2] ~ 0x01) + + // Recompute H. + salt := x[xlen - hash_len - salt_len - 1:] + tmp: [hash.MAX_DIGEST_SIZE]byte + h := tmp[:hash_len] + ctx: hash.Context = --- + hash.init(&ctx, data_algo) + hash.update(&ctx, tmp[:8]) + hash.update(&ctx, digest) + hash.update(&ctx, salt[:salt_len]) + hash.final(&ctx, h) + + // Check that the recomputed H value matches the one appearing + // in the string. + x = x[xlen - hash_len - 1:] + r |= subtle.eq0(u32(crypto.compare_constant_time(h, x[:hash_len]))) + + return subtle.eq0(r) +} + +@(private="file", require_results) +pss_sig_pad :: proc( + data_algo: hash.Algorithm, + mgf1_algo: hash.Algorithm, + digest: []byte, + salt_len: int, + n_bitlen_: int, + sig: []byte, +) -> u32 { + x, n_bitlen := sig, n_bitlen_ + hash_len := hash.DIGEST_SIZES[data_algo] + + // The padded string is one bit smaller than the modulus; + // notably, if the modulus length is equal to 1 modulo 8, then + // the padded string will be one _byte_ smaller, and the first + // byte will be set to 0. We apply these transformations here. + n_bitlen -= 1 + if (n_bitlen & 7) == 0 { + x[0] = 0 + x = x[1:] + } + xlen := int((n_bitlen + 7) >> 3) + + // Check that the modulus is large enough for the hash value + // length combined with the intended salt length. + if hash_len > xlen || salt_len > xlen || (hash_len + salt_len + 2) > xlen { + return 0 + } + + // Produce a random salt. + salt := x[xlen - hash_len - salt_len - 1:] + salt = salt[:salt_len] + if salt_len != 0 { + crypto.rand_bytes(salt) + } + + // Compute the seed for MGF1. + seed := x[xlen - hash_len - 1:] + seed = seed[:hash_len] + ctx: hash.Context = --- + hash.init(&ctx, data_algo) + intrinsics.mem_zero(raw_data(seed), 8) + hash.update(&ctx, seed[:8]) + hash.update(&ctx, digest) + hash.update(&ctx, salt) + hash.final(&ctx, seed) + + // Prepare string PS (padded salt). The salt is already at the + // right place. + intrinsics.mem_zero(raw_data(x), xlen - salt_len - hash_len - 2) + x[xlen - salt_len - hash_len - 2] = 0x01 + + // Generate the mask and XOR it into PS. + mgf1_xor(x[:xlen - hash_len - 1], mgf1_algo, seed) + + // Clear the top bits to ensure the value is lower than the + // modulus. + x[0] &= 0xFF >> ((u32(xlen) << 3) - u32(n_bitlen)) + + // The seed (H) is already in the right place. We just set the + // last byte. + x[xlen - 1] = 0xBC + + return 1 +} diff --git a/core/crypto/rsa/rsa_test_key.odin b/core/crypto/rsa/rsa_test_key.odin new file mode 100644 index 000000000..28020d21f --- /dev/null +++ b/core/crypto/rsa/rsa_test_key.odin @@ -0,0 +1,180 @@ +package rsa + +// private_key_set_insecure_test sets the private key to the +// pregenerated INSECURE test key "testRSA2048" from RFC 9500 2.1. +// +// WARNING: This key MUST only be used for testing purposes. +@(require_results) +private_key_set_insecure_test :: proc(priv_key: ^Private_Key) -> bool { + // RFC 9500 2.1 "testRSA2048" + return private_key_set_bytes( + priv_key, + // n + []byte{ + 0xB0, 0xF9, 0xE8, 0x19, 0x43, 0xA7, 0xAE, 0x98, + 0x92, 0xAA, 0xDE, 0x17, 0xCA, 0x7C, 0x40, 0xF8, + 0x74, 0x4F, 0xED, 0x2F, 0x81, 0x48, 0xE6, 0xC8, + 0xEA, 0xA2, 0x7B, 0x7D, 0x00, 0x15, 0x48, 0xFB, + 0x51, 0x92, 0xAB, 0x28, 0xB5, 0x6C, 0x50, 0x60, + 0xB1, 0x18, 0xCC, 0xD1, 0x31, 0xE5, 0x94, 0x87, + 0x4C, 0x6C, 0xA9, 0x89, 0xB5, 0x6C, 0x27, 0x29, + 0x6F, 0x09, 0xFB, 0x93, 0xA0, 0x34, 0xDF, 0x32, + 0xE9, 0x7C, 0x6F, 0xF0, 0x99, 0x8C, 0xFD, 0x8E, + 0x6F, 0x42, 0xDD, 0xA5, 0x8A, 0xCD, 0x1F, 0xA9, + 0x79, 0x86, 0xF1, 0x44, 0xF3, 0xD1, 0x54, 0xD6, + 0x76, 0x50, 0x17, 0x5E, 0x68, 0x54, 0xB3, 0xA9, + 0x52, 0x00, 0x3B, 0xC0, 0x68, 0x87, 0xB8, 0x45, + 0x5A, 0xC2, 0xB1, 0x9F, 0x7B, 0x2F, 0x76, 0x50, + 0x4E, 0xBC, 0x98, 0xEC, 0x94, 0x55, 0x71, 0xB0, + 0x78, 0x92, 0x15, 0x0D, 0xDC, 0x6A, 0x74, 0xCA, + 0x0F, 0xBC, 0xD3, 0x54, 0x97, 0xCE, 0x81, 0x53, + 0x4D, 0xAF, 0x94, 0x18, 0x84, 0x4B, 0x13, 0xAE, + 0xA3, 0x1F, 0x9D, 0x5A, 0x6B, 0x95, 0x57, 0xBB, + 0xDF, 0x61, 0x9E, 0xFD, 0x4E, 0x88, 0x7F, 0x2D, + 0x42, 0xB8, 0xDD, 0x8B, 0xC9, 0x87, 0xEA, 0xE1, + 0xBF, 0x89, 0xCA, 0xB8, 0x5E, 0xE2, 0x1E, 0x35, + 0x63, 0x05, 0xDF, 0x6C, 0x07, 0xA8, 0x83, 0x8E, + 0x3E, 0xF4, 0x1C, 0x59, 0x5D, 0xCC, 0xE4, 0x3D, + 0xAF, 0xC4, 0x91, 0x23, 0xEF, 0x4D, 0x8A, 0xBB, + 0xA9, 0x3D, 0x39, 0x05, 0xE4, 0x02, 0x8D, 0x7B, + 0xA9, 0x14, 0x84, 0xA2, 0x75, 0x96, 0xE0, 0x7B, + 0x4B, 0x6E, 0xD9, 0x92, 0xF0, 0x77, 0xB5, 0x24, + 0xD3, 0xDC, 0xFE, 0x7D, 0xDD, 0x55, 0x49, 0xBE, + 0x7C, 0xCE, 0x8D, 0xA0, 0x35, 0xCF, 0xA0, 0xB3, + 0xFB, 0x8F, 0x9E, 0x46, 0xF7, 0x32, 0xB2, 0xA8, + 0x6B, 0x46, 0x01, 0x65, 0xC0, 0x8F, 0x53, 0x13, + }, + // e + []byte{0x01, 0x00, 0x01}, + // d + []byte{ + 0x41, 0x18, 0x8B, 0x20, 0xCF, 0xDB, 0xDB, 0xC2, + 0xCF, 0x1F, 0xFE, 0x75, 0x2D, 0xCB, 0xAA, 0x72, + 0x39, 0x06, 0x35, 0x2E, 0x26, 0x15, 0xD4, 0x9D, + 0xCE, 0x80, 0x59, 0x7F, 0xCF, 0x0A, 0x05, 0x40, + 0x3B, 0xEF, 0x00, 0xFA, 0x06, 0x51, 0x82, 0xF7, + 0x2D, 0xEC, 0xFB, 0x59, 0x6F, 0x4B, 0x0C, 0xE8, + 0xFF, 0x59, 0x70, 0xBA, 0xF0, 0x7A, 0x89, 0xA5, + 0x19, 0xEC, 0xC8, 0x16, 0xB2, 0xF4, 0xFF, 0xAC, + 0x50, 0x69, 0xAF, 0x1B, 0x06, 0xBF, 0xEF, 0x7B, + 0xF6, 0xBC, 0xD7, 0x9E, 0x4E, 0x81, 0xC8, 0xC5, + 0xA3, 0xA7, 0xD9, 0x13, 0x0D, 0xC3, 0xCF, 0xBA, + 0xDA, 0xE5, 0xF6, 0xD2, 0x88, 0xF9, 0xAE, 0xE3, + 0xF6, 0xFF, 0x92, 0xFA, 0xE0, 0xF8, 0x1A, 0xF5, + 0x97, 0xBE, 0xC9, 0x6A, 0xE9, 0xFA, 0xB9, 0x40, + 0x2C, 0xD5, 0xFE, 0x41, 0xF7, 0x05, 0xBE, 0xBD, + 0xB4, 0x7B, 0xB7, 0x36, 0xD3, 0xFE, 0x6C, 0x5A, + 0x51, 0xE0, 0xE2, 0x07, 0x32, 0xA9, 0x7B, 0x5E, + 0x46, 0xC1, 0xCB, 0xDB, 0x26, 0xD7, 0x48, 0x54, + 0xC6, 0xB6, 0x60, 0x4A, 0xED, 0x46, 0x37, 0x35, + 0xFF, 0x90, 0x76, 0x04, 0x65, 0x57, 0xCA, 0xF9, + 0x49, 0xBF, 0x44, 0x88, 0x95, 0xC2, 0x04, 0x32, + 0xC1, 0xE0, 0x9C, 0x01, 0x4E, 0xA7, 0x56, 0x60, + 0x43, 0x4F, 0x1A, 0x0F, 0x3B, 0xE2, 0x94, 0xBA, + 0xBC, 0x5D, 0x53, 0x0E, 0x6A, 0x10, 0x21, 0x3F, + 0x53, 0xB6, 0x03, 0x75, 0xFC, 0x84, 0xA7, 0x57, + 0x3F, 0x2A, 0xF1, 0x21, 0x55, 0x84, 0xF5, 0xB4, + 0xBD, 0xA6, 0xD4, 0xE8, 0xF9, 0xE1, 0x7A, 0x78, + 0xD9, 0x7E, 0x77, 0xB8, 0x6D, 0xA4, 0xA1, 0x84, + 0x64, 0x75, 0x31, 0x8A, 0x7A, 0x10, 0xA5, 0x61, + 0x01, 0x4E, 0xFF, 0xA2, 0x3A, 0x81, 0xEC, 0x56, + 0xE9, 0xE4, 0x10, 0x9D, 0xEF, 0x8C, 0xB3, 0xF7, + 0x97, 0x22, 0x3F, 0x7D, 0x8D, 0x0D, 0x43, 0x51, + }, + // p + []byte{ + 0xDD, 0x10, 0x57, 0x02, 0x38, 0x2F, 0x23, 0x2B, + 0x36, 0x81, 0xF5, 0x37, 0x91, 0xE2, 0x26, 0x17, + 0xC7, 0xBF, 0x4E, 0x9A, 0xCB, 0x81, 0xED, 0x48, + 0xDA, 0xF6, 0xD6, 0x99, 0x5D, 0xA3, 0xEA, 0xB6, + 0x42, 0x83, 0x9A, 0xFF, 0x01, 0x2D, 0x2E, 0xA6, + 0x28, 0xB9, 0x0A, 0xF2, 0x79, 0xFD, 0x3E, 0x6F, + 0x7C, 0x93, 0xCD, 0x80, 0xF0, 0x72, 0xF0, 0x1F, + 0xF2, 0x44, 0x3B, 0x3E, 0xE8, 0xF2, 0x4E, 0xD4, + 0x69, 0xA7, 0x96, 0x13, 0xA4, 0x1B, 0xD2, 0x40, + 0x20, 0xF9, 0x2F, 0xD1, 0x10, 0x59, 0xBD, 0x1D, + 0x0F, 0x30, 0x1B, 0x5B, 0xA7, 0xA9, 0xD3, 0x63, + 0x7C, 0xA8, 0xD6, 0x5C, 0x1A, 0x98, 0x15, 0x41, + 0x7D, 0x8E, 0xAB, 0x73, 0x4B, 0x0B, 0x4F, 0x3A, + 0x2C, 0x66, 0x1D, 0x9A, 0x1A, 0x82, 0xF3, 0xAC, + 0x73, 0x4C, 0x40, 0x53, 0x06, 0x69, 0xAB, 0x8E, + 0x47, 0x30, 0x45, 0xA5, 0x8E, 0x65, 0x53, 0x9D, + }, + // q + []byte{ + 0xCC, 0xF1, 0xE5, 0xBB, 0x90, 0xC8, 0xE9, 0x78, + 0x1E, 0xA7, 0x5B, 0xEB, 0xF1, 0x0B, 0xC2, 0x52, + 0xE1, 0x1E, 0xB0, 0x23, 0xA0, 0x26, 0x0F, 0x18, + 0x87, 0x55, 0x2A, 0x56, 0x86, 0x3F, 0x4A, 0x64, + 0x21, 0xE8, 0xC6, 0x00, 0xBF, 0x52, 0x3D, 0x6C, + 0xB1, 0xB0, 0xAD, 0xBD, 0xD6, 0x5B, 0xFE, 0xE4, + 0xA8, 0x8A, 0x03, 0x7E, 0x3D, 0x1A, 0x41, 0x5E, + 0x5B, 0xB9, 0x56, 0x48, 0xDA, 0x5A, 0x0C, 0xA2, + 0x6B, 0x54, 0xF4, 0xA6, 0x39, 0x48, 0x52, 0x2C, + 0x3D, 0x5F, 0x89, 0xB9, 0x4A, 0x72, 0xEF, 0xFF, + 0x95, 0x13, 0x4D, 0x59, 0x40, 0xCE, 0x45, 0x75, + 0x8F, 0x30, 0x89, 0x80, 0x90, 0x89, 0x56, 0x58, + 0x8E, 0xEF, 0x57, 0x5B, 0x3E, 0x4B, 0xC4, 0xC3, + 0x68, 0xCF, 0xE8, 0x13, 0xEE, 0x9C, 0x25, 0x2C, + 0x2B, 0x02, 0xE0, 0xDF, 0x91, 0xF1, 0xAA, 0x01, + 0x93, 0x8D, 0x38, 0x68, 0x5D, 0x60, 0xBA, 0x6F, + }, + // dp + []byte{ + 0x09, 0xED, 0x54, 0xEA, 0xED, 0x98, 0xF8, 0x4C, + 0x55, 0x7B, 0x4A, 0x86, 0xBF, 0x4F, 0x57, 0x84, + 0x93, 0xDC, 0xBC, 0x6B, 0xE9, 0x1D, 0xA1, 0x89, + 0x37, 0x04, 0x04, 0xA9, 0x08, 0x72, 0x76, 0xF4, + 0xCE, 0x51, 0xD8, 0xA1, 0x00, 0xED, 0x85, 0x7D, + 0xC2, 0xB0, 0x64, 0x94, 0x74, 0xF3, 0xF1, 0x5C, + 0xD2, 0x4C, 0x54, 0xDB, 0x28, 0x71, 0x10, 0xE5, + 0x6E, 0x5C, 0xB0, 0x08, 0x68, 0x2F, 0x91, 0x68, + 0xAA, 0x81, 0xF3, 0x14, 0x58, 0xB7, 0x43, 0x1E, + 0xCC, 0x1C, 0x44, 0x90, 0x6F, 0xDA, 0x87, 0xCA, + 0x89, 0x47, 0x10, 0xC3, 0x71, 0xE9, 0x07, 0x6C, + 0x1D, 0x49, 0xFB, 0xAE, 0x51, 0x27, 0x69, 0x34, + 0xF2, 0xAD, 0x78, 0x77, 0x89, 0xF4, 0x2D, 0x0F, + 0xA0, 0xB4, 0xC9, 0x39, 0x85, 0x5D, 0x42, 0x12, + 0x09, 0x6F, 0x70, 0x28, 0x0A, 0x4E, 0xAE, 0x7C, + 0x8A, 0x27, 0xD9, 0xC8, 0xD0, 0x77, 0x2E, 0x65, + }, + // dq + []byte{ + 0x8C, 0xB6, 0x85, 0x7A, 0x7B, 0xD5, 0x46, 0x5F, + 0x80, 0x04, 0x7E, 0x9B, 0x87, 0xBC, 0x00, 0x27, + 0x31, 0x84, 0x05, 0x81, 0xE0, 0x62, 0x61, 0x39, + 0x01, 0x2A, 0x5B, 0x50, 0x5F, 0x0A, 0x33, 0x84, + 0x7E, 0xB7, 0xB8, 0xC3, 0x28, 0x99, 0x49, 0xAD, + 0x48, 0x6F, 0x3B, 0x4B, 0x3D, 0x53, 0x9A, 0xB5, + 0xDA, 0x76, 0x30, 0x21, 0xCB, 0xC8, 0x2C, 0x1B, + 0xA2, 0x34, 0xA5, 0x66, 0x8D, 0xED, 0x08, 0x01, + 0xB8, 0x59, 0xF3, 0x43, 0xF1, 0xCE, 0x93, 0x04, + 0xE6, 0xFA, 0xA2, 0xB0, 0x02, 0xCA, 0xD9, 0xB7, + 0x8C, 0xDE, 0x5C, 0xDC, 0x2C, 0x1F, 0xB4, 0x17, + 0x1C, 0x42, 0x42, 0x16, 0x70, 0xA6, 0xAB, 0x0F, + 0x50, 0xCC, 0x4A, 0x19, 0x4E, 0xB3, 0x6D, 0x1C, + 0x91, 0xE9, 0x35, 0xBA, 0x01, 0xB9, 0x59, 0xD8, + 0x72, 0x8B, 0x9E, 0x64, 0x42, 0x6B, 0x3F, 0xC3, + 0xA7, 0x50, 0x6D, 0xEB, 0x52, 0x39, 0xA8, 0xA7, + }, + // iq (aka u) + []byte{ + 0x0A, 0x81, 0xD8, 0xA6, 0x18, 0x31, 0x4A, 0x80, + 0x3A, 0xF6, 0x1C, 0x06, 0x71, 0x1F, 0x2C, 0x39, + 0xB2, 0x66, 0xFF, 0x41, 0x4D, 0x53, 0x47, 0x6D, + 0x1D, 0xA5, 0x2A, 0x43, 0x18, 0xAA, 0xFE, 0x4B, + 0x96, 0xF0, 0xDA, 0x07, 0x15, 0x5F, 0x8A, 0x51, + 0x34, 0xDA, 0xB8, 0x8E, 0xE2, 0x9E, 0x81, 0x68, + 0x07, 0x6F, 0xCD, 0x78, 0xCA, 0x79, 0x1A, 0xC6, + 0x34, 0x42, 0xA8, 0x1C, 0xD0, 0x69, 0x39, 0x27, + 0xD8, 0x08, 0xE3, 0x35, 0xE8, 0xD8, 0xCB, 0xF2, + 0x12, 0x19, 0x07, 0x50, 0x9A, 0x57, 0x75, 0x9B, + 0x4F, 0x9A, 0x18, 0xFA, 0x3A, 0x7B, 0x33, 0x37, + 0x79, 0xED, 0xDE, 0x7A, 0x45, 0x93, 0x84, 0xF8, + 0x44, 0x4A, 0xDA, 0xEC, 0xFF, 0xEC, 0x95, 0xFD, + 0x55, 0x2B, 0x0C, 0xFC, 0xB6, 0xC7, 0xF6, 0x92, + 0x62, 0x6D, 0xDE, 0x1E, 0xF2, 0x68, 0xA4, 0x0D, + 0x2F, 0x67, 0xB5, 0xC8, 0xAA, 0x38, 0x7F, 0xF7, + }, + ) +} \ No newline at end of file diff --git a/core/encoding/cbor/coding.odin b/core/encoding/cbor/coding.odin index bfeb147c4..537368cf4 100644 --- a/core/encoding/cbor/coding.odin +++ b/core/encoding/cbor/coding.odin @@ -411,12 +411,13 @@ _decode_bytes :: proc(d: Decoder, add: Add, type: Major = .Bytes, allocator := c io.copy_n(buf_stream, d.reader, i64(n)) or_return } - v = buf.buf[:] - // Write zero byte so this can be converted to cstring. strings.write_byte(&buf, 0) if .Shrink_Excess in d.flags { shrink(&buf.buf) } + + v = buf.buf[:len(buf.buf)-1] + return } @@ -884,4 +885,4 @@ _encode_deterministic_f64 :: proc(w: io.Writer, v: f64) -> io.Error { } return _encode_f64_exact(w, v) -} \ No newline at end of file +} diff --git a/core/encoding/cbor/unmarshal.odin b/core/encoding/cbor/unmarshal.odin index 6925ce1db..a4c7176ec 100644 --- a/core/encoding/cbor/unmarshal.odin +++ b/core/encoding/cbor/unmarshal.odin @@ -509,7 +509,7 @@ _unmarshal_array :: proc(d: Decoder, v: any, ti: ^reflect.Type_Info, hdr: Header case reflect.Type_Info_Slice: length, scap := err_conv(_decode_len_container(d, add)) or_return - data := mem.alloc_bytes_non_zeroed(t.elem.size * scap, t.elem.align, allocator=allocator, loc=loc) or_return + data := mem.alloc_bytes(t.elem.size * scap, t.elem.align, allocator=allocator, loc=loc) or_return defer if err != nil { mem.free_bytes(data, allocator=allocator, loc=loc) } da := mem.Raw_Dynamic_Array{raw_data(data), 0, scap, context.allocator } @@ -529,7 +529,7 @@ _unmarshal_array :: proc(d: Decoder, v: any, ti: ^reflect.Type_Info, hdr: Header case reflect.Type_Info_Dynamic_Array: length, scap := err_conv(_decode_len_container(d, add)) or_return - data := mem.alloc_bytes_non_zeroed(t.elem.size * scap, t.elem.align, loc=loc) or_return + data := mem.alloc_bytes(t.elem.size * scap, t.elem.align, loc=loc) or_return defer if err != nil { mem.free_bytes(data, allocator=allocator, loc=loc) } raw := (^mem.Raw_Dynamic_Array)(v.data) diff --git a/core/encoding/entity/entity.odin b/core/encoding/entity/entity.odin index a756bdf39..3f3d41953 100644 --- a/core/encoding/entity/entity.odin +++ b/core/encoding/entity/entity.odin @@ -146,6 +146,7 @@ decode_xml :: proc(input: string, options := XML_Decode_Options{}, allocator := for i in 0.. Regist return .None } +@(require_results) marshal :: proc(v: any, opt: Marshal_Options = {}, allocator := context.allocator, loc := #caller_location) -> (data: []byte, err: Marshal_Error) { b := strings.builder_make(allocator, loc) defer if err != nil { diff --git a/core/encoding/json/match.odin b/core/encoding/json/match.odin new file mode 100644 index 000000000..d9598c626 --- /dev/null +++ b/core/encoding/json/match.odin @@ -0,0 +1,68 @@ +package encoding_json + +Match_Key_Variant :: union #no_nil { + int, // Index + string, // Key +} + +Match_Error :: enum { + None, + Invalid_Argument, + Invalid_Type_For_Index, + Invalid_Type_For_Key, + Key_Not_Found, + Out_Of_Bounds_Index, +} + +Match_Flags :: distinct bit_set[Match_Flag] +Match_Flag :: enum { + Ignore_Key_Not_Found, + Allow_String_Indexing_By_Byte, +} + +@(require_results) +match :: proc(value: Value, args: ..Match_Key_Variant, flags: Match_Flags = nil) -> (found: Value, err: Match_Error) { + found = value + arg_loop: for arg in args { + switch k in arg { + case int: + #partial switch v in found { + case Array: + if 0 <= k && k < len(v) { + found = v[k] + continue arg_loop + } + err = .Out_Of_Bounds_Index + return + case String: + if .Allow_String_Indexing_By_Byte in flags { + if 0 <= k && k < len(v) { + found = Integer(v[k]) + continue arg_loop + } + err = .Out_Of_Bounds_Index + return + } + } + err = .Invalid_Type_For_Index + return + case string: + v, ok := found.(Object) + if !ok { + err = .Invalid_Type_For_Key + return + } + vfound, vok := v[k] + if vok || (.Ignore_Key_Not_Found in flags) { + found = vfound + continue arg_loop + } + err = .Key_Not_Found + return + case: + err = .Invalid_Argument + return + } + } + return +} \ No newline at end of file diff --git a/core/encoding/json/parser.odin b/core/encoding/json/parser.odin index 58a47d308..91bc42e4c 100644 --- a/core/encoding/json/parser.odin +++ b/core/encoding/json/parser.odin @@ -14,9 +14,16 @@ Parser :: struct { parse_integers: bool, } -make_parser :: proc(data: []byte, spec := DEFAULT_SPECIFICATION, parse_integers := false, allocator := context.allocator) -> Parser { +make_parser :: proc{ + make_parser_from_bytes, + make_parser_from_string, +} + +@(require_results) +make_parser_from_bytes :: proc(data: []byte, spec := DEFAULT_SPECIFICATION, parse_integers := false, allocator := context.allocator) -> Parser { return make_parser_from_string(string(data), spec, parse_integers, allocator) } +@(require_results) make_parser_from_string :: proc(data: string, spec := DEFAULT_SPECIFICATION, parse_integers := false, allocator := context.allocator) -> Parser { p: Parser p.tok = make_tokenizer(data, spec, parse_integers) @@ -27,11 +34,18 @@ make_parser_from_string :: proc(data: string, spec := DEFAULT_SPECIFICATION, par return p } +parse :: proc{ + parse_bytes, + parse_string, +} -parse :: proc(data: []byte, spec := DEFAULT_SPECIFICATION, parse_integers := false, allocator := context.allocator, loc := #caller_location) -> (Value, Error) { + +@(require_results) +parse_bytes :: proc(data: []byte, spec := DEFAULT_SPECIFICATION, parse_integers := false, allocator := context.allocator, loc := #caller_location) -> (Value, Error) { return parse_string(string(data), spec, parse_integers, allocator, loc) } +@(require_results) parse_string :: proc(data: string, spec := DEFAULT_SPECIFICATION, parse_integers := false, allocator := context.allocator, loc := #caller_location) -> (Value, Error) { context.allocator = allocator p := make_parser_from_string(data, spec, parse_integers, allocator) @@ -51,6 +65,7 @@ parse_string :: proc(data: string, spec := DEFAULT_SPECIFICATION, parse_integers return parse_object(&p, loc) } +@(require_results) token_end_pos :: proc(tok: Token) -> Pos { end := tok.pos end.offset += len(tok.text) @@ -65,6 +80,7 @@ advance_token :: proc(p: ^Parser) -> (Token, Error) { } +@(require_results) allow_token :: proc(p: ^Parser, kind: Token_Kind) -> bool { if p.curr_token.kind == kind { advance_token(p) @@ -73,6 +89,7 @@ allow_token :: proc(p: ^Parser, kind: Token_Kind) -> bool { return false } +@(require_results) expect_token :: proc(p: ^Parser, kind: Token_Kind) -> Error { prev := p.curr_token advance_token(p) @@ -83,6 +100,7 @@ expect_token :: proc(p: ^Parser, kind: Token_Kind) -> Error { } +@(require_results) parse_colon :: proc(p: ^Parser) -> (err: Error) { colon_err := expect_token(p, .Colon) if colon_err == nil { @@ -91,6 +109,7 @@ parse_colon :: proc(p: ^Parser) -> (err: Error) { return .Expected_Colon_After_Key } +@(require_results) parse_comma :: proc(p: ^Parser) -> (do_break: bool) { switch p.spec { case .JSON5, .MJSON: @@ -106,6 +125,7 @@ parse_comma :: proc(p: ^Parser) -> (do_break: bool) { return false } +@(require_results) parse_value :: proc(p: ^Parser, loc := #caller_location) -> (value: Value, err: Error) { err = .None token := p.curr_token @@ -176,6 +196,7 @@ parse_value :: proc(p: ^Parser, loc := #caller_location) -> (value: Value, err: return } +@(require_results) parse_array :: proc(p: ^Parser, loc := #caller_location) -> (value: Value, err: Error) { err = .None expect_token(p, .Open_Bracket) or_return @@ -203,7 +224,7 @@ parse_array :: proc(p: ^Parser, loc := #caller_location) -> (value: Value, err: return } -@(private) +@(private, require_results) bytes_make :: proc(size, alignment: int, allocator: mem.Allocator, loc := #caller_location) -> (bytes: []byte, err: Error) { b, berr := mem.alloc_bytes(size, alignment, allocator, loc) if berr != nil { @@ -217,6 +238,7 @@ bytes_make :: proc(size, alignment: int, allocator: mem.Allocator, loc := #calle return } +@(require_results) clone_string :: proc(s: string, allocator: mem.Allocator, loc := #caller_location) -> (str: string, err: Error) { n := len(s) b := bytes_make(n+1, 1, allocator, loc) or_return @@ -228,6 +250,7 @@ clone_string :: proc(s: string, allocator: mem.Allocator, loc := #caller_locatio return } +@(require_results) parse_object_key :: proc(p: ^Parser, key_allocator: mem.Allocator, loc := #caller_location) -> (key: string, err: Error) { tok := p.curr_token if p.spec != .JSON { @@ -242,6 +265,7 @@ parse_object_key :: proc(p: ^Parser, key_allocator: mem.Allocator, loc := #calle return unquote_string(tok, p.spec, key_allocator, loc) } +@(require_results) parse_object_body :: proc(p: ^Parser, end_token: Token_Kind, loc := #caller_location) -> (obj: Object, err: Error) { obj = make(Object, allocator=p.allocator, loc=loc) @@ -282,6 +306,7 @@ parse_object_body :: proc(p: ^Parser, end_token: Token_Kind, loc := #caller_loca return obj, .None } +@(require_results) parse_object :: proc(p: ^Parser, loc := #caller_location) -> (value: Value, err: Error) { expect_token(p, .Open_Brace) or_return obj := parse_object_body(p, .Close_Brace, loc) or_return @@ -291,6 +316,7 @@ parse_object :: proc(p: ^Parser, loc := #caller_location) -> (value: Value, err: // IMPORTANT NOTE(bill): unquote_string assumes a mostly valid string +@(require_results) unquote_string :: proc(token: Token, spec: Specification, allocator := context.allocator, loc := #caller_location) -> (value: string, err: Error) { get_u2_rune :: proc(s: string) -> rune { if len(s) < 4 || s[0] != '\\' || s[1] != 'x' { diff --git a/core/encoding/json/tokenizer.odin b/core/encoding/json/tokenizer.odin index ad928b7d9..153c41b29 100644 --- a/core/encoding/json/tokenizer.odin +++ b/core/encoding/json/tokenizer.odin @@ -49,11 +49,12 @@ Tokenizer :: struct { curr_line_offset: int, spec: Specification, parse_integers: bool, - insert_comma: bool, + insert_comma: bool, } +@(require_results) make_tokenizer :: proc(data: string, spec := DEFAULT_SPECIFICATION, parse_integers := false) -> Tokenizer { t := Tokenizer{pos = {line=1}, data = data, spec = spec, parse_integers = parse_integers} next_rune(&t) @@ -78,6 +79,7 @@ next_rune :: proc(t: ^Tokenizer) -> rune #no_bounds_check { } +@(require_results) get_token :: proc(t: ^Tokenizer) -> (token: Token, err: Error) { skip_digits :: proc(t: ^Tokenizer) { for t.offset < len(t.data) { @@ -101,6 +103,7 @@ get_token :: proc(t: ^Tokenizer) -> (token: Token, err: Error) { } } + @(require_results) scan_escape :: proc(t: ^Tokenizer) -> bool { switch t.r { case '"', '\'', '\\', '/', 'b', 'n', 'r', 't', 'f': @@ -125,7 +128,7 @@ get_token :: proc(t: ^Tokenizer) -> (token: Token, err: Error) { return false } - skip_whitespace :: proc(t: ^Tokenizer, on_newline: bool) -> rune { + skip_whitespace :: proc(t: ^Tokenizer, on_newline: bool) { loop: for t.offset < len(t.data) { switch t.r { case ' ', '\t', '\v', '\f', '\r': @@ -149,7 +152,7 @@ get_token :: proc(t: ^Tokenizer) -> (token: Token, err: Error) { break loop } } - return t.r + return } skip_to_next_line :: proc(t: ^Tokenizer) { @@ -310,7 +313,7 @@ get_token :: proc(t: ^Tokenizer) -> (token: Token, err: Error) { break } if r == '\\' { - scan_escape(t) + _ = scan_escape(t) } } @@ -347,10 +350,8 @@ get_token :: proc(t: ^Tokenizer) -> (token: Token, err: Error) { case '*': // None-nested multi-line comments for t.offset < len(t.data) { - next_rune(t) - if t.r == '*' { - next_rune(t) - if t.r == '/' { + if next_rune(t) == '*' { + if next_rune(t) == '/' { next_rune(t) return get_token(t) } @@ -385,6 +386,7 @@ get_token :: proc(t: ^Tokenizer) -> (token: Token, err: Error) { +@(require_results) is_valid_number :: proc(str: string, spec: Specification) -> bool { s := str if s == "" { @@ -473,6 +475,7 @@ is_valid_number :: proc(str: string, spec: Specification) -> bool { return s == "" } +@(require_results) is_valid_string_literal :: proc(str: string, spec: Specification) -> bool { s := str if len(s) < 2 { diff --git a/core/encoding/json/types.odin b/core/encoding/json/types.odin index 6b6d8aae9..66ff5c700 100644 --- a/core/encoding/json/types.odin +++ b/core/encoding/json/types.odin @@ -112,6 +112,7 @@ destroy_value :: proc(value: Value, allocator := context.allocator, loc := #call } } +@(require_results) clone_value :: proc(value: Value, allocator := context.allocator) -> Value { value := value context.allocator = allocator diff --git a/core/encoding/json/unmarshal.odin b/core/encoding/json/unmarshal.odin index d3cb083d1..0f8c19b5d 100644 --- a/core/encoding/json/unmarshal.odin +++ b/core/encoding/json/unmarshal.odin @@ -635,7 +635,7 @@ unmarshal_object :: proc(p: ^Parser, v: any, end_token: Token_Kind) -> (err: Unm defer p.allocator = allocator p.allocator = mem.nil_allocator() - parse_value(p) or_return + _ = parse_value(p) or_return if parse_comma(p) { break struct_loop } diff --git a/core/encoding/json/validator.odin b/core/encoding/json/validator.odin index e90270335..e8b5cc49d 100644 --- a/core/encoding/json/validator.odin +++ b/core/encoding/json/validator.odin @@ -3,6 +3,7 @@ package encoding_json import "core:mem" // NOTE(bill): is_valid will not check for duplicate keys +@(require_results) is_valid :: proc(data: []byte, spec := DEFAULT_SPECIFICATION, parse_integers := false) -> bool { p := make_parser(data, spec, parse_integers, mem.nil_allocator()) @@ -21,6 +22,7 @@ is_valid :: proc(data: []byte, spec := DEFAULT_SPECIFICATION, parse_integers := return validate_object(&p) } +@(require_results) validate_object_key :: proc(p: ^Parser) -> bool { if p.spec != .JSON { if allow_token(p, .Ident) { @@ -31,6 +33,7 @@ validate_object_key :: proc(p: ^Parser) -> bool { return err == .None } +@(require_results) validate_object_body :: proc(p: ^Parser, end_token: Token_Kind) -> bool { for p.curr_token.kind != end_token { if !validate_object_key(p) { @@ -48,6 +51,7 @@ validate_object_body :: proc(p: ^Parser, end_token: Token_Kind) -> bool { return true } +@(require_results) validate_object :: proc(p: ^Parser) -> bool { if err := expect_token(p, .Open_Brace); err != .None { return false @@ -61,6 +65,7 @@ validate_object :: proc(p: ^Parser) -> bool { return true } +@(require_results) validate_array :: proc(p: ^Parser) -> bool { if err := expect_token(p, .Open_Bracket); err != .None { return false @@ -83,6 +88,7 @@ validate_array :: proc(p: ^Parser) -> bool { return true } +@(require_results) validate_value :: proc(p: ^Parser) -> bool { token := p.curr_token diff --git a/core/math/linalg/general.odin b/core/math/linalg/general.odin index 7013de244..90ff301bd 100644 --- a/core/math/linalg/general.odin +++ b/core/math/linalg/general.odin @@ -244,27 +244,18 @@ quaternion_mul_quaternion :: proc "contextless" (q1, q2: $Q) -> Q where IS_QUATE @(require_results) quaternion64_mul_vector3 :: proc "contextless" (q: $Q/quaternion64, v: $V/[3]$F/f16) -> V { - q := transmute(runtime.Raw_Quaternion64_Vector_Scalar)q - v := v - - t := cross(2*q.vector, v) - return V(v + q.scalar*t + cross(q.vector, t)) + t := cross(2*q.xyz, v) + return V(v + q.w*t + cross(q.xyz, t)) } @(require_results) quaternion128_mul_vector3 :: proc "contextless" (q: $Q/quaternion128, v: $V/[3]$F/f32) -> V { - q := transmute(runtime.Raw_Quaternion128_Vector_Scalar)q - v := v - - t := cross(2*q.vector, v) - return V(v + q.scalar*t + cross(q.vector, t)) + t := cross(2*q.xyz, v) + return V(v + q.w*t + cross(q.xyz, t)) } @(require_results) quaternion256_mul_vector3 :: proc "contextless" (q: $Q/quaternion256, v: $V/[3]$F/f64) -> V { - q := transmute(runtime.Raw_Quaternion256_Vector_Scalar)q - v := v - - t := cross(2*q.vector, v) - return V(v + q.scalar*t + cross(q.vector, t)) + t := cross(2*q.xyz, v) + return V(v + q.w*t + cross(q.xyz, t)) } quaternion_mul_vector3 :: proc{quaternion64_mul_vector3, quaternion128_mul_vector3, quaternion256_mul_vector3} diff --git a/core/math/rand/rand.odin b/core/math/rand/rand.odin index 738de07e5..bb3dc7556 100644 --- a/core/math/rand/rand.odin +++ b/core/math/rand/rand.odin @@ -1044,8 +1044,8 @@ Example: Possible Output: - [7201011, 3, 9123, 231131] - [19578, 910081, 131, 7] + [3, 2, 0, 1] + [1, 0, 2, 3] */ @(require_results) diff --git a/core/odin/parser/parser.odin b/core/odin/parser/parser.odin index 5e681728d..2ad291aef 100644 --- a/core/odin/parser/parser.odin +++ b/core/odin/parser/parser.odin @@ -381,6 +381,9 @@ advance_token :: proc(p: ^Parser) -> tokenizer.Token { #partial switch p.curr_tok.kind { case .Comment: consume_comment_groups(p, prev) + if p.curr_tok.kind == .Semicolon && p.expr_level > 0 && p.curr_tok.text == "\n" { + advance_token(p) + } case .Semicolon: if p.expr_level > 0 && p.curr_tok.text == "\n" { advance_token(p) @@ -3622,7 +3625,9 @@ parse_binary_expr :: proc(p: ^Parser, lhs: bool, prec_in: int) -> ^ast.Expr { case .If, .When: if p.prev_tok.pos.line < op.pos.line { // NOTE(bill): Check to see if the `if` or `when` is on the same line of the `lhs` condition - break loop + if p.expr_level <= 0 { + break loop + } } } diff --git a/core/os/file.odin b/core/os/file.odin index 61582c528..7e70332ce 100644 --- a/core/os/file.odin +++ b/core/os/file.odin @@ -536,7 +536,7 @@ is_directory :: proc(path: string) -> bool { } /* - `copy_file` copies a file from `src_path` to `dst_path` and returns an error if any was encountered. + `is_tty` returns `true` if `f` is a TTY, `false` if not. */ @(require_results) is_tty :: proc "contextless" (f: ^File) -> bool { diff --git a/core/sys/info/platform_windows.odin b/core/sys/info/platform_windows.odin index fbaf071b9..8e5c96507 100644 --- a/core/sys/info/platform_windows.odin +++ b/core/sys/info/platform_windows.odin @@ -263,8 +263,8 @@ _ram_stats :: proc "contextless" () -> (total_ram, free_ram, total_swap, free_sw total_ram = i64(state.ullTotalPhys) free_ram = i64(state.ullAvailPhys) - total_swap = i64(state.ullTotalPageFil) - free_swap = i64(state.ullAvailPageFil) + total_swap = i64(state.ullTotalPageFile) + free_swap = i64(state.ullAvailPageFile) ok = true return diff --git a/core/sys/windows/types.odin b/core/sys/windows/types.odin index 0f80e0b18..8d2e4bf53 100644 --- a/core/sys/windows/types.odin +++ b/core/sys/windows/types.odin @@ -4802,8 +4802,8 @@ MEMORYSTATUSEX :: struct { dwMemoryLoad: DWORD, ullTotalPhys: DWORDLONG, ullAvailPhys: DWORDLONG, - ullTotalPageFil: DWORDLONG, - ullAvailPageFil: DWORDLONG, + ullTotalPageFile: DWORDLONG, + ullAvailPageFile: DWORDLONG, ullTotalVirtual: DWORDLONG, ullAvailVirtual: DWORDLONG, ullAvailExtendedVirtual: DWORDLONG, diff --git a/core/time/time.odin b/core/time/time.odin index 438d74569..00549558e 100644 --- a/core/time/time.odin +++ b/core/time/time.odin @@ -65,7 +65,7 @@ Capable of representing any time within the following range: - `max: 2262-04-11 23:47:16.854775807 +0000 UTC` */ Time :: struct { - _nsec: i64, // Measured in UNIX nanonseconds + _nsec: i64, // Measured in UNIX nanoseconds } /* diff --git a/examples/all/all_js.odin b/examples/all/all_js.odin index 8dbc320d0..10d2595a0 100644 --- a/examples/all/all_js.odin +++ b/examples/all/all_js.odin @@ -49,6 +49,7 @@ package all @(require) import "core:crypto/pbkdf2" @(require) import "core:crypto/poly1305" @(require) import "core:crypto/ristretto255" +@(require) import "core:crypto/rsa" @(require) import "core:crypto/sha2" @(require) import "core:crypto/sha3" @(require) import "core:crypto/shake" diff --git a/examples/all/all_main.odin b/examples/all/all_main.odin index ad7c8c14c..2df886465 100644 --- a/examples/all/all_main.odin +++ b/examples/all/all_main.odin @@ -54,6 +54,7 @@ package all @(require) import "core:crypto/pbkdf2" @(require) import "core:crypto/poly1305" @(require) import "core:crypto/ristretto255" +@(require) import "core:crypto/rsa" @(require) import "core:crypto/sha2" @(require) import "core:crypto/sha3" @(require) import "core:crypto/shake" diff --git a/examples/all/all_vendor_windows.odin b/examples/all/all_vendor_windows.odin index 8a0c29eaf..02db53b77 100644 --- a/examples/all/all_vendor_windows.odin +++ b/examples/all/all_vendor_windows.odin @@ -6,5 +6,6 @@ package all @(require) import "vendor:wgpu/sdl2glue" @(require) import "vendor:wgpu" @(require) import "vendor:box2d" +@(require) import "vendor:box3d" @(require) import "vendor:windows/GameInput" @(require) import "vendor:windows/XAudio2" \ No newline at end of file diff --git a/examples/demo/demo.odin b/examples/demo/demo.odin index a12217548..544327686 100644 --- a/examples/demo/demo.odin +++ b/examples/demo/demo.odin @@ -670,7 +670,7 @@ union_type :: proc() { { // NOTE(bill): A union can be used to achieve something similar. Instead // of embedding the base data into the derived types, the derived data - // in embedded into the base type. Below is the same example of the + // is embedded into the base type. Below is the same example of the // basic game Entity but using an union. Entity :: struct { @@ -757,7 +757,7 @@ using_statement :: proc() { // `using` as a struct field modifier remains available always fmt.println("\n# using statement") - // using can used to bring entities declared in a scope/namespace + // using can be used to bring entities declared in a scope/namespace // into the current scope. This can be applied to import names, struct // fields, procedure fields, and struct values. @@ -768,7 +768,7 @@ using_statement :: proc() { orientation: quaternion128, } - // It can used like this: + // It can be used like this: foo0 :: proc(entity: ^Entity) { fmt.println(entity.position.x, entity.position.y, entity.position.z) } diff --git a/src/big_int.cpp b/src/big_int.cpp index a8ea2079b..f59dccf24 100644 --- a/src/big_int.cpp +++ b/src/big_int.cpp @@ -353,9 +353,15 @@ gb_internal void big_int_shl(BigInt *dst, BigInt const *x, BigInt const *y) { gb_internal void big_int_shr(BigInt *dst, BigInt const *x, BigInt const *y) { u32 yy = mp_get_u32(y); - BigInt d = {}; - mp_div_2d(x, yy, dst, &d); - big_int_dealloc(&d); + + BigInt rem = {}; + defer (mp_clear(&rem)); + + mp_div_2d(x, yy, dst, &rem); + + if (mp_isneg(x) && !mp_iszero(&rem)) { + mp_sub_d(dst, 1, dst); + } } gb_internal void big_int_mul_u64(BigInt *dst, BigInt const *x, u64 y) { diff --git a/src/build_settings.cpp b/src/build_settings.cpp index e6267bb3d..d767b4890 100644 --- a/src/build_settings.cpp +++ b/src/build_settings.cpp @@ -171,6 +171,7 @@ enum Subtarget : u32 { Subtarget_iPhone, Subtarget_iPhoneSimulator, Subtarget_Android, + Subtarget_Playdate, Subtarget_COUNT, Subtarget_Invalid, // NOTE(harold): Must appear after _COUNT as this is not a real subtarget @@ -181,6 +182,7 @@ gb_global String subtarget_strings[Subtarget_COUNT] = { str_lit("iphone"), str_lit("iphonesimulator"), str_lit("android"), + str_lit("playdate"), }; @@ -879,7 +881,7 @@ gb_global TargetMetrics target_freestanding_arm32 = { TargetOs_freestanding, TargetArch_arm32, 4, 4, 8, 16, - str_lit("arm-unknown-unknown-gnueabihf"), + str_lit("arm-none-eabihf"), }; gb_global TargetMetrics target_freestanding_riscv64 = { TargetOs_freestanding, @@ -1957,6 +1959,16 @@ gb_internal void init_build_context(TargetMetrics *cross_target, Subtarget subta bc->no_plt = LLVM_VERSION_MAJOR >= 19; break; } + } else if (subtarget == Subtarget_Playdate) { + // Playdate uses the cortex-m7 as well and an FPU, requiring thumb instructions and hard floats. + bc->microarch = str_lit("cortex-m7"); + bc->metrics.target_triplet = str_lit("thumbv7em-none-eabihf"); + // Remove MOVW/MOVT instructions as playdate only handles R_ARM_ABS32 relocations. + if(bc->target_features_string.len > 0) { + bc->target_features_string = concatenate_strings(permanent_allocator(), bc->target_features_string, str_lit(",no-movt")); + } else { + bc->target_features_string = str_lit("no-movt"); + } } if (metrics->os == TargetOs_windows || @@ -2397,12 +2409,32 @@ gb_internal bool init_build_paths(String init_filename) { GB_PANIC("Unhandled build mode/target combination.\n"); } + bool output_should_be_directory = false; + if (bc->out_filepath.len > 0) { bc->build_paths[BuildPath_Output] = path_from_string(ha, bc->out_filepath); - if (build_context.metrics.os == TargetOs_windows) { - String output_file = path_to_string(ha, bc->build_paths[BuildPath_Output]); - defer (gb_free(ha, output_file.text)); - if (path_is_directory(bc->build_paths[BuildPath_Output])) { + bool output_is_directory = path_is_directory(bc->build_paths[BuildPath_Output]); + + String output_file = path_to_string(ha, bc->build_paths[BuildPath_Output]); + defer (gb_free(ha, output_file.text)); + + // NOTE(Jeroen): For LLVM-IR, we want `-out` to specify a directory. + // For other outputs we expect it to be a file path. + if (build_context.build_mode == BuildMode_LLVM_IR) { + if (!output_is_directory) { + gb_printf_err("Output path %.*s should be a directory for LLVM-IR output.\n", LIT(output_file)); + return false; + } + output_should_be_directory = true; + + } else if (build_context.build_mode == BuildMode_Object) { + // Both directory or filename prefix allowed + + } else if (build_context.build_mode == BuildMode_Assembly) { + // Both directory or filename prefix allowed + + } else if (build_context.metrics.os == TargetOs_windows) { + if (output_is_directory) { gb_printf_err("Output path %.*s is a directory.\n", LIT(output_file)); return false; } else if (bc->build_paths[BuildPath_Output].ext.len == 0) { @@ -2513,7 +2545,11 @@ gb_internal bool init_build_paths(String init_filename) { // Do we have an extension? We might not if the output filename was supplied. if (bc->build_paths[BuildPath_Output].ext.len == 0) { if (build_context.metrics.os == TargetOs_windows || is_arch_wasm() || build_context.build_mode != BuildMode_Executable) { - bc->build_paths[BuildPath_Output].ext = copy_string(ha, output_extension); + + // NOTE(Jeroen): If build mode is LLVM_IR and a custom output was set + if (!output_should_be_directory) { + bc->build_paths[BuildPath_Output].ext = copy_string(ha, output_extension); + } } } @@ -2521,7 +2557,7 @@ gb_internal bool init_build_paths(String init_filename) { defer (gb_free(ha, output_file.text)); // Check if output path is a directory. - if (path_is_directory(bc->build_paths[BuildPath_Output])) { + if (!output_should_be_directory && path_is_directory(bc->build_paths[BuildPath_Output])) { gb_printf_err("Output path %.*s is a directory.\n", LIT(output_file)); return false; } diff --git a/src/check_builtin.cpp b/src/check_builtin.cpp index 90048f004..e87c75e88 100644 --- a/src/check_builtin.cpp +++ b/src/check_builtin.cpp @@ -765,8 +765,10 @@ gb_internal bool check_builtin_c_procedure(CheckerContext *c, Operand *operand, return false; } + c->allow_c_vararg_param = true; Operand args = {}; check_expr(c, &args, ce->args[1]); + c->allow_c_vararg_param = false; if (list.mode == Addressing_Invalid) { return false; } diff --git a/src/check_expr.cpp b/src/check_expr.cpp index 5a95a5dbb..7b95da1ad 100644 --- a/src/check_expr.cpp +++ b/src/check_expr.cpp @@ -1142,7 +1142,7 @@ gb_internal void check_assignment(CheckerContext *c, Operand *operand, Type *typ if (operand->mode == Addressing_Type && is_type_typeid(type)) { add_type_info_type(c, operand->type); - add_type_and_value(c, operand->expr, Addressing_Value, type, exact_value_typeid(operand->type)); + add_type_and_value(c, operand->expr, Addressing_Constant, type, exact_value_typeid(operand->type)); return; } @@ -1968,6 +1968,14 @@ gb_internal Entity *check_ident(CheckerContext *c, Operand *o, Ast *n, Type *nam break; case Entity_Variable: + if ((e->flags & EntityFlag_CVarArg) && !c->allow_c_vararg_param) { + ERROR_BLOCK(); + error(o->expr, "'#c_vararg' parameter '%.*s' cannot be used directly", LIT(name)); + error_line("\tSuggestion: use c_va_start to convert C varargs to c_va_list\n"); + o->mode = Addressing_Invalid; + o->type = t_invalid; + return e; + } e->flags |= EntityFlag_Used; if (type == t_invalid) { o->type = t_invalid; @@ -3926,6 +3934,11 @@ gb_internal void check_cast(CheckerContext *c, Operand *x, Type *type, bool forb } } + // In this case, the cast involves array programming + // so the operand needs to be a computed value + if (!is_type_array_like(x->type) && is_type_array_like(type)) { + x->mode = Addressing_Value; + } x->type = type; } @@ -5161,16 +5174,12 @@ gb_internal void convert_to_typed(CheckerContext *c, Operand *operand, Type *tar if (valid_count == 1) { Type *new_type = t->Union.variants[first_success_index]; - target_type = new_type; - if (is_type_union(new_type)) { - convert_to_typed(c, operand, new_type); - break; - } - operand->type = new_type; if (operand->mode != Addressing_Constant || - !elem_type_can_be_constant(operand->type)) { + !elem_type_can_be_constant(new_type)) { operand->mode = Addressing_Value; } + convert_to_typed(c, operand, new_type); + target_type = new_type; break; } else if (valid_count > 1) { ERROR_BLOCK(); @@ -5204,7 +5213,7 @@ gb_internal void convert_to_typed(CheckerContext *c, Operand *operand, Type *tar operand->mode = Addressing_Invalid; convert_untyped_error(c, operand, target_type, true); if (count > 0) { - error_line("'%s' is a union which only excepts the following types:\n", type_str); + error_line("'%s' is a union which only accepts the following types:\n", type_str); error_line("\t"); for (i32 i = 0; i < count; i++) { @@ -5363,7 +5372,8 @@ gb_internal bool check_index_value(CheckerContext *c, Type *main_type, bool open TEMPORARY_ALLOCATOR_GUARD(); String idx_str = big_int_to_string(temporary_allocator(), &i); gbString expr_str = expr_to_string(operand.expr, temporary_allocator()); - error(operand.expr, "Index '%s' is out of bounds range 0..<%lld, got %.*s", expr_str, max_count, LIT(idx_str)); + char range_type = open_range ? '=' : '<'; + error(operand.expr, "Index '%s' is out of bounds range 0..%c%lld, got %.*s", expr_str, range_type, max_count, LIT(idx_str)); return false; } @@ -5819,7 +5829,7 @@ gb_internal Entity *check_selector(CheckerContext *c, Operand *operand, Ast *nod if (e != nullptr && (e->kind == Entity_Procedure || e->kind == Entity_ProcGroup) && selector->kind == Ast_Ident) { gbString sel_str = expr_to_string(selector); - error(node, "'%s' is not declared by by '%.*s'", sel_str, LIT(e->token.string)); + error(node, "'%s' is not declared by '%.*s'", sel_str, LIT(e->token.string)); gb_string_free(sel_str); operand->mode = Addressing_Invalid; operand->expr = node; @@ -6774,7 +6784,7 @@ gb_internal CallArgumentError check_call_arguments_internal(CheckerContext *c, A if (!check_is_assignable_to_with_score(c, o, param_type, &s, param_is_variadic, allow_array_programming)) { bool ok = false; if (e && (e->flags & EntityFlag_AnyInt)) { - if (is_type_integer(param_type) && (is_type_integer(o->type) || is_type_enum(o->type))) { + if (o->mode != Addressing_Type && is_type_integer(param_type) && (is_type_integer(o->type) || is_type_enum(o->type))) { ok = check_is_castable_to(c, o, param_type); } } @@ -9898,6 +9908,7 @@ gb_internal ExprKind check_or_else_expr(CheckerContext *c, Operand *o, Ast *node if (is_diverging_expr(y.expr)) { // Allow y.mode = Addressing_Value; + y.type = x.type; y_is_diverging = true; } else { error_operand_no_value(&y); @@ -10217,7 +10228,7 @@ gb_internal void check_compound_literal_field_values(CheckerContext *c, Slicefield; if (ident->kind == Ast_ImplicitSelectorExpr) { gbString expr_str = expr_to_string(ident); - error(ident, "Field names do not start with a '.', remove the '.' in %.*s", expr_str, LIT(assignment_str)); + error(ident, "Field names do not start with a '.', remove the '.' from '%s' in %.*s", expr_str, LIT(assignment_str)); gb_string_free(expr_str); ident = ident->ImplicitSelectorExpr.selector; @@ -12832,6 +12843,12 @@ gb_internal gbString write_expr_to_string(gbString str, Ast *node, bool shorthan } else { str = gb_string_appendc(str, " ---"); } + // NOTE(tf2spi): + // Two proc literals with the same signature output the same expr above + // which poses challenges for name canonicalization. Include the below + // discriminator with the file ID and offset to help with this. + TokenPos pos = ast_token(node).pos; + str = gb_string_append_fmt(str, " /* %d!%d */", pos.file_id, pos.offset); case_end; case_ast_node(cl, CompoundLit, node); diff --git a/src/check_stmt.cpp b/src/check_stmt.cpp index 222222559..f9ab8198f 100644 --- a/src/check_stmt.cpp +++ b/src/check_stmt.cpp @@ -432,11 +432,24 @@ gb_internal Type *check_assignment_variable(CheckerContext *ctx, Operand *lhs, O // NOTE(bill): Ignore assignments to '_' if (is_blank_ident(node)) { - check_assignment(ctx, rhs, nullptr, str_lit("assignment to '_' identifier")); - if (rhs->mode == Addressing_Invalid) { - return nullptr; + String context_name = str_lit("assignment to '_' identifier"); + check_assignment(ctx, rhs, nullptr, context_name); + switch (rhs->mode) { + case Addressing_ProcGroup: { + gbString expr_str = expr_to_string(rhs->expr); + defer (gb_string_free(expr_str)); + + error(rhs->expr, + "Cannot assign procedure group '%s' in %.*s", + expr_str, + LIT(context_name)); + rhs->mode = Addressing_Invalid; + } + case Addressing_Invalid: + return nullptr; + default: + return rhs->type; } - return rhs->type; } Entity *e = nullptr; diff --git a/src/check_type.cpp b/src/check_type.cpp index 212f7dbe4..01bc16983 100644 --- a/src/check_type.cpp +++ b/src/check_type.cpp @@ -1764,7 +1764,10 @@ gb_internal ParameterValue handle_parameter_value(CheckerContext *ctx, Type *in_ check_assignment(ctx, &o, in_type, str_lit("parameter value")); } } else { - if (in_type) { + expr = unparen_expr(expr); + if (expr && expr->kind == Ast_Uninit) { + error(expr, "Default parameter cannot be ---"); + } else if (in_type) { check_expr_with_type_hint(ctx, &o, expr, in_type); } else { check_expr(ctx, &o, expr); @@ -2174,6 +2177,10 @@ gb_internal Type *check_get_params(CheckerContext *ctx, Scope *scope, Ast *_para if (!valid) { if (op.mode == Addressing_Constant) { poly_const = op.value; + if (poly_const.kind == ExactValue_Integer && is_type_float(type)) { + poly_const.kind = ExactValue_Float; + poly_const.value_float = big_int_to_f64(&poly_const.value_integer); + } } else { if (!ctx->in_proc_group) { error(op.expr, "Expected a constant value for this polymorphic name parameter, got %s", expr_to_string(op.expr)); @@ -2221,8 +2228,9 @@ gb_internal Type *check_get_params(CheckerContext *ctx, Scope *scope, Ast *_para } if (p->flags&FieldFlag_no_alias) { - if (!is_type_pointer(type) && !is_type_multi_pointer(type)) { - error(name, "'#no_alias' can only be applied pointer or multi-pointer typed parameters"); + bool ok = is_type_internally_pointer_like(type); + if (!ok) { + error(name, "'#no_alias' can only be applied pointer-like type parameters"); p->flags &= ~FieldFlag_no_alias; // Remove the flag } } diff --git a/src/checker.cpp b/src/checker.cpp index dd61f6fc3..7bcb06d05 100644 --- a/src/checker.cpp +++ b/src/checker.cpp @@ -1109,7 +1109,6 @@ gb_internal i64 odin_compile_timestamp(void) { return ns_after_1970; } -gb_internal bool lb_use_new_pass_system(void); gb_internal void init_universal(void) { BuildContext *bc = &build_context; @@ -1247,6 +1246,7 @@ gb_internal void init_universal(void) { {"iPhone", Subtarget_iPhone}, {"iPhoneSimulator", Subtarget_iPhoneSimulator}, {"Android", Subtarget_Android}, + {"Playdate", Subtarget_Playdate}, }; auto fields = add_global_enum_type(str_lit("Odin_Platform_Subtarget_Type"), values, gb_count_of(values)); @@ -1382,7 +1382,8 @@ gb_internal void init_universal(void) { } { - bool f16_supported = lb_use_new_pass_system(); + // Available since LLVM 17 / new pass system, which is the minimum now. + bool f16_supported = true; if (is_arch_wasm()) { f16_supported = false; } else if (build_context.metrics.os == TargetOs_darwin && build_context.metrics.arch == TargetArch_amd64) { @@ -6883,6 +6884,13 @@ gb_internal void check_deferred_procedures(Checker *c) { continue; } + if (entity_has_deferred_procedure(dst)) { + error(src->token, + "Deferred procedure '%.*s' cannot be used as the target of '%.*s' because it has a deferred procedure itself (deferred procedure chaining is not allowed)", + LIT(dst->token.string), LIT(src->token.string)); + continue; + } + if (is_type_polymorphic(src->type) || is_type_polymorphic(dst->type)) { error(src->token, "'%s' cannot be used with a polymorphic procedure", attribute); continue; @@ -6894,7 +6902,10 @@ gb_internal void check_deferred_procedures(Checker *c) { continue; } - GB_ASSERT(is_type_proc(src->type)); + if (!is_type_proc(src->type)) { + error(src->token, "Invalid procedure type found during deferred procedure checking"); + continue; + } GB_ASSERT(is_type_proc(dst->type)); Type *src_params = base_type(src->type)->Proc.params; Type *src_results = base_type(src->type)->Proc.results; diff --git a/src/checker.hpp b/src/checker.hpp index cd2e580d8..1a53018e3 100644 --- a/src/checker.hpp +++ b/src/checker.hpp @@ -834,6 +834,7 @@ struct CheckerContext { bool hide_polymorphic_errors; bool in_polymorphic_specialization; bool allow_arrow_right_selector_expr; + bool allow_c_vararg_param; u8 bit_field_bit_size; Scope * polymorphic_scope; diff --git a/src/exact_value.cpp b/src/exact_value.cpp index 184a8f2e4..545b415a6 100644 --- a/src/exact_value.cpp +++ b/src/exact_value.cpp @@ -1067,8 +1067,8 @@ gb_internal bool compare_exact_values(TokenKind op, ExactValue x, ExactValue y) case ExactValue_Procedure: switch (op) { - case Token_CmpEq: return x.value_typeid == y.value_typeid; - case Token_NotEq: return x.value_typeid != y.value_typeid; + case Token_CmpEq: return x.value_procedure == y.value_procedure; + case Token_NotEq: return x.value_procedure != y.value_procedure; } break; @@ -1132,12 +1132,13 @@ gb_internal gbString write_exact_value_to_string(gbString str, ExactValue const gb_free(heap_allocator(), s.text); return str; } + // NOTE(tf2spi): %.17g is specific enough to canonically serialize f64 case ExactValue_Float: - return gb_string_append_fmt(str, "%f", v.value_float); + return gb_string_append_fmt(str, "%.17g", v.value_float); case ExactValue_Complex: - return gb_string_append_fmt(str, "%f+%fi", v.value_complex->real, v.value_complex->imag); + return gb_string_append_fmt(str, "%.17g+%.17gi", v.value_complex->real, v.value_complex->imag); case ExactValue_Quaternion: - return gb_string_append_fmt(str, "%f+%fi+%fj+%fk", v.value_quaternion->real, v.value_quaternion->imag, v.value_quaternion->jmag, v.value_quaternion->kmag); + return gb_string_append_fmt(str, "%.17g+%.17gi+%.17gj+%.17gk", v.value_quaternion->real, v.value_quaternion->imag, v.value_quaternion->jmag, v.value_quaternion->kmag); case ExactValue_Pointer: return str; diff --git a/src/gb/gb.h b/src/gb/gb.h index 84bbc2e0e..793011028 100644 --- a/src/gb/gb.h +++ b/src/gb/gb.h @@ -5613,7 +5613,7 @@ gb_inline b32 gb_path_is_absolute(char const *path) { b32 result = false; GB_ASSERT(path != NULL); #if defined(GB_SYSTEM_WINDOWS) - result == (gb_strlen(path) > 2) && + result = (gb_strlen(path) > 2) && gb_char_is_alpha(path[0]) && (path[1] == ':' && path[2] == GB_PATH_SEPARATOR); #else diff --git a/src/linker.cpp b/src/linker.cpp index faba0a27f..cb3cbe023 100644 --- a/src/linker.cpp +++ b/src/linker.cpp @@ -13,6 +13,7 @@ struct LinkerData { gb_internal i32 system_exec_command_line_app(char const *name, char const *fmt, ...); gb_internal bool system_exec_command_line_app_output(char const *command, gbString *output); +// No longer required not that LLVM 14 is removed(?) gb_internal void linker_enable_system_library_linking(LinkerData *ld) { ld->needs_system_library_linked = true; } diff --git a/src/llvm_abi.cpp b/src/llvm_abi.cpp index 6aabe456f..12c3ff58c 100644 --- a/src/llvm_abi.cpp +++ b/src/llvm_abi.cpp @@ -186,9 +186,11 @@ gb_internal void lb_add_function_type_attributes(LLVMValueRef fn, lbFunctionType lbCallingConventionKind cc_kind = lbCallingConvention_C; // TODO(bill): Clean up this logic - if (!is_arch_wasm()) { + if (selected_subtarget == Subtarget_Playdate) { + cc_kind = lbCallingConvention_ARM_AAPCS_VFP; + } else if (!is_arch_wasm()) { cc_kind = lb_calling_convention_map[calling_convention]; - } + } // if (build_context.metrics.arch == TargetArch_amd64) { // if (build_context.metrics.os == TargetOs_windows) { // if (cc_kind == lbCallingConvention_C) { @@ -1554,14 +1556,14 @@ namespace lbAbiWasm { namespace lbAbiArm32 { gb_internal Array compute_arg_types(LLVMContextRef c, LLVMTypeRef *arg_types, unsigned arg_count, ProcCallingConvention calling_convention); - gb_internal lbArgType compute_return_type(LLVMContextRef c, LLVMTypeRef return_type, bool return_is_defined); + gb_internal lbArgType compute_return_type(LLVMContextRef c, LLVMTypeRef return_type, bool return_is_defined, ProcCallingConvention calling_convention); gb_internal LB_ABI_INFO(abi_info) { LLVMContextRef c = m->ctx; lbFunctionType *ft = permanent_alloc_item(); ft->ctx = c; ft->args = compute_arg_types(c, arg_types, arg_count, calling_convention); - ft->ret = compute_return_type(c, return_type, return_is_defined); + ft->ret = compute_return_type(c, return_type, return_is_defined, calling_convention); ft->calling_convention = calling_convention; return ft; } @@ -1604,7 +1606,10 @@ namespace lbAbiArm32 { } else { i64 sz = lb_sizeof(t); i64 a = lb_alignof(t); - if (is_calling_convention_odin(calling_convention) && sz > 8) { + // Added to support hard floats included in the playdates cortex-m7. + if (calling_convention == ProcCC_CDecl && selected_subtarget == Subtarget_Playdate) { + args[i] = lb_arg_type_direct(t); + } else if (is_calling_convention_odin(calling_convention) && sz > 8) { // Minor change to improve performance using the Odin calling conventions args[i] = lb_arg_type_indirect(t, nullptr); } else if (a <= 4) { @@ -1619,10 +1624,13 @@ namespace lbAbiArm32 { return args; } - gb_internal lbArgType compute_return_type(LLVMContextRef c, LLVMTypeRef return_type, bool return_is_defined) { + gb_internal lbArgType compute_return_type(LLVMContextRef c, LLVMTypeRef return_type, bool return_is_defined, ProcCallingConvention calling_convention) { if (!return_is_defined) { return lb_arg_type_direct(LLVMVoidTypeInContext(c)); } else if (!is_register(return_type, true)) { + if (calling_convention == ProcCC_CDecl && selected_subtarget == Subtarget_Playdate) { + return lb_arg_type_direct(return_type); + } switch (lb_sizeof(return_type)) { case 1: return lb_arg_type_direct(LLVMIntTypeInContext(c, 8), return_type, nullptr, nullptr); case 2: return lb_arg_type_direct(LLVMIntTypeInContext(c, 16), return_type, nullptr, nullptr); diff --git a/src/llvm_backend.cpp b/src/llvm_backend.cpp index f9e049621..4bdde8305 100644 --- a/src/llvm_backend.cpp +++ b/src/llvm_backend.cpp @@ -1571,7 +1571,10 @@ gb_internal void lb_finalize_objc_names(lbGenerator *gen, lbProcedure *p) { for (Entity *e = {}; mpsc_dequeue(&gen->info->objc_class_implementations, &e); /**/) { GB_ASSERT(e->kind == Entity_TypeName && e->TypeName.objc_is_implementation); lb_handle_objc_find_or_register_class(p, e->TypeName.objc_class_name, e->type); - error(e->token, "Objective-C related things are not allowed with '-bedrock'"); + + if (build_context.bedrock) { + error(e->token, "Objective-C related things are not allowed with '-bedrock'"); + } } // Ensure classes that have been implicitly referenced through @@ -1684,7 +1687,7 @@ gb_internal void lb_finalize_objc_names(lbGenerator *gen, lbProcedure *p) { Type *superclass = tn.objc_superclass; if (superclass != nullptr) { - auto& superclass_global = string_map_must_get(&global_class_map, tn.objc_class_name); + auto &superclass_global = string_map_must_get(&global_class_map, superclass->Named.type_name->TypeName.objc_class_name); superclass_value = superclass_global.class_value; } @@ -2408,10 +2411,6 @@ gb_internal WORKER_TASK_PROC(lb_llvm_function_pass_per_module) { LLVMInitializeFunctionPassManager(m->function_pass_managers[i]); } - lb_populate_function_pass_manager(m, m->function_pass_managers[lbFunctionPassManager_default], false, build_context.optimization_level); - lb_populate_function_pass_manager(m, m->function_pass_managers[lbFunctionPassManager_default_without_memcpy], true, build_context.optimization_level); - lb_populate_function_pass_manager_specific(m, m->function_pass_managers[lbFunctionPassManager_none], -1); - for (i32 i = 0; i < lbFunctionPassManager_COUNT; i++) { LLVMFinalizeFunctionPassManager(m->function_pass_managers[i]); } @@ -2480,11 +2479,8 @@ gb_internal WORKER_TASK_PROC(lb_llvm_module_pass_worker_proc) { auto wd = cast(lbLLVMModulePassWorkerData *)data; LLVMPassManagerRef module_pass_manager = LLVMCreatePassManager(); - lb_populate_module_pass_manager(wd->target_machine, module_pass_manager, build_context.optimization_level); LLVMRunPassManager(module_pass_manager, wd->m->mod); - -#if LB_USE_NEW_PASS_SYSTEM auto passes = array_make(heap_allocator(), 0, 64); defer (array_free(&passes)); @@ -2560,7 +2556,6 @@ gb_internal WORKER_TASK_PROC(lb_llvm_module_pass_worker_proc) { exit_with_errors(); return 1; } -#endif if (LLVM_IGNORE_VERIFICATION) { return 0; @@ -2713,9 +2708,12 @@ gb_internal String lb_filepath_ll_for_module(lbModule *m) { s.len -= prefix.len; } - path = concatenate_strings(permanent_allocator(), path, s); - path = concatenate_strings(permanent_allocator(), s, STR_LIT(".ll")); - + if (build_context.out_filepath.len > 0) { + path = concatenate_strings(permanent_allocator(), path, s); + path = concatenate_strings(permanent_allocator(), path, STR_LIT(".ll")); + } else { + path = concatenate_strings(permanent_allocator(), s, STR_LIT(".ll")); + } return path; } @@ -2736,6 +2734,8 @@ gb_internal String lb_filepath_obj_for_module(lbModule *m) { gbString path = gb_string_make_length(heap_allocator(), basename.text, basename.len); path = gb_string_appendc(path, "/"); + bool output_is_directory = path_is_directory(make_string_c(path)); + if (USE_SEPARATE_MODULES) { GB_ASSERT(m->module_name != nullptr); String s = make_string_c(m->module_name); @@ -2760,10 +2760,14 @@ gb_internal String lb_filepath_obj_for_module(lbModule *m) { if (build_context.lto_kind != LTO_None) { ext = STR_LIT("bc"); } else if (build_context.build_mode == BuildMode_Assembly) { - ext = STR_LIT("S"); + // Allow a user override for the asm extension. + // If that's a directory, we force the `.S` extension + ext = output_is_directory ? STR_LIT("S") : build_context.build_paths[BuildPath_Output].ext; } else if (build_context.build_mode == BuildMode_Object) { // Allow a user override for the object extension. - ext = build_context.build_paths[BuildPath_Output].ext; + // If that's a directory, we force the `.obj` extension + ext = output_is_directory ? STR_LIT("obj") : build_context.build_paths[BuildPath_Output].ext; + } else { ext = infer_object_extension_from_build_context(); } @@ -2855,7 +2859,6 @@ gb_internal bool lb_llvm_object_generation(lbGenerator *gen, bool do_threading) gb_internal lbProcedure *lb_create_main_procedure(lbModule *m, lbProcedure *startup_runtime, lbProcedure *cleanup_runtime) { LLVMPassManagerRef default_function_pass_manager = LLVMCreateFunctionPassManagerForModule(m->mod); - lb_populate_function_pass_manager(m, default_function_pass_manager, false, build_context.optimization_level); LLVMFinalizeFunctionPassManager(default_function_pass_manager); Type *params = alloc_type_tuple(); @@ -3164,9 +3167,6 @@ gb_internal bool lb_generate_code(lbGenerator *gen) { // GB_ASSERT_MSG(LLVMTargetHasAsmBackend(target)); LLVMCodeGenOptLevel code_gen_level = LLVMCodeGenLevelNone; - if (!LB_USE_NEW_PASS_SYSTEM) { - build_context.optimization_level = gb_clamp(build_context.optimization_level, -1, 2); - } switch (build_context.optimization_level) { default:/*fallthrough*/ case 0: code_gen_level = LLVMCodeGenLevelNone; break; diff --git a/src/llvm_backend.hpp b/src/llvm_backend.hpp index fb8d0f89a..e8315966c 100644 --- a/src/llvm_backend.hpp +++ b/src/llvm_backend.hpp @@ -4,6 +4,10 @@ #include #endif +#if LLVM_VERSION_MAJOR < 17 +#error "LLVM Version 17 is the minimum required" +#endif + #include #include #include @@ -11,46 +15,14 @@ #include #include #include -#if LLVM_VERSION_MAJOR >= 17 #include -#else -#include -#include -#include -#include -#include -#include -#include -#endif -#if LLVM_VERSION_MAJOR < 14 -#error "LLVM Version 14 is the minimum required" -#endif -#if LLVM_VERSION_MAJOR > 14 || (LLVM_VERSION_MAJOR == 14 && LLVM_VERSION_MINOR >= 0 && LLVM_VERSION_PATCH > 0) -#define ODIN_LLVM_MINIMUM_VERSION_14 1 -#else -#define ODIN_LLVM_MINIMUM_VERSION_14 0 -#endif - -#if LLVM_VERSION_MAJOR == 15 || LLVM_VERSION_MAJOR == 16 -#error "LLVM versions 15 and 16 are not supported" -#endif - -#if LLVM_VERSION_MAJOR >= 17 -#define LB_USE_NEW_PASS_SYSTEM 1 -#else -#define LB_USE_NEW_PASS_SYSTEM 0 -#endif #if LLVM_VERSION_MAJOR >= 19 #define LLVMDIBuilderInsertDeclareAtEnd(...) LLVMDIBuilderInsertDeclareRecordAtEnd(__VA_ARGS__) #endif -gb_internal bool lb_use_new_pass_system(void) { - return LB_USE_NEW_PASS_SYSTEM; -} - struct lbProcedure; struct lbValue { @@ -408,13 +380,6 @@ struct lbProcedure { #define ABI_PKG_NAME_SEPARATOR "." #endif - -#if !ODIN_LLVM_MINIMUM_VERSION_14 -#define LLVMConstGEP2(Ty__, ConstantVal__, ConstantIndices__, NumIndices__) LLVMConstGEP(ConstantVal__, ConstantIndices__, NumIndices__) -#define LLVMConstInBoundsGEP2(Ty__, ConstantVal__, ConstantIndices__, NumIndices__) LLVMConstInBoundsGEP(ConstantVal__, ConstantIndices__, NumIndices__) -#define LLVMBuildPtrDiff2(Builder__, Ty__, LHS__, RHS__, Name__) LLVMBuildPtrDiff(Builder__, LHS__, RHS__, Name__) -#endif - gb_internal bool lb_init_generator(lbGenerator *gen, Checker *c); gb_internal String lb_mangle_name(Entity *e); @@ -595,7 +560,7 @@ gb_internal lbValue lb_emit_logical_binary_expr(lbProcedure *p, TokenKind op, As gb_internal lbValue lb_build_cond(lbProcedure *p, Ast *cond, lbBlock *true_block, lbBlock *false_block); gb_internal LLVMValueRef llvm_const_named_struct(lbModule *m, Type *t, LLVMValueRef *values, isize value_count_); -gb_internal LLVMValueRef llvm_const_named_struct_internal(lbModule *m, LLVMTypeRef t, LLVMValueRef *values, isize value_count_); +gb_internal LLVMValueRef llvm_const_named_struct_internal(lbModule *m, LLVMTypeRef t, LLVMValueRef *values, isize value_count_, bool force_non_named=false); gb_internal void lb_set_entity_from_other_modules_linkage_correctly(lbModule *other_module, Entity *e, String const &name); gb_internal lbValue lb_expr_untyped_const_to_typed(lbModule *m, Ast *expr, Type *t); @@ -641,11 +606,7 @@ gb_internal lbValue lb_emit_source_code_location_as_global_ptr(lbProcedure *p, S gb_internal LLVMMetadataRef lb_debug_location_from_token_pos(lbProcedure *p, TokenPos pos); gb_internal LLVMTypeRef llvm_array_type(LLVMTypeRef ElementType, uint64_t ElementCount) { -#if LB_USE_NEW_PASS_SYSTEM return LLVMArrayType2(ElementType, ElementCount); -#else - return LLVMArrayType(ElementType, cast(unsigned)ElementCount); -#endif } gb_internal lbValue lb_emit_struct_iv(lbProcedure *p, lbValue agg, lbValue field, i32 index); diff --git a/src/llvm_backend_const.cpp b/src/llvm_backend_const.cpp index 8cd069772..0b2d21c51 100644 --- a/src/llvm_backend_const.cpp +++ b/src/llvm_backend_const.cpp @@ -109,68 +109,7 @@ gb_internal LLVMValueRef llvm_const_cast(lbModule *m, LLVMValueRef val, LLVMType return LLVMConstPointerCast(val, dst); } case LLVMStructTypeKind: { - GB_ASSERT_MSG(lb_sizeof(dst) == lb_sizeof(src), "dst:%s vs src:%s (dst:%lld vs src:%lld)", LLVMPrintTypeToString(dst), LLVMPrintTypeToString(src), - cast(long long)lb_sizeof(dst), - cast(long long)lb_sizeof(src)); - - unsigned src_n = LLVMCountStructElementTypes(src); - unsigned dst_n = LLVMCountStructElementTypes(dst); - if (src_n != dst_n) goto failure; - - // bool skip_cast = true; - // for (unsigned i = 0; i < dst_n; i++) { - // LLVMTypeKind dt = LLVMGetTypeKind(LLVMStructGetTypeAtIndex(dst, i)); - // LLVMTypeKind st = LLVMGetTypeKind(LLVMStructGetTypeAtIndex(src, i)); - // if (dt != st) { - // skip_cast = false; - // } - // if (dt == LLVMIntegerTypeKind) { - // continue; - // } - // if (dt != LLVMArrayTypeKind) { - // skip_cast = false; - // break; - // } - - // LLVMValueRef field_val = llvm_const_extract_value(m, val, i); - // if (field_val == nullptr) goto failure; - - // LLVMTypeRef dst_elem_ty = LLVMStructGetTypeAtIndex(dst, i); - // LLVMTypeRef src_elem_ty = LLVMTypeOf(field_val); - // if (lb_sizeof(dst_elem_ty) > lb_sizeof(src_elem_ty)) { - // skip_cast = true; - // continue; - // } - - // } - // if (skip_cast) { - // return val; - // } - - LLVMValueRef *field_vals = temporary_alloc_array(dst_n); - for (unsigned i = 0; i < dst_n; i++) { - LLVMValueRef field_val = llvm_const_extract_value(m, val, i); - if (field_val == nullptr) goto failure; - - LLVMTypeRef dst_elem_ty = LLVMStructGetTypeAtIndex(dst, i); - LLVMTypeRef src_elem_ty = LLVMTypeOf(field_val); - - GB_ASSERT_MSG(lb_sizeof(dst_elem_ty) == lb_sizeof(src_elem_ty), "dst:%s vs src:%s (dst:%lld vs src:%lld) to %s from %s", LLVMPrintTypeToString(dst_elem_ty), LLVMPrintTypeToString(src_elem_ty), - cast(long long)lb_sizeof(dst_elem_ty), - cast(long long)lb_sizeof(src_elem_ty), - LLVMPrintTypeToString(dst), - LLVMPrintTypeToString(src) - ); - - field_vals[i] = llvm_const_cast(m, field_val, dst_elem_ty, failure_); - if (failure_ && *failure_) goto failure; - } - - if (!LLVMIsLiteralStruct(dst)) { - return LLVMConstNamedStruct(dst, field_vals, dst_n); - } else { - return LLVMConstStructInContext(m->ctx, field_vals, dst_n, LLVMIsPackedStruct(dst)); - } + goto failure; } case LLVMArrayTypeKind: { goto failure; @@ -261,10 +200,15 @@ gb_internal LLVMValueRef llvm_const_named_struct(lbModule *m, Type *t, LLVMValue return llvm_const_named_struct_internal(m, struct_type, values_with_padding, values_with_padding_count); } -gb_internal LLVMValueRef llvm_const_named_struct_internal(lbModule *m, LLVMTypeRef t, LLVMValueRef *values, isize value_count_) { +gb_internal LLVMValueRef llvm_const_named_struct_internal(lbModule *m, LLVMTypeRef t, LLVMValueRef *values, isize value_count_, bool force_non_named) { unsigned value_count = cast(unsigned)value_count_; unsigned elem_count = LLVMCountStructElementTypes(t); GB_ASSERT_MSG(value_count == elem_count, "%s %u %u", LLVMPrintTypeToString(t), value_count, elem_count); + + if (force_non_named) { + return LLVMConstStructInContext(m->ctx, values, value_count, true); + } + bool failure = false; for (unsigned i = 0; i < elem_count; i++) { LLVMTypeRef elem_type = LLVMStructGetTypeAtIndex(t, i); @@ -638,104 +582,6 @@ gb_internal bool lb_is_nested_possibly_constant(Type *ft, Selection const &sel, return lb_is_elem_const(elem, ft); } -#if LLVM_VERSION_MAJOR == 14 -LLVMValueRef llvm_const_pad_to_size(lbModule *m, LLVMValueRef val, LLVMTypeRef dst_ty) { - LLVMContextRef ctx = m->ctx; - LLVMTargetDataRef td = LLVMGetModuleDataLayout(m->mod); - LLVMTypeRef src_ty = LLVMTypeOf(val); - unsigned src_bits = (unsigned)LLVMSizeOfTypeInBits(td, src_ty); - unsigned dst_bits = (unsigned)LLVMSizeOfTypeInBits(td, dst_ty); - - LLVMValueRef as_int = nullptr; - LLVMTypeKind src_kind = LLVMGetTypeKind(src_ty); - - if (src_kind == LLVMIntegerTypeKind || - src_kind == LLVMFloatTypeKind || - src_kind == LLVMDoubleTypeKind || - src_kind == LLVMPointerTypeKind || - src_kind == LLVMVectorTypeKind) { - LLVMTypeRef src_int_ty = LLVMIntTypeInContext(ctx, src_bits); - as_int = LLVMConstBitCast(val, src_int_ty); - - } else if (src_kind == LLVMArrayTypeKind) { - unsigned elem_count = LLVMGetArrayLength(src_ty); - LLVMTypeRef elem_ty = LLVMGetElementType(src_ty); - unsigned elem_bits = (unsigned)LLVMSizeOfTypeInBits(td, elem_ty); - LLVMTypeRef src_int_ty = LLVMIntTypeInContext(ctx, src_bits); - as_int = LLVMConstInt(src_int_ty, 0, false); - - for (unsigned i = 0; i < elem_count; i++) { - LLVMValueRef elem = llvm_const_extract_value(m, val, i); - LLVMTypeRef elem_int_ty = LLVMIntTypeInContext(ctx, elem_bits); - LLVMValueRef elem_int = llvm_const_pad_to_size(m, elem, elem_int_ty); - LLVMValueRef shifted = LLVMConstShl(LLVMConstZExt(elem_int, src_int_ty), LLVMConstInt(src_int_ty, i * elem_bits, false)); - as_int = LLVMConstOr(as_int, shifted); - } - } else if (src_kind == LLVMStructTypeKind) { - unsigned field_count = LLVMCountStructElementTypes(src_ty); - LLVMTypeRef src_int_ty = LLVMIntTypeInContext(ctx, src_bits); - as_int = LLVMConstInt(src_int_ty, 0, false); - - for (unsigned i = 0; i < field_count; i++) { - LLVMTypeRef field_ty = LLVMStructGetTypeAtIndex(src_ty, i); - unsigned field_bits = (unsigned)LLVMSizeOfTypeInBits(td, field_ty); - LLVMValueRef field = llvm_const_extract_value(m, val, i); - - LLVMTypeRef field_int_ty = LLVMIntTypeInContext(ctx, field_bits); - LLVMValueRef field_int = llvm_const_pad_to_size(m, field, field_int_ty); - - uint64_t field_offset_bytes = LLVMOffsetOfElement(td, src_ty, i); - uint64_t field_offset_bits = field_offset_bytes * 8; - - LLVMValueRef shifted = LLVMConstShl(LLVMConstZExt(field_int, src_int_ty), LLVMConstInt(src_int_ty, field_offset_bits, false)); - as_int = LLVMConstOr(as_int, shifted); - } - } else { - gb_printf_err("unsupported const_pad source type: %s\n", LLVMPrintTypeToString(src_ty)); - return nullptr; - } - - if (src_bits != dst_bits) { - LLVMTypeRef dst_int_ty = LLVMIntTypeInContext(ctx, dst_bits); - if (src_bits < dst_bits) { - as_int = LLVMConstZExt(as_int, dst_int_ty); - } else { - as_int = LLVMConstTrunc(as_int, dst_int_ty); - } - } - - LLVMTypeKind dst_kind = LLVMGetTypeKind(dst_ty); - - if (dst_kind == LLVMIntegerTypeKind || - dst_kind == LLVMFloatTypeKind || - dst_kind == LLVMDoubleTypeKind || - dst_kind == LLVMPointerTypeKind || - dst_kind == LLVMVectorTypeKind) { - return LLVMConstBitCast(as_int, dst_ty); - - } else if (dst_kind == LLVMArrayTypeKind) { - unsigned elem_count = LLVMGetArrayLength(dst_ty); - LLVMTypeRef elem_ty = LLVMGetElementType(dst_ty); - unsigned elem_bits = (unsigned)LLVMSizeOfTypeInBits(td, elem_ty); - - LLVMValueRef *elems = temporary_alloc_array(elem_count); - LLVMTypeRef as_int_ty = LLVMTypeOf(as_int); - - for (unsigned i = 0; i < elem_count; i++) { - LLVMValueRef shifted = LLVMConstLShr(as_int, LLVMConstInt(as_int_ty, i * elem_bits, false)); - LLVMTypeRef elem_int_ty = LLVMIntTypeInContext(ctx, elem_bits); - LLVMValueRef trunc = LLVMConstTrunc(shifted, elem_int_ty); - elems[i] = llvm_const_pad_to_size(m, trunc, elem_ty); - } - - return LLVMConstArray(elem_ty, elems, elem_count); - } - - gb_printf_err("unsupported const_pad destination type: %s\n", LLVMPrintTypeToString(dst_ty)); - return nullptr; -} -#endif - gb_internal void lb_const_array_spread(lbModule *m, lbConstContext cc, Type *array, ExactValue value, lbValue *res, Type *value_type) { GB_ASSERT(array->kind == Type_Array); @@ -1070,16 +916,11 @@ gb_internal lbValue lb_const_value(lbModule *m, Type *type, ExactValue value, Ty LLVMValueRef values[4] = {}; unsigned value_count = 0; - #if LLVM_VERSION_MAJOR == 14 - LLVMTypeRef block_type = lb_type_internal_union_block_type(m, bt); - values[value_count++] = llvm_const_pad_to_size(m, cv.value, block_type); - #else values[value_count++] = cv.value; if (block_size != type_size_of(variant_type)) { LLVMTypeRef padding_type = lb_type_padding_filler(m, block_size - type_size_of(variant_type), 1); values[value_count++] = LLVMConstNull(padding_type); } - #endif Type *tag_type = union_tag_type(bt); LLVMTypeRef llvm_tag_type = lb_type(m, tag_type); @@ -2101,6 +1942,8 @@ gb_internal lbValue lb_const_value(lbModule *m, Type *type, ExactValue value, Ty LLVMTypeRef struct_type = lb_type(m, original_type); + bool force_non_named = false; + auto field_remapping = lb_get_struct_remapping(m, type); unsigned value_count = LLVMCountStructElementTypes(struct_type); @@ -2137,7 +1980,6 @@ gb_internal lbValue lb_const_value(lbModule *m, Type *type, ExactValue value, Ty visited[index] = true; } - unsigned idx_list_len = cast(unsigned)sel.index.count-1; unsigned *idx_list = gb_alloc_array(temporary_allocator(), unsigned, idx_list_len); @@ -2175,7 +2017,12 @@ gb_internal lbValue lb_const_value(lbModule *m, Type *type, ExactValue value, Ty if (is_constant) { LLVMValueRef elem_value = lb_const_value(m, cv_type, tav.value, tav.type, cc).value; if (LLVMIsConstant(elem_value) && LLVMIsConstant(values[index])) { - values[index] = llvm_const_insert_value(m, values[index], elem_value, idx_list, idx_list_len); + if (is_type_union(cv_type) || is_type_raw_union(cv_type)) { + force_non_named = true; + values[index] = llvm_const_insert_value_with_rebuild(m, values[index], elem_value, idx_list, idx_list_len); + } else { + values[index] = llvm_const_insert_value(m, values[index], elem_value, idx_list, idx_list_len); + } } else if (is_local) { #if 1 lbProcedure *p = m->curr_procedure; @@ -2222,13 +2069,6 @@ gb_internal lbValue lb_const_value(lbModule *m, Type *type, ExactValue value, Ty Entity *f = type->Struct.fields[i+multiple_return_offset]; TypeAndValue tav = cl->elems[i]->tav; - // INFO: dead code: - - // ExactValue val = {}; - // if (tav.mode != Addressing_Invalid) { - // val = tav.value; - // } - if (is_type_tuple(tav.type)){ multiple_return_offset += tav.type->Tuple.variables.count-1; } @@ -2268,7 +2108,7 @@ gb_internal lbValue lb_const_value(lbModule *m, Type *type, ExactValue value, Ty } if (is_constant) { - res.value = llvm_const_named_struct_internal(m, struct_type, values, cast(unsigned)value_count); + res.value = llvm_const_named_struct_internal(m, struct_type, values, cast(unsigned)value_count, force_non_named); LLVMTypeRef res_type = LLVMTypeOf(res.value); GB_ASSERT(lb_sizeof(res_type) == lb_sizeof(struct_type)); return res; diff --git a/src/llvm_backend_expr.cpp b/src/llvm_backend_expr.cpp index 7eb32279e..8824c1014 100644 --- a/src/llvm_backend_expr.cpp +++ b/src/llvm_backend_expr.cpp @@ -1982,19 +1982,20 @@ handle_op:; LLVMValueRef lhsval = lhs.value; LLVMValueRef bits = rhs.value; bool is_unsigned = is_type_unsigned(integral_type); - LLVMValueRef bit_size = LLVMConstInt(lb_type(p->module, rhs.type), 8*type_size_of(lhs.type), false); - LLVMValueRef width_test = LLVMBuildICmp(p->builder, LLVMIntULT, bits, bit_size, ""); if (is_unsigned) { res.value = LLVMBuildLShr(p->builder, lhsval, bits, ""); + + LLVMValueRef zero = LLVMConstNull(lb_type(p->module, lhs.type)); + res.value = LLVMBuildSelect(p->builder, width_test, res.value, zero, ""); } else { + LLVMValueRef bit_size_minus_one = LLVMConstInt(lb_type(p->module, rhs.type), 8*type_size_of(lhs.type)-1, false); + bits = LLVMBuildSelect(p->builder, width_test, bits, bit_size_minus_one, ""); + res.value = LLVMBuildAShr(p->builder, lhsval, bits, ""); } - - LLVMValueRef zero = LLVMConstNull(lb_type(p->module, lhs.type)); - res.value = LLVMBuildSelect(p->builder, width_test, res.value, zero, ""); return res; } case Token_AndNot: @@ -3878,6 +3879,7 @@ gb_internal lbValue lb_emit_comp(lbProcedure *p, TokenKind op_kind, lbValue left break; } + GB_ASSERT(res.value != nullptr); return res; } else if (is_type_soa_pointer(a)) { diff --git a/src/llvm_backend_general.cpp b/src/llvm_backend_general.cpp index d79cf5ed7..d85f0d564 100644 --- a/src/llvm_backend_general.cpp +++ b/src/llvm_backend_general.cpp @@ -166,10 +166,6 @@ gb_internal bool lb_init_generator(lbGenerator *gen, Checker *c) { String init_fullpath = c->parser->init_fullpath; linker_data_init(gen, &c->info, init_fullpath); - #if defined(GB_SYSTEM_OSX) && (LLVM_VERSION_MAJOR < 14) - linker_enable_system_library_linking(gen); - #endif - gen->info = &c->info; map_init(&gen->modules, gen->info->packages.count*2); @@ -461,6 +457,27 @@ gb_internal LLVMValueRef llvm_const_insert_value(lbModule *m, LLVMValueRef agg, return extracted_value; } +gb_internal LLVMValueRef llvm_const_insert_value_with_rebuild(lbModule *m, LLVMValueRef agg, LLVMValueRef val, unsigned *indices, isize count) { + GB_ASSERT(LLVMIsConstant(agg)); + GB_ASSERT(LLVMIsConstant(val)); + GB_ASSERT(count > 0); + + unsigned value_count = LLVMCountStructElementTypes(LLVMTypeOf(agg)); + LLVMValueRef *values = gb_alloc_array(heap_allocator(), LLVMValueRef, count); + defer (gb_free(heap_allocator(), values)); + + for (unsigned i = 0; i < value_count; i++) { + values[i] = llvm_const_extract_value(m, agg, i); + } + if (count == 1) { + values[indices[0]] = val; + } else { + values[indices[0]] = llvm_const_insert_value_with_rebuild(m, values[indices[0]], val, indices+1, count-1); + } + + return LLVMConstStructInContext(m->ctx, values, value_count, true); +} + @@ -1647,12 +1664,14 @@ gb_internal lbValue lb_emit_union_tag_ptr(lbProcedure *p, lbValue u) { unsigned element_count = LLVMCountStructElementTypes(uvt); GB_ASSERT_MSG(element_count >= 2, "element_count=%u (%s) != (%s)", element_count, type_to_string(ut), LLVMPrintTypeToString(uvt)); + LLVMValueRef ptr = u.value; ptr = LLVMBuildPointerCast(p->builder, ptr, LLVMPointerType(uvt, 0), ""); lbValue tag_ptr = {}; tag_ptr.value = LLVMBuildStructGEP2(p->builder, uvt, ptr, 1, ""); tag_ptr.type = alloc_type_pointer(tag_type); + tag_ptr.value = LLVMBuildPointerCast(p->builder, tag_ptr.value, lb_type(p->module, tag_ptr.type), ""); return tag_ptr; } @@ -2402,8 +2421,9 @@ gb_internal LLVMTypeRef lb_type_internal(lbModule *m, Type *type) { i64 full_type_align = type_align_of(type); GB_ASSERT(full_type_size % full_type_align == 0); - if (type->Struct.is_raw_union) { + bool requires_packing = type->Struct.is_packed; + if (type->Struct.is_raw_union) { lbStructFieldRemapping field_remapping = {}; slice_init(&field_remapping, permanent_allocator(), 1); @@ -2411,7 +2431,7 @@ gb_internal LLVMTypeRef lb_type_internal(lbModule *m, Type *type) { fields[0] = lb_type_padding_filler(m, full_type_size, full_type_align); field_remapping[0] = 0; - LLVMTypeRef struct_type = LLVMStructTypeInContext(ctx, fields, gb_count_of(fields), false); + LLVMTypeRef struct_type = LLVMStructTypeInContext(ctx, fields, gb_count_of(fields), requires_packing); map_set(&m->struct_field_remapping, cast(void *)struct_type, field_remapping); map_set(&m->struct_field_remapping, cast(void *)type, field_remapping); return struct_type; @@ -2431,7 +2451,6 @@ gb_internal LLVMTypeRef lb_type_internal(lbModule *m, Type *type) { } i64 prev_offset = 0; - bool requires_packing = type->Struct.is_packed; for (i32 field_index : struct_fields_index_by_increasing_offset(temporary_allocator(), type)) { Entity *field = type->Struct.fields[field_index]; i64 offset = type->Struct.offsets[field_index]; @@ -2521,12 +2540,6 @@ gb_internal LLVMTypeRef lb_type_internal(lbModule *m, Type *type) { LLVMTypeRef tag_type = lb_type(m, union_tag_type(type)); array_add(&fields, block_type); - // #if LLVM_VERSION_MAJOR > 14 - // { // NOTE(bill): always add a zero-byte pad to make inline constant unions work - // LLVMTypeRef padding_type = lb_type_padding_filler(m, 0, 1); - // array_add(&fields, padding_type); - // } - // #endif array_add(&fields, tag_type); i64 used_size = lb_sizeof(block_type) + lb_sizeof(tag_type); i64 padding = size - used_size; @@ -2648,6 +2661,13 @@ gb_internal LLVMTypeRef lb_type(lbModule *m, Type *type) { m->internal_type_level += 1; llvm_type = lb_type_internal(m, type); m->internal_type_level -= 1; + + // { + // i64 tsz = type_size_of(type); + // i64 lsz = lb_sizeof(llvm_type); + // GB_ASSERT_MSG(tsz == lsz, "%s %lld vs %lld %s", type_to_string(type), cast(long long)tsz, cast(long long)lsz, LLVMPrintTypeToString(llvm_type)); + // } + if (m->internal_type_level == 0) { map_set(&m->types, type, llvm_type); } diff --git a/src/llvm_backend_opt.cpp b/src/llvm_backend_opt.cpp index 47ea90703..0f925301c 100644 --- a/src/llvm_backend_opt.cpp +++ b/src/llvm_backend_opt.cpp @@ -32,11 +32,6 @@ **************************************************************************/ -gb_internal void lb_populate_function_pass_manager(lbModule *m, LLVMPassManagerRef fpm, bool ignore_memcpy_pass, i32 optimization_level); -gb_internal void lb_add_function_simplifcation_passes(LLVMPassManagerRef mpm, i32 optimization_level); -gb_internal void lb_populate_module_pass_manager(LLVMTargetMachineRef target_machine, LLVMPassManagerRef mpm, i32 optimization_level); -gb_internal void lb_populate_function_pass_manager_specific(lbModule *m, LLVMPassManagerRef fpm, i32 optimization_level); - // gb_internal LLVMBool lb_must_preserve_predicate_callback(LLVMValueRef value, void *user_data) { // lbModule *m = cast(lbModule *)user_data; // if (m == nullptr) { @@ -48,221 +43,6 @@ gb_internal void lb_populate_function_pass_manager_specific(lbModule *m, LLVMPas // return LLVMIsAAllocaInst(value) != nullptr; // } -gb_internal bool lb_opt_ignore(i32 optimization_level) { - return optimization_level < 0; -} - -gb_internal void lb_basic_populate_function_pass_manager(LLVMPassManagerRef fpm, i32 optimization_level) { - if (lb_opt_ignore(optimization_level)) { - return; - } - -#if !LB_USE_NEW_PASS_SYSTEM - if (false && optimization_level <= 0 && build_context.ODIN_DEBUG) { - LLVMAddMergedLoadStoreMotionPass(fpm); - } else { - LLVMAddPromoteMemoryToRegisterPass(fpm); - LLVMAddMergedLoadStoreMotionPass(fpm); - if (!build_context.ODIN_DEBUG) { - LLVMAddEarlyCSEPass(fpm); - } - } -#endif -} - -gb_internal void lb_populate_function_pass_manager(lbModule *m, LLVMPassManagerRef fpm, bool ignore_memcpy_pass, i32 optimization_level) { - if (lb_opt_ignore(optimization_level)) { - return; - } - -#if !LB_USE_NEW_PASS_SYSTEM - if (ignore_memcpy_pass) { - lb_basic_populate_function_pass_manager(fpm, optimization_level); - return; - } else if (optimization_level <= 0) { - LLVMAddMemCpyOptPass(fpm); - lb_basic_populate_function_pass_manager(fpm, optimization_level); - return; - } - -#if 0 - LLVMPassManagerBuilderRef pmb = LLVMPassManagerBuilderCreate(); - LLVMPassManagerBuilderSetOptLevel(pmb, optimization_level); - LLVMPassManagerBuilderSetSizeLevel(pmb, optimization_level); - LLVMPassManagerBuilderPopulateFunctionPassManager(pmb, fpm); -#else - LLVMAddMemCpyOptPass(fpm); - lb_basic_populate_function_pass_manager(fpm, optimization_level); - - LLVMAddSCCPPass(fpm); - - LLVMAddPromoteMemoryToRegisterPass(fpm); - LLVMAddUnifyFunctionExitNodesPass(fpm); - - LLVMAddCFGSimplificationPass(fpm); - LLVMAddEarlyCSEPass(fpm); - LLVMAddLowerExpectIntrinsicPass(fpm); -#endif -#endif -} - -gb_internal void lb_populate_function_pass_manager_specific(lbModule *m, LLVMPassManagerRef fpm, i32 optimization_level) { - if (lb_opt_ignore(optimization_level)) { - return; - } - -#if !LB_USE_NEW_PASS_SYSTEM - if (optimization_level <= 0) { - LLVMAddMemCpyOptPass(fpm); - lb_basic_populate_function_pass_manager(fpm, optimization_level); - return; - } - -#if 1 - LLVMPassManagerBuilderRef pmb = LLVMPassManagerBuilderCreate(); - LLVMPassManagerBuilderSetOptLevel(pmb, optimization_level); - LLVMPassManagerBuilderSetSizeLevel(pmb, optimization_level); - LLVMPassManagerBuilderPopulateFunctionPassManager(pmb, fpm); -#else - LLVMAddMemCpyOptPass(fpm); - LLVMAddPromoteMemoryToRegisterPass(fpm); - LLVMAddMergedLoadStoreMotionPass(fpm); - LLVMAddEarlyCSEPass(fpm); - - LLVMAddMergedLoadStoreMotionPass(fpm); - LLVMAddPromoteMemoryToRegisterPass(fpm); - LLVMAddCFGSimplificationPass(fpm); - - LLVMAddSCCPPass(fpm); - - LLVMAddPromoteMemoryToRegisterPass(fpm); - LLVMAddUnifyFunctionExitNodesPass(fpm); - - LLVMAddCFGSimplificationPass(fpm); - LLVMAddEarlyCSEPass(fpm); - LLVMAddLowerExpectIntrinsicPass(fpm); -#endif -#endif -} - -gb_internal void lb_add_function_simplifcation_passes(LLVMPassManagerRef mpm, i32 optimization_level) { -#if !LB_USE_NEW_PASS_SYSTEM - LLVMAddCFGSimplificationPass(mpm); - - LLVMAddJumpThreadingPass(mpm); - - LLVMAddSimplifyLibCallsPass(mpm); - - LLVMAddTailCallEliminationPass(mpm); - LLVMAddCFGSimplificationPass(mpm); - LLVMAddReassociatePass(mpm); - - LLVMAddLoopRotatePass(mpm); - LLVMAddLICMPass(mpm); - LLVMAddLoopUnswitchPass(mpm); - - LLVMAddCFGSimplificationPass(mpm); - LLVMAddLoopIdiomPass(mpm); - LLVMAddLoopDeletionPass(mpm); - - LLVMAddMergedLoadStoreMotionPass(mpm); - - LLVMAddMemCpyOptPass(mpm); - LLVMAddSCCPPass(mpm); - - LLVMAddBitTrackingDCEPass(mpm); - - LLVMAddJumpThreadingPass(mpm); - LLVMAddLICMPass(mpm); - - LLVMAddLoopRerollPass(mpm); - LLVMAddAggressiveDCEPass(mpm); - LLVMAddCFGSimplificationPass(mpm); -#endif -} - - -gb_internal void lb_populate_module_pass_manager(LLVMTargetMachineRef target_machine, LLVMPassManagerRef mpm, i32 optimization_level) { - - // NOTE(bill): Treat -opt:3 as if it was -opt:2 - // TODO(bill): Determine which opt definitions should exist in the first place - if (optimization_level <= 0 && build_context.ODIN_DEBUG) { - return; - } -#if !LB_USE_NEW_PASS_SYSTEM - LLVMAddAlwaysInlinerPass(mpm); - LLVMAddStripDeadPrototypesPass(mpm); - LLVMAddAnalysisPasses(target_machine, mpm); - LLVMAddPruneEHPass(mpm); - if (optimization_level <= 0) { - return; - } - - LLVMAddGlobalDCEPass(mpm); - - if (optimization_level >= 2) { - // NOTE(bill, 2021-03-29: use this causes invalid code generation) - // LLVMPassManagerBuilderRef pmb = LLVMPassManagerBuilderCreate(); - // LLVMPassManagerBuilderSetOptLevel(pmb, optimization_level); - // LLVMPassManagerBuilderPopulateModulePassManager(pmb, mpm); - // LLVMPassManagerBuilderPopulateLTOPassManager(pmb, mpm, false, true); - // return; - } - - - LLVMAddIPSCCPPass(mpm); - LLVMAddCalledValuePropagationPass(mpm); - - LLVMAddGlobalOptimizerPass(mpm); - LLVMAddDeadArgEliminationPass(mpm); - - LLVMAddCFGSimplificationPass(mpm); - - LLVMAddPruneEHPass(mpm); - if (optimization_level < 2) { - return; - } - - LLVMAddFunctionInliningPass(mpm); - - - lb_add_function_simplifcation_passes(mpm, optimization_level); - - LLVMAddGlobalDCEPass(mpm); - LLVMAddGlobalOptimizerPass(mpm); - - - LLVMAddLoopRotatePass(mpm); - - LLVMAddLoopVectorizePass(mpm); - - if (optimization_level >= 2) { - LLVMAddEarlyCSEPass(mpm); - LLVMAddLICMPass(mpm); - LLVMAddLoopUnswitchPass(mpm); - LLVMAddCFGSimplificationPass(mpm); - } - - LLVMAddCFGSimplificationPass(mpm); - - LLVMAddSLPVectorizePass(mpm); - LLVMAddLICMPass(mpm); - - LLVMAddAlignmentFromAssumptionsPass(mpm); - - LLVMAddStripDeadPrototypesPass(mpm); - - if (optimization_level >= 2) { - LLVMAddGlobalDCEPass(mpm); - LLVMAddConstantMergePass(mpm); - } - - LLVMAddCFGSimplificationPass(mpm); -#endif -} - - - /************************************************************************** IMPORTANT NOTE(bill, 2021-11-06): Custom Passes diff --git a/src/llvm_backend_proc.cpp b/src/llvm_backend_proc.cpp index 7994ab4ba..9f1b5dd3f 100644 --- a/src/llvm_backend_proc.cpp +++ b/src/llvm_backend_proc.cpp @@ -452,7 +452,9 @@ gb_internal lbProcedure *lb_create_dummy_procedure(lbModule *m, String link_name Type *pt = p->type; lbCallingConventionKind cc_kind = lbCallingConvention_C; - if (!is_arch_wasm()) { + if (selected_subtarget == Subtarget_Playdate) { + cc_kind = lbCallingConvention_ARM_AAPCS_VFP; + } else if (!is_arch_wasm()) { cc_kind = lb_calling_convention_map[pt->Proc.calling_convention]; } LLVMSetFunctionCallConv(p->value, cc_kind); @@ -846,8 +848,9 @@ gb_internal void lb_build_nested_proc(lbProcedure *p, AstProcLit *pd, Entity *e) GB_ASSERT(pd->body != nullptr); lbModule *m = p->module; - if (e->min_dep_count.load(std::memory_order_relaxed) == 0) { + if (e->min_dep_count.load(std::memory_order_relaxed) == 0 || e->code_gen_procedure != nullptr) { // NOTE(bill): Nothing depends upon it so doesn't need to be built + // NOTE(tf2spi): Code may already be generated (i.e. loop unrolling), don't regen it return; } @@ -965,8 +968,7 @@ gb_internal lbValue lb_emit_call_internal(lbProcedure *p, lbValue value, lbValue for (unsigned i = 0; i < param_count; i++) { LLVMTypeRef param_type = param_types[i]; LLVMTypeRef arg_type = LLVMTypeOf(args[i]); - if (LB_USE_NEW_PASS_SYSTEM && - arg_type != param_type) { + if (arg_type != param_type) { LLVMTypeKind arg_kind = LLVMGetTypeKind(arg_type); LLVMTypeKind param_kind = LLVMGetTypeKind(param_type); if (arg_kind == param_kind) { @@ -994,6 +996,9 @@ gb_internal lbValue lb_emit_call_internal(lbProcedure *p, lbValue value, lbValue LLVMValueRef ret = LLVMBuildCall2(p->builder, fnp, fn, args, arg_count, ""); auto llvm_cc = lb_calling_convention_map[proc_type->Proc.calling_convention]; + if (selected_subtarget == Subtarget_Playdate) { + llvm_cc = lbCallingConvention_ARM_AAPCS_VFP; + } LLVMSetInstructionCallConv(ret, llvm_cc); LLVMAttributeIndex param_offset = LLVMAttributeIndex_FirstArgIndex; @@ -1693,7 +1698,8 @@ gb_internal lbValue lb_build_builtin_simd_proc(lbProcedure *p, Ast *expr, TypeAn return res; case BuiltinProc_simd_min: if (is_float) { - return lb_emit_min(p, res.type, arg0, arg1); + LLVMValueRef cond = LLVMBuildFCmp(p->builder, LLVMRealOLT, arg0.value, arg1.value, ""); + res.value = LLVMBuildSelect(p->builder, cond, arg0.value, arg1.value, ""); } else { LLVMValueRef cond = LLVMBuildICmp(p->builder, is_signed ? LLVMIntSLT : LLVMIntULT, arg0.value, arg1.value, ""); res.value = LLVMBuildSelect(p->builder, cond, arg0.value, arg1.value, ""); @@ -1701,7 +1707,8 @@ gb_internal lbValue lb_build_builtin_simd_proc(lbProcedure *p, Ast *expr, TypeAn return res; case BuiltinProc_simd_max: if (is_float) { - return lb_emit_max(p, res.type, arg0, arg1); + LLVMValueRef cond = LLVMBuildFCmp(p->builder, LLVMRealOGT, arg0.value, arg1.value, ""); + res.value = LLVMBuildSelect(p->builder, cond, arg0.value, arg1.value, ""); } else { LLVMValueRef cond = LLVMBuildICmp(p->builder, is_signed ? LLVMIntSGT : LLVMIntUGT, arg0.value, arg1.value, ""); res.value = LLVMBuildSelect(p->builder, cond, arg0.value, arg1.value, ""); @@ -4762,14 +4769,22 @@ gb_internal lbValue lb_build_call_expr(lbProcedure *p, Ast *expr, lbValue *sret_ return res; } -gb_internal void lb_add_values_to_array(lbProcedure *p, Array *args, lbValue value) { +gb_internal void lb_add_values_to_array(lbProcedure *p, Array *args, lbValue value, Type *c_vararg_type = nullptr) { if (is_type_tuple(value.type)) { for_array(i, value.type->Tuple.variables) { lbValue sub_value = lb_emit_struct_ev(p, value, cast(i32)i); - array_add(args, sub_value); + if (c_vararg_type) { + array_add(args, lb_emit_c_vararg(p, sub_value, c_vararg_type)); + } else { + array_add(args, sub_value); + } } } else { - array_add(args, value); + if (c_vararg_type) { + array_add(args, lb_emit_c_vararg(p, value, c_vararg_type)); + } else { + array_add(args, value); + } } } @@ -4890,9 +4905,9 @@ gb_internal lbValue lb_build_call_expr_internal(lbProcedure *p, Ast *expr, lbVal if (is_type_untyped_nil(arg.type)) { arg = lb_const_nil(p->module, t_rawptr); } - array_add(&args, lb_emit_c_vararg(p, arg, arg.type)); + lb_add_values_to_array(p, &args, arg, arg.type); } else { - array_add(&args, lb_emit_c_vararg(p, arg, elem_type)); + lb_add_values_to_array(p, &args, arg, elem_type); } } break; @@ -5018,15 +5033,15 @@ gb_internal lbValue lb_build_call_expr_internal(lbProcedure *p, Ast *expr, lbVal if (is_type_untyped_nil(arg.type)) { arg = lb_const_nil(p->module, t_rawptr); } - array_add(&args, lb_emit_c_vararg(p, arg, arg.type)); + lb_add_values_to_array(p, &args, arg, arg.type); } else { - array_add(&args, lb_emit_c_vararg(p, arg, elem_type)); + lb_add_values_to_array(p, &args, arg, elem_type); } } } else { lbValue value = lb_build_expr(p, fv->value); GB_ASSERT(!is_type_tuple(value.type)); - array_add(&args, lb_emit_c_vararg(p, value, value.type)); + lb_add_values_to_array(p, &args, value, value.type); } } else { lbValue value = lb_build_expr(p, fv->value); diff --git a/src/llvm_backend_utility.cpp b/src/llvm_backend_utility.cpp index a92e8610e..e00254bc5 100644 --- a/src/llvm_backend_utility.cpp +++ b/src/llvm_backend_utility.cpp @@ -153,7 +153,7 @@ gb_internal lbValue lb_emit_select(lbProcedure *p, lbValue cond, lbValue x, lbVa gb_internal lbValue lb_emit_min(lbProcedure *p, Type *t, lbValue x, lbValue y) { x = lb_emit_conv(p, x, t); y = lb_emit_conv(p, y, t); - bool use_llvm_intrinsic = !is_arch_wasm() && (is_type_float(t) || (is_type_simd_vector(t) && is_type_float(base_array_type(t)))); + bool use_llvm_intrinsic = !is_arch_wasm() && is_type_float(t); if (use_llvm_intrinsic) { LLVMValueRef args[2] = {x.value, y.value}; LLVMTypeRef types[1] = {lb_type(p->module, t)}; @@ -169,7 +169,7 @@ gb_internal lbValue lb_emit_min(lbProcedure *p, Type *t, lbValue x, lbValue y) { gb_internal lbValue lb_emit_max(lbProcedure *p, Type *t, lbValue x, lbValue y) { x = lb_emit_conv(p, x, t); y = lb_emit_conv(p, y, t); - bool use_llvm_intrinsic = !is_arch_wasm() && (is_type_float(t) || (is_type_simd_vector(t) && is_type_float(base_array_type(t)))); + bool use_llvm_intrinsic = !is_arch_wasm() && is_type_float(t); if (use_llvm_intrinsic) { LLVMValueRef args[2] = {x.value, y.value}; LLVMTypeRef types[1] = {lb_type(p->module, t)}; @@ -1206,18 +1206,19 @@ gb_internal lbValue lb_emit_struct_ep_internal(lbProcedure *p, lbValue s, i32 in i32 original_index = index; index = lb_convert_struct_index(p->module, t, index); + lbModule *m = p->module; if (lb_is_const(s)) { // NOTE(bill): this cannot be replaced with lb_emit_epi - lbModule *m = p->module; lbValue res = {}; LLVMValueRef indices[2] = {llvm_zero(m), LLVMConstInt(lb_type(m, t_i32), index, false)}; - res.value = LLVMConstGEP2(lb_type(m, type_deref(s.type)), s.value, indices, gb_count_of(indices)); res.type = alloc_type_pointer(result_type); + res.value = LLVMConstGEP2(lb_type(m, type_deref(s.type)), s.value, indices, gb_count_of(indices)); + res.value = LLVMConstPointerCast(res.value, lb_type(m, res.type)); return res; } else { lbValue res = {}; - LLVMTypeRef st = lb_type(p->module, type_deref(s.type)); + LLVMTypeRef st = lb_type(m, type_deref(s.type)); // gb_printf_err("%s\n", type_to_string(s.type)); // gb_printf_err("%s\n", LLVMPrintTypeToString(LLVMTypeOf(s.value))); // gb_printf_err("%d\n", index); @@ -1225,8 +1226,9 @@ gb_internal lbValue lb_emit_struct_ep_internal(lbProcedure *p, lbValue s, i32 in unsigned count = LLVMCountStructElementTypes(st); GB_ASSERT_MSG(count >= cast(unsigned)index, "%u %d %d", count, index, original_index); - res.value = LLVMBuildStructGEP2(p->builder, st, s.value, cast(unsigned)index, ""); res.type = alloc_type_pointer(result_type); + res.value = LLVMBuildStructGEP2(p->builder, st, s.value, cast(unsigned)index, ""); + res.value = LLVMBuildPointerCast(p->builder, res.value, lb_type(m, res.type), ""); return res; } } @@ -1274,6 +1276,17 @@ gb_internal lbValue lb_emit_struct_ep(lbProcedure *p, lbValue s, i32 index) { case 1: result_type = ft; break; case 2: result_type = ft; break; case 3: result_type = ft; break; + case -1: + { + lbValue res = {}; + res.type = alloc_type_pointer(alloc_type_array(ft, 3)); + if (lb_is_const(s)) { + res.value = LLVMConstPointerCast(s.value, lb_type(p->module, res.type)); + } else { + res.value = LLVMBuildPointerCast(p->builder, s.value, lb_type(p->module, res.type), ""); + } + return res; + } } } else if (is_type_slice(t)) { switch (index) { @@ -1443,6 +1456,11 @@ gb_internal lbValue lb_emit_struct_ev(lbProcedure *p, lbValue s, i32 index) { case 1: result_type = ft; break; case 2: result_type = ft; break; case 3: result_type = ft; break; + case -1: { + lbValue ptr_q = lb_address_from_load_or_generate_local(p, s); + lbValue ptr_xyz = lb_emit_struct_ep(p, ptr_q, -1); + return lb_emit_load(p, ptr_xyz); + } } break; } diff --git a/src/main.cpp b/src/main.cpp index da0e6c5b4..c4b4441d8 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -875,7 +875,7 @@ gb_internal bool parse_build_flags(Array args) { } else if (value.value_string == "speed") { build_context.custom_optimization_level = true; build_context.optimization_level = 2; - } else if (value.value_string == "aggressive" && LB_USE_NEW_PASS_SYSTEM) { + } else if (value.value_string == "aggressive") { build_context.custom_optimization_level = true; build_context.optimization_level = 3; } else { @@ -884,9 +884,7 @@ gb_internal bool parse_build_flags(Array args) { gb_printf_err("\tminimal\n"); gb_printf_err("\tsize\n"); gb_printf_err("\tspeed\n"); - if (LB_USE_NEW_PASS_SYSTEM) { - gb_printf_err("\taggressive\n"); - } + gb_printf_err("\taggressive\n"); gb_printf_err("\tnone (useful for -debug builds)\n"); bad_flags = true; } @@ -1199,8 +1197,10 @@ gb_internal bool parse_build_flags(Array args) { bool found = false; if (selected_target_metrics->metrics->os != TargetOs_darwin && - selected_target_metrics->metrics->os != TargetOs_linux ) { - gb_printf_err("-subtarget can only be used with darwin and linux based targets at the moment\n"); + selected_target_metrics->metrics->os != TargetOs_linux && + (selected_target_metrics->metrics->os != TargetOs_freestanding || + selected_target_metrics->metrics->arch != TargetArch_arm32)) { + gb_printf_err("-subtarget can only be used with darwin, linux or freestanding_arm32 based targets at the moment\n"); bad_flags = true; break; } @@ -2743,10 +2743,11 @@ gb_internal int print_show_help(String const arg0, String command, String option if (check) { if (print_flag("-collection:=")) { - print_usage_line(2, "Defines a library collection used for imports."); + print_usage_line(2, "Defines a library collection used for imports and foreign imports."); print_usage_line(2, "Example: -collection:shared=dir/to/shared"); print_usage_line(2, "Usage in Code:"); print_usage_line(3, "import \"shared:foo\""); + print_usage_line(3, "foreign import lib \"shared:libfoo.a\""); } if (print_flag("-custom-attribute:")) { @@ -3041,9 +3042,7 @@ gb_internal int print_show_help(String const arg0, String command, String option print_usage_line(3, "-o:minimal"); print_usage_line(3, "-o:size"); print_usage_line(3, "-o:speed"); - if (LB_USE_NEW_PASS_SYSTEM) { print_usage_line(3, "-o:aggressive (use this with caution)"); - } print_usage_line(2, "The default is -o:minimal. If -debug is set, the default is -o:none."); } @@ -3102,7 +3101,6 @@ gb_internal int print_show_help(String const arg0, String command, String option if (print_flag("-stack-protector:")) { print_usage_line(2, "Specifies the stack protector."); print_usage_line(2, "Available options:"); - print_usage_line(3, "-stack-protector:default"); print_usage_line(3, "-stack-protector:none"); print_usage_line(3, "-stack-protector:base"); print_usage_line(3, "-stack-protector:all"); @@ -3202,7 +3200,7 @@ gb_internal int print_show_help(String const arg0, String command, String option if (build) { if (print_flag("-subtarget:")) { - print_usage_line(2, "[Darwin and Linux only]"); + print_usage_line(2, "[Darwin, Linux and Freestanding ARM32 only]"); print_usage_line(2, "Available subtargets:"); String prefix = str_lit("-subtarget:"); for (u32 i = 1; i < Subtarget_COUNT; i++) { @@ -4219,12 +4217,6 @@ int main(int arg_count, char const **arg_ptr) { gb_printf_err("missing required target feature: \"%.*s\", enable it by setting a different -microarch or explicitly adding it through -target-features\n", LIT(disabled)); gb_exit(1); } - - // NOTE(laytan): some weird errors on LLVM 14 that LLVM 17 fixes. - if (LLVM_VERSION_MAJOR < 17) { - gb_printf_err("Invalid LLVM version %s, RISC-V targets require at least LLVM 17\n", LLVM_VERSION_STRING); - gb_exit(1); - } } if (build_context.show_debug_messages) { diff --git a/src/microsoft_craziness.h b/src/microsoft_craziness.h index 933607a2a..56af7252d 100644 --- a/src/microsoft_craziness.h +++ b/src/microsoft_craziness.h @@ -761,17 +761,27 @@ gb_internal void find_visual_studio_paths_from_env_vars(Find_Result *result) { gb_internal Find_Result find_visual_studio_and_windows_sdk() { Find_Result r = {}; find_windows_kit_paths(&r); - find_visual_studio_by_fighting_through_microsoft_craziness(&r); + + // Prefer the toolset exported into the environment (e.g. by vsdevcmd) so the + // linker uses the same MSVC toolset of other externally compiled C++ dependencies. + // The COM autodetection grabs the first installed Visual Studio, which may be an older + // toolset whose runtime libraries lack symbols that the newer-built libraries + // reference. Fall back to autodetection when the environment is bare. + find_visual_studio_paths_from_env_vars(&r); + + bool vs_found = + r.vs_exe_path.len && + r.vs_library_path.len ; + + if (!vs_found) { + find_visual_studio_by_fighting_through_microsoft_craziness(&r); + } bool sdk_found = r.windows_sdk_bin_path.len && r.windows_sdk_um_library_path.len && r.windows_sdk_ucrt_library_path.len ; - bool vs_found = - r.vs_exe_path.len && - r.vs_library_path.len ; - if (!sdk_found) { find_windows_kit_paths_from_env_vars(&r); } diff --git a/src/name_canonicalization.cpp b/src/name_canonicalization.cpp index 67ca9e278..13fa01830 100644 --- a/src/name_canonicalization.cpp +++ b/src/name_canonicalization.cpp @@ -516,9 +516,17 @@ gb_internal u64 type_hash_canonical_type(Type *type) { return prev_hash; } + // NOTE(tf2spi): Unwrap type aliases similar to are_types_identical* + Type *type_unaliased = type; + if (type->kind == Type_Named) { + Entity *e = type->Named.type_name; + if (e->TypeName.is_type_alias) { + type_unaliased = type->Named.base; + } + } TypeWriter w = {}; type_writer_make_hasher(&w, &w.hash_ctx); - write_type_to_canonical_string(&w, type); + write_type_to_canonical_string(&w, type_unaliased); u64 hash = typeid_hash_context_fini(&w.hash_ctx); if (build_context.webkit_switch_workaround) { // Clear the top bit so every `typeid` is in [1, 2^63). A `switch` over a @@ -972,6 +980,12 @@ gb_internal void write_type_to_canonical_string(TypeWriter *w, Type *type) { type_writer_appendc(w, "->"); write_canonical_params(w, type->Proc.results); } + if (type->Proc.diverging) { + type_writer_appendc(w, "!"); + } + if (type->Proc.optional_ok) { + type_writer_appendc(w, "#optional_ok"); + } return; case Type_Generic: @@ -1010,4 +1024,4 @@ gb_internal void write_type_to_canonical_string(TypeWriter *w, Type *type) { } return; -} \ No newline at end of file +} diff --git a/src/parser.cpp b/src/parser.cpp index 03e9073ca..64112e085 100644 --- a/src/parser.cpp +++ b/src/parser.cpp @@ -1529,6 +1529,9 @@ gb_internal Token advance_token(AstFile *f) { switch (f->curr_token.kind) { case Token_Comment: consume_comment_groups(f, prev); + if (f->curr_token.kind == Token_Semicolon && ignore_newlines(f) && f->curr_token.string == "\n") { + advance_token(f); + } break; case Token_Semicolon: if (ignore_newlines(f) && f->curr_token.string == "\n") { @@ -3582,7 +3585,9 @@ gb_internal Ast *parse_binary_expr(AstFile *f, bool lhs, i32 prec_in) { case Token_when: if (prev.pos.line < op.pos.line) { // NOTE(bill): Check to see if the `if` or `when` is on the same line of the `lhs` condition - goto loop_end; + if (f->expr_level <= 0) { + goto loop_end; + } } break; } @@ -6049,6 +6054,19 @@ gb_internal bool is_import_path_valid(String const &path) { return false; } +gb_internal bool is_import_path_absolute(String const &path) { + if (path.len > 0 && path[0] == '/') { + return true; + } + if (path.len > 2 && + gb_char_is_alpha(path[0]) && + path[1] == ':' && + (path[2] == '/' || path[2] == '\\')) { + return true; + } + return false; +} + gb_internal bool is_build_flag_path_valid(String const &path) { if (path.len > 0) { u8 *start = path.text; @@ -6111,12 +6129,13 @@ gb_internal bool determine_path_from_string(BlockingMutex *file_mutex, Ast *node do_warning = &syntax_warning; if (use_check_errors) { do_error = &error; - do_error = &warning; + do_warning = &warning; } // NOTE(bill): if file_mutex == nullptr, this means that the code is used within the semantics stage String collection_name = {}; + bool is_import_decl_path = node->kind == Ast_ImportDecl || node->kind == Ast_ForeignImportDecl; isize colon_pos = -1; for (isize j = 0; j < original_string.len; j++) { @@ -6129,11 +6148,13 @@ gb_internal bool determine_path_from_string(BlockingMutex *file_mutex, Ast *node bool has_windows_drive = false; #if defined(GB_SYSTEM_WINDOWS) if (file_mutex == nullptr) { - if (colon_pos == 1 && original_string.len > 2) { - if (original_string[2] == '/' || original_string[2] == '\\') { - colon_pos = -1; - has_windows_drive = true; - } + if (!is_import_decl_path && + colon_pos == 1 && + original_string.len > 2 && + gb_char_is_alpha(original_string[0]) && + (original_string[2] == '/' || original_string[2] == '\\')) { + colon_pos = -1; + has_windows_drive = true; } for (isize i = 0; i < original_string.len; i++) { @@ -6157,6 +6178,10 @@ gb_internal bool determine_path_from_string(BlockingMutex *file_mutex, Ast *node file_str = original_string; } + if (is_import_decl_path && is_import_path_absolute(file_str)) { + do_error(node, "Invalid import path: '%.*s'", LIT(file_str)); + return false; + } if (has_windows_drive) { String sub_file_path = substring(file_str, 3, file_str.len); @@ -6229,8 +6254,7 @@ gb_internal bool determine_path_from_string(BlockingMutex *file_mutex, Ast *node if (has_windows_drive) { *path = file_str; } else { - bool ok = false; - String fullpath = string_trim_whitespace(get_fullpath_relative(permanent_allocator(), base_dir, file_str, &ok)); + String fullpath = string_trim_whitespace(get_fullpath_relative(permanent_allocator(), base_dir, file_str, nullptr)); *path = fullpath; } return true; @@ -6282,6 +6306,12 @@ gb_internal void parse_setup_file_decls(Parser *p, AstFile *f, String const &bas ast_node(id, ImportDecl, node); String original_string = string_trim_whitespace(string_value_from_token(f, id->relpath)); + if (is_import_path_absolute(original_string)) { + syntax_error(node, "Invalid import path: '%.*s'", LIT(original_string)); + decls[i] = ast_bad_decl(f, id->relpath, id->relpath); + continue; + } + String import_path = {}; bool ok = determine_path_from_string(&p->file_decl_mutex, node, base_dir, original_string, &import_path); if (!ok) { @@ -6308,6 +6338,12 @@ gb_internal void parse_setup_file_decls(Parser *p, AstFile *f, String const &bas GB_ASSERT(fp->kind == Ast_BasicLit); Token fp_token = fp->BasicLit.token; String file_str = string_trim_whitespace(string_value_from_token(f, fp_token)); + if (is_import_path_absolute(file_str)) { + syntax_error(node, "Invalid import path: '%.*s'", LIT(file_str)); + decls[i] = ast_bad_decl(f, fp_token, fp_token); + goto end; + } + String fullpath = file_str; if (!is_arch_wasm() || string_ends_with(fullpath, str_lit(".o"))) { String foreign_path = {}; @@ -7095,4 +7131,3 @@ gb_internal ParseFileError parse_packages(Parser *p, String init_filename) { return ParseFile_None; } - diff --git a/src/threading.cpp b/src/threading.cpp index bc92ad3f2..524c70e29 100644 --- a/src/threading.cpp +++ b/src/threading.cpp @@ -643,7 +643,7 @@ gb_internal void thread_init(ThreadPool *pool, Thread *t, isize idx) { gb_internal void thread_init_and_start(ThreadPool *pool, Thread *t, isize idx) { thread_init(pool, t, idx); - isize stack_size = 0; + isize stack_size = 1 * 1024 * 1024; // 1 MiB (LLVM takes a lot of stack space) #if defined(GB_SYSTEM_WINDOWS) t->win32_handle = CreateThread(NULL, stack_size, internal_thread_proc, t, 0, NULL); @@ -1067,4 +1067,4 @@ void atomic_freelist_put(std::atomic *> &head_list, AtomicFree } } -} \ No newline at end of file +} diff --git a/src/types.cpp b/src/types.cpp index d85f01c41..a71e22f9f 100644 --- a/src/types.cpp +++ b/src/types.cpp @@ -1471,6 +1471,10 @@ gb_internal bool is_type_constant_type(Type *t) { return is_type_constant_type(t->Array.elem); case Type_EnumeratedArray: return is_type_constant_type(t->EnumeratedArray.elem); + case Type_SimdVector: + return is_type_constant_type(t->SimdVector.elem); + case Type_Matrix: + return is_type_constant_type(t->Matrix.elem); } return false; } @@ -1525,7 +1529,7 @@ gb_internal bool is_type_multi_pointer(Type *t) { return t->kind == Type_MultiPointer; } gb_internal bool is_type_internally_pointer_like(Type *t) { - return is_type_pointer(t) || is_type_multi_pointer(t) || is_type_cstring(t) || is_type_proc(t); + return is_type_pointer(t) || is_type_multi_pointer(t) || is_type_cstring(t) || is_type_cstring16(t) || is_type_proc(t); } gb_internal bool is_type_tuple(Type *t) { @@ -3977,6 +3981,9 @@ gb_internal Selection lookup_field_with_selection(Type *type_, InternedString fi gb_local_persist Entity *entity__y = alloc_entity_field(nullptr, make_token_ident(y), t_f16, false, 1); gb_local_persist Entity *entity__z = alloc_entity_field(nullptr, make_token_ident(z), t_f16, false, 2); + gb_local_persist String xyz = str_lit("xyz"); + gb_local_persist Entity *entity__xyz = alloc_entity_field(nullptr, make_token_ident(xyz), alloc_type_array(t_f16, 3), false, -1); + String n = field_name.string(); if (n == w) { selection_add_index(&sel, 3); @@ -3994,6 +4001,10 @@ gb_internal Selection lookup_field_with_selection(Type *type_, InternedString fi selection_add_index(&sel, 2); sel.entity = entity__z; return sel; + } else if (n == xyz) { + selection_add_index(&sel, -1); + sel.entity = entity__xyz; + return sel; } } break; @@ -4008,6 +4019,9 @@ gb_internal Selection lookup_field_with_selection(Type *type_, InternedString fi gb_local_persist Entity *entity__y = alloc_entity_field(nullptr, make_token_ident(y), t_f32, false, 1); gb_local_persist Entity *entity__z = alloc_entity_field(nullptr, make_token_ident(z), t_f32, false, 2); + gb_local_persist String xyz = str_lit("xyz"); + gb_local_persist Entity *entity__xyz = alloc_entity_field(nullptr, make_token_ident(xyz), alloc_type_array(t_f32, 3), false, -1); + String n = field_name.string(); if (n == w) { selection_add_index(&sel, 3); @@ -4025,6 +4039,10 @@ gb_internal Selection lookup_field_with_selection(Type *type_, InternedString fi selection_add_index(&sel, 2); sel.entity = entity__z; return sel; + } else if (n == xyz) { + selection_add_index(&sel, -1); + sel.entity = entity__xyz; + return sel; } } break; @@ -4039,6 +4057,9 @@ gb_internal Selection lookup_field_with_selection(Type *type_, InternedString fi gb_local_persist Entity *entity__y = alloc_entity_field(nullptr, make_token_ident(y), t_f64, false, 1); gb_local_persist Entity *entity__z = alloc_entity_field(nullptr, make_token_ident(z), t_f64, false, 2); + gb_local_persist String xyz = str_lit("xyz"); + gb_local_persist Entity *entity__xyz = alloc_entity_field(nullptr, make_token_ident(xyz), alloc_type_array(t_f64, 3), false, -1); + String n = field_name.string(); if (n == w) { selection_add_index(&sel, 3); @@ -4056,6 +4077,10 @@ gb_internal Selection lookup_field_with_selection(Type *type_, InternedString fi selection_add_index(&sel, 2); sel.entity = entity__z; return sel; + } else if (n == xyz) { + selection_add_index(&sel, -1); + sel.entity = entity__xyz; + return sel; } } break; diff --git a/src/unicode.cpp b/src/unicode.cpp index cb9fb12ab..b38c6d601 100644 --- a/src/unicode.cpp +++ b/src/unicode.cpp @@ -127,7 +127,7 @@ gb_internal isize utf8_decode(u8 const *str, isize str_len, Rune *codepoint_out) sz = x&7; accept = global__utf8_accept_ranges[x>>4]; - if (str_len < gb_size_of(sz)) + if (str_len < sz) goto invalid_codepoint; b1 = str[1]; diff --git a/tests/benchmark/crypto/benchmark_rsa.odin b/tests/benchmark/crypto/benchmark_rsa.odin new file mode 100644 index 000000000..5954662fc --- /dev/null +++ b/tests/benchmark/crypto/benchmark_rsa.odin @@ -0,0 +1,160 @@ +package benchmark_core_crypto + +import "base:runtime" +import "core:log" +import "core:testing" +import "core:text/table" +import "core:time" + +import "core:crypto" +import "core:crypto/rsa" + +// RSA key generation is time consuming and high variance, so it takes +// an unreasonable amount of time to get a semi-sensible value, so this +// is skipped by default. +RSA_BENCH_KEYGEN: bool : #config(ODIN_BENCHMARK_RSA_KEYGEN, false) + +@(private = "file") +KEYGEN_ITERS :: 100 +@(private = "file") +SIGN_ITERS :: 5000 +@(private = "file") +ENCRYPT_ITERS :: 5000 + +@(test) +benchmark_crypto_rsa :: proc(t: ^testing.T) { + runtime.DEFAULT_TEMP_ALLOCATOR_TEMP_GUARD() + + tbl: table.Table + table.init(&tbl) + defer table.destroy(&tbl) + + table.caption(&tbl, "RSA") + table.aligned_header_of_values(&tbl, .Right, "Operation", "Avg. Time") + + if RSA_BENCH_KEYGEN { + bench_keygen_2048(&tbl) + table.row_of_values(&tbl) + } + bench_pkcs1_2048(&tbl) + table.row_of_values(&tbl) + bench_pss_2048(&tbl) + table.row_of_values(&tbl) + bench_oaep_2048(&tbl) + + log_table(&tbl) +} + +@(private="file") +bench_keygen_2048 :: proc(tbl: ^table.Table) { + if !crypto.HAS_RAND_BYTES { + log.warnf("rsa: keygen benchmarks skipped, no system entropy source") + } + + priv_key: rsa.Private_Key + start := time.tick_now() + for _ in 0 ..< KEYGEN_ITERS { + ok := rsa.private_key_generate(&priv_key, 2048) + assert(ok, "keygen should succeed") + } + taken := time.tick_since(start) / KEYGEN_ITERS + + append_tbl(tbl, "Keygen/2048", taken) +} + +@(private="file") +bench_pkcs1_2048 :: proc(tbl: ^table.Table) { + priv_key: rsa.Private_Key + _ = rsa.private_key_set_insecure_test(&priv_key) + + msg_bytes := transmute([]byte)(SIG_MSG) + sig_bytes: [2048 >> 3]byte + + start := time.tick_now() + for _ in 0 ..< SIGN_ITERS { + ok := rsa.sign_pkcs1(&priv_key, .SHA256, msg_bytes, sig_bytes[:]) + assert(ok, "signing should succeed") + } + taken := time.tick_since(start) / SIGN_ITERS + + append_tbl(tbl, "PKCS1/2048/SHA256/sign", taken) + + start = time.tick_now() + for _ in 0 ..< KEYGEN_ITERS { + ok := rsa.verify_pkcs1(&priv_key._pub_key, .SHA256, msg_bytes, sig_bytes[:]) + assert(ok, "verify should succeed") + } + taken = time.tick_since(start) / SIGN_ITERS + + append_tbl(tbl, "PKCS1/2048/SHA256/verify", taken) +} + +@(private="file") +bench_pss_2048 :: proc(tbl: ^table.Table) { + priv_key: rsa.Private_Key + _ = rsa.private_key_set_insecure_test(&priv_key) + + msg_bytes := transmute([]byte)(SIG_MSG) + sig_bytes: [2048 >> 3]byte + + start := time.tick_now() + for _ in 0 ..< SIGN_ITERS { + ok := rsa.sign_pss(&priv_key, .SHA256, 32, msg_bytes, sig_bytes[:]) + assert(ok, "signing should succeed") + } + taken := time.tick_since(start) / SIGN_ITERS + + append_tbl(tbl, "PSS/2048/SHA256/sign", taken) + + start = time.tick_now() + for _ in 0 ..< KEYGEN_ITERS { + ok := rsa.verify_pss(&priv_key._pub_key, .SHA256, 32, msg_bytes, sig_bytes[:]) + assert(ok, "verify should succeed") + } + taken = time.tick_since(start) / SIGN_ITERS + + append_tbl(tbl, "PSS/2048/SHA256/verify", taken) +} + +@(private="file") +bench_oaep_2048 :: proc(tbl: ^table.Table) { + if !crypto.HAS_RAND_BYTES { + log.info("rand_bytes not supported - skipping") + return + } + + priv_key: rsa.Private_Key + _ = rsa.private_key_set_insecure_test(&priv_key) + + msg_bytes := transmute([]byte)(SIG_MSG) + ciphertext_bytes: [2048 >> 3]byte + buf: [32]byte + + start := time.tick_now() + for _ in 0 ..< SIGN_ITERS { + ok := rsa.encrypt_oaep(&priv_key._pub_key, .SHA256, msg_bytes, ciphertext_bytes[:]) + assert(ok, "encryption should succeed") + } + taken := time.tick_since(start) / ENCRYPT_ITERS + + append_tbl(tbl, "OAEP/2048/SHA256/encrypt", taken) + + start = time.tick_now() + for _ in 0 ..< KEYGEN_ITERS { + _, ok := rsa.decrypt_oaep(&priv_key, .SHA256, ciphertext_bytes[:], buf[:]) + assert(ok, "decrypt should succeed") + } + taken = time.tick_since(start) / ENCRYPT_ITERS + + append_tbl(tbl, "OAEP/2048/SHA256/decrypt", taken) +} + +@(private="file") +append_tbl :: proc(tbl: ^table.Table, op_name: string, avg_time: time.Duration) { + table.aligned_row_of_values( + tbl, + .Right, + op_name, + table.format(tbl, "%8M", avg_time), + ) +} diff --git a/tests/core/container/test_core_rbtree.odin b/tests/core/container/test_core_rbtree.odin index 78d710b7f..0cb2e1952 100644 --- a/tests/core/container/test_core_rbtree.odin +++ b/tests/core/container/test_core_rbtree.odin @@ -135,9 +135,30 @@ test_rbtree_integer :: proc(t: ^testing.T, $Key: typeid, $Value: typeid) { testing.expect(t, rb.len(tree) == entry_count - 1, "iterator/remove: len should drop by 1") rb.destroy(&tree) - testing.expect(t, rb.len(tree) == 0, "destroy: len should be 0") + testing.expect(t, rb.len(tree) == 0, "destroy: len should be 0") testing.expectf(t, callback_count == 0, "remove: on_remove should've been called %v times, it was %v", entry_count, callback_count) + // Test upsert + rb.init(&tree) + clear(&inserted_map) + + for i := 0; i < NR_INSERTS * 4; i += 1 { + k := Key(i) & 0x7 + v := Value(i) + + existing_node, in_map := inserted_map[k] + n, inserted, _ := rb.upsert(&tree, k, v) + testing.expect(t, in_map != inserted, "upsert: inserted should match inverse of map lookup") + if inserted { + inserted_map[k] = n + } else { + testing.expect(t, existing_node == n, "upsert: expecting existing node") + testing.expect_value(t, v, n.value) // And updated value + } + } + + rb.destroy(&tree) + // print_tree_node(tree._root) delete(inserted_map) delete(inserted_keys) diff --git a/tests/core/crypto/bigint/int.odin b/tests/core/crypto/bigint/int.odin new file mode 100644 index 000000000..c7db6c09a --- /dev/null +++ b/tests/core/crypto/bigint/int.odin @@ -0,0 +1,310 @@ +// Tests for the constant time RSA primitives +package test_core_crypto_bigint + +import "base:runtime" +import "core:crypto/_bigint" +import "core:log" +import "core:slice" +import "core:testing" + +ROUNDS :: 100_000 + +i31_equal :: proc(a, b: []u32) -> bool { + if a[0] != b[0] { return false } + + bits := uint(a[0]) + idx := 1 + for bits > 0 { + ex := min(bits, 31) + mask := u32(1< N || len(v.b) > N || len(v.res) > N { + log.infof("Skipped %v, not enough scratch space", v) + continue + } + if !(len(v.a) == len(v.b) && len(v.b) == len(v.res)) { + log.infof("Skipped %v, expected `a`, `b` and `res` lengths to be equal", v) + continue + } + + // Copy into writable memory + copy(res[:], v.a[:]) + + // Add b to "a" in place + cc := _bigint.i31_add(res[:], v.b[:], 1) + + testing.expect(t, slice.equal(res[:len(v.res)], v.res)) + testing.expect_value(t, cc, v.carry) + } +} + +@(test) +i31_sub :: proc(t: ^testing.T) { + N :: 5 + res: [N]u32 + + for v in i31_sub_test_vectors { + if len(v.a) > N || len(v.b) > N || len(v.res) > N { + log.infof("Skipped %v, not enough scratch space", v) + continue + } + if !(len(v.a) == len(v.b) && len(v.b) == len(v.res)) { + log.infof("Skipped %v, expected `a`, `b` and `res` lengths to be equal", v) + continue + } + + // Copy into writable memory + copy(res[:], v.a[:]) + + // Add b to "a" in place + cc := _bigint.i31_sub(res[:], v.b[:], 1) + + testing.expect(t, slice.equal(res[:len(v.res)], v.res)) + testing.expect_value(t, cc, v.carry) + } +} + +@(test) +i31_bit_length :: proc (t: ^testing.T) { + for v in i31_add_test_vectors { + a_len := _bigint.i31_bit_length(v.a[1:]) + b_len := _bigint.i31_bit_length(v.b[1:]) + + testing.expect_value(t, a_len, v.a[0]) + testing.expect_value(t, b_len, v.b[0]) + } +} + +@(test) +i31_decode :: proc(t: ^testing.T) { + N :: 10 + res: [N]u32 + + mod := []u32{42, 0x7fff_fffe, 0x3ff, 0} + + for v in i31_decode_test_vectors { + if len(v.decode) > N || len(v.mod) > N { + log.infof("Skipped %v, not enough scratch space", v) + continue + } + + _bigint.i31_decode(res[:], v.src) + testing.expect(t, slice.equal(res[:len(v.decode)], v.decode)) + + slice.zero(res[:]) + + mod_res := _bigint.i31_decode_mod(res[:], v.src, mod) + testing.expect(t, slice.equal(res[:len(v.mod)], v.mod)) + testing.expect_value(t, mod_res, v.mod_res) + + encoded: [32]u8 = 0 + _bigint.i31_encode(encoded[:], v.decode) + testing.expect(t, slice.equal(encoded[32 - len(v.src):], v.src)) + } +} + +@(test) +i31_rshift :: proc(t: ^testing.T) { + N :: 4 + res: [N]u32 + for v in i31_rshift_test_vectors { + if len(v.orig) > N || len(v.res) > N { + log.infof("Skipped %v, not enough scratch space", v) + continue + } + + if v.shift < 0 || v.shift > 31 { + log.infof("Skipped %v, invalid shift amount", v) + continue + } + + copy(res[:], v.orig) + _bigint.i31_rshift(res[:], v.shift) + testing.expect(t, slice.equal(res[:len(v.res)], v.res)) + } +} + +@(test) +i31_reduce :: proc(t: ^testing.T) { + N :: 12 + res: [N]u32 = --- + mod := []u32{42, 0x7fff_fffe, 0x3ff, 0} + + for v in i31_reduce_test_vectors { + if len(v.orig) > N || len(v.res) > N { + log.infof("Skipped %v, not enough scratch space", v) + continue + } + + slice.zero(res[:]) + _bigint.i31_reduce(res[:], v.orig, mod) + testing.expect(t, i31_equal(res[:], v.res)) + } +} + +@(test) +i31_decode_reduce :: proc(t: ^testing.T) { + N :: 4 + res: [N]u32 = 0 + mod := []u32{42, 0x7fff_fffe, 0x3ff, 0} + + for v in i31_decode_reduce_test_vectors { + if len(v.decode) > N { + log.infof("Skipped %v, not enough scratch space", v) + continue + } + + res = 0 + _bigint.i31_decode_reduce(res[:], v.src, mod) + testing.expect(t, i31_equal(res[:], v.decode)) + } +} + +@(test) +i31_muladd_small :: proc(t: ^testing.T) { + mod := []u32{42, 0x7fff_fffe, 0x3ff, 0} + + for v in i31_mul_add_test_vectors { + if len(v.orig) > len(mod) || len(v.res) > len(mod) { + log.infof("Skipped %v, not enough scratch space", v) + continue + } + + res: [3]u32 = 0 + copy(res[:], v.orig) + + _bigint.i31_muladd_small(res[:], v.z, mod) + l := len(v.res) + testing.expect(t, slice.equal(res[:l], v.res[:l])) + } +} + +@(test) +i31_encode :: proc(t: ^testing.T) { + for v in i31_encode_test_vectors { + decoded: [10]u32 = 0 + _bigint.i31_decode(decoded[:], v.encoded) + + l := len(v.orig) + testing.expect(t, slice.equal(decoded[:l], v.orig[:l])) + + encoded: [32]u8 = 0 + _bigint.i31_encode(encoded[:], v.orig) + + testing.expect(t, slice.equal(encoded[:], v.encoded)) + } +} + +@(test) +i31_monty_mul :: proc(t: ^testing.T) { + for v in i31_monty_mul_test_vectors { + res: [6]u32 = 0 + + m0i := _bigint.i31_ninv31(v.m[1]) + if m0i == 0 { + log.infof("Expected _bigint.i31_ninv31(%v) to not be 0, m[1] must be even. Skipped.", v.m[1]) + continue + } + + _bigint.i31_montymul(res[:], v.x, v.y, v.m, m0i) + testing.expect(t, slice.equal(res[:], v.res)) + } +} + +@(test) +i31_to_monty :: proc(t: ^testing.T) { + for v in i31_to_monty_test_vectors { + res: [6]u32 = 0 + copy(res[:], v.orig) + + _bigint.i31_to_monty(res[:], v.m) + testing.expect(t, slice.equal(res[:], v.x)) + + m0i := _bigint.i31_ninv31(v.m[1]) + if m0i == 0 { + log.infof("Expected _bigint.i31_ninv31(%v) to not be 0, m[1] must be even. Skipped.", v.m[1]) + continue + } + + _bigint.i31_from_monty(res[:], v.m, m0i) + testing.expect(t, slice.equal(res[:], v.orig)) + } +} + +@(test) +i31_modpow :: proc(t: ^testing.T) { + for v in i31_mod_pow_test_vectors { + x_out: [6]u32 + temp: [100]u32 + + copy(x_out[:], v.orig) + + m0i := _bigint.i31_ninv31(v.m[1]) + assert(m0i != 0) + _bigint.i31_modpow(x_out[:], v.e, v.m, m0i, temp[:6], temp[6:][:6]) + + testing.expect(t, slice.equal(x_out[:], v.x)) + } +} + +@(test) +i31_mulacc :: proc(t: ^testing.T) { + for v in i31_mul_acc_test_vectors { + res: [12]u32 = 0 + copy(res[:], v.d) + + assert(v.d[0] == v.a[0]) + + _bigint.i31_mulacc(res[:], v.a, v.b) + + testing.expect(t, slice.equal(res[:], v.res)) + } +} + +@(test) +internal_div_rem_u32 :: proc(t: ^testing.T) { + for v in i31_div_rem_test_vectors { + den := u64(v.hi) << 32 + u64(v.lo) + res := u64(v.quo) * u64(v.div) + u64(v.rem) + assert(den == res) + + quo, rem := _bigint.div_rem_u32(v.hi, v.lo, v.div) + testing.expect_value(t, quo, v.quo) + testing.expect_value(t, rem, v.rem) + } +} diff --git a/tests/core/crypto/bigint/test_vectors.odin b/tests/core/crypto/bigint/test_vectors.odin new file mode 100644 index 000000000..94cb41013 --- /dev/null +++ b/tests/core/crypto/bigint/test_vectors.odin @@ -0,0 +1,577 @@ +package test_core_crypto_bigint + +// Generated using BearSSL bindings. + +I31_Test_Vector_Binary :: struct { + a: []u32, + b: []u32, + res: []u32, + carry: u32, +} + +@(rodata) +i31_add_test_vectors := []I31_Test_Vector_Binary { + {a = {127, 0x051558fb, 0x578cb6cf, 0x3d2097aa, 0x7512fbf6}, b = {127, 0x6b2123f1, 0x4ad77a17, 0x705c1500, 0x508d965d}, res = {127, 0x70367cec, 0x226430e6, 0x2d7cacab, 0x45a09254}, carry = 1}, + {a = {127, 0x0248fd90, 0x7825baae, 0x6de2183c, 0x4f5d0505}, b = {125, 0x492ba26a, 0x1bc7df54, 0x5114950e, 0x1ae989b2}, res = {127, 0x4b749ffa, 0x13ed9a02, 0x3ef6ad4b, 0x6a468eb8}, carry = 0}, + {a = {126, 0x4833d6ef, 0x37a112d4, 0x2f71a920, 0x3093d1c3}, b = {127, 0x20574627, 0x39305532, 0x20b76825, 0x6e2eeea3}, res = {126, 0x688b1d16, 0x70d16806, 0x50291145, 0x1ec2c066}, carry = 1}, + {a = {127, 0x55fb7b29, 0x21bef5c8, 0x67fe29e8, 0x798dabac}, b = {127, 0x274221db, 0x562cbe9c, 0x785bf5dc, 0x7058bbfa}, res = {127, 0x7d3d9d04, 0x77ebb464, 0x605a1fc4, 0x69e667a7}, carry = 1}, + {a = {127, 0x2ffa533d, 0x59e20331, 0x59323704, 0x42cf6e75}, b = {127, 0x7ac812c0, 0x32ca1723, 0x15abc385, 0x410265b0}, res = {127, 0x2ac265fd, 0x0cac1a55, 0x6eddfa8a, 0x03d1d425}, carry = 1}, + {a = {126, 0x789b52f9, 0x6535ff6a, 0x2744cf6e, 0x2e4342a6}, b = {127, 0x32dd2f41, 0x64d80309, 0x1ae80d4b, 0x405d1799}, res = {126, 0x2b78823a, 0x4a0e0274, 0x422cdcba, 0x6ea05a3f}, carry = 0}, + {a = {127, 0x62003ba4, 0x2b58e836, 0x4587aced, 0x760bdb36}, b = {126, 0x0201e795, 0x50230e49, 0x30039ab5, 0x36f17032}, res = {127, 0x64022339, 0x7b7bf67f, 0x758b47a2, 0x2cfd4b68}, carry = 1}, + {a = {124, 0x4ef8170f, 0x65e714cc, 0x5500f2a4, 0x090fac28}, b = {127, 0x0a03290f, 0x0d3fafb1, 0x74ebb1b5, 0x7a30e9c6}, res = {124, 0x58fb401e, 0x7326c47d, 0x49eca459, 0x034095ef}, carry = 1}, + {a = {127, 0x34995a2f, 0x58f39080, 0x376ab5ef, 0x555b4fdf}, b = {122, 0x1c0928d9, 0x037fc1ea, 0x35c6d9b0, 0x0206ff68}, res = {127, 0x50a28308, 0x5c73526a, 0x6d318f9f, 0x57624f47}, carry = 0}, + {a = {127, 0x7029eb02, 0x465899f5, 0x71ab22fc, 0x604f1ccf}, b = {124, 0x703e163c, 0x50f72eb1, 0x0a0f0bae, 0x0a2028fe}, res = {127, 0x6068013e, 0x174fc8a7, 0x7bba2eab, 0x6a6f45cd}, carry = 0}, + {a = {125, 0x70cc03f6, 0x4d984b06, 0x4b990b9d, 0x16dd5097}, b = {125, 0x6eed2b72, 0x6001ddd2, 0x12c50807, 0x14f176cb}, res = {125, 0x5fb92f68, 0x2d9a28d9, 0x5e5e13a5, 0x2bcec762}, carry = 0}, + {a = {127, 0x5e6a99af, 0x3ffc66eb, 0x31e47de6, 0x5bcd2f4b}, b = {124, 0x0f6f4335, 0x07d4bf78, 0x61e99233, 0x0fe21745}, res = {127, 0x6dd9dce4, 0x47d12663, 0x13ce1019, 0x6baf4691}, carry = 0}, + {a = {127, 0x45de8b13, 0x010684b5, 0x01c77d90, 0x7464b4c8}, b = {127, 0x672a5a37, 0x33cb5670, 0x4ccc9e85, 0x7c0b88e1}, res = {127, 0x2d08e54a, 0x34d1db26, 0x4e941c15, 0x70703da9}, carry = 1}, + {a = {127, 0x012fe1a4, 0x741e0733, 0x44d3fb4a, 0x79d78453}, b = {126, 0x66d849d4, 0x427b3ff0, 0x5b2b72e8, 0x3c88987d}, res = {127, 0x68082b78, 0x36994723, 0x1fff6e33, 0x36601cd1}, carry = 1}, + {a = {125, 0x3ee54e09, 0x6bc71a68, 0x53cb44c8, 0x1f803699}, b = {124, 0x506ff07c, 0x63b155a0, 0x2c87590f, 0x0c7347e4}, res = {125, 0x0f553e85, 0x4f787009, 0x00529dd8, 0x2bf37e7e}, carry = 0}, + {a = {125, 0x4847d126, 0x4c6af2f2, 0x663970b3, 0x11d97a24}, b = {126, 0x67c8e79a, 0x313d0fc2, 0x786fdac1, 0x23c16f5c}, res = {125, 0x3010b8c0, 0x7da802b5, 0x5ea94b74, 0x359ae981}, carry = 0}, + {a = {127, 0x035a5f5c, 0x5fad2c0b, 0x6df6c2f3, 0x6cf28780}, b = {125, 0x09719328, 0x6adec2fa, 0x07d51f17, 0x1467d4f6}, res = {127, 0x0ccbf284, 0x4a8bef05, 0x75cbe20b, 0x015a5c76}, carry = 1}, + {a = {127, 0x48a484fe, 0x4a2eb1e7, 0x04a04363, 0x52dd3789}, b = {126, 0x136b08b1, 0x4d63762d, 0x52072be9, 0x2f4ef42b}, res = {127, 0x5c0f8daf, 0x17922814, 0x56a76f4d, 0x022c2bb4}, carry = 1}, + {a = {123, 0x119e8d05, 0x36510131, 0x38c3b084, 0x07bfe00d}, b = {127, 0x2ecdd4e9, 0x0afaddad, 0x77058092, 0x6b9f72dd}, res = {123, 0x406c61ee, 0x414bdede, 0x2fc93116, 0x735f52eb}, carry = 0}, + {a = {126, 0x459b051a, 0x6cfaeee6, 0x461ccc17, 0x36a7eb37}, b = {126, 0x53726a35, 0x34443d10, 0x2d29d052, 0x27d37b9d}, res = {126, 0x190d6f4f, 0x213f2bf7, 0x73469c6a, 0x5e7b66d4}, carry = 0}, + {a = {126, 0x73ccf104, 0x352c20d6, 0x580eeeca, 0x28ffb3bc}, b = {126, 0x1aba27fc, 0x31a85479, 0x05e9d689, 0x338a99a8}, res = {126, 0x0e871900, 0x66d47550, 0x5df8c553, 0x5c8a4d64}, carry = 0}, + {a = {127, 0x0220ed88, 0x4ccecbb5, 0x31016917, 0x7bc75c14}, b = {127, 0x19d9b7b6, 0x763136a7, 0x783931cf, 0x58fec945}, res = {127, 0x1bfaa53e, 0x4300025c, 0x293a9ae7, 0x54c6255a}, carry = 1}, + {a = {127, 0x14696b6a, 0x63348532, 0x71d09a50, 0x77ec660c}, b = {126, 0x67251ffd, 0x1f9606e1, 0x1205eb43, 0x32c3b9f7}, res = {127, 0x7b8e8b67, 0x02ca8c13, 0x03d68594, 0x2ab02004}, carry = 1}, + {a = {127, 0x570637d9, 0x22723669, 0x77a7b284, 0x4e1e69ac}, b = {127, 0x32c70945, 0x072c2f80, 0x7ef86c2f, 0x595e375a}, res = {127, 0x09cd411e, 0x299e65ea, 0x76a01eb3, 0x277ca107}, carry = 1}, + {a = {126, 0x47ab18d6, 0x4918a98b, 0x51c48283, 0x39a6cb59}, b = {127, 0x2430ac5b, 0x5328674a, 0x1fdffc7d, 0x69dced9b}, res = {126, 0x6bdbc531, 0x1c4110d5, 0x71a47f01, 0x2383b8f4}, carry = 1}, + {a = {127, 0x30e85990, 0x65afbf0b, 0x3a20ce3d, 0x5a014c71}, b = {127, 0x27d0d3a9, 0x2483d56f, 0x7e96f8e9, 0x54c679f8}, res = {127, 0x58b92d39, 0x0a33947a, 0x38b7c727, 0x2ec7c66a}, carry = 1}, + {a = {127, 0x6ec2ebc5, 0x2fe87e6e, 0x5c214f86, 0x46ce9354}, b = {127, 0x7b6b78e3, 0x0de8a5eb, 0x1cf27da3, 0x43d7077f}, res = {127, 0x6a2e64a8, 0x3dd1245a, 0x7913cd29, 0x0aa59ad3}, carry = 1}, + {a = {127, 0x532479dc, 0x38cb5eb9, 0x6b8afdb8, 0x624a0f13}, b = {127, 0x74ab8b56, 0x27e5b9da, 0x62e7b771, 0x50f115a6}, res = {127, 0x47d00532, 0x60b11894, 0x4e72b529, 0x333b24ba}, carry = 1}, + {a = {126, 0x7fe1b477, 0x4477a7c5, 0x04b0164d, 0x31d00838}, b = {125, 0x321ef997, 0x579ab7cf, 0x54fb3455, 0x133928f5}, res = {126, 0x3200ae0e, 0x1c125f95, 0x59ab4aa3, 0x4509312d}, carry = 0}, + {a = {127, 0x0dd79881, 0x120bcfae, 0x73cc2c5d, 0x43b522e7}, b = {126, 0x0b81b7af, 0x3c00816c, 0x5658e7d3, 0x30483552}, res = {127, 0x19595030, 0x4e0c511a, 0x4a251430, 0x73fd583a}, carry = 0}, + {a = {126, 0x0c52d45e, 0x698fd6df, 0x29a362fc, 0x3b6b5882}, b = {127, 0x5e851c19, 0x72428920, 0x417745c0, 0x437e8c53}, res = {126, 0x6ad7f077, 0x5bd25fff, 0x6b1aa8bd, 0x7ee9e4d5}, carry = 0}, + {a = {126, 0x5d82d3a5, 0x328636c2, 0x175e0f86, 0x270f7afa}, b = {126, 0x7c06e395, 0x21b77837, 0x5fb98ab1, 0x225d6181}, res = {126, 0x5989b73a, 0x543daefa, 0x77179a37, 0x496cdc7b}, carry = 0}, + {a = {127, 0x16be1fea, 0x48971493, 0x4a8f5617, 0x6b2a9968}, b = {127, 0x47387295, 0x2cecd451, 0x59abfdc3, 0x40028930}, res = {127, 0x5df6927f, 0x7583e8e4, 0x243b53da, 0x2b2d2299}, carry = 1}, + {a = {127, 0x3cea792f, 0x62a42211, 0x18aa6284, 0x4ca7e07f}, b = {127, 0x5ea60937, 0x5e7c6b69, 0x5bd789bd, 0x774ddc8e}, res = {127, 0x1b908266, 0x41208d7b, 0x7481ec42, 0x43f5bd0d}, carry = 1}, + {a = {127, 0x0e21cb57, 0x34087c35, 0x7bcad13b, 0x59c8dc35}, b = {125, 0x08a0ae9f, 0x145d8ebb, 0x1bcb683a, 0x1caefdbd}, res = {127, 0x16c279f6, 0x48660af0, 0x17963975, 0x7677d9f3}, carry = 0}, + {a = {127, 0x0b0dd998, 0x1210d9cc, 0x01074e0a, 0x762996da}, b = {126, 0x3c80570a, 0x00fbed3c, 0x77bb5d04, 0x258d2213}, res = {127, 0x478e30a2, 0x130cc708, 0x78c2ab0e, 0x1bb6b8ed}, carry = 1}, + {a = {127, 0x4e88ee76, 0x335e5d85, 0x634e3d26, 0x56cff5de}, b = {126, 0x15f3b966, 0x1c0a3f86, 0x1da6a59b, 0x35edda6f}, res = {127, 0x647ca7dc, 0x4f689d0b, 0x00f4e2c1, 0x0cbdd04e}, carry = 1}, + {a = {127, 0x374f8369, 0x00230d40, 0x76f627a4, 0x45cb7dff}, b = {127, 0x0e437bcd, 0x5524b249, 0x6aa80d40, 0x578666e8}, res = {127, 0x4592ff36, 0x5547bf89, 0x619e34e4, 0x1d51e4e8}, carry = 1}, + {a = {126, 0x7804142f, 0x66b7d326, 0x2f6fbe1b, 0x340e91e8}, b = {127, 0x5650e768, 0x22201fc5, 0x1a849718, 0x608b4950}, res = {126, 0x4e54fb97, 0x08d7f2ec, 0x49f45534, 0x1499db38}, carry = 1}, + {a = {127, 0x409a9e5d, 0x6b53d3a4, 0x284aa18f, 0x60899852}, b = {126, 0x340a5074, 0x05dc9720, 0x6c400c75, 0x2c0a2e6d}, res = {127, 0x74a4eed1, 0x71306ac4, 0x148aae04, 0x0c93c6c0}, carry = 1}, + {a = {127, 0x484f4d07, 0x45bf1a9a, 0x7679ef61, 0x58da55e9}, b = {125, 0x1672599a, 0x25b9fde9, 0x1dd52ce2, 0x1b792755}, res = {127, 0x5ec1a6a1, 0x6b791883, 0x144f1c43, 0x74537d3f}, carry = 0}, + {a = {127, 0x46f0b919, 0x258efe9f, 0x25ac2909, 0x7ee35251}, b = {126, 0x26292ec9, 0x052799f6, 0x26e8cd9f, 0x2a91ea77}, res = {127, 0x6d19e7e2, 0x2ab69895, 0x4c94f6a8, 0x29753cc8}, carry = 1}, + {a = {127, 0x662c1549, 0x7d1d8413, 0x2820005b, 0x640fa366}, b = {127, 0x1a1c703a, 0x7c960a8a, 0x0468ab46, 0x76636006}, res = {127, 0x00488583, 0x79b38e9e, 0x2c88aba2, 0x5a73036c}, carry = 1}, + {a = {127, 0x32c078a6, 0x09144c2e, 0x7216eb1e, 0x575eb4a5}, b = {127, 0x014b7cd8, 0x7374bc3f, 0x79bad310, 0x41c309b3}, res = {127, 0x340bf57e, 0x7c89086d, 0x6bd1be2e, 0x1921be59}, carry = 1}, + {a = {127, 0x5d152378, 0x0db3b779, 0x27cbd808, 0x729e2498}, b = {127, 0x639a2cac, 0x4e2dc8f3, 0x22846201, 0x538c5c6c}, res = {127, 0x40af5024, 0x5be1806d, 0x4a503a09, 0x462a8104}, carry = 1}, + {a = {126, 0x4f52ebe5, 0x2e2ce1fe, 0x740060a5, 0x2b602c28}, b = {127, 0x23613e9d, 0x5236ec37, 0x744c1397, 0x6a0e964f}, res = {126, 0x72b42a82, 0x0063ce35, 0x684c743d, 0x156ec278}, carry = 1}, + {a = {126, 0x7cab43a4, 0x7b2c3d9d, 0x44c551d5, 0x3e795426}, b = {127, 0x25f21ad6, 0x50b3905d, 0x4477f701, 0x64063b12}, res = {126, 0x229d5e7a, 0x4bdfcdfb, 0x093d48d7, 0x227f8f39}, carry = 1}, + {a = {126, 0x54d5908a, 0x4db32799, 0x4e170b52, 0x32d0557d}, b = {126, 0x24f21022, 0x0e431da5, 0x52beb40c, 0x2e5eada4}, res = {126, 0x79c7a0ac, 0x5bf6453e, 0x20d5bf5e, 0x612f0322}, carry = 0}, + {a = {127, 0x2fd0c679, 0x429d2164, 0x3b7d1340, 0x478c197e}, b = {126, 0x5b9f625b, 0x15cfdd9b, 0x0eb39653, 0x3f2a9e1a}, res = {127, 0x0b7028d4, 0x586cff00, 0x4a30a993, 0x06b6b798}, carry = 1}, + {a = {127, 0x47e483cd, 0x635dad05, 0x334178a6, 0x5be2703c}, b = {127, 0x12487678, 0x0b47445a, 0x2b468da6, 0x5cf39115}, res = {127, 0x5a2cfa45, 0x6ea4f15f, 0x5e88064c, 0x38d60151}, carry = 1}, + {a = {127, 0x16bd1190, 0x6f358b1b, 0x43fef060, 0x57246c18}, b = {125, 0x52f9efe3, 0x1d8e35d7, 0x45422cbc, 0x1e9a7d06}, res = {127, 0x69b70173, 0x0cc3c0f2, 0x09411d1d, 0x75bee91f}, carry = 0}, + {a = {124, 0x300566bd, 0x4043d1da, 0x07083a42, 0x0ac4490a}, b = {126, 0x7bc7aaa4, 0x089b207d, 0x7d73159e, 0x20475102}, res = {124, 0x2bcd1161, 0x48def258, 0x047b4fe0, 0x2b0b9a0d}, carry = 0}, + {a = {127, 0x1879a134, 0x2d5dce4b, 0x4fb09e72, 0x6f409165}, b = {127, 0x224335a0, 0x20c2b717, 0x748b2f65, 0x738d9650}, res = {127, 0x3abcd6d4, 0x4e208562, 0x443bcdd7, 0x62ce27b6}, carry = 1}, + {a = {127, 0x49d61045, 0x5b207515, 0x2e191bd8, 0x64903107}, b = {127, 0x08359044, 0x2f1aff57, 0x730d9fb4, 0x7f03816e}, res = {127, 0x520ba089, 0x0a3b746c, 0x2126bb8d, 0x6393b276}, carry = 1}, + {a = {127, 0x623eb7a3, 0x4d52e07d, 0x4a90b87c, 0x4641403a}, b = {125, 0x08f857d7, 0x3761ae9d, 0x46ead119, 0x13a35973}, res = {127, 0x6b370f7a, 0x04b48f1a, 0x117b8996, 0x59e499ae}, carry = 0}, + {a = {126, 0x71b46b67, 0x1edc966c, 0x42558b0c, 0x29be2808}, b = {127, 0x666102d3, 0x573e7bfe, 0x65d18c72, 0x67b4562e}, res = {126, 0x58156e3a, 0x761b126b, 0x2827177e, 0x11727e37}, carry = 1}, + {a = {127, 0x50500966, 0x0c5234da, 0x3e8cf39e, 0x62fbe6ce}, b = {127, 0x6c96a29c, 0x0ffb4fc4, 0x5f3a488f, 0x60d3a913}, res = {127, 0x3ce6ac02, 0x1c4d849f, 0x1dc73c2d, 0x43cf8fe2}, carry = 1}, + {a = {125, 0x508f7e5a, 0x2a6c24f8, 0x7cb4ae59, 0x1816c2cd}, b = {126, 0x1e8cc9df, 0x16ed73e1, 0x5910fa15, 0x3a24b307}, res = {125, 0x6f1c4839, 0x415998d9, 0x55c5a86e, 0x523b75d5}, carry = 0}, + {a = {127, 0x510325a0, 0x1ded9a89, 0x51e31db4, 0x6ddc474b}, b = {127, 0x22f4ab1e, 0x6fb69633, 0x51a5308e, 0x6eb15ef6}, res = {127, 0x73f7d0be, 0x0da430bc, 0x23884e43, 0x5c8da642}, carry = 1}, + {a = {127, 0x12108d0c, 0x386688ec, 0x69d02125, 0x628e00f3}, b = {124, 0x11f211b8, 0x2a6ff08f, 0x33daa305, 0x0c610086}, res = {127, 0x24029ec4, 0x62d6797b, 0x1daac42a, 0x6eef017a}, carry = 0}, + {a = {126, 0x0c264fba, 0x0884a2d3, 0x20569035, 0x304b267d}, b = {126, 0x6ff360e6, 0x12c6ad2b, 0x33f44806, 0x21d94783}, res = {126, 0x7c19b0a0, 0x1b4b4ffe, 0x544ad83b, 0x52246e00}, carry = 0}, + {a = {127, 0x43ab14a8, 0x7c15842c, 0x636094f1, 0x53d57eea}, b = {126, 0x15682230, 0x35fc075d, 0x59910014, 0x23e605cb}, res = {127, 0x591336d8, 0x32118b89, 0x3cf19506, 0x77bb84b6}, carry = 0}, + {a = {126, 0x5a7805ff, 0x32dd0387, 0x75f7a085, 0x3ea23d06}, b = {127, 0x4c55c285, 0x26e33530, 0x03d565c7, 0x724d2fcb}, res = {126, 0x26cdc884, 0x59c038b8, 0x79cd064c, 0x30ef6cd1}, carry = 1}, + {a = {125, 0x1a9e1657, 0x1af68cda, 0x4602b940, 0x17acd817}, b = {127, 0x539ac7fe, 0x54adc300, 0x1fe07458, 0x403f5f12}, res = {125, 0x6e38de55, 0x6fa44fda, 0x65e32d98, 0x57ec3729}, carry = 0}, + {a = {127, 0x62b5821b, 0x2c747aa2, 0x49572e10, 0x691ea596}, b = {127, 0x2e8da32f, 0x1325dd52, 0x08bf290a, 0x7b103bf2}, res = {127, 0x1143254a, 0x3f9a57f5, 0x5216571a, 0x642ee188}, carry = 1}, + {a = {127, 0x215fb007, 0x0dfc51a6, 0x7014c1ee, 0x5e30b9d9}, b = {125, 0x52f164a9, 0x300a98d7, 0x2cab73e3, 0x132af275}, res = {127, 0x745114b0, 0x3e06ea7d, 0x1cc035d1, 0x715bac4f}, carry = 0}, + {a = {127, 0x4dfcc7e5, 0x10d63d11, 0x443f7365, 0x7eb2535a}, b = {127, 0x543fd063, 0x041977bb, 0x294f5562, 0x7fabe576}, res = {127, 0x223c9848, 0x14efb4cd, 0x6d8ec8c7, 0x7e5e38d0}, carry = 1}, + {a = {127, 0x1a8c810b, 0x49c32b54, 0x5dfbd7fe, 0x49ce1a98}, b = {127, 0x5166f071, 0x0bfea258, 0x3325555d, 0x7ebb7c16}, res = {127, 0x6bf3717c, 0x55c1cdac, 0x11212d5b, 0x488996af}, carry = 1}, + {a = {124, 0x4b8a34ea, 0x5b0935d4, 0x3568d516, 0x0d16387a}, b = {126, 0x620a0226, 0x65058ce5, 0x6dd1444f, 0x2f49abb0}, res = {124, 0x2d943710, 0x400ec2ba, 0x233a1966, 0x3c5fe42b}, carry = 0}, + {a = {126, 0x5e834973, 0x4242f578, 0x03a1e339, 0x2c61fc03}, b = {127, 0x04a255ce, 0x01ec5f4b, 0x4e56d825, 0x5d998057}, res = {126, 0x63259f41, 0x442f54c3, 0x51f8bb5e, 0x09fb7c5a}, carry = 1}, + {a = {127, 0x6cb09c4b, 0x293baae1, 0x7e2830b9, 0x4edf0b6b}, b = {127, 0x4a59137b, 0x4b516bec, 0x0e882e9a, 0x67fd68cb}, res = {127, 0x3709afc6, 0x748d16ce, 0x0cb05f53, 0x36dc7437}, carry = 1}, + {a = {126, 0x49d63bd1, 0x20f7d1ab, 0x78eec602, 0x21eea324}, b = {127, 0x4bf2e9c3, 0x55004234, 0x012195b5, 0x725e1f50}, res = {126, 0x15c92594, 0x75f813e0, 0x7a105bb7, 0x144cc274}, carry = 1}, + {a = {118, 0x009708e5, 0x0f6eae6a, 0x22717ea3, 0x0027f75e}, b = {125, 0x73b69ce9, 0x05c9d5e7, 0x079ce44c, 0x1e0e4b57}, res = {118, 0x744da5ce, 0x15388451, 0x2a0e62ef, 0x1e3642b5}, carry = 0}, + {a = {127, 0x7d5fcca1, 0x703581e8, 0x5ebea294, 0x6d59d7b6}, b = {127, 0x072dc186, 0x2556d525, 0x6550936e, 0x483050d7}, res = {127, 0x048d8e27, 0x158c570e, 0x440f3603, 0x358a288e}, carry = 1}, + {a = {125, 0x1ee5e478, 0x6cbc0fd8, 0x5a08ca0d, 0x160799a6}, b = {124, 0x20c5baa0, 0x6893f6b8, 0x2e615746, 0x08b55361}, res = {125, 0x3fab9f18, 0x55500690, 0x086a2154, 0x1ebced08}, carry = 0}, + {a = {126, 0x5ba7f924, 0x69e6da01, 0x7012307c, 0x257f31e9}, b = {124, 0x4f354878, 0x5c6c44df, 0x580de770, 0x088dc09c}, res = {126, 0x2add419c, 0x46531ee1, 0x482017ed, 0x2e0cf286}, carry = 0}, + {a = {126, 0x53c283ee, 0x19ddd382, 0x401fa681, 0x39b9198a}, b = {126, 0x3e9c90df, 0x3091f3aa, 0x1ec070d3, 0x2f2473fa}, res = {126, 0x125f14cd, 0x4a6fc72d, 0x5ee01754, 0x68dd8d84}, carry = 0}, + {a = {125, 0x2aab90f9, 0x0ee5a976, 0x4b5b0203, 0x1672068b}, b = {125, 0x53c43b94, 0x020d4782, 0x260b9c3a, 0x1faca98d}, res = {125, 0x7e6fcc8d, 0x10f2f0f8, 0x71669e3d, 0x361eb018}, carry = 0}, + {a = {127, 0x1741cb5d, 0x42adb37f, 0x16f4e290, 0x7c26149a}, b = {127, 0x5c12de2e, 0x77a67be5, 0x2ef8e62e, 0x4a9fdb63}, res = {127, 0x7354a98b, 0x3a542f64, 0x45edc8bf, 0x46c5effd}, carry = 1}, + {a = {125, 0x21437747, 0x41ce24dd, 0x70efb0ac, 0x1d4aac67}, b = {126, 0x1235886f, 0x7bc15966, 0x1097debf, 0x29249284}, res = {125, 0x3378ffb6, 0x3d8f7e43, 0x01878f6c, 0x466f3eec}, carry = 0}, + {a = {126, 0x0e70b5d5, 0x43bd3fd6, 0x1bdbe98b, 0x2033fc0b}, b = {127, 0x7ce4daf7, 0x16ace9f7, 0x073703bc, 0x5704964f}, res = {126, 0x0b5590cc, 0x5a6a29ce, 0x2312ed47, 0x7738925a}, carry = 0}, + {a = {120, 0x62a94a7e, 0x604154fa, 0x3deeb80c, 0x00df6634}, b = {125, 0x6fd3f3b5, 0x3cfc6cef, 0x63fd704a, 0x1678e57f}, res = {120, 0x527d3e33, 0x1d3dc1ea, 0x21ec2857, 0x17584bb4}, carry = 0}, + {a = {123, 0x7978a2ad, 0x5d491b8a, 0x71574e8a, 0x0663444b}, b = {126, 0x0bc05c34, 0x7c0f280f, 0x015a49fc, 0x38502a7f}, res = {123, 0x0538fee1, 0x5958439a, 0x72b19887, 0x3eb36eca}, carry = 0}, + {a = {125, 0x51d6b6f2, 0x079bd814, 0x6ce81e2d, 0x18c4c981}, b = {127, 0x54f420d0, 0x2dc663dc, 0x13bc81b9, 0x4060575c}, res = {125, 0x26cad7c2, 0x35623bf1, 0x00a49fe6, 0x592520de}, carry = 0}, + {a = {127, 0x36485bbe, 0x09796514, 0x63ee7f58, 0x5625e4e2}, b = {127, 0x21458fe6, 0x7c9fa294, 0x54b76a7e, 0x6d695c7f}, res = {127, 0x578deba4, 0x061907a8, 0x38a5e9d7, 0x438f4162}, carry = 1}, + {a = {127, 0x33d70ec2, 0x1f127a0b, 0x2717bb82, 0x5fd43cd2}, b = {125, 0x3f0797eb, 0x0d5c370b, 0x3d594b1d, 0x1752300b}, res = {127, 0x72dea6ad, 0x2c6eb116, 0x6471069f, 0x77266cdd}, carry = 0}, + {a = {127, 0x37eae763, 0x19004adb, 0x38b7eff2, 0x688fe1c5}, b = {127, 0x2cac471c, 0x37811110, 0x1f3987d5, 0x7fe15712}, res = {127, 0x64972e7f, 0x50815beb, 0x57f177c7, 0x687138d7}, carry = 1}, + {a = {126, 0x6c225971, 0x0437a4c8, 0x1bcb8b3e, 0x28ceb662}, b = {127, 0x48cd5436, 0x7e421cca, 0x29d44db3, 0x55711f42}, res = {126, 0x34efada7, 0x0279c193, 0x459fd8f2, 0x7e3fd5a4}, carry = 0}, + {a = {126, 0x2574a51d, 0x5de32cbd, 0x6c67c9cc, 0x376694a2}, b = {126, 0x138f84b7, 0x46f2b3f1, 0x47583471, 0x3b15472c}, res = {126, 0x390429d4, 0x24d5e0ae, 0x33bffe3e, 0x727bdbcf}, carry = 0}, + {a = {127, 0x588e6a4d, 0x03204eb0, 0x400a06ba, 0x469c62fd}, b = {127, 0x33609fc4, 0x65a624e4, 0x6111951d, 0x7648f1ae}, res = {127, 0x0bef0a11, 0x68c67395, 0x211b9bd7, 0x3ce554ac}, carry = 1}, + {a = {127, 0x03178062, 0x2c0793b9, 0x6b91ed2a, 0x436a75d5}, b = {127, 0x364e9acd, 0x20881e30, 0x609f019a, 0x5d437f92}, res = {127, 0x39661b2f, 0x4c8fb1e9, 0x4c30eec4, 0x20adf568}, carry = 1}, + {a = {126, 0x582502a7, 0x76d9bbd7, 0x6ec18736, 0x3d15fed5}, b = {127, 0x45e8a9e7, 0x0cfae275, 0x4c164029, 0x641d8e58}, res = {126, 0x1e0dac8e, 0x03d49e4d, 0x3ad7c760, 0x21338d2e}, carry = 1}, + {a = {127, 0x31a44c73, 0x4c970755, 0x433f481c, 0x63704395}, b = {127, 0x1a3979dc, 0x0e9d5437, 0x592a1251, 0x5cb4e212}, res = {127, 0x4bddc64f, 0x5b345b8c, 0x1c695a6d, 0x402525a8}, carry = 1}, + {a = {126, 0x31499a0a, 0x2df3b6ac, 0x4cf3e9b4, 0x3fdaa87e}, b = {126, 0x6749f6f2, 0x28c62023, 0x3e54e11b, 0x30a577bf}, res = {126, 0x189390fc, 0x56b9d6d0, 0x0b48cacf, 0x7080203e}, carry = 0}, + {a = {126, 0x2d6e6ed4, 0x3c9f5183, 0x27326fac, 0x35b95ad6}, b = {125, 0x28a46b0d, 0x4143c903, 0x182ab95c, 0x17b09437}, res = {126, 0x5612d9e1, 0x7de31a86, 0x3f5d2908, 0x4d69ef0d}, carry = 0}, + {a = {126, 0x26f20408, 0x2af94d52, 0x440bf192, 0x360df26a}, b = {127, 0x7b2198cb, 0x763bc4c5, 0x78ac7d5a, 0x5de9efc9}, res = {126, 0x22139cd3, 0x21351218, 0x3cb86eed, 0x13f7e234}, carry = 1}, + {a = {127, 0x47128b80, 0x670a3dbb, 0x63efbba7, 0x74a10d1e}, b = {124, 0x7f481747, 0x59187cd8, 0x51e741bc, 0x0a8ea0cd}, res = {127, 0x465aa2c7, 0x4022ba94, 0x35d6fd64, 0x7f2fadec}, carry = 0}, + {a = {127, 0x5a47d043, 0x052b9ec0, 0x401cbe7c, 0x49104294}, b = {127, 0x7427cee8, 0x06334bc8, 0x4528bd14, 0x741e6df6}, res = {127, 0x4e6f9f2b, 0x0b5eea89, 0x05457b90, 0x3d2eb08b}, carry = 1}, + {a = {127, 0x6520f6a0, 0x5b37902f, 0x31d3c910, 0x5f9fe4e6}, b = {127, 0x6aa5d0dc, 0x61ad0b30, 0x3b782608, 0x7dd0cd5f}, res = {127, 0x4fc6c77c, 0x3ce49b60, 0x6d4bef19, 0x5d70b245}, carry = 1}, + {a = {126, 0x34bdc731, 0x2de09f84, 0x3896e197, 0x3fee6ac9}, b = {127, 0x5b5d8627, 0x3c22bdb9, 0x5e7b4aaf, 0x555e4e2a}, res = {126, 0x101b4d58, 0x6a035d3e, 0x17122c46, 0x154cb8f4}, carry = 1}, +} + +@(rodata) +i31_sub_test_vectors := []I31_Test_Vector_Binary { + {a = {126, 0x75832201, 0x72b1ddb3, 0x3e8d7744, 0x325d7cb5}, b = {127, 0x52a23cfc, 0x51a47476, 0x19fd5fa3, 0x5a5e5d48}, res = {126, 0x22e0e505, 0x210d693d, 0x249017a1, 0x57ff1f6d}, carry = 1}, + {a = {127, 0x02585014, 0x15b77a75, 0x66fa4e0f, 0x5368a27c}, b = {126, 0x1aaa3973, 0x288030e7, 0x51ede1ff, 0x37bbad71}, res = {127, 0x67ae16a1, 0x6d37498d, 0x150c6c0f, 0x1bacf50b}, carry = 0}, + {a = {125, 0x3b43d5aa, 0x3de09ea7, 0x18004f89, 0x113b0b5e}, b = {127, 0x3a318699, 0x10ee38f1, 0x6d8c06d8, 0x7ac6ef8f}, res = {125, 0x01124f11, 0x2cf265b6, 0x2a7448b1, 0x16741bce}, carry = 1}, + {a = {127, 0x047e69b6, 0x5455ee19, 0x012e5e13, 0x73a56c35}, b = {127, 0x10606c28, 0x33f1e053, 0x18f48327, 0x69bc2be6}, res = {127, 0x741dfd8e, 0x20640dc5, 0x6839daec, 0x09e9404e}, carry = 0}, + {a = {127, 0x02c391a7, 0x07feb24a, 0x3a32b686, 0x5ee14ddd}, b = {126, 0x3f3b74ff, 0x2c1596d7, 0x677a7df4, 0x2ce6395d}, res = {127, 0x43881ca8, 0x5be91b72, 0x52b83891, 0x31fb147f}, carry = 0}, + {a = {125, 0x4b26152d, 0x24f5c3a3, 0x6f1d3b6f, 0x1586f40a}, b = {125, 0x7380829e, 0x1bd90254, 0x382393b6, 0x1b85a0c5}, res = {125, 0x57a5928f, 0x091cc14e, 0x36f9a7b9, 0x7a015345}, carry = 1}, + {a = {126, 0x4fa59569, 0x643c2005, 0x72e9a332, 0x2e315696}, b = {121, 0x6de61d40, 0x6b932a39, 0x52568d6d, 0x01e8488c}, res = {126, 0x61bf7829, 0x78a8f5cb, 0x209315c4, 0x2c490e0a}, carry = 0}, + {a = {127, 0x5379b430, 0x467ac863, 0x297696ab, 0x70622b6f}, b = {127, 0x37aac8dc, 0x1adda229, 0x58df1d01, 0x505ee996}, res = {127, 0x1bceeb54, 0x2b9d263a, 0x509779aa, 0x200341d8}, carry = 0}, + {a = {127, 0x1ccf6e2c, 0x11dc002c, 0x561b6361, 0x46990123}, b = {127, 0x3e9f8554, 0x42397834, 0x4fdd5416, 0x7e39420f}, res = {127, 0x5e2fe8d8, 0x4fa287f7, 0x063e0f4a, 0x485fbf14}, carry = 1}, + {a = {126, 0x2150add9, 0x26e965d1, 0x5e4ac34d, 0x2a55c9de}, b = {127, 0x6669738d, 0x3d3ff599, 0x582b736f, 0x4dd82557}, res = {126, 0x3ae73a4c, 0x69a97037, 0x061f4fdd, 0x5c7da487}, carry = 1}, + {a = {127, 0x61a3eb42, 0x2796bb5e, 0x5b90dcd6, 0x78085440}, b = {127, 0x51c6a80f, 0x0a65c487, 0x6179dc58, 0x562966ef}, res = {127, 0x0fdd4333, 0x1d30f6d7, 0x7a17007e, 0x21deed50}, carry = 0}, + {a = {127, 0x51d4085f, 0x72b1daa2, 0x7560d03e, 0x47df6842}, b = {125, 0x27993d6b, 0x1b36ba5a, 0x78a56068, 0x1811037d}, res = {127, 0x2a3acaf4, 0x577b2048, 0x7cbb6fd6, 0x2fce64c4}, carry = 0}, + {a = {125, 0x2cb4937a, 0x48c3af52, 0x7c4e70a7, 0x1578b00c}, b = {127, 0x20263089, 0x3feb52a0, 0x1b877614, 0x4f5677bc}, res = {125, 0x0c8e62f1, 0x08d85cb2, 0x60c6fa93, 0x46223850}, carry = 1}, + {a = {126, 0x338493e9, 0x4ff90c99, 0x5dc3827f, 0x2a9898da}, b = {122, 0x588232b4, 0x1c53bb05, 0x17ef86ab, 0x03bce2b4}, res = {126, 0x5b026135, 0x33a55193, 0x45d3fbd4, 0x26dbb626}, carry = 0}, + {a = {124, 0x40625bc9, 0x204b059d, 0x48deea64, 0x0a019e61}, b = {127, 0x661bd15a, 0x48ead2c5, 0x2a0e651f, 0x7999457b}, res = {124, 0x5a468a6f, 0x576032d7, 0x1ed08544, 0x106858e6}, carry = 1}, + {a = {127, 0x5874a542, 0x71947250, 0x054ff642, 0x7d66c0f6}, b = {125, 0x780fcb4e, 0x290dc73d, 0x2687ab55, 0x107555db}, res = {127, 0x6064d9f4, 0x4886ab12, 0x5ec84aed, 0x6cf16b1a}, carry = 0}, + {a = {126, 0x6fd19206, 0x50f14417, 0x7f82f1ed, 0x2bbb502b}, b = {127, 0x0468cf8a, 0x67942b1c, 0x7c399be0, 0x6306501c}, res = {126, 0x6b68c27c, 0x695d18fb, 0x0349560c, 0x48b5000f}, carry = 1}, + {a = {124, 0x51a0623b, 0x7b984ebd, 0x0fb05efe, 0x0fd13b63}, b = {127, 0x2a6b91ec, 0x68eaf51b, 0x3265f0fd, 0x4a4f4fb3}, res = {124, 0x2734d04f, 0x12ad59a2, 0x5d4a6e01, 0x4581ebaf}, carry = 1}, + {a = {127, 0x0f1be1af, 0x76ef0418, 0x489dab08, 0x4998335f}, b = {127, 0x24c297f4, 0x6fabbbe8, 0x01529fff, 0x6c5b3c15}, res = {127, 0x6a5949bb, 0x0743482f, 0x474b0b09, 0x5d3cf74a}, carry = 1}, + {a = {125, 0x53a96664, 0x6e05147a, 0x1017a26d, 0x19d02282}, b = {127, 0x46f7e29b, 0x466f513b, 0x1c9bb1fc, 0x56817f94}, res = {125, 0x0cb183c9, 0x2795c33f, 0x737bf071, 0x434ea2ed}, carry = 1}, + {a = {127, 0x2fcad8be, 0x60aea5d7, 0x63f1ddde, 0x5e392547}, b = {127, 0x3006e08d, 0x560753cd, 0x7e2304f9, 0x48c18d08}, res = {127, 0x7fc3f831, 0x0aa75209, 0x65ced8e5, 0x1577983e}, carry = 0}, + {a = {125, 0x140a0bd3, 0x0af4e07b, 0x1d4d97a3, 0x12530f8a}, b = {124, 0x66378508, 0x26164d79, 0x0b3ff9ba, 0x0b94a13a}, res = {125, 0x2dd286cb, 0x64de9301, 0x120d9de8, 0x06be6e50}, carry = 0}, + {a = {127, 0x16d0c935, 0x6e1afbd1, 0x08ef273a, 0x694e551c}, b = {125, 0x0f34a643, 0x167cbfb7, 0x2c1977e7, 0x1bd4f3cc}, res = {127, 0x079c22f2, 0x579e3c1a, 0x5cd5af53, 0x4d79614f}, carry = 0}, + {a = {127, 0x3d62a534, 0x3c284b2d, 0x53524377, 0x6016b0bd}, b = {125, 0x7cf4d88b, 0x68fb44c3, 0x61c8f0ad, 0x10e57b36}, res = {127, 0x406dcca9, 0x532d0669, 0x718952c9, 0x4f313586}, carry = 0}, + {a = {127, 0x6f015d33, 0x536fbab1, 0x26915f4b, 0x562f74ee}, b = {122, 0x63e75212, 0x68694786, 0x3f82de9a, 0x02f4382a}, res = {127, 0x0b1a0b21, 0x6b06732b, 0x670e80b0, 0x533b3cc3}, carry = 0}, + {a = {127, 0x66a82756, 0x4025f122, 0x42ad0938, 0x58fbe956}, b = {123, 0x150d6a84, 0x23439905, 0x7f8687ac, 0x057badf7}, res = {127, 0x519abcd2, 0x1ce2581d, 0x4326818c, 0x53803b5e}, carry = 0}, + {a = {127, 0x1b0466bd, 0x69accc1e, 0x55da7183, 0x71f64927}, b = {126, 0x12a653b6, 0x4be3b926, 0x174ecdc5, 0x2e28e368}, res = {127, 0x085e1307, 0x1dc912f8, 0x3e8ba3be, 0x43cd65bf}, carry = 0}, + {a = {119, 0x119a6115, 0x130e1918, 0x4e2c62c0, 0x00586779}, b = {127, 0x7042f5f4, 0x6c596133, 0x0b8e23ed, 0x4fded5b4}, res = {119, 0x21576b21, 0x26b4b7e4, 0x429e3ed2, 0x307991c5}, carry = 1}, + {a = {127, 0x05b5cfd3, 0x2ae55120, 0x46abe106, 0x70b46cd4}, b = {127, 0x4a8e2790, 0x5557a5e9, 0x3e14dfb6, 0x62ff997b}, res = {127, 0x3b27a843, 0x558dab36, 0x0897014f, 0x0db4d359}, carry = 0}, + {a = {126, 0x40c5bd09, 0x1bc7d36e, 0x5787d3c1, 0x2dadafd4}, b = {127, 0x7d36195e, 0x67d20645, 0x384c8cf2, 0x7bc9c106}, res = {126, 0x438fa3ab, 0x33f5cd28, 0x1f3b46ce, 0x31e3eece}, carry = 1}, + {a = {121, 0x582c3425, 0x5e591a55, 0x64bc368c, 0x01171496}, b = {127, 0x7ff455d7, 0x1d082b60, 0x69a595c6, 0x7506f22f}, res = {121, 0x5837de4e, 0x4150eef4, 0x7b16a0c6, 0x0c102266}, carry = 1}, + {a = {126, 0x76e444c8, 0x5d162950, 0x0b4c9cb8, 0x350172ce}, b = {126, 0x6fae6b50, 0x457e2833, 0x22e790fd, 0x222f32e8}, res = {126, 0x0735d978, 0x1798011d, 0x68650bbb, 0x12d23fe5}, carry = 0}, + {a = {126, 0x0a94f6cb, 0x7e659900, 0x21e27381, 0x20898f13}, b = {125, 0x6c0e1d51, 0x26db9379, 0x4aa78374, 0x13e8525f}, res = {126, 0x1e86d97a, 0x578a0586, 0x573af00d, 0x0ca13cb3}, carry = 0}, + {a = {126, 0x2df85daa, 0x089e925f, 0x372ad1cd, 0x282da3da}, b = {126, 0x5f6eb57d, 0x666b94e8, 0x72523b11, 0x2e872674}, res = {126, 0x4e89a82d, 0x2232fd76, 0x44d896bb, 0x79a67d65}, carry = 1}, + {a = {127, 0x59c5b936, 0x14069767, 0x00e21797, 0x7ab2135d}, b = {127, 0x4a74250d, 0x65c5e230, 0x5562bf62, 0x5c25dec5}, res = {127, 0x0f519429, 0x2e40b537, 0x2b7f5834, 0x1e8c3497}, carry = 0}, + {a = {125, 0x74ce4c1e, 0x59cb9a18, 0x5e0560c1, 0x142bdb97}, b = {126, 0x0e08a60d, 0x4c39a45f, 0x0929589f, 0x22096dd5}, res = {125, 0x66c5a611, 0x0d91f5b9, 0x54dc0822, 0x72226dc2}, carry = 1}, + {a = {127, 0x65273636, 0x653e4647, 0x2cf1b1f4, 0x74f9fddd}, b = {122, 0x6e960ace, 0x7ea2dfda, 0x35a317bd, 0x0202a322}, res = {127, 0x76912b68, 0x669b666c, 0x774e9a36, 0x72f75aba}, carry = 0}, + {a = {126, 0x45582ae7, 0x6996c2bb, 0x09380ea2, 0x30b4897c}, b = {127, 0x246ce79f, 0x758393b5, 0x00926c53, 0x40dd24dc}, res = {126, 0x20eb4348, 0x74132f06, 0x08a5a24e, 0x6fd764a0}, carry = 1}, + {a = {127, 0x7face3b5, 0x4e099393, 0x1e3f7fe6, 0x50946a47}, b = {126, 0x50b06b20, 0x36db7586, 0x40ec226f, 0x33e64381}, res = {127, 0x2efc7895, 0x172e1e0d, 0x5d535d77, 0x1cae26c5}, carry = 0}, + {a = {124, 0x5bae9309, 0x23a74c8d, 0x4c7f0704, 0x0fd4a29c}, b = {127, 0x07e40cc7, 0x31e632d3, 0x1ae2cbd2, 0x6f8f930a}, res = {124, 0x53ca8642, 0x71c119ba, 0x319c3b31, 0x20450f92}, carry = 1}, + {a = {126, 0x69c3634b, 0x0c4ffbaa, 0x390a326e, 0x25e998b2}, b = {123, 0x4d164145, 0x6cf31f2c, 0x60273791, 0x05ea1210}, res = {126, 0x1cad2206, 0x1f5cdc7e, 0x58e2fadc, 0x1fff86a1}, carry = 0}, + {a = {124, 0x25a0c384, 0x38438ee6, 0x67b92d68, 0x0b8d4052}, b = {127, 0x65152a5c, 0x6af4d890, 0x78f3790f, 0x516ebf7b}, res = {124, 0x408b9928, 0x4d4eb655, 0x6ec5b458, 0x3a1e80d6}, carry = 1}, + {a = {120, 0x27e60366, 0x1158cd09, 0x7f574484, 0x00eb60e2}, b = {126, 0x5ee2ec9b, 0x17d9507a, 0x283fcdf8, 0x3d95a50e}, res = {120, 0x490316cb, 0x797f7c8e, 0x5717768b, 0x4355bbd4}, carry = 1}, + {a = {126, 0x7a6cfbf1, 0x3b47fde4, 0x1f96a1b0, 0x3415815d}, b = {127, 0x6db4865c, 0x68d15d67, 0x29492ffd, 0x55b34d59}, res = {126, 0x0cb87595, 0x5276a07d, 0x764d71b2, 0x5e623403}, carry = 1}, + {a = {126, 0x78bc926b, 0x06872ef9, 0x77dc59ee, 0x341e1fe5}, b = {127, 0x4aafbaf4, 0x0f29aa6c, 0x6e2dcd74, 0x5ae5ab56}, res = {126, 0x2e0cd777, 0x775d848d, 0x09ae8c79, 0x5938748f}, carry = 1}, + {a = {127, 0x5339599f, 0x6d388285, 0x1fb30e96, 0x653d70d1}, b = {127, 0x26e44c38, 0x360bdfc0, 0x317ed433, 0x4e44f71e}, res = {127, 0x2c550d67, 0x372ca2c5, 0x6e343a63, 0x16f879b2}, carry = 0}, + {a = {126, 0x5f57436e, 0x5d7b8fc4, 0x3b913a49, 0x289c171d}, b = {127, 0x63106f0e, 0x181ab5a1, 0x1a35ecdc, 0x4f6fd3de}, res = {126, 0x7c46d460, 0x4560da22, 0x215b4d6d, 0x592c433f}, carry = 1}, + {a = {125, 0x0a9e7523, 0x4011ed51, 0x573e92c1, 0x1465e918}, b = {125, 0x190d6359, 0x595c3a46, 0x76ee73b0, 0x1163ef5f}, res = {125, 0x719111ca, 0x66b5b30a, 0x60501f10, 0x0301f9b8}, carry = 0}, + {a = {126, 0x5b4c4e1c, 0x368e93d7, 0x1d7f383f, 0x3893cc55}, b = {126, 0x15361741, 0x192e05a0, 0x69808f62, 0x3d0b8de0}, res = {126, 0x461636db, 0x1d608e37, 0x33fea8dd, 0x7b883e74}, carry = 1}, + {a = {127, 0x711129f3, 0x1221b384, 0x75a6ce65, 0x75cc9dda}, b = {125, 0x05242048, 0x29449c5e, 0x299b4ec4, 0x1179eca2}, res = {127, 0x6bed09ab, 0x68dd1726, 0x4c0b7fa0, 0x6452b138}, carry = 0}, + {a = {126, 0x23d8928a, 0x6bc1710b, 0x4f1a2853, 0x3920c780}, b = {127, 0x3ed60b0f, 0x639f16e2, 0x26d5472a, 0x7a6d17fb}, res = {126, 0x6502877b, 0x08225a28, 0x2844e129, 0x3eb3af85}, carry = 1}, + {a = {127, 0x0d9cfe5d, 0x619328e7, 0x542c700d, 0x6997d292}, b = {127, 0x1cedd3b7, 0x5edfbdf6, 0x45d76cf4, 0x7357a00a}, res = {127, 0x70af2aa6, 0x02b36af0, 0x0e550319, 0x76403288}, carry = 1}, + {a = {127, 0x03e30b55, 0x4013c48f, 0x729f2a80, 0x58b486a3}, b = {126, 0x786328fe, 0x37cc20dc, 0x25a645e1, 0x27cf6abb}, res = {127, 0x0b7fe257, 0x0847a3b2, 0x4cf8e49f, 0x30e51be8}, carry = 0}, + {a = {125, 0x7ac1580f, 0x2c93cbec, 0x527026f9, 0x124b767e}, b = {127, 0x68ba663a, 0x4df0c1d0, 0x4a5c9da6, 0x5da272db}, res = {125, 0x1206f1d5, 0x5ea30a1c, 0x08138952, 0x34a903a3}, carry = 1}, + {a = {127, 0x56b6b76a, 0x3ece11a5, 0x296c5e68, 0x59ab3003}, b = {127, 0x6e69a755, 0x7099639a, 0x357c87d6, 0x4c9f33fd}, res = {127, 0x684d1015, 0x4e34ae0a, 0x73efd691, 0x0d0bfc05}, carry = 0}, + {a = {127, 0x4404fa8a, 0x5866a5c4, 0x0c22add4, 0x5ef97523}, b = {127, 0x1c21bae1, 0x256e56eb, 0x77662e5f, 0x7376f17a}, res = {127, 0x27e33fa9, 0x32f84ed9, 0x14bc7f75, 0x6b8283a8}, carry = 1}, + {a = {126, 0x2c0d6109, 0x3509e87e, 0x53519777, 0x3c09bbc0}, b = {127, 0x3a60646e, 0x3f868a8e, 0x461edbdd, 0x777a0c2c}, res = {126, 0x71acfc9b, 0x75835def, 0x0d32bb99, 0x448faf94}, carry = 1}, + {a = {126, 0x5f8c1f7a, 0x40041120, 0x463c5e66, 0x35a4cd6e}, b = {127, 0x08cf40d3, 0x2edbcf34, 0x784ab86d, 0x4de2eb71}, res = {126, 0x56bcdea7, 0x112841ec, 0x4df1a5f9, 0x67c1e1fc}, carry = 1}, + {a = {127, 0x2266e65d, 0x0553ee53, 0x0c667741, 0x4fd147e2}, b = {127, 0x3fb1e4a2, 0x7cb1c12a, 0x535dbe75, 0x67bfcc87}, res = {127, 0x62b501bb, 0x08a22d28, 0x3908b8cb, 0x68117b5a}, carry = 1}, + {a = {127, 0x76bfd2fb, 0x40e3235f, 0x7110f948, 0x55262313}, b = {126, 0x56f35745, 0x288792fc, 0x3759df43, 0x36acfcf9}, res = {127, 0x1fcc7bb6, 0x185b9063, 0x39b71a05, 0x1e79261a}, carry = 0}, + {a = {125, 0x79452908, 0x28cb7d35, 0x033fb36e, 0x1e3d1dc2}, b = {126, 0x625452fe, 0x6f0e270e, 0x4a783375, 0x21ed361b}, res = {125, 0x16f0d60a, 0x39bd5627, 0x38c77ff8, 0x7c4fe7a6}, carry = 1}, + {a = {126, 0x7372a659, 0x3cd43111, 0x79c40547, 0x2d1afece}, b = {127, 0x7f8726fd, 0x24d0aa04, 0x1f6ecb14, 0x4e053ba9}, res = {126, 0x73eb7f5c, 0x1803870c, 0x5a553a33, 0x5f15c325}, carry = 1}, + {a = {126, 0x2f15690a, 0x4de81324, 0x3e9f55fe, 0x373cfdd3}, b = {125, 0x02bb7d7f, 0x3f2d94bb, 0x769d851b, 0x10f5260a}, res = {126, 0x2c59eb8b, 0x0eba7e69, 0x4801d0e3, 0x2647d7c8}, carry = 0}, + {a = {125, 0x54d7b5c6, 0x3932181a, 0x53521bf5, 0x1c5cf5ee}, b = {125, 0x03d8109e, 0x4d5cbcea, 0x49faabc0, 0x1d3e3e9a}, res = {125, 0x50ffa528, 0x6bd55b30, 0x09577034, 0x7f1eb754}, carry = 1}, + {a = {125, 0x43482a38, 0x1ce74ab0, 0x624445b9, 0x176cc140}, b = {125, 0x202ec3d7, 0x1cdd0bba, 0x4493150c, 0x13241d43}, res = {125, 0x23196661, 0x000a3ef6, 0x1db130ad, 0x0448a3fd}, carry = 0}, + {a = {126, 0x63a9b9c1, 0x5557f9c9, 0x4a536191, 0x3c4994c8}, b = {127, 0x5ee2af7f, 0x7dda218e, 0x53e931bb, 0x4270e93f}, res = {126, 0x04c70a42, 0x577dd83b, 0x766a2fd5, 0x79d8ab88}, carry = 1}, + {a = {127, 0x4950f74f, 0x6aad0a67, 0x43e36683, 0x76afb633}, b = {127, 0x2213721e, 0x76557c01, 0x299aae91, 0x44c18e55}, res = {127, 0x273d8531, 0x74578e66, 0x1a48b7f1, 0x31ee27de}, carry = 0}, + {a = {126, 0x0935ca8f, 0x1200cc80, 0x2f3661fb, 0x23960e06}, b = {126, 0x4624b76e, 0x14091fd6, 0x6286cb7a, 0x3e1aa872}, res = {126, 0x43111321, 0x7df7aca9, 0x4caf9680, 0x657b6593}, carry = 1}, + {a = {124, 0x52ac128b, 0x45fa9e02, 0x6427aa53, 0x0feaf8b5}, b = {127, 0x18aa3cc4, 0x19e96b87, 0x6c0e4dcc, 0x45da3929}, res = {124, 0x3a01d5c7, 0x2c11327b, 0x78195c87, 0x4a10bf8b}, carry = 1}, + {a = {126, 0x4111b645, 0x1c825213, 0x2a8b75b3, 0x213c3a3e}, b = {126, 0x23f60c22, 0x490b2da9, 0x353a3844, 0x29d5dd47}, res = {126, 0x1d1baa23, 0x5377246a, 0x75513d6e, 0x77665cf6}, carry = 1}, + {a = {127, 0x09c853b4, 0x7541862d, 0x36e91f14, 0x6e89e67d}, b = {127, 0x21d3bfe2, 0x61403f04, 0x554b57fc, 0x76d413df}, res = {127, 0x67f493d2, 0x14014728, 0x619dc718, 0x77b5d29d}, carry = 1}, + {a = {127, 0x67761ecf, 0x3837a139, 0x335e0875, 0x79923e83}, b = {127, 0x76813044, 0x7c0f5491, 0x59acc858, 0x5a7a5795}, res = {127, 0x70f4ee8b, 0x3c284ca7, 0x59b1401c, 0x1f17e6ed}, carry = 0}, + {a = {126, 0x6429a8c8, 0x635056e0, 0x76e76b1d, 0x3837df4c}, b = {127, 0x531005db, 0x115acaa3, 0x69231eee, 0x43d19db2}, res = {126, 0x1119a2ed, 0x51f58c3d, 0x0dc44c2f, 0x7466419a}, carry = 1}, + {a = {127, 0x5014a598, 0x068dbb0f, 0x22fdc5bf, 0x6a670715}, b = {126, 0x7c4f94e9, 0x2e78116b, 0x224d3768, 0x2b95833a}, res = {127, 0x53c510af, 0x5815a9a3, 0x00b08e56, 0x3ed183db}, carry = 0}, + {a = {126, 0x75b94a38, 0x71d62d52, 0x6c557c79, 0x2d8cfb9f}, b = {127, 0x743c6840, 0x3b04bcff, 0x50744d2c, 0x608611a3}, res = {126, 0x017ce1f8, 0x36d17053, 0x1be12f4d, 0x4d06e9fc}, carry = 1}, + {a = {126, 0x04d24ee0, 0x37e1c03c, 0x50704c96, 0x2577ba6b}, b = {127, 0x6dd14735, 0x03c4f523, 0x379aa43a, 0x4fbfbf80}, res = {126, 0x170107ab, 0x341ccb18, 0x18d5a85c, 0x55b7faeb}, carry = 1}, + {a = {123, 0x572d0f53, 0x732fb14c, 0x7975e187, 0x05706d99}, b = {127, 0x51a90823, 0x707dd091, 0x2f6c63b4, 0x789adddf}, res = {123, 0x05840730, 0x02b1e0bb, 0x4a097dd3, 0x0cd58fba}, carry = 1}, + {a = {126, 0x68306d82, 0x51d883c8, 0x52a45e7f, 0x339e6fc7}, b = {127, 0x57da71c8, 0x72d5dd1c, 0x21a11ff7, 0x5d6ea34b}, res = {126, 0x1055fbba, 0x5f02a6ac, 0x31033e87, 0x562fcc7c}, carry = 1}, + {a = {125, 0x47b20e6e, 0x39411f59, 0x0324640b, 0x19996204}, b = {124, 0x4e55588b, 0x64846ee8, 0x57d16f9e, 0x0a3db8bc}, res = {125, 0x795cb5e3, 0x54bcb070, 0x2b52f46c, 0x0f5ba947}, carry = 0}, + {a = {127, 0x5a6ab51b, 0x0051b988, 0x1a192e89, 0x42b3d43a}, b = {125, 0x2daa3aa4, 0x063f37e9, 0x7ba7850f, 0x13dcea8c}, res = {127, 0x2cc07a77, 0x7a12819f, 0x1e71a979, 0x2ed6e9ad}, carry = 0}, + {a = {125, 0x46813953, 0x6920da78, 0x5b6db2f9, 0x12219a9c}, b = {126, 0x5666e4fb, 0x5458ccf2, 0x097952e7, 0x38f4e37f}, res = {125, 0x701a5458, 0x14c80d85, 0x51f46012, 0x592cb71d}, carry = 1}, + {a = {127, 0x47633dea, 0x6405989b, 0x1d019f24, 0x7dab4fa6}, b = {127, 0x686dbe43, 0x5715424a, 0x2027ac93, 0x70c8d29c}, res = {127, 0x5ef57fa7, 0x0cf05650, 0x7cd9f291, 0x0ce27d09}, carry = 0}, + {a = {127, 0x22705ca5, 0x2c180e47, 0x393c2083, 0x40d0fea1}, b = {124, 0x37ce1e5a, 0x17b2b88d, 0x49bd2a90, 0x0d5d9087}, res = {127, 0x6aa23e4b, 0x146555b9, 0x6f7ef5f3, 0x33736e19}, carry = 0}, + {a = {126, 0x5d61d089, 0x1391e7a4, 0x780aeb70, 0x3eb6e8e2}, b = {127, 0x7a54a013, 0x28f31254, 0x3d63ef25, 0x5d2cf7ff}, res = {126, 0x630d3076, 0x6a9ed54f, 0x3aa6fc4a, 0x6189f0e3}, carry = 1}, + {a = {126, 0x505f4672, 0x275eb845, 0x665c7a55, 0x31c26f48}, b = {127, 0x3af113f0, 0x6a0358aa, 0x009cd3d5, 0x551cd2f6}, res = {126, 0x156e3282, 0x3d5b5f9b, 0x65bfa67f, 0x5ca59c52}, carry = 1}, + {a = {126, 0x1a313b22, 0x04332012, 0x4cfb19fe, 0x3aa25e6c}, b = {127, 0x0525c494, 0x1a177649, 0x10bd52f2, 0x5d114ab1}, res = {126, 0x150b768e, 0x6a1ba9c9, 0x3c3dc70b, 0x5d9113bb}, carry = 1}, + {a = {126, 0x52981c2d, 0x4c46b130, 0x37b3c3cf, 0x231f6fc4}, b = {126, 0x76da8c93, 0x5e52130f, 0x7efb6a4f, 0x2e3c4907}, res = {126, 0x5bbd8f9a, 0x6df49e20, 0x38b8597f, 0x74e326bc}, carry = 1}, + {a = {127, 0x05600262, 0x0bb65331, 0x19f8d9cb, 0x5ddc2c62}, b = {127, 0x687b227e, 0x27348976, 0x072f2464, 0x4fbe7ebe}, res = {127, 0x1ce4dfe4, 0x6481c9ba, 0x12c9b566, 0x0e1dada4}, carry = 0}, + {a = {127, 0x6a2d2e4a, 0x1b8b56dd, 0x50946d73, 0x64b0886d}, b = {122, 0x68ceaba5, 0x5344cea7, 0x7aeae02f, 0x03444737}, res = {127, 0x015e82a5, 0x48468836, 0x55a98d43, 0x616c4135}, carry = 0}, + {a = {126, 0x2be80971, 0x7a1ffbd6, 0x7d5cd745, 0x310e6aa8}, b = {126, 0x31c62e38, 0x08a3a28d, 0x4d67f9a3, 0x30493db3}, res = {126, 0x7a21db39, 0x717c5948, 0x2ff4dda2, 0x00c52cf5}, carry = 0}, + {a = {125, 0x030ba3d3, 0x162c1b78, 0x3f878cb3, 0x1a5108ff}, b = {127, 0x1f37aec7, 0x01e1153a, 0x325aab1e, 0x4b9a6619}, res = {125, 0x63d3f50c, 0x144b063d, 0x0d2ce195, 0x4eb6a2e6}, carry = 1}, + {a = {126, 0x15a0528b, 0x29cbf0ad, 0x3048884b, 0x2c03d1c4}, b = {127, 0x5c32f40c, 0x192de971, 0x21a7f87d, 0x558c0542}, res = {126, 0x396d5e7f, 0x109e073b, 0x0ea08fce, 0x5677cc82}, carry = 1}, + {a = {126, 0x0b48d92b, 0x5b09cd3a, 0x73960e62, 0x3f0ed1cd}, b = {124, 0x7a3d70fe, 0x5ff9c6b1, 0x4bd0e555, 0x0cc51487}, res = {126, 0x110b682d, 0x7b100688, 0x27c5290c, 0x3249bd46}, carry = 0}, + {a = {127, 0x1bf5bd1c, 0x54104aef, 0x0c7ce316, 0x420fe407}, b = {126, 0x409409a2, 0x3d04fecd, 0x3e06417a, 0x3078c80b}, res = {127, 0x5b61b37a, 0x170b4c21, 0x4e76a19c, 0x11971bfb}, carry = 0}, + {a = {125, 0x33d00217, 0x1c394fa4, 0x574f8331, 0x1d57d174}, b = {127, 0x6630d00d, 0x666f444c, 0x1a90af22, 0x484d8ca0}, res = {125, 0x4d9f320a, 0x35ca0b57, 0x3cbed40e, 0x550a44d4}, carry = 1}, + {a = {127, 0x166e17c2, 0x1ecc3694, 0x1d8ad0ad, 0x5cfd9dd8}, b = {125, 0x130183de, 0x537699d8, 0x353fa45d, 0x114feeeb}, res = {127, 0x036c93e4, 0x4b559cbc, 0x684b2c4f, 0x4badaeec}, carry = 0}, + {a = {127, 0x2736ea4c, 0x34a77f3b, 0x5d62185c, 0x49f65789}, b = {126, 0x10ef92f4, 0x6d50c8cd, 0x3b2c8da0, 0x26d91a49}, res = {127, 0x16475758, 0x4756b66e, 0x22358abb, 0x231d3d40}, carry = 0}, + {a = {127, 0x7c90f6d6, 0x6e8c2441, 0x56303ec7, 0x648b71e2}, b = {127, 0x5efcffab, 0x35738e3f, 0x6a0010b5, 0x54fd6ef0}, res = {127, 0x1d93f72b, 0x39189602, 0x6c302e12, 0x0f8e02f1}, carry = 0}, + {a = {127, 0x09d4aa13, 0x39bee362, 0x48f85dfd, 0x54d4270d}, b = {121, 0x75609829, 0x5a20f480, 0x2a01c588, 0x0146be72}, res = {127, 0x147411ea, 0x5f9deee1, 0x1ef69874, 0x538d689b}, carry = 0}, + {a = {123, 0x5ca62e2f, 0x5bd68bd9, 0x009238b5, 0x04524739}, b = {126, 0x0f74d025, 0x68d0ff9b, 0x3cd174c2, 0x23b408c2}, res = {123, 0x4d315e0a, 0x73058c3e, 0x43c0c3f2, 0x609e3e76}, carry = 1}, +} + +I31_Test_Vector_Decode :: struct { + src: []byte, + decode: []u32, + mod: []u32, + mod_res: u32, +} + +@(rodata) +i31_decode_test_vectors := []I31_Test_Vector_Decode { + {src = {0x49}, decode = {7, 0x00000049}, mod = {42, 0x00000049}, mod_res = 1}, + {src = {0x27, 0xdb}, decode = {14, 0x000027db}, mod = {42, 0x000027db}, mod_res = 1}, + {src = {0xb8, 0xd7, 0x72}, decode = {24, 0x00b8d772}, mod = {42, 0x00b8d772}, mod_res = 1}, + {src = {0xa0, 0x94, 0x6e, 0xac}, decode = {33, 0x20946eac, 0x00000001}, mod = {42, 0x20946eac, 0x00000001}, mod_res = 1}, + {src = {0xed, 0x8a, 0xc2, 0xe4, 0x45}, decode = {41, 0x0ac2e445, 0x000001db}, mod = {42, 0x0ac2e445, 0x000001db}, mod_res = 1}, + {src = {0xfb, 0x37, 0x36, 0x46, 0x28, 0x4e}, decode = {49, 0x3646284e, 0x0001f66e}, mod = {42, 0x00000000, 0x00000000}, mod_res = 0}, + {src = {0xd8, 0xcc, 0xbb, 0xf2, 0x13, 0x66, 0xdf}, decode = {57, 0x721366df, 0x01b19977}, mod = {42, 0x00000000, 0x00000000}, mod_res = 0}, + {src = {0xcb, 0xc3, 0xe2, 0xa8, 0x4c, 0xfd, 0x9e, 0x8a}, decode = {66, 0x4cfd9e8a, 0x1787c550, 0x00000003}, mod = {42, 0x00000000, 0x00000000, 0x00000000}, mod_res = 0}, + {src = {0x0d, 0xd5, 0x76, 0xc1, 0xb4, 0x02, 0xdb, 0xb4, 0xb9}, decode = {70, 0x02dbb4b9, 0x2aed8368, 0x00000037}, mod = {42, 0x00000000, 0x00000000, 0x00000000}, mod_res = 0}, + {src = {0xf6, 0xb6, 0x08, 0xb3, 0xfb, 0x0c, 0x7e, 0x8e, 0x58, 0xa7}, decode = {82, 0x7e8e58a7, 0x1167f618, 0x0003dad8}, mod = {42, 0x00000000, 0x00000000, 0x00000000}, mod_res = 0}, + {src = {0xf9, 0x51, 0xde, 0x6a, 0x29, 0x54, 0x48, 0xf5, 0x3c, 0x78, 0x85}, decode = {90, 0x753c7885, 0x5452a891, 0x03e54779}, mod = {42, 0x00000000, 0x00000000, 0x00000000}, mod_res = 0}, + {src = {0xb5, 0x5a, 0x92, 0x18, 0x67, 0x09, 0x6e, 0x21, 0x5d, 0x84, 0x10, 0xce}, decode = {99, 0x5d8410ce, 0x4e12dc42, 0x556a4861, 0x00000005}, mod = {42, 0x00000000, 0x00000000, 0x00000000, 0x00000000}, mod_res = 0}, + {src = {0xd3, 0xa4, 0x7b, 0xa0, 0x90, 0x26, 0x36, 0x73, 0xa6, 0x7f, 0x28, 0x66, 0x4e}, decode = {107, 0x7f28664e, 0x4c6ce74c, 0x11ee8240, 0x0000069d}, mod = {42, 0x00000000, 0x00000000, 0x00000000, 0x00000000}, mod_res = 0}, + {src = {0x23, 0xfa, 0xe7, 0x0c, 0x6a, 0x07, 0x00, 0x86, 0xa2, 0xcf, 0x22, 0xa1, 0x1f, 0x62}, decode = {113, 0x22a11f62, 0x010d459e, 0x1c31a81c, 0x00011fd7}, mod = {42, 0x00000000, 0x00000000, 0x00000000, 0x00000000}, mod_res = 0}, + {src = {0x9c, 0x39, 0xf8, 0x67, 0x8a, 0x9f, 0x85, 0xb4, 0xdb, 0x92, 0x4b, 0xe6, 0xdc, 0x12, 0xa5}, decode = {123, 0x66dc12a5, 0x69b72497, 0x1e2a7e16, 0x04e1cfc3}, mod = {42, 0x00000000, 0x00000000, 0x00000000, 0x00000000}, mod_res = 0}, + {src = {0x8f, 0xc7, 0xcc, 0xda, 0x43, 0x21, 0x0b, 0x39, 0x0e, 0xc3, 0x51, 0xb0, 0xbd, 0xce, 0xbb, 0x73}, decode = {132, 0x3dcebb73, 0x1d86a361, 0x0c842ce4, 0x7e3e66d2, 0x00000008}, mod = {42, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000}, mod_res = 0}, + {src = {0xc5, 0x8d, 0xd3, 0x1b, 0xda, 0x0d, 0x21, 0xfd, 0xb2, 0x69, 0xca, 0x12, 0xd4, 0x47, 0x44, 0x4e, 0x61}, decode = {140, 0x47444e61, 0x539425a8, 0x3487f6c9, 0x6e98ded0, 0x00000c58}, mod = {42, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000}, mod_res = 0}, + {src = {0x4f, 0x00, 0x7f, 0x9c, 0x00, 0x11, 0x29, 0x8d, 0x94, 0xd6, 0x7e, 0x1c, 0xc3, 0x26, 0x78, 0xb7, 0xfb, 0xbb}, decode = {147, 0x78b7fbbb, 0x7c39864c, 0x26365359, 0x7ce00089, 0x0004f007}, mod = {42, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000}, mod_res = 0}, + {src = {0xb8, 0x85, 0x3f, 0xe0, 0x03, 0x89, 0x20, 0x72, 0xf8, 0xa6, 0x66, 0x48, 0x0f, 0xdb, 0x63, 0x19, 0x8e, 0xbb, 0x00}, decode = {156, 0x198ebb00, 0x101fb6c6, 0x4be29999, 0x001c4903, 0x0b8853fe}, mod = {42, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000}, mod_res = 0}, + {src = {0x3c, 0x88, 0x99, 0xf2, 0x23, 0xc8, 0x65, 0x6d, 0x6a, 0x93, 0xa0, 0xf6, 0xef, 0x8d, 0x28, 0xd8, 0x85, 0x9e, 0x8f, 0x85}, decode = {163, 0x059e8f85, 0x5f1a51b1, 0x2a4e83db, 0x1e432b6b, 0x48899f22, 0x00000007}, mod = {42, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000}, mod_res = 0}, + {src = {0xd8, 0x28, 0x9e, 0xae, 0x7c, 0xc2, 0xf6, 0x9c, 0x72, 0xe7, 0x25, 0xf6, 0x0c, 0xec, 0xcc, 0x78, 0x5d, 0xf9, 0xb0, 0xe9, 0x9f}, decode = {173, 0x79b0e99f, 0x5998f0bb, 0x1c97d833, 0x17b4e397, 0x09eae7cc, 0x00001b05}, mod = {42, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000}, mod_res = 0}, + {src = {0xec, 0x5f, 0x68, 0xa7, 0xb6, 0xa4, 0x7d, 0x01, 0x88, 0x39, 0xeb, 0x88, 0x3a, 0x33, 0x60, 0x8a, 0x59, 0x67, 0xaa, 0xfe, 0x67, 0xc2}, decode = {181, 0x2afe67c2, 0x4114b2cf, 0x2e20e8cd, 0x680c41cf, 0x0a7b6a47, 0x001d8bed}, mod = {42, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000}, mod_res = 0}, + {src = {0x34, 0x8a, 0x87, 0xdb, 0xbf, 0x60, 0xa6, 0x58, 0x23, 0xe3, 0x99, 0x9a, 0x43, 0x5c, 0x43, 0x9e, 0xfe, 0xb2, 0xbd, 0xd7, 0xd2, 0xa3, 0x74}, decode = {187, 0x57d2a374, 0x3dfd657b, 0x690d710e, 0x411f1ccc, 0x3bf60a65, 0x069150fb}, mod = {42, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000}, mod_res = 0}, + {src = {0xe6, 0xe7, 0x48, 0x95, 0x31, 0x9e, 0xdb, 0x97, 0xd7, 0x37, 0x17, 0x1a, 0x9a, 0x51, 0xde, 0x02, 0xa4, 0x7f, 0x84, 0xbd, 0x25, 0x02, 0xc7, 0xe1}, decode = {198, 0x2502c7e1, 0x48ff097a, 0x6947780a, 0x39b8b8d4, 0x19edb97d, 0x5ce912a6, 0x00000039}, mod = {42, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000}, mod_res = 0}, + {src = {0x5c, 0xd5, 0xa2, 0x66, 0xf4, 0x01, 0x12, 0xba, 0xe8, 0x05, 0x63, 0xa0, 0xc0, 0xfa, 0xaf, 0xe1, 0x3d, 0xe4, 0xc5, 0x21, 0xab, 0xa9, 0x52, 0x99, 0x3f}, decode = {205, 0x2952993f, 0x498a4357, 0x6abf84f7, 0x2b1d0607, 0x112bae80, 0x344cde80, 0x00001735}, mod = {42, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000}, mod_res = 0}, + {src = {0x8f, 0x93, 0xfc, 0x0c, 0x73, 0x29, 0x7d, 0x28, 0xdc, 0x71, 0x7b, 0x01, 0x53, 0x4f, 0x87, 0xdd, 0xf5, 0x43, 0xe2, 0xb0, 0x97, 0x1d, 0xe1, 0xf7, 0x7f, 0xcf}, decode = {214, 0x61f77fcf, 0x45612e3b, 0x1f77d50f, 0x580a9a7c, 0x528dc717, 0x018e652f, 0x0023e4ff}, mod = {42, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000}, mod_res = 0}, + {src = {0xd7, 0x86, 0x9b, 0x8d, 0x91, 0x97, 0xff, 0x98, 0xbe, 0xa7, 0x67, 0x73, 0xda, 0x7a, 0x07, 0xe7, 0x07, 0xf3, 0x61, 0xc4, 0x2d, 0x01, 0x96, 0x68, 0xbf, 0xd6, 0x36}, decode = {222, 0x68bfd636, 0x085a032c, 0x1c1fcd87, 0x1ed3d03f, 0x0bea7677, 0x3232fff3, 0x35e1a6e3}, mod = {42, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000}, mod_res = 0}, + {src = {0xf7, 0xec, 0xfa, 0x44, 0x18, 0x5e, 0x6a, 0x51, 0x98, 0x3d, 0x3b, 0xc9, 0xcf, 0xb5, 0x9f, 0x97, 0xa8, 0x7d, 0xdd, 0xda, 0xf3, 0xf3, 0x1c, 0x02, 0x9c, 0x40, 0x5c, 0x8a}, decode = {231, 0x1c405c8a, 0x67e63805, 0x21f7776b, 0x7dacfcbd, 0x03d3bc9c, 0x0bcd4a33, 0x7b3e9106, 0x0000007b}, mod = {42, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000}, mod_res = 0}, + {src = {0xca, 0x32, 0x04, 0xef, 0xc8, 0x40, 0x55, 0xc9, 0x67, 0x2e, 0x3b, 0x1a, 0xbe, 0x2c, 0xf1, 0xdd, 0xe0, 0xaa, 0x46, 0x31, 0x40, 0xc5, 0xf6, 0xaa, 0x25, 0x88, 0x0d, 0x0f, 0x30}, decode = {239, 0x080d0f30, 0x0bed544b, 0x2918c503, 0x678eef05, 0x63b1abe2, 0x0ab92ce5, 0x013bf210, 0x00006519}, mod = {42, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000}, mod_res = 0}, + {src = {0x96, 0xb5, 0x62, 0x03, 0x50, 0x9f, 0x04, 0xfc, 0xc3, 0x06, 0x25, 0xc1, 0xa7, 0x21, 0x6d, 0xeb, 0x60, 0x8d, 0xb0, 0x02, 0x34, 0x18, 0xaf, 0xd1, 0xb0, 0x23, 0x0f, 0x6a, 0xe6, 0x63}, decode = {247, 0x0f6ae663, 0x5fa36046, 0x4008d062, 0x6f5b046d, 0x5c1a7216, 0x1f9860c4, 0x00d427c1, 0x004b5ab1}, mod = {42, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000}, mod_res = 0}, + {src = {0x5c, 0x00, 0x1a, 0x67, 0x07, 0x8e, 0x49, 0x72, 0x30, 0x6f, 0x2b, 0xa6, 0x93, 0x37, 0x6a, 0x2b, 0x5a, 0x8f, 0xb4, 0x30, 0x65, 0x89, 0xa1, 0x7b, 0xbc, 0xf9, 0x85, 0x3f, 0x44, 0x64, 0x48}, decode = {254, 0x3f446448, 0x7779f30a, 0x41962685, 0x5ad47da1, 0x693376a2, 0x460de574, 0x41e3925c, 0x2e000d33, 0x00000000}, mod = {42, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000}, mod_res = 0}, +} + +I31_Test_RShift :: struct { + orig: []u32, + res: []u32, + shift: i32, +} + +@(rodata) +i31_rshift_test_vectors := []I31_Test_RShift { + {orig = {92, 0x4b451fff, 0x2874869d, 0x0d1b97a7}, res = {92, 0x4b451fff, 0x2874869d, 0x0d1b97a7}, shift = 0}, + {orig = {94, 0x44709cd4, 0x60d36574, 0x3a587e31}, res = {94, 0x22384e6a, 0x7069b2ba, 0x1d2c3f18}, shift = 1}, + {orig = {94, 0x77ea5771, 0x78324ed5, 0x3d9ad39d}, res = {94, 0x3dfa95dc, 0x3e0c93b5, 0x0f66b4e7}, shift = 2}, + {orig = {94, 0x631db445, 0x248c243a, 0x3f1cc589}, res = {94, 0x2c63b688, 0x14918487, 0x07e398b1}, shift = 3}, + {orig = {95, 0x6f23a8ed, 0x7d5a3c91, 0x7e913a13}, res = {95, 0x0ef23a8e, 0x1fd5a3c9, 0x07e913a1}, shift = 4}, + {orig = {94, 0x03dd8abf, 0x5f02ee65, 0x249f3f58}, res = {94, 0x141eec55, 0x62f81773, 0x0124f9fa}, shift = 5}, + {orig = {94, 0x2927bee6, 0x4f308116, 0x3279228a}, res = {94, 0x2ca49efb, 0x153cc204, 0x00c9e48a}, shift = 6}, + {orig = {95, 0x22687024, 0x73cb5fd3, 0x52d71799}, res = {95, 0x5344d0e0, 0x19e796bf, 0x00a5ae2f}, shift = 7}, + {orig = {94, 0x71a3bca0, 0x27642d52, 0x23e2059f}, res = {94, 0x2971a3bc, 0x4fa7642d, 0x0023e205}, shift = 8}, + {orig = {95, 0x180d85aa, 0x12a8516d, 0x4797741e}, res = {95, 0x5b4c06c2, 0x07895428, 0x0023cbba}, shift = 9}, + {orig = {95, 0x7b30aeeb, 0x3ababf4f, 0x6a446aff}, res = {95, 0x69fecc2b, 0x5feeaeaf, 0x001a911a}, shift = 10}, + {orig = {94, 0x3b2d5f09, 0x7bde80e9, 0x33ed8ce0}, res = {94, 0x0e9765ab, 0x4e0f7bd0, 0x00067db1}, shift = 11}, + {orig = {93, 0x62c89828, 0x20d78d11, 0x14bbd081}, res = {93, 0x688e2c89, 0x040a0d78, 0x00014bbd}, shift = 12}, + {orig = {95, 0x102c28e6, 0x31f2a99f, 0x6c337ed1}, res = {95, 0x267c8161, 0x7b458f95, 0x0003619b}, shift = 13}, + {orig = {94, 0x78097c9b, 0x6b3f99c8, 0x2ea10fbb}, res = {94, 0x3391e025, 0x1f77acfe, 0x0000ba84}, shift = 14}, + {orig = {92, 0x4684f360, 0x56d54074, 0x0dd7bfa7}, res = {92, 0x40748d09, 0x3fa7adaa, 0x00001baf}, shift = 15}, + {orig = {93, 0x59e9b272, 0x1792ca1d, 0x134ce6d8}, res = {93, 0x650ed9e9, 0x736c1792, 0x0000134c}, shift = 16}, + {orig = {94, 0x2be2d592, 0x566508a1, 0x3a8622ab}, res = {94, 0x422855f1, 0x08aaeb32, 0x00001d43}, shift = 17}, + {orig = {95, 0x3082be20, 0x118cde6a, 0x57a1832c}, res = {95, 0x1bcd4c20, 0x30658463, 0x000015e8}, shift = 18}, + {orig = {95, 0x7b7ec9fc, 0x30304a84, 0x7cb524f7}, res = {95, 0x04a84f6f, 0x524f7606, 0x00000f96}, shift = 19}, + {orig = {94, 0x7a038786, 0x5902ae15, 0x2ee36b72}, res = {94, 0x1570afa0, 0x1b5b9590, 0x000002ee}, shift = 20}, + {orig = {95, 0x2a127f63, 0x5d1b323f, 0x7dc23bd7}, res = {95, 0x6cc8fd50, 0x08ef5ee8, 0x000003ee}, shift = 21}, + {orig = {95, 0x3cf76a47, 0x30310973, 0x69bc9430}, res = {95, 0x6212e6f3, 0x792860c0, 0x000001a6}, shift = 22}, + {orig = {94, 0x3ca70baa, 0x07747e80, 0x3a710be4}, res = {94, 0x747e8079, 0x710be40e, 0x00000074}, shift = 23}, + {orig = {95, 0x6bc0e3b9, 0x77b2972f, 0x5b9f893c}, res = {95, 0x594b97eb, 0x4fc49e77, 0x0000005b}, shift = 24}, + {orig = {90, 0x49e59ccc, 0x7d67d332, 0x02b4f127}, res = {90, 0x59f4cca4, 0x2d3c49fe, 0x00000001}, shift = 25}, + {orig = {94, 0x55e1b1d7, 0x7a1c0bdb, 0x217e8547}, res = {94, 0x43817b75, 0x2fd0a8fe, 0x00000008}, shift = 26}, + {orig = {94, 0x67fb980c, 0x20a9410b, 0x25de2ffd}, res = {94, 0x0a9410bc, 0x5de2ffd4, 0x00000004}, shift = 27}, + {orig = {92, 0x18363569, 0x0b492aba, 0x0a37f42c}, res = {92, 0x5a4955d1, 0x51bfa160, 0x00000000}, shift = 28}, + {orig = {95, 0x23bb0e53, 0x4bb6c3aa, 0x77932915}, res = {95, 0x2edb0ea9, 0x5e4ca456, 0x00000003}, shift = 29}, + {orig = {95, 0x725acb18, 0x1da81862, 0x6d6c306b}, res = {95, 0x3b5030c5, 0x5ad860d6, 0x00000001}, shift = 30}, + {orig = {95, 0x70d51c12, 0x44e8b652, 0x4a0d2a6b}, res = {95, 0x44e8b652, 0x4a0d2a6b, 0x00000000}, shift = 31}, +} + +I31_Test_Reduce :: struct { + orig: []u32, + res: []u32, +} + +@(rodata) +i31_reduce_test_vectors := []I31_Test_Reduce { + {orig = {62, 0x27da8fd9, 0x2fea2339}, res = {42, 0x27f284e9, 0x00000339}}, + {orig = {95, 0x37856cc1, 0x54ad3e73, 0x718777f1}, res = {42, 0x33efdfc1, 0x0000022f, 0x718777f1}}, + {orig = {123, 0x4787b519, 0x47fa51cf, 0x11ef98ae, 0x058b1a99}, res = {42, 0x567ed6bd, 0x000002fe, 0x11ef98ae, 0x058b1a99}}, + {orig = {158, 0x328fbfb6, 0x6e9ec225, 0x0241df84, 0x4a3627d0, 0x3a79e4a4}, res = {42, 0x005f59f0, 0x000001aa, 0x0241df84, 0x4a3627d0, 0x3a79e4a4}}, + {orig = {191, 0x451321f5, 0x45f677b9, 0x2c43b0b8, 0x7cb722c1, 0x70e594e6, 0x4c81757c}, res = {42, 0x684ba25d, 0x0000008d, 0x2c43b0b8, 0x7cb722c1, 0x70e594e6, 0x4c81757c}}, + {orig = {222, 0x0749fe0b, 0x3c45795b, 0x21e6f14c, 0x265b264d, 0x37def307, 0x35019b6b, 0x25ab0a3e}, res = {42, 0x36c99d3d, 0x00000173, 0x21e6f14c, 0x265b264d, 0x37def307, 0x35019b6b, 0x25ab0a3e}}, + {orig = {255, 0x1c2630cd, 0x0c0d9a7e, 0x375154d0, 0x67249adf, 0x0df9ec39, 0x73b5ad9e, 0x396d0f52, 0x4ed9b56a}, res = {42, 0x79997cc9, 0x000002b4, 0x375154d0, 0x67249adf, 0x0df9ec39, 0x73b5ad9e, 0x396d0f52, 0x4ed9b56a}}, + {orig = {286, 0x35b7eae2, 0x033002a6, 0x149766aa, 0x5d4a5a16, 0x7d09704c, 0x380c8cf2, 0x249df2ff, 0x03caabad, 0x214fc645}, res = {42, 0x20f488ba, 0x000003e1, 0x149766aa, 0x5d4a5a16, 0x7d09704c, 0x380c8cf2, 0x249df2ff, 0x03caabad, 0x214fc645}}, + {orig = {315, 0x7a5813dc, 0x76309cbf, 0x03e432bc, 0x218df9a1, 0x1a0d0525, 0x793ad550, 0x280cbf6e, 0x18356492, 0x4b6f39a1, 0x04adeb0d}, res = {42, 0x0016ad0c, 0x000000a8, 0x03e432bc, 0x218df9a1, 0x1a0d0525, 0x793ad550, 0x280cbf6e, 0x18356492, 0x4b6f39a1, 0x04adeb0d}}, + {orig = {351, 0x0ded15c8, 0x3875535e, 0x4627ebc1, 0x101a8369, 0x7300acd4, 0x22113509, 0x2a441bc0, 0x25902fec, 0x230133c0, 0x7ecaa587, 0x439bfdcd}, res = {42, 0x60547c04, 0x00000144, 0x4627ebc1, 0x101a8369, 0x7300acd4, 0x22113509, 0x2a441bc0, 0x25902fec, 0x230133c0, 0x7ecaa587, 0x439bfdcd}}, +} + +@(rodata) +i31_decode_reduce_test_vectors := []I31_Test_Vector_Decode { + {src = {171, 54, 46}, decode = {42, 0x00ab362e, 0x00000000, 0x00000000}}, + {src = {87, 80, 187, 242}, decode = {42, 0x5750bbf2, 0x00000000, 0x00000000}}, + {src = {181, 9, 43, 65, 203}, decode = {42, 0x092b41cb, 0x0000016a, 0x00000000}}, + {src = {196, 160, 88, 214, 25, 234}, decode = {42, 0x58d61aae, 0x00000140, 0x00000000}}, + {src = {223, 248, 213, 188, 56, 226, 125}, decode = {42, 0x3c39c275, 0x000001ab, 0x00000000}}, + {src = {252, 152, 213, 96, 59, 205, 38, 68}, decode = {42, 0x3cc9bf18, 0x000002c0, 0x00000000}}, + {src = {61, 151, 139, 227, 56, 129, 245, 7, 245}, decode = {42, 0x3f8c93d7, 0x00000271, 0x00000000}}, + {src = {50, 242, 36, 43, 28, 76, 75, 80, 191, 213}, decode = {42, 0x3d74eaf1, 0x000000fe, 0x00000000}}, + {src = {45, 174, 63, 171, 108, 66, 180, 223, 196, 24, 202}, decode = {42, 0x1f6f853a, 0x000000c6, 0x00000000}}, + {src = {106, 140, 119, 25, 145, 178, 50, 134, 118, 181, 241, 205}, decode = {42, 0x10480e8b, 0x000001fb, 0x00000000}}, +} + +I31_Test_Mul_Add_Small :: struct { + orig: []u32, + res: []u32, + z: u32, +} + +@(rodata) +i31_mul_add_test_vectors := []I31_Test_Mul_Add_Small { + {orig = {63, 0x157df37a, 0x5e97b10e}, res = {63, 0x438abf24, 0x7fffff7a}, z = 42}, + {orig = {63, 0x6c11d92a, 0x74b6b943}, res = {63, 0x50f60940, 0x0000012a}, z = 84}, + {orig = {63, 0x0bf17f5a, 0x756c85bb}, res = {63, 0x6ec5f93e, 0x7fffff5a}, z = 126}, + {orig = {63, 0x0d24e4eb, 0x5891203e}, res = {63, 0x0f86931a, 0x000000eb}, z = 168}, + {orig = {63, 0x41dda071, 0x6deb6411}, res = {63, 0x0460efa2, 0x00000071}, z = 210}, + {orig = {63, 0x29793d56, 0x4d5204e5}, res = {63, 0x3954bd9a, 0x00000156}, z = 252}, + {orig = {58, 0x40e72062, 0x039ba033}, res = {58, 0x0ce074b6, 0x00000062}, z = 294}, + {orig = {63, 0x6e9b147b, 0x60dcf3ef}, res = {63, 0x7bf74edc, 0x7ffffc7c}, z = 336}, + {orig = {62, 0x74572099, 0x3af8369a}, res = {62, 0x26ba2d0a, 0x0000009a}, z = 378}, +} + +I31_Test_Vector_Encode :: struct { + orig: []u32, + encoded: []u8, +} + +@(rodata) +i31_encode_test_vectors := []I31_Test_Vector_Encode { + {orig = {30, 0x2003ca33}, encoded = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 32, 3, 202, 51}}, + {orig = {61, 0x13d86a42, 0x1c0fc55a}, encoded = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 14, 7, 226, 173, 19, 216, 106, 66}}, + {orig = {93, 0x0adffd1b, 0x3a03c1bc, 0x19682bb9}, encoded = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 6, 90, 10, 238, 93, 1, 224, 222, 10, 223, 253, 27}}, + {orig = {127, 0x193841a2, 0x5cf0aa2f, 0x57594f6d, 0x4fc77899}, encoded = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 9, 248, 239, 19, 53, 214, 83, 219, 110, 120, 85, 23, 153, 56, 65, 162}}, + {orig = {158, 0x219d0fd8, 0x623e21ae, 0x5c9ad413, 0x6dc292d4, 0x2b2f85fe}, encoded = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 2, 178, 248, 95, 237, 184, 82, 90, 151, 38, 181, 4, 241, 31, 16, 215, 33, 157, 15, 216}}, + {orig = {189, 0x69363092, 0x4b751e5d, 0x663b4745, 0x2e93e19e, 0x1a3dabce, 0x1d4caa3f}, encoded = {0, 0, 0, 0, 0, 0, 0, 0, 0, 234, 101, 81, 249, 163, 218, 188, 229, 210, 124, 51, 217, 142, 209, 209, 101, 186, 143, 46, 233, 54, 48, 146}}, + {orig = {222, 0x016b86ed, 0x29c31304, 0x15cca452, 0x6abefa4a, 0x7410f562, 0x450559df, 0x28ad9252}, encoded = {0, 0, 0, 0, 0, 162, 182, 73, 74, 40, 42, 206, 255, 65, 15, 86, 45, 87, 223, 73, 69, 115, 41, 20, 148, 225, 137, 130, 1, 107, 134, 237}}, + {orig = {253, 0x4b6d0cd3, 0x357f8b7d, 0x4bd86436, 0x5838a86c, 0x10280aa0, 0x22f9b902, 0x4bdb5c1c, 0x19a20f9e}, encoded = {0, 51, 68, 31, 61, 47, 109, 112, 113, 23, 205, 200, 17, 2, 128, 170, 11, 7, 21, 13, 146, 246, 25, 13, 154, 191, 197, 190, 203, 109, 12, 211}}, +} + +I31_Test_Ninv :: struct { + orig: []u32, + ninv: []u32, +} + +@(rodata) +i31_ninv_test_vectors := I31_Test_Ninv { + orig = {0x00000c5c, 0x47faf728, 0x69a8e9d5, 0x49f5015c, 0x4ea9aea5, 0x164bcf32, 0x3fc395b4, 0x1c0a908a, 0x795f47f2, 0x79aa0c9a, 0x1a37680f, 0x2834cf17, 0x499235c8, 0x6d239632, 0x0560438b, 0x2cf4b82b, 0x5133fd25, 0x3b63a8b9, 0x45b66eca, 0x0213c13c, 0x6cd589f3, 0x4ed03f68, 0x722eb913, 0x670c76f4, 0x444d31d0, 0x1809da41, 0x3656a4af, 0x2ab43d09, 0x281c0e85, 0x426b3fd3, 0x680413c0, 0x065884c2, 0x55e4db17, 0x3839a23f, 0x3cc508b0, 0x3c492cda, 0x43325992, 0x5bc31283, 0x3891deaa, 0x5ddddd43, 0x64314225, 0x43a1c73b, 0x5e0ec431, 0x27f583bf, 0x491a73dc, 0x377982b8, 0x0b760791, 0x7835c61d, 0x0192b0f7, 0x129ca7ca, 0x16333e2d, 0x39c07864, 0x4d4b0d1b, 0x252fdb31, 0x2ca1bef8, 0x0358e216, 0x6aeda289, 0x201bca8f, 0x6ded1451, 0x5fbb04bb, 0x03d86c3b, 0x2f402c96, 0x4567a0ad, 0x6159d4de, 0x494f89b9, 0x7035fbdf, 0x5c3cfbd6, 0x40bfdbc8, 0x5e27b49c, 0x4fee508e, 0x76bd2f29, 0x45b49e47, 0x7da53921, 0x3d9380aa, 0x5d5bce86, 0x212b2912, 0x2e3e0a61, 0x218cfe04, 0x22ebe499, 0x70713a1c, 0x5b690632, 0x3f5c8dc1, 0x323815a5, 0x1f9f4c8f, 0x56a54a21, 0x469a2122, 0x6fb2beea, 0x643ac847, 0x01d2c5bc, 0x7dc51683, 0x0b876ebe, 0x214a069a, 0x535258ed, 0x529b3dce, 0x2760b2c2, 0x78cf5cd3, 0x19d1d3f2, 0x572ebc02, 0x1ab91604, 0x0b5f09a0}, + ninv = {0x00000000, 0x00000000, 0x66e2f883, 0x00000000, 0x210176d3, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x40ad7911, 0x3954a759, 0x00000000, 0x00000000, 0x657333dd, 0x11c1b97d, 0x1c122953, 0x40642277, 0x00000000, 0x00000000, 0x7f3dc8c5, 0x00000000, 0x166a06e5, 0x00000000, 0x00000000, 0x605cca3f, 0x3548cdb1, 0x35b19ec7, 0x681845b3, 0x1c438fa5, 0x00000000, 0x00000000, 0x2beff359, 0x79f2b241, 0x00000000, 0x00000000, 0x00000000, 0x6040b3d5, 0x00000000, 0x1ee96895, 0x1f18f653, 0x7f80860d, 0x3901eb2f, 0x56ff93c1, 0x00000000, 0x00000000, 0x562e668f, 0x72bad3cb, 0x3de0ef39, 0x00000000, 0x6be23e5b, 0x00000000, 0x3a327aed, 0x0fae622f, 0x00000000, 0x00000000, 0x004d8c47, 0x5a1feb91, 0x76f34b4f, 0x68675f8d, 0x7dec730d, 0x00000000, 0x5a953cdb, 0x00000000, 0x65605377, 0x7aa67fe1, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x4eaf22e7, 0x4a585489, 0x3fa2751f, 0x00000000, 0x00000000, 0x00000000, 0x7107e65f, 0x00000000, 0x5f81d057, 0x00000000, 0x00000000, 0x2d7b7dbf, 0x0e2f35d3, 0x75e7ad91, 0x4ed7461f, 0x00000000, 0x00000000, 0x4a199e89, 0x00000000, 0x45bf97d5, 0x00000000, 0x00000000, 0x4ada3b1b, 0x00000000, 0x00000000, 0x7456a4a5, 0x00000000, 0x00000000, 0x00000000, 0x00000000}, +} + +I31_Test_Monty_Mul :: struct { + x: []u32, + y: []u32, + m: []u32, + res: []u32, +} + +@(rodata) +i31_monty_mul_test_vectors := []I31_Test_Monty_Mul { + {x = {127, 0x7c5a766e, 0x6ce75a35, 0x1c64cb75, 0x51263ee0, 0x00000000}, y = {127, 0x75063ae3, 0x3f506a6f, 0x1d1ad6d5, 0x47a85d53, 0x00000000}, m = {159, 0x7e0a5a33, 0x41c36cc5, 0x065bfbbd, 0x4608e748, 0x69ac6740, 0x00000000}, res = {159, 0x3e2b1a07, 0x4990ba63, 0x15f1a1ea, 0x6e8ff678, 0x13ba5b66}}, + {x = {124, 0x5232b140, 0x341f2854, 0x3e6bde54, 0x092324e1, 0x00000000}, y = {127, 0x6c66ba58, 0x51577b9e, 0x72a600d4, 0x53bd0450, 0x00000000}, m = {159, 0x153010a5, 0x7d0ddec2, 0x13c7f180, 0x1cd216b3, 0x5818bdd7, 0x00000000}, res = {159, 0x6de73758, 0x21d8ff38, 0x69b18e1e, 0x5d1c7d47, 0x41c75523}}, + {x = {126, 0x176e0f5b, 0x0a98100a, 0x475e0a72, 0x348f9141, 0x00000000}, y = {127, 0x334e2dfe, 0x2c988363, 0x73e23cf8, 0x606dcc5f, 0x00000000}, m = {159, 0x7023de77, 0x54103e3b, 0x338cd291, 0x0e2c1e77, 0x6fa41147, 0x00000000}, res = {159, 0x488db6ba, 0x3ec6b26b, 0x0dddf8b5, 0x4e0334db, 0x2125e61c}}, + {x = {127, 0x1018738e, 0x233c04d7, 0x46a665c0, 0x79c84e0a, 0x00000000}, y = {127, 0x783e44fe, 0x14a1ea17, 0x31bea107, 0x66c49111, 0x00000000}, m = {158, 0x580ca6c1, 0x61ad5aad, 0x238b9bdb, 0x0ce14c93, 0x2749fc5c, 0x00000000}, res = {158, 0x37557af5, 0x5ca2f6fb, 0x7fdca455, 0x4d436f9b, 0x18d60f4f}}, + {x = {126, 0x45da293a, 0x1665982a, 0x0e8fc303, 0x231c2772, 0x00000000}, y = {126, 0x542485c1, 0x3f2f7d3f, 0x684047a6, 0x3abaed33, 0x00000000}, m = {159, 0x03d31fd1, 0x6f0c8ff3, 0x12825696, 0x199fda81, 0x76457daf, 0x00000000}, res = {159, 0x6c6852a3, 0x0d1fd2b0, 0x67e96590, 0x768c4fca, 0x3ad66382}}, + {x = {126, 0x21d23a0e, 0x5ba5d464, 0x29697487, 0x338d2cff, 0x00000000}, y = {125, 0x29e49a2c, 0x144564ae, 0x3d17958d, 0x101f7e41, 0x00000000}, m = {158, 0x4545452b, 0x6e68f705, 0x2302e752, 0x07b5d5d6, 0x3657a220, 0x00000000}, res = {158, 0x3e90a0ed, 0x457cb17b, 0x53b4f9f5, 0x7a6324ce, 0x35d2cdd7}}, + {x = {125, 0x4732d8b6, 0x45573e7d, 0x07f4963f, 0x12a290dd, 0x00000000}, y = {127, 0x39baf88d, 0x5d858830, 0x4580eb58, 0x6d196a63, 0x00000000}, m = {159, 0x0cb6812b, 0x482b08af, 0x4f1b863c, 0x3cef2e92, 0x50f7c7f1, 0x00000000}, res = {159, 0x6ad503c6, 0x27c67766, 0x5fca2845, 0x15bbcb6b, 0x46c3cb6c}}, + {x = {127, 0x354a4328, 0x651d0b7f, 0x43a452fb, 0x7bd74c98, 0x00000000}, y = {126, 0x3cfc2842, 0x71256a54, 0x3719176f, 0x34407271, 0x00000000}, m = {159, 0x306eba87, 0x772cb87d, 0x4907c954, 0x5dc5e91b, 0x74d5eb17, 0x00000000}, res = {159, 0x49e12a70, 0x7e89a281, 0x1ea3548a, 0x54711053, 0x713bfe04}}, + {x = {126, 0x4affb101, 0x52a6864d, 0x7152422e, 0x3cca5f4e, 0x00000000}, y = {126, 0x3f4b1e18, 0x32aaa89a, 0x635dcdc7, 0x3c44b1de, 0x00000000}, m = {158, 0x01432ccf, 0x2bcf7f6d, 0x7561291c, 0x7064f0cb, 0x3381e843, 0x00000000}, res = {158, 0x03895437, 0x6792e4bb, 0x68391714, 0x5a6104d6, 0x16bbc670}}, + {x = {127, 0x56c42650, 0x2723c136, 0x4d64beac, 0x5e656e91, 0x00000000}, y = {126, 0x1aa1d10f, 0x01fffab5, 0x44fd097e, 0x3483b271, 0x00000000}, m = {159, 0x2f77d067, 0x1f191f5b, 0x1a1a8568, 0x52026c09, 0x4692f6e6, 0x00000000}, res = {159, 0x1b1bfa15, 0x3726bd88, 0x1af57d8e, 0x2de30fb3, 0x3f96e86f}}, +} + +I31_Test_To_Monty :: struct { + orig: []u32, + x: []u32, + m: []u32, +} + +@(rodata) +i31_to_monty_test_vectors := []I31_Test_To_Monty { + {orig = {123, 0x3f3aff0e, 0x459fe053, 0x4bba523d, 0x04b91a78, 0x00000000}, x = {123, 0x2eab2260, 0x4d3d8e5d, 0x2943569d, 0x01d1c5f3, 0x00000000}, m = {123, 0x3f3aff1b, 0x459fe061, 0x4bba524b, 0x04b91a78, 0x00000000}}, + {orig = {127, 0x111641a2, 0x4fcfb2b4, 0x1cf1576b, 0x6b088658, 0x00000000}, x = {127, 0x25c08f99, 0x3bdc976a, 0x3a808f23, 0x5be48931, 0x00000000}, m = {127, 0x111641af, 0x4fcfb2c1, 0x1cf15779, 0x6b088658, 0x00000000}}, + {orig = {126, 0x03accd4a, 0x34a7e2d7, 0x749af3f3, 0x25febe05, 0x00000000}, x = {126, 0x763fca85, 0x61a5498c, 0x28eb0856, 0x1dab3aee, 0x00000000}, m = {126, 0x03accd57, 0x34a7e2e5, 0x749af401, 0x25febe05, 0x00000000}}, + {orig = {127, 0x6d8c4617, 0x01cffa24, 0x3475f32e, 0x7c9ba743, 0x00000000}, x = {127, 0x7304a0f6, 0x653ea916, 0x6c412db9, 0x427e57a8, 0x00000000}, m = {127, 0x6d8c4625, 0x01cffa31, 0x3475f33b, 0x7c9ba743, 0x00000000}}, + {orig = {127, 0x2da2a286, 0x152f4795, 0x3f383173, 0x6622cb58, 0x00000000}, x = {127, 0x1e118541, 0x63461082, 0x57a7e060, 0x3529988e, 0x00000000}, m = {127, 0x2da2a293, 0x152f47a3, 0x3f383181, 0x6622cb58, 0x00000000}}, + {orig = {127, 0x2a17a287, 0x52b707dd, 0x30d2b30f, 0x6334fa45, 0x00000000}, x = {127, 0x06823214, 0x2c2ddf09, 0x286443db, 0x231af948, 0x00000000}, m = {127, 0x2a17a295, 0x52b707eb, 0x30d2b31d, 0x6334fa45, 0x00000000}}, + {orig = {127, 0x41152b43, 0x0a898780, 0x5a517da6, 0x72eb605e, 0x00000000}, x = {127, 0x2761082a, 0x1e1ff23b, 0x3f2516a3, 0x0b165b25, 0x00000000}, m = {127, 0x41152b51, 0x0a89878d, 0x5a517db3, 0x72eb605e, 0x00000000}}, + {orig = {127, 0x6b29c7ff, 0x6297d9f9, 0x3225bebb, 0x71bc4932, 0x00000000}, x = {127, 0x4bb7368d, 0x12320331, 0x5cc9ce0e, 0x1eb2ad59, 0x00000000}, m = {127, 0x6b29c80d, 0x6297da07, 0x3225bec9, 0x71bc4932, 0x00000000}}, + {orig = {127, 0x6cb514c0, 0x3ff230d8, 0x6a6915f5, 0x73655603, 0x00000000}, x = {127, 0x1ebae285, 0x28f81b38, 0x37f80ed8, 0x21dc723e, 0x00000000}, m = {127, 0x6cb514cd, 0x3ff230e5, 0x6a691603, 0x73655603, 0x00000000}}, + {orig = {127, 0x632339fa, 0x30508879, 0x038d4f85, 0x4bff3ced, 0x00000000}, x = {127, 0x3a3502a9, 0x4364df08, 0x3c8b1fdc, 0x20b2fa04, 0x00000000}, m = {127, 0x63233a07, 0x30508887, 0x038d4f93, 0x4bff3ced, 0x00000000}}, +} + +I31_Test_Mod_Pow :: struct { + orig: []u32, + x: []u32, + e: []u8, + m: []u32, +} + +@(rodata) +i31_mod_pow_test_vectors := []I31_Test_Mod_Pow { + {orig = {123, 0x2eab2260, 0x4d3d8e5d, 0x2943569d, 0x01d1c5f3, 0x00000000}, x = {123, 0x030692d4, 0x14836f1f, 0x7c343636, 0x014fdd77, 0x00000000}, e = {81, 188, 252, 55, 213, 144, 254, 197, 116, 215, 162}, m = {123, 0x3f3aff1b, 0x459fe061, 0x4bba524b, 0x04b91a78, 0x00000000}}, + {orig = {127, 0x25c08f99, 0x3bdc976a, 0x3a808f23, 0x5be48931, 0x00000000}, x = {127, 0x3cd4569e, 0x6474f743, 0x23fc7461, 0x05e72909, 0x00000000}, e = {145, 145, 164, 213, 254, 94, 63, 192, 72, 205, 16}, m = {127, 0x111641af, 0x4fcfb2c1, 0x1cf15779, 0x6b088658, 0x00000000}}, + {orig = {126, 0x763fca85, 0x61a5498c, 0x28eb0856, 0x1dab3aee, 0x00000000}, x = {126, 0x0eb01d51, 0x178d8313, 0x044cbdfa, 0x022d38b4, 0x00000000}, e = {254, 224, 156, 88, 224, 176, 213, 125, 112, 43, 72}, m = {126, 0x03accd57, 0x34a7e2e5, 0x749af401, 0x25febe05, 0x00000000}}, + {orig = {127, 0x7304a0f6, 0x653ea916, 0x6c412db9, 0x427e57a8, 0x00000000}, x = {127, 0x3b1cad0e, 0x15e996b3, 0x6ccc145e, 0x63b7aa04, 0x00000000}, e = {250, 142, 147, 53, 107, 204, 201, 168, 122, 147, 143}, m = {127, 0x6d8c4625, 0x01cffa31, 0x3475f33b, 0x7c9ba743, 0x00000000}}, + {orig = {127, 0x1e118541, 0x63461082, 0x57a7e060, 0x3529988e, 0x00000000}, x = {127, 0x3ed716f7, 0x09f828bb, 0x00840e9d, 0x19c59712, 0x00000000}, e = {244, 245, 26, 174, 174, 246, 9, 106, 176, 161, 120}, m = {127, 0x2da2a293, 0x152f47a3, 0x3f383181, 0x6622cb58, 0x00000000}}, + {orig = {127, 0x06823214, 0x2c2ddf09, 0x286443db, 0x231af948, 0x00000000}, x = {127, 0x79062be3, 0x5787887e, 0x59a8999f, 0x3f1c71f1, 0x00000000}, e = {212, 118, 203, 181, 125, 193, 238, 33, 133, 239, 12}, m = {127, 0x2a17a295, 0x52b707eb, 0x30d2b31d, 0x6334fa45, 0x00000000}}, + {orig = {127, 0x2761082a, 0x1e1ff23b, 0x3f2516a3, 0x0b165b25, 0x00000000}, x = {127, 0x7a632df1, 0x1775c47f, 0x30b8030b, 0x562da059, 0x00000000}, e = {139, 161, 176, 68, 175, 27, 137, 101, 59, 37, 210}, m = {127, 0x41152b51, 0x0a89878d, 0x5a517db3, 0x72eb605e, 0x00000000}}, + {orig = {127, 0x4bb7368d, 0x12320331, 0x5cc9ce0e, 0x1eb2ad59, 0x00000000}, x = {127, 0x00db078c, 0x078b7daf, 0x2c19dc27, 0x5e45284f, 0x00000000}, e = {154, 81, 208, 248, 214, 247, 12, 230, 127, 200, 184}, m = {127, 0x6b29c80d, 0x6297da07, 0x3225bec9, 0x71bc4932, 0x00000000}}, + {orig = {127, 0x1ebae285, 0x28f81b38, 0x37f80ed8, 0x21dc723e, 0x00000000}, x = {127, 0x1f004d80, 0x15576afe, 0x7dccb09d, 0x31928c9a, 0x00000000}, e = {164, 131, 3, 178, 189, 171, 249, 186, 241, 118, 179}, m = {127, 0x6cb514cd, 0x3ff230e5, 0x6a691603, 0x73655603, 0x00000000}}, + {orig = {127, 0x3a3502a9, 0x4364df08, 0x3c8b1fdc, 0x20b2fa04, 0x00000000}, x = {127, 0x13718bdd, 0x3ea119db, 0x1d2488e5, 0x02324bed, 0x00000000}, e = {107, 45, 140, 104, 190, 245, 72, 158, 102, 160, 168}, m = {127, 0x63233a07, 0x30508887, 0x038d4f93, 0x4bff3ced, 0x00000000}}, +} + +I31_Test_Mul_Acc :: struct { + res: []u32, + d: []u32, + a: []u32, + b: []u32, +} + +@(rodata) +i31_mul_acc_test_vectors := []I31_Test_Mul_Acc { + {res = {317, 0x535a1b12, 0x44ccd6ec, 0x1fa7b212, 0x584b46e2, 0x222bf023, 0x4580794b, 0x67411ea6, 0x47b4ac1d, 0x229d50a9, 0x1818f1a8, 0x00000000}, d = {157, 0x79ef7e23, 0x4387243c, 0x0f107f29, 0x3567d945, 0x24e08c78, 0x00000000}, a = {157, 0x06deb1d1, 0x7bba988f, 0x46c80867, 0x490dafbc, 0x1f998ea4, 0x00000000}, b = {159, 0x1c1f52bf, 0x56cb5e69, 0x4f743315, 0x56bd7776, 0x619c427b, 0x00000000}}, + {res = {317, 0x3ef33eed, 0x50471631, 0x244d5b99, 0x3aace6d9, 0x3152bb58, 0x13b7c355, 0x0e321425, 0x567217ce, 0x31b3e5b9, 0x0d74845e, 0x00000000}, d = {157, 0x736962be, 0x7712398c, 0x7b098d1f, 0x43804b77, 0x3bd3f8bf, 0x00000000}, a = {157, 0x12a5d163, 0x4ae379f1, 0x1387fd7f, 0x1dec4842, 0x195d1d4c, 0x00000000}, b = {159, 0x6023c9c5, 0x3740c9c3, 0x612c755b, 0x6f3d0941, 0x43e704ff, 0x00000000}}, + {res = {318, 0x5c85ea39, 0x54f32864, 0x507dd4d4, 0x6250b7ce, 0x1334f373, 0x445b332d, 0x14557fc2, 0x7e1738be, 0x21bab745, 0x2f0aa313, 0x00000000}, d = {158, 0x77f935c9, 0x1e13e902, 0x43c028fd, 0x4937d854, 0x7e23bf79, 0x00000000}, a = {158, 0x1d618801, 0x79681bf4, 0x7f92559f, 0x2f1cccaf, 0x3faefa11, 0x00000000}, b = {159, 0x32413470, 0x5aff5bf1, 0x05f98105, 0x53055486, 0x5e8cf955, 0x00000000}}, + {res = {319, 0x3e013eac, 0x07fe4a8a, 0x65782090, 0x5b880ea5, 0x7408456c, 0x5bf575a0, 0x2a3cd440, 0x23d148fa, 0x418bc6d7, 0x31cc34a1, 0x00000000}, d = {159, 0x102ef9f4, 0x1bc81608, 0x02f901f1, 0x668b0bd6, 0x283c9d2b, 0x00000000}, a = {159, 0x7da4773e, 0x360df7cb, 0x2b3be4f2, 0x79faa607, 0x5e09cc20, 0x00000000}, b = {159, 0x57ef4024, 0x32c96c31, 0x4559d5d3, 0x3b4d08ae, 0x43c831a5, 0x00000000}}, + {res = {317, 0x2588c09a, 0x7a89223b, 0x41126c82, 0x321cdef0, 0x3faf4f56, 0x54b95fd5, 0x44abcdbd, 0x4c9acfbb, 0x5d732ab6, 0x1e44b843, 0x00000000}, d = {158, 0x524131a8, 0x6388c83c, 0x4a50e580, 0x0df9f5b8, 0x4fb833b3, 0x00000000}, a = {158, 0x523a18b9, 0x465a2251, 0x2dd0ebec, 0x7a14c3c3, 0x3caa78eb, 0x00000000}, b = {158, 0x30ec8982, 0x37707644, 0x7f244b46, 0x6f958529, 0x3fdd26db, 0x00000000}}, + {res = {318, 0x61bad587, 0x761aa039, 0x6ab9a7d4, 0x41f18ba1, 0x0f77af87, 0x557292c8, 0x3428f9a7, 0x7c028945, 0x405a01c3, 0x1f3c11e1, 0x00000000}, d = {159, 0x62a6cc33, 0x07c52956, 0x4b78094d, 0x0210300b, 0x59a0cc24, 0x00000000}, a = {159, 0x6538ecfc, 0x666ac7d2, 0x62ada152, 0x1b6f0537, 0x40b88005, 0x00000000}, b = {158, 0x4f69016b, 0x2b90ac7e, 0x547c3f0e, 0x2f1de4c9, 0x3dc60ec7, 0x00000000}}, + {res = {318, 0x7c1d72ed, 0x4c4d8e1f, 0x461bb74f, 0x38b75f85, 0x0c340951, 0x37882272, 0x5bb93d3e, 0x5f8ed31e, 0x7bd1add0, 0x258ae081, 0x00000000}, d = {158, 0x007d10b7, 0x7e03550f, 0x28189fea, 0x3407d2f0, 0x44f16726, 0x00000000}, a = {158, 0x152afd5f, 0x71ab1c22, 0x6fb41945, 0x522bd719, 0x3025871d, 0x00000000}, b = {159, 0x7272538a, 0x795fd146, 0x7111af44, 0x2b400ac9, 0x63cef8dc, 0x00000000}}, + {res = {317, 0x6ac9a927, 0x37bf839e, 0x72166cf4, 0x66e97192, 0x0f1010a8, 0x475dd251, 0x730eb0ff, 0x295b1515, 0x340bb235, 0x14da57c9, 0x00000000}, d = {157, 0x026ce5ab, 0x49a7c219, 0x7d86a513, 0x47701297, 0x56e7b169, 0x00000000}, a = {157, 0x6f6a9a0e, 0x7e018fa4, 0x23b35254, 0x2566e0a9, 0x158799e8, 0x00000000}, b = {159, 0x12e6bed2, 0x3b186bb1, 0x3bddcfc9, 0x75ca6013, 0x7bf9ee6e, 0x00000000}}, + {res = {317, 0x277fe2cd, 0x00e40ce6, 0x2c35b175, 0x59bd9a99, 0x2a53689e, 0x3972c269, 0x4db20926, 0x67d98c0d, 0x57bbbc3f, 0x0bf1a543, 0x00000000}, d = {159, 0x17d3efaf, 0x59a9c4ff, 0x69349a09, 0x535b0031, 0x677b2a0d, 0x00000000}, a = {159, 0x67e1ef0e, 0x7d1e406e, 0x5111e847, 0x321572bc, 0x547d3b3a, 0x00000000}, b = {157, 0x1c685fb9, 0x2f3d09a2, 0x6611e3c9, 0x25575675, 0x12184aec, 0x00000000}}, + {res = {317, 0x11c8df23, 0x0d48a117, 0x0a6b8c50, 0x0fef02e9, 0x3eb1ae96, 0x32cd060b, 0x0cdb0651, 0x2dc87323, 0x16ea6bb3, 0x09cf6776, 0x00000000}, d = {159, 0x53b8638b, 0x4e180172, 0x3a3cdcac, 0x614f28ca, 0x0f644498, 0x00000000}, a = {159, 0x72ac2c95, 0x196ed06f, 0x43412b53, 0x59441827, 0x4364974f, 0x00000000}, b = {157, 0x0e220f38, 0x02785026, 0x6566e3fe, 0x1a270c17, 0x12a1eeaa, 0x00000000}}, +} + +I31_Test_Div_Rem :: struct { + hi: u32, + lo: u32, + div: u32, + quo: u32, + rem: u32, +} + +@(rodata) +i31_div_rem_test_vectors := []I31_Test_Div_Rem { + {hi = 0x632c115c, lo = 0x4b2bf821, div = 0xb8481290, quo = 0x89c490d1, rem = 0x07a3d091}, + {hi = 0x1cb7e3aa, lo = 0x63e1d659, div = 0xd94a9eb0, quo = 0x21d58fe1, rem = 0x02380da9}, + {hi = 0xbe78690c, lo = 0x88cc1bd7, div = 0xbf1211a9, quo = 0xff32200f, rem = 0x4a85f2f0}, + {hi = 0xa91488a1, lo = 0x2f8f64ac, div = 0xb5027f82, quo = 0xef20d04a, rem = 0x86fce918}, + {hi = 0x074838b4, lo = 0xc1b7bbbe, div = 0x0cc1ef1e, quo = 0x92207a42, rem = 0x0c03ca02}, + {hi = 0x415c651d, lo = 0xe696b3e5, div = 0x5a782289, quo = 0xb8f37d3e, rem = 0x149671b7}, + {hi = 0x26454389, lo = 0x986fc51a, div = 0xebc02a27, quo = 0x298ec804, rem = 0x27dea47e}, + {hi = 0x5a3b3e87, lo = 0xf475705b, div = 0x5e6f1d1a, quo = 0xf49b708c, rem = 0x4c382623}, + {hi = 0x8b1580f2, lo = 0x4ca1db17, div = 0xafd69ac8, quo = 0xca7d730d, rem = 0x938c26ef}, + {hi = 0x0dd6f07e, lo = 0xde23b70c, div = 0x5bd60fda, quo = 0x26943342, rem = 0x05c332d8}, + {hi = 0xede68f1d, lo = 0x07813b19, div = 0xff2b167e, quo = 0xeead1023, rem = 0x1c0f47df}, + {hi = 0x5c5ec31f, lo = 0xd985218f, div = 0xae020a42, quo = 0x87e50b9b, rem = 0x6cce1599}, + {hi = 0x6639e3c0, lo = 0xe8690a35, div = 0x6c241512, quo = 0xf1ff7b0a, rem = 0x69f29181}, + {hi = 0x0a45fb42, lo = 0x113dd8cc, div = 0x2ee12611, quo = 0x3819d4aa, rem = 0x0c8b7d82}, + {hi = 0x3ea35d11, lo = 0x5e2a590f, div = 0xd465b470, quo = 0x4b7f3d2b, rem = 0x21865a3f}, + {hi = 0xcb7cee11, lo = 0x25cea707, div = 0xe191debc, quo = 0xe6f0751f, rem = 0x7218c243}, + {hi = 0x816b74f4, lo = 0xb66d7312, div = 0x83e048ef, quo = 0xfb3b4f1d, rem = 0x6b6e6eff}, + {hi = 0x4863d8a6, lo = 0x7de42a8b, div = 0x588fc2d3, quo = 0xd140fa7c, rem = 0x3c3fbe57}, + {hi = 0x2540246a, lo = 0xe24d9d05, div = 0x8320712a, quo = 0x48b98123, rem = 0x047dfa47}, + {hi = 0xc074131e, lo = 0xdc4d465e, div = 0xe1dc958c, quo = 0xda22448b, rem = 0x8d36e35a}, + {hi = 0xf6ef68bc, lo = 0xc22f3f3e, div = 0xfa5c4162, quo = 0xfc7f66e0, rem = 0x07cafd7e}, + {hi = 0x061d173b, lo = 0xeccf58ec, div = 0xb792ff7a, quo = 0x08869175, rem = 0x3a107c2a}, + {hi = 0xb2ee2b9d, lo = 0x7805c28d, div = 0xf610e48b, quo = 0xba276d3a, rem = 0xb7b5cc0f}, + {hi = 0x38849ac0, lo = 0x67df2bd4, div = 0xa314f95e, quo = 0x58b84824, rem = 0x0739aa9c}, + {hi = 0x32dc45d0, lo = 0x2c7f42e9, div = 0xfafc8a2a, quo = 0x33e05b19, rem = 0xa1f8d6cf}, + {hi = 0x4240d91f, lo = 0xec3b2dd8, div = 0x7a1bc3fe, quo = 0x8ae65d59, rem = 0x602cc48a}, + {hi = 0x3c0836b4, lo = 0x275cd2a6, div = 0x871c1085, quo = 0x71bf0a5e, rem = 0x6a2e8fd0}, + {hi = 0x49de09c7, lo = 0xf62ac65b, div = 0x9b7ae8a1, quo = 0x799f9edf, rem = 0x1587c41c}, + {hi = 0x3da17dbc, lo = 0xe4437bf3, div = 0xf64f9bca, quo = 0x400e1fa2, rem = 0x5cf9701f}, + {hi = 0x4e22607b, lo = 0xa8798c83, div = 0xfe90aa77, quo = 0x4e931fa6, rem = 0xedb19a59}, + {hi = 0x43dd8e64, lo = 0xd2aee2d3, div = 0xfa05bf14, quo = 0x457ceca2, rem = 0x5d35882b}, + {hi = 0x74ae120e, lo = 0x4a567457, div = 0x80e12b3b, quo = 0xe7c46e8b, rem = 0x3954a14e}, + {hi = 0x25c33e85, lo = 0xd4fe503d, div = 0xab5e1c7a, quo = 0x38698d75, rem = 0x4f421a7b}, + {hi = 0xba818477, lo = 0xf1edb77a, div = 0xf5028225, quo = 0xc2df32d7, rem = 0x472c3067}, + {hi = 0xce85c4d5, lo = 0xc1530b0c, div = 0xe5a6d9ab, quo = 0xe63795d9, rem = 0x94770219}, + {hi = 0x08adb5bb, lo = 0xa277cb09, div = 0xab1ee4c6, quo = 0x0cfbb916, rem = 0x045b0c05}, + {hi = 0x8dc1580e, lo = 0xe73d2e39, div = 0xadf26902, quo = 0xd09f833f, rem = 0x349b50bb}, + {hi = 0x4435ddf6, lo = 0x42a05702, div = 0x594f5cd1, quo = 0xc3850b8d, rem = 0x3d583ce5}, + {hi = 0x6cb4f9c6, lo = 0xcc9b6e48, div = 0xa7bbed72, quo = 0xa5e948ca, rem = 0x00c80254}, + {hi = 0x3cb3b260, lo = 0xf2f7a398, div = 0xecf16edf, quo = 0x419586bc, rem = 0x6ad67dd4}, + {hi = 0x1720b73c, lo = 0x347ff447, div = 0x1724839d, quo = 0xffd5fbb6, rem = 0x0ede73a9}, + {hi = 0xe3003f2c, lo = 0xe1cb1bdf, div = 0xf2298637, quo = 0xeff8ef99, rem = 0x04648c00}, + {hi = 0x558a5621, lo = 0x1f717ddc, div = 0xf03e2bca, quo = 0x5b269cff, rem = 0xa0d8c7a6}, + {hi = 0x949fb36a, lo = 0xce76c920, div = 0xd215ba56, quo = 0xb51b3884, rem = 0x456de4c8}, + {hi = 0xa0f2fc11, lo = 0xd0259ff1, div = 0xc8626cac, quo = 0xcd9ea15a, rem = 0xa90b3f79}, + {hi = 0x70c3fa84, lo = 0x74d29bb9, div = 0xf9e5a764, quo = 0x7384fbe9, rem = 0x9c1e35b5}, + {hi = 0x2da900de, lo = 0xcf74abff, div = 0x99abc1da, quo = 0x4c10ae3a, rem = 0x6b28949b}, + {hi = 0x3664397e, lo = 0x2ba486dc, div = 0x7ea469c9, quo = 0x6df304d3, rem = 0x39af3231}, + {hi = 0x095d67a6, lo = 0xe3b31742, div = 0xdbe7549c, quo = 0x0ae6ee14, rem = 0x88cf7312}, + {hi = 0x9a402804, lo = 0x462e5dcd, div = 0xa736e128, quo = 0xec272359, rem = 0x76399ee5}, + {hi = 0x12809274, lo = 0x09011d2f, div = 0xac4c1332, quo = 0x1b7d9f66, rem = 0x7d5b6943}, + {hi = 0x6a897aed, lo = 0x1d121571, div = 0xef42fe4b, quo = 0x71fd7b8c, rem = 0xa921fb6d}, + {hi = 0x55ed8b10, lo = 0x9090ea62, div = 0x9cd2c065, quo = 0x8c45083c, rem = 0x21efaab6}, + {hi = 0x3930a3f2, lo = 0x541cf31f, div = 0xa7c1fef1, quo = 0x5745c1d4, rem = 0x884d228b}, + {hi = 0xa7167582, lo = 0x0abb9b9f, div = 0xf63dc17e, quo = 0xadb5a66e, rem = 0x3ca4c37b}, + {hi = 0x74b72666, lo = 0x4abb1c94, div = 0x7ee27c4a, quo = 0xeb7b903b, rem = 0x0110d786}, + {hi = 0x88a89e33, lo = 0x2dada2d6, div = 0xe8725e91, quo = 0x9681856f, rem = 0x5be44cf7}, + {hi = 0x5affad0b, lo = 0x68c7b744, div = 0x8bf402e7, quo = 0xa6740fb8, rem = 0x6a8e183c}, + {hi = 0x18400a64, lo = 0x91c19fee, div = 0xececa37b, quo = 0x1a33df48, rem = 0xbd4a8056}, + {hi = 0x0ca1d509, lo = 0x5ace5b38, div = 0x405fe35b, quo = 0x323c1088, rem = 0x1a53e2e0}, + {hi = 0x49c3d579, lo = 0xb57e3052, div = 0x8fe8c087, quo = 0x833871ec, rem = 0x1b691cde}, + {hi = 0x055e153e, lo = 0x876ba83b, div = 0xd0a7b0e1, quo = 0x0695deae, rem = 0x1119514d}, + {hi = 0x1ab22cf1, lo = 0xc9c28af3, div = 0x626e63e5, quo = 0x456e54c0, rem = 0x18ca7b33}, + {hi = 0x5ce6ba97, lo = 0x46dbc829, div = 0xbf4a582f, quo = 0x7c53ef3f, rem = 0x6fff3398}, + {hi = 0x280dd6e5, lo = 0x13c19c79, div = 0xe67fd7e2, quo = 0x2c7c3e0b, rem = 0xcc8299c3}, + {hi = 0x1c7c2a51, lo = 0x8aecb540, div = 0x3decec9d, quo = 0x75c1d104, rem = 0x02afd5cc}, + {hi = 0x3a6e806b, lo = 0x142a6ad0, div = 0x56b24a8d, quo = 0xac89eeee, rem = 0x17a505ba}, + {hi = 0x23a1b1d0, lo = 0xda933d16, div = 0x85938fe1, quo = 0x4449c850, rem = 0x08e57ec6}, + {hi = 0x1f177f2e, lo = 0x107c53c1, div = 0xdef2389a, quo = 0x23b390c4, rem = 0x7d845dd9}, + {hi = 0x735cb354, lo = 0xbcf96584, div = 0xacc0d360, quo = 0xaaf3fec8, rem = 0x107b0284}, + {hi = 0x5ecb810e, lo = 0x51b4a6d1, div = 0x84db55d8, quo = 0xb6a8bd00, rem = 0x7d942ed1}, + {hi = 0x4765b107, lo = 0xded18aa4, div = 0x540b7c9f, quo = 0xd979b39b, rem = 0x4592e95f}, + {hi = 0xddb19fc7, lo = 0xe30727aa, div = 0xfaf579d9, quo = 0xe225a80a, rem = 0xcf1cfd30}, + {hi = 0x85f9707c, lo = 0x5511a037, div = 0x8e318efd, quo = 0xf133d2a5, rem = 0x5e6ded26}, + {hi = 0x0c2c41f1, lo = 0xbf357e4a, div = 0x62d254f1, quo = 0x1f88bf7d, rem = 0x421a359d}, + {hi = 0x335d8188, lo = 0x00fd4b21, div = 0xfcf1e085, quo = 0x33fc54b7, rem = 0x507e280e}, + {hi = 0x4eef80e4, lo = 0xfcd9e9de, div = 0xf2d2e5c0, quo = 0x53380265, rem = 0x07d9c51e}, + {hi = 0x431aeca5, lo = 0x07963290, div = 0xe6ca948b, quo = 0x4a6f5427, rem = 0xcfb6f563}, + {hi = 0x13638612, lo = 0x71f2880c, div = 0x25f44434, quo = 0x82c6db4c, rem = 0x0eddcc9c}, + {hi = 0x24dd96bb, lo = 0xd6b8ffca, div = 0xb3697b2b, quo = 0x349a55fc, rem = 0x61207a76}, + {hi = 0x3201b855, lo = 0x0e6b7026, div = 0x65a232ae, quo = 0x7df5a4a0, rem = 0x2a0e4b66}, + {hi = 0x1e2536d5, lo = 0xfbef9964, div = 0x97c9027e, quo = 0x32d7cea9, rem = 0x54699036}, + {hi = 0x03486ba3, lo = 0x4ae30d34, div = 0x5f768217, quo = 0x08cdbad7, rem = 0x1f6c15e3}, + {hi = 0x7000b42d, lo = 0xd9357d65, div = 0xc0b7f01f, quo = 0x94c7bd20, rem = 0x61d79685}, + {hi = 0x2f088bd1, lo = 0x098fe330, div = 0x3a2fc060, quo = 0xceee1d21, rem = 0x075d36d0}, + {hi = 0x30ede94b, lo = 0x1200278e, div = 0x42fc5880, quo = 0xbafe65ba, rem = 0x08bd5a8e}, + {hi = 0x5ed1fc0f, lo = 0x0537fbbf, div = 0x6ae246f5, quo = 0xe31b295f, rem = 0x630b69d4}, + {hi = 0x1d2496a2, lo = 0xecca6436, div = 0xaa60d4ea, quo = 0x2bc9d3a0, rem = 0x270e73f6}, + {hi = 0x97ae9a4e, lo = 0xba69862d, div = 0xea9984e9, quo = 0xa584c095, rem = 0x563c6a90}, + {hi = 0x12cf1435, lo = 0x99a99414, div = 0x2644910d, quo = 0x7dd36379, rem = 0x1871fdef}, + {hi = 0x7fc1828d, lo = 0x70c995ca, div = 0xdebea8f1, quo = 0x92d45c4d, rem = 0x9326294d}, + {hi = 0x32a003d1, lo = 0x6c09aff4, div = 0xd460becc, quo = 0x3d05fb48, rem = 0x7fc60294}, + {hi = 0x3add17a2, lo = 0xb4e2c499, div = 0x45b4eb3a, quo = 0xd82db19b, rem = 0x20833e7b}, + {hi = 0xc4a6d715, lo = 0xb1576cc0, div = 0xcf15af86, quo = 0xf31a46cc, rem = 0xb874e9f8}, + {hi = 0x2da93388, lo = 0x980b3f98, div = 0xf8e18784, quo = 0x2ef78f93, rem = 0x3b7bb2cc}, + {hi = 0x5d77f50a, lo = 0xc8f5c09b, div = 0xaa3cc6c4, quo = 0x8c8e703a, rem = 0x2f82f833}, + {hi = 0x4898d91b, lo = 0xe7956338, div = 0x4e4a2805, quo = 0xed62bc94, rem = 0x3c689454}, + {hi = 0xb9620ae8, lo = 0x0e9ed51d, div = 0xc281fe4d, quo = 0xf3fd8e35, rem = 0x458d792c}, + {hi = 0xc342d71a, lo = 0xecafbf43, div = 0xfbfa840c, quo = 0xc6609971, rem = 0x2db049f7}, + {hi = 0x4e2d4c4b, lo = 0x4609de2e, div = 0xc737fab5, quo = 0x64757da5, rem = 0x40d1e685}, +} diff --git a/tests/core/crypto/common/common.odin b/tests/core/crypto/common/common.odin index 8cef8ce86..a1fdef824 100644 --- a/tests/core/crypto/common/common.odin +++ b/tests/core/crypto/common/common.odin @@ -1,6 +1,7 @@ package test_crypto_common import "core:bytes" +import "core:encoding/base64" import "core:encoding/hex" // Common helpers for cryptography tests. @@ -23,3 +24,13 @@ hexbytes_decode :: proc(x: Hex_Bytes, allocator := context.allocator) -> []byte return dst } +Jwk_Bytes :: string + +jwkbytes_decode :: proc(s: Jwk_Bytes, allocator := context.allocator) -> []byte { + dst, err := base64.decode(s, base64.DEC_URL_TABLE, allocator = allocator) + if err != nil { + panic("Jwk_Bytes: invalid hex encoding") + } + + return dst +} diff --git a/tests/core/crypto/test_core_crypto_rsa.odin b/tests/core/crypto/test_core_crypto_rsa.odin new file mode 100644 index 000000000..525c440e3 --- /dev/null +++ b/tests/core/crypto/test_core_crypto_rsa.odin @@ -0,0 +1,419 @@ +#+build !riscv64 +package test_core_crypto + +import "core:bytes" +import "core:encoding/hex" +import "core:log" +import "core:testing" + +import "core:crypto" +import "core:crypto/rsa" + +// BUG/yawning: RISC-V fails the PSS test in CI with a nonsensical +// bounds-checking error suggesting something spooky, as the failure +// was not reproducible on qemu whole system emulation. + +@(private="file") +TEST_MSG: string : "don't let them immanentize the eschaton" +@(private="file", rodata) +TEST_MSG_SHA256 := []byte{ + 0x50, 0x95, 0x66, 0x8e, 0x7c, 0xd0, 0xd5, 0x8e, + 0x9d, 0x59, 0xf8, 0x4a, 0x1a, 0x46, 0x5a, 0x8a, + 0x2e, 0x69, 0xcc, 0xad, 0x6a, 0xca, 0x8b, 0xb7, + 0x55, 0x55, 0x15, 0x71, 0x9b, 0xc7, 0x50, 0x88, +} +@(private="file", rodata) +TEST_TLS_PMS := []byte{ + 0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07, 0x08, + 0x09, 0x0a, 0x0b, 0x0c, 0x0d, 0x0e, 0x0f, 0x10, + 0x11, 0x12, 0x13, 0x14, 0x15, 0x16, 0x17, 0x18, + 0x19, 0x1a, 0x1b, 0x1c, 0x1d, 0x1e, 0x1f, 0x20, + 0x21, 0x22, 0x23, 0x24, 0x25, 0x26, 0x27, 0x28, + 0x29, 0x2a, 0x2b, 0x2c, 0x2d, 0x2e, 0x2f, 0x30, +} + +@(test) +test_rsa_private_key :: proc(t: ^testing.T) { + priv_key: rsa.Private_Key + if !testing.expectf( + t, + rsa.private_key_set_insecure_test(&priv_key), + "rsa: failed to set test key", + ) { + return + } + + if !crypto.HAS_RAND_BYTES { + log.info("rand_bytes not supported - skipping generate") + return + } + + if !testing.expectf( + t, + rsa.private_key_generate(&priv_key), + "rsa: failed to generate private key", + ) { + return + } + + log.debugf("n=0x%s", hex.encode(priv_key._pub_key._n.v[:priv_key._pub_key._n.v_len], context.temp_allocator)) + log.debugf("e=%d", priv_key._pub_key._e) + log.debugf("d=0x%s", hex.encode(priv_key._d.v[:priv_key._d.v_len], context.temp_allocator)) + log.debugf("p=0x%s", hex.encode(priv_key._p.v[:priv_key._p.v_len], context.temp_allocator)) + log.debugf("q=0x%s", hex.encode(priv_key._q.v[:priv_key._q.v_len], context.temp_allocator)) + log.debugf("dp=0x%s", hex.encode(priv_key._dp.v[:priv_key._dp.v_len], context.temp_allocator)) + log.debugf("dq=0x%s", hex.encode(priv_key._dq.v[:priv_key._dq.v_len], context.temp_allocator)) + log.debugf("iq=0x%s", hex.encode(priv_key._iq.v[:priv_key._iq.v_len], context.temp_allocator)) + + pub_key: rsa.Public_Key + rsa.public_key_set_priv(&pub_key, &priv_key) + + if !testing.expectf( + t, + rsa.public_key_equal(&priv_key._pub_key, &pub_key), + "rsa: failed clone public key", + ) { + return + } +} + +@(test) +test_rsa_sign_pkcs1 :: proc(t: ^testing.T) { + test_msg := transmute([]byte)(TEST_MSG) + + priv_key: rsa.Private_Key + _ = rsa.private_key_set_insecure_test(&priv_key) + + pub_key: rsa.Public_Key + rsa.public_key_set_priv(&pub_key, &priv_key) + + // Generated with Go's crypto code. Signatures are + // deterministic. + expected_sig := []byte{ + 0x65, 0x0e, 0xa9, 0xd1, 0x66, 0xb3, 0x61, 0x4c, + 0x67, 0x8f, 0xd6, 0x9d, 0x2e, 0xc9, 0x24, 0xdd, + 0xcc, 0xa1, 0x0b, 0xdc, 0xbb, 0x35, 0x60, 0xc5, + 0x03, 0xb1, 0xa7, 0x10, 0x64, 0x53, 0x83, 0x02, + 0x7a, 0x8b, 0xc2, 0x83, 0x7f, 0xd6, 0xfc, 0xc3, + 0xe1, 0x4d, 0x33, 0x57, 0x90, 0x81, 0x52, 0xea, + 0xcd, 0x7c, 0xaa, 0xa5, 0x98, 0x59, 0x90, 0xd1, + 0x88, 0x21, 0x87, 0xc2, 0x9d, 0x51, 0x4d, 0x45, + 0x18, 0x06, 0xa2, 0xde, 0x7a, 0xc9, 0xc9, 0x1b, + 0x3d, 0x27, 0x07, 0xe4, 0xad, 0x46, 0xe8, 0x09, + 0xe1, 0xd5, 0xbd, 0x33, 0x1a, 0x9c, 0x7f, 0x3e, + 0x1b, 0x22, 0xc0, 0xfa, 0xa1, 0x30, 0xfb, 0xda, + 0x3b, 0x09, 0x9f, 0x6f, 0x44, 0x0e, 0xa8, 0x9b, + 0x65, 0x36, 0x48, 0x07, 0x95, 0xfc, 0xf7, 0x2b, + 0x61, 0xa0, 0x95, 0x36, 0x94, 0x66, 0x01, 0x90, + 0x5f, 0xf6, 0x72, 0x05, 0x64, 0x0c, 0x50, 0x8a, + 0xce, 0xfc, 0x75, 0xc7, 0x1e, 0x05, 0x62, 0xd9, + 0x8e, 0xdf, 0x85, 0x7e, 0x5d, 0x84, 0x8c, 0x0b, + 0x18, 0x88, 0x71, 0x4a, 0x94, 0x11, 0x5e, 0x16, + 0x53, 0xd0, 0xdc, 0x8e, 0x62, 0x73, 0x9d, 0x94, + 0x66, 0x1a, 0xdf, 0xf4, 0x01, 0xaa, 0xa5, 0x33, + 0xb3, 0x0c, 0x14, 0x53, 0x5d, 0xe7, 0xdb, 0x66, + 0xf0, 0x3e, 0xf0, 0x08, 0x81, 0x77, 0x40, 0x79, + 0xa5, 0xc6, 0x48, 0x68, 0xcc, 0xa2, 0xb6, 0xf3, + 0x1f, 0xf9, 0xd5, 0xb5, 0x26, 0x04, 0x97, 0xbc, + 0x93, 0xa3, 0x19, 0x29, 0x26, 0x86, 0x89, 0xf8, + 0x11, 0xe5, 0xdd, 0xbf, 0x35, 0x6b, 0x96, 0x25, + 0xa5, 0x78, 0xab, 0x02, 0x69, 0xdf, 0x54, 0x95, + 0xc5, 0x6b, 0x95, 0xcc, 0x38, 0x7b, 0x11, 0xfa, + 0x8c, 0x98, 0xa9, 0x95, 0x7e, 0x39, 0x7c, 0xf5, + 0x2f, 0xea, 0x42, 0x6d, 0xf5, 0xaa, 0xb3, 0x16, + 0x10, 0x2f, 0x29, 0xf3, 0x4a, 0xbc, 0x47, 0xfb, + } + + // Note: The key generate/set routines use the prehashed + // mode. + sig: [2048 >> 3]byte + ok := rsa.sign_pkcs1( + &priv_key, + .SHA256, + test_msg, + sig[:], + ) + if !testing.expectf(t, ok, "rsa/pkcs1: signing failed") { + return + } + + if !testing.expectf( + t, + bytes.equal(sig[:], expected_sig), + "rsa/pkcs1: signature mismatch: %x (expected %x)", + sig[:], + expected_sig, + ) { + return + } + + ok = rsa.verify_pkcs1( + &pub_key, + .SHA256, + test_msg, + expected_sig, + ) + if !testing.expectf(t, ok, "rsa/pkcs1: verify failed") { + return + } +} + +@(test) +test_rsa_sign_pss :: proc(t: ^testing.T) { + test_msg := transmute([]byte)(TEST_MSG) + + priv_key: rsa.Private_Key + _ = rsa.private_key_set_insecure_test(&priv_key) + + pub_key: rsa.Public_Key + rsa.public_key_set_priv(&pub_key, &priv_key) + + // Generated with Go's crypto code. Signatures are + // NOT deterministic. + test_sig := []byte{ + 0x46, 0xd3, 0x7e, 0xb2, 0x2c, 0x82, 0xe0, 0x68, + 0x80, 0x58, 0x8c, 0x07, 0x0e, 0xe9, 0x41, 0x3f, + 0xde, 0x45, 0x4a, 0xa0, 0x3f, 0xff, 0xa3, 0xa8, + 0xc2, 0x76, 0x74, 0xf0, 0xe8, 0x44, 0x07, 0x2d, + 0xdb, 0x12, 0xd5, 0x57, 0xcc, 0x28, 0x15, 0xfa, + 0xeb, 0xb6, 0x14, 0x76, 0x10, 0x3f, 0xfc, 0xba, + 0xb1, 0x6e, 0x7f, 0x65, 0x6c, 0x9b, 0x1a, 0x62, + 0x60, 0xc4, 0xfa, 0xb0, 0x03, 0x07, 0x2b, 0x6b, + 0x6a, 0x5a, 0x84, 0x10, 0xe4, 0xf5, 0x64, 0xad, + 0xfa, 0xd3, 0x5f, 0xc9, 0xf4, 0xac, 0x70, 0xde, + 0x54, 0x06, 0x14, 0x59, 0xf7, 0x60, 0x15, 0xc9, + 0xa2, 0xe2, 0x54, 0xbc, 0x79, 0xa8, 0x02, 0x72, + 0xba, 0x6a, 0x68, 0x08, 0x15, 0x6b, 0xcb, 0xc8, + 0x55, 0x83, 0x63, 0x06, 0xe4, 0x28, 0x36, 0x6b, + 0xe6, 0x15, 0x2a, 0x21, 0xc9, 0x4f, 0xc0, 0x0e, + 0x30, 0x54, 0x3b, 0xf1, 0xa4, 0x79, 0x83, 0xb9, + 0x02, 0xd9, 0x2e, 0xa1, 0x31, 0xf4, 0x5b, 0x1b, + 0xbe, 0x44, 0x17, 0xd8, 0x42, 0x6d, 0xb3, 0x38, + 0x3f, 0x23, 0x82, 0x9e, 0x76, 0x52, 0x0c, 0x5e, + 0xa0, 0xcd, 0xd1, 0xcc, 0xe2, 0x5b, 0x71, 0xb5, + 0xca, 0x28, 0x33, 0xc3, 0x03, 0x30, 0xc5, 0xa6, + 0xdc, 0x6e, 0xfd, 0xd7, 0x34, 0x0a, 0xd2, 0x30, + 0x2c, 0x80, 0x2d, 0x31, 0xea, 0xe9, 0x44, 0x2a, + 0x7e, 0x1d, 0xde, 0x71, 0xa3, 0x7e, 0xe2, 0x5e, + 0x8a, 0x91, 0x61, 0x23, 0x5b, 0x26, 0x6d, 0x3b, + 0x44, 0xfc, 0x6b, 0x36, 0x40, 0xb1, 0xdb, 0xe7, + 0xf9, 0xe7, 0x8a, 0x12, 0x7c, 0xba, 0xd8, 0x33, + 0x1b, 0xac, 0x70, 0x59, 0x24, 0x83, 0x3a, 0x8b, + 0x2a, 0x51, 0x1b, 0x97, 0xa3, 0x8e, 0x34, 0xd1, + 0xbb, 0xc1, 0x4e, 0x00, 0xab, 0x21, 0x12, 0x53, + 0xeb, 0xda, 0x36, 0x77, 0x30, 0xea, 0x82, 0x92, + 0xb5, 0xfb, 0x07, 0xb1, 0x34, 0x37, 0x78, 0x69, + } + + ok := rsa.verify_pss( + &pub_key, + .SHA256, + 32, + test_msg, + test_sig, + ) + if !testing.expectf(t, ok, "rsa/pss: verify (pregenerated) failed") { + return + } + + if !crypto.HAS_RAND_BYTES { + log.info("rand_bytes not supported - skipping round trip tests") + return + } + + // Just do a simple round-trip test, as the failure + // cases are covered by wycheproof, and de-randomizing + // PSS is an API nightmare. + + sig: [2048 >> 3]byte + ok = rsa.sign_pss( + &priv_key, + .SHA256, + 32, + TEST_MSG_SHA256, + sig[:], + true, + ) + if !testing.expectf(t, ok, "rsa/pss: signing failed") { + return + } + + ok = rsa.verify_pss( + &pub_key, + .SHA256, + 32, + TEST_MSG_SHA256, + sig[:], + true, + ) + if !testing.expectf(t, ok, "rsa/pss: verify failed") { + return + } +} + +@(test) +test_rsa_enc_dec_oaep :: proc(t: ^testing.T) { + test_msg := transmute([]byte)(TEST_MSG) + + priv_key: rsa.Private_Key + _ = rsa.private_key_set_insecure_test(&priv_key) + + pub_key: rsa.Public_Key + rsa.public_key_set_priv(&pub_key, &priv_key) + + // Generated with Go's crypto code. Ciphertexts are + // NOT deterministic. + test_ciphertext := []byte{ + 0x51, 0x7e, 0x55, 0xe8, 0xf8, 0x69, 0x9e, 0x68, + 0x8e, 0x2f, 0x38, 0xec, 0x11, 0xfb, 0x5f, 0x1e, + 0xf1, 0x9b, 0x2c, 0xea, 0x8a, 0xfb, 0x13, 0x04, + 0x2a, 0xbd, 0x4f, 0x69, 0xca, 0x11, 0x31, 0x1b, + 0xb0, 0x00, 0x18, 0x69, 0x32, 0x88, 0xe3, 0x07, + 0x82, 0xe8, 0x1d, 0x34, 0x09, 0x26, 0xdf, 0x41, + 0x1c, 0xc3, 0xf1, 0x39, 0x31, 0x45, 0xb6, 0x67, + 0xa5, 0x7c, 0xa4, 0xaf, 0x48, 0x5e, 0x96, 0x26, + 0x9b, 0x78, 0x76, 0x4e, 0xd2, 0x6e, 0x53, 0xd0, + 0x51, 0xc9, 0x80, 0x71, 0xea, 0x67, 0x8a, 0x44, + 0x1e, 0xb0, 0x81, 0x2e, 0xce, 0x43, 0x9a, 0xd9, + 0x1c, 0xea, 0x5c, 0x8b, 0x94, 0x3e, 0x1e, 0x5c, + 0xb0, 0x17, 0xb9, 0x50, 0x44, 0x22, 0xc9, 0x17, + 0xd0, 0x73, 0x54, 0x2b, 0x15, 0x68, 0xe2, 0xcf, + 0xbe, 0x0b, 0xef, 0x91, 0x11, 0xfc, 0xa6, 0x78, + 0x14, 0xdd, 0x62, 0xf3, 0xba, 0x8c, 0x8d, 0x4b, + 0x7f, 0x4b, 0xfa, 0x8b, 0x9c, 0x91, 0x08, 0x9f, + 0x39, 0x47, 0x27, 0xba, 0x9a, 0xfd, 0x2a, 0xb8, + 0x1e, 0x70, 0xa3, 0x9c, 0xe1, 0x23, 0x21, 0xc5, + 0xca, 0x00, 0x2a, 0x9b, 0x23, 0x0f, 0x15, 0xe2, + 0x9a, 0x62, 0xc2, 0x20, 0xb6, 0xe8, 0x85, 0x5f, + 0x94, 0xba, 0x72, 0x06, 0x55, 0xcf, 0x5a, 0xd6, + 0xc6, 0xc0, 0x89, 0xff, 0xd3, 0x72, 0xf9, 0x34, + 0x7a, 0x12, 0xfc, 0xe3, 0x74, 0x64, 0x00, 0xfe, + 0xa1, 0x35, 0x78, 0x66, 0x56, 0x1a, 0xde, 0x6a, + 0x83, 0x6b, 0x20, 0x06, 0xe2, 0x51, 0xae, 0xc7, + 0x27, 0x44, 0x5b, 0x21, 0x4f, 0xdf, 0xf6, 0x52, + 0x8e, 0x3a, 0x84, 0x07, 0x26, 0xc8, 0xe3, 0x6a, + 0x18, 0xd4, 0x49, 0x44, 0xd8, 0x24, 0x08, 0x94, + 0xe1, 0x67, 0xde, 0x4a, 0x8e, 0x6a, 0x2a, 0x28, + 0x72, 0x0e, 0x68, 0x9c, 0x7f, 0x55, 0x13, 0x54, + 0x13, 0x32, 0xdb, 0xe7, 0x31, 0x84, 0x90, 0xaf, + } + + buf: [2048 >> 3]byte + + derived_msg, ok := rsa.decrypt_oaep( + &priv_key, + .SHA256, + test_ciphertext, + buf[:], + ) + if !testing.expectf(t, ok, "rsa/oaep: decryption (pregenerated) failed") { + return + } + if !testing.expectf( + t, + bytes.equal(test_msg, derived_msg), + "rsa/oaep: unexpected plaintext: %x", + derived_msg, + ) { + return + } + + if !crypto.HAS_RAND_BYTES { + log.info("rand_bytes not supported - skipping") + return + } + + // Just do a simple round-trip test, as the failure + // cases are covered by wycheproof, and de-randomizing + // OAEP is an API nightmare. + + ok = rsa.encrypt_oaep( + &pub_key, + .SHA512, + test_msg, + buf[:], + ) + if !testing.expectf(t, ok, "rsa/oaep: encryption failed") { + return + } + + derived_msg, ok = rsa.decrypt_oaep( + &priv_key, + .SHA512, + buf[:], + buf[:], + ) + if !testing.expectf(t, ok, "rsa/oaep: decryption failed") { + return + } + if !testing.expectf( + t, + bytes.equal(test_msg, derived_msg), + "rsa/oaep: unexpected plaintext: %x", + derived_msg, + ) { + return + } +} + +@(test) +test_rsa_dec_pms :: proc(t: ^testing.T) { + priv_key: rsa.Private_Key + _ = rsa.private_key_set_insecure_test(&priv_key) + + // Generated with Go's crypto code. Go's PKCS1v15 + // encryption output is NOT deterministic. + test_ciphertext := []byte{ + 0x0c, 0x18, 0x48, 0x23, 0x0b, 0x4a, 0xa9, 0x20, + 0xbb, 0xa6, 0x38, 0xbf, 0x25, 0xda, 0x16, 0xd7, + 0x92, 0x72, 0x3a, 0x81, 0xbc, 0xee, 0x74, 0x6b, + 0xd8, 0x55, 0x26, 0x72, 0x81, 0x70, 0x73, 0x32, + 0xd4, 0x30, 0x56, 0xe2, 0xeb, 0x1b, 0x57, 0xf3, + 0xf5, 0x7d, 0xea, 0x3e, 0x3c, 0x62, 0x35, 0x78, + 0x06, 0x88, 0x27, 0x4d, 0x6e, 0xa0, 0x45, 0x91, + 0x62, 0x8c, 0xe9, 0x93, 0xd0, 0xde, 0x85, 0x71, + 0x01, 0x24, 0x7a, 0x1a, 0x5d, 0x03, 0x49, 0x92, + 0x73, 0x5a, 0x5b, 0x1e, 0x1e, 0x3b, 0xf0, 0xf2, + 0xea, 0x04, 0x9a, 0x87, 0x32, 0x20, 0x52, 0xbd, + 0x72, 0xc8, 0xa8, 0x38, 0xd1, 0x29, 0x97, 0x87, + 0x0b, 0x76, 0xc1, 0x68, 0xe5, 0x05, 0x72, 0xb4, + 0x4d, 0xd1, 0x95, 0xb2, 0xa8, 0x19, 0x3f, 0xc3, + 0x1e, 0xee, 0x34, 0x19, 0x72, 0xac, 0x1e, 0x4b, + 0x01, 0xd7, 0x60, 0xeb, 0x27, 0xf1, 0x12, 0x7c, + 0xbc, 0x07, 0x5d, 0xf6, 0xb9, 0xac, 0xf9, 0xdb, + 0x1a, 0xaf, 0x47, 0x13, 0x22, 0x16, 0xb5, 0x05, + 0x5a, 0x9c, 0x45, 0x69, 0x0a, 0xf1, 0x36, 0x6b, + 0xab, 0x96, 0xd7, 0x7f, 0x66, 0xff, 0x16, 0x3c, + 0x29, 0xc4, 0x10, 0x03, 0xe1, 0x35, 0xcc, 0xae, + 0x71, 0x08, 0x14, 0xff, 0x57, 0x4f, 0x3d, 0x79, + 0x7a, 0xa7, 0x19, 0x2e, 0x23, 0x08, 0xad, 0xb2, + 0xe5, 0xb1, 0xe8, 0x47, 0x6d, 0xe1, 0x24, 0x4a, + 0xd8, 0x1f, 0xe4, 0x52, 0x21, 0x3e, 0xf1, 0xcd, + 0x07, 0x72, 0xa4, 0xb8, 0x06, 0x98, 0x3a, 0x17, + 0xfd, 0xca, 0x74, 0x93, 0xb1, 0x2b, 0xd8, 0x76, + 0x7c, 0x6f, 0x71, 0xfc, 0x16, 0xef, 0x99, 0xa1, + 0xf9, 0x13, 0xeb, 0xfc, 0x34, 0x90, 0xb5, 0x00, + 0xbf, 0xdc, 0x19, 0x99, 0xb4, 0x12, 0x85, 0x25, + 0x3a, 0x49, 0x70, 0x63, 0x2f, 0xfc, 0xbc, 0xca, + 0x38, 0x2f, 0x7a, 0x0e, 0x78, 0x8d, 0x7b, 0x87, + } + + ok := rsa.unsafe_decrypt_tls_pms(&priv_key, test_ciphertext) + if !testing.expectf( + t, + ok == 1, + "rsa/tls_pms: decryption failed: %x", + test_ciphertext[:48], + ) { + return + } + + if !testing.expectf( + t, + bytes.equal(test_ciphertext[:48], TEST_TLS_PMS), + "rsa/tls_pms: unexpected plaintext: %x", + test_ciphertext[:48], + ) { + return + } +} diff --git a/tests/core/crypto/wycheproof/main.odin b/tests/core/crypto/wycheproof/main.odin index bfb4884cd..3c7e765e2 100644 --- a/tests/core/crypto/wycheproof/main.odin +++ b/tests/core/crypto/wycheproof/main.odin @@ -51,6 +51,45 @@ import "core:testing" // - pbkdf2_hmacsha256_test.json // - pbkdf2_hmacsha384_test.json // - pbkdf2_hmacsha512_test.json +// - crypto/rsa +// - rsa_pkcs1_1024_sig_gen_test.json +// - rsa_pkcs1_1536_sig_gen_test.json +// - rsa_pkcs1_2048_sig_gen_test.json +// - rsa_pkcs1_3072_sig_gen_test.json +// - rsa_pkcs1_4096_sig_gen_test.json +// - rsa_pss_2048_sha1_mgf1_20_test.json +// - rsa_pss_2048_sha256_mgf1_0_test.json +// - rsa_pss_2048_sha256_mgf1_32_test.json +// - rsa_pss_2048_sha256_mgf1sha1_20_test.json +// - rsa_pss_2048_sha384_mgf1_48_test.json +// - rsa_pss_2048_sha512_256_mgf1_32_test.json +// - rsa_pss_3072_sha256_mgf1_32_test.json +// - rsa_pss_4096_sha256_mgf1_32_test.json +// - rsa_pss_4096_sha384_mgf1_48_test.json +// - rsa_pss_4096_sha512_mgf1_32_test.json +// - rsa_pss_4096_sha512_mgf1_64_test.json +// - rsa_pss_misc_test.json +// - rsa_oaep_2048_sha1_mgf1sha1_test.json +// - rsa_oaep_2048_sha224_mgf1sha1_test.json +// - rsa_oaep_2048_sha224_mgf1sha224_test.json +// - rsa_oaep_2048_sha256_mgf1sha1_test.json +// - rsa_oaep_2048_sha256_mgf1sha256_test.json +// - rsa_oaep_2048_sha384_mgf1sha1_test.json +// - rsa_oaep_2048_sha384_mgf1sha384_test.json +// - rsa_oaep_2048_sha512_224_mgf1sha1_test.json +// - rsa_oaep_2048_sha512_mgf1sha1_test.json +// - rsa_oaep_2048_sha512_mgf1sha512_test.json +// - rsa_oaep_3072_sha256_mgf1sha1_test.json +// - rsa_oaep_3072_sha256_mgf1sha256_test.json +// - rsa_oaep_3072_sha512_256_mgf1sha1_test.json +// - rsa_oaep_3072_sha512_256_mgf1sha512_256_test.json +// - rsa_oaep_3072_sha512_mgf1sha1_test.json +// - rsa_oaep_3072_sha512_mgf1sha512_test.json +// - rsa_oaep_4096_sha256_mgf1sha1_test.json +// - rsa_oaep_4096_sha256_mgf1sha256_test.json +// - rsa_oaep_4096_sha512_mgf1sha1_test.json +// - rsa_oaep_4096_sha512_mgf1sha512_test.json +// - rsa_oaep_misc_test.json // - crypto/siphash // - siphash_1_3_test.json // - siphash_2_4_test.json diff --git a/tests/core/crypto/wycheproof/rsa.odin b/tests/core/crypto/wycheproof/rsa.odin new file mode 100644 index 000000000..00561c407 --- /dev/null +++ b/tests/core/crypto/wycheproof/rsa.odin @@ -0,0 +1,541 @@ +package test_wycheproof + +import "core:encoding/hex" +import "core:fmt" +import "core:log" +import "core:mem" +import "core:os" +import "core:testing" + +import "core:crypto/rsa" + +import "../common" + +@(test) +test_rsa_pkcs1_signature :: proc(t: ^testing.T) { + arena: mem.Arena + arena_backing := make([]byte, ARENA_SIZE) + defer delete(arena_backing) + mem.arena_init(&arena, arena_backing) + context.allocator = mem.arena_allocator(&arena) + + log.debug("rsa/pkcs1/signatures: starting") + + files := []string { + "rsa_pkcs1_1024_sig_gen_test.json", + "rsa_pkcs1_1536_sig_gen_test.json", + "rsa_pkcs1_2048_sig_gen_test.json", + "rsa_pkcs1_3072_sig_gen_test.json", + "rsa_pkcs1_4096_sig_gen_test.json", + } + for f in files { + mem.free_all() + + fn, _ := os.join_path([]string{BASE_PATH, f}, context.allocator) + + test_vectors: Test_Vectors(Rsa_Pkcs1_Sig_Test_Group) + load_ok := load(&test_vectors, fn) + if !testing.expectf(t, load_ok, "Unable to load {}", f) { + continue + } + + testing.expectf(t, test_rsa_pkcs1_sig(t, &test_vectors), "RSA PKCS1 signature failed") + } +} + +test_rsa_pkcs1_sig :: proc(t: ^testing.T, test_vectors: ^Test_Vectors(Rsa_Pkcs1_Sig_Test_Group)) -> bool { + JWK_KTY :: "RSA" + + params_str := fmt.aprintf("RSA-%d/PKCS1/Signature", test_vectors.test_groups[0].key_size) + log.debugf("%s: starting", params_str) + + num_ran, num_passed, num_failed, num_skipped: int + for &test_group, tg_id in test_vectors.test_groups { + hash_str := test_group.sha + hash_alg, _ := hash_name_to_algorithm(hash_str) + if hash_alg == .Invalid { + log.infof("%s: unsupported hash: %s", params_str, hash_str) + num_ran += len(test_group.tests) + num_skipped += len(test_group.tests) + continue + } + + priv_key: rsa.Private_Key + pub_key := &priv_key._pub_key + + ok, have_priv: bool + if test_group.private_key_jwk.kty == JWK_KTY { + ok = jwk_to_private_key(&test_group.private_key_jwk, &priv_key) + have_priv = true + } else { + ok = rsa.public_key_set_bytes( + pub_key, + common.hexbytes_decode(test_group.private_key.modulus), + common.hexbytes_decode(test_group.private_key.public_exponent), + ) + } + + if !testing.expectf(t, ok, "%s/%d: invalid RSA key: %v", params_str, tg_id, test_group.private_key) { + num_ran += len(test_group.tests) + num_failed += len(test_group.tests) + continue + } + + for &test_vector in test_group.tests { + num_ran += 1 + + if comment := test_vector.comment; comment != "" { + log.debugf( + "%s/%s/%d/%d: %s: %+v", + params_str, + hash_str, + tg_id, + test_vector.tc_id, + comment, + test_vector.flags, + ) + } else { + log.debugf("%s/%s/%d/%d: %+v", params_str, hash_str, tg_id, test_vector.tc_id, test_vector.flags) + } + + msg := common.hexbytes_decode(test_vector.msg) + sig := common.hexbytes_decode(test_vector.sig) + + verify_ok := rsa.verify_pkcs1(pub_key, hash_alg, msg, sig) + if !testing.expectf( + t, + result_check(test_vector.result, verify_ok, false), + "%s/%s/%d/%d: verify failed: expected %s actual %v", + params_str, + hash_str, + tg_id, + test_vector.tc_id, + test_vector.result, + verify_ok, + ) { + num_failed += 1 + continue + } + + if have_priv && verify_ok { + sign_ok := rsa.sign_pkcs1(&priv_key, hash_alg, msg, sig) + if !testing.expectf( + t, + sign_ok, + "%s/%s/%d/%d: sign failed", + params_str, + hash_str, + tg_id, + test_vector.tc_id, + ) { + num_failed += 1 + continue + } + if !testing.expectf( + t, + common.hexbytes_compare(test_vector.sig, sig), + "%s/%s/%d/%d: sign failed: expected %s actual %s", + params_str, + hash_str, + tg_id, + test_vector.tc_id, + test_vector.sig, + hex.encode(sig), + ) { + num_failed += 1 + continue + } + } + + num_passed += 1 + } + } + + assert(num_ran == test_vectors.number_of_tests) + assert(num_passed + num_failed + num_skipped == num_ran) + + log.infof( + "%s: ran %d, passed %d, failed %d, skipped %d", + params_str, + num_ran, + num_passed, + num_failed, + num_skipped, + ) + + return num_failed == 0 +} + +@(test) +test_rsa_pss_signature :: proc(t: ^testing.T) { + arena: mem.Arena + arena_backing := make([]byte, ARENA_SIZE) + defer delete(arena_backing) + mem.arena_init(&arena, arena_backing) + context.allocator = mem.arena_allocator(&arena) + + log.debug("rsa/pss/signatures: starting") + + files := []string { + "rsa_pss_2048_sha1_mgf1_20_test.json", + "rsa_pss_2048_sha256_mgf1_0_test.json", + "rsa_pss_2048_sha256_mgf1_32_test.json", + "rsa_pss_2048_sha256_mgf1sha1_20_test.json", + "rsa_pss_2048_sha384_mgf1_48_test.json", + "rsa_pss_2048_sha512_256_mgf1_32_test.json", + "rsa_pss_3072_sha256_mgf1_32_test.json", + "rsa_pss_4096_sha256_mgf1_32_test.json", + "rsa_pss_4096_sha384_mgf1_48_test.json", + "rsa_pss_4096_sha512_mgf1_32_test.json", + "rsa_pss_4096_sha512_mgf1_64_test.json", + "rsa_pss_misc_test.json", + + // These tests include the MGF1 parameters in the public key, + // which we do not support with our existing API. + // + // "rsa_pss_2048_sha1_mgf1_20_params_test.json", + // "rsa_pss_2048_sha256_mgf1_0_params_test.json", + // "rsa_pss_2048_sha256_mgf1_32_params_test.json", + // "rsa_pss_2048_sha512_mgf1sha256_32_params_test.json", + // "rsa_pss_3072_sha256_mgf1_32_params_test.json", + // "rsa_pss_4096_sha512_mgf1_32_params_test.json", + // "rsa_pss_4096_sha512_mgf1_64_params_test.json", + // "rsa_pss_misc_params_test.json", + + // Unsupported hash algorithm: + // "rsa_pss_2048_sha512_224_mgf1_28_test.json", + } + for f in files { + mem.free_all() + + fn, _ := os.join_path([]string{BASE_PATH, f}, context.allocator) + + test_vectors: Test_Vectors(Rsa_Pss_Sig_Test_Group) + load_ok := load(&test_vectors, fn) + if !testing.expectf(t, load_ok, "Unable to load {}", f) { + continue + } + + testing.expectf(t, test_rsa_pss_sig(t, &test_vectors), "RSA PSS signature failed") + } +} + +test_rsa_pss_sig :: proc(t: ^testing.T, test_vectors: ^Test_Vectors(Rsa_Pss_Sig_Test_Group)) -> bool { + MGF1 :: "MGF1" + + params_str := fmt.aprintf("RSA-%d/PSS/Signature", test_vectors.test_groups[0].key_size) + log.debugf("%s: starting", params_str) + + num_ran, num_passed, num_failed, num_skipped: int + for &test_group, tg_id in test_vectors.test_groups { + if test_group.mgf != MGF1 { + log.infof("%s: unsupported MGF: %s", params_str, test_group.mgf) + num_ran += len(test_group.tests) + num_skipped += len(test_group.tests) + continue + } + + hash_str := test_group.sha + hash_alg, _ := hash_name_to_algorithm(hash_str) + if hash_alg == .Invalid { + log.infof("%s: unsupported hash: %s", params_str, hash_str) + num_ran += len(test_group.tests) + num_skipped += len(test_group.tests) + continue + } + + mgf_hash_str := test_group.mfg_sha + mgf_hash_alg, _ := hash_name_to_algorithm(mgf_hash_str) + if mgf_hash_alg == .Invalid { + log.infof("%s: unsupported MGF hash: %s", params_str, mgf_hash_str) + num_ran += len(test_group.tests) + num_skipped += len(test_group.tests) + continue + } + + hash_params_str := fmt.aprintf("%s/%s(%s)", hash_str, test_group.mgf, mgf_hash_str) + + pub_key: rsa.Public_Key + ok := rsa.public_key_set_bytes( + &pub_key, + common.hexbytes_decode(test_group.public_key.modulus), + common.hexbytes_decode(test_group.public_key.public_exponent), + ) + if !testing.expectf(t, ok, "%s/%d: invalid RSA key: %v", params_str, tg_id, test_group.public_key) { + num_ran += len(test_group.tests) + num_failed += len(test_group.tests) + continue + } + + for &test_vector in test_group.tests { + num_ran += 1 + + if comment := test_vector.comment; comment != "" { + log.debugf( + "%s/%s/%d/%d: %s: %+v", + params_str, + hash_params_str, + tg_id, + test_vector.tc_id, + comment, + test_vector.flags, + ) + } else { + log.debugf("%s/%s/%d/%d: %+v", params_str, hash_str, tg_id, test_vector.tc_id, test_vector.flags) + } + + msg := common.hexbytes_decode(test_vector.msg) + sig := common.hexbytes_decode(test_vector.sig) + + verify_ok := rsa.verify_pss( + &pub_key, + hash_alg, + test_group.s_len, + msg, + sig, + false, + mgf_hash_alg, + ) + if !testing.expectf( + t, + result_check(test_vector.result, verify_ok, false), + "%s/%s/%d/%d: verify failed: expected %s actual %v", + params_str, + hash_params_str, + tg_id, + test_vector.tc_id, + test_vector.result, + verify_ok, + ) { + num_failed += 1 + continue + } + + num_passed += 1 + } + } + + assert(num_ran == test_vectors.number_of_tests) + assert(num_passed + num_failed + num_skipped == num_ran) + + log.infof( + "%s: ran %d, passed %d, failed %d, skipped %d", + params_str, + num_ran, + num_passed, + num_failed, + num_skipped, + ) + + return num_failed == 0 +} + +@(test) +test_rsa_oaep_decryption :: proc(t: ^testing.T) { + arena: mem.Arena + arena_backing := make([]byte, ARENA_SIZE) + defer delete(arena_backing) + mem.arena_init(&arena, arena_backing) + context.allocator = mem.arena_allocator(&arena) + + log.debug("rsa/oaep/decryption: starting") + + files := []string { + "rsa_oaep_2048_sha1_mgf1sha1_test.json", + "rsa_oaep_2048_sha224_mgf1sha1_test.json", + "rsa_oaep_2048_sha224_mgf1sha224_test.json", + "rsa_oaep_2048_sha256_mgf1sha1_test.json", + "rsa_oaep_2048_sha256_mgf1sha256_test.json", + "rsa_oaep_2048_sha384_mgf1sha1_test.json", + "rsa_oaep_2048_sha384_mgf1sha384_test.json", + "rsa_oaep_2048_sha512_224_mgf1sha1_test.json", + "rsa_oaep_2048_sha512_mgf1sha1_test.json", + "rsa_oaep_2048_sha512_mgf1sha512_test.json", + "rsa_oaep_3072_sha256_mgf1sha1_test.json", + "rsa_oaep_3072_sha256_mgf1sha256_test.json", + "rsa_oaep_3072_sha512_256_mgf1sha1_test.json", + "rsa_oaep_3072_sha512_256_mgf1sha512_256_test.json", + "rsa_oaep_3072_sha512_mgf1sha1_test.json", + "rsa_oaep_3072_sha512_mgf1sha512_test.json", + "rsa_oaep_4096_sha256_mgf1sha1_test.json", + "rsa_oaep_4096_sha256_mgf1sha256_test.json", + "rsa_oaep_4096_sha512_mgf1sha1_test.json", + "rsa_oaep_4096_sha512_mgf1sha512_test.json", + "rsa_oaep_misc_test.json", + + // Unsupported hash algorithm: + // "rsa_oaep_2048_sha512_224_mgf1sha512_224_test.json", + } + for f in files { + mem.free_all() + + fn, _ := os.join_path([]string{BASE_PATH, f}, context.allocator) + + test_vectors: Test_Vectors(Rsa_Oaep_Dec_Test_Group) + load_ok := load(&test_vectors, fn) + if !testing.expectf(t, load_ok, "Unable to load {}", f) { + continue + } + + testing.expectf(t, test_rsa_oaep_dec(t, &test_vectors), "RSA OAEP decryption failed") + } +} + +test_rsa_oaep_dec :: proc(t: ^testing.T, test_vectors: ^Test_Vectors(Rsa_Oaep_Dec_Test_Group)) -> bool { + MGF1 :: "MGF1" + + params_str := fmt.aprintf("RSA-%d/OAEP/Decryption", test_vectors.test_groups[0].key_size) + log.debugf("%s: starting", params_str) + + num_ran, num_passed, num_failed, num_skipped: int + for &test_group, tg_id in test_vectors.test_groups { + if test_group.key_size > rsa.MODULUS_MAX_SIZE { + log.infof("%s: unsupported key size: %d", params_str, test_group.key_size) + num_ran += len(test_group.tests) + num_skipped += len(test_group.tests) + continue + } + if test_group.mgf != MGF1 { + log.infof("%s: unsupported MGF: %s", params_str, test_group.mgf) + num_ran += len(test_group.tests) + num_skipped += len(test_group.tests) + continue + } + + hash_str := test_group.sha + hash_alg, _ := hash_name_to_algorithm(hash_str) + if hash_alg == .Invalid { + log.infof("%s: unsupported hash: %s", params_str, hash_str) + num_ran += len(test_group.tests) + num_skipped += len(test_group.tests) + continue + } + + mgf_hash_str := test_group.mfg_sha + mgf_hash_alg, _ := hash_name_to_algorithm(mgf_hash_str) + if mgf_hash_alg == .Invalid { + log.infof("%s: unsupported MGF hash: %s", params_str, mgf_hash_str) + num_ran += len(test_group.tests) + num_skipped += len(test_group.tests) + continue + } + + hash_params_str := fmt.aprintf("%s/%s(%s)", hash_str, test_group.mgf, mgf_hash_str) + + priv_key: rsa.Private_Key + ok := json_to_private_key(&test_group.private_key, &priv_key) + if !testing.expectf(t, ok, "%s/%d: invalid RSA key: %v", params_str, tg_id, test_group.private_key) { + num_ran += len(test_group.tests) + num_failed += len(test_group.tests) + continue + } + + dst: [rsa.MODULUS_MAX_SIZE >> 3]byte = --- + for &test_vector in test_group.tests { + num_ran += 1 + + if comment := test_vector.comment; comment != "" { + log.debugf( + "%s/%s/%d/%d: %s: %+v", + params_str, + hash_params_str, + tg_id, + test_vector.tc_id, + comment, + test_vector.flags, + ) + } else { + log.debugf("%s/%s/%d/%d: %+v", params_str, hash_str, tg_id, test_vector.tc_id, test_vector.flags) + } + + label := common.hexbytes_decode(test_vector.label) + ct := common.hexbytes_decode(test_vector.ct) + + pt, decrypt_ok := rsa.decrypt_oaep( + &priv_key, + hash_alg, + ct, + dst[:], + label, + mgf_hash_alg, + ) + + if !testing.expectf( + t, + result_check(test_vector.result, decrypt_ok, false), + "%s/%s/%d/%d: decrypt failed: expected %s actual %v", + params_str, + hash_params_str, + tg_id, + test_vector.tc_id, + test_vector.result, + decrypt_ok, + ) { + num_failed += 1 + continue + } + if decrypt_ok { + if !testing.expectf( + t, + common.hexbytes_compare(test_vector.msg, pt), + "%s/%s/%d/%d: decrypt failed: expected %s actual %s", + params_str, + hash_str, + tg_id, + test_vector.tc_id, + test_vector.msg, + hex.encode(pt), + ) { + num_failed += 1 + continue + } + } + + num_passed += 1 + } + } + + assert(num_ran == test_vectors.number_of_tests) + assert(num_passed + num_failed + num_skipped == num_ran) + + log.infof( + "%s: ran %d, passed %d, failed %d, skipped %d", + params_str, + num_ran, + num_passed, + num_failed, + num_skipped, + ) + + return num_failed == 0 +} + +@(private="file") +jwk_to_private_key :: proc(jwk: ^Rsa_Jwk_Private_Key, priv_key: ^rsa.Private_Key) -> bool { + return rsa.private_key_set_bytes( + priv_key, + common.jwkbytes_decode(jwk.n), + common.jwkbytes_decode(jwk.e), + common.jwkbytes_decode(jwk.d), + common.jwkbytes_decode(jwk.p), + common.jwkbytes_decode(jwk.q), + common.jwkbytes_decode(jwk.dp), + common.jwkbytes_decode(jwk.dq), + common.jwkbytes_decode(jwk.qi), + ) +} + +@(private="file") +json_to_private_key :: proc(json: ^Rsa_Private_Key, priv_key: ^rsa.Private_Key) -> bool { + return rsa.private_key_set_bytes( + priv_key, + common.hexbytes_decode(json.modulus), + common.hexbytes_decode(json.public_exponent), + common.hexbytes_decode(json.private_exponent), + common.hexbytes_decode(json.prime1), + common.hexbytes_decode(json.prime2), + common.hexbytes_decode(json.exponent1), + common.hexbytes_decode(json.exponent2), + common.hexbytes_decode(json.coefficient), + ) +} \ No newline at end of file diff --git a/tests/core/crypto/wycheproof/schemas.odin b/tests/core/crypto/wycheproof/schemas.odin index 2e72a1098..39270a379 100644 --- a/tests/core/crypto/wycheproof/schemas.odin +++ b/tests/core/crypto/wycheproof/schemas.odin @@ -150,10 +150,10 @@ Eddsa_Key :: struct { } Eddsa_Jwk :: struct { - kid: string `json:"kid"`, - crv: string `json:"crv"`, - kty: string `json:"kty"`, - x: string `json:"x"`, + kid: string `json:"kid"`, + crv: string `json:"crv"`, + kty: string `json:"kty"`, + x: common.Jwk_Bytes `json:"x"`, } Ecdsa_Key :: struct { @@ -241,3 +241,96 @@ Mldsa_Test_Vector :: struct { result: Result `json:"result"`, flags: []string `json:"flags"`, } + +Rsa_Pkcs1_Sig_Test_Group :: struct { + private_key: Rsa_Private_Key `json:"privateKey"`, + key_asn: common.Hex_Bytes `json:"keyAsn"`, + key_der: common.Hex_Bytes `json:"keyDer"`, + key_pem: string `json:"keyPem"`, + key_size: int `json:"keySize"`, + private_key_jwk: Rsa_Jwk_Private_Key `json:"privateKeyJwk"`, + private_key_pem: string `json:"privateKeyPem"`, + private_key_pkcs8: common.Hex_Bytes `json:"privateKeyPkcs8"`, + sha: string `json:"sha"`, + type: string `json:"type"`, + source: Test_Group_Source `json:"source"`, + tests: []Rsa_Sig_Test_Vector `json:"tests"`, +} + +Rsa_Sig_Test_Vector :: struct { + tc_id: int `json:"tcId"`, + comment: string `json:"comment"`, + msg: common.Hex_Bytes `json:"msg"`, + sig: common.Hex_Bytes `json:"sig"`, + result: Result `json:"result"`, + flags: []string `json:"flags"`, +} + +Rsa_Pss_Sig_Test_Group :: struct { + public_key: Rsa_Public_Key `json:"publicKey"`, + public_key_asn: common.Hex_Bytes `json:"publicKeyAsn"`, + public_key_der: common.Hex_Bytes `json:"publicKeyDer"`, + public_key_pem: string `json:"publicKeyPem"`, + key_size: int `json:"keySize"`, + sha: string `json:"sha"`, + mgf: string `json:"mgf"`, + mfg_sha: string `json:"mgfSha"`, + s_len: int `json:"sLen"`, + type: string `json:"type"`, + source: Test_Group_Source `json:"source"`, + tests: []Rsa_Sig_Test_Vector `json:"tests"`, +} + +Rsa_Oaep_Dec_Test_Group :: struct { + key_size: int `json:"keySize"`, + private_key: Rsa_Private_Key `json:"privateKey"`, + private_key_jwk: Rsa_Jwk_Private_Key `json:"privateKeyJwk"`, + private_key_pem: string `json:"privateKeyPem"`, + private_key_pkcs8: common.Hex_Bytes `json:"privateKeyPkcs8"`, + sha: string `json:"sha"`, + mgf: string `json:"mgf"`, + mfg_sha: string `json:"mgfSha"`, + type: string `json:"type"`, + source: Test_Group_Source `json:"source"`, + tests: []Rsa_Oaep_Test_Vector `json:"tests"`, +} + +Rsa_Oaep_Test_Vector :: struct { + tc_id: int `json:"tcId"`, + comment: string `json:"comment"`, + msg: common.Hex_Bytes `json:"msg"`, + ct: common.Hex_Bytes `json:"ct"`, + label: common.Hex_Bytes `json:"label"`, + result: Result `json:"result"`, + flags: []string `json:"flags"`, +} + +Rsa_Public_Key :: struct { + modulus: common.Hex_Bytes `json:"modulus"`, + public_exponent: common.Hex_Bytes `json:"publicExponent"`, +} + +Rsa_Private_Key :: struct { + modulus: common.Hex_Bytes `json:"modulus"`, + private_exponent: common.Hex_Bytes `json:"privateExponent"`, + public_exponent: common.Hex_Bytes `json:"publicExponent"`, + prime1: common.Hex_Bytes `json:"prime1"`, + prime2: common.Hex_Bytes `json:"prime2"`, + exponent1: common.Hex_Bytes `json:"exponent1"`, + exponent2: common.Hex_Bytes `json:"exponent2"`, + coefficient: common.Hex_Bytes `json:"coefficient"`, +} + +Rsa_Jwk_Private_Key :: struct { + akg: string `json:"alg"`, + kid: string `json:"kid"`, + kty: string `json:"kty"`, + d: common.Jwk_Bytes `json:"d"`, + dp: common.Jwk_Bytes `json:"dp"`, + dq: common.Jwk_Bytes `json:"dq"`, + e: common.Jwk_Bytes `json:"e"`, + n: common.Jwk_Bytes `json:"n"`, + p: common.Jwk_Bytes `json:"p"`, + q: common.Jwk_Bytes `json:"q"`, + qi: common.Jwk_Bytes `json:"qi"`, +} diff --git a/tests/core/encoding/xml/test_core_xml.odin b/tests/core/encoding/xml/test_core_xml.odin index c03dfe29d..564ee05d6 100644 --- a/tests/core/encoding/xml/test_core_xml.odin +++ b/tests/core/encoding/xml/test_core_xml.odin @@ -1,5 +1,6 @@ package test_core_xml +import "core:encoding/entity" import "core:encoding/xml" import "core:testing" import "core:strings" @@ -217,6 +218,29 @@ run_test :: proc(t: ^testing.T, test: TEST, loc := #caller_location) { } } +@(test) +test_normalize_whitespace :: proc(t: ^testing.T) { + s := "A & B" + normalized_entity_decode, _ := entity.decode_xml(s, {.Normalize_Whitespace}) + defer delete(normalized_entity_decode) + + testing.expect_value(t, normalized_entity_decode, "A & B") + + s = `A & B` + + opts := xml.Options{ + flags = {.Ignore_Unsupported, .Decode_SGML_Entities}, + } + + doc, err := xml.parse_bytes(transmute([]byte)s, opts) + defer xml.destroy(doc) + assert(err == .None) + + testing.expect_value(t, doc.elements[0].value[0], "A & B") + attr := doc.elements[0].attribs + testing.expect_value(t, attr[0].val, "A & B") +} + @(private) doc_to_string :: proc(doc: ^xml.Document) -> (result: string) { /* @@ -298,4 +322,4 @@ doc_to_string :: proc(doc: ^xml.Document) -> (result: string) { print(strings.to_writer(&buf), doc) return strings.clone(strings.to_string(buf)) -} +} \ No newline at end of file diff --git a/tests/core/odin/test_parser.odin b/tests/core/odin/test_parser.odin index 3fb15d893..e60a9f8f0 100644 --- a/tests/core/odin/test_parser.odin +++ b/tests/core/odin/test_parser.odin @@ -131,3 +131,78 @@ my_func :: proc (cond: bool, a: string, b: string) -> string { testing.expect(t, ok, "bad parse") testing.expect(t, file.syntax_error_count == 0, "should contain zero errors") } + + +@test +test_parse_multiline_ternary_infix_with_comment :: proc(t: ^testing.T) { + context.allocator = context.temp_allocator + runtime.DEFAULT_TEMP_ALLOCATOR_TEMP_GUARD() + file := ast.File{ + fullpath = "test.odin", + src = ` + package main + + my_func :: proc (cond: bool, a: string, b: string) -> string { + out := ( + cond + ? a // This is a comment! + : b + ) + return out + } + `, + } + + p := parser.default_parser() + + p.err = proc(pos: tokenizer.Pos, format: string, args: ..any) { + message := fmt.tprintf(format, ..args) + log.errorf("%s(%d:%d): %s", pos.file, pos.line, pos.column, message) + } + + p.warn = proc(pos: tokenizer.Pos, format: string, args: ..any) { + message := fmt.tprintf(format, ..args) + log.warnf("%s(%d:%d): %s", pos.file, pos.line, pos.column, message) + } + + ok := parser.parse_file(&p, &file) + testing.expect(t, ok, "bad parse") + testing.expect(t, file.syntax_error_count == 0, "should contain zero errors") +} + +@test +test_parse_ternary_if_statements_with_comment :: proc(t: ^testing.T) { + context.allocator = context.temp_allocator + runtime.DEFAULT_TEMP_ALLOCATOR_TEMP_GUARD() + file := ast.File{ + fullpath = "test.odin", + src = ` + package main + + my_func :: proc (cond: bool, a: string, b: string) -> string { + out := ( + cond + if a // This is a comment! + else b + ) + return out + } + `, + } + + p := parser.default_parser() + + p.err = proc(pos: tokenizer.Pos, format: string, args: ..any) { + message := fmt.tprintf(format, ..args) + log.errorf("%s(%d:%d): %s", pos.file, pos.line, pos.column, message) + } + + p.warn = proc(pos: tokenizer.Pos, format: string, args: ..any) { + message := fmt.tprintf(format, ..args) + log.warnf("%s(%d:%d): %s", pos.file, pos.line, pos.column, message) + } + + ok := parser.parse_file(&p, &file) + testing.expect(t, ok, "bad parse") + testing.expect(t, file.syntax_error_count == 0, "should contain zero errors") +} diff --git a/tests/core/speed.odin b/tests/core/speed.odin index 5ea9bbc21..0da7c1b82 100644 --- a/tests/core/speed.odin +++ b/tests/core/speed.odin @@ -2,6 +2,7 @@ package tests_core @(require) import "crypto" +@(require) import "crypto/bigint" @(require) import "hash" @(require) import "image" @(require) import "math/big" \ No newline at end of file diff --git a/tests/core/sys/windows/util.odin b/tests/core/sys/windows/util.odin index 777f85776..96f52e8a9 100644 --- a/tests/core/sys/windows/util.odin +++ b/tests/core/sys/windows/util.odin @@ -222,7 +222,7 @@ utf8_to_wstring_alloc_test :: proc(t : ^testing.T) { buf : [^]u16 result = win32.utf8_to_wstring_alloc("Hello\x00, World!", allocator) - buf = transmute([^]u16)result + buf = cast([^]u16)result testing.expect(t, result != nil) testing.expect_value(t, buf[4], 'o') testing.expect_value(t, buf[5], 0) @@ -231,12 +231,12 @@ utf8_to_wstring_alloc_test :: proc(t : ^testing.T) { testing.expect_value(t, buf[14], 0) result = win32.utf8_to_wstring_alloc("H\x00\x00", allocator) - buf = transmute([^]u16)result + buf = cast([^]u16)result testing.expect(t, result != nil) testing.expect_value(t, buf[1], 0) result = win32.utf8_to_wstring_alloc("你好，世界！", allocator) - buf = transmute([^]u16)result + buf = cast([^]u16)result testing.expect(t, result != nil) testing.expect_value(t, buf[0], 0x4F60) testing.expect_value(t, buf[1], 0x597D) @@ -247,7 +247,7 @@ utf8_to_wstring_alloc_test :: proc(t : ^testing.T) { testing.expect_value(t, buf[6], 0) result = win32.utf8_to_wstring_alloc("", allocator) - buf = transmute([^]u16)result + buf = cast([^]u16)result // Valid, and distinguishable from an error. testing.expect(t, result != nil) testing.expect_value(t, buf[0], 0) diff --git a/tests/issues/run.bat b/tests/issues/run.bat index 43a7136b7..11ade8657 100644 --- a/tests/issues/run.bat +++ b/tests/issues/run.bat @@ -37,6 +37,7 @@ set COMMON=-define:ODIN_TEST_FANCY=false -file -vet -strict-style -ignore-unused ..\..\..\odin test ..\test_pr_6476.odin %COMMON% || exit /b ..\..\..\odin check ..\test_issue_6484.odin -no-entry-point %COMMON% || exit /b ..\..\..\odin check ..\test_issue_6874.odin %COMMON% 2>&1 | find /c "Error:" | findstr /x "1" || exit /b +..\..\..\odin check ..\test_issue_6979.odin -no-entry-point %COMMON% || exit /b @echo off diff --git a/tests/issues/run.sh b/tests/issues/run.sh index 3633a7606..4ab2fbd69 100755 --- a/tests/issues/run.sh +++ b/tests/issues/run.sh @@ -79,6 +79,7 @@ else echo "SUCCESSFUL 0/1" exit 1 fi +$ODIN check ../test_issue_6979.odin -no-entry-point $COMMON set +x diff --git a/tests/issues/test_issue_6979.odin b/tests/issues/test_issue_6979.odin new file mode 100644 index 000000000..f13c1f39d --- /dev/null +++ b/tests/issues/test_issue_6979.odin @@ -0,0 +1,7 @@ +// Tests issue https://github.com/odin-lang/Odin/issues/6979 +package test_issues + +error :: proc() -> typeid { + data :: struct{type: typeid}{int} + return data.type +} diff --git a/vendor/README.md b/vendor/README.md index 53ff46179..08598d73d 100644 --- a/vendor/README.md +++ b/vendor/README.md @@ -4,6 +4,8 @@ The `vendor:` prefix for Odin imports is a package collection that comes with th Its use is similar to that of `core:` packages, which would be available in any Odin implementation. +These packages and their associated binary files are curated and maintained by the Odin team. Please don't open pull requests for `vendor:` without first consulting them, whether it be to propose a new package or update bindings to a newer version. In the best of cases this often results in a duplication of effort. Localized fixes where type or procedure definitions diverge between the Odin binding and upstream are welcome. + Presently, the `vendor:` collection comprises the following packages: ## cgltf diff --git a/vendor/box2d/box2d.odin b/vendor/box2d/box2d.odin index 2cdfea191..24bcff5c4 100644 --- a/vendor/box2d/box2d.odin +++ b/vendor/box2d/box2d.odin @@ -988,22 +988,22 @@ foreign lib { // Create a circle shape and attach it to a body. The shape definition and geometry are fully cloned. // Contacts are not created until the next time step. // @return the shape id for accessing the shape - CreateCircleShape :: proc(bodyId: BodyId, #by_ptr def: ShapeDef, #by_ptr circle: Circle) -> ShapeId --- + CreateCircleShape :: proc(bodyId: BodyId, #by_ptr def: ShapeDef, circle: ^Circle) -> ShapeId --- // Create a line segment shape and attach it to a body. The shape definition and geometry are fully cloned. // Contacts are not created until the next time step. // @return the shape id for accessing the shape - CreateSegmentShape :: proc(bodyId: BodyId, #by_ptr def: ShapeDef, #by_ptr segment: Segment) -> ShapeId --- + CreateSegmentShape :: proc(bodyId: BodyId, #by_ptr def: ShapeDef, segment: ^Segment) -> ShapeId --- // Create a capsule shape and attach it to a body. The shape definition and geometry are fully cloned. // Contacts are not created until the next time step. // @return the shape id for accessing the shape - CreateCapsuleShape :: proc(bodyId: BodyId, #by_ptr def: ShapeDef, #by_ptr capsule: Capsule) -> ShapeId --- + CreateCapsuleShape :: proc(bodyId: BodyId, #by_ptr def: ShapeDef, capsule: ^Capsule) -> ShapeId --- // Create a polygon shape and attach it to a body. The shape definition and geometry are fully cloned. // Contacts are not created until the next time step. // @return the shape id for accessing the shape - CreatePolygonShape :: proc(bodyId: BodyId, #by_ptr def: ShapeDef, #by_ptr polygon: Polygon) -> ShapeId --- + CreatePolygonShape :: proc(bodyId: BodyId, #by_ptr def: ShapeDef, polygon: ^Polygon) -> ShapeId --- // Destroy a shape. You may defer the body mass update which can improve performance if several shapes on a // body are destroyed at once. diff --git a/vendor/box3d/box3d.odin b/vendor/box3d/box3d.odin new file mode 100644 index 000000000..e42a7b878 --- /dev/null +++ b/vendor/box3d/box3d.odin @@ -0,0 +1,1859 @@ +// Bindings for Box3D +package vendor_box3d + +import "base:intrinsics" +import "base:runtime" +import "core:c" + +ENABLE_VALIDATION :: false + +BOX3D_SHARED :: #config(BOX3D_SHARED, false) + +when ODIN_OS == .Windows { + @(export) + foreign import lib { + "lib/box3d.lib", + } +} else when ODIN_OS == .Linux && ODIN_ARCH == .amd64 && !BOX3D_SHARED { + @(export) + foreign import lib { + "lib/linux-amd64/libbox3d.a", + } +} else when ODIN_OS == .Linux && ODIN_ARCH == .arm64 && !BOX3D_SHARED { + @(export) + foreign import lib { + "lib/linux-arm64/libbox3d.a", + } +} else when ODIN_OS == .Darwin && (ODIN_ARCH == .amd64 || ODIN_ARCH == .arm64) && !BOX3D_SHARED { + @(export) + foreign import lib { + "lib/darwin/libbox3d.a", + } +} else { + @(export) + foreign import lib { + "system:box3d", + } +} + +// This is used to indicate null for interfaces that work with indices instead of pointers +NULL_INDEX :: -1 + +// Prototype for user allocation function. +// @param size the allocation size in bytes +// @param alignment the required alignment, guaranteed to be a power of 2 +AllocFcn :: proc "c" (size, alignment: i32) -> rawptr + +// Prototype for user free function. +// @param mem the memory previously allocated through `AllocFcn` +FreeFcn :: proc "c" (mem: rawptr) + +// Prototype for the user assert callback. Return 0 to skip the debugger break. +AssertFcn :: proc "c" (condition: cstring, fileName: cstring, lineNumber: c.int) -> c.int + +// Prototype for user log callback. Used to log warnings. +LogFcn :: proc "c" (message: rawptr) + + +BREAKPOINT :: intrinsics.debug_trap + +@(link_prefix="b3", default_calling_convention="c", require_results) +foreign lib { + // This allows the user to override the allocation functions. These should be + // set during application startup. + SetAllocator :: proc(allocFcn: AllocFcn, freeFcn: FreeFcn) --- + + // Total bytes allocated by Box3D + GetByteCount :: proc() -> c.int --- + + // Override the default assert callback. + // @param assertFcn a non-null assert callback + SetAssertFcn :: proc(assertFcn: AssertFcn) --- + + // Internal assertion handler. Allows for host intervention. + InternalAssert :: proc(condition: cstring, fileName: cstring, lineNumber: c.int) -> c.int --- + + // Override the default logging callback. + SetLogFcn :: proc(logFcn: LogFcn) --- +} + +// Version numbering scheme. +// See https://semver.org/ +Version :: struct { + // Significant changes + major: c.int, + + // Incremental changes + minor: c.int, + + // Bug fixes + revision: c.int, +} + +HASH_INIT :: 5381 + +@(link_prefix="b3", default_calling_convention="c", require_results) +foreign lib { + // Get the current version of Box3D + GetVersion :: proc() -> Version --- + + // @return true if the library was built with BOX3D_DOUBLE_PRECISION (large world mode) + IsDoublePrecision :: proc() -> bool --- + + // Get the absolute number of system ticks. The value is platform specific. + GetTicks :: proc() -> u64 --- + + // Get the milliseconds passed from an initial tick value. + GetMilliseconds :: proc(ticks: u64) -> f32 --- + + // Get the milliseconds passed from an initial tick value. + GetMillisecondsAndReset :: proc(ticks: ^u64) -> f32 --- + + // Yield to be used in a busy loop. + Yield :: proc() --- + + // Sleep the current thread for a number of milliseconds. + Sleep :: proc(milliseconds: c.int) --- + + // Simple djb2 hash function for determinism testing + Hash :: proc(hash: u32, data: [^]u8, count: c.int) -> u32 --- + + // // Dump file support functions + // WriteBinaryFile :: proc(data: rawptr, size: c.int, fileName: cstring) --- + // ReadBinaryFile :: proc(prefix: cstring, fileName: cstring, memSize: ^c.int) -> rawptr --- +} + + +@(disabled=ODIN_DISABLE_ASSERT) +ASSERT :: proc "c" (condition: bool, message := #caller_expression(condition), loc := #caller_location) { + if !condition { + @(cold) + internal :: proc "c" (message: string, loc: runtime.Source_Code_Location) { + _ = InternalAssert(cstring(raw_data(message)), cstring(raw_data(loc.file_path)), loc.line) + } + internal(message, loc) + } +} + +@(disabled=!ENABLE_VALIDATION) +VALIDATE :: proc "c" (condition: bool, message := #caller_expression(condition), loc := #caller_location) { + ASSERT(condition, message, loc) +} + + + +/** + * @defgroup world World + * These functions allow you to create a simulation world. + * + * You can add rigid bodies and joint constraints to the world and run the simulation. You can get contact + * information to get contact points and normals as well as events. You can query the world, checking for overlaps and casting + * rays or shapes. There is also debugging information such as debug draw, timing information, and counters. You can find + * documentation here: https://box2d.org/ + * @{ + */ + + // Opaque recording handle. Create with CreateRecording, destroy with DestroyRecording. +Recording :: struct{} + + +// Opaque incremental replay player with a keyframe ring for O(interval) backward seek. +RecPlayer :: struct{} + +// Summary of a recording, read once at open so a viewer can frame and label it. +RecPlayerInfo :: struct { + frameCount: c.int, // total recorded steps + workerCount: c.int, // worker count requested for the replay world + timeStep: f32, // dt of the recorded steps + subStepCount: c.int, // recorded sub-steps + lengthScale: f32, // length units per meter in effect when recorded + bounds: AABB, // accumulated world bounds over the recording, zero-extent if unavailable +} + +// The kind of a recorded spatial query, matching the public query and cast functions. +RecQueryType :: enum c.int { + OverlapAABB, + OverlapShape, + CastRay, + CastShape, + CastRayClosest, + CastMover, + CollideMover, +} + +// A spatial query recorded during a replayed frame, exposed for inspection. +RecQueryInfo :: struct { + type: RecQueryType, + filter: QueryFilter, + aabb: AABB, // world-space bounds of the query, swept for casts + origin: Pos, // query origin (zero for overlap AABB) + translation: Vec3, // ray and cast translation + hitCount: c.int, // number of recorded results + key: u64, // identity key, the hash of (id, name), 0 if untagged + id: u64, // query id, 0 if none + name: cstring, // query label, NULL if none +} + +// One result of a recorded spatial query. +RecQueryHit :: struct { + shape: ShapeId, + point: Pos, + normal: Vec3, + fraction: f32, +} + + +@(link_prefix="b3", default_calling_convention="c", require_results) +foreign lib { + // Create a world for rigid body simulation. A world contains bodies, shapes, and constraints. You may create + // up to 128 worlds. Each world is completely independent and may be simulated in parallel. + // @return the world id. + CreateWorld :: proc(#by_ptr def: WorldDef) -> WorldId --- + + // Destroy a world + DestroyWorld :: proc(worldId: WorldId) --- + + // Get the current number of worlds + GetWorldCount :: proc() -> c.int --- + + // Get the maximum number of simultaneous worlds that have been created + GetMaxWorldCount :: proc() -> c.int --- + + // World id validation. Provides validation for up to 64K allocations. + World_IsValid :: proc(id: WorldId) -> bool --- + + // Simulate a world for one time step. This performs collision detection, integration, and constraint solution. + // @param worldId The world to simulate + // @param timeStep The amount of time to simulate, this should be a fixed number. Usually 1/60. + // @param subStepCount The number of sub-steps, increasing the sub-step count can increase accuracy. Usually 4. + World_Step :: proc(worldId: WorldId, timeStep: f32, subStepCount: c.int) --- + + // Call this to draw shapes and other debug draw data + World_Draw :: proc(worldId: WorldId, draw: ^DebugDraw, maskBits: u64) --- + + // Get the world's bounds. This is the bounding box that covers the current simulation. May have a small + // amount of padding. + World_GetBounds :: proc(worldId: WorldId) -> AABB --- + + // Get the body events for the current time step. The event data is transient. Do not store a reference to this data. + World_GetBodyEvents :: proc(worldId: WorldId) -> BodyEvents --- + + // Get sensor events for the current time step. The event data is transient. Do not store a reference to this data. + World_GetSensorEvents :: proc(worldId: WorldId) -> SensorEvents --- + + // Get contact events for this current time step. The event data is transient. Do not store a reference to this data. + World_GetContactEvents :: proc(worldId: WorldId) -> ContactEvents --- + + // Get the joint events for the current time step. The event data is transient. Do not store a reference to this data. + World_GetJointEvents :: proc(worldId: WorldId) -> JointEvents --- + + // Overlap test for all shapes that *potentially* overlap the provided AABB + World_OverlapAABB :: proc(worldId: WorldId, aabb: AABB, filter: QueryFilter, fcn: OverlapResultFcn, ctx: rawptr) -> TreeStats --- + + // Overlap test for all shapes that overlap the provided shape proxy. The proxy points are relative + // to the world origin, which lets the query stay precise far from the world origin. + World_OverlapShape :: proc(worldId: WorldId, origin: Pos, #by_ptr proxy: ShapeProxy, filter: QueryFilter, fcn: OverlapResultFcn, ctx: rawptr) -> TreeStats --- + + // Cast a ray into the world to collect shapes in the path of the ray. + // Your callback function controls whether you get the closest point, any point, or n-points. + // @note The callback function may receive shapes in any order + // @param worldId The world to cast the ray against + // @param origin The start point of the ray + // @param translation The translation of the ray from the start point to the end point + // @param filter Contains bit flags to filter unwanted shapes from the results + // @param fcn A user implemented callback function + // @param context A user context that is passed along to the callback function + // @return traversal performance counters + World_CastRay :: proc(worldId: WorldId, origin: Pos, translation: Vec3, filter: QueryFilter, fcn: CastResultFcn, ctx: rawptr) -> TreeStats --- + + // Cast a ray into the world to collect the closest hit. This is a convenience function. Ignores initial overlap. + // This is less general than World_CastRay() and does not allow for custom filtering. + World_CastRayClosest :: proc(worldId: WorldId, origin: Pos, translation: Vec3, filter: QueryFilter) -> RayResult --- + + // Cast a shape through the world. Similar to a cast ray except that a shape is cast instead of a point. + // The proxy points are relative to the origin and the hit points come back as world positions, so the + // cast stays precise far from the world origin. + // @see World_CastRay + World_CastShape :: proc(worldId: WorldId, origin: Pos, #by_ptr proxy: ShapeProxy, translation: Vec3, filter: QueryFilter, fcn: CastResultFcn, ctx: rawptr) -> TreeStats --- + + // Cast a capsule mover through the world. This is a special shape cast that handles sliding along other shapes while reducing + // clipping. This is not a good source of information about what the mover is touching. Instead use the planes returned by + // World_CollideMover. + // @param worldId World to cast the mover against + // @param origin World position the mover capsule is relative to + // @param mover Capsule mover, relative to the origin + // @param translation Desired mover translation + // @param filter Contains bit flags to filter unwanted shapes from the results + // @param fcn Optional callback for custom shape filtering + // @param context A user context that is passed along to the callback function + // @return the translation fraction + World_CastMover :: proc(worldId: WorldId, origin: Pos, #by_ptr mover: Capsule, translation: Vec3, filter: QueryFilter, fcn: MoverFilterFcn, ctx: rawptr) -> f32 --- + + // Collide a capsule mover with the world, gathering collision planes that can be fed to SolvePlanes. Useful for + // kinematic character movement. The mover and the returned planes are relative to the origin. + World_CollideMover :: proc(worldId: WorldId, origin: Pos, #by_ptr mover: Capsule, filter: QueryFilter, fcn: PlaneResultFcn, ctx: rawptr) --- + + // Enable/disable sleep. If your application does not need sleeping, you can gain some performance + // by disabling sleep completely at the world level. + // @see WorldDef + World_EnableSleeping :: proc(worldId: WorldId, flag: bool) --- + + // Is body sleeping enabled? + World_IsSleepingEnabled :: proc(worldId: WorldId) -> bool --- + + // Enable/disable continuous collision between dynamic and static bodies. Generally you should keep continuous + // collision enabled to prevent fast moving objects from going through static objects. The performance gain from + // disabling continuous collision is minor. + // @see WorldDef + World_EnableContinuous :: proc(worldId: WorldId, flag: bool) --- + + // Is continuous collision enabled? + World_IsContinuousEnabled :: proc(worldId: WorldId) -> bool --- + + // Adjust the restitution threshold. It is recommended not to make this value very small + // because it will prevent bodies from sleeping. Usually in meters per second. + // @see WorldDef + World_SetRestitutionThreshold :: proc(worldId: WorldId, value: f32) --- + + // Get the restitution speed threshold. Usually in meters per second. + World_GetRestitutionThreshold :: proc(worldId: WorldId) -> f32 --- + + // Adjust the hit event threshold. This controls the collision speed needed to generate a ContactHitEvent. + // Usually in meters per second. + // @see WorldDef::hitEventThreshold + World_SetHitEventThreshold :: proc(worldId: WorldId, value: f32) --- + + // Get the hit event speed threshold. Usually in meters per second. + World_GetHitEventThreshold :: proc(worldId: WorldId) -> f32 --- + + // Register the custom filter callback. This is optional. + World_SetCustomFilterCallback :: proc(worldId: WorldId, fcn: CustomFilterFcn, ctx: rawptr) --- + + // Register the pre-solve callback. This is optional. + World_SetPreSolveCallback :: proc(worldId: WorldId, fcn: PreSolveFcn, ctx: rawptr) --- + + // Set the gravity vector for the entire world. Box3D has no concept of an up direction and this + // is left as a decision for the application. Usually in m/s^2. + // @see WorldDef + World_SetGravity :: proc(worldId: WorldId, gravity: Vec3) --- + + // Get the gravity vector + World_GetGravity :: proc(worldId: WorldId) -> Vec3 --- + + // Apply a radial explosion + // @param worldId The world id + // @param explosionDef The explosion definition + World_Explode :: proc(worldId: WorldId, #by_ptr explosionDef: ExplosionDef) --- + + // Adjust contact tuning parameters + // @param worldId The world id + // @param hertz The contact stiffness (cycles per second) + // @param dampingRatio The contact bounciness with 1 being critical damping (non-dimensional) + // @param contactSpeed The maximum contact constraint push out speed (meters per second) + // @note Advanced feature + World_SetContactTuning :: proc(worldId: WorldId, hertz: f32, dampingRatio: f32, contactSpeed: f32) --- + + // Set the contact point recycling distance. Setting this to zero disables contact point recycling. + // Usually in meters. + World_SetContactRecycleDistance :: proc(worldId: WorldId, recycleDistance: f32) --- + + // Get the contact point recycling distance. Usually in meters. + World_GetContactRecycleDistance :: proc(worldId: WorldId) -> f32 --- + + // Set the maximum linear speed. Usually in m/s. + World_SetMaximumLinearSpeed :: proc(worldId: WorldId, maximumLinearSpeed: f32) --- + + // Get the maximum linear speed. Usually in m/s. + World_GetMaximumLinearSpeed :: proc(worldId: WorldId) -> f32 --- + + // Enable/disable constraint warm starting. Advanced feature for testing. Disabling + // warm starting greatly reduces stability and provides no performance gain. + World_EnableWarmStarting :: proc(worldId: WorldId, flag: bool) --- + + // Is constraint warm starting enabled? + World_IsWarmStartingEnabled :: proc(worldId: WorldId) -> bool --- + + // Get the number of awake bodies + World_GetAwakeBodyCount :: proc(worldId: WorldId) -> c.int --- + + // Get the current world performance profile + World_GetProfile :: proc(worldId: WorldId) -> Profile --- + + // Get world counters and sizes + World_GetCounters :: proc(worldId: WorldId) -> Counters --- + + // Get max capacity. This can be used with WorldDef to avoid run-time allocations and copies + World_GetMaxCapacity :: proc(worldId: WorldId) -> Capacity --- + + // Set the user data pointer. + World_SetUserData :: proc(worldId: WorldId, userData: rawptr) --- + + // Get the user data pointer. + World_GetUserData :: proc(worldId: WorldId) -> rawptr --- + + // Set the friction callback. Passing NULL resets to default. + World_SetFrictionCallback :: proc(worldId: WorldId, callback: FrictionCallback) --- + + // Set the restitution callback. Passing NULL resets to default. + World_SetRestitutionCallback :: proc(worldId: WorldId, callback: RestitutionCallback) --- + + // Set the worker count. Must be in the range [1, B3_MAX_WORKERS] + World_SetWorkerCount :: proc(worldId: WorldId, count: c.int) --- + + // Get the worker count. + World_GetWorkerCount :: proc(worldId: WorldId) -> c.int --- + + // Dump memory stats to log. + World_DumpMemoryStats :: proc(worldId: WorldId) --- + + // Dump shape bounds to box3d_bounds.txt + World_DumpShapeBounds :: proc(worldId: WorldId, type: BodyType) --- + + // This is for internal testing + World_RebuildStaticTree :: proc(worldId: WorldId) --- + + // This is for internal testing + World_EnableSpeculative :: proc(worldId: WorldId, flag: bool) --- + + // Dump world to a text file. Saves only awake bodies and associated static bodies. + // Meshes are saved to binary m files. + World_DumpAwake :: proc(worldId: WorldId) --- + + // Dump world to a text file. Meshes are saved to binary m files. + World_Dump :: proc(worldId: WorldId) --- + + /** + * @defgroup recording Recording + * @brief Record and replay world state for debugging. + * @{ + */ + + // Create a recording buffer with an optional initial byte capacity. + // Pass 0 to use the default (64 KiB). The buffer grows on demand. + // @return a new recording, owned by the caller + CreateRecording :: proc(byteCapacity: c.int) -> Recording --- + + // Destroy a recording and free its buffer. + // @param recording may be NULL + DestroyRecording :: proc(recording: ^Recording) --- + + // Get a pointer to the raw recording bytes. + // Valid until the recording buffer is modified or destroyed. + // @param recording the recording handle + // @return pointer to the byte buffer, or NULL if no bytes have been written + Recording_GetData :: proc(#by_ptr recording: Recording) -> [^]u8 --- + + // Get the number of bytes currently in the recording buffer. + // @param recording the recording handle + Recording_GetSize :: proc(#by_ptr recording: Recording) -> c.int --- + + // Begin recording world mutations into the provided buffer. + // The buffer is reset on each call so a single Recording can be reused for multiple sessions. + // @param worldId the world to record + // @param recording the recording handle to write into + World_StartRecording :: proc(worldId: WorldId, recording: ^Recording) --- + + // End the current recording session. Writes the trailing geometry registry and + // backpatches the header. The buffer remains valid until the recording is destroyed. + // @param worldId the world currently being recorded + World_StopRecording :: proc(worldId: WorldId) --- + + // Save the recording buffer to a file. Returns true on success. + // @param recording the recording to save + // @param path file path to write + SaveRecordingToFile :: proc(#by_ptr recording: Recording, path: cstring) -> bool --- + + // Load a recording from a file. Returns NULL on failure (file not found, wrong magic). + // The caller owns the returned recording and must destroy it with DestroyRecording. + // @param path file path to read + LoadRecordingFromFile :: proc(path: cstring) -> Recording --- + + // Replay a recording from memory and verify it reproduces the same world-state hashes. + // Stands up a fresh world, restores the seed snapshot, replays every op, and checks each embedded + // StateHash record. Returns true if replay completed without id mismatches or hash divergences. + // @param data pointer to recording bytes + // @param size byte count of the recording + // @param workerCount reserved for future multithreaded replay; pass 1 for now + ValidateReplay :: proc(data: rawptr, size: c.int, workerCount: c.int) -> bool --- + + + // Create a player over a recording. Owns a private copy of the bytes. + // @param data pointer to recording bytes + // @param size byte count of the recording + // @param workerCount worker count for the replay world; pass 1 to match a serial recording. + // Replaying at a different count re-partitions the constraint graph, so the StateHash check + // becomes a cross-thread determinism test. Adjustable later with RecPlayer_SetWorkerCount. + // @return a new player, or NULL on bad header or deserialization failure + RecPlayer_Create :: proc(data: rawptr, size: c.int, workerCount: c.int) -> RecPlayer --- + + // Destroy the player and free all memory. Restores the previous global length scale. + RecPlayer_Destroy :: proc(player: ^RecPlayer) --- + + // Advance one frame: dispatch ops until the next Step completes. + // @return true when a frame was stepped, false at end-of-recording + RecPlayer_StepFrame :: proc(player: ^RecPlayer) -> bool --- + + // Sub-step one frame. This will sub-step and return immediately after body creation. + // The next call will execute the time step. This allows bodies to be rendered + // at the creation pose. + RecPlayer_SubStepFrame :: proc(player: ^RecPlayer) --- + + + // Rewind to frame 0 (in-place restore so the world id stays stable). + RecPlayer_Restart :: proc(player: ^RecPlayer) --- + + // Seek to a specific frame. Forward seek steps op-by-op; backward seek restores + // the nearest keyframe then re-steps the remaining gap. + RecPlayer_SeekFrame :: proc(player: ^RecPlayer, targetFrame: c.int) --- + + // @return the world currently driven by this player + RecPlayer_GetWorldId :: proc(#by_ptr player: RecPlayer) -> WorldId --- + + // @return the last fully-stepped frame index (0 before any step) + RecPlayer_GetFrame :: proc(#by_ptr player: RecPlayer) -> c.int --- + + // @return total number of recorded frames + RecPlayer_GetFrameCount :: proc(#by_ptr player: RecPlayer) -> c.int --- + + // @return true when the op stream is exhausted + RecPlayer_IsAtEnd :: proc(#by_ptr player: RecPlayer) -> bool --- + + // @return true when the op stream is paused between body creation and world step. + RecPlayer_IsAtPreStep :: proc(#by_ptr player: RecPlayer) -> bool --- + + // @return true when any StateHash mismatch has been detected + RecPlayer_HasDiverged :: proc(#by_ptr player: RecPlayer) -> bool --- + + // @return a summary of the recording read at open: frame count, recorded tuning, and bounds + RecPlayer_GetInfo :: proc(#by_ptr player: RecPlayer) -> RecPlayerInfo --- + + // @return the first frame at which replay diverged, or -1 if it has not diverged + RecPlayer_GetDivergeFrame :: proc(#by_ptr player: RecPlayer) -> c.int --- + + // Set the worker count of the replay world. Clamped to [1, B3_MAX_WORKERS]. Applied to the live + // world at once and reused whenever the player rebuilds its world on Restart or a backward seek. + // Replaying at a different count than recorded re-partitions the constraint graph, so the StateHash + // check becomes a cross-thread determinism test. + RecPlayer_SetWorkerCount :: proc(player: ^RecPlayer, count: c.int) --- + + // Tune the keyframe ring used to speed up backward seeking. A keyframe is a periodic snapshot the + // player restores from instead of replaying from the start, trading memory for seek speed. + // @param player the recording player + // @param budgetBytes memory cap for the kept snapshots; the spacing widens to stay under it + // @param minIntervalFrames finest spacing between keyframes, in frames + // A zero budget or a non-positive interval keeps that value. Clears the existing ring, so call + // RecPlayer_Restart afterward to repopulate it under the new policy. + RecPlayer_SetKeyframePolicy :: proc(player: ^RecPlayer, budgetBytes: uint, minIntervalFrames: c.int) --- + + // @return the keyframe memory budget in bytes + RecPlayer_GetKeyframeBudget :: proc(#by_ptr player: RecPlayer) -> uint --- + + // @return the finest keyframe spacing in frames + RecPlayer_GetKeyframeMinInterval :: proc(#by_ptr player: RecPlayer) -> c.int --- + + // @return the current keyframe spacing in frames; starts at the min interval and doubles as the + // ring evicts to stay under budget, so it reflects the effective backward-seek granularity now + RecPlayer_GetKeyframeInterval :: proc(#by_ptr player: RecPlayer) -> c.int --- + + // @return the memory currently held by keyframe snapshots, in bytes + RecPlayer_GetKeyframeBytes :: proc(#by_ptr player: RecPlayer) -> uint --- + + // @return the number of bodies tracked in creation order (including holes for destroyed bodies) + RecPlayer_GetBodyCount :: proc(#by_ptr player: RecPlayer) -> c.int --- + + // Resolve a creation ordinal to the live body id at the current frame. + // @return the body id, or a null id if that ordinal is out of range or its body is destroyed + RecPlayer_GetBodyId :: proc(#by_ptr player: RecPlayer, index: c.int) -> BodyId --- + + // Wire host debug-shape callbacks into the player's replay world so a renderer can build + // per-shape draw resources (the 3D sample needs this or the replay world draws nothing). + // Rebuilds the current world under the new callbacks and rewinds to frame 0, so call it + // once right after RecPlayer_Create and re-read the world id afterward. The callbacks + // persist across Restart and backward seeks, which recreate the world internally. + // @param player the player to configure + // @param createDebugShape called when a replayed shape is added; returns a user draw handle + // @param destroyDebugShape called when a replayed shape is removed; may be NULL + // @param context user context passed to both callbacks + RecPlayer_SetDebugShapeCallbacks :: proc(player: ^RecPlayer, createDebugShape: CreateDebugShapeCallback, destroyDebugShape: DestroyDebugShapeCallback, ctx: rawptr) --- + + // Draw the spatial queries recorded during the most recently replayed frame, layered on top of the + // world. Call after World_Draw. NULL draw function pointers are skipped. + // @param player a valid player handle + // @param draw debug draw callbacks + // @param queryIndex index of the frame query to draw, or -1 to draw all of them + // @param selectedIndex index of the query to emphasize (reserved color plus a label), or -1 for none + RecPlayer_DrawFrameQueries :: proc(player: ^RecPlayer, draw: ^DebugDraw, queryIndex: c.int, selectedIndex: c.int) --- + + + // @return the number of spatial queries recorded for the most recently replayed frame + RecPlayer_GetFrameQueryCount :: proc(#by_ptr player: RecPlayer) -> c.int --- + + // Get a recorded query from the most recently replayed frame by index. + RecPlayer_GetFrameQuery :: proc(#by_ptr player: RecPlayer, index: c.int) -> RecQueryInfo --- + + // Get one result of a recorded query from the most recently replayed frame. + RecPlayer_GetFrameQueryHit :: proc(#by_ptr player: RecPlayer, queryIndex: c.int, hitIndex: c.int) -> RecQueryHit --- + + /**@}*/ // recording + + /** @} */ // world + + /** + * @defgroup body Body + * This is the body API. + * @{ + */ + + // Create a rigid body given a definition. No reference to the definition is retained. So you can create the definition + // on the stack and pass it as a pointer. + // @code{.c} + // BodyDef bodyDef = DefaultBodyDef(); + // BodyId myBodyId = CreateBody(myWorldId, &bodyDef); + // @endcode + // @warning This function is locked during callbacks. + CreateBody :: proc(worldId: WorldId, #by_ptr def: BodyDef) -> BodyId --- + + // Destroy a rigid body given an id. This destroys all shapes and joints attached to the body. + // Do not keep references to the associated shapes and joints. + DestroyBody :: proc(bodyId: BodyId) --- + + // Body identifier validation. A valid body exists in a world and is non-null. + // This can be used to detect orphaned ids. Provides validation for up to 64K allocations. + Body_IsValid :: proc(id: BodyId) -> bool --- + + // Get the body type: static, kinematic, or dynamic + Body_GetType :: proc(bodyId: BodyId) -> BodyType --- + + // Change the body type. This is an expensive operation. This automatically updates the mass + // properties regardless of the automatic mass setting. + Body_SetType :: proc(bodyId: BodyId, type: BodyType) --- + + // Set the body name. + Body_SetName :: proc(bodyId: BodyId, name: cstring) --- + + // Get the body name. + Body_GetName :: proc(bodyId: BodyId) -> cstring --- + + // Set the user data for a body + Body_SetUserData :: proc(bodyId: BodyId, userData: rawptr) --- + + // Get the user data stored in a body + Body_GetUserData :: proc(bodyId: BodyId) -> rawptr --- + + // Get the world position of a body. This is the location of the body origin. + Body_GetPosition :: proc(bodyId: BodyId) -> Pos --- + + // Get the world rotation of a body as a quaternion + Body_GetRotation :: proc(bodyId: BodyId) -> Quat --- + + // Get the world transform of a body. + Body_GetTransform :: proc(bodyId: BodyId) -> WorldTransform --- + + // Set the world transform of a body. This acts as a teleport and is fairly expensive. + // @note Generally you should create a body with the intended transform. + // @see BodyDef::position and BodyDef::rotation + Body_SetTransform :: proc(bodyId: BodyId, position: Pos, rotation: Quat) --- + + // Get a local point on a body given a world point + Body_GetLocalPoint :: proc(bodyId: BodyId, worldPoint: Pos) -> Vec3 --- + + // Get a world point on a body given a local point + Body_GetWorldPoint :: proc(bodyId: BodyId, localPoint: Vec3) -> Pos --- + + // Get a local vector on a body given a world vector + Body_GetLocalVector :: proc(bodyId: BodyId, worldVector: Vec3) -> Vec3 --- + + // Get a world vector on a body given a local vector + Body_GetWorldVector :: proc(bodyId: BodyId, localVector: Vec3) -> Vec3 --- + + // Get the linear velocity of a body's center of mass. Usually in meters per second. + Body_GetLinearVelocity :: proc(bodyId: BodyId) -> Vec3 --- + + // Get the angular velocity of a body in radians per second + Body_GetAngularVelocity :: proc(bodyId: BodyId) -> Vec3 --- + + // Set the linear velocity of a body. Usually in meters per second. + Body_SetLinearVelocity :: proc(bodyId: BodyId, linearVelocity: Vec3) --- + + // Set the angular velocity of a body in radians per second + Body_SetAngularVelocity :: proc(bodyId: BodyId, angularVelocity: Vec3) --- + + // Set the velocity to reach the given transform after a given time step. + // The result will be close but maybe not exact. This is meant for kinematic bodies. + // The target is not applied if the velocity would be below the sleep threshold. + // This will optionally wake the body if asleep, but only if the movement is significant. + Body_SetTargetTransform :: proc(bodyId: BodyId, target: WorldTransform, timeStep: f32, wake: bool) --- + + // Get the linear velocity of a local point attached to a body. Usually in meters per second. + Body_GetLocalPointVelocity :: proc(bodyId: BodyId, localPoint: Vec3) -> Vec3 --- + + // Get the linear velocity of a world point attached to a body. Usually in meters per second. + Body_GetWorldPointVelocity :: proc(bodyId: BodyId, worldPoint: Pos) -> Vec3 --- + + // Apply a force at a world point. If the force is not applied at the center of mass, + // it will generate a torque and affect the angular velocity. This optionally wakes up the body. + // The force is ignored if the body is not awake. + // @param bodyId The body id + // @param force The world force vector, usually in newtons (N) + // @param point The world position of the point of application + // @param wake Option to wake up the body + Body_ApplyForce :: proc(bodyId: BodyId, force: Vec3, point: Pos, wake: bool) --- + + // Apply a force to the center of mass. This optionally wakes up the body. + // The force is ignored if the body is not awake. + // @param bodyId The body id + // @param force the world force vector, usually in newtons (N). + // @param wake also wake up the body + Body_ApplyForceToCenter :: proc(bodyId: BodyId, force: Vec3, wake: bool) --- + + // Apply a torque. This affects the angular velocity without affecting the linear velocity. + // This optionally wakes the body. The torque is ignored if the body is not awake. + // @param bodyId The body id + // @param torque the world torque vector, usually in N*m. + // @param wake also wake up the body + Body_ApplyTorque :: proc(bodyId: BodyId, torque: Vec3, wake: bool) --- + + // Apply an impulse at a point. This immediately modifies the velocity. + // It also modifies the angular velocity if the point of application + // is not at the center of mass. This optionally wakes the body. + // The impulse is ignored if the body is not awake. + // @param bodyId The body id + // @param impulse the world impulse vector, usually in N*s or kg*m/s. + // @param point the world position of the point of application. + // @param wake also wake up the body + // @warning This should be used for one-shot impulses. If you need a steady force, + // use a force instead, which will work better with the sub-stepping solver. + Body_ApplyLinearImpulse :: proc(bodyId: BodyId, impulse: Vec3, point: Pos, wake: bool) --- + + // Apply an impulse to the center of mass. This immediately modifies the velocity. + // The impulse is ignored if the body is not awake. This optionally wakes the body. + // @param bodyId The body id + // @param impulse the world impulse vector, usually in N*s or kg*m/s. + // @param wake also wake up the body + // @warning This should be used for one-shot impulses. If you need a steady force, + // use a force instead, which will work better with the sub-stepping solver. + Body_ApplyLinearImpulseToCenter :: proc(bodyId: BodyId, impulse: Vec3, wake: bool) --- + + // Apply an angular impulse in world space. The impulse is ignored if the body is not awake. + // This optionally wakes the body. + // @param bodyId The body id + // @param impulse the world angular impulse vector, usually in units of kg*m*m/s + // @param wake also wake up the body + // @warning This should be used for one-shot impulses. If you need a steady torque, + // use a torque instead, which will work better with the sub-stepping solver. + Body_ApplyAngularImpulse :: proc(bodyId: BodyId, impulse: Vec3, wake: bool) --- + + // Get the mass of the body, usually in kilograms + Body_GetMass :: proc(bodyId: BodyId) -> f32 --- + + // Get the rotational inertia of the body in local space, usually in kg*m^2 + Body_GetLocalRotationalInertia :: proc(bodyId: BodyId) -> Matrix3 --- + + // Get the inverse mass of the body, usually in 1/kilograms + Body_GetInverseMass :: proc(bodyId: BodyId) -> f32 --- + + // Get the inverse rotational inertia of the body in world space, usually in 1/kg*m^2 + Body_GetWorldInverseRotationalInertia :: proc(bodyId: BodyId) -> Matrix3 --- + + // Get the center of mass position of the body in local space + Body_GetLocalCenter :: proc(bodyId: BodyId) -> Vec3 --- + + // Get the center of mass position of the body in world space + Body_GetWorldCenter :: proc(bodyId: BodyId) -> Pos --- + + // Override the body's mass properties. Normally this is computed automatically using the + // shape geometry and density. This information is lost if a shape is added or removed or if the + // body type changes. + Body_SetMassData :: proc(bodyId: BodyId, massData: MassData) --- + + // Get the mass data for a body + Body_GetMassData :: proc(bodyId: BodyId) -> MassData --- + + // This updates the mass properties to the sum of the mass properties of the shapes. + // This normally does not need to be called unless you called SetMassData to override + // the mass and you later want to reset the mass. + // You may also use this when automatic mass computation has been disabled. + // You should call this regardless of body type. + Body_ApplyMassFromShapes :: proc(bodyId: BodyId) --- + + // Adjust the linear damping. Normally this is set in BodyDef before creation. + Body_SetLinearDamping :: proc(bodyId: BodyId, linearDamping: f32) --- + + // Get the current linear damping. + Body_GetLinearDamping :: proc(bodyId: BodyId) -> f32 --- + + // Adjust the angular damping. Normally this is set in BodyDef before creation. + Body_SetAngularDamping :: proc(bodyId: BodyId, angularDamping: f32) --- + + // Get the current angular damping. + Body_GetAngularDamping :: proc(bodyId: BodyId) -> f32 --- + + // Adjust the gravity scale. Normally this is set in BodyDef before creation. + // @see BodyDef::gravityScale + Body_SetGravityScale :: proc(bodyId: BodyId, gravityScale: f32) --- + + // Get the current gravity scale + Body_GetGravityScale :: proc(bodyId: BodyId) -> f32 --- + + // @return true if this body is awake + Body_IsAwake :: proc(bodyId: BodyId) -> bool --- + + // Wake a body from sleep. This wakes the entire island the body is touching. + // @warning Putting a body to sleep will put the entire island of bodies touching this body to sleep, + // which can be expensive and possibly unintuitive. + Body_SetAwake :: proc(bodyId: BodyId, awake: bool) --- + + // Enable or disable sleeping for this body. If sleeping is disabled the body will wake. + Body_EnableSleep :: proc(bodyId: BodyId, enableSleep: bool) --- + + // Returns true if sleeping is enabled for this body + Body_IsSleepEnabled :: proc(bodyId: BodyId) -> bool --- + + // Set the sleep threshold, usually in meters per second + Body_SetSleepThreshold :: proc(bodyId: BodyId, sleepThreshold: f32) --- + + // Get the sleep threshold, usually in meters per second. + Body_GetSleepThreshold :: proc(bodyId: BodyId) -> f32 --- + + // Returns true if this body is enabled + Body_IsEnabled :: proc(bodyId: BodyId) -> bool --- + + // Disable a body by removing it completely from the simulation. This is expensive. + Body_Disable :: proc(bodyId: BodyId) --- + + // Enable a body by adding it to the simulation. This is expensive. + Body_Enable :: proc(bodyId: BodyId) --- + + // Set the motion locks on this body. + Body_SetMotionLocks :: proc(bodyId: BodyId, locks: MotionLocks) --- + + // Get the motion locks for this body. + Body_GetMotionLocks :: proc(bodyId: BodyId) -> MotionLocks --- + + // Set this body to be a bullet. A bullet does continuous collision detection + // against dynamic bodies (but not other bullets). + Body_SetBullet :: proc(bodyId: BodyId, flag: bool) --- + + // Is this body a bullet? + Body_IsBullet :: proc(bodyId: BodyId) -> bool --- + + // Enable or disable contact recycling for this body. Contact recycling is a performance optimization + // that reuses contact manifolds when bodies move slightly. Disabling it can avoid ghost collisions + // on characters at the cost of higher per-step work. Existing contacts retain their prior setting; + // only contacts created after this call see the new value. + // @see BodyDef::enableContactRecycling + Body_EnableContactRecycling :: proc(bodyId: BodyId, flag: bool) --- + + // Is contact recycling enabled on this body? + Body_IsContactRecyclingEnabled :: proc(bodyId: BodyId) -> bool --- + + // Enable/disable hit events on all shapes + // @see ShapeDef::enableHitEvents + Body_EnableHitEvents :: proc(bodyId: BodyId, flag: bool) --- + + // Get the world that owns this body + Body_GetWorld :: proc(bodyId: BodyId) -> WorldId --- + + // Get the number of shapes on this body + Body_GetShapeCount :: proc(bodyId: BodyId) -> c.int --- + + // Get the shape ids for all shapes on this body, up to the provided capacity. + // @returns the number of shape ids stored in the user array + Body_GetShapes :: proc(bodyId: BodyId, shapeArray: [^]ShapeId, capacity: c.int) -> c.int --- + + // Get the number of joints on this body + Body_GetJointCount :: proc(bodyId: BodyId) -> c.int --- + + // Get the joint ids for all joints on this body, up to the provided capacity + // @returns the number of joint ids stored in the user array + Body_GetJoints :: proc(bodyId: BodyId, jointArray: [^]JointId, capacity: c.int) -> c.int --- + + // Get the maximum capacity required for retrieving all the touching contacts on a body + Body_GetContactCapacity :: proc(bodyId: BodyId) -> c.int --- + + // Get the touching contact data for a body + Body_GetContactData :: proc(bodyId: BodyId, contactData: [^]ContactData, capacity: c.int) -> c.int --- + + // Get the current world AABB that contains all the attached shapes. Note that this may not encompass the body origin. + // If there are no shapes attached then the returned AABB is empty and centered on the body origin. + Body_ComputeAABB :: proc(bodyId: BodyId) -> AABB --- + + // Get the closest point on a body to a world target. + Body_GetClosestPoint :: proc(bodyId: BodyId, result: ^Vec3, target: Vec3) -> f32 --- + + // Cast a ray at a specific body using a specified body transform. + Body_CastRay :: proc(bodyId: BodyId, origin: Pos, translation: Vec3, filter: QueryFilter, maxFraction: f32, bodyTransform: WorldTransform) -> BodyCastResult --- + + // Cast a shape at a specific body using a specified body transform. + Body_CastShape :: proc(bodyId: BodyId, origin: Pos, #by_ptr proxy: ShapeProxy, translation: Vec3, filter: QueryFilter, maxFraction: f32, canEncroach: b32, bodyTransform: WorldTransform) -> BodyCastResult --- + + // Overlap a shape with a specific body using a specified body transform. + Body_OverlapShape :: proc(bodyId: BodyId, origin: Pos, #by_ptr proxy: ShapeProxy, filter: QueryFilter, bodyTransform: WorldTransform) -> bool --- + + // Collide a character mover with a specific body using a specified body transform. + Body_CollideMover :: proc(bodyId: BodyId, bodyPlanes: [^]BodyPlaneResult, planeCapacity: c.int, origin: Pos, #by_ptr mover: Capsule, filter: QueryFilter, bodyTransform: WorldTransform) -> c.int --- + + /** @} */ // body + + /** + * @defgroup shape Shape + * Functions to create, destroy, and access. + * Shapes bind raw geometry to bodies and hold material properties including friction and restitution. + * @{ + */ + + // Create a circle shape and attach it to a body. The shape definition and geometry are fully cloned. + // Contacts are not created until the next time step. + // @return the shape id for accessing the shape + CreateSphereShape :: proc(bodyId: BodyId, #by_ptr def: ShapeDef, sphere: ^Sphere) -> ShapeId --- + + // Create a capsule shape and attach it to a body. The shape definition and geometry are fully cloned. + // Contacts are not created until the next time step. + // @return the shape id for accessing the shape + CreateCapsuleShape :: proc(bodyId: BodyId, #by_ptr def: ShapeDef, capsule: ^Capsule) -> ShapeId --- + + // Create a convex hull shape and attach it to a body. The shape definition is fully cloned. Contacts are not created + // until the next time step. + // @return the shape id for accessing the shape + CreateHullShape :: proc(bodyId: BodyId, #by_ptr def: ShapeDef, hull: ^HullData) -> ShapeId --- + + // Create a convex hull shape and attach it to a body. The hull is cloned then transformed with scale applied first. + // Use this for non-uniform or mirrored scale or a baked local transform. The baked result is shared through the + // world hull database. The shape definition and geometry are fully cloned. Contacts are not created until the next time step. + // @return the shape id for accessing the shape + CreateTransformedHullShape :: proc(bodyId: BodyId, #by_ptr def: ShapeDef, hull: ^HullData, transform: Transform, scale: Vec3) -> ShapeId --- + + // Create a mesh hull shape and attach it to a body. The shape definition is fully cloned but the mesh is not. + // Contacts are not created until the next time step. + // Mesh collision only creates contacts on static bodies. + // @warning this holds reference to the input mesh data which must remain valid for the lifetime of this shape + // @return the shape id for accessing the shape + CreateMeshShape :: proc(bodyId: BodyId, #by_ptr def: ShapeDef, mesh: ^MeshData, scale: Vec3) -> ShapeId --- + + // Create a height-field shape and attach it to a body. The shape definition is fully cloned but the height field is not. + // Contacts are not created until the next time step. + // Height field is only allowed on static bodies. + // @warning this holds reference to the input height field which must remain valid for the lifetime of this shape + // @return the shape id for accessing the shape + CreateHeightFieldShape :: proc(bodyId: BodyId, #by_ptr def: ShapeDef, heightField: ^HeightFieldData) -> ShapeId --- + + // Compound shapes are only allowed on static bodies. + CreateCompoundShape :: proc(bodyId: BodyId, #by_ptr def: ShapeDef, compound: ^CompoundData) -> ShapeId --- + + // Destroy a shape. You may defer the body mass update which can improve performance if several shapes on a + // body are destroyed at once. + // @see Body_ApplyMassFromShapes + DestroyShape :: proc(shapeId: ShapeId, updateBodyMass: bool) --- + + // Shape identifier validation. Provides validation for up to 64K allocations. + Shape_IsValid :: proc(id: ShapeId) -> bool --- + + // Get the type of a shape + Shape_GetType :: proc(shapeId: ShapeId) -> ShapeType --- + + // Get the id of the body that a shape is attached to + Shape_GetBody :: proc(shapeId: ShapeId) -> BodyId --- + + // Get the world that owns this shape + Shape_GetWorld :: proc(shapeId: ShapeId) -> WorldId --- + + // Returns true if the shape is a sensor + Shape_IsSensor :: proc(shapeId: ShapeId) -> bool --- + + // Set the shape name. + Shape_SetName :: proc(shapeId: ShapeId, name: cstring) --- + + // Get the shape name. Returns an empty string if the name isn't set. + Shape_GetName :: proc(shapeId: ShapeId) -> cstring --- + + // Set the user data for a shape + Shape_SetUserData :: proc(shapeId: ShapeId, userData: rawptr) --- + + // Get the user data for a shape. This is useful when you get a shape id + // from an event or query. + Shape_GetUserData :: proc(shapeId: ShapeId) -> rawptr --- + + // Set the mass density of a shape, usually in kg/m^3. + // This will optionally update the mass properties on the parent body. + // @see ShapeDef::density, Body_ApplyMassFromShapes + Shape_SetDensity :: proc(shapeId: ShapeId, density: f32, updateBodyMass: bool) --- + + // Get the density of a shape, usually in kg/m^3 + Shape_GetDensity :: proc(shapeId: ShapeId) -> f32 --- + + // Set the friction on a shape + Shape_SetFriction :: proc(shapeId: ShapeId, friction: f32) --- + + // Get the friction of a shape + Shape_GetFriction :: proc(shapeId: ShapeId) -> f32 --- + + // Set the shape restitution (bounciness) + Shape_SetRestitution :: proc(shapeId: ShapeId, restitution: f32) --- + + // Get the shape restitution + Shape_GetRestitution :: proc(shapeId: ShapeId) -> f32 --- + + // Set the shape base surface material. Does not change per triangle materials. + Shape_SetSurfaceMaterial :: proc(shapeId: ShapeId, surfaceMaterial: SurfaceMaterial) --- + + // Get the base shape surface material. + Shape_GetSurfaceMaterial :: proc(shapeId: ShapeId) -> SurfaceMaterial --- + + // Get the number of mesh surface materials. + Shape_GetMeshMaterialCount :: proc(shapeId: ShapeId) -> c.int --- + + // Set a surface material for a mesh shape. + Shape_SetMeshMaterial :: proc(shapeId: ShapeId, surfaceMaterial: SurfaceMaterial, index: c.int) --- + + // Get a surface material for a mesh shape + Shape_GetMeshSurfaceMaterial :: proc(shapeId: ShapeId, index: c.int) -> SurfaceMaterial --- + + // Get the shape filter + Shape_GetFilter :: proc(shapeId: ShapeId) -> Filter --- + + // Set the current filter. This is almost as expensive as recreating the shape. + // @see ShapeDef::filter + // @param shapeId the shape + // @param filter the new filter + // @param invokeContacts if true then the shape will have all contacts recomputed the next time step (expensive) + Shape_SetFilter :: proc(shapeId: ShapeId, filter: Filter, invokeContacts: bool) --- + + // Enable sensor events for this shape. Only applies to kinematic and dynamic bodies. Ignored for sensors. + // @see ShapeDef::isSensor + Shape_EnableSensorEvents :: proc(shapeId: ShapeId, flag: bool) --- + + // Returns true if sensor events are enabled + Shape_AreSensorEventsEnabled :: proc(shapeId: ShapeId) -> bool --- + + // Enable contact events for this shape. Only applies to kinematic and dynamic bodies. Ignored for sensors. + // @see ShapeDef::enableContactEvents + Shape_EnableContactEvents :: proc(shapeId: ShapeId, flag: bool) --- + + // Returns true if contact events are enabled + Shape_AreContactEventsEnabled :: proc(shapeId: ShapeId) -> bool --- + + // Enable pre-solve contact events for this shape. Only applies to dynamic bodies. These are expensive + // and must be carefully handled due to multithreading. Ignored for sensors. + // @see PreSolveFcn + Shape_EnablePreSolveEvents :: proc(shapeId: ShapeId, flag: bool) --- + + // Returns true if pre-solve events are enabled + Shape_ArePreSolveEventsEnabled :: proc(shapeId: ShapeId) -> bool --- + + // Enable contact hit events for this shape. Ignored for sensors. + // @see WorldDef.hitEventThreshold + Shape_EnableHitEvents :: proc(shapeId: ShapeId, flag: bool) --- + + // Returns true if hit events are enabled + Shape_AreHitEventsEnabled :: proc(shapeId: ShapeId) -> bool --- + + // Ray cast a shape directly. The ray runs from origin to origin + translation and the hit point + // comes back as a world position, so the cast stays precise far from the world origin. + Shape_RayCast :: proc(shapeId: ShapeId, origin: Pos, translation: Vec3) -> WorldCastOutput --- + + // Get a copy of the shape's sphere. Asserts the type is correct. + Shape_GetSphere :: proc(shapeId: ShapeId) -> Sphere --- + + // Get a copy of the shape's capsule. Asserts the type is correct. + Shape_GetCapsule :: proc(shapeId: ShapeId) -> Capsule --- + + // Get the shape's convex hull. Asserts the type is correct. + Shape_GetHull :: proc(shapeId: ShapeId) -> ^HullData --- + + // Get the shape's mesh. Asserts the type is correct. + Shape_GetMesh :: proc(shapeId: ShapeId) -> Mesh --- + + // Get the shape's height field. Asserts the type is correct. + Shape_GetHeightField :: proc(shapeId: ShapeId) -> ^HeightFieldData --- + + // Allows you to change a shape to be a sphere or update the current sphere. + // This does not modify the mass properties. + // @see Body_ApplyMassFromShapes + Shape_SetSphere :: proc(shapeId: ShapeId, #by_ptr sphere: Sphere) --- + + // Allows you to change a shape to be a capsule or update the current capsule. + // This does not modify the mass properties. + // @see Body_ApplyMassFromShapes + Shape_SetCapsule :: proc(shapeId: ShapeId, #by_ptr capsule: Capsule) --- + + // Allows you to change a shape to be a hull or update the current hull. + // This does not modify the mass properties. + // @see Body_ApplyMassFromShapes + Shape_SetHull :: proc(shapeId: ShapeId, #by_ptr hull: HullData) --- + + // Allows you to change a shape to be a mesh or update the current mesh. + // This does not modify the mass properties. + // @see Body_ApplyMassFromShapes + Shape_SetMesh :: proc(shapeId: ShapeId, #by_ptr meshData: MeshData, scale: Vec3) --- + + // Get the maximum capacity required for retrieving all the touching contacts on a shape + Shape_GetContactCapacity :: proc(shapeId: ShapeId) -> c.int --- + + // Get the touching contact data for a shape. The provided shapeId will be either shapeIdA or shapeIdB on the contact data. + // @note Box3D uses speculative collision so some contact points may be separated. + // @returns the number of elements filled in the provided array + // @warning do not ignore the return value, it specifies the valid number of elements + Shape_GetContactData :: proc(shapeId: ShapeId, contactData: [^]ContactData, capacity: c.int) -> c.int --- + + // Get the maximum capacity required for retrieving all the overlapped shapes on a sensor shape. + // This returns 0 if the provided shape is not a sensor. + // @param shapeId the id of a sensor shape + // @returns the required capacity to get all the overlaps in Shape_GetSensorOverlaps + Shape_GetSensorCapacity :: proc(shapeId: ShapeId) -> c.int --- + + // Get the overlap data for a sensor shape. + // @param shapeId the id of a sensor shape + // @param visitorIds a user allocated array that is filled with the overlapping shapes (visitors) + // @param capacity the capacity of overlappedShapes + // @returns the number of elements filled in the provided array + // @warning do not ignore the return value, it specifies the valid number of elements + // @warning overlaps may contain destroyed shapes so use Shape_IsValid to confirm each overlap + Shape_GetSensorData :: proc(shapeId: ShapeId, visitorIds: [^]ShapeId, capacity: c.int) -> c.int --- + + // Get the current world AABB + Shape_GetAABB :: proc(shapeId: ShapeId) -> AABB --- + + // Compute the mass data for a shape + Shape_ComputeMassData :: proc(shapeId: ShapeId) -> MassData --- + + // Get the closest point on a shape to a target point. Target and result are in world space. + Shape_GetClosestPoint :: proc(shapeId: ShapeId, target: Vec3) -> Vec3 --- + + // Apply a wind force to the body for this shape using the density of air. This considers + // the projected area of the shape in the wind direction. This also considers + // the relative velocity of the shape. + // @param shapeId the shape id + // @param wind the wind velocity in world space + // @param drag the drag coefficient, the force that opposes the relative velocity + // @param lift the lift coefficient, the force that is perpendicular to the relative velocity + // @param maxSpeed the maximum relative speed. Speed cap is necessary for stability. Typically 10m/s or less. + // @param wake should this wake the body + Shape_ApplyWind :: proc(shapeId: ShapeId, wind: Vec3 , drag, lift: f32, maxSpeed: f32, wake: bool) --- + + /** @} */ // shape + + /** + * @defgroup joint Joint + * @brief Joints allow you to connect rigid bodies together while allowing various forms of relative motions. + * @{ + */ + + // Destroy a joint + DestroyJoint :: proc(jointId: JointId, wakeAttached: bool) --- + + // Joint identifier validation. Provides validation for up to 64K allocations. + Joint_IsValid :: proc(id: JointId) -> bool --- + + // Get the joint type + Joint_GetType :: proc(jointId: JointId) -> JointType --- + + // Get body A id on a joint + Joint_GetBodyA :: proc(jointId: JointId) -> BodyId --- + + // Get body B id on a joint + Joint_GetBodyB :: proc(jointId: JointId) -> BodyId --- + + // Get the world that owns this joint + Joint_GetWorld :: proc(jointId: JointId) -> WorldId --- + + // Set the local frame on bodyA + Joint_SetLocalFrameA :: proc(jointId: JointId, localFrame: Transform) --- + + // Get the local frame on bodyA + Joint_GetLocalFrameA :: proc(jointId: JointId) -> Transform --- + + // Set the local frame on bodyB + Joint_SetLocalFrameB :: proc(jointId: JointId, localFrame: Transform) --- + + // Get the local frame on bodyB + Joint_GetLocalFrameB :: proc(jointId: JointId) -> Transform --- + + // Toggle collision between connected bodies + Joint_SetCollideConnected :: proc(jointId: JointId, shouldCollide: bool) --- + + // Is collision allowed between connected bodies? + Joint_GetCollideConnected :: proc(jointId: JointId) -> bool --- + + // Set the user data on a joint + Joint_SetUserData :: proc(jointId: JointId, userData: rawptr) --- + + // Get the user data on a joint + Joint_GetUserData :: proc(jointId: JointId) -> rawptr --- + + // Wake the bodies connect to this joint + Joint_WakeBodies :: proc(jointId: JointId) --- + + // Get the current constraint force for this joint + Joint_GetConstraintForce :: proc(jointId: JointId) -> Vec3 --- + + // Get the current constraint torque for this joint + Joint_GetConstraintTorque :: proc(jointId: JointId) -> Vec3 --- + + // Get the current linear separation error for this joint. Does not consider admissible movement. Usually in meters. + Joint_GetLinearSeparation :: proc(jointId: JointId) -> f32 --- + + // Get the current angular separation error for this joint. Does not consider admissible movement. Usually in radians. + Joint_GetAngularSeparation :: proc(jointId: JointId) -> f32 --- + + // Set the joint constraint tuning. Advanced feature. + // @param jointId the joint + // @param hertz the stiffness in Hertz (cycles per second) + // @param dampingRatio the non-dimensional damping ratio (one for critical damping) + Joint_SetConstraintTuning :: proc(jointId: JointId, hertz: f32, dampingRatio: f32) --- + + // Get the joint constraint tuning. Advanced feature. + Joint_GetConstraintTuning :: proc(jointId: JointId, hertz: ^f32, dampingRatio: ^f32) --- + + // Set the force threshold for joint events (Newtons) + Joint_SetForceThreshold :: proc(jointId: JointId, threshold: f32) --- + + // Get the force threshold for joint events (Newtons) + Joint_GetForceThreshold :: proc(jointId: JointId) -> f32 --- + + // Set the torque threshold for joint events (N-m) + Joint_SetTorqueThreshold :: proc(jointId: JointId, threshold: f32) --- + + // Get the torque threshold for joint events (N-m) + Joint_GetTorqueThreshold :: proc(jointId: JointId) -> f32 --- + + /** + * @defgroup parallel_joint Parallel Joint + * @brief Functions for the parallel joint. + * @{ + */ + + // Create a parallel joint + // @see ParallelJointDef for details + CreateParallelJoint :: proc(worldId: WorldId, #by_ptr def: ParallelJointDef) -> JointId --- + + // Set the spring stiffness in Hertz + ParallelJoint_SetSpringHertz :: proc(jointId: JointId, hertz: f32) --- + + // Set the spring damping ratio, non-dimensional + ParallelJoint_SetSpringDampingRatio :: proc(jointId: JointId, dampingRatio: f32) --- + + // Get the spring Hertz + ParallelJoint_GetSpringHertz :: proc(jointId: JointId) -> f32 --- + + // Get the spring damping ratio + ParallelJoint_GetSpringDampingRatio :: proc(jointId: JointId) -> f32 --- + + // Set the maximum spring torque, usually in newton-meters + ParallelJoint_SetMaxTorque :: proc(jointId: JointId, force: f32) --- + + // Get the maximum spring torque, usually in newton-meters + ParallelJoint_GetMaxTorque :: proc(jointId: JointId) -> f32 --- + + /** @} */ // parallel_joint + + /** + * @defgroup distance_joint Distance Joint + * @brief Functions for the distance joint. + * @{ + */ + + // Create a distance joint + // @see DistanceJointDef for details + CreateDistanceJoint :: proc(worldId: WorldId, #by_ptr def: DistanceJointDef) -> JointId --- + + // Set the rest length of a distance joint + // @param jointId The id for a distance joint + // @param length The new distance joint length + DistanceJoint_SetLength :: proc(jointId: JointId, length: f32) --- + + // Get the rest length of a distance joint + DistanceJoint_GetLength :: proc(jointId: JointId) -> f32 --- + + // Enable/disable the distance joint spring. When disabled the distance joint is rigid. + DistanceJoint_EnableSpring :: proc(jointId: JointId, enableSpring: bool) --- + + // Is the distance joint spring enabled? + DistanceJoint_IsSpringEnabled :: proc(jointId: JointId) -> bool --- + + // Set the force range for the spring. + DistanceJoint_SetSpringForceRange :: proc(jointId: JointId, lowerForce, upperForce: f32) --- + + // Get the force range for the spring. + DistanceJoint_GetSpringForceRange :: proc(jointId: JointId, lowerForce, upperForce: ^f32) --- + + // Set the spring stiffness in Hertz + DistanceJoint_SetSpringHertz :: proc(jointId: JointId, hertz: f32) --- + + // Set the spring damping ratio, non-dimensional + DistanceJoint_SetSpringDampingRatio :: proc(jointId: JointId, dampingRatio: f32) --- + + // Get the spring Hertz + DistanceJoint_GetSpringHertz :: proc(jointId: JointId) -> f32 --- + + // Get the spring damping ratio + DistanceJoint_GetSpringDampingRatio :: proc(jointId: JointId) -> f32 --- + + // Enable joint limit. The limit only works if the joint spring is enabled. Otherwise the joint is rigid + // and the limit has no effect. + DistanceJoint_EnableLimit :: proc(jointId: JointId, enableLimit: bool) --- + + // Is the distance joint limit enabled? + DistanceJoint_IsLimitEnabled :: proc(jointId: JointId) -> bool --- + + // Set the minimum and maximum length parameters of a distance joint + DistanceJoint_SetLengthRange :: proc(jointId: JointId, minLength, maxLength: f32) --- + + // Get the distance joint minimum length + DistanceJoint_GetMinLength :: proc(jointId: JointId) -> f32 --- + + // Get the distance joint maximum length + DistanceJoint_GetMaxLength :: proc(jointId: JointId) -> f32 --- + + // Get the current length of a distance joint + DistanceJoint_GetCurrentLength :: proc(jointId: JointId) -> f32 --- + + // Enable/disable the distance joint motor + DistanceJoint_EnableMotor :: proc(jointId: JointId, enableMotor: bool) --- + + // Is the distance joint motor enabled? + DistanceJoint_IsMotorEnabled :: proc(jointId: JointId) -> bool --- + + // Set the distance joint motor speed, usually in meters per second + DistanceJoint_SetMotorSpeed :: proc(jointId: JointId, motorSpeed: f32) --- + + // Get the distance joint motor speed, usually in meters per second + DistanceJoint_GetMotorSpeed :: proc(jointId: JointId) -> f32 --- + + // Set the distance joint maximum motor force, usually in newtons + DistanceJoint_SetMaxMotorForce :: proc(jointId: JointId, force: f32) --- + + // Get the distance joint maximum motor force, usually in newtons + DistanceJoint_GetMaxMotorForce :: proc(jointId: JointId) -> f32 --- + + // Get the distance joint current motor force, usually in newtons + DistanceJoint_GetMotorForce :: proc(jointId: JointId) -> f32 --- + + /** @} */ // distance_joint + + /** + * @defgroup motor_joint Motor Joint + * @brief Functions for the motor joint. + * + * The motor joint is designed to control the movement of a body while still being + * responsive to collisions. A spring controls the position and rotation. A velocity motor + * can be used to control velocity and allows for friction in top-down games. Both types + * of control can be combined. For example, you can have a spring with friction. + * Position and velocity control have force and torque limits. + * @{ + */ + + // Create a motor joint + // @see MotorJointDef for details + CreateMotorJoint :: proc(worldId: WorldId, #by_ptr def: MotorJointDef) -> JointId --- + + // Set the desired relative linear velocity in meters per second + MotorJoint_SetLinearVelocity :: proc(jointId: JointId, velocity: Vec3) --- + + // Get the desired relative linear velocity in meters per second + MotorJoint_GetLinearVelocity :: proc(jointId: JointId) -> Vec3 --- + + // Set the desired relative angular velocity in radians per second + MotorJoint_SetAngularVelocity :: proc(jointId: JointId, velocity: Vec3) --- + + // Get the desired relative angular velocity in radians per second + MotorJoint_GetAngularVelocity :: proc(jointId: JointId) -> Vec3 --- + + // Set the motor joint maximum force, usually in newtons + MotorJoint_SetMaxVelocityForce :: proc(jointId: JointId, maxForce: f32) --- + + // Get the motor joint maximum force, usually in newtons + MotorJoint_GetMaxVelocityForce :: proc(jointId: JointId) -> f32 --- + + // Set the motor joint maximum torque, usually in newton-meters + MotorJoint_SetMaxVelocityTorque :: proc(jointId: JointId, maxTorque: f32) --- + + // Get the motor joint maximum torque, usually in newton-meters + MotorJoint_GetMaxVelocityTorque :: proc(jointId: JointId) -> f32 --- + + // Set the spring linear hertz stiffness + MotorJoint_SetLinearHertz :: proc(jointId: JointId, hertz: f32) --- + + // Get the spring linear hertz stiffness + MotorJoint_GetLinearHertz :: proc(jointId: JointId) -> f32 --- + + // Set the spring linear damping ratio. Use 1.0 for critical damping. + MotorJoint_SetLinearDampingRatio :: proc(jointId: JointId, damping: f32) --- + + // Get the spring linear damping ratio. + MotorJoint_GetLinearDampingRatio :: proc(jointId: JointId) -> f32 --- + + // Set the spring angular hertz stiffness + MotorJoint_SetAngularHertz :: proc(jointId: JointId, hertz: f32) --- + + // Get the spring angular hertz stiffness + MotorJoint_GetAngularHertz :: proc(jointId: JointId) -> f32 --- + + // Set the spring angular damping ratio. Use 1.0 for critical damping. + MotorJoint_SetAngularDampingRatio :: proc(jointId: JointId, damping: f32) --- + + // Get the spring angular damping ratio. + MotorJoint_GetAngularDampingRatio :: proc(jointId: JointId) -> f32 --- + + // Set the maximum spring force in newtons. + MotorJoint_SetMaxSpringForce :: proc(jointId: JointId, maxForce: f32) --- + + // Get the maximum spring force in newtons. + MotorJoint_GetMaxSpringForce :: proc(jointId: JointId) -> f32 --- + + // Set the maximum spring torque in newtons * meters + MotorJoint_SetMaxSpringTorque :: proc(jointId: JointId, maxTorque: f32) --- + + // Get the maximum spring torque in newtons * meters + MotorJoint_GetMaxSpringTorque :: proc(jointId: JointId) -> f32 --- + + /**@}*/ // motor_joint + + /** + * @defgroup filter_joint Filter Joint + * @brief Functions for the filter joint. + * + * The filter joint is used to disable collision between two bodies. As a side effect of being a joint, it also + * keeps the two bodies in the same simulation island. + * @{ + */ + + // Create a filter joint. + // @see FilterJointDef for details + CreateFilterJoint :: proc(worldId: WorldId, #by_ptr def: FilterJointDef) -> JointId --- + + /**@}*/ // filter_joint + + /** + * @defgroup prismatic_joint Prismatic Joint + * @brief A prismatic joint allows for translation along a single axis with no rotation. + * + * The prismatic joint is useful for things like pistons and moving platforms, where you want a body to translate + * along an axis and have no rotation. Also called a *slider* joint. + * @{ + */ + + // Create a prismatic (slider) joint. + // @see PrismaticJointDef for details + CreatePrismaticJoint :: proc(worldId: WorldId, #by_ptr def: PrismaticJointDef) -> JointId --- + + // Enable/disable the joint spring. + PrismaticJoint_EnableSpring :: proc(jointId: JointId, enableSpring: bool) --- + + // Is the prismatic joint spring enabled or not? + PrismaticJoint_IsSpringEnabled :: proc(jointId: JointId) -> bool --- + + // Set the prismatic joint stiffness in Hertz. + // This should usually be less than a quarter of the simulation rate. For example, if the simulation + // runs at 60Hz then the joint stiffness should be 15Hz or less. + PrismaticJoint_SetSpringHertz :: proc(jointId: JointId, hertz: f32) --- + + // Get the prismatic joint stiffness in Hertz + PrismaticJoint_GetSpringHertz :: proc(jointId: JointId) -> f32 --- + + // Set the prismatic joint damping ratio (non-dimensional) + PrismaticJoint_SetSpringDampingRatio :: proc(jointId: JointId, dampingRatio: f32) --- + + // Get the prismatic spring damping ratio (non-dimensional) + PrismaticJoint_GetSpringDampingRatio :: proc(jointId: JointId) -> f32 --- + + // Set the prismatic joint target translation. Usually in meters. + PrismaticJoint_SetTargetTranslation :: proc(jointId: JointId, targetTranslation: f32) --- + + // Get the prismatic joint target translation. Usually in meters. + PrismaticJoint_GetTargetTranslation :: proc(jointId: JointId) -> f32 --- + + // Enable/disable a prismatic joint limit + PrismaticJoint_EnableLimit :: proc(jointId: JointId, enableLimit: bool) --- + + // Is the prismatic joint limit enabled? + PrismaticJoint_IsLimitEnabled :: proc(jointId: JointId) -> bool --- + + // Get the prismatic joint lower limit + PrismaticJoint_GetLowerLimit :: proc(jointId: JointId) -> f32 --- + + // Get the prismatic joint upper limit + PrismaticJoint_GetUpperLimit :: proc(jointId: JointId) -> f32 --- + + // Set the prismatic joint limits + PrismaticJoint_SetLimits :: proc(jointId: JointId, lower, upper: f32) --- + + // Enable/disable a prismatic joint motor + PrismaticJoint_EnableMotor :: proc(jointId: JointId, enableMotor: bool) --- + + // Is the prismatic joint motor enabled? + PrismaticJoint_IsMotorEnabled :: proc(jointId: JointId) -> bool --- + + // Set the prismatic joint motor speed, usually in meters per second + PrismaticJoint_SetMotorSpeed :: proc(jointId: JointId, motorSpeed: f32) --- + + // Get the prismatic joint motor speed, usually in meters per second + PrismaticJoint_GetMotorSpeed :: proc(jointId: JointId) -> f32 --- + + // Set the prismatic joint maximum motor force, usually in newtons + PrismaticJoint_SetMaxMotorForce :: proc(jointId: JointId, force: f32) --- + + // Get the prismatic joint maximum motor force, usually in newtons + PrismaticJoint_GetMaxMotorForce :: proc(jointId: JointId) -> f32 --- + + // Get the prismatic joint current motor force, usually in newtons + PrismaticJoint_GetMotorForce :: proc(jointId: JointId) -> f32 --- + + // Get the current joint translation, usually in meters. + PrismaticJoint_GetTranslation :: proc(jointId: JointId) -> f32 --- + + // Get the current joint translation speed, usually in meters per second. + PrismaticJoint_GetSpeed :: proc(jointId: JointId) -> f32 --- + + /**@}*/ // prismatic_joint + + /** + * @defgroup revolute_joint Revolute Joint + * @brief A revolute joint allows for relative rotation about a single axis with no relative translation. + * + * Also called a *hinge* or *pin* joint. + * @{ + */ + + // Create a revolute joint + // @see RevoluteJointDef for details + CreateRevoluteJoint :: proc(worldId: WorldId, #by_ptr def: RevoluteJointDef) -> JointId --- + + // Enable/disable the revolute joint spring + RevoluteJoint_EnableSpring :: proc(jointId: JointId, enableSpring: bool) --- + + // Is the revolute angular spring enabled? + RevoluteJoint_IsSpringEnabled :: proc(jointId: JointId) -> bool --- + + // Set the revolute joint spring stiffness in Hertz + RevoluteJoint_SetSpringHertz :: proc(jointId: JointId, hertz: f32) --- + + // Get the revolute joint spring stiffness in Hertz + RevoluteJoint_GetSpringHertz :: proc(jointId: JointId) -> f32 --- + + // Set the revolute joint spring damping ratio, non-dimensional + RevoluteJoint_SetSpringDampingRatio :: proc(jointId: JointId, dampingRatio: f32) --- + + // Get the revolute joint spring damping ratio, non-dimensional + RevoluteJoint_GetSpringDampingRatio :: proc(jointId: JointId) -> f32 --- + + // Set the revolute joint target angle in radians + RevoluteJoint_SetTargetAngle :: proc(jointId: JointId, targetRadians: f32) --- + + // Get the revolute joint target angle in radians + RevoluteJoint_GetTargetAngle :: proc(jointId: JointId) -> f32 --- + + // Get the revolute joint current angle in radians relative to the reference angle + // @see RevoluteJointDef::referenceAngle + RevoluteJoint_GetAngle :: proc(jointId: JointId) -> f32 --- + + // Enable/disable the revolute joint limit + RevoluteJoint_EnableLimit :: proc(jointId: JointId, enableLimit: bool) --- + + // Is the revolute joint limit enabled? + RevoluteJoint_IsLimitEnabled :: proc(jointId: JointId) -> bool --- + + // Get the revolute joint lower limit in radians + RevoluteJoint_GetLowerLimit :: proc(jointId: JointId) -> f32 --- + + // Get the revolute joint upper limit in radians + RevoluteJoint_GetUpperLimit :: proc(jointId: JointId) -> f32 --- + + // Set the revolute joint limits in radians + RevoluteJoint_SetLimits :: proc(jointId: JointId, lowerLimitRadians, upperLimitRadians: f32) --- + + // Enable/disable a revolute joint motor + RevoluteJoint_EnableMotor :: proc(jointId: JointId, enableMotor: bool) --- + + // Is the revolute joint motor enabled? + RevoluteJoint_IsMotorEnabled :: proc(jointId: JointId) -> bool --- + + // Set the revolute joint motor speed in radians per second + RevoluteJoint_SetMotorSpeed :: proc(jointId: JointId, motorSpeed: f32) --- + + // Get the revolute joint motor speed in radians per second + RevoluteJoint_GetMotorSpeed :: proc(jointId: JointId) -> f32 --- + + // Get the revolute joint current motor torque, usually in newton-meters + RevoluteJoint_GetMotorTorque :: proc(jointId: JointId) -> f32 --- + + // Set the revolute joint maximum motor torque, usually in newton-meters + RevoluteJoint_SetMaxMotorTorque :: proc(jointId: JointId, torque: f32) --- + + // Get the revolute joint maximum motor torque, usually in newton-meters + RevoluteJoint_GetMaxMotorTorque :: proc(jointId: JointId) -> f32 --- + + /**@}*/ // revolute_joint + + /** + * @defgroup spherical_joint Spherical Joint + * @brief A spherical joint allows for relative rotation in the 3D space with no relative translation. + * + * Also called a *ball-in-socket* or *point-to-point* joint. + * @{ + */ + + // Create a spherical joint + // @see SphericalJointDef for details + CreateSphericalJoint :: proc(worldId: WorldId, #by_ptr def: SphericalJointDef) -> JointId --- + + // Enable/disable the spherical joint cone limit + SphericalJoint_EnableConeLimit :: proc(jointId: JointId, enableLimit: bool) --- + + // Is the spherical joint cone limit enabled? + SphericalJoint_IsConeLimitEnabled :: proc(jointId: JointId) -> bool --- + + // Get the spherical joint cone limit in radians + SphericalJoint_GetConeLimit :: proc(jointId: JointId) -> f32 --- + + // Set the spherical joint limits in radians + SphericalJoint_SetConeLimit :: proc(jointId: JointId, angleRadians: f32) --- + + // Get the spherical joint current cone angle in radians. + SphericalJoint_GetConeAngle :: proc(jointId: JointId) -> f32 --- + + // Enable/disable the spherical joint limit + SphericalJoint_EnableTwistLimit :: proc(jointId: JointId, enableLimit: bool) --- + + // Is the spherical joint limit enabled? + SphericalJoint_IsTwistLimitEnabled :: proc(jointId: JointId) -> bool --- + + // Get the spherical joint lower limit in radians + SphericalJoint_GetLowerTwistLimit :: proc(jointId: JointId) -> f32 --- + + // Get the spherical joint upper limit in radians + SphericalJoint_GetUpperTwistLimit :: proc(jointId: JointId) -> f32 --- + + // Set the spherical joint limits in radians + SphericalJoint_SetTwistLimits :: proc(jointId: JointId, lowerLimitRadians, upperLimitRadians: f32) --- + + // Get the spherical joint current twist angle in radians. + SphericalJoint_GetTwistAngle :: proc(jointId: JointId) -> f32 --- + + // Enable/disable the spherical joint spring + SphericalJoint_EnableSpring :: proc(jointId: JointId, enableSpring: bool) --- + + // Is the spherical angular spring enabled? + SphericalJoint_IsSpringEnabled :: proc(jointId: JointId) -> bool --- + + // Set the spherical joint spring stiffness in Hertz + SphericalJoint_SetSpringHertz :: proc(jointId: JointId, hertz: f32) --- + + // Get the spherical joint spring stiffness in Hertz + SphericalJoint_GetSpringHertz :: proc(jointId: JointId) -> f32 --- + + // Set the spherical joint spring damping ratio, non-dimensional + SphericalJoint_SetSpringDampingRatio :: proc(jointId: JointId, dampingRatio: f32) --- + + // Get the spherical joint spring damping ratio, non-dimensional + SphericalJoint_GetSpringDampingRatio :: proc(jointId: JointId) -> f32 --- + + // Set the spherical joint spring target rotation + SphericalJoint_SetTargetRotation :: proc(jointId: JointId, targetRotation: Quat) --- + + // Get the spherical joint spring target rotation + SphericalJoint_GetTargetRotation :: proc(jointId: JointId) -> Quat --- + + // Enable/disable a spherical joint motor + SphericalJoint_EnableMotor :: proc(jointId: JointId, enableMotor: bool) --- + + // Is the spherical joint motor enabled? + SphericalJoint_IsMotorEnabled :: proc(jointId: JointId) -> bool --- + + // Set the spherical joint motor velocity in radians per second + SphericalJoint_SetMotorVelocity :: proc(jointId: JointId, motorVelocity: Vec3) --- + + // Get the spherical joint motor velocity in radians per second + SphericalJoint_GetMotorVelocity :: proc(jointId: JointId) -> Vec3 --- + + // Get the spherical joint current motor torque, usually in newton-meters + SphericalJoint_GetMotorTorque :: proc(jointId: JointId) -> Vec3 --- + + // Set the spherical joint maximum motor torque, usually in newton-meters + SphericalJoint_SetMaxMotorTorque :: proc(jointId: JointId, torque: f32) --- + + // Get the spherical joint maximum motor torque, usually in newton-meters + SphericalJoint_GetMaxMotorTorque :: proc(jointId: JointId) -> f32 --- + + /**@}*/ // spherical_joint + + /** + * @defgroup weld_joint Weld Joint + * @brief A weld joint fully constrains the relative transform between two bodies while allowing for springiness + * + * A weld joint constrains the relative rotation and translation between two bodies. Both rotation and translation + * can have damped springs. + * + * @note The accuracy of weld joint is limited by the accuracy of the solver. Long chains of weld joints may flex. + * @{ + */ + + // Create a weld joint + // @see WeldJointDef for details + CreateWeldJoint :: proc(worldId: WorldId, #by_ptr def: WeldJointDef) -> JointId --- + + // Set the weld joint linear stiffness in Hertz. 0 is rigid. + WeldJoint_SetLinearHertz :: proc(jointId: JointId, hertz: f32) --- + + // Get the weld joint linear stiffness in Hertz + WeldJoint_GetLinearHertz :: proc(jointId: JointId) -> f32 --- + + // Set the weld joint linear damping ratio (non-dimensional) + WeldJoint_SetLinearDampingRatio :: proc(jointId: JointId, dampingRatio: f32) --- + + // Get the weld joint linear damping ratio (non-dimensional) + WeldJoint_GetLinearDampingRatio :: proc(jointId: JointId) -> f32 --- + + // Set the weld joint angular stiffness in Hertz. 0 is rigid. + WeldJoint_SetAngularHertz :: proc(jointId: JointId, hertz: f32) --- + + // Get the weld joint angular stiffness in Hertz + WeldJoint_GetAngularHertz :: proc(jointId: JointId) -> f32 --- + + // Set weld joint angular damping ratio, non-dimensional + WeldJoint_SetAngularDampingRatio :: proc(jointId: JointId, dampingRatio: f32) --- + + // Get the weld joint angular damping ratio, non-dimensional + WeldJoint_GetAngularDampingRatio :: proc(jointId: JointId) -> f32 --- + + /**@}*/ // weld_joint + + /** + * @defgroup wheel_joint Wheel Joint + * The wheel joint can be used to simulate wheels on vehicles. + * + * The wheel joint restricts body B to move along a local axis in body A. Body B is free to + * rotate. Supports a linear spring, linear limits, and a rotational motor. + * + * @{ + */ + + // Create a wheel joint. + // @see WheelJointDef for details. + CreateWheelJoint :: proc(worldId: WorldId, #by_ptr def: WheelJointDef) -> JointId --- + + // Enable/disable the wheel joint spring. + WheelJoint_EnableSuspension :: proc(jointId: JointId, flag: bool) --- + + // Is the wheel joint spring enabled? + WheelJoint_IsSuspensionEnabled :: proc(jointId: JointId) -> bool --- + + // Set the wheel joint stiffness in Hertz. + WheelJoint_SetSuspensionHertz :: proc(jointId: JointId, hertz: f32) --- + + // Get the wheel joint stiffness in Hertz. + WheelJoint_GetSuspensionHertz :: proc(jointId: JointId) -> f32 --- + + // Set the wheel joint damping ratio, non-dimensional. + WheelJoint_SetSuspensionDampingRatio :: proc(jointId: JointId, dampingRatio: f32) --- + + // Get the wheel joint damping ratio, non-dimensional. + WheelJoint_GetSuspensionDampingRatio :: proc(jointId: JointId) -> f32 --- + + // Enable/disable the wheel joint limit. + WheelJoint_EnableSuspensionLimit :: proc(jointId: JointId, flag: bool) --- + + // Is the wheel joint limit enabled? + WheelJoint_IsSuspensionLimitEnabled :: proc(jointId: JointId) -> bool --- + + // Get the wheel joint lower limit. + WheelJoint_GetLowerSuspensionLimit :: proc(jointId: JointId) -> f32 --- + + // Get the wheel joint upper limit. + WheelJoint_GetUpperSuspensionLimit :: proc(jointId: JointId) -> f32 --- + + // Set the wheel joint limits. + WheelJoint_SetSuspensionLimits :: proc(jointId: JointId, lower, upper: f32) --- + + // Enable/disable the wheel joint motor. + WheelJoint_EnableSpinMotor :: proc(jointId: JointId, flag: bool) --- + + // Is the wheel joint motor enabled? + WheelJoint_IsSpinMotorEnabled :: proc(jointId: JointId) -> bool --- + + // Set the wheel joint motor speed in radians per second. + WheelJoint_SetSpinMotorSpeed :: proc(jointId: JointId, speed: f32) --- + + // Get the wheel joint motor speed in radians per second. + WheelJoint_GetSpinMotorSpeed :: proc(jointId: JointId) -> f32 --- + + // Set the wheel joint maximum motor torque, usually in newton-meters. + WheelJoint_SetMaxSpinTorque :: proc(jointId: JointId, torque: f32) --- + + // Get the wheel joint maximum motor torque, usually in newton-meters. + WheelJoint_GetMaxSpinTorque :: proc(jointId: JointId) -> f32 --- + + // Get the current spin speed in radians per second. + WheelJoint_GetSpinSpeed :: proc(jointId: JointId) -> f32 --- + + // Get the wheel joint current motor torque, usually in newton-meters. + WheelJoint_GetSpinTorque :: proc(jointId: JointId) -> f32 --- + + // Enable/disable wheel steering. Steering allows the wheel to rotate about the suspension axis. + WheelJoint_EnableSteering :: proc(jointId: JointId, flag: bool) --- + + // Can the wheel steer? + WheelJoint_IsSteeringEnabled :: proc(jointId: JointId) -> bool --- + + // Set the wheel joint steering stiffness in Hertz. + WheelJoint_SetSteeringHertz :: proc(jointId: JointId, hertz: f32) --- + + // Get the wheel joint steering stiffness in Hertz. + WheelJoint_GetSteeringHertz :: proc(jointId: JointId) -> f32 --- + + // Set the wheel joint steering damping ratio, non-dimensional. + WheelJoint_SetSteeringDampingRatio :: proc(jointId: JointId, dampingRatio: f32) --- + + // Get the wheel joint steering damping ratio, non-dimensional. + WheelJoint_GetSteeringDampingRatio :: proc(jointId: JointId) -> f32 --- + + // Set the wheel joint maximum steering torque in N*m. + WheelJoint_SetMaxSteeringTorque :: proc(jointId: JointId, torque: f32) --- + + // Get the wheel joint maximum steering torque in N*m. + WheelJoint_GetMaxSteeringTorque :: proc(jointId: JointId) -> f32 --- + + // Enable/disable the wheel joint steering limit. + WheelJoint_EnableSteeringLimit :: proc(jointId: JointId, flag: bool) --- + + // Is the wheel joint steering limit enabled? + WheelJoint_IsSteeringLimitEnabled :: proc(jointId: JointId) -> bool --- + + // Get the wheel joint lower steering limit in radians. + WheelJoint_GetLowerSteeringLimit :: proc(jointId: JointId) -> f32 --- + + // Get the wheel joint upper steering limit in radians. + WheelJoint_GetUpperSteeringLimit :: proc(jointId: JointId) -> f32 --- + + // Set the wheel joint steering limits in radians. + WheelJoint_SetSteeringLimits :: proc(jointId: JointId, lowerRadians, upperRadians: f32) --- + + // Set the wheel joint target steering angle in radians. + WheelJoint_SetTargetSteeringAngle :: proc(jointId: JointId, radians: f32) --- + + // Get the wheel joint target steering angle in radians. + WheelJoint_GetTargetSteeringAngle :: proc(jointId: JointId) -> f32 --- + + // Get the current steering angle in radians. + WheelJoint_GetSteeringAngle :: proc(jointId: JointId) -> f32 --- + + // Get the current steering torque in N*m. + WheelJoint_GetSteeringTorque :: proc(jointId: JointId) -> f32 --- + + /**@}*/ // wheel_joint + + /**@}*/ // joint + + /** + * @defgroup contact Contact + * Access to contacts + * @{ + */ + + // Contact identifier validation. Provides validation for up to 2^32 allocations. + Contact_IsValid :: proc(id: ContactId) -> bool --- + + // Get the manifolds for a contact. The manifold may have no points if the contact is not touching. + Contact_GetData :: proc(contactId: ContactId) -> ContactData --- + + /**@}*/ // contact +} diff --git a/vendor/box3d/box3d_collision.odin b/vendor/box3d/box3d_collision.odin new file mode 100644 index 000000000..e80584f7a --- /dev/null +++ b/vendor/box3d/box3d_collision.odin @@ -0,0 +1,616 @@ +package vendor_box3d + +import "core:c" + +// Query callback. +MeshQueryFcn :: proc "c" (x, y, z: Vec3, triangleIndex: c.int, ctx: rawptr) -> bool + + +@(link_prefix="b3", default_calling_convention="c", require_results) +foreign lib { + /** + * @addtogroup tree + * @{ + */ + + // Constructing the tree initializes the node pool. + DynamicTree_Create :: proc(proxyCapacity: c.int) -> DynamicTree --- + + // Destroy the tree, freeing the node pool. + DynamicTree_Destroy :: proc(tree: ^DynamicTree) --- + + // Create a proxy. Provide an AABB and a userData value. + DynamicTree_CreateProxy :: proc(tree: ^DynamicTree, aabb: AABB, categoryBits: u64, userData: u64) -> c.int --- + + // Destroy a proxy. This asserts if the id is invalid. + DynamicTree_DestroyProxy :: proc(tree: ^DynamicTree, proxyId: c.int) --- + + // Move a proxy to a new AABB by removing and reinserting into the tree. + DynamicTree_MoveProxy :: proc(tree: ^DynamicTree, proxyId: c.int, aabb: AABB) --- + + // Enlarge a proxy and enlarge ancestors as necessary. + DynamicTree_EnlargeProxy :: proc(tree: ^DynamicTree, proxyId: c.int, aabb: AABB) --- + + // Modify the category bits on a proxy. This is an expensive operation. + DynamicTree_SetCategoryBits :: proc(tree: ^DynamicTree, proxyId: c.int, categoryBits: u64) --- + + // Get the category bits on a proxy. + DynamicTree_GetCategoryBits :: proc(tree: ^DynamicTree, proxyId: c.int) -> u64 --- + + // Query an AABB for overlapping proxies. The callback function is called for each proxy that overlaps the supplied AABB. + // @return performance data + DynamicTree_Query :: proc(#by_ptr tree: DynamicTree, aabb: AABB, maskBits: u64, requireAllBits: bool, callback: TreeQueryCallbackFcn, ctx: rawptr) -> TreeStats --- + + // Query an AABB for the closest object. The callback function is called for each proxy that might be closest to the supplied point. + // @param tree the dynamic tree to query + // @param point the query point + // @param maskBits nodes are skipped if the bit-wise AND with the node category bits is zero + // @param requireAllBits nodes are skipped if the bit-wise AND with the node category bits does not equal the maskBits + // @param callback a user provided instance of TreeQueryClosestCallbackFcn + // @param context a user context object that is provided to the callback + // @param minDistanceSqr the initial and final minimum squared distance. Provide a small initial to restrict the search and + // improve performance. If the value is large this query has performance that scales linearly with the number of proxies and + // would be slower than a brute force search. + // @return performance data + DynamicTree_QueryClosest :: proc(#by_ptr tree: DynamicTree, point: Vec3, maskBits: u64, requireAllBits: bool, callback: TreeQueryClosestCallbackFcn, ctx: rawptr, minDistanceSqr: ^f32) -> TreeStats --- + + // Ray cast against the proxies in the tree. This relies on the callback + // to perform an exact ray cast in the case where the proxy contains a shape. + // The callback also performs any collision filtering. This has performance + // roughly equal to k * log(n), where k is the number of collisions and n is the + // number of proxies in the tree. + // Bit-wise filtering using mask bits can greatly improve performance in some scenarios. + // However, this filtering may be approximate, so the user should still apply filtering to results. + // @param tree the dynamic tree to ray cast + // @param input the ray cast input data. The ray extends from p1 to p1 + maxFraction * (p2 - p1) + // @param maskBits bit mask test: `bool accept = (maskBits & node->categoryBits) != 0;` + // @param requireAllBits modifies bit mask test: `bool accept = (maskBits & node->categoryBits) == maskBits;` + // @param callback a callback function that is called for each proxy that is hit by the ray + // @param context user context that is passed to the callback + // @return performance data + DynamicTree_RayCast :: proc(#by_ptr tree: DynamicTree, #by_ptr input: RayCastInput, maskBits: u64, requireAllBits: bool, callback: TreeRayCastCallbackFcn, ctx: rawptr) -> TreeStats --- + + // Sweep an AABB through the tree. The box is in the tree's world float frame and the callback + // re-differences each shape at full precision aganist the query origin. Used by the large world + // spatial queries so the tree traversal stays float while the narrow phase stays precise. + DynamicTree_BoxCast :: proc(#by_ptr tree: DynamicTree, #by_ptr input: BoxCastInput, maskBits: u64, requireAllBits: bool, callback: TreeBoxCastCallbackFcn, ctx: rawptr) -> TreeStats --- + + // Validate this tree. For testing. + DynamicTree_Validate :: proc(#by_ptr tree: DynamicTree) --- + + // Get the height of the binary tree. + DynamicTree_GetHeight :: proc(#by_ptr tree: DynamicTree) -> c.int --- + + // Get the ratio of the sum of the node areas to the root area. + DynamicTree_GetAreaRatio :: proc(#by_ptr tree: DynamicTree) -> f32 --- + + // Get the bounding box that contains the entire tree + DynamicTree_GetRootBounds :: proc(#by_ptr tree: DynamicTree) -> AABB --- + + // Get the number of proxies created + DynamicTree_GetProxyCount :: proc(#by_ptr tree: DynamicTree) -> c.int --- + + // Rebuild the tree while retaining subtrees that haven't changed. Returns the number of boxes sorted. + DynamicTree_Rebuild :: proc(tree: ^DynamicTree, fullBuild: bool) -> c.int --- + + // Get the number of bytes used by this tree + DynamicTree_GetByteCount :: proc(#by_ptr tree: DynamicTree) -> c.int --- + + // Validate this tree has no enlarged AABBs. For testing. + DynamicTree_ValidateNoEnlarged :: proc(#by_ptr tree: DynamicTree) --- + + // Save this tree to a file for debugging + DynamicTree_Save :: proc(#by_ptr tree: DynamicTree, fileName: cstring) --- + + // Load a file for debugging + DynamicTree_Load :: proc(fileName: cstring, scale: f32) -> DynamicTree --- + + + /**@}*/ // tree + + /** + * @addtogroup hull + * @{ + */ + + // Create a tessellated cylinder as a hull. + CreateCylinder :: proc(height: f32, radius: f32, yOffset: f32, sides: c.int) -> ^HullData --- + + // Create a tessellated cone as a hull. + CreateCone :: proc(height: f32, radius1: f32, radius2: f32, slices: c.int) -> ^HullData --- + + // Create a rock shaped hull. + CreateRock :: proc(radius: f32) -> ^HullData --- + + // Create a generic convex hull. + CreateHull :: proc(points: [^]Vec3, pointCount: c.int, maxVertexCount: c.int) -> ^HullData --- + + // Deep clone a hull. + CloneHull :: proc(#by_ptr hull: HullData) -> ^HullData --- + + // Clone and transform a hull. Supports non-uniform and mirroring scale. + CloneAndTransformHull :: proc(#by_ptr original: HullData, transform: Transform, scale: Vec3) -> ^HullData --- + + // Destroy a hull. + DestroyHull :: proc(hull: ^HullData) --- + + // Make a cube as a hull. Do not call DestroyHull on this. + MakeCubeHull :: proc(halfWidth: f32) -> BoxHull --- + + // Make a box as a hull. Do not call DestroyHull on this. + MakeBoxHull :: proc(hx, hy, hz: f32) -> BoxHull --- + + // Make an offset box as a hull. Do not call DestroyHull on this. + MakeOffsetBoxHull :: proc(hx, hy, hz: f32, offset: Vec3) -> BoxHull --- + + // Make a transformed box as a hull. Do not call DestroyHull on this. + // @param hx, hy, hz positive half widths + // @param transform local transform of box + MakeTransformedBoxHull :: proc(hx, hy, hz: f32, transform: Transform) -> BoxHull --- + + // This makes a transformed box hull with post scaling. This is useful for boxes that are scaled in + // a level editor. Such scaling can have reflection and shear. In the case of shear the result + // may be approximate. If you need to support shear consider using CreateHull. + // Do not call DestroyHull on this. + // @param halfWidths positive half widths + // @param transform local transform of box + // @param postScale scale applied after the transform, may be negative + MakeScaledBoxHull :: proc(halfWidths: Vec3, transform: Transform, postScale: Vec3) -> BoxHull --- + + // This takes a box with a transform and post scale and converts it into a box with the post scale + // resolved with new half-widths and transform. This accepts non-uniform and negative scale. + // This is approximate if there is shear. + // @param halfWidths [in/out] the box half widths + // @param transform [in/out] the box transform with rotation and translation + // @param postScale the post scale being applied to the box after the transform + // @param minHalfWidth the minimum half width after scale is applied + ScaleBox :: proc(halfWidths: ^Vec3, transform: ^Transform, postScale: Vec3, minHalfWidth: f32) --- + + /**@}*/ // hull + + /** + * @addtogroup mesh + * @{ + */ + + + // Create a grid mesh along the x and z axes. + // @param xCount the number of rows in the x direction + // @param zCount the number of rows in the z direction + // @param cellWidth the width of each cell + // @param materialCount the number of materials to generate + // @param identifyEdges compute adjacency information + CreateGridMesh :: proc(xCount: c.int, zCount: c.int, cellWidth: f32, materialCount: c.int, identifyEdges: bool) -> ^MeshData --- + + // Create a wave mesh along the x and z axes. + CreateWaveMesh :: proc(xCount: c.int, zCount: c.int, cellWidth: f32, amplitude: f32, rowFrequency: f32, columnFrequency: f32) -> ^MeshData --- + + // Create a torus mesh. + CreateTorusMesh :: proc(radialResolution: c.int, tubularResolution: c.int, radius: f32, thickness: f32) -> ^MeshData --- + + // Create a box mesh. + CreateBoxMesh :: proc(center: Vec3, extent: Vec3, identifyEdges: bool) -> ^MeshData --- + + // Create a hollow box mesh. + CreateHollowBoxMesh :: proc(center: Vec3, extent: Vec3) -> ^MeshData --- + + // Create a platform mesh. A truncated pyramid. + CreatePlatformMesh :: proc(center: Vec3, height, topWidth, bottomWidth: f32) -> ^MeshData --- + + // Create a generic mesh. + CreateMesh :: proc(#by_ptr def: MeshDef, degenerateTriangleIndices: [^]c.int, degenerateCapacity: c.int) -> ^MeshData --- + + // Destroy a mesh. + DestroyMesh :: proc(mesh: ^MeshData) --- + + // Get the height of the mesh BVH. + GetHeight :: proc(#by_ptr mesh: MeshData) -> c.int --- + + /**@}*/ // mesh + + /** + * @addtogroup height_field + * @{ + */ + + // Create a generic height field. + CreateHeightField :: proc(#by_ptr data: HeightFieldDef) -> ^HeightFieldData --- + + // Create a grid as a height field. + CreateGrid :: proc(rowCount, columnCount: c.int, scale: Vec3, makeHoles: bool) -> ^HeightFieldData --- + + // Create a wave grid as a height field. + CreateWave :: proc(rowCount, columnCount: c.int, scale: Vec3, rowFrequency: f32, columnFrequency: f32, makeHoles: bool) -> ^HeightFieldData --- + + // Destroy a height field. + DestroyHeightField :: proc(heightField: ^HeightFieldData) --- + + // Save input height data to a file + DumpHeightData :: proc(#by_ptr data: HeightFieldDef, fileName: cstring) --- + + // Create a height field by loading a previously saved height data + LoadHeightField :: proc(fileName: cstring) -> ^HeightFieldData --- + + /**@}*/ // height_field + + /** + * @addtogroup compound + * @{ + */ + + // Get a child shape of a compound. + GetCompoundChild :: proc(#by_ptr compound: CompoundData, childIndex: c.int) -> ChildShape --- + + // Query a compound shape for children that overlap an AABB. + QueryCompound :: proc(#by_ptr compound: CompoundData, aabb: AABB, fcn: CompoundQueryFcn, ctx: rawptr) --- + + // Access a child capsule by index. + GetCompoundCapsule :: proc(#by_ptr compound: CompoundData, index: c.int) -> CompoundCapsule --- + + // Access a child hull by index. + GetCompoundHull :: proc(#by_ptr compound: CompoundData, index: c.int) -> CompoundHull --- + + // Access a child mesh by index. + GetCompoundMesh :: proc(#by_ptr compound: CompoundData, index: c.int) -> CompoundMesh --- + + // Access a child sphere by index. + GetCompoundSphere :: proc(#by_ptr compound: CompoundData, index: c.int) -> CompoundSphere --- + + // Access the compound material array. + GetCompoundMaterials :: proc(#by_ptr compound: CompoundData) -> ^SurfaceMaterial --- + + // Create a compound shape. All input data in the definition is cloned into the resulting compound. + CreateCompound :: proc(#by_ptr def: CompoundDef) -> ^CompoundData --- + + // Destroy a compound shape. + DestroyCompound :: proc(compound: ^CompoundData) --- + + // If bytes is null then this returns the number of required bytes. This clones all the + // data into the bytes buffer. This is expected to run offline or asynchronously. + // This mutates the compound to nullify pointers, leaving the compound in an unusable state. + ConvertCompoundToBytes :: proc(compound: ^CompoundData) -> [^]u8 --- + + // Convert bytes to compound. This does not clone. The bytes must remain in scope while the + // compound is used. This is done to improve run-time performance and allow for instancing. + // The bytes are mutated to fixup pointers. + ConvertBytesToCompound :: proc(bytes: [^]u8, byteCount: c.int) -> ^CompoundData --- + + /**@}*/ // compound + + /** + * @addtogroup geometry + * @{ + */ + + // Compute mass properties of a sphere + ComputeSphereMass :: proc(#by_ptr shape: Sphere, density: f32) -> MassData --- + + // Compute mass properties of a capsule + ComputeCapsuleMass :: proc(#by_ptr shape: Capsule, density: f32) -> MassData --- + + // Compute mass properties of a hull + ComputeHullMass :: proc(#by_ptr shape: HullData, density: f32) -> MassData --- + + // Compute the bounding box of a transformed sphere + ComputeSphereAABB :: proc(#by_ptr shape: Sphere, transform: Transform) -> AABB --- + + // Compute the bounding box of a transformed capsule + ComputeCapsuleAABB :: proc(#by_ptr shape: Capsule, transform: Transform) -> AABB --- + + // Compute the bounding box of a transformed hull + ComputeHullAABB :: proc(#by_ptr shape: HullData, transform: Transform) -> AABB --- + + // Compute the bounding box of a transformed mesh. Scale may be non-uniform and have negative components. + ComputeMeshAABB :: proc(#by_ptr shape: MeshData, transform: Transform, scale: Vec3) -> AABB --- + + // Compute the bounding box of a transformed height-field + ComputeHeightFieldAABB :: proc(#by_ptr shape: HeightFieldData, transform: Transform) -> AABB --- + + // Compute the bounding box of a compound + ComputeCompoundAABB :: proc(#by_ptr shape: CompoundData, transform: Transform) -> AABB --- + + /**@}*/ // geometry + + /** + * @addtogroup query + * @{ + */ + + // Use this to ensure your ray cast input is valid and avoid internal assertions. + IsValidRay :: proc(#by_ptr input: RayCastInput) -> bool --- + + // Overlap shape versus capsule + OverlapCapsule :: proc(#by_ptr shape: Capsule, shapeTransform: Transform, #by_ptr proxy: ShapeProxy) -> bool --- + + // Overlap shape versus compound + OverlapCompound :: proc(#by_ptr shape: CompoundData, shapeTransform: Transform, #by_ptr proxy: ShapeProxy) -> bool --- + + // Overlap shape versus height field + OverlapHeightField :: proc(#by_ptr shape: HeightFieldData, shapeTransform: Transform, #by_ptr proxy: ShapeProxy) -> bool --- + + // Overlap shape versus hull + OverlapHull :: proc(#by_ptr shape: HullData, shapeTransform: Transform, #by_ptr proxy: ShapeProxy) -> bool --- + + // Overlap shape versus mesh + OverlapMesh :: proc(#by_ptr shape: Mesh, shapeTransform: Transform, #by_ptr proxy: ShapeProxy) -> bool --- + + // Overlap shape versus sphere + OverlapSphere :: proc(#by_ptr shape: Sphere, shapeTransform: Transform, #by_ptr proxy: ShapeProxy) -> bool --- + + // Ray cast versus sphere in local space. A zero length ray is a point query. Initial overlap + // reports a hit at the ray origin with zero fraction and zero normal. + RayCastSphere :: proc(#by_ptr shape: Sphere, #by_ptr input: RayCastInput) -> CastOutput --- + + // Ray cast versus a hollow sphere shell in local space. Unlike the solid sphere a ray starting + // inside is not an overlap: it passes through and hits the far wall. + RayCastHollowSphere :: proc(#by_ptr shape: Sphere, #by_ptr input: RayCastInput) -> CastOutput --- + + // Ray cast versus capsule in local space. A zero length ray is a point query. Initial overlap + // reports a hit at the ray origin with zero fraction and zero normal. + RayCastCapsule :: proc(#by_ptr shape: Capsule, #by_ptr input: RayCastInput) -> CastOutput --- + + // Ray cast versus compound in local space. A zero length ray is a point query. Initial overlap + // with a child reports a hit at the ray origin with zero fraction and zero normal. + RayCastCompound :: proc(#by_ptr shape: CompoundData, #by_ptr input: RayCastInput) -> CastOutput --- + + // Ray cast versus hull shape in local space. A zero length ray is a point query. Initial overlap + // reports a hit at the ray origin with zero fraction and zero normal. + RayCastHull :: proc(#by_ptr shape: HullData, #by_ptr input: RayCastInput) -> CastOutput --- + + // Ray cast versus mesh in local space. A thin surface with no interior, so there is no overlap case. + RayCastMesh :: proc(#by_ptr shape: Mesh, #by_ptr input: RayCastInput) -> CastOutput --- + + // Ray cast versus height field in local space. A thin surface with no interior, so there is no overlap case. + RayCastHeightField :: proc(#by_ptr shape: HeightFieldData, #by_ptr input: RayCastInput) -> CastOutput --- + + // Shape cast versus a sphere. Initial overlap is treated as a miss. + ShapeCastSphere :: proc(#by_ptr shape: Sphere, #by_ptr input: ShapeCastInput) -> CastOutput --- + + // Shape cast versus a capsule. Initial overlap is treated as a miss. + ShapeCastCapsule :: proc(#by_ptr shape: Capsule, #by_ptr input: ShapeCastInput) -> CastOutput --- + + // Shape cast versus compound. Initial overlap is treated as a miss. + ShapeCastCompound :: proc(#by_ptr shape: CompoundData, #by_ptr input: ShapeCastInput) -> CastOutput --- + + // Shape cast versus a hull. Initial overlap is treated as a miss. + ShapeCastHull :: proc(#by_ptr shape: HullData, #by_ptr input: ShapeCastInput) -> CastOutput --- + + // Shape cast versus a mesh. Initial overlap is treated as a miss. + ShapeCastMesh :: proc(#by_ptr shape: Mesh, #by_ptr input: ShapeCastInput) -> CastOutput --- + + // Shape cast versus a height field. Initial overlap is treated as a miss. + ShapeCastHeightField :: proc(#by_ptr shape: HeightFieldData, #by_ptr input: ShapeCastInput) -> CastOutput --- + + + // Query a mesh for triangles overlapping a bounding box in local space. May have false positives. Useful for debug draw. + // @param mesh the mesh to query, includes scale + // @param bounds the bounding box in local space + // @param fcn a user function to collect triangles + // @param context the context sent to the user function. + QueryMesh :: proc(#by_ptr mesh: Mesh, bounds: AABB, fcn: MeshQueryFcn, ctx: rawptr) --- + + // Query a height field for triangles overlapping a bounding box in local space. May have false positives. Useful for debug draw. + // @param heightField the height field to query + // @param bounds the bounding box in local space + // @param fcn a user function to collect triangles + // @param context the context sent to the user function. + QueryHeightField :: proc(#by_ptr heightField: HeightFieldData, bounds: AABB, fcn: MeshQueryFcn, ctx: rawptr) --- + + // Compute the closest points between two shapes represented as point clouds. + // SimplexCache cache is input/output. On the first call set SimplexCache.count to zero. + // The query runs in frame A, so the witness points and normal are returned in frame A. + // The underlying GJK algorithm may be debugged by passing in debug simplexes and capacity. You may pass in NULL and 0 for these. + ShapeDistance :: proc(#by_ptr input: DistanceInput, cache: ^SimplexCache, simplexes: [^]Simplex, simplexCapacity: c.int) -> DistanceOutput --- + + // Perform a linear shape cast of shape B moving and shape A fixed. Determines the hit point, normal, and translation fraction. + // The query runs in frame A, so the hit point and normal are returned in frame A. Initially touching shapes are a miss. + ShapeCast :: proc(#by_ptr input: ShapeCastPairInput) -> CastOutput --- + + // Evaluate the transform sweep at a specific time. + GetSweepTransform :: proc(#by_ptr sweep: Sweep, time: f32) -> Transform --- + + // Compute the upper bound on time before two shapes penetrate. Time is represented as + // a fraction between [0,tMax]. This uses a swept separating axis and may miss some intermediate, + // non-tunneling collisions. If you change the time interval, you should call this function + // again. + TimeOfImpact :: proc(#by_ptr input: TOIInput) -> TOIOutput --- + + /**@}*/ // query + + /** + * @addtogroup collision + * @{ + */ + + // Collide two spheres. + CollideSpheres :: proc(manifold: ^LocalManifold, capacity: c.int, #by_ptr sphereA, sphereB: Sphere, transformBtoA: Transform) --- + + // Collide a capsule and a sphere. + CollideCapsuleAndSphere :: proc(manifold: ^LocalManifold, capacity: c.int, #by_ptr capsuleA: Capsule, #by_ptr sphereB: Sphere, transformBtoA: Transform) --- + + // Collide a hull and a sphere. + CollideHullAndSphere :: proc(manifold: ^LocalManifold, capacity: c.int, #by_ptr hullA: HullData, #by_ptr sphereB: Sphere, transformBtoA: Transform, cache: ^SimplexCache) --- + + // Collide two capsules. + CollideCapsules :: proc(manifold: ^LocalManifold, capacity: c.int, #by_ptr capsuleA, capsuleB: Capsule, transformBtoA: Transform) --- + + // Collide a hull and a capsule. + CollideHullAndCapsule :: proc(manifold: ^LocalManifold, capacity: c.int, #by_ptr hullA: HullData, #by_ptr capsuleB: Capsule, transformBtoA: Transform, cache: ^SimplexCache) --- + + // Collide two hulls. + CollideHulls :: proc(manifold: ^LocalManifold, capacity: c.int, #by_ptr hullA: HullData, #by_ptr hullB: HullData, transformBtoA: Transform, cache: ^SATCache) --- + + // Collide a capsule and a triangle. + CollideCapsuleAndTriangle :: proc(manifold: ^LocalManifold, capacity: c.int, #by_ptr capsuleA: Capsule, #by_ptr triangleB: [3]Vec3, cache: ^SimplexCache) --- + + // Collide a hull and a triangle. + CollideHullAndTriangle :: proc(manifold: ^LocalManifold, capacity: c.int, #by_ptr hullA: HullData, v1, v2, v3: Vec3, + triangleFlags: c.int, cache: ^SATCache, enableSpeculative: bool) --- + + // Collide a sphere and a triangle. + CollideSphereAndTriangle :: proc(manifold: ^LocalManifold, capacity: c.int, #by_ptr sphereA: Sphere, #by_ptr triangleB: [3]Vec3) --- + + /**@}*/ // collision + + /** + * @addtogroup character + * @{ + */ + + // Solves the position of a mover that satisfies the given collision planes. + // @param targetDelta the desired translation from the position used to generate the collision planes + // @param planes the collision planes + // @param count the number of collision planes + SolvePlanes :: proc(targetDelta: Vec3, planes: [^]CollisionPlane, count: c.int) -> PlaneSolverResult --- + + // Clips the velocity against the given collision planes. Planes with zero push or clipVelocity + // set to false are skipped. + ClipVector :: proc(vector: Vec3, planes: [^]CollisionPlane, count: c.int) -> Vec3 --- + + /**@}*/ // character +} + +// Get proxy user data +@(require_results) +DynamicTree_GetUserData :: proc "c" (#by_ptr tree: DynamicTree, proxyId: c.int) -> u64 { + return tree.nodes[proxyId].userData +} + +// Get the AABB of a proxy +@(require_results) +DynamicTree_GetAABB :: proc "c" (#by_ptr tree: DynamicTree, proxyId: c.int) -> AABB { + return tree.nodes[proxyId].aabb +} + + +// Get read only hull vertices. +@(require_results) +GetHullVertices :: proc "c" (hull: ^HullData) -> Maybe(^HullVertex) { + if hull.vertexOffset == 0 { + return nil + } + + return (^HullVertex)(uintptr(hull) + uintptr(hull.vertexOffset)) +} + +// Get read only hull points. +@(require_results) +GetHullPoints :: proc "c" (hull: ^HullData) -> Maybe(^Vec3) { + if hull.pointOffset == 0 { + return nil + } + + return (^Vec3)(uintptr(hull) + uintptr(hull.pointOffset)) +} + +// Get read only hull half edges. +@(require_results) +GetHullEdges :: proc "c" (hull: ^HullData) -> Maybe(^HullHalfEdge) { + if hull.edgeOffset == 0 { + return nil + } + + return (^HullHalfEdge)(uintptr(hull) + uintptr(hull.edgeOffset)) +} + +// Get read only hull faces. +@(require_results) +GetHullFaces :: proc "c" (hull: ^HullData) -> Maybe(^HullFace) { + if hull.faceOffset == 0 { + return nil + } + + return (^HullFace)(uintptr(hull) + uintptr(hull.faceOffset)) +} + +// Get read only hull planes. +@(require_results) +GetHullPlanes :: proc "c" (hull: ^HullData) -> Maybe(^Plane) { + if hull.planeOffset == 0 { + return nil + } + + return (^Plane)(uintptr(hull) + uintptr(hull.planeOffset)) +} + + +// Get read only mesh BVH nodes. +@(require_results) +GetMeshNodes :: proc "c" (mesh: ^MeshData) -> Maybe(^MeshNode) { + if mesh.nodeOffset == 0 { + return nil + } + + return (^MeshNode)(uintptr(mesh) + uintptr(mesh.nodeOffset)) +} + +// Get read only mesh vertices. +@(require_results) +GetMeshVertices :: proc "c" (mesh: ^MeshData) -> Maybe(^Vec3) { + if mesh.vertexOffset == 0 { + return nil + } + + return (^Vec3)(uintptr(mesh) + uintptr(mesh.vertexOffset)) +} + +// Get read only mesh triangles. +@(require_results) +GetMeshTriangles :: proc "c" (mesh: ^MeshData) -> Maybe(^MeshTriangle) { + if mesh.triangleOffset == 0 { + return nil + } + + return (^MeshTriangle)(uintptr(mesh) + uintptr(mesh.triangleOffset)) +} + +// Get read only mesh materials. The count is equal to the triangle count. +@(require_results) +GetMeshMaterialIndices :: proc "c" (mesh: ^MeshData) -> Maybe([^]u8) { + if mesh.materialOffset == 0 { + return nil + } + + return ([^]u8)(uintptr(mesh) + uintptr(mesh.materialOffset)) +} + +// Get read only mesh flags. The count is equal to the triangle count. +@(require_results) +GetMeshFlags :: proc "c" (mesh: ^MeshData) -> [^]u8 { + if mesh.flagsOffset == 0 { + return nil + } + + return ([^]u8)(uintptr(mesh) + uintptr(mesh.flagsOffset)) +} + + +// Get read only compressed heights. One u16 per grid point. +@(require_results) +GetHeightFieldCompressedHeights :: proc "c" (hf: ^HeightFieldData) -> [^]u16 { + if hf.heightsOffset == 0 { + return nil + } + + return ([^]u16)(uintptr(hf) + uintptr(hf.heightsOffset)) +} + +// Get read only material indices. One u8 per cell. +@(require_results) +GetHeightFieldMaterialIndices :: proc "c" (hf: ^HeightFieldData) -> [^]u8 { + if hf.materialOffset == 0 { + return nil + } + + return ([^]u8)(uintptr(hf) + uintptr(hf.materialOffset)) +} + +// Get read only triangle flags. One u8 per triangle. +@(require_results) +GetHeightFieldFlags :: proc "c" (hf: ^HeightFieldData) -> [^]u8 { + if hf.flagsOffset == 0 { + return nil + } + + return ([^]u8)(uintptr(hf) + uintptr(hf.flagsOffset)) +} diff --git a/vendor/box3d/box3d_constants.odin b/vendor/box3d/box3d_constants.odin new file mode 100644 index 000000000..ccb93678a --- /dev/null +++ b/vendor/box3d/box3d_constants.odin @@ -0,0 +1,131 @@ +package vendor_box3d + + +@(link_prefix="b3", default_calling_convention="c") +foreign lib { + // Box3D bases all length units on meters, but you may need different units for your game. + // You can set this value to use different units. This should be done at application startup + // and only modified once. Default value is 1. + // @warning This must be modified before any calls to Box3D + SetLengthUnitsPerMeter :: proc(lengthUnits: f32) --- + + // Get the current length units per meter. + GetLengthUnitsPerMeter :: proc() -> f32 --- + + // Set the threshold for logging stalls. + SetStallThreshold :: proc(seconds: f32) --- + + // Get the threshold for logging stalls. + GetStallThreshold :: proc() -> f32 --- +} + +// Used to detect bad values. In float mode positions greater than about 16km have precision +// problems, so 100km is a safe limit. Large world mode keeps coordinates accurate much farther +// from the origin, so the sanity limit widens to keep valid far-field positions from tripping it. +@(require_results) +HUGE :: #force_inline proc "c" () -> f32 { + when DOUBLE_PRECISION { + return 1.0e9 * GetLengthUnitsPerMeter() + } else { + return 1.0e5 * GetLengthUnitsPerMeter() + } +} + +// Maximum parallel workers. Used for some fixed size arrays. +MAX_WORKERS :: 32 + +// Maximum number of tasks queued per world step. b3EnqueueTaskCallback will never be called +// more than this per world step. This is related to B3_MAX_WORKERS. With 32 workers, +// the maximum observed task count is 130. This allows an external task system to use a fixed +// size array for Box3D task, which may help with creating stable user task pointers. +MAX_TASKS :: 256 + +// Maximum number of colors in the constraint graph. Constraints that cannot +// find a color are added to the overflow set which are solved single-threaded. +// The compound barrel benchmark has minor overflow with 24 colors +GRAPH_COLOR_COUNT :: 24 + +// Number of contact point buckets for counting the number of contact points per +// shape contact pair. This is just for reporting and doesn't affect simulation. +CONTACT_MANIFOLD_COUNT_BUCKETS :: 8 + +// A small length used as a collision and constraint tolerance. Usually it is +// chosen to be numerically significant, but visually insignificant. In meters. +// @warning modifying this can have a significant impact on stability +@(require_results) +LINEAR_SLOP :: #force_inline proc "c" () -> f32 { + return 0.005 * GetLengthUnitsPerMeter() +} + +@(require_results) +MIN_CAPSULE_LENGTH :: #force_inline proc "c" () -> f32 { + return LINEAR_SLOP() +} + +// The distance between shapes where they are considered overlapped. This is needed +// because GJK may return small positive values for overlapped shapes in degenerate +// configurations. +@(require_results) +OVERLAP_SLOP :: #force_inline proc "c" () -> f32 { + return 0.1 * LINEAR_SLOP() +} + +// Maximum number of simultaneous worlds that can be allocated +MAX_WORLDS :: 128 + +// The maximum rotation of a body per time step. This limit is very large and is used +// to prevent numerical problems. You shouldn't need to adjust this. +// @warning increasing this to 0.5f * B3_PI or greater will break continuous collision. +MAX_ROTATION :: 0.25 * PI + +// @warning modifying this can have a significant impact on performance and stability +@(require_results) +SPECULATIVE_DISTANCE :: #force_inline proc "c" () -> f32 { + return 4.0 * LINEAR_SLOP() +} + +// The rest offset is used for mesh contact to reduce ghost collisions and assist with CCD. +// The rest offset adjusts the contact point separation value, making the solver push the shapes +// apart by this distance. +// Must be at least B3_LINEAR_SLOP and less than B3_SPECULATIVE_DISTANCE. +@(require_results) +MESH_REST_OFFSET :: #force_inline proc "c" () -> f32 { + return 1.0 * LINEAR_SLOP() +} + +// The default contact recycling distance. +@(require_results) +CONTACT_RECYCLE_DISTANCE :: #force_inline proc "c" () -> f32 { + return 10.0 * LINEAR_SLOP() +} + +// The default contact recycling world angle threshold. For performance this value +// is cos(angle/2)^2. This value corresponds to 10 degrees. +CONTACT_RECYCLE_ANGULAR_DISTANCE :: 0.99240388 + +// This is used to fatten AABBs in the dynamic tree. This allows proxies +// to move by a small amount without triggering a tree adjustment. This is in meters. +// @warning modifying this can have a significant impact on performance +@(require_results) +MAX_AABB_MARGIN :: #force_inline proc "c" () -> f32 { + return 0.05 * GetLengthUnitsPerMeter() +} + +// Per-shape AABB margin is a fraction of the shape extent (capped by B3_MAX_AABB_MARGIN). +// Small shapes get small margins; large shapes are clamped to the cap. +AABB_MARGIN_FRACTION :: 0.125 + +// The time that a body must be still before it will go to sleep. In seconds. +TIME_TO_SLEEP :: 0.5 + +// The maximum number of contact points between two touching shapes. +MAX_MANIFOLD_POINTS :: 4 + +// The maximum number points to use for shape cast proxies (swept point cloud). +MAX_SHAPE_CAST_POINTS :: 64 + +// These generous limits allow for easy hashing. See b3ShapePairKey. +SHAPE_POWER :: 22 +CHILD_POWER :: 64 - 2 * SHAPE_POWER +MAX_SHAPES :: 1 << SHAPE_POWER +MAX_CHILD_SHAPES :: 1 << CHILD_POWER \ No newline at end of file diff --git a/vendor/box3d/box3d_id.odin b/vendor/box3d/box3d_id.odin new file mode 100644 index 000000000..69453c4ba --- /dev/null +++ b/vendor/box3d/box3d_id.odin @@ -0,0 +1,129 @@ +package vendor_box3d + +/// World id references a world instance. This should be treated as an opaque handle. +WorldId :: struct { + index1: u16, + generation: u16, +} + +/// Body id references a body instance. This should be treated as an opaque handle. +BodyId :: struct { + index1: i32, + world0: u16, + generation: u16, +} + +/// Shape id references a shape instance. This should be treated as an opaque handle. +ShapeId :: struct { + index1: i32, + world0: u16, + generation: u16, +} + +/// Joint id references a joint instance. This should be treated as an opaque handle. +JointId :: struct { + index1: i32, + world0: u16, + generation: u16, +} + +/// Contact id references a contact instance. This should be treated as an opaque handle. +ContactId :: struct { + index1: i32, + world0: u16, + _: i16, + generation: u32, +} + + +/// Use these to make your identifiers null. +/// You may also use zero initialization to get null. +nullWorldId :: WorldId{} +nullBodyId :: BodyId{} +nullShapeId :: ShapeId{} +nullJointId :: JointId{} +nullContactId :: ContactId{} + +/// Macro to determine if any id is null. +@(require_results) +IS_NULL :: #force_inline proc "c" (id: $T) -> bool { + return id.index1 == 0 +} + +/// Macro to determine if any id is non-null. +@(require_results) +IS_NON_NULL :: #force_inline proc "c" (id: $T) -> bool { + return id.index1 != 0 +} + +/// Compare two ids for equality. Doesn't work for b3WorldId. Don't mix types. +@(require_results) +ID_EQUALS :: #force_inline proc "c" (id1, id2: $T) -> bool { + return id1.index1 == id2.index1 && id1.world0 == id2.world0 && id1.generation == id2.generation +} + +/// Store a world id into a u32. +@(require_results) +StoreWorldId :: #force_inline proc "c" (id: WorldId) -> u32 { + return u32(id.index1) << 16 | u32(id.generation) +} + +/// Load a u32 into a world id. +@(require_results) +LoadWorldId :: #force_inline proc "c" (x: u32) -> WorldId { + return WorldId{u16(x >> 16), u16(x)} +} + +/// Store a body id into a u64. +@(require_results) +StoreBodyId :: #force_inline proc "c" (id: BodyId) -> u64 { + return (u64(id.index1) << 32) | u64(id.world0) << 16 | u64(id.generation) +} + +/// Load a u64 into a body id. +@(require_results) +LoadBodyId :: #force_inline proc "c" (x: u64) -> BodyId { + return BodyId{i32(x >> 32), u16(x >> 16), u16(x)} +} + +/// Store a shape id into a u64. +@(require_results) +StoreShapeId :: #force_inline proc "c" (id: ShapeId) -> u64 { + return (u64(id.index1) << 32) | u64(id.world0) << 16 | u64(id.generation) +} + +/// Load a u64 into a shape id. +@(require_results) +LoadShapeId :: #force_inline proc "c" (x: u64) -> ShapeId { + return ShapeId{i32(x >> 32), u16(x >> 16), u16(x)} +} + +/// Store a joint id into a u64. +@(require_results) +StoreJointId :: #force_inline proc "c" (id: JointId) -> u64 { + return (u64(id.index1) << 32) | u64(id.world0) << 16 | u64(id.generation) +} + +/// Load a u64 into a joint id. +@(require_results) +LoadJointId :: #force_inline proc "c" (x: u64) -> JointId { + return JointId{i32(x >> 32), u16(x >> 16), u16(x)} +} + +/// Store a contact id into three uint32 values +@(require_results) +StoreContactId :: #force_inline proc "c" (id: ContactId) -> (values: [3]u32) { + values[0] = u32(id.index1) + values[1] = u32(id.world0) + values[2] = u32(id.generation) + return +} + +/// Load a contact id from three uint32 values. +@(require_results) +LoadContactId :: #force_inline proc "c" (values: [3]u32) -> (id: ContactId) { + id.index1 = i32(values[0]) + id.world0 = u16(values[1]) + id.generation = u32(values[2]) + return +} \ No newline at end of file diff --git a/vendor/box3d/box3d_math.odin b/vendor/box3d/box3d_math.odin new file mode 100644 index 000000000..43c43d5e0 --- /dev/null +++ b/vendor/box3d/box3d_math.odin @@ -0,0 +1,743 @@ +package vendor_box3d + +import "base:builtin" +import "core:math" +import "core:math/linalg" + +DOUBLE_PRECISION :: false + +// https://en.wikipedia.org/wiki/Pi +PI :: math.PI + +// Convenience constant to convert from degrees to radians. +DEG_TO_RAD :: math.RAD_PER_DEG +// Convenience constant to convert from radians to degrees. +RAD_TO_DEG :: math.DEG_PER_RAD + + +// Minimum scale used for scaling collision meshes, etc. +MIN_SCALE :: 0.01 + +Vec2 :: [2]f32 +Vec3 :: [3]f32 + +// Cosine and sine pair. +// This uses a custom implementation designed for cross-platform determinism. +CosSin :: struct { + cosine: f32, + sine: f32, +} + +Quat :: quaternion128 + +Transform :: struct { + p: Vec3, + q: Quat, +} + +Pos :: [3]f64 when DOUBLE_PRECISION else [3]f32 + +WorldTransform :: struct { + p: Pos, + q: Quat, +} + +Matrix3 :: matrix[3, 3]f32 + +// Axis aligned bounding box. +AABB :: struct { + lowerBound: Vec3, + upperBound: Vec3, +} + +// A plane. +// separation = dot(normal, point) - offset +Plane :: struct { + normal: Vec3, + offset: f32, +} + +Vec3_zero :: Vec3{0.0, 0.0, 0.0} +Vec3_one :: Vec3{1.0, 1.0, 1.0} +Vec3_axisX :: Vec3{1.0, 0.0, 0.0} +Vec3_axisY :: Vec3{0.0, 1.0, 0.0} +Vec3_axisZ :: Vec3{0.0, 0.0, 1.0} +Quat_identity :: Quat(1) +Transform_identity :: Transform{p={0, 0, 0}, q=1} +Mat3_zero :: Matrix3(0) +Mat3_identity :: Matrix3(1) + +Pos_zero :: Pos{0.0, 0.0, 0.0} +WorldTransform_identity :: WorldTransform{p={0, 0, 0}, q=1} + + + +// @return the minimum of two integers. + +MinInt :: builtin.min +MaxInt :: builtin.max +ClampInt :: builtin.clamp + +// The closest points between to segments or infinite lines. +SegmentDistanceResult :: struct { + point1: Vec3, + fraction1: f32, + point2: Vec3, + fraction2: f32, +} + + +@(link_prefix="b3", default_calling_convention="c", require_results) +foreign lib { + // Compute an approximate arctangent in the range [-pi, pi] + // This is hand coded for cross-platform determinism. The atan2f + // function in the standard library is not cross-platform deterministic. + // Accurate to around 0.0023 degrees. + Atan2 :: proc(y, x: f32) -> f32 --- + + // Compute the cosine and sine of an angle in radians. Implemented + // for cross-platform determinism. + ComputeCosSin :: proc(radians: f32) -> CosSin --- + + // Compute the closest point on the segment a-b to the target q. + PointToSegmentDistance :: proc(a, b: Vec3, q: Vec3) -> Vec3 --- + + // Compute the closest points on two infinite lines. + LineDistance :: proc(p1, d1: Vec3, p2, d2: Vec3) -> SegmentDistanceResult --- + + // Compute the closest points on two line segments. + SegmentDistance :: proc(p1, q1: Vec3, p2, q2: Vec3) -> SegmentDistanceResult --- + + // Is this a valid vector? Not NaN or infinity. + IsValidVec3 :: proc(a: Vec3) -> bool --- + + // Is this a valid quaternion? Not NaN or infinity. Is normalized. + IsValidQuat :: proc(q: Quat) -> bool --- + + // Is this a valid transform? Not NaN or infinity. Is normalized. + IsValidTransform :: proc(a: Transform) -> bool --- + + // Is this a valid matrix? Not NaN or infinity. + IsValidMatrix3 :: proc(a: Matrix3) -> bool --- + + // Is this a valid bounding box? Not Nan or infinity. Upper bound greater than or equal to lower bound. + IsValidAABB :: proc(a: AABB) -> bool --- + + // Is this AABB reasonably close to the origin? See B3_HUGE. + IsBoundedAABB :: proc(a: AABB) -> bool --- + + // Is this AABB valid and reasonable? + IsSaneAABB :: proc(a: AABB) -> bool --- + + // Is this a valid plane? Normal is a unit vector. Not Nan or infinity. + IsValidPlane :: proc(a: Plane) -> bool --- + + // Is this a valid world position? Not NaN or infinity. + IsValidPosition :: proc(p: Pos) -> bool --- + + // Is this a valid world transform? Not NaN or infinity. Rotation is normalized. + IsValidWorldTransform :: proc(t: WorldTransform) -> bool --- + + // Get the inertia tensor of an offset point. + // https://en.wikipedia.org/wiki/Parallel_axis_theorem + Steiner :: proc(mass: f32, origin: Vec3) -> Matrix3 --- + + // Extract a quaternion from a rotation matrix. + MakeQuatFromMatrix :: proc(#by_ptr m: Matrix3) -> Quat --- + + // Find a quaternion that rotates one vector to another. + ComputeQuatBetweenUnitVectors :: proc(v1, v2: Vec3) -> Quat --- + + + +} + +AbsFloat :: builtin.abs +MinFloat :: builtin.min +MaxFloat :: builtin.max +ClampFloat :: builtin.clamp +LerpFloat :: math.lerp + +// Convert any angle into the range [-pi, pi]. +@(require_results) +UnwindAngle :: #force_inline proc "c" (radians: f32) -> f32 { + // Assuming this is deterministic + return math.remainder(radians, 2.0 * PI) +} + +// Vector dot product. +Dot :: linalg.dot +Length :: linalg.length +LengthSquared :: linalg.length2 + + +// Distance between two points. +@(require_results) +Distance :: #force_inline proc "c" (a, b: Vec3) -> f32 { + dv := b - a + return Length(dv) +} + +// Squared distance between two points. +@(require_results) +DistanceSquared :: #force_inline proc "c" (a, b: Vec3) -> f32 { + dv := b - a + return LengthSquared(dv) +} + +FLT_EPSILON :: 1.1920928955078125e-07 /* 0x0.000002p0 */ + + +// Normalize a vector. Returns a zero vector if the input vector is very small. +@(require_results) +Normalize :: proc "c" (a: Vec3) -> Vec3 { + lengthSquared := a.x * a.x + a.y * a.y + a.z * a.z + + if lengthSquared > 1000.0 * min(f32) { + s := 1.0 / math.sqrt(lengthSquared) + return Vec3{ s * a.x, s * a.y, s * a.z } + } + + return Vec3{0.0, 0.0, 0.0} +} + +// Normalize a vector and return the length. Returns a zero vector +// if the input is very small. +@(require_results) +GetLengthAndNormalize :: proc "c" (a: Vec3) -> (length: f32, n: Vec3) { + length = Length(a) + if length < FLT_EPSILON { + return + } + + invLength := 1.0 / length + n = {invLength * a.x, invLength * a.y, invLength * a.z} + return +} + +// Get a unit vector that is perpendicular to the supplied vector. +@(require_results) +Perp :: proc "c" (a: Vec3) -> Vec3 { + // Suppose vector a has all equal components and is a unit vector: a = (s, s, s) + // Then 3*s*s = 1, s = sqrt(1/3) = 0.57735. This means that at least one component + // of a unit vector must be greater or equal to 0.57735. + p: Vec3 + if a.x < -0.5 || 0.5 < a.x { + p = {a.y, -a.x, 0.0} + } else { + p = {0.0, a.z, -a.y} + } + + return Normalize(p) +} + +// Is a vector normalized? In other words, does it have unit length? +@(require_results) +IsNormalized :: proc "c" (a: Vec3) -> bool { + aa := Dot(a, a) + return AbsFloat(1.0 - aa) < 100.0 * FLT_EPSILON +} +// https://en.wikipedia.org/wiki/Cross_product +Cross :: linalg.cross + +// Linearly interpolate between two vectors. +Lerp :: linalg.lerp + +// Blend two vectors: s * a + t * b +@(require_results) +Blend2 :: proc "c" (s: f32, a: Vec3, t: f32, b: Vec3) -> Vec3 { + return { + s * a.x + t * b.x, + s * a.y + t * b.y, + s * a.z + t * b.z, + } +} + +// Component-wise absolute value. +Abs :: linalg.abs + +// Component-wise -1 or 1 (1 if zero). +Sign :: linalg.sign + +// Component-wise minimum value. +Min :: linalg.min + +// Component-wise maximum value. +Max :: linalg.max + +// Component-wise clamped value. +Clamp :: linalg.clamp + +// Create a safe scaling value for scaling collision. This allows +// negative scale, but keeps scale sufficiently far from zero. +@(require_results) +SafeScale :: proc "c" (a: Vec3) -> Vec3 { + absScale := Abs(a) + minScale := Vec3{MIN_SCALE, MIN_SCALE, MIN_SCALE} + safeScale := Sign(a) * Max(absScale, minScale) + return safeScale +} + +// Does the supplied quaternion have unit length? +@(require_results) +IsNormalizedQuat :: proc "c" (q: Quat) -> bool { + qq := q.x * q.x + q.y * q.y + q.z * q.z + q.w * q.w + return 1.0 - 20.0 * FLT_EPSILON < qq && qq < 1.0 + 20.0 * FLT_EPSILON +} + +// Rotate a vector. +@(require_results) +RotateVector :: proc "c" (q: Quat, v: Vec3) -> Vec3 { + // v + 2 * cross(q.xyz, cross(q.xyz, v) + q.w * v) + // B3_ASSERT(IsNormalizedQuat(q)); + t1 := Cross(q.xyz, v) + t2 := t1 * q.w + v + t3 := Cross(q.xyz, t2) + return v * 2.0 + t3 +} + +// Inverse rotate a vector. +@(require_results) +InvRotateVector :: proc "c" (q: Quat, v: Vec3) -> Vec3 { + // v + 2 * cross(q.xyz, cross(q.xyz, v) - q.w * v) + // B3_ASSERT(IsNormalizedQuat(q)); + t1 := Cross(q.xyz, v) + t2 := t1 * q.w - v + t3 := Cross(q.xyz, t2) + return v * 2.0 + t3 +} + +// Compute dot product of two quaternions. Useful for polarity tests. +@(require_results) +DotQuat :: linalg.dot + +// Multiply two quaternions. +@(require_results) +MulQuat :: proc "c" (q1, q2: Quat) -> Quat { + return q1 * q2 +} + +// Compute a relative quaternion. +// inv(q1) * q2 +@(require_results) +InvMulQuat :: proc "c" (q1, q2: Quat) -> Quat { + t1 := Cross(q2.xyz, q1.xyz) + t2 := t1 * q1.w + q2.xyz + t3 := t2 * q2.w - q1.xyz + return quaternion(x=t3.x, y=t3.y, z=t3.z, w=q1.w * q2.w + Dot(q1.xyz, q2.xyz)) +} + +// Quaternion conjugate (cheap inverse). +Conjugate :: builtin.conj + +// Component-wise quaternion negation. +@(require_results) +NegateQuat :: proc "c" (q: Quat) -> Quat { + return -q +} + +// Normalize a quaternion. +@(require_results) +NormalizeQuat :: proc "c" (q: Quat) -> Quat { + lengthSq := DotQuat(q, q) + if lengthSq > 1000.0 * min(f32) { + s := 1.0 / math.sqrt(lengthSq) + return Quat(s) * q + } + + return Quat_identity +} + +// Make a quaternion that is equivalent to rotating around an axis by a specified angle. +@(require_results) +MakeQuatFromAxisAngle :: proc "c" (axis: Vec3, radians: f32) -> Quat { + ASSERT(IsNormalized(axis)) + cs := ComputeCosSin(0.5 * radians) + return quaternion(x=cs.sine * axis.x, y=cs.sine * axis.y, z=cs.sine * axis.z, w=cs.cosine) +} + +// Get the axis and angle from a quaternion. Assumes the quaternion is normalized. +@(require_results) +GetAxisAngle :: proc "c" (q: Quat) -> (radians: f32, axis: Vec3) { + length := math.sqrt(q.x * q.x + q.y * q.y + q.z * q.z) + radians = 2.0 * Atan2(length, q.w) + if length > 0.0 { + invLength := 1.0 / length + axis = {invLength * q.x, invLength * q.y, invLength * q.z} + } + return +} + +// Get the angle for a quaternion in radians +@(require_results) +GetQuatAngle :: proc "c" (q: Quat) -> f32 { + length := math.sqrt(q.x * q.x + q.y * q.y + q.z * q.z) + return 2.0 * Atan2(length, q.w) +} + +// Twist angle around the z-axis, used for twist limit and revolute angle limit +@(require_results) +GetTwistAngle :: proc "c" (q: Quat) -> f32 { + // Account for polarity to keep the twist angle in range. + // This is simpler than asking the user to check polarity or unwinding. + twist := q.w < 0.0 ? Atan2(-q.z, -q.w) : Atan2(q.z, q.w) + twist *= 2.0 + ASSERT(-PI <= twist && twist <= PI) + return twist +} + +// Swing angle used for cone limit +@(require_results) +GetSwingAngle :: proc "c" (q: Quat) -> f32 { + // Polarity should not matter because all terms are squared. + x := math.sqrt(q.z * q.z + q.w * q.w) + y := math.sqrt(q.x * q.x + q.y * q.y) + swing := 2.0 * Atan2(y, x) + ASSERT(0.0 <= swing && swing <= PI) + return swing +} + +// Linearly interpolate and normalize between two quaternions +@(require_results) +NLerp :: proc "c" (q1, q2: Quat, alpha: f32) -> Quat { + VALIDATE(0.0 <= alpha && alpha <= 1.0) + q1 := q1 + if DotQuat(q1, q2) < 0.0 { + q1 = -q1 + } + + q: Quat + q.xyz = Lerp(q1.xyz, q2.xyz, alpha) + q.w = (1.0 - alpha) * q1.w + alpha * q2.w + + return NormalizeQuat(q) +} + +// Multiply two transforms. If the result is applied to a point p local to frame B, +// the transform would first convert p to a point local to frame A, then into a point +// in the world frame. This is useful if frame B is a child of frame A. +@(require_results) +MulTransforms :: proc "c" (a, b: Transform) -> (out: Transform) { + out.p = RotateVector(a.q, b.p) + a.p + out.q = a.q * b.q + return +} + +// Creates a transform that converts a local point in frame B to a local point in frame A. +// This is useful for transforming points between the local spaces of two frames that are +// in world space. +@(require_results) +InvMulTransforms :: #force_inline proc "c" (a, b: Transform) -> (out: Transform) { + out.p = InvRotateVector(a.q, b.p - a.p) + out.q = InvMulQuat(a.q, b.q) + return +} + +// Get the inverse of a transform. +@(require_results) +InvertTransform :: proc "c" (t: Transform) -> (out: Transform) { + out.p = InvRotateVector(t.q, -t.p) + out.q = Conjugate(t.q) + return +} + +// Transform a point. +@(require_results) +TransformPoint :: proc "c" (t: Transform, v: Vec3) -> Vec3 { + rv := RotateVector(t.q, v) + return rv + t.p +} + +// Inverse transform a point. +@(require_results) +InvTransformPoint :: proc "c" (t: Transform, v: Vec3) -> Vec3 { + return InvRotateVector(t.q, v - t.p) +} + +// World position boundary. These cross between the double precision world space at the public +// boundary and the float interior. One set of bodies serves both modes: the typedefs collapse +// the types in float mode and the explicit float casts become no-ops. + +// Convert a vector to a world position. +@(require_results) +ToPos :: proc "c" (v: Vec3) -> Pos { + T :: type_of(Pos{}.x) + return {T(v.x), T(v.y), T(v.z)} +} + +// Lossy conversion of a world position to a float vector. +@(require_results) +ToVec3 :: proc "c" (p: Pos) -> Vec3 { + return {f32(p.x), f32(p.y), f32(p.z)} +} + +// Narrow a world coordinate to float, rounding toward negative infinity. Use with +// RoundUpFloat to build a conservative float box that always contains the double bounds, +// where plain rounding far from the origin could clip. nextafterf is an exact IEEE operation, +// so this is cross-platform deterministic. With large world mode off this is a plain conversion. +@(require_results) +RoundDownFloat :: proc "c" (x: f64) -> f32 { + when DOUBLE_PRECISION { + f := f32(x) + return f64(f) > f64(x) ? math.nextafter(f, -max(f32)) : f + } else { + return f32(x) + } +} + +// Narrow a world coordinate to float, rounding toward positive infinity. +@(require_results) +RoundUpFloat :: proc "c" (x: f32) -> f32 { + when DOUBLE_PRECISION { + f := f32(x) + return f64(f) < f64(x) ? math.nextafter(f, max(f32)) : f + } else { + return f32(x) + } +} + +// p + d +@(require_results) +OffsetPos :: proc "c" (p: Pos, d: Vec3) -> Pos { + T :: type_of(Pos{}.x) + return {p.x + T(d.x), p.y + T(d.y), p.z + T(d.z)} +} + +// World position interpolation for sweeps and sampling. +LerpPosition :: linalg.lerp + +// Transform a local point to a world position. Rotation in float, translation in double. +@(require_results) +TransformWorldPoint :: proc "c" (t: WorldTransform, p: Vec3) -> Pos { + T :: type_of(Pos{}.x) + r := RotateVector(t.q, p) + return {T(t.p.x + r.x), T(t.p.y + r.y), T(t.p.z + r.z)} +} + +// Transform a world position to a local point. One double subtraction, then float. +@(require_results) +InvTransformWorldPoint :: proc "c" (t: WorldTransform, p: Pos) -> Vec3 { + d := Vec3{f32(p.x - t.p.x), f32(p.y - t.p.y), f32(p.z - t.p.z)} + return InvRotateVector(t.q, d) +} + +// Relative transform of frame B in frame A. The narrow phase boundary. +@(require_results) +InvMulWorldTransforms :: proc "c" (A, B: WorldTransform) -> (C: Transform) { + C.q = InvMulQuat(A.q, B.q) + d := Vec3{f32(B.p.x - A.p.x), f32(B.p.y - A.p.y), f32(B.p.z - A.p.z)} + C.p = InvRotateVector(A.q, d) + return +} + +// Compose a world transform with a local transform. +@(require_results) +MulWorldTransforms :: proc "c" (A: WorldTransform, B: Transform) -> (C: WorldTransform) { + T :: type_of(Pos{}.x) + C.q = A.q * B.q + r := RotateVector(A.q, B.p) + C.p = Pos{T(A.p.x + r.x), T(A.p.y + r.y), T(A.p.z + r.z)} + return +} + +// Shift a world transform into the frame of a base position. +@(require_results) +ToRelativeTransform :: proc "c" (t: WorldTransform, base: Pos) -> (r: Transform) { + r.q = t.q + r.p = {f32(t.p.x - base.x), f32(t.p.y - base.y), f32(t.p.z - base.z)} + return +} + +// Promote a float transform to a world transform. Lossless. +@(require_results) +MakeWorldTransform :: proc "c" (t: Transform) -> (w: WorldTransform) { + w.p = ToPos(t.p) + w.q = t.q + return +} + +// Translate a local AABB by a world origin, rounding outward so the float box always contains +// the double box. Far from the origin a plain conversion could clip a shape out of its own box. +// In float mode the origin is float and the rounding is a no-op. +@(require_results) +OffsetAABB :: proc "c" (localBox: AABB, origin: Pos) -> (out: AABB) { + out.lowerBound.x = RoundDownFloat(f64(origin.x + localBox.lowerBound.x)) + out.lowerBound.y = RoundDownFloat(f64(origin.y + localBox.lowerBound.y)) + out.lowerBound.z = RoundDownFloat(f64(origin.z + localBox.lowerBound.z)) + out.upperBound.x = RoundUpFloat(f32(origin.x + localBox.upperBound.x)) + out.upperBound.y = RoundUpFloat(f32(origin.y + localBox.upperBound.y)) + out.upperBound.z = RoundUpFloat(f32(origin.z + localBox.upperBound.z)) + return +} + +// Compute the determinant of a 3-by-3 matrix. +Det :: linalg.determinant + +// Matrix transpose. +Transpose :: linalg.transpose + +// General matrix inverse. +@(require_results) +InvertMatrix :: proc "c" (m: Matrix3) -> Matrix3 { + det := Det(m) + if AbsFloat(det) > 1000.0 * min(f32) { + invDet := 1.0 / det + out: Matrix3 + out[0] = invDet * Cross(m[1], m[2]) + out[1] = invDet * Cross(m[2], m[0]) + out[2] = invDet * Cross(m[0], m[1]) + return Transpose(out) + } + return Mat3_zero +} + +// Solve a matrix equation. +// @return inv(m) * a +@(require_results) +Solve3 :: proc "c" (m: Matrix3, a: Vec3) -> Vec3 { + return InvertMatrix(m) * a +} + +// Invert a matrix. +@(require_results) +InvertT :: proc "c" (m: Matrix3) -> Matrix3 { + det := Det(m) + if AbsFloat(det) > 1000.0 * min(f32) { + invDet := 1.0 / det + out: Matrix3 + out[0] = invDet * Cross(m[1], m[2]) + out[1] = invDet * Cross(m[2], m[0]) + out[2] = invDet * Cross(m[0], m[1]) + return out + } + return Mat3_zero +} + +// Get the component-wise absolute value of a matrix. +@(require_results) +AbsMatrix3 :: proc "c" (m: Matrix3) -> (out: Matrix3) { + out[0] = Abs(m[0]) + out[1] = Abs(m[1]) + out[2] = Abs(m[2]) + return +} + +// Make a matrix from a quaternion. This is useful if you need to +// rotate many vectors. +// The force inline improves the performance of ShapeDistance. +@(require_results) +MakeMatrixFromQuat :: proc "c" (q: Quat) -> (out: Matrix3) { + xx := q.x * q.x + yy := q.y * q.y + zz := q.z * q.z + xy := q.x * q.y + xz := q.x * q.z + xw := q.x * q.w + yz := q.y * q.z + yw := q.y * q.w + zw := q.z * q.w + + out[0] = { 1.0 - 2.0 * (yy + zz), 2.0 * (xy + zw), 2.0 * (xz - yw) } + out[1] = { 2.0 * (xy - zw), 1.0 - 2.0 * (xx + zz), 2.0 * (yz + xw) } + out[2] = { 2.0 * (xz + yw), 2.0 * (yz - xw), 1.0 - 2.0 * (xx + yy) } + return +} + +// Get the AABB of a point cloud. +@(require_results) +MakeAABB :: proc "c" (points: []Vec3, radius: f32) -> (a: AABB) #no_bounds_check { + ASSERT(len(points) > 0) + a = AABB{points[0], points[0]} + for i in 1.. bool { + switch { + case a.lowerBound.x > b.lowerBound.x || b.upperBound.x > a.upperBound.x: + return false + case a.lowerBound.y > b.lowerBound.y || b.upperBound.y > a.upperBound.y: + return false + case a.lowerBound.z > b.lowerBound.z || b.upperBound.z > a.upperBound.z: + return false + } + return true +} + +// Get the surface area of an axis-aligned bounding box. +@(require_results) +AABB_Area :: proc "c" (a: AABB) -> f32 { + delta := a.upperBound - a.lowerBound + return 2.0 * (delta.x * delta.y + delta.y * delta.z + delta.z * delta.x) +} + +// Get the center of an axis-aligned bounding box. +@(require_results) +AABB_Center :: proc "c" (a: AABB) -> Vec3 { + return 0.5 * (a.upperBound + a.lowerBound) +} + +// Get the extents (half-widths) of an axis-aligned bounding box. +@(require_results) +AABB_Extents :: proc "c" (a: AABB) -> Vec3 { + return 0.5 * (a.upperBound - a.lowerBound) +} + +// Get the union of two axis-aligned bounding boxes. +@(require_results) +AABB_Union :: proc "c" (a, b: AABB) -> (out: AABB) { + out.lowerBound = Min(a.lowerBound, b.lowerBound) + out.upperBound = Max(a.upperBound, b.upperBound) + return +} + +// Add uniform padding to an axis-aligned bounding box. +@(require_results) +AABB_Inflate :: proc "c" (a: AABB, extension: f32) -> (out: AABB) { + radius := Vec3{extension, extension, extension} + + out.lowerBound = a.lowerBound - radius + out.upperBound = a.upperBound + radius + return +} + +// Do two axis-aligned boxes overlap? +@(require_results) +AABB_Overlaps :: proc "c" (a, b: AABB) -> bool { + // No intersection if separated along one axis + switch { + case a.upperBound.x < b.lowerBound.x || a.lowerBound.x > b.upperBound.x: + return false + case a.upperBound.y < b.lowerBound.y || a.lowerBound.y > b.upperBound.y: + return false + case a.upperBound.z < b.lowerBound.z || a.lowerBound.z > b.upperBound.z: + return false + } + + // Overlapping on all axis means bounds are intersecting + return true +} + +// Transform an axis-aligned bounding box. This can create a larger box +// than if you recomputed the AABB of the original shape with the transform +// applied. +@(require_results) +AABB_Transform :: proc "c" (transform: Transform, a: AABB) -> AABB { + center := TransformPoint(transform, AABB_Center(a)) + m := MakeMatrixFromQuat(transform.q) + extent := AbsMatrix3(m) * AABB_Extents(a) + return AABB{center - extent, center + extent} +} + +// Get the closest point on an axis-aligned bounding box. +@(require_results) +ClosestPointToAABB :: proc "c" (point: Vec3, a: AABB) -> Vec3 { + return Clamp(point, a.lowerBound, a.upperBound) +} diff --git a/vendor/box3d/box3d_types.odin b/vendor/box3d/box3d_types.odin new file mode 100644 index 000000000..42b72615f --- /dev/null +++ b/vendor/box3d/box3d_types.odin @@ -0,0 +1,2937 @@ +package vendor_box3d + +import "core:c" + +DEFAULT_CATEGORY_BITS :: max(u64) +DEFAULT_MASK_BITS :: max(u64) + + +// Task interface +// This is the prototype for a Box3D task. Your task system is expected to run this callback on a worker thread, +// exactly once per enqueue, passing back the same taskContext pointer supplied to EnqueueTaskCallback. +// @ingroup world +TaskCallback :: proc "c" (taskContext: rawptr) + +// These functions can be provided to Box3D to invoke a task system. +// Returns a pointer to the user's task object. May be nullptr. A nullptr indicates to Box3D that the work was executed +// serially within the callback and there is no need to call b3FinishTaskCallback. Otherwise the returned +// value must be non-null will be passed to b3FinishTaskCallback as the userTask. +// @param task the Box3D task to be called by the scheduler +// @param taskContext the Box3D context object that the scheduler must pass to the task +// @param userContext the scheduler context object that is opaque to Box3D +// @param taskName the Box3D task name that the scheduler can use for diagnostics +// @ingroup world +EnqueueTaskCallback :: proc "c" (task: rawptr, taskContext: rawptr, userContext: rawptr, taskName: cstring) -> rawptr + +// Finishes a user task object that wraps a Box3D task. This must block until the task has completed. +// The step blocks here on the tasks it spawned, so b3World_Step holds its stack across every +// fork/join. Drive it from a thread you can dedicate to the step, or from a fiber this callback can +// park to free the underlying thread. In a job system that cannot park a job's stack, do not call +// b3World_Step from inside a job: a job that blocks on its own sub-jobs without yielding its thread +// can deadlock. The in-tree scheduler instead runs other pending tasks on the waiting thread. +// @ingroup world +FinishTaskCallback :: proc "c" (userTask: rawptr, userContext: rawptr) + + +// The user needs to be able to create debug draw shapes for multi-pass rendering to work efficiently. +// These user shapes are created and destroyed via callback so they can be bound to shape lifetime and scaling updates. +// @ingroup debug_draw +CreateDebugShapeCallback :: proc "c" (debugShape: ^DebugShape, userContext: rawptr) -> rawptr +DestroyDebugShapeCallback :: proc "c" (userShape: rawptr, userContext: rawptr) + +// Optional friction mixing callback. This intentionally provides no context objects because this is called +// from a worker thread. +// @warning This function should not attempt to modify Box3D state or user application state. +// @ingroup world +FrictionCallback :: proc "c" (frictionA: f32, userMaterialIdA: u64, frictionB: f32, userMaterialIdB: u64) -> f32 + +// Optional restitution mixing callback. This intentionally provides no context objects because this is called +// from a worker thread. +// @warning This function should not attempt to modify Box3D state or user application state. +// @ingroup world +RestitutionCallback :: proc "c" (restitutionA: f32, userMaterialIdA: u64, restitutionB: f32, userMaterialIdB: u64) -> f32 + +// Prototype for a contact filter callback. +// This is called when a contact pair is considered for collision. This allows you to +// perform custom logic to prevent collision between shapes. This is only called if +// one of the two shapes has custom filtering enabled. @see b3ShapeDef. +// Notes: +// - this function must be thread-safe +// - this is only called if one of the two shapes has enabled custom filtering +// - this is called only for awake dynamic bodies +// Return false if you want to disable the collision +// @warning Do not attempt to modify the world inside this callback +// @ingroup world +CustomFilterFcn :: proc "c" (shapeIdA: ShapeId, shapeIdB: ShapeId, ctx: rawptr) -> bool + +// Prototype for a pre-solve callback. +// This is called after a contact is updated. This allows you to inspect a +// collision before it goes to the solver. +// Notes: +// - this function must be thread-safe +// - this is only called if the shape has enabled pre-solve events +// - this may be called for awake dynamic bodies and sensors +// - this is not called for sensors +// Return false if you want to disable the contact this step +// This has limited information because it is used during CCD which does not have the +// full contact manifold. +// @warning Do not attempt to modify the world inside this callback +// @ingroup world +PreSolveFcn :: proc "c" (shapeIdA: ShapeId, shapeIdB: ShapeId, point: Pos, normal: Vec3, ctx: rawptr) -> bool + +// Prototype callback for overlap queries. +// Called for each shape found in the query. +// @see b3World_OverlapAABB +// @return false to terminate the query. +// @ingroup world +OverlapResultFcn :: proc "c" (shapeId: ShapeId, ctx: rawptr) -> bool + +// Prototype callback for ray casts. +// Called for each shape found in the query. You control how the ray cast +// proceeds by returning a float: +// return -1: ignore this shape and continue +// return 0: terminate the ray cast +// return fraction: clip the ray to this point +// return 1: don't clip the ray and continue +// @param shapeId the shape hit by the ray +// @param point the point of initial intersection +// @param normal the normal vector at the point of intersection +// @param fraction the fraction along the ray at the point of intersection +// @param userMaterialId the shape or triangle surface type +// @param triangleIndex the triangle index for mesh or height field shapes or -1 for other shape types +// @param childIndex the child shape index for compound shapes +// @param context the user context +// @return -1 to filter, 0 to terminate, fraction to clip the ray for closest hit, 1 to continue +// @see b3World_CastRay +// @ingroup world +CastResultFcn :: proc "c" (shapeId: ShapeId, point: Pos, normal: Vec3, fraction: f32, userMateriald: u64, triangleIndex: c.int, childIndex: c.int, ctx: rawptr) -> f32 + +@(link_prefix="b3", default_calling_convention="c", require_results) +foreign lib { + // Use this to initialize your world definition + // @ingroup world + DefaultWorldDef :: proc() -> WorldDef --- + + // Use this to initialize your body definition + // @ingroup body + DefaultBodyDef :: proc() -> BodyDef --- + + // Use this to initialize your filter + // @ingroup shape + DefaultFilter :: proc() -> Filter --- + + // Use this to initialize your surface material + // @ingroup shape + DefaultSurfaceMaterial :: proc() -> SurfaceMaterial --- + + // Use this to initialize your shape definition + // @ingroup shape + DefaultShapeDef :: proc() -> ShapeDef --- + + // Use this to initialize your joint definition + // @ingroup distance_joint + DefaultDistanceJointDef :: proc() -> DistanceJointDef --- + + // Use this to initialize your joint definition + // @ingroup motor_joint + DefaultMotorJointDef :: proc() -> MotorJointDef --- + + // Use this to initialize your joint definition + // @ingroup filter_joint + DefaultFilterJointDef :: proc() -> FilterJointDef --- + + + // Use this to initialize your joint definition + // @ingroup parallel_joint + DefaultParallelJointDef :: proc() -> ParallelJointDef --- + + + // Use this to initialize your joint definition + // @ingroup prismatic_joint + DefaultPrismaticJointDef :: proc() -> PrismaticJointDef --- + + + // Use this to initialize your joint definition. + // @ingroup revolute_joint + DefaultRevoluteJointDef :: proc() -> RevoluteJointDef --- + + // Use this to initialize your joint definition. + // @ingroup spherical_joint + DefaultSphericalJointDef :: proc() -> SphericalJointDef --- + + + // Use this to initialize your joint definition + // @ingroup weld_joint + DefaultWeldJointDef :: proc() -> WeldJointDef --- + + // Use this to initialize your joint definition + // @ingroup wheel_joint + DefaultWheelJointDef :: proc() -> WheelJointDef --- + + // Use this to initialize your explosion definition + // @ingroup world + DefaultExplosionDef :: proc() -> ExplosionDef --- + + // Use this to initialize your query filter + DefaultQueryFilter :: proc() -> QueryFilter --- + + // Get the visualization color assigned to a constraint graph color slot. The last index + // (B3_GRAPH_COLOR_COUNT - 1) is the overflow color. + GetGraphColor :: proc(index: c.int) -> HexColor --- + + // Create a debug draw struct with default values. + DefaultDebugDraw :: proc() -> DebugDraw --- +} + +// Pack an RGB color with a material preset for debug draw. The preset rides in +// the high byte where the color converters ignore it. +@(require_results) +MakeDebugColor :: #force_inline proc "c" (rgb: HexColor, material: DebugMaterial) -> u32 { + return (u32(rgb)&0x00FFFFFF) | (u32(material)<<24) +} + + + + +// Optional world capacities that can be use to avoid run-time allocations +// @ingroup world +Capacity :: struct { + // Number of expected static shapes. + staticShapeCount: c.int, + + // Number of expected dynamic and kinematic shapes. + dynamicShapeCount: c.int, + + // Number of expected static bodies. + staticBodyCount: c.int, + + // Number of expected dynamic and kinematic bodies. + dynamicBodyCount: c.int, + + // Number of expected contacts. + contactCount: c.int, +} + + +// World definition used to create a simulation world. Must be initialized using b3DefaultWorldDef. +// @ingroup world +WorldDef :: struct { + // Gravity vector. Box3D has no up-vector defined. + gravity: Vec3, + + // Restitution speed threshold, usually in m/s. Collisions above this + // speed have restitution applied (will bounce). + restitutionThreshold: f32, + + // Hit event speed threshold, usually in m/s. Collisions above this + // speed can generate hit events if the shape also enables hit events. + hitEventThreshold: f32, + + // Contact stiffness. Cycles per second. Increasing this increases the speed of overlap recovery, but can introduce jitter. + contactHertz: f32, + + // Contact bounciness. Non-dimensional. You can speed up overlap recovery by decreasing this with + // the trade-off that overlap resolution becomes more energetic. + contactDampingRatio: f32, + + // This parameter controls how fast overlap is resolved and usually has units of meters per second. This only + // puts a cap on the resolution speed. The resolution speed is increased by increasing the hertz and/or + // decreasing the damping ratio. + contactSpeed: f32, + + // Maximum linear speed. Usually meters per second. + maximumLinearSpeed: f32, + + // Optional mixing callback for friction. The default uses sqrt(frictionA * frictionB). + frictionCallback: FrictionCallback, + + // Optional mixing callback for restitution. The default uses max(restitutionA, restitutionB). + restitutionCallback: RestitutionCallback, + + // Can bodies go to sleep to improve performance + enableSleep: bool, + + // Enable continuous collision + enableContinuous: bool, + + // Number of workers to use with the provided task system. Box3D performs best when using only + // performance cores and accessing a single L2 cache. Efficiency cores and hyper-threading provide + // little benefit and may even harm performance. + // This is clamped to the range [1, B3_MAX_WORKERS]. Using a value above 1 will turn on multithreading. + // If task callbacks are provided then Box3D will use the user provided task system. Otherwise Box3D + // will create threads and use an internal scheduler. + workerCount: u32, + + // function to spawn task + enqueueTask: EnqueueTaskCallback, + + // function to finish a task + finishTask: FinishTaskCallback, + + // User context that is provided to enqueueTask and finishTask + userTaskContext: rawptr, + + // User data associated with a world + userData: rawptr, + + // Used to create debug draw shapes. This is called when a shape is + // first drawn using b3DebugDraw. + createDebugShape: CreateDebugShapeCallback, + + // Used to destroy debug draw shapes. This is called when a shape is modified or destroyed. + destroyDebugShape: DestroyDebugShapeCallback, + + // This is passed to the debug shape callbacks to provide a user context. + userDebugShapeContext: rawptr, + + // Optional initial capacities + capacity: Capacity, + + // Used internally to detect a valid definition. DO NOT SET. + internalValue: c.int, +} + + +// The body simulation type. +// Each body is one of these three types. The type determines how the body behaves in the simulation. +// @ingroup body +BodyType :: enum c.int { + // zero mass, zero velocity, may be manually moved + staticBody = 0, + + // zero mass, velocity set by user, moved by solver + kinematicBody = 1, + + // positive mass, velocity determined by forces, moved by solver + dynamicBody = 2, +} + +// Motion locks to restrict the body movement +// @ingroup body +MotionLocks :: struct { + // Prevent translation along the x-axis + linearX: bool, + + // Prevent translation along the y-axis + linearY: bool, + + // Prevent translation along the z-axis + linearZ: bool, + + // Prevent rotation around the x-axis + angularX: bool, + + // Prevent rotation around the y-axis + angularY: bool, + + // Prevent rotation around the z-axis + angularZ: bool, +} + +// A body definition holds all the data needed to construct a rigid body. +// You can safely re-use body definitions. Shapes are added to a body after construction. +// Body definitions are temporary objects used to bundle creation parameters. +// Must be initialized using b3DefaultBodyDef(). +// @ingroup body +BodyDef :: struct{ + // The body type: static, kinematic, or dynamic. + type: BodyType, + + // The initial world position of the body. Bodies should be created with the desired position. + // @note Creating bodies at the origin and then moving them nearly doubles the cost of body creation, especially + // if the body is moved after shapes have been added. + position: Pos, + + // The initial world rotation of the body. + rotation: Quat, + + // The initial linear velocity of the body's origin. Usually in meters per second. + linearVelocity: Vec3, + + // The initial angular velocity of the body. Radians per second. + angularVelocity: Vec3, + + // Linear damping is used to reduce the linear velocity. The damping parameter + // can be larger than 1 but the damping effect becomes sensitive to the + // time step when the damping parameter is large. + // Generally linear damping is undesirable because it makes objects move slowly + // as if they are floating. + linearDamping: f32, + + // Angular damping is used to reduce the angular velocity. The damping parameter + // can be larger than 1.0f but the damping effect becomes sensitive to the + // time step when the damping parameter is large. + // Angular damping can be used to slow down rotating bodies. + angularDamping: f32, + + // Scale the gravity applied to this body. Non-dimensional. + gravityScale: f32, + + // Sleep speed threshold, default is 0.05 meters per second + sleepThreshold: f32, + + // Optional body name for debugging. + name: cstring, + + // Use this to store application specific body data. + userData: rawptr, + + // Motions locks to restrict linear and angular movement + motionLocks: MotionLocks, + + // Set this flag to false if this body should never fall asleep. + enableSleep: bool, + + // Is this body initially awake or sleeping? + isAwake: bool, + + // Treat this body as a high speed object that performs continuous collision detection + // against dynamic and kinematic bodies, but not other bullet bodies. + // @warning Bullets should be used sparingly. They are not a solution for general dynamic-versus-dynamic + // continuous collision. They do not guarantee accurate collision if both bodies are fast moving because + // the bullet does a continuous check after all non-bullet bodies have moved. You could get unlucky and have + // the bullet body end a time step very close to a non-bullet body and the non-bullet body then moves over + // the bullet body. In continuous collision, initial overlap is ignored to avoid freezing bodies in place. + // I do not recommend using them for game projectiles if precise collision timing is needed. Instead consider + // using a ray or shape cast. You can use a marching ray or shape cast for projectile that moves over time. + // If you want a fast moving projectile to collide with a fast moving target, you need to consider the relative + // movement in your ray or shape cast. This is out of the scope of Box3D. + // So what are good use cases for bullets? Pinball games or games with dynamic containers that hold other objects. + // It should be a use case where it doesn't break the game if there is a collision missed, but having them + // captured improves the quality of the game. + isBullet: bool, + + // Used to disable a body. A disabled body does not move or collide. + isEnabled: bool, + + // This allows this body to bypass rotational speed limits. Should only be used + // for circular objects, like wheels. + allowFastRotation: bool, + + // Enable contact recycling. True by default. Leaving this enabled improves performance + // but may lead to ghost collision that should be avoided on characters. + enableContactRecycling: bool, + + // Used internally to detect a valid definition. DO NOT SET. + internalValue: c.int, +} + + +// This is used to filter collision on shapes. It affects shape-vs-shape collision +// and shape-versus-query collision (such as b3World_CastRay). +// @ingroup shape +Filter :: struct { + // The collision category bits. Normally you would just set one bit. The category bits should + // represent your application object types. For example: + // @code{.odin} + // MyCategories :: bit_set[MyCategory; u64] + // MyCategory :: enum { + // Static = 0, + // Dynamic = 1, + // Debris = 2, + // Player = 3, + // // etc + // }; + // @endcode + categoryBits: u64, + + // The collision mask bits. This states the categories that this + // shape would accept for collision. + // For example, you may want your player to only collide with static objects + // and other players. + // @code{.odin} + // maskBits = {/Static, .Player}; + // @endcode + maskBits: u64, + + // Collision groups allow a certain group of objects to never collide (negative) + // or always collide (positive). A group index of zero has no effect. Non-zero group filtering + // always wins against the mask bits. + // For example, you may want ragdolls to collide with other ragdolls but you don't want + // ragdoll self-collision. In this case you would give each ragdoll a unique negative group index + // and apply that group index to all shapes on the ragdoll. + groupIndex: c.int, +} + + +// Material properties supported per triangle on meshes and height fields +// @ingroup shape +SurfaceMaterial :: struct { + // The Coulomb (dry) friction coefficient, usually in the range [0,1]. + friction: f32, + + // The coefficient of restitution (bounce) usually in the range [0,1]. + // https://en.wikipedia.org/wiki/Coefficient_of_restitution + restitution: f32, + + // The rolling resistance usually in the range [0,1]. This is only used for spheres and capsules. + rollingResistance: f32, + + // The tangent velocity for conveyor belts. This is local to the shape and will be projected + // onto the contact surface. + tangentVelocity: Vec3, + + // User material identifier. This is passed with query results and to friction and restitution + // combining functions. It is not used internally. + userMaterialId: u64, + + // Custom debug draw color. Ignored if 0. The low 24 bits are RGB. The high byte may + // carry a b3DebugMaterial preset, see b3MakeDebugColor. + // @see b3HexColor + customColor: u32, +} + + +// Shape type +// @ingroup shape +ShapeType :: enum c.int { + // A capsule is an extruded sphere + capsuleShape, + + // A baked compound shape composed of spheres, capsules, hulls, and meshes + compoundShape, + + // A height field useful for terrain + heightShape, + + // A convex hull + hullShape, + + // A triangle soup + meshShape, + + // A sphere with an offset + sphereShape, +} + +// Used to create a shape +// @ingroup shape +ShapeDef :: struct { + // Optional shape name for debugging + name: cstring, + + // Use this to store application specific shape data. + userData: rawptr, + + // Surface material used on mesh shapes per triangle. Ignored for convex shapes. Ignored for compound shapes. + materials: [^]SurfaceMaterial `fmt:"v,materialCount"`, + + // Surface material count. + materialCount: c.int, + + // The base surface material. Ignored for compound shapes. + baseMaterial: SurfaceMaterial, + + // The density, usually in kg/m^3. + density: f32, + + // Explosion scale for b3World_Explode. non-dimensional + explosionScale: f32, + + // Contact filtering data. + filter: Filter, + + // Enable custom filtering. Only one of the two shapes needs to enable custom filtering. See b3WorldDef. + enableCustomFiltering: bool, + + // A sensor shape generates overlap events but never generates a collision response. + // Sensors do not have continuous collision. Instead, use a ray or shape cast for those scenarios. + // Sensors still contribute to the body mass if they have non-zero density. + // @note Sensor events are disabled by default. + // @see enableSensorEvents + isSensor: bool, + + // Enable sensor events for this shape. This applies to sensors and non-sensors. False by default, even for sensors. + enableSensorEvents: bool, + + // Enable contact events for this shape. Only applies to kinematic and dynamic bodies. Ignored for sensors. False by default. + enableContactEvents: bool, + + // Enable hit events for this shape. Only applies to kinematic and dynamic bodies. Ignored for sensors. False by default. + enableHitEvents: bool, + + // Enable pre-solve contact events for this shape. Only applies to dynamic bodies. These are expensive + // and must be carefully handled due to multithreading. Ignored for sensors. + enablePreSolveEvents: bool, + + // When shapes are created they will scan the environment for collision the next time step. This can significantly slow down + // static body creation when there are many static shapes. + // This is flag is ignored for dynamic and kinematic shapes which always invoke contact creation. + invokeContactCreation: bool, + + // Should the body update the mass properties when this shape is created. Default is true. + // Warning: if this is false, you MUST call b3Body_ApplyMassFromShapes or b3Body_SetMassData before simulating the world. + updateBodyMass: bool, + + // Enable speculative collision. Leave this true unless you care about reducing ghost collision + // more than continuous collision under rotation. + // Experimental: this can only disable speculative contact between hulls and triangles (meshes and height fields). + enableSpeculativeContact: bool, + + // Used internally to detect a valid definition. DO NOT SET. + internalValue: c.int, +} + +//! @cond +// Profiling data. Times are in milliseconds. +// @ingroup world +Profile :: struct { + step: f32, + pairs: f32, + collide: f32, + solve: f32, + solverSetup: f32, + constraints: f32, + prepareConstraints: f32, + integrateVelocities: f32, + warmStart: f32, + solveImpulses: f32, + integratePositions: f32, + relaxImpulses: f32, + applyRestitution: f32, + storeImpulses: f32, + splitIslands: f32, + transforms: f32, + sensorHits: f32, + jointEvents: f32, + hitEvents: f32, + refit: f32, + bullets: f32, + sleepIslands: f32, + sensors: f32, +} + +// Counters that give details of the simulation size. +// @ingroup world +Counters :: struct { + bodyCount: c.int, + shapeCount: c.int, + contactCount: c.int, + jointCount: c.int, + islandCount: c.int, + stackUsed: c.int, + arenaCapacity: c.int, + staticTreeHeight: c.int, + treeHeight: c.int, + satCallCount: c.int, + satCacheHitCount: c.int, + byteCount: c.int, + taskCount: c.int, + colorCounts: [24]c.int, + manifoldCounts: [CONTACT_MANIFOLD_COUNT_BUCKETS]c.int, + + // Number of contacts touched by the collide pass + // graph contacts + awake-set non-touching + awakeContactCount: c.int, + + // Number of contacts recycled in the most recent step. + recycledContactCount: c.int, + + // Maximum number of time of impact iterations + distanceIterations: c.int, + pushBackIterations: c.int, + rootIterations: c.int, +} +//! @endcond + +// Joint type enumeration. This is useful because all joint types use b3JointId and sometimes you +// want to get the type of a joint. +// @ingroup joint +JointType :: enum c.int { + parallelJoint, + distanceJoint, + filterJoint, + motorJoint, + prismaticJoint, + revoluteJoint, + sphericalJoint, + weldJoint, + wheelJoint, +} + +// Base joint definition used by all joint types. The local frames are measured from the +// body's origin rather than the center of mass because: +// 1. You might not know where the center of mass will be. +// 2. If you add/remove shapes from a body and recompute the mass, the joints will be broken. +// @ingroup joint +JointDef :: struct { + // User data pointer + userData: rawptr, + + // The first attached body + bodyIdA: BodyId, + + // The second attached body + bodyIdB: BodyId, + + // The first local joint frame + localFrameA: Transform, + + // The second local joint frame + localFrameB: Transform, + + // Force threshold for joint events + forceThreshold: f32, + + // Torque threshold for joint events + torqueThreshold: f32, + + // Constraint hertz (advanced feature) + constraintHertz: f32, + + // Constraint damping ratio (advanced feature) + constraintDampingRatio: f32, + + // Debug draw scale + drawScale: f32, + + // Set this flag to true if the attached bodies should collide + collideConnected: bool, + + // Used internally to detect a valid definition. DO NOT SET. + internalValue: c.int, +} + +// Distance joint definition. +// Connects a point on body A with a point on body B by a segment. +// Useful for ropes and springs. +// @ingroup distance_joint +DistanceJointDef :: struct { + // Base joint definition + using base: JointDef, + + // The rest length of this joint. Clamped to a stable minimum value. + length: f32, + + // Enable the distance constraint to behave like a spring. If false + // then the distance joint will be rigid, overriding the limit and motor. + enableSpring: bool, + + // The lower spring force controls how much tension it can sustain + lowerSpringForce: f32, + + // The upper spring force controls how much compression it can sustain + upperSpringForce: f32, + + // The spring linear stiffness Hertz, cycles per second + hertz: f32, + + // The spring linear damping ratio, non-dimensional + dampingRatio: f32, + + // Enable/disable the joint limit + enableLimit: bool, + + // Minimum length. Clamped to a stable minimum value. + minLength: f32, + + // Maximum length. Must be greater than or equal to the minimum length. + maxLength: f32, + + // Enable/disable the joint motor + enableMotor: bool, + + // The maximum motor force, usually in newtons + maxMotorForce: f32, + + // The desired motor speed, usually in meters per second + motorSpeed: f32, +} + + +// A motor joint is used to control the relative position and velocity between two bodies. +// @ingroup motor_joint +MotorJointDef :: struct { + // Base joint definition + using base: JointDef, + + // The desired linear velocity + linearVelocity: Vec3, + + // The maximum motor force in newtons + maxVelocityForce: f32, + + // The desired angular velocity + angularVelocity: Vec3, + + // The maximum motor torque in newton-meters + maxVelocityTorque: f32, + + // Linear spring hertz for position control + linearHertz: f32, + + // Linear spring damping ratio + linearDampingRatio: f32, + + // Maximum spring force in newtons + maxSpringForce: f32, + + // Angular spring hertz for position control + angularHertz: f32, + + // Angular spring damping ratio + angularDampingRatio: f32, + + // Maximum spring torque in newton-meters + maxSpringTorque: f32, +} + +// A filter joint is used to disable collision between two specific bodies. +// @ingroup filter_joint +FilterJointDef :: struct { + // Base joint definition + using base: JointDef, +} + + +// Parallel joint definition. Constrains the angle between axis z in body A and axis z in body B +// using a spring. Useful to keep a body upright. +// @ingroup parallel_joint +ParallelJointDef :: struct { + // Base joint definition + using base: JointDef, + + // The spring stiffness Hertz, cycles per second + hertz: f32, + + // The spring damping ratio, non-dimensional + dampingRatio: f32, + + // The maximum spring torque, typically in newton-meters. + maxTorque: f32, +} + +// Prismatic joint definition. Body B may slide along the x-axis in local frame A. +// Body B cannot rotate relative to body A. The joint translation is zero when the +// local frame origins coincide in world space. +// @ingroup prismatic_joint +PrismaticJointDef :: struct { + // Base joint definition + using base: JointDef, + + // Enable a linear spring along the prismatic joint axis + enableSpring: bool, + + // The spring stiffness Hertz, cycles per second + hertz: f32, + + // The spring damping ratio, non-dimensional + dampingRatio: f32, + + // The target translation for the joint in meters. The spring-damper will drive + // to this translation. + targetTranslation: f32, + + // Enable/disable the joint limit + enableLimit: bool, + + // The lower translation limit + lowerTranslation: f32, + + // The upper translation limit + upperTranslation: f32, + + // Enable/disable the joint motor + enableMotor: bool, + + // The maximum motor force, typically in newtons + maxMotorForce: f32, + + // The desired motor speed, typically in meters per second + motorSpeed: f32, +} + +// Revolute joint definition. A point on body B is fixed to a point on body A. +// Allows relative rotation about the z-axis. +// @ingroup revolute_joint +RevoluteJointDef :: struct { + // Base joint definition. + using base: JointDef, + + // The bodyB angle minus bodyA angle in the reference state (radians). + // This defines the zero angle for the joint limit. + targetAngle: f32, + + // Enable a rotational spring on the revolute hinge axis. + enableSpring: bool, + + // The spring stiffness Hertz, cycles per second. + hertz: f32, + + // The spring damping ratio, non-dimensional. + dampingRatio: f32, + + // A flag to enable joint limits. + enableLimit: bool, + + // The lower angle for the joint limit in radians. Minimum of -0.99*pi radians. + lowerAngle: f32, + + // The upper angle for the joint limit in radians. Maximum of 0.99*pi radians. + upperAngle: f32, + + // A flag to enable the joint motor. + enableMotor: bool, + + // The maximum motor torque, typically in newton-meters. + maxMotorTorque: f32, + + // The desired motor speed in radians per second. + motorSpeed: f32, +} + +// Spherical joint definition. A point on body B is fixed to a point on body A. +// Allows rotation about the shared point. +// @ingroup spherical_joint +SphericalJointDef :: struct { + // Base joint definition + using base: JointDef, + + // Enable a rotational spring that attempts to align the two joint frames. + enableSpring: bool, + + // The spring stiffness Hertz, cycles per second. This may be clamped internally + // according to the time step to maintain stability. Non-negative number. + hertz: f32, + + // The spring damping ratio, non-dimensional. Non-negative number. + dampingRatio: f32, + + // Target spring rotation, joint frame B relative to joint frame A. + targetRotation: Quat, + + // A flag to enable the cone limit. The cone is centered on the frameA z-axis. + enableConeLimit: bool, + + // The angle for the cone limit in radians. Valid range is [0, pi] + coneAngle: f32, + + // A flag to enable the twist limit. The twist is centered on the frameB z-axis. + enableTwistLimit: bool, + + // The angle for the lower twist limit in radians. Minimum of -0.99*pi radians. + lowerTwistAngle: f32, + + // The angle for the upper twist limit in radians. Maximum of 0.99*pi radians. + upperTwistAngle: f32, + + // A flag to enable the joint motor + enableMotor: bool, + + // The maximum motor torque, typically in newton-meters. Non-negative number. + maxMotorTorque: f32, + + // The desired motor angular velocity in radians per second. + motorVelocity: Vec3, +} + + +// Weld joint definition +// Connects two bodies together rigidly. This constraint provides springs to mimic +// soft-body simulation. +// @note The approximate solver in Box3D cannot hold many bodies together rigidly +// @ingroup weld_joint +WeldJointDef :: struct { + // Base joint definition + using base: JointDef, + + // Linear stiffness expressed as Hertz (cycles per second). Use zero for maximum stiffness. + linearHertz: f32, + + // Angular stiffness as Hertz (cycles per second). Use zero for maximum stiffness. + angularHertz: f32, + + // Linear damping ratio, non-dimensional. Use 1 for critical damping. + linearDampingRatio: f32, + + // Linear damping ratio, non-dimensional. Use 1 for critical damping. + angularDampingRatio: f32, +} + +// Wheel joint definition +// Body A is the chassis and body B is the wheel. +// The wheel rotates around the local z-axis in frame B. +// The wheel translates along the local x-axis in frame A. +// The wheel can optionally steer along the x-axis in frame A. +// @ingroup wheel_joint +WheelJointDef :: struct { + // Base joint definition + using base: JointDef, + + // Enable a linear spring along the local axis + enableSuspensionSpring: bool, + + // Spring stiffness in Hertz + suspensionHertz: f32, + + // Spring damping ratio, non-dimensional + suspensionDampingRatio: f32, + + // Enable/disable the joint linear limit + enableSuspensionLimit: bool, + + // The lower suspension translation limit + lowerSuspensionLimit: f32, + + // The upper translation limit + upperSuspensionLimit: f32, + + // Enable/disable the joint rotational motor + enableSpinMotor: bool, + + // The maximum motor torque, typically in newton-meters + maxSpinTorque: f32, + + // The desired motor speed in radians per second + spinSpeed: f32, + + // Enable steering, otherwise the steering is fixed forward + enableSteering: bool, + + // Steering stiffness in Hertz + steeringHertz: f32, + + // Spring damping ratio, non-dimensional + steeringDampingRatio: f32, + + // The target steering angle in radians + targetSteeringAngle: f32, + + // The maximum steering torque in N*m + maxSteeringTorque: f32, + + // Enable/disable the steering angular limit + enableSteeringLimit: bool, + + // The lower steering angle in radians + lowerSteeringLimit: f32, + + // The upper steering angle in radians + upperSteeringLimit: f32, +} + + +// The explosion definition is used to configure options for explosions. Explosions +// consider shape geometry when computing the impulse. +// @ingroup world +ExplosionDef :: struct { + // Mask bits to filter shapes + maskBits: u64, + + // The center of the explosion in world space + position: Pos, + + // The radius of the explosion + radius: f32, + + // The falloff distance beyond the radius. Impulse is reduced to zero at this distance. + falloff: f32, + + // Impulse per unit area. This applies an impulse according to the shape area that + // is facing the explosion. Explosions only apply to spheres, capsules, and hulls. This + // may be negative for implosions. + impulsePerArea: f32, +} + +/** + * @defgroup event Events + * World event types. + * + * Events are used to collect events that occur during the world time step. These events + * are then available to query after the time step is complete. This is preferable to callbacks + * because Box3D uses multithreaded simulation. + * + * Also when events occur in the simulation step it may be problematic to modify the world, which is + * often what applications want to do when events occur. + * + * With event arrays, you can scan the events in a loop and modify the world. However, you need to be careful + * that some event data may become invalid. There are several samples that show how to do this safely. + * + * @{ + */ + +// A begin-touch event is generated when a shape starts to overlap a sensor shape. +SensorBeginTouchEvent :: struct { + // The id of the sensor shape + sensorShapeId: ShapeId, + + // The id of the shape that began touching the sensor shape + visitorShapeId: ShapeId, +} + +// An end touch event is generated when a shape stops overlapping a sensor shape. +// These include things like setting the transform, destroying a body or shape, or changing +// a filter. You will also get an end event if the sensor or visitor are destroyed. +// Therefore you should always confirm the shape id is valid using b3Shape_IsValid. +SensorEndTouchEvent :: struct { + // The id of the sensor shape + // @warning this shape may have been destroyed + // @see b3Shape_IsValid + sensorShapeId: ShapeId, + + // The id of the shape that stopped touching the sensor shape + // @warning this shape may have been destroyed + // @see b3Shape_IsValid + visitorShapeId: ShapeId, +} + +// Sensor events are buffered in the world and are available +// as begin/end overlap event arrays after the time step is complete. +// Note: these may become invalid if bodies and/or shapes are destroyed +SensorEvents :: struct { + // Array of sensor begin touch events + beginEvents: [^]SensorBeginTouchEvent `fmt:"v,beginCount"`, + + // Array of sensor end touch events + endEvents: [^]SensorEndTouchEvent `fmt:"v,endCount"`, + + // The number of begin touch events + beginCount: c.int, + + // The number of end touch events + endCount: c.int, +} + +// A begin-touch event is generated when two shapes begin touching. +ContactBeginTouchEvent :: struct { + // Id of the first shape + shapeIdA: ShapeId, + + // Id of the second shape + shapeIdB: ShapeId, + + // The transient contact id. This contact may be destroyed automatically when the world is modified or simulated. + // Use b3Contact_IsValid before using this id. + contactId: ContactId, +} + +// An end touch event is generated when two shapes stop touching. +// You will get an end event if you do anything that destroys contacts previous to the last +// world step. These include things like setting the transform, destroying a body +// or shape, or changing a filter or body type. +ContactEndTouchEvent :: struct { + // Id of the first shape + // @warning this shape may have been destroyed + // @see b3Shape_IsValid + shapeIdA: ShapeId, + + // Id of the first shape + // @warning this shape may have been destroyed + // @see b3Shape_IsValid + shapeIdB: ShapeId, + + // Id of the contact. + // @warning this contact may have been destroyed + // @see b3Contact_IsValid + contactId: ContactId, +} + +// A hit touch event is generated when two shapes collide with a speed faster than the hit speed threshold. +// This may be reported for speculative contacts that have a confirmed impulse. +ContactHitEvent :: struct { + // Id of the first shape + shapeIdA: ShapeId, + + // Id of the second shape + shapeIdB: ShapeId, + + // Id of the contact. + // @warning this contact may have been destroyed + // @see b3Contact_IsValid + contactId: ContactId, + + // Point where the shapes hit at the beginning of the time step. + // This is a mid-point between the two surfaces. It could be at speculative + // point where the two shapes were not touching at the beginning of the time step. + point: Pos, + + // Normal vector pointing from shape A to shape B + normal: Vec3, + + // The speed the shapes are approaching. Always positive. Typically in meters per second. + approachSpeed: f32, + + // User material on shape A + userMaterialIdA: u64, + + // User material on shape B + userMaterialIdB: u64, +} + +// Contact events are buffered in the world and are available +// as event arrays after the time step is complete. +// Note: these may become invalid if bodies and/or shapes are destroyed +ContactEvents :: struct { + // Array of begin touch events + beginEvents: [^]ContactBeginTouchEvent `fmt:"v,beginCount"`, + + // Array of end touch events + endEvents: [^]ContactEndTouchEvent `fmt:"v,endCount"`, + + // Array of hit events + hitEvents: [^]ContactHitEvent `fmt:"v,hitCount"`, + + // Number of begin touch events + beginCount: c.int, + + // Number of end touch events + endCount: c.int, + + // Number of hit events + hitCount: c.int, +} + +// Body move events triggered when a body moves. +// Triggered when a body moves due to simulation. Not reported for bodies moved by the user. +// This also has a flag to indicate that the body went to sleep so the application can also +// sleep that actor/entity/object associated with the body. +// On the other hand if the flag does not indicate the body went to sleep then the application +// can treat the actor/entity/object associated with the body as awake. +// This is an efficient way for an application to update game object transforms rather than +// calling functions such as b3Body_GetTransform() because this data is delivered as a contiguous array +// and it is only populated with bodies that have moved. +// @note If sleeping is disabled all dynamic and kinematic bodies will trigger move events. +BodyMoveEvent :: struct { + // The body user data. + userData: rawptr, + + // The body transform. + transform: WorldTransform, + + // The body id. + bodyId: BodyId, + + // Did the body fall asleep this time step? + fellAsleep: bool, +} + +// Body events are buffered in the world and are available +// as event arrays after the time step is complete. +// Note: this data becomes invalid if bodies are destroyed +BodyEvents :: struct { + // Array of move events + moveEvents: [^]BodyMoveEvent `fmt:"v,moveCount"`, + + // Number of move events + moveCount: c.int, +} + +// Joint events report joints that are awake and have a force and/or torque exceeding the threshold +// The observed forces and torques are not returned for efficiency reasons. +JointEvent :: struct { + // The joint id + jointId: JointId, + + // The user data from the joint for convenience + userData: rawptr, +} + +// Joint events are buffered in the world and are available +// as event arrays after the time step is complete. +// Note: this data becomes invalid if joints are destroyed +JointEvents :: struct { + // Array of events + jointEvents: [^]JointEvent `fmt:"v,count"`, + + // Number of events + count: c.int, +} + +// The contact data for two shapes. By convention the manifold normal points +// from shape A to shape B. +// @see b3Shape_GetContactData() and b3Body_GetContactData() +ContactData :: struct { + // The contact id. You may hold onto this to track a contact across time steps. + // This id may become orphaned. Use b3Contact_IsValid before using it for other functions. + contactId: ContactId, + + // The first shape id. + shapeIdA: ShapeId, + + // The second shape id. + shapeIdB: ShapeId, + + // The contact manifold. This points to internal data and may become invalid. Do not store + // this pointer. + manifolds: [^]Manifold `fmt:"v,manifoldCount"`, + + // The number of contact manifolds. For mesh and height-field collision there can be multiple manifolds. + manifoldCount: c.int, +} + +/**@}*/ // event + +/** + * @defgroup query Query + * @brief Query types and functions + * + * Queries include ray casts, shapes casts, overlap, distance, and time of impact. + * @{ + */ + +// The query filter is used to filter collisions between queries and shapes. For example, +// you may want a ray-cast representing a projectile to hit players and the static environment +// but not debris. +QueryFilter :: struct { + // The collision category bits of this query. Normally you would just set one bit. + categoryBits: u64, + + // The collision mask bits. This states the shape categories that this + // query would accept for collision. + maskBits: u64, + + // Optional id combined with @ref name to identify this query in a recording, e.g. an entity id. + // Need not be unique on its own. 0 with a null name means untagged. Ignored when not recording. + id: u64, + + // Optional label combined with @ref id to identify this query, e.g. "bullet". Need not be unique + // on its own. The recorder hashes (id, name) into one stable key the viewer tracks the query by, + // so the same id and name pair identifies the same query across frames. NULL means none. Ignored + // when not recording. + name: cstring, +} + + +// Low level ray cast input data. +RayCastInput :: struct { + // Start point of the ray cast. + origin: Vec3, + + // Translation of the ray cast. + // end = start + translation. + translation: Vec3, + + // The maximum fraction of the translation to consider, typically 1 + maxFraction: f32, +} + +// Result from b3World_RayCastClosest. +RayResult :: struct { + // The shape hit. + shapeId: ShapeId, + + // The world point of the hit. + point: Pos, + + // The world normal of the shape surface at the hit point. + normal: Vec3, + + // The user material id at the hit point. This can be per triangle + // if the shape is a mesh, height-field, or compound with child mesh. + userMaterialId: u64, + + // The fraction of the input ray. + fraction: f32, + + // The triangle index if the shape is a mesh, height-field, or compound with + // child mesh. + triangleIndex: c.int, + + // The child index if the shape is a compound. + childIndex: c.int, + + // The number of BVH nodes visited. Diagnostic. + nodeVisits: c.int, + + // The number of BVH leaves visited. Diagnostic. + leafVisits: c.int, + + // Did the ray hit? If false, all other data is invalid. + hit: bool, +} + +// A shape proxy is used by the GJK algorithm. It can represent a convex shape. +ShapeProxy :: struct { + // The point cloud. + points: [^]Vec3 `fmt:"v,count"`, + + // The number of points. Do not exceed B3_MAX_SHAPE_CAST_POINTS. + count: c.int, + + // The external radius of the point cloud. + radius: f32, +} + +// Low level shape cast input in generic form. This allows casting an arbitrary point +// cloud wrap with a radius. For example, a sphere is a single point with a non-zero radius. +// A capsule is two points with a non-zero radius. A box is four points with a zero radius. +ShapeCastInput :: struct { + // A generic query shape. + proxy: ShapeProxy, + + // The translation of the shape cast. + translation: Vec3, + + // The maximum fraction of the translation to consider, typically 1. + maxFraction: f32, + + // Allow shape cast to encroach when initially touching. This only works if the radius is greater than zero. + canEncroach: bool, +} + +// Input for sweeping an AABB through a dynamic tree. The box is in the tree's world float frame. +// The caller folds the cast shape radius and any world origin into the box, so the tree traversal +// stays a conservative box sweep and the precise narrow phase happens per shape in the callback. +BoxCastInput :: struct { + // The AABB to cast, in the tree's frame. + box: AABB, + + // The sweep translation. + translation: Vec3, + + // The maximum fraction of the translation to consider, typically 1. + maxFraction: f32, +} + +// Low level ray cast or shape-cast output data. +CastOutput :: struct { + // The surface normal at the hit point. + normal: Vec3, + + // The surface hit point. + point: Vec3, + + // The fraction of the input translation at collision. + fraction: f32, + + // The number of iterations used. + iterations: c.int, + + // The index of the mesh or height field triangle hit. + triangleIndex: c.int, + + // The index of the compound child shape. + childIndex: c.int, + + // The material index. May be -1 for null. + materialIndex: c.int, + + // Did the cast hit? + hit: bool, +} + + +// Ray cast or shape-cast output in world space. The hit point is a world position so the result +// stays precise far from the world origin. Mirrors b3CastOutput with a double precision point. +WorldCastOutput :: struct { + // The surface normal at the hit point. + normal: Vec3, + + // The surface hit point in world space. + point: Pos, + + // The fraction of the input translation at collision. + fraction: f32, + + // The number of iterations used. + iterations: c.int, + + // The index of the mesh or height field triangle hit. + triangleIndex: c.int, + + // The index of the compound child shape. + childIndex: c.int, + + // The material index. May be -1 for null. + materialIndex: c.int, + + // Did the cast hit? + hit: bool, +} + + +// Body cast result for ray and shape casts. +BodyCastResult :: struct { + // The shape hit. + shapeId: ShapeId, + + // The world point on the shape surface. + point: Pos, + + // The world normal vector on the shape surface. + normal: Vec3, + + // The fraction along the ray hit. + // hit point = origin + fraction * translation + fraction: f32, + + // The triangle index if the shape is a mesh or height-field. + triangleIndex: c.int, + + // The user material id at the hit point. This can be per triangle + // if the shape is a mesh, height-field, or compound with child mesh. + userMaterialId: u64, + + // The number of iterations used. Diagnostic. + iterations: c.int, + + // Did the cast hit? If false, all other fields are invalid. + hit: bool, +} + +// Used to warm start the GJK simplex. If you call this function multiple times with nearby +// transforms this might improve performance. Otherwise you can zero initialize this. +// The distance cache must be initialized to zero on the first call. +// Users should generally just zero initialize this structure for each call. +SimplexCache :: struct { + // Value use to compare length, area, volume of two simplexes. + metric: f32, + + // todo use an index of 0xFF as a sentinel and remove the count + // The number of stored simplex points + count: u16, + + // The cached simplex indices on shape A + indexA: [4]u8, + + // The cached simplex indices on shape B + indexB: [4]u8, +} + +emptyDistanceCache :: SimplexCache{} + +// Input parameters for b3ShapeCast +ShapeCastPairInput :: struct { + proxyA: ShapeProxy, //< The proxy for shape A + proxyB: ShapeProxy, //< The proxy for shape B + transform: Transform, //< Transform of shape B in shape A's frame, the relative pose B in A + translationB: Vec3, //< The translation of shape B, in A's frame + maxFraction: f32, //< The fraction of the translation to consider, typically 1 + canEncroach: bool, //< Allows shapes with a radius to move slightly closer if already touching +} + +// Input for ShapeDistance +DistanceInput :: struct { + // The proxy for shape A + proxyA: ShapeProxy, + + // The proxy for shape B + proxyB: ShapeProxy, + + // Transform of shape B in shape A's frame, the relative pose B in A + // (b3InvMulWorldTransforms( worldA, worldB )). The query is origin independent and runs in frame A. + transform: Transform, + + // Should the proxy radius be considered? + useRadii: bool, +} + +// Output for b3ShapeDistance +DistanceOutput :: struct { + pointA: Vec3, //< Closest point on shapeA, in shape A's frame + pointB: Vec3, //< Closest point on shapeB, in shape A's frame + normal: Vec3, //< A to B normal in shape A's frame. Invalid if distance is zero. + distance: f32, //< The final distance, zero if overlapped + iterations: c.int, //< Number of GJK iterations used + simplexCount: c.int, //< The number of simplexes stored in the simplex array +} + +// Simplex vertex for debugging the GJK algorithm +SimplexVertex :: struct { + wA: Vec3, //< support point in proxyA + wB: Vec3, //< support point in proxyB + w: Vec3, //< wB - wA + a: f32, //< barycentric coordinates + indexA: c.int, //< wA index + indexB: c.int, //< wB index +} + +// Simplex from the GJK algorithm +Simplex :: struct { + vertices: [4]SimplexVertex `fmt:"v,count"`, //< vertices + count: c.int, //< number of valid vertices +} + +// This describes the motion of a body/shape for TOI computation. Shapes are defined with respect to the body origin, +// which may not coincide with the center of mass. However, to support dynamics we must interpolate the center of mass +// position. +Sweep :: struct { + localCenter: Vec3, //< Local center of mass position + c1: Vec3, //< Starting center of mass world position + c2: Vec3, //< Ending center of mass world position + q1: Quat, //< Starting world rotation + q2: Quat, //< Ending world rotation +} + +// Time of impact input +TOIInput :: struct { + proxyA: ShapeProxy, //< The proxy for shape A + proxyB: ShapeProxy, //< The proxy for shape B + sweepA: Sweep, //< The movement of shape A + sweepB: Sweep, //< The movement of shape B + maxFraction: f32, //< Defines the sweep interval [0, tMax] +} + +// Describes the TOI output +TOIState :: enum c.int { + Unknown, + Failed, + Overlapped, + Hit, + Separated, +} + +// Time of impact output +TOIOutput :: struct { + // The type of result + state: TOIState, + + // The hit point + point: Vec3, + + // The hit normal + normal: Vec3, + + // The sweep time of the collision + fraction: f32, + + // The final distance + distance: f32, + + // Number of outer iterations + distanceIterations: c.int, + + // Total number of push back iterations + pushBackIterations: c.int, + + // Total number of root iterations + rootIterations: c.int, + + // Indicates that the time of impact detected initial + // overlap and used a fallback sphere as a last ditch effort + // to prevent tunneling. + usedFallback: bool, +} + +/**@}*/ // query + +/** + * @defgroup tree Dynamic Tree + * The dynamic tree is a binary AABB tree to organize and query large numbers of geometric objects + * + * Box3D uses the dynamic tree internally to sort collision shapes into a binary bounding volume hierarchy. + * This data structure may have uses in games for organizing other geometry data and may be used independently + * of Box3D rigid body simulation. + * + * A dynamic AABB tree broad-phase, inspired by Nathanael Presson's btDbvt. + * A dynamic tree arranges data in a binary tree to accelerate + * queries such as AABB queries and ray casts. Leaf nodes are proxies + * with an AABB. These are used to hold a user collision object. + * Nodes are pooled and relocatable, so I use node indices rather than pointers. + * The dynamic tree is made available for advanced users that would like to use it to organize + * spatial game data besides rigid bodies. + * @{ + */ + +// Flags for tree nodes. For internal usage. +TreeNodeFlags :: distinct bit_set[TreeNodeFlag; u16] + +TreeNodeFlag :: enum u16 { + allocatedNode = 0, + enlargedNode = 1, + leafNode = 2, +} + +// Tree node child indices. For internal usage. +TreeNodeChildren :: struct { + child1: c.int, //< child node index 1 + child2: c.int, //< child node index 2 +} + +// A node in the dynamic tree. This is private data placed here for performance reasons. +// todo test padding to 64 bytes to avoid straddling cache lines +TreeNode :: struct { + // The node bounding box + aabb: AABB, // 24 + + // Category bits for collision filtering + categoryBits: u64, // 8 + + using _: struct #raw_union { + // Children (internal node) + children: TreeNodeChildren, + + // User data (leaf node) + userData: u64, + }, // 8 + + using _: struct #raw_union { + // The node parent index (allocated node) + parent: c.int, + + // The node freelist next index (free node) + next: c.int, + }, // 4 + + // Height of the node. Leaves have a height of 0. + height: u16, // 2 + + // @see TreeNodeFlags + flags: TreeNodeFlags, // 2 +} + +// Dynamic tree version for compatibility testing. +DYNAMIC_TREE_VERSION :: 0x93EDAF889FD30B4A + +// The dynamic tree structure. This should be considered private data. +// It is placed here for performance reasons. +DynamicTree :: struct { + // The dynamic tree version. Always the first field. Useful + // if the tree is serialized. + version: u64, + + // The tree nodes + nodes: [^]TreeNode `fmt:"v,nodeCapacity"`, + + // The root index + root: c.int, + + // The number of nodes + nodeCount: c.int, + + // The allocated node space + nodeCapacity: c.int, + + // Number of proxies created + proxyCount: c.int, + + // Node free list + freeList: c.int, + + // Leaf indices for rebuild + leafIndices: [^]int, + + // Leaf bounding boxes for rebuild + leafBoxes: [^]AABB, + + // Leaf bounding box centers for rebuild + leafCenters: [^]Vec3, + + // Bins for sorting during rebuild + binIndices: [^]c.int, + + // Allocated space for rebuilding + rebuildCapacity: c.int, +} + +// These are performance results returned by dynamic tree queries. +TreeStats :: struct { + // Number of internal nodes visited during the query + nodeVisits: c.int, + + // Number of leaf nodes visited during the query + leafVisits: c.int, +} + +// This function receives proxies found in the AABB query. +// @return true if the query should continue +TreeQueryCallbackFcn :: proc "c" (proxyId: c.int, userData: u64, ctx: rawptr) -> bool + +// This function receives the minimum distance squared so far and proxy to check in the closest query. +// @return minimum distance squared to user objects in the proxy +TreeQueryClosestCallbackFcn :: proc "c" (distanceSqrMin: f32, proxyId: c.int, userData: u64, ctx: rawptr) -> f32 + +// This function receives clipped AABB cast input for a proxy. The function returns the new cast +// fraction. +// - return a value of 0 to terminate the cast +// - return a value less than input->maxFraction to clip the cast +// - return a value of input->maxFraction to continue the cast without clipping +TreeBoxCastCallbackFcn :: proc "c" (#by_ptr input: BoxCastInput, proxyId: c.int, userData: u64, ctx: rawptr) -> f32 + +// This function receives clipped ray cast input for a proxy. The function +// returns the new ray fraction. +// - return a value of 0 to terminate the ray cast +// - return a value less than input->maxFraction to clip the ray +// - return a value of input->maxFraction to continue the ray cast without clipping +TreeRayCastCallbackFcn :: proc "c" (#by_ptr input: RayCastInput, proxyId: c.int, userData: u64, ctx: rawptr) -> f32 + +/**@}*/ // tree + +/** + * @defgroup character Character Mover + * Character movement solver + * @{ + */ + +// The plane between a character mover and a shape +PlaneResult :: struct { + // Outward pointing plane. + plane: Plane, + + // Closest point on the shape. May not be unique. + point: Vec3, +} + +// These are collision planes that can be fed to b3SolvePlanes. Normally +// this is assembled by the user from plane results in b3PlaneResult. +CollisionPlane :: struct { + // The collision plane between the mover and some shape. + plane: Plane, + + // Setting this to FLT_MAX makes the plane as rigid as possible. Lower values can + // make the plane collision soft. Usually in meters. + pushLimit: f32, + + // The push on the mover determined by b3SolvePlanes. Usually in meters. + push: f32, + + // Indicates if b3ClipVector should clip against this plane. Should be false for soft collision. + clipVelocity: bool, +} + +// Result returned by b3SolvePlanes. +PlaneSolverResult :: struct { + // The final relative translation. + delta: Vec3, + + // The number of iterations used by the plane solver. For diagnostics. + iterationCount: c.int, +} + +// Body plane result for movers. +BodyPlaneResult :: struct { + // The shape id on the body. + shapeId: ShapeId, + + // The plane result. + result: PlaneResult, +} + +// Used to collect collision planes for character movers. +// Return true to continue gathering planes. +PlaneResultFcn :: proc "c" (shapeId: ShapeId, plane: [^]PlaneResult, planeCount: c.int, ctx: rawptr) -> bool + +// Used to filter shapes for shape casting character movers. +// Return true to accept the collision +MoverFilterFcn :: proc "c" (shapeId: ShapeId, ctx: rawptr) -> bool + +/**@}*/ // mover + +/** + * @defgroup geometry Geometry + * @brief Geometry types and algorithms + * + * Definitions of spheres, capsules, hulls, meshes, height fields, and compounds. + * @{ + */ + +// This holds the mass data computed for a shape. +MassData :: struct { + // The shape mass + mass: f32, + + // The local center of mass position. + center: Vec3, + + // The inertia tensor about the shape center of mass. + inertia: Matrix3, +} + +/** + * @defgroup sphere Sphere + * @brief Sphere primitive + * @{ + */ + +// A solid sphere +Sphere :: struct { + // The local center + center: Vec3, + + // The radius + radius: f32, +} + +/**@}*/ // sphere + +/** + * @defgroup capsule Capsule + * @brief Capsule primitive + * @{ + */ + +// A solid capsule can be viewed as two hemispheres connected +// by a rectangle. +Capsule :: struct { + // Local center of the first hemisphere + center1: Vec3, + + // Local center of the second hemisphere + center2: Vec3, + + // The radius of the hemispheres + radius: f32, +} + +/**@}*/ // capsule + +/** + * @defgroup hull Convex Hull + * @brief Convex hull primitive + * @{ + */ + +// A hull vertex. Identified by a half-edge with this +// vertex as its tail. +HullVertex :: struct { + // A half-edge that has this vertex as the origin + // Can be used along with edge twins and winding order + // to traverse all the edges connected to this vertex. + edge: u8, +} + +// Half-edge for hull data structure +HullHalfEdge :: struct { + // Next edge index CCW + next: u8, + + // Twin edge index + twin: u8, + + // index of origin vertex and point + origin: u8, + + // Face to the left of this edge + face: u8, +} + +// A hull face. Hulls use a half-edge data structure, so a face +// can be determined from a single half-edge index. +HullFace :: struct { + // An arbitrary half-edge on this face + edge: u8, +} + +// 64-bit hull version. Useful for validating serialized data. +HULL_VERSION :: 0x9D4716CE3793900E + +// A convex hull. +// @note This data structure has data hanging off the end and cannot be directly copied. +HullData :: struct { + // Version must be first and match B3_HULL_VERSION + version: u64, + + // The total number of bytes for this hull. + byteCount: c.int, + + // Hash of this hull (this field is zero when the hash is computed). + hash: u32, + + // Axis-aligned box in local space. + aabb: AABB, + + // Surface area, typically in squared meters. + surfaceArea: f32, + + // Volume, typically in m^3. + volume: f32, + + // The radius of the largest sphere at the center. + innerRadius: f32, + + // The local centroid + center: Vec3, + + // The inertia tensor about the centroid. + centralInertia: Matrix3, + + // The vertex count. + vertexCount: c.int, + + // Offset of the vertex array in bytes from the struct address. + vertexOffset: c.int, + + // Offset of the point array in bytes from the struct address. + pointOffset: c.int, + + // This is the half-edge count (double the edge count) + edgeCount: c.int, + + // Offset of the edge array in bytes from the struct address. + edgeOffset: c.int, + + // The face count. Hulls faces are convex polygons. + faceCount: c.int, + + // Offset of the face array in bytes from the struct address. + faceOffset: c.int, + + // Offset of the face plane array in bytes from the struct address. + planeOffset: c.int, + + // Explicit padding. Hull identity is a content hash and memcmp over raw bytes, + // so there must be no unnamed padding for struct copies to scramble. + padding: c.int, +} + +// Efficient box hull +BoxHull :: struct { + // The embedded hull. So the offsets index into the arrays that follow. + base: HullData, + boxVertices: [8]HullVertex, //< Box vertices. + boxPoints: [8]Vec3, //< Box points. + boxEdges: [24]HullHalfEdge, //< Box half-edges. + boxFaces: [6]HullFace, //< Box faces. + padding: [2]u8, //< Explicit padding, see b3HullData::padding. + boxPlanes: [6]Plane, //< Box face planes. +} + +/**@}*/ // hull + +/** + * @defgroup mesh Triangle Mesh + * @brief Triangle mesh collision shape + * @{ + */ + +// This is used to create a re-usable collision mesh +MeshDef :: struct { + // Triangle vertices + vertices: [^]Vec3 `fmt:"v,vertexCount"`, + + // Triangle vertex indices. 3 for each triangle. + indices: [^]i32, + + // Triangle material index. 1 per triangle. Indexes into b3ShapeDef::materials. + // This allows different run-time material data to be associated with different + // instances of this mesh. + materialIndices: [^]u8, + + // Tolerance for vertex welding in length units. + weldTolerance: f32, + + // The vertex count. Must be 3 or more. + vertexCount: c.int, + + // The triangle count. Must be 1 or more. + triangleCount: c.int, + + // Optionally weld nearby vertices. + weldVertices: bool, + + // Use the median split instead of SAH to speed up mesh creation. Good + // for meshes that are structured like a grid. + useMedianSplit: bool, + + // Compute triangle adjacency information using shared edges + identifyEdges: bool, +} + +// 64-bit mesh version. Useful for validating serialized data. +MESH_VERSION :: 0xABD11AB62A6E886D + +// Triangle mesh edge flags. + +MeshEdgeFlags :: distinct bit_set[MeshEdgeFlag; c.uint] +MeshEdgeFlag :: enum c.uint { + concaveEdge1 = 0, + concaveEdge2 = 1, + concaveEdge3 = 2, + + inverseConcaveEdge1 = 4, + inverseConcaveEdge2 = 5, + inverseConcaveEdge3 = 6, +} + +concaveEdge1 :: MeshEdgeFlags{.concaveEdge1} +concaveEdge2 :: MeshEdgeFlags{.concaveEdge2} +concaveEdge3 :: MeshEdgeFlags{.concaveEdge3} +inverseConcaveEdge1 :: MeshEdgeFlags{.inverseConcaveEdge1} +inverseConcaveEdge2 :: MeshEdgeFlags{.inverseConcaveEdge2} +inverseConcaveEdge3 :: MeshEdgeFlags{.inverseConcaveEdge3} +allConcaveEdges :: concaveEdge1 + concaveEdge2 + concaveEdge3 +flatEdge1 :: concaveEdge1 + inverseConcaveEdge1 +flatEdge2 :: concaveEdge2 + inverseConcaveEdge2 +flatEdge3 :: concaveEdge3 + inverseConcaveEdge3 +allFlatEdges :: flatEdge1 + flatEdge2 + flatEdge3 + +// A mesh triangle. +MeshTriangle :: struct { + index1: i32, //< Index of vertex 1. + index2: i32, //< Index of vertex 2. + index3: i32, //< Index of vertex 3. +} + +// A mesh BVH node. +MeshNode :: struct { + // The lower bound of the node AABB. Strategic placement for SIMD. + lowerBound: Vec3, + + // Anonymous union. + data: struct #raw_union { + // Internal node + asNode: bit_field u32 { + // Split axis. 0, 1, or 2. + axis: u32 | 2, + // Offset of the second child node. + childOffset: u32 | 30, + }, + + // Leaf node + asLeaf: bit_field u32 { + // Aligned with axis above and has value of 3 if this is a leaf. + type: u32 | 2, + + // The number of triangles for this leaf node. + triangleCount: u32 | 30, + }, + }, + + // The upper bound of the node AABB. Strategic placement for SIMD. + upperBound: Vec3, + + // The index of the leaf triangles. + triangleOffset: u32, +} + +// This is a sorted triangle collision bounding volume hierarchy. +// @note This struct has data hanging off the end and cannot be directly copied. +MeshData :: struct { + // Version must be first. + version: u64, + + // The total number of bytes for this mesh. + byteCount: c.int, + + // Hash of this mesh (this field is zero when the hash is computed) + hash: u32, + + // Local axis-aligned box. + bounds: AABB, + + // Combined surface area of all triangles. Single-sided. + surfaceArea: f32, + + // The height of the bounding volume hierarchy. + treeHeight: c.int, + + // The number of degenerate triangles. Diagnostic. + degenerateCount: c.int, + + // Offset of the node array in bytes from the struct address. + nodeOffset: c.int, + + // The number of BVH nodes. + nodeCount: c.int, + + // Offset of the vertex array in bytes from the struct address. + vertexOffset: c.int, + + // The number of vertices. + vertexCount: c.int, + + // Offset of the triangle array in bytes from the struct address. + triangleOffset: c.int, + + // The number of triangles. + triangleCount: c.int, + + // Offset of the material array in bytes from the struct address. + materialOffset: c.int, + + // The number of materials. + materialCount: c.int, + + // Offset of the triangle flag array in bytes from the struct address. + flagsOffset: c.int, +} + +// This allows mesh data to be re-used with different scales. +Mesh :: struct { + // Immutable pointer to the mesh data. + data: ^MeshData, + + // This scale may be non-uniform and have negative components. However, + // no component may be very small in magnitude. + scale: Vec3, +} + +/**@}*/ // mesh + +/** + * @defgroup height_field Height Field + * @brief Height field collision shape + * @{ + */ + +// Data used to create a height field +HeightFieldDef :: struct { + // Grid point heights + // count = countX * countZ + heights: [^]f32, + + // Grid cell material + // A value of 0xFF is reserved for holes + // count = (countX - 1) * (countZ - 1) + materialIndices: [^]u8, + + // The height field scale. All components must be positive values. + scale: Vec3, + + // The number of grid lines along the x-axis. + countX: c.int, + + // The number of grid lines along the z-axis. + countZ: c.int, + + // Global minimum and maximum heights used for quantization. This is important + // if you want height fields to be placed next to each other and line up exactly. + // In that case, both height fields should use the same minimum and maximum heights. + // All height values are clamped to this range. + // These values are in unscaled space. + globalMinimumHeight: f32, + + // The maximum. + globalMaximumHeight: f32, + + // Use clock-wise winding. This effectively inverts the height-field along the y-axis. + clockwiseWinding: bool, +} + +// This material index is used to designate holes in a height field. +HEIGHT_FIELD_HOLE :: 0xFF + +// 64-bit height-field version. Useful for validating serialized data. +HEIGHT_FIELD_VERSION :: 0x8B18CBD138A6BC84 + +// A height field with compressed storage. +// @note This data structure has data hanging off the end and cannot be directly copied. +HeightFieldData :: struct { + // Version must be first and match B3_HEIGHT_FIELD_VERSION + version: i64, + + // The total number of bytes for this height field. + byteCount: c.int, + + // Hash of this height field (this field is zero when the hash is computed). + hash: u32, + + // The local axis-aligned bounding box. + aabb: AABB, + + // The minimum y value. + minHeight: f32, + + // The maximum y value + maxHeight: f32, + + // The quantization scale. + heightScale: f32, + + // The overall scale. + scale: Vec3, + + // The number of grid columns along the local x-axis. + columnCount: c.int, + + // The number of grid rows along the local z-axis. + rowCount: c.int, + + // Offset of the compressed height array in bytes from the struct address. + // uint16_t, one per grid point. + heightsOffset: c.int, + + // Offset of the material index array in bytes from the struct address. + // uint8_t, one per cell. + materialOffset: c.int, + + // Offset of the flag array in bytes from the struct address. + // uint8_t, one per triangle. + flagsOffset: c.int, + + // Triangle winding. + clockwise: bool, + + // Explicit padding. Identity is a content hash over raw bytes, so there must + // be no unnamed padding for struct copies to scramble. + padding: [3]u8, +} + +/**@}*/ // height_field + +/** + * @defgroup compound Compound + * @brief Compound collision shape + * @{ + */ + +// Definition for a capsule in a compound shape. +CompoundCapsuleDef :: struct { + // Local capsule. + capsule: Capsule, + + // Material properties. + material: SurfaceMaterial, +} + +// Definition for a convex hull in a compound shape. +CompoundHullDef :: struct { + // Shared hull. + hull: ^HullData, + + // Transform of the shared hull into compound local space. + transform: Transform, + + // Material properties. + material: SurfaceMaterial, +} + +// Definition for a triangle mesh in a compound shape. +CompoundMeshDef :: struct { + // Shared mesh. + meshData: ^MeshData, + + // Transform of the shared mesh into compound local space. + transform: Transform, + + // Local space non-uniform mesh scale. May have negative components. + scale: Vec3, + + // Material properties. + // This array must line up with the material indices on the triangles. + materials: [^]SurfaceMaterial `fmt:"v,materialCount"`, + + // Number of materials. + materialCount: c.int, +} + +// Definition for a sphere in a compound shape. +CompoundSphereDef :: struct { + // Local sphere. + sphere: Sphere, + + // Material properties. + material: SurfaceMaterial, +} + +// Definition for creating a compound shape. All this data is fully cloned +// into the run-time compound shape. +CompoundDef :: struct { + // Capsule instances. + capsules: [^]CompoundCapsuleDef `fmt:"v,capsuleCount"`, + + // Number of capsules. + capsuleCount: int, + + // Hulls instances. + hulls: [^]CompoundHullDef `fmt:"v,hullCount"`, + + // Number of hull instances. + hullCount: int, + + // Mesh instances. + meshes: [^]CompoundMeshDef `fmt:"v,meshCount"`, + + // Number of mesh instances. + meshCount: int, + + // Sphere instances. + spheres: [^]CompoundSphereDef `fmt:"v,sphereCount"`, + + // Number of spheres. + sphereCount: int, +} + +// The compound version depends on the tree, mesh, and hull versions. +COMPOUND_VERSION :: 0x830778DB07086EB4 ~ DYNAMIC_TREE_VERSION ~ MESH_VERSION ~ HULL_VERSION + +// Meshes used in compounds have limited space for materials. If you have +// a mesh with many materials, you can use it outside of the compound. +MAX_COMPOUND_MESH_MATERIALS :: 4 + +// The runtime data for a baked compound shape. This is a potentially large yet highly optimized +// data structure. It can contain thousands of child shapes, yet at runtime it populates +// into the world as a single shape in the runtime broad-phase. +// This data structure has data living off the end and must be accessed using offsets. +// Accessors are provided for user relevant data. +// Note: you don't need to use this to create runtime compounds. For runtime compounds you can +// add multiple shapes to a body using the regular shape creation functions. +CompoundData :: struct { + // The compound version is always first. + version: u64, + + // The total number of bytes for this compound. + byteCount: c.int, + + // Offset of the tree node array in bytes from the struct address. + nodeOffset: c.int, + + // Immutable dynamic tree. The tree node pointer must be fixed up using the node offset + tree: DynamicTree, + + // Offset of the material array in bytes from the struct address. + materialOffset: c.int, + + // The number of materials. + materialCount: c.int, + + // Offset of the capsule array in bytes from the struct address. + capsuleOffset: c.int, + + // The number of capsules. + capsuleCount: c.int, + + // Offset of the hull instance array in bytes from the struct address. + hullOffset: c.int, + + // The number of hull instances. + hullCount: c.int, + + // The number of unique hulls. Diagnostic. + sharedHullCount: c.int, + + // Offset of the mesh instance array in bytes from the struct address. + meshOffset: c.int, + + // The number of mesh instances. + meshCount: c.int, + + // The number of unique meshes. Diagnostic. + sharedMeshCount: c.int, + + // Offset of the sphere array in bytes from the struct address. + sphereOffset: c.int, + + // The number of spheres. + sphereCount: c.int, +} + +// A capsule that lives in a compound. +CompoundCapsule :: struct { + // Local capsule. + capsule: Capsule, + + // Index to a shared material. + materialIndex: c.int, +} + +// A hull that lives in a compound. +CompoundHull :: struct { + // Pointer to the unique shared hull. + hull: ^HullData, + + // The transform of this hull instance. + transform: Transform, + + // Index to a shared material. + materialIndex: c.int, +} + +// A mesh with non-uniform scale that lives in a compound. +CompoundMesh :: struct { + // Pointer to the unique shared mesh. + meshData: [^]MeshData, + + // The transform of this mesh instance. + transform: Transform, + + // Non-uniform scale of this mesh instance. + scale: Vec3, + + // This is used to access the surface material from b3GetCompoundMaterials. + // Requires an extra level of indirection. The triangle material index + // is clamped to B3_MAX_COMPOUND_MESH_MATERIALS. + // materialIndex = materialIndices[triangle->materialIndex] + materialIndices: [MAX_COMPOUND_MESH_MATERIALS]c.int, +} + +// A sphere that lives in a compound. +CompoundSphere :: struct { + // Local sphere. + sphere: Sphere, + + // Index to a shared material. + materialIndex: c.int, +} + +// Child shape of a compound +ChildShape :: struct { + // Tagged union. + using _: struct #raw_union { + capsule: Capsule, //< Capsule. + hull: ^HullData, //< Hull. + mesh: Mesh, //< Mesh. + sphere: Sphere, //< Sphere. + }, + + // Transform of the shape into compound local space. + transform: Transform, + + // Material indices. Index 0 is used for convex shapes. + // todo limit to 64K? + materialIndices: [MAX_COMPOUND_MESH_MATERIALS]c.int, + + // The shape type (union tag). + type: ShapeType, +} + +// Callback for compound overlap queries. +CompoundQueryFcn :: proc "c" (#by_ptr compound: CompoundData, childIndex: c.int, ctx: rawptr) -> bool + +/**@}*/ // compound + +/**@}*/ // geometry + +/** + * @defgroup collision Shape Collision + * Collide pairs of shapes. + * @{ + */ + +// A manifold point is a contact point belonging to a contact manifold. +// It holds details related to the geometry and dynamics of the contact points. +// Box3D uses speculative collision so some contact points may be separated. +// You may use the maxNormalImpulse to determine if there was an interaction during +// the time step. +ManifoldPoint :: struct { + // Location of the contact point relative to the bodyA center of mass in world space. + anchorA: Vec3, + + // Location of the contact point relative to the bodyB center of mass in world space. + anchorB: Vec3, + + // The separation of the contact point, negative if penetrating + separation: f32, + + // Cached separation used for contact recycling + baseSeparation: f32, + + // The impulse along the manifold normal vector. Since Box3D uses sub-stepping, this is + // result from the final sub-step. + normalImpulse: f32, + + // The total normal impulse applied during sub-stepping. This is important + // to identify speculative contact points that had an interaction in the time step. + totalNormalImpulse: f32, + + // Relative normal velocity pre-solve. Used for hit events. If the normal impulse is + // zero then there was no hit. Negative means shapes are approaching. + normalVelocity: f32, + + // Local point for matching + // Uniquely identifies a contact point between two shapes + featureId: u32, + + // Triangle index if one of the shapes is a mesh or height field + triangleIndex: c.int, + + // Did this contact point exist in the previous step? + persisted: bool, +} + +// A contact manifold describes the contact points between colliding shapes. +// @note Box3D uses speculative collision so some contact points may be separated. +Manifold :: struct { + // The manifold points. There may be 1 to 4 valid points. + points: [MAX_MANIFOLD_POINTS]ManifoldPoint, + + // The unit normal vector in world space, points from shape A to shape B + normal: Vec3, + + // Central friction angular impulse (applied about the normal) + twistImpulse: f32, + + // Central friction linear impulse + frictionImpulse: Vec3, + + // Rolling resistance angular impulse + rollingImpulse: Vec3, + + // The number of contact points, will be 0 to 4 + pointCount: c.int, +} + +// Cached separating axis feature. +SeparatingFeature :: enum c.int { + invalidAxis = 0, + backsideAxis, + faceAxisA, + faceAxisB, + edgePairAxis, + closestPointsAxis, + + // These are for testing + manualFaceAxisA, + manualFaceAxisB, + manualEdgePairAxis, +} + +// Cached triangle feature. +TriangleFeature :: enum c.int { + featureNone = 0, + featureTriangleFace, + featureHullFace, + // v1-v2 + featureEdge1, + // v2-v3 + featureEdge2, + // v3-v1 + featureEdge3, + featureVertex1, + featureVertex2, + featureVertex3, +} + +// Separating axis test cache. Provides temporal acceleration of collision routines. +SATCache :: struct { + // The separation when the cache is populated. Negative for overlap. + separation: f32, + + // b3SeparatingFeature. + type: u8, + + // Index of the feature on shape A. + indexA: u8, + + // Index of the feature on shape B. + indexB: u8, + + // Was the cache re-used? + hit: u8, +} + +// Contact points are always the result of two edges intersecting. +// It can be two edges of the same shape, which is just a shape vertex. +// Or a contact point can be the result of two edges crossing from different shapes. +// This is designed to support hull versus hull, but it is adapted to work +// with all shape types. The feature pair is used to identify contact points +// for temporal coherence and warm starting. +FeaturePair :: struct { + // Incoming type (either edge on shape A or shape B) + owner1: u8, + // Incoming edge index (into associated shape array) + index1: u8, + // Outgoing type (either edge on shape A or shape B) + owner2: u8, + // Outgoing edge index (into associated shape array) + index2: u8, +} + +// A local manifold point and normal in frame A. +LocalManifoldPoint :: struct { + // Local point in frame A. + point: Vec3, + + // The contact point separation. Negative for overlap. + separation: f32, + + // The feature pair for this point. + pair: FeaturePair, + + // The triangle index when collide with a mesh or height-field. + triangleIndex: c.int, +} + +// A local manifold with no dynamic information. Used by b3Collide functions. +LocalManifold :: struct { + // Local normal in frame A. + normal: Vec3, + + // The triangle normal. + triangleNormal: Vec3, + + // The manifold points. From a point buffer. + points: LocalManifoldPoint, + + // The number of manifold points. Only bounded by the buffer capacity. + pointCount: c.int, + + // The index of the triangle. + triangleIndex: c.int, + + i1: c.int, //< Vertex 1 index. + i2: c.int, //< Vertex 2 index. + i3: c.int, //< Vertex 3 index. + + // The squared distance of a sphere from a triangle. For ghost collision reduction. + squaredDistance: f32, + + // The triangle feature involved. + feature: TriangleFeature, + + // MeshEdgeFlags. + triangleFlags: MeshEdgeFlags, +} + +/**@}*/ // collision + +/** + * @defgroup debug_draw Debug Draw + * @{ + */ + +// These colors are used for debug draw and mostly match the named SVG colors. +// See https://www.rapidtables.com/web/color/index.html +// https://johndecember.com/html/spec/colorsvg.html +// https://upload.wikimedia.org/wikipedia/commons/2/2b/SVG_Recognized_color_keyword_names.svg +HexColor :: enum c.uint { + AliceBlue = 0xF0F8FF, + AntiqueWhite = 0xFAEBD7, + Aqua = 0x00FFFF, + Aquamarine = 0x7FFFD4, + Azure = 0xF0FFFF, + Beige = 0xF5F5DC, + Bisque = 0xFFE4C4, + Black = 0x000000, + BlanchedAlmond = 0xFFEBCD, + Blue = 0x0000FF, + BlueViolet = 0x8A2BE2, + Brown = 0xA52A2A, + Burlywood = 0xDEB887, + CadetBlue = 0x5F9EA0, + Chartreuse = 0x7FFF00, + Chocolate = 0xD2691E, + Coral = 0xFF7F50, + CornflowerBlue = 0x6495ED, + Cornsilk = 0xFFF8DC, + Crimson = 0xDC143C, + Cyan = 0x00FFFF, + DarkBlue = 0x00008B, + DarkCyan = 0x008B8B, + DarkGoldenRod = 0xB8860B, + DarkGray = 0xA9A9A9, + DarkGreen = 0x006400, + DarkKhaki = 0xBDB76B, + DarkMagenta = 0x8B008B, + DarkOliveGreen = 0x556B2F, + DarkOrange = 0xFF8C00, + DarkOrchid = 0x9932CC, + DarkRed = 0x8B0000, + DarkSalmon = 0xE9967A, + DarkSeaGreen = 0x8FBC8F, + DarkSlateBlue = 0x483D8B, + DarkSlateGray = 0x2F4F4F, + DarkTurquoise = 0x00CED1, + DarkViolet = 0x9400D3, + DeepPink = 0xFF1493, + DeepSkyBlue = 0x00BFFF, + DimGray = 0x696969, + DodgerBlue = 0x1E90FF, + FireBrick = 0xB22222, + FloralWhite = 0xFFFAF0, + ForestGreen = 0x228B22, + Fuchsia = 0xFF00FF, + Gainsboro = 0xDCDCDC, + GhostWhite = 0xF8F8FF, + Gold = 0xFFD700, + GoldenRod = 0xDAA520, + Gray = 0x808080, + Green = 0x008000, + GreenYellow = 0xADFF2F, + HoneyDew = 0xF0FFF0, + HotPink = 0xFF69B4, + IndianRed = 0xCD5C5C, + Indigo = 0x4B0082, + Ivory = 0xFFFFF0, + Khaki = 0xF0E68C, + Lavender = 0xE6E6FA, + LavenderBlush = 0xFFF0F5, + LawnGreen = 0x7CFC00, + LemonChiffon = 0xFFFACD, + LightBlue = 0xADD8E6, + LightCoral = 0xF08080, + LightCyan = 0xE0FFFF, + LightGoldenRodYellow = 0xFAFAD2, + LightGray = 0xD3D3D3, + LightGreen = 0x90EE90, + LightPink = 0xFFB6C1, + LightSalmon = 0xFFA07A, + LightSeaGreen = 0x20B2AA, + LightSkyBlue = 0x87CEFA, + LightSlateGray = 0x778899, + LightSteelBlue = 0xB0C4DE, + LightYellow = 0xFFFFE0, + Lime = 0x00FF00, + LimeGreen = 0x32CD32, + Linen = 0xFAF0E6, + Magenta = 0xFF00FF, + Maroon = 0x800000, + MediumAquaMarine = 0x66CDAA, + MediumBlue = 0x0000CD, + MediumOrchid = 0xBA55D3, + MediumPurple = 0x9370DB, + MediumSeaGreen = 0x3CB371, + MediumSlateBlue = 0x7B68EE, + MediumSpringGreen = 0x00FA9A, + MediumTurquoise = 0x48D1CC, + MediumVioletRed = 0xC71585, + MidnightBlue = 0x191970, + MintCream = 0xF5FFFA, + MistyRose = 0xFFE4E1, + Moccasin = 0xFFE4B5, + NavajoWhite = 0xFFDEAD, + Navy = 0x000080, + OldLace = 0xFDF5E6, + Olive = 0x808000, + OliveDrab = 0x6B8E23, + Orange = 0xFFA500, + OrangeRed = 0xFF4500, + Orchid = 0xDA70D6, + PaleGoldenRod = 0xEEE8AA, + PaleGreen = 0x98FB98, + PaleTurquoise = 0xAFEEEE, + PaleVioletRed = 0xDB7093, + PapayaWhip = 0xFFEFD5, + PeachPuff = 0xFFDAB9, + Peru = 0xCD853F, + Pink = 0xFFC0CB, + Plum = 0xDDA0DD, + PowderBlue = 0xB0E0E6, + Purple = 0x800080, + RebeccaPurple = 0x663399, + Red = 0xFF0000, + RosyBrown = 0xBC8F8F, + RoyalBlue = 0x4169E1, + SaddleBrown = 0x8B4513, + Salmon = 0xFA8072, + SandyBrown = 0xF4A460, + SeaGreen = 0x2E8B57, + SeaShell = 0xFFF5EE, + Sienna = 0xA0522D, + Silver = 0xC0C0C0, + SkyBlue = 0x87CEEB, + SlateBlue = 0x6A5ACD, + SlateGray = 0x708090, + Snow = 0xFFFAFA, + SpringGreen = 0x00FF7F, + SteelBlue = 0x4682B4, + Tan = 0xD2B48C, + Teal = 0x008080, + Thistle = 0xD8BFD8, + Tomato = 0xFF6347, + Turquoise = 0x40E0D0, + Violet = 0xEE82EE, + Wheat = 0xF5DEB3, + White = 0xFFFFFF, + WhiteSmoke = 0xF5F5F5, + Yellow = 0xFFFF00, + YellowGreen = 0x9ACD32, + + Box2DRed = 0xDC3132, + Box2DBlue = 0x30AEBF, + Box2DGreen = 0x8CC924, + Box2DYellow = 0xFFEE8C, +} + +// Debug draw material preset. Optionally packed into the unused high byte of a +// b3HexColor (or b3SurfaceMaterial::customColor) to drive the renderer's PBR +// roughness and metalness. The low 24 bits stay RGB, so a plain 0xRRGGBB color +// reads as b3_debugMaterialDefault and keeps the renderer's per-body-type look. +DebugMaterial :: enum c.uint { + Default = 0, + Matte, + Soft, + Dead, + Glossy, + Metallic, +} + + +// This is sent to the user for debug shape creation. The user should know the type in case they have +// custom sphere or capsule rendering. +DebugShape :: struct { + // Shape id. + shapeId: ShapeId, + + // Shape type. + type: ShapeType, + + // Tagged union. + using _: struct #raw_union { + capsule: ^Capsule `raw_union_tag:"type=.capsuleShape"`, //< Capsule shape. + compound: ^CompoundData `raw_union_tag:"type=.compoundShape"`, //< Compound shape. + heightField: ^HeightFieldData `raw_union_tag:"type=.heightShape"`, //< Height-field shape. + hull: ^HullData `raw_union_tag:"type=.hullShape"`, //< Convex hull shape. + mesh: ^Mesh `raw_union_tag:"type=.meshShape"`, //< Mesh shape with scale. + sphere: ^Sphere `raw_union_tag:"type=.sphereShape"`, //< Sphere shape. + }, +} + +// This struct is passed to b3World_Draw to draw a debug view of the simulation world. +// Callbacks receive world coordinates. In large world mode the translation is double precision so +// it stays accurate far from the origin. Shift into your own camera frame inside the callbacks. +DebugDraw :: struct { + // Draws a shape and returns true if drawing should continue + DrawShapeFcn: proc "c" (userShape: rawptr, transform: WorldTransform, color: HexColor, ctx: rawptr) -> bool, + + // Draw a line segment. + DrawSegmentFcn: proc "c" (p1, p2: Pos, color: HexColor, ctx: rawptr), + + // Draw a transform. Choose your own length scale. + DrawTransformFcn: proc "c" (transform: WorldTransform, ctx: rawptr), + + // Draw a point. + DrawPointFcn: proc "c" (p: Pos, size: f32, color: HexColor, ctx: rawptr), + + // Draw a sphere. + DrawSphereFcn: proc "c" (p: Pos, radius: f32, color: HexColor, alpha: f32, ctx: rawptr), + + // Draw a capsule. + DrawCapsuleFcn: proc "c" (p1, p2: Pos, radius: f32, color: HexColor, alpha: f32, ctx: rawptr), + + // Draw a bounding box. + DrawBoundsFcn: proc "c" (aabb: AABB, color: HexColor, ctx: rawptr), + + // Draw an oriented box. + DrawBoxFcn: proc "c" (extents: Vec3, transform: WorldTransform, color: HexColor, ctx: rawptr), + + // Draw a string in world space + DrawStringFcn: proc "c" (p: Pos, s: cstring, color: HexColor, ctx: rawptr), + + // World bounds to use for debug draw + drawingBounds: AABB, + + // Scale to use when drawing forces + forceScale: f32, + + // Global scaling for joint drawing + jointScale: f32, + + // Option to draw shapes + drawShapes: bool, + + // Option to draw joints + drawJoints: bool, + + // Option to draw additional information for joints + drawJointExtras: bool, + + // Option to draw the bounding boxes for shapes + drawBounds: bool, + + // Option to draw the mass and center of mass of dynamic bodies + drawMass: bool, + + // Option to draw the sleep information for dynamic and kinematic bodies + drawSleep: bool, + + // Option to draw body names + drawBodyNames: bool, + + // Option to draw contact points + drawContacts: bool, + + // Draw contact anchor A or B + drawAnchorA: c.int, + + // Option to visualize the graph coloring used for contacts and joints + drawGraphColors: bool, + + // Option to draw contact features + drawContactFeatures: bool, + + // Option to draw contact normals + drawContactNormals: bool, + + // Option to draw contact normal forces + drawContactForces: bool, + + // Option to draw contact friction forces + drawFrictionForces: bool, + + // Option to draw islands as bounding boxes + drawIslands: bool, + + // User context that is passed as an argument to drawing callback functions + ctx: rawptr, +} diff --git a/vendor/box3d/lib/box3d.lib b/vendor/box3d/lib/box3d.lib new file mode 100644 index 000000000..38b5dad36 Binary files /dev/null and b/vendor/box3d/lib/box3d.lib differ diff --git a/vendor/box3d/lib/darwin/libbox3d.a b/vendor/box3d/lib/darwin/libbox3d.a new file mode 100644 index 000000000..712d53de5 --- /dev/null +++ b/vendor/box3d/lib/darwin/libbox3d.a @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:6b000b0b69028af14d6a481b6bf6ca3b70d2c3a29cd9e73eab8b96e0deffa07d +size 2524600 diff --git a/vendor/box3d/lib/linux-amd64/libbox3d.a b/vendor/box3d/lib/linux-amd64/libbox3d.a new file mode 100644 index 000000000..ac6223456 --- /dev/null +++ b/vendor/box3d/lib/linux-amd64/libbox3d.a @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:948651aca53740a4a47a8251b68982b839fa62da91689035132be1c273cbd7da +size 1536158 diff --git a/vendor/box3d/src/build.bat b/vendor/box3d/src/build.bat new file mode 100644 index 000000000..55145a77c --- /dev/null +++ b/vendor/box3d/src/build.bat @@ -0,0 +1,8 @@ +@echo off + +if not exist "..\lib" mkdir ..\lib + +cl -nologo -MT -TC -O2 -c -std:c17 -I "include" src\*.c +lib -nologo *.obj -out:..\lib\box3d.lib + +del *.obj diff --git a/vendor/box3d/src/build.sh b/vendor/box3d/src/build.sh new file mode 100755 index 000000000..7d3a99543 --- /dev/null +++ b/vendor/box3d/src/build.sh @@ -0,0 +1,72 @@ +#!/usr/bin/env bash + +LIB_NAME="libbox3d.a" +# Detect architecture +ARCH=$(uname -m) +case "$ARCH" in + x86_64|amd64) + ARCH="amd64" + ;; + arm64|aarch64) + ARCH="arm64" + ;; + *) + echo "Error: Unsupported architecture: $ARCH" + exit 1 + ;; +esac + +# Detect OS +if [[ "$OSTYPE" == "darwin"* ]]; then + INSTALL_DIR="../lib/darwin" + mkdir -p "$INSTALL_DIR" build/x86_64 build/arm64 + + # Building box3d for amd64 + for src in src/*.c; do + obj="build/x86_64/$(basename "${src%.c}.o")" + clang -c -O2 -std=c17 -fPIC \ + -arch x86_64 \ + -mmacosx-version-min=11.0 \ + -Iinclude \ + "$src" -o "$obj" + done + + # Building box3d for arm64 + for src in src/*.c; do + obj="build/arm64/$(basename "${src%.c}.o")" + clang -c -O2 -std=c17 -fPIC \ + -arch arm64 \ + -mmacosx-version-min=11.0 \ + -Iinclude \ + "$src" -o "$obj" + done + + # Turn them into their respective *.a files + ar rcs "$INSTALL_DIR/libbox3d_x86_64.a" build/x86_64/*.o + ar rcs "$INSTALL_DIR/libbox3d_arm64.a" build/arm64/*.o + + ranlib "$INSTALL_DIR/libbox3d_x86_64.a" + ranlib "$INSTALL_DIR/libbox3d_arm64.a" + + # Bundle them into a universal library + lipo -create \ + "$INSTALL_DIR/libbox3d_x86_64.a" \ + "$INSTALL_DIR/libbox3d_arm64.a" \ + -output "$INSTALL_DIR/$LIB_NAME" + + # Clean up the single arch .a files and build temp + rm "$INSTALL_DIR/libbox3d_x86_64.a" "$INSTALL_DIR/libbox3d_arm64.a" + rm -rf build + +elif [[ "$OSTYPE" == "linux-gnu"* ]]; then + OS="linux" + LIB_DIR="../lib/${OS}-${ARCH}" + mkdir -p "$LIB_DIR" + cc -c -O2 -std=c17 -fPIC -Iinclude src/*.c + ar rcs "$LIB_DIR/$LIB_NAME" *.o + rm -f *.o + +else + echo "Error: Unsupported operating system: $OSTYPE" + exit 1 +fi \ No newline at end of file diff --git a/vendor/box3d/src/include/box3d/base.h b/vendor/box3d/src/include/box3d/base.h new file mode 100644 index 000000000..25828b3fa --- /dev/null +++ b/vendor/box3d/src/include/box3d/base.h @@ -0,0 +1,189 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +// Compile-time options. Edit box3d/config.h, or define BOX3D_USER_CONFIG to +// point at your own copy. +#ifdef BOX3D_USER_CONFIG +#include BOX3D_USER_CONFIG +#endif +#include "config.h" + +// clang-format off +// +// Shared library macros +// Predefine BOX3D_EXPORT to reuse an existing export/import scheme, for example +// when compiling Box3D into another shared library. +#ifndef BOX3D_EXPORT +#if defined(_WIN32) && defined(box3d_EXPORTS) + // build the Windows DLL + #define BOX3D_EXPORT __declspec(dllexport) +#elif defined(_WIN32) && defined(BOX3D_DLL) + // using the Windows DLL + #define BOX3D_EXPORT __declspec(dllimport) +#elif defined(box3d_EXPORTS) + // building or using the shared library + #define BOX3D_EXPORT __attribute__((visibility("default"))) +#else + // static library + #define BOX3D_EXPORT +#endif +#endif + +// C++ macros +#ifdef __cplusplus + #define B3_API extern "C" BOX3D_EXPORT + #define B3_INLINE inline + +#if defined( _MSC_VER ) + #define B3_FORCE_INLINE __forceinline +#elif defined( __GNUC__ ) || defined( __clang__ ) + #define B3_FORCE_INLINE inline __attribute__((always_inline)) +#else + #define B3_FORCE_INLINE inline +#endif + + #define B3_LITERAL(T) T + #define B3_ZERO_INIT {} +#else + #define B3_API BOX3D_EXPORT + #define B3_INLINE static inline + +#if defined( _MSC_VER ) + #define B3_FORCE_INLINE static __forceinline +#elif defined( __GNUC__ ) || defined( __clang__ ) + #define B3_FORCE_INLINE static inline __attribute__((always_inline)) +#else + #define B3_FORCE_INLINE static inline +#endif + +/// Used for C literals like (b3Vec3){1.0f, 2.0f, 3.0f} where C++ requires b3Vec3{1.0f, 2.0f, 3.0f} + #define B3_LITERAL(T) (T) + #define B3_ZERO_INIT {0} +#endif +// clang-format on + +#if defined( BOX3D_VALIDATE ) && !defined( NDEBUG ) +#define B3_ENABLE_VALIDATION 1 +#else +#define B3_ENABLE_VALIDATION 0 +#endif + +/** + * @defgroup base Base + * Base functionality + * @{ + */ + +/// This is used to indicate null for interfaces that work with indices instead of pointers +#define B3_NULL_INDEX -1 + +/// Prototype for user allocation function. +/// @param size the allocation size in bytes +/// @param alignment the required alignment, guaranteed to be a power of 2 +typedef void* b3AllocFcn( int32_t size, int32_t alignment ); + +/// Prototype for user free function. +/// @param mem the memory previously allocated through `b3AllocFcn` +typedef void b3FreeFcn( void* mem ); + +/// Prototype for the user assert callback. Return 0 to skip the debugger break. +typedef int b3AssertFcn( const char* condition, const char* fileName, int lineNumber ); + +/// Prototype for user log callback. Used to log warnings. +typedef void b3LogFcn( const char* message ); + +/// This allows the user to override the allocation functions. These should be +/// set during application startup. +B3_API void b3SetAllocator( b3AllocFcn* allocFcn, b3FreeFcn* freeFcn ); + +/// Total bytes allocated by Box3D +B3_API int b3GetByteCount( void ); + +/// Override the default assert callback. +/// @param assertFcn a non-null assert callback +B3_API void b3SetAssertFcn( b3AssertFcn* assertFcn ); + +/// see https://github.com/scottt/debugbreak +#if defined( _MSC_VER ) +/// Break to the debugger +#define B3_BREAKPOINT __debugbreak() +#elif defined( __GNUC__ ) || defined( __clang__ ) +#define B3_BREAKPOINT __builtin_trap() +#else +/// Unknown compiler +#include +#define B3_BREAKPOINT assert( 0 ) +#endif + +#if !defined( NDEBUG ) || defined( B3_ENABLE_ASSERT ) +/// Internal assertion handler. Allows for host intervention. +B3_API int b3InternalAssert( const char* condition, const char* fileName, int lineNumber ); +/// Assert that a condition is true. +#define B3_ASSERT( condition ) \ + ( (void)( ( !!( condition ) ) || ( b3InternalAssert( #condition, __FILE__, (int)( __LINE__ ) ), 0 ) ) ) +#else +#define B3_ASSERT( ... ) ( (void)0 ) +#endif + +#if B3_ENABLE_VALIDATION +/// Validation is typically only enabled in debug builds. +/// Floating point tolerance checks should use this instead of the regular assertion +#define B3_VALIDATE( condition ) B3_ASSERT( condition ) +#else +/// Validation is typically only enabled in debug builds. +/// Floating point tolerance checks should use this instead of the regular assertion +#define B3_VALIDATE( ... ) ( (void)0 ) +#endif + +/// Override the default logging callback. +B3_API void b3SetLogFcn( b3LogFcn* logFcn ); + +/// Version numbering scheme. +/// See https://semver.org/ +typedef struct b3Version +{ + /// Significant changes + int major; + + /// Incremental changes + int minor; + + /// Bug fixes + int revision; +} b3Version; + +/// Get the current version of Box3D +B3_API b3Version b3GetVersion( void ); + +/// @return true if the library was built with BOX3D_DOUBLE_PRECISION (large world mode) +B3_API bool b3IsDoublePrecision( void ); + +/**@}*/ + +//! @cond + +/// Get the absolute number of system ticks. The value is platform specific. +B3_API uint64_t b3GetTicks( void ); + +/// Get the milliseconds passed from an initial tick value. +B3_API float b3GetMilliseconds( uint64_t ticks ); + +/// Get the milliseconds passed from an initial tick value. +B3_API float b3GetMillisecondsAndReset( uint64_t* ticks ); + +/// Yield to be used in a busy loop. +B3_API void b3Yield( void ); + +/// Sleep the current thread for a number of milliseconds. +B3_API void b3Sleep( int milliseconds ); + +// Simple djb2 hash function for determinism testing +#define B3_HASH_INIT 5381 +B3_API uint32_t b3Hash( uint32_t hash, const uint8_t* data, int count ); + +//! @endcond diff --git a/vendor/box3d/src/include/box3d/box3d.h b/vendor/box3d/src/include/box3d/box3d.h new file mode 100644 index 000000000..05b69dd63 --- /dev/null +++ b/vendor/box3d/src/include/box3d/box3d.h @@ -0,0 +1,1737 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +#pragma once + +#include "base.h" +#include "collision.h" +#include "id.h" +#include "math_functions.h" +#include "types.h" + +#include + +/** + * @defgroup world World + * These functions allow you to create a simulation world. + * + * You can add rigid bodies and joint constraints to the world and run the simulation. You can get contact + * information to get contact points and normals as well as events. You can query the world, checking for overlaps and casting + * rays or shapes. There is also debugging information such as debug draw, timing information, and counters. You can find + * documentation here: https://box2d.org/ + * @{ + */ + +#if defined( BOX3D_DOUBLE_PRECISION ) +// Force a link error if the application and library disagree on precision. A float app linking +// a double precision library, or the reverse, gets one unresolved external on the first call +// every program makes. CMake consumers inherit the define and cannot mismatch. +#define b3CreateWorld b3CreateWorldDoublePrecision +#endif + +/// Create a world for rigid body simulation. A world contains bodies, shapes, and constraints. You may create +/// up to 128 worlds. Each world is completely independent and may be simulated in parallel. +/// @return the world id. +B3_API b3WorldId b3CreateWorld( const b3WorldDef* def ); + +/// Destroy a world +B3_API void b3DestroyWorld( b3WorldId worldId ); + +/// Get the current number of worlds +B3_API int b3GetWorldCount( void ); + +/// Get the maximum number of simultaneous worlds that have been created +B3_API int b3GetMaxWorldCount( void ); + +/// World id validation. Provides validation for up to 64K allocations. +B3_API bool b3World_IsValid( b3WorldId id ); + +/// Simulate a world for one time step. This performs collision detection, integration, and constraint solution. +/// @param worldId The world to simulate +/// @param timeStep The amount of time to simulate, this should be a fixed number. Usually 1/60. +/// @param subStepCount The number of sub-steps, increasing the sub-step count can increase accuracy. Usually 4. +B3_API void b3World_Step( b3WorldId worldId, float timeStep, int subStepCount ); + +/// Call this to draw shapes and other debug draw data +B3_API void b3World_Draw( b3WorldId worldId, b3DebugDraw* draw, uint64_t maskBits ); + +/// Get the world's bounds. This is the bounding box that covers the current simulation. May have a small +/// amount of padding. +B3_API b3AABB b3World_GetBounds( b3WorldId worldId ); + +/// Get the body events for the current time step. The event data is transient. Do not store a reference to this data. +B3_API b3BodyEvents b3World_GetBodyEvents( b3WorldId worldId ); + +/// Get sensor events for the current time step. The event data is transient. Do not store a reference to this data. +B3_API b3SensorEvents b3World_GetSensorEvents( b3WorldId worldId ); + +/// Get contact events for this current time step. The event data is transient. Do not store a reference to this data. +B3_API b3ContactEvents b3World_GetContactEvents( b3WorldId worldId ); + +/// Get the joint events for the current time step. The event data is transient. Do not store a reference to this data. +B3_API b3JointEvents b3World_GetJointEvents( b3WorldId worldId ); + +/// Overlap test for all shapes that *potentially* overlap the provided AABB +B3_API b3TreeStats b3World_OverlapAABB( b3WorldId worldId, b3AABB aabb, b3QueryFilter filter, b3OverlapResultFcn* fcn, + void* context ); + +/// Overlap test for all shapes that overlap the provided shape proxy. The proxy points are relative +/// to the world origin, which lets the query stay precise far from the world origin. +B3_API b3TreeStats b3World_OverlapShape( b3WorldId worldId, b3Pos origin, const b3ShapeProxy* proxy, b3QueryFilter filter, + b3OverlapResultFcn* fcn, void* context ); + +/// Cast a ray into the world to collect shapes in the path of the ray. +/// Your callback function controls whether you get the closest point, any point, or n-points. +/// @note The callback function may receive shapes in any order +/// @param worldId The world to cast the ray against +/// @param origin The start point of the ray +/// @param translation The translation of the ray from the start point to the end point +/// @param filter Contains bit flags to filter unwanted shapes from the results +/// @param fcn A user implemented callback function +/// @param context A user context that is passed along to the callback function +/// @return traversal performance counters +B3_API b3TreeStats b3World_CastRay( b3WorldId worldId, b3Pos origin, b3Vec3 translation, b3QueryFilter filter, + b3CastResultFcn* fcn, void* context ); + +/// Cast a ray into the world to collect the closest hit. This is a convenience function. Ignores initial overlap. +/// This is less general than b3World_CastRay() and does not allow for custom filtering. +B3_API b3RayResult b3World_CastRayClosest( b3WorldId worldId, b3Pos origin, b3Vec3 translation, b3QueryFilter filter ); + +/// Cast a shape through the world. Similar to a cast ray except that a shape is cast instead of a point. +/// The proxy points are relative to the origin and the hit points come back as world positions, so the +/// cast stays precise far from the world origin. +/// @see b3World_CastRay +B3_API b3TreeStats b3World_CastShape( b3WorldId worldId, b3Pos origin, const b3ShapeProxy* proxy, b3Vec3 translation, + b3QueryFilter filter, b3CastResultFcn* fcn, void* context ); + +/// Cast a capsule mover through the world. This is a special shape cast that handles sliding along other shapes while reducing +/// clipping. This is not a good source of information about what the mover is touching. Instead use the planes returned by +/// b3World_CollideMover. +/// @param worldId World to cast the mover against +/// @param origin World position the mover capsule is relative to +/// @param mover Capsule mover, relative to the origin +/// @param translation Desired mover translation +/// @param filter Contains bit flags to filter unwanted shapes from the results +/// @param fcn Optional callback for custom shape filtering +/// @param context A user context that is passed along to the callback function +/// @return the translation fraction +B3_API float b3World_CastMover( b3WorldId worldId, b3Pos origin, const b3Capsule* mover, b3Vec3 translation, b3QueryFilter filter, + b3MoverFilterFcn* fcn, void* context ); + +/// Collide a capsule mover with the world, gathering collision planes that can be fed to b3SolvePlanes. Useful for +/// kinematic character movement. The mover and the returned planes are relative to the origin. +B3_API void b3World_CollideMover( b3WorldId worldId, b3Pos origin, const b3Capsule* mover, b3QueryFilter filter, + b3PlaneResultFcn* fcn, void* context ); + +/// Enable/disable sleep. If your application does not need sleeping, you can gain some performance +/// by disabling sleep completely at the world level. +/// @see b3WorldDef +B3_API void b3World_EnableSleeping( b3WorldId worldId, bool flag ); + +/// Is body sleeping enabled? +B3_API bool b3World_IsSleepingEnabled( b3WorldId worldId ); + +/// Enable/disable continuous collision between dynamic and static bodies. Generally you should keep continuous +/// collision enabled to prevent fast moving objects from going through static objects. The performance gain from +/// disabling continuous collision is minor. +/// @see b3WorldDef +B3_API void b3World_EnableContinuous( b3WorldId worldId, bool flag ); + +/// Is continuous collision enabled? +B3_API bool b3World_IsContinuousEnabled( b3WorldId worldId ); + +/// Adjust the restitution threshold. It is recommended not to make this value very small +/// because it will prevent bodies from sleeping. Usually in meters per second. +/// @see b3WorldDef +B3_API void b3World_SetRestitutionThreshold( b3WorldId worldId, float value ); + +/// Get the restitution speed threshold. Usually in meters per second. +B3_API float b3World_GetRestitutionThreshold( b3WorldId worldId ); + +/// Adjust the hit event threshold. This controls the collision speed needed to generate a b3ContactHitEvent. +/// Usually in meters per second. +/// @see b3WorldDef::hitEventThreshold +B3_API void b3World_SetHitEventThreshold( b3WorldId worldId, float value ); + +/// Get the hit event speed threshold. Usually in meters per second. +B3_API float b3World_GetHitEventThreshold( b3WorldId worldId ); + +/// Register the custom filter callback. This is optional. +B3_API void b3World_SetCustomFilterCallback( b3WorldId worldId, b3CustomFilterFcn* fcn, void* context ); + +/// Register the pre-solve callback. This is optional. +B3_API void b3World_SetPreSolveCallback( b3WorldId worldId, b3PreSolveFcn* fcn, void* context ); + +/// Set the gravity vector for the entire world. Box3D has no concept of an up direction and this +/// is left as a decision for the application. Usually in m/s^2. +/// @see b3WorldDef +B3_API void b3World_SetGravity( b3WorldId worldId, b3Vec3 gravity ); + +/// Get the gravity vector +B3_API b3Vec3 b3World_GetGravity( b3WorldId worldId ); + +/// Apply a radial explosion +/// @param worldId The world id +/// @param explosionDef The explosion definition +B3_API void b3World_Explode( b3WorldId worldId, const b3ExplosionDef* explosionDef ); + +/// Adjust contact tuning parameters +/// @param worldId The world id +/// @param hertz The contact stiffness (cycles per second) +/// @param dampingRatio The contact bounciness with 1 being critical damping (non-dimensional) +/// @param contactSpeed The maximum contact constraint push out speed (meters per second) +/// @note Advanced feature +B3_API void b3World_SetContactTuning( b3WorldId worldId, float hertz, float dampingRatio, float contactSpeed ); + +/// Set the contact point recycling distance. Setting this to zero disables contact point recycling. +/// Usually in meters. +B3_API void b3World_SetContactRecycleDistance( b3WorldId worldId, float recycleDistance ); + +/// Get the contact point recycling distance. Usually in meters. +B3_API float b3World_GetContactRecycleDistance( b3WorldId worldId ); + +/// Set the maximum linear speed. Usually in m/s. +B3_API void b3World_SetMaximumLinearSpeed( b3WorldId worldId, float maximumLinearSpeed ); + +/// Get the maximum linear speed. Usually in m/s. +B3_API float b3World_GetMaximumLinearSpeed( b3WorldId worldId ); + +/// Enable/disable constraint warm starting. Advanced feature for testing. Disabling +/// warm starting greatly reduces stability and provides no performance gain. +B3_API void b3World_EnableWarmStarting( b3WorldId worldId, bool flag ); + +/// Is constraint warm starting enabled? +B3_API bool b3World_IsWarmStartingEnabled( b3WorldId worldId ); + +/// Get the number of awake bodies +B3_API int b3World_GetAwakeBodyCount( b3WorldId worldId ); + +/// Get the current world performance profile +B3_API b3Profile b3World_GetProfile( b3WorldId worldId ); + +/// Get world counters and sizes +B3_API b3Counters b3World_GetCounters( b3WorldId worldId ); + +/// Get max capacity. This can be used with b3WorldDef to avoid run-time allocations and copies +B3_API b3Capacity b3World_GetMaxCapacity( b3WorldId worldId ); + +/// Set the user data pointer. +B3_API void b3World_SetUserData( b3WorldId worldId, void* userData ); + +/// Get the user data pointer. +B3_API void* b3World_GetUserData( b3WorldId worldId ); + +/// Set the friction callback. Passing NULL resets to default. +B3_API void b3World_SetFrictionCallback( b3WorldId worldId, b3FrictionCallback* callback ); + +/// Set the restitution callback. Passing NULL resets to default. +B3_API void b3World_SetRestitutionCallback( b3WorldId worldId, b3RestitutionCallback* callback ); + +/// Set the worker count. Must be in the range [1, B3_MAX_WORKERS] +B3_API void b3World_SetWorkerCount( b3WorldId worldId, int count ); + +/// Get the worker count. +B3_API int b3World_GetWorkerCount( b3WorldId worldId ); + +/// Dump memory stats to log. +B3_API void b3World_DumpMemoryStats( b3WorldId worldId ); + +/// Dump shape bounds to box3d_bounds.txt +B3_API void b3World_DumpShapeBounds( b3WorldId worldId, b3BodyType type ); + +/// This is for internal testing +B3_API void b3World_RebuildStaticTree( b3WorldId worldId ); + +/// This is for internal testing +B3_API void b3World_EnableSpeculative( b3WorldId worldId, bool flag ); + +/** + * @defgroup recording Recording + * @brief Record and replay world state for debugging. + * @{ + */ + +/// Opaque recording handle. Create with b3CreateRecording, destroy with b3DestroyRecording. +typedef struct b3Recording b3Recording; + +/// Create a recording buffer with an optional initial byte capacity. +/// Pass 0 to use the default (64 KiB). The buffer grows on demand. +/// @return a new recording, owned by the caller +B3_API b3Recording* b3CreateRecording( int byteCapacity ); + +/// Destroy a recording and free its buffer. +/// @param recording may be NULL +B3_API void b3DestroyRecording( b3Recording* recording ); + +/// Get a pointer to the raw recording bytes. +/// Valid until the recording buffer is modified or destroyed. +/// @param recording the recording handle +/// @return pointer to the byte buffer, or NULL if no bytes have been written +B3_API const uint8_t* b3Recording_GetData( const b3Recording* recording ); + +/// Get the number of bytes currently in the recording buffer. +/// @param recording the recording handle +B3_API int b3Recording_GetSize( const b3Recording* recording ); + +/// Begin recording world mutations into the provided buffer. +/// The buffer is reset on each call so a single b3Recording can be reused for multiple sessions. +/// @param worldId the world to record +/// @param recording the recording handle to write into +B3_API void b3World_StartRecording( b3WorldId worldId, b3Recording* recording ); + +/// End the current recording session. Writes the trailing geometry registry and +/// backpatches the header. The buffer remains valid until the recording is destroyed. +/// @param worldId the world currently being recorded +B3_API void b3World_StopRecording( b3WorldId worldId ); + +/// Save the recording buffer to a file. Returns true on success. +/// @param recording the recording to save +/// @param path file path to write +B3_API bool b3SaveRecordingToFile( const b3Recording* recording, const char* path ); + +/// Load a recording from a file. Returns NULL on failure (file not found, wrong magic). +/// The caller owns the returned recording and must destroy it with b3DestroyRecording. +/// @param path file path to read +B3_API b3Recording* b3LoadRecordingFromFile( const char* path ); + +/// Replay a recording from memory and verify it reproduces the same world-state hashes. +/// Stands up a fresh world, restores the seed snapshot, replays every op, and checks each embedded +/// StateHash record. Returns true if replay completed without id mismatches or hash divergences. +/// @param data pointer to recording bytes +/// @param size byte count of the recording +/// @param workerCount reserved for future multithreaded replay; pass 1 for now +B3_API bool b3ValidateReplay( const void* data, int size, int workerCount ); + +/// Opaque incremental replay player with a keyframe ring for O(interval) backward seek. +typedef struct b3RecPlayer b3RecPlayer; + +/// Summary of a recording, read once at open so a viewer can frame and label it. +typedef struct b3RecPlayerInfo +{ + int frameCount; // total recorded steps + int workerCount; // worker count requested for the replay world + float timeStep; // dt of the recorded steps + int subStepCount; // recorded sub-steps + float lengthScale; // length units per meter in effect when recorded + b3AABB bounds; // accumulated world bounds over the recording, zero-extent if unavailable +} b3RecPlayerInfo; + +/// Create a player over a recording. Owns a private copy of the bytes. +/// @param data pointer to recording bytes +/// @param size byte count of the recording +/// @param workerCount worker count for the replay world; pass 1 to match a serial recording. +/// Replaying at a different count re-partitions the constraint graph, so the StateHash check +/// becomes a cross-thread determinism test. Adjustable later with b3RecPlayer_SetWorkerCount. +/// @return a new player, or NULL on bad header or deserialization failure +B3_API b3RecPlayer* b3RecPlayer_Create( const void* data, int size, int workerCount ); + +/// Destroy the player and free all memory. Restores the previous global length scale. +B3_API void b3RecPlayer_Destroy( b3RecPlayer* player ); + +/// Advance one frame. dispatch ops until the next Step completes. +/// @return true when a frame was stepped, false at end-of-recording +B3_API bool b3RecPlayer_StepFrame( b3RecPlayer* player ); + +/// Sub-step one frame. This will sub-step and return immediately after body creation. +/// The next call will execute the time step. This allows bodies to be rendered +/// at the creation pose. +B3_API void b3RecPlayer_SubStepFrame( b3RecPlayer* player ); + +/// Rewind to frame 0 (in-place restore so the world id stays stable). +B3_API void b3RecPlayer_Restart( b3RecPlayer* player ); + +/// Seek to a specific frame. Forward seek steps op-by-op; backward seek restores +/// the nearest keyframe then re-steps the remaining gap. +B3_API void b3RecPlayer_SeekFrame( b3RecPlayer* player, int targetFrame ); + +/// @return the world currently driven by this player +B3_API b3WorldId b3RecPlayer_GetWorldId( const b3RecPlayer* player ); + +/// @return the last fully-stepped frame index (0 before any step) +B3_API int b3RecPlayer_GetFrame( const b3RecPlayer* player ); + +/// @return total number of recorded frames +B3_API int b3RecPlayer_GetFrameCount( const b3RecPlayer* player ); + +/// @return true when the op stream is exhausted +B3_API bool b3RecPlayer_IsAtEnd( const b3RecPlayer* player ); + +/// @return true when the op stream is paused between body creation and world step. +B3_API bool b3RecPlayer_IsAtPreStep( const b3RecPlayer* player ); + +/// @return true when any StateHash mismatch has been detected +B3_API bool b3RecPlayer_HasDiverged( const b3RecPlayer* player ); + +/// @return a summary of the recording read at open: frame count, recorded tuning, and bounds +B3_API b3RecPlayerInfo b3RecPlayer_GetInfo( const b3RecPlayer* player ); + +/// @return the first frame at which replay diverged, or -1 if it has not diverged +B3_API int b3RecPlayer_GetDivergeFrame( const b3RecPlayer* player ); + +/// Set the worker count of the replay world. Clamped to [1, B3_MAX_WORKERS]. Applied to the live +/// world at once and reused whenever the player rebuilds its world on Restart or a backward seek. +/// Replaying at a different count than recorded re-partitions the constraint graph, so the StateHash +/// check becomes a cross-thread determinism test. +B3_API void b3RecPlayer_SetWorkerCount( b3RecPlayer* player, int count ); + +/// Tune the keyframe ring used to speed up backward seeking. A keyframe is a periodic snapshot the +/// player restores from instead of replaying from the start, trading memory for seek speed. +/// @param player the recording player +/// @param budgetBytes memory cap for the kept snapshots; the spacing widens to stay under it +/// @param minIntervalFrames finest spacing between keyframes, in frames +/// A zero budget or a non-positive interval keeps that value. Clears the existing ring, so call +/// b3RecPlayer_Restart afterward to repopulate it under the new policy. +B3_API void b3RecPlayer_SetKeyframePolicy( b3RecPlayer* player, size_t budgetBytes, int minIntervalFrames ); + +/// @return the keyframe memory budget in bytes +B3_API size_t b3RecPlayer_GetKeyframeBudget( const b3RecPlayer* player ); + +/// @return the finest keyframe spacing in frames +B3_API int b3RecPlayer_GetKeyframeMinInterval( const b3RecPlayer* player ); + +/// @return the current keyframe spacing in frames; starts at the min interval and doubles as the +/// ring evicts to stay under budget, so it reflects the effective backward-seek granularity now +B3_API int b3RecPlayer_GetKeyframeInterval( const b3RecPlayer* player ); + +/// @return the memory currently held by keyframe snapshots, in bytes +B3_API size_t b3RecPlayer_GetKeyframeBytes( const b3RecPlayer* player ); + +/// @return the number of bodies tracked in creation order (including holes for destroyed bodies) +B3_API int b3RecPlayer_GetBodyCount( const b3RecPlayer* player ); + +/// Resolve a creation ordinal to the live body id at the current frame. +/// @return the body id, or a null id if that ordinal is out of range or its body is destroyed +B3_API b3BodyId b3RecPlayer_GetBodyId( const b3RecPlayer* player, int index ); + +/// Wire host debug-shape callbacks into the player's replay world so a renderer can build +/// per-shape draw resources (the 3D sample needs this or the replay world draws nothing). +/// Rebuilds the current world under the new callbacks and rewinds to frame 0, so call it +/// once right after b3RecPlayer_Create and re-read the world id afterward. The callbacks +/// persist across Restart and backward seeks, which recreate the world internally. +/// @param player the player to configure +/// @param createDebugShape called when a replayed shape is added; returns a user draw handle +/// @param destroyDebugShape called when a replayed shape is removed; may be NULL +/// @param context user context passed to both callbacks +B3_API void b3RecPlayer_SetDebugShapeCallbacks( b3RecPlayer* player, b3CreateDebugShapeCallback* createDebugShape, + b3DestroyDebugShapeCallback* destroyDebugShape, void* context ); + +/// Draw the spatial queries recorded during the most recently replayed frame, layered on top of the +/// world. Call after b3World_Draw. NULL draw function pointers are skipped. +/// @param player a valid player handle +/// @param draw debug draw callbacks +/// @param queryIndex index of the frame query to draw, or -1 to draw all of them +/// @param selectedIndex index of the query to emphasize (reserved color plus a label), or -1 for none +B3_API void b3RecPlayer_DrawFrameQueries( b3RecPlayer* player, b3DebugDraw* draw, int queryIndex, int selectedIndex ); + +/// The kind of a recorded spatial query, matching the public query and cast functions. +typedef enum b3RecQueryType +{ + b3_recQueryOverlapAABB, + b3_recQueryOverlapShape, + b3_recQueryCastRay, + b3_recQueryCastShape, + b3_recQueryCastRayClosest, + b3_recQueryCastMover, + b3_recQueryCollideMover, +} b3RecQueryType; + +/// A spatial query recorded during a replayed frame, exposed for inspection. +typedef struct b3RecQueryInfo +{ + b3RecQueryType type; + b3QueryFilter filter; + b3AABB aabb; // world-space bounds of the query, swept for casts + b3Pos origin; // query origin (zero for overlap AABB) + b3Vec3 translation; // ray and cast translation + int hitCount; // number of recorded results + uint64_t key; // identity key, the hash of (id, name), 0 if untagged + uint64_t id; // query id, 0 if none + const char* name; // query label, NULL if none +} b3RecQueryInfo; + +/// One result of a recorded spatial query. +typedef struct b3RecQueryHit +{ + b3ShapeId shape; + b3Pos point; + b3Vec3 normal; + float fraction; +} b3RecQueryHit; + +/// @return the number of spatial queries recorded for the most recently replayed frame +B3_API int b3RecPlayer_GetFrameQueryCount( const b3RecPlayer* player ); + +/// Get a recorded query from the most recently replayed frame by index. +B3_API b3RecQueryInfo b3RecPlayer_GetFrameQuery( const b3RecPlayer* player, int index ); + +/// Get one result of a recorded query from the most recently replayed frame. +B3_API b3RecQueryHit b3RecPlayer_GetFrameQueryHit( const b3RecPlayer* player, int queryIndex, int hitIndex ); + +/**@}*/ // recording + +/** @} */ // world + +/** + * @defgroup body Body + * This is the body API. + * @{ + */ + +/// Create a rigid body given a definition. No reference to the definition is retained. So you can create the definition +/// on the stack and pass it as a pointer. +/// @code{.c} +/// b3BodyDef bodyDef = b3DefaultBodyDef(); +/// b3BodyId myBodyId = b3CreateBody(myWorldId, &bodyDef); +/// @endcode +/// @warning This function is locked during callbacks. +B3_API b3BodyId b3CreateBody( b3WorldId worldId, const b3BodyDef* def ); + +/// Destroy a rigid body given an id. This destroys all shapes and joints attached to the body. +/// Do not keep references to the associated shapes and joints. +B3_API void b3DestroyBody( b3BodyId bodyId ); + +/// Body identifier validation. A valid body exists in a world and is non-null. +/// This can be used to detect orphaned ids. Provides validation for up to 64K allocations. +B3_API bool b3Body_IsValid( b3BodyId id ); + +/// Get the body type: static, kinematic, or dynamic +B3_API b3BodyType b3Body_GetType( b3BodyId bodyId ); + +/// Change the body type. This is an expensive operation. This automatically updates the mass +/// properties regardless of the automatic mass setting. +B3_API void b3Body_SetType( b3BodyId bodyId, b3BodyType type ); + +/// Set the body name. +B3_API void b3Body_SetName( b3BodyId bodyId, const char* name ); + +/// Get the body name. Returns an empty string if the name isn't set. +B3_API const char* b3Body_GetName( b3BodyId bodyId ); + +/// Set the user data for a body +B3_API void b3Body_SetUserData( b3BodyId bodyId, void* userData ); + +/// Get the user data stored in a body +B3_API void* b3Body_GetUserData( b3BodyId bodyId ); + +/// Get the world position of a body. This is the location of the body origin. +B3_API b3Pos b3Body_GetPosition( b3BodyId bodyId ); + +/// Get the world rotation of a body as a quaternion +B3_API b3Quat b3Body_GetRotation( b3BodyId bodyId ); + +/// Get the world transform of a body. +B3_API b3WorldTransform b3Body_GetTransform( b3BodyId bodyId ); + +/// Set the world transform of a body. This acts as a teleport and is fairly expensive. +/// @note Generally you should create a body with the intended transform. +/// @see b3BodyDef::position and b3BodyDef::rotation +B3_API void b3Body_SetTransform( b3BodyId bodyId, b3Pos position, b3Quat rotation ); + +/// Get a local point on a body given a world point +B3_API b3Vec3 b3Body_GetLocalPoint( b3BodyId bodyId, b3Pos worldPoint ); + +/// Get a world point on a body given a local point +B3_API b3Pos b3Body_GetWorldPoint( b3BodyId bodyId, b3Vec3 localPoint ); + +/// Get a local vector on a body given a world vector +B3_API b3Vec3 b3Body_GetLocalVector( b3BodyId bodyId, b3Vec3 worldVector ); + +/// Get a world vector on a body given a local vector +B3_API b3Vec3 b3Body_GetWorldVector( b3BodyId bodyId, b3Vec3 localVector ); + +/// Get the linear velocity of a body's center of mass. Usually in meters per second. +B3_API b3Vec3 b3Body_GetLinearVelocity( b3BodyId bodyId ); + +/// Get the angular velocity of a body in radians per second +B3_API b3Vec3 b3Body_GetAngularVelocity( b3BodyId bodyId ); + +/// Set the linear velocity of a body. Usually in meters per second. +B3_API void b3Body_SetLinearVelocity( b3BodyId bodyId, b3Vec3 linearVelocity ); + +/// Set the angular velocity of a body in radians per second +B3_API void b3Body_SetAngularVelocity( b3BodyId bodyId, b3Vec3 angularVelocity ); + +/// Set the velocity to reach the given transform after a given time step. +/// The result will be close but maybe not exact. This is meant for kinematic bodies. +/// The target is not applied if the velocity would be below the sleep threshold. +/// This will optionally wake the body if asleep, but only if the movement is significant. +B3_API void b3Body_SetTargetTransform( b3BodyId bodyId, b3WorldTransform target, float timeStep, bool wake ); + +/// Get the linear velocity of a local point attached to a body. Usually in meters per second. +B3_API b3Vec3 b3Body_GetLocalPointVelocity( b3BodyId bodyId, b3Vec3 localPoint ); + +/// Get the linear velocity of a world point attached to a body. Usually in meters per second. +B3_API b3Vec3 b3Body_GetWorldPointVelocity( b3BodyId bodyId, b3Pos worldPoint ); + +/// Apply a force at a world point. If the force is not applied at the center of mass, +/// it will generate a torque and affect the angular velocity. This optionally wakes up the body. +/// The force is ignored if the body is not awake. +/// @param bodyId The body id +/// @param force The world force vector, usually in newtons (N) +/// @param point The world position of the point of application +/// @param wake Option to wake up the body +B3_API void b3Body_ApplyForce( b3BodyId bodyId, b3Vec3 force, b3Pos point, bool wake ); + +/// Apply a force to the center of mass. This optionally wakes up the body. +/// The force is ignored if the body is not awake. +/// @param bodyId The body id +/// @param force the world force vector, usually in newtons (N). +/// @param wake also wake up the body +B3_API void b3Body_ApplyForceToCenter( b3BodyId bodyId, b3Vec3 force, bool wake ); + +/// Apply a torque. This affects the angular velocity without affecting the linear velocity. +/// This optionally wakes the body. The torque is ignored if the body is not awake. +/// @param bodyId The body id +/// @param torque the world torque vector, usually in N*m. +/// @param wake also wake up the body +B3_API void b3Body_ApplyTorque( b3BodyId bodyId, b3Vec3 torque, bool wake ); + +/// Apply an impulse at a point. This immediately modifies the velocity. +/// It also modifies the angular velocity if the point of application +/// is not at the center of mass. This optionally wakes the body. +/// The impulse is ignored if the body is not awake. +/// @param bodyId The body id +/// @param impulse the world impulse vector, usually in N*s or kg*m/s. +/// @param point the world position of the point of application. +/// @param wake also wake up the body +/// @warning This should be used for one-shot impulses. If you need a steady force, +/// use a force instead, which will work better with the sub-stepping solver. +B3_API void b3Body_ApplyLinearImpulse( b3BodyId bodyId, b3Vec3 impulse, b3Pos point, bool wake ); + +/// Apply an impulse to the center of mass. This immediately modifies the velocity. +/// The impulse is ignored if the body is not awake. This optionally wakes the body. +/// @param bodyId The body id +/// @param impulse the world impulse vector, usually in N*s or kg*m/s. +/// @param wake also wake up the body +/// @warning This should be used for one-shot impulses. If you need a steady force, +/// use a force instead, which will work better with the sub-stepping solver. +B3_API void b3Body_ApplyLinearImpulseToCenter( b3BodyId bodyId, b3Vec3 impulse, bool wake ); + +/// Apply an angular impulse in world space. The impulse is ignored if the body is not awake. +/// This optionally wakes the body. +/// @param bodyId The body id +/// @param impulse the world angular impulse vector, usually in units of kg*m*m/s +/// @param wake also wake up the body +/// @warning This should be used for one-shot impulses. If you need a steady torque, +/// use a torque instead, which will work better with the sub-stepping solver. +B3_API void b3Body_ApplyAngularImpulse( b3BodyId bodyId, b3Vec3 impulse, bool wake ); + +/// Get the mass of the body, usually in kilograms +B3_API float b3Body_GetMass( b3BodyId bodyId ); + +/// Get the rotational inertia of the body in local space, usually in kg*m^2 +B3_API b3Matrix3 b3Body_GetLocalRotationalInertia( b3BodyId bodyId ); + +/// Get the inverse mass of the body, usually in 1/kilograms +B3_API float b3Body_GetInverseMass( b3BodyId bodyId ); + +/// Get the inverse rotational inertia of the body in world space, usually in 1/kg*m^2 +B3_API b3Matrix3 b3Body_GetWorldInverseRotationalInertia( b3BodyId bodyId ); + +/// Get the center of mass position of the body in local space +B3_API b3Vec3 b3Body_GetLocalCenter( b3BodyId bodyId ); + +/// Get the center of mass position of the body in world space +B3_API b3Pos b3Body_GetWorldCenter( b3BodyId bodyId ); + +/// Override the body's mass properties. Normally this is computed automatically using the +/// shape geometry and density. This information is lost if a shape is added or removed or if the +/// body type changes. +B3_API void b3Body_SetMassData( b3BodyId bodyId, b3MassData massData ); + +/// Get the mass data for a body +B3_API b3MassData b3Body_GetMassData( b3BodyId bodyId ); + +/// This updates the mass properties to the sum of the mass properties of the shapes. +/// This normally does not need to be called unless you called SetMassData to override +/// the mass and you later want to reset the mass. +/// You may also use this when automatic mass computation has been disabled. +/// You should call this regardless of body type. +B3_API void b3Body_ApplyMassFromShapes( b3BodyId bodyId ); + +/// Adjust the linear damping. Normally this is set in b3BodyDef before creation. +B3_API void b3Body_SetLinearDamping( b3BodyId bodyId, float linearDamping ); + +/// Get the current linear damping. +B3_API float b3Body_GetLinearDamping( b3BodyId bodyId ); + +/// Adjust the angular damping. Normally this is set in b3BodyDef before creation. +B3_API void b3Body_SetAngularDamping( b3BodyId bodyId, float angularDamping ); + +/// Get the current angular damping. +B3_API float b3Body_GetAngularDamping( b3BodyId bodyId ); + +/// Adjust the gravity scale. Normally this is set in b3BodyDef before creation. +/// @see b3BodyDef::gravityScale +B3_API void b3Body_SetGravityScale( b3BodyId bodyId, float gravityScale ); + +/// Get the current gravity scale +B3_API float b3Body_GetGravityScale( b3BodyId bodyId ); + +/// @return true if this body is awake +B3_API bool b3Body_IsAwake( b3BodyId bodyId ); + +/// Wake a body from sleep. This wakes the entire island the body is touching. +/// @warning Putting a body to sleep will put the entire island of bodies touching this body to sleep, +/// which can be expensive and possibly unintuitive. +B3_API void b3Body_SetAwake( b3BodyId bodyId, bool awake ); + +/// Enable or disable sleeping for this body. If sleeping is disabled the body will wake. +B3_API void b3Body_EnableSleep( b3BodyId bodyId, bool enableSleep ); + +/// Returns true if sleeping is enabled for this body +B3_API bool b3Body_IsSleepEnabled( b3BodyId bodyId ); + +/// Set the sleep threshold, usually in meters per second +B3_API void b3Body_SetSleepThreshold( b3BodyId bodyId, float sleepThreshold ); + +/// Get the sleep threshold, usually in meters per second. +B3_API float b3Body_GetSleepThreshold( b3BodyId bodyId ); + +/// Returns true if this body is enabled +B3_API bool b3Body_IsEnabled( b3BodyId bodyId ); + +/// Disable a body by removing it completely from the simulation. This is expensive. +B3_API void b3Body_Disable( b3BodyId bodyId ); + +/// Enable a body by adding it to the simulation. This is expensive. +B3_API void b3Body_Enable( b3BodyId bodyId ); + +/// Set the motion locks on this body. +B3_API void b3Body_SetMotionLocks( b3BodyId bodyId, b3MotionLocks locks ); + +/// Get the motion locks for this body. +B3_API b3MotionLocks b3Body_GetMotionLocks( b3BodyId bodyId ); + +/// Set this body to be a bullet. A bullet does continuous collision detection +/// against dynamic bodies (but not other bullets). +B3_API void b3Body_SetBullet( b3BodyId bodyId, bool flag ); + +/// Is this body a bullet? +B3_API bool b3Body_IsBullet( b3BodyId bodyId ); + +/// Enable or disable contact recycling for this body. Contact recycling is a performance optimization +/// that reuses contact manifolds when bodies move slightly. Disabling it can avoid ghost collisions +/// on characters at the cost of higher per-step work. Existing contacts retain their prior setting; +/// only contacts created after this call see the new value. +/// @see b3BodyDef::enableContactRecycling +B3_API void b3Body_EnableContactRecycling( b3BodyId bodyId, bool flag ); + +/// Is contact recycling enabled on this body? +B3_API bool b3Body_IsContactRecyclingEnabled( b3BodyId bodyId ); + +/// Enable/disable hit events on all shapes +/// @see b3ShapeDef::enableHitEvents +B3_API void b3Body_EnableHitEvents( b3BodyId bodyId, bool flag ); + +/// Get the world that owns this body +B3_API b3WorldId b3Body_GetWorld( b3BodyId bodyId ); + +/// Get the number of shapes on this body +B3_API int b3Body_GetShapeCount( b3BodyId bodyId ); + +/// Get the shape ids for all shapes on this body, up to the provided capacity. +/// @returns the number of shape ids stored in the user array +B3_API int b3Body_GetShapes( b3BodyId bodyId, b3ShapeId* shapeArray, int capacity ); + +/// Get the number of joints on this body +B3_API int b3Body_GetJointCount( b3BodyId bodyId ); + +/// Get the joint ids for all joints on this body, up to the provided capacity +/// @returns the number of joint ids stored in the user array +B3_API int b3Body_GetJoints( b3BodyId bodyId, b3JointId* jointArray, int capacity ); + +/// Get the maximum capacity required for retrieving all the touching contacts on a body +B3_API int b3Body_GetContactCapacity( b3BodyId bodyId ); + +/// Get the touching contact data for a body +B3_API int b3Body_GetContactData( b3BodyId bodyId, b3ContactData* contactData, int capacity ); + +/// Get the current world AABB that contains all the attached shapes. Note that this may not encompass the body origin. +/// If there are no shapes attached then the returned AABB is empty and centered on the body origin. +B3_API b3AABB b3Body_ComputeAABB( b3BodyId bodyId ); + +/// Get the closest point on a body to a world target. +B3_API float b3Body_GetClosestPoint( b3BodyId bodyId, b3Vec3* result, b3Vec3 target ); + +/// Cast a ray at a specific body using a specified body transform. +B3_API b3BodyCastResult b3Body_CastRay( b3BodyId bodyId, b3Pos origin, b3Vec3 translation, b3QueryFilter filter, + float maxFraction, b3WorldTransform bodyTransform ); + +/// Cast a shape at a specific body using a specified body transform. +B3_API b3BodyCastResult b3Body_CastShape( b3BodyId bodyId, b3Pos origin, const b3ShapeProxy* proxy, b3Vec3 translation, + b3QueryFilter filter, float maxFraction, bool canEncroach, + b3WorldTransform bodyTransform ); + +/// Overlap a shape with a specific body using a specified body transform. +B3_API bool b3Body_OverlapShape( b3BodyId bodyId, b3Pos origin, const b3ShapeProxy* proxy, b3QueryFilter filter, + b3WorldTransform bodyTransform ); + +/// Collide a character mover with a specific body using a specified body transform. +B3_API int b3Body_CollideMover( b3BodyId bodyId, b3BodyPlaneResult* bodyPlanes, int planeCapacity, b3Pos origin, + const b3Capsule* mover, b3QueryFilter filter, b3WorldTransform bodyTransform ); + +/** @} */ // body + +/** + * @defgroup shape Shape + * Functions to create, destroy, and access. + * Shapes bind raw geometry to bodies and hold material properties including friction and restitution. + * @{ + */ + +/// Create a circle shape and attach it to a body. The shape definition and geometry are fully cloned. +/// Contacts are not created until the next time step. +/// @return the shape id for accessing the shape +B3_API b3ShapeId b3CreateSphereShape( b3BodyId bodyId, const b3ShapeDef* def, const b3Sphere* sphere ); + +/// Create a capsule shape and attach it to a body. The shape definition and geometry are fully cloned. +/// Contacts are not created until the next time step. +/// @return the shape id for accessing the shape +B3_API b3ShapeId b3CreateCapsuleShape( b3BodyId bodyId, const b3ShapeDef* def, const b3Capsule* capsule ); + +/// Create a convex hull shape and attach it to a body. The shape definition is fully cloned. Contacts are not created +/// until the next time step. +/// @return the shape id for accessing the shape +B3_API b3ShapeId b3CreateHullShape( b3BodyId bodyId, const b3ShapeDef* def, const b3HullData* hull ); + +/// Create a convex hull shape and attach it to a body. The hull is cloned then transformed with scale applied first. +/// Use this for non-uniform or mirrored scale or a baked local transform. The baked result is shared through the +/// world hull database. The shape definition and geometry are fully cloned. Contacts are not created until the next time step. +/// @return the shape id for accessing the shape +B3_API b3ShapeId b3CreateTransformedHullShape( b3BodyId bodyId, const b3ShapeDef* def, const b3HullData* hull, + b3Transform transform, b3Vec3 scale ); + +/// Create a mesh hull shape and attach it to a body. The shape definition is fully cloned but the mesh is not. +/// Contacts are not created until the next time step. +/// Mesh collision only creates contacts on static bodies. +/// @warning this holds reference to the input mesh data which must remain valid for the lifetime of this shape +/// @return the shape id for accessing the shape +B3_API b3ShapeId b3CreateMeshShape( b3BodyId bodyId, const b3ShapeDef* def, const b3MeshData* mesh, b3Vec3 scale ); + +/// Create a height-field shape and attach it to a body. The shape definition is fully cloned but the height field is not. +/// Contacts are not created until the next time step. +/// Height field is only allowed on static bodies. +/// @warning this holds reference to the input height field which must remain valid for the lifetime of this shape +/// @return the shape id for accessing the shape +B3_API b3ShapeId b3CreateHeightFieldShape( b3BodyId bodyId, const b3ShapeDef* def, const b3HeightFieldData* heightField ); + +/// Compound shapes are only allowed on static bodies. +B3_API b3ShapeId b3CreateCompoundShape( b3BodyId bodyId, b3ShapeDef* def, const b3CompoundData* compound ); + +/// Destroy a shape. You may defer the body mass update which can improve performance if several shapes on a +/// body are destroyed at once. +/// @see b3Body_ApplyMassFromShapes +B3_API void b3DestroyShape( b3ShapeId shapeId, bool updateBodyMass ); + +/// Shape identifier validation. Provides validation for up to 64K allocations. +B3_API bool b3Shape_IsValid( b3ShapeId id ); + +/// Get the type of a shape +B3_API b3ShapeType b3Shape_GetType( b3ShapeId shapeId ); + +/// Get the id of the body that a shape is attached to +B3_API b3BodyId b3Shape_GetBody( b3ShapeId shapeId ); + +/// Get the world that owns this shape +B3_API b3WorldId b3Shape_GetWorld( b3ShapeId shapeId ); + +/// Returns true if the shape is a sensor +B3_API bool b3Shape_IsSensor( b3ShapeId shapeId ); + +/// Set the shape name. +B3_API void b3Shape_SetName( b3ShapeId shapeId, const char* name ); + +/// Get the shape name. Returns an empty string if the name isn't set. +B3_API const char* b3Shape_GetName( b3ShapeId shapeId ); + +/// Set the user data for a shape +B3_API void b3Shape_SetUserData( b3ShapeId shapeId, void* userData ); + +/// Get the user data for a shape. This is useful when you get a shape id +/// from an event or query. +B3_API void* b3Shape_GetUserData( b3ShapeId shapeId ); + +/// Set the mass density of a shape, usually in kg/m^3. +/// This will optionally update the mass properties on the parent body. +/// @see b3ShapeDef::density, b3Body_ApplyMassFromShapes +B3_API void b3Shape_SetDensity( b3ShapeId shapeId, float density, bool updateBodyMass ); + +/// Get the density of a shape, usually in kg/m^3 +B3_API float b3Shape_GetDensity( b3ShapeId shapeId ); + +/// Set the friction on a shape +B3_API void b3Shape_SetFriction( b3ShapeId shapeId, float friction ); + +/// Get the friction of a shape +B3_API float b3Shape_GetFriction( b3ShapeId shapeId ); + +/// Set the shape restitution (bounciness) +B3_API void b3Shape_SetRestitution( b3ShapeId shapeId, float restitution ); + +/// Get the shape restitution +B3_API float b3Shape_GetRestitution( b3ShapeId shapeId ); + +/// Set the shape base surface material. Does not change per triangle materials. +B3_API void b3Shape_SetSurfaceMaterial( b3ShapeId shapeId, b3SurfaceMaterial surfaceMaterial ); + +/// Get the base shape surface material. +B3_API b3SurfaceMaterial b3Shape_GetSurfaceMaterial( b3ShapeId shapeId ); + +/// Get the number of mesh surface materials. +B3_API int b3Shape_GetMeshMaterialCount( b3ShapeId shapeId ); + +/// Set a surface material for a mesh shape. +B3_API void b3Shape_SetMeshMaterial( b3ShapeId shapeId, b3SurfaceMaterial surfaceMaterial, int index ); + +/// Get a surface material for a mesh shape +B3_API b3SurfaceMaterial b3Shape_GetMeshSurfaceMaterial( b3ShapeId shapeId, int index ); + +/// Get the shape filter +B3_API b3Filter b3Shape_GetFilter( b3ShapeId shapeId ); + +/// Set the current filter. This is almost as expensive as recreating the shape. +/// @see b3ShapeDef::filter +/// @param shapeId the shape +/// @param filter the new filter +/// @param invokeContacts if true then the shape will have all contacts recomputed the next time step (expensive) +B3_API void b3Shape_SetFilter( b3ShapeId shapeId, b3Filter filter, bool invokeContacts ); + +/// Enable sensor events for this shape. Only applies to kinematic and dynamic bodies. Ignored for sensors. +/// @see b3ShapeDef::isSensor +B3_API void b3Shape_EnableSensorEvents( b3ShapeId shapeId, bool flag ); + +/// Returns true if sensor events are enabled +B3_API bool b3Shape_AreSensorEventsEnabled( b3ShapeId shapeId ); + +/// Enable contact events for this shape. Only applies to kinematic and dynamic bodies. Ignored for sensors. +/// @see b3ShapeDef::enableContactEvents +B3_API void b3Shape_EnableContactEvents( b3ShapeId shapeId, bool flag ); + +/// Returns true if contact events are enabled +B3_API bool b3Shape_AreContactEventsEnabled( b3ShapeId shapeId ); + +/// Enable pre-solve contact events for this shape. Only applies to dynamic bodies. These are expensive +/// and must be carefully handled due to multithreading. Ignored for sensors. +/// @see b3PreSolveFcn +B3_API void b3Shape_EnablePreSolveEvents( b3ShapeId shapeId, bool flag ); + +/// Returns true if pre-solve events are enabled +B3_API bool b3Shape_ArePreSolveEventsEnabled( b3ShapeId shapeId ); + +/// Enable contact hit events for this shape. Ignored for sensors. +/// @see b3WorldDef.hitEventThreshold +B3_API void b3Shape_EnableHitEvents( b3ShapeId shapeId, bool flag ); + +/// Returns true if hit events are enabled +B3_API bool b3Shape_AreHitEventsEnabled( b3ShapeId shapeId ); + +/// Ray cast a shape directly. The ray runs from origin to origin + translation and the hit point +/// comes back as a world position, so the cast stays precise far from the world origin. +B3_API b3WorldCastOutput b3Shape_RayCast( b3ShapeId shapeId, b3Pos origin, b3Vec3 translation ); + +/// Get a copy of the shape's sphere. Asserts the type is correct. +B3_API b3Sphere b3Shape_GetSphere( b3ShapeId shapeId ); + +/// Get a copy of the shape's capsule. Asserts the type is correct. +B3_API b3Capsule b3Shape_GetCapsule( b3ShapeId shapeId ); + +/// Get the shape's convex hull. Asserts the type is correct. +B3_API const b3HullData* b3Shape_GetHull( b3ShapeId shapeId ); + +/// Get the shape's mesh. Asserts the type is correct. +B3_API b3Mesh b3Shape_GetMesh( b3ShapeId shapeId ); + +/// Get the shape's height field. Asserts the type is correct. +B3_API const b3HeightFieldData* b3Shape_GetHeightField( b3ShapeId shapeId ); + +/// Allows you to change a shape to be a sphere or update the current sphere. +/// This does not modify the mass properties. +/// @see b3Body_ApplyMassFromShapes +B3_API void b3Shape_SetSphere( b3ShapeId shapeId, const b3Sphere* sphere ); + +/// Allows you to change a shape to be a capsule or update the current capsule. +/// This does not modify the mass properties. +/// @see b3Body_ApplyMassFromShapes +B3_API void b3Shape_SetCapsule( b3ShapeId shapeId, const b3Capsule* capsule ); + +/// Allows you to change a shape to be a hull or update the current hull. +/// This does not modify the mass properties. +/// @see b3Body_ApplyMassFromShapes +B3_API void b3Shape_SetHull( b3ShapeId shapeId, const b3HullData* hull ); + +/// Allows you to change a shape to be a mesh or update the current mesh. +/// This does not modify the mass properties. +/// @see b3Body_ApplyMassFromShapes +B3_API void b3Shape_SetMesh( b3ShapeId shapeId, const b3MeshData* meshData, b3Vec3 scale ); + +/// Get the maximum capacity required for retrieving all the touching contacts on a shape +B3_API int b3Shape_GetContactCapacity( b3ShapeId shapeId ); + +/// Get the touching contact data for a shape. The provided shapeId will be either shapeIdA or shapeIdB on the contact data. +/// @note Box3D uses speculative collision so some contact points may be separated. +/// @returns the number of elements filled in the provided array +/// @warning do not ignore the return value, it specifies the valid number of elements +B3_API int b3Shape_GetContactData( b3ShapeId shapeId, b3ContactData* contactData, int capacity ); + +/// Get the maximum capacity required for retrieving all the overlapped shapes on a sensor shape. +/// This returns 0 if the provided shape is not a sensor. +/// @param shapeId the id of a sensor shape +/// @returns the required capacity to get all the overlaps in b3Shape_GetSensorOverlaps +B3_API int b3Shape_GetSensorCapacity( b3ShapeId shapeId ); + +/// Get the overlap data for a sensor shape. +/// @param shapeId the id of a sensor shape +/// @param visitorIds a user allocated array that is filled with the overlapping shapes (visitors) +/// @param capacity the capacity of overlappedShapes +/// @returns the number of elements filled in the provided array +/// @warning do not ignore the return value, it specifies the valid number of elements +/// @warning overlaps may contain destroyed shapes so use b3Shape_IsValid to confirm each overlap +B3_API int b3Shape_GetSensorData( b3ShapeId shapeId, b3ShapeId* visitorIds, int capacity ); + +/// Get the current world AABB +B3_API b3AABB b3Shape_GetAABB( b3ShapeId shapeId ); + +/// Compute the mass data for a shape +B3_API b3MassData b3Shape_ComputeMassData( b3ShapeId shapeId ); + +/// Get the closest point on a shape to a target point. Target and result are in world space. +B3_API b3Vec3 b3Shape_GetClosestPoint( b3ShapeId shapeId, b3Vec3 target ); + +/// Apply a wind force to the body for this shape using the density of air. This considers +/// the projected area of the shape in the wind direction. This also considers +/// the relative velocity of the shape. +/// @param shapeId the shape id +/// @param wind the wind velocity in world space +/// @param drag the drag coefficient, the force that opposes the relative velocity +/// @param lift the lift coefficient, the force that is perpendicular to the relative velocity +/// @param maxSpeed the maximum relative speed. Speed cap is necessary for stability. Typically 10m/s or less. +/// @param wake should this wake the body +B3_API void b3Shape_ApplyWind( b3ShapeId shapeId, b3Vec3 wind, float drag, float lift, float maxSpeed, bool wake ); + +/** @} */ // shape + +/** + * @defgroup joint Joint + * @brief Joints allow you to connect rigid bodies together while allowing various forms of relative motions. + * @{ + */ + +/// Destroy a joint +B3_API void b3DestroyJoint( b3JointId jointId, bool wakeAttached ); + +/// Joint identifier validation. Provides validation for up to 64K allocations. +B3_API bool b3Joint_IsValid( b3JointId id ); + +/// Get the joint type +B3_API b3JointType b3Joint_GetType( b3JointId jointId ); + +/// Get body A id on a joint +B3_API b3BodyId b3Joint_GetBodyA( b3JointId jointId ); + +/// Get body B id on a joint +B3_API b3BodyId b3Joint_GetBodyB( b3JointId jointId ); + +/// Get the world that owns this joint +B3_API b3WorldId b3Joint_GetWorld( b3JointId jointId ); + +/// Set the local frame on bodyA +B3_API void b3Joint_SetLocalFrameA( b3JointId jointId, b3Transform localFrame ); + +/// Get the local frame on bodyA +B3_API b3Transform b3Joint_GetLocalFrameA( b3JointId jointId ); + +/// Set the local frame on bodyB +B3_API void b3Joint_SetLocalFrameB( b3JointId jointId, b3Transform localFrame ); + +/// Get the local frame on bodyB +B3_API b3Transform b3Joint_GetLocalFrameB( b3JointId jointId ); + +/// Toggle collision between connected bodies +B3_API void b3Joint_SetCollideConnected( b3JointId jointId, bool shouldCollide ); + +/// Is collision allowed between connected bodies? +B3_API bool b3Joint_GetCollideConnected( b3JointId jointId ); + +/// Set the user data on a joint +B3_API void b3Joint_SetUserData( b3JointId jointId, void* userData ); + +/// Get the user data on a joint +B3_API void* b3Joint_GetUserData( b3JointId jointId ); + +/// Wake the bodies connect to this joint +B3_API void b3Joint_WakeBodies( b3JointId jointId ); + +/// Get the current constraint force for this joint +B3_API b3Vec3 b3Joint_GetConstraintForce( b3JointId jointId ); + +/// Get the current constraint torque for this joint +B3_API b3Vec3 b3Joint_GetConstraintTorque( b3JointId jointId ); + +/// Get the current linear separation error for this joint. Does not consider admissible movement. Usually in meters. +B3_API float b3Joint_GetLinearSeparation( b3JointId jointId ); + +/// Get the current angular separation error for this joint. Does not consider admissible movement. Usually in radians. +B3_API float b3Joint_GetAngularSeparation( b3JointId jointId ); + +/// Set the joint constraint tuning. Advanced feature. +/// @param jointId the joint +/// @param hertz the stiffness in Hertz (cycles per second) +/// @param dampingRatio the non-dimensional damping ratio (one for critical damping) +B3_API void b3Joint_SetConstraintTuning( b3JointId jointId, float hertz, float dampingRatio ); + +/// Get the joint constraint tuning. Advanced feature. +B3_API void b3Joint_GetConstraintTuning( b3JointId jointId, float* hertz, float* dampingRatio ); + +/// Set the force threshold for joint events (Newtons) +B3_API void b3Joint_SetForceThreshold( b3JointId jointId, float threshold ); + +/// Get the force threshold for joint events (Newtons) +B3_API float b3Joint_GetForceThreshold( b3JointId jointId ); + +/// Set the torque threshold for joint events (N-m) +B3_API void b3Joint_SetTorqueThreshold( b3JointId jointId, float threshold ); + +/// Get the torque threshold for joint events (N-m) +B3_API float b3Joint_GetTorqueThreshold( b3JointId jointId ); + +/** + * @defgroup parallel_joint Parallel Joint + * @brief Functions for the parallel joint. + * @{ + */ + +/// Create a parallel joint +/// @see b3ParallelJointDef for details +B3_API b3JointId b3CreateParallelJoint( b3WorldId worldId, const b3ParallelJointDef* def ); + +/// Set the spring stiffness in Hertz +B3_API void b3ParallelJoint_SetSpringHertz( b3JointId jointId, float hertz ); + +/// Set the spring damping ratio, non-dimensional +B3_API void b3ParallelJoint_SetSpringDampingRatio( b3JointId jointId, float dampingRatio ); + +/// Get the spring Hertz +B3_API float b3ParallelJoint_GetSpringHertz( b3JointId jointId ); + +/// Get the spring damping ratio +B3_API float b3ParallelJoint_GetSpringDampingRatio( b3JointId jointId ); + +/// Set the maximum spring torque, usually in newton-meters +B3_API void b3ParallelJoint_SetMaxTorque( b3JointId jointId, float force ); + +/// Get the maximum spring torque, usually in newton-meters +B3_API float b3ParallelJoint_GetMaxTorque( b3JointId jointId ); + +/** @} */ // parallel_joint + +/** + * @defgroup distance_joint Distance Joint + * @brief Functions for the distance joint. + * @{ + */ + +/// Create a distance joint +/// @see b3DistanceJointDef for details +B3_API b3JointId b3CreateDistanceJoint( b3WorldId worldId, const b3DistanceJointDef* def ); + +/// Set the rest length of a distance joint +/// @param jointId The id for a distance joint +/// @param length The new distance joint length +B3_API void b3DistanceJoint_SetLength( b3JointId jointId, float length ); + +/// Get the rest length of a distance joint +B3_API float b3DistanceJoint_GetLength( b3JointId jointId ); + +/// Enable/disable the distance joint spring. When disabled the distance joint is rigid. +B3_API void b3DistanceJoint_EnableSpring( b3JointId jointId, bool enableSpring ); + +/// Is the distance joint spring enabled? +B3_API bool b3DistanceJoint_IsSpringEnabled( b3JointId jointId ); + +/// Set the force range for the spring. +B3_API void b3DistanceJoint_SetSpringForceRange( b3JointId jointId, float lowerForce, float upperForce ); + +/// Get the force range for the spring. +B3_API void b3DistanceJoint_GetSpringForceRange( b3JointId jointId, float* lowerForce, float* upperForce ); + +/// Set the spring stiffness in Hertz +B3_API void b3DistanceJoint_SetSpringHertz( b3JointId jointId, float hertz ); + +/// Set the spring damping ratio, non-dimensional +B3_API void b3DistanceJoint_SetSpringDampingRatio( b3JointId jointId, float dampingRatio ); + +/// Get the spring Hertz +B3_API float b3DistanceJoint_GetSpringHertz( b3JointId jointId ); + +/// Get the spring damping ratio +B3_API float b3DistanceJoint_GetSpringDampingRatio( b3JointId jointId ); + +/// Enable joint limit. The limit only works if the joint spring is enabled. Otherwise the joint is rigid +/// and the limit has no effect. +B3_API void b3DistanceJoint_EnableLimit( b3JointId jointId, bool enableLimit ); + +/// Is the distance joint limit enabled? +B3_API bool b3DistanceJoint_IsLimitEnabled( b3JointId jointId ); + +/// Set the minimum and maximum length parameters of a distance joint +B3_API void b3DistanceJoint_SetLengthRange( b3JointId jointId, float minLength, float maxLength ); + +/// Get the distance joint minimum length +B3_API float b3DistanceJoint_GetMinLength( b3JointId jointId ); + +/// Get the distance joint maximum length +B3_API float b3DistanceJoint_GetMaxLength( b3JointId jointId ); + +/// Get the current length of a distance joint +B3_API float b3DistanceJoint_GetCurrentLength( b3JointId jointId ); + +/// Enable/disable the distance joint motor +B3_API void b3DistanceJoint_EnableMotor( b3JointId jointId, bool enableMotor ); + +/// Is the distance joint motor enabled? +B3_API bool b3DistanceJoint_IsMotorEnabled( b3JointId jointId ); + +/// Set the distance joint motor speed, usually in meters per second +B3_API void b3DistanceJoint_SetMotorSpeed( b3JointId jointId, float motorSpeed ); + +/// Get the distance joint motor speed, usually in meters per second +B3_API float b3DistanceJoint_GetMotorSpeed( b3JointId jointId ); + +/// Set the distance joint maximum motor force, usually in newtons +B3_API void b3DistanceJoint_SetMaxMotorForce( b3JointId jointId, float force ); + +/// Get the distance joint maximum motor force, usually in newtons +B3_API float b3DistanceJoint_GetMaxMotorForce( b3JointId jointId ); + +/// Get the distance joint current motor force, usually in newtons +B3_API float b3DistanceJoint_GetMotorForce( b3JointId jointId ); + +/** @} */ // distance_joint + +/** + * @defgroup motor_joint Motor Joint + * @brief Functions for the motor joint. + * + * The motor joint is designed to control the movement of a body while still being + * responsive to collisions. A spring controls the position and rotation. A velocity motor + * can be used to control velocity and allows for friction in top-down games. Both types + * of control can be combined. For example, you can have a spring with friction. + * Position and velocity control have force and torque limits. + * @{ + */ + +/// Create a motor joint +/// @see b3MotorJointDef for details +B3_API b3JointId b3CreateMotorJoint( b3WorldId worldId, const b3MotorJointDef* def ); + +/// Set the desired relative linear velocity in meters per second +B3_API void b3MotorJoint_SetLinearVelocity( b3JointId jointId, b3Vec3 velocity ); + +/// Get the desired relative linear velocity in meters per second +B3_API b3Vec3 b3MotorJoint_GetLinearVelocity( b3JointId jointId ); + +/// Set the desired relative angular velocity in radians per second +B3_API void b3MotorJoint_SetAngularVelocity( b3JointId jointId, b3Vec3 velocity ); + +/// Get the desired relative angular velocity in radians per second +B3_API b3Vec3 b3MotorJoint_GetAngularVelocity( b3JointId jointId ); + +/// Set the motor joint maximum force, usually in newtons +B3_API void b3MotorJoint_SetMaxVelocityForce( b3JointId jointId, float maxForce ); + +/// Get the motor joint maximum force, usually in newtons +B3_API float b3MotorJoint_GetMaxVelocityForce( b3JointId jointId ); + +/// Set the motor joint maximum torque, usually in newton-meters +B3_API void b3MotorJoint_SetMaxVelocityTorque( b3JointId jointId, float maxTorque ); + +/// Get the motor joint maximum torque, usually in newton-meters +B3_API float b3MotorJoint_GetMaxVelocityTorque( b3JointId jointId ); + +/// Set the spring linear hertz stiffness +B3_API void b3MotorJoint_SetLinearHertz( b3JointId jointId, float hertz ); + +/// Get the spring linear hertz stiffness +B3_API float b3MotorJoint_GetLinearHertz( b3JointId jointId ); + +/// Set the spring linear damping ratio. Use 1.0 for critical damping. +B3_API void b3MotorJoint_SetLinearDampingRatio( b3JointId jointId, float damping ); + +/// Get the spring linear damping ratio. +B3_API float b3MotorJoint_GetLinearDampingRatio( b3JointId jointId ); + +/// Set the spring angular hertz stiffness +B3_API void b3MotorJoint_SetAngularHertz( b3JointId jointId, float hertz ); + +/// Get the spring angular hertz stiffness +B3_API float b3MotorJoint_GetAngularHertz( b3JointId jointId ); + +/// Set the spring angular damping ratio. Use 1.0 for critical damping. +B3_API void b3MotorJoint_SetAngularDampingRatio( b3JointId jointId, float damping ); + +/// Get the spring angular damping ratio. +B3_API float b3MotorJoint_GetAngularDampingRatio( b3JointId jointId ); + +/// Set the maximum spring force in newtons. +B3_API void b3MotorJoint_SetMaxSpringForce( b3JointId jointId, float maxForce ); + +/// Get the maximum spring force in newtons. +B3_API float b3MotorJoint_GetMaxSpringForce( b3JointId jointId ); + +/// Set the maximum spring torque in newtons * meters +B3_API void b3MotorJoint_SetMaxSpringTorque( b3JointId jointId, float maxTorque ); + +/// Get the maximum spring torque in newtons * meters +B3_API float b3MotorJoint_GetMaxSpringTorque( b3JointId jointId ); + +/**@}*/ // motor_joint + +/** + * @defgroup filter_joint Filter Joint + * @brief Functions for the filter joint. + * + * The filter joint is used to disable collision between two bodies. As a side effect of being a joint, it also + * keeps the two bodies in the same simulation island. + * @{ + */ + +/// Create a filter joint. +/// @see b3FilterJointDef for details +B3_API b3JointId b3CreateFilterJoint( b3WorldId worldId, const b3FilterJointDef* def ); + +/**@}*/ // filter_joint + +/** + * @defgroup prismatic_joint Prismatic Joint + * @brief A prismatic joint allows for translation along a single axis with no rotation. + * + * The prismatic joint is useful for things like pistons and moving platforms, where you want a body to translate + * along an axis and have no rotation. Also called a *slider* joint. + * @{ + */ + +/// Create a prismatic (slider) joint. +/// @see b3PrismaticJointDef for details +B3_API b3JointId b3CreatePrismaticJoint( b3WorldId worldId, const b3PrismaticJointDef* def ); + +/// Enable/disable the joint spring. +B3_API void b3PrismaticJoint_EnableSpring( b3JointId jointId, bool enableSpring ); + +/// Is the prismatic joint spring enabled or not? +B3_API bool b3PrismaticJoint_IsSpringEnabled( b3JointId jointId ); + +/// Set the prismatic joint stiffness in Hertz. +/// This should usually be less than a quarter of the simulation rate. For example, if the simulation +/// runs at 60Hz then the joint stiffness should be 15Hz or less. +B3_API void b3PrismaticJoint_SetSpringHertz( b3JointId jointId, float hertz ); + +/// Get the prismatic joint stiffness in Hertz +B3_API float b3PrismaticJoint_GetSpringHertz( b3JointId jointId ); + +/// Set the prismatic joint damping ratio (non-dimensional) +B3_API void b3PrismaticJoint_SetSpringDampingRatio( b3JointId jointId, float dampingRatio ); + +/// Get the prismatic spring damping ratio (non-dimensional) +B3_API float b3PrismaticJoint_GetSpringDampingRatio( b3JointId jointId ); + +/// Set the prismatic joint target translation. Usually in meters. +B3_API void b3PrismaticJoint_SetTargetTranslation( b3JointId jointId, float targetTranslation ); + +/// Get the prismatic joint target translation. Usually in meters. +B3_API float b3PrismaticJoint_GetTargetTranslation( b3JointId jointId ); + +/// Enable/disable a prismatic joint limit +B3_API void b3PrismaticJoint_EnableLimit( b3JointId jointId, bool enableLimit ); + +/// Is the prismatic joint limit enabled? +B3_API bool b3PrismaticJoint_IsLimitEnabled( b3JointId jointId ); + +/// Get the prismatic joint lower limit +B3_API float b3PrismaticJoint_GetLowerLimit( b3JointId jointId ); + +/// Get the prismatic joint upper limit +B3_API float b3PrismaticJoint_GetUpperLimit( b3JointId jointId ); + +/// Set the prismatic joint limits +B3_API void b3PrismaticJoint_SetLimits( b3JointId jointId, float lower, float upper ); + +/// Enable/disable a prismatic joint motor +B3_API void b3PrismaticJoint_EnableMotor( b3JointId jointId, bool enableMotor ); + +/// Is the prismatic joint motor enabled? +B3_API bool b3PrismaticJoint_IsMotorEnabled( b3JointId jointId ); + +/// Set the prismatic joint motor speed, usually in meters per second +B3_API void b3PrismaticJoint_SetMotorSpeed( b3JointId jointId, float motorSpeed ); + +/// Get the prismatic joint motor speed, usually in meters per second +B3_API float b3PrismaticJoint_GetMotorSpeed( b3JointId jointId ); + +/// Set the prismatic joint maximum motor force, usually in newtons +B3_API void b3PrismaticJoint_SetMaxMotorForce( b3JointId jointId, float force ); + +/// Get the prismatic joint maximum motor force, usually in newtons +B3_API float b3PrismaticJoint_GetMaxMotorForce( b3JointId jointId ); + +/// Get the prismatic joint current motor force, usually in newtons +B3_API float b3PrismaticJoint_GetMotorForce( b3JointId jointId ); + +/// Get the current joint translation, usually in meters. +B3_API float b3PrismaticJoint_GetTranslation( b3JointId jointId ); + +/// Get the current joint translation speed, usually in meters per second. +B3_API float b3PrismaticJoint_GetSpeed( b3JointId jointId ); + +/**@}*/ // prismatic_joint + +/** + * @defgroup revolute_joint Revolute Joint + * @brief A revolute joint allows for relative rotation about a single axis with no relative translation. + * + * Also called a *hinge* or *pin* joint. + * @{ + */ + +/// Create a revolute joint +/// @see b3RevoluteJointDef for details +B3_API b3JointId b3CreateRevoluteJoint( b3WorldId worldId, const b3RevoluteJointDef* def ); + +/// Enable/disable the revolute joint spring +B3_API void b3RevoluteJoint_EnableSpring( b3JointId jointId, bool enableSpring ); + +/// Is the revolute angular spring enabled? +B3_API bool b3RevoluteJoint_IsSpringEnabled( b3JointId jointId ); + +/// Set the revolute joint spring stiffness in Hertz +B3_API void b3RevoluteJoint_SetSpringHertz( b3JointId jointId, float hertz ); + +/// Get the revolute joint spring stiffness in Hertz +B3_API float b3RevoluteJoint_GetSpringHertz( b3JointId jointId ); + +/// Set the revolute joint spring damping ratio, non-dimensional +B3_API void b3RevoluteJoint_SetSpringDampingRatio( b3JointId jointId, float dampingRatio ); + +/// Get the revolute joint spring damping ratio, non-dimensional +B3_API float b3RevoluteJoint_GetSpringDampingRatio( b3JointId jointId ); + +/// Set the revolute joint target angle in radians +B3_API void b3RevoluteJoint_SetTargetAngle( b3JointId jointId, float targetRadians ); + +/// Get the revolute joint target angle in radians +B3_API float b3RevoluteJoint_GetTargetAngle( b3JointId jointId ); + +/// Get the revolute joint current angle in radians relative to the reference angle +/// @see b3RevoluteJointDef::referenceAngle +B3_API float b3RevoluteJoint_GetAngle( b3JointId jointId ); + +/// Enable/disable the revolute joint limit +B3_API void b3RevoluteJoint_EnableLimit( b3JointId jointId, bool enableLimit ); + +/// Is the revolute joint limit enabled? +B3_API bool b3RevoluteJoint_IsLimitEnabled( b3JointId jointId ); + +/// Get the revolute joint lower limit in radians +B3_API float b3RevoluteJoint_GetLowerLimit( b3JointId jointId ); + +/// Get the revolute joint upper limit in radians +B3_API float b3RevoluteJoint_GetUpperLimit( b3JointId jointId ); + +/// Set the revolute joint limits in radians +B3_API void b3RevoluteJoint_SetLimits( b3JointId jointId, float lowerLimitRadians, float upperLimitRadians ); + +/// Enable/disable a revolute joint motor +B3_API void b3RevoluteJoint_EnableMotor( b3JointId jointId, bool enableMotor ); + +/// Is the revolute joint motor enabled? +B3_API bool b3RevoluteJoint_IsMotorEnabled( b3JointId jointId ); + +/// Set the revolute joint motor speed in radians per second +B3_API void b3RevoluteJoint_SetMotorSpeed( b3JointId jointId, float motorSpeed ); + +/// Get the revolute joint motor speed in radians per second +B3_API float b3RevoluteJoint_GetMotorSpeed( b3JointId jointId ); + +/// Get the revolute joint current motor torque, usually in newton-meters +B3_API float b3RevoluteJoint_GetMotorTorque( b3JointId jointId ); + +/// Set the revolute joint maximum motor torque, usually in newton-meters +B3_API void b3RevoluteJoint_SetMaxMotorTorque( b3JointId jointId, float torque ); + +/// Get the revolute joint maximum motor torque, usually in newton-meters +B3_API float b3RevoluteJoint_GetMaxMotorTorque( b3JointId jointId ); + +/**@}*/ // revolute_joint + +/** + * @defgroup spherical_joint Spherical Joint + * @brief A spherical joint allows for relative rotation in the 3D space with no relative translation. + * + * Also called a *ball-in-socket* or *point-to-point* joint. + * @{ + */ + +/// Create a spherical joint +/// @see b3SphericalJointDef for details +B3_API b3JointId b3CreateSphericalJoint( b3WorldId worldId, const b3SphericalJointDef* def ); + +/// Enable/disable the spherical joint cone limit +B3_API void b3SphericalJoint_EnableConeLimit( b3JointId jointId, bool enableLimit ); + +/// Is the spherical joint cone limit enabled? +B3_API bool b3SphericalJoint_IsConeLimitEnabled( b3JointId jointId ); + +/// Get the spherical joint cone limit in radians +B3_API float b3SphericalJoint_GetConeLimit( b3JointId jointId ); + +/// Set the spherical joint limits in radians +B3_API void b3SphericalJoint_SetConeLimit( b3JointId jointId, float angleRadians ); + +/// Get the spherical joint current cone angle in radians. +B3_API float b3SphericalJoint_GetConeAngle( b3JointId jointId ); + +/// Enable/disable the spherical joint limit +B3_API void b3SphericalJoint_EnableTwistLimit( b3JointId jointId, bool enableLimit ); + +/// Is the spherical joint limit enabled? +B3_API bool b3SphericalJoint_IsTwistLimitEnabled( b3JointId jointId ); + +/// Get the spherical joint lower limit in radians +B3_API float b3SphericalJoint_GetLowerTwistLimit( b3JointId jointId ); + +/// Get the spherical joint upper limit in radians +B3_API float b3SphericalJoint_GetUpperTwistLimit( b3JointId jointId ); + +/// Set the spherical joint limits in radians +B3_API void b3SphericalJoint_SetTwistLimits( b3JointId jointId, float lowerLimitRadians, float upperLimitRadians ); + +/// Get the spherical joint current twist angle in radians. +B3_API float b3SphericalJoint_GetTwistAngle( b3JointId jointId ); + +/// Enable/disable the spherical joint spring +B3_API void b3SphericalJoint_EnableSpring( b3JointId jointId, bool enableSpring ); + +/// Is the spherical angular spring enabled? +B3_API bool b3SphericalJoint_IsSpringEnabled( b3JointId jointId ); + +/// Set the spherical joint spring stiffness in Hertz +B3_API void b3SphericalJoint_SetSpringHertz( b3JointId jointId, float hertz ); + +/// Get the spherical joint spring stiffness in Hertz +B3_API float b3SphericalJoint_GetSpringHertz( b3JointId jointId ); + +/// Set the spherical joint spring damping ratio, non-dimensional +B3_API void b3SphericalJoint_SetSpringDampingRatio( b3JointId jointId, float dampingRatio ); + +/// Get the spherical joint spring damping ratio, non-dimensional +B3_API float b3SphericalJoint_GetSpringDampingRatio( b3JointId jointId ); + +/// Set the spherical joint spring target rotation +B3_API void b3SphericalJoint_SetTargetRotation( b3JointId jointId, b3Quat targetRotation ); + +/// Get the spherical joint spring target rotation +B3_API b3Quat b3SphericalJoint_GetTargetRotation( b3JointId jointId ); + +/// Enable/disable a spherical joint motor +B3_API void b3SphericalJoint_EnableMotor( b3JointId jointId, bool enableMotor ); + +/// Is the spherical joint motor enabled? +B3_API bool b3SphericalJoint_IsMotorEnabled( b3JointId jointId ); + +/// Set the spherical joint motor velocity in radians per second +B3_API void b3SphericalJoint_SetMotorVelocity( b3JointId jointId, b3Vec3 motorVelocity ); + +/// Get the spherical joint motor velocity in radians per second +B3_API b3Vec3 b3SphericalJoint_GetMotorVelocity( b3JointId jointId ); + +/// Get the spherical joint current motor torque, usually in newton-meters +B3_API b3Vec3 b3SphericalJoint_GetMotorTorque( b3JointId jointId ); + +/// Set the spherical joint maximum motor torque, usually in newton-meters +B3_API void b3SphericalJoint_SetMaxMotorTorque( b3JointId jointId, float torque ); + +/// Get the spherical joint maximum motor torque, usually in newton-meters +B3_API float b3SphericalJoint_GetMaxMotorTorque( b3JointId jointId ); + +/**@}*/ // spherical_joint + +/** + * @defgroup weld_joint Weld Joint + * @brief A weld joint fully constrains the relative transform between two bodies while allowing for springiness + * + * A weld joint constrains the relative rotation and translation between two bodies. Both rotation and translation + * can have damped springs. + * + * @note The accuracy of weld joint is limited by the accuracy of the solver. Long chains of weld joints may flex. + * @{ + */ + +/// Create a weld joint +/// @see b3WeldJointDef for details +B3_API b3JointId b3CreateWeldJoint( b3WorldId worldId, const b3WeldJointDef* def ); + +/// Set the weld joint linear stiffness in Hertz. 0 is rigid. +B3_API void b3WeldJoint_SetLinearHertz( b3JointId jointId, float hertz ); + +/// Get the weld joint linear stiffness in Hertz +B3_API float b3WeldJoint_GetLinearHertz( b3JointId jointId ); + +/// Set the weld joint linear damping ratio (non-dimensional) +B3_API void b3WeldJoint_SetLinearDampingRatio( b3JointId jointId, float dampingRatio ); + +/// Get the weld joint linear damping ratio (non-dimensional) +B3_API float b3WeldJoint_GetLinearDampingRatio( b3JointId jointId ); + +/// Set the weld joint angular stiffness in Hertz. 0 is rigid. +B3_API void b3WeldJoint_SetAngularHertz( b3JointId jointId, float hertz ); + +/// Get the weld joint angular stiffness in Hertz +B3_API float b3WeldJoint_GetAngularHertz( b3JointId jointId ); + +/// Set weld joint angular damping ratio, non-dimensional +B3_API void b3WeldJoint_SetAngularDampingRatio( b3JointId jointId, float dampingRatio ); + +/// Get the weld joint angular damping ratio, non-dimensional +B3_API float b3WeldJoint_GetAngularDampingRatio( b3JointId jointId ); + +/**@}*/ // weld_joint + +/** + * @defgroup wheel_joint Wheel Joint + * The wheel joint can be used to simulate wheels on vehicles. + * + * The wheel joint restricts body B to move along a local axis in body A. Body B is free to + * rotate. Supports a linear spring, linear limits, and a rotational motor. + * + * @{ + */ + +/// Create a wheel joint. +/// @see b3WheelJointDef for details. +B3_API b3JointId b3CreateWheelJoint( b3WorldId worldId, const b3WheelJointDef* def ); + +/// Enable/disable the wheel joint spring. +B3_API void b3WheelJoint_EnableSuspension( b3JointId jointId, bool flag ); + +/// Is the wheel joint spring enabled? +B3_API bool b3WheelJoint_IsSuspensionEnabled( b3JointId jointId ); + +/// Set the wheel joint stiffness in Hertz. +B3_API void b3WheelJoint_SetSuspensionHertz( b3JointId jointId, float hertz ); + +/// Get the wheel joint stiffness in Hertz. +B3_API float b3WheelJoint_GetSuspensionHertz( b3JointId jointId ); + +/// Set the wheel joint damping ratio, non-dimensional. +B3_API void b3WheelJoint_SetSuspensionDampingRatio( b3JointId jointId, float dampingRatio ); + +/// Get the wheel joint damping ratio, non-dimensional. +B3_API float b3WheelJoint_GetSuspensionDampingRatio( b3JointId jointId ); + +/// Enable/disable the wheel joint limit. +B3_API void b3WheelJoint_EnableSuspensionLimit( b3JointId jointId, bool flag ); + +/// Is the wheel joint limit enabled? +B3_API bool b3WheelJoint_IsSuspensionLimitEnabled( b3JointId jointId ); + +/// Get the wheel joint lower limit. +B3_API float b3WheelJoint_GetLowerSuspensionLimit( b3JointId jointId ); + +/// Get the wheel joint upper limit. +B3_API float b3WheelJoint_GetUpperSuspensionLimit( b3JointId jointId ); + +/// Set the wheel joint limits. +B3_API void b3WheelJoint_SetSuspensionLimits( b3JointId jointId, float lower, float upper ); + +/// Enable/disable the wheel joint motor. +B3_API void b3WheelJoint_EnableSpinMotor( b3JointId jointId, bool flag ); + +/// Is the wheel joint motor enabled? +B3_API bool b3WheelJoint_IsSpinMotorEnabled( b3JointId jointId ); + +/// Set the wheel joint motor speed in radians per second. +B3_API void b3WheelJoint_SetSpinMotorSpeed( b3JointId jointId, float speed ); + +/// Get the wheel joint motor speed in radians per second. +B3_API float b3WheelJoint_GetSpinMotorSpeed( b3JointId jointId ); + +/// Set the wheel joint maximum motor torque, usually in newton-meters. +B3_API void b3WheelJoint_SetMaxSpinTorque( b3JointId jointId, float torque ); + +/// Get the wheel joint maximum motor torque, usually in newton-meters. +B3_API float b3WheelJoint_GetMaxSpinTorque( b3JointId jointId ); + +/// Get the current spin speed in radians per second. +B3_API float b3WheelJoint_GetSpinSpeed( b3JointId jointId ); + +/// Get the wheel joint current motor torque, usually in newton-meters. +B3_API float b3WheelJoint_GetSpinTorque( b3JointId jointId ); + +/// Enable/disable wheel steering. Steering allows the wheel to rotate about the suspension axis. +B3_API void b3WheelJoint_EnableSteering( b3JointId jointId, bool flag ); + +/// Can the wheel steer? +B3_API bool b3WheelJoint_IsSteeringEnabled( b3JointId jointId ); + +/// Set the wheel joint steering stiffness in Hertz. +B3_API void b3WheelJoint_SetSteeringHertz( b3JointId jointId, float hertz ); + +/// Get the wheel joint steering stiffness in Hertz. +B3_API float b3WheelJoint_GetSteeringHertz( b3JointId jointId ); + +/// Set the wheel joint steering damping ratio, non-dimensional. +B3_API void b3WheelJoint_SetSteeringDampingRatio( b3JointId jointId, float dampingRatio ); + +/// Get the wheel joint steering damping ratio, non-dimensional. +B3_API float b3WheelJoint_GetSteeringDampingRatio( b3JointId jointId ); + +/// Set the wheel joint maximum steering torque in N*m. +B3_API void b3WheelJoint_SetMaxSteeringTorque( b3JointId jointId, float torque ); + +/// Get the wheel joint maximum steering torque in N*m. +B3_API float b3WheelJoint_GetMaxSteeringTorque( b3JointId jointId ); + +/// Enable/disable the wheel joint steering limit. +B3_API void b3WheelJoint_EnableSteeringLimit( b3JointId jointId, bool flag ); + +/// Is the wheel joint steering limit enabled? +B3_API bool b3WheelJoint_IsSteeringLimitEnabled( b3JointId jointId ); + +/// Get the wheel joint lower steering limit in radians. +B3_API float b3WheelJoint_GetLowerSteeringLimit( b3JointId jointId ); + +/// Get the wheel joint upper steering limit in radians. +B3_API float b3WheelJoint_GetUpperSteeringLimit( b3JointId jointId ); + +/// Set the wheel joint steering limits in radians. +B3_API void b3WheelJoint_SetSteeringLimits( b3JointId jointId, float lowerRadians, float upperRadians ); + +/// Set the wheel joint target steering angle in radians. +B3_API void b3WheelJoint_SetTargetSteeringAngle( b3JointId jointId, float radians ); + +/// Get the wheel joint target steering angle in radians. +B3_API float b3WheelJoint_GetTargetSteeringAngle( b3JointId jointId ); + +/// Get the current steering angle in radians. +B3_API float b3WheelJoint_GetSteeringAngle( b3JointId jointId ); + +/// Get the current steering torque in N*m. +B3_API float b3WheelJoint_GetSteeringTorque( b3JointId jointId ); + +/**@}*/ // wheel_joint + +/**@}*/ // joint + +/** + * @defgroup contact Contact + * Access to contacts + * @{ + */ + +/// Contact identifier validation. Provides validation for up to 2^32 allocations. +B3_API bool b3Contact_IsValid( b3ContactId id ); + +/// Get the manifolds for a contact. The manifold may have no points if the contact is not touching. +B3_API b3ContactData b3Contact_GetData( b3ContactId contactId ); + +/**@}*/ // contact diff --git a/vendor/box3d/src/include/box3d/collision.h b/vendor/box3d/src/include/box3d/collision.h new file mode 100644 index 000000000..1ba0070a6 --- /dev/null +++ b/vendor/box3d/src/include/box3d/collision.h @@ -0,0 +1,642 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +#pragma once + +#include "base.h" +#include "math_functions.h" +#include "types.h" + +#include +#include + +/** + * @addtogroup tree + * @{ + */ + +/// Constructing the tree initializes the node pool. +B3_API b3DynamicTree b3DynamicTree_Create( int proxyCapacity ); + +/// Destroy the tree, freeing the node pool. +B3_API void b3DynamicTree_Destroy( b3DynamicTree* tree ); + +/// Create a proxy. Provide an AABB and a userData value. +B3_API int b3DynamicTree_CreateProxy( b3DynamicTree* tree, b3AABB aabb, uint64_t categoryBits, uint64_t userData ); + +/// Destroy a proxy. This asserts if the id is invalid. +B3_API void b3DynamicTree_DestroyProxy( b3DynamicTree* tree, int proxyId ); + +/// Move a proxy to a new AABB by removing and reinserting into the tree. +B3_API void b3DynamicTree_MoveProxy( b3DynamicTree* tree, int proxyId, b3AABB aabb ); + +/// Enlarge a proxy and enlarge ancestors as necessary. +B3_API void b3DynamicTree_EnlargeProxy( b3DynamicTree* tree, int proxyId, b3AABB aabb ); + +/// Modify the category bits on a proxy. This is an expensive operation. +B3_API void b3DynamicTree_SetCategoryBits( b3DynamicTree* tree, int proxyId, uint64_t categoryBits ); + +/// Get the category bits on a proxy. +B3_API uint64_t b3DynamicTree_GetCategoryBits( b3DynamicTree* tree, int proxyId ); + +/// Query an AABB for overlapping proxies. The callback function is called for each proxy that overlaps the supplied AABB. +/// @return performance data +B3_API b3TreeStats b3DynamicTree_Query( const b3DynamicTree* tree, b3AABB aabb, uint64_t maskBits, bool requireAllBits, + b3TreeQueryCallbackFcn* callback, void* context ); + +/// Query an AABB for the closest object. The callback function is called for each proxy that might be closest to the supplied point. +/// @param tree the dynamic tree to query +/// @param point the query point +/// @param maskBits nodes are skipped if the bit-wise AND with the node category bits is zero +/// @param requireAllBits nodes are skipped if the bit-wise AND with the node category bits does not equal the maskBits +/// @param callback a user provided instance of b3TreeQueryClosestCallbackFcn +/// @param context a user context object that is provided to the callback +/// @param minDistanceSqr the initial and final minimum squared distance. Provide a small initial to restrict the search and +/// improve performance. If the value is large this query has performance that scales linearly with the number of proxies and +/// would be slower than a brute force search. +/// @return performance data +B3_API b3TreeStats b3DynamicTree_QueryClosest( const b3DynamicTree* tree, b3Vec3 point, uint64_t maskBits, bool requireAllBits, + b3TreeQueryClosestCallbackFcn* callback, void* context, float* minDistanceSqr ); + +/// Ray cast against the proxies in the tree. This relies on the callback +/// to perform an exact ray cast in the case where the proxy contains a shape. +/// The callback also performs any collision filtering. This has performance +/// roughly equal to k * log(n), where k is the number of collisions and n is the +/// number of proxies in the tree. +/// Bit-wise filtering using mask bits can greatly improve performance in some scenarios. +/// However, this filtering may be approximate, so the user should still apply filtering to results. +/// @param tree the dynamic tree to ray cast +/// @param input the ray cast input data. The ray extends from p1 to p1 + maxFraction * (p2 - p1) +/// @param maskBits bit mask test: `bool accept = (maskBits & node->categoryBits) != 0;` +/// @param requireAllBits modifies bit mask test: `bool accept = (maskBits & node->categoryBits) == maskBits;` +/// @param callback a callback function that is called for each proxy that is hit by the ray +/// @param context user context that is passed to the callback +/// @return performance data +B3_API b3TreeStats b3DynamicTree_RayCast( const b3DynamicTree* tree, const b3RayCastInput* input, uint64_t maskBits, + bool requireAllBits, b3TreeRayCastCallbackFcn* callback, void* context ); + +/// Sweep an AABB through the tree. The box is in the tree's world float frame and the callback +/// re-differences each shape at full precision against the query origin. Used by the large world +/// spatial queries so the tree traversal stays float while the narrow phase stays precise. +B3_API b3TreeStats b3DynamicTree_BoxCast( const b3DynamicTree* tree, const b3BoxCastInput* input, uint64_t maskBits, + bool requireAllBits, b3TreeBoxCastCallbackFcn* callback, void* context ); + +/// Get the height of the binary tree. +B3_API int b3DynamicTree_GetHeight( const b3DynamicTree* tree ); + +/// Get the ratio of the sum of the node areas to the root area. +B3_API float b3DynamicTree_GetAreaRatio( const b3DynamicTree* tree ); + +/// Get the bounding box that contains the entire tree +B3_API b3AABB b3DynamicTree_GetRootBounds( const b3DynamicTree* tree ); + +/// Get the number of proxies created +B3_API int b3DynamicTree_GetProxyCount( const b3DynamicTree* tree ); + +/// Rebuild the tree while retaining subtrees that haven't changed. Returns the number of boxes sorted. +B3_API int b3DynamicTree_Rebuild( b3DynamicTree* tree, bool fullBuild ); + +/// Get the number of bytes used by this tree +B3_API int b3DynamicTree_GetByteCount( const b3DynamicTree* tree ); + +/// Validate this tree. For testing. +B3_API void b3DynamicTree_Validate( const b3DynamicTree* tree ); + +/// Validate this tree has no enlarged AABBs. For testing. +B3_API void b3DynamicTree_ValidateNoEnlarged( const b3DynamicTree* tree ); + +/// Save this tree to a file for debugging +B3_API void b3DynamicTree_Save( const b3DynamicTree* tree, const char* fileName ); + +/// Load a file for debugging +B3_API b3DynamicTree b3DynamicTree_Load( const char* fileName, float scale ); + +/// Get proxy user data +B3_INLINE uint64_t b3DynamicTree_GetUserData( const b3DynamicTree* tree, int proxyId ) +{ + return tree->nodes[proxyId].userData; +} + +/// Get the AABB of a proxy +B3_INLINE b3AABB b3DynamicTree_GetAABB( const b3DynamicTree* tree, int proxyId ) +{ + return tree->nodes[proxyId].aabb; +} + +/**@}*/ // tree + +/** + * @addtogroup hull + * @{ + */ + +/// Get read only hull vertices. +B3_INLINE const b3HullVertex* b3GetHullVertices( const b3HullData* hull ) +{ + if ( hull->vertexOffset == 0 ) + { + return NULL; + } + + return (const b3HullVertex*)( (intptr_t)hull + hull->vertexOffset ); +} + +/// Get read only hull points. +B3_INLINE const b3Vec3* b3GetHullPoints( const b3HullData* hull ) +{ + if ( hull->pointOffset == 0 ) + { + return NULL; + } + + return (const b3Vec3*)( (intptr_t)hull + hull->pointOffset ); +} + +/// Get read only hull half edges. +B3_INLINE const b3HullHalfEdge* b3GetHullEdges( const b3HullData* hull ) +{ + if ( hull->edgeOffset == 0 ) + { + return NULL; + } + + return (const b3HullHalfEdge*)( (intptr_t)hull + hull->edgeOffset ); +} + +/// Get read only hull faces. +B3_INLINE const b3HullFace* b3GetHullFaces( const b3HullData* hull ) +{ + if ( hull->faceOffset == 0 ) + { + return NULL; + } + + return (const b3HullFace*)( (intptr_t)hull + hull->faceOffset ); +} + +/// Get read only hull planes. +B3_INLINE const b3Plane* b3GetHullPlanes( const b3HullData* hull ) +{ + if ( hull->planeOffset == 0 ) + { + return NULL; + } + + return (const b3Plane*)( (intptr_t)hull + hull->planeOffset ); +} + +/// Create a tessellated cylinder as a hull. +B3_API b3HullData* b3CreateCylinder( float height, float radius, float yOffset, int sides ); + +/// Create a tessellated cone as a hull. +B3_API b3HullData* b3CreateCone( float height, float radius1, float radius2, int slices ); + +/// Create a rock shaped hull. +B3_API b3HullData* b3CreateRock( float radius ); + +/// Create a generic convex hull. +B3_API b3HullData* b3CreateHull( const b3Vec3* points, int pointCount, int maxVertexCount ); + +/// Deep clone a hull. +B3_API b3HullData* b3CloneHull( const b3HullData* hull ); + +/// Clone and transform a hull. Supports non-uniform and mirroring scale. +B3_API b3HullData* b3CloneAndTransformHull( const b3HullData* original, b3Transform transform, b3Vec3 scale ); + +/// Destroy a hull. +B3_API void b3DestroyHull( b3HullData* hull ); + +/// Make a cube as a hull. Do not call b3DestroyHull on this. +B3_API b3BoxHull b3MakeCubeHull( float halfWidth ); + +/// Make a box as a hull. Do not call b3DestroyHull on this. +B3_API b3BoxHull b3MakeBoxHull( float hx, float hy, float hz ); + +/// Make an offset box as a hull. Do not call b3DestroyHull on this. +B3_API b3BoxHull b3MakeOffsetBoxHull( float hx, float hy, float hz, b3Vec3 offset ); + +/// Make a transformed box as a hull. Do not call b3DestroyHull on this. +/// @param hx, hy, hz positive half widths +/// @param transform local transform of box +B3_API b3BoxHull b3MakeTransformedBoxHull( float hx, float hy, float hz, b3Transform transform ); + +/// This makes a transformed box hull with post scaling. This is useful for boxes that are scaled in +/// a level editor. Such scaling can have reflection and shear. In the case of shear the result +/// may be approximate. If you need to support shear consider using b3CreateHull. +/// Do not call b3DestroyHull on this. +/// @param halfWidths positive half widths +/// @param transform local transform of box +/// @param postScale scale applied after the transform, may be negative +B3_API b3BoxHull b3MakeScaledBoxHull( b3Vec3 halfWidths, b3Transform transform, b3Vec3 postScale ); + +/// This takes a box with a transform and post scale and converts it into a box with the post scale +/// resolved with new half-widths and transform. This accepts non-uniform and negative scale. +/// This is approximate if there is shear. +/// @param halfWidths [in/out] the box half widths +/// @param transform [in/out] the box transform with rotation and translation +/// @param postScale the post scale being applied to the box after the transform +/// @param minHalfWidth the minimum half width after scale is applied +B3_API void b3ScaleBox( b3Vec3* halfWidths, b3Transform* transform, b3Vec3 postScale, float minHalfWidth ); + +/**@}*/ // hull + +/** + * @addtogroup mesh + * @{ + */ + +/// Get read only mesh BVH nodes. +B3_INLINE const b3MeshNode* b3GetMeshNodes( const b3MeshData* mesh ) +{ + if ( mesh->nodeOffset == 0 ) + { + return NULL; + } + + return (const b3MeshNode*)( (intptr_t)mesh + mesh->nodeOffset ); +} + +/// Get read only mesh vertices. +B3_INLINE const b3Vec3* b3GetMeshVertices( const b3MeshData* mesh ) +{ + if ( mesh->vertexOffset == 0 ) + { + return NULL; + } + + return (const b3Vec3*)( (intptr_t)mesh + mesh->vertexOffset ); +} + +/// Get read only mesh triangles. +B3_INLINE const b3MeshTriangle* b3GetMeshTriangles( const b3MeshData* mesh ) +{ + if ( mesh->triangleOffset == 0 ) + { + return NULL; + } + + return (const b3MeshTriangle*)( (intptr_t)mesh + mesh->triangleOffset ); +} + +/// Get read only mesh materials. The count is equal to the triangle count. +B3_INLINE const uint8_t* b3GetMeshMaterialIndices( const b3MeshData* mesh ) +{ + if ( mesh->materialOffset == 0 ) + { + return NULL; + } + + return (const uint8_t*)( (intptr_t)mesh + mesh->materialOffset ); +} + +/// Get read only mesh flags. The count is equal to the triangle count. +B3_INLINE const uint8_t* b3GetMeshFlags( const b3MeshData* mesh ) +{ + if ( mesh->flagsOffset == 0 ) + { + return NULL; + } + + return (const uint8_t*)( (intptr_t)mesh + mesh->flagsOffset ); +} + +/// Create a grid mesh along the x and z axes. +/// @param xCount the number of rows in the x direction +/// @param zCount the number of rows in the z direction +/// @param cellWidth the width of each cell +/// @param materialCount the number of materials to generate +/// @param identifyEdges compute adjacency information +B3_API b3MeshData* b3CreateGridMesh( int xCount, int zCount, float cellWidth, int materialCount, bool identifyEdges ); + +/// Create a wave mesh along the x and z axes. +B3_API b3MeshData* b3CreateWaveMesh( int xCount, int zCount, float cellWidth, float amplitude, float rowFrequency, + float columnFrequency ); + +/// Create a torus mesh. +B3_API b3MeshData* b3CreateTorusMesh( int radialResolution, int tubularResolution, float radius, float thickness ); + +/// Create a box mesh. +B3_API b3MeshData* b3CreateBoxMesh( b3Vec3 center, b3Vec3 extent, bool identifyEdges ); + +/// Create a hollow box mesh. +B3_API b3MeshData* b3CreateHollowBoxMesh( b3Vec3 center, b3Vec3 extent ); + +/// Create a platform mesh. A truncated pyramid. +B3_API b3MeshData* b3CreatePlatformMesh( b3Vec3 center, float height, float topWidth, float bottomWidth ); + +/// Create a generic mesh. +B3_API b3MeshData* b3CreateMesh( const b3MeshDef* def, int* degenerateTriangleIndices, int degenerateCapacity ); + +/// Destroy a mesh. +B3_API void b3DestroyMesh( b3MeshData* mesh ); + +/// Get the height of the mesh BVH. +B3_API int b3GetHeight( const b3MeshData* mesh ); + +/**@}*/ // mesh + +/** + * @addtogroup height_field + * @{ + */ + +/// Get read only compressed heights. One uint16_t per grid point. +B3_INLINE const uint16_t* b3GetHeightFieldCompressedHeights( const b3HeightFieldData* hf ) +{ + if ( hf->heightsOffset == 0 ) + { + return NULL; + } + + return (const uint16_t*)( (intptr_t)hf + hf->heightsOffset ); +} + +/// Get read only material indices. One uint8_t per cell. +B3_INLINE const uint8_t* b3GetHeightFieldMaterialIndices( const b3HeightFieldData* hf ) +{ + if ( hf->materialOffset == 0 ) + { + return NULL; + } + + return (const uint8_t*)( (intptr_t)hf + hf->materialOffset ); +} + +/// Get read only triangle flags. One uint8_t per triangle. +B3_INLINE const uint8_t* b3GetHeightFieldFlags( const b3HeightFieldData* hf ) +{ + if ( hf->flagsOffset == 0 ) + { + return NULL; + } + + return (const uint8_t*)( (intptr_t)hf + hf->flagsOffset ); +} + +/// Create a generic height field. +B3_API b3HeightFieldData* b3CreateHeightField( const b3HeightFieldDef* data ); + +/// Create a grid as a height field. +B3_API b3HeightFieldData* b3CreateGrid( int rowCount, int columnCount, b3Vec3 scale, bool makeHoles ); + +/// Create a wave grid as a height field. +B3_API b3HeightFieldData* b3CreateWave( int rowCount, int columnCount, b3Vec3 scale, float rowFrequency, float columnFrequency, + bool makeHoles ); + +/// Destroy a height field. +B3_API void b3DestroyHeightField( b3HeightFieldData* heightField ); + +/// Save input height data to a file +B3_API void b3DumpHeightData( const b3HeightFieldDef* data, const char* fileName ); + +/// Create a height field by loading a previously saved height data +B3_API b3HeightFieldData* b3LoadHeightField( const char* fileName ); + +/**@}*/ // height_field + +/** + * @addtogroup compound + * @{ + */ + +/// Get a child shape of a compound. +B3_API b3ChildShape b3GetCompoundChild( const b3CompoundData* compound, int childIndex ); + +/// Query a compound shape for children that overlap an AABB. +B3_API void b3QueryCompound( const b3CompoundData* compound, b3AABB aabb, b3CompoundQueryFcn* fcn, void* context ); + +/// Access a child capsule by index. +B3_API b3CompoundCapsule b3GetCompoundCapsule( const b3CompoundData* compound, int index ); + +/// Access a child hull by index. +B3_API b3CompoundHull b3GetCompoundHull( const b3CompoundData* compound, int index ); + +/// Access a child mesh by index. +B3_API b3CompoundMesh b3GetCompoundMesh( const b3CompoundData* compound, int index ); + +/// Access a child sphere by index. +B3_API b3CompoundSphere b3GetCompoundSphere( const b3CompoundData* compound, int index ); + +/// Access the compound material array. +B3_API const b3SurfaceMaterial* b3GetCompoundMaterials( const b3CompoundData* compound ); + +/// Create a compound shape. All input data in the definition is cloned into the resulting compound. +B3_API b3CompoundData* b3CreateCompound( const b3CompoundDef* def ); + +/// Destroy a compound shape. +B3_API void b3DestroyCompound( b3CompoundData* compound ); + +/// If bytes is null then this returns the number of required bytes. This clones all the +/// data into the bytes buffer. This is expected to run offline or asynchronously. +/// This mutates the compound to nullify pointers, leaving the compound in an unusable state. +B3_API uint8_t* b3ConvertCompoundToBytes( b3CompoundData* compound ); + +/// Convert bytes to compound. This does not clone. The bytes must remain in scope while the +/// compound is used. This is done to improve run-time performance and allow for instancing. +/// The bytes are mutated to fixup pointers. +B3_API b3CompoundData* b3ConvertBytesToCompound( uint8_t* bytes, int byteCount ); + +/**@}*/ // compound + +/** + * @addtogroup geometry + * @{ + */ + +/// Compute mass properties of a sphere +B3_API b3MassData b3ComputeSphereMass( const b3Sphere* shape, float density ); + +/// Compute mass properties of a capsule +B3_API b3MassData b3ComputeCapsuleMass( const b3Capsule* shape, float density ); + +/// Compute mass properties of a hull +B3_API b3MassData b3ComputeHullMass( const b3HullData* shape, float density ); + +/// Compute the bounding box of a transformed sphere +B3_API b3AABB b3ComputeSphereAABB( const b3Sphere* shape, b3Transform transform ); + +/// Compute the bounding box of a transformed capsule +B3_API b3AABB b3ComputeCapsuleAABB( const b3Capsule* shape, b3Transform transform ); + +/// Compute the bounding box of a transformed hull +B3_API b3AABB b3ComputeHullAABB( const b3HullData* shape, b3Transform transform ); + +/// Compute the bounding box of a transformed mesh. Scale may be non-uniform and have negative components. +B3_API b3AABB b3ComputeMeshAABB( const b3MeshData* shape, b3Transform transform, b3Vec3 scale ); + +/// Compute the bounding box of a transformed height-field +B3_API b3AABB b3ComputeHeightFieldAABB( const b3HeightFieldData* shape, b3Transform transform ); + +/// Compute the bounding box of a compound +B3_API b3AABB b3ComputeCompoundAABB( const b3CompoundData* shape, b3Transform transform ); + +/**@}*/ // geometry + +/** + * @addtogroup query + * @{ + */ + +/// Use this to ensure your ray cast input is valid and avoid internal assertions. +B3_API bool b3IsValidRay( const b3RayCastInput* input ); + +/// Overlap shape versus capsule +B3_API bool b3OverlapCapsule( const b3Capsule* shape, b3Transform shapeTransform, const b3ShapeProxy* proxy ); + +/// Overlap shape versus compound +B3_API bool b3OverlapCompound( const b3CompoundData* shape, b3Transform shapeTransform, const b3ShapeProxy* proxy ); + +/// Overlap shape versus height field +B3_API bool b3OverlapHeightField( const b3HeightFieldData* shape, b3Transform shapeTransform, const b3ShapeProxy* proxy ); + +/// Overlap shape versus hull +B3_API bool b3OverlapHull( const b3HullData* shape, b3Transform shapeTransform, const b3ShapeProxy* proxy ); + +/// Overlap shape versus mesh +B3_API bool b3OverlapMesh( const b3Mesh* shape, b3Transform shapeTransform, const b3ShapeProxy* proxy ); + +/// Overlap shape versus sphere +B3_API bool b3OverlapSphere( const b3Sphere* shape, b3Transform shapeTransform, const b3ShapeProxy* proxy ); + +/// Ray cast versus sphere in local space. A zero length ray is a point query. Initial overlap +/// reports a hit at the ray origin with zero fraction and zero normal. +B3_API b3CastOutput b3RayCastSphere( const b3Sphere* shape, const b3RayCastInput* input ); + +/// Ray cast versus a hollow sphere shell in local space. Unlike the solid sphere a ray starting +/// inside is not an overlap: it passes through and hits the far wall. +B3_API b3CastOutput b3RayCastHollowSphere( const b3Sphere* shape, const b3RayCastInput* input ); + +/// Ray cast versus capsule in local space. A zero length ray is a point query. Initial overlap +/// reports a hit at the ray origin with zero fraction and zero normal. +B3_API b3CastOutput b3RayCastCapsule( const b3Capsule* shape, const b3RayCastInput* input ); + +/// Ray cast versus compound in local space. A zero length ray is a point query. Initial overlap +/// with a child reports a hit at the ray origin with zero fraction and zero normal. +B3_API b3CastOutput b3RayCastCompound( const b3CompoundData* shape, const b3RayCastInput* input ); + +/// Ray cast versus hull shape in local space. A zero length ray is a point query. Initial overlap +/// reports a hit at the ray origin with zero fraction and zero normal. +B3_API b3CastOutput b3RayCastHull( const b3HullData* shape, const b3RayCastInput* input ); + +/// Ray cast versus mesh in local space. A thin surface with no interior, so there is no overlap case. +B3_API b3CastOutput b3RayCastMesh( const b3Mesh* shape, const b3RayCastInput* input ); + +/// Ray cast versus height field in local space. A thin surface with no interior, so there is no overlap case. +B3_API b3CastOutput b3RayCastHeightField( const b3HeightFieldData* shape, const b3RayCastInput* input ); + +/// Shape cast versus a sphere. Initial overlap is treated as a miss. +B3_API b3CastOutput b3ShapeCastSphere( const b3Sphere* shape, const b3ShapeCastInput* input ); + +/// Shape cast versus a capsule. Initial overlap is treated as a miss. +B3_API b3CastOutput b3ShapeCastCapsule( const b3Capsule* shape, const b3ShapeCastInput* input ); + +/// Shape cast versus compound. Initial overlap is treated as a miss. +B3_API b3CastOutput b3ShapeCastCompound( const b3CompoundData* shape, const b3ShapeCastInput* input ); + +/// Shape cast versus a hull. Initial overlap is treated as a miss. +B3_API b3CastOutput b3ShapeCastHull( const b3HullData* shape, const b3ShapeCastInput* input ); + +/// Shape cast versus a mesh. Initial overlap is treated as a miss. +B3_API b3CastOutput b3ShapeCastMesh( const b3Mesh* shape, const b3ShapeCastInput* input ); + +/// Shape cast versus a height field. Initial overlap is treated as a miss. +B3_API b3CastOutput b3ShapeCastHeightField( const b3HeightFieldData* shape, const b3ShapeCastInput* input ); + +/// Query callback. +typedef bool b3MeshQueryFcn( b3Vec3 a, b3Vec3 b, b3Vec3 c, int triangleIndex, void* context ); + +/// Query a mesh for triangles overlapping a bounding box in local space. May have false positives. Useful for debug draw. +/// @param mesh the mesh to query, includes scale +/// @param bounds the bounding box in local space +/// @param fcn a user function to collect triangles +/// @param context the context sent to the user function. +B3_API void b3QueryMesh( const b3Mesh* mesh, const b3AABB bounds, b3MeshQueryFcn* fcn, void* context ); + +/// Query a height field for triangles overlapping a bounding box in local space. May have false positives. Useful for debug draw. +/// @param heightField the height field to query +/// @param bounds the bounding box in local space +/// @param fcn a user function to collect triangles +/// @param context the context sent to the user function. +B3_API void b3QueryHeightField( const b3HeightFieldData* heightField, b3AABB bounds, b3MeshQueryFcn* fcn, void* context ); + +/// Compute the closest points between two shapes represented as point clouds. +/// b3SimplexCache cache is input/output. On the first call set b3SimplexCache.count to zero. +/// The query runs in frame A, so the witness points and normal are returned in frame A. +/// The underlying GJK algorithm may be debugged by passing in debug simplexes and capacity. You may pass in NULL and 0 for these. +B3_API b3DistanceOutput b3ShapeDistance( const b3DistanceInput* input, b3SimplexCache* cache, b3Simplex* simplexes, + int simplexCapacity ); + +/// Perform a linear shape cast of shape B moving and shape A fixed. Determines the hit point, normal, and translation fraction. +/// The query runs in frame A, so the hit point and normal are returned in frame A. Initially touching shapes are a miss. +B3_API b3CastOutput b3ShapeCast( const b3ShapeCastPairInput* input ); + +/// Evaluate the transform sweep at a specific time. +B3_API b3Transform b3GetSweepTransform( const b3Sweep* sweep, float time ); + +/// Compute the upper bound on time before two shapes penetrate. Time is represented as +/// a fraction between [0,tMax]. This uses a swept separating axis and may miss some intermediate, +/// non-tunneling collisions. If you change the time interval, you should call this function +/// again. +B3_API b3TOIOutput b3TimeOfImpact( const b3TOIInput* input ); + +/**@}*/ // query + +/** + * @addtogroup collision + * @{ + */ + +/// Collide two spheres. +B3_API void b3CollideSpheres( b3LocalManifold* manifold, int capacity, const b3Sphere* sphereA, const b3Sphere* sphereB, + b3Transform transformBtoA ); + +/// Collide a capsule and a sphere. +B3_API void b3CollideCapsuleAndSphere( b3LocalManifold* manifold, int capacity, const b3Capsule* capsuleA, + const b3Sphere* sphereB, b3Transform transformBtoA ); + +/// Collide a hull and a sphere. +B3_API void b3CollideHullAndSphere( b3LocalManifold* manifold, int capacity, const b3HullData* hullA, const b3Sphere* sphereB, + b3Transform transformBtoA, b3SimplexCache* cache ); + +/// Collide two capsules. +B3_API void b3CollideCapsules( b3LocalManifold* manifold, int capacity, const b3Capsule* capsuleA, const b3Capsule* capsuleB, + b3Transform transformBtoA ); + +/// Collide a hull and a capsule. +B3_API void b3CollideHullAndCapsule( b3LocalManifold* manifold, int capacity, const b3HullData* hullA, const b3Capsule* capsuleB, + b3Transform transformBtoA, b3SimplexCache* cache ); + +/// Collide two hulls. +B3_API void b3CollideHulls( b3LocalManifold* manifold, int capacity, const b3HullData* hullA, const b3HullData* hullB, + b3Transform transformBtoA, b3SATCache* cache ); + +/// Collide a capsule and a triangle. +B3_API void b3CollideCapsuleAndTriangle( b3LocalManifold* manifold, int capacity, const b3Capsule* capsuleA, + const b3Vec3* triangleB, b3SimplexCache* cache ); + +/// Collide a hull and a triangle. +B3_API void b3CollideHullAndTriangle( b3LocalManifold* manifold, int capacity, const b3HullData* hullA, b3Vec3 v1, b3Vec3 v2, + b3Vec3 v3, int triangleFlags, b3SATCache* cache, bool enableSpeculative ); + +/// Collide a sphere and a triangle. +B3_API void b3CollideSphereAndTriangle( b3LocalManifold* manifold, int capacity, const b3Sphere* sphereA, + const b3Vec3* triangleB ); + +/**@}*/ // collision + +/** + * @addtogroup character + * @{ + */ + +/// Solves the position of a mover that satisfies the given collision planes. +/// @param targetDelta the desired translation from the position used to generate the collision planes +/// @param planes the collision planes +/// @param count the number of collision planes +B3_API b3PlaneSolverResult b3SolvePlanes( b3Vec3 targetDelta, b3CollisionPlane* planes, int count ); + +/// Clips the velocity against the given collision planes. Planes with zero push or clipVelocity +/// set to false are skipped. +B3_API b3Vec3 b3ClipVector( b3Vec3 vector, const b3CollisionPlane* planes, int count ); + +/**@}*/ // character diff --git a/vendor/box3d/src/include/box3d/config.h b/vendor/box3d/src/include/box3d/config.h new file mode 100644 index 000000000..2790fc0b8 --- /dev/null +++ b/vendor/box3d/src/include/box3d/config.h @@ -0,0 +1,31 @@ +// SPDX-FileCopyrightText: 2026 Erin Catto +// SPDX-License-Identifier: MIT + +#pragma once + +// Box3D compile-time options. +// +// Normally set by the CMake BOX3D_* options. If you build Box3D without its +// CMake (dropping the sources into another project), set them here so the +// library and your code agree. Edit this file, or keep your settings elsewhere +// and point Box3D at them from your build: +// +// #define BOX3D_USER_CONFIG "my_box3d_config.h" +// +// A define passed on the compiler command line still wins over this file. + +// Large world mode. Stores world positions in double precision. Affects ABI. +//#define BOX3D_DOUBLE_PRECISION + +// Build the scalar fallback instead of SSE2/NEON. +//#define BOX3D_DISABLE_SIMD + +// Enable internal validation in debug builds. +//#define BOX3D_VALIDATE + +// Decorate the public API with your own export macro instead of Box3D's +// box3d_EXPORTS/BOX3D_DLL scheme, for example when compiling Box3D into another +// shared library. A single value cannot switch between dllexport and dllimport, +// so this suits embedding more than shipping Box3D as its own DLL. +//#define BOX3D_EXPORT MYENGINE_API + diff --git a/vendor/box3d/src/include/box3d/constants.h b/vendor/box3d/src/include/box3d/constants.h new file mode 100644 index 000000000..87dd56d7e --- /dev/null +++ b/vendor/box3d/src/include/box3d/constants.h @@ -0,0 +1,110 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +#pragma once + +#include "base.h" + +/// Box3D bases all length units on meters, but you may need different units for your game. +/// You can set this value to use different units. This should be done at application startup +/// and only modified once. Default value is 1. +/// @warning This must be modified before any calls to Box3D +B3_API void b3SetLengthUnitsPerMeter( float lengthUnits ); + +/// Get the current length units per meter. +B3_API float b3GetLengthUnitsPerMeter( void ); + +/// Set the threshold for logging stalls. +B3_API void b3SetStallThreshold( float seconds ); + +/// Get the threshold for logging stalls. +B3_API float b3GetStallThreshold( void ); + +// Used to detect bad values. In float mode positions greater than about 16km have precision +// problems, so 100km is a safe limit. Large world mode keeps coordinates accurate much farther +// from the origin, so the sanity limit widens to keep valid far-field positions from tripping it. +#if defined( BOX3D_DOUBLE_PRECISION ) +#define B3_HUGE ( 1.0e9f * b3GetLengthUnitsPerMeter() ) +#else +#define B3_HUGE ( 1.0e5f * b3GetLengthUnitsPerMeter() ) +#endif + +/// Maximum parallel workers. Used for some fixed size arrays. +#define B3_MAX_WORKERS 32 + +/// Maximum number of tasks queued per world step. b3EnqueueTaskCallback will never be called +/// more than this per world step. This is related to B3_MAX_WORKERS. With 32 workers, +/// the maximum observed task count is 130. This allows an external task system to use a fixed +/// size array for Box3D task, which may help with creating stable user task pointers. +#define B3_MAX_TASKS 256 + +// Maximum number of colors in the constraint graph. Constraints that cannot +// find a color are added to the overflow set which are solved single-threaded. +// The compound barrel benchmark has minor overflow with 24 colors +#define B3_GRAPH_COLOR_COUNT 24 + +// Number of contact point buckets for counting the number of contact points per +// shape contact pair. This is just for reporting and doesn't affect simulation. +#define B3_CONTACT_MANIFOLD_COUNT_BUCKETS 8 + +// A small length used as a collision and constraint tolerance. Usually it is +// chosen to be numerically significant, but visually insignificant. In meters. +// @warning modifying this can have a significant impact on stability +#define B3_LINEAR_SLOP ( 0.005f * b3GetLengthUnitsPerMeter() ) + +#define B3_MIN_CAPSULE_LENGTH ( B3_LINEAR_SLOP ) + +/// The distance between shapes where they are considered overlapped. This is needed +/// because GJK may return small positive values for overlapped shapes in degenerate +/// configurations. +#define B3_OVERLAP_SLOP ( 0.1f * B3_LINEAR_SLOP ) + +/// Maximum number of simultaneous worlds that can be allocated +#ifndef B3_MAX_WORLDS +#define B3_MAX_WORLDS 128 +#endif + +/// The maximum rotation of a body per time step. This limit is very large and is used +/// to prevent numerical problems. You shouldn't need to adjust this. +/// @warning increasing this to 0.5f * B3_PI or greater will break continuous collision. +#define B3_MAX_ROTATION ( 0.25f * B3_PI ) + +/// @warning modifying this can have a significant impact on performance and stability +#define B3_SPECULATIVE_DISTANCE ( 4.0f * B3_LINEAR_SLOP ) + +/// The rest offset is used for mesh contact to reduce ghost collisions and assist with CCD. +/// The rest offset adjusts the contact point separation value, making the solver push the shapes +/// apart by this distance. +/// Must be at least B3_LINEAR_SLOP and less than B3_SPECULATIVE_DISTANCE. +#define B3_MESH_REST_OFFSET ( 1.0f * B3_LINEAR_SLOP ) + +/// The default contact recycling distance. +#define B3_CONTACT_RECYCLE_DISTANCE ( 10.0f * B3_LINEAR_SLOP ) + +/// The default contact recycling world angle threshold. For performance this value +/// is cos(angle/2)^2. This value corresponds to 10 degrees. +#define B3_CONTACT_RECYCLE_ANGULAR_DISTANCE ( 0.99240388f ) + +/// This is used to fatten AABBs in the dynamic tree. This allows proxies +/// to move by a small amount without triggering a tree adjustment. This is in meters. +/// @warning modifying this can have a significant impact on performance +#define B3_MAX_AABB_MARGIN ( 0.05f * b3GetLengthUnitsPerMeter() ) + +/// Per-shape AABB margin is a fraction of the shape extent (capped by B3_MAX_AABB_MARGIN). +/// Small shapes get small margins; large shapes are clamped to the cap. +#define B3_AABB_MARGIN_FRACTION 0.125f + +/// The time that a body must be still before it will go to sleep. In seconds. +#define B3_TIME_TO_SLEEP 0.5f + +/// The maximum number of contact points between two touching shapes. +#define B3_MAX_MANIFOLD_POINTS 4 + +/// The maximum number points to use for shape cast proxies (swept point cloud). +#define B3_MAX_SHAPE_CAST_POINTS 64 + +/// These generous limits allow for easy hashing. See b3ShapePairKey. +#define B3_SHAPE_POWER 22 +#define B3_CHILD_POWER ( 64 - 2 * B3_SHAPE_POWER ) +#define B3_MAX_SHAPES ( 1 << B3_SHAPE_POWER ) +#define B3_MAX_CHILD_SHAPES ( 1 << B3_CHILD_POWER ) diff --git a/vendor/box3d/src/include/box3d/id.h b/vendor/box3d/src/include/box3d/id.h new file mode 100644 index 000000000..447fd34ed --- /dev/null +++ b/vendor/box3d/src/include/box3d/id.h @@ -0,0 +1,178 @@ +// SPDX-FileCopyrightText: 2026 Erin Catto +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +// Note: this file should be stand-alone + +/** + * @defgroup id Ids + * These ids serve as handles to internal Box3D objects. + * These should be considered opaque data and passed by value. + * Include this header if you need the id types and not the whole Box3D API. + * All ids are considered null if initialized to zero. + * + * For example in C++: + * + * @code{.cxx} + * b3WorldId worldId = {}; + * @endcode + * + * Or in C: + * + * @code{.c} + * b3WorldId worldId = {0}; + * @endcode + * + * These are both considered null. + * + * @warning Do not use the internals of these ids. They are subject to change. Ids should be treated as opaque objects. + * @warning You should use ids to access objects in Box3D. Do not access files within the src folder. Such usage is unsupported. + * @{ + */ + +/// World id references a world instance. This should be treated as an opaque handle. +typedef struct b3WorldId +{ + uint16_t index1; + uint16_t generation; +} b3WorldId; + +/// Body id references a body instance. This should be treated as an opaque handle. +typedef struct b3BodyId +{ + int32_t index1; + uint16_t world0; + uint16_t generation; +} b3BodyId; + +/// Shape id references a shape instance. This should be treated as an opaque handle. +typedef struct b3ShapeId +{ + int32_t index1; + uint16_t world0; + uint16_t generation; +} b3ShapeId; + +/// Joint id references a joint instance. This should be treated as an opaque handle. +typedef struct b3JointId +{ + int32_t index1; + uint16_t world0; + uint16_t generation; +} b3JointId; + +/// Contact id references a contact instance. This should be treated as an opaque handle. +typedef struct b3ContactId +{ + int32_t index1; + uint16_t world0; + int16_t padding; + uint32_t generation; +} b3ContactId; + +// clang-format off +#ifdef __cplusplus + /// A null id. Works for any id type. + #define B3_NULL_ID {} + #define B3_ID_INLINE inline +#else + /// A null id. Works for any id type. + #define B3_NULL_ID { 0 } + + /// This macro bridges C and C++ inline functions. C++ has the one definition rule that C lacks. + #define B3_ID_INLINE static inline +#endif +// clang-format on + +/// Use these to make your identifiers null. +/// You may also use zero initialization to get null. +static const b3WorldId b3_nullWorldId = B3_NULL_ID; +static const b3BodyId b3_nullBodyId = B3_NULL_ID; +static const b3ShapeId b3_nullShapeId = B3_NULL_ID; +static const b3JointId b3_nullJointId = B3_NULL_ID; +static const b3ContactId b3_nullContactId = B3_NULL_ID; + +/// Macro to determine if any id is null. +#define B3_IS_NULL( id ) ( id.index1 == 0 ) + +/// Macro to determine if any id is non-null. +#define B3_IS_NON_NULL( id ) ( id.index1 != 0 ) + +/// Compare two ids for equality. Doesn't work for b3WorldId. Don't mix types. +#define B3_ID_EQUALS( id1, id2 ) ( id1.index1 == id2.index1 && id1.world0 == id2.world0 && id1.generation == id2.generation ) + +/// Store a world id into a uint32_t. +B3_ID_INLINE uint32_t b3StoreWorldId( b3WorldId id ) +{ + return ( (uint32_t)id.index1 << 16 ) | (uint32_t)id.generation; +} + +/// Load a uint32_t into a world id. +B3_ID_INLINE b3WorldId b3LoadWorldId( uint32_t x ) +{ + b3WorldId id = { (uint16_t)( x >> 16 ), (uint16_t)( x ) }; + return id; +} + +/// Store a body id into a uint64_t. +B3_ID_INLINE uint64_t b3StoreBodyId( b3BodyId id ) +{ + return ( (uint64_t)id.index1 << 32 ) | ( (uint64_t)id.world0 ) << 16 | (uint64_t)id.generation; +} + +/// Load a uint64_t into a body id. +B3_ID_INLINE b3BodyId b3LoadBodyId( uint64_t x ) +{ + b3BodyId id = { (int32_t)( x >> 32 ), (uint16_t)( x >> 16 ), (uint16_t)( x ) }; + return id; +} + +/// Store a shape id into a uint64_t. +B3_ID_INLINE uint64_t b3StoreShapeId( b3ShapeId id ) +{ + return ( (uint64_t)id.index1 << 32 ) | ( (uint64_t)id.world0 ) << 16 | (uint64_t)id.generation; +} + +/// Load a uint64_t into a shape id. +B3_ID_INLINE b3ShapeId b3LoadShapeId( uint64_t x ) +{ + b3ShapeId id = { (int32_t)( x >> 32 ), (uint16_t)( x >> 16 ), (uint16_t)( x ) }; + return id; +} + +/// Store a joint id into a uint64_t. +B3_ID_INLINE uint64_t b3StoreJointId( b3JointId id ) +{ + return ( (uint64_t)id.index1 << 32 ) | ( (uint64_t)id.world0 ) << 16 | (uint64_t)id.generation; +} + +/// Load a uint64_t into a joint id. +B3_ID_INLINE b3JointId b3LoadJointId( uint64_t x ) +{ + b3JointId id = { (int32_t)( x >> 32 ), (uint16_t)( x >> 16 ), (uint16_t)( x ) }; + return id; +} + +/// Store a contact id into three uint32 values +B3_ID_INLINE void b3StoreContactId( b3ContactId id, uint32_t values[3] ) +{ + values[0] = (uint32_t)id.index1; + values[1] = (uint32_t)id.world0; + values[2] = (uint32_t)id.generation; +} + +/// Load a contact id from three uint32 values. +B3_ID_INLINE b3ContactId b3LoadContactId( uint32_t values[3] ) +{ + b3ContactId id; + id.index1 = (int32_t)values[0]; + id.world0 = (uint16_t)values[1]; + id.padding = 0; + id.generation = (uint32_t)values[2]; + return id; +} + +/**@}*/ diff --git a/vendor/box3d/src/include/box3d/math_functions.h b/vendor/box3d/src/include/box3d/math_functions.h new file mode 100644 index 000000000..b1f65acec --- /dev/null +++ b/vendor/box3d/src/include/box3d/math_functions.h @@ -0,0 +1,1205 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +#pragma once + +#include "base.h" + +#include + +// for sqrtf and remainderf +#include +#include + +/** + * @defgroup math Math + * @brief Vector math types and functions + * @{ + */ + +/// https://en.wikipedia.org/wiki/Pi +#define B3_PI 3.14159265359f + +/// Convenience macro to convert from degrees to radians. +#define B3_DEG_TO_RAD 0.01745329251f + +/// Convenience macro to convert from radians to degrees. +#define B3_RAD_TO_DEG 57.2957795131f + +/// Minimum scale used for scaling collision meshes, etc. +#define B3_MIN_SCALE 0.01f + +/// A 2D vector. +typedef struct b3Vec2 +{ + float x; + float y; +} b3Vec2; + +/// A 3D vector. +typedef struct b3Vec3 +{ + float x; + float y; + float z; +} b3Vec3; + +/// Cosine and sine pair. +/// This uses a custom implementation designed for cross-platform determinism. +typedef struct b3CosSin +{ + /// cosine and sine + float cosine; + float sine; +} b3CosSin; + +/// A quaternion. +typedef struct b3Quat +{ + b3Vec3 v; + float s; +} b3Quat; + +/// A rigid transform. +typedef struct b3Transform +{ + b3Vec3 p; + b3Quat q; +} b3Transform; + +#if defined( BOX3D_DOUBLE_PRECISION ) + +/// A world position. Double precision in large world mode so coordinates stay accurate far +/// from the origin. +typedef struct b3Pos +{ + double x, y, z; +} b3Pos; + +/// A world transform with double precision translation and float quaternion rotation. Rotation +/// is frame local and never needs the extra range, the same split as Jolt's DMat44. +typedef struct b3WorldTransform +{ + b3Pos p; + b3Quat q; +} b3WorldTransform; + +#else + +/// In single precision mode these types are the same. +typedef b3Vec3 b3Pos; + +/// In single precision mode these types are the same. +typedef b3Transform b3WorldTransform; + +#endif + +/// A 3x3 matrix. +typedef struct b3Matrix3 +{ + b3Vec3 cx, cy, cz; +} b3Matrix3; + +/// Axis aligned bounding box. +typedef struct b3AABB +{ + b3Vec3 lowerBound; + b3Vec3 upperBound; +} b3AABB; + +/// A plane. +/// separation = dot(normal, point) - offset +typedef struct b3Plane +{ + b3Vec3 normal; + float offset; +} b3Plane; + +static const b3Vec3 b3Vec3_zero = { 0.0f, 0.0f, 0.0f }; +static const b3Vec3 b3Vec3_one = { 1.0f, 1.0f, 1.0f }; +static const b3Vec3 b3Vec3_axisX = { 1.0f, 0.0f, 0.0f }; +static const b3Vec3 b3Vec3_axisY = { 0.0f, 1.0f, 0.0f }; +static const b3Vec3 b3Vec3_axisZ = { 0.0f, 0.0f, 1.0f }; +static const b3Quat b3Quat_identity = { { 0.0f, 0.0f, 0.0f }, 1.0f }; +static const b3Transform b3Transform_identity = { { 0.0f, 0.0f, 0.0f }, { { 0.0f, 0.0f, 0.0f }, 1.0f } }; +static const b3Matrix3 b3Mat3_zero = { + { 0.0f, 0.0f, 0.0f }, + { 0.0f, 0.0f, 0.0f }, + { 0.0f, 0.0f, 0.0f }, +}; +static const b3Matrix3 b3Mat3_identity = { + { 1.0f, 0.0f, 0.0f }, + { 0.0f, 1.0f, 0.0f }, + { 0.0f, 0.0f, 1.0f }, +}; + +// Valid in both modes: 0.0f promotes to double, the identity rotation stays float +static const b3Pos b3Pos_zero = { 0.0f, 0.0f, 0.0f }; +static const b3WorldTransform b3WorldTransform_identity = { { 0.0f, 0.0f, 0.0f }, { { 0.0f, 0.0f, 0.0f }, 1.0f } }; + +/// @return the minimum of two integers. +B3_INLINE int b3MinInt( int a, int b ) +{ + return a < b ? a : b; +} + +/// @return the maximum of two integers. +B3_INLINE int b3MaxInt( int a, int b ) +{ + return a > b ? a : b; +} + +/// @return an integer clamped between a lower and upper bound. +B3_INLINE int b3ClampInt( int a, int lower, int upper ) +{ + return a < lower ? lower : ( upper < a ? upper : a ); +} + +/// @return the absolute value of a float. +B3_INLINE float b3AbsFloat( float a ) +{ + return a < 0 ? -a : a; +} + +/// @return the minimum of two floats. +B3_INLINE float b3MinFloat( float a, float b ) +{ + return a < b ? a : b; +} + +/// @return the maximum of two floats. +B3_INLINE float b3MaxFloat( float a, float b ) +{ + return a > b ? a : b; +} + +/// @return a float clamped between a lower and upper bound. +B3_INLINE float b3ClampFloat( float a, float lower, float upper ) +{ + return a < lower ? lower : ( upper < a ? upper : a ); +} + +/// Interpolate a scalar. +B3_INLINE float b3LerpFloat( float a, float b, float alpha ) +{ + return ( 1.0f - alpha ) * a + alpha * b; +} + +/// Compute an approximate arctangent in the range [-pi, pi] +/// This is hand coded for cross-platform determinism. The atan2f +/// function in the standard library is not cross-platform deterministic. +/// Accurate to around 0.0023 degrees. +B3_API float b3Atan2( float y, float x ); + +/// Compute the cosine and sine of an angle in radians. Implemented +/// for cross-platform determinism. +B3_API b3CosSin b3ComputeCosSin( float radians ); + +/// @deprecated +B3_INLINE float b3Sin( float radians ) +{ + b3CosSin cs = b3ComputeCosSin( radians ); + return cs.sine; +} + +/// @deprecated +B3_INLINE float b3Cos( float radians ) +{ + b3CosSin cs = b3ComputeCosSin( radians ); + return cs.cosine; +} + +/// Convert any angle into the range [-pi, pi]. +B3_INLINE float b3UnwindAngle( float radians ) +{ + // Assuming this is deterministic + return remainderf( radians, 2.0f * B3_PI ); +} + +/// Vector addition. +B3_INLINE b3Vec3 b3Add( b3Vec3 a, b3Vec3 b ) +{ + return B3_LITERAL( b3Vec3 ){ a.x + b.x, a.y + b.y, a.z + b.z }; +} + +/// Vector subtraction. +B3_INLINE b3Vec3 b3Sub( b3Vec3 a, b3Vec3 b ) +{ + return B3_LITERAL( b3Vec3 ){ a.x - b.x, a.y - b.y, a.z - b.z }; +} + +/// Vector component-wise multiplication. +B3_INLINE b3Vec3 b3Mul( b3Vec3 a, b3Vec3 b ) +{ + return B3_LITERAL( b3Vec3 ){ a.x * b.x, a.y * b.y, a.z * b.z }; +} + +/// Vector negation. +B3_INLINE b3Vec3 b3Neg( b3Vec3 a ) +{ + return B3_LITERAL( b3Vec3 ){ -a.x, -a.y, -a.z }; +} + +/// Vector dot product. +B3_INLINE float b3Dot( b3Vec3 a, b3Vec3 b ) +{ + return a.x * b.x + a.y * b.y + a.z * b.z; +} + +/// Vector length. +B3_INLINE float b3Length( b3Vec3 v ) +{ + return sqrtf( b3Dot( v, v ) ); +} + +/// Vector length squared. +B3_INLINE float b3LengthSquared( b3Vec3 a ) +{ + return a.x * a.x + a.y * a.y + a.z * a.z; +} + +/// Distance between two points. +B3_INLINE float b3Distance( b3Vec3 a, b3Vec3 b ) +{ + b3Vec3 dv = { b.x - a.x, b.y - a.y, b.z - a.z }; + return b3Length( dv ); +} + +/// Squared distance between two points. +B3_INLINE float b3DistanceSquared( b3Vec3 a, b3Vec3 b ) +{ + b3Vec3 dv = { b.x - a.x, b.y - a.y, b.z - a.z }; + return dv.x * dv.x + dv.y * dv.y + dv.z * dv.z; +} + +/// Normalize a vector. Returns a zero vector if the input vector is very small. +B3_INLINE b3Vec3 b3Normalize( b3Vec3 a ) +{ + float lengthSquared = a.x * a.x + a.y * a.y + a.z * a.z; + + if ( lengthSquared > 1000.0f * FLT_MIN ) + { + float s = 1.0f / sqrtf( lengthSquared ); + b3Vec3 u = { s * a.x, s * a.y, s * a.z }; + return u; + } + + return B3_LITERAL( b3Vec3 ){ 0.0f, 0.0f, 0.0f }; +} + +/// Normalize a vector and return the length. Returns a zero vector +/// if the input is very small. +B3_INLINE b3Vec3 b3GetLengthAndNormalize( float* length, b3Vec3 a ) +{ + *length = b3Length( a ); + if ( *length < FLT_EPSILON ) + { + return b3Vec3_zero; + } + + float invLength = 1.0f / *length; + b3Vec3 n = { invLength * a.x, invLength * a.y, invLength * a.z }; + return n; +} + +/// Get a unit vector that is perpendicular to the supplied vector. +B3_INLINE b3Vec3 b3Perp( b3Vec3 a ) +{ + // Suppose vector a has all equal components and is a unit vector: a = (s, s, s) + // Then 3*s*s = 1, s = sqrt(1/3) = 0.57735. This means that at least one component + // of a unit vector must be greater or equal to 0.57735. + b3Vec3 p; + if ( a.x < -0.5f || 0.5f < a.x ) + { + p = B3_LITERAL( b3Vec3 ){ a.y, -a.x, 0.0f }; + } + else + { + p = B3_LITERAL( b3Vec3 ){ 0.0f, a.z, -a.y }; + } + + return b3Normalize( p ); +} + +/// Is a vector normalized? In other words, does it have unit length? +B3_INLINE bool b3IsNormalized( b3Vec3 a ) +{ + float aa = b3Dot( a, a ); + return b3AbsFloat( 1.0f - aa ) < 100.0f * FLT_EPSILON; +} + +/// a + s * b +B3_INLINE b3Vec3 b3MulAdd( b3Vec3 a, float s, b3Vec3 b ) +{ + return B3_LITERAL( b3Vec3 ){ a.x + s * b.x, a.y + s * b.y, a.z + s * b.z }; +} + +/// a - s * b +B3_INLINE b3Vec3 b3MulSub( b3Vec3 a, float s, b3Vec3 b ) +{ + return B3_LITERAL( b3Vec3 ){ a.x - s * b.x, a.y - s * b.y, a.z - s * b.z }; +} + +/// s * a +B3_INLINE b3Vec3 b3MulSV( float s, b3Vec3 a ) +{ + return B3_LITERAL( b3Vec3 ){ s * a.x, s * a.y, s * a.z }; +} + +/// https://en.wikipedia.org/wiki/Cross_product +B3_INLINE b3Vec3 b3Cross( b3Vec3 a, b3Vec3 b ) +{ + b3Vec3 c; + c.x = a.y * b.z - a.z * b.y; + c.y = a.z * b.x - a.x * b.z; + c.z = a.x * b.y - a.y * b.x; + return c; +} + +/// Linearly interpolate between two vectors. +B3_INLINE b3Vec3 b3Lerp( b3Vec3 a, b3Vec3 b, float alpha ) +{ + B3_ASSERT( 0.0f <= alpha && alpha <= 1.0f ); + + b3Vec3 c = { + ( 1.0f - alpha ) * a.x + alpha * b.x, + ( 1.0f - alpha ) * a.y + alpha * b.y, + ( 1.0f - alpha ) * a.z + alpha * b.z, + }; + return c; +} + +/// Blend two vectors: s * a + t * b +B3_INLINE b3Vec3 b3Blend2( float s, b3Vec3 a, float t, b3Vec3 b ) +{ + b3Vec3 d = { + s * a.x + t * b.x, + s * a.y + t * b.y, + s * a.z + t * b.z, + }; + return d; +} + +/// Component-wise absolute value. +B3_INLINE b3Vec3 b3Abs( b3Vec3 a ) +{ + return B3_LITERAL( b3Vec3 ){ + b3AbsFloat( a.x ), + b3AbsFloat( a.y ), + b3AbsFloat( a.z ), + }; +} + +/// Component-wise -1 or 1 (1 if zero). +B3_INLINE b3Vec3 b3Sign( b3Vec3 a ) +{ + return B3_LITERAL( b3Vec3 ){ + a.x >= 0.0f ? 1.0f : -1.0f, + a.y >= 0.0f ? 1.0f : -1.0f, + a.z >= 0.0f ? 1.0f : -1.0f, + }; +} + +/// Component-wise minimum value. +B3_INLINE b3Vec3 b3Min( b3Vec3 a, b3Vec3 b ) +{ + return B3_LITERAL( b3Vec3 ){ + b3MinFloat( a.x, b.x ), + b3MinFloat( a.y, b.y ), + b3MinFloat( a.z, b.z ), + }; +} + +/// Component-wise maximum value. +B3_INLINE b3Vec3 b3Max( b3Vec3 a, b3Vec3 b ) +{ + return B3_LITERAL( b3Vec3 ){ + b3MaxFloat( a.x, b.x ), + b3MaxFloat( a.y, b.y ), + b3MaxFloat( a.z, b.z ), + }; +} + +/// Component-wise clamped value. +B3_INLINE b3Vec3 b3Clamp( b3Vec3 a, b3Vec3 lower, b3Vec3 upper ) +{ + b3Vec3 b; + b.x = b3ClampFloat( a.x, lower.x, upper.x ); + b.y = b3ClampFloat( a.y, lower.y, upper.y ); + b.z = b3ClampFloat( a.z, lower.z, upper.z ); + return b; +} + +/// Create a safe scaling value for scaling collision. This allows +/// negative scale, but keeps scale sufficiently far from zero. +B3_INLINE b3Vec3 b3SafeScale( b3Vec3 a ) +{ + b3Vec3 absScale = b3Abs( a ); + b3Vec3 minScale = { B3_MIN_SCALE, B3_MIN_SCALE, B3_MIN_SCALE }; + b3Vec3 safeScale = b3Mul( b3Sign( a ), b3Max( absScale, minScale ) ); + return safeScale; +} + +/// Does the supplied quaternion have unit length? +B3_INLINE bool b3IsNormalizedQuat( b3Quat q ) +{ + float qq = q.v.x * q.v.x + q.v.y * q.v.y + q.v.z * q.v.z + q.s * q.s; + return 1.0f - 20.0f * FLT_EPSILON < qq && qq < 1.0f + 20.0f * FLT_EPSILON; +} + +/// Rotate a vector. +B3_INLINE b3Vec3 b3RotateVector( b3Quat q, b3Vec3 v ) +{ + // v + 2 * cross(q.v, cross(q.v, v) + q.s * v) + // B3_ASSERT( b3IsNormalizedQuat( q ) ); + b3Vec3 t1 = b3Cross( q.v, v ); + b3Vec3 t2 = b3MulAdd( t1, q.s, v ); + b3Vec3 t3 = b3Cross( q.v, t2 ); + return b3MulAdd( v, 2.0f, t3 ); +} + +/// Inverse rotate a vector. +B3_INLINE b3Vec3 b3InvRotateVector( b3Quat q, b3Vec3 v ) +{ + // v + 2 * cross(q.v, cross(q.v, v) - q.s * v) + // B3_ASSERT( b3IsNormalizedQuat( q ) ); + b3Vec3 t1 = b3Cross( q.v, v ); + b3Vec3 t2 = b3MulSub( t1, q.s, v ); + b3Vec3 t3 = b3Cross( q.v, t2 ); + return b3MulAdd( v, 2.0f, t3 ); +} + +/// Compute dot product of two quaternions. Useful for polarity tests. +B3_INLINE float b3DotQuat( b3Quat a, b3Quat b ) +{ + return a.v.x * b.v.x + a.v.y * b.v.y + a.v.z * b.v.z + a.s * b.s; +} + +/// Multiply two quaternions. +B3_INLINE b3Quat b3MulQuat( b3Quat q1, b3Quat q2 ) +{ + b3Vec3 t1 = b3Cross( q1.v, q2.v ); + b3Vec3 t2 = b3MulAdd( t1, q1.s, q2.v ); + b3Vec3 t3 = b3MulAdd( t2, q2.s, q1.v ); + b3Quat q = { t3, q1.s * q2.s - b3Dot( q1.v, q2.v ) }; + return q; +} + +/// Compute a relative quaternion. +/// inv(q1) * q2 +B3_INLINE b3Quat b3InvMulQuat( b3Quat q1, b3Quat q2 ) +{ + b3Vec3 t1 = b3Cross( q2.v, q1.v ); + b3Vec3 t2 = b3MulAdd( t1, q1.s, q2.v ); + b3Vec3 t3 = b3MulSub( t2, q2.s, q1.v ); + b3Quat q = { t3, q1.s * q2.s + b3Dot( q1.v, q2.v ) }; + return q; +} + +/// Quaternion conjugate (cheap inverse). +B3_INLINE b3Quat b3Conjugate( b3Quat q ) +{ + return B3_LITERAL( b3Quat ){ { -q.v.x, -q.v.y, -q.v.z }, q.s }; +} + +/// Component-wise quaternion negation. +B3_INLINE b3Quat b3NegateQuat( b3Quat q ) +{ + return B3_LITERAL( b3Quat ){ { -q.v.x, -q.v.y, -q.v.z }, -q.s }; +} + +/// Normalize a quaternion. +B3_INLINE b3Quat b3NormalizeQuat( b3Quat q ) +{ + float lengthSq = b3DotQuat( q, q ); + if ( lengthSq > 1000.0f * FLT_MIN ) + { + float s = 1.0f / sqrtf( lengthSq ); + b3Quat qn = { { s * q.v.x, s * q.v.y, s * q.v.z }, s * q.s }; + return qn; + } + + return b3Quat_identity; +} + +/// Make a quaternion that is equivalent to rotating around an axis by a specified angle. +B3_INLINE b3Quat b3MakeQuatFromAxisAngle( b3Vec3 axis, float radians ) +{ + B3_ASSERT( b3IsNormalized( axis ) ); + b3CosSin cs = b3ComputeCosSin( 0.5f * radians ); + b3Quat q = { { cs.sine * axis.x, cs.sine * axis.y, cs.sine * axis.z }, cs.cosine }; + return q; +} + +/// Get the axis and angle from a quaternion. Assumes the quaternion is normalized. +B3_INLINE b3Vec3 b3GetAxisAngle( float* radians, b3Quat q ) +{ + float length = sqrtf( q.v.x * q.v.x + q.v.y * q.v.y + q.v.z * q.v.z ); + *radians = 2.0f * b3Atan2( length, q.s ); + if ( length > 0.0f ) + { + float invLength = 1.0f / length; + b3Vec3 axis = { invLength * q.v.x, invLength * q.v.y, invLength * q.v.z }; + return axis; + } + + return b3Vec3_zero; +} + +/// Get the angle for a quaternion in radians +B3_INLINE float b3GetQuatAngle( b3Quat q ) +{ + float length = sqrtf( q.v.x * q.v.x + q.v.y * q.v.y + q.v.z * q.v.z ); + return 2.0f * b3Atan2( length, q.s ); +} + +/// Extract a quaternion from a rotation matrix. +B3_API b3Quat b3MakeQuatFromMatrix( const b3Matrix3* m ); + +/// Find a quaternion that rotates one vector to another. +B3_API b3Quat b3ComputeQuatBetweenUnitVectors( b3Vec3 v1, b3Vec3 v2 ); + +/// Twist angle around the z-axis, used for twist limit and revolute angle limit +B3_INLINE float b3GetTwistAngle( b3Quat q ) +{ + // Account for polarity to keep the twist angle in range. + // This is simpler than asking the user to check polarity or unwinding. + float twist = q.s < 0.0f ? b3Atan2( -q.v.z, -q.s ) : b3Atan2( q.v.z, q.s ); + twist *= 2.0f; + B3_ASSERT( -B3_PI <= twist && twist <= B3_PI ); + return twist; +} + +/// Swing angle used for cone limit +B3_INLINE float b3GetSwingAngle( b3Quat q ) +{ + // Polarity should not matter because all terms are squared. + float x = sqrtf( q.v.z * q.v.z + q.s * q.s ); + float y = sqrtf( q.v.x * q.v.x + q.v.y * q.v.y ); + float swing = 2.0f * b3Atan2( y, x ); + B3_ASSERT( 0.0f <= swing && swing <= B3_PI ); + return swing; +} + +/// Linearly interpolate and normalize between two quaternions +B3_INLINE b3Quat b3NLerp( b3Quat q1, b3Quat q2, float alpha ) +{ + B3_VALIDATE( 0.0f <= alpha && alpha <= 1.0f ); + if ( b3DotQuat( q1, q2 ) < 0.0f ) + { + q1 = B3_LITERAL( b3Quat ){ { -q1.v.x, -q1.v.y, -q1.v.z }, -q1.s }; + } + + b3Quat q; + q.v = b3Lerp( q1.v, q2.v, alpha ); + q.s = ( 1.0f - alpha ) * q1.s + alpha * q2.s; + + return b3NormalizeQuat( q ); +} + +/// Multiply two transforms. If the result is applied to a point p local to frame B, +/// the transform would first convert p to a point local to frame A, then into a point +/// in the world frame. This is useful if frame B is a child of frame A. +B3_INLINE b3Transform b3MulTransforms( b3Transform a, b3Transform b ) +{ + b3Transform out; + out.p = b3Add( b3RotateVector( a.q, b.p ), a.p ); + out.q = b3MulQuat( a.q, b.q ); + return out; +} + +/// Creates a transform that converts a local point in frame B to a local point in frame A. +/// This is useful for transforming points between the local spaces of two frames that are +/// in world space. +B3_FORCE_INLINE b3Transform b3InvMulTransforms( b3Transform a, b3Transform b ) +{ + b3Transform out; + out.p = b3InvRotateVector( a.q, b3Sub( b.p, a.p ) ); + out.q = b3InvMulQuat( a.q, b.q ); + return out; +} + +/// Get the inverse of a transform. +B3_INLINE b3Transform b3InvertTransform( b3Transform t ) +{ + b3Transform out; + out.p = b3InvRotateVector( t.q, b3Neg( t.p ) ); + out.q = b3Conjugate( t.q ); + return out; +} + +/// Transform a point. +B3_INLINE b3Vec3 b3TransformPoint( b3Transform t, b3Vec3 v ) +{ + b3Vec3 rv = b3RotateVector( t.q, v ); + return b3Add( rv, t.p ); +} + +/// Inverse transform a point. +B3_INLINE b3Vec3 b3InvTransformPoint( b3Transform t, b3Vec3 v ) +{ + return b3InvRotateVector( t.q, b3Sub( v, t.p ) ); +} + +// World position boundary. These cross between the double precision world space at the public +// boundary and the float interior. One set of bodies serves both modes: the typedefs collapse +// the types in float mode and the explicit float casts become no-ops. + +/// Convert a vector to a world position. +B3_INLINE b3Pos b3ToPos( b3Vec3 v ) +{ + return B3_LITERAL( b3Pos ){ v.x, v.y, v.z }; +} + +/// Lossy conversion of a world position to a float vector. +B3_INLINE b3Vec3 b3ToVec3( b3Pos p ) +{ + return B3_LITERAL( b3Vec3 ){ (float)p.x, (float)p.y, (float)p.z }; +} + +/// Narrow a world coordinate to float, rounding toward negative infinity. Use with +/// b3RoundUpFloat to build a conservative float box that always contains the double bounds, +/// where plain rounding far from the origin could clip. nextafterf is an exact IEEE operation, +/// so this is cross-platform deterministic. With large world mode off this is a plain conversion. +B3_INLINE float b3RoundDownFloat( double x ) +{ +#if defined( BOX3D_DOUBLE_PRECISION ) + float f = (float)x; + return (double)f > x ? nextafterf( f, -FLT_MAX ) : f; +#else + return (float)x; +#endif +} + +/// Narrow a world coordinate to float, rounding toward positive infinity. +B3_INLINE float b3RoundUpFloat( double x ) +{ +#if defined( BOX3D_DOUBLE_PRECISION ) + float f = (float)x; + return (double)f < x ? nextafterf( f, FLT_MAX ) : f; +#else + return (float)x; +#endif +} + +/// a - b, demoted to float. The primary precision boundary operation. +B3_INLINE b3Vec3 b3SubPos( b3Pos a, b3Pos b ) +{ + return B3_LITERAL( b3Vec3 ){ (float)( a.x - b.x ), (float)( a.y - b.y ), (float)( a.z - b.z ) }; +} + +/// p + d +B3_INLINE b3Pos b3OffsetPos( b3Pos p, b3Vec3 d ) +{ + return B3_LITERAL( b3Pos ){ p.x + d.x, p.y + d.y, p.z + d.z }; +} + +/// World position interpolation for sweeps and sampling. +B3_INLINE b3Pos b3LerpPosition( b3Pos a, b3Pos b, float t ) +{ + return B3_LITERAL( b3Pos ){ + ( 1.0f - t ) * a.x + t * b.x, + ( 1.0f - t ) * a.y + t * b.y, + ( 1.0f - t ) * a.z + t * b.z, + }; +} + +/// Transform a local point to a world position. Rotation in float, translation in double. +B3_INLINE b3Pos b3TransformWorldPoint( b3WorldTransform t, b3Vec3 p ) +{ + b3Vec3 r = b3RotateVector( t.q, p ); + return B3_LITERAL( b3Pos ){ t.p.x + r.x, t.p.y + r.y, t.p.z + r.z }; +} + +/// Transform a world position to a local point. One double subtraction, then float. +B3_INLINE b3Vec3 b3InvTransformWorldPoint( b3WorldTransform t, b3Pos p ) +{ + b3Vec3 d = { (float)( p.x - t.p.x ), (float)( p.y - t.p.y ), (float)( p.z - t.p.z ) }; + return b3InvRotateVector( t.q, d ); +} + +/// Relative transform of frame B in frame A. The narrow phase boundary. +B3_INLINE b3Transform b3InvMulWorldTransforms( b3WorldTransform A, b3WorldTransform B ) +{ + b3Transform C; + C.q = b3InvMulQuat( A.q, B.q ); + b3Vec3 d = { (float)( B.p.x - A.p.x ), (float)( B.p.y - A.p.y ), (float)( B.p.z - A.p.z ) }; + C.p = b3InvRotateVector( A.q, d ); + return C; +} + +/// Compose a world transform with a local transform. +B3_INLINE b3WorldTransform b3MulWorldTransforms( b3WorldTransform A, b3Transform B ) +{ + b3WorldTransform C; + C.q = b3MulQuat( A.q, B.q ); + b3Vec3 r = b3RotateVector( A.q, B.p ); + C.p = B3_LITERAL( b3Pos ){ A.p.x + r.x, A.p.y + r.y, A.p.z + r.z }; + return C; +} + +/// Shift a world transform into the frame of a base position. +B3_INLINE b3Transform b3ToRelativeTransform( b3WorldTransform t, b3Pos base ) +{ + b3Transform r; + r.q = t.q; + r.p = B3_LITERAL( b3Vec3 ){ (float)( t.p.x - base.x ), (float)( t.p.y - base.y ), (float)( t.p.z - base.z ) }; + return r; +} + +/// Promote a float transform to a world transform. Lossless. +B3_INLINE b3WorldTransform b3MakeWorldTransform( b3Transform t ) +{ + b3WorldTransform w; + w.p = b3ToPos( t.p ); + w.q = t.q; + return w; +} + +/// Translate a local AABB by a world origin, rounding outward so the float box always contains +/// the double box. Far from the origin a plain conversion could clip a shape out of its own box. +/// In float mode the origin is float and the rounding is a no-op. +B3_INLINE b3AABB b3OffsetAABB( b3AABB localBox, b3Pos origin ) +{ + b3AABB out; + out.lowerBound.x = b3RoundDownFloat( origin.x + localBox.lowerBound.x ); + out.lowerBound.y = b3RoundDownFloat( origin.y + localBox.lowerBound.y ); + out.lowerBound.z = b3RoundDownFloat( origin.z + localBox.lowerBound.z ); + out.upperBound.x = b3RoundUpFloat( origin.x + localBox.upperBound.x ); + out.upperBound.y = b3RoundUpFloat( origin.y + localBox.upperBound.y ); + out.upperBound.z = b3RoundUpFloat( origin.z + localBox.upperBound.z ); + return out; +} + +/// Compute the determinant of a 3-by-3 matrix. +B3_INLINE float b3Det( b3Matrix3 m ) +{ + return b3Dot( m.cx, b3Cross( m.cy, m.cz ) ); +} + +/// Multiply a matrix times a column vector. +B3_INLINE b3Vec3 b3MulMV( b3Matrix3 m, b3Vec3 a ) +{ + b3Vec3 b = { + m.cx.x * a.x + m.cy.x * a.y + m.cz.x * a.z, + m.cx.y * a.x + m.cy.y * a.y + m.cz.y * a.z, + m.cx.z * a.x + m.cy.z * a.y + m.cz.z * a.z, + }; + return b; +} + +/// Negate a matrix. +B3_INLINE b3Matrix3 b3NegateMat3( b3Matrix3 a ) +{ + return B3_LITERAL( b3Matrix3 ){ + { -a.cx.x, -a.cx.y, -a.cx.z }, + { -a.cy.x, -a.cy.y, -a.cy.z }, + { -a.cz.x, -a.cz.y, -a.cz.z }, + }; +} + +/// Matrix addition. +/// @return a + b +B3_INLINE b3Matrix3 b3AddMM( b3Matrix3 a, b3Matrix3 b ) +{ + return B3_LITERAL( b3Matrix3 ){ + { a.cx.x + b.cx.x, a.cx.y + b.cx.y, a.cx.z + b.cx.z }, + { a.cy.x + b.cy.x, a.cy.y + b.cy.y, a.cy.z + b.cy.z }, + { a.cz.x + b.cz.x, a.cz.y + b.cz.y, a.cz.z + b.cz.z }, + }; +} + +/// Matrix subtraction. +/// @return a - b +B3_INLINE b3Matrix3 b3SubMM( b3Matrix3 a, b3Matrix3 b ) +{ + return B3_LITERAL( b3Matrix3 ){ + { a.cx.x - b.cx.x, a.cx.y - b.cx.y, a.cx.z - b.cx.z }, + { a.cy.x - b.cy.x, a.cy.y - b.cy.y, a.cy.z - b.cy.z }, + { a.cz.x - b.cz.x, a.cz.y - b.cz.y, a.cz.z - b.cz.z }, + }; +} + +/// Multiply a matrix by a scalar, component-wise. +B3_INLINE b3Matrix3 b3MulSM( float s, b3Matrix3 a ) +{ + return B3_LITERAL( b3Matrix3 ){ + { s * a.cx.x, s * a.cx.y, s * a.cx.z }, + { s * a.cy.x, s * a.cy.y, s * a.cy.z }, + { s * a.cz.x, s * a.cz.y, s * a.cz.z }, + }; +} + +/// Matrix multiplication. +/// @return a * b +B3_INLINE b3Matrix3 b3MulMM( b3Matrix3 a, b3Matrix3 b ) +{ + b3Matrix3 out; + out.cx = b3MulMV( a, b.cx ); + out.cy = b3MulMV( a, b.cy ); + out.cz = b3MulMV( a, b.cz ); + return out; +} + +/// Matrix transpose. +B3_INLINE b3Matrix3 b3Transpose( b3Matrix3 m ) +{ + b3Matrix3 out; + out.cx = B3_LITERAL( b3Vec3 ){ m.cx.x, m.cy.x, m.cz.x }; + out.cy = B3_LITERAL( b3Vec3 ){ m.cx.y, m.cy.y, m.cz.y }; + out.cz = B3_LITERAL( b3Vec3 ){ m.cx.z, m.cy.z, m.cz.z }; + + return out; +} + +/// General matrix inverse. +B3_INLINE b3Matrix3 b3InvertMatrix( b3Matrix3 m ) +{ + float det = b3Det( m ); + if ( b3AbsFloat( det ) > 1000.0f * FLT_MIN ) + { + float invDet = 1.0f / det; + b3Matrix3 out; + out.cx = b3MulSV( invDet, b3Cross( m.cy, m.cz ) ); + out.cy = b3MulSV( invDet, b3Cross( m.cz, m.cx ) ); + out.cz = b3MulSV( invDet, b3Cross( m.cx, m.cy ) ); + + return b3Transpose( out ); + } + + return b3Mat3_zero; +} + +/// Solve a matrix equation. +/// @return inv(m) * a +B3_INLINE b3Vec3 b3Solve3( b3Matrix3 m, b3Vec3 a ) +{ + float det = b3Det( m ); + if ( b3AbsFloat( det ) > 1000.0f * FLT_MIN ) + { + float invDet = 1.0f / det; + b3Matrix3 s; + s.cx = b3Cross( m.cy, m.cz ); + s.cy = b3Cross( m.cz, m.cx ); + s.cz = b3Cross( m.cx, m.cy ); + + b3Vec3 b = { + invDet * b3Dot( s.cx, a ), + invDet * b3Dot( s.cy, a ), + invDet * b3Dot( s.cz, a ), + }; + + return b; + } + + return b3Vec3_zero; +} + +/// Invert a matrix. +B3_INLINE b3Matrix3 b3InvertT( b3Matrix3 m ) +{ + float det = b3Det( m ); + if ( b3AbsFloat( det ) > 1000.0f * FLT_MIN ) + { + float invDet = 1.0f / det; + b3Matrix3 out; + out.cx = b3MulSV( invDet, b3Cross( m.cy, m.cz ) ); + out.cy = b3MulSV( invDet, b3Cross( m.cz, m.cx ) ); + out.cz = b3MulSV( invDet, b3Cross( m.cx, m.cy ) ); + return out; + } + + return b3Mat3_zero; +} + +/// Get the component-wise absolute value of a matrix. +B3_INLINE b3Matrix3 b3AbsMatrix3( b3Matrix3 m ) +{ + b3Matrix3 out; + out.cx = b3Abs( m.cx ); + out.cy = b3Abs( m.cy ); + out.cz = b3Abs( m.cz ); + + return out; +} + +/// Make a matrix from a quaternion. This is useful if you need to +/// rotate many vectors. +/// The force inline improves the performance of b3ShapeDistance. +B3_FORCE_INLINE b3Matrix3 b3MakeMatrixFromQuat( b3Quat q ) +{ + float xx = q.v.x * q.v.x; + float yy = q.v.y * q.v.y; + float zz = q.v.z * q.v.z; + float xy = q.v.x * q.v.y; + float xz = q.v.x * q.v.z; + float xw = q.v.x * q.s; + float yz = q.v.y * q.v.z; + float yw = q.v.y * q.s; + float zw = q.v.z * q.s; + + return B3_LITERAL( b3Matrix3 ){ + { 1.0f - 2.0f * ( yy + zz ), 2.0f * ( xy + zw ), 2.0f * ( xz - yw ) }, + { 2.0f * ( xy - zw ), 1.0f - 2.0f * ( xx + zz ), 2.0f * ( yz + xw ) }, + { 2.0f * ( xz + yw ), 2.0f * ( yz - xw ), 1.0f - 2.0f * ( xx + yy ) }, + }; +} + +/// Get the inertia tensor of an offset point. +/// https://en.wikipedia.org/wiki/Parallel_axis_theorem +B3_API b3Matrix3 b3Steiner( float mass, b3Vec3 origin ); + +/// Get the AABB of a point cloud. +B3_INLINE b3AABB b3MakeAABB( const b3Vec3* points, int count, float radius ) +{ + B3_ASSERT( count > 0 ); + b3AABB a = { points[0], points[0] }; + for ( int i = 1; i < count; ++i ) + { + a.lowerBound = b3Min( a.lowerBound, points[i] ); + a.upperBound = b3Max( a.upperBound, points[i] ); + } + + b3Vec3 r = { radius, radius, radius }; + a.lowerBound = b3Sub( a.lowerBound, r ); + a.upperBound = b3Add( a.upperBound, r ); + + return a; +} + +/// Does a fully contain b? +B3_INLINE bool b3AABB_Contains( b3AABB a, b3AABB b ) +{ + if ( a.lowerBound.x > b.lowerBound.x || b.upperBound.x > a.upperBound.x ) + return false; + if ( a.lowerBound.y > b.lowerBound.y || b.upperBound.y > a.upperBound.y ) + return false; + if ( a.lowerBound.z > b.lowerBound.z || b.upperBound.z > a.upperBound.z ) + return false; + + return true; +} + +/// Get the surface area of an axis-aligned bounding box. +B3_INLINE float b3AABB_Area( b3AABB a ) +{ + b3Vec3 delta = b3Sub( a.upperBound, a.lowerBound ); + return 2.0f * ( delta.x * delta.y + delta.y * delta.z + delta.z * delta.x ); +} + +/// Get the center of an axis-aligned bounding box. +B3_INLINE b3Vec3 b3AABB_Center( b3AABB a ) +{ + return b3MulSV( 0.5f, b3Add( a.upperBound, a.lowerBound ) ); +} + +/// Get the extents (half-widths) of an axis-aligned bounding box. +B3_INLINE b3Vec3 b3AABB_Extents( b3AABB a ) +{ + return b3MulSV( 0.5f, b3Sub( a.upperBound, a.lowerBound ) ); +} + +/// Get the union of two axis-aligned bounding boxes. +B3_INLINE b3AABB b3AABB_Union( b3AABB a, b3AABB b ) +{ + b3AABB out; + out.lowerBound = b3Min( a.lowerBound, b.lowerBound ); + out.upperBound = b3Max( a.upperBound, b.upperBound ); + return out; +} + +/// Add uniform padding to an axis-aligned bounding box. +B3_INLINE b3AABB b3AABB_Inflate( b3AABB a, float extension ) +{ + b3Vec3 radius = { extension, extension, extension }; + + b3AABB out; + out.lowerBound = b3Sub( a.lowerBound, radius ); + out.upperBound = b3Add( a.upperBound, radius ); + return out; +} + +/// Do two axis-aligned boxes overlap? +B3_INLINE bool b3AABB_Overlaps( b3AABB a, b3AABB b ) +{ + // No intersection if separated along one axis + if ( a.upperBound.x < b.lowerBound.x || a.lowerBound.x > b.upperBound.x ) + return false; + if ( a.upperBound.y < b.lowerBound.y || a.lowerBound.y > b.upperBound.y ) + return false; + if ( a.upperBound.z < b.lowerBound.z || a.lowerBound.z > b.upperBound.z ) + return false; + + // Overlapping on all axis means bounds are intersecting + return true; +} + +/// Transform an axis-aligned bounding box. This can create a larger box +/// than if you recomputed the AABB of the original shape with the transform +/// applied. +B3_INLINE b3AABB b3AABB_Transform( b3Transform transform, b3AABB a ) +{ + b3Vec3 center = b3TransformPoint( transform, b3AABB_Center( a ) ); + b3Matrix3 m = b3MakeMatrixFromQuat( transform.q ); + b3Vec3 extent = b3MulMV( b3AbsMatrix3( m ), b3AABB_Extents( a ) ); + b3AABB out = { b3Sub( center, extent ), b3Add( center, extent ) }; + return out; +} + +/// Get the closest point on an axis-aligned bounding box. +B3_INLINE b3Vec3 b3ClosestPointToAABB( b3Vec3 point, b3AABB a ) +{ + return b3Clamp( point, a.lowerBound, a.upperBound ); +} + +/// The closest points between to segments or infinite lines. +typedef struct b3SegmentDistanceResult +{ + b3Vec3 point1; + float fraction1; + b3Vec3 point2; + float fraction2; +} b3SegmentDistanceResult; + +/// Compute the closest point on the segment a-b to the target q. +B3_API b3Vec3 b3PointToSegmentDistance( b3Vec3 a, b3Vec3 b, b3Vec3 q ); + +/// Compute the closest points on two infinite lines. +B3_API b3SegmentDistanceResult b3LineDistance( b3Vec3 p1, b3Vec3 d1, b3Vec3 p2, b3Vec3 d2 ); + +/// Compute the closest points on two line segments. +B3_API b3SegmentDistanceResult b3SegmentDistance( b3Vec3 p1, b3Vec3 q1, b3Vec3 p2, b3Vec3 q2 ); + +/// Is this a valid number? Not NaN or infinity. +B3_API bool b3IsValidFloat( float a ); + +/// Is this a valid vector? Not NaN or infinity. +B3_API bool b3IsValidVec3( b3Vec3 a ); + +/// Is this a valid quaternion? Not NaN or infinity. Is normalized. +B3_API bool b3IsValidQuat( b3Quat q ); + +/// Is this a valid transform? Not NaN or infinity. Is normalized. +B3_API bool b3IsValidTransform( b3Transform a ); + +/// Is this a valid matrix? Not NaN or infinity. +B3_API bool b3IsValidMatrix3( b3Matrix3 a ); + +/// Is this a valid bounding box? Not Nan or infinity. Upper bound greater than or equal to lower bound. +B3_API bool b3IsValidAABB( b3AABB a ); + +/// Is this AABB reasonably close to the origin? See B3_HUGE. +B3_API bool b3IsBoundedAABB( b3AABB a ); + +/// Is this AABB valid and reasonable? +B3_API bool b3IsSaneAABB( b3AABB a ); + +/// Is this a valid plane? Normal is a unit vector. Not Nan or infinity. +B3_API bool b3IsValidPlane( b3Plane a ); + +/// Is this a valid world position? Not NaN or infinity. +B3_API bool b3IsValidPosition( b3Pos p ); + +/// Is this a valid world transform? Not NaN or infinity. Rotation is normalized. +B3_API bool b3IsValidWorldTransform( b3WorldTransform t ); + +/**@}*/ // math + +/** + * @defgroup math_cpp C++ Math + * @brief Math operator overloads for C++ + * Some of the simpler ones are expanded to improve debug performance. + * See math_functions.h for details. + * @{ + */ + +#ifdef __cplusplus + +/// Vector addition. +B3_FORCE_INLINE b3Vec3& operator+=( b3Vec3& a, b3Vec3 b ) +{ + a.x += b.x; + a.y += b.y; + a.z += b.z; + return a; +} + +/// Vector subtraction. +B3_FORCE_INLINE b3Vec3& operator-=( b3Vec3& a, b3Vec3 b ) +{ + a.x -= b.x; + a.y -= b.y; + a.z -= b.z; + return a; +} + +/// Vector scaling. +B3_FORCE_INLINE b3Vec3& operator*=( b3Vec3& a, float s ) +{ + a.x *= s; + a.y *= s; + a.z *= s; + return a; +} + +/// Vector negation. +B3_FORCE_INLINE b3Vec3 operator-( b3Vec3 a ) +{ + return { -a.x, -a.y, -a.z }; +} + +/// Vector scaling. +B3_FORCE_INLINE b3Vec3 operator*( float s, b3Vec3 a ) +{ + return { s * a.x, s * a.y, s * a.z }; +} + +/// Vector scaling. +B3_FORCE_INLINE b3Vec3 operator*( b3Vec3 a, float s ) +{ + return { s * a.x, s * a.y, s * a.z }; +} + +/// Component-wise vector multiplication. +B3_FORCE_INLINE b3Vec3 operator*( b3Vec3 a, b3Vec3 b ) +{ + return { a.x * b.x, a.y * b.y, a.z * b.z }; +} + +/// Vector addition. +B3_FORCE_INLINE b3Vec3 operator+( b3Vec3 a, b3Vec3 b ) +{ + return { a.x + b.x, a.y + b.y, a.z + b.z }; +} + +/// Vector subtraction. +B3_FORCE_INLINE b3Vec3 operator-( b3Vec3 a, b3Vec3 b ) +{ + return { a.x - b.x, a.y - b.y, a.z - b.z }; +} + +#if defined( BOX3D_DOUBLE_PRECISION ) + +/// Offset a world position by a vector. +B3_FORCE_INLINE b3Pos operator+( b3Pos a, b3Vec3 b ) +{ + return { a.x + b.x, a.y + b.y, a.z + b.z }; +} + +/// Offset a world position by a vector. +B3_FORCE_INLINE b3Pos operator-( b3Pos a, b3Vec3 b ) +{ + return { a.x - b.x, a.y - b.y, a.z - b.z }; +} + +/// Delta between two world positions, demoted to float. +B3_FORCE_INLINE b3Vec3 operator-( b3Pos a, b3Pos b ) +{ + return { (float)( a.x - b.x ), (float)( a.y - b.y ), (float)( a.z - b.z ) }; +} + +#endif + +#endif + +/**@}*/ // math_cpp diff --git a/vendor/box3d/src/include/box3d/types.h b/vendor/box3d/src/include/box3d/types.h new file mode 100644 index 000000000..40e685357 --- /dev/null +++ b/vendor/box3d/src/include/box3d/types.h @@ -0,0 +1,3048 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +#pragma once + +#include "base.h" +#include "constants.h" +#include "id.h" +#include "math_functions.h" + +#include + +#define B3_DEFAULT_CATEGORY_BITS UINT64_MAX +#define B3_DEFAULT_MASK_BITS UINT64_MAX + +/// Task interface +/// This is the prototype for a Box3D task. Your task system is expected to run this callback on a worker thread, +/// exactly once per enqueue, passing back the same taskContext pointer supplied to b3EnqueueTaskCallback. +/// @ingroup world +typedef void b3TaskCallback( void* taskContext ); + +/// These functions can be provided to Box3D to invoke a task system. +/// Returns a pointer to the user's task object. May be nullptr. A nullptr indicates to Box3D that the work was executed +/// serially within the callback and there is no need to call b3FinishTaskCallback. Otherwise the returned +/// value must be non-null will be passed to b3FinishTaskCallback as the userTask. +/// @param task the Box3D task to be called by the scheduler +/// @param taskContext the Box3D context object that the scheduler must pass to the task +/// @param userContext the scheduler context object that is opaque to Box3D +/// @param taskName the Box3D task name that the scheduler can use for diagnostics +/// @ingroup world +typedef void* b3EnqueueTaskCallback( b3TaskCallback* task, void* taskContext, void* userContext, const char* taskName ); + +/// Finishes a user task object that wraps a Box3D task. This must block until the task has completed. +/// The step blocks here on the tasks it spawned, so b3World_Step holds its stack across every +/// fork/join. Drive it from a thread you can dedicate to the step, or from a fiber this callback can +/// park to free the underlying thread. In a job system that cannot park a job's stack, do not call +/// b3World_Step from inside a job: a job that blocks on its own sub-jobs without yielding its thread +/// can deadlock. The in-tree scheduler instead runs other pending tasks on the waiting thread. +/// @ingroup world +typedef void b3FinishTaskCallback( void* userTask, void* userContext ); + +typedef struct b3DebugShape b3DebugShape; + +/// The user needs to be able to create debug draw shapes for multi-pass rendering to work efficiently. +/// These user shapes are created and destroyed via callback so they can be bound to shape lifetime and scaling updates. +/// @ingroup debug_draw +typedef void* b3CreateDebugShapeCallback( const b3DebugShape* debugShape, void* userContext ); +typedef void b3DestroyDebugShapeCallback( void* userShape, void* userContext ); + +/// Optional friction mixing callback. This intentionally provides no context objects because this is called +/// from a worker thread. +/// @warning This function should not attempt to modify Box3D state or user application state. +/// @ingroup world +typedef float b3FrictionCallback( float frictionA, uint64_t userMaterialIdA, float frictionB, uint64_t userMaterialIdB ); + +/// Optional restitution mixing callback. This intentionally provides no context objects because this is called +/// from a worker thread. +/// @warning This function should not attempt to modify Box3D state or user application state. +/// @ingroup world +typedef float b3RestitutionCallback( float restitutionA, uint64_t userMaterialIdA, float restitutionB, uint64_t userMaterialIdB ); + +/// Prototype for a contact filter callback. +/// This is called when a contact pair is considered for collision. This allows you to +/// perform custom logic to prevent collision between shapes. This is only called if +/// one of the two shapes has custom filtering enabled. @see b3ShapeDef. +/// Notes: +/// - this function must be thread-safe +/// - this is only called if one of the two shapes has enabled custom filtering +/// - this is called only for awake dynamic bodies +/// Return false if you want to disable the collision +/// @warning Do not attempt to modify the world inside this callback +/// @ingroup world +typedef bool b3CustomFilterFcn( b3ShapeId shapeIdA, b3ShapeId shapeIdB, void* context ); + +/// Prototype for a pre-solve callback. +/// This is called after a contact is updated. This allows you to inspect a +/// collision before it goes to the solver. +/// Notes: +/// - this function must be thread-safe +/// - this is only called if the shape has enabled pre-solve events +/// - this may be called for awake dynamic bodies and sensors +/// - this is not called for sensors +/// Return false if you want to disable the contact this step +/// This has limited information because it is used during CCD which does not have the +/// full contact manifold. +/// @warning Do not attempt to modify the world inside this callback +/// @ingroup world +typedef bool b3PreSolveFcn( b3ShapeId shapeIdA, b3ShapeId shapeIdB, b3Pos point, b3Vec3 normal, void* context ); + +/// Prototype callback for overlap queries. +/// Called for each shape found in the query. +/// @see b3World_OverlapAABB +/// @return false to terminate the query. +/// @ingroup world +typedef bool b3OverlapResultFcn( b3ShapeId shapeId, void* context ); + +/// Prototype callback for ray casts. +/// Called for each shape found in the query. You control how the ray cast +/// proceeds by returning a float: +/// return -1: ignore this shape and continue +/// return 0: terminate the ray cast +/// return fraction: clip the ray to this point +/// return 1: don't clip the ray and continue +/// @param shapeId the shape hit by the ray +/// @param point the point of initial intersection +/// @param normal the normal vector at the point of intersection +/// @param fraction the fraction along the ray at the point of intersection +/// @param userMaterialId the shape or triangle surface type +/// @param triangleIndex the triangle index for mesh or height field shapes or -1 for other shape types +/// @param childIndex the child shape index for compound shapes +/// @param context the user context +/// @return -1 to filter, 0 to terminate, fraction to clip the ray for closest hit, 1 to continue +/// @see b3World_CastRay +/// @ingroup world +typedef float b3CastResultFcn( b3ShapeId shapeId, b3Pos point, b3Vec3 normal, float fraction, uint64_t userMaterialId, + int triangleIndex, int childIndex, void* context ); + +/// Optional world capacities that can be use to avoid run-time allocations +/// @ingroup world +typedef struct b3Capacity +{ + /// Number of expected static shapes. + int staticShapeCount; + + /// Number of expected dynamic and kinematic shapes. + int dynamicShapeCount; + + /// Number of expected static bodies. + int staticBodyCount; + + /// Number of expected dynamic and kinematic bodies. + int dynamicBodyCount; + + /// Number of expected contacts. + int contactCount; +} b3Capacity; + +/// World definition used to create a simulation world. Must be initialized using b3DefaultWorldDef. +/// @ingroup world +typedef struct b3WorldDef +{ + /// Gravity vector. Box3D has no up-vector defined. + b3Vec3 gravity; + + /// Restitution speed threshold, usually in m/s. Collisions above this + /// speed have restitution applied (will bounce). + float restitutionThreshold; + + /// Hit event speed threshold, usually in m/s. Collisions above this + /// speed can generate hit events if the shape also enables hit events. + float hitEventThreshold; + + /// Contact stiffness. Cycles per second. Increasing this increases the speed of overlap recovery, but can introduce jitter. + float contactHertz; + + /// Contact bounciness. Non-dimensional. You can speed up overlap recovery by decreasing this with + /// the trade-off that overlap resolution becomes more energetic. + float contactDampingRatio; + + /// This parameter controls how fast overlap is resolved and usually has units of meters per second. This only + /// puts a cap on the resolution speed. The resolution speed is increased by increasing the hertz and/or + /// decreasing the damping ratio. + float contactSpeed; + + /// Maximum linear speed. Usually meters per second. + float maximumLinearSpeed; + + /// Optional mixing callback for friction. The default uses sqrt(frictionA * frictionB). + b3FrictionCallback* frictionCallback; + + /// Optional mixing callback for restitution. The default uses max(restitutionA, restitutionB). + b3RestitutionCallback* restitutionCallback; + + /// Can bodies go to sleep to improve performance + bool enableSleep; + + /// Enable continuous collision + bool enableContinuous; + + /// Number of workers to use with the provided task system. Box3D performs best when using only + /// performance cores and accessing a single L2 cache. Efficiency cores and hyper-threading provide + /// little benefit and may even harm performance. + /// This is clamped to the range [1, B3_MAX_WORKERS]. Using a value above 1 will turn on multithreading. + /// If task callbacks are provided then Box3D will use the user provided task system. Otherwise Box3D + /// will create threads and use an internal scheduler. + uint32_t workerCount; + + /// function to spawn task + b3EnqueueTaskCallback* enqueueTask; + + /// function to finish a task + b3FinishTaskCallback* finishTask; + + /// User context that is provided to enqueueTask and finishTask + void* userTaskContext; + + /// User data associated with a world + void* userData; + + /// Used to create debug draw shapes. This is called when a shape is + /// first drawn using b3DebugDraw. + b3CreateDebugShapeCallback* createDebugShape; + + /// Used to destroy debug draw shapes. This is called when a shape is modified or destroyed. + b3DestroyDebugShapeCallback* destroyDebugShape; + + /// This is passed to the debug shape callbacks to provide a user context. + void* userDebugShapeContext; + + /// Optional initial capacities + b3Capacity capacity; + + /// Used internally to detect a valid definition. DO NOT SET. + int internalValue; +} b3WorldDef; + +/// Use this to initialize your world definition +/// @ingroup world +B3_API b3WorldDef b3DefaultWorldDef( void ); + +/// The body simulation type. +/// Each body is one of these three types. The type determines how the body behaves in the simulation. +/// @ingroup body +typedef enum b3BodyType +{ + /// zero mass, zero velocity, may be manually moved + b3_staticBody = 0, + + /// zero mass, velocity set by user, moved by solver + b3_kinematicBody = 1, + + /// positive mass, velocity determined by forces, moved by solver + b3_dynamicBody = 2, + + /// number of body types + b3_bodyTypeCount, +} b3BodyType; + +/// Motion locks to restrict the body movement +/// @ingroup body +typedef struct b3MotionLocks +{ + /// Prevent translation along the x-axis + bool linearX; + + /// Prevent translation along the y-axis + bool linearY; + + /// Prevent translation along the z-axis + bool linearZ; + + /// Prevent rotation around the x-axis + bool angularX; + + /// Prevent rotation around the y-axis + bool angularY; + + /// Prevent rotation around the z-axis + bool angularZ; +} b3MotionLocks; + +/// A body definition holds all the data needed to construct a rigid body. +/// You can safely re-use body definitions. Shapes are added to a body after construction. +/// Body definitions are temporary objects used to bundle creation parameters. +/// Must be initialized using b3DefaultBodyDef(). +/// @ingroup body +typedef struct b3BodyDef +{ + /// The body type: static, kinematic, or dynamic. + b3BodyType type; + + /// The initial world position of the body. Bodies should be created with the desired position. + /// @note Creating bodies at the origin and then moving them nearly doubles the cost of body creation, especially + /// if the body is moved after shapes have been added. + b3Pos position; + + /// The initial world rotation of the body. + b3Quat rotation; + + /// The initial linear velocity of the body's origin. Usually in meters per second. + b3Vec3 linearVelocity; + + /// The initial angular velocity of the body. Radians per second. + b3Vec3 angularVelocity; + + /// Linear damping is used to reduce the linear velocity. The damping parameter + /// can be larger than 1 but the damping effect becomes sensitive to the + /// time step when the damping parameter is large. + /// Generally linear damping is undesirable because it makes objects move slowly + /// as if they are floating. + float linearDamping; + + /// Angular damping is used to reduce the angular velocity. The damping parameter + /// can be larger than 1.0f but the damping effect becomes sensitive to the + /// time step when the damping parameter is large. + /// Angular damping can be used to slow down rotating bodies. + float angularDamping; + + /// Scale the gravity applied to this body. Non-dimensional. + float gravityScale; + + /// Sleep speed threshold, default is 0.05 meters per second + float sleepThreshold; + + /// Optional body name for debugging. + const char* name; + + /// Use this to store application specific body data. + void* userData; + + /// Motions locks to restrict linear and angular movement + b3MotionLocks motionLocks; + + /// Set this flag to false if this body should never fall asleep. + bool enableSleep; + + /// Is this body initially awake or sleeping? + bool isAwake; + + /// Treat this body as a high speed object that performs continuous collision detection + /// against dynamic and kinematic bodies, but not other bullet bodies. + /// @warning Bullets should be used sparingly. They are not a solution for general dynamic-versus-dynamic + /// continuous collision. They do not guarantee accurate collision if both bodies are fast moving because + /// the bullet does a continuous check after all non-bullet bodies have moved. You could get unlucky and have + /// the bullet body end a time step very close to a non-bullet body and the non-bullet body then moves over + /// the bullet body. In continuous collision, initial overlap is ignored to avoid freezing bodies in place. + /// I do not recommend using them for game projectiles if precise collision timing is needed. Instead consider + /// using a ray or shape cast. You can use a marching ray or shape cast for projectile that moves over time. + /// If you want a fast moving projectile to collide with a fast moving target, you need to consider the relative + /// movement in your ray or shape cast. This is out of the scope of Box3D. + /// So what are good use cases for bullets? Pinball games or games with dynamic containers that hold other objects. + /// It should be a use case where it doesn't break the game if there is a collision missed, but having them + /// captured improves the quality of the game. + bool isBullet; + + /// Used to disable a body. A disabled body does not move or collide. + bool isEnabled; + + /// This allows this body to bypass rotational speed limits. Should only be used + /// for circular objects, like wheels. + bool allowFastRotation; + + /// Enable contact recycling. True by default. Leaving this enabled improves performance + /// but may lead to ghost collision that should be avoided on characters. + bool enableContactRecycling; + + /// Used internally to detect a valid definition. DO NOT SET. + int internalValue; +} b3BodyDef; + +/// Use this to initialize your body definition +/// @ingroup body +B3_API b3BodyDef b3DefaultBodyDef( void ); + +/// This is used to filter collision on shapes. It affects shape-vs-shape collision +/// and shape-versus-query collision (such as b3World_CastRay). +/// @ingroup shape +typedef struct b3Filter +{ + /// The collision category bits. Normally you would just set one bit. The category bits should + /// represent your application object types. For example: + /// @code{.cpp} + /// enum MyCategories + /// { + /// Static = 0x00000001, + /// Dynamic = 0x00000002, + /// Debris = 0x00000004, + /// Player = 0x00000008, + /// // etc + /// }; + /// @endcode + uint64_t categoryBits; + + /// The collision mask bits. This states the categories that this + /// shape would accept for collision. + /// For example, you may want your player to only collide with static objects + /// and other players. + /// @code{.c} + /// maskBits = Static | Player; + /// @endcode + uint64_t maskBits; + + /// Collision groups allow a certain group of objects to never collide (negative) + /// or always collide (positive). A group index of zero has no effect. Non-zero group filtering + /// always wins against the mask bits. + /// For example, you may want ragdolls to collide with other ragdolls but you don't want + /// ragdoll self-collision. In this case you would give each ragdoll a unique negative group index + /// and apply that group index to all shapes on the ragdoll. + int groupIndex; +} b3Filter; + +/// Use this to initialize your filter +/// @ingroup shape +B3_API b3Filter b3DefaultFilter( void ); + +/// Material properties supported per triangle on meshes and height fields +/// @ingroup shape +typedef struct b3SurfaceMaterial +{ + /// The Coulomb (dry) friction coefficient, usually in the range [0,1]. + float friction; + + /// The coefficient of restitution (bounce) usually in the range [0,1]. + /// https://en.wikipedia.org/wiki/Coefficient_of_restitution + float restitution; + + /// The rolling resistance usually in the range [0,1]. This is only used for spheres and capsules. + float rollingResistance; + + /// The tangent velocity for conveyor belts. This is local to the shape and will be projected + /// onto the contact surface. + b3Vec3 tangentVelocity; + + /// User material identifier. This is passed with query results and to friction and restitution + /// combining functions. It is not used internally. + uint64_t userMaterialId; + + /// Custom debug draw color. Ignored if 0. The low 24 bits are RGB. The high byte may + /// carry a b3DebugMaterial preset, see b3MakeDebugColor. + /// @see b3HexColor + uint32_t customColor; +} b3SurfaceMaterial; + +/// Use this to initialize your surface material +/// @ingroup shape +B3_API b3SurfaceMaterial b3DefaultSurfaceMaterial( void ); + +/// Shape type +/// @ingroup shape +typedef enum b3ShapeType +{ + /// A capsule is an extruded sphere + b3_capsuleShape, + + /// A baked compound shape composed of spheres, capsules, hulls, and meshes + b3_compoundShape, + + /// A height field useful for terrain + b3_heightShape, + + /// A convex hull + b3_hullShape, + + /// A triangle soup + b3_meshShape, + + /// A sphere with an offset + b3_sphereShape, + + /// The number of shape types + b3_shapeTypeCount +} b3ShapeType; + +/// Used to create a shape +/// @ingroup shape +typedef struct b3ShapeDef +{ + /// Optional shape name for debugging + const char* name; + + /// Use this to store application specific shape data. + void* userData; + + /// Surface material used on mesh shapes per triangle. Ignored for convex shapes. Ignored for compound shapes. + b3SurfaceMaterial* materials; + + /// Surface material count. + int materialCount; + + /// The base surface material. Ignored for compound shapes. + b3SurfaceMaterial baseMaterial; + + /// The density, usually in kg/m^3. + float density; + + /// Explosion scale for b3World_Explode. non-dimensional + float explosionScale; + + /// Contact filtering data. + b3Filter filter; + + /// Enable custom filtering. Only one of the two shapes needs to enable custom filtering. See b3WorldDef. + bool enableCustomFiltering; + + /// A sensor shape generates overlap events but never generates a collision response. + /// Sensors do not have continuous collision. Instead, use a ray or shape cast for those scenarios. + /// Sensors still contribute to the body mass if they have non-zero density. + /// @note Sensor events are disabled by default. + /// @see enableSensorEvents + bool isSensor; + + /// Enable sensor events for this shape. This applies to sensors and non-sensors. False by default, even for sensors. + bool enableSensorEvents; + + /// Enable contact events for this shape. Only applies to kinematic and dynamic bodies. Ignored for sensors. False by default. + bool enableContactEvents; + + /// Enable hit events for this shape. Only applies to kinematic and dynamic bodies. Ignored for sensors. False by default. + bool enableHitEvents; + + /// Enable pre-solve contact events for this shape. Only applies to dynamic bodies. These are expensive + /// and must be carefully handled due to multithreading. Ignored for sensors. + bool enablePreSolveEvents; + + /// When shapes are created they will scan the environment for collision the next time step. This can significantly slow down + /// static body creation when there are many static shapes. + /// This is flag is ignored for dynamic and kinematic shapes which always invoke contact creation. + bool invokeContactCreation; + + /// Should the body update the mass properties when this shape is created. Default is true. + /// Warning: if this is false, you MUST call b3Body_ApplyMassFromShapes or b3Body_SetMassData before simulating the world. + bool updateBodyMass; + + /// Enable speculative collision. Leave this true unless you care about reducing ghost collision + /// more than continuous collision under rotation. + /// Experimental: this can only disable speculative contact between hulls and triangles (meshes and height fields). + bool enableSpeculativeContact; + + /// Used internally to detect a valid definition. DO NOT SET. + int internalValue; + +} b3ShapeDef; + +/// Use this to initialize your shape definition +/// @ingroup shape +B3_API b3ShapeDef b3DefaultShapeDef( void ); + +//! @cond +/// Profiling data. Times are in milliseconds. +/// @ingroup world +typedef struct b3Profile +{ + float step; + float pairs; + float collide; + float solve; + float solverSetup; + float constraints; + float prepareConstraints; + float integrateVelocities; + float warmStart; + float solveImpulses; + float integratePositions; + float relaxImpulses; + float applyRestitution; + float storeImpulses; + float splitIslands; + float transforms; + float sensorHits; + float jointEvents; + float hitEvents; + float refit; + float bullets; + float sleepIslands; + float sensors; +} b3Profile; + +/// Counters that give details of the simulation size. +/// @ingroup world +typedef struct b3Counters +{ + int bodyCount; + int shapeCount; + int contactCount; + int jointCount; + int islandCount; + int stackUsed; + int arenaCapacity; + int staticTreeHeight; + int treeHeight; + int satCallCount; + int satCacheHitCount; + int byteCount; + int taskCount; + int colorCounts[24]; + int manifoldCounts[B3_CONTACT_MANIFOLD_COUNT_BUCKETS]; + + /// Number of contacts touched by the collide pass + /// graph contacts + awake-set non-touching + int awakeContactCount; + + /// Number of contacts recycled in the most recent step. + int recycledContactCount; + + /// Maximum number of time of impact iterations + int distanceIterations; + int pushBackIterations; + int rootIterations; +} b3Counters; +//! @endcond + +/// Joint type enumeration. This is useful because all joint types use b3JointId and sometimes you +/// want to get the type of a joint. +/// @ingroup joint +typedef enum b3JointType +{ + b3_parallelJoint, + b3_distanceJoint, + b3_filterJoint, + b3_motorJoint, + b3_prismaticJoint, + b3_revoluteJoint, + b3_sphericalJoint, + b3_weldJoint, + b3_wheelJoint, +} b3JointType; + +/// Base joint definition used by all joint types. The local frames are measured from the +/// body's origin rather than the center of mass because: +/// 1. You might not know where the center of mass will be. +/// 2. If you add/remove shapes from a body and recompute the mass, the joints will be broken. +/// @ingroup joint +typedef struct b3JointDef +{ + /// User data pointer + void* userData; + + /// The first attached body + b3BodyId bodyIdA; + + /// The second attached body + b3BodyId bodyIdB; + + /// The first local joint frame + b3Transform localFrameA; + + /// The second local joint frame + b3Transform localFrameB; + + /// Force threshold for joint events + float forceThreshold; + + /// Torque threshold for joint events + float torqueThreshold; + + /// Constraint hertz (advanced feature) + float constraintHertz; + + /// Constraint damping ratio (advanced feature) + float constraintDampingRatio; + + /// Debug draw scale + float drawScale; + + /// Set this flag to true if the attached bodies should collide + bool collideConnected; + + /// Used internally to detect a valid definition. DO NOT SET. + int internalValue; +} b3JointDef; + +/// Distance joint definition. +/// Connects a point on body A with a point on body B by a segment. +/// Useful for ropes and springs. +/// @ingroup distance_joint +typedef struct b3DistanceJointDef +{ + /// Base joint definition + b3JointDef base; + + /// The rest length of this joint. Clamped to a stable minimum value. + float length; + + /// Enable the distance constraint to behave like a spring. If false + /// then the distance joint will be rigid, overriding the limit and motor. + bool enableSpring; + + /// The lower spring force controls how much tension it can sustain + float lowerSpringForce; + + /// The upper spring force controls how much compression it can sustain + float upperSpringForce; + + /// The spring linear stiffness Hertz, cycles per second + float hertz; + + /// The spring linear damping ratio, non-dimensional + float dampingRatio; + + /// Enable/disable the joint limit + bool enableLimit; + + /// Minimum length. Clamped to a stable minimum value. + float minLength; + + /// Maximum length. Must be greater than or equal to the minimum length. + float maxLength; + + /// Enable/disable the joint motor + bool enableMotor; + + /// The maximum motor force, usually in newtons + float maxMotorForce; + + /// The desired motor speed, usually in meters per second + float motorSpeed; +} b3DistanceJointDef; + +/// Use this to initialize your joint definition +/// @ingroup distance_joint +B3_API b3DistanceJointDef b3DefaultDistanceJointDef( void ); + +/// A motor joint is used to control the relative position and velocity between two bodies. +/// @ingroup motor_joint +typedef struct b3MotorJointDef +{ + /// Base joint definition + b3JointDef base; + + /// The desired linear velocity + b3Vec3 linearVelocity; + + /// The maximum motor force in newtons + float maxVelocityForce; + + /// The desired angular velocity + b3Vec3 angularVelocity; + + /// The maximum motor torque in newton-meters + float maxVelocityTorque; + + /// Linear spring hertz for position control + float linearHertz; + + /// Linear spring damping ratio + float linearDampingRatio; + + /// Maximum spring force in newtons + float maxSpringForce; + + /// Angular spring hertz for position control + float angularHertz; + + /// Angular spring damping ratio + float angularDampingRatio; + + /// Maximum spring torque in newton-meters + float maxSpringTorque; +} b3MotorJointDef; + +/// Use this to initialize your joint definition +/// @ingroup motor_joint +B3_API b3MotorJointDef b3DefaultMotorJointDef( void ); + +/// A filter joint is used to disable collision between two specific bodies. +/// @ingroup filter_joint +typedef struct b3FilterJointDef +{ + /// Base joint definition + b3JointDef base; +} b3FilterJointDef; + +/// Use this to initialize your joint definition +/// @ingroup filter_joint +B3_API b3FilterJointDef b3DefaultFilterJointDef( void ); + +/// Parallel joint definition. Constrains the angle between axis z in body A and axis z in body B +/// using a spring. Useful to keep a body upright. +/// @ingroup parallel_joint +typedef struct b3ParallelJointDef +{ + /// Base joint definition + b3JointDef base; + + /// The spring stiffness Hertz, cycles per second + float hertz; + + /// The spring damping ratio, non-dimensional + float dampingRatio; + + /// The maximum spring torque, typically in newton-meters. + float maxTorque; + +} b3ParallelJointDef; + +/// Use this to initialize your joint definition +/// @ingroup parallel_joint +B3_API b3ParallelJointDef b3DefaultParallelJointDef( void ); + +/// Prismatic joint definition. Body B may slide along the x-axis in local frame A. +/// Body B cannot rotate relative to body A. The joint translation is zero when the +/// local frame origins coincide in world space. +/// @ingroup prismatic_joint +typedef struct b3PrismaticJointDef +{ + /// Base joint definition + b3JointDef base; + + /// Enable a linear spring along the prismatic joint axis + bool enableSpring; + + /// The spring stiffness Hertz, cycles per second + float hertz; + + /// The spring damping ratio, non-dimensional + float dampingRatio; + + /// The target translation for the joint in meters. The spring-damper will drive + /// to this translation. + float targetTranslation; + + /// Enable/disable the joint limit + bool enableLimit; + + /// The lower translation limit + float lowerTranslation; + + /// The upper translation limit + float upperTranslation; + + /// Enable/disable the joint motor + bool enableMotor; + + /// The maximum motor force, typically in newtons + float maxMotorForce; + + /// The desired motor speed, typically in meters per second + float motorSpeed; +} b3PrismaticJointDef; + +/// Use this to initialize your joint definition +/// @ingroup prismatic_joint +B3_API b3PrismaticJointDef b3DefaultPrismaticJointDef( void ); + +/// Revolute joint definition. A point on body B is fixed to a point on body A. +/// Allows relative rotation about the z-axis. +/// @ingroup revolute_joint +typedef struct b3RevoluteJointDef +{ + /// Base joint definition. + b3JointDef base; + + /// The bodyB angle minus bodyA angle in the reference state (radians). + /// This defines the zero angle for the joint limit. + float targetAngle; + + /// Enable a rotational spring on the revolute hinge axis. + bool enableSpring; + + /// The spring stiffness Hertz, cycles per second. + float hertz; + + /// The spring damping ratio, non-dimensional. + float dampingRatio; + + /// A flag to enable joint limits. + bool enableLimit; + + /// The lower angle for the joint limit in radians. Minimum of -0.99*pi radians. + float lowerAngle; + + /// The upper angle for the joint limit in radians. Maximum of 0.99*pi radians. + float upperAngle; + + /// A flag to enable the joint motor. + bool enableMotor; + + /// The maximum motor torque, typically in newton-meters. + float maxMotorTorque; + + /// The desired motor speed in radians per second. + float motorSpeed; +} b3RevoluteJointDef; + +/// Use this to initialize your joint definition. +/// @ingroup revolute_joint +B3_API b3RevoluteJointDef b3DefaultRevoluteJointDef( void ); + +/// Spherical joint definition. A point on body B is fixed to a point on body A. +/// Allows rotation about the shared point. +/// @ingroup spherical_joint +typedef struct b3SphericalJointDef +{ + /// Base joint definition + b3JointDef base; + + /// Enable a rotational spring that attempts to align the two joint frames. + bool enableSpring; + + /// The spring stiffness Hertz, cycles per second. This may be clamped internally + /// according to the time step to maintain stability. Non-negative number. + float hertz; + + /// The spring damping ratio, non-dimensional. Non-negative number. + float dampingRatio; + + /// Target spring rotation, joint frame B relative to joint frame A. + b3Quat targetRotation; + + /// A flag to enable the cone limit. The cone is centered on the frameA z-axis. + bool enableConeLimit; + + /// The angle for the cone limit in radians. Valid range is [0, pi] + float coneAngle; + + /// A flag to enable the twist limit. The twist is centered on the frameB z-axis. + bool enableTwistLimit; + + /// The angle for the lower twist limit in radians. Minimum of -0.99*pi radians. + float lowerTwistAngle; + + /// The angle for the upper twist limit in radians. Maximum of 0.99*pi radians. + float upperTwistAngle; + + /// A flag to enable the joint motor + bool enableMotor; + + /// The maximum motor torque, typically in newton-meters. Non-negative number. + float maxMotorTorque; + + /// The desired motor angular velocity in radians per second. + b3Vec3 motorVelocity; +} b3SphericalJointDef; + +/// Use this to initialize your joint definition. +/// @ingroup spherical_joint +B3_API b3SphericalJointDef b3DefaultSphericalJointDef( void ); + +/// Weld joint definition +/// Connects two bodies together rigidly. This constraint provides springs to mimic +/// soft-body simulation. +/// @note The approximate solver in Box3D cannot hold many bodies together rigidly +/// @ingroup weld_joint +typedef struct b3WeldJointDef +{ + /// Base joint definition + b3JointDef base; + + /// Linear stiffness expressed as Hertz (cycles per second). Use zero for maximum stiffness. + float linearHertz; + + /// Angular stiffness as Hertz (cycles per second). Use zero for maximum stiffness. + float angularHertz; + + /// Linear damping ratio, non-dimensional. Use 1 for critical damping. + float linearDampingRatio; + + /// Linear damping ratio, non-dimensional. Use 1 for critical damping. + float angularDampingRatio; +} b3WeldJointDef; + +/// Use this to initialize your joint definition +/// @ingroup weld_joint +B3_API b3WeldJointDef b3DefaultWeldJointDef( void ); + +/// Wheel joint definition +/// Body A is the chassis and body B is the wheel. +/// The wheel rotates around the local z-axis in frame B. +/// The wheel translates along the local x-axis in frame A. +/// The wheel can optionally steer along the x-axis in frame A. +/// @ingroup wheel_joint +typedef struct b3WheelJointDef +{ + /// Base joint definition + b3JointDef base; + + /// Enable a linear spring along the local axis + bool enableSuspensionSpring; + + /// Spring stiffness in Hertz + float suspensionHertz; + + /// Spring damping ratio, non-dimensional + float suspensionDampingRatio; + + /// Enable/disable the joint linear limit + bool enableSuspensionLimit; + + /// The lower suspension translation limit + float lowerSuspensionLimit; + + /// The upper translation limit + float upperSuspensionLimit; + + /// Enable/disable the joint rotational motor + bool enableSpinMotor; + + /// The maximum motor torque, typically in newton-meters + float maxSpinTorque; + + /// The desired motor speed in radians per second + float spinSpeed; + + /// Enable steering, otherwise the steering is fixed forward + bool enableSteering; + + /// Steering stiffness in Hertz + float steeringHertz; + + /// Spring damping ratio, non-dimensional + float steeringDampingRatio; + + /// The target steering angle in radians + float targetSteeringAngle; + + /// The maximum steering torque in N*m + float maxSteeringTorque; + + /// Enable/disable the steering angular limit + bool enableSteeringLimit; + + /// The lower steering angle in radians + float lowerSteeringLimit; + + /// The upper steering angle in radians + float upperSteeringLimit; +} b3WheelJointDef; + +/// Use this to initialize your joint definition +/// @ingroup wheel_joint +B3_API b3WheelJointDef b3DefaultWheelJointDef( void ); + +/// The explosion definition is used to configure options for explosions. Explosions +/// consider shape geometry when computing the impulse. +/// @ingroup world +typedef struct b3ExplosionDef +{ + /// Mask bits to filter shapes + uint64_t maskBits; + + /// The center of the explosion in world space + b3Pos position; + + /// The radius of the explosion + float radius; + + /// The falloff distance beyond the radius. Impulse is reduced to zero at this distance. + float falloff; + + /// Impulse per unit area. This applies an impulse according to the shape area that + /// is facing the explosion. Explosions only apply to spheres, capsules, and hulls. This + /// may be negative for implosions. + float impulsePerArea; +} b3ExplosionDef; + +/// Use this to initialize your explosion definition +/// @ingroup world +B3_API b3ExplosionDef b3DefaultExplosionDef( void ); + +/** + * @defgroup event Events + * World event types. + * + * Events are used to collect events that occur during the world time step. These events + * are then available to query after the time step is complete. This is preferable to callbacks + * because Box3D uses multithreaded simulation. + * + * Also when events occur in the simulation step it may be problematic to modify the world, which is + * often what applications want to do when events occur. + * + * With event arrays, you can scan the events in a loop and modify the world. However, you need to be careful + * that some event data may become invalid. There are several samples that show how to do this safely. + * + * @{ + */ + +/// A begin-touch event is generated when a shape starts to overlap a sensor shape. +typedef struct b3SensorBeginTouchEvent +{ + /// The id of the sensor shape + b3ShapeId sensorShapeId; + + /// The id of the shape that began touching the sensor shape + b3ShapeId visitorShapeId; +} b3SensorBeginTouchEvent; + +/// An end touch event is generated when a shape stops overlapping a sensor shape. +/// These include things like setting the transform, destroying a body or shape, or changing +/// a filter. You will also get an end event if the sensor or visitor are destroyed. +/// Therefore you should always confirm the shape id is valid using b3Shape_IsValid. +typedef struct b3SensorEndTouchEvent +{ + /// The id of the sensor shape + /// @warning this shape may have been destroyed + /// @see b3Shape_IsValid + b3ShapeId sensorShapeId; + + /// The id of the shape that stopped touching the sensor shape + /// @warning this shape may have been destroyed + /// @see b3Shape_IsValid + b3ShapeId visitorShapeId; +} b3SensorEndTouchEvent; + +/// Sensor events are buffered in the world and are available +/// as begin/end overlap event arrays after the time step is complete. +/// Note: these may become invalid if bodies and/or shapes are destroyed +typedef struct b3SensorEvents +{ + /// Array of sensor begin touch events + b3SensorBeginTouchEvent* beginEvents; + + /// Array of sensor end touch events + b3SensorEndTouchEvent* endEvents; + + /// The number of begin touch events + int beginCount; + + /// The number of end touch events + int endCount; +} b3SensorEvents; + +/// A begin-touch event is generated when two shapes begin touching. +typedef struct b3ContactBeginTouchEvent +{ + /// Id of the first shape + b3ShapeId shapeIdA; + + /// Id of the second shape + b3ShapeId shapeIdB; + + /// The transient contact id. This contact may be destroyed automatically when the world is modified or simulated. + /// Use b3Contact_IsValid before using this id. + b3ContactId contactId; +} b3ContactBeginTouchEvent; + +/// An end touch event is generated when two shapes stop touching. +/// You will get an end event if you do anything that destroys contacts previous to the last +/// world step. These include things like setting the transform, destroying a body +/// or shape, or changing a filter or body type. +typedef struct b3ContactEndTouchEvent +{ + /// Id of the first shape + /// @warning this shape may have been destroyed + /// @see b3Shape_IsValid + b3ShapeId shapeIdA; + + /// Id of the first shape + /// @warning this shape may have been destroyed + /// @see b3Shape_IsValid + b3ShapeId shapeIdB; + + /// Id of the contact. + /// @warning this contact may have been destroyed + /// @see b3Contact_IsValid + b3ContactId contactId; +} b3ContactEndTouchEvent; + +/// A hit touch event is generated when two shapes collide with a speed faster than the hit speed threshold. +/// This may be reported for speculative contacts that have a confirmed impulse. +typedef struct b3ContactHitEvent +{ + /// Id of the first shape + b3ShapeId shapeIdA; + + /// Id of the second shape + b3ShapeId shapeIdB; + + /// Id of the contact. + /// @warning this contact may have been destroyed + /// @see b3Contact_IsValid + b3ContactId contactId; + + /// Point where the shapes hit at the beginning of the time step. + /// This is a mid-point between the two surfaces. It could be at speculative + /// point where the two shapes were not touching at the beginning of the time step. + b3Pos point; + + /// Normal vector pointing from shape A to shape B + b3Vec3 normal; + + /// The speed the shapes are approaching. Always positive. Typically in meters per second. + float approachSpeed; + + /// User material on shape A + uint64_t userMaterialIdA; + + /// User material on shape B + uint64_t userMaterialIdB; + +} b3ContactHitEvent; + +/// Contact events are buffered in the world and are available +/// as event arrays after the time step is complete. +/// Note: these may become invalid if bodies and/or shapes are destroyed +typedef struct b3ContactEvents +{ + /// Array of begin touch events + b3ContactBeginTouchEvent* beginEvents; + + /// Array of end touch events + b3ContactEndTouchEvent* endEvents; + + /// Array of hit events + b3ContactHitEvent* hitEvents; + + /// Number of begin touch events + int beginCount; + + /// Number of end touch events + int endCount; + + /// Number of hit events + int hitCount; +} b3ContactEvents; + +/// Body move events triggered when a body moves. +/// Triggered when a body moves due to simulation. Not reported for bodies moved by the user. +/// This also has a flag to indicate that the body went to sleep so the application can also +/// sleep that actor/entity/object associated with the body. +/// On the other hand if the flag does not indicate the body went to sleep then the application +/// can treat the actor/entity/object associated with the body as awake. +/// This is an efficient way for an application to update game object transforms rather than +/// calling functions such as b3Body_GetTransform() because this data is delivered as a contiguous array +/// and it is only populated with bodies that have moved. +/// @note If sleeping is disabled all dynamic and kinematic bodies will trigger move events. +typedef struct b3BodyMoveEvent +{ + /// The body user data. + void* userData; + + /// The body transform. + b3WorldTransform transform; + + /// The body id. + b3BodyId bodyId; + + /// Did the body fall asleep this time step? + bool fellAsleep; +} b3BodyMoveEvent; + +/// Body events are buffered in the world and are available +/// as event arrays after the time step is complete. +/// Note: this data becomes invalid if bodies are destroyed +typedef struct b3BodyEvents +{ + /// Array of move events + b3BodyMoveEvent* moveEvents; + + /// Number of move events + int moveCount; +} b3BodyEvents; + +/// Joint events report joints that are awake and have a force and/or torque exceeding the threshold +/// The observed forces and torques are not returned for efficiency reasons. +typedef struct b3JointEvent +{ + /// The joint id + b3JointId jointId; + + /// The user data from the joint for convenience + void* userData; +} b3JointEvent; + +/// Joint events are buffered in the world and are available +/// as event arrays after the time step is complete. +/// Note: this data becomes invalid if joints are destroyed +typedef struct b3JointEvents +{ + /// Array of events + b3JointEvent* jointEvents; + + /// Number of events + int count; +} b3JointEvents; + +/// The contact data for two shapes. By convention the manifold normal points +/// from shape A to shape B. +/// @see b3Shape_GetContactData() and b3Body_GetContactData() +typedef struct b3ContactData +{ + /// The contact id. You may hold onto this to track a contact across time steps. + /// This id may become orphaned. Use b3Contact_IsValid before using it for other functions. + b3ContactId contactId; + + /// The first shape id. + b3ShapeId shapeIdA; + + /// The second shape id. + b3ShapeId shapeIdB; + + /// The contact manifold. This points to internal data and may become invalid. Do not store + /// this pointer. + const struct b3Manifold* manifolds; + + /// The number of contact manifolds. For mesh and height-field collision there can be multiple manifolds. + int manifoldCount; +} b3ContactData; + +/**@}*/ // event + +/** + * @defgroup query Query + * @brief Query types and functions + * + * Queries include ray casts, shapes casts, overlap, distance, and time of impact. + * @{ + */ + +/// The query filter is used to filter collisions between queries and shapes. For example, +/// you may want a ray-cast representing a projectile to hit players and the static environment +/// but not debris. +typedef struct b3QueryFilter +{ + /// The collision category bits of this query. Normally you would just set one bit. + uint64_t categoryBits; + + /// The collision mask bits. This states the shape categories that this + /// query would accept for collision. + uint64_t maskBits; + + /// Optional id combined with @ref name to identify this query in a recording, e.g. an entity id. + /// Need not be unique on its own. 0 with a null name means untagged. Ignored when not recording. + uint64_t id; + + /// Optional label combined with @ref id to identify this query, e.g. "bullet". Need not be unique + /// on its own. The recorder hashes (id, name) into one stable key the viewer tracks the query by, + /// so the same id and name pair identifies the same query across frames. NULL means none. Ignored + /// when not recording. + const char* name; +} b3QueryFilter; + +/// Use this to initialize your query filter +B3_API b3QueryFilter b3DefaultQueryFilter( void ); + +/// Low level ray cast input data. +typedef struct b3RayCastInput +{ + /// Start point of the ray cast. + b3Vec3 origin; + + /// Translation of the ray cast. + /// end = start + translation. + b3Vec3 translation; + + /// The maximum fraction of the translation to consider, typically 1 + float maxFraction; +} b3RayCastInput; + +/// Result from b3World_RayCastClosest. +typedef struct b3RayResult +{ + /// The shape hit. + b3ShapeId shapeId; + + /// The world point of the hit. + b3Pos point; + + /// The world normal of the shape surface at the hit point. + b3Vec3 normal; + + /// The user material id at the hit point. This can be per triangle + /// if the shape is a mesh, height-field, or compound with child mesh. + uint64_t userMaterialId; + + /// The fraction of the input ray. + float fraction; + + /// The triangle index if the shape is a mesh, height-field, or compound with + /// child mesh. + int triangleIndex; + + /// The child index if the shape is a compound. + int childIndex; + + /// The number of BVH nodes visited. Diagnostic. + int nodeVisits; + + /// The number of BVH leaves visited. Diagnostic. + int leafVisits; + + /// Did the ray hit? If false, all other data is invalid. + bool hit; +} b3RayResult; + +/// A shape proxy is used by the GJK algorithm. It can represent a convex shape. +typedef struct b3ShapeProxy +{ + /// The point cloud. + const b3Vec3* points; + + /// The number of points. Do not exceed B3_MAX_SHAPE_CAST_POINTS. + int count; + + /// The external radius of the point cloud. + float radius; +} b3ShapeProxy; + +/// Low level shape cast input in generic form. This allows casting an arbitrary point +/// cloud wrap with a radius. For example, a sphere is a single point with a non-zero radius. +/// A capsule is two points with a non-zero radius. A box is four points with a zero radius. +typedef struct b3ShapeCastInput +{ + /// A generic query shape. + b3ShapeProxy proxy; + + /// The translation of the shape cast. + b3Vec3 translation; + + /// The maximum fraction of the translation to consider, typically 1. + float maxFraction; + + /// Allow shape cast to encroach when initially touching. This only works if the radius is greater than zero. + bool canEncroach; +} b3ShapeCastInput; + +/// Input for sweeping an AABB through a dynamic tree. The box is in the tree's world float frame. +/// The caller folds the cast shape radius and any world origin into the box, so the tree traversal +/// stays a conservative box sweep and the precise narrow phase happens per shape in the callback. +typedef struct b3BoxCastInput +{ + /// The AABB to cast, in the tree's frame. + b3AABB box; + + /// The sweep translation. + b3Vec3 translation; + + /// The maximum fraction of the translation to consider, typically 1. + float maxFraction; +} b3BoxCastInput; + +/// Low level ray cast or shape-cast output data. +typedef struct b3CastOutput +{ + /// The surface normal at the hit point. + b3Vec3 normal; + + /// The surface hit point. + b3Vec3 point; + + /// The fraction of the input translation at collision. + float fraction; + + /// The number of iterations used. + int iterations; + + /// The index of the mesh or height field triangle hit. + int triangleIndex; + + /// The index of the compound child shape. + int childIndex; + + /// The material index. May be -1 for null. + int materialIndex; + + /// Did the cast hit? + bool hit; +} b3CastOutput; + +#if defined( BOX3D_DOUBLE_PRECISION ) + +/// Ray cast or shape-cast output in world space. The hit point is a world position so the result +/// stays precise far from the world origin. Mirrors b3CastOutput with a double precision point. +typedef struct b3WorldCastOutput +{ + /// The surface normal at the hit point. + b3Vec3 normal; + + /// The surface hit point in world space. + b3Pos point; + + /// The fraction of the input translation at collision. + float fraction; + + /// The number of iterations used. + int iterations; + + /// The index of the mesh or height field triangle hit. + int triangleIndex; + + /// The index of the compound child shape. + int childIndex; + + /// The material index. May be -1 for null. + int materialIndex; + + /// Did the cast hit? + bool hit; +} b3WorldCastOutput; + +#else + +/// Same type in single precision. +typedef b3CastOutput b3WorldCastOutput; + +#endif + +/// Body cast result for ray and shape casts. +typedef struct b3BodyCastResult +{ + /// The shape hit. + b3ShapeId shapeId; + + /// The world point on the shape surface. + b3Pos point; + + /// The world normal vector on the shape surface. + b3Vec3 normal; + + /// The fraction along the ray hit. + /// hit point = origin + fraction * translation + float fraction; + + /// The triangle index if the shape is a mesh or height-field. + int triangleIndex; + + /// The user material id at the hit point. This can be per triangle + /// if the shape is a mesh, height-field, or compound with child mesh. + uint64_t userMaterialId; + + /// The number of iterations used. Diagnostic. + int iterations; + + /// Did the cast hit? If false, all other fields are invalid. + bool hit; +} b3BodyCastResult; + +/// Used to warm start the GJK simplex. If you call this function multiple times with nearby +/// transforms this might improve performance. Otherwise you can zero initialize this. +/// The distance cache must be initialized to zero on the first call. +/// Users should generally just zero initialize this structure for each call. +typedef struct b3SimplexCache +{ + /// Value use to compare length, area, volume of two simplexes. + float metric; + + // todo use an index of 0xFF as a sentinel and remove the count + /// The number of stored simplex points + uint16_t count; + + /// The cached simplex indices on shape A + uint8_t indexA[4]; + + /// The cached simplex indices on shape B + uint8_t indexB[4]; + +} b3SimplexCache; + +static const b3SimplexCache b3_emptyDistanceCache = B3_ZERO_INIT; + +/// Input parameters for b3ShapeCast +typedef struct b3ShapeCastPairInput +{ + b3ShapeProxy proxyA; ///< The proxy for shape A + b3ShapeProxy proxyB; ///< The proxy for shape B + b3Transform transform; ///< Transform of shape B in shape A's frame, the relative pose B in A + b3Vec3 translationB; ///< The translation of shape B, in A's frame + float maxFraction; ///< The fraction of the translation to consider, typically 1 + bool canEncroach; ///< Allows shapes with a radius to move slightly closer if already touching +} b3ShapeCastPairInput; + +/// Input for b3ShapeDistance +typedef struct b3DistanceInput +{ + /// The proxy for shape A + b3ShapeProxy proxyA; + + /// The proxy for shape B + b3ShapeProxy proxyB; + + /// Transform of shape B in shape A's frame, the relative pose B in A + /// (b3InvMulWorldTransforms( worldA, worldB )). The query is origin independent and runs in frame A. + b3Transform transform; + + /// Should the proxy radius be considered? + bool useRadii; +} b3DistanceInput; + +/// Output for b3ShapeDistance +typedef struct b3DistanceOutput +{ + b3Vec3 pointA; ///< Closest point on shapeA, in shape A's frame + b3Vec3 pointB; ///< Closest point on shapeB, in shape A's frame + b3Vec3 normal; ///< A to B normal in shape A's frame. Invalid if distance is zero. + float distance; ///< The final distance, zero if overlapped + int iterations; ///< Number of GJK iterations used + int simplexCount; ///< The number of simplexes stored in the simplex array +} b3DistanceOutput; + +/// Simplex vertex for debugging the GJK algorithm +typedef struct b3SimplexVertex +{ + b3Vec3 wA; ///< support point in proxyA + b3Vec3 wB; ///< support point in proxyB + b3Vec3 w; ///< wB - wA + float a; ///< barycentric coordinates + int indexA; ///< wA index + int indexB; ///< wB index +} b3SimplexVertex; + +/// Simplex from the GJK algorithm +typedef struct b3Simplex +{ + b3SimplexVertex vertices[4]; ///< vertices + int count; ///< number of valid vertices +} b3Simplex; + +/// This describes the motion of a body/shape for TOI computation. Shapes are defined with respect to the body origin, +/// which may not coincide with the center of mass. However, to support dynamics we must interpolate the center of mass +/// position. +typedef struct b3Sweep +{ + b3Vec3 localCenter; ///< Local center of mass position + b3Vec3 c1; ///< Starting center of mass world position + b3Vec3 c2; ///< Ending center of mass world position + b3Quat q1; ///< Starting world rotation + b3Quat q2; ///< Ending world rotation +} b3Sweep; + +/// Time of impact input +typedef struct b3TOIInput +{ + b3ShapeProxy proxyA; ///< The proxy for shape A + b3ShapeProxy proxyB; ///< The proxy for shape B + b3Sweep sweepA; ///< The movement of shape A + b3Sweep sweepB; ///< The movement of shape B + float maxFraction; ///< Defines the sweep interval [0, tMax] +} b3TOIInput; + +/// Describes the TOI output +typedef enum b3TOIState +{ + b3_toiStateUnknown, + b3_toiStateFailed, + b3_toiStateOverlapped, + b3_toiStateHit, + b3_toiStateSeparated +} b3TOIState; + +/// Time of impact output +typedef struct b3TOIOutput +{ + /// The type of result + b3TOIState state; + + /// The hit point + b3Vec3 point; + + /// The hit normal + b3Vec3 normal; + + /// The sweep time of the collision + float fraction; + + /// The final distance + float distance; + + /// Number of outer iterations + int distanceIterations; + + /// Total number of push back iterations + int pushBackIterations; + + /// Total number of root iterations + int rootIterations; + + /// Indicates that the time of impact detected initial + /// overlap and used a fallback sphere as a last ditch effort + /// to prevent tunneling. + bool usedFallback; +} b3TOIOutput; + +/**@}*/ // query + +/** + * @defgroup tree Dynamic Tree + * The dynamic tree is a binary AABB tree to organize and query large numbers of geometric objects + * + * Box3D uses the dynamic tree internally to sort collision shapes into a binary bounding volume hierarchy. + * This data structure may have uses in games for organizing other geometry data and may be used independently + * of Box3D rigid body simulation. + * + * A dynamic AABB tree broad-phase, inspired by Nathanael Presson's btDbvt. + * A dynamic tree arranges data in a binary tree to accelerate + * queries such as AABB queries and ray casts. Leaf nodes are proxies + * with an AABB. These are used to hold a user collision object. + * Nodes are pooled and relocatable, so I use node indices rather than pointers. + * The dynamic tree is made available for advanced users that would like to use it to organize + * spatial game data besides rigid bodies. + * @{ + */ + +/// Flags for tree nodes. For internal usage. +typedef enum b3TreeNodeFlags +{ + b3_allocatedNode = 0x0001, + b3_enlargedNode = 0x0002, + b3_leafNode = 0x0004, +} b3TreeNodeFlags; + +/// Tree node child indices. For internal usage. +typedef struct b3TreeNodeChildren +{ + int child1; ///< child node index 1 + int child2; ///< child node index 2 +} b3TreeNodeChildren; + +/// A node in the dynamic tree. This is private data placed here for performance reasons. +/// todo test padding to 64 bytes to avoid straddling cache lines +typedef struct b3TreeNode +{ + /// The node bounding box + b3AABB aabb; // 24 + + /// Category bits for collision filtering + uint64_t categoryBits; // 8 + + union + { + /// Children (internal node) + b3TreeNodeChildren children; + + /// User data (leaf node) + uint64_t userData; + }; // 8 + + union + { + /// The node parent index (allocated node) + int parent; + + /// The node freelist next index (free node) + int next; + }; // 4 + + /// Height of the node. Leaves have a height of 0. + uint16_t height; // 2 + + /// @see b3TreeNodeFlags + uint16_t flags; // 2 +} b3TreeNode; + +/// Dynamic tree version for compatibility testing. +#define B3_DYNAMIC_TREE_VERSION 0x93EDAF889FD30B4Aull + +/// The dynamic tree structure. This should be considered private data. +/// It is placed here for performance reasons. +typedef struct b3DynamicTree +{ + /// The dynamic tree version. Always the first field. Useful + /// if the tree is serialized. + uint64_t version; + + /// The tree nodes + b3TreeNode* nodes; + + /// The root index + int root; + + /// The number of nodes + int nodeCount; + + /// The allocated node space + int nodeCapacity; + + /// Number of proxies created + int proxyCount; + + /// Node free list + int freeList; + + /// Leaf indices for rebuild + int* leafIndices; + + /// Leaf bounding boxes for rebuild + b3AABB* leafBoxes; + + /// Leaf bounding box centers for rebuild + b3Vec3* leafCenters; + + /// Bins for sorting during rebuild + int* binIndices; + + /// Allocated space for rebuilding + int rebuildCapacity; +} b3DynamicTree; + +/// These are performance results returned by dynamic tree queries. +typedef struct b3TreeStats +{ + /// Number of internal nodes visited during the query + int nodeVisits; + + /// Number of leaf nodes visited during the query + int leafVisits; +} b3TreeStats; + +/// This function receives proxies found in the AABB query. +/// @return true if the query should continue +typedef bool b3TreeQueryCallbackFcn( int proxyId, uint64_t userData, void* context ); + +/// This function receives the minimum distance squared so far and proxy to check in the closest query. +/// @return minimum distance squared to user objects in the proxy +typedef float b3TreeQueryClosestCallbackFcn( float distanceSqrMin, int proxyId, uint64_t userData, void* context ); + +/// This function receives clipped AABB cast input for a proxy. The function returns the new cast +/// fraction. +/// - return a value of 0 to terminate the cast +/// - return a value less than input->maxFraction to clip the cast +/// - return a value of input->maxFraction to continue the cast without clipping +typedef float b3TreeBoxCastCallbackFcn( const b3BoxCastInput* input, int proxyId, uint64_t userData, void* context ); + +/// This function receives clipped ray cast input for a proxy. The function +/// returns the new ray fraction. +/// - return a value of 0 to terminate the ray cast +/// - return a value less than input->maxFraction to clip the ray +/// - return a value of input->maxFraction to continue the ray cast without clipping +typedef float b3TreeRayCastCallbackFcn( const b3RayCastInput* input, int proxyId, uint64_t userData, void* context ); + +/**@}*/ // tree + +/** + * @defgroup character Character Mover + * Character movement solver + * @{ + */ + +/// The plane between a character mover and a shape +typedef struct b3PlaneResult +{ + /// Outward pointing plane. + b3Plane plane; + + /// Closest point on the shape. May not be unique. + b3Vec3 point; + +} b3PlaneResult; + +/// These are collision planes that can be fed to b3SolvePlanes. Normally +/// this is assembled by the user from plane results in b3PlaneResult. +typedef struct b3CollisionPlane +{ + /// The collision plane between the mover and some shape. + b3Plane plane; + + /// Setting this to FLT_MAX makes the plane as rigid as possible. Lower values can + /// make the plane collision soft. Usually in meters. + float pushLimit; + + /// The push on the mover determined by b3SolvePlanes. Usually in meters. + float push; + + /// Indicates if b3ClipVector should clip against this plane. Should be false for soft collision. + bool clipVelocity; +} b3CollisionPlane; + +/// Result returned by b3SolvePlanes. +typedef struct b3PlaneSolverResult +{ + /// The final relative translation. + b3Vec3 delta; + + /// The number of iterations used by the plane solver. For diagnostics. + int iterationCount; +} b3PlaneSolverResult; + +/// Body plane result for movers. +typedef struct b3BodyPlaneResult +{ + /// The shape id on the body. + b3ShapeId shapeId; + + /// The plane result. + b3PlaneResult result; +} b3BodyPlaneResult; + +/// Used to collect collision planes for character movers. +/// Return true to continue gathering planes. +typedef bool b3PlaneResultFcn( b3ShapeId shapeId, const b3PlaneResult* plane, int planeCount, void* context ); + +/// Used to filter shapes for shape casting character movers. +/// Return true to accept the collision +typedef bool b3MoverFilterFcn( b3ShapeId shapeId, void* context ); + +/**@}*/ // mover + +/** + * @defgroup geometry Geometry + * @brief Geometry types and algorithms + * + * Definitions of spheres, capsules, hulls, meshes, height fields, and compounds. + * @{ + */ + +/// This holds the mass data computed for a shape. +typedef struct b3MassData +{ + /// The shape mass + float mass; + + /// The local center of mass position. + b3Vec3 center; + + /// The inertia tensor about the shape center of mass. + b3Matrix3 inertia; +} b3MassData; + +/** + * @defgroup sphere Sphere + * @brief Sphere primitive + * @{ + */ + +/// A solid sphere +typedef struct b3Sphere +{ + /// The local center + b3Vec3 center; + + /// The radius + float radius; +} b3Sphere; + +/**@}*/ // sphere + +/** + * @defgroup capsule Capsule + * @brief Capsule primitive + * @{ + */ + +/// A solid capsule can be viewed as two hemispheres connected +/// by a rectangle. +typedef struct b3Capsule +{ + /// Local center of the first hemisphere + b3Vec3 center1; + + /// Local center of the second hemisphere + b3Vec3 center2; + + /// The radius of the hemispheres + float radius; +} b3Capsule; + +/**@}*/ // capsule + +/** + * @defgroup hull Convex Hull + * @brief Convex hull primitive + * @{ + */ + +/// A hull vertex. Identified by a half-edge with this +/// vertex as its tail. +typedef struct b3HullVertex +{ + /// A half-edge that has this vertex as the origin + /// Can be used along with edge twins and winding order + /// to traverse all the edges connected to this vertex. + uint8_t edge; +} b3HullVertex; + +/// Half-edge for hull data structure +typedef struct b3HullHalfEdge +{ + /// Next edge index CCW + uint8_t next; + + /// Twin edge index + uint8_t twin; + + /// index of origin vertex and point + uint8_t origin; + + /// Face to the left of this edge + uint8_t face; +} b3HullHalfEdge; + +/// A hull face. Hulls use a half-edge data structure, so a face +/// can be determined from a single half-edge index. +typedef struct b3HullFace +{ + /// An arbitrary half-edge on this face + uint8_t edge; +} b3HullFace; + +/// 64-bit hull version. Useful for validating serialized data. +#define B3_HULL_VERSION 0x9D4716CE3793900Eull + +/// A convex hull. +/// @note This data structure has data hanging off the end and cannot be directly copied. +typedef struct b3HullData +{ + /// Version must be first and match B3_HULL_VERSION + uint64_t version; + + /// The total number of bytes for this hull. + int byteCount; + + /// Hash of this hull (this field is zero when the hash is computed). + uint32_t hash; + + /// Axis-aligned box in local space. + b3AABB aabb; + + /// Surface area, typically in squared meters. + float surfaceArea; + + /// Volume, typically in m^3. + float volume; + + /// The radius of the largest sphere at the center. + float innerRadius; + + /// The local centroid + b3Vec3 center; + + /// The inertia tensor about the centroid. + b3Matrix3 centralInertia; + + /// The vertex count. + int vertexCount; + + /// Offset of the vertex array in bytes from the struct address. + int vertexOffset; + + /// Offset of the point array in bytes from the struct address. + int pointOffset; + + /// This is the half-edge count (double the edge count) + int edgeCount; + + /// Offset of the edge array in bytes from the struct address. + int edgeOffset; + + /// The face count. Hulls faces are convex polygons. + int faceCount; + + /// Offset of the face array in bytes from the struct address. + int faceOffset; + + /// Offset of the face plane array in bytes from the struct address. + int planeOffset; + + /// Explicit padding. Hull identity is a content hash and memcmp over raw bytes, + /// so there must be no unnamed padding for struct copies to scramble. + int padding; +} b3HullData; + +/// Efficient box hull +typedef struct b3BoxHull +{ + /// The embedded hull. So the offsets index into the arrays that follow. + b3HullData base; + b3HullVertex boxVertices[8]; ///< Box vertices. + b3Vec3 boxPoints[8]; ///< Box points. + b3HullHalfEdge boxEdges[24]; ///< Box half-edges. + b3HullFace boxFaces[6]; ///< Box faces. + uint8_t padding[2]; ///< Explicit padding, see b3HullData::padding. + b3Plane boxPlanes[6]; ///< Box face planes. +} b3BoxHull; + +/**@}*/ // hull + +/** + * @defgroup mesh Triangle Mesh + * @brief Triangle mesh collision shape + * @{ + */ + +/// This is used to create a re-usable collision mesh +typedef struct b3MeshDef +{ + /// Triangle vertices + b3Vec3* vertices; + + /// Triangle vertex indices. 3 for each triangle. + int32_t* indices; + + /// Triangle material index. 1 per triangle. Indexes into b3ShapeDef::materials. + /// This allows different run-time material data to be associated with different + /// instances of this mesh. + uint8_t* materialIndices; + + /// Tolerance for vertex welding in length units. + float weldTolerance; + + /// The vertex count. Must be 3 or more. + int vertexCount; + + /// The triangle count. Must be 1 or more. + int triangleCount; + + /// Optionally weld nearby vertices. + bool weldVertices; + + /// Use the median split instead of SAH to speed up mesh creation. Good + /// for meshes that are structured like a grid. + bool useMedianSplit; + + /// Compute triangle adjacency information using shared edges + bool identifyEdges; +} b3MeshDef; + +/// 64-bit mesh version. Useful for validating serialized data. +#define B3_MESH_VERSION 0xABD11AB62A6E886Dull + +/// Triangle mesh edge flags. +typedef enum b3MeshEdgeFlags +{ + b3_concaveEdge1 = 0x01, + b3_concaveEdge2 = 0x02, + b3_concaveEdge3 = 0x04, + + b3_inverseConcaveEdge1 = 0x10, + b3_inverseConcaveEdge2 = 0x20, + b3_inverseConcaveEdge3 = 0x40, + + b3_allConcaveEdges = b3_concaveEdge1 | b3_concaveEdge2 | b3_concaveEdge3, + + b3_flatEdge1 = b3_concaveEdge1 | b3_inverseConcaveEdge1, + b3_flatEdge2 = b3_concaveEdge2 | b3_inverseConcaveEdge2, + b3_flatEdge3 = b3_concaveEdge3 | b3_inverseConcaveEdge3, + + b3_allFlatEdges = b3_flatEdge1 | b3_flatEdge2 | b3_flatEdge3, + +} b3MeshEdgeFlags; + +/// A mesh triangle. +typedef struct b3MeshTriangle +{ + int32_t index1; ///< Index of vertex 1. + int32_t index2; ///< Index of vertex 2. + int32_t index3; ///< Index of vertex 3. +} b3MeshTriangle; + +/// A mesh BVH node. +typedef struct b3MeshNode +{ + /// The lower bound of the node AABB. Strategic placement for SIMD. + b3Vec3 lowerBound; + + /// Anonymous union. + union + { + /// Internal node + struct + { + /// Split axis. 0, 1, or 2. + uint32_t axis : 2; + /// Offset of the second child node. + uint32_t childOffset : 30; + } asNode; + + /// Leaf node + struct + { + /// Aligned with axis above and has value of 3 if this is a leaf. + uint32_t type : 2; + + /// The number of triangles for this leaf node. + uint32_t triangleCount : 30; + } asLeaf; + } data; + + /// The upper bound of the node AABB. Strategic placement for SIMD. + b3Vec3 upperBound; + + /// The index of the leaf triangles. + uint32_t triangleOffset; +} b3MeshNode; + +/// This is a sorted triangle collision bounding volume hierarchy. +/// @note This struct has data hanging off the end and cannot be directly copied. +typedef struct b3MeshData +{ + /// Version must be first. + uint64_t version; + + /// The total number of bytes for this mesh. + int byteCount; + + /// Hash of this mesh (this field is zero when the hash is computed) + uint32_t hash; + + /// Local axis-aligned box. + b3AABB bounds; + + /// Combined surface area of all triangles. Single-sided. + float surfaceArea; + + /// The height of the bounding volume hierarchy. + int treeHeight; + + /// The number of degenerate triangles. Diagnostic. + int degenerateCount; + + /// Offset of the node array in bytes from the struct address. + int nodeOffset; + + /// The number of BVH nodes. + int nodeCount; + + /// Offset of the vertex array in bytes from the struct address. + int vertexOffset; + + /// The number of vertices. + int vertexCount; + + /// Offset of the triangle array in bytes from the struct address. + int triangleOffset; + + /// The number of triangles. + int triangleCount; + + /// Offset of the material array in bytes from the struct address. + int materialOffset; + + /// The number of materials. + int materialCount; + + /// Offset of the triangle flag array in bytes from the struct address. + int flagsOffset; +} b3MeshData; + +/// This allows mesh data to be re-used with different scales. +typedef struct b3Mesh +{ + /// Immutable pointer to the mesh data. + const b3MeshData* data; + + /// This scale may be non-uniform and have negative components. However, + /// no component may be very small in magnitude. + b3Vec3 scale; +} b3Mesh; + +/**@}*/ // mesh + +/** + * @defgroup height_field Height Field + * @brief Height field collision shape + * @{ + */ + +/// Data used to create a height field +typedef struct b3HeightFieldDef +{ + /// Grid point heights + /// count = countX * countZ + float* heights; + + /// Grid cell material + /// A value of 0xFF is reserved for holes + /// count = (countX - 1) * (countZ - 1) + uint8_t* materialIndices; + + /// The height field scale. All components must be positive values. + b3Vec3 scale; + + /// The number of grid lines along the x-axis. + int countX; + + /// The number of grid lines along the z-axis. + int countZ; + + /// Global minimum and maximum heights used for quantization. This is important + /// if you want height fields to be placed next to each other and line up exactly. + /// In that case, both height fields should use the same minimum and maximum heights. + /// All height values are clamped to this range. + /// These values are in unscaled space. + float globalMinimumHeight; + + /// The maximum. + float globalMaximumHeight; + + /// Use clock-wise winding. This effectively inverts the height-field along the y-axis. + bool clockwiseWinding; +} b3HeightFieldDef; + +/// This material index is used to designate holes in a height field. +#define B3_HEIGHT_FIELD_HOLE 0xFF + +/// 64-bit height-field version. Useful for validating serialized data. +#define B3_HEIGHT_FIELD_VERSION 0x8B18CBD138A6BC84ull + +/// A height field with compressed storage. +/// @note This data structure has data hanging off the end and cannot be directly copied. +typedef struct b3HeightFieldData +{ + /// Version must be first and match B3_HEIGHT_FIELD_VERSION + uint64_t version; + + /// The total number of bytes for this height field. + int byteCount; + + /// Hash of this height field (this field is zero when the hash is computed). + uint32_t hash; + + /// The local axis-aligned bounding box. + b3AABB aabb; + + /// The minimum y value. + float minHeight; + + /// The maximum y value + float maxHeight; + + /// The quantization scale. + float heightScale; + + /// The overall scale. + b3Vec3 scale; + + /// The number of grid columns along the local x-axis. + int columnCount; + + /// The number of grid rows along the local z-axis. + int rowCount; + + /// Offset of the compressed height array in bytes from the struct address. + /// uint16_t, one per grid point. + int heightsOffset; + + /// Offset of the material index array in bytes from the struct address. + /// uint8_t, one per cell. + int materialOffset; + + /// Offset of the flag array in bytes from the struct address. + /// uint8_t, one per triangle. + int flagsOffset; + + /// Triangle winding. + bool clockwise; + + /// Explicit padding. Identity is a content hash over raw bytes, so there must + /// be no unnamed padding for struct copies to scramble. + uint8_t padding[3]; +} b3HeightFieldData; + +/**@}*/ // height_field + +/** + * @defgroup compound Compound + * @brief Compound collision shape + * @{ + */ + +/// Definition for a capsule in a compound shape. +typedef struct b3CompoundCapsuleDef +{ + /// Local capsule. + b3Capsule capsule; + + /// Material properties. + b3SurfaceMaterial material; +} b3CompoundCapsuleDef; + +/// Definition for a convex hull in a compound shape. +typedef struct b3CompoundHullDef +{ + /// Shared hull. + const b3HullData* hull; + + /// Transform of the shared hull into compound local space. + b3Transform transform; + + /// Material properties. + b3SurfaceMaterial material; +} b3CompoundHullDef; + +/// Definition for a triangle mesh in a compound shape. +typedef struct b3CompoundMeshDef +{ + /// Shared mesh. + const b3MeshData* meshData; + + /// Transform of the shared mesh into compound local space. + b3Transform transform; + + /// Local space non-uniform mesh scale. May have negative components. + b3Vec3 scale; + + /// Material properties. + /// This array must line up with the material indices on the triangles. + const b3SurfaceMaterial* materials; + + /// Number of materials. + int materialCount; +} b3CompoundMeshDef; + +/// Definition for a sphere in a compound shape. +typedef struct b3CompoundSphereDef +{ + /// Local sphere. + b3Sphere sphere; + + /// Material properties. + b3SurfaceMaterial material; +} b3CompoundSphereDef; + +/// Definition for creating a compound shape. All this data is fully cloned +/// into the run-time compound shape. +typedef struct b3CompoundDef +{ + /// Capsule instances. + b3CompoundCapsuleDef* capsules; + + /// Number of capsules. + int capsuleCount; + + /// Hulls instances. + b3CompoundHullDef* hulls; + + /// Number of hull instances. + int hullCount; + + /// Mesh instances. + b3CompoundMeshDef* meshes; + + /// Number of mesh instances. + int meshCount; + + /// Sphere instances. + b3CompoundSphereDef* spheres; + + /// Number of spheres. + int sphereCount; +} b3CompoundDef; + +/// The compound version depends on the tree, mesh, and hull versions. +#define B3_COMPOUND_VERSION ( 0x830778DB07086EB4ull ^ B3_DYNAMIC_TREE_VERSION ^ B3_MESH_VERSION ^ B3_HULL_VERSION ) + +/// Meshes used in compounds have limited space for materials. If you have +/// a mesh with many materials, you can use it outside of the compound. +#define B3_MAX_COMPOUND_MESH_MATERIALS 4 + +/// The runtime data for a baked compound shape. This is a potentially large yet highly optimized +/// data structure. It can contain thousands of child shapes, yet at runtime it populates +/// into the world as a single shape in the runtime broad-phase. +/// This data structure has data living off the end and must be accessed using offsets. +/// Accessors are provided for user relevant data. +/// Note: you don't need to use this to create runtime compounds. For runtime compounds you can +/// add multiple shapes to a body using the regular shape creation functions. +typedef struct b3CompoundData +{ + /// The compound version is always first. + uint64_t version; + + /// The total number of bytes for this compound. + int byteCount; + + /// Offset of the tree node array in bytes from the struct address. + int nodeOffset; + + /// Immutable dynamic tree. The tree node pointer must be fixed up using the node offset + b3DynamicTree tree; + + /// Offset of the material array in bytes from the struct address. + int materialOffset; + + /// The number of materials. + int materialCount; + + /// Offset of the capsule array in bytes from the struct address. + int capsuleOffset; + + /// The number of capsules. + int capsuleCount; + + /// Offset of the hull instance array in bytes from the struct address. + int hullOffset; + + /// The number of hull instances. + int hullCount; + + /// The number of unique hulls. Diagnostic. + int sharedHullCount; + + /// Offset of the mesh instance array in bytes from the struct address. + int meshOffset; + + /// The number of mesh instances. + int meshCount; + + /// The number of unique meshes. Diagnostic. + int sharedMeshCount; + + /// Offset of the sphere array in bytes from the struct address. + int sphereOffset; + + /// The number of spheres. + int sphereCount; +} b3CompoundData; + +/// A capsule that lives in a compound. +typedef struct b3CompoundCapsule +{ + /// Local capsule. + b3Capsule capsule; + + /// Index to a shared material. + int materialIndex; +} b3CompoundCapsule; + +/// A hull that lives in a compound. +typedef struct b3CompoundHull +{ + /// Pointer to the unique shared hull. + const b3HullData* hull; + + /// The transform of this hull instance. + b3Transform transform; + + /// Index to a shared material. + int materialIndex; +} b3CompoundHull; + +/// A mesh with non-uniform scale that lives in a compound. +typedef struct b3CompoundMesh +{ + /// Pointer to the unique shared mesh. + const b3MeshData* meshData; + + /// The transform of this mesh instance. + b3Transform transform; + + /// Non-uniform scale of this mesh instance. + b3Vec3 scale; + + /// This is used to access the surface material from b3GetCompoundMaterials. + /// Requires an extra level of indirection. The triangle material index + /// is clamped to B3_MAX_COMPOUND_MESH_MATERIALS. + /// materialIndex = materialIndices[triangle->materialIndex] + int materialIndices[B3_MAX_COMPOUND_MESH_MATERIALS]; +} b3CompoundMesh; + +/// A sphere that lives in a compound. +typedef struct b3CompoundSphere +{ + /// Local sphere. + b3Sphere sphere; + + /// Index to a shared material. + int materialIndex; +} b3CompoundSphere; + +/// Child shape of a compound +typedef struct b3ChildShape +{ + /// Tagged union. + union + { + b3Capsule capsule; ///< Capsule. + const b3HullData* hull; ///< Hull. + b3Mesh mesh; ///< Mesh. + b3Sphere sphere; ///< Sphere. + }; + + /// Transform of the shape into compound local space. + b3Transform transform; + + /// Material indices. Index 0 is used for convex shapes. + /// todo limit to 64K? + int materialIndices[B3_MAX_COMPOUND_MESH_MATERIALS]; + + /// The shape type (union tag). + b3ShapeType type; +} b3ChildShape; + +/// Callback for compound overlap queries. +typedef bool b3CompoundQueryFcn( const b3CompoundData* compound, int childIndex, void* context ); + +/**@}*/ // compound + +/**@}*/ // geometry + +/** + * @defgroup collision Shape Collision + * Collide pairs of shapes. + * @{ + */ + +/// A manifold point is a contact point belonging to a contact manifold. +/// It holds details related to the geometry and dynamics of the contact points. +/// Box3D uses speculative collision so some contact points may be separated. +/// You may use the maxNormalImpulse to determine if there was an interaction during +/// the time step. +typedef struct b3ManifoldPoint +{ + /// Location of the contact point relative to the bodyA center of mass in world space. + b3Vec3 anchorA; + + /// Location of the contact point relative to the bodyB center of mass in world space. + b3Vec3 anchorB; + + /// The separation of the contact point, negative if penetrating + float separation; + + /// Cached separation used for contact recycling + float baseSeparation; + + /// The impulse along the manifold normal vector. Since Box3D uses sub-stepping, this is + /// result from the final sub-step. + float normalImpulse; + + /// The total normal impulse applied during sub-stepping. This is important + /// to identify speculative contact points that had an interaction in the time step. + float totalNormalImpulse; + + /// Relative normal velocity pre-solve. Used for hit events. If the normal impulse is + /// zero then there was no hit. Negative means shapes are approaching. + float normalVelocity; + + /// Local point for matching + /// Uniquely identifies a contact point between two shapes + uint32_t featureId; + + /// Triangle index if one of the shapes is a mesh or height field + int triangleIndex; + + /// Did this contact point exist in the previous step? + bool persisted; +} b3ManifoldPoint; + +/// A contact manifold describes the contact points between colliding shapes. +/// @note Box3D uses speculative collision so some contact points may be separated. +typedef struct b3Manifold +{ + /// The manifold points. There may be 1 to 4 valid points. + b3ManifoldPoint points[B3_MAX_MANIFOLD_POINTS]; + + /// The unit normal vector in world space, points from shape A to shape B + b3Vec3 normal; + + /// Central friction angular impulse (applied about the normal) + float twistImpulse; + + /// Central friction linear impulse + b3Vec3 frictionImpulse; + + /// Rolling resistance angular impulse + b3Vec3 rollingImpulse; + + /// The number of contact points, will be 0 to 4 + int pointCount; + +} b3Manifold; + +/// Cached separating axis feature. +typedef enum +{ + b3_invalidAxis = 0, + b3_backsideAxis, + b3_faceAxisA, + b3_faceAxisB, + b3_edgePairAxis, + b3_closestPointsAxis, + + /// These are for testing + b3_manualFaceAxisA, + b3_manualFaceAxisB, + b3_manualEdgePairAxis, +} b3SeparatingFeature; + +/// Cached triangle feature. +typedef enum +{ + b3_featureNone = 0, + b3_featureTriangleFace, + b3_featureHullFace, + /// v1-v2 + b3_featureEdge1, + /// v2-v3 + b3_featureEdge2, + /// v3-v1 + b3_featureEdge3, + b3_featureVertex1, + b3_featureVertex2, + b3_featureVertex3 +} b3TriangleFeature; + +/// Separating axis test cache. Provides temporal acceleration of collision routines. +typedef struct +{ + /// The separation when the cache is populated. Negative for overlap. + float separation; + + /// b3SeparatingFeature. + uint8_t type; + + /// Index of the feature on shape A. + uint8_t indexA; + + /// Index of the feature on shape B. + uint8_t indexB; + + /// Was the cache re-used? + uint8_t hit; +} b3SATCache; + +/// Contact points are always the result of two edges intersecting. +/// It can be two edges of the same shape, which is just a shape vertex. +/// Or a contact point can be the result of two edges crossing from different shapes. +/// This is designed to support hull versus hull, but it is adapted to work +/// with all shape types. The feature pair is used to identify contact points +/// for temporal coherence and warm starting. +typedef struct b3FeaturePair +{ + /// Incoming type (either edge on shape A or shape B) + uint8_t owner1; + /// Incoming edge index (into associated shape array) + uint8_t index1; + /// Outgoing type (either edge on shape A or shape B) + uint8_t owner2; + /// Outgoing edge index (into associated shape array) + uint8_t index2; +} b3FeaturePair; + +/// A local manifold point and normal in frame A. +typedef struct b3LocalManifoldPoint +{ + /// Local point in frame A. + b3Vec3 point; + + /// The contact point separation. Negative for overlap. + float separation; + + /// The feature pair for this point. + b3FeaturePair pair; + + /// The triangle index when collide with a mesh or height-field. + int triangleIndex; +} b3LocalManifoldPoint; + +/// A local manifold with no dynamic information. Used by b3Collide functions. +typedef struct b3LocalManifold +{ + /// Local normal in frame A. + b3Vec3 normal; + + /// The triangle normal. + b3Vec3 triangleNormal; + + /// The manifold points. From a point buffer. + b3LocalManifoldPoint* points; + + /// The number of manifold points. Only bounded by the buffer capacity. + int pointCount; + + /// The index of the triangle. + int triangleIndex; + + int i1; ///< Vertex 1 index. + int i2; ///< Vertex 2 index. + int i3; ///< Vertex 3 index. + + /// The squared distance of a sphere from a triangle. For ghost collision reduction. + float squaredDistance; + + /// The triangle feature involved. + b3TriangleFeature feature; + + /// b3MeshEdgeFlags. + int triangleFlags; +} b3LocalManifold; + +/**@}*/ // collision + +/** + * @defgroup debug_draw Debug Draw + * @{ + */ + +/// These colors are used for debug draw and mostly match the named SVG colors. +/// See https://www.rapidtables.com/web/color/index.html +/// https://johndecember.com/html/spec/colorsvg.html +/// https://upload.wikimedia.org/wikipedia/commons/2/2b/SVG_Recognized_color_keyword_names.svg +typedef enum b3HexColor +{ + b3_colorAliceBlue = 0xF0F8FF, + b3_colorAntiqueWhite = 0xFAEBD7, + b3_colorAqua = 0x00FFFF, + b3_colorAquamarine = 0x7FFFD4, + b3_colorAzure = 0xF0FFFF, + b3_colorBeige = 0xF5F5DC, + b3_colorBisque = 0xFFE4C4, + b3_colorBlack = 0x000000, + b3_colorBlanchedAlmond = 0xFFEBCD, + b3_colorBlue = 0x0000FF, + b3_colorBlueViolet = 0x8A2BE2, + b3_colorBrown = 0xA52A2A, + b3_colorBurlywood = 0xDEB887, + b3_colorCadetBlue = 0x5F9EA0, + b3_colorChartreuse = 0x7FFF00, + b3_colorChocolate = 0xD2691E, + b3_colorCoral = 0xFF7F50, + b3_colorCornflowerBlue = 0x6495ED, + b3_colorCornsilk = 0xFFF8DC, + b3_colorCrimson = 0xDC143C, + b3_colorCyan = 0x00FFFF, + b3_colorDarkBlue = 0x00008B, + b3_colorDarkCyan = 0x008B8B, + b3_colorDarkGoldenRod = 0xB8860B, + b3_colorDarkGray = 0xA9A9A9, + b3_colorDarkGreen = 0x006400, + b3_colorDarkKhaki = 0xBDB76B, + b3_colorDarkMagenta = 0x8B008B, + b3_colorDarkOliveGreen = 0x556B2F, + b3_colorDarkOrange = 0xFF8C00, + b3_colorDarkOrchid = 0x9932CC, + b3_colorDarkRed = 0x8B0000, + b3_colorDarkSalmon = 0xE9967A, + b3_colorDarkSeaGreen = 0x8FBC8F, + b3_colorDarkSlateBlue = 0x483D8B, + b3_colorDarkSlateGray = 0x2F4F4F, + b3_colorDarkTurquoise = 0x00CED1, + b3_colorDarkViolet = 0x9400D3, + b3_colorDeepPink = 0xFF1493, + b3_colorDeepSkyBlue = 0x00BFFF, + b3_colorDimGray = 0x696969, + b3_colorDodgerBlue = 0x1E90FF, + b3_colorFireBrick = 0xB22222, + b3_colorFloralWhite = 0xFFFAF0, + b3_colorForestGreen = 0x228B22, + b3_colorFuchsia = 0xFF00FF, + b3_colorGainsboro = 0xDCDCDC, + b3_colorGhostWhite = 0xF8F8FF, + b3_colorGold = 0xFFD700, + b3_colorGoldenRod = 0xDAA520, + b3_colorGray = 0x808080, + b3_colorGreen = 0x008000, + b3_colorGreenYellow = 0xADFF2F, + b3_colorHoneyDew = 0xF0FFF0, + b3_colorHotPink = 0xFF69B4, + b3_colorIndianRed = 0xCD5C5C, + b3_colorIndigo = 0x4B0082, + b3_colorIvory = 0xFFFFF0, + b3_colorKhaki = 0xF0E68C, + b3_colorLavender = 0xE6E6FA, + b3_colorLavenderBlush = 0xFFF0F5, + b3_colorLawnGreen = 0x7CFC00, + b3_colorLemonChiffon = 0xFFFACD, + b3_colorLightBlue = 0xADD8E6, + b3_colorLightCoral = 0xF08080, + b3_colorLightCyan = 0xE0FFFF, + b3_colorLightGoldenRodYellow = 0xFAFAD2, + b3_colorLightGray = 0xD3D3D3, + b3_colorLightGreen = 0x90EE90, + b3_colorLightPink = 0xFFB6C1, + b3_colorLightSalmon = 0xFFA07A, + b3_colorLightSeaGreen = 0x20B2AA, + b3_colorLightSkyBlue = 0x87CEFA, + b3_colorLightSlateGray = 0x778899, + b3_colorLightSteelBlue = 0xB0C4DE, + b3_colorLightYellow = 0xFFFFE0, + b3_colorLime = 0x00FF00, + b3_colorLimeGreen = 0x32CD32, + b3_colorLinen = 0xFAF0E6, + b3_colorMagenta = 0xFF00FF, + b3_colorMaroon = 0x800000, + b3_colorMediumAquaMarine = 0x66CDAA, + b3_colorMediumBlue = 0x0000CD, + b3_colorMediumOrchid = 0xBA55D3, + b3_colorMediumPurple = 0x9370DB, + b3_colorMediumSeaGreen = 0x3CB371, + b3_colorMediumSlateBlue = 0x7B68EE, + b3_colorMediumSpringGreen = 0x00FA9A, + b3_colorMediumTurquoise = 0x48D1CC, + b3_colorMediumVioletRed = 0xC71585, + b3_colorMidnightBlue = 0x191970, + b3_colorMintCream = 0xF5FFFA, + b3_colorMistyRose = 0xFFE4E1, + b3_colorMoccasin = 0xFFE4B5, + b3_colorNavajoWhite = 0xFFDEAD, + b3_colorNavy = 0x000080, + b3_colorOldLace = 0xFDF5E6, + b3_colorOlive = 0x808000, + b3_colorOliveDrab = 0x6B8E23, + b3_colorOrange = 0xFFA500, + b3_colorOrangeRed = 0xFF4500, + b3_colorOrchid = 0xDA70D6, + b3_colorPaleGoldenRod = 0xEEE8AA, + b3_colorPaleGreen = 0x98FB98, + b3_colorPaleTurquoise = 0xAFEEEE, + b3_colorPaleVioletRed = 0xDB7093, + b3_colorPapayaWhip = 0xFFEFD5, + b3_colorPeachPuff = 0xFFDAB9, + b3_colorPeru = 0xCD853F, + b3_colorPink = 0xFFC0CB, + b3_colorPlum = 0xDDA0DD, + b3_colorPowderBlue = 0xB0E0E6, + b3_colorPurple = 0x800080, + b3_colorRebeccaPurple = 0x663399, + b3_colorRed = 0xFF0000, + b3_colorRosyBrown = 0xBC8F8F, + b3_colorRoyalBlue = 0x4169E1, + b3_colorSaddleBrown = 0x8B4513, + b3_colorSalmon = 0xFA8072, + b3_colorSandyBrown = 0xF4A460, + b3_colorSeaGreen = 0x2E8B57, + b3_colorSeaShell = 0xFFF5EE, + b3_colorSienna = 0xA0522D, + b3_colorSilver = 0xC0C0C0, + b3_colorSkyBlue = 0x87CEEB, + b3_colorSlateBlue = 0x6A5ACD, + b3_colorSlateGray = 0x708090, + b3_colorSnow = 0xFFFAFA, + b3_colorSpringGreen = 0x00FF7F, + b3_colorSteelBlue = 0x4682B4, + b3_colorTan = 0xD2B48C, + b3_colorTeal = 0x008080, + b3_colorThistle = 0xD8BFD8, + b3_colorTomato = 0xFF6347, + b3_colorTurquoise = 0x40E0D0, + b3_colorViolet = 0xEE82EE, + b3_colorWheat = 0xF5DEB3, + b3_colorWhite = 0xFFFFFF, + b3_colorWhiteSmoke = 0xF5F5F5, + b3_colorYellow = 0xFFFF00, + b3_colorYellowGreen = 0x9ACD32, + + b3_colorBox2DRed = 0xDC3132, + b3_colorBox2DBlue = 0x30AEBF, + b3_colorBox2DGreen = 0x8CC924, + b3_colorBox2DYellow = 0xFFEE8C +} b3HexColor; + +/// Debug draw material preset. Optionally packed into the unused high byte of a +/// b3HexColor (or b3SurfaceMaterial::customColor) to drive the renderer's PBR +/// roughness and metalness. The low 24 bits stay RGB, so a plain 0xRRGGBB color +/// reads as b3_debugMaterialDefault and keeps the renderer's per-body-type look. +typedef enum b3DebugMaterial +{ + b3_debugMaterialDefault = 0, + b3_debugMaterialMatte, + b3_debugMaterialSoft, + b3_debugMaterialDead, + b3_debugMaterialGlossy, + b3_debugMaterialMetallic +} b3DebugMaterial; + +/// Pack an RGB color with a material preset for debug draw. The preset rides in +/// the high byte where the color converters ignore it. +B3_INLINE uint32_t b3MakeDebugColor( b3HexColor rgb, b3DebugMaterial material ) +{ + return ( (uint32_t)rgb & 0x00FFFFFFu ) | ( (uint32_t)material << 24 ); +} + +/// Get the visualization color assigned to a constraint graph color slot. The last index +/// (B3_GRAPH_COLOR_COUNT - 1) is the overflow color. +B3_API b3HexColor b3GetGraphColor( int index ); + +/// This is sent to the user for debug shape creation. The user should know the type in case they have +/// custom sphere or capsule rendering. +typedef struct b3DebugShape +{ + /// Shape id. + b3ShapeId shapeId; + + /// Shape type. + b3ShapeType type; + + /// Tagged union. + union + { + const b3Capsule* capsule; ///< Capsule shape. + const b3CompoundData* compound; ///< Compound shape. + const b3HeightFieldData* heightField; ///< Height-field shape. + const b3HullData* hull; ///< Convex hull shape. + const b3Mesh* mesh; ///< Mesh shape with scale. + const b3Sphere* sphere; ///< Sphere shape. + }; +} b3DebugShape; + +/// This struct is passed to b3World_Draw to draw a debug view of the simulation world. +/// Callbacks receive world coordinates. In large world mode the translation is double precision so +/// it stays accurate far from the origin. Shift into your own camera frame inside the callbacks. +typedef struct b3DebugDraw +{ + /// Draws a shape and returns true if drawing should continue + bool ( *DrawShapeFcn )( void* userShape, b3WorldTransform transform, b3HexColor color, void* context ); + + /// Draw a line segment. + void ( *DrawSegmentFcn )( b3Pos p1, b3Pos p2, b3HexColor color, void* context ); + + /// Draw a transform. Choose your own length scale. + void ( *DrawTransformFcn )( b3WorldTransform transform, void* context ); + + /// Draw a point. + void ( *DrawPointFcn )( b3Pos p, float size, b3HexColor color, void* context ); + + /// Draw a sphere. + void ( *DrawSphereFcn )( b3Pos p, float radius, b3HexColor color, float alpha, void* context ); + + /// Draw a capsule. + void ( *DrawCapsuleFcn )( b3Pos p1, b3Pos p2, float radius, b3HexColor color, float alpha, void* context ); + + /// Draw a bounding box. + void ( *DrawBoundsFcn )( b3AABB aabb, b3HexColor color, void* context ); + + /// Draw an oriented box. + void ( *DrawBoxFcn )( b3Vec3 extents, b3WorldTransform transform, b3HexColor color, void* context ); + + /// Draw a string in world space + void ( *DrawStringFcn )( b3Pos p, const char* s, b3HexColor color, void* context ); + + /// World bounds to use for debug draw + b3AABB drawingBounds; + + /// Scale to use when drawing forces + float forceScale; + + /// Global scaling for joint drawing + float jointScale; + + /// Option to draw shapes + bool drawShapes; + + /// Option to draw joints + bool drawJoints; + + /// Option to draw additional information for joints + bool drawJointExtras; + + /// Option to draw the bounding boxes for shapes + bool drawBounds; + + /// Option to draw the mass and center of mass of dynamic bodies + bool drawMass; + + /// Option to draw the sleep information for dynamic and kinematic bodies + bool drawSleep; + + /// Option to draw body names + bool drawBodyNames; + + /// Option to draw contact points + bool drawContacts; + + /// Draw contact anchor A or B + int drawAnchorA; + + /// Option to visualize the graph coloring used for contacts and joints + bool drawGraphColors; + + /// Option to draw contact features + bool drawContactFeatures; + + /// Option to draw contact normals + bool drawContactNormals; + + /// Option to draw contact normal forces + bool drawContactForces; + + /// Option to draw contact friction forces + bool drawFrictionForces; + + /// Option to draw islands as bounding boxes + bool drawIslands; + + /// User context that is passed as an argument to drawing callback functions + void* context; +} b3DebugDraw; + +/// Create a debug draw struct with default values. +B3_API b3DebugDraw b3DefaultDebugDraw( void ); + +/**@}*/ // debug_draw diff --git a/vendor/box3d/src/src/CMakeLists.txt b/vendor/box3d/src/src/CMakeLists.txt new file mode 100644 index 000000000..0460f85ba --- /dev/null +++ b/vendor/box3d/src/src/CMakeLists.txt @@ -0,0 +1,256 @@ +include(GNUInstallDirs) + +set(BOX3D_SOURCE_FILES + aabb.c + aabb.h + algorithm.h + arena_allocator.c + arena_allocator.h + bitset.c + bitset.h + block_allocator.c + block_allocator.h + body.c + body.h + broad_phase.c + broad_phase.h + capsule.c + compound.c + compound.h + constraint_graph.c + constraint_graph.h + contact.c + contact.h + contact_solver.c + contact_solver.h + container.h + convex_manifold.c + core.c + core.h + ctz.h + distance.c + distance_joint.c + dynamic_tree.c + height_field.c + hull.c + id_pool.c + id_pool.h + island.c + island.h + joint.c + joint.h + manifold.c + manifold.h + math_functions.c + math_internal.h + mesh.c + mesh_contact.c + motor_joint.c + mover.c + name_cache.c + name_cache.h + parallel_for.c + parallel_for.h + parallel_joint.c + physics_world.c + physics_world.h + platform.h + prismatic_joint.c + qsort.h + recording.c + recording.h + recording_ops.inl + recording_replay.c + recording_replay.h + world_snapshot.c + world_snapshot.h + revolute_joint.c + scheduler.c + scheduler.h + sensor.c + sensor.h + shape.c + shape.h + simd.c + simd.h + solver.c + solver.h + solver_set.c + solver_set.h + sphere.c + spherical_joint.c + table.c + table.h + timer.c + triangle_manifold.c + types.c + verstable.h + weld_joint.c + wheel_joint.c + + box3d.natvis +) + +set(BOX3D_INCLUDE_FILES + ../include/box3d/base.h + ../include/box3d/box3d.h + ../include/box3d/collision.h + ../include/box3d/config.h + ../include/box3d/constants.h + ../include/box3d/id.h + ../include/box3d/math_functions.h + ../include/box3d/types.h +) + +# Hide internal functions +# https://gcc.gnu.org/wiki/Visibility +set(CMAKE_C_VISIBILITY_PRESET hidden) +set(CMAKE_VISIBILITY_INLINES_HIDDEN 1) + +add_library(box3d ${BOX3D_SOURCE_FILES} ${BOX3D_INCLUDE_FILES}) + +# target_link_libraries(box3d PRIVATE eastl) + +target_include_directories(box3d + PUBLIC + $ + $ + $ + PRIVATE + ${CMAKE_CURRENT_SOURCE_DIR} +) + +set(CMAKE_DEBUG_POSTFIX "d") + +# Box3D uses C17 for _Static_assert and anonymous unions +set_target_properties(box3d PROPERTIES + C_STANDARD 17 + C_STANDARD_REQUIRED YES + C_EXTENSIONS YES + VERSION ${PROJECT_VERSION} + SOVERSION ${PROJECT_VERSION_MAJOR} + DEBUG_POSTFIX ${CMAKE_DEBUG_POSTFIX} +) + +if (BOX3D_COMPILE_WARNING_AS_ERROR) + set_target_properties(box3d PROPERTIES COMPILE_WARNING_AS_ERROR ON) +endif() + +if (BOX3D_PROFILE) + target_compile_definitions(box3d PRIVATE BOX3D_PROFILE) + set(TRACY_DELAYED_INIT ON CACHE BOOL "Enable delayed init for Tracy" FORCE) + set(TRACY_MANUAL_LIFETIME ON CACHE BOOL "Enable manual lifetime for Tracy" FORCE) + + FetchContent_Declare( + tracy + GIT_REPOSITORY https://github.com/wolfpld/tracy.git + GIT_TAG v0.13.1 + GIT_SHALLOW TRUE + GIT_PROGRESS TRUE + ) + FetchContent_MakeAvailable(tracy) + + target_link_libraries(box3d PUBLIC TracyClient) +endif() + +if(BOX3D_VALIDATE) + message(STATUS "Box3D validation ON") + target_compile_definitions(box3d PRIVATE BOX3D_VALIDATE) +endif() + +if (BOX3D_DISABLE_SIMD) + message(STATUS "Box3D SIMD disabled") + target_compile_definitions(box3d PRIVATE BOX3D_DISABLE_SIMD) +endif() + +if (BOX3D_DOUBLE_PRECISION) + message(STATUS "Box3D double precision ON") + # PUBLIC: consumers must see the same precision mode in the headers or they cannot match the ABI + target_compile_definitions(box3d PUBLIC BOX3D_DOUBLE_PRECISION) +endif() + +if (MSVC) + message(STATUS "Box3D on MSVC") + if (BUILD_SHARED_LIBS) + # this is needed by DLL users to import Box3D symbols + target_compile_definitions(box3d INTERFACE BOX3D_DLL) + endif() + + # Visual Studio won't load the natvis unless it is in the project + target_sources(box3d PRIVATE box3d.natvis) + + # Enable asserts in release with debug info + target_compile_definitions(box3d PUBLIC "$<$:B3_ENABLE_ASSERT>") + + # Warnings Wall is problematic in mixed C/C++, so using W4 + target_compile_options(box3d PRIVATE /W4) + # target_compile_options(box3d PRIVATE /wd4820 /wd5045 /wd4061 /wd4711 /wd4514 /wd4365 /wd5219 /wd5039) + target_compile_options(box3d PRIVATE /wd4820 /wd5045 /wd4061 /wd4711) + # 4710 - warn about inline functions that are not inlined + target_compile_options(box3d PRIVATE /wd4710 ) + + if (${CMAKE_CXX_COMPILER_ID} STREQUAL "Clang") + target_compile_options(box3d PRIVATE -Wmissing-prototypes) + endif() + +elseif (MINGW) + message(STATUS "Box3D on MinGW") +elseif (APPLE) + message(STATUS "Box3D on Apple") + target_compile_options(box3d PRIVATE -Wmissing-prototypes -Wall -Wextra -pedantic) +elseif (EMSCRIPTEN) + message(STATUS "Box3D on Emscripten") + # SSE2/wasm SIMD flags are set directory-wide in the top-level CMakeLists so the + # tests, which include simd.h, get them too. +elseif (UNIX) + message(STATUS "Box3D using Unix") + target_compile_options(box3d PRIVATE -Wmissing-prototypes -Wall -Wextra -pedantic -Wno-unused-value) + if ("${CMAKE_HOST_SYSTEM_PROCESSOR}" STREQUAL "aarch64") + # raspberry pi + # -mfpu=neon + # target_compile_options(box3d PRIVATE) + else() + endif() +endif() + +source_group(TREE "${CMAKE_CURRENT_SOURCE_DIR}" PREFIX "src" FILES ${BOX3D_SOURCE_FILES}) +source_group(TREE "${CMAKE_CURRENT_SOURCE_DIR}/../include" PREFIX "include" FILES ${BOX3D_INCLUDE_FILES}) + +add_library(box3d::box3d ALIAS box3d) + +# libm is required on many Unix toolchains for sqrtf, etc. +if(UNIX AND NOT APPLE AND NOT EMSCRIPTEN) + target_link_libraries(box3d PUBLIC m) +endif() + +install( + TARGETS box3d + EXPORT box3dConfig + LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} + ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR} + RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR} +) + +install( + EXPORT box3dConfig + NAMESPACE box3d:: + DESTINATION "${CMAKE_INSTALL_LIBDIR}/cmake/box3d" +) + +install( + FILES ${BOX3D_INCLUDE_FILES} + DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/box3d +) + +include(CMakePackageConfigHelpers) + +write_basic_package_version_file( + "${CMAKE_CURRENT_BINARY_DIR}/box3dConfigVersion.cmake" + VERSION ${PROJECT_VERSION} + COMPATIBILITY SameMajorVersion +) + +install( + FILES "${CMAKE_CURRENT_BINARY_DIR}/box3dConfigVersion.cmake" + DESTINATION "${CMAKE_INSTALL_LIBDIR}/cmake/box3d" +) diff --git a/vendor/box3d/src/src/aabb.c b/vendor/box3d/src/src/aabb.c new file mode 100644 index 000000000..6045bd82f --- /dev/null +++ b/vendor/box3d/src/src/aabb.c @@ -0,0 +1,176 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +#include "aabb.h" + +#include "math_internal.h" + +#include "box3d/math_functions.h" + +#include + +// Similar to Real-time Collision Detection, p179. +// todo try +// https://www.scratchapixel.com/lessons/3d-basic-rendering/minimal-ray-tracer-rendering-simple-shapes/ray-box-intersection.html +bool b3RayCastAABB( b3AABB a, b3Vec3 p1, b3Vec3 p2, float* minFraction, float* maxFraction ) +{ + // Ray direction and length + b3Vec3 d = b3Sub( p2, p1 ); + float rayLength = b3Length( d ); + + // Handle degenerate ray + if ( rayLength < FLT_EPSILON ) + { + // Check if point is inside AABB + if ( p1.x >= a.lowerBound.x && p1.x <= a.upperBound.x && p1.y >= a.lowerBound.y && p1.y <= a.upperBound.y && + p1.z >= a.lowerBound.z && p1.z <= a.upperBound.z ) + { + *minFraction = 0.0f; + *maxFraction = 0.0f; + return true; + } + + return false; + } + + b3Vec3 rayDir = b3MulSV( 1.0f / rayLength, d ); + + // Slab method for ray-AABB intersection + float tMin = 0.0f; + float tMax = rayLength; + + // x-axis + { + float rayComponent = rayDir.x; + float rayStart = p1.x; + float boxMin = a.lowerBound.x; + float boxMax = a.upperBound.x; + + if ( b3AbsFloat( rayComponent ) < FLT_EPSILON ) + { + // Ray is parallel to slab, check if ray origin is within slab + if ( rayStart < boxMin || rayStart > boxMax ) + { + return false; + } + } + else + { + // Compute intersection distances + float t1 = ( boxMin - rayStart ) / rayComponent; + float t2 = ( boxMax - rayStart ) / rayComponent; + + // Ensure t1 <= t2 + if ( t1 > t2 ) + { + float temp = t1; + t1 = t2; + t2 = temp; + } + + // Update intersection interval + tMin = b3MaxFloat( tMin, t1 ); + tMax = b3MinFloat( tMax, t2 ); + + // Check for no intersection + if ( tMin > tMax ) + { + return false; + } + } + } + + // y-axis + { + float rayComponent = rayDir.y; + float rayStart = p1.y; + float boxMin = a.lowerBound.y; + float boxMax = a.upperBound.y; + + if ( b3AbsFloat( rayComponent ) < FLT_EPSILON ) + { + // Ray is parallel to slab, check if ray origin is within slab + if ( rayStart < boxMin || rayStart > boxMax ) + { + return false; + } + } + else + { + // Compute intersection distances + float t1 = ( boxMin - rayStart ) / rayComponent; + float t2 = ( boxMax - rayStart ) / rayComponent; + + // Ensure t1 <= t2 + if ( t1 > t2 ) + { + float temp = t1; + t1 = t2; + t2 = temp; + } + + // Update intersection interval + tMin = b3MaxFloat( tMin, t1 ); + tMax = b3MinFloat( tMax, t2 ); + + // Check for no intersection + if ( tMin > tMax ) + { + return false; + } + } + } + + // z-axis + { + float rayComponent = rayDir.z; + float rayStart = p1.z; + float boxMin = a.lowerBound.z; + float boxMax = a.upperBound.z; + + if ( b3AbsFloat( rayComponent ) < FLT_EPSILON ) + { + // Ray is parallel to slab, check if ray origin is within slab + if ( rayStart < boxMin || rayStart > boxMax ) + { + return false; + } + } + else + { + // Compute intersection distances + float t1 = ( boxMin - rayStart ) / rayComponent; + float t2 = ( boxMax - rayStart ) / rayComponent; + + // Ensure t1 <= t2 + if ( t1 > t2 ) + { + float temp = t1; + t1 = t2; + t2 = temp; + } + + // Update intersection interval + tMin = b3MaxFloat( tMin, t1 ); + tMax = b3MinFloat( tMax, t2 ); + + // Check for no intersection + if ( tMin > tMax ) + { + return false; + } + } + } + + // Check if intersection is behind ray start + if ( tMax < 0.0f ) + { + return false; + } + + // Convert distances to fractions + *minFraction = b3ClampFloat( tMin / rayLength, 0.0f, 1.0f ); + *maxFraction = b3ClampFloat( tMax / rayLength, 0.0f, 1.0f ); + + return true; +} diff --git a/vendor/box3d/src/src/aabb.h b/vendor/box3d/src/src/aabb.h new file mode 100644 index 000000000..97a00b189 --- /dev/null +++ b/vendor/box3d/src/src/aabb.h @@ -0,0 +1,80 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +#pragma once + +#include "box3d/types.h" + +// Ray cast an AABB. This is a custom function used by height fields. +bool b3RayCastAABB( b3AABB a, b3Vec3 p1, b3Vec3 p2, float* minFraction, float* maxFraction ); + +// Get the surface area (perimeter) +static inline float b3Perimeter( b3AABB a ) +{ + float wx = a.upperBound.x - a.lowerBound.x; + float wy = a.upperBound.y - a.lowerBound.y; + float wz = a.upperBound.z - a.lowerBound.z; + return 2.0f * ( wx * wz + wy * wx + wz * wy ); +} + +/// Enlarge a to contain b +/// @return true if the AABB grew +static inline bool b3EnlargeAABB( b3AABB* a, b3AABB b ) +{ + bool changed = false; + if ( b.lowerBound.x < a->lowerBound.x ) + { + a->lowerBound.x = b.lowerBound.x; + changed = true; + } + + if ( b.lowerBound.y < a->lowerBound.y ) + { + a->lowerBound.y = b.lowerBound.y; + changed = true; + } + + if ( b.lowerBound.z < a->lowerBound.z ) + { + a->lowerBound.z = b.lowerBound.z; + changed = true; + } + + if ( a->upperBound.x < b.upperBound.x ) + { + a->upperBound.x = b.upperBound.x; + changed = true; + } + + if ( a->upperBound.y < b.upperBound.y ) + { + a->upperBound.y = b.upperBound.y; + changed = true; + } + + if ( a->upperBound.z < b.upperBound.z ) + { + a->upperBound.z = b.upperBound.z; + changed = true; + } + + return changed; +} + +#if 0 +/// Do a and b overlap +inline bool b3OverlapAABBs( b3AABB a, b3AABB b ) +{ + return !( b.lowerBound.x > a.upperBound.x || b.lowerBound.y > a.upperBound.y || b.lowerBound.z > a.upperBound.z || + a.lowerBound.x > b.upperBound.x || a.lowerBound.y > b.upperBound.y || a.lowerBound.z > b.upperBound.z ); +} +#endif + +static inline b3Vec3 b3FarthestPointOnAABB( b3AABB b, b3Vec3 p ) +{ + return (b3Vec3){ + .x = ( p.x - b.lowerBound.x ) > ( b.upperBound.x - p.x ) ? b.lowerBound.x : b.upperBound.x, + .y = ( p.y - b.lowerBound.y ) > ( b.upperBound.y - p.y ) ? b.lowerBound.y : b.upperBound.y, + .z = ( p.z - b.lowerBound.z ) > ( b.upperBound.z - p.z ) ? b.lowerBound.z : b.upperBound.z, + }; +} diff --git a/vendor/box3d/src/src/algorithm.h b/vendor/box3d/src/src/algorithm.h new file mode 100644 index 000000000..155bb310d --- /dev/null +++ b/vendor/box3d/src/src/algorithm.h @@ -0,0 +1,19 @@ +// SPDX-FileCopyrightText: 2026 Erin Catto +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +// Swap two same-size lvalues through a byte buffer. Avoids __typeof__ so it builds on any C compiler. +#define B3_SWAP( x, y ) \ + do \ + { \ + _Static_assert( sizeof( x ) == sizeof( y ), "size mismatch" ); \ + char B3_SWAP_TEMP[sizeof( x )]; \ + memcpy( B3_SWAP_TEMP, &( x ), sizeof( x ) ); \ + memcpy( &( x ), &( y ), sizeof( x ) ); \ + memcpy( &( y ), B3_SWAP_TEMP, sizeof( x ) ); \ + } \ + while ( 0 ) diff --git a/vendor/box3d/src/src/arena_allocator.c b/vendor/box3d/src/src/arena_allocator.c new file mode 100644 index 000000000..8543e5782 --- /dev/null +++ b/vendor/box3d/src/src/arena_allocator.c @@ -0,0 +1,178 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +#include "arena_allocator.h" + +#include "core.h" + +#include +#include + +b3Stack b3CreateStack( int capacity ) +{ + B3_ASSERT( capacity >= 0 ); + b3Stack stack = { 0 }; + stack.capacity = capacity; + stack.memory = (char*)b3Alloc( capacity ); + return stack; +} + +void b3DestroyStack( b3Stack* stack ) +{ + b3Free( stack->memory, stack->capacity ); +} + +void* b3StackAlloc( b3Stack* stack, int size, const char* name ) +{ + if ( stack->entryCount == B3_MAX_STACK_ENTRIES ) + { + B3_ASSERT( false ); + return NULL; + } + + int alignedSize = ( ( size - 1 ) | ( B3_ALIGNMENT - 1 ) ) + 1; + + b3StackEntry entry; + entry.size = alignedSize; + entry.name = name; + if ( stack->index + alignedSize > stack->capacity ) + { + // fall back to the heap (undesirable) + entry.data = (char*)b3Alloc( alignedSize ); + entry.usedMalloc = true; + + B3_ASSERT( ( (uintptr_t)entry.data & ( B3_ALIGNMENT - 1 ) ) == 0 ); + } + else + { + entry.data = stack->memory + stack->index; + entry.usedMalloc = false; + stack->index += alignedSize; + + B3_ASSERT( ( (uintptr_t)entry.data & ( B3_ALIGNMENT - 1 ) ) == 0 ); + } + + stack->allocation += alignedSize; + if ( stack->allocation > stack->maxAllocation ) + { + stack->maxAllocation = stack->allocation; + } + + stack->entries[stack->entryCount] = entry; + stack->entryCount += 1; + return entry.data; +} + +void b3StackFree( b3Stack* stack, void* mem ) +{ + int entryCount = stack->entryCount; + B3_ASSERT( entryCount > 0 ); + b3StackEntry* entry = stack->entries + ( entryCount - 1 ); + B3_ASSERT( mem == entry->data ); + if ( entry->usedMalloc ) + { + b3Free( mem, entry->size ); + } + else + { + stack->index -= entry->size; + } + stack->allocation -= entry->size; + stack->entryCount -= 1; +} + +void b3GrowStack( b3Stack* stack ) +{ + // Stack must not be in use + B3_ASSERT( stack->allocation == 0 ); + + if ( stack->maxAllocation > stack->capacity ) + { + b3Free( stack->memory, stack->capacity ); + stack->capacity = stack->maxAllocation + stack->maxAllocation / 2; + stack->memory = (char*)b3Alloc( stack->capacity ); + } +} + +int b3GetStackCapacity( b3Stack* stack ) +{ + return stack->capacity; +} + +int b3GetStackAllocation( b3Stack* stack ) +{ + return stack->allocation; +} + +int b3GetMaxStackAllocation( b3Stack* stack ) +{ + return stack->maxAllocation; +} + +b3Arena b3CreateArena( int capacity ) +{ + int c = capacity > 8 ? capacity : 8; + b3ArenaSharedState* shared = (b3ArenaSharedState*)b3Alloc( sizeof( b3ArenaSharedState ) ); + *shared = (b3ArenaSharedState){ 0 }; + + return (b3Arena){ + .memory = (char*)b3Alloc( c ), + .capacity = c, + .index = 0, + .shared = shared, + }; +} + +void b3DestroyArena( b3Arena* arena ) +{ + b3ArenaSharedState* shared = arena->shared; + if ( shared != NULL ) + { + for ( int i = 0; i < shared->overflows.count; ++i ) + { + b3Free( shared->overflows.data[i].data, shared->overflows.data[i].size ); + } + b3Array_Destroy( shared->overflows ); + b3Free( shared, sizeof( b3ArenaSharedState ) ); + } + b3Free( arena->memory, arena->capacity ); + *arena = (b3Arena){ 0 }; +} + +void* b3ArenaOverflowAlloc( b3Arena* arena, int size ) +{ + b3ArenaSharedState* shared = arena->shared; + char* data = (char*)b3Alloc( size ); + b3OverflowBlock block = { data, size }; + b3Array_Push( shared->overflows, block ); + shared->overflowBytes += size; + return data; +} + +void b3ArenaSync( b3Arena* arena ) +{ + b3ArenaSharedState* shared = arena->shared; + + for ( int i = 0; i < shared->overflows.count; ++i ) + { + b3Free( shared->overflows.data[i].data, shared->overflows.data[i].size ); + } + b3Array_Clear( shared->overflows ); + + int demand = shared->maxIndex + shared->overflowBytes; + if ( demand > shared->peakDemand ) + { + shared->peakDemand = demand; + } + if ( demand > arena->capacity ) + { + b3Free( arena->memory, arena->capacity ); + int newCapacity = demand + demand / 2; + arena->memory = (char*)b3Alloc( newCapacity ); + arena->capacity = newCapacity; + } + + arena->index = 0; + shared->maxIndex = 0; + shared->overflowBytes = 0; +} diff --git a/vendor/box3d/src/src/arena_allocator.h b/vendor/box3d/src/src/arena_allocator.h new file mode 100644 index 000000000..41422b91b --- /dev/null +++ b/vendor/box3d/src/src/arena_allocator.h @@ -0,0 +1,114 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +#pragma once +#include "box3d/base.h" +#include "container.h" + +#include +#include + +#define B3_MAX_STACK_ENTRIES 32 + +typedef struct b3StackEntry +{ + char* data; + const char* name; + int size; + bool usedMalloc; +} b3StackEntry; + +// This is a stack-like arena allocator used for fast per step allocations. +// You must nest allocate/free pairs. The code will B3_ASSERT +// if you try to interleave multiple allocate/free pairs. +// This allocator uses the heap if space is insufficient. +// I could remove the need to free entries individually. +typedef struct b3Stack +{ + char* memory; + int capacity; + int index; + + int allocation; + int maxAllocation; + + b3StackEntry entries[B3_MAX_STACK_ENTRIES]; + int entryCount; +} b3Stack; + +// Heap-allocated fallback block tracked when an arena bump overflows. +typedef struct b3OverflowBlock +{ + char* data; + int size; +} b3OverflowBlock; + +b3DeclareArray( b3OverflowBlock ); + +// Shared, heap-allocated state co-owned by every copy of a b3Arena. +// b3Arena is passed by value so its bump pointer auto-restores on +// function return, but overflow tracking and watermarks must persist +// across copies -- hence this pointer-shared block. +typedef struct b3ArenaSharedState +{ + b3Array( b3OverflowBlock ) overflows; + int maxIndex; // high water mark of the bump pointer this step + int overflowBytes; // total bytes in overflow blocks this step + int peakDemand; // all-time peak of (maxIndex + overflowBytes), survives sync +} b3ArenaSharedState; + +typedef struct b3Arena +{ + char* memory; + int capacity; + int index; + b3ArenaSharedState* shared; +} b3Arena; + +// 16-byte alignment for SSE2 + typical struct alignment. +#define B3_ARENA_ALIGNMENT 16 + +b3Stack b3CreateStack( int capacity ); +void b3DestroyStack( b3Stack* stack ); + +void* b3StackAlloc( b3Stack* stack, int size, const char* name ); +void b3StackFree( b3Stack* stack, void* mem ); + +// Grow the stack based on usage +void b3GrowStack( b3Stack* stack ); + +int b3GetStackCapacity( b3Stack* stack ); +int b3GetStackAllocation( b3Stack* stack ); +int b3GetMaxStackAllocation( b3Stack* stack ); + +b3Arena b3CreateArena( int capacity ); +void b3DestroyArena( b3Arena* arena ); + +// Heap-allocate an overflow block, register it in the shared state, return it. +void* b3ArenaOverflowAlloc( b3Arena* arena, int size ); + +// Call between simulation steps. Frees this step's overflow blocks and grows +// the backing capacity if last step's demand (maxIndex + overflowBytes) exceeded it. +void b3ArenaSync( b3Arena* arena ); + +static inline void* b3Bump( b3Arena* arena, int size ) +{ + if ( size == 0 ) + { + return NULL; + } + + int aligned = ( arena->index + ( B3_ARENA_ALIGNMENT - 1 ) ) & ~( B3_ARENA_ALIGNMENT - 1 ); + + if ( aligned + size > arena->capacity ) + { + return b3ArenaOverflowAlloc( arena, size ); + } + + arena->index = aligned + size; + if ( arena->index > arena->shared->maxIndex ) + { + arena->shared->maxIndex = arena->index; + } + return arena->memory + aligned; +} diff --git a/vendor/box3d/src/src/bitset.c b/vendor/box3d/src/src/bitset.c new file mode 100644 index 000000000..319725bfc --- /dev/null +++ b/vendor/box3d/src/src/bitset.c @@ -0,0 +1,66 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +#include "bitset.h" + +#include + +b3BitSet b3CreateBitSet( uint32_t bitCapacity ) +{ + b3BitSet bitSet = { 0 }; + bitSet.blockCapacity = ( bitCapacity + sizeof( uint64_t ) * 8 - 1 ) / ( sizeof( uint64_t ) * 8 ); + bitSet.blockCount = 0; + bitSet.bits = (uint64_t*)b3Alloc( bitSet.blockCapacity * sizeof( uint64_t ) ); + memset( bitSet.bits, 0, bitSet.blockCapacity * sizeof( uint64_t ) ); + return bitSet; +} + +void b3DestroyBitSet( b3BitSet* bitSet ) +{ + b3Free( bitSet->bits, bitSet->blockCapacity * sizeof( uint64_t ) ); + bitSet->blockCapacity = 0; + bitSet->blockCount = 0; + bitSet->bits = NULL; +} + +void b3SetBitCountAndClear( b3BitSet* bitSet, uint32_t bitCount ) +{ + uint32_t blockCount = ( bitCount + sizeof( uint64_t ) * 8 - 1 ) / ( sizeof( uint64_t ) * 8 ); + if ( bitSet->blockCapacity < blockCount ) + { + b3DestroyBitSet( bitSet ); + uint32_t newBitCapacity = bitCount + ( bitCount >> 1 ); + *bitSet = b3CreateBitSet( newBitCapacity ); + } + + bitSet->blockCount = blockCount; + memset( bitSet->bits, 0, bitSet->blockCount * sizeof( uint64_t ) ); +} + +void b3GrowBitSet( b3BitSet* bitSet, uint32_t blockCount ) +{ + B3_ASSERT( blockCount > bitSet->blockCount ); + if ( blockCount > bitSet->blockCapacity ) + { + uint32_t oldCapacity = bitSet->blockCapacity; + bitSet->blockCapacity = blockCount + blockCount / 2; + uint64_t* newBits = (uint64_t*)b3Alloc( bitSet->blockCapacity * sizeof( uint64_t ) ); + memset( newBits, 0, bitSet->blockCapacity * sizeof( uint64_t ) ); + B3_ASSERT( bitSet->bits != NULL ); + memcpy( newBits, bitSet->bits, oldCapacity * sizeof( uint64_t ) ); + b3Free( bitSet->bits, oldCapacity * sizeof( uint64_t ) ); + bitSet->bits = newBits; + } + + bitSet->blockCount = blockCount; +} + +void b3InPlaceUnion( b3BitSet* __restrict setA, const b3BitSet* __restrict setB ) +{ + B3_ASSERT( setA->blockCount == setB->blockCount ); + uint32_t blockCount = setA->blockCount; + for ( uint32_t i = 0; i < blockCount; ++i ) + { + setA->bits[i] |= setB->bits[i]; + } +} diff --git a/vendor/box3d/src/src/bitset.h b/vendor/box3d/src/src/bitset.h new file mode 100644 index 000000000..ff771d967 --- /dev/null +++ b/vendor/box3d/src/src/bitset.h @@ -0,0 +1,67 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +#pragma once + +#include "core.h" + +#include +#include + +// Bit set provides fast operations on large arrays of bits. +typedef struct b3BitSet +{ + uint64_t* bits; + uint32_t blockCapacity; + uint32_t blockCount; +} b3BitSet; + + +b3BitSet b3CreateBitSet( uint32_t bitCapacity ); +void b3DestroyBitSet( b3BitSet* bitSet ); +void b3SetBitCountAndClear( b3BitSet* bitSet, uint32_t bitCount ); +void b3InPlaceUnion( b3BitSet* setA, const b3BitSet* setB ); +void b3GrowBitSet( b3BitSet* bitSet, uint32_t blockCount ); +int b3CountSetBits( b3BitSet* bitSet ); + +static inline void b3SetBit( b3BitSet* bitSet, uint32_t bitIndex ) +{ + uint32_t blockIndex = bitIndex / 64; + B3_ASSERT( blockIndex < bitSet->blockCount ); + bitSet->bits[blockIndex] |= ( (uint64_t)1 << bitIndex % 64 ); +} + +static inline void b3SetBitGrow( b3BitSet* bitSet, uint32_t bitIndex ) +{ + uint32_t blockIndex = bitIndex / 64; + if ( blockIndex >= bitSet->blockCount ) + { + b3GrowBitSet( bitSet, blockIndex + 1 ); + } + bitSet->bits[blockIndex] |= ( (uint64_t)1 << bitIndex % 64 ); +} + +static inline void b3ClearBit( b3BitSet* bitSet, uint32_t bitIndex ) +{ + uint32_t blockIndex = bitIndex / 64; + if ( blockIndex >= bitSet->blockCount ) + { + return; + } + bitSet->bits[blockIndex] &= ~( (uint64_t)1 << bitIndex % 64 ); +} + +static inline bool b3GetBit( const b3BitSet* bitSet, uint32_t bitIndex ) +{ + uint32_t blockIndex = bitIndex / 64; + if ( blockIndex >= bitSet->blockCount ) + { + return false; + } + return ( bitSet->bits[blockIndex] & ( (uint64_t)1 << bitIndex % 64 ) ) != 0; +} + +static inline int b3GetBitSetBytes( b3BitSet* bitSet ) +{ + return bitSet->blockCapacity * sizeof( uint64_t ); +} diff --git a/vendor/box3d/src/src/block_allocator.c b/vendor/box3d/src/src/block_allocator.c new file mode 100644 index 000000000..4c1f1544e --- /dev/null +++ b/vendor/box3d/src/src/block_allocator.c @@ -0,0 +1,90 @@ +// SPDX-FileCopyrightText: 2026 Erin Catto +// SPDX-License-Identifier: MIT + +#include "block_allocator.h" + +#include "core.h" + +b3BlockAllocator b3CreateBlockAllocator( int elementSize, int initialCount ) +{ + B3_ASSERT( elementSize >= (int)sizeof( void* ) ); + + b3BlockAllocator allocator = { 0 }; + + b3Array_Create( allocator.blocks ); + allocator.elementSize = elementSize; + allocator.freeList = NULL; + allocator.nextIndex = 0; + allocator.allocationCount = 0; + + if ( initialCount > 0 ) + { + int count = ( initialCount + B3_BLOCK_SIZE - 1 ) >> B3_BLOCK_EXPONENT; + b3Array_Resize( allocator.blocks, count ); + + for ( int i = 0; i < allocator.blocks.count; ++i ) + { + allocator.blocks.data[i].memory = b3Alloc( B3_BLOCK_SIZE * elementSize ); + } + } + + return allocator; +} + +void b3DestroyBlockAllocator( b3BlockAllocator* allocator ) +{ + for ( int i = 0; i < allocator->blocks.count; ++i ) + { + b3Free( allocator->blocks.data[i].memory, B3_BLOCK_SIZE * allocator->elementSize ); + } + + b3Array_Destroy( allocator->blocks ); +} + +void* b3AllocateElement( b3BlockAllocator* allocator ) +{ + B3_ASSERT( allocator != NULL ); + + allocator->allocationCount += 1; + + // Pop from free list first + if ( allocator->freeList != NULL ) + { + void* element = allocator->freeList; + allocator->freeList = *(void**)element; + return element; + } + + int index = allocator->nextIndex++; + + int requiredBlockCount = ( index >> B3_BLOCK_EXPONENT ) + 1; + + if ( requiredBlockCount > allocator->blocks.count ) + { + int oldCount = allocator->blocks.count; + b3Array_Resize( allocator->blocks, requiredBlockCount ); + + for ( int i = oldCount; i < requiredBlockCount; ++i ) + { + allocator->blocks.data[i].memory = b3Alloc( B3_BLOCK_SIZE * allocator->elementSize ); + } + } + + int blockIndex = index >> B3_BLOCK_EXPONENT; + int blockOffset = index & ( B3_BLOCK_SIZE - 1 ); + + return allocator->blocks.data[blockIndex].memory + blockOffset * allocator->elementSize; +} + +void b3FreeElement( b3BlockAllocator* allocator, void* element ) +{ + B3_ASSERT( allocator != NULL ); + B3_ASSERT( element != NULL ); + B3_ASSERT( allocator->allocationCount > 0 ); + + allocator->allocationCount -= 1; + + // Push onto free list + *(void**)element = allocator->freeList; + allocator->freeList = element; +} diff --git a/vendor/box3d/src/src/block_allocator.h b/vendor/box3d/src/src/block_allocator.h new file mode 100644 index 000000000..e9922d012 --- /dev/null +++ b/vendor/box3d/src/src/block_allocator.h @@ -0,0 +1,35 @@ +// SPDX-FileCopyrightText: 2026 Erin Catto +// SPDX-License-Identifier: MIT + +#pragma once + +#include "container.h" + +#include "box3d/base.h" + +#define B3_BLOCK_EXPONENT 8 +#define B3_BLOCK_SIZE ( 1 << B3_BLOCK_EXPONENT ) + +typedef struct b3Block +{ + char* memory; +} b3Block; + +b3DeclareArray( b3Block ); + +typedef struct b3BlockAllocator +{ + b3Array( b3Block ) blocks; + void* freeList; + int elementSize; + int nextIndex; + int allocationCount; +} b3BlockAllocator; + +// Element must be large enough to hold a pointer +b3BlockAllocator b3CreateBlockAllocator( int elementSize, int initialCount ); +void b3DestroyBlockAllocator( b3BlockAllocator* allocator ); + +// Returns one element of elementSize contiguous bytes. Address is stable until freed. +void* b3AllocateElement( b3BlockAllocator* allocator ); +void b3FreeElement( b3BlockAllocator* allocator, void* element ); diff --git a/vendor/box3d/src/src/body.c b/vendor/box3d/src/src/body.c new file mode 100644 index 000000000..b78642c6c --- /dev/null +++ b/vendor/box3d/src/src/body.c @@ -0,0 +1,2454 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +#include "body.h" + +#include "aabb.h" +#include "contact.h" +#include "core.h" +#include "id_pool.h" +#include "island.h" +#include "joint.h" +#include "physics_world.h" +#include "recording.h" +#include "sensor.h" +#include "shape.h" +#include "solver_set.h" + +// needed for dll export +#include "box3d/box3d.h" +#include "box3d/id.h" + +#include + +// Get a validated body from a world using an id. +b3Body* b3GetBodyFullId( b3World* world, b3BodyId bodyId ) +{ + B3_ASSERT( b3Body_IsValid( bodyId ) ); + + // id index starts at one so that zero can represent null + // id index starts at one so that zero can represent null + return b3Array_Get( world->bodies, bodyId.index1 - 1 ); +} + +b3WorldTransform b3GetBodyTransformQuick( b3World* world, b3Body* body ) +{ + b3SolverSet* set = b3Array_Get( world->solverSets, body->setIndex ); + b3BodySim* bodySim = b3Array_Get( set->bodySims, body->localIndex ); + return bodySim->transform; +} + +b3WorldTransform b3GetBodyTransform( b3World* world, int bodyId ) +{ + b3Body* body = b3Array_Get( world->bodies, bodyId ); + return b3GetBodyTransformQuick( world, body ); +} + +// Create a b3BodyId from a raw id. +b3BodyId b3MakeBodyId( b3World* world, int bodyId ) +{ + b3Body* body = b3Array_Get( world->bodies, bodyId ); + return (b3BodyId){ bodyId + 1, world->worldId, body->generation }; +} + +b3BodySim* b3GetBodySim( b3World* world, b3Body* body ) +{ + b3SolverSet* set = b3Array_Get( world->solverSets, body->setIndex ); + b3BodySim* bodySim = b3Array_Get( set->bodySims, body->localIndex ); + return bodySim; +} + +b3BodyState* b3GetBodyState( b3World* world, b3Body* body ) +{ + if ( body->setIndex == b3_awakeSet ) + { + b3SolverSet* set = b3Array_Get( world->solverSets, b3_awakeSet ); + return b3Array_Get( set->bodyStates, body->localIndex ); + } + + return NULL; +} + +void b3SyncBodyFlags( b3World* world, b3Body* body ) +{ + // Never sync transient flags + uint32_t flags = body->flags & ~b3_bodyTransientFlags; + + b3BodySim* bodySim = b3GetBodySim( world, body ); + bodySim->flags = flags; + + b3BodyState* bodyState = b3GetBodyState( world, body ); + if ( bodyState != NULL ) + { + bodyState->flags = flags; + } +} + +static void b3CreateIslandForBody( b3World* world, int setIndex, b3Body* body ) +{ + B3_ASSERT( body->islandId == B3_NULL_INDEX ); + B3_ASSERT( setIndex != b3_disabledSet ); + + b3Island* island = b3CreateIsland( world, setIndex ); + b3Array_Push( island->bodies, body->id ); + body->islandId = island->islandId; + body->islandIndex = 0; + + b3ValidateIsland( world, island->islandId ); +} + +static void b3RemoveBodyFromIsland( b3World* world, b3Body* body ) +{ + if ( body->islandId == B3_NULL_INDEX ) + { + B3_ASSERT( body->islandIndex == B3_NULL_INDEX ); + return; + } + + int islandId = body->islandId; + b3Island* island = b3Array_Get( world->islands, islandId ); + { + int localIndex = body->islandIndex; + int movedBodyId = island->bodies.data[island->bodies.count - 1]; + island->bodies.data[localIndex] = movedBodyId; + B3_VALIDATE( world->bodies.data[movedBodyId].islandIndex == island->bodies.count - 1 ); + world->bodies.data[movedBodyId].islandIndex = localIndex; + island->bodies.count -= 1; + } + + if ( island->bodies.count == 0 ) + { + // Destroy empty island + B3_ASSERT( island->contacts.count == 0 ); + B3_ASSERT( island->joints.count == 0 ); + + // Free the island + b3DestroyIsland( world, island->islandId ); + } + else + { + b3ValidateIsland( world, islandId ); + } + + body->islandId = B3_NULL_INDEX; + body->islandIndex = B3_NULL_INDEX; +} + +static void b3DestroyBodyContacts( b3World* world, b3Body* body, bool wakeBodies ) +{ + // Destroy the attached contacts + int edgeKey = body->headContactKey; + while ( edgeKey != B3_NULL_INDEX ) + { + int contactId = edgeKey >> 1; + int edgeIndex = edgeKey & 1; + + b3Contact* contact = b3Array_Get( world->contacts, contactId ); + edgeKey = contact->edges[edgeIndex].nextKey; + b3DestroyContact( world, contact, wakeBodies ); + } + + b3ValidateSolverSets( world ); +} + +b3BodyId b3CreateBody( b3WorldId worldId, const b3BodyDef* def ) +{ + B3_CHECK_DEF( def ); + B3_ASSERT( b3IsValidPosition( def->position ) ); + B3_ASSERT( b3IsValidQuat( def->rotation ) ); + B3_ASSERT( b3IsValidVec3( def->linearVelocity ) ); + B3_ASSERT( b3IsValidVec3( def->angularVelocity ) ); + B3_ASSERT( b3IsValidFloat( def->linearDamping ) && def->linearDamping >= 0.0f ); + B3_ASSERT( b3IsValidFloat( def->angularDamping ) && def->angularDamping >= 0.0f ); + B3_ASSERT( b3IsValidFloat( def->sleepThreshold ) && def->sleepThreshold >= 0.0f ); + B3_ASSERT( b3IsValidFloat( def->gravityScale ) ); + + b3World* world = b3GetUnlockedWorldFromId( worldId ); + + if ( world == NULL ) + { + return b3_nullBodyId; + } + + world->locked = true; + + bool isAwake = ( def->isAwake || def->enableSleep == false ) && def->isEnabled; + + // determine the solver set + int setId; + if ( def->isEnabled == false ) + { + // any body type can be disabled + setId = b3_disabledSet; + } + else if ( def->type == b3_staticBody ) + { + setId = b3_staticSet; + } + else if ( isAwake == true ) + { + setId = b3_awakeSet; + } + else + { + // new set for a sleeping body in its own island + setId = b3AllocId( &world->solverSetIdPool ); + if ( setId == world->solverSets.count ) + { + // Create a zero initialized solver set. All sub-arrays are also zero initialized. + b3Array_Push( world->solverSets, (b3SolverSet){ 0 } ); + } + else + { + B3_ASSERT( world->solverSets.data[setId].setIndex == B3_NULL_INDEX ); + } + + world->solverSets.data[setId].setIndex = setId; + } + + B3_ASSERT( 0 <= setId && setId < world->solverSets.count ); + + int bodyId = b3AllocId( &world->bodyIdPool ); + + uint32_t lockFlags = 0; + lockFlags |= def->motionLocks.linearX ? b3_lockLinearX : 0; + lockFlags |= def->motionLocks.linearY ? b3_lockLinearY : 0; + lockFlags |= def->motionLocks.linearZ ? b3_lockLinearZ : 0; + lockFlags |= def->motionLocks.angularX ? b3_lockAngularX : 0; + lockFlags |= def->motionLocks.angularY ? b3_lockAngularY : 0; + lockFlags |= def->motionLocks.angularZ ? b3_lockAngularZ : 0; + + b3SolverSet* set = b3Array_Get( world->solverSets, setId ); + b3BodySim* bodySim = b3Array_Emplace( set->bodySims ); + *bodySim = (b3BodySim){ 0 }; + bodySim->transform.p = def->position; + bodySim->transform.q = def->rotation; + bodySim->center = def->position; + bodySim->rotation0 = bodySim->transform.q; + bodySim->center0 = bodySim->center; + bodySim->localCenter = b3Vec3_zero; + bodySim->force = b3Vec3_zero; + bodySim->torque = b3Vec3_zero; + bodySim->invMass = 0.0f; + bodySim->invInertiaLocal = b3Mat3_zero; + bodySim->minExtent = B3_HUGE; + bodySim->maxExtent = b3Vec3_zero; + bodySim->linearDamping = def->linearDamping; + bodySim->angularDamping = def->angularDamping; + bodySim->gravityScale = def->gravityScale; + bodySim->bodyId = bodyId; + bodySim->flags = lockFlags; + bodySim->flags |= def->isBullet ? b3_isBullet : 0; + bodySim->flags |= def->allowFastRotation ? b3_allowFastRotation : 0; + bodySim->flags |= def->type == b3_dynamicBody ? b3_dynamicFlag : 0; + bodySim->flags |= def->enableSleep ? b3_enableSleep : 0; + bodySim->flags |= def->enableContactRecycling ? b3_bodyEnableContactRecycling : 0; + + if ( setId == b3_awakeSet ) + { + b3BodyState* bodyState = b3Array_Emplace( set->bodyStates ); + + *bodyState = (b3BodyState){ 0 }; + bodyState->linearVelocity = def->linearVelocity; + bodyState->angularVelocity = def->angularVelocity; + bodyState->deltaRotation = b3Quat_identity; + bodyState->flags = bodySim->flags; + + bodySim->maxAngularVelocity = b3Length( def->angularVelocity ) + 5.0f; + } + + if ( bodyId == world->bodies.count ) + { + b3Array_Push( world->bodies, (b3Body){ 0 } ); + } + else + { + B3_ASSERT( world->bodies.data[bodyId].id == B3_NULL_INDEX ); + } + + b3Body* body = b3Array_Get( world->bodies, bodyId ); + body->userData = def->userData; + body->setIndex = setId; + body->localIndex = set->bodySims.count - 1; + body->generation += 1; + body->headShapeId = B3_NULL_INDEX; + body->shapeCount = 0; + body->headChainId = B3_NULL_INDEX; + body->headContactKey = B3_NULL_INDEX; + body->contactCount = 0; + body->headJointKey = B3_NULL_INDEX; + body->jointCount = 0; + body->islandId = B3_NULL_INDEX; + body->islandIndex = B3_NULL_INDEX; + body->bodyMoveIndex = B3_NULL_INDEX; + body->id = bodyId; + body->sleepThreshold = def->sleepThreshold; + body->sleepTime = 0.0f; + body->sleepVelocity = 0.0f; + body->mass = 0.0f; + body->inertia = b3Mat3_zero; + body->nameId = b3AddName( &world->names, def->name ); + body->type = def->type; + body->flags = bodySim->flags; + + // dynamic and kinematic bodies that are enabled need a island + if ( setId >= b3_awakeSet ) + { + b3CreateIslandForBody( world, setId, body ); + } + + b3ValidateSolverSets( world ); + + b3BodyId id = { bodyId + 1, world->worldId, body->generation }; + + world->locked = false; + + B3_REC_CREATE( world, CreateBody, id, worldId, *def ); + + return id; +} + +bool b3IsBodyAwake( b3World* world, b3Body* body ) +{ + B3_UNUSED( world ); + return body->setIndex == b3_awakeSet; +} + +bool b3WakeBody( b3World* world, b3Body* body ) +{ + if ( body->setIndex >= b3_firstSleepingSet ) + { + b3WakeSolverSet( world, body->setIndex ); + b3ValidateSolverSets( world ); + return true; + } + + return false; +} + +bool b3WakeBodyWithLock( b3World* world, b3Body* body ) +{ + B3_ASSERT( world->locked == false ); + world->locked = true; + bool woke = b3WakeBody( world, body ); + world->locked = false; + return woke; +} + +void b3DestroyBody( b3BodyId bodyId ) +{ + b3World* world = b3GetUnlockedWorld( bodyId.world0 ); + if ( world == NULL ) + { + return; + } + + B3_REC( world, DestroyBody, bodyId ); + + world->locked = true; + + b3Body* body = b3GetBodyFullId( world, bodyId ); + + // Wake bodies attached to this body, even if this body is static. + bool wakeBodies = true; + + // Destroy the attached joints + int edgeKey = body->headJointKey; + while ( edgeKey != B3_NULL_INDEX ) + { + int jointId = edgeKey >> 1; + int edgeIndex = edgeKey & 1; + + b3Joint* joint = b3Array_Get( world->joints, jointId ); + edgeKey = joint->edges[edgeIndex].nextKey; + + // Careful because this modifies the list being traversed + b3DestroyJointInternal( world, joint, wakeBodies ); + } + + // Destroy all contacts attached to this body. + b3DestroyBodyContacts( world, body, wakeBodies ); + + // Destroy the attached shapes and their broad-phase proxies. + int shapeId = body->headShapeId; + while ( shapeId != B3_NULL_INDEX ) + { + b3Shape* shape = b3Array_Get( world->shapes, shapeId ); + + if ( shape->sensorIndex != B3_NULL_INDEX ) + { + b3DestroySensor( world, shape ); + } + + b3DestroyShapeProxy( shape, &world->broadPhase ); + + b3DestroyShapeAllocations( world, shape ); + + // Return shape to free list. + b3FreeId( &world->shapeIdPool, shapeId ); + shape->id = B3_NULL_INDEX; + + shapeId = shape->nextShapeId; + } + + b3RemoveBodyFromIsland( world, body ); + + // Remove body sim from solver set that owns it + b3SolverSet* set = b3Array_Get( world->solverSets, body->setIndex ); + int movedIndex = b3Array_RemoveSwap( set->bodySims, body->localIndex ); + if ( movedIndex != B3_NULL_INDEX ) + { + // Fix moved body index + b3BodySim* movedSim = set->bodySims.data + body->localIndex; + int movedId = movedSim->bodyId; + b3Body* movedBody = b3Array_Get( world->bodies, movedId ); + B3_ASSERT( movedBody->localIndex == movedIndex ); + movedBody->localIndex = body->localIndex; + } + + // Remove body state from awake set + if ( body->setIndex == b3_awakeSet ) + { + int result = b3Array_RemoveSwap( set->bodyStates, body->localIndex ); + B3_UNUSED( result ); + B3_ASSERT( result == movedIndex ); + } + else if ( set->setIndex >= b3_firstSleepingSet && set->bodySims.count == 0 ) + { + // Remove solver set if it's now an orphan. + b3DestroySolverSet( world, set->setIndex ); + } + + // Free body and id (preserve body revision) + b3FreeId( &world->bodyIdPool, body->id ); + + body->setIndex = B3_NULL_INDEX; + body->localIndex = B3_NULL_INDEX; + body->id = B3_NULL_INDEX; + + b3ValidateSolverSets( world ); + + world->locked = false; +} + +int b3Body_GetContactCapacity( b3BodyId bodyId ) +{ + b3World* world = b3GetUnlockedWorld( bodyId.world0 ); + if ( world == NULL ) + { + return 0; + } + + b3Body* body = b3GetBodyFullId( world, bodyId ); + + // Conservative and fast + return body->contactCount; +} + +int b3Body_GetContactData( b3BodyId bodyId, b3ContactData* contactData, int capacity ) +{ + b3World* world = b3GetUnlockedWorld( bodyId.world0 ); + if ( world == NULL ) + { + return 0; + } + + b3Body* body = b3GetBodyFullId( world, bodyId ); + + int contactKey = body->headContactKey; + int index = 0; + while ( contactKey != B3_NULL_INDEX && index < capacity ) + { + int contactId = contactKey >> 1; + int edgeIndex = contactKey & 1; + + b3Contact* contact = b3Array_Get( world->contacts, contactId ); + + // Is contact touching? + if ( contact->flags & b3_contactTouchingFlag ) + { + b3Shape* shapeA = b3Array_Get( world->shapes, contact->shapeIdA ); + b3Shape* shapeB = b3Array_Get( world->shapes, contact->shapeIdB ); + + contactData[index].contactId = (b3ContactId){ contact->contactId + 1, bodyId.world0, 0, contact->generation }; + contactData[index].shapeIdA = (b3ShapeId){ shapeA->id + 1, bodyId.world0, shapeA->generation }; + contactData[index].shapeIdB = (b3ShapeId){ shapeB->id + 1, bodyId.world0, shapeB->generation }; + contactData[index].manifolds = contact->manifolds; + contactData[index].manifoldCount = contact->manifoldCount; + index += 1; + } + + contactKey = contact->edges[edgeIndex].nextKey; + } + + B3_ASSERT( index <= capacity ); + + return index; +} + +b3AABB b3Body_ComputeAABB( b3BodyId bodyId ) +{ + b3World* world = b3GetUnlockedWorld( bodyId.world0 ); + if ( world == NULL ) + { + return (b3AABB){ 0 }; + } + + b3Body* body = b3GetBodyFullId( world, bodyId ); + if ( body->headShapeId == B3_NULL_INDEX ) + { + b3WorldTransform transform = b3GetBodyTransform( world, body->id ); + b3Vec3 p = b3ToVec3( transform.p ); + return (b3AABB){ p, p }; + } + + b3Shape* shape = b3Array_Get( world->shapes, body->headShapeId ); + b3AABB aabb = shape->aabb; + while ( shape->nextShapeId != B3_NULL_INDEX ) + { + shape = b3Array_Get( world->shapes, shape->nextShapeId ); + aabb = b3AABB_Union( aabb, shape->aabb ); + } + + return aabb; +} + +float b3Body_GetClosestPoint( b3BodyId bodyId, b3Vec3* result, b3Vec3 target ) +{ + b3World* world = b3GetUnlockedWorld( bodyId.world0 ); + if ( world == NULL ) + { + *result = (b3Vec3){ 0 }; + return 0.0f; + } + + b3Body* body = b3GetBodyFullId( world, bodyId ); + b3WorldTransform worldTransform = b3GetBodyTransform( world, body->id ); + b3Transform transform = b3ToRelativeTransform( worldTransform, b3Pos_zero ); + + float closestDistance = FLT_MAX; + b3Vec3 closestPoint = transform.p; + + b3DistanceInput input = { 0 }; + input.proxyA = (b3ShapeProxy){ &target, 1, 0.0f }; + + // Target rides in frame A at the origin, so the relative pose of the shape in A is the body transform + input.transform = transform; + input.useRadii = false; + + int shapeId = body->headShapeId; + while ( shapeId != B3_NULL_INDEX ) + { + b3Shape* shape = b3Array_Get( world->shapes, shapeId ); + shapeId = shape->nextShapeId; + + b3ShapeType type = shape->type; + if ( type != b3_sphereShape && type != b3_capsuleShape && type != b3_hullShape ) + { + continue; + } + + input.proxyB = b3MakeShapeProxy( shape ); + + b3SimplexCache cache = { 0 }; + b3DistanceOutput output = b3ShapeDistance( &input, &cache, NULL, 0 ); + if ( output.distance < closestDistance ) + { + closestDistance = output.distance; + closestPoint = output.pointB; + } + } + + *result = closestPoint; + return closestDistance; +} + +b3BodyCastResult b3Body_CastRay( b3BodyId bodyId, b3Pos origin, b3Vec3 translation, b3QueryFilter filter, float maxFraction, + b3WorldTransform bodyTransform ) +{ + b3World* world = b3GetUnlockedWorld( bodyId.world0 ); + if ( world == NULL ) + { + return (b3BodyCastResult){ 0 }; + } + + b3BodyCastResult result = { 0 }; + b3Body* body = b3GetBodyFullId( world, bodyId ); + + // The consistent framing is to center on the ray origin. + b3RayCastInput shapeInput = { 0 }; + shapeInput.origin = b3Vec3_zero; + shapeInput.translation = translation; + shapeInput.maxFraction = maxFraction; + + b3Transform transform = b3ToRelativeTransform( bodyTransform, origin ); + + int shapeId = body->headShapeId; + while ( shapeId != B3_NULL_INDEX ) + { + b3Shape* shape = b3Array_Get( world->shapes, shapeId ); + shapeId = shape->nextShapeId; + + if ( b3ShouldQueryCollide( &shape->filter, &filter ) == false ) + { + continue; + } + + b3CastOutput shapeOutput = b3RayCastShape( shape, transform, &shapeInput ); + + if ( shapeOutput.hit == false ) + { + continue; + } + + if ( shapeOutput.fraction > shapeInput.maxFraction ) + { + continue; + } + + // Careful with id, shapeId is the next shape. + b3ShapeId id = { shape->id + 1, bodyId.world0, shape->generation }; + + int materialIndex = b3ClampInt( shapeOutput.materialIndex, 0, shape->materialCount - 1 ); + uint64_t userMaterialId = b3GetShapeMaterials( shape )[materialIndex].userMaterialId; + + result = (b3BodyCastResult){ + .shapeId = id, + .point = b3OffsetPos( origin, shapeOutput.point ), + .normal = shapeOutput.normal, + .fraction = shapeOutput.fraction, + .triangleIndex = shapeOutput.triangleIndex, + .userMaterialId = userMaterialId, + .iterations = shapeOutput.iterations, + .hit = true, + }; + + shapeInput.maxFraction = shapeOutput.fraction; + } + + return result; +} + +b3BodyCastResult b3Body_CastShape( b3BodyId bodyId, b3Pos origin, const b3ShapeProxy* proxy, b3Vec3 translation, + b3QueryFilter filter, float maxFraction, bool canEncroach, b3WorldTransform bodyTransform ) +{ + b3World* world = b3GetUnlockedWorld( bodyId.world0 ); + if ( world == NULL ) + { + return (b3BodyCastResult){ 0 }; + } + + b3BodyCastResult result = { 0 }; + b3Body* body = b3GetBodyFullId( world, bodyId ); + + b3Transform transform = b3ToRelativeTransform( bodyTransform, origin ); + + b3ShapeCastInput shapeInput = { 0 }; + shapeInput.proxy = *proxy; + shapeInput.translation = translation; + shapeInput.maxFraction = maxFraction; + shapeInput.canEncroach = canEncroach; + + int shapeId = body->headShapeId; + while ( shapeId != B3_NULL_INDEX ) + { + b3Shape* shape = b3Array_Get( world->shapes, shapeId ); + shapeId = shape->nextShapeId; + + if ( b3ShouldQueryCollide( &shape->filter, &filter ) == false ) + { + continue; + } + + b3CastOutput shapeOutput = b3ShapeCastShape( shape, transform, &shapeInput ); + + if ( shapeOutput.hit == false ) + { + continue; + } + + if ( shapeOutput.fraction > shapeInput.maxFraction ) + { + continue; + } + + // Careful with id, shapeId is the next shape. + b3ShapeId id = { shape->id + 1, bodyId.world0, shape->generation }; + int materialIndex = b3ClampInt( shapeOutput.materialIndex, 0, shape->materialCount - 1 ); + uint64_t userMaterialId = b3GetShapeMaterials( shape )[materialIndex].userMaterialId; + + result = (b3BodyCastResult){ + .shapeId = id, + .point = b3OffsetPos( origin, shapeOutput.point ), + .normal = shapeOutput.normal, + .fraction = shapeOutput.fraction, + .triangleIndex = shapeOutput.triangleIndex, + .userMaterialId = userMaterialId, + .iterations = shapeOutput.iterations, + .hit = true, + }; + + shapeInput.maxFraction = shapeOutput.fraction; + } + + return result; +} + +bool b3Body_OverlapShape( b3BodyId bodyId, b3Pos origin, const b3ShapeProxy* proxy, b3QueryFilter filter, + b3WorldTransform bodyTransform ) +{ + b3World* world = b3GetUnlockedWorld( bodyId.world0 ); + if ( world == NULL ) + { + return false; + } + + b3Body* body = b3GetBodyFullId( world, bodyId ); + b3Transform transform = b3ToRelativeTransform( bodyTransform, origin ); + + int shapeId = body->headShapeId; + while ( shapeId != B3_NULL_INDEX ) + { + b3Shape* shape = b3Array_Get( world->shapes, shapeId ); + shapeId = shape->nextShapeId; + + if ( b3ShouldQueryCollide( &shape->filter, &filter ) == false ) + { + continue; + } + + bool overlaps = b3OverlapShape( shape, transform, proxy ); + if ( overlaps ) + { + return true; + } + } + + return false; +} + +int b3Body_CollideMover( b3BodyId bodyId, b3BodyPlaneResult* bodyPlanes, int planeCapacity, b3Pos origin, const b3Capsule* mover, + b3QueryFilter filter, b3WorldTransform bodyTransform ) +{ + b3World* world = b3GetUnlockedWorld( bodyId.world0 ); + if ( world == NULL ) + { + return 0; + } + + if ( planeCapacity == 0 ) + { + return 0; + } + + int resultCount = 0; + b3Body* body = b3GetBodyFullId( world, bodyId ); + + b3Transform transform = b3ToRelativeTransform( bodyTransform, origin ); + + int shapeId = body->headShapeId; + while ( shapeId != B3_NULL_INDEX ) + { + b3Shape* shape = b3Array_Get( world->shapes, shapeId ); + shapeId = shape->nextShapeId; + + if ( b3ShouldQueryCollide( &shape->filter, &filter ) == false ) + { + continue; + } + + b3ShapeType type = shape->type; + if ( type != b3_sphereShape && type != b3_capsuleShape && type != b3_hullShape ) + { + continue; + } + + b3PlaneResult plane; + int count = b3CollideMover( &plane, 1, shape, transform, mover ); + + if ( count > 0 ) + { + b3ShapeId id = { shape->id + 1, bodyId.world0, shape->generation }; + bodyPlanes[resultCount] = (b3BodyPlaneResult){ .shapeId = id, .result = plane }; + resultCount += 1; + if ( resultCount == planeCapacity ) + { + return resultCount; + } + } + } + + return resultCount; +} + +void b3UpdateBodyMassData( b3World* world, b3Body* body ) +{ + b3BodySim* bodySim = b3GetBodySim( world, body ); + + // Mass is no longer dirty + body->flags &= ~b3_dirtyMass; + b3SyncBodyFlags( world, body ); + + // Compute mass data from shapes. Each shape has its own density. + body->mass = 0.0f; + body->inertia = b3Mat3_zero; + + bodySim->invMass = 0.0f; + bodySim->invInertiaLocal = b3Mat3_zero; + bodySim->invInertiaWorld = b3Mat3_zero; + bodySim->localCenter = b3Vec3_zero; + bodySim->minExtent = B3_HUGE; + bodySim->maxExtent = b3Vec3_zero; + + if ( body->headShapeId == B3_NULL_INDEX ) + { + return; + } + + // Static and kinematic sims have zero mass. + if ( body->type != b3_dynamicBody ) + { + bodySim->center = bodySim->transform.p; + bodySim->center0 = bodySim->center; + + // Need extents for kinematic bodies for sleeping to work correctly. + if ( body->type == b3_kinematicBody ) + { + int shapeId = body->headShapeId; + while ( shapeId != B3_NULL_INDEX ) + { + const b3Shape* s = b3Array_Get( world->shapes, shapeId ); + + b3ShapeExtent extent = b3ComputeShapeExtent( s, b3Vec3_zero ); + bodySim->minExtent = b3MinFloat( bodySim->minExtent, extent.minExtent ); + bodySim->maxExtent = b3Max( bodySim->maxExtent, extent.maxExtent ); + + shapeId = s->nextShapeId; + } + } + + return; + } + + int shapeCount = body->shapeCount; + b3MassData* masses = b3StackAlloc( &world->stack, shapeCount * sizeof( b3MassData ), "mass data" ); + + // Accumulate mass over all shapes. + b3Vec3 localCenter = b3Vec3_zero; + int shapeId = body->headShapeId; + int shapeIndex = 0; + while ( shapeId != B3_NULL_INDEX ) + { + const b3Shape* s = b3Array_Get( world->shapes, shapeId ); + shapeId = s->nextShapeId; + + if ( s->density == 0.0f ) + { + masses[shapeIndex] = (b3MassData){ 0 }; + shapeIndex += 1; + continue; + } + + b3MassData massData = b3ComputeShapeMass( s ); + body->mass += massData.mass; + localCenter = b3MulAdd( localCenter, massData.mass, massData.center ); + + masses[shapeIndex] = massData; + shapeIndex += 1; + } + + // Compute center of mass. + if ( body->mass > 0.0f ) + { + bodySim->invMass = 1.0f / body->mass; + localCenter = b3MulSV( bodySim->invMass, localCenter ); + } + + // Second loop to accumulate the rotational inertia about the center of mass + for ( shapeIndex = 0; shapeIndex < shapeCount; ++shapeIndex ) + { + b3MassData massData = masses[shapeIndex]; + if ( massData.mass == 0.0f ) + { + continue; + } + + // Shift to center of mass. This is safe because it can only increase. + b3Vec3 offset = b3Sub( localCenter, massData.center ); + b3Matrix3 inertia = b3AddMM( massData.inertia, b3Steiner( massData.mass, offset ) ); + body->inertia = b3AddMM( body->inertia, inertia ); + } + + b3StackFree( &world->stack, masses ); + masses = NULL; + + float det = b3Det( body->inertia ); + B3_ASSERT( det >= 0.0f ); + + if ( det > 0.0f ) + { + // This call is faster than b3Invert + bodySim->invInertiaLocal = b3InvertT( body->inertia ); + + b3Matrix3 rotationMatrix = b3MakeMatrixFromQuat( bodySim->transform.q ); + bodySim->invInertiaWorld = b3MulMM( b3MulMM( rotationMatrix, bodySim->invInertiaLocal ), b3Transpose( rotationMatrix ) ); + } + + // Move center of mass. + b3Pos oldCenter = bodySim->center; + bodySim->localCenter = localCenter; + bodySim->center = b3TransformWorldPoint( bodySim->transform, bodySim->localCenter ); + bodySim->center0 = bodySim->center; + + // Update center of mass velocity + b3BodyState* state = b3GetBodyState( world, body ); + if ( state != NULL ) + { + b3Vec3 deltaLinear = b3Cross( state->angularVelocity, b3SubPos( bodySim->center, oldCenter ) ); + state->linearVelocity = b3Add( state->linearVelocity, deltaLinear ); + } + + // Compute body extents relative to center of mass + shapeId = body->headShapeId; + while ( shapeId != B3_NULL_INDEX ) + { + b3Shape* s = b3Array_Get( world->shapes, shapeId ); + + b3ShapeExtent extent = b3ComputeShapeExtent( s, localCenter ); + bodySim->minExtent = b3MinFloat( bodySim->minExtent, extent.minExtent ); + bodySim->maxExtent = b3Max( bodySim->maxExtent, extent.maxExtent ); + + shapeId = s->nextShapeId; + } + + // Apply fixed rotation + if ( ( bodySim->flags & b3_fixedRotation ) == b3_fixedRotation ) + { + body->inertia = b3Mat3_zero; + bodySim->invInertiaLocal = b3Mat3_zero; + bodySim->invInertiaWorld = b3Mat3_zero; + } +} + +b3Pos b3Body_GetPosition( b3BodyId bodyId ) +{ + b3World* world = b3GetWorld( bodyId.world0 ); + b3Body* body = b3GetBodyFullId( world, bodyId ); + b3WorldTransform transform = b3GetBodyTransformQuick( world, body ); + return transform.p; +} + +b3Quat b3Body_GetRotation( b3BodyId bodyId ) +{ + b3World* world = b3GetWorld( bodyId.world0 ); + b3Body* body = b3GetBodyFullId( world, bodyId ); + b3WorldTransform transform = b3GetBodyTransformQuick( world, body ); + return transform.q; +} + +b3WorldTransform b3Body_GetTransform( b3BodyId bodyId ) +{ + b3World* world = b3GetWorld( bodyId.world0 ); + b3Body* body = b3GetBodyFullId( world, bodyId ); + return b3GetBodyTransformQuick( world, body ); +} + +b3Vec3 b3Body_GetLocalPoint( b3BodyId bodyId, b3Pos worldPoint ) +{ + b3World* world = b3GetWorld( bodyId.world0 ); + b3Body* body = b3GetBodyFullId( world, bodyId ); + b3WorldTransform transform = b3GetBodyTransformQuick( world, body ); + return b3InvTransformWorldPoint( transform, worldPoint ); +} + +b3Pos b3Body_GetWorldPoint( b3BodyId bodyId, b3Vec3 localPoint ) +{ + b3World* world = b3GetWorld( bodyId.world0 ); + b3Body* body = b3GetBodyFullId( world, bodyId ); + b3WorldTransform transform = b3GetBodyTransformQuick( world, body ); + return b3TransformWorldPoint( transform, localPoint ); +} + +b3Vec3 b3Body_GetLocalVector( b3BodyId bodyId, b3Vec3 worldVector ) +{ + b3World* world = b3GetWorld( bodyId.world0 ); + b3Body* body = b3GetBodyFullId( world, bodyId ); + b3WorldTransform transform = b3GetBodyTransformQuick( world, body ); + return b3InvRotateVector( transform.q, worldVector ); +} + +b3Vec3 b3Body_GetWorldVector( b3BodyId bodyId, b3Vec3 localVector ) +{ + b3World* world = b3GetWorld( bodyId.world0 ); + b3Body* body = b3GetBodyFullId( world, bodyId ); + b3WorldTransform transform = b3GetBodyTransformQuick( world, body ); + return b3RotateVector( transform.q, localVector ); +} + +void b3Body_SetTransform( b3BodyId bodyId, b3Pos position, b3Quat rotation ) +{ + B3_ASSERT( b3IsValidPosition( position ) ); + B3_ASSERT( b3IsValidQuat( rotation ) ); + B3_ASSERT( b3Body_IsValid( bodyId ) ); + b3World* world = b3GetWorld( bodyId.world0 ); + B3_ASSERT( world->locked == false ); + + B3_REC( world, BodySetTransform, bodyId, position, rotation ); + + b3Body* body = b3GetBodyFullId( world, bodyId ); + b3BodySim* bodySim = b3GetBodySim( world, body ); + + bodySim->transform.p = position; + bodySim->transform.q = rotation; + bodySim->center = b3TransformWorldPoint( bodySim->transform, bodySim->localCenter ); + + b3Matrix3 rotationMatrix = b3MakeMatrixFromQuat( bodySim->transform.q ); + bodySim->invInertiaWorld = b3MulMM( b3MulMM( rotationMatrix, bodySim->invInertiaLocal ), b3Transpose( rotationMatrix ) ); + + bodySim->rotation0 = bodySim->transform.q; + bodySim->center0 = bodySim->center; + + b3BroadPhase* broadPhase = &world->broadPhase; + + b3WorldTransform transform = bodySim->transform; + const float speculativeDistance = B3_SPECULATIVE_DISTANCE; + + int shapeId = body->headShapeId; + while ( shapeId != B3_NULL_INDEX ) + { + b3Shape* shape = b3Array_Get( world->shapes, shapeId ); + b3AABB aabb = b3ComputeFatShapeAABB( shape, transform, speculativeDistance ); + shape->aabb = aabb; + + if ( b3AABB_Contains( shape->fatAABB, aabb ) == false ) + { + float margin = shape->aabbMargin; + b3AABB fatAABB; + fatAABB.lowerBound.x = aabb.lowerBound.x - margin; + fatAABB.lowerBound.y = aabb.lowerBound.y - margin; + fatAABB.lowerBound.z = aabb.lowerBound.z - margin; + fatAABB.upperBound.x = aabb.upperBound.x + margin; + fatAABB.upperBound.y = aabb.upperBound.y + margin; + fatAABB.upperBound.z = aabb.upperBound.z + margin; + shape->fatAABB = fatAABB; + + // The body could be disabled + if ( shape->proxyKey != B3_NULL_INDEX ) + { + b3BroadPhase_MoveProxy( broadPhase, shape->proxyKey, fatAABB ); + } + } + + shapeId = shape->nextShapeId; + } +} + +b3Vec3 b3Body_GetLinearVelocity( b3BodyId bodyId ) +{ + b3World* world = b3GetWorld( bodyId.world0 ); + b3Body* body = b3GetBodyFullId( world, bodyId ); + b3BodyState* state = b3GetBodyState( world, body ); + if ( state != NULL ) + { + return state->linearVelocity; + } + return b3Vec3_zero; +} + +b3Vec3 b3Body_GetAngularVelocity( b3BodyId bodyId ) +{ + b3World* world = b3GetWorld( bodyId.world0 ); + b3Body* body = b3GetBodyFullId( world, bodyId ); + b3BodyState* state = b3GetBodyState( world, body ); + if ( state != NULL ) + { + return state->angularVelocity; + } + return b3Vec3_zero; +} + +void b3Body_SetLinearVelocity( b3BodyId bodyId, b3Vec3 linearVelocity ) +{ + B3_ASSERT( b3IsValidVec3( linearVelocity ) ); + + b3World* world = b3GetWorld( bodyId.world0 ); + + B3_REC( world, BodySetLinearVelocity, bodyId, linearVelocity ); + + b3Body* body = b3GetBodyFullId( world, bodyId ); + + if ( body->type == b3_staticBody ) + { + return; + } + + if ( b3LengthSquared( linearVelocity ) > 0.0f ) + { + b3WakeBodyWithLock( world, body ); + } + + b3BodyState* state = b3GetBodyState( world, body ); + if ( state == NULL ) + { + return; + } + + state->linearVelocity = linearVelocity; +} + +void b3Body_SetAngularVelocity( b3BodyId bodyId, b3Vec3 angularVelocity ) +{ + B3_ASSERT( b3IsValidVec3( angularVelocity ) ); + + b3World* world = b3GetWorld( bodyId.world0 ); + + B3_REC( world, BodySetAngularVelocity, bodyId, angularVelocity ); + + b3Body* body = b3GetBodyFullId( world, bodyId ); + + if ( body->type == b3_staticBody ) + { + return; + } + + // Apply locks to avoid waking + b3Vec3 w; + w.x = ( body->flags & b3_lockAngularX ) ? 0.0f : angularVelocity.x; + w.y = ( body->flags & b3_lockAngularY ) ? 0.0f : angularVelocity.y; + w.z = ( body->flags & b3_lockAngularZ ) ? 0.0f : angularVelocity.z; + + if ( b3LengthSquared( w ) != 0.0f ) + { + b3WakeBodyWithLock( world, body ); + } + + b3BodyState* state = b3GetBodyState( world, body ); + if ( state == NULL ) + { + return; + } + + state->angularVelocity = w; +} + +void b3Body_SetTargetTransform( b3BodyId bodyId, b3WorldTransform target, float timeStep, bool wake ) +{ + B3_ASSERT( b3IsValidWorldTransform( target ) ); + + b3World* world = b3GetWorld( bodyId.world0 ); + + B3_REC( world, BodySetTargetTransform, bodyId, target, timeStep, wake ); + + b3Body* body = b3GetBodyFullId( world, bodyId ); + + if ( body->setIndex == b3_disabledSet ) + { + return; + } + + if ( body->type == b3_staticBody || timeStep <= 0.0f ) + { + return; + } + + if ( body->setIndex != b3_awakeSet && wake == false ) + { + return; + } + + b3BodySim* sim = b3GetBodySim( world, body ); + + // Compute linear velocity. The center difference is taken in world precision then demoted. + b3Pos center1 = sim->center; + b3Pos center2 = b3TransformWorldPoint( target, sim->localCenter ); + float invTimeStep = 1.0f / timeStep; + b3Vec3 linearVelocity = b3MulSV( invTimeStep, b3SubPos( center2, center1 ) ); + + // Compute angular velocity: + // q' = 0.5 * w * q + // <~> ( q2 - q1 ) / dt = 0.5 * w * q1 + // <=> w = 2 * ( q2 - q1 ) * Conjugate( q1 ) / dt + b3Quat q1 = sim->transform.q; + b3Quat q2 = target.q; + + // Use the shortest arc quaternion + if ( b3DotQuat( q1, q2 ) < 0.0f ) + { + q2 = b3NegateQuat( q2 ); + } + + b3Quat dq = { b3Sub( q2.v, q1.v ), q2.s - q1.s }; + b3Quat omega = b3MulQuat( dq, b3Conjugate( q1 ) ); + b3Vec3 angularVelocity = b3MulSV( 2.0f * invTimeStep, omega.v ); + + // Early out if the body is asleep already and the desired movement is small + if ( body->setIndex != b3_awakeSet ) + { + float maxVelocity = b3Length( linearVelocity ) + b3Length( b3Mul( angularVelocity, sim->maxExtent ) ); + + // Return if velocity would be sleepy + if ( maxVelocity < body->sleepThreshold ) + { + return; + } + + // Must wake for state to exist + b3WakeBodyWithLock( world, body ); + } + + B3_ASSERT( body->setIndex == b3_awakeSet ); + + b3BodyState* state = b3GetBodyState( world, body ); + state->linearVelocity = linearVelocity; + state->angularVelocity = angularVelocity; +} + +b3Vec3 b3Body_GetLocalPointVelocity( b3BodyId bodyId, b3Vec3 localPoint ) +{ + b3World* world = b3GetWorld( bodyId.world0 ); + b3Body* body = b3GetBodyFullId( world, bodyId ); + b3BodyState* state = b3GetBodyState( world, body ); + if ( state == NULL ) + { + return b3Vec3_zero; + } + + b3SolverSet* set = b3Array_Get( world->solverSets, body->setIndex ); + b3BodySim* bodySim = b3Array_Get( set->bodySims, body->localIndex ); + + b3Vec3 r = b3RotateVector( bodySim->transform.q, b3Sub( localPoint, bodySim->localCenter ) ); + b3Vec3 v = b3Add( state->linearVelocity, b3Cross( state->angularVelocity, r ) ); + return v; +} + +b3Vec3 b3Body_GetWorldPointVelocity( b3BodyId bodyId, b3Pos worldPoint ) +{ + b3World* world = b3GetWorld( bodyId.world0 ); + b3Body* body = b3GetBodyFullId( world, bodyId ); + b3BodyState* state = b3GetBodyState( world, body ); + if ( state == NULL ) + { + return b3Vec3_zero; + } + + b3SolverSet* set = b3Array_Get( world->solverSets, body->setIndex ); + b3BodySim* bodySim = b3Array_Get( set->bodySims, body->localIndex ); + + b3Vec3 r = b3SubPos( worldPoint, bodySim->center ); + b3Vec3 v = b3Add( state->linearVelocity, b3Cross( state->angularVelocity, r ) ); + return v; +} + +void b3Body_ApplyForce( b3BodyId bodyId, b3Vec3 force, b3Pos point, bool wake ) +{ + B3_ASSERT( b3IsValidVec3( force ) ); + + b3World* world = b3GetWorld( bodyId.world0 ); + + B3_REC( world, BodyApplyForce, bodyId, force, point, wake ); + + b3Body* body = b3GetBodyFullId( world, bodyId ); + + if ( wake && body->setIndex >= b3_firstSleepingSet ) + { + b3WakeBodyWithLock( world, body ); + } + + if ( body->setIndex == b3_awakeSet ) + { + b3BodySim* bodySim = b3GetBodySim( world, body ); + bodySim->force = b3Add( bodySim->force, force ); + bodySim->torque = b3Add( bodySim->torque, b3Cross( b3SubPos( point, bodySim->center ), force ) ); + } +} + +void b3Body_ApplyForceToCenter( b3BodyId bodyId, b3Vec3 force, bool wake ) +{ + B3_ASSERT( b3IsValidVec3( force ) ); + + b3World* world = b3GetWorld( bodyId.world0 ); + + B3_REC( world, BodyApplyForceToCenter, bodyId, force, wake ); + + b3Body* body = b3GetBodyFullId( world, bodyId ); + + if ( wake && body->setIndex >= b3_firstSleepingSet ) + { + b3WakeBodyWithLock( world, body ); + } + + if ( body->setIndex == b3_awakeSet ) + { + b3BodySim* bodySim = b3GetBodySim( world, body ); + bodySim->force = b3Add( bodySim->force, force ); + } +} + +void b3Body_ApplyTorque( b3BodyId bodyId, b3Vec3 torque, bool wake ) +{ + B3_ASSERT( b3IsValidVec3( torque ) ); + + b3World* world = b3GetWorld( bodyId.world0 ); + + B3_REC( world, BodyApplyTorque, bodyId, torque, wake ); + + b3Body* body = b3GetBodyFullId( world, bodyId ); + + if ( wake && body->setIndex >= b3_firstSleepingSet ) + { + b3WakeBodyWithLock( world, body ); + } + + if ( body->setIndex == b3_awakeSet ) + { + b3BodySim* bodySim = b3GetBodySim( world, body ); + bodySim->torque = b3Add( bodySim->torque, torque ); + } +} + +void b3Body_ApplyLinearImpulse( b3BodyId bodyId, b3Vec3 impulse, b3Pos point, bool wake ) +{ + B3_ASSERT( b3IsValidVec3( impulse ) ); + B3_ASSERT( b3IsValidPosition( point ) ); + + b3World* world = b3GetWorld( bodyId.world0 ); + + B3_REC( world, BodyApplyLinearImpulse, bodyId, impulse, point, wake ); + + b3Body* body = b3GetBodyFullId( world, bodyId ); + + if ( wake && body->setIndex >= b3_firstSleepingSet ) + { + b3WakeBodyWithLock( world, body ); + } + + if ( body->setIndex == b3_awakeSet ) + { + int localIndex = body->localIndex; + b3SolverSet* set = b3Array_Get( world->solverSets, b3_awakeSet ); + b3BodyState* state = b3Array_Get( set->bodyStates, localIndex ); + b3BodySim* bodySim = b3Array_Get( set->bodySims, localIndex ); + + state->linearVelocity = b3MulAdd( state->linearVelocity, bodySim->invMass, impulse ); + + float maxLinearSpeed = world->maxLinearSpeed; + if ( b3LengthSquared( state->linearVelocity ) > maxLinearSpeed * maxLinearSpeed ) + { + state->linearVelocity = b3MulSV( maxLinearSpeed, b3Normalize( state->linearVelocity ) ); + } + + b3Vec3 delta = b3MulMV( bodySim->invInertiaWorld, b3Cross( b3SubPos( point, bodySim->center ), impulse ) ); + state->angularVelocity = b3Add( state->angularVelocity, delta ); + } +} + +void b3Body_ApplyLinearImpulseToCenter( b3BodyId bodyId, b3Vec3 impulse, bool wake ) +{ + B3_ASSERT( b3IsValidVec3( impulse ) ); + + b3World* world = b3GetWorld( bodyId.world0 ); + + B3_REC( world, BodyApplyLinearImpulseToCenter, bodyId, impulse, wake ); + + b3Body* body = b3GetBodyFullId( world, bodyId ); + + if ( wake && body->setIndex >= b3_firstSleepingSet ) + { + b3WakeBodyWithLock( world, body ); + } + + if ( body->setIndex == b3_awakeSet ) + { + int localIndex = body->localIndex; + b3SolverSet* set = b3Array_Get( world->solverSets, b3_awakeSet ); + b3BodyState* state = b3Array_Get( set->bodyStates, localIndex ); + b3BodySim* bodySim = b3Array_Get( set->bodySims, localIndex ); + state->linearVelocity = b3MulAdd( state->linearVelocity, bodySim->invMass, impulse ); + + float maxLinearSpeed = world->maxLinearSpeed; + if ( b3LengthSquared( state->linearVelocity ) > maxLinearSpeed * maxLinearSpeed ) + { + state->linearVelocity = b3MulSV( maxLinearSpeed, b3Normalize( state->linearVelocity ) ); + } + } +} + +void b3Body_ApplyAngularImpulse( b3BodyId bodyId, b3Vec3 impulse, bool wake ) +{ + B3_ASSERT( b3IsValidVec3( impulse ) ); + B3_ASSERT( b3Body_IsValid( bodyId ) ); + + b3World* world = b3GetWorld( bodyId.world0 ); + + B3_REC( world, BodyApplyAngularImpulse, bodyId, impulse, wake ); + + int id = bodyId.index1 - 1; + b3Body* body = b3Array_Get( world->bodies, id ); + B3_ASSERT( body->generation == bodyId.generation ); + + if ( wake && body->setIndex >= b3_firstSleepingSet ) + { + // this will not invalidate body pointer + b3WakeBodyWithLock( world, body ); + } + + if ( body->setIndex == b3_awakeSet ) + { + int localIndex = body->localIndex; + b3SolverSet* set = b3Array_Get( world->solverSets, b3_awakeSet ); + b3BodyState* state = b3Array_Get( set->bodyStates, localIndex ); + b3BodySim* bodySim = b3Array_Get( set->bodySims, localIndex ); + + b3Vec3 localImpulse = b3InvRotateVector( bodySim->transform.q, impulse ); + b3Vec3 localAngularVelocityDelta = b3MulMV( bodySim->invInertiaLocal, localImpulse ); + state->angularVelocity = + b3Add( state->angularVelocity, b3RotateVector( bodySim->transform.q, localAngularVelocityDelta ) ); + } +} + +b3BodyType b3Body_GetType( b3BodyId bodyId ) +{ + b3World* world = b3GetWorld( bodyId.world0 ); + b3Body* body = b3GetBodyFullId( world, bodyId ); + return body->type; +} + +// This should follow similar steps as you would get destroying and recreating the body, shapes, and joints. +// Contacts are difficult to preserve because the broad-phase pairs change, so I just destroy them. +// todo with a bit more effort I could support an option to let the body sleep +// +// Revised steps: +// 1 Skip disabled bodies +// 2 Destroy all contacts on the body +// 3 Wake the body +// 4 For all joints attached to the body +// - wake attached bodies +// - remove from island +// - move to static set temporarily +// 5 Change the body type and transfer the body +// 6 If the body was static +// - create an island for the body +// Else if the body is becoming static +// - remove it from the island +// 7 For all joints +// - if either body is non-static +// - link into island +// - transfer to constraint graph +// 8 For all shapes +// - Destroy proxy in old tree +// - Create proxy in new tree +// Notes: +// - the implementation below tries to minimize the number of predicates, so some +// operations may have no effect, such as transferring a joint to the same set +void b3Body_SetType( b3BodyId bodyId, b3BodyType type ) +{ + b3World* world = b3GetUnlockedWorld( bodyId.world0 ); + if ( world == NULL ) + { + return; + } + + B3_REC( world, BodySetType, bodyId, (int32_t)type ); + + world->locked = true; + b3Body* body = b3GetBodyFullId( world, bodyId ); + + b3BodyType originalType = body->type; + if ( originalType == type ) + { + world->locked = false; + return; + } + + if ( type != b3_staticBody ) + { + int shapeId = body->headShapeId; + while ( shapeId != B3_NULL_INDEX ) + { + b3Shape* shape = b3Array_Get( world->shapes, shapeId ); + if ( shape->type == b3_compoundShape || shape->type == b3_heightShape ) + { + // Setting the body type is not supported for bodies with compound shapes + return; + } + + shapeId = shape->nextShapeId; + } + } + + // Stage 1: skip disabled bodies + if ( body->setIndex == b3_disabledSet ) + { + // Disabled bodies don't change solver sets or islands when they change type. + body->type = type; + + if ( type == b3_dynamicBody ) + { + body->flags |= b3_dynamicFlag; + } + else + { + body->flags &= ~b3_dynamicFlag; + } + + b3SyncBodyFlags( world, body ); + + // Body type affects the mass properties + b3UpdateBodyMassData( world, body ); + world->locked = false; + return; + } + + // Stage 2: destroy all contacts but don't wake bodies (because we don't need to) + bool wakeBodies = false; + b3DestroyBodyContacts( world, body, wakeBodies ); + + // Stage 3: wake this body (does nothing if body is static), otherwise it will also wake + // all bodies in the same sleeping solver set. + b3WakeBody( world, body ); + + // Stage 4: move joints to temporary storage + b3SolverSet* staticSet = b3Array_Get( world->solverSets, b3_staticSet ); + + int jointKey = body->headJointKey; + while ( jointKey != B3_NULL_INDEX ) + { + int jointId = jointKey >> 1; + int edgeIndex = jointKey & 1; + + b3Joint* joint = b3Array_Get( world->joints, jointId ); + jointKey = joint->edges[edgeIndex].nextKey; + + // Joint may be disabled by other body + if ( joint->setIndex == b3_disabledSet ) + { + continue; + } + + // Wake attached bodies. The b3WakeBody call above does not wake bodies + // attached to a static body. But it is necessary because the body may have + // no joints. + b3Body* bodyA = b3Array_Get( world->bodies, joint->edges[0].bodyId ); + b3Body* bodyB = b3Array_Get( world->bodies, joint->edges[1].bodyId ); + b3WakeBody( world, bodyA ); + b3WakeBody( world, bodyB ); + + // Remove joint from island + b3UnlinkJoint( world, joint ); + + // It is necessary to transfer all joints to the static set + // so they can be added to the constraint graph below and acquire consistent colors. + b3SolverSet* jointSourceSet = b3Array_Get( world->solverSets, joint->setIndex ); + b3TransferJoint( world, staticSet, jointSourceSet, joint ); + } + + // Stage 5: change the body type and transfer body + body->type = type; + + if ( type == b3_dynamicBody ) + { + body->flags |= b3_dynamicFlag; + } + else + { + body->flags &= ~b3_dynamicFlag; + } + + b3SolverSet* awakeSet = b3Array_Get( world->solverSets, b3_awakeSet ); + b3SolverSet* sourceSet = b3Array_Get( world->solverSets, body->setIndex ); + b3SolverSet* targetSet = type == b3_staticBody ? staticSet : awakeSet; + + // Transfer body + b3TransferBody( world, targetSet, sourceSet, body ); + + // Stage 6: update island participation for the body + if ( originalType == b3_staticBody ) + { + // Create island for body + b3CreateIslandForBody( world, b3_awakeSet, body ); + } + else if ( type == b3_staticBody ) + { + // Remove body from island. + b3RemoveBodyFromIsland( world, body ); + } + + // Stage 7: Transfer joints to the target set + jointKey = body->headJointKey; + while ( jointKey != B3_NULL_INDEX ) + { + int jointId = jointKey >> 1; + int edgeIndex = jointKey & 1; + + b3Joint* joint = b3Array_Get( world->joints, jointId ); + + jointKey = joint->edges[edgeIndex].nextKey; + + // Joint may be disabled by other body + if ( joint->setIndex == b3_disabledSet ) + { + continue; + } + + // All joints were transferred to the static set in an earlier stage + B3_ASSERT( joint->setIndex == b3_staticSet ); + + b3Body* bodyA = b3Array_Get( world->bodies, joint->edges[0].bodyId ); + b3Body* bodyB = b3Array_Get( world->bodies, joint->edges[1].bodyId ); + B3_ASSERT( bodyA->setIndex == b3_staticSet || bodyA->setIndex == b3_awakeSet ); + B3_ASSERT( bodyB->setIndex == b3_staticSet || bodyB->setIndex == b3_awakeSet ); + + if ( bodyA->type == b3_dynamicBody || bodyB->type == b3_dynamicBody ) + { + b3TransferJoint( world, awakeSet, staticSet, joint ); + } + } + + // Recreate shape proxies in broadphase + b3WorldTransform transform = b3GetBodyTransformQuick( world, body ); + int shapeId = body->headShapeId; + while ( shapeId != B3_NULL_INDEX ) + { + b3Shape* shape = b3Array_Get( world->shapes, shapeId ); + + // Setting the body type is not supported for bodies with compound shapes + B3_ASSERT( shape->type != b3_compoundShape ); + + shapeId = shape->nextShapeId; + b3DestroyShapeProxy( shape, &world->broadPhase ); + bool forcePairCreation = true; + b3CreateShapeProxy( shape, &world->broadPhase, type, transform, forcePairCreation ); + } + + // Relink all joints + jointKey = body->headJointKey; + while ( jointKey != B3_NULL_INDEX ) + { + int jointId = jointKey >> 1; + int edgeIndex = jointKey & 1; + + b3Joint* joint = b3Array_Get( world->joints, jointId ); + jointKey = joint->edges[edgeIndex].nextKey; + + int otherEdgeIndex = edgeIndex ^ 1; + int otherBodyId = joint->edges[otherEdgeIndex].bodyId; + b3Body* otherBody = b3Array_Get( world->bodies, otherBodyId ); + + if ( otherBody->setIndex == b3_disabledSet ) + { + continue; + } + + if ( body->type != b3_dynamicBody && otherBody->type != b3_dynamicBody ) + { + continue; + } + + b3LinkJoint( world, joint ); + } + + b3SyncBodyFlags( world, body ); + + // Body type affects the mass + b3UpdateBodyMassData( world, body ); + + b3ValidateSolverSets( world ); + b3ValidateIsland( world, body->islandId ); + + world->locked = false; +} + +void b3Body_SetName( b3BodyId bodyId, const char* name ) +{ + b3World* world = b3GetWorld( bodyId.world0 ); + + B3_REC( world, BodySetName, bodyId, name ); + + b3Body* body = b3GetBodyFullId( world, bodyId ); + body->nameId = b3AddName( &world->names, name ); +} + +const char* b3Body_GetName( b3BodyId bodyId ) +{ + b3World* world = b3GetWorld( bodyId.world0 ); + b3Body* body = b3GetBodyFullId( world, bodyId ); + return b3FindNameWithDefault( &world->names, body->nameId, "" ); +} + +void b3Body_SetUserData( b3BodyId bodyId, void* userData ) +{ + b3World* world = b3GetWorld( bodyId.world0 ); + b3Body* body = b3GetBodyFullId( world, bodyId ); + body->userData = userData; +} + +void* b3Body_GetUserData( b3BodyId bodyId ) +{ + b3World* world = b3GetWorld( bodyId.world0 ); + b3Body* body = b3GetBodyFullId( world, bodyId ); + return body->userData; +} + +float b3Body_GetMass( b3BodyId bodyId ) +{ + b3World* world = b3GetWorld( bodyId.world0 ); + b3Body* body = b3GetBodyFullId( world, bodyId ); + return body->mass; +} + +b3Matrix3 b3Body_GetLocalRotationalInertia( b3BodyId bodyId ) +{ + b3World* world = b3GetWorld( bodyId.world0 ); + b3Body* body = b3GetBodyFullId( world, bodyId ); + return body->inertia; +} + +float b3Body_GetInverseMass( b3BodyId bodyId ) +{ + b3World* world = b3GetWorld( bodyId.world0 ); + b3Body* body = b3GetBodyFullId( world, bodyId ); + b3BodySim* sim = b3GetBodySim( world, body ); + return sim->invMass; +} + +b3Matrix3 b3Body_GetWorldInverseRotationalInertia( b3BodyId bodyId ) +{ + b3World* world = b3GetWorld( bodyId.world0 ); + b3Body* body = b3GetBodyFullId( world, bodyId ); + b3BodySim* sim = b3GetBodySim( world, body ); + return sim->invInertiaWorld; +} + +b3Vec3 b3Body_GetLocalCenter( b3BodyId bodyId ) +{ + b3World* world = b3GetWorld( bodyId.world0 ); + b3Body* body = b3GetBodyFullId( world, bodyId ); + b3BodySim* bodySim = b3GetBodySim( world, body ); + return bodySim->localCenter; +} + +b3Pos b3Body_GetWorldCenter( b3BodyId bodyId ) +{ + b3World* world = b3GetWorld( bodyId.world0 ); + b3Body* body = b3GetBodyFullId( world, bodyId ); + b3BodySim* bodySim = b3GetBodySim( world, body ); + return bodySim->center; +} + +void b3Body_SetMassData( b3BodyId bodyId, b3MassData massData ) +{ + B3_ASSERT( b3IsValidFloat( massData.mass ) && massData.mass >= 0.0f ); + B3_ASSERT( b3IsValidMatrix3( massData.inertia ) ); + B3_ASSERT( b3IsValidVec3( massData.center ) ); + + b3World* world = b3GetUnlockedWorld( bodyId.world0 ); + if ( world == NULL ) + { + return; + } + + B3_REC( world, BodySetMassData, bodyId, massData ); + + b3Body* body = b3GetBodyFullId( world, bodyId ); + b3BodySim* bodySim = b3GetBodySim( world, body ); + + // Mass is no longer dirty + body->flags &= ~b3_dirtyMass; + b3SyncBodyFlags( world, body ); + + body->mass = massData.mass; + body->inertia = massData.inertia; + bodySim->localCenter = massData.center; + + b3Pos oldCenter = bodySim->center; + b3Pos center = b3TransformWorldPoint( bodySim->transform, massData.center ); + bodySim->center = center; + bodySim->center0 = center; + bodySim->invMass = body->mass > 0.0f ? 1.0f / body->mass : 0.0f; + + // Update center of mass velocity + b3BodyState* state = b3GetBodyState( world, body ); + if ( state != NULL ) + { + b3Vec3 deltaLinear = b3Cross( state->angularVelocity, b3SubPos( bodySim->center, oldCenter ) ); + state->linearVelocity = b3Add( state->linearVelocity, deltaLinear ); + } + + float det = b3Det( body->inertia ); + B3_ASSERT( det >= 0.0f ); + + if ( det > 0.0f ) + { + // This call is faster than b3Invert + bodySim->invInertiaLocal = b3InvertT( body->inertia ); + + b3Matrix3 rotationMatrix = b3MakeMatrixFromQuat( bodySim->transform.q ); + bodySim->invInertiaWorld = b3MulMM( b3MulMM( rotationMatrix, bodySim->invInertiaLocal ), b3Transpose( rotationMatrix ) ); + } + else + { + bodySim->invInertiaLocal = b3Mat3_zero; + bodySim->invInertiaWorld = b3Mat3_zero; + } + + // Apply fixed rotation + if ( ( bodySim->flags & b3_fixedRotation ) == b3_fixedRotation ) + { + body->inertia = b3Mat3_zero; + bodySim->invInertiaLocal = b3Mat3_zero; + bodySim->invInertiaWorld = b3Mat3_zero; + } + + // Update extents using supplied mass center. + bodySim->minExtent = B3_HUGE; + bodySim->maxExtent = b3Vec3_zero; + int shapeId = body->headShapeId; + while ( shapeId != B3_NULL_INDEX ) + { + const b3Shape* s = b3Array_Get( world->shapes, shapeId ); + b3ShapeExtent extent = b3ComputeShapeExtent( s, massData.center ); + bodySim->minExtent = b3MinFloat( bodySim->minExtent, extent.minExtent ); + bodySim->maxExtent = b3Max( bodySim->maxExtent, extent.maxExtent ); + shapeId = s->nextShapeId; + } +} + +b3MassData b3Body_GetMassData( b3BodyId bodyId ) +{ + b3World* world = b3GetWorld( bodyId.world0 ); + b3Body* body = b3GetBodyFullId( world, bodyId ); + b3BodySim* bodySim = b3GetBodySim( world, body ); + b3MassData massData = { body->mass, bodySim->localCenter, body->inertia }; + return massData; +} + +void b3Body_ApplyMassFromShapes( b3BodyId bodyId ) +{ + b3World* world = b3GetUnlockedWorld( bodyId.world0 ); + if ( world == NULL ) + { + return; + } + + B3_REC( world, BodyApplyMassFromShapes, bodyId ); + + b3Body* body = b3GetBodyFullId( world, bodyId ); + b3UpdateBodyMassData( world, body ); +} + +void b3Body_SetLinearDamping( b3BodyId bodyId, float linearDamping ) +{ + B3_ASSERT( b3IsValidFloat( linearDamping ) && linearDamping >= 0.0f ); + + b3World* world = b3GetUnlockedWorld( bodyId.world0 ); + if ( world == NULL ) + { + return; + } + + B3_REC( world, BodySetLinearDamping, bodyId, linearDamping ); + + b3Body* body = b3GetBodyFullId( world, bodyId ); + b3BodySim* bodySim = b3GetBodySim( world, body ); + bodySim->linearDamping = linearDamping; +} + +float b3Body_GetLinearDamping( b3BodyId bodyId ) +{ + b3World* world = b3GetWorld( bodyId.world0 ); + b3Body* body = b3GetBodyFullId( world, bodyId ); + b3BodySim* bodySim = b3GetBodySim( world, body ); + return bodySim->linearDamping; +} + +void b3Body_SetAngularDamping( b3BodyId bodyId, float angularDamping ) +{ + B3_ASSERT( b3IsValidFloat( angularDamping ) && angularDamping >= 0.0f ); + + b3World* world = b3GetUnlockedWorld( bodyId.world0 ); + if ( world == NULL ) + { + return; + } + + B3_REC( world, BodySetAngularDamping, bodyId, angularDamping ); + + b3Body* body = b3GetBodyFullId( world, bodyId ); + b3BodySim* bodySim = b3GetBodySim( world, body ); + bodySim->angularDamping = angularDamping; +} + +float b3Body_GetAngularDamping( b3BodyId bodyId ) +{ + b3World* world = b3GetWorld( bodyId.world0 ); + b3Body* body = b3GetBodyFullId( world, bodyId ); + b3BodySim* bodySim = b3GetBodySim( world, body ); + return bodySim->angularDamping; +} + +void b3Body_SetGravityScale( b3BodyId bodyId, float gravityScale ) +{ + B3_ASSERT( b3Body_IsValid( bodyId ) ); + B3_ASSERT( b3IsValidFloat( gravityScale ) ); + + b3World* world = b3GetUnlockedWorld( bodyId.world0 ); + if ( world == NULL ) + { + return; + } + + B3_REC( world, BodySetGravityScale, bodyId, gravityScale ); + + b3Body* body = b3GetBodyFullId( world, bodyId ); + b3BodySim* bodySim = b3GetBodySim( world, body ); + bodySim->gravityScale = gravityScale; +} + +float b3Body_GetGravityScale( b3BodyId bodyId ) +{ + B3_ASSERT( b3Body_IsValid( bodyId ) ); + b3World* world = b3GetWorld( bodyId.world0 ); + b3Body* body = b3GetBodyFullId( world, bodyId ); + b3BodySim* bodySim = b3GetBodySim( world, body ); + return bodySim->gravityScale; +} + +bool b3Body_IsAwake( b3BodyId bodyId ) +{ + b3World* world = b3GetWorld( bodyId.world0 ); + b3Body* body = b3GetBodyFullId( world, bodyId ); + return body->setIndex == b3_awakeSet; +} + +void b3Body_SetAwake( b3BodyId bodyId, bool awake ) +{ + b3World* world = b3GetUnlockedWorld( bodyId.world0 ); + if ( world == NULL ) + { + return; + } + + B3_REC( world, BodySetAwake, bodyId, awake ); + + world->locked = true; + + b3Body* body = b3GetBodyFullId( world, bodyId ); + + if ( awake && body->setIndex >= b3_firstSleepingSet ) + { + b3WakeBody( world, body ); + } + else if ( awake == false && body->setIndex == b3_awakeSet ) + { + b3Island* island = b3Array_Get( world->islands, body->islandId ); + if ( island->constraintRemoveCount > 0 ) + { + // Must split the island before sleeping. This is expensive. + b3SplitIsland( world, body->islandId ); + } + + b3TrySleepIsland( world, body->islandId ); + } + + world->locked = false; +} + +bool b3Body_IsEnabled( b3BodyId bodyId ) +{ + b3World* world = b3GetWorld( bodyId.world0 ); + b3Body* body = b3GetBodyFullId( world, bodyId ); + return body->setIndex != b3_disabledSet; +} + +bool b3Body_IsSleepEnabled( b3BodyId bodyId ) +{ + b3World* world = b3GetWorld( bodyId.world0 ); + b3Body* body = b3GetBodyFullId( world, bodyId ); + return ( body->flags & b3_enableSleep ) == b3_enableSleep; +} + +void b3Body_SetSleepThreshold( b3BodyId bodyId, float sleepThreshold ) +{ + b3World* world = b3GetWorld( bodyId.world0 ); + + B3_REC( world, BodySetSleepThreshold, bodyId, sleepThreshold ); + + b3Body* body = b3GetBodyFullId( world, bodyId ); + body->sleepThreshold = sleepThreshold; +} + +float b3Body_GetSleepThreshold( b3BodyId bodyId ) +{ + b3World* world = b3GetWorld( bodyId.world0 ); + b3Body* body = b3GetBodyFullId( world, bodyId ); + return body->sleepThreshold; +} + +void b3Body_EnableSleep( b3BodyId bodyId, bool enableSleep ) +{ + b3World* world = b3GetUnlockedWorld( bodyId.world0 ); + if ( world == NULL ) + { + return; + } + + B3_REC( world, BodyEnableSleep, bodyId, enableSleep ); + + b3Body* body = b3GetBodyFullId( world, bodyId ); + + bool flag = ( body->flags & b3_enableSleep ) == b3_enableSleep; + if ( enableSleep == flag ) + { + return; + } + + world->locked = true; + + body->flags = enableSleep ? body->flags | b3_enableSleep : body->flags & ~b3_enableSleep; + b3SyncBodyFlags( world, body ); + + if ( enableSleep == false ) + { + b3WakeBody( world, body ); + } + + world->locked = false; +} + +// Disabling a body requires a lot of detailed bookkeeping, but it is a valuable feature. +// The most challenging aspect that joints may connect to bodies that are not disabled. +void b3Body_Disable( b3BodyId bodyId ) +{ + b3World* world = b3GetUnlockedWorld( bodyId.world0 ); + if ( world == NULL ) + { + return; + } + + B3_REC( world, BodyDisable, bodyId ); + + world->locked = true; + + b3Body* body = b3GetBodyFullId( world, bodyId ); + if ( body->setIndex == b3_disabledSet ) + { + world->locked = false; + return; + } + + // Destroy contacts and wake bodies touching this body. This avoid floating bodies. + // This is necessary even for static bodies. + bool wakeBodies = true; + b3DestroyBodyContacts( world, body, wakeBodies ); + + // The current solver set of the body + b3SolverSet* set = b3Array_Get( world->solverSets, body->setIndex ); + + // Disabled bodies and connected joints are moved to the disabled set + b3SolverSet* disabledSet = b3Array_Get( world->solverSets, b3_disabledSet ); + + // Unlink joints and transfer them to the disabled set + int jointKey = body->headJointKey; + while ( jointKey != B3_NULL_INDEX ) + { + int jointId = jointKey >> 1; + int edgeIndex = jointKey & 1; + + b3Joint* joint = b3Array_Get( world->joints, jointId ); + jointKey = joint->edges[edgeIndex].nextKey; + + // joint may already be disabled by other body + if ( joint->setIndex == b3_disabledSet ) + { + continue; + } + + B3_ASSERT( joint->setIndex == set->setIndex || set->setIndex == b3_staticSet ); + + // Remove joint from island + b3UnlinkJoint( world, joint ); + + // Transfer joint to disabled set + b3SolverSet* jointSet = b3Array_Get( world->solverSets, joint->setIndex ); + b3TransferJoint( world, disabledSet, jointSet, joint ); + } + + // Remove shapes from broad-phase + int shapeId = body->headShapeId; + while ( shapeId != B3_NULL_INDEX ) + { + b3Shape* shape = b3Array_Get( world->shapes, shapeId ); + shapeId = shape->nextShapeId; + b3DestroyShapeProxy( shape, &world->broadPhase ); + } + + // Disabled bodies are not in an island. If the island becomes empty it will be destroyed. + b3RemoveBodyFromIsland( world, body ); + + // Transfer body sim + b3TransferBody( world, disabledSet, set, body ); + + b3ValidateConnectivity( world ); + b3ValidateSolverSets( world ); + + world->locked = false; +} + +void b3Body_Enable( b3BodyId bodyId ) +{ + b3World* world = b3GetUnlockedWorld( bodyId.world0 ); + if ( world == NULL ) + { + return; + } + + B3_REC( world, BodyEnable, bodyId ); + + b3Body* body = b3GetBodyFullId( world, bodyId ); + if ( body->setIndex != b3_disabledSet ) + { + return; + } + + b3SolverSet* disabledSet = b3Array_Get( world->solverSets, b3_disabledSet ); + int setId = body->type == b3_staticBody ? b3_staticSet : b3_awakeSet; + b3SolverSet* targetSet = b3Array_Get( world->solverSets, setId ); + + b3TransferBody( world, targetSet, disabledSet, body ); + + b3WorldTransform transform = b3GetBodyTransformQuick( world, body ); + + // Add shapes to broad-phase + b3BodyType proxyType = body->type; + bool forcePairCreation = true; + int shapeId = body->headShapeId; + while ( shapeId != B3_NULL_INDEX ) + { + b3Shape* shape = b3Array_Get( world->shapes, shapeId ); + shapeId = shape->nextShapeId; + + b3CreateShapeProxy( shape, &world->broadPhase, proxyType, transform, forcePairCreation ); + } + + if ( setId != b3_staticSet ) + { + b3CreateIslandForBody( world, setId, body ); + } + + // Transfer joints. If the other body is disabled, don't transfer. + // If the other body is sleeping, wake it. + int jointKey = body->headJointKey; + while ( jointKey != B3_NULL_INDEX ) + { + int jointId = jointKey >> 1; + int edgeIndex = jointKey & 1; + + b3Joint* joint = b3Array_Get( world->joints, jointId ); + B3_ASSERT( joint->setIndex == b3_disabledSet ); + B3_ASSERT( joint->islandId == B3_NULL_INDEX ); + + jointKey = joint->edges[edgeIndex].nextKey; + + b3Body* bodyA = b3Array_Get( world->bodies, joint->edges[0].bodyId ); + b3Body* bodyB = b3Array_Get( world->bodies, joint->edges[1].bodyId ); + + if ( bodyA->setIndex == b3_disabledSet || bodyB->setIndex == b3_disabledSet ) + { + // one body is still disabled + continue; + } + + // Transfer joint first + int jointSetId; + if ( bodyA->setIndex == b3_staticSet && bodyB->setIndex == b3_staticSet ) + { + jointSetId = b3_staticSet; + } + else if ( bodyA->setIndex == b3_staticSet ) + { + jointSetId = bodyB->setIndex; + } + else + { + jointSetId = bodyA->setIndex; + } + + b3SolverSet* jointSet = b3Array_Get( world->solverSets, jointSetId ); + b3TransferJoint( world, jointSet, disabledSet, joint ); + + // Now that the joint is in the correct set, I can link the joint in the island. + if ( jointSetId != b3_staticSet ) + { + b3LinkJoint( world, joint ); + } + } + + b3ValidateSolverSets( world ); +} + +void b3Body_SetMotionLocks( b3BodyId bodyId, b3MotionLocks locks ) +{ + b3World* world = b3GetUnlockedWorld( bodyId.world0 ); + if ( world == NULL ) + { + return; + } + + B3_REC( world, BodySetMotionLocks, bodyId, locks ); + + uint32_t newLocks = 0; + newLocks |= locks.linearX ? b3_lockLinearX : 0; + newLocks |= locks.linearY ? b3_lockLinearY : 0; + newLocks |= locks.linearZ ? b3_lockLinearZ : 0; + newLocks |= locks.angularX ? b3_lockAngularX : 0; + newLocks |= locks.angularY ? b3_lockAngularY : 0; + newLocks |= locks.angularZ ? b3_lockAngularZ : 0; + + b3Body* body = b3GetBodyFullId( world, bodyId ); + if ( ( body->flags & b3_allLocks ) == newLocks ) + { + return; + } + + bool fixedRotation1 = ( body->flags & b3_fixedRotation ) == b3_fixedRotation; + bool fixedRotation2 = ( newLocks & b3_fixedRotation ) == b3_fixedRotation; + + body->flags &= ~b3_allLocks; + body->flags |= newLocks; + + b3SyncBodyFlags( world, body ); + + b3BodyState* state = b3GetBodyState( world, body ); + + if ( state != NULL ) + { + if ( locks.linearX ) + { + state->linearVelocity.x = 0.0f; + } + + if ( locks.linearY ) + { + state->linearVelocity.y = 0.0f; + } + + if ( locks.linearZ ) + { + state->linearVelocity.z = 0.0f; + } + + if ( locks.angularX ) + { + state->angularVelocity.x = 0.0f; + } + + if ( locks.angularY ) + { + state->angularVelocity.y = 0.0f; + } + + if ( locks.angularZ ) + { + state->angularVelocity.z = 0.0f; + } + } + + if ( fixedRotation1 != fixedRotation2 ) + { + b3UpdateBodyMassData( world, body ); + } +} + +b3MotionLocks b3Body_GetMotionLocks( b3BodyId bodyId ) +{ + b3World* world = b3GetWorld( bodyId.world0 ); + b3Body* body = b3GetBodyFullId( world, bodyId ); + + b3MotionLocks locks; + locks.linearX = ( body->flags & b3_lockLinearX ); + locks.linearY = ( body->flags & b3_lockLinearY ); + locks.linearZ = ( body->flags & b3_lockLinearZ ); + locks.angularX = ( body->flags & b3_lockAngularX ); + locks.angularY = ( body->flags & b3_lockAngularY ); + locks.angularZ = ( body->flags & b3_lockAngularZ ); + return locks; +} + +void b3Body_SetBullet( b3BodyId bodyId, bool flag ) +{ + b3World* world = b3GetUnlockedWorld( bodyId.world0 ); + if ( world == NULL ) + { + return; + } + + B3_REC( world, BodySetBullet, bodyId, flag ); + + uint32_t newFlag = flag ? b3_isBullet : 0; + + b3Body* body = b3GetBodyFullId( world, bodyId ); + if ( ( body->flags & b3_isBullet ) == newFlag ) + { + return; + } + + body->flags &= ~b3_isBullet; + body->flags |= newFlag; + + b3SyncBodyFlags( world, body ); +} + +bool b3Body_IsBullet( b3BodyId bodyId ) +{ + b3World* world = b3GetWorld( bodyId.world0 ); + b3Body* body = b3GetBodyFullId( world, bodyId ); + return ( body->flags & b3_isBullet ) != 0; +} + +void b3Body_EnableContactRecycling( b3BodyId bodyId, bool flag ) +{ + b3World* world = b3GetUnlockedWorld( bodyId.world0 ); + if ( world == NULL ) + { + return; + } + + B3_REC( world, BodyEnableContactRecycling, bodyId, flag ); + + uint32_t newFlag = flag ? b3_bodyEnableContactRecycling : 0; + + b3Body* body = b3GetBodyFullId( world, bodyId ); + if ( ( body->flags & b3_bodyEnableContactRecycling ) == newFlag ) + { + return; + } + + body->flags &= ~b3_bodyEnableContactRecycling; + body->flags |= newFlag; + + b3SyncBodyFlags( world, body ); +} + +bool b3Body_IsContactRecyclingEnabled( b3BodyId bodyId ) +{ + b3World* world = b3GetWorld( bodyId.world0 ); + b3Body* body = b3GetBodyFullId( world, bodyId ); + return ( body->flags & b3_bodyEnableContactRecycling ) != 0; +} + +void b3Body_EnableHitEvents( b3BodyId bodyId, bool flag ) +{ + b3World* world = b3GetWorld( bodyId.world0 ); + + B3_REC( world, BodyEnableHitEvents, bodyId, flag ); + + b3Body* body = b3GetBodyFullId( world, bodyId ); + int shapeId = body->headShapeId; + while ( shapeId != B3_NULL_INDEX ) + { + b3Shape* shape = b3Array_Get( world->shapes, shapeId ); + shape->flags = flag ? shape->flags | b3_enableHitEvents : shape->flags & ~b3_enableHitEvents; + shapeId = shape->nextShapeId; + } +} + +b3WorldId b3Body_GetWorld( b3BodyId bodyId ) +{ + b3World* world = b3GetWorld( bodyId.world0 ); + return (b3WorldId){ (uint16_t)( bodyId.world0 + 1 ), world->generation }; +} + +int b3Body_GetShapeCount( b3BodyId bodyId ) +{ + b3World* world = b3GetWorld( bodyId.world0 ); + b3Body* body = b3GetBodyFullId( world, bodyId ); + return body->shapeCount; +} + +int b3Body_GetShapes( b3BodyId bodyId, b3ShapeId* shapeArray, int capacity ) +{ + b3World* world = b3GetWorld( bodyId.world0 ); + b3Body* body = b3GetBodyFullId( world, bodyId ); + int shapeId = body->headShapeId; + int shapeCount = 0; + while ( shapeId != B3_NULL_INDEX && shapeCount < capacity ) + { + b3Shape* shape = b3Array_Get( world->shapes, shapeId ); + b3ShapeId id = { shape->id + 1, bodyId.world0, shape->generation }; + shapeArray[shapeCount] = id; + shapeCount += 1; + + shapeId = shape->nextShapeId; + } + + return shapeCount; +} + +int b3Body_GetJointCount( b3BodyId bodyId ) +{ + b3World* world = b3GetWorld( bodyId.world0 ); + b3Body* body = b3GetBodyFullId( world, bodyId ); + return body->jointCount; +} + +int b3Body_GetJoints( b3BodyId bodyId, b3JointId* jointArray, int capacity ) +{ + b3World* world = b3GetWorld( bodyId.world0 ); + b3Body* body = b3GetBodyFullId( world, bodyId ); + int jointKey = body->headJointKey; + + int jointCount = 0; + while ( jointKey != B3_NULL_INDEX && jointCount < capacity ) + { + int jointId = jointKey >> 1; + int edgeIndex = jointKey & 1; + + b3Joint* joint = b3Array_Get( world->joints, jointId ); + + b3JointId id = { jointId + 1, bodyId.world0, joint->generation }; + jointArray[jointCount] = id; + jointCount += 1; + + jointKey = joint->edges[edgeIndex].nextKey; + } + + return jointCount; +} + +bool b3ShouldBodiesCollide( b3World* world, b3Body* bodyA, b3Body* bodyB ) +{ + if ( bodyA->type != b3_dynamicBody && bodyB->type != b3_dynamicBody ) + { + return false; + } + + int jointKey; + int otherBodyId; + if ( bodyA->jointCount < bodyB->jointCount ) + { + jointKey = bodyA->headJointKey; + otherBodyId = bodyB->id; + } + else + { + jointKey = bodyB->headJointKey; + otherBodyId = bodyA->id; + } + + while ( jointKey != B3_NULL_INDEX ) + { + int jointId = jointKey >> 1; + int edgeIndex = jointKey & 1; + int otherEdgeIndex = edgeIndex ^ 1; + + b3Joint* joint = b3Array_Get( world->joints, jointId ); + if ( joint->collideConnected == false && joint->edges[otherEdgeIndex].bodyId == otherBodyId ) + { + return false; + } + + jointKey = joint->edges[edgeIndex].nextKey; + } + + return true; +} diff --git a/vendor/box3d/src/src/body.h b/vendor/box3d/src/src/body.h new file mode 100644 index 000000000..5eebf67ab --- /dev/null +++ b/vendor/box3d/src/src/body.h @@ -0,0 +1,256 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +#pragma once + +#include "box3d/constants.h" +#include "box3d/math_functions.h" +#include "box3d/types.h" + +typedef struct b3World b3World; + +enum b3BodyFlags +{ + // This body has fixed translation along the x-axis + b3_lockLinearX = 0x00000001, + + // This body has fixed translation along the y-axis + b3_lockLinearY = 0x00000002, + + // This body has fixed translation along the z-axis + b3_lockLinearZ = 0x00000004, + + // This body has fixed rotation around the x-axis + b3_lockAngularX = 0x00000008, + + // This body has fixed rotation around the y-axis + b3_lockAngularY = 0x00000010, + + // This body has fixed rotation around the z-axis + b3_lockAngularZ = 0x00000020, + + // This flag is used for debug draw + b3_isFast = 0x00000040, + + // This dynamic body does a final CCD pass against all body types, but not other bullets + b3_isBullet = 0x00000080, + + // This body was speed capped in the current time step + b3_isSpeedCapped = 0x00000100, + + // This body had a time of impact event in the current time step + b3_hadTimeOfImpact = 0x00000200, + + // This body has no limit on angular velocity + b3_allowFastRotation = 0x00000400, + + // This body need's to have its AABB increased + b3_enlargeBounds = 0x00000800, + + // This body is dynamic so the solver should write to it. + // This prevents writing to kinematic bodies that causes a multithreaded sharing + // cache coherence problem even when the values are not changing. + // Used for b3BodyState flags. + b3_dynamicFlag = 0x00001000, + + b3_enableSleep = 0x00002000, + + b3_bodyEnableContactRecycling = 0x00004000, + + // The user deferred mass computation via the updateBodyMass shape option and mass + // data still hasn't been set. + b3_dirtyMass = 0x00008000, + + // All lock flags + b3_allLocks = b3_lockLinearX | b3_lockLinearY | b3_lockLinearZ | b3_lockAngularX | b3_lockAngularY | b3_lockAngularZ, + + // If all these flags are set then the body has fixed rotation + b3_fixedRotation = b3_lockAngularX | b3_lockAngularY | b3_lockAngularZ, + + // These flags are transient per time step. These may be different across b3Body, b3BodySim, and b3BodyState. + b3_bodyTransientFlags = b3_isFast | b3_isSpeedCapped | b3_hadTimeOfImpact, +}; + +// Body organizational details that are not used in the solver. +typedef struct b3Body +{ + void* userData; + + // index of solver set stored in b3World + // may be B3_NULL_INDEX + int setIndex; + + // body sim and state index within set + // may be B3_NULL_INDEX + int localIndex; + + // [31 : contactId | 1 : edgeIndex] + int headContactKey; + int contactCount; + + // todo maybe move this to the body sim + int headShapeId; + int shapeCount; + + int headChainId; + + // [31 : jointId | 1 : edgeIndex] + int headJointKey; + int jointCount; + + // All enabled dynamic and kinematic bodies are in an island. + int islandId; + + // Index into the island's bodies array for O(1) swap-removal. + // B3_NULL_INDEX when not in an island. + int islandIndex; + + float sleepThreshold; + float sleepTime; + float sleepVelocity; + float mass; + + // local space inertia + b3Matrix3 inertia; + + // this is used to adjust the fellAsleep flag in the body move array + int bodyMoveIndex; + + int id; + + // b3BodyFlags + uint32_t flags; + uint32_t nameId; + + b3BodyType type; + + // This is monotonically advanced when a body is allocated in this slot + // Used to check for invalid b3BodyId + uint16_t generation; +} b3Body; + +// Body State +// The body state is designed for fast conversion to and from SIMD via scatter-gather. +// Only awake dynamic and kinematic bodies have a body state. +// This is used in the performance critical constraint solver +// +// The solver operates on the body state. The body state array does not hold static bodies. Static bodies are shared +// across worker threads. It would be okay to read their states, but writing to them would cause cache thrashing across +// workers, even if the values don't change. +// This causes some trouble when computing anchors. I rotate joint anchors using the body rotation every sub-step. For static +// bodies the anchor doesn't rotate. Body A or B could be static and this can lead to lots of branching. This branching +// should be minimized. +// +// Solution 1: +// Use delta rotations. This means anchors need to be prepared in world space. The delta rotation for static bodies will be +// identity using a dummy state. Base separation and angles need to be computed. Manifolds will be behind a frame, but that +// is probably best if bodies move fast. +// +// Solution 2: +// Use full rotation. The anchors for static bodies will be in world space while the anchors for dynamic bodies will be in local +// space. Potentially confusing and bug prone. +// +// Note: +// I rotate joint anchors each sub-step but not contact anchors. Joint stability improves a lot by rotating joint anchors +// according to substep progress. Contacts have reduced stability when anchors are rotated during substeps, especially for +// round shapes. +// +// 56 bytes +// todo_erin measure perf padding to 64 bytes +typedef struct b3BodyState +{ + b3Vec3 linearVelocity; // 12 + b3Vec3 angularVelocity; // 12 + + // Using delta position reduces round-off error far from the origin + b3Vec3 deltaPosition; // 12 + + // Using delta rotation because I cannot access the full rotation on static bodies in + // the solver and must use zero delta rotation for static bodies (c,s) = (1,0) + b3Quat deltaRotation; // 16 + + // b3BodyFlags + // Important flags: locking, dynamic + uint32_t flags; // 4 +} b3BodyState; + +// Identity body state, notice the deltaRotation is identity +static const b3BodyState b3_identityBodyState = { + { 0.0f, 0.0f, 0.0f }, { 0.0f, 0.0f, 0.0f }, { 0.0f, 0.0f, 0.0f }, { { 0.0f, 0.0f, 0.0f }, 1.0f }, 0, +}; + +// Body simulation data used for integration of position and velocity +// Transform data used for collision and solver preparation. +typedef struct b3BodySim +{ + // transform for body origin, double translation in large world mode + b3WorldTransform transform; + + // center of mass position in world space + b3Pos center; + + // previous rotation and COM for TOI + b3Quat rotation0; + b3Pos center0; + + // location of center of mass relative to the body origin + b3Vec3 localCenter; + + b3Vec3 force; + b3Vec3 torque; + + float invMass; + + // Rotational inertia about the center of mass. The world space inverse inertia tensor + // must be updated whenever the body rotation is modified. + b3Matrix3 invInertiaLocal; + b3Matrix3 invInertiaWorld; + + float minExtent; + b3Vec3 maxExtent; + float maxAngularVelocity; + float linearDamping; + float angularDamping; + float gravityScale; + + // Index of b3Body + int bodyId; + + // b3BodyFlags + uint32_t flags; +} b3BodySim; + +// Get a validated body from a world using an id. +b3Body* b3GetBodyFullId( b3World* world, b3BodyId bodyId ); + +b3WorldTransform b3GetBodyTransformQuick( b3World* world, b3Body* body ); +b3WorldTransform b3GetBodyTransform( b3World* world, int bodyId ); + +// Create a b3BodyId from a raw id. +b3BodyId b3MakeBodyId( b3World* world, int bodyId ); + +bool b3ShouldBodiesCollide( b3World* world, b3Body* bodyA, b3Body* bodyB ); +bool b3IsBodyAwake( b3World* world, b3Body* body ); + +b3BodySim* b3GetBodySim( b3World* world, b3Body* body ); +b3BodyState* b3GetBodyState( b3World* world, b3Body* body ); + +// careful calling this because it can invalidate body, state, joint, and contact pointers +bool b3WakeBody( b3World* world, b3Body* body ); +bool b3WakeBodyWithLock( b3World* world, b3Body* body ); + +void b3UpdateBodyMassData( b3World* world, b3Body* body ); +void b3SyncBodyFlags( b3World* world, b3Body* body ); + +// Make a sweep relative to a base position to keep TOI in float precision far from the origin. +static inline b3Sweep b3MakeRelativeSweep( const b3BodySim* bodySim, b3Pos base ) +{ + b3Sweep s; + s.c1 = b3SubPos( bodySim->center0, base ); + s.c2 = b3SubPos( bodySim->center, base ); + s.q1 = bodySim->rotation0; + s.q2 = bodySim->transform.q; + s.localCenter = bodySim->localCenter; + return s; +} + diff --git a/vendor/box3d/src/src/box3d.natvis b/vendor/box3d/src/src/box3d.natvis new file mode 100644 index 000000000..68d36e1c2 --- /dev/null +++ b/vendor/box3d/src/src/box3d.natvis @@ -0,0 +1,147 @@ + + + + + + [ {x}, {y} ] + + + + [ {x}, {y}, {z} ] + + + + [ {v}, {s} ] + + + + + [ {x}, {y}, {z} ] + + + + [ {p}, {q} ] + + + + index: {index1} + + b3_worlds[ index1 - 1 ] + + + + + + {b3_worlds[world0].bodies.data[index1-1].name,na}, + {b3_worlds[world0].bodies.data[index1-1].type} + + + b3_worlds[ world0 ].bodies.data[index1-1] + + + + + + {b3_worlds[world0].shapes.data[index1-1].type} + + + + + + {point}, sep = {separation} + + + + + + {int(owner1)}:{int(index1)}, {int(owner2)}:{int(index2)} + + + + + count: { pointCount }, triangle: { triangleIndex }, cluster: {clusterIndex} + + normal + feature + triangleFlags + + pointCount + points + + + + + + {{{count} : {radius}}} + + count + radius + + count + points + + + + + + type: {type} + + witnessA + witnessB + sweepA + sweepB + proxyA + proxyB + + + + + {{{count} / {capacity}}} + + count + capacity + + count + data + + + + + + + {{{count} / {capacity}}} + + count + capacity + + count + data + + + + + + {{{count} / {capacity}}} + + count + capacity + + count + data + + + + + + {{{count} / {capacity}}} + + count + capacity + + count + data + + + + + diff --git a/vendor/box3d/src/src/broad_phase.c b/vendor/box3d/src/src/broad_phase.c new file mode 100644 index 000000000..a369ca62a --- /dev/null +++ b/vendor/box3d/src/src/broad_phase.c @@ -0,0 +1,535 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +#include "broad_phase.h" + +#include "aabb.h" +#include "arena_allocator.h" +#include "body.h" +#include "contact.h" +#include "core.h" +#include "parallel_for.h" +#include "physics_world.h" +#include "platform.h" +#include "shape.h" + +#include + +void b3CreateBroadPhase( b3BroadPhase* bp, const b3Capacity* capacity ) +{ + _Static_assert( b3_bodyTypeCount == 3, "must be three body types" ); + + bp->movedProxies[b3_staticBody] = b3CreateBitSet( b3MaxInt( 16, capacity->staticShapeCount ) ); + bp->movedProxies[b3_kinematicBody] = b3CreateBitSet( 16 ); + bp->movedProxies[b3_dynamicBody] = b3CreateBitSet( b3MaxInt( 16, capacity->dynamicShapeCount ) ); + b3Array_Reserve( bp->moveArray, capacity->dynamicShapeCount ); + bp->moveResults = NULL; + bp->movePairs = NULL; + bp->movePairCapacity = 0; + b3AtomicStoreInt( &bp->movePairIndex, 0 ); + bp->pairSet = b3CreateSet( 2 * capacity->contactCount ); + + int staticCapacity = b3MaxInt( 16, capacity->staticShapeCount ); + bp->trees[b3_staticBody] = b3DynamicTree_Create( staticCapacity ); + + int kinematicCapacity = 16; + bp->trees[b3_kinematicBody] = b3DynamicTree_Create( kinematicCapacity ); + + int dynamicCapacity = b3MaxInt( 16, capacity->dynamicShapeCount ); + bp->trees[b3_dynamicBody] = b3DynamicTree_Create( dynamicCapacity ); +} + +void b3DestroyBroadPhase( b3BroadPhase* bp ) +{ + for ( int i = 0; i < b3_bodyTypeCount; ++i ) + { + b3DynamicTree_Destroy( bp->trees + i ); + } + + for ( int i = 0; i < b3_bodyTypeCount; ++i ) + { + b3DestroyBitSet( &bp->movedProxies[i] ); + } + b3Array_Destroy( bp->moveArray ); + b3DestroySet( &bp->pairSet ); + + *bp = (b3BroadPhase){ 0 }; + + memset( bp, 0, sizeof( b3BroadPhase ) ); +} + +static void b3UnBufferMove( b3BroadPhase* bp, int proxyKey ) +{ + b3BodyType proxyType = B3_PROXY_TYPE( proxyKey ); + int proxyId = B3_PROXY_ID( proxyKey ); + b3BitSet* set = &bp->movedProxies[proxyType]; + + if ( b3GetBit( set, proxyId ) ) + { + b3ClearBit( set, proxyId ); + + // Purge from move buffer. Linear search. + // todo if I can iterate the move set then I don't need the moveArray + int count = bp->moveArray.count; + for ( int i = 0; i < count; ++i ) + { + if ( bp->moveArray.data[i] == proxyKey ) + { + b3Array_RemoveSwap( bp->moveArray, i ); + break; + } + } + } +} + +int b3BroadPhase_CreateProxy( b3BroadPhase* bp, b3BodyType proxyType, b3AABB aabb, uint64_t categoryBits, int shapeIndex, + bool forcePairCreation ) +{ + B3_ASSERT( 0 <= proxyType && proxyType < b3_bodyTypeCount ); + int proxyId = b3DynamicTree_CreateProxy( bp->trees + proxyType, aabb, categoryBits, shapeIndex ); + int proxyKey = B3_PROXY_KEY( proxyId, proxyType ); + if ( proxyType != b3_staticBody || forcePairCreation ) + { + b3BufferMove( bp, proxyKey ); + } + return proxyKey; +} + +void b3BroadPhase_DestroyProxy( b3BroadPhase* bp, int proxyKey ) +{ + b3UnBufferMove( bp, proxyKey ); + + b3BodyType proxyType = B3_PROXY_TYPE( proxyKey ); + int proxyId = B3_PROXY_ID( proxyKey ); + + B3_ASSERT( 0 <= proxyType && proxyType <= b3_bodyTypeCount ); + b3DynamicTree_DestroyProxy( bp->trees + proxyType, proxyId ); +} + +void b3BroadPhase_MoveProxy( b3BroadPhase* bp, int proxyKey, b3AABB aabb ) +{ + b3BodyType proxyType = B3_PROXY_TYPE( proxyKey ); + int proxyId = B3_PROXY_ID( proxyKey ); + + b3DynamicTree_MoveProxy( bp->trees + proxyType, proxyId, aabb ); + b3BufferMove( bp, proxyKey ); +} + +void b3BroadPhase_EnlargeProxy( b3BroadPhase* bp, int proxyKey, b3AABB aabb ) +{ + B3_ASSERT( proxyKey != B3_NULL_INDEX ); + int typeIndex = B3_PROXY_TYPE( proxyKey ); + int proxyId = B3_PROXY_ID( proxyKey ); + + B3_ASSERT( typeIndex != b3_staticBody ); + + b3DynamicTree_EnlargeProxy( bp->trees + typeIndex, proxyId, aabb ); + b3BufferMove( bp, proxyKey ); +} + +typedef struct b3MovePair +{ + int shapeIndexA; + int shapeIndexB; + int childIndex; + b3MovePair* next; + bool heap; +} b3MovePair; + +typedef struct b3MoveResult +{ + b3MovePair* pairList; +} b3MoveResult; + +typedef struct b3QueryPairContext +{ + b3World* world; + b3MoveResult* moveResult; + b3AABB aabb; + b3BodyType queryTreeType; + int queryProxyKey; + int queryShapeIndex; + + int compoundProxyId; + int compoundShapeIndex; +} b3QueryPairContext; + +// This is called from b3DynamicTree::Query when we are gathering pairs. +static bool b3PairQueryCallback( int proxyId, uint64_t userData, void* context ) +{ + b3QueryPairContext* queryContext = (b3QueryPairContext*)context; + b3World* world = queryContext->world; + int shapeIndex; + int childIndex = 0; + + if ( queryContext->compoundShapeIndex == B3_NULL_INDEX ) + { + // Outer query: userData is a shape index. + shapeIndex = (int)userData; + + // A proxy cannot form a pair with itself. + if ( shapeIndex == queryContext->queryShapeIndex ) + { + return true; + } + + b3Shape* shape = b3Array_Get( world->shapes, shapeIndex ); + if ( shape->type == b3_compoundShape ) + { + // Query bounds are float world space, so the demoted transform is the matching float frame + b3Transform compoundTransform = b3ToRelativeTransform( b3GetBodyTransform( world, shape->bodyId ), b3Pos_zero ); + b3AABB localAABB = b3AABB_Transform( b3InvertTransform( compoundTransform ), queryContext->aabb ); + + // recurse + queryContext->compoundShapeIndex = shapeIndex; + queryContext->compoundProxyId = proxyId; + + b3DynamicTree_Query( &shape->compound->tree, localAABB, B3_DEFAULT_MASK_BITS, false, b3PairQueryCallback, context ); + queryContext->compoundShapeIndex = B3_NULL_INDEX; + queryContext->compoundProxyId = B3_NULL_INDEX; + return true; + } + } + else + { + // Inner query into a compound shape: userData is the compound child index, not a shape + // index, so do not compare it against queryShapeIndex. + shapeIndex = queryContext->compoundShapeIndex; + proxyId = queryContext->compoundProxyId; + childIndex = (int)userData; + } + + b3BroadPhase* broadPhase = &queryContext->world->broadPhase; + + int proxyKey = B3_PROXY_KEY( proxyId, queryContext->queryTreeType ); + int queryProxyKey = queryContext->queryProxyKey; + + // A proxy cannot form a pair with itself. + B3_ASSERT( proxyKey != queryContext->queryProxyKey ); + + b3BodyType treeType = queryContext->queryTreeType; + b3BodyType queryProxyType = B3_PROXY_TYPE( queryProxyKey ); + + // De-duplication + // It is important to prevent duplicate contacts from being created. Ideally I can prevent duplicates + // early and in the worker. Most of the time the movedProxies bit sets contain dynamic and kinematic + // proxies, but sometimes static proxies are in there too (b3ShapeDef::invokeContactCreation or a + // modified static shape), so we always have to check. + + // Is this proxy also moving? + if ( queryProxyType == b3_dynamicBody ) + { + if ( treeType == b3_dynamicBody && proxyKey < queryProxyKey ) + { + bool moved = b3GetBit( &broadPhase->movedProxies[treeType], proxyId ); + if ( moved ) + { + // Both proxies are moving. Avoid duplicate pairs. + return true; + } + } + } + else + { + B3_ASSERT( treeType == b3_dynamicBody ); + bool moved = b3GetBit( &broadPhase->movedProxies[treeType], proxyId ); + if ( moved ) + { + // Both proxies are moving. Avoid duplicate pairs. + return true; + } + } + + uint64_t pairKey = b3ShapePairKey( shapeIndex, queryContext->queryShapeIndex, childIndex ); + if ( b3ContainsKey( &broadPhase->pairSet, pairKey ) ) + { + // contact exists + return true; + } + + // Order shapes so that B3_SHAPE_PAIR_KEY works correctly + int shapeIdA = shapeIndex; + int shapeIdB = queryContext->queryShapeIndex; + b3Shape* shapeA = b3Array_Get( world->shapes, shapeIdA ); + b3Shape* shapeB = b3Array_Get( world->shapes, shapeIdB ); + int bodyIdA = shapeA->bodyId; + int bodyIdB = shapeB->bodyId; + + // Are the shapes on the same body? + if ( bodyIdA == bodyIdB ) + { + return true; + } + + // Sensors are handled elsewhere + if ( shapeA->sensorIndex != B3_NULL_INDEX || shapeB->sensorIndex != B3_NULL_INDEX ) + { + return true; + } + + if ( b3ShouldShapesCollide( shapeA->filter, shapeB->filter ) == false ) + { + return true; + } + + // Does a joint override collision? + b3Body* bodyA = b3Array_Get( world->bodies, bodyIdA ); + b3Body* bodyB = b3Array_Get( world->bodies, bodyIdB ); + if ( b3ShouldBodiesCollide( world, bodyA, bodyB ) == false ) + { + return true; + } + + // Custom user filter + if ( ( shapeA->flags & b3_enableCustomFiltering ) || ( shapeB->flags & b3_enableCustomFiltering ) ) + { + b3CustomFilterFcn* customFilterFcn = queryContext->world->customFilterFcn; + if ( customFilterFcn != NULL ) + { + b3ShapeId idA = { shapeIdA + 1, world->worldId, shapeA->generation }; + b3ShapeId idB = { shapeIdB + 1, world->worldId, shapeB->generation }; + bool shouldCollide = customFilterFcn( idA, idB, queryContext->world->customFilterContext ); + if ( shouldCollide == false ) + { + return true; + } + } + } + + // todo per thread to eliminate atomic? + int pairIndex = b3AtomicFetchAddInt( &broadPhase->movePairIndex, 1 ); + + b3MovePair* pair; + if ( pairIndex < broadPhase->movePairCapacity ) + { + pair = broadPhase->movePairs + pairIndex; + pair->heap = false; + } + else + { + // todo experimenting with ignoring this pair if we ran out of space + return true; + // pair = (b3MovePair*)b3Alloc( sizeof( b3MovePair ) ); + // pair->heap = true; + } + + pair->shapeIndexA = shapeIdA; + pair->shapeIndexB = shapeIdB; + pair->childIndex = childIndex; + pair->next = queryContext->moveResult->pairList; + queryContext->moveResult->pairList = pair; + + // continue the query + return true; +} + +static void b3FindPairsTask( int startIndex, int endIndex, int workerIndex, void* context ) +{ + b3TracyCZoneNC( pair_task, "Pair Task", b3_colorAquamarine, true ); + + B3_UNUSED( workerIndex ); + + b3World* world = (b3World*)context; + b3BroadPhase* bp = &world->broadPhase; + + b3QueryPairContext queryContext = { 0 }; + queryContext.world = world; + queryContext.compoundShapeIndex = B3_NULL_INDEX; + + for ( int i = startIndex; i < endIndex; ++i ) + { + // Initialize move result for this moved proxy + queryContext.moveResult = bp->moveResults + i; + queryContext.moveResult->pairList = NULL; + + int proxyKey = bp->moveArray.data[i]; + b3BodyType proxyType = B3_PROXY_TYPE( proxyKey ); + + int proxyId = B3_PROXY_ID( proxyKey ); + queryContext.queryProxyKey = proxyKey; + + const b3DynamicTree* baseTree = bp->trees + proxyType; + + // We have to query the tree with the fat AABB so that + // we don't fail to create a contact that may touch later. + b3AABB fatAABB = b3DynamicTree_GetAABB( baseTree, proxyId ); + queryContext.queryShapeIndex = (int)b3DynamicTree_GetUserData( baseTree, proxyId ); + queryContext.aabb = fatAABB; + + // Compound shape collision invocation is not supported + B3_VALIDATE( world->shapes.data[queryContext.queryShapeIndex].type != b3_compoundShape ); + + // Query trees. Only dynamic proxies collide with kinematic and static proxies. + // Using B3_DEFAULT_MASK_BITS so that b3Filter::groupIndex works. + // consider using bits = groupIndex > 0 ? B3_DEFAULT_MASK_BITS : maskBits + bool requireAllBits = false; + if ( proxyType == b3_dynamicBody ) + { + queryContext.queryTreeType = b3_kinematicBody; + b3DynamicTree_Query( bp->trees + b3_kinematicBody, fatAABB, B3_DEFAULT_MASK_BITS, requireAllBits, b3PairQueryCallback, + &queryContext ); + + queryContext.queryTreeType = b3_staticBody; + b3DynamicTree_Query( bp->trees + b3_staticBody, fatAABB, B3_DEFAULT_MASK_BITS, requireAllBits, b3PairQueryCallback, + &queryContext ); + } + + // All proxies collide with dynamic proxies + // Using B3_DEFAULT_MASK_BITS so that b3Filter::groupIndex works. + queryContext.queryTreeType = b3_dynamicBody; + b3DynamicTree_Query( bp->trees + b3_dynamicBody, fatAABB, B3_DEFAULT_MASK_BITS, requireAllBits, b3PairQueryCallback, + &queryContext ); + } + + b3TracyCZoneEnd( pair_task ); +} + +static void b3UpdateTreesTask( void* context ) +{ + b3TracyCZoneNC( tree_task, "Rebuild Trees", b3_colorFireBrick, true ); + + b3World* world = (b3World*)context; + b3DynamicTree_Rebuild( world->broadPhase.trees + b3_dynamicBody, false ); + b3DynamicTree_Rebuild( world->broadPhase.trees + b3_kinematicBody, false ); + + b3TracyCZoneEnd( tree_task ); +} + +void b3UpdateBroadPhasePairs( b3World* world ) +{ + b3BroadPhase* bp = &world->broadPhase; + + int moveCount = bp->moveArray.count; + + if ( moveCount == 0 ) + { + return; + } + + b3TracyCZoneNC( update_pairs, "Pairs", b3_colorMediumSlateBlue, true ); + + b3Stack* alloc = &world->stack; + + // todo these could be in the step context + bp->moveResults = (b3MoveResult*)b3StackAlloc( alloc, moveCount * sizeof( b3MoveResult ), "move results" ); + bp->movePairCapacity = 16 * moveCount; + bp->movePairs = (b3MovePair*)b3StackAlloc( alloc, bp->movePairCapacity * sizeof( b3MovePair ), "move pairs" ); + + b3AtomicStoreInt( &bp->movePairIndex, 0 ); + +#ifndef NDEBUG + extern b3AtomicInt b3_probeCount; + b3AtomicStoreInt( &b3_probeCount, 0 ); +#endif + + int minRange = 64; + b3ParallelFor( world, b3FindPairsTask, moveCount, minRange, world, "pairs" ); + + b3TracyCZoneNC( create_contacts, "Create Contacts", b3_colorCoral, true ); + + // Task that can be done in parallel with the narrow-phase + // - rebuild the collision tree for dynamic and kinematic bodies to keep their query performance good + if ( world->taskCount < B3_MAX_TASKS ) + { + world->userTreeTask = world->enqueueTaskFcn( &b3UpdateTreesTask, world, world->userTaskContext, "rebuild tree" ); + world->taskCount += 1; + world->activeTaskCount += world->userTreeTask == NULL ? 0 : 1; + } + else + { + world->userTreeTask = NULL; + b3UpdateTreesTask( world ); + } + + // Single-threaded work + // - Clear move flags + // - Create contacts in deterministic order + for ( int i = 0; i < moveCount; ++i ) + { + b3MoveResult* result = bp->moveResults + i; + b3MovePair* pair = result->pairList; + while ( pair != NULL ) + { + int shapeIdA = pair->shapeIndexA; + int shapeIdB = pair->shapeIndexB; + int childIndex = pair->childIndex; + + b3Shape* shapeA = b3Array_Get( world->shapes, shapeIdA ); + b3Shape* shapeB = b3Array_Get( world->shapes, shapeIdB ); + + b3CreateContact( world, shapeA, shapeB, childIndex ); + + if ( pair->heap ) + { + b3MovePair* temp = pair; + pair = pair->next; + b3Free( temp, sizeof( b3MovePair ) ); + } + else + { + pair = pair->next; + } + } + } + + // Reset move buffer: clear only the bits that were set this step. + // Invariant: bit set in movedProxies[type] iff proxyKey is present in moveArray. + for ( int i = 0; i < bp->moveArray.count; ++i ) + { + int proxyKey = bp->moveArray.data[i]; + b3ClearBit( &bp->movedProxies[B3_PROXY_TYPE( proxyKey )], B3_PROXY_ID( proxyKey ) ); + } + b3Array_Clear( bp->moveArray ); + + b3StackFree( alloc, bp->movePairs ); + bp->movePairs = NULL; + b3StackFree( alloc, bp->moveResults ); + bp->moveResults = NULL; + + b3ValidateSolverSets( world ); + + b3TracyCZoneEnd( create_contacts ); + + b3TracyCZoneEnd( update_pairs ); +} + +bool b3BroadPhase_TestOverlap( const b3BroadPhase* bp, int proxyKeyA, int proxyKeyB ) +{ + int typeIndexA = B3_PROXY_TYPE( proxyKeyA ); + int proxyIdA = B3_PROXY_ID( proxyKeyA ); + int typeIndexB = B3_PROXY_TYPE( proxyKeyB ); + int proxyIdB = B3_PROXY_ID( proxyKeyB ); + + b3AABB aabbA = b3DynamicTree_GetAABB( bp->trees + typeIndexA, proxyIdA ); + b3AABB aabbB = b3DynamicTree_GetAABB( bp->trees + typeIndexB, proxyIdB ); + return b3AABB_Overlaps( aabbA, aabbB ); +} + +int b3BroadPhase_GetShapeIndex( b3BroadPhase* bp, int proxyKey ) +{ + int typeIndex = B3_PROXY_TYPE( proxyKey ); + int proxyId = B3_PROXY_ID( proxyKey ); + + return (int)b3DynamicTree_GetUserData( bp->trees + typeIndex, proxyId ); +} + +void b3ValidateBroadPhase( const b3BroadPhase* bp ) +{ + b3DynamicTree_Validate( bp->trees + b3_dynamicBody ); + b3DynamicTree_Validate( bp->trees + b3_kinematicBody ); + + // todo validate every shape AABB is contained in tree AABB +} + +void b3ValidateNoEnlarged( const b3BroadPhase* bp ) +{ +#if B3_ENABLE_VALIDATION == 1 + for ( int j = 0; j < b3_bodyTypeCount; ++j ) + { + const b3DynamicTree* tree = bp->trees + j; + b3DynamicTree_ValidateNoEnlarged( tree ); + } +#else + B3_UNUSED( bp ); +#endif +} diff --git a/vendor/box3d/src/src/broad_phase.h b/vendor/box3d/src/src/broad_phase.h new file mode 100644 index 000000000..dcd626b3a --- /dev/null +++ b/vendor/box3d/src/src/broad_phase.h @@ -0,0 +1,79 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +#pragma once + +#include "bitset.h" +#include "container.h" +#include "table.h" + +#include "box3d/collision.h" +#include "box3d/types.h" + +typedef struct b3Shape b3Shape; +typedef struct b3MovePair b3MovePair; +typedef struct b3MoveResult b3MoveResult; +typedef struct b3Stack b3Stack; +typedef struct b3World b3World; + +// Store the proxy type in the lower 2 bits of the proxy key. This leaves 30 bits for the id. +#define B3_PROXY_TYPE( KEY ) ( (b3BodyType)( ( KEY ) & 3 ) ) +#define B3_PROXY_ID( KEY ) ( ( KEY ) >> 2 ) +#define B3_PROXY_KEY( ID, TYPE ) ( ( ( ID ) << 2 ) | ( TYPE ) ) + +/// The broad-phase is used for computing pairs and performing volume queries and ray casts. +/// This broad-phase does not persist pairs. Instead, this reports potentially new pairs. +/// It is up to the client to consume the new pairs and to track subsequent overlap. +typedef struct b3BroadPhase +{ + b3DynamicTree trees[b3_bodyTypeCount]; + + // Per body-type bit sets indexed by proxyId, marking proxies moved this step. + // Paired with moveArray which preserves deterministic insertion order for pair queries. + b3BitSet movedProxies[b3_bodyTypeCount]; + b3Array( int ) moveArray; + + // These are the results from the pair query and are used to create new contacts + // in deterministic order. + // todo these could be in the step context + b3MoveResult* moveResults; + b3MovePair* movePairs; + int movePairCapacity; + b3AtomicInt movePairIndex; + + // Tracks shape pairs that have a b3Contact + // todo pairSet can grow quite large on the first time step and remain large + b3HashSet pairSet; +} b3BroadPhase; + +void b3CreateBroadPhase( b3BroadPhase* bp, const b3Capacity* capacity ); +void b3DestroyBroadPhase( b3BroadPhase* bp ); + +int b3BroadPhase_CreateProxy( b3BroadPhase* bp, b3BodyType proxyType, b3AABB aabb, uint64_t categoryBits, int shapeIndex, + bool forcePairCreation ); +void b3BroadPhase_DestroyProxy( b3BroadPhase* bp, int proxyKey ); + +void b3BroadPhase_MoveProxy( b3BroadPhase* bp, int proxyKey, b3AABB aabb ); +void b3BroadPhase_EnlargeProxy( b3BroadPhase* bp, int proxyKey, b3AABB aabb ); + +int b3BroadPhase_GetShapeIndex( b3BroadPhase* bp, int proxyKey ); + +void b3UpdateBroadPhasePairs( b3World* world ); +bool b3BroadPhase_TestOverlap( const b3BroadPhase* bp, int proxyKeyA, int proxyKeyB ); + +void b3ValidateBroadPhase( const b3BroadPhase* bp ); +void b3ValidateNoEnlarged( const b3BroadPhase* bp ); + +// This is what triggers new contact pairs to be created +// Warning: this must be called in deterministic order +static inline void b3BufferMove( b3BroadPhase* bp, int queryProxy ) +{ + b3BodyType proxyType = B3_PROXY_TYPE( queryProxy ); + int proxyId = B3_PROXY_ID( queryProxy ); + b3BitSet* set = &bp->movedProxies[proxyType]; + if ( b3GetBit( set, proxyId ) == false ) + { + b3SetBitGrow( set, proxyId ); + b3Array_Push( bp->moveArray, queryProxy ); + } +} diff --git a/vendor/box3d/src/src/capsule.c b/vendor/box3d/src/src/capsule.c new file mode 100644 index 000000000..249e8cbf2 --- /dev/null +++ b/vendor/box3d/src/src/capsule.c @@ -0,0 +1,385 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +#include "math_internal.h" +#include "shape.h" + +#include "box3d/base.h" +#include "box3d/collision.h" +#include "box3d/constants.h" + +b3MassData b3ComputeCapsuleMass( const b3Capsule* shape, float density ) +{ + b3Vec3 c1 = shape->center1; + b3Vec3 c2 = shape->center2; + float r = shape->radius; + + // Cylinder + float cylinderHeight = b3Distance( c1, c2 ); + float cylinderVolume = B3_PI * r * r * cylinderHeight; + float cylinderMass = cylinderVolume * density; + + // Sphere + float sphereVolume = ( 4.0f / 3.0f ) * B3_PI * r * r * r; + float sphereMass = sphereVolume * density; + + // Local accumulated inertia + b3Matrix3 inertia = b3AddMM( b3CylinderInertia( cylinderMass, r, cylinderHeight ), b3SphereInertia( sphereMass, r ) ); + + float steiner = 0.125f * sphereMass * ( 3.0f * r + 2.0f * cylinderHeight ) * cylinderHeight; + inertia.cx.x += steiner; + inertia.cz.z += steiner; + + // Align capsule axis with chosen up-axis + b3Matrix3 rotation = b3Mat3_identity; + if ( cylinderHeight * cylinderHeight > 1000.0f * FLT_MIN ) + { + b3Vec3 direction = b3Normalize( b3Sub( c2, c1 ) ); + b3Quat q = b3ComputeQuatBetweenUnitVectors( b3Vec3_axisY, direction ); + rotation = b3MakeMatrixFromQuat( q ); + } + + float mass = sphereMass + cylinderMass; + b3Vec3 center = b3MulSV( 0.5f, b3Add( c1, c2 ) ); + + b3MassData out; + out.mass = mass; + out.center = center; + + // Rotate the central inertia into the shape frame + out.inertia = b3MulMM( rotation, b3MulMM( inertia, b3Transpose( rotation ) ) ); + + return out; +} + +b3AABB b3ComputeCapsuleAABB( const b3Capsule* shape, b3Transform transform ) +{ + float r = shape->radius; + + b3Vec3 center1 = b3TransformPoint( transform, shape->center1 ); + b3Vec3 center2 = b3TransformPoint( transform, shape->center2 ); + b3Vec3 extent = { r, r, r }; + + b3AABB aabb; + aabb.lowerBound = b3Sub( b3Min( center1, center2 ), extent ); + aabb.upperBound = b3Add( b3Max( center1, center2 ), extent ); + return aabb; +} + +b3AABB b3ComputeSweptCapsuleAABB( const b3Capsule* shape, b3Transform xf1, b3Transform xf2 ) +{ + b3Vec3 r = { shape->radius, shape->radius, shape->radius }; + b3Vec3 a = b3TransformPoint( xf1, shape->center1 ); + b3Vec3 b = b3TransformPoint( xf1, shape->center2 ); + b3Vec3 c = b3TransformPoint( xf2, shape->center1 ); + b3Vec3 d = b3TransformPoint( xf2, shape->center2 ); + + b3AABB aabb = { + .lowerBound = b3Sub( b3Min( b3Min( a, b ), b3Min( c, d ) ), r ), + .upperBound = b3Add( b3Max( b3Max( a, b ), b3Max( c, d ) ), r ), + }; + return aabb; +} + +bool b3OverlapCapsule( const b3Capsule* shape, b3Transform shapeTransform, const b3ShapeProxy* proxy ) +{ + b3DistanceInput input; + input.proxyA = (b3ShapeProxy){ &shape->center1, 2, shape->radius }; + input.proxyB = *proxy; + input.transform = b3InvMulTransforms( shapeTransform, b3Transform_identity ); + input.useRadii = true; + + b3SimplexCache cache = { 0 }; + b3DistanceOutput output = b3ShapeDistance( &input, &cache, NULL, 0 ); + return output.distance < B3_OVERLAP_SLOP; +} + +// Precision Improvements for Ray / Sphere Intersection - Ray Tracing Gems 2019 +// http://www.codercorner.com/blog/?p=321 +b3CastOutput b3RayCastCapsule( const b3Capsule* shape, const b3RayCastInput* input ) +{ + B3_ASSERT( b3IsValidRay( input ) ); + + b3Vec3 c1 = shape->center1; + b3Vec3 c2 = shape->center2; + float r = shape->radius; + + // Initialize result structure + b3CastOutput output = { 0 }; + + b3Vec3 d = b3Sub( c2, c1 ); + + // Fall back to sphere if the capsule is short + float tol = 0.01f * B3_LINEAR_SLOP; + float lengthSquared = b3LengthSquared( d ); + if ( lengthSquared < tol * tol ) + { + b3Vec3 sphereCenter = b3MulSV( 0.5f, b3Add( shape->center1, shape->center2 ) ); + b3Sphere sphere = { sphereCenter, shape->radius }; + return b3RayCastSphere( &sphere, input ); + } + + // Vector from first center to ray origin. + b3Vec3 s = b3Sub( input->origin, c1 ); + + // Capsule axis + float length = sqrtf( lengthSquared ); + b3Vec3 axis = b3MulSV( 1.0f / length, d ); + + // Project ray origin onto capsule axis. + float u = b3Dot( s, axis ); + + // Closest point on infinite capsule axis, relative to c1. + b3Vec3 c = b3MulSV( u, axis ); + + // Vector from closest point to ray origin + b3Vec3 sc = b3Sub( s, c ); + + // Squared distance from ray origin to capsule axis + float sc2 = b3LengthSquared( sc ); + + // Is the ray origin within the infinite cylinder along the capsule axis? + if ( sc2 < r * r ) + { + // Clamped barycentric coordinate of ray origin projected onto capsule axis. + float uClamped = b3ClampFloat( u, 0.0f, length ); + + // The closest point on the bounded capsule segment, relative to c1. + b3Vec3 cp = b3MulSV( uClamped, axis ); + + // Vector from ray origin to closest point on segment. + b3Vec3 scp = b3Sub( s, cp ); + + // Squared distance of ray origin from capsule segment. + float scp2 = b3LengthSquared( scp ); + + // Is the ray origin within the capsule? + if ( scp2 < r * r ) + { + output.hit = true; + output.point = input->origin; + return output; + } + + // The ray can hit an endcap. + b3Sphere sphere = { + .center = b3Add( c1, cp ), + .radius = r, + }; + + return b3RayCastSphere( &sphere, input ); + } + + // Ray axis. A zero length ray reaching here starts outside the capsule, so it misses. + // Same zero length convention as b3RayCastSphere. + b3Vec3 dr = input->translation; + float rayLength; + b3Vec3 rayAxis = b3GetLengthAndNormalize( &rayLength, dr ); + if ( rayLength == 0.0f ) + { + return output; + } + + // Barycentric coordinate of ray end point. + float v = u + input->maxFraction * b3Dot( dr, axis ); + + // Early out: does the projected ray fall outside the capsule? + if ( ( u < -r && v < -r ) || ( length + r < u && length + r < v ) ) + { + return output; + } + + // Compute the closest point between the ray segment and the capsule segment. + // See Real-Time Collision Detection, section 5.1.9 + + // Closest point on capsule : a1 = segment unit axis, t1 = unknown fraction + // p1 = t1 * a1 + + // Closet point on ray : a2 = ray unit axis, t2 = unknown fraction + // p2 = s + t2 * a2 + + // Closest point perpendicularity conditions. + // dot(p2 - p1, a1) = 0 + // dot(p2 - p1, a2) = 0 + + // Expand + // dot(t1 * a1 - s - t2 * a2, a1) = 0 + // dot(t1 * a1 - s - t2 * a2, a2) = 0 + + // Expand + // t1 - dot(s, a1) - t2 * dot(a1, a2) = 0 + // t1 * dot(a1, a2) - dot(s, a2) - t2 = 0 + + // Group : a12 = dot(a1, a2), sa1 = dot(s, a1), sa2 = dot(s, a2) + // t1 - a12 * t2 = sa1 + // a12 * t1 - t2 = sa2 + + // Solve + // https://en.wikipedia.org/wiki/Cramer%27s_rule + // I've flipped the signs of the numerator and denominator to give a positive determinant. + // det = 1 - a12 * a12 + // t1 = (sa1 - a12 * sa2) / det + // t2 = (a12 * sa1 - sa2) / det + + b3Vec3 a1 = axis; + b3Vec3 a2 = rayAxis; + float a12 = b3Dot( a1, a2 ); + + // Ray distance to the near intersection with the infinite cylinder. Length units. + float tr; + + float det = 1.0f - a12 * a12; + if ( det < FLT_EPSILON ) + { + // Solve the 2D problem of ray versus circle starting at the ray origin, where the circle is + // the axial view of the infinite capsule cylinder. This works well when the ray origin is + // not too far from the capsule axis. + + // Instead of a cross product, subtract the parallel part to get a perpendicular vector. Non-dimensional. + b3Vec3 perp = b3MulSub( a2, a12, a1 ); + float perp2 = b3LengthSquared( perp ); + + // Project to origin to infinite capsule axis vector onto the perpendicular vector. beta has length units. + float beta = b3Dot( sc, perp ); + + // Setup quadratic root finder. + float gamma = sc2 - r * r; + + // Discriminant + float disc = beta * beta - perp2 * gamma; + + // Casting away from the axis, or the perpendicular gap never closes to the radius. + if ( beta >= 0.0f || disc < 0.0f ) + { + return output; + } + + // Quadratic near root. Expressed in an alternate form to avoid the (-beta - sqrt) cancellation as + // the ray nears parallel. + tr = gamma / ( -beta + sqrtf( disc ) ); + } + else + { + // Ray and capsules axes are not parallel. + + // Closest points between the infinite ray and the infinite capsule axis. + float invDet = 1.0f / det; + float sa1 = u; + float sa2 = b3Dot( s, a2 ); + + float t1 = ( sa1 - a12 * sa2 ) * invDet; + float t2 = ( a12 * sa1 - sa2 ) * invDet; + + // Closest points + b3Vec3 p1 = b3MulSV( t1, a1 ); + b3Vec3 p2 = b3MulAdd( s, t2, a2 ); + + // Vector from closest point on infinite capsule to infinite ray. + b3Vec3 g = b3Sub( p2, p1 ); + + float g2 = b3LengthSquared( g ); + if ( g2 > r * r ) + { + // Early out: closest point on infinite ray is outside infinite cylinder. + return output; + } + + // Intersect the infinite ray with the infinite cylinder. Like ray versus sphere this is done + // relative to the closest point to avoid round-off errors. Length units, not a fraction. + // https://en.wikipedia.org/wiki/Line-cylinder_intersection + float h = sqrtf( ( r * r - g2 ) * invDet ); + + tr = t2 - h; + } + + // Outside ray? + if ( tr < 0.0f || input->maxFraction * rayLength < tr ) + { + return output; + } + + // The corresponding distance on the capsule axis. Length units. + float tc = u + tr * a12; + + // Outside c1 end? + if ( tc < 0.0f ) + { + // Ray cast sphere 1. + b3Sphere sphere = { + .center = c1, + .radius = r, + }; + + return b3RayCastSphere( &sphere, input ); + } + + // Outside c2 end? + if (length < tc) + { + // Ray cast sphere 2. + b3Sphere sphere = { + .center = c2, + .radius = r, + }; + + return b3RayCastSphere( &sphere, input ); + } + + // Hit point on capsule side, relative to c1. + b3Vec3 p = b3MulAdd( s, tr, rayAxis ); + + // Hit normal. + b3Vec3 normal = b3MulSub( p, tc, axis ); + normal = b3Normalize( normal ); + + output.point = b3Add( c1, p ); + output.normal = normal; + output.fraction = b3ClampFloat( tr / rayLength, 0.0f, input->maxFraction ); + output.hit = true; + return output; +} + +b3CastOutput b3ShapeCastCapsule( const b3Capsule* capsule, const b3ShapeCastInput* input ) +{ + b3ShapeCastPairInput pairInput; + pairInput.proxyA = (b3ShapeProxy){ &capsule->center1, 2, capsule->radius }; + pairInput.proxyB = input->proxy; + pairInput.transform = b3Transform_identity; + pairInput.translationB = input->translation; + pairInput.maxFraction = input->maxFraction; + pairInput.canEncroach = input->canEncroach; + + b3CastOutput output = b3ShapeCast( &pairInput ); + return output; +} + +int b3CollideMoverAndCapsule( b3PlaneResult* result, const b3Capsule* shape, const b3Capsule* mover ) +{ + float totalRadius = mover->radius + shape->radius; + + b3SegmentDistanceResult approach = b3SegmentDistance( shape->center1, shape->center2, mover->center1, mover->center2 ); + + // The normal points from the shape toward the mover. + float distance; + b3Vec3 normal = b3GetLengthAndNormalize( &distance, b3Sub( approach.point2, approach.point1 ) ); + + if ( distance > totalRadius ) + { + return 0; + } + + float linearSlop = B3_LINEAR_SLOP; + if ( distance < linearSlop ) + { + // Deep overlap: the core segments intersect. Pick an arbitrary direction perpendicular + // the to capsule axis. + float moverLength; + b3Vec3 moverAxis = b3GetLengthAndNormalize( &moverLength, b3Sub( mover->center2, mover->center1 ) ); + normal = moverLength > linearSlop ? b3Perp( moverAxis ) : b3Vec3_axisY; + distance = 0.0f; + } + + b3Plane plane = { normal, totalRadius - distance }; + *result = (b3PlaneResult){ plane, approach.point1 }; + return 1; +} diff --git a/vendor/box3d/src/src/compound.c b/vendor/box3d/src/src/compound.c new file mode 100644 index 000000000..29da3c662 --- /dev/null +++ b/vendor/box3d/src/src/compound.c @@ -0,0 +1,1196 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +#include "compound.h" + +#include "hull_map.h" +#include "math_internal.h" +#include "shape.h" + +#include "box3d/base.h" +#include "box3d/collision.h" +#include "box3d/constants.h" +#include "box3d/types.h" + +#include +#include + +typedef struct b3SharedHull +{ + const b3HullData* hull; + int hullOffset; +} b3SharedHull; + +typedef struct b3SharedMesh +{ + const b3MeshData* meshData; + int meshOffset; +} b3SharedMesh; + +typedef struct b3HullInstance +{ + b3Transform transform; + uint32_t hullOffset; + uint32_t materialIndex; +} b3HullInstance; + +typedef struct b3MeshInstance +{ + b3Transform transform; + b3Vec3 scale; + uint32_t meshOffset; + uint32_t materialIndices[B3_MAX_COMPOUND_MESH_MATERIALS]; +} b3MeshInstance; + +static inline b3TreeNode* b3GetCompoundNodes( b3CompoundData* compound ) +{ + if ( compound->nodeOffset == 0 ) + { + return NULL; + } + + return (b3TreeNode*)( (intptr_t)compound + compound->nodeOffset ); +} + +const b3SurfaceMaterial* b3GetCompoundMaterials( const b3CompoundData* compound ) +{ + if ( compound->materialOffset == 0 ) + { + return NULL; + } + + return (b3SurfaceMaterial*)( (intptr_t)compound + compound->materialOffset ); +} + +b3CompoundCapsule b3GetCompoundCapsule( const b3CompoundData* compound, int index ) +{ + B3_ASSERT( 0 <= index && index < compound->capsuleCount && compound->capsuleOffset > 0 ); + + b3CompoundCapsule result = { 0 }; + if ( compound->capsuleOffset == 0 ) + { + return result; + } + + const b3CompoundCapsule* capsules = (const b3CompoundCapsule*)( (intptr_t)compound + compound->capsuleOffset ); + return capsules[index]; +} + +b3CompoundHull b3GetCompoundHull( const b3CompoundData* compound, int index ) +{ + B3_ASSERT( 0 <= index && index < compound->hullCount && compound->hullOffset > 0 ); + + b3CompoundHull result = { 0 }; + if ( compound->hullOffset == 0 ) + { + return result; + } + + const b3HullInstance* hullInstances = (const b3HullInstance*)( (intptr_t)compound + compound->hullOffset ); + uint32_t hullOffset = hullInstances[index].hullOffset; + B3_ASSERT( hullOffset >= compound->hullOffset + compound->hullCount * sizeof( b3HullInstance ) ); + result.hull = (const b3HullData*)( (intptr_t)compound + hullOffset ); + result.transform = hullInstances[index].transform; + result.materialIndex = hullInstances[index].materialIndex; + return result; +} + +b3CompoundMesh b3GetCompoundMesh( const b3CompoundData* compound, int index ) +{ + B3_ASSERT( 0 <= index && index < compound->meshCount && compound->meshOffset > 0 ); + + b3CompoundMesh result = { 0 }; + if ( compound->meshOffset == 0 ) + { + return result; + } + + const b3MeshInstance* meshInstances = (const b3MeshInstance*)( (intptr_t)compound + compound->meshOffset ); + uint32_t meshOffset = meshInstances[index].meshOffset; + B3_ASSERT( meshOffset >= compound->meshOffset + compound->meshCount * sizeof( b3HullInstance ) ); + result.meshData = (const b3MeshData*)( (intptr_t)compound + meshOffset ); + result.transform = meshInstances[index].transform; + result.scale = meshInstances[index].scale; + for ( int i = 0; i < B3_MAX_COMPOUND_MESH_MATERIALS; ++i ) + { + result.materialIndices[i] = meshInstances[index].materialIndices[i]; + } + return result; +} + +b3CompoundSphere b3GetCompoundSphere( const b3CompoundData* compound, int index ) +{ + B3_ASSERT( 0 <= index && index < compound->sphereCount && compound->sphereOffset > 0 ); + + b3CompoundSphere result = { 0 }; + if ( compound->sphereOffset == 0 ) + { + return result; + } + + const b3CompoundSphere* spheres = (const b3CompoundSphere*)( (intptr_t)compound + compound->sphereOffset ); + return spheres[index]; +} + +b3ChildShape b3GetCompoundChild( const b3CompoundData* compound, int childIndex ) +{ + // Capsule? + if ( 0 <= childIndex && childIndex < compound->capsuleCount ) + { + b3CompoundCapsule compoundCapsule = b3GetCompoundCapsule( compound, childIndex ); + return (b3ChildShape){ + .capsule = compoundCapsule.capsule, + .transform = b3Transform_identity, + .materialIndices = { compoundCapsule.materialIndex }, + .type = b3_capsuleShape, + }; + } + childIndex -= compound->capsuleCount; + + // Hull? + if ( 0 <= childIndex && childIndex < compound->hullCount ) + { + b3CompoundHull compoundHull = b3GetCompoundHull( compound, childIndex ); + return (b3ChildShape){ + .hull = compoundHull.hull, + .transform = compoundHull.transform, + .materialIndices = { compoundHull.materialIndex }, + .type = b3_hullShape, + }; + } + childIndex -= compound->hullCount; + + // Mesh? + if ( 0 <= childIndex && childIndex < compound->meshCount ) + { + b3CompoundMesh compoundMesh = b3GetCompoundMesh( compound, childIndex ); + const int* m = compoundMesh.materialIndices; + _Static_assert( B3_MAX_COMPOUND_MESH_MATERIALS == 4, "too many materials in compound mesh" ); + + return (b3ChildShape){ + .mesh = + { + .data = compoundMesh.meshData, + .scale = compoundMesh.scale, + }, + .transform = compoundMesh.transform, + .materialIndices = { m[0], m[1], m[2], m[3] }, + .type = b3_meshShape, + }; + } + childIndex -= compound->meshCount; + + B3_ASSERT( 0 <= childIndex && childIndex < compound->sphereCount ); + + // Sphere? + { + b3CompoundSphere compoundSphere = b3GetCompoundSphere( compound, childIndex ); + return (b3ChildShape){ + .sphere = compoundSphere.sphere, + .transform = b3Transform_identity, + .materialIndices = { compoundSphere.materialIndex }, + .type = b3_sphereShape, + }; + } +} + +static inline size_t vt_wyhash( const void* key, size_t len ); + +static inline uint64_t b3HashMesh( const b3MeshData* mesh ) +{ + return vt_wyhash( mesh, mesh->byteCount ); +} + +static bool b3CompareMeshes( const b3MeshData* mesh1, const b3MeshData* mesh2 ) +{ + if ( mesh1 == mesh2 ) + { + return true; + } + + if ( mesh1->byteCount != mesh2->byteCount ) + { + return false; + } + + int result = memcmp( mesh1, mesh2, mesh1->byteCount ); + return result == 0; +} + +#define NAME b3MeshMap +#define KEY_TY const b3MeshData* +#define VAL_TY int +#define HASH_FN b3HashMesh +#define CMPR_FN b3CompareMeshes +#define MALLOC_FN b3Alloc +#define FREE_FN b3Free +#include "verstable.h" + +static inline uint64_t b3HashMaterial( const b3SurfaceMaterial* material ) +{ + return vt_wyhash( material, sizeof( b3SurfaceMaterial ) ); +} + +static bool b3CompareMaterials( const b3SurfaceMaterial* mat1, const b3SurfaceMaterial* mat2 ) +{ + if ( mat1 == mat2 ) + { + return true; + } + + int result = memcmp( mat1, mat2, sizeof( b3SurfaceMaterial ) ); + return result == 0; +} + +#define NAME b3MaterialMap +#define KEY_TY const b3SurfaceMaterial* +#define VAL_TY int +#define HASH_FN b3HashMaterial +#define CMPR_FN b3CompareMaterials +#define MALLOC_FN b3Alloc +#define FREE_FN b3Free +#include "verstable.h" + +b3CompoundData* b3CreateCompound( const b3CompoundDef* def ) +{ + int convexCount = def->capsuleCount + def->hullCount + def->sphereCount; + int shapeCount = convexCount + def->meshCount; + + if ( shapeCount >= B3_MAX_CHILD_SHAPES ) + { + B3_ASSERT( false ); + return NULL; + } + + b3DynamicTree tree = b3DynamicTree_Create( shapeCount ); + + int childIndex = 0; + + // Instances + int capsuleCount = def->capsuleCount; + b3CompoundCapsule* capsuleInstances = b3AllocZeroed( capsuleCount * sizeof( b3CompoundCapsule ) ); + int hullCount = def->hullCount; + b3HullInstance* hullInstances = b3AllocZeroed( hullCount * sizeof( b3HullInstance ) ); + int meshCount = def->meshCount; + b3MeshInstance* meshInstances = b3AllocZeroed( meshCount * sizeof( b3MeshInstance ) ); + int sphereCount = def->sphereCount; + b3CompoundSphere* sphereInstances = b3AllocZeroed( sphereCount * sizeof( b3CompoundSphere ) ); + + // Determine material capacity + int materialCapacity = convexCount; + for ( int i = 0; i < def->meshCount; ++i ) + { + B3_ASSERT( def->meshes[i].materialCount > 0 ); + materialCapacity += def->meshes[i].materialCount; + } + + // Material map for convex material sharing. Mesh materials are not shared for simplicity. + b3MaterialMap materialMap; + b3MaterialMap_init( &materialMap ); + b3MaterialMap_reserve( &materialMap, materialCapacity ); + b3SurfaceMaterial* materials = b3AllocZeroed( materialCapacity * sizeof( b3SurfaceMaterial ) ); + int materialCount = 0; + + for ( int i = 0; i < def->capsuleCount; ++i ) + { + const b3CompoundCapsuleDef* capsuleDef = def->capsules + i; + capsuleInstances[i].capsule = capsuleDef->capsule; + + // Look for an existing material + b3MaterialMap_itr materialItr = b3MaterialMap_get_or_insert( &materialMap, &capsuleDef->material, materialCount ); + + // Get the shared material index + int materialIndex = materialItr.data->val; + capsuleInstances[i].materialIndex = materialIndex; + + // Is this a new material? + if ( materialIndex == materialCount ) + { + materials[materialIndex] = capsuleDef->material; + materialCount += 1; + } + + b3AABB aabb = b3ComputeCapsuleAABB( &capsuleDef->capsule, b3Transform_identity ); + b3DynamicTree_CreateProxy( &tree, aabb, ~0ull, childIndex ); + childIndex += 1; + } + + // Hulls + b3SharedHull* sharedHulls = b3AllocZeroed( hullCount * sizeof( b3SharedHull ) ); + int sharedHullCount = 0; + + if ( hullCount > 0 ) + { + b3HullMap hullMap; + b3HullMap_init( &hullMap ); + b3HullMap_reserve( &hullMap, hullCount ); + + for ( int i = 0; i < hullCount; ++i ) + { + const b3CompoundHullDef* hullDef = def->hulls + i; + const b3HullData* hull = hullDef->hull; + b3AABB aabb = b3ComputeHullAABB( hull, hullDef->transform ); + b3DynamicTree_CreateProxy( &tree, aabb, ~0ull, childIndex ); + childIndex += 1; + + // Look for an existing material + b3MaterialMap_itr materialItr = b3MaterialMap_get_or_insert( &materialMap, &hullDef->material, materialCount ); + + // Get the shared material index + int materialIndex = materialItr.data->val; + hullInstances[i].materialIndex = materialIndex; + + // Is this a new material? + if ( materialIndex == materialCount ) + { + materials[materialIndex] = hullDef->material; + materialCount += 1; + } + + hullInstances[i].transform = hullDef->transform; + + // Look for an existing matching hull + b3HullMap_itr itr = b3HullMap_get_or_insert( &hullMap, hull, sharedHullCount ); + + // Get the unique index for this hull + int sharedHullIndex = itr.data->val; + + // The offset isn't known yet, so store the index of the shared hull + hullInstances[i].hullOffset = sharedHullIndex; + + // Is this a new hull? + if ( sharedHullIndex == sharedHullCount ) + { + // Create a shared hull. The offset is determined below. + sharedHulls[sharedHullIndex].hull = hull; + sharedHulls[sharedHullIndex].hullOffset = B3_NULL_INDEX; + sharedHullCount += 1; + } + } + + b3HullMap_cleanup( &hullMap ); + } + + // Meshes + b3SharedMesh* sharedMeshes = b3AllocZeroed( meshCount * sizeof( b3SharedMesh ) ); + int sharedMeshCount = 0; + + if ( meshCount > 0 ) + { + b3MeshMap meshMap; + b3MeshMap_init( &meshMap ); + b3MeshMap_reserve( &meshMap, meshCount ); + + for ( int i = 0; i < meshCount; ++i ) + { + const b3CompoundMeshDef* meshDef = def->meshes + i; + + const b3MeshData* meshData = meshDef->meshData; + b3AABB aabb = b3ComputeMeshAABB( meshData, meshDef->transform, meshDef->scale ); + b3DynamicTree_CreateProxy( &tree, aabb, ~0ull, childIndex ); + childIndex += 1; + + // No effort to share mesh materials. It would be easier to do if the number of materials was limited. + B3_ASSERT( meshData->materialCount == meshDef->materialCount ); + + for ( int j = 0; j < meshDef->materialCount; ++j ) + { + // Look for an existing material + b3MaterialMap_itr materialItr = + b3MaterialMap_get_or_insert( &materialMap, &meshDef->materials[j], materialCount ); + + // Get the shared material index + int materialIndex = materialItr.data->val; + meshInstances[i].materialIndices[j] = materialIndex; + + // Is this a new material? + if ( materialIndex == materialCount ) + { + materials[materialIndex] = meshDef->materials[j]; + materialCount += 1; + } + } + + // Look for an existing matching mesh + b3MeshMap_itr itr = b3MeshMap_get_or_insert( &meshMap, meshData, sharedMeshCount ); + + // Get the shared mesh index + int sharedMeshIndex = itr.data->val; + + // Create mesh instance + meshInstances[i].transform = def->meshes[i].transform; + meshInstances[i].scale = def->meshes[i].scale; + + // The offset isn't known yet, so store the index of the shared mesh + meshInstances[i].meshOffset = sharedMeshIndex; + + // Is this a new mesh? + if ( sharedMeshIndex == sharedMeshCount ) + { + // Create a shared mesh. The offset is determined below. + sharedMeshes[sharedMeshIndex].meshData = meshData; + sharedMeshes[sharedMeshIndex].meshOffset = B3_NULL_INDEX; + sharedMeshCount += 1; + } + } + + b3MeshMap_cleanup( &meshMap ); + } + + // Spheres + for ( int i = 0; i < def->sphereCount; ++i ) + { + const b3CompoundSphereDef* sphereDef = def->spheres + i; + sphereInstances[i].sphere = sphereDef->sphere; + + // Look for an existing material + b3MaterialMap_itr materialItr = b3MaterialMap_get_or_insert( &materialMap, &sphereDef->material, materialCount ); + + // Get the shared material index + int materialIndex = materialItr.data->val; + sphereInstances[i].materialIndex = materialIndex; + + // Is this a new material? + if ( materialIndex == materialCount ) + { + materials[materialIndex] = sphereDef->material; + materialCount += 1; + } + + b3AABB aabb = b3ComputeSphereAABB( &sphereDef->sphere, b3Transform_identity ); + b3DynamicTree_CreateProxy( &tree, aabb, ~0ull, childIndex ); + childIndex += 1; + } + + B3_ASSERT( materialCount <= materialCapacity ); + B3_ASSERT( tree.nodeCount > 0 ); + + b3DynamicTree_Rebuild( &tree, true ); + + int byteCount = sizeof( b3CompoundData ); + + // Tree nodes - todo 64 byte alignment + int nodeOffset = byteCount; + byteCount += tree.nodeCapacity * sizeof( b3TreeNode ); + + int materialOffset = byteCount; + byteCount += materialCount * sizeof( b3SurfaceMaterial ); + + int capsuleOffset = byteCount; + byteCount += def->capsuleCount * sizeof( b3CompoundCapsule ); + + // Hull data layout has another level of indirection to allow for tight data packing + // 1. hull instance array : hull count array of b3HullInstance with individual hull transforms and offsets + // 2. heterogeneous array of shared hull data : each shared hull can have a different byte count, so direct indexing is not + // possible + int hullArrayOffset = byteCount; + + // Array of hull instances + byteCount += hullCount * sizeof( b3HullInstance ); + + // Packed shared hull blobs + for ( int i = 0; i < sharedHullCount; ++i ) + { + sharedHulls[i].hullOffset = byteCount; + byteCount += sharedHulls[i].hull->byteCount; + } + + // Mesh data layout has another level of indirection to allow for tight data packing + // 1. mesh instance array : mesh count array of b3MeshInstance with individual mesh transform, scale, and offset + // 2. heterogeneous array of shared mesh data : each shared mesh can have a different byte count, so direct indexing is not + // possible + int meshArrayOffset = byteCount; + + // Array of mesh instances + byteCount += meshCount * sizeof( b3MeshInstance ); + + // Packed shared mesh blobs + for ( int i = 0; i < sharedMeshCount; ++i ) + { + sharedMeshes[i].meshOffset = byteCount; + byteCount += sharedMeshes[i].meshData->byteCount; + } + + int sphereOffset = byteCount; + byteCount += def->sphereCount * sizeof( b3CompoundSphere ); + + b3CompoundData* compound = b3Alloc( byteCount ); + memset( compound, 0, byteCount ); + + compound->version = B3_COMPOUND_VERSION; + compound->byteCount = byteCount; + compound->nodeOffset = nodeOffset; + memcpy( &compound->tree, &tree, sizeof( b3DynamicTree ) ); + + // todo clean up this mess + compound->tree.freeList = 0; + compound->tree.leafIndices = NULL; + compound->tree.leafBoxes = NULL; + compound->tree.leafCenters = NULL; + compound->tree.binIndices = NULL; + compound->tree.rebuildCapacity = 0; + + compound->tree.nodes = NULL; + compound->materialOffset = materialOffset; + compound->materialCount = materialCount; + compound->capsuleOffset = capsuleOffset; + compound->capsuleCount = capsuleCount; + compound->hullOffset = hullArrayOffset; + compound->hullCount = hullCount; + compound->meshOffset = meshArrayOffset; + compound->meshCount = meshCount; + compound->sphereOffset = sphereOffset; + compound->sphereCount = sphereCount; + + // Tree nodes + b3TreeNode* nodes = b3GetCompoundNodes( compound ); + memcpy( nodes, tree.nodes, tree.nodeCapacity * sizeof( b3TreeNode ) ); + compound->tree.nodes = nodes; + + // Materials + B3_ASSERT( materialCount > 0 ); + b3SurfaceMaterial* destinationMaterials = (b3SurfaceMaterial*)( (intptr_t)compound + compound->materialOffset ); + if ( materials != NULL ) + { + memcpy( destinationMaterials, materials, materialCount * sizeof( b3SurfaceMaterial ) ); + } + + // Capsules + if ( def->capsuleCount > 0 ) + { + B3_ASSERT( compound->capsuleOffset > 0 ); + b3CompoundCapsule* capsules = (b3CompoundCapsule*)( (intptr_t)compound + compound->capsuleOffset ); + memcpy( capsules, capsuleInstances, capsuleCount * sizeof( b3CompoundCapsule ) ); + } + + // Hulls + for ( int i = 0; i < hullCount; ++i ) + { + // Fix up offsets + int sharedIndex = hullInstances[i].hullOffset; + B3_ASSERT( 0 <= sharedIndex && sharedIndex < sharedHullCount ); + hullInstances[i].hullOffset = sharedHulls[sharedIndex].hullOffset; + } + + b3HullInstance* destinationHullInstances = (b3HullInstance*)( (intptr_t)compound + hullArrayOffset ); + memcpy( destinationHullInstances, hullInstances, hullCount * sizeof( b3HullInstance ) ); + + for ( int i = 0; i < sharedHullCount; ++i ) + { + int offset = sharedHulls[i].hullOffset; + b3HullData* destinationHull = (b3HullData*)( (intptr_t)compound + offset ); + memcpy( destinationHull, sharedHulls[i].hull, sharedHulls[i].hull->byteCount ); + } + + compound->sharedHullCount = sharedHullCount; + + // Meshes + for ( int i = 0; i < meshCount; ++i ) + { + // Fix up offsets + int sharedIndex = meshInstances[i].meshOffset; + B3_ASSERT( 0 <= sharedIndex && sharedIndex < sharedMeshCount ); + meshInstances[i].meshOffset = sharedMeshes[sharedIndex].meshOffset; + } + + b3MeshInstance* destinationMeshInstances = (b3MeshInstance*)( (intptr_t)compound + meshArrayOffset ); + memcpy( destinationMeshInstances, meshInstances, meshCount * sizeof( b3MeshInstance ) ); + + for ( int i = 0; i < sharedMeshCount; ++i ) + { + int offset = sharedMeshes[i].meshOffset; + b3MeshData* destinationMesh = (b3MeshData*)( (intptr_t)compound + offset ); + memcpy( destinationMesh, sharedMeshes[i].meshData, sharedMeshes[i].meshData->byteCount ); + } + + compound->sharedMeshCount = sharedMeshCount; + + // Spheres + if ( def->sphereCount > 0 ) + { + B3_ASSERT( compound->sphereOffset > 0 ); + b3CompoundSphere* spheres = (b3CompoundSphere*)( (intptr_t)compound + compound->sphereOffset ); + memcpy( spheres, sphereInstances, sphereCount * sizeof( b3CompoundSphere ) ); + } + + b3MaterialMap_cleanup( &materialMap ); + b3Free( sharedHulls, hullCount * sizeof( b3SharedHull ) ); + b3Free( sharedMeshes, meshCount * sizeof( b3SharedMesh ) ); + b3Free( capsuleInstances, capsuleCount * sizeof( b3CompoundCapsule ) ); + b3Free( hullInstances, hullCount * sizeof( b3HullInstance ) ); + b3Free( meshInstances, meshCount * sizeof( b3MeshInstance ) ); + b3Free( sphereInstances, sphereCount * sizeof( b3CompoundSphere ) ); + b3Free( materials, materialCapacity * sizeof( b3SurfaceMaterial ) ); + b3DynamicTree_Destroy( &tree ); + + return compound; +} + +void b3DestroyCompound( b3CompoundData* compound ) +{ + b3Free( compound, compound->byteCount ); +} + +uint8_t* b3ConvertCompoundToBytes( b3CompoundData* compound ) +{ + // scrub this pointer before serialization + compound->tree.nodes = NULL; + return (uint8_t*)compound; +} + +b3CompoundData* b3ConvertBytesToCompound( uint8_t* bytes, int byteCount ) +{ + b3CompoundData* compound = (b3CompoundData*)bytes; + if ( compound->version != B3_COMPOUND_VERSION ) + { + return NULL; + } + + if ( compound->byteCount < (int)sizeof( b3CompoundData ) ) + { + return NULL; + } + + if ( byteCount != compound->byteCount ) + { + return NULL; + } + + if ( compound->nodeOffset <= 0 ) + { + return NULL; + } + + // this mutates the input bytes + compound->tree.nodes = (b3TreeNode*)( (intptr_t)compound + compound->nodeOffset ); + return compound; +} + +b3AABB b3ComputeCompoundAABB( const b3CompoundData* shape, b3Transform transform ) +{ + B3_ASSERT( shape->nodeOffset > 0 ); + + const b3TreeNode* nodes = (const b3TreeNode*)( (intptr_t)shape + shape->nodeOffset ); + int root = shape->tree.root; + b3AABB aabb = nodes[root].aabb; + return b3AABB_Transform( transform, aabb ); +} + +struct b3CompoundOverlapContext +{ + const b3CompoundData* compound; + // transform of the compound + b3Transform transform; + b3ShapeProxy proxy; + bool overlap; +}; + +static bool b3CompoundOverlapCallback( int proxyId, uint64_t userData, void* context ) +{ + B3_UNUSED( proxyId ); + + int childIndex = (int)userData; + struct b3CompoundOverlapContext* overlapContext = context; + b3ChildShape child = b3GetCompoundChild( overlapContext->compound, childIndex ); + + b3Transform transform = b3MulTransforms( overlapContext->transform, child.transform ); + + bool overlap = false; + switch ( child.type ) + { + case b3_capsuleShape: + overlap = b3OverlapCapsule( &child.capsule, transform, &overlapContext->proxy ); + break; + + case b3_hullShape: + overlap = b3OverlapHull( child.hull, transform, &overlapContext->proxy ); + break; + + case b3_meshShape: + overlap = b3OverlapMesh( &child.mesh, transform, &overlapContext->proxy ); + break; + + case b3_sphereShape: + overlap = b3OverlapSphere( &child.sphere, transform, &overlapContext->proxy ); + break; + + default: + B3_ASSERT( false ); + break; + } + + if ( overlap ) + { + // Done + overlapContext->overlap = true; + return false; + } + + // Continue the query if there is no overlap + return true; +} + +bool b3OverlapCompound( const b3CompoundData* shape, b3Transform shapeTransform, const b3ShapeProxy* proxy ) +{ + struct b3CompoundOverlapContext context = { + .compound = shape, + .transform = shapeTransform, + .proxy = *proxy, + .overlap = false, + }; + + b3AABB aabb = { proxy->points[0], proxy->points[0] }; + for ( int i = 1; i < proxy->count; ++i ) + { + aabb.lowerBound = b3Min( aabb.lowerBound, proxy->points[i] ); + aabb.upperBound = b3Max( aabb.upperBound, proxy->points[i] ); + } + + b3Vec3 r = { proxy->radius, proxy->radius, proxy->radius }; + aabb.lowerBound = b3Sub( aabb.lowerBound, r ); + aabb.upperBound = b3Add( aabb.upperBound, r ); + + (void)b3DynamicTree_Query( &shape->tree, aabb, ~0ull, false, b3CompoundOverlapCallback, &context ); + + return context.overlap; +} + +struct b3CompoundCastContext +{ + const b3CompoundData* compound; + b3CastOutput* output; + // origin of the shape cast, the box cast callback only carries the advancing fraction + const b3ShapeCastInput* shapeInput; +}; + +static float b3CompoundRayCastCallback( const b3RayCastInput* input, int proxyId, uint64_t userData, void* context ) +{ + B3_UNUSED( proxyId ); + + struct b3CompoundCastContext* castContext = context; + const b3CompoundData* compound = castContext->compound; + + int childIndex = (int)userData; + + b3ChildShape child = b3GetCompoundChild( compound, childIndex ); + + b3RayCastInput localInput = *input; + localInput.origin = b3InvTransformPoint( child.transform, input->origin ); + localInput.translation = b3InvRotateVector( child.transform.q, input->translation ); + + b3CastOutput output = { 0 }; + + switch ( child.type ) + { + case b3_capsuleShape: + output = b3RayCastCapsule( &child.capsule, &localInput ); + output.materialIndex = child.materialIndices[0]; + break; + + case b3_hullShape: + output = b3RayCastHull( child.hull, &localInput ); + output.materialIndex = child.materialIndices[0]; + break; + + case b3_meshShape: + { + output = b3RayCastMesh( &child.mesh, &localInput ); + B3_ASSERT( 0 <= output.materialIndex ); + int childMaterialIndex = b3MinInt( output.materialIndex, B3_MAX_COMPOUND_MESH_MATERIALS - 1 ); + output.materialIndex = child.materialIndices[childMaterialIndex]; + } + break; + + case b3_sphereShape: + output = b3RayCastSphere( &child.sphere, &localInput ); + output.materialIndex = child.materialIndices[0]; + break; + + default: + B3_ASSERT( false ); + break; + } + + if ( output.hit ) + { + output.point = b3TransformPoint( child.transform, output.point ); + output.normal = b3RotateVector( child.transform.q, output.normal ); + output.childIndex = childIndex; + *castContext->output = output; + return output.fraction; + } + + return input->maxFraction; +} + +b3CastOutput b3RayCastCompound( const b3CompoundData* shape, const b3RayCastInput* input ) +{ + b3CastOutput result = { 0 }; + + struct b3CompoundCastContext context = { + .compound = shape, + .output = &result, + }; + (void)b3DynamicTree_RayCast( &shape->tree, input, ~0ull, false, b3CompoundRayCastCallback, &context ); + return result; +} + +static float b3CompoundShapeCastCallback( const b3BoxCastInput* input, int proxyId, uint64_t userData, void* context ) +{ + B3_UNUSED( proxyId ); + + struct b3CompoundCastContext* castContext = context; + const b3CompoundData* compound = castContext->compound; + const b3ShapeCastInput* shapeInput = castContext->shapeInput; + + int childIndex = (int)userData; + + b3ChildShape child = b3GetCompoundChild( compound, childIndex ); + + // Rebuild from the carried shape cast input, taking only the advancing fraction from the tree + b3ShapeCastInput localInput = *shapeInput; + localInput.maxFraction = input->maxFraction; + b3Vec3 localPoints[B3_MAX_SHAPE_CAST_POINTS]; + + localInput.proxy.count = b3MinInt( shapeInput->proxy.count, B3_MAX_SHAPE_CAST_POINTS ); + + b3Transform invTransform = b3InvertTransform( child.transform ); + b3Matrix3 R = b3MakeMatrixFromQuat( invTransform.q ); + + for ( int i = 0; i < localInput.proxy.count; ++i ) + { + localPoints[i] = b3Add( b3MulMV( R, shapeInput->proxy.points[i] ), invTransform.p ); + } + + localInput.proxy.points = localPoints; + localInput.translation = b3MulMV( R, shapeInput->translation ); + + b3CastOutput output = { 0 }; + + switch ( child.type ) + { + case b3_capsuleShape: + output = b3ShapeCastCapsule( &child.capsule, &localInput ); + output.materialIndex = child.materialIndices[0]; + break; + + case b3_hullShape: + output = b3ShapeCastHull( child.hull, &localInput ); + output.materialIndex = child.materialIndices[0]; + break; + + case b3_meshShape: + { + output = b3ShapeCastMesh( &child.mesh, &localInput ); + B3_ASSERT( 0 <= output.materialIndex ); + int childMaterialIndex = b3MinInt( output.materialIndex, B3_MAX_COMPOUND_MESH_MATERIALS - 1 ); + output.materialIndex = child.materialIndices[childMaterialIndex]; + } + break; + + case b3_sphereShape: + output = b3ShapeCastSphere( &child.sphere, &localInput ); + output.materialIndex = child.materialIndices[0]; + break; + + default: + B3_ASSERT( false ); + break; + } + + if ( output.hit ) + { + output.point = b3TransformPoint( child.transform, output.point ); + output.normal = b3RotateVector( child.transform.q, output.normal ); + output.childIndex = childIndex; + *castContext->output = output; + return output.fraction; + } + + return input->maxFraction; +} + +b3CastOutput b3ShapeCastCompound( const b3CompoundData* shape, const b3ShapeCastInput* input ) +{ + b3CastOutput result = { 0 }; + + if ( input->proxy.count == 0 ) + { + return result; + } + + struct b3CompoundCastContext context = { + .compound = shape, + .output = &result, + .shapeInput = input, + }; + + // The compound tree is in the compound local frame, so the proxy box needs no origin offset + b3AABB box = b3MakeAABB( input->proxy.points, input->proxy.count, input->proxy.radius ); + b3BoxCastInput treeInput = { box, input->translation, input->maxFraction }; + (void)b3DynamicTree_BoxCast( &shape->tree, &treeInput, ~0ull, false, b3CompoundShapeCastCallback, &context ); + return result; +} + +struct b3CompoundQueryContext +{ + const b3CompoundData* compound; + b3CompoundQueryFcn* fcn; + void* userContext; +}; + +static bool TreeQueryCallbackFcn( int proxyId, uint64_t userData, void* treeContext ) +{ + B3_UNUSED( proxyId ); + struct b3CompoundQueryContext* context = treeContext; + return context->fcn( context->compound, (int)userData, context->userContext ); +} + +void b3QueryCompound( const b3CompoundData* compound, b3AABB aabb, b3CompoundQueryFcn* fcn, void* context ) +{ + struct b3CompoundQueryContext compoundContext = { + .compound = compound, + .fcn = fcn, + .userContext = context, + }; + + b3DynamicTree_Query( &compound->tree, aabb, B3_DEFAULT_MASK_BITS, false, TreeQueryCallbackFcn, &compoundContext ); +} + +#if 0 +struct b3CompoundImpactContext +{ + b3TOIInput toiInput; + b3TOIOutput toiOutput; + b3Transform compoundTransform; + + // Bounds local to compound + b3AABB localSweepBoundsB; + + // Centroid of shape in body B local space + b3Vec3 localCentroidB; + float fallbackRadius; +}; + +static bool b3CompoundTimeOfImpactFcn( const b3CompoundData* compound, int childIndex, void* context ) +{ + b3CompoundImpactContext* toiContext = (b3CompoundImpactContext*)context; + + b3ChildShape child = b3GetCompoundChild( compound, childIndex ); + + b3TOIOutput output = {0 }; + toiContext->toiInput.sweepA = b3MakeCompoundChildSweep( toiContext->compoundTransform, child.transform ); + + switch ( child.type ) + { + case b3_capsuleShape: + { + toiContext->toiInput.proxyA.points = &child.capsule.center1; + toiContext->toiInput.proxyA.count = 2; + toiContext->toiInput.proxyA.radius = child.capsule.radius; + output = b3TimeOfImpact( &toiContext->toiInput ); + } + break; + + case b3_hullShape: + { + toiContext->toiInput.proxyA.points = b3GetHullPoints( child.hull ); + toiContext->toiInput.proxyA.count = child.hull->vertexCount; + toiContext->toiInput.proxyA.radius = 0.0f; + output = b3TimeOfImpact( &toiContext->toiInput ); + } + break; + + case b3_meshShape: + { + b3MeshImpactContext meshContext = {0}; + meshContext.toiInput = toiContext->toiInput; + meshContext.isSensor = false; + meshContext.localCentroidB = toiContext->localCentroidB; + meshContext.fallbackRadius = toiContext->fallbackRadius; + + b3Transform meshWorldTransform = b3MulTransforms( toiContext->compoundTransform, child.transform ); + + const b3Sweep* sweepB = &toiContext->toiInput.sweepB; + b3Transform xfB1 = { + .p = sweepB->c1 - b3RotateVector( sweepB->q1, sweepB->localCenter ), + .q = sweepB->q1, + }; + + b3Transform xfB2 = { + .p = sweepB->c2 - b3RotateVector( sweepB->q2, sweepB->localCenter ), + .q = sweepB->q2, + }; + + meshContext.meshLocalCentroidB1 = + b3InvTransformPoint( meshWorldTransform, b3TransformPoint( xfB1, meshContext.localCentroidB ) ); + meshContext.meshLocalCentroidB2 = + b3InvTransformPoint( meshWorldTransform, b3TransformPoint( xfB2, meshContext.localCentroidB ) ); + + // Bounds local to mesh + b3AABB localBounds = b3AABB_Transform( b3InvertTransform( child.transform ), toiContext->localSweepBoundsB ); + + b3QueryMesh( &child.mesh, localBounds, b3MeshTimeOfImpactFcn, &meshContext ); + + output = meshContext.toiOutput; + } + break; + + case b3_sphereShape: + { + toiContext->toiInput.proxyA.points = &child.sphere.center; + toiContext->toiInput.proxyA.count = 1; + toiContext->toiInput.proxyA.radius = child.sphere.radius; + output = b3TimeOfImpact( &toiContext->toiInput ); + } + break; + + default: + B3_ASSERT( false ); + break; + } + + if ( 0.0f < output.fraction && output.fraction < toiContext->toiInput.maxFraction ) + { + toiContext->toiOutput = output; + toiContext->toiInput.maxFraction = output.fraction; + } + + // Clear this to be safe + toiContext->toiInput.proxyA = {0}; + + // Continue the query + return true; +} + +b3TOIOutput b3CompoundTimeOfImpact(const b3CompoundData* compound, b3Transform transform, const b3ShapeProxy* proxy, + const b3Sweep* sweep, float maxFraction) +{ + b3CompoundImpactContext context = {0}; + context.toiInput.proxyB = b3MakeShapeProxy( shapeB ); + context.toiInput.sweepB = *sweepB; + context.toiInput.maxFraction = maxFraction; + + context.compoundTransform = { + .p = sweepA->c1, + .q = sweepA->q1, + }; + + b3Vec3 localCentroidB = b3GetShapeCentroid( shapeB ); + context.localCentroidB = localCentroidB; + + b3ShapeExtent extents = b3ComputeShapeExtent( shapeB, context.localCentroidB ); + context.fallbackRadius = b3MaxFloat( 0.5f * extents.minExtent, B3_SPECULATIVE_DISTANCE ); + + // Swept bounds of shapeB + b3AABB aabb = b3ComputeSweptShapeAABB( shapeB, sweepB, maxFraction ); + + // Bounds local to mesh + b3AABB localBounds = b3AABB_Transform( b3InvertTransform( context.compoundTransform ), bounds ); + context.localSweepBoundsB = localBounds; + + b3DynamicTree_Query( &compound->tree, aabb, B3_DEFAULT_MASK_BITS, false, TreeQueryCallbackFcn, &compoundContext ); + + return context.toiOutput; +} +#endif + +// xf = xfP * xfC +b3Sweep b3MakeCompoundChildSweep( b3Transform compoundTransform, b3Transform childTransform ) +{ + b3Transform xf = b3MulTransforms( compoundTransform, childTransform ); + return (b3Sweep){ + .localCenter = b3Vec3_zero, + .c1 = xf.p, + .c2 = xf.p, + .q1 = xf.q, + .q2 = xf.q, + }; +} + +struct b3CompoundMoverContext +{ + const b3CompoundData* compound; + b3PlaneResult* planes; + int planeCapacity; + int planeCount; + b3Capsule mover; +}; + +static bool b3CompoundMoverCallback( int proxyId, uint64_t userData, void* context ) +{ + B3_UNUSED( proxyId ); + + int childIndex = (int)userData; + struct b3CompoundMoverContext* moverContext = context; + b3ChildShape child = b3GetCompoundChild( moverContext->compound, childIndex ); + + // Transform mover to child space + b3Capsule localMover; + localMover.center1 = b3InvTransformPoint( child.transform, moverContext->mover.center1 ); + localMover.center2 = b3InvTransformPoint( child.transform, moverContext->mover.center2 ); + localMover.radius = moverContext->mover.radius; + + int capacity = moverContext->planeCapacity - moverContext->planeCount; + B3_ASSERT( capacity > 0 ); + + b3PlaneResult* planes = moverContext->planes + moverContext->planeCount; + int planeCount = 0; + + switch ( child.type ) + { + case b3_capsuleShape: + planeCount = b3CollideMoverAndCapsule( planes, &child.capsule, &localMover ); + break; + + case b3_hullShape: + planeCount = b3CollideMoverAndHull( planes, child.hull, &localMover ); + break; + + case b3_meshShape: + planeCount = b3CollideMoverAndMesh( planes, capacity, &child.mesh, &localMover ); + break; + + case b3_sphereShape: + planeCount = b3CollideMoverAndSphere( planes, &child.sphere, &localMover ); + break; + + default: + B3_ASSERT( false ); + break; + } + + // Transform results back to shape space + for ( int i = 0; i < planeCount; ++i ) + { + planes[i].plane.normal = b3RotateVector( child.transform.q, planes[i].plane.normal ); + planes[i].point = b3TransformPoint( child.transform, planes[i].point ); + } + + moverContext->planeCount += planeCount; + + // Continue query while there is room for more planes + return moverContext->planeCount < moverContext->planeCapacity; +} + +int b3CollideMoverAndCompound( b3PlaneResult* planes, int capacity, const b3CompoundData* shape, const b3Capsule* mover ) +{ + struct b3CompoundMoverContext context = { + .compound = shape, + .planes = planes, + .planeCapacity = capacity, + .planeCount = 0, + .mover = *mover, + }; + + b3AABB aabb; + aabb.lowerBound = b3Min( mover->center1, mover->center2 ); + aabb.upperBound = b3Max( mover->center1, mover->center2 ); + b3Vec3 r = { mover->radius, mover->radius, mover->radius }; + aabb.lowerBound = b3Sub( aabb.lowerBound, r ); + aabb.upperBound = b3Add( aabb.upperBound, r ); + + (void)b3DynamicTree_Query( &shape->tree, aabb, ~0ull, false, b3CompoundMoverCallback, &context ); + + return context.planeCount; +} diff --git a/vendor/box3d/src/src/compound.h b/vendor/box3d/src/src/compound.h new file mode 100644 index 000000000..1f4d7e508 --- /dev/null +++ b/vendor/box3d/src/src/compound.h @@ -0,0 +1,14 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +#pragma once + +#include "box3d/types.h" + +b3TOIOutput b3CompoundTimeOfImpact( const b3CompoundData* compound, b3Transform transform, const b3ShapeProxy* proxy, + const b3Sweep* sweep, float maxFraction ); + +// Transforms a sweep for a compound child shape +b3Sweep b3MakeCompoundChildSweep( b3Transform compoundTransform, b3Transform childTransform ); + +int b3CollideMoverAndCompound( b3PlaneResult* planes, int capacity, const b3CompoundData* shape, const b3Capsule* mover ); diff --git a/vendor/box3d/src/src/constraint_graph.c b/vendor/box3d/src/src/constraint_graph.c new file mode 100644 index 000000000..9f06e9e4e --- /dev/null +++ b/vendor/box3d/src/src/constraint_graph.c @@ -0,0 +1,328 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +#include "constraint_graph.h" + +#include "bitset.h" +#include "body.h" +#include "contact.h" +#include "joint.h" +#include "physics_world.h" + +#include + +// Solver using graph coloring. Islands are only used for sleep. +// High-Performance Physical Simulations on Next-Generation Architecture with Many Cores +// http://web.eecs.umich.edu/~msmelyan/papers/physsim_onmanycore_itj.pdf + +// Kinematic bodies have to be treated like dynamic bodies in graph coloring. Unlike static bodies, we cannot use a dummy solver +// body for kinematic bodies. We cannot access a kinematic body from multiple threads efficiently because the SIMD solver body +// scatter would write to the same kinematic body from multiple threads. Even if these writes don't modify the body, they will +// cause horrible cache stalls. To make this feasible I would need a way to block these writes. + +// This is used for debugging by making all constraints be assigned to overflow. +#define B3_FORCE_OVERFLOW 0 + +static const b3HexColor b3_graphColors[B3_GRAPH_COLOR_COUNT] = { + b3_colorRed, b3_colorOrange, b3_colorYellow, b3_colorLimeGreen, b3_colorSpringGreen, + b3_colorAqua, b3_colorDodgerBlue, b3_colorBlueViolet, b3_colorMagenta, b3_colorDeepPink, + b3_colorCrimson, b3_colorCoral, b3_colorGold, b3_colorGreenYellow, b3_colorMediumSeaGreen, + b3_colorTurquoise, b3_colorDeepSkyBlue, b3_colorCornflowerBlue, b3_colorMediumSlateBlue, b3_colorMediumOrchid, + b3_colorHotPink, b3_colorTomato, b3_colorKhaki, b3_colorSilver, +}; + +b3HexColor b3GetGraphColor( int index ) +{ + B3_ASSERT( 0 <= index && index < B3_GRAPH_COLOR_COUNT ); + return b3_graphColors[index]; +} + +void b3CreateGraph( b3ConstraintGraph* graph, int bodyCapacity ) +{ + _Static_assert( B3_GRAPH_COLOR_COUNT >= 2, "must have at least two constraint graph colors" ); + _Static_assert( B3_OVERFLOW_INDEX == B3_GRAPH_COLOR_COUNT - 1, "bad over flow index" ); + + *graph = (b3ConstraintGraph){ 0 }; + + bodyCapacity = b3MaxInt( bodyCapacity, 8 ); + + // Initialize graph color bit set. + // No bitset for overflow color. + for ( int i = 0; i < B3_OVERFLOW_INDEX; ++i ) + { + b3GraphColor* color = graph->colors + i; + color->bodySet = b3CreateBitSet( bodyCapacity ); + b3SetBitCountAndClear( &color->bodySet, bodyCapacity ); + } +} + +void b3DestroyGraph( b3ConstraintGraph* graph ) +{ + for ( int i = 0; i < B3_GRAPH_COLOR_COUNT; ++i ) + { + b3GraphColor* color = graph->colors + i; + + // The bit set should never be used on the overflow color + B3_ASSERT( i != B3_OVERFLOW_INDEX || color->bodySet.bits == NULL ); + + b3DestroyBitSet( &color->bodySet ); + + b3Array_Destroy( color->convexContacts ); + b3Array_Destroy( color->contacts ); + b3Array_Destroy( color->jointSims ); + } +} + +// Contacts are always created as non-touching. They get cloned into the constraint +// graph once they are found to be touching. +void b3AddContactToGraph( b3World* world, b3Contact* contact ) +{ + B3_ASSERT( contact->manifoldCount > 0 ); + B3_ASSERT( contact->flags & b3_contactTouchingFlag ); + + b3ConstraintGraph* graph = &world->constraintGraph; + int colorIndex = B3_OVERFLOW_INDEX; + + int bodyIdA = contact->edges[0].bodyId; + int bodyIdB = contact->edges[1].bodyId; + b3Body* bodyA = b3Array_Get( world->bodies, bodyIdA ); + b3Body* bodyB = b3Array_Get( world->bodies, bodyIdB ); + + b3BodyType typeA = bodyA->type; + b3BodyType typeB = bodyB->type; + B3_ASSERT( typeA == b3_dynamicBody || typeB == b3_dynamicBody ); + +#if B3_FORCE_OVERFLOW == 0 + if ( typeA == b3_dynamicBody && typeB == b3_dynamicBody ) + { + // Dynamic constraint colors cannot encroach on colors reserved for static constraints + for ( int i = 0; i < B3_DYNAMIC_COLOR_COUNT; ++i ) + { + b3GraphColor* color = graph->colors + i; + if ( b3GetBit( &color->bodySet, bodyIdA ) || b3GetBit( &color->bodySet, bodyIdB ) ) + { + continue; + } + + b3SetBitGrow( &color->bodySet, bodyIdA ); + b3SetBitGrow( &color->bodySet, bodyIdB ); + colorIndex = i; + break; + } + } + else if ( typeA == b3_dynamicBody ) + { + // Static constraint colors build from the end to get higher priority than dyn-dyn constraints + for ( int i = B3_OVERFLOW_INDEX - 1; i >= 1; --i ) + { + b3GraphColor* color = graph->colors + i; + if ( b3GetBit( &color->bodySet, bodyIdA ) ) + { + continue; + } + + b3SetBitGrow( &color->bodySet, bodyIdA ); + colorIndex = i; + break; + } + } + else if ( typeB == b3_dynamicBody ) + { + // Static constraint colors build from the end to get higher priority than dyn-dyn constraints + for ( int i = B3_OVERFLOW_INDEX - 1; i >= 1; --i ) + { + b3GraphColor* color = graph->colors + i; + if ( b3GetBit( &color->bodySet, bodyIdB ) ) + { + continue; + } + + b3SetBitGrow( &color->bodySet, bodyIdB ); + colorIndex = i; + break; + } + } +#endif + + bool isScalar = ( contact->flags & b3_simMeshContact ) || colorIndex == B3_OVERFLOW_INDEX; + + b3GraphColor* color = graph->colors + colorIndex; + contact->colorIndex = colorIndex; + contact->localIndex = isScalar ? color->contacts.count : color->convexContacts.count; + contact->bodySimIndexA = bodyA->type == b3_staticBody ? B3_NULL_INDEX : bodyA->localIndex; + contact->bodySimIndexB = bodyB->type == b3_staticBody ? B3_NULL_INDEX : bodyB->localIndex; + + if ( isScalar ) + { + B3_ASSERT( contact->manifoldCount < UINT16_MAX ); + b3ContactSpec spec = { + .contactId = contact->contactId, + .manifoldStart = 0, + .manifoldCount = (uint16_t)contact->manifoldCount, + }; + b3Array_Push( color->contacts, spec ); + } + else + { + b3Array_Push( color->convexContacts, contact->contactId ); + } +} + +void b3RemoveContactFromGraph( b3World* world, int bodyIdA, int bodyIdB, int colorIndex, int localIndex, bool meshContact ) +{ + b3ConstraintGraph* graph = &world->constraintGraph; + + B3_ASSERT( 0 <= colorIndex && colorIndex < B3_GRAPH_COLOR_COUNT ); + b3GraphColor* color = graph->colors + colorIndex; + + if ( colorIndex != B3_OVERFLOW_INDEX ) + { + // This might clear a bit for a static body, but this has no effect + b3ClearBit( &color->bodySet, bodyIdA ); + b3ClearBit( &color->bodySet, bodyIdB ); + } + + if ( meshContact || colorIndex == B3_OVERFLOW_INDEX ) + { + int movedIndex = b3Array_RemoveSwap( color->contacts, localIndex ); + if ( movedIndex != B3_NULL_INDEX ) + { + // Fix index on swapped contact + int movedContactId = color->contacts.data[localIndex].contactId; + b3Contact* movedContact = b3Array_Get( world->contacts, movedContactId ); + B3_ASSERT( movedContact->setIndex == b3_awakeSet ); + B3_ASSERT( movedContact->colorIndex == colorIndex ); + B3_ASSERT( movedContact->localIndex == movedIndex ); + movedContact->localIndex = localIndex; + } + } + else + { + int movedIndex = b3Array_RemoveSwap( color->convexContacts, localIndex ); + if ( movedIndex != B3_NULL_INDEX ) + { + // Fix index on swapped contact + int movedContactId = color->convexContacts.data[localIndex]; + b3Contact* movedContact = b3Array_Get( world->contacts, movedContactId ); + B3_ASSERT( movedContact->setIndex == b3_awakeSet ); + B3_ASSERT( movedContact->colorIndex == colorIndex ); + B3_ASSERT( movedContact->localIndex == movedIndex ); + B3_ASSERT( ( movedContact->flags & b3_simMeshContact ) == 0 ); + movedContact->localIndex = localIndex; + } + } +} + +static int b3AssignJointColor( b3ConstraintGraph* graph, int bodyIdA, int bodyIdB, b3BodyType typeA, b3BodyType typeB ) +{ + B3_ASSERT( typeA == b3_dynamicBody || typeB == b3_dynamicBody ); + B3_UNUSED( graph ); + B3_UNUSED( bodyIdA ); + B3_UNUSED( bodyIdB ); + B3_UNUSED( typeA ); + B3_UNUSED( typeB ); + +#if B3_FORCE_OVERFLOW == 0 + if ( typeA == b3_dynamicBody && typeB == b3_dynamicBody ) + { + // Dynamic constraint colors cannot encroach on colors reserved for static constraints + for ( int i = 0; i < B3_DYNAMIC_COLOR_COUNT; ++i ) + { + b3GraphColor* color = graph->colors + i; + if ( b3GetBit( &color->bodySet, bodyIdA ) || b3GetBit( &color->bodySet, bodyIdB ) ) + { + continue; + } + + b3SetBitGrow( &color->bodySet, bodyIdA ); + b3SetBitGrow( &color->bodySet, bodyIdB ); + return i; + } + } + else if ( typeA == b3_dynamicBody ) + { + // Static constraint colors build from the end to get higher priority than dyn-dyn constraints + for ( int i = B3_OVERFLOW_INDEX - 1; i >= 1; --i ) + { + b3GraphColor* color = graph->colors + i; + if ( b3GetBit( &color->bodySet, bodyIdA ) ) + { + continue; + } + + b3SetBitGrow( &color->bodySet, bodyIdA ); + return i; + } + } + else if ( typeB == b3_dynamicBody ) + { + // Static constraint colors build from the end to get higher priority than dyn-dyn constraints + for ( int i = B3_OVERFLOW_INDEX - 1; i >= 1; --i ) + { + b3GraphColor* color = graph->colors + i; + if ( b3GetBit( &color->bodySet, bodyIdB ) ) + { + continue; + } + + b3SetBitGrow( &color->bodySet, bodyIdB ); + return i; + } + } +#endif + + return B3_OVERFLOW_INDEX; +} + +b3JointSim* b3CreateJointInGraph( b3World* world, b3Joint* joint ) +{ + b3ConstraintGraph* graph = &world->constraintGraph; + + int bodyIdA = joint->edges[0].bodyId; + int bodyIdB = joint->edges[1].bodyId; + b3Body* bodyA = b3Array_Get( world->bodies, bodyIdA ); + b3Body* bodyB = b3Array_Get( world->bodies, bodyIdB ); + + int colorIndex = b3AssignJointColor( graph, bodyIdA, bodyIdB, bodyA->type, bodyB->type ); + + b3JointSim* jointSim = b3Array_Emplace( graph->colors[colorIndex].jointSims ); + memset( jointSim, 0, sizeof( b3JointSim ) ); + + joint->colorIndex = colorIndex; + joint->localIndex = graph->colors[colorIndex].jointSims.count - 1; + return jointSim; +} + +void b3AddJointToGraph( b3World* world, b3JointSim* jointSim, b3Joint* joint ) +{ + b3JointSim* jointDst = b3CreateJointInGraph( world, joint ); + memcpy( jointDst, jointSim, sizeof( b3JointSim ) ); +} + +void b3RemoveJointFromGraph( b3World* world, int bodyIdA, int bodyIdB, int colorIndex, int localIndex ) +{ + b3ConstraintGraph* graph = &world->constraintGraph; + + B3_ASSERT( 0 <= colorIndex && colorIndex < B3_GRAPH_COLOR_COUNT ); + b3GraphColor* color = graph->colors + colorIndex; + + if ( colorIndex != B3_OVERFLOW_INDEX ) + { + // May clear static bodies, no effect + b3ClearBit( &color->bodySet, bodyIdA ); + b3ClearBit( &color->bodySet, bodyIdB ); + } + + int movedIndex = b3Array_RemoveSwap( color->jointSims, localIndex ); + if ( movedIndex != B3_NULL_INDEX ) + { + // Fix moved joint + b3JointSim* movedJointSim = color->jointSims.data + localIndex; + int movedId = movedJointSim->jointId; + b3Joint* movedJoint = b3Array_Get( world->joints, movedId ); + B3_ASSERT( movedJoint->setIndex == b3_awakeSet ); + B3_ASSERT( movedJoint->colorIndex == colorIndex ); + B3_ASSERT( movedJoint->localIndex == movedIndex ); + movedJoint->localIndex = localIndex; + } +} diff --git a/vendor/box3d/src/src/constraint_graph.h b/vendor/box3d/src/src/constraint_graph.h new file mode 100644 index 000000000..ab17e9ac8 --- /dev/null +++ b/vendor/box3d/src/src/constraint_graph.h @@ -0,0 +1,77 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +#pragma once + +#include "bitset.h" +#include "contact.h" +#include "container.h" +#include "solver.h" +#include "solver_set.h" +#include "box3d/constants.h" + +typedef struct b3Body b3Body; +typedef struct b3Contact b3Contact; +typedef struct b3JointSim b3JointSim; +typedef struct b3Joint b3Joint; +typedef struct b3StepContext b3StepContext; +typedef struct b3World b3World; + +// This holds constraints that cannot fit the graph color limit. This happens when a single dynamic body +// is touching many other bodies. +#define B3_OVERFLOW_INDEX ( B3_GRAPH_COLOR_COUNT - 1 ) + +// This keeps constraints involving two dynamic bodies at a lower solver priority than constraints +// involving a dynamic and static bodies. This reduces tunneling due to push through. +#define B3_DYNAMIC_COLOR_COUNT ( B3_GRAPH_COLOR_COUNT - 4 ) + +// todo optimize mesh contact constraints +// They can be lumped with convex contacts and I can use the bitset event to re-link if the manifold count increases +// Could also create a group for two wide manifolds and use bitset event +// This could create ping-pong jitter so may need to pin to high water mark or introduce hysteresis somehow +// +// Dirk has the idea to do graph coloring based on manifolds. This suggests mesh contact will have manifolds +// in multiple graph colors. So each manifold with have a color and local index. +// Some concerns about this: +// - manifolds don't have a strong identity, would this affect stability/jitter? +// - this creates a lot of static graph colors and can overflow + +typedef struct b3GraphColor +{ + // This bitset is indexed by bodyId so this is over-sized to encompass static bodies + // however I never traverse these bits or use the bit count for anything + // This bitset is unused on the overflow color. + b3BitSet bodySet; + + // cache friendly arrays + b3Array( b3JointSim ) jointSims; + + b3Array( int ) convexContacts; + b3Array( b3ContactSpec ) contacts; + + // These are used for convex contacts + struct b3ContactConstraintWide* wideConstraints; + int wideConstraintCount; + + // These are used for mesh and overflow contacts + struct b3ManifoldConstraint* manifoldConstraints; + int manifoldConstraintCount; + struct b3ContactConstraint* contactConstraints; + int contactConstraintCount; +} b3GraphColor; + +typedef struct b3ConstraintGraph +{ + // including overflow at the end + b3GraphColor colors[B3_GRAPH_COLOR_COUNT]; +} b3ConstraintGraph; + +void b3CreateGraph( b3ConstraintGraph* graph, int bodyCapacity ); +void b3DestroyGraph( b3ConstraintGraph* graph ); + +void b3AddContactToGraph( b3World* world, b3Contact* contact ); +void b3RemoveContactFromGraph( b3World* world, int bodyIdA, int bodyIdB, int colorIndex, int localIndex, bool meshContact ); + +b3JointSim* b3CreateJointInGraph( b3World* world, b3Joint* joint ); +void b3AddJointToGraph( b3World* world, b3JointSim* jointSim, b3Joint* joint ); +void b3RemoveJointFromGraph( b3World* world, int bodyIdA, int bodyIdB, int colorIndex, int localIndex ); diff --git a/vendor/box3d/src/src/contact.c b/vendor/box3d/src/src/contact.c new file mode 100644 index 000000000..92e38b018 --- /dev/null +++ b/vendor/box3d/src/src/contact.c @@ -0,0 +1,873 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +#include "contact.h" + +#include "algorithm.h" +#include "body.h" +#include "compound.h" +#include "island.h" +#include "manifold.h" +#include "physics_world.h" +#include "shape.h" +#include "solver_set.h" +#include "table.h" + +#include "box3d/box3d.h" + +// Contacts and determinism +// A deterministic simulation requires contacts to exist in the same order in b3Island no matter the thread count. +// The order must reproduce from run to run. This is necessary because the Gauss-Seidel constraint solver is order dependent. +// +// Creation: +// - Contacts are created using results from b3UpdateBroadPhasePairs +// - These results are ordered according to the order of the broad-phase move array +// - The move array is ordered according to the shape creation order using a bitset. +// - The island/shape/body order is determined by creation order +// - Logically contacts are only created for awake bodies, so they are immediately added to the awake contact array (serially) +// +// Island linking: +// - The awake contact array is built from the body-contact graph for all awake bodies in awake islands. +// - Awake contacts are solved in parallel and they generate contact state changes. +// - These state changes may link islands together using union find. +// - The state changes are ordered using a bit array that encompasses all contacts +// - As long as contacts are created in deterministic order, island link order is deterministic. +// - This keeps the order of contacts in islands deterministic + +// Manifold functions should compute important results in local space to improve precision. However, this +// interface function takes two world transforms instead of a relative transform for these reasons: +// +// First: +// The anchors need to be computed relative to the shape origin in world space. This is necessary so the +// solver does not need to access static body transforms. Not even in constraint preparation. This approach +// has world space vectors yet retains precision. +// +// Second: +// b3ManifoldPoint::point is very useful for debugging and it is in world space. +// +// Third: +// The user may call the manifold functions directly and they should be easy to use and have easy to use +// results. +// typedef b3Manifold b3ManifoldFcn( const b3Shape* shapeA, b3Transform xfA, const b3Shape* shapeB, b3Transform xfB, +// b3ContactCache* cache ); + +static b3Contact* b3GetContactFullId( b3World* world, b3ContactId contactId ) +{ + int id = contactId.index1 - 1; + b3Contact* contact = b3Array_Get( world->contacts, id ); + B3_ASSERT( contact->contactId == id && contact->generation == contactId.generation ); + return contact; +} + +b3ContactData b3Contact_GetData( b3ContactId contactId ) +{ + b3World* world = b3GetWorld( contactId.world0 ); + b3Contact* contact = b3GetContactFullId( world, contactId ); + + const b3Shape* shapeA = b3Array_Get( world->shapes, contact->shapeIdA ); + const b3Shape* shapeB = b3Array_Get( world->shapes, contact->shapeIdB ); + + b3ContactData data = { 0 }; + data.contactId = contactId; + data.shapeIdA = (b3ShapeId){ + .index1 = shapeA->id + 1, + .world0 = contactId.world0, + .generation = shapeA->generation, + }; + data.shapeIdB = (b3ShapeId){ + .index1 = shapeB->id + 1, + .world0 = contactId.world0, + .generation = shapeB->generation, + }; + + if ( contact->manifoldCount > 0 ) + { + data.manifolds = contact->manifolds; + data.manifoldCount = contact->manifoldCount; + } + else + { + data.manifolds = NULL; + data.manifoldCount = 0; + } + + return data; +} + +struct b3ContactRegister +{ + // b3ManifoldFcn* fcn; + bool supported; + bool primary; +}; + +static struct b3ContactRegister s_registers[b3_shapeTypeCount][b3_shapeTypeCount]; +static bool s_initialized = false; + +static void b3AddType( b3ShapeType type1, b3ShapeType type2 ) +{ + B3_ASSERT( 0 <= type1 && type1 < b3_shapeTypeCount ); + B3_ASSERT( 0 <= type2 && type2 < b3_shapeTypeCount ); + + s_registers[type1][type2].supported = true; + s_registers[type1][type2].primary = true; + + if ( type1 != type2 ) + { + s_registers[type2][type1].supported = true; + s_registers[type2][type1].primary = false; + } +} + +void b3InitializeContactRegisters( void ) +{ + if ( s_initialized == false ) + { + b3AddType( b3_sphereShape, b3_sphereShape ); + b3AddType( b3_capsuleShape, b3_sphereShape ); + b3AddType( b3_capsuleShape, b3_capsuleShape ); + b3AddType( b3_compoundShape, b3_sphereShape ); + b3AddType( b3_compoundShape, b3_capsuleShape ); + b3AddType( b3_compoundShape, b3_hullShape ); + b3AddType( b3_hullShape, b3_sphereShape ); + b3AddType( b3_hullShape, b3_capsuleShape ); + b3AddType( b3_hullShape, b3_hullShape ); + b3AddType( b3_meshShape, b3_sphereShape ); + b3AddType( b3_meshShape, b3_capsuleShape ); + b3AddType( b3_meshShape, b3_hullShape ); + b3AddType( b3_heightShape, b3_sphereShape ); + b3AddType( b3_heightShape, b3_capsuleShape ); + b3AddType( b3_heightShape, b3_hullShape ); + s_initialized = true; + } +} + +void b3CreateContact( b3World* world, b3Shape* shapeA, b3Shape* shapeB, int childIndex ) +{ + b3ShapeType typeA = shapeA->type; + b3ShapeType typeB = shapeB->type; + + B3_ASSERT( 0 <= typeA && typeA < b3_shapeTypeCount ); + B3_ASSERT( 0 <= typeB && typeB < b3_shapeTypeCount ); + + if ( s_registers[typeA][typeB].supported == false ) + { + // For example, no mesh vs mesh collision + return; + } + + if ( s_registers[typeA][typeB].primary == false ) + { + // flip order + b3CreateContact( world, shapeB, shapeA, childIndex ); + return; + } + + b3Body* bodyA = b3Array_Get( world->bodies, shapeA->bodyId ); + b3Body* bodyB = b3Array_Get( world->bodies, shapeB->bodyId ); + + B3_ASSERT( bodyA->setIndex != b3_disabledSet && bodyB->setIndex != b3_disabledSet ); + B3_ASSERT( bodyA->setIndex != b3_staticSet || bodyB->setIndex != b3_staticSet ); + + int setIndex; + if ( bodyA->setIndex == b3_awakeSet || bodyB->setIndex == b3_awakeSet ) + { + setIndex = b3_awakeSet; + } + else + { + // sleeping and non-touching contacts live in the disabled set + // later if this set is found to be touching then the sleeping + // islands will be linked and the contact moved to the merged island + + // This is possible if a shape moves slightly then falls asleep + setIndex = b3_disabledSet; + } + + b3SolverSet* set = b3Array_Get( world->solverSets, setIndex ); + + // Create contact key and contact + int contactId = b3AllocId( &world->contactIdPool ); + if ( contactId == world->contacts.count ) + { + b3Contact emptyContact = { 0 }; + b3Array_Push( world->contacts, emptyContact ); + } + + int shapeIdA = shapeA->id; + int shapeIdB = shapeB->id; + + b3Contact* contact = b3Array_Get( world->contacts, contactId ); + int generation = contact->generation; + *contact = (b3Contact){ 0 }; + contact->contactId = contactId; + contact->generation = generation + 1; + contact->setIndex = setIndex; + contact->colorIndex = B3_NULL_INDEX; + contact->localIndex = set->contactIndices.count; + contact->islandId = B3_NULL_INDEX; + contact->islandIndex = B3_NULL_INDEX; + contact->shapeIdA = shapeIdA; + contact->shapeIdB = shapeIdB; + contact->childIndex = childIndex; + + // Both bodies must enable recycling + if ( ( bodyA->flags & b3_bodyEnableContactRecycling ) != 0 && ( bodyB->flags & b3_bodyEnableContactRecycling ) != 0 ) + { + contact->flags |= b3_contactRecycleFlag; + } + + if ( shapeA->type == b3_meshShape || shapeA->type == b3_heightShape ) + { + contact->flags |= b3_simMeshContact; + } + else if ( shapeA->type == b3_compoundShape ) + { + b3ChildShape child = b3GetCompoundChild( shapeA->compound, childIndex ); + if ( child.type == b3_meshShape ) + { + contact->flags |= b3_simMeshContact; + } + } + + // todo impose these restrictions to make life easier + B3_ASSERT( shapeB->type == b3_sphereShape || shapeB->type == b3_capsuleShape || shapeB->type == b3_hullShape ); + // B3_ASSERT( bodyB->type != b3_staticBody ); + + // Is either body static? + // Note: it is possible to have a dynamic mesh collide with a static convex shape. Maybe I should disallow this. + if ( bodyA->type == b3_staticBody || bodyB->type == b3_staticBody ) + { + contact->flags |= b3_contactStaticFlag; + } + + B3_ASSERT( shapeA->sensorIndex == B3_NULL_INDEX && shapeB->sensorIndex == B3_NULL_INDEX ); + + if ( ( shapeA->flags & b3_enableContactEvents ) || ( shapeB->flags & b3_enableContactEvents ) ) + { + contact->flags |= b3_contactEnableContactEvents; + } + + if ( ( shapeA->flags & b3_enableSpeculative ) && ( shapeB->flags & b3_enableSpeculative ) ) + { + contact->flags |= b3_enableSpeculativePoints; + } + + // Connect to body A + { + contact->edges[0].bodyId = shapeA->bodyId; + contact->edges[0].prevKey = B3_NULL_INDEX; + contact->edges[0].nextKey = bodyA->headContactKey; + + int keyA = ( contactId << 1 ) | 0; + int headContactKey = bodyA->headContactKey; + if ( headContactKey != B3_NULL_INDEX ) + { + b3Contact* headContact = b3Array_Get( world->contacts, headContactKey >> 1 ); + headContact->edges[headContactKey & 1].prevKey = keyA; + } + bodyA->headContactKey = keyA; + bodyA->contactCount += 1; + } + + // Connect to body B + { + contact->edges[1].bodyId = shapeB->bodyId; + contact->edges[1].prevKey = B3_NULL_INDEX; + contact->edges[1].nextKey = bodyB->headContactKey; + + int keyB = ( contactId << 1 ) | 1; + int headContactKey = bodyB->headContactKey; + if ( bodyB->headContactKey != B3_NULL_INDEX ) + { + b3Contact* headContact = b3Array_Get( world->contacts, headContactKey >> 1 ); + headContact->edges[headContactKey & 1].prevKey = keyB; + } + bodyB->headContactKey = keyB; + bodyB->contactCount += 1; + } + + // Add to pair set for fast lookup + uint64_t pairKey = b3ShapePairKey( shapeIdA, shapeIdB, childIndex ); + b3AddKey( &world->broadPhase.pairSet, pairKey ); + + // Contacts are created as non-touching. Later if they are found to be touching + // they will link islands and be moved into the constraint graph. + b3Array_Push( set->contactIndices, contactId ); + + float radiusA = 0.0f; + if ( typeA == b3_sphereShape ) + { + radiusA = shapeA->sphere.radius; + } + else if ( typeA == b3_capsuleShape ) + { + radiusA = shapeA->capsule.radius; + } + + float radiusB = 0.0f; + if ( typeB == b3_sphereShape ) + { + radiusB = shapeB->sphere.radius; + } + else if ( typeB == b3_capsuleShape ) + { + radiusB = shapeB->capsule.radius; + } + + float maxRadius = b3MaxFloat( radiusA, radiusB ); + + // Assuming the rolling resistance doesn't change + contact->rollingResistance = + b3MaxFloat( b3GetShapeMaterials( shapeA )[0].rollingResistance, b3GetShapeMaterials( shapeB )[0].rollingResistance ) * + maxRadius; + + if ( ( shapeA->flags & b3_enablePreSolveEvents ) || ( shapeB->flags & b3_enablePreSolveEvents ) ) + { + contact->flags |= b3_simEnablePreSolveEvents; + } +} + +// A contact is destroyed when: +// - broad-phase proxies stop overlapping +// - a body is destroyed +// - a body is disabled +// - a body changes type from dynamic to kinematic or static +// - a shape is destroyed +// - contact filtering is modified +void b3DestroyContact( b3World* world, b3Contact* contact, bool wakeBodies ) +{ + // Remove pair from set + uint64_t pairKey = b3ShapePairKey( contact->shapeIdA, contact->shapeIdB, contact->childIndex ); + b3RemoveKey( &world->broadPhase.pairSet, pairKey ); + + b3FreeManifolds( world, contact->manifolds, contact->manifoldCount ); + contact->manifolds = NULL; + contact->manifoldCount = 0; + + b3ContactEdge* edgeA = contact->edges + 0; + b3ContactEdge* edgeB = contact->edges + 1; + + int bodyIdA = edgeA->bodyId; + int bodyIdB = edgeB->bodyId; + b3Body* bodyA = b3Array_Get( world->bodies, bodyIdA ); + b3Body* bodyB = b3Array_Get( world->bodies, bodyIdB ); + + uint32_t flags = contact->flags; + bool touching = ( flags & b3_contactTouchingFlag ) != 0; + + // End touch event + if ( touching && ( flags & b3_contactEnableContactEvents ) != 0 ) + { + uint16_t worldId = world->worldId; + const b3Shape* shapeA = b3Array_Get( world->shapes, contact->shapeIdA ); + const b3Shape* shapeB = b3Array_Get( world->shapes, contact->shapeIdB ); + b3ShapeId shapeIdA = { shapeA->id + 1, worldId, shapeA->generation }; + b3ShapeId shapeIdB = { shapeB->id + 1, worldId, shapeB->generation }; + + b3ContactId contactId = { + .index1 = contact->contactId + 1, + .world0 = world->worldId, + .padding = 0, + .generation = contact->generation, + }; + + b3ContactEndTouchEvent event = { + .shapeIdA = shapeIdA, + .shapeIdB = shapeIdB, + .contactId = contactId, + }; + + b3Array_Push( world->contactEndEvents[world->endEventArrayIndex], event ); + } + + // Remove from body A + if ( edgeA->prevKey != B3_NULL_INDEX ) + { + b3Contact* prevContact = b3Array_Get( world->contacts, edgeA->prevKey >> 1 ); + b3ContactEdge* prevEdge = prevContact->edges + ( edgeA->prevKey & 1 ); + prevEdge->nextKey = edgeA->nextKey; + } + + if ( edgeA->nextKey != B3_NULL_INDEX ) + { + b3Contact* nextContact = b3Array_Get( world->contacts, edgeA->nextKey >> 1 ); + b3ContactEdge* nextEdge = nextContact->edges + ( edgeA->nextKey & 1 ); + nextEdge->prevKey = edgeA->prevKey; + } + + int contactId = contact->contactId; + + int edgeKeyA = ( contactId << 1 ) | 0; + if ( bodyA->headContactKey == edgeKeyA ) + { + bodyA->headContactKey = edgeA->nextKey; + } + + bodyA->contactCount -= 1; + + // Remove from body B + if ( edgeB->prevKey != B3_NULL_INDEX ) + { + b3Contact* prevContact = b3Array_Get( world->contacts, edgeB->prevKey >> 1 ); + b3ContactEdge* prevEdge = prevContact->edges + ( edgeB->prevKey & 1 ); + prevEdge->nextKey = edgeB->nextKey; + } + + if ( edgeB->nextKey != B3_NULL_INDEX ) + { + b3Contact* nextContact = b3Array_Get( world->contacts, edgeB->nextKey >> 1 ); + b3ContactEdge* nextEdge = nextContact->edges + ( edgeB->nextKey & 1 ); + nextEdge->prevKey = edgeB->prevKey; + } + + int edgeKeyB = ( contactId << 1 ) | 1; + if ( bodyB->headContactKey == edgeKeyB ) + { + bodyB->headContactKey = edgeB->nextKey; + } + + bodyB->contactCount -= 1; + + if ( contact->flags & b3_simMeshContact ) + { + b3Array_Destroy( contact->meshContact.triangleCache ); + } + + // Remove contact from the array that owns it + if ( contact->islandId != B3_NULL_INDEX ) + { + b3UnlinkContact( world, contact ); + } + + if ( contact->colorIndex != B3_NULL_INDEX ) + { + // contact is an active constraint + B3_ASSERT( contact->setIndex == b3_awakeSet ); + bool meshContact = contact->flags & b3_simMeshContact; + b3RemoveContactFromGraph( world, bodyIdA, bodyIdB, contact->colorIndex, contact->localIndex, meshContact ); + } + else + { + // contact is non-touching or is sleeping or is a sensor + B3_ASSERT( contact->setIndex != b3_awakeSet || ( contact->flags & b3_contactTouchingFlag ) == 0 ); + b3SolverSet* set = b3Array_Get( world->solverSets, contact->setIndex ); + + int localIndex = contact->localIndex; + int movedIndex = b3Array_RemoveSwap( set->contactIndices, localIndex ); + if ( movedIndex != B3_NULL_INDEX ) + { + int movedContactIndex = set->contactIndices.data[localIndex]; + b3Contact* movedContact = b3Array_Get( world->contacts, movedContactIndex ); + movedContact->localIndex = localIndex; + } + } + + // Free contact and id (preserve generation) + contact->contactId = B3_NULL_INDEX; + contact->setIndex = B3_NULL_INDEX; + contact->colorIndex = B3_NULL_INDEX; + contact->localIndex = B3_NULL_INDEX; + b3FreeId( &world->contactIdPool, contactId ); + + if ( wakeBodies && touching ) + { + b3WakeBody( world, bodyA ); + b3WakeBody( world, bodyB ); + } +} + +static bool b3ComputeConvexManifold( b3World* world, int workerIndex, b3Contact* contact, const b3Shape* shapeA, + b3WorldTransform xfA, const b3Shape* shapeB, b3WorldTransform xfB, b3Arena arena ) +{ + b3ShapeType typeA = shapeA->type; + b3ShapeType typeB = shapeB->type; + + b3ContactCache* cache = &contact->convexContact.cache; + + int pointCapacity = 32; + b3LocalManifoldPoint* pointBuffer = (b3LocalManifoldPoint*)b3Bump( &arena, pointCapacity * sizeof( b3LocalManifoldPoint ) ); + + b3LocalManifold geomManifold = { 0 }; + geomManifold.points = pointBuffer; + + b3Transform transformBtoA = b3InvMulWorldTransforms( xfA, xfB ); + + if ( typeA == b3_sphereShape ) + { + B3_ASSERT( typeB == b3_sphereShape ); + b3CollideSpheres( &geomManifold, pointCapacity, &shapeA->sphere, &shapeB->sphere, transformBtoA ); + } + else if ( typeA == b3_capsuleShape ) + { + if ( typeB == b3_sphereShape ) + { + b3CollideCapsuleAndSphere( &geomManifold, pointCapacity, &shapeA->capsule, &shapeB->sphere, transformBtoA ); + } + else + { + B3_ASSERT( typeB == b3_capsuleShape ); + b3CollideCapsules( &geomManifold, pointCapacity, &shapeA->capsule, &shapeB->capsule, transformBtoA ); + } + } + else + { + B3_ASSERT( typeA == b3_hullShape ); + + if ( typeB == b3_sphereShape ) + { + b3CollideHullAndSphere( &geomManifold, pointCapacity, shapeA->hull, &shapeB->sphere, transformBtoA, + &cache->simplexCache ); + } + else if ( typeB == b3_capsuleShape ) + { + b3CollideHullAndCapsule( &geomManifold, pointCapacity, shapeA->hull, &shapeB->capsule, transformBtoA, + &cache->simplexCache ); + } + else + { + B3_ASSERT( typeB == b3_hullShape ); + b3CollideHulls( &geomManifold, pointCapacity, shapeA->hull, shapeB->hull, transformBtoA, &cache->satCache ); + world->taskContexts.data[workerIndex].satCallCount += 1; + world->taskContexts.data[workerIndex].satCacheHitCount += cache->satCache.hit; + } + } + + if ( geomManifold.pointCount == 0 ) + { + if ( contact->manifoldCount > 0 ) + { + b3FreeManifolds( world, contact->manifolds, contact->manifoldCount ); + contact->manifolds = NULL; + contact->manifoldCount = 0; + } + + return false; + } + + b3ManifoldPoint oldPoints[B3_MAX_MANIFOLD_POINTS]; + int oldCount = 0; + + if ( contact->manifoldCount == 0 ) + { + contact->manifolds = b3AllocateManifolds( world, 1 ); + contact->manifoldCount = 1; + } + else + { + oldCount = contact->manifolds[0].pointCount; + memcpy( oldPoints, contact->manifolds[0].points, oldCount * sizeof( b3ManifoldPoint ) ); + } + + b3Manifold* manifold = contact->manifolds; + manifold->pointCount = geomManifold.pointCount; + + b3Matrix3 matrixA = b3MakeMatrixFromQuat( xfA.q ); + manifold->normal = b3MulMV( matrixA, geomManifold.normal ); + + // Store point data in contact + for ( int i = 0; i < geomManifold.pointCount; ++i ) + { + const b3LocalManifoldPoint* source = geomManifold.points + i; + b3ManifoldPoint* target = manifold->points + i; + + // Contact points are computed in frame A + target->anchorA = b3MulMV( matrixA, source->point ); + target->anchorB = b3Add( target->anchorA, b3SubPos( xfA.p, xfB.p ) ); + target->separation = source->separation; + target->featureId = b3MakeFeatureId( source->pair ); + target->triangleIndex = B3_NULL_INDEX; + target->normalVelocity = 0.0f; + } + + // Copy impulses from old points + for ( int i = 0; i < geomManifold.pointCount; ++i ) + { + b3ManifoldPoint* pt2 = manifold->points + i; + pt2->totalNormalImpulse = 0.0f; + pt2->persisted = false; + + for ( int j = 0; j < oldCount; ++j ) + { + b3ManifoldPoint* pt1 = oldPoints + j; + + if ( pt2->featureId == pt1->featureId ) + { + pt2->normalImpulse = pt1->normalImpulse; + pt2->persisted = true; + + // claimed + pt1->featureId = UINT32_MAX; + + break; + } + } + + if ( pt2->persisted == false ) + { + pt2->normalImpulse = 0.0f; + } + } + + return true; +} + +static bool b3UpdateConvexContact( b3World* world, int workerIndex, b3Contact* contact, b3Shape* shapeA, b3WorldTransform xfA, + b3Shape* shapeB, b3WorldTransform xfB, bool flip, b3Arena arena ) +{ + // Compute new manifold + bool touching = b3ComputeConvexManifold( world, workerIndex, contact, shapeA, xfA, shapeB, xfB, arena ); + + if ( touching == false ) + { + B3_ASSERT( contact->manifolds == NULL && contact->manifoldCount == 0 ); + return false; + } + + B3_ASSERT( contact->manifoldCount == 1 ); + + if ( flip ) + { + // Not flipping the feature ids because they just need to match and flipping is consistent. + b3Manifold* manifold = contact->manifolds + 0; + manifold->normal = b3Neg( manifold->normal ); + int pointCount = manifold->pointCount; + for ( int i = 0; i < pointCount; ++i ) + { + b3ManifoldPoint* mp = manifold->points + i; + B3_SWAP( mp->anchorA, mp->anchorB ); + } + } + + const b3SurfaceMaterial* materialA = b3GetShapeMaterials( shapeA ); + const b3SurfaceMaterial* materialB = b3GetShapeMaterials( shapeB ); + + // Keep these updated in case the values on the shapes are modified + contact->friction = + world->frictionCallback( materialA->friction, materialA->userMaterialId, materialB->friction, materialB->userMaterialId ); + contact->restitution = world->restitutionCallback( materialA->restitution, materialA->userMaterialId, materialB->restitution, + materialB->userMaterialId ); + + if ( materialA->rollingResistance > 0.0f || materialB->rollingResistance > 0.0f ) + { + b3ShapeType typeA = shapeA->type; + b3ShapeType typeB = shapeB->type; + + float radiusA = 0.0f; + if ( typeA == b3_sphereShape ) + { + radiusA = shapeA->sphere.radius; + } + else if ( typeA == b3_capsuleShape ) + { + radiusA = shapeA->capsule.radius; + } + else if ( typeA == b3_hullShape ) + { + radiusA = 0.25f * shapeA->hull->innerRadius; + } + + float radiusB = 0.0f; + if ( typeB == b3_sphereShape ) + { + radiusB = shapeB->sphere.radius; + } + else if ( typeB == b3_capsuleShape ) + { + radiusB = shapeB->capsule.radius; + } + else if ( typeB == b3_hullShape ) + { + radiusB = 0.25f * shapeB->hull->innerRadius; + } + + float maxRadius = b3MaxFloat( radiusA, radiusB ); + contact->rollingResistance = b3MaxFloat( materialA->rollingResistance, materialB->rollingResistance ) * maxRadius; + } + else + { + contact->rollingResistance = 0.0f; + } + + b3Vec3 tangentVelocityA = b3RotateVector( xfA.q, materialA->tangentVelocity ); + b3Vec3 tangentVelocityB = b3RotateVector( xfB.q, materialB->tangentVelocity ); + contact->tangentVelocity = b3Sub( tangentVelocityA, tangentVelocityB ); + + if ( world->preSolveFcn && ( contact->flags & b3_simEnablePreSolveEvents ) != 0 ) + { + b3ShapeId shapeIdA = { shapeA->id + 1, world->worldId, shapeA->generation }; + b3ShapeId shapeIdB = { shapeB->id + 1, world->worldId, shapeB->generation }; + + // this call assumes thread safety + b3Pos point = b3OffsetPos( xfA.p, contact->manifolds[0].points[0].anchorA ); + b3Vec3 normal = contact->manifolds[0].normal; + touching = world->preSolveFcn( shapeIdA, shapeIdB, point, normal, world->preSolveContext ); + if ( touching == false ) + { + // disable contact + b3FreeManifolds( world, contact->manifolds, contact->manifoldCount ); + contact->manifolds = NULL; + contact->manifoldCount = 0; + return false; + } + } + + if ( ( shapeA->flags & b3_enableHitEvents ) || ( shapeB->flags & b3_enableHitEvents ) ) + { + contact->flags |= b3_simEnableHitEvent; + } + else + { + contact->flags &= ~b3_simEnableHitEvent; + } + + return true; +} + +// Update the contact manifold and touching status. +// Note: do not assume the shape AABBs are overlapping or are valid. +bool b3UpdateContact( b3World* world, int workerIndex, b3Contact* contact, b3Shape* shapeA, b3Vec3 localCenterA, + b3WorldTransform xfA, b3Shape* shapeB, b3Vec3 localCenterB, b3WorldTransform xfB, bool isFast, + b3Arena arena ) +{ + bool touching; + + B3_ASSERT( shapeB->type != b3_compoundShape ); + + if ( shapeA->type == b3_compoundShape ) + { + int childIndex = contact->childIndex; + b3ChildShape child = b3GetCompoundChild( shapeA->compound, childIndex ); + + // Temporary child shape to match existing function signatures + b3Shape childShapeA; + memcpy( &childShapeA, shapeA, sizeof( b3Shape ) ); + + childShapeA.type = child.type; + + if ( child.type == b3_capsuleShape ) + { + childShapeA.capsule = child.capsule; + if ( shapeB->type == b3_hullShape ) + { + // Flip + bool flip = true; + touching = b3UpdateConvexContact( world, workerIndex, contact, shapeB, xfB, &childShapeA, xfA, flip, arena ); + } + else + { + bool flip = false; + touching = b3UpdateConvexContact( world, workerIndex, contact, &childShapeA, xfA, shapeB, xfB, flip, arena ); + } + } + else if ( child.type == b3_hullShape ) + { + childShapeA.hull = child.hull; + b3WorldTransform xfChild = b3MulWorldTransforms( xfA, child.transform ); + bool flip = false; + touching = b3UpdateConvexContact( world, workerIndex, contact, &childShapeA, xfChild, shapeB, xfB, flip, arena ); + } + else if ( child.type == b3_meshShape ) + { + childShapeA.mesh = child.mesh; + b3WorldTransform xfChild = b3MulWorldTransforms( xfA, child.transform ); + + touching = b3ComputeMeshManifolds( world, workerIndex, contact, &childShapeA, child.materialIndices, xfChild, shapeB, + xfB, isFast, arena ); + + if ( touching && ( ( shapeA->flags & b3_enableHitEvents ) || ( shapeB->flags & b3_enableHitEvents ) ) ) + { + contact->flags |= b3_simEnableHitEvent; + } + else + { + contact->flags &= ~b3_simEnableHitEvent; + } + + B3_ASSERT( ( touching == true && contact->manifoldCount > 0 ) || + ( touching == false && contact->manifoldCount == 0 ) ); + } + else + { + B3_ASSERT( child.type == b3_sphereShape ); + + childShapeA.sphere = child.sphere; + if ( shapeB->type == b3_capsuleShape || shapeB->type == b3_hullShape ) + { + // Flip + bool flip = true; + touching = b3UpdateConvexContact( world, workerIndex, contact, shapeB, xfB, &childShapeA, xfA, flip, arena ); + } + else + { + bool flip = false; + touching = b3UpdateConvexContact( world, workerIndex, contact, &childShapeA, xfA, shapeB, xfB, flip, arena ); + } + } + + // The anchor is relative to the child origin but oriented in world space. + // Offset the anchor to be relative to the compound origin. + int manifoldCount = contact->manifoldCount; + b3Vec3 offset = b3RotateVector( xfA.q, child.transform.p ); + for ( int i = 0; i < manifoldCount; ++i ) + { + b3Manifold* manifold = contact->manifolds + i; + int pointCount = manifold->pointCount; + for ( int j = 0; j < pointCount; ++j ) + { + b3ManifoldPoint* mp = manifold->points + j; + mp->anchorA = b3Add( mp->anchorA, offset ); + } + } + } + else if ( shapeA->type == b3_meshShape || shapeA->type == b3_heightShape ) + { + // Does this contact touch a mesh or height-field? + + // Compute mesh manifolds + touching = b3ComputeMeshManifolds( world, workerIndex, contact, shapeA, NULL, xfA, shapeB, xfB, isFast, arena ); + + if ( touching && ( ( shapeA->flags & b3_enableHitEvents ) || ( shapeB->flags & b3_enableHitEvents ) ) ) + { + contact->flags |= b3_simEnableHitEvent; + } + else + { + contact->flags &= ~b3_simEnableHitEvent; + } + + B3_ASSERT( ( touching == true && contact->manifoldCount > 0 ) || ( touching == false && contact->manifoldCount == 0 ) ); + } + else + { + // Convex-vs-convex + bool flip = false; + touching = b3UpdateConvexContact( world, workerIndex, contact, shapeA, xfA, shapeB, xfB, flip, arena ); + } + + if ( touching ) + { + b3Vec3 centerA = b3RotateVector( xfA.q, localCenterA ); + b3Vec3 centerB = b3RotateVector( xfB.q, localCenterB ); + + // Adjust anchors to be relative to center of mass + for ( int i = 0; i < contact->manifoldCount; ++i ) + { + b3Manifold* manifold = contact->manifolds + i; + for ( int j = 0; j < manifold->pointCount; ++j ) + { + b3ManifoldPoint* mp = manifold->points + j; + mp->anchorA = b3Sub( mp->anchorA, centerA ); + mp->anchorB = b3Sub( mp->anchorB, centerB ); + } + } + + contact->flags |= b3_simTouchingFlag; + } + else + { + contact->flags &= ~b3_simTouchingFlag; + } + + return touching; +} diff --git a/vendor/box3d/src/src/contact.h b/vendor/box3d/src/src/contact.h new file mode 100644 index 000000000..ad9fa74d6 --- /dev/null +++ b/vendor/box3d/src/src/contact.h @@ -0,0 +1,183 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +#pragma once + +#include "arena_allocator.h" +#include "container.h" + +#include "box3d/collision.h" +#include "box3d/types.h" + +#define B3_FORCE_GHOST_COLLISIONS 0 + +typedef struct b3Shape b3Shape; +typedef struct b3World b3World; + +typedef union b3ContactCache +{ + b3SATCache satCache; + b3SimplexCache simplexCache; +} b3ContactCache; + +typedef struct b3TriangleCache +{ + int triangleIndex; + b3ContactCache cache; +} b3TriangleCache; + +b3DeclareArray( b3TriangleCache ); + +enum b3ContactFlags +{ + // Set when the solid shapes are touching. + b3_contactTouchingFlag = 0x00000001, + + // Contact has a hit event + b3_contactHitEventFlag = 0x00000002, + + // This contact wants contact events + b3_contactEnableContactEvents = 0x00000004, + + // This is contact is between a dynamic and static body + b3_contactStaticFlag = 0x00000008, + + b3_contactRecycleFlag = 0x00000010, + + // Set when the shapes are touching + b3_simTouchingFlag = 0x00010000, + + // This contact no longer has overlapping AABBs + b3_simDisjoint = 0x00020000, + + // This contact started touching + b3_simStartedTouching = 0x00040000, + + // This contact stopped touching + b3_simStoppedTouching = 0x00080000, + + // This contact has a hit event + b3_simEnableHitEvent = 0x00100000, + + // This contact wants pre-solve events + b3_simEnablePreSolveEvents = 0x00200000, + + // This is a mesh contact + b3_simMeshContact = 0x00400000, + + // Relative transform is cached for contact recycling + b3_relativeTransformValid = 0x00800000, + + // Enable speculative contact points + b3_enableSpeculativePoints = 0x01000000, +}; + +// A contact edge is used to connect bodies and contacts together +// in a contact graph where each body is a node and each contact +// is an edge. A contact edge belongs to a doubly linked list +// maintained in each attached body. Each contact has two contact +// edges, one for each attached body. +typedef struct b3ContactEdge +{ + int bodyId; + int prevKey; + int nextKey; +} b3ContactEdge; + +typedef struct b3MeshContact +{ + b3Array( b3TriangleCache ) triangleCache; + b3AABB queryBounds; +} b3MeshContact; + +typedef struct b3ConvexContact +{ + b3ContactCache cache; +} b3ConvexContact; + +// Represents the persistent interaction between two shapes +typedef struct b3Contact +{ + // index of simulation set stored in b3World + // B3_NULL_INDEX when slot is free + int setIndex; + + // index into the constraint graph color array + // B3_NULL_INDEX for non-touching or sleeping contacts + // B3_NULL_INDEX when slot is free + int colorIndex; + + // contact index within set or graph color + // B3_NULL_INDEX when slot is free + int localIndex; + + b3ContactEdge edges[2]; + int shapeIdA; + int shapeIdB; + int childIndex; + + // A contact only belongs to an island if touching, otherwise B3_NULL_INDEX. + int islandId; + + // Index into the island's contacts array for O(1) swap-removal. + // B3_NULL_INDEX when not in an island. + int islandIndex; + + // Back index into b3World::contacts + int contactId; + + // These are transient and cached for improved performance. B3_NULL_INDEX for static bodies. + int bodySimIndexA; + int bodySimIndexB; + + // b3ContactFlags + uint32_t flags; + + b3Manifold* manifolds; + int manifoldCount; + + // Cache for contact recycling. + b3Quat cachedRotationA; + b3Quat cachedRotationB; + b3Transform cachedRelativePose; + + // Mixed friction and restitution + float friction; + + // Usage determined by b3_simMeshContact in simFlags + union + { + b3ConvexContact convexContact; + b3MeshContact meshContact; + }; + + float restitution; + float rollingResistance; + b3Vec3 tangentVelocity; + + // This is monotonically advanced when a contact is allocated in this slot + // Used to check for invalid b3ContactId + uint32_t generation; +} b3Contact; + +typedef struct b3ContactSpec +{ + int contactId; + + // Start of the global manifold constraint array + int manifoldStart; + uint16_t manifoldCount; +} b3ContactSpec; + +b3DeclareArray( b3ContactSpec ); + +void b3InitializeContactRegisters( void ); + +void b3CreateContact( b3World* world, b3Shape* shapeA, b3Shape* shapeB, int childIndex ); +void b3DestroyContact( b3World* world, b3Contact* contact, bool wakeBodies ); + +bool b3UpdateContact( b3World* world, int workerIndex, b3Contact* contact, b3Shape* shapeA, b3Vec3 localCenterA, b3WorldTransform xfA, + b3Shape* shapeB, b3Vec3 localCenterB, b3WorldTransform xfB, bool isFast, b3Arena arena ); + +bool b3ComputeMeshManifolds( b3World* world, int workerIndex, b3Contact* contact, const b3Shape* shapeA, const int* materialMap, + b3WorldTransform xfA, const b3Shape* shapeB, b3WorldTransform xfB, bool isFast, b3Arena arena ); diff --git a/vendor/box3d/src/src/contact_solver.c b/vendor/box3d/src/src/contact_solver.c new file mode 100644 index 000000000..501f85876 --- /dev/null +++ b/vendor/box3d/src/src/contact_solver.c @@ -0,0 +1,2472 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +#include "contact_solver.h" + +#include "body.h" +#include "constraint_graph.h" +#include "contact.h" +#include "core.h" +#include "math_internal.h" +#include "physics_world.h" +#include "platform.h" +#include "solver_set.h" + +#if B3_ENABLE_VALIDATION +#include "shape.h" +#endif + +#define FIXED_ANCHORS 1 + +// contact separation for sub-stepping +// s = s0 + dot(cB + rB - cA - rA, normal) +// normal is held constant +// body positions c can translation and anchors r can rotate +// s(t) = s0 + dot(cB(t) + rB(t) - cA(t) - rA(t), normal) +// s(t) = s0 + dot(cB0 + dpB + rot(dqB, rB0) - cA0 - dpA - rot(dqA, rA0), normal) +// s(t) = s0 + dot(cB0 - cA0, normal) + dot(dpB - dpA + rot(dqB, rB0) - rot(dqA, rA0), normal) +// s_base = s0 + dot(cB0 - cA0, normal) + +// Prepare a mesh constraints +void b3PrepareContacts_Mesh( b3SolverBlock block, b3StepContext* context ) +{ + b3TracyCZoneNC( prepare_contact, "Prepare Contact", b3_colorYellow, true ); + + b3World* world = context->world; + b3BodySim* bodySims = context->sims; + b3BodyState* bodyStates = context->states; + + float warmStartScale = world->enableWarmStarting ? 1.0f : 0.0f; + + // Need to use spans in order to find the associated b2Contact, which is per color + b3ContactPrepareSpan* spans = context->contactPrepareSpans; + b3ManifoldConstraint* manifoldBase = context->manifoldConstraints; + b3ContactConstraint* base = context->contactConstraints; + + // Overflow constraints are stored separately + if ( block.blockType == b3_overflowBlock ) + { + b3GraphColor* overflow = world->constraintGraph.colors + B3_OVERFLOW_INDEX; + spans = context->overflowSpans; + manifoldBase = overflow->manifoldConstraints; + base = overflow->contactConstraints; + } + + int index = block.startIndex; + int endIndex = block.startIndex + block.count; + + // Find color for start index. Linear search but fast. + int colorIndex = 0; + while ( spans[colorIndex + 1].start <= index ) + { + colorIndex += 1; + } + + // Loop over block + while ( index < endIndex ) + { + int colorStart = spans[colorIndex].start; + int colorEndIndex = b3MinInt( spans[colorIndex + 1].start, endIndex ); + b3ContactSpec* specs = spans[colorIndex].contacts; + + // Loop over color + for ( ; index < colorEndIndex; ++index ) + { + b3ContactConstraint* contactConstraint = base + index; + + int localIndex = index - colorStart; + B3_ASSERT( 0 <= localIndex && localIndex < spans[colorIndex].count ); + int contactId = specs[localIndex].contactId; + b3Contact* contact = b3Array_Get( world->contacts, contactId ); + B3_ASSERT( contact->contactId == contactId ); + + int indexA = contact->bodySimIndexA; + int indexB = contact->bodySimIndexB; + +#if B3_ENABLE_VALIDATION + if ( indexA != B3_NULL_INDEX ) + { + b3Body* bodyA = b3Array_Get( world->bodies, contact->edges[0].bodyId ); + B3_ASSERT( indexA == bodyA->localIndex ); + } + + if ( indexB != B3_NULL_INDEX ) + { + b3Body* bodyB = b3Array_Get( world->bodies, contact->edges[1].bodyId ); + B3_ASSERT( indexB == bodyB->localIndex ); + } +#endif + + // Body A data + float mA; + b3Matrix3 iA; + b3Vec3 vA; + b3Vec3 wA; + + if ( indexA == B3_NULL_INDEX ) + { + mA = 0.0f; + iA = b3Mat3_zero; + vA = b3Vec3_zero; + wA = b3Vec3_zero; + } + else + { + b3BodySim* simA = bodySims + indexA; + mA = simA->invMass; + iA = simA->invInertiaWorld; + + b3BodyState* stateA = bodyStates + indexA; + vA = stateA->linearVelocity; + wA = stateA->angularVelocity; + } + + // Body B data + float mB; + b3Matrix3 iB; + b3Vec3 vB; + b3Vec3 wB; + + if ( indexB == B3_NULL_INDEX ) + { + mB = 0.0f; + iB = b3Mat3_zero; + vB = b3Vec3_zero; + wB = b3Vec3_zero; + } + else + { + b3BodySim* simB = bodySims + indexB; + mB = simB->invMass; + iB = simB->invInertiaWorld; + + b3BodyState* stateB = bodyStates + indexB; + vB = stateB->linearVelocity; + wB = stateB->angularVelocity; + } + + int manifoldCount = contact->manifoldCount; + contactConstraint->contact = contact; + contactConstraint->manifoldCount = manifoldCount; + contactConstraint->indexA = indexA; + contactConstraint->indexB = indexB; + contactConstraint->invIA = iA; + contactConstraint->invMassA = mA; + contactConstraint->invIB = iB; + contactConstraint->invMassB = mB; + contactConstraint->rollingMass = b3InvertMatrix( b3AddMM( iA, iB ) ); + contactConstraint->softness = + ( contact->flags & b3_contactStaticFlag ) != 0 ? context->staticSoftness : context->contactSoftness; + contactConstraint->friction = contact->friction; + contactConstraint->restitution = contact->restitution; + contactConstraint->rollingResistance = contact->rollingResistance; + + b3ManifoldConstraint* manifoldConstraints = manifoldBase + specs[localIndex].manifoldStart; + contactConstraint->constraints = manifoldConstraints; + + for ( int manifoldIndex = 0; manifoldIndex < manifoldCount; ++manifoldIndex ) + { + b3Manifold* manifold = contact->manifolds + manifoldIndex; + b3ManifoldConstraint* constraint = manifoldConstraints + manifoldIndex; + int pointCount = manifold->pointCount; + b3Vec3 normal = manifold->normal; + b3Vec3 tangent1 = b3Perp( normal ); + b3Vec3 tangent2 = b3Cross( tangent1, normal ); + + constraint->pointCount = pointCount; + constraint->normal = normal; + constraint->tangent1 = tangent1; + constraint->tangent2 = tangent2; + + // Stiffer for static contacts to avoid bodies getting pushed through the ground + constraint->tangentVelocity1 = b3Dot( contact->tangentVelocity, constraint->tangent1 ); + constraint->tangentVelocity2 = b3Dot( contact->tangentVelocity, constraint->tangent2 ); + + b3Vec3 centerA = b3Vec3_zero; + b3Vec3 centerB = b3Vec3_zero; + + for ( int pointIndex = 0; pointIndex < pointCount; ++pointIndex ) + { + b3ManifoldConstraintPoint* cp = constraint->points + pointIndex; + + // Copy data from manifold point + b3ManifoldPoint* mp = manifold->points + pointIndex; + cp->rA = mp->anchorA; + cp->rB = mp->anchorB; + cp->baseSeparation = mp->separation - b3Dot( b3Sub( cp->rB, cp->rA ), normal ); + cp->normalImpulse = warmStartScale * mp->normalImpulse; + cp->totalNormalImpulse = 0.0f; + + b3Vec3 rA = cp->rA; + b3Vec3 rB = cp->rB; + + b3Vec3 rnA = b3Cross( rA, normal ); + b3Vec3 rnB = b3Cross( rB, normal ); + float kNormal = mA + mB + b3Dot( rnA, b3MulMV( iA, rnA ) ) + b3Dot( rnB, b3MulMV( iB, rnB ) ); + cp->normalMass = kNormal > 0.0f ? 1.0f / kNormal : 0.0f; + + // Save relative velocity for restitution + b3Vec3 vrA = b3Add( vA, b3Cross( wA, rA ) ); + b3Vec3 vrB = b3Add( vB, b3Cross( wB, rB ) ); + cp->relativeVelocity = b3Dot( normal, b3Sub( vrB, vrA ) ); + + centerA = b3Add( centerA, rA ); + centerB = b3Add( centerB, rB ); + } + + float invCount = 1.0f / pointCount; + centerA = b3MulSV( invCount, centerA ); + centerB = b3MulSV( invCount, centerB ); + constraint->originA = centerA; + constraint->originB = centerB; + + for ( int pointIndex = 0; pointIndex < pointCount; ++pointIndex ) + { + b3ManifoldConstraintPoint* cp = constraint->points + pointIndex; + cp->leverArm = b3Distance( cp->rA, centerA ); + } + + b3Vec3 rtA1 = b3Cross( centerA, tangent1 ); + b3Vec3 rtA2 = b3Cross( centerA, tangent2 ); + b3Vec3 rtB1 = b3Cross( centerB, tangent1 ); + b3Vec3 rtB2 = b3Cross( centerB, tangent2 ); + + { + b3Matrix2 k; + k.cx.x = mA + mB + b3Dot( rtA1, b3MulMV( iA, rtA1 ) ) + b3Dot( rtB1, b3MulMV( iB, rtB1 ) ); + k.cy.y = mA + mB + b3Dot( rtA2, b3MulMV( iA, rtA2 ) ) + b3Dot( rtB2, b3MulMV( iB, rtB2 ) ); + k.cx.y = k.cy.x = b3Dot( rtA1, b3MulMV( iA, rtA2 ) ) + b3Dot( rtB1, b3MulMV( iB, rtB2 ) ); + + constraint->tangentMass = b3Invert2( k ); + constraint->frictionImpulse.x = warmStartScale * b3Dot( manifold->frictionImpulse, tangent1 ); + constraint->frictionImpulse.y = warmStartScale * b3Dot( manifold->frictionImpulse, tangent2 ); + } + + { + float k = b3Dot( normal, b3MulMV( b3AddMM( iA, iB ), normal ) ); + constraint->twistMass = k > 0.0f ? 1.0f / k : 0.0f; + constraint->twistImpulse = warmStartScale * manifold->twistImpulse; + } + + { + constraint->rollingImpulse = b3MulSV( warmStartScale, manifold->rollingImpulse ); + } + } + } + + // Advance to next color + colorIndex += 1; + } + + b3TracyCZoneEnd( prepare_contact ); +} + +void b3WarmStartContacts_Mesh( b3SolverBlock block, b3StepContext* context ) +{ + b3World* world = context->world; + b3GraphColor* color = world->constraintGraph.colors + block.colorIndex; + b3SolverSet* awakeSet = b3Array_Get( world->solverSets, b3_awakeSet ); + b3BodyState* states = awakeSet->bodyStates.data; + b3ContactConstraint* constraints = color->contactConstraints; + + // This is a dummy state to represent a static body because static bodies don't have a solver body. + b3BodyState dummyState = b3_identityBodyState; + + int startIndex = block.startIndex; + int endIndex = startIndex + block.count; + + for ( int constraintIndex = startIndex; constraintIndex < endIndex; ++constraintIndex ) + { + const b3ContactConstraint* contactConstraint = constraints + constraintIndex; + int indexA = contactConstraint->indexA; + int indexB = contactConstraint->indexB; + + b3BodyState* stateA = indexA == B3_NULL_INDEX ? &dummyState : states + indexA; + b3BodyState* stateB = indexB == B3_NULL_INDEX ? &dummyState : states + indexB; + + b3Vec3 vA = stateA->linearVelocity; + b3Vec3 wA = stateA->angularVelocity; + b3Vec3 vB = stateB->linearVelocity; + b3Vec3 wB = stateB->angularVelocity; + + float mA = contactConstraint->invMassA; + b3Matrix3 iA = contactConstraint->invIA; + float mB = contactConstraint->invMassB; + b3Matrix3 iB = contactConstraint->invIB; + + int manifoldCount = contactConstraint->manifoldCount; + for ( int manifoldIndex = 0; manifoldIndex < manifoldCount; ++manifoldIndex ) + { + b3ManifoldConstraint* constraint = contactConstraint->constraints + manifoldIndex; + + // Normal impulses + b3Vec3 normal = constraint->normal; + int pointCount = constraint->pointCount; + for ( int j = 0; j < pointCount; ++j ) + { + const b3ManifoldConstraintPoint* cp = constraint->points + j; + + // fixed anchors + b3Vec3 rA = cp->rA; + b3Vec3 rB = cp->rB; + + b3Vec3 impulse = b3MulSV( cp->normalImpulse, normal ); + wA = b3Sub( wA, b3MulMV( iA, b3Cross( rA, impulse ) ) ); + vA = b3MulSub( vA, mA, impulse ); + wB = b3Add( wB, b3MulMV( iB, b3Cross( rB, impulse ) ) ); + vB = b3MulAdd( vB, mB, impulse ); + } + + // Central friction + { + b3Vec3 rA = constraint->originA; + b3Vec3 rB = constraint->originB; + b3Vec3 impulse = b3MulSV( constraint->frictionImpulse.x, constraint->tangent1 ); + impulse = b3Add( impulse, b3MulSV( constraint->frictionImpulse.y, constraint->tangent2 ) ); + + wA = b3Sub( wA, b3MulMV( iA, b3Cross( rA, impulse ) ) ); + vA = b3MulSub( vA, mA, impulse ); + wB = b3Add( wB, b3MulMV( iB, b3Cross( rB, impulse ) ) ); + vB = b3MulAdd( vB, mB, impulse ); + } + + // Central twist friction + { + b3Vec3 impulse = b3MulSV( constraint->twistImpulse, constraint->normal ); + wA = b3Sub( wA, b3MulMV( iA, impulse ) ); + wB = b3Add( wB, b3MulMV( iB, impulse ) ); + } + + // Rolling resistance + { + b3Vec3 impulse = constraint->rollingImpulse; + wA = b3Sub( wA, b3MulMV( iA, impulse ) ); + wB = b3Add( wB, b3MulMV( iB, impulse ) ); + } + } + + if ( stateA->flags & b3_dynamicFlag ) + { + stateA->linearVelocity = vA; + stateA->angularVelocity = wA; + } + + if ( stateB->flags & b3_dynamicFlag ) + { + stateB->linearVelocity = vB; + stateB->angularVelocity = wB; + } + } +} + +// Merged normal and friction loops. This is much more stable for the Jenga stack. +void b3SolveContacts_Mesh( b3SolverBlock block, b3StepContext* context, bool useBias ) +{ + b3World* world = context->world; + b3GraphColor* color = world->constraintGraph.colors + block.colorIndex; + b3ContactConstraint* contactConstraints = color->contactConstraints; + b3BodyState* states = context->states; + + // This is a dummy state to represent a static body because static bodies don't have a solver body. + b3BodyState dummyState = b3_identityBodyState; + + // The last block might not be full + int startIndex = block.startIndex; + int endIndex = startIndex + block.count; + + float inv_h = context->inv_h; + const float contactSpeed = context->world->contactSpeed; + + for ( int i = startIndex; i < endIndex; ++i ) + { + b3ContactConstraint* contactConstraint = contactConstraints + i; + int manifoldCount = contactConstraint->manifoldCount; + + int indexA = contactConstraint->indexA; + int indexB = contactConstraint->indexB; + + float mA = contactConstraint->invMassA; + b3Matrix3 iA = contactConstraint->invIA; + float mB = contactConstraint->invMassB; + b3Matrix3 iB = contactConstraint->invIB; + + b3BodyState* stateA = indexA == B3_NULL_INDEX ? &dummyState : states + indexA; + b3Vec3 vA = stateA->linearVelocity; + b3Vec3 wA = stateA->angularVelocity; + b3Quat dqA = stateA->deltaRotation; + + b3BodyState* stateB = indexB == B3_NULL_INDEX ? &dummyState : states + indexB; + b3Vec3 vB = stateB->linearVelocity; + b3Vec3 wB = stateB->angularVelocity; + b3Quat dqB = stateB->deltaRotation; + + b3Vec3 dp = b3Sub( stateB->deltaPosition, stateA->deltaPosition ); + b3Softness softness = contactConstraint->softness; + float friction = contactConstraint->friction; + float rollingResistance = contactConstraint->rollingResistance; + + for ( int j = 0; j < manifoldCount; ++j ) + { + b3ManifoldConstraint* constraint = contactConstraint->constraints + j; + + int pointCount = constraint->pointCount; + b3Vec3 normal = constraint->normal; + + float totalNormalImpulse = 0.0f; + float totalTwistLimit = 0.0f; + + for ( int pointIndex = 0; pointIndex < pointCount; ++pointIndex ) + { + b3ManifoldConstraintPoint* cp = constraint->points + pointIndex; + + // Fixed anchor points for applying impulses + b3Vec3 rA = cp->rA; + b3Vec3 rB = cp->rB; + + // compute current separation + // this is subject to round-off error if the anchor is far from the body center of mass + b3Vec3 ds = b3Add( dp, b3Sub( b3RotateVector( dqB, rB ), b3RotateVector( dqA, rA ) ) ); + float s = b3Dot( ds, normal ) + cp->baseSeparation; + + float velocityBias = 0.0f; + float massScale = 1.0f; + float impulseScale = 0.0f; + if ( s > 0.0f ) + { + // speculative bias + velocityBias = s * inv_h; + } + else if ( useBias ) + { + velocityBias = b3MaxFloat( softness.massScale * softness.biasRate * s, -contactSpeed ); + massScale = softness.massScale; + impulseScale = softness.impulseScale; + } + + // relative normal velocity at contact + b3Vec3 vrA = b3Add( vA, b3Cross( wA, rA ) ); + b3Vec3 vrB = b3Add( vB, b3Cross( wB, rB ) ); + float vn = b3Dot( b3Sub( vrB, vrA ), normal ); + + // incremental normal impulse + float deltaImpulse = -cp->normalMass * ( massScale * vn + velocityBias ) - impulseScale * cp->normalImpulse; + + // clamp the accumulated impulse + float newImpulse = b3MaxFloat( cp->normalImpulse + deltaImpulse, 0.0f ); + deltaImpulse = newImpulse - cp->normalImpulse; + cp->normalImpulse = newImpulse; + cp->totalNormalImpulse += newImpulse; + + totalNormalImpulse += newImpulse; + totalTwistLimit += cp->leverArm * cp->normalImpulse; + + // apply normal impulse + b3Vec3 P = b3MulSV( deltaImpulse, normal ); + vA = b3MulSub( vA, mA, P ); + wA = b3Sub( wA, b3MulMV( iA, b3Cross( rA, P ) ) ); + + vB = b3MulAdd( vB, mB, P ); + wB = b3Add( wB, b3MulMV( iB, b3Cross( rB, P ) ) ); + } + + // No friction when applying bias + if ( useBias == true ) + { + // Go to next manifold + continue; + } + + // Central twist friction + { + float twistSpeed = b3Dot( constraint->normal, b3Sub( wB, wA ) ); + float maxImpulse = friction * totalTwistLimit; + float deltaImpulse = -constraint->twistMass * twistSpeed; + float oldImpulse = constraint->twistImpulse; + constraint->twistImpulse = b3ClampFloat( oldImpulse + deltaImpulse, -maxImpulse, maxImpulse ); + deltaImpulse = constraint->twistImpulse - oldImpulse; + + wA = b3Sub( wA, b3MulMV( iA, b3MulSV( deltaImpulse, constraint->normal ) ) ); + wB = b3Add( wB, b3MulMV( iB, b3MulSV( deltaImpulse, constraint->normal ) ) ); + } + + // Rolling resistance + if ( rollingResistance > 0.0f ) + { + b3Vec3 deltaImpulse = b3Neg( b3MulMV( contactConstraint->rollingMass, b3Sub( wB, wA ) ) ); + b3Vec3 oldImpulse = constraint->rollingImpulse; + constraint->rollingImpulse = b3Add( oldImpulse, deltaImpulse ); + + float maxImpulse = rollingResistance * totalNormalImpulse; + float magSqr = b3Dot( constraint->rollingImpulse, constraint->rollingImpulse ); + if ( magSqr > maxImpulse * maxImpulse + FLT_EPSILON ) + { + constraint->rollingImpulse = b3MulSV( maxImpulse / sqrtf( magSqr ), constraint->rollingImpulse ); + } + + deltaImpulse = b3Sub( constraint->rollingImpulse, oldImpulse ); + + wA = b3Sub( wA, b3MulMV( iA, deltaImpulse ) ); + wB = b3Add( wB, b3MulMV( iB, deltaImpulse ) ); + } + + // Central friction + { + b3Vec3 tangent1 = constraint->tangent1; + b3Vec3 tangent2 = constraint->tangent2; + + // Fixed anchor points for applying impulses + b3Vec3 rA = constraint->originA; + b3Vec3 rB = constraint->originB; + + // Relative tangent velocity at contact + b3Vec3 vrA = b3Add( vA, b3Cross( wA, rA ) ); + b3Vec3 vrB = b3Add( vB, b3Cross( wB, rB ) ); + b3Vec3 vr = b3Sub( vrB, vrA ); + b3Vec2 vt = { + b3Dot( vr, tangent1 ) - constraint->tangentVelocity1, + b3Dot( vr, tangent2 ) - constraint->tangentVelocity2, + }; + + // Incremental tangent impulse + b3Vec2 tm = b3MulMV2( constraint->tangentMass, vt ); + b3Vec2 deltaImpulse = { -tm.x, -tm.y }; + b3Vec2 newImpulse = { + constraint->frictionImpulse.x + deltaImpulse.x, + constraint->frictionImpulse.y + deltaImpulse.y, + }; + + float maxImpulse = friction * totalNormalImpulse; + + // Clamp the accumulated impulse + float lengthSquared = b3Dot2( newImpulse, newImpulse ); + if ( lengthSquared > maxImpulse * maxImpulse ) + { + float scale = maxImpulse / sqrtf( lengthSquared ); + newImpulse.x *= scale; + newImpulse.y *= scale; + } + deltaImpulse = b3Sub2( newImpulse, constraint->frictionImpulse ); + constraint->frictionImpulse = newImpulse; + + // Apply delta impulse + b3Vec3 P = b3Blend2( deltaImpulse.x, tangent1, deltaImpulse.y, tangent2 ); + vA = b3MulSub( vA, mA, P ); + wA = b3Sub( wA, b3MulMV( iA, b3Cross( rA, P ) ) ); + vB = b3MulAdd( vB, mB, P ); + wB = b3Add( wB, b3MulMV( iB, b3Cross( rB, P ) ) ); + } + } + + if ( stateA->flags & b3_dynamicFlag ) + { + stateA->linearVelocity = vA; + stateA->angularVelocity = wA; + } + + if ( stateB->flags & b3_dynamicFlag ) + { + stateB->linearVelocity = vB; + stateB->angularVelocity = wB; + } + } +} + +void b3ApplyRestitution_Mesh( b3SolverBlock block, b3StepContext* context ) +{ + b3World* world = context->world; + b3GraphColor* color = world->constraintGraph.colors + block.colorIndex; + b3BodyState* states = context->states; + b3ContactConstraint* constraints = color->contactConstraints; + + // This is a dummy state to represent a static body because static bodies don't have a solver body. + b3BodyState dummyState = b3_identityBodyState; + + int startIndex = block.startIndex; + int endIndex = startIndex + block.count; + + float threshold = context->world->restitutionThreshold; + + for ( int constraintIndex = startIndex; constraintIndex < endIndex; ++constraintIndex ) + { + const b3ContactConstraint* contactConstraint = constraints + constraintIndex; + float restitution = contactConstraint->restitution; + if ( restitution == 0.0f ) + { + continue; + } + + int indexA = contactConstraint->indexA; + int indexB = contactConstraint->indexB; + + b3BodyState* stateA = indexA == B3_NULL_INDEX ? &dummyState : states + indexA; + b3BodyState* stateB = indexB == B3_NULL_INDEX ? &dummyState : states + indexB; + + b3Vec3 vA = stateA->linearVelocity; + b3Vec3 wA = stateA->angularVelocity; + b3Vec3 vB = stateB->linearVelocity; + b3Vec3 wB = stateB->angularVelocity; + + float mA = contactConstraint->invMassA; + b3Matrix3 iA = contactConstraint->invIA; + float mB = contactConstraint->invMassB; + b3Matrix3 iB = contactConstraint->invIB; + + int manifoldCount = contactConstraint->manifoldCount; + for ( int manifoldIndex = 0; manifoldIndex < manifoldCount; ++manifoldIndex ) + { + b3ManifoldConstraint* cm = contactConstraint->constraints + manifoldIndex; + + b3Vec3 normal = cm->normal; + int pointCount = cm->pointCount; + B3_ASSERT( 0 < pointCount && pointCount <= B3_MAX_MANIFOLD_POINTS ); + + for ( int pointIndex = 0; pointIndex < pointCount; ++pointIndex ) + { + b3ManifoldConstraintPoint* cp = cm->points + pointIndex; + + // If the total normal impulse is zero then there was no collision + // this skips speculative contact points that didn't generate an impulse + // The max normal impulse is used in case there was a collision that moved away within the sub-step + // process + if ( cp->relativeVelocity > -threshold || cp->totalNormalImpulse == 0.0f ) + { + continue; + } + + // fixed anchor points + b3Vec3 rA = cp->rA; + b3Vec3 rB = cp->rB; + + // relative normal velocity at contact + b3Vec3 vrB = b3Add( vB, b3Cross( wB, rB ) ); + b3Vec3 vrA = b3Add( vA, b3Cross( wA, rA ) ); + float vn = b3Dot( b3Sub( vrB, vrA ), normal ); + + // compute normal impulse + float impulse = -cp->normalMass * ( vn + restitution * cp->relativeVelocity ); + + // clamp the accumulated impulse + float newImpulse = b3MaxFloat( cp->normalImpulse + impulse, 0.0f ); + impulse = newImpulse - cp->normalImpulse; + cp->normalImpulse = newImpulse; + cp->totalNormalImpulse += impulse; + + // apply contact impulse + b3Vec3 P = b3MulSV( impulse, normal ); + vA = b3MulSub( vA, mA, P ); + wA = b3Sub( wA, b3MulMV( iA, b3Cross( rA, P ) ) ); + vB = b3MulAdd( vB, mB, P ); + wB = b3Add( wB, b3MulMV( iB, b3Cross( rB, P ) ) ); + } + + if ( stateA->flags & b3_dynamicFlag ) + { + stateA->linearVelocity = vA; + stateA->angularVelocity = wA; + } + + if ( stateB->flags & b3_dynamicFlag ) + { + stateB->linearVelocity = vB; + stateB->angularVelocity = wB; + } + } + } +} + +// Don't need to use spans for colors for this because the constraint to contact association +// is already linked by pointer. +void b3StoreImpulses_Mesh( b3SolverBlock block, b3StepContext* context, int workerIndex ) +{ + b3World* world = context->world; + + // Mirror b3PrepareContacts_Mesh: the per-color flat arrays and the overflow color + // each have their own (base, spans, manifoldBase). + b3ContactPrepareSpan* spans = context->contactPrepareSpans; + b3ContactConstraint* base = context->contactConstraints; + + if ( block.blockType == b3_overflowBlock ) + { + b3GraphColor* overflow = world->constraintGraph.colors + B3_OVERFLOW_INDEX; + spans = context->overflowSpans; + base = overflow->contactConstraints; + } + + b3TaskContext* taskContext = world->taskContexts.data + workerIndex; + b3BitSet* hitEventBitSet = &taskContext->hitEventBitSet; + bool hasHitEvents = taskContext->hasHitEvents; + float negHitThreshold = -world->hitEventThreshold; + + int index = block.startIndex; + int endIndex = block.startIndex + block.count; + + // Find color for start index. Linear search but fast. + int colorIndex = 0; + while ( spans[colorIndex + 1].start <= index ) + { + colorIndex += 1; + } + + // Loop over block + while ( index < endIndex ) + { + int colorStart = spans[colorIndex].start; + int colorEndIndex = b3MinInt( spans[colorIndex + 1].start, endIndex ); + + // Loop over color + for ( ; index < colorEndIndex; ++index ) + { + b3ContactConstraint* contactConstraint = base + index; + + int localIndex = index - colorStart; + B3_UNUSED( localIndex ); + B3_ASSERT( 0 <= localIndex && localIndex < spans[colorIndex].count ); + + // Having this contact pointer simplifies impulse storage + b3Contact* contact = contactConstraint->contact; + B3_ASSERT( contact != NULL ); + + // Catches the wrong-(base, spans) pairing: the contact pointer stashed by + // b3PrepareContacts_Mesh at this flat slot must reference the same contact + // the span at this slot describes. + B3_VALIDATE( contact->contactId == spans[colorIndex].contacts[localIndex].contactId ); + + int manifoldCount = contactConstraint->manifoldCount; + B3_ASSERT( manifoldCount == contact->manifoldCount ); + + bool checkHitEvents = ( contact->flags & b3_simEnableHitEvent ) != 0; + bool flagged = false; + + for ( int manifoldIndex = 0; manifoldIndex < manifoldCount; ++manifoldIndex ) + { + b3Manifold* manifold = contact->manifolds + manifoldIndex; + b3ManifoldConstraint* constraint = contactConstraint->constraints + manifoldIndex; + manifold->twistImpulse = constraint->twistImpulse; + manifold->frictionImpulse = b3Blend2( constraint->frictionImpulse.x, constraint->tangent1, + constraint->frictionImpulse.y, constraint->tangent2 ); + manifold->rollingImpulse = constraint->rollingImpulse; + + int count = constraint->pointCount; + B3_ASSERT( count == manifold->pointCount ); + for ( int pointIndex = 0; pointIndex < count; ++pointIndex ) + { + b3ManifoldConstraintPoint* cp = constraint->points + pointIndex; + b3ManifoldPoint* mp = manifold->points + pointIndex; + mp->normalImpulse = cp->normalImpulse; + mp->totalNormalImpulse = cp->totalNormalImpulse; + mp->normalVelocity = cp->relativeVelocity; + + if ( checkHitEvents && flagged == false && + mp->normalVelocity < negHitThreshold && mp->totalNormalImpulse > 0.0f ) + { + b3SetBit( hitEventBitSet, contact->contactId ); + hasHitEvents = true; + flagged = true; + } + } + } + } + + // Advance to next color + colorIndex += 1; + } + + taskContext->hasHitEvents = hasHitEvents; +} + +#if defined( B3_SIMD_NEON ) + +#include + +// wide float holds 4 numbers +typedef float32x4_t b3FloatW; + +#elif defined( B3_SIMD_SSE2 ) + +#include + +// wide float holds 4 numbers +typedef __m128 b3FloatW; + +#else + +// scalar math +typedef struct b3FloatW +{ + float x, y, z, w; +} b3FloatW; + +#endif + +// Wide vec2 +typedef struct b3Vec2W +{ + b3FloatW x, y; +} b3Vec2W; + +// Wide vec3 +typedef struct b3Vec3W +{ + b3FloatW X, Y, Z; +} b3Vec3W; + +// Wide quaternion +typedef struct b3QuatW +{ + b3Vec3W V; + b3FloatW S; +} b3QuatW; + +// Wide symmetric matrix2 +typedef struct b3SymMatrix2W +{ + b3FloatW cxx, cxy, cyy; +} b3SymMatrix2W; + +// Wide symmetric matrix3 +typedef struct b3SymMatrix3W +{ + b3FloatW cxx, cxy, cxz, cyy, cyz, czz; +} b3SymMatrix3W; + +#if defined( B3_SIMD_NEON ) + +static inline b3FloatW b3ZeroW( void ) +{ + return vdupq_n_f32( 0.0f ); +} + +static inline b3FloatW b3SplatW( float scalar ) +{ + return vdupq_n_f32( scalar ); +} + +static inline b3FloatW b3NegW( b3FloatW a ) +{ + return vnegq_f32( a ); +} + +static inline b3FloatW b3SetW( float a, float b, float c, float d ) +{ + float32_t array[4] = { a, b, c, d }; + return vld1q_f32( array ); +} + +static inline b3FloatW b3AddW( b3FloatW a, b3FloatW b ) +{ + return vaddq_f32( a, b ); +} + +static inline b3FloatW b3SubW( b3FloatW a, b3FloatW b ) +{ + return vsubq_f32( a, b ); +} + +static inline b3FloatW b3MulW( b3FloatW a, b3FloatW b ) +{ + return vmulq_f32( a, b ); +} + +static inline b3FloatW b3DivW( b3FloatW a, b3FloatW b ) +{ + return vdivq_f32( a, b ); +} + +static inline b3FloatW b3SqrtW( b3FloatW a ) +{ + return vsqrtq_f32( a ); +} + +// Cannot use real FMA because it doesn't match the non-SIMD path +static inline b3FloatW b3MulAddW( b3FloatW a, b3FloatW b, b3FloatW c ) +{ + return vaddq_f32( a, vmulq_f32( b, c ) ); +} + +// static inline b3FloatW b3MulSubW( b3FloatW a, b3FloatW b, b3FloatW c ) +//{ +// return vsubq_f32( a, vmulq_f32( b, c ) ); +// } + +static inline b3FloatW b3MinW( b3FloatW a, b3FloatW b ) +{ + return vminq_f32( a, b ); +} + +static inline b3FloatW b3MaxW( b3FloatW a, b3FloatW b ) +{ + return vmaxq_f32( a, b ); +} + +// clamp a to [-b, b] +static inline b3FloatW b3SymClampW( b3FloatW a, b3FloatW b ) +{ + b3FloatW nb = b3NegW( b ); + b3FloatW c = b3MaxW( nb, a ); + return b3MinW( c, b ); +} + +static inline b3FloatW b3OrW( b3FloatW a, b3FloatW b ) +{ + return vreinterpretq_f32_u32( vorrq_u32( vreinterpretq_u32_f32( a ), vreinterpretq_u32_f32( b ) ) ); +} + +static inline b3FloatW b3GreaterThanW( b3FloatW a, b3FloatW b ) +{ + return vreinterpretq_f32_u32( vcgtq_f32( a, b ) ); +} + +static inline b3FloatW b3EqualsW( b3FloatW a, b3FloatW b ) +{ + return vreinterpretq_f32_u32( vceqq_f32( a, b ) ); +} + +static inline bool b3AllZeroW( b3FloatW a ) +{ + // Create a zero vector for comparison + b3FloatW zero = vdupq_n_f32( 0.0f ); + + // Compare the input vector with zero + uint32x4_t cmp_result = vceqq_f32( a, zero ); + +// Check if all comparison results are non-zero using vminvq +#ifdef __ARM_FEATURE_SVE + // ARM v8.2+ has horizontal minimum instruction + return vminvq_u32( cmp_result ) != 0; +#else + // For older ARM architectures, we need to manually check all lanes + return vgetq_lane_u32( cmp_result, 0 ) != 0 && vgetq_lane_u32( cmp_result, 1 ) != 0 && vgetq_lane_u32( cmp_result, 2 ) != 0 && + vgetq_lane_u32( cmp_result, 3 ) != 0; +#endif +} + +// component-wise returns mask ? b : a +static inline b3FloatW b3BlendW( b3FloatW a, b3FloatW b, b3FloatW mask ) +{ + uint32x4_t mask32 = vreinterpretq_u32_f32( mask ); + return vbslq_f32( mask32, b, a ); +} + +#elif defined( B3_SIMD_SSE2 ) + +static inline b3FloatW b3ZeroW( void ) +{ + return _mm_setzero_ps(); +} + +static inline b3FloatW b3SplatW( float scalar ) +{ + return _mm_set1_ps( scalar ); +} + +static inline b3FloatW b3NegW( b3FloatW a ) +{ + // Create a mask with the sign bit set for each element + __m128 mask = _mm_set1_ps( -0.0f ); + + // XOR the input with the mask to negate each element + return _mm_xor_ps( a, mask ); +} + +static inline b3FloatW b3SetW( float a, float b, float c, float d ) +{ + return _mm_setr_ps( a, b, c, d ); +} + +static inline b3FloatW b3AddW( b3FloatW a, b3FloatW b ) +{ + return _mm_add_ps( a, b ); +} + +static inline b3FloatW b3SubW( b3FloatW a, b3FloatW b ) +{ + return _mm_sub_ps( a, b ); +} + +static inline b3FloatW b3MulW( b3FloatW a, b3FloatW b ) +{ + return _mm_mul_ps( a, b ); +} + +static inline b3FloatW b3DivW( b3FloatW a, b3FloatW b ) +{ + return _mm_div_ps( a, b ); +} + +static inline b3FloatW b3SqrtW( b3FloatW a ) +{ + return _mm_sqrt_ps( a ); +} + +static inline b3FloatW b3MulAddW( b3FloatW a, b3FloatW b, b3FloatW c ) +{ + return _mm_add_ps( a, _mm_mul_ps( b, c ) ); +} + +// static inline b3FloatW b3MulSubW( b3FloatW a, b3FloatW b, b3FloatW c ) +//{ +// return _mm_sub_ps( a, _mm_mul_ps( b, c ) ); +// } + +static inline b3FloatW b3MinW( b3FloatW a, b3FloatW b ) +{ + return _mm_min_ps( a, b ); +} + +static inline b3FloatW b3MaxW( b3FloatW a, b3FloatW b ) +{ + return _mm_max_ps( a, b ); +} + +// clamp a to [-b, b] +static inline b3FloatW b3SymClampW( b3FloatW a, b3FloatW b ) +{ + b3FloatW nb = b3NegW( b ); + b3FloatW c = b3MaxW( nb, a ); + return b3MinW( c, b ); +} + +static inline b3FloatW b3OrW( b3FloatW a, b3FloatW b ) +{ + return _mm_or_ps( a, b ); +} + +static inline b3FloatW b3GreaterThanW( b3FloatW a, b3FloatW b ) +{ + return _mm_cmpgt_ps( a, b ); +} + +static inline b3FloatW b3EqualsW( b3FloatW a, b3FloatW b ) +{ + return _mm_cmpeq_ps( a, b ); +} + +static inline bool b3AllZeroW( b3FloatW a ) +{ + // Compare each element with zero + b3FloatW zero = _mm_setzero_ps(); + b3FloatW cmp = _mm_cmpeq_ps( a, zero ); + + // Create a mask from the comparison results + int mask = _mm_movemask_ps( cmp ); + + // If all elements are zero, the mask will be 0xF (1111 in binary) + return mask == 0xF; +} + +// component-wise returns mask ? b : a +static inline b3FloatW b3BlendW( b3FloatW a, b3FloatW b, b3FloatW mask ) +{ + return _mm_or_ps( _mm_and_ps( mask, b ), _mm_andnot_ps( mask, a ) ); +} + +#else + +static inline b3FloatW b3ZeroW( void ) +{ + return (b3FloatW){ 0.0f, 0.0f, 0.0f, 0.0f }; +} + +static inline b3FloatW b3SplatW( float scalar ) +{ + return (b3FloatW){ scalar, scalar, scalar, scalar }; +} + +static inline b3FloatW b3NegW( b3FloatW a ) +{ + return (b3FloatW){ -a.x, -a.y, -a.z, -a.w }; +} + +static inline b3FloatW b3AddW( b3FloatW a, b3FloatW b ) +{ + return (b3FloatW){ a.x + b.x, a.y + b.y, a.z + b.z, a.w + b.w }; +} + +static inline b3FloatW b3SubW( b3FloatW a, b3FloatW b ) +{ + return (b3FloatW){ a.x - b.x, a.y - b.y, a.z - b.z, a.w - b.w }; +} + +static inline b3FloatW b3MulW( b3FloatW a, b3FloatW b ) +{ + return (b3FloatW){ a.x * b.x, a.y * b.y, a.z * b.z, a.w * b.w }; +} + +static inline b3FloatW b3DivW( b3FloatW a, b3FloatW b ) +{ + return (b3FloatW){ a.x / b.x, a.y / b.y, a.z / b.z, a.w / b.w }; +} + +static inline b3FloatW b3SqrtW( b3FloatW a ) +{ + return (b3FloatW){ sqrtf( a.x ), sqrtf( a.y ), sqrtf( a.z ), sqrtf( a.w ) }; +} + +static inline b3FloatW b3MulAddW( b3FloatW a, b3FloatW b, b3FloatW c ) +{ + return (b3FloatW){ a.x + b.x * c.x, a.y + b.y * c.y, a.z + b.z * c.z, a.w + b.w * c.w }; +} + +// static inline b3FloatW b3MulSubW( b3FloatW a, b3FloatW b, b3FloatW c ) +//{ +// return { a.x - b.x * c.x, a.y - b.y * c.y, a.z - b.z * c.z, a.w - b.w * c.w }; +// } + +// static inline b3FloatW b3MinW( b3FloatW a, b3FloatW b ) +//{ +// b3FloatW r; +// r.x = a.x <= b.x ? a.x : b.x; +// r.y = a.y <= b.y ? a.y : b.y; +// r.z = a.z <= b.z ? a.z : b.z; +// r.w = a.w <= b.w ? a.w : b.w; +// return r; +// } + +static inline b3FloatW b3MaxW( b3FloatW a, b3FloatW b ) +{ + b3FloatW r; + r.x = a.x >= b.x ? a.x : b.x; + r.y = a.y >= b.y ? a.y : b.y; + r.z = a.z >= b.z ? a.z : b.z; + r.w = a.w >= b.w ? a.w : b.w; + return r; +} + +// clamp a to [-b, b] +static inline b3FloatW b3SymClampW( b3FloatW a, b3FloatW b ) +{ + b3FloatW r; + r.x = a.x <= b.x ? a.x : b.x; + r.y = a.y <= b.y ? a.y : b.y; + r.z = a.z <= b.z ? a.z : b.z; + r.w = a.w <= b.w ? a.w : b.w; + r.x = r.x <= -b.x ? -b.x : r.x; + r.y = r.y <= -b.y ? -b.y : r.y; + r.z = r.z <= -b.z ? -b.z : r.z; + r.w = r.w <= -b.w ? -b.w : r.w; + return r; +} + +static inline b3FloatW b3OrW( b3FloatW a, b3FloatW b ) +{ + b3FloatW r; + r.x = a.x != 0.0f || b.x != 0.0f ? 1.0f : 0.0f; + r.y = a.y != 0.0f || b.y != 0.0f ? 1.0f : 0.0f; + r.z = a.z != 0.0f || b.z != 0.0f ? 1.0f : 0.0f; + r.w = a.w != 0.0f || b.w != 0.0f ? 1.0f : 0.0f; + return r; +} + +static inline b3FloatW b3GreaterThanW( b3FloatW a, b3FloatW b ) +{ + b3FloatW r; + r.x = a.x > b.x ? 1.0f : 0.0f; + r.y = a.y > b.y ? 1.0f : 0.0f; + r.z = a.z > b.z ? 1.0f : 0.0f; + r.w = a.w > b.w ? 1.0f : 0.0f; + return r; +} + +static inline b3FloatW b3EqualsW( b3FloatW a, b3FloatW b ) +{ + b3FloatW r; + r.x = a.x == b.x ? 1.0f : 0.0f; + r.y = a.y == b.y ? 1.0f : 0.0f; + r.z = a.z == b.z ? 1.0f : 0.0f; + r.w = a.w == b.w ? 1.0f : 0.0f; + return r; +} + +static inline bool b3AllZeroW( b3FloatW a ) +{ + return a.x == 0.0f && a.y == 0.0f && a.z == 0.0f && a.w == 0.0f; +} + +// component-wise returns mask ? b : a +static inline b3FloatW b3BlendW( b3FloatW a, b3FloatW b, b3FloatW mask ) +{ + b3FloatW r; + r.x = mask.x != 0.0f ? b.x : a.x; + r.y = mask.y != 0.0f ? b.y : a.y; + r.z = mask.z != 0.0f ? b.z : a.z; + r.w = mask.w != 0.0f ? b.w : a.w; + return r; +} + +#endif + +// s * a +static inline b3Vec3W b3MulSVW( b3FloatW s, b3Vec3W a ) +{ + return (b3Vec3W){ b3MulW( s, a.X ), b3MulW( s, a.Y ), b3MulW( s, a.Z ) }; +} + +// a - s * b +static inline b3Vec3W b3MulSubSVW( b3Vec3W a, b3FloatW s, b3Vec3W b ) +{ + return (b3Vec3W){ b3SubW( a.X, b3MulW( s, b.X ) ), b3SubW( a.Y, b3MulW( s, b.Y ) ), b3SubW( a.Z, b3MulW( s, b.Z ) ) }; +} + +// a + s * b +static inline b3Vec3W b3MulAddSVW( b3Vec3W a, b3FloatW s, b3Vec3W b ) +{ + return (b3Vec3W){ b3AddW( a.X, b3MulW( s, b.X ) ), b3AddW( a.Y, b3MulW( s, b.Y ) ), b3AddW( a.Z, b3MulW( s, b.Z ) ) }; +} + +// a + b +static inline b3Vec2W b3AddV2W( b3Vec2W a, b3Vec2W b ) +{ + return (b3Vec2W){ + b3AddW( a.x, b.x ), + b3AddW( a.y, b.y ), + }; +} + +// a - b +static inline b3Vec3W b3SubVW( b3Vec3W a, b3Vec3W b ) +{ + return (b3Vec3W){ + b3SubW( a.X, b.X ), + b3SubW( a.Y, b.Y ), + b3SubW( a.Z, b.Z ), + }; +} + +// a + b +static inline b3Vec3W b3AddVW( b3Vec3W a, b3Vec3W b ) +{ + return (b3Vec3W){ + b3AddW( a.X, b.X ), + b3AddW( a.Y, b.Y ), + b3AddW( a.Z, b.Z ), + }; +} + +// m * a +static inline b3Vec2W b3MulMV2W( b3SymMatrix2W m, b3Vec2W a ) +{ + b3Vec2W b = { + b3AddW( b3MulW( m.cxx, a.x ), b3MulW( m.cxy, a.y ) ), + b3AddW( b3MulW( m.cxy, a.x ), b3MulW( m.cyy, a.y ) ), + }; + + return b; +} + +// m * a +static inline b3Vec3W b3MulMVW( b3SymMatrix3W m, b3Vec3W a ) +{ + b3Vec3W b = { + b3AddW( b3MulW( m.cxx, a.X ), b3AddW( b3MulW( m.cxy, a.Y ), b3MulW( m.cxz, a.Z ) ) ), + b3AddW( b3MulW( m.cxy, a.X ), b3AddW( b3MulW( m.cyy, a.Y ), b3MulW( m.cyz, a.Z ) ) ), + b3AddW( b3MulW( m.cxz, a.X ), b3AddW( b3MulW( m.cyz, a.Y ), b3MulW( m.czz, a.Z ) ) ), + }; + + return b; +} + +// a - m * b +static inline b3Vec3W b3MulSubMVW( b3Vec3W a, b3SymMatrix3W m, b3Vec3W b ) +{ + b3Vec3W c = { + b3AddW( b3MulW( m.cxx, b.X ), b3AddW( b3MulW( m.cxy, b.Y ), b3MulW( m.cxz, b.Z ) ) ), + b3AddW( b3MulW( m.cxy, b.X ), b3AddW( b3MulW( m.cyy, b.Y ), b3MulW( m.cyz, b.Z ) ) ), + b3AddW( b3MulW( m.cxz, b.X ), b3AddW( b3MulW( m.cyz, b.Y ), b3MulW( m.czz, b.Z ) ) ), + }; + + return (b3Vec3W){ b3SubW( a.X, c.X ), b3SubW( a.Y, c.Y ), b3SubW( a.Z, c.Z ) }; +} + +// a + m * b +static inline b3Vec3W b3MulAddMVW( b3Vec3W a, b3SymMatrix3W m, b3Vec3W b ) +{ + b3Vec3W c = { + b3AddW( b3MulW( m.cxx, b.X ), b3AddW( b3MulW( m.cxy, b.Y ), b3MulW( m.cxz, b.Z ) ) ), + b3AddW( b3MulW( m.cxy, b.X ), b3AddW( b3MulW( m.cyy, b.Y ), b3MulW( m.cyz, b.Z ) ) ), + b3AddW( b3MulW( m.cxz, b.X ), b3AddW( b3MulW( m.cyz, b.Y ), b3MulW( m.czz, b.Z ) ) ), + }; + + return (b3Vec3W){ b3AddW( a.X, c.X ), b3AddW( a.Y, c.Y ), b3AddW( a.Z, c.Z ) }; +} + +static inline b3FloatW b3DotW( b3Vec3W a, b3Vec3W b ) +{ + return b3AddW( b3AddW( b3MulW( a.X, b.X ), b3MulW( a.Y, b.Y ) ), b3MulW( a.Z, b.Z ) ); +} + +static inline b3Vec3W b3CrossW( b3Vec3W a, b3Vec3W b ) +{ + b3Vec3W c; + c.X = b3SubW( b3MulW( a.Y, b.Z ), b3MulW( a.Z, b.Y ) ); + c.Y = b3SubW( b3MulW( a.Z, b.X ), b3MulW( a.X, b.Z ) ); + c.Z = b3SubW( b3MulW( a.X, b.Y ), b3MulW( a.Y, b.X ) ); + return c; +} + +static inline b3Vec3W b3RotateVectorW( b3QuatW q, b3Vec3W a ) +{ + b3Vec3W t1 = b3CrossW( q.V, a ); + b3Vec3W t2; + t2.X = b3MulAddW( t1.X, q.S, a.X ); + t2.Y = b3MulAddW( t1.Y, q.S, a.Y ); + t2.Z = b3MulAddW( t1.Z, q.S, a.Z ); + b3Vec3W t3 = b3CrossW( q.V, t2 ); + b3FloatW two = b3SplatW( 2.0f ); + b3Vec3W b; + b.X = b3MulAddW( a.X, two, t3.X ); + b.Y = b3MulAddW( a.Y, two, t3.Y ); + b.Z = b3MulAddW( a.Z, two, t3.Z ); + return b; +} + +// Soft contact constraints with sub-stepping support +// Uses fixed anchors for Jacobians for better behavior on rolling shapes (circles & capsules) +// http://mmacklin.com/smallsteps.pdf +// https://box2d.org/files/ErinCatto_SoftConstraints_GDC2011.pdf + +typedef struct b3ContactConstraintPointWide +{ + b3Vec3W anchorAs, anchorBs; + b3FloatW baseSeparations; + b3FloatW normalImpulses; + b3FloatW totalNormalImpulses; + b3FloatW normalMasses; + b3FloatW leverArms; + b3FloatW relativeVelocities; +} b3ContactConstraintPointWide; + +// Solves four points +typedef struct b3ContactConstraintWide +{ + // These are base 1 + int indexA[B3_SIMD_WIDTH]; + int indexB[B3_SIMD_WIDTH]; + + b3FloatW invMassA, invMassB; + b3SymMatrix3W invIA, invIB; + b3Vec3W normal; + + // todo test computing the tangents on the fly, at least tangent2 + b3Vec3W tangent1; + b3Vec3W tangent2; + + b3Vec3W originA, originB; + b3FloatW twistMass; + b3FloatW twistImpulse; + b3SymMatrix2W tangentMass; + b3Vec2W frictionImpulse; + b3SymMatrix3W rollingMass; + b3Vec3W rollingImpulse; + b3FloatW friction; + b3FloatW rollingResistance; + b3FloatW tangentVelocity1; + b3FloatW tangentVelocity2; + + b3FloatW biasRate; + b3FloatW massScale; + b3FloatW impulseScale; + b3FloatW restitution; + + b3Manifold* manifolds[B3_SIMD_WIDTH]; + + // todo store the maximum point count per wide constraint + // to make this work I need zero initialization which is too + // expensive for all the wide constraint data. Instead + // the graph color should store the point count as a compact secondary + // transient array with zero initialization. + b3ContactConstraintPointWide points[B3_MAX_MANIFOLD_POINTS]; + +} b3ContactConstraintWide; + +int b3GetWideContactConstraintByteCount( void ) +{ + return sizeof( b3ContactConstraintWide ); +} + +// wide version of b3BodyState +typedef struct b3BodyStateW +{ + b3Vec3W v; + b3Vec3W w; + b3Vec3W dp; + b3QuatW dq; +} b3BodyStateW; + +#if defined( B3_SIMD_SSE2 ) || defined( B3_SIMD_NEON ) + +static b3BodyStateW b3GatherBodies( const b3BodyState* states, int* indices ) +{ + b3BodyState dummy = { 0 }; + dummy.deltaRotation.s = 1.0f; + + // Indices are 0 for null + b3BodyState b1 = indices[0] == 0 ? dummy : states[indices[0] - 1]; + b3BodyState b2 = indices[1] == 0 ? dummy : states[indices[1] - 1]; + b3BodyState b3 = indices[2] == 0 ? dummy : states[indices[2] - 1]; + b3BodyState b4 = indices[3] == 0 ? dummy : states[indices[3] - 1]; + + b3BodyStateW s; + s.v.X = b3SetW( b1.linearVelocity.x, b2.linearVelocity.x, b3.linearVelocity.x, b4.linearVelocity.x ); + s.v.Y = b3SetW( b1.linearVelocity.y, b2.linearVelocity.y, b3.linearVelocity.y, b4.linearVelocity.y ); + s.v.Z = b3SetW( b1.linearVelocity.z, b2.linearVelocity.z, b3.linearVelocity.z, b4.linearVelocity.z ); + + s.w.X = b3SetW( b1.angularVelocity.x, b2.angularVelocity.x, b3.angularVelocity.x, b4.angularVelocity.x ); + s.w.Y = b3SetW( b1.angularVelocity.y, b2.angularVelocity.y, b3.angularVelocity.y, b4.angularVelocity.y ); + s.w.Z = b3SetW( b1.angularVelocity.z, b2.angularVelocity.z, b3.angularVelocity.z, b4.angularVelocity.z ); + + s.dp.X = b3SetW( b1.deltaPosition.x, b2.deltaPosition.x, b3.deltaPosition.x, b4.deltaPosition.x ); + s.dp.Y = b3SetW( b1.deltaPosition.y, b2.deltaPosition.y, b3.deltaPosition.y, b4.deltaPosition.y ); + s.dp.Z = b3SetW( b1.deltaPosition.z, b2.deltaPosition.z, b3.deltaPosition.z, b4.deltaPosition.z ); + + s.dq.V.X = b3SetW( b1.deltaRotation.v.x, b2.deltaRotation.v.x, b3.deltaRotation.v.x, b4.deltaRotation.v.x ); + s.dq.V.Y = b3SetW( b1.deltaRotation.v.y, b2.deltaRotation.v.y, b3.deltaRotation.v.y, b4.deltaRotation.v.y ); + s.dq.V.Z = b3SetW( b1.deltaRotation.v.z, b2.deltaRotation.v.z, b3.deltaRotation.v.z, b4.deltaRotation.v.z ); + s.dq.S = b3SetW( b1.deltaRotation.s, b2.deltaRotation.s, b3.deltaRotation.s, b4.deltaRotation.s ); + return s; +} + +// This writes only the velocities back to the solver bodies +static void b3ScatterBodies( b3BodyState* states, int* indices, const b3BodyStateW* simdBody ) +{ + const float* vx = (const float*)&simdBody->v.X; + const float* vy = (const float*)&simdBody->v.Y; + const float* vz = (const float*)&simdBody->v.Z; + const float* wx = (const float*)&simdBody->w.X; + const float* wy = (const float*)&simdBody->w.Y; + const float* wz = (const float*)&simdBody->w.Z; + + // I don't use any dummy body in the body array because this will lead to multithreaded sharing and the + // associated cache flushing. + + // Warning: indices start at 1 with 0 indicating null + + if ( indices[0] != 0 && ( states[indices[0] - 1].flags & b3_dynamicFlag ) != 0 ) + { + b3BodyState* s = states + ( indices[0] - 1 ); + + b3Vec3 v = { vx[0], vy[0], vz[0] }; + b3Vec3 w = { wx[0], wy[0], wz[0] }; + + uint32_t flags = s->flags; + if ( flags & b3_allLocks ) + { + v.x = ( flags & b3_lockLinearX ) ? 0.0f : v.x; + v.y = ( flags & b3_lockLinearY ) ? 0.0f : v.y; + v.z = ( flags & b3_lockLinearZ ) ? 0.0f : v.z; + w.x = ( flags & b3_lockAngularX ) ? 0.0f : w.x; + w.y = ( flags & b3_lockAngularY ) ? 0.0f : w.y; + w.z = ( flags & b3_lockAngularZ ) ? 0.0f : w.z; + } + + s->linearVelocity = v; + s->angularVelocity = w; + } + + if ( indices[1] != 0 && ( states[indices[1] - 1].flags & b3_dynamicFlag ) != 0 ) + { + b3BodyState* s = states + ( indices[1] - 1 ); + + b3Vec3 v = { vx[1], vy[1], vz[1] }; + b3Vec3 w = { wx[1], wy[1], wz[1] }; + + uint32_t flags = s->flags; + if ( flags & b3_allLocks ) + { + v.x = ( flags & b3_lockLinearX ) ? 0.0f : v.x; + v.y = ( flags & b3_lockLinearY ) ? 0.0f : v.y; + v.z = ( flags & b3_lockLinearZ ) ? 0.0f : v.z; + w.x = ( flags & b3_lockAngularX ) ? 0.0f : w.x; + w.y = ( flags & b3_lockAngularY ) ? 0.0f : w.y; + w.z = ( flags & b3_lockAngularZ ) ? 0.0f : w.z; + } + + s->linearVelocity = v; + s->angularVelocity = w; + } + + if ( indices[2] != 0 && ( states[indices[2] - 1].flags & b3_dynamicFlag ) != 0 ) + { + b3BodyState* s = states + ( indices[2] - 1 ); + + b3Vec3 v = { vx[2], vy[2], vz[2] }; + b3Vec3 w = { wx[2], wy[2], wz[2] }; + + uint32_t flags = s->flags; + if ( flags & b3_allLocks ) + { + v.x = ( flags & b3_lockLinearX ) ? 0.0f : v.x; + v.y = ( flags & b3_lockLinearY ) ? 0.0f : v.y; + v.z = ( flags & b3_lockLinearZ ) ? 0.0f : v.z; + w.x = ( flags & b3_lockAngularX ) ? 0.0f : w.x; + w.y = ( flags & b3_lockAngularY ) ? 0.0f : w.y; + w.z = ( flags & b3_lockAngularZ ) ? 0.0f : w.z; + } + + s->linearVelocity = v; + s->angularVelocity = w; + } + + if ( indices[3] != 0 && ( states[indices[3] - 1].flags & b3_dynamicFlag ) != 0 ) + { + b3BodyState* s = states + ( indices[3] - 1 ); + + b3Vec3 v = { vx[3], vy[3], vz[3] }; + b3Vec3 w = { wx[3], wy[3], wz[3] }; + + uint32_t flags = s->flags; + if ( flags & b3_allLocks ) + { + v.x = ( flags & b3_lockLinearX ) ? 0.0f : v.x; + v.y = ( flags & b3_lockLinearY ) ? 0.0f : v.y; + v.z = ( flags & b3_lockLinearZ ) ? 0.0f : v.z; + w.x = ( flags & b3_lockAngularX ) ? 0.0f : w.x; + w.y = ( flags & b3_lockAngularY ) ? 0.0f : w.y; + w.z = ( flags & b3_lockAngularZ ) ? 0.0f : w.z; + } + + s->linearVelocity = v; + s->angularVelocity = w; + } +} + +#else // non-simd + +static b3BodyStateW b3GatherBodies( const b3BodyState* states, int* indices ) +{ + b3BodyState identity = b3_identityBodyState; + + b3BodyState s1 = indices[0] == 0 ? identity : states[indices[0] - 1]; + b3BodyState s2 = indices[1] == 0 ? identity : states[indices[1] - 1]; + b3BodyState s3 = indices[2] == 0 ? identity : states[indices[2] - 1]; + b3BodyState s4 = indices[3] == 0 ? identity : states[indices[3] - 1]; + + b3BodyStateW simdBody; + simdBody.v.X = (b3FloatW){ s1.linearVelocity.x, s2.linearVelocity.x, s3.linearVelocity.x, s4.linearVelocity.x }; + simdBody.v.Y = (b3FloatW){ s1.linearVelocity.y, s2.linearVelocity.y, s3.linearVelocity.y, s4.linearVelocity.y }; + simdBody.v.Z = (b3FloatW){ s1.linearVelocity.z, s2.linearVelocity.z, s3.linearVelocity.z, s4.linearVelocity.z }; + simdBody.w.X = (b3FloatW){ s1.angularVelocity.x, s2.angularVelocity.x, s3.angularVelocity.x, s4.angularVelocity.x }; + simdBody.w.Y = (b3FloatW){ s1.angularVelocity.y, s2.angularVelocity.y, s3.angularVelocity.y, s4.angularVelocity.y }; + simdBody.w.Z = (b3FloatW){ s1.angularVelocity.z, s2.angularVelocity.z, s3.angularVelocity.z, s4.angularVelocity.z }; + simdBody.dp.X = (b3FloatW){ s1.deltaPosition.x, s2.deltaPosition.x, s3.deltaPosition.x, s4.deltaPosition.x }; + simdBody.dp.Y = (b3FloatW){ s1.deltaPosition.y, s2.deltaPosition.y, s3.deltaPosition.y, s4.deltaPosition.y }; + simdBody.dp.Z = (b3FloatW){ s1.deltaPosition.z, s2.deltaPosition.z, s3.deltaPosition.z, s4.deltaPosition.z }; + simdBody.dq.V.X = (b3FloatW){ s1.deltaRotation.v.x, s2.deltaRotation.v.x, s3.deltaRotation.v.x, s4.deltaRotation.v.x }; + simdBody.dq.V.Y = (b3FloatW){ s1.deltaRotation.v.y, s2.deltaRotation.v.y, s3.deltaRotation.v.y, s4.deltaRotation.v.y }; + simdBody.dq.V.Z = (b3FloatW){ s1.deltaRotation.v.z, s2.deltaRotation.v.z, s3.deltaRotation.v.z, s4.deltaRotation.v.z }; + simdBody.dq.S = (b3FloatW){ s1.deltaRotation.s, s2.deltaRotation.s, s3.deltaRotation.s, s4.deltaRotation.s }; + + return simdBody; +} + +// This writes only the velocities back to the solver bodies +static void b3ScatterBodies( b3BodyState* states, int* indices, const b3BodyStateW* simdBody ) +{ + int index1 = indices[0] - 1; + if ( index1 != -1 && ( states[index1].flags & b3_dynamicFlag ) != 0 ) + { + b3BodyState* state = states + index1; + state->linearVelocity.x = simdBody->v.X.x; + state->linearVelocity.y = simdBody->v.Y.x; + state->linearVelocity.z = simdBody->v.Z.x; + state->angularVelocity.x = simdBody->w.X.x; + state->angularVelocity.y = simdBody->w.Y.x; + state->angularVelocity.z = simdBody->w.Z.x; + } + + int index2 = indices[1] - 1; + if ( index2 != -1 && ( states[index2].flags & b3_dynamicFlag ) != 0 ) + { + b3BodyState* state = states + index2; + state->linearVelocity.x = simdBody->v.X.y; + state->linearVelocity.y = simdBody->v.Y.y; + state->linearVelocity.z = simdBody->v.Z.y; + state->angularVelocity.x = simdBody->w.X.y; + state->angularVelocity.y = simdBody->w.Y.y; + state->angularVelocity.z = simdBody->w.Z.y; + } + + int index3 = indices[2] - 1; + if ( index3 != -1 && ( states[index3].flags & b3_dynamicFlag ) != 0 ) + { + b3BodyState* state = states + index3; + state->linearVelocity.x = simdBody->v.X.z; + state->linearVelocity.y = simdBody->v.Y.z; + state->linearVelocity.z = simdBody->v.Z.z; + state->angularVelocity.x = simdBody->w.X.z; + state->angularVelocity.y = simdBody->w.Y.z; + state->angularVelocity.z = simdBody->w.Z.z; + } + + int index4 = indices[3] - 1; + if ( index4 != -1 && ( states[index4].flags & b3_dynamicFlag ) != 0 ) + { + b3BodyState* state = states + index4; + state->linearVelocity.x = simdBody->v.X.w; + state->linearVelocity.y = simdBody->v.Y.w; + state->linearVelocity.z = simdBody->v.Z.w; + state->angularVelocity.x = simdBody->w.X.w; + state->angularVelocity.y = simdBody->w.Y.w; + state->angularVelocity.z = simdBody->w.Z.w; + } +} +#endif + +// Prepare convex contact constraints +void b3PrepareContacts_Convex( b3SolverBlock block, b3StepContext* context ) +{ + b3TracyCZoneNC( prepare_contact, "Prepare Contact", b3_colorYellow, true ); + b3World* world = context->world; + b3BodySim* sims = context->sims; + b3BodyState* states = context->states; +#if B3_ENABLE_VALIDATION + b3Body* bodies = world->bodies.data; +#endif + b3WidePrepareSpan* spans = context->widePrepareSpans; + b3ContactConstraintWide* wideBase = context->wideConstraints; + + // Stiffer for static contacts to avoid bodies getting pushed through the ground + b3Softness contactSoftness = context->contactSoftness; + b3Softness staticSoftness = context->staticSoftness; + + float warmStartScale = world->enableWarmStarting ? 1.0f : 0.0f; + + int wideIndex = block.startIndex; + int endWideIndex = block.startIndex + block.count; + + // Find color for start index. Linear search but fast. + int colorIndex = 0; + while ( spans[colorIndex + 1].start <= wideIndex ) + { + colorIndex += 1; + } + + // Loop over block + while ( wideIndex < endWideIndex ) + { + int colorWideStart = spans[colorIndex].start; + int colorWideEndIndex = b3MinInt( spans[colorIndex + 1].start, endWideIndex ); + int colorContactCount = spans[colorIndex].count; + int* contactIds = spans[colorIndex].contacts; + + // Loop over color + for ( ; wideIndex < colorWideEndIndex; ++wideIndex ) + { + b3ContactConstraintWide* constraint = wideBase + wideIndex; + int localWideIndex = wideIndex - colorWideStart; + + for ( int lane = 0; lane < B3_SIMD_WIDTH; ++lane ) + { + int contactIndex = B3_SIMD_WIDTH * localWideIndex + lane; + if ( contactIndex >= colorContactCount ) + { + // Remainder lanes were zeroed in solver setup. + break; + } + + int contactId = contactIds[contactIndex]; + b3Contact* contact = b3Array_Get( world->contacts, contactId ); + B3_ASSERT( contact->manifoldCount == 1 ); + b3Manifold* manifold = contact->manifolds + 0; + + int indexA = contact->bodySimIndexA; + int indexB = contact->bodySimIndexB; + +#if B3_ENABLE_VALIDATION + b3Body* bodyA = bodies + contact->edges[0].bodyId; + int validIndexA = bodyA->setIndex == b3_awakeSet ? bodyA->localIndex : B3_NULL_INDEX; + b3Body* bodyB = bodies + contact->edges[1].bodyId; + int validIndexB = bodyB->setIndex == b3_awakeSet ? bodyB->localIndex : B3_NULL_INDEX; + B3_ASSERT( indexA == validIndexA ); + B3_ASSERT( indexB == validIndexB ); +#endif + + // 0 for null + constraint->indexA[lane] = indexA + 1; + constraint->indexB[lane] = indexB + 1; + constraint->manifolds[lane] = manifold; + + // Body A data + float mA; + b3Matrix3 iA; + b3Vec3 vA; + b3Vec3 wA; + + if ( indexA == B3_NULL_INDEX ) + { + mA = 0.0f; + iA = b3Mat3_zero; + vA = b3Vec3_zero; + wA = b3Vec3_zero; + } + else + { + b3BodySim* simA = sims + indexA; + mA = simA->invMass; + iA = simA->invInertiaWorld; + + b3BodyState* stateA = states + indexA; + vA = stateA->linearVelocity; + wA = stateA->angularVelocity; + } + + // Body B data + float mB; + b3Matrix3 iB; + b3Vec3 vB; + b3Vec3 wB; + + if ( indexB == B3_NULL_INDEX ) + { + mB = 0.0f; + iB = b3Mat3_zero; + vB = b3Vec3_zero; + wB = b3Vec3_zero; + } + else + { + b3BodySim* simB = sims + indexB; + mB = simB->invMass; + iB = simB->invInertiaWorld; + + b3BodyState* stateB = states + indexB; + vB = stateB->linearVelocity; + wB = stateB->angularVelocity; + } + + ( (float*)&constraint->invMassA )[lane] = mA; + ( (float*)&constraint->invMassB )[lane] = mB; + + ( (float*)&constraint->invIA.cxx )[lane] = iA.cx.x; + ( (float*)&constraint->invIA.cxy )[lane] = iA.cx.y; + ( (float*)&constraint->invIA.cxz )[lane] = iA.cx.z; + ( (float*)&constraint->invIA.cyy )[lane] = iA.cy.y; + ( (float*)&constraint->invIA.cyz )[lane] = iA.cy.z; + ( (float*)&constraint->invIA.czz )[lane] = iA.cz.z; + + ( (float*)&constraint->invIB.cxx )[lane] = iB.cx.x; + ( (float*)&constraint->invIB.cxy )[lane] = iB.cx.y; + ( (float*)&constraint->invIB.cxz )[lane] = iB.cx.z; + ( (float*)&constraint->invIB.cyy )[lane] = iB.cy.y; + ( (float*)&constraint->invIB.cyz )[lane] = iB.cy.z; + ( (float*)&constraint->invIB.czz )[lane] = iB.cz.z; + + b3Softness soft = ( indexA == B3_NULL_INDEX || indexB == B3_NULL_INDEX ) ? staticSoftness : contactSoftness; + + b3Vec3 normal = manifold->normal; + ( (float*)&constraint->normal.X )[lane] = normal.x; + ( (float*)&constraint->normal.Y )[lane] = normal.y; + ( (float*)&constraint->normal.Z )[lane] = normal.z; + + b3Vec3 tangent1 = b3Perp( normal ); + ( (float*)&constraint->tangent1.X )[lane] = tangent1.x; + ( (float*)&constraint->tangent1.Y )[lane] = tangent1.y; + ( (float*)&constraint->tangent1.Z )[lane] = tangent1.z; + + b3Vec3 tangent2 = b3Cross( tangent1, normal ); + ( (float*)&constraint->tangent2.X )[lane] = tangent2.x; + ( (float*)&constraint->tangent2.Y )[lane] = tangent2.y; + ( (float*)&constraint->tangent2.Z )[lane] = tangent2.z; + + ( (float*)&constraint->friction )[lane] = contact->friction; + ( (float*)&constraint->restitution )[lane] = contact->restitution; + ( (float*)&constraint->rollingResistance )[lane] = contact->rollingResistance; + + ( (float*)&constraint->tangentVelocity1 )[lane] = b3Dot( contact->tangentVelocity, tangent1 ); + ( (float*)&constraint->tangentVelocity2 )[lane] = b3Dot( contact->tangentVelocity, tangent2 ); + + ( (float*)&constraint->biasRate )[lane] = soft.biasRate; + ( (float*)&constraint->massScale )[lane] = soft.massScale; + ( (float*)&constraint->impulseScale )[lane] = soft.impulseScale; + + int pointCount = manifold->pointCount; + b3Vec3 originA = b3Vec3_zero; + b3Vec3 originB = b3Vec3_zero; + + for ( int pointIndex = 0; pointIndex < pointCount; ++pointIndex ) + { + const b3ManifoldPoint* mp = manifold->points + pointIndex; + b3ContactConstraintPointWide* cp = constraint->points + pointIndex; + + b3Vec3 rA = mp->anchorA; + b3Vec3 rB = mp->anchorB; + originA = b3Add( originA, rA ); + originB = b3Add( originB, rB ); + + ( (float*)&cp->anchorAs.X )[lane] = rA.x; + ( (float*)&cp->anchorAs.Y )[lane] = rA.y; + ( (float*)&cp->anchorAs.Z )[lane] = rA.z; + + ( (float*)&cp->anchorBs.X )[lane] = rB.x; + ( (float*)&cp->anchorBs.Y )[lane] = rB.y; + ( (float*)&cp->anchorBs.Z )[lane] = rB.z; + + float baseSeparation = mp->separation - b3Dot( b3Sub( rB, rA ), normal ); + ( (float*)&cp->baseSeparations )[lane] = baseSeparation; + + ( (float*)&cp->normalImpulses )[lane] = warmStartScale * mp->normalImpulse; + ( (float*)&cp->totalNormalImpulses )[lane] = 0.0f; + + b3Vec3 rnA = b3Cross( rA, normal ); + b3Vec3 rnB = b3Cross( rB, normal ); + float kNormal = mA + mB + b3Dot( rnA, b3MulMV( iA, rnA ) ) + b3Dot( rnB, b3MulMV( iB, rnB ) ); + ( (float*)&cp->normalMasses )[lane] = kNormal > 0.0f ? 1.0f / kNormal : 0.0f; + + // Save relative velocity for restitution + b3Vec3 vrA = b3Add( vA, b3Cross( wA, rA ) ); + b3Vec3 vrB = b3Add( vB, b3Cross( wB, rB ) ); + ( (float*)&cp->relativeVelocities )[lane] = b3Dot( normal, b3Sub( vrB, vrA ) ); + } + + float invCount = 1.0f / pointCount; + originA = b3MulSV( invCount, originA ); + originB = b3MulSV( invCount, originB ); + + ( (float*)&constraint->originA.X )[lane] = originA.x; + ( (float*)&constraint->originA.Y )[lane] = originA.y; + ( (float*)&constraint->originA.Z )[lane] = originA.z; + ( (float*)&constraint->originB.X )[lane] = originB.x; + ( (float*)&constraint->originB.Y )[lane] = originB.y; + ( (float*)&constraint->originB.Z )[lane] = originB.z; + + for ( int pointIndex = 0; pointIndex < pointCount; ++pointIndex ) + { + const b3ManifoldPoint* mp = manifold->points + pointIndex; + b3ContactConstraintPointWide* cp = constraint->points + pointIndex; + ( (float*)&cp->leverArms )[lane] = b3Distance( mp->anchorA, originA ); + } + + b3Vec3 rtA1 = b3Cross( originA, tangent1 ); + b3Vec3 rtA2 = b3Cross( originA, tangent2 ); + + b3Vec3 rtB1 = b3Cross( originB, tangent1 ); + b3Vec3 rtB2 = b3Cross( originB, tangent2 ); + + { + b3Matrix2 k; + k.cx.x = mA + mB + b3Dot( rtA1, b3MulMV( iA, rtA1 ) ) + b3Dot( rtB1, b3MulMV( iB, rtB1 ) ); + k.cy.y = mA + mB + b3Dot( rtA2, b3MulMV( iA, rtA2 ) ) + b3Dot( rtB2, b3MulMV( iB, rtB2 ) ); + k.cx.y = k.cy.x = b3Dot( rtA1, b3MulMV( iA, rtA2 ) ) + b3Dot( rtB1, b3MulMV( iB, rtB2 ) ); + b3Matrix2 tangentMass = b3Invert2( k ); + + ( (float*)&constraint->tangentMass.cxx )[lane] = tangentMass.cx.x; + ( (float*)&constraint->tangentMass.cxy )[lane] = tangentMass.cx.y; + ( (float*)&constraint->tangentMass.cyy )[lane] = tangentMass.cy.y; + + ( (float*)&constraint->frictionImpulse.x )[lane] = + warmStartScale * b3Dot( manifold->frictionImpulse, tangent1 ); + ( (float*)&constraint->frictionImpulse.y )[lane] = + warmStartScale * b3Dot( manifold->frictionImpulse, tangent2 ); + } + + { + float k = b3Dot( normal, b3MulMV( b3AddMM( iA, iB ), normal ) ); + ( (float*)&constraint->twistMass )[lane] = k > 0.0f ? 1.0f / k : 0.0f; + ( (float*)&constraint->twistImpulse )[lane] = warmStartScale * manifold->twistImpulse; + } + + { + b3Matrix3 rollingMass = b3InvertMatrix( b3AddMM( iA, iB ) ); + + ( (float*)&constraint->rollingMass.cxx )[lane] = rollingMass.cx.x; + ( (float*)&constraint->rollingMass.cxy )[lane] = rollingMass.cx.y; + ( (float*)&constraint->rollingMass.cxz )[lane] = rollingMass.cx.z; + ( (float*)&constraint->rollingMass.cyy )[lane] = rollingMass.cy.y; + ( (float*)&constraint->rollingMass.cyz )[lane] = rollingMass.cy.z; + ( (float*)&constraint->rollingMass.czz )[lane] = rollingMass.cz.z; + + ( (float*)&constraint->rollingImpulse.X )[lane] = warmStartScale * manifold->rollingImpulse.x; + ( (float*)&constraint->rollingImpulse.Y )[lane] = warmStartScale * manifold->rollingImpulse.y; + ( (float*)&constraint->rollingImpulse.Z )[lane] = warmStartScale * manifold->rollingImpulse.z; + } + + // zero remaining points + for ( int pointIndex = pointCount; pointIndex < B3_MAX_MANIFOLD_POINTS; ++pointIndex ) + { + b3ContactConstraintPointWide* cp = constraint->points + pointIndex; + ( (float*)&cp->anchorAs.X )[lane] = 0.0f; + ( (float*)&cp->anchorAs.Y )[lane] = 0.0f; + ( (float*)&cp->anchorAs.Z )[lane] = 0.0f; + ( (float*)&cp->anchorBs.X )[lane] = 0.0f; + ( (float*)&cp->anchorBs.Y )[lane] = 0.0f; + ( (float*)&cp->anchorBs.Z )[lane] = 0.0f; + ( (float*)&cp->baseSeparations )[lane] = 0.0f; + ( (float*)&cp->normalImpulses )[lane] = 0.0f; + ( (float*)&cp->totalNormalImpulses )[lane] = 0.0f; + ( (float*)&cp->normalMasses )[lane] = 0.0f; + ( (float*)&cp->relativeVelocities )[lane] = 0.0f; + ( (float*)&cp->leverArms )[lane] = 0.0f; + } + } + } + + // Advance to next color + colorIndex += 1; + } + + b3TracyCZoneEnd( prepare_contact ); +} + +void b3WarmStartContacts_Convex( b3SolverBlock block, b3StepContext* context ) +{ + b3TracyCZoneNC( warm_start_contact, "Warm Start", b3_colorGreen, true ); + + b3BodyState* states = context->states; + b3ContactConstraintWide* constraints = context->graph->colors[block.colorIndex].wideConstraints; + + for ( int i = block.startIndex; i < block.startIndex + block.count; ++i ) + { + b3ContactConstraintWide* c = constraints + i; + b3BodyStateW bA = b3GatherBodies( states, c->indexA ); + b3BodyStateW bB = b3GatherBodies( states, c->indexB ); + + // Normal impulses + for ( int j = 0; j < B3_MAX_MANIFOLD_POINTS; ++j ) + { + b3ContactConstraintPointWide* cp = c->points + j; + + b3Vec3W rA = cp->anchorAs; + b3Vec3W rB = cp->anchorBs; + + b3Vec3W impulse; + impulse.X = b3MulW( cp->normalImpulses, c->normal.X ); + impulse.Y = b3MulW( cp->normalImpulses, c->normal.Y ); + impulse.Z = b3MulW( cp->normalImpulses, c->normal.Z ); + + bA.w = b3MulSubMVW( bA.w, c->invIA, b3CrossW( rA, impulse ) ); + bA.v = b3MulSubSVW( bA.v, c->invMassA, impulse ); + bB.w = b3MulAddMVW( bB.w, c->invIB, b3CrossW( rB, impulse ) ); + bB.v = b3MulAddSVW( bB.v, c->invMassB, impulse ); + } + + // Central friction + { + b3Vec3W rA = c->originA; + b3Vec3W rB = c->originB; + b3Vec3W impulse = b3MulSVW( c->frictionImpulse.x, c->tangent1 ); + impulse = b3MulAddSVW( impulse, c->frictionImpulse.y, c->tangent2 ); + + bA.w = b3MulSubMVW( bA.w, c->invIA, b3CrossW( rA, impulse ) ); + bA.v = b3MulSubSVW( bA.v, c->invMassA, impulse ); + bB.w = b3MulAddMVW( bB.w, c->invIB, b3CrossW( rB, impulse ) ); + bB.v = b3MulAddSVW( bB.v, c->invMassB, impulse ); + } + + // Central twist friction + { + b3Vec3W impulse = b3MulSVW( c->twistImpulse, c->normal ); + bA.w = b3MulSubMVW( bA.w, c->invIA, impulse ); + bB.w = b3MulAddMVW( bB.w, c->invIB, impulse ); + } + + // Rolling resistance + { + b3Vec3W impulse = c->rollingImpulse; + bA.w = b3MulSubMVW( bA.w, c->invIA, impulse ); + bB.w = b3MulAddMVW( bB.w, c->invIB, impulse ); + } + + b3ScatterBodies( states, c->indexA, &bA ); + b3ScatterBodies( states, c->indexB, &bB ); + } + + b3TracyCZoneEnd( warm_start_contact ); +} + +void b3SolveContacts_Convex( b3SolverBlock block, b3StepContext* context, bool useBias ) +{ + b3TracyCZoneNC( solve_contact, "Solve Contact", b3_colorAliceBlue, true ); + + b3BodyState* states = context->states; + b3ContactConstraintWide* constraints = context->graph->colors[block.colorIndex].wideConstraints; + b3FloatW inv_h = b3SplatW( context->inv_h ); + b3FloatW contactSpeed = b3SplatW( -context->world->contactSpeed ); + b3FloatW oneW = b3SplatW( 1.0f ); + b3FloatW epsilonW = b3SplatW( FLT_EPSILON ); + + for ( int wideIndex = block.startIndex; wideIndex < block.startIndex + block.count; ++wideIndex ) + { + b3ContactConstraintWide* c = constraints + wideIndex; + + b3BodyStateW bA = b3GatherBodies( states, c->indexA ); + b3BodyStateW bB = b3GatherBodies( states, c->indexB ); + + b3FloatW biasRate, massScale, impulseScale; + if ( useBias ) + { + biasRate = b3MulW( c->massScale, c->biasRate ); + massScale = c->massScale; + impulseScale = c->impulseScale; + } + else + { + biasRate = b3ZeroW(); + massScale = oneW; + impulseScale = b3ZeroW(); + } + + b3Vec3W dp = b3SubVW( bB.dp, bA.dp ); + + b3FloatW totalNormalImpulse = b3ZeroW(); + b3FloatW totalTwistLimit = b3ZeroW(); + + // todo_erin use the max point count of the four manifolds + for ( int pointIndex = 0; pointIndex < B3_MAX_MANIFOLD_POINTS; ++pointIndex ) + { + b3ContactConstraintPointWide* cp = c->points + pointIndex; + + // Fixed anchor points for applying impulses + b3Vec3W rA = cp->anchorAs; + b3Vec3W rB = cp->anchorBs; + + // Moving anchors for current separation + // todo speed this up using matrices + b3Vec3W rsA = b3RotateVectorW( bA.dq, rA ); + b3Vec3W rsB = b3RotateVectorW( bB.dq, rB ); + + // compute current separation + // this is subject to round-off error if the anchor is far from the body center of mass + b3Vec3W ds = b3AddVW( dp, b3SubVW( rsB, rsA ) ); + b3FloatW s = b3AddW( b3DotW( c->normal, ds ), cp->baseSeparations ); + + // Apply speculative bias if separation is greater than zero, otherwise apply soft constraint bias + b3FloatW mask = b3GreaterThanW( s, b3ZeroW() ); + b3FloatW specBias = b3MulW( s, inv_h ); + b3FloatW softBias = b3MaxW( b3MulW( biasRate, s ), contactSpeed ); + b3FloatW bias = b3BlendW( softBias, specBias, mask ); + + b3FloatW pointMassScale = b3BlendW( massScale, oneW, mask ); + b3FloatW pointImpulseScale = b3BlendW( impulseScale, b3ZeroW(), mask ); + + // Relative velocity at contact + b3Vec3W vrA = b3AddVW( bA.v, b3CrossW( bA.w, rA ) ); + b3Vec3W vrB = b3AddVW( bB.v, b3CrossW( bB.w, rB ) ); + b3FloatW vn = b3DotW( b3SubVW( vrB, vrA ), c->normal ); + + // Compute normal impulse + b3FloatW negImpulse = b3AddW( b3MulW( cp->normalMasses, b3AddW( b3MulW( pointMassScale, vn ), bias ) ), + b3MulW( pointImpulseScale, cp->normalImpulses ) ); + + // Clamp the accumulated impulse + b3FloatW newImpulse = b3MaxW( b3SubW( cp->normalImpulses, negImpulse ), b3ZeroW() ); + b3FloatW deltaImpulse = b3SubW( newImpulse, cp->normalImpulses ); + cp->normalImpulses = newImpulse; + cp->totalNormalImpulses = b3AddW( cp->totalNormalImpulses, newImpulse ); + + totalNormalImpulse = b3AddW( totalNormalImpulse, newImpulse ); + totalTwistLimit = b3AddW( totalTwistLimit, b3MulW( cp->leverArms, newImpulse ) ); + + // Apply contact impulse + b3Vec3W P = b3MulSVW( deltaImpulse, c->normal ); + bA.w = b3MulSubMVW( bA.w, c->invIA, b3CrossW( rA, P ) ); + bA.v = b3MulSubSVW( bA.v, c->invMassA, P ); + bB.w = b3MulAddMVW( bB.w, c->invIB, b3CrossW( rB, P ) ); + bB.v = b3MulAddSVW( bB.v, c->invMassB, P ); + } + + // No friction when applying bias + if ( useBias == false ) + { + // Rolling resistance + if ( b3AllZeroW( c->rollingResistance ) == false ) + { + // flip A/B order to negate + b3Vec3W deltaImpulse = b3MulMVW( c->rollingMass, b3SubVW( bA.w, bB.w ) ); + b3Vec3W oldImpulse = c->rollingImpulse; + c->rollingImpulse = b3AddVW( oldImpulse, deltaImpulse ); + + b3FloatW maxImpulse = b3MulW( c->rollingResistance, totalNormalImpulse ); + b3FloatW lengthSquared = b3DotW( c->rollingImpulse, c->rollingImpulse ); + + // if ( magSqr > maxLambda * maxLambda + FLT_EPSILON ) + //{ + // c->rollingImpulse *= maxLambda / sqrtf( magSqr ); + // } + + b3FloatW mask = b3GreaterThanW( lengthSquared, b3MulAddW( epsilonW, maxImpulse, maxImpulse ) ); + + // No approximate _mm_rsqrt_ps here to maintain cross-platform determinism + b3FloatW normalize = b3DivW( maxImpulse, b3AddW( b3SqrtW( lengthSquared ), epsilonW ) ); + b3FloatW scale = b3BlendW( oneW, normalize, mask ); + + // Ensure zero rolling resistance yields no impulse + b3FloatW rollingMask = b3GreaterThanW( c->rollingResistance, b3ZeroW() ); + scale = b3BlendW( b3ZeroW(), scale, rollingMask ); + + c->rollingImpulse = b3MulSVW( scale, c->rollingImpulse ); + + deltaImpulse = b3SubVW( c->rollingImpulse, oldImpulse ); + + bA.w = b3MulSubMVW( bA.w, c->invIA, deltaImpulse ); + bB.w = b3MulAddMVW( bB.w, c->invIB, deltaImpulse ); + } + + // Central twist friction + { + b3FloatW twistSpeed = b3DotW( c->normal, b3SubVW( bB.w, bA.w ) ); + b3FloatW maxLambda = b3MulW( c->friction, totalTwistLimit ); + b3FloatW deltaImpulse = b3NegW( b3MulW( c->twistMass, twistSpeed ) ); + b3FloatW oldImpulse = c->twistImpulse; + c->twistImpulse = b3SymClampW( b3AddW( oldImpulse, deltaImpulse ), maxLambda ); + deltaImpulse = b3SubW( c->twistImpulse, oldImpulse ); + + b3Vec3W L = b3MulSVW( deltaImpulse, c->normal ); + bA.w = b3MulSubMVW( bA.w, c->invIA, L ); + bB.w = b3MulAddMVW( bB.w, c->invIB, L ); + } + + // Central friction + { + b3Vec3W tangent1 = c->tangent1; + b3Vec3W tangent2 = c->tangent2; + + // Fixed anchor points for applying impulses + b3Vec3W rA = c->originA; + b3Vec3W rB = c->originB; + + // Relative tangent velocity at contact + b3Vec3W vrA = b3AddVW( bA.v, b3CrossW( bA.w, rA ) ); + b3Vec3W vrB = b3AddVW( bB.v, b3CrossW( bB.w, rB ) ); + b3Vec3W vr = b3SubVW( vrB, vrA ); + b3Vec2W vt = { + b3SubW( b3DotW( vr, tangent1 ), c->tangentVelocity1 ), + b3SubW( b3DotW( vr, tangent2 ), c->tangentVelocity2 ), + }; + + // Incremental tangent impulse + b3Vec2W deltaImpulse = b3MulMV2W( c->tangentMass, vt ); + deltaImpulse = (b3Vec2W){ b3NegW( deltaImpulse.x ), b3NegW( deltaImpulse.y ) }; + b3Vec2W newImpulse = b3AddV2W( c->frictionImpulse, deltaImpulse ); + + b3FloatW friction = c->friction; + b3FloatW maxImpulse = b3MulW( friction, totalNormalImpulse ); + + // Clamp the accumulated impulse + b3FloatW lengthSquared = b3AddW( b3MulW( newImpulse.x, newImpulse.x ), b3MulW( newImpulse.y, newImpulse.y ) ); + + // Max impulse can be zero + b3FloatW mask = b3GreaterThanW( lengthSquared, b3MulW( maxImpulse, maxImpulse ) ); + + // No approximate _mm_rsqrt_ps here to maintain cross-platform determinism. Add epsilon to avoid divide by + // zero. + b3FloatW normalize = b3DivW( maxImpulse, b3AddW( b3SqrtW( lengthSquared ), epsilonW ) ); + b3FloatW scale = b3BlendW( oneW, normalize, mask ); + newImpulse = (b3Vec2W){ + b3MulW( scale, newImpulse.x ), + b3MulW( scale, newImpulse.y ), + }; + + deltaImpulse = (b3Vec2W){ + b3SubW( newImpulse.x, c->frictionImpulse.x ), + b3SubW( newImpulse.y, c->frictionImpulse.y ), + }; + + c->frictionImpulse = newImpulse; + + // Apply delta impulse + b3Vec3W P = b3AddVW( b3MulSVW( deltaImpulse.x, tangent1 ), b3MulSVW( deltaImpulse.y, tangent2 ) ); + bA.w = b3MulSubMVW( bA.w, c->invIA, b3CrossW( rA, P ) ); + bA.v = b3MulSubSVW( bA.v, c->invMassA, P ); + bB.w = b3MulAddMVW( bB.w, c->invIB, b3CrossW( rB, P ) ); + bB.v = b3MulAddSVW( bB.v, c->invMassB, P ); + } + } + + b3ScatterBodies( states, c->indexA, &bA ); + b3ScatterBodies( states, c->indexB, &bB ); + } + + b3TracyCZoneEnd( solve_contact ); +} + +void b3ApplyRestitution_Convex( b3SolverBlock block, b3StepContext* context ) +{ + b3TracyCZoneNC( restitution, "Restitution", b3_colorDodgerBlue, true ); + + b3BodyState* states = context->states; + b3ContactConstraintWide* constraints = context->graph->colors[block.colorIndex].wideConstraints; + b3FloatW threshold = b3SplatW( context->world->restitutionThreshold ); + b3FloatW zero = b3ZeroW(); + + for ( int i = block.startIndex; i < block.startIndex + block.count; ++i ) + { + b3ContactConstraintWide* c = constraints + i; + + if ( b3AllZeroW( c->restitution ) ) + { + // No lanes have restitution. Common case. + continue; + } + + // Single gather for all manifolds + b3BodyStateW bA = b3GatherBodies( states, c->indexA ); + b3BodyStateW bB = b3GatherBodies( states, c->indexB ); + + // Create a mask based on restitution so that lanes with no restitution are not affected + // by the calculations below. + b3FloatW restitutionMask = b3EqualsW( c->restitution, zero ); + + for ( int pointIndex = 0; pointIndex < B3_MAX_MANIFOLD_POINTS; ++pointIndex ) + { + b3ContactConstraintPointWide* cp = c->points + pointIndex; + + // Set effective mass to zero if restitution should not be applied + b3FloatW mask1 = b3GreaterThanW( b3AddW( cp->relativeVelocities, threshold ), zero ); + b3FloatW mask2 = b3EqualsW( cp->totalNormalImpulses, zero ); + b3FloatW mask = b3OrW( b3OrW( mask1, mask2 ), restitutionMask ); + b3FloatW mass = b3BlendW( cp->normalMasses, zero, mask ); + + // Fixed anchors for impulses + b3Vec3W rA = cp->anchorAs; + b3Vec3W rB = cp->anchorBs; + + // Relative velocity at contact + b3Vec3W vrA = b3AddVW( bA.v, b3CrossW( bA.w, rA ) ); + b3Vec3W vrB = b3AddVW( bB.v, b3CrossW( bB.w, rB ) ); + b3FloatW vn = b3DotW( b3SubVW( vrB, vrA ), c->normal ); + + // Compute normal impulse + b3FloatW negImpulse = b3MulW( mass, b3AddW( vn, b3MulW( c->restitution, cp->relativeVelocities ) ) ); + + // Clamp the accumulated impulse + b3FloatW newImpulse = b3MaxW( b3SubW( cp->normalImpulses, negImpulse ), b3ZeroW() ); + b3FloatW deltaImpulse = b3SubW( newImpulse, cp->normalImpulses ); + cp->normalImpulses = newImpulse; + cp->totalNormalImpulses = b3AddW( cp->totalNormalImpulses, deltaImpulse ); + + // Apply contact impulse + b3Vec3W P = b3MulSVW( deltaImpulse, c->normal ); + bA.w = b3MulSubMVW( bA.w, c->invIA, b3CrossW( rA, P ) ); + bA.v = b3MulSubSVW( bA.v, c->invMassA, P ); + bB.w = b3MulAddMVW( bB.w, c->invIB, b3CrossW( rB, P ) ); + bB.v = b3MulAddSVW( bB.v, c->invMassB, P ); + } + + b3ScatterBodies( states, c->indexA, &bA ); + b3ScatterBodies( states, c->indexB, &bB ); + } + + b3TracyCZoneEnd( restitution ); +} + +// Store impulses by contact constraint +void b3StoreImpulses_Convex( b3SolverBlock block, b3StepContext* context, int workerIndex ) +{ + b3TracyCZoneNC( store_impulses, "Store", b3_colorFireBrick, true ); + + b3World* world = context->world; + b3WidePrepareSpan* spans = context->widePrepareSpans; + const b3ContactConstraintWide* wideBase = context->wideConstraints; + b3TaskContext* taskContext = world->taskContexts.data + workerIndex; + b3BitSet* hitEventBitSet = &taskContext->hitEventBitSet; + bool hasHitEvents = taskContext->hasHitEvents; + float negHitThreshold = -world->hitEventThreshold; + + int wideIndex = block.startIndex; + int endWideIndex = block.startIndex + block.count; + + // Find color for start index + int colorIndex = 0; + while ( spans[colorIndex + 1].start <= wideIndex ) + { + colorIndex += 1; + } + + while ( wideIndex < endWideIndex ) + { + int colorWideStart = spans[colorIndex].start; + int colorWideEndIndex = b3MinInt( spans[colorIndex + 1].start, endWideIndex ); + int colorContactCount = spans[colorIndex].count; + int* contactIds = spans[colorIndex].contacts; + + for ( ; wideIndex < colorWideEndIndex; ++wideIndex ) + { + const b3ContactConstraintWide* c = wideBase + wideIndex; + const float* frictionImpulse1 = (float*)&c->frictionImpulse.x; + const float* frictionImpulse2 = (float*)&c->frictionImpulse.y; + const float* tangent1X = (float*)&c->tangent1.X; + const float* tangent1Y = (float*)&c->tangent1.Y; + const float* tangent1Z = (float*)&c->tangent1.Z; + const float* tangent2X = (float*)&c->tangent2.X; + const float* tangent2Y = (float*)&c->tangent2.Y; + const float* tangent2Z = (float*)&c->tangent2.Z; + const float* twistImpulse = (float*)&c->twistImpulse; + const float* rollingImpulseX = (float*)&c->rollingImpulse.X; + const float* rollingImpulseY = (float*)&c->rollingImpulse.Y; + const float* rollingImpulseZ = (float*)&c->rollingImpulse.Z; + + int localWideIndex = wideIndex - colorWideStart; + + for ( int lane = 0; lane < B3_SIMD_WIDTH; ++lane ) + { + int contactIndex = B3_SIMD_WIDTH * localWideIndex + lane; + if ( contactIndex >= colorContactCount ) + { + break; + } + + b3Manifold* m = c->manifolds[lane]; + if ( m == NULL ) + { + continue; + } + + float f1 = frictionImpulse1[lane]; + float f2 = frictionImpulse2[lane]; + m->frictionImpulse = (b3Vec3){ + f1 * tangent1X[lane] + f2 * tangent2X[lane], + f1 * tangent1Y[lane] + f2 * tangent2Y[lane], + f1 * tangent1Z[lane] + f2 * tangent2Z[lane], + }; + m->twistImpulse = twistImpulse[lane]; + m->rollingImpulse = (b3Vec3){ + rollingImpulseX[lane], + rollingImpulseY[lane], + rollingImpulseZ[lane], + }; + + int pointCount = m->pointCount; + for ( int pointIndex = 0; pointIndex < pointCount; ++pointIndex ) + { + const b3ContactConstraintPointWide* cp = c->points + pointIndex; + const float* normalImpulse = (float*)&cp->normalImpulses; + const float* totalNormalImpulse = (float*)&cp->totalNormalImpulses; + const float* normalVelocity = (float*)&cp->relativeVelocities; + + b3ManifoldPoint* mp = m->points + pointIndex; + mp->normalImpulse = normalImpulse[lane]; + mp->totalNormalImpulse = totalNormalImpulse[lane]; + mp->normalVelocity = normalVelocity[lane]; + } + + int contactId = contactIds[contactIndex]; + b3Contact* contact = b3Array_Get( world->contacts, contactId ); + if ( ( contact->flags & b3_simEnableHitEvent ) != 0 ) + { + for ( int k = 0; k < pointCount; ++k ) + { + b3ManifoldPoint* mp = m->points + k; + + // Need to check total impulse because the point may be speculative and not colliding + if ( mp->normalVelocity < negHitThreshold && mp->totalNormalImpulse > 0.0f ) + { + b3SetBit( hitEventBitSet, contact->contactId ); + hasHitEvents = true; + break; + } + } + } + } + } + + colorIndex += 1; + } + + taskContext->hasHitEvents = hasHitEvents; + + b3TracyCZoneEnd( store_impulses ); +} + +void b3PrepareContacts_Overflow( b3StepContext* context ) +{ + b3ConstraintGraph* graph = context->graph; + b3GraphColor* color = graph->colors + B3_OVERFLOW_INDEX; + + uint16_t count = (uint16_t)color->contacts.count; + if (count == 0) + { + return; + } + + b3SolverBlock block = { + .startIndex = 0, + .count = count, + .blockType = b3_overflowBlock, + .colorIndex = B3_OVERFLOW_INDEX, + }; + + b3PrepareContacts_Mesh( block, context ); +} + +void b3WarmStartContacts_Overflow( b3StepContext* context ) +{ + b3ConstraintGraph* graph = context->graph; + b3GraphColor* color = graph->colors + B3_OVERFLOW_INDEX; + + uint16_t count = (uint16_t)color->contacts.count; + if ( count == 0 ) + { + return; + } + + b3SolverBlock block = { + .startIndex = 0, + .count = count, + .blockType = b3_overflowBlock, + .colorIndex = B3_OVERFLOW_INDEX, + }; + + b3WarmStartContacts_Mesh( block, context ); +} + +void b3SolveContacts_Overflow( b3StepContext* context, bool useBias ) +{ + b3ConstraintGraph* graph = context->graph; + b3GraphColor* color = graph->colors + B3_OVERFLOW_INDEX; + + uint16_t count = (uint16_t)color->contacts.count; + if ( count == 0 ) + { + return; + } + + b3SolverBlock block = { + .startIndex = 0, + .count = count, + .blockType = b3_overflowBlock, + .colorIndex = B3_OVERFLOW_INDEX, + }; + + b3SolveContacts_Mesh( block, context, useBias ); +} + +void b3ApplyRestitution_Overflow( b3StepContext* context ) +{ + b3ConstraintGraph* graph = context->graph; + b3GraphColor* color = graph->colors + B3_OVERFLOW_INDEX; + + uint16_t count = (uint16_t)color->contacts.count; + if ( count == 0 ) + { + return; + } + + b3SolverBlock block = { + .startIndex = 0, + .count = count, + .blockType = b3_overflowBlock, + .colorIndex = B3_OVERFLOW_INDEX, + }; + + b3ApplyRestitution_Mesh( block, context ); +} + +void b3StoreImpulses_Overflow( b3StepContext* context ) +{ + b3ConstraintGraph* graph = context->graph; + b3GraphColor* color = graph->colors + B3_OVERFLOW_INDEX; + + uint16_t count = (uint16_t)color->contacts.count; + if ( count == 0 ) + { + return; + } + + b3SolverBlock block = { + .startIndex = 0, + .count = count, + .blockType = b3_overflowBlock, + .colorIndex = B3_OVERFLOW_INDEX, + }; + + b3StoreImpulses_Mesh( block, context, 0 ); +} diff --git a/vendor/box3d/src/src/contact_solver.h b/vendor/box3d/src/src/contact_solver.h new file mode 100644 index 000000000..fab17ee53 --- /dev/null +++ b/vendor/box3d/src/src/contact_solver.h @@ -0,0 +1,73 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +#pragma once + +#include "math_internal.h" +#include "solver.h" + +typedef struct b3ManifoldConstraintPoint +{ + b3Vec3 rA, rB; + float baseSeparation; + float relativeVelocity; + float normalImpulse; + float totalNormalImpulse; + float normalMass; + float leverArm; +} b3ManifoldConstraintPoint; + +typedef struct b3ManifoldConstraint +{ + // todo use pointer buffer + b3ManifoldConstraintPoint points[4]; + int pointCount; + b3Vec3 normal; + b3Vec3 tangent1; + b3Vec3 tangent2; + b3Vec3 originA, originB; + float twistMass; + float twistImpulse; + b3Matrix2 tangentMass; + b3Vec2 frictionImpulse; + b3Vec3 rollingImpulse; + float tangentVelocity1; + float tangentVelocity2; +} b3ManifoldConstraint; + +typedef struct b3ContactConstraint +{ + b3ManifoldConstraint* constraints; + struct b3Contact* contact; + int indexA; + int indexB; + float invMassA, invMassB; + b3Matrix3 invIA, invIB; + b3Softness softness; + b3Matrix3 rollingMass; + float friction; + float restitution; + float rollingResistance; + int manifoldCount; +} b3ContactConstraint; + +int b3GetWideContactConstraintByteCount( void ); + +// Overflow contacts don't fit into the constraint graph coloring +void b3PrepareContacts_Overflow( b3StepContext* context ); +void b3WarmStartContacts_Overflow( b3StepContext* context ); +void b3SolveContacts_Overflow( b3StepContext* context, bool useBias ); +void b3ApplyRestitution_Overflow( b3StepContext* context ); +void b3StoreImpulses_Overflow( b3StepContext* context ); + +void b3PrepareContacts_Mesh( b3SolverBlock block, b3StepContext* context ); +void b3WarmStartContacts_Mesh( b3SolverBlock block, b3StepContext* context ); +void b3SolveContacts_Mesh( b3SolverBlock block, b3StepContext* context, bool useBias ); +void b3ApplyRestitution_Mesh( b3SolverBlock block, b3StepContext* context ); +void b3StoreImpulses_Mesh( b3SolverBlock block, b3StepContext* context, int workerIndex ); + +void b3PrepareContacts_Convex( b3SolverBlock block, b3StepContext* context ); +void b3WarmStartContacts_Convex( b3SolverBlock block, b3StepContext* context ); +void b3SolveContacts_Convex( b3SolverBlock block, b3StepContext* context, bool useBias ); +void b3ApplyRestitution_Convex( b3SolverBlock block, b3StepContext* context ); +void b3StoreImpulses_Convex( b3SolverBlock block, b3StepContext* context, int workerIndex ); diff --git a/vendor/box3d/src/src/container.h b/vendor/box3d/src/src/container.h new file mode 100644 index 000000000..2831cb6ff --- /dev/null +++ b/vendor/box3d/src/src/container.h @@ -0,0 +1,184 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +#pragma once + +#include "algorithm.h" +#include "core.h" + +#include +#include + +#define b3DeclareArray( T ) \ + typedef struct b3DynamicArray_##T \ + { \ + struct T* data; \ + int count; \ + int capacity; \ + } b3DynamicArray_##T + +#define b3DeclareArrayNative( T ) \ + typedef struct b3DynamicArray_##T \ + { \ + T* data; \ + int count; \ + int capacity; \ + } b3DynamicArray_##T + +// Define an array. +// It may be zero initialized: +// b3Array(int) myArray = { 0 }; +#define b3Array( T ) b3DynamicArray_##T + +// Alternative to zero initialization +#define b3Array_Create( a ) \ + do \ + { \ + ( a ).data = NULL; \ + ( a ).count = 0; \ + ( a ).capacity = 0; \ + } \ + while ( 0 ) + +#define b3Array_CreateN( a, n ) \ + do \ + { \ + ( a ).data = ( n ) > 0 ? b3GrowAlloc( NULL, 0, ( n ) * sizeof( *( a ).data ) ) : NULL; \ + ( a ).count = 0; \ + ( a ).capacity = ( n ); \ + } \ + while ( 0 ) + +#define b3Array_Destroy( a ) \ + do \ + { \ + b3Free( ( a ).data, ( a ).capacity * sizeof( *( a ).data ) ); \ + ( a ).data = NULL; \ + ( a ).count = 0; \ + ( a ).capacity = 0; \ + } \ + while ( 0 ) + +#define b3Array_Reserve( a, n ) \ + do \ + { \ + if ( ( a ).capacity < n ) \ + { \ + int oldSize = ( a ).capacity * sizeof( *( a ).data ); \ + int newSize = ( n ) * sizeof( *( a ).data ); \ + ( a ).data = b3GrowAlloc( ( a ).data, oldSize, newSize ); \ + ( a ).capacity = ( n ); \ + } \ + } \ + while ( 0 ) + +#define b3Array_Resize( a, n ) \ + do \ + { \ + b3Array_Reserve( a, n ); \ + ( a ).count = ( n ); \ + } \ + while ( 0 ) + +// Push a new element by value +#define b3Array_Push( a, value ) \ + do \ + { \ + if ( ( a ).count >= ( a ).capacity ) \ + { \ + int oldSize = ( a ).capacity * sizeof( *( a ).data ); \ + int newCapacity = ( a ).capacity == 0 ? 8 : 2 * ( a ).capacity; \ + int newSize = newCapacity * sizeof( *( a ).data ); \ + ( a ).data = b3GrowAlloc( ( a ).data, oldSize, newSize ); \ + ( a ).capacity = newCapacity; \ + } \ + ( a ).data[( a ).count++] = ( value ); \ + } \ + while ( 0 ) + +// Get a pointer to an element +#define b3Array_Get( a, index ) ( B3_ASSERT( 0 <= ( index ) && ( index ) < ( a ).count ), ( a ).data + ( index ) ) + +// Create a new uninitialized element and return a pointer to it +#define b3Array_Emplace( a ) \ + ( b3EmplaceHelper( (void**)&( a ).data, &( a ).count, &( a ).capacity, sizeof( *( a ).data ) ) ) + +// Remove the last element and return it by value. +#define b3Array_Pop( a ) ( B3_ASSERT( 0 < ( a ).count ), ( a ).data[-1 + ( a ).count--] ) + +// Add an uninitialized element and return its index. +#define b3Array_AddIndex( a ) \ + ( b3EmplaceHelper( (void**)&( a ).data, &( a ).count, &( a ).capacity, sizeof( *( a ).data ) ), ( a ).count - 1 ) + +// Append a contiguous run of values. _n is used to cache the input count while avoiding naming conflicts. +#define b3Array_Append( a, src, n ) \ + do \ + { \ + int _n = ( n ); \ + if ( ( a ).count + _n > ( a ).capacity ) \ + { \ + int req = ( a ).count + _n; \ + int newCapacity = req > 2 ? req + ( req >> 1 ) : 8; \ + int oldSize = ( a ).capacity * sizeof( *( a ).data ); \ + int newSize = newCapacity * sizeof( *( a ).data ); \ + ( a ).data = b3GrowAlloc( ( a ).data, oldSize, newSize ); \ + ( a ).capacity = newCapacity; \ + } \ + memcpy( ( a ).data + ( a ).count, ( src ), _n * sizeof( *( a ).data ) ); \ + ( a ).count += _n; \ + } \ + while ( 0 ) + +// Zero the entire allocated buffer (capacity, not just count). +#define b3Array_MemZero( a ) \ + do \ + { \ + if ( ( a ).capacity > 0 ) \ + { \ + memset( ( a ).data, 0, ( a ).capacity * sizeof( *( a ).data ) ); \ + } \ + } \ + while ( 0 ) + +// Remove and element by swapping with the last element. If the index is the last element it returns +// B3_NULL_INDEX, otherwise it returns the index of the last element (which is now out of bounds). +#define b3Array_RemoveSwap( a, index ) b3RemoveHelper( ( a ).data, &( a ).count, ( index ), sizeof( *( a ).data ) ) + +B3_INLINE void* b3EmplaceHelper( void** data, int* count, int* capacity, int elem_size ) +{ + if ( *count >= *capacity ) + { + int oldCapacity = *capacity; + int oldSize = oldCapacity * elem_size; + int newCapacity = ( oldCapacity == 0 ? 16 : 2 * oldCapacity ); + int newSize = newCapacity * elem_size; + *data = b3GrowAlloc( *data, oldSize, newSize ); + *capacity = newCapacity; + } + return (char*)*data + ( *count )++ * elem_size; +} + +B3_INLINE int b3RemoveHelper( void* data, int* count, int index, int elementSize ) +{ + B3_ASSERT( 0 <= index && index < *count && "Array index out of bounds" ); + + ( *count )--; + if ( index != *count ) + { + memcpy( (char*)data + index * elementSize, (char*)data + ( *count ) * elementSize, elementSize ); + return *count; + } + + return B3_NULL_INDEX; +} + +#define b3Array_Clear( a ) \ + do \ + { \ + ( a ).count = 0; \ + } \ + while ( 0 ) + +#define b3Array_ByteCount( a ) ( ( a ).capacity * (int)sizeof( *( a ).data ) ) + +b3DeclareArrayNative( int ); diff --git a/vendor/box3d/src/src/convex_manifold.c b/vendor/box3d/src/src/convex_manifold.c new file mode 100644 index 000000000..2422567c4 --- /dev/null +++ b/vendor/box3d/src/src/convex_manifold.c @@ -0,0 +1,1600 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +#include "algorithm.h" +#include "manifold.h" +#include "shape.h" + +#include "box3d/base.h" +#include "box3d/collision.h" +#include "box3d/constants.h" + +#include +#include + +static inline bool b3IsMinkowskiFaceIsolated( b3Vec3 a, b3Vec3 b, b3Vec3 n ) +{ + // An isolated edge (e.g. like in a capsule) defines a circle through the + // origin on the Gauss map. So testing for overlap between this circle and + // the arc AB simplifies to a simple plane test. + float an = b3Dot( a, n ); + float bn = b3Dot( b, n ); + + return an * bn <= 0.0f; +} + +// bxa = cross(b, a) and dxc = cross(d, c) +// but in practice we use the edge vector between the faces for robustness +static inline bool b3IsMinkowskiFace( b3Vec3 a, b3Vec3 b, b3Vec3 bxa, b3Vec3 c, b3Vec3 d, b3Vec3 dxc ) +{ + // Two edges build a face on the Minkowski sum if the associated arcs ab and cd intersect on the Gauss map. + // The associated arcs are defined by the adjacent face normals of each edge. + float cba = b3Dot( c, bxa ); + float dba = b3Dot( d, bxa ); + float adc = b3Dot( a, dxc ); + float bdc = b3Dot( b, dxc ); + + return cba * dba < 0.0f && adc * bdc < 0.0f && cba * bdc > 0.0f; +} + +static int b3ClipSegment( b3ClipVertex segment[2], b3Plane plane ) +{ + int vertexCount = 0; + b3ClipVertex vertex1 = segment[0]; + b3ClipVertex vertex2 = segment[1]; + + float distance1 = b3PlaneSeparation( plane, vertex1.position ); + float distance2 = b3PlaneSeparation( plane, vertex2.position ); + + // If the points are behind the plane + if ( distance1 <= 0.0f ) + { + segment[vertexCount++] = vertex1; + } + if ( distance2 <= 0.0f ) + { + segment[vertexCount++] = vertex2; + } + + // If the points are on different sides of the plane + if ( distance1 * distance2 < 0.0f ) + { + // Find intersection point of edge and plane + float t = distance1 / ( distance1 - distance2 ); + segment[vertexCount].position = b3Add( b3MulSV( 1.0f - t, vertex1.position ), b3MulSV( t, vertex2.position ) ); + segment[vertexCount].pair = distance1 > 0.0f ? vertex1.pair : vertex2.pair; + vertexCount++; + } + + return vertexCount; +} + +static int b3ClipSegmentToHullFace( b3ClipVertex segment[2], const b3HullData* hull, int refFace ) +{ + const b3HullFace* faces = b3GetHullFaces( hull ); + const b3Plane* planes = b3GetHullPlanes( hull ); + const b3HullHalfEdge* edges = b3GetHullEdges( hull ); + const b3Vec3* points = b3GetHullPoints( hull ); + + b3Plane refPlane = planes[refFace]; + + const b3HullFace* face = faces + refFace; + + int edgeIndex = face->edge; + + do + { + const b3HullHalfEdge* edge = edges + edgeIndex; + int nextEdgeIndex = edge->next; + const b3HullHalfEdge* next = edges + nextEdgeIndex; + + b3Vec3 vertex1 = points[edge->origin]; + b3Vec3 vertex2 = points[next->origin]; + b3Vec3 tangent = b3Normalize( b3Sub( vertex2, vertex1 ) ); + b3Vec3 binormal = b3Cross( tangent, refPlane.normal ); + + int pointCount = b3ClipSegment( segment, b3MakePlaneFromNormalAndPoint( binormal, vertex1 ) ); + if ( pointCount < 2 ) + { + return 0; + } + + edgeIndex = nextEdgeIndex; + } + while ( edgeIndex != face->edge ); + + return 2; +} + +static b3FaceQuery b3QueryFaceDirectionHullAndCapsule( const b3HullData* hull, const b3Capsule* capsule, + b3Transform capsuleTransform ) +{ + int maxFaceIndex = -1; + int maxVertexIndex = -1; + float maxFaceSeparation = -FLT_MAX; + const b3Plane* planes = b3GetHullPlanes( hull ); + + b3Vec3 capsulePoints[2] = { + b3TransformPoint( capsuleTransform, capsule->center1 ), + b3TransformPoint( capsuleTransform, capsule->center2 ), + }; + + for ( int faceIndex = 0; faceIndex < hull->faceCount; ++faceIndex ) + { + b3Plane plane = planes[faceIndex]; + + int vertexIndex = b3GetPointSupport( capsulePoints, 2, b3Neg( plane.normal ) ); + b3Vec3 support = capsulePoints[vertexIndex]; + float separation = b3PlaneSeparation( plane, support ); + if ( separation > maxFaceSeparation ) + { + maxVertexIndex = vertexIndex; + maxFaceIndex = faceIndex; + maxFaceSeparation = separation; + } + } + + return (b3FaceQuery){ + .separation = maxFaceSeparation, + .faceIndex = (uint8_t)maxFaceIndex, + .vertexIndex = (uint8_t)maxVertexIndex, + }; +} + +static b3FaceQuery b3QueryFaceDirections( const b3HullData* hullA, const b3HullData* hullB, b3Transform relativeTransform ) +{ + // We perform all computations in local space of the second hull + b3Transform transform = b3InvertTransform( relativeTransform ); + const b3Plane* planesA = b3GetHullPlanes( hullA ); + const b3Vec3* pointsB = b3GetHullPoints( hullB ); + + int maxFaceIndex = -1; + int maxVertexIndex = -1; + float maxFaceSeparation = -FLT_MAX; + + for ( int faceIndex = 0; faceIndex < hullA->faceCount; ++faceIndex ) + { + b3Plane plane = b3TransformPlane( transform, planesA[faceIndex] ); + + int vertexIndex = b3FindHullSupportVertex( hullB, b3Neg( plane.normal ) ); + b3Vec3 support = pointsB[vertexIndex]; + float separation = b3PlaneSeparation( plane, support ); + if ( separation > maxFaceSeparation ) + { + maxFaceIndex = faceIndex; + maxVertexIndex = vertexIndex; + maxFaceSeparation = separation; + } + } + + return (b3FaceQuery){ + .separation = maxFaceSeparation, + .faceIndex = (uint8_t)maxFaceIndex, + .vertexIndex = (uint8_t)maxVertexIndex, + }; +} + +static b3EdgeQuery b3QueryEdgeDirectionHullAndCapsule( const b3HullData* hull, const b3Capsule* capsule, + b3Transform capsuleTransform ) +{ + // Find axis of minimum penetration + float maxSeparation = -FLT_MAX; + int maxIndex1 = -1; + int maxIndex2 = -1; + + // We perform all computations in local space of the hull + b3Vec3 p1 = b3TransformPoint( capsuleTransform, capsule->center1 ); + b3Vec3 q1 = b3TransformPoint( capsuleTransform, capsule->center2 ); + b3Vec3 e1 = b3Sub( q1, p1 ); + + const b3HullHalfEdge* edges = b3GetHullEdges( hull ); + const b3Vec3* points = b3GetHullPoints( hull ); + const b3Plane* planes = b3GetHullPlanes( hull ); + + for ( int index = 0; index < hull->edgeCount; index += 2 ) + { + const b3HullHalfEdge* edge = edges + index; + const b3HullHalfEdge* twin = edges + index + 1; + B3_ASSERT( edge->twin == index + 1 && twin->twin == index ); + + b3Vec3 p2 = points[edge->origin]; + b3Vec3 q2 = points[twin->origin]; + b3Vec3 e2 = b3Sub( q2, p2 ); + + b3Vec3 u2 = planes[edge->face].normal; + b3Vec3 v2 = planes[twin->face].normal; + + if ( b3IsMinkowskiFaceIsolated( u2, v2, e1 ) ) + { + // We can pass any point on the edge and choose + // the edge centers for better numerical precision. + b3Vec3 c1 = b3MulSV( 0.5f, b3Add( q1, p1 ) ); + b3Vec3 c2 = hull->center; + float separation = b3EdgeEdgeSeparation( q1, e1, c1, q2, e2, c2 ); + if ( separation > maxSeparation ) + { + // Note: We don't exit early if we find a separating axis here since we want to + // find the best one for caching and account for the convex radius later. + maxSeparation = separation; + maxIndex1 = 0; + maxIndex2 = index; + } + } + } + + // Save result + return (b3EdgeQuery){ + .separation = maxSeparation, + .indexA = (uint8_t)maxIndex1, + .indexB = (uint8_t)maxIndex2, + }; +} + +static b3EdgeQuery b3QueryEdgeDirections( const b3HullData* hullA, const b3HullData* hullB, b3Transform transformBtoA ) +{ + // Find axis of minimum penetration + float maxSeparation = -FLT_MAX; + int maxIndexA = B3_NULL_INDEX; + int maxIndexB = B3_NULL_INDEX; + + const b3HullHalfEdge* edgesA = b3GetHullEdges( hullA ); + const b3Vec3* pointsA = b3GetHullPoints( hullA ); + const b3Plane* planesA = b3GetHullPlanes( hullA ); + const b3HullHalfEdge* edgesB = b3GetHullEdges( hullB ); + const b3Vec3* pointsB = b3GetHullPoints( hullB ); + const b3Plane* planesB = b3GetHullPlanes( hullB ); + + // Work in frame A + b3Matrix3 matrix = b3MakeMatrixFromQuat( transformBtoA.q ); + + // Arranged to minimize transform operations + for ( int indexB = 0; indexB < hullB->edgeCount; indexB += 2 ) + { + const b3HullHalfEdge* edgeB = edgesB + indexB; + const b3HullHalfEdge* twinB = edgesB + indexB + 1; + B3_ASSERT( edgeB->twin == indexB + 1 && twinB->twin == indexB ); + + b3Vec3 qB = pointsB[twinB->origin]; + b3Vec3 eB = b3MulMV( matrix, b3Sub( qB, pointsB[edgeB->origin] ) ); + qB = b3Add( b3MulMV( matrix, qB ), transformBtoA.p ); + + b3Vec3 uB = b3MulMV( matrix, planesB[edgeB->face].normal ); + b3Vec3 vB = b3MulMV( matrix, planesB[twinB->face].normal ); + + for ( int indexA = 0; indexA < hullA->edgeCount; indexA += 2 ) + { + const b3HullHalfEdge* edgeA = edgesA + indexA; + const b3HullHalfEdge* twinA = edgesA + indexA + 1; + B3_ASSERT( edgeA->twin == indexA + 1 && twinA->twin == indexA ); + + b3Vec3 qA = pointsA[twinA->origin]; + b3Vec3 eA = b3Sub( qA, pointsA[edgeA->origin] ); + b3Vec3 uA = planesA[edgeA->face].normal; + b3Vec3 vA = planesA[twinA->face].normal; + + bool isMinkowski; + { + // Two edges build a face on the Minkowski sum if the associated arcs AB and CD intersect on the Gauss map. + // The associated arcs are defined by the adjacent face normals of each edge. + float cba = b3Dot( uB, eA ); + float dba = b3Dot( vB, eA ); + float adc = -b3Dot( uA, eB ); + float bdc = -b3Dot( vA, eB ); + + isMinkowski = cba * dba < 0.0f && adc * bdc < 0.0f && cba * bdc > 0.0f; + } + + if ( isMinkowski ) + { + b3Vec3 centerA = hullA->center; + b3Vec3 centerB = b3TransformPoint( transformBtoA, hullB->center ); + float separation = b3EdgeEdgeSeparation( qA, eA, centerA, qB, eB, centerB ); + + if ( separation > maxSeparation ) + { + // Continues to find the maximum separating axis + maxSeparation = separation; + maxIndexA = indexA; + maxIndexB = indexB; + } + } + } + } + + return (b3EdgeQuery){ + .separation = maxSeparation, + .indexA = maxIndexA, + .indexB = maxIndexB, + }; +} + +// Reduce the manifold points to a maximum of 4 points. +// Note: this modifies the input point array to improve performance +static void b3ReduceManifoldPoints( b3LocalManifold* manifold, int capacity, b3LocalManifoldPoint* points, int count ) +{ + if ( capacity < 4 ) + { + return; + } + + if ( count <= 4 ) + { + for ( int i = 0; i < count; ++i ) + { + manifold->points[i] = points[i]; + } + + manifold->pointCount = count; + return; + } + + b3Vec3 normal = manifold->normal; + // float linearSlop = B3_LINEAR_SLOP; + float speculativeDistance = B3_SPECULATIVE_DISTANCE; + float tolSqr = speculativeDistance * speculativeDistance; + + // This bias is very important for contact point consistency across time steps. + // It creates a pecking order to avoid flickering between candidates with similar scores. + float bias = 0.95f; + + // Step 1: find extreme point that is touching + int bestIndex = B3_NULL_INDEX; + float bestScore = -FLT_MAX; + + // Arbitrary tangent direction + // b3Vec3 perp1 = b3Perp( normal ); + // b3Vec3 perp2 = b3Cross( perp1, normal ); + // b3Vec3 searchDirection = -0.4535961214255773f * perp1 + 0.8912073600614354f * perp2; + b3Vec3 searchDirection = b3ArbitraryPerp( normal ); + for ( int index = 0; index < count; ++index ) + { + b3LocalManifoldPoint* pt = points + index; + + if ( pt->separation > speculativeDistance ) + { + continue; + } + + // The deeper the better + float score = -pt->separation + b3Dot( searchDirection, pt->point ); + if ( bias * score > bestScore ) + { + bestIndex = index; + bestScore = score; + } + } + + B3_VALIDATE( 0 <= bestIndex && bestIndex < count ); + if ( bestIndex == B3_NULL_INDEX ) + { + manifold->pointCount = 0; + return; + } + + manifold->points[0] = points[bestIndex]; + manifold->pointCount = 1; + + // Remove best point from array + points[bestIndex] = points[count - 1]; + count -= 1; + + b3Vec3 a = manifold->points[0].point; + + // Step 2: Find farthest point in 2D + bestScore = 0.0f; + bestIndex = B3_NULL_INDEX; + float maxDistanceSquared = 0.0f; + + for ( int index = 0; index < count; ++index ) + { + b3Vec3 p = points[index].point; + b3Vec3 d = b3Sub( p, a ); + b3Vec3 v = b3MulSub( d, b3Dot( d, normal ), normal ); + float distanceSquared = b3LengthSquared( v ); + maxDistanceSquared = b3MaxFloat( maxDistanceSquared, distanceSquared ); + float separation = b3MaxFloat( 0.0f, -points[index].separation ); + float score = distanceSquared + 4.0f * separation * separation; + if ( bias * score > bestScore ) + { + bestScore = score; + bestIndex = index; + } + } + + if ( bestScore < tolSqr ) + { + return; + } + + B3_ASSERT( 0 <= bestIndex && bestIndex < count ); + manifold->points[1] = points[bestIndex]; + manifold->pointCount = 2; + + // Remove best point from array + points[bestIndex] = points[count - 1]; + count -= 1; + + b3Vec3 b = manifold->points[1].point; + + // Step 3: Find the point with the maximum triangular area + bestScore = tolSqr; + bestIndex = B3_NULL_INDEX; + float bestSignedArea = 0.0f; + b3Vec3 ba = b3Sub( b, a ); + for ( int index = 0; index < count; ++index ) + { + b3Vec3 p = points[index].point; + float signedArea = b3Dot( normal, b3Cross( ba, b3Sub( p, a ) ) ); + float score = b3AbsFloat( signedArea ); + if ( bias * score >= bestScore ) + { + bestScore = score; + bestIndex = index; + bestSignedArea = signedArea; + } + } + + if ( bestIndex == B3_NULL_INDEX ) + { + return; + } + + B3_ASSERT( bestIndex != B3_NULL_INDEX ); + + manifold->points[2] = points[bestIndex]; + manifold->pointCount = 3; + points[bestIndex] = points[count - 1]; + count -= 1; + + b3Vec3 c = manifold->points[2].point; + + // Step 4: get the point that adds the most area outside the current triangle + bestScore = tolSqr; + bestIndex = B3_NULL_INDEX; + float sign = bestSignedArea < 0.0f ? -1.0f : 1.0f; + for ( int index = 0; index < count; ++index ) + { + b3Vec3 p = points[index].point; + float u1 = sign * b3Dot( normal, b3Cross( b3Sub( p, a ), ba ) ); + float u2 = sign * b3Dot( normal, b3Cross( b3Sub( p, b ), b3Sub( c, b ) ) ); + float u3 = sign * b3Dot( normal, b3Cross( b3Sub( p, c ), b3Sub( a, c ) ) ); + float score = b3MaxFloat( u1, b3MaxFloat( u2, u3 ) ); + + if ( bias * score > bestScore ) + { + bestScore = score; + bestIndex = index; + } + } + + if ( bestIndex != B3_NULL_INDEX ) + { + manifold->points[manifold->pointCount] = points[bestIndex]; + manifold->pointCount += 1; + } +} + +void b3CollideSpheres( b3LocalManifold* manifold, int capacity, const b3Sphere* sphereA, const b3Sphere* sphereB, + b3Transform transformBtoA ) +{ + if ( capacity == 0 ) + { + return; + } + + // Work in shapeB coordinates + b3Vec3 center1 = sphereA->center; + b3Vec3 center2 = b3TransformPoint( transformBtoA, sphereB->center ); + + float totalRadius = sphereA->radius + sphereB->radius; + b3Vec3 offset = b3Sub( center2, center1 ); + float distanceSq = b3LengthSquared( offset ); + + if ( distanceSq > totalRadius * totalRadius ) + { + // We found a separating axis + return; + } + + b3Vec3 normal = { 0.0f, 1.0f, 0.0f }; + float distance = sqrtf( distanceSq ); + if ( distance * distance > 1000.0f * FLT_MIN ) + { + normal = b3MulSV( 1.0f / distance, offset ); + } + + // Contact at the midpoint + // 0.5 * ( ((c1 + rA*n) + c2) - rB*n ) + b3Vec3 point = + b3MulSV( 0.5f, b3MulSub( b3Add( b3MulAdd( center1, sphereA->radius, normal ), center2 ), sphereB->radius, normal ) ); + + // Manifold in frame B + manifold->normal = normal; + manifold->pointCount = 1; + + b3LocalManifoldPoint* pt = manifold->points + 0; + pt->point = point; + pt->separation = distance - totalRadius; + pt->pair = b3FeaturePair_single; +} + +void b3CollideCapsuleAndSphere( b3LocalManifold* manifold, int capacity, const b3Capsule* capsuleA, const b3Sphere* sphereB, + b3Transform transformBtoA ) +{ + manifold->pointCount = 0; + + if ( capacity < 1 ) + { + return; + } + + // Work in shape B coordinates + b3Vec3 center = b3TransformPoint( transformBtoA, sphereB->center ); + b3Vec3 center1 = capsuleA->center1; + b3Vec3 center2 = capsuleA->center2; + + float totalRadius = sphereB->radius + capsuleA->radius; + + b3Vec3 closestPoint = b3PointToSegmentDistance( center1, center2, center ); + b3Vec3 offset = b3Sub( center, closestPoint ); + float distanceSq = b3LengthSquared( offset ); + + if ( distanceSq > totalRadius * totalRadius ) + { + // We found a separating axis + return; + } + + b3Vec3 normal = { 0.0f, 1.0f, 0.0f }; + float distance = sqrtf( distanceSq ); + if ( distance * distance > 1000.0f * FLT_MIN ) + { + normal = b3MulSV( 1.0f / distance, offset ); + } + + // Contact at the midpoint + // 0.5 * (((center - sB*n) + closestPoint) + cA*n) + b3Vec3 point = + b3MulSV( 0.5f, b3MulAdd( b3Add( b3MulSub( center, sphereB->radius, normal ), closestPoint ), capsuleA->radius, normal ) ); + + // Manifold in frame B + manifold->normal = normal; + manifold->pointCount = 1; + + b3LocalManifoldPoint* pt = manifold->points + 0; + pt->point = point; + pt->separation = distance - totalRadius; + pt->pair = b3FeaturePair_single; +} + +void b3CollideHullAndSphere( b3LocalManifold* manifold, int capacity, const b3HullData* hullA, const b3Sphere* sphereB, + b3Transform transformBtoA, b3SimplexCache* cache ) +{ + manifold->pointCount = 0; + + if ( capacity == 0 ) + { + return; + } + + b3Vec3 center = b3TransformPoint( transformBtoA, sphereB->center ); + + const float speculativeDistance = B3_SPECULATIVE_DISTANCE; + + // Work in shapeA coordinates + + b3DistanceInput distanceInput; + distanceInput.proxyA = (b3ShapeProxy){ b3GetHullPoints( hullA ), hullA->vertexCount, 0.0f }; + distanceInput.proxyB = (b3ShapeProxy){ ¢er, 1, 0.0f }; + distanceInput.transform = b3Transform_identity; + distanceInput.useRadii = false; + + float radiusA = 0.0f; + float radiusB = sphereB->radius; + float radius = radiusA + radiusB; + + b3DistanceOutput distanceOutput = b3ShapeDistance( &distanceInput, cache, NULL, 0 ); + + if ( distanceOutput.distance > radius + speculativeDistance ) + { + // We found a separating axis + *cache = (b3SimplexCache){ 0 }; + return; + } + + if ( distanceOutput.distance > 100.0f * FLT_EPSILON ) + { + // Shallow penetration + b3Vec3 normal = b3Normalize( b3Sub( distanceOutput.pointB, distanceOutput.pointA ) ); + + // cA is the projection of the sphere center onto to the hull (pointA if radiusA == 0). + b3Vec3 cA = b3MulAdd( center, radiusA - b3Dot( b3Sub( center, distanceOutput.pointA ), normal ), normal ); + + // cB is the deepest point on the sphere with respect to the reference f + b3Vec3 cB = b3MulSub( center, radiusB, normal ); + + b3Vec3 point = b3Lerp( cA, cB, 0.5f ); + + // Manifold in frame A + manifold->normal = normal; + manifold->pointCount = 1; + + b3LocalManifoldPoint* pt = manifold->points + 0; + pt->point = point; + pt->separation = distanceOutput.distance - radius; + pt->pair = b3FeaturePair_single; + } + else + { + // Deep penetration + int bestIndex = -1; + float bestDistance = -FLT_MAX; + const b3Plane* planes = b3GetHullPlanes( hullA ); + + for ( int index = 0; index < hullA->faceCount; ++index ) + { + b3Plane plane = planes[index]; + + float distance = b3PlaneSeparation( plane, center ); + if ( distance > bestDistance ) + { + bestIndex = index; + bestDistance = distance; + } + } + B3_ASSERT( bestIndex >= 0 ); + + b3Vec3 normal = planes[bestIndex].normal; + + // cA is the projection of the sphere center onto to the hull + b3Vec3 cA = b3MulAdd( center, radiusA - b3Dot( b3Sub( center, distanceOutput.pointA ), normal ), normal ); + + // cB is the deepest point on the sphere with respect to the reference f + b3Vec3 cB = b3MulSub( center, radiusB, normal ); + + b3Vec3 point = b3Lerp( cA, cB, 0.5f ); + + // Manifold in frame A + manifold->normal = normal; + manifold->pointCount = 1; + + b3LocalManifoldPoint* pt = manifold->points + 0; + pt->point = point; + pt->separation = bestDistance - radius; + pt->pair = b3FeaturePair_single; + } +} + +void b3CollideCapsules( b3LocalManifold* manifold, int capacity, const b3Capsule* capsuleA, const b3Capsule* capsuleB, + b3Transform transformBtoA ) +{ + manifold->pointCount = 0; + + if ( capacity < 2 ) + { + return; + } + + // Work in shapeA coordinates + b3Vec3 centerA1 = capsuleA->center1; + b3Vec3 centerA2 = capsuleA->center2; + b3Vec3 centerB1 = b3TransformPoint( transformBtoA, capsuleB->center1 ); + b3Vec3 centerB2 = b3TransformPoint( transformBtoA, capsuleB->center2 ); + + float radius = capsuleA->radius + capsuleB->radius; + float maxDistance = radius + B3_SPECULATIVE_DISTANCE; + + b3SegmentDistanceResult result = b3SegmentDistance( centerA1, centerA2, centerB1, centerB2 ); + b3Vec3 offset = b3Sub( result.point2, result.point1 ); + float distanceSquared = b3LengthSquared( offset ); + float linearSlop = B3_LINEAR_SLOP; + float minDistance = 0.01f * linearSlop; + + if ( distanceSquared > maxDistance * maxDistance || distanceSquared < minDistance * minDistance ) + { + // We found a separating axis + return; + } + + float lengthA; + b3Vec3 segmentA = b3Sub( centerA2, centerA1 ); + b3Vec3 edgeA = b3GetLengthAndNormalize( &lengthA, segmentA ); + if ( lengthA < B3_MIN_CAPSULE_LENGTH ) + { + return; + } + + float lengthB; + b3Vec3 segmentB = b3Sub( centerB2, centerB1 ); + b3Vec3 edgeB = b3GetLengthAndNormalize( &lengthB, segmentB ); + if ( lengthB < B3_MIN_CAPSULE_LENGTH ) + { + return; + } + + // Parallel edges: |eA x eB| = sin(alpha) + const float alphaTol = 0.05f; + const float alphaTolSqr = alphaTol * alphaTol; + b3Vec3 axis = b3Cross( edgeA, edgeB ); + + // Try to create two contact points if the capsules are nearly parallel + if ( b3LengthSquared( axis ) < alphaTolSqr ) + { + // Clip segment B against side planes of segment A + + // Sides planes of A + b3Plane planesA[2]; + planesA[0].normal = b3Neg( edgeA ); + planesA[0].offset = -b3Dot( edgeA, capsuleA->center1 ); + planesA[1].normal = edgeA; + planesA[1].offset = b3Dot( edgeA, capsuleA->center2 ); + + // Clip points for B + b3ClipVertex verticesB[2]; + verticesB[0].position = centerB1; + verticesB[0].separation = 0.0f; + verticesB[0].pair = b3MakeFeaturePair( b3_featureShapeA, 0, b3_featureShapeA, 0 ); + verticesB[1].position = centerB2; + verticesB[1].separation = 0.0f; + verticesB[1].pair = b3MakeFeaturePair( b3_featureShapeA, 1, b3_featureShapeA, 1 ); + + int pointCount = b3ClipSegment( verticesB, planesA[0] ); + if ( pointCount == 2 ) + { + pointCount = b3ClipSegment( verticesB, planesA[1] ); + } + + if ( pointCount == 2 ) + { + // Closest points on A to the clipped points on B. + b3Vec3 closestPoint1 = b3PointToSegmentDistance( centerA1, centerA2, verticesB[0].position ); + b3Vec3 closestPoint2 = b3PointToSegmentDistance( centerA1, centerA2, verticesB[1].position ); + + float distance1 = b3Distance( closestPoint1, verticesB[0].position ); + float distance2 = b3Distance( closestPoint2, verticesB[1].position ); + if ( distance1 <= radius && distance2 <= radius ) + { + if ( distance1 < minDistance || distance2 < minDistance ) + { + // Avoid divide by zero + return; + } + + b3Vec3 normal1 = b3MulSV( 1.0f / distance1, b3Sub( verticesB[0].position, closestPoint1 ) ); + b3Vec3 normal2 = b3MulSV( 1.0f / distance2, b3Sub( verticesB[1].position, closestPoint2 ) ); + b3Vec3 normal = b3Normalize( b3Add( normal1, normal2 ) ); + float radiusA = capsuleA->radius; + float radiusB = capsuleB->radius; + + // Contact is at the midpoint: 0.5 * (((vB.pos + rA*nK) + cP) - rB*n) + b3Vec3 point1 = + b3MulSV( 0.5f, b3MulSub( b3Add( b3MulAdd( verticesB[0].position, radiusA, normal1 ), closestPoint1 ), radiusB, + normal ) ); + b3Vec3 point2 = + b3MulSV( 0.5f, b3MulSub( b3Add( b3MulAdd( verticesB[1].position, radiusA, normal2 ), closestPoint2 ), radiusB, + normal ) ); + + // Manifold in frame A + manifold->normal = normal; + manifold->pointCount = 2; + + b3LocalManifoldPoint* pt1 = manifold->points + 0; + pt1->point = point1; + pt1->separation = distance1 - radius; + pt1->pair = verticesB[0].pair; + + b3LocalManifoldPoint* pt2 = manifold->points + 1; + pt2->point = point2; + pt2->separation = distance2 - radius; + pt2->pair = verticesB[1].pair; + + return; + } + } + } + + float distance; + b3Vec3 normal = b3GetLengthAndNormalize( &distance, offset ); + // Contact at the midpoint 0.5 * (((p1 + rA*n) + p2) - rB*n) + b3Vec3 point = b3MulSV( + 0.5f, b3MulSub( b3Add( b3MulAdd( result.point1, capsuleA->radius, normal ), result.point2 ), capsuleB->radius, normal ) ); + + // Manifold in frame A + manifold->normal = normal; + manifold->pointCount = 1; + + b3LocalManifoldPoint* pt = manifold->points + 0; + pt->point = point; + pt->separation = distance - radius; + pt->pair = b3FeaturePair_single; +} + +static bool b3BuildHullFaceAndCapsuleContact( b3LocalManifold* manifold, const b3HullData* hullA, const b3Capsule* capsuleB, + b3Transform transformBtoA, b3FaceQuery query ) +{ + // Work in shapeA coordinates + const b3Plane* planes = b3GetHullPlanes( hullA ); + + // Clip the capsule edge against the side planes of the reference face + int refFace = query.faceIndex; + b3Plane refPlane = planes[refFace]; + + b3ClipVertex segmentB[2]; + segmentB[0].position = b3TransformPoint( transformBtoA, capsuleB->center1 ); + segmentB[0].separation = 0.0f; + segmentB[0].pair = b3MakeFeaturePair( b3_featureShapeA, 0, b3_featureShapeA, 0 ); + segmentB[1].position = b3TransformPoint( transformBtoA, capsuleB->center2 ); + segmentB[1].separation = 0.0f; + segmentB[1].pair = b3MakeFeaturePair( b3_featureShapeA, 1, b3_featureShapeA, 1 ); + + int pointCount = b3ClipSegmentToHullFace( segmentB, hullA, refFace ); + if ( pointCount < 2 ) + { + return false; + } + + float distance1 = b3PlaneSeparation( refPlane, segmentB[0].position ); + float distance2 = b3PlaneSeparation( refPlane, segmentB[1].position ); + const float speculativeDistance = B3_SPECULATIVE_DISTANCE; + + if ( distance1 <= speculativeDistance || distance2 <= speculativeDistance ) + { + b3Vec3 normal = refPlane.normal; + b3Vec3 point1 = b3MulSub( segmentB[0].position, 0.5f * ( distance1 + capsuleB->radius ), normal ); + b3Vec3 point2 = b3MulSub( segmentB[1].position, 0.5f * ( distance2 + capsuleB->radius ), normal ); + + // Manifold in frame A + manifold->normal = normal; + manifold->pointCount = 2; + + b3LocalManifoldPoint* pt1 = manifold->points + 0; + pt1->point = point1; + pt1->separation = distance1 - capsuleB->radius; + pt1->pair = segmentB[0].pair; + + b3LocalManifoldPoint* pt2 = manifold->points + 1; + pt2->point = point2; + pt2->separation = distance2 - capsuleB->radius; + pt2->pair = segmentB[1].pair; + + return true; + } + + return false; +} + +static inline float b3DeepestPointSeparation( const b3LocalManifold* manifold ) +{ + // Deepest point + float minSeparation = FLT_MAX; + int pointCount = manifold->pointCount; + for ( int i = 0; i < pointCount; ++i ) + { + minSeparation = b3MinFloat( minSeparation, manifold->points[i].separation ); + } + + return minSeparation; +} + +static bool b3BuildHullAndCapsuleEdgeContact( b3LocalManifold* manifold, int capacity, const b3HullData* hullA, + const b3Capsule* capsuleB, b3Transform transformBtoA, b3EdgeQuery query ) +{ + if ( capacity < 1 ) + { + return false; + } + + // Work in shapeA coordinates + + b3Vec3 pc = b3TransformPoint( transformBtoA, capsuleB->center1 ); + b3Vec3 qc = b3TransformPoint( transformBtoA, capsuleB->center2 ); + b3Vec3 ec = b3Sub( qc, pc ); + + const b3HullHalfEdge* edges = b3GetHullEdges( hullA ); + const b3Vec3* points = b3GetHullPoints( hullA ); + + const b3HullHalfEdge* edge2 = edges + query.indexB; + const b3HullHalfEdge* twin2 = edges + edge2->twin; + b3Vec3 ch = hullA->center; + b3Vec3 ph = points[edge2->origin]; + b3Vec3 qh = points[twin2->origin]; + b3Vec3 eh = b3Sub( qh, ph ); + + b3Vec3 normal = b3Cross( ec, eh ); + normal = b3Normalize( normal ); + + // Normal should point outward from hull + if ( b3Dot( normal, b3Sub( ph, ch ) ) < 0.0f ) + { + normal = b3Neg( normal ); + } + + b3SegmentDistanceResult result = b3LineDistance( ph, eh, pc, ec ); + + if ( b3IsWithinSegments( &result ) == false ) + { + // closest point beyond end points + return false; + } + + b3Vec3 point = b3MulSV( 0.5f, b3Add( b3MulSub( result.point1, capsuleB->radius, normal ), result.point2 ) ); + + float separation = b3Dot( normal, b3Sub( result.point2, result.point1 ) ); + B3_VALIDATE( b3AbsFloat( separation - query.separation ) < B3_LINEAR_SLOP ); + + // Manifold in frame A + manifold->normal = normal; + manifold->pointCount = 1; + + b3LocalManifoldPoint* pt = manifold->points + 0; + pt->point = point; + pt->separation = separation - capsuleB->radius; + pt->pair = b3MakeFeaturePair( b3_featureShapeA, query.indexA, b3_featureShapeB, query.indexB ); + return true; +} + +void b3CollideHullAndCapsule( b3LocalManifold* manifold, int capacity, const b3HullData* hullA, const b3Capsule* capsuleB, + b3Transform transformBtoA, b3SimplexCache* cache ) +{ + manifold->pointCount = 0; + + if ( capacity < 2 ) + { + return; + } + + // Work in shapeA coordinates + b3DistanceInput distanceInput; + distanceInput.proxyA = (b3ShapeProxy){ b3GetHullPoints( hullA ), hullA->vertexCount, 0.0f }; + distanceInput.proxyB = (b3ShapeProxy){ &capsuleB->center1, 2, 0.0f }; + distanceInput.transform = transformBtoA; + distanceInput.useRadii = false; + + b3DistanceOutput distanceOutput = b3ShapeDistance( &distanceInput, cache, NULL, 0 ); + const float speculativeDistance = B3_SPECULATIVE_DISTANCE; + + if ( distanceOutput.distance > capsuleB->radius + speculativeDistance ) + { + // We found a separating axis + *cache = (b3SimplexCache){ 0 }; + return; + } + + if ( distanceOutput.distance > 100.0f * FLT_EPSILON ) + { + const b3Plane* planes = b3GetHullPlanes( hullA ); + + // Shallow penetration + b3Vec3 delta = distanceOutput.normal; + int refFace = b3FindHullSupportFace( hullA, delta ); + b3Plane refPlane = planes[refFace]; + + // Try to create two contact points if closest + // points difference is nearly parallel to face normal + const float kTolerance = 0.998f; + if ( b3AbsFloat( b3Dot( refPlane.normal, delta ) ) > kTolerance ) + { + // Clip capsule segment against side planes of reference face + b3ClipVertex verticesB[2]; + verticesB[0].position = b3TransformPoint( transformBtoA, capsuleB->center1 ); + verticesB[0].separation = 0.0f; + verticesB[0].pair = b3MakeFeaturePair( b3_featureShapeA, 0, b3_featureShapeA, 0 ); + verticesB[1].position = b3TransformPoint( transformBtoA, capsuleB->center2 ); + verticesB[1].separation = 0.0f; + verticesB[1].pair = b3MakeFeaturePair( b3_featureShapeA, 1, b3_featureShapeA, 1 ); + + int pointCount = b3ClipSegmentToHullFace( verticesB, hullA, refFace ); + + if ( pointCount == 2 ) + { + float distance1 = b3PlaneSeparation( refPlane, verticesB[0].position ); + float distance2 = b3PlaneSeparation( refPlane, verticesB[1].position ); + if ( distance1 <= capsuleB->radius + speculativeDistance || distance2 <= capsuleB->radius + speculativeDistance ) + { + b3Vec3 normal = refPlane.normal; + b3Vec3 point1 = b3MulSub( verticesB[0].position, 0.5f * ( capsuleB->radius + distance1 ), normal ); + b3Vec3 point2 = b3MulSub( verticesB[1].position, 0.5f * ( capsuleB->radius + distance2 ), normal ); + + // Manifold in frame A + manifold->normal = normal; + manifold->pointCount = 2; + + b3LocalManifoldPoint* pt1 = manifold->points + 0; + pt1->point = point1; + pt1->separation = distance1 - capsuleB->radius; + pt1->pair = verticesB[0].pair; + + b3LocalManifoldPoint* pt2 = manifold->points + 1; + pt2->point = point2; + pt2->separation = distance2 - capsuleB->radius; + pt2->pair = verticesB[1].pair; + + return; + } + } + } + + // Create contact from closest points + b3Vec3 point = + b3MulSV( 0.5f, b3Add( b3MulSub( distanceOutput.pointA, capsuleB->radius, delta ), distanceOutput.pointB ) ); + + // Manifold in frame A + manifold->normal = delta; + manifold->pointCount = 1; + + b3LocalManifoldPoint* pt = manifold->points + 0; + pt->point = point; + pt->separation = distanceOutput.distance - capsuleB->radius; + pt->pair = b3FeaturePair_single; + return; + } + + // Deep penetration + + b3FaceQuery faceQuery = b3QueryFaceDirectionHullAndCapsule( hullA, capsuleB, transformBtoA ); + if ( faceQuery.separation > capsuleB->radius ) + { + // We found a separating axis + return; + } + + b3EdgeQuery edgeQuery = b3QueryEdgeDirectionHullAndCapsule( hullA, capsuleB, transformBtoA ); + if ( edgeQuery.separation > capsuleB->radius ) + { + // We found a separating axis + return; + } + + // Create face contact + float faceSeparation = faceQuery.separation - capsuleB->radius; + b3BuildHullFaceAndCapsuleContact( manifold, hullA, capsuleB, transformBtoA, faceQuery ); + if ( manifold->pointCount > 1 ) + { + // If ( Out.PointCount <= 1 ) -> Compare with unclipped separation + // If ( Out.PointCount > 1 ) -> Be aggressive and compare with clipped separation + // Face contact can be empty if it does not realize the axis of minimum penetration + faceSeparation = b3DeepestPointSeparation( manifold ); + } + B3_VALIDATE( faceSeparation <= 0.0f ); + + // Face contact can be empty if it does not realize the axis of minimum penetration. + // Create edge contact if face contact fails or edge contact is significantly better! + const float kRelEdgeTolerance = 0.90f; + const float kAbsTolerance = 0.5f * B3_LINEAR_SLOP; + float edgeSeparation = edgeQuery.separation - capsuleB->radius; + if ( manifold->pointCount == 0 || edgeSeparation > kRelEdgeTolerance * faceSeparation + kAbsTolerance ) + { + // Edge contact + b3BuildHullAndCapsuleEdgeContact( manifold, capacity, hullA, capsuleB, transformBtoA, edgeQuery ); + } +} + +static int b3BuildPolygon( b3ClipVertex* out, b3Transform transform, const b3HullData* hull, int incFace, b3Plane refPlane ) +{ + const b3HullFace* faces = b3GetHullFaces( hull ); + const b3HullHalfEdge* edges = b3GetHullEdges( hull ); + const b3Vec3* points = b3GetHullPoints( hull ); + + const b3HullFace* face = faces + incFace; + int edgeIndex = face->edge; + B3_ASSERT( edges[edgeIndex].face == incFace ); + + int outCount = 0; + + b3Matrix3 matrix = b3MakeMatrixFromQuat( transform.q ); + + do + { + const b3HullHalfEdge* edge = edges + edgeIndex; + + int nextEdgeIndex = edge->next; + const b3HullHalfEdge* next = edges + nextEdgeIndex; + + b3ClipVertex vertex; + vertex.position = b3Add( b3MulMV( matrix, points[next->origin] ), transform.p ); + vertex.separation = b3PlaneSeparation( refPlane, vertex.position ); + vertex.pair = b3MakeFeaturePair( b3_featureShapeB, edgeIndex, b3_featureShapeB, nextEdgeIndex ); + + out[outCount] = vertex; + outCount += 1; + + edgeIndex = nextEdgeIndex; + } + while ( edgeIndex != face->edge && outCount < B3_MAX_CLIP_POINTS ); + + B3_VALIDATE( b3ValidatePolygon( out, outCount ) ); + + return outCount; +} + +static bool b3BuildFaceAContact( b3LocalManifold* manifold, int capacity, const b3HullData* hullA, const b3HullData* hullB, + b3Transform transformBtoA, b3FaceQuery query, b3SATCache* cache ) +{ + const b3HullFace* facesA = b3GetHullFaces( hullA ); + const b3HullHalfEdge* edgesA = b3GetHullEdges( hullA ); + const b3Plane* planesA = b3GetHullPlanes( hullA ); + const b3Vec3* pointsA = b3GetHullPoints( hullA ); + + // Reference face + int refFace = query.faceIndex; + b3Plane refPlane = planesA[refFace]; + + // Find incident face + b3Vec3 refNormalInB = b3InvRotateVector( transformBtoA.q, refPlane.normal ); + int incFace = b3FindIncidentFace( hullB, refNormalInB, query.vertexIndex ); + + // Build clip polygon from incident face in frame A + b3ClipVertex buffer1[B3_MAX_CLIP_POINTS], buffer2[B3_MAX_CLIP_POINTS]; + int pointCount = b3BuildPolygon( buffer1, transformBtoA, hullB, incFace, refPlane ); + + // Clip incident face against side planes of reference face + b3ClipVertex* input = buffer1; + b3ClipVertex* output = buffer2; + + const b3HullFace* face = facesA + refFace; + int edgeIndex = face->edge; + + do + { + const b3HullHalfEdge* edge = edgesA + edgeIndex; + int nextEdgeIndex = edge->next; + const b3HullHalfEdge* next = edgesA + nextEdgeIndex; + b3Vec3 vertex1 = pointsA[edge->origin]; + b3Vec3 vertex2 = pointsA[next->origin]; + b3Vec3 tangent = b3Normalize( b3Sub( vertex2, vertex1 ) ); + b3Vec3 binormal = b3Cross( tangent, refPlane.normal ); + + b3Plane clipPlane = b3MakePlaneFromNormalAndPoint( binormal, vertex1 ); + + pointCount = b3ClipPolygon( output, input, pointCount, clipPlane, edgeIndex, refPlane ); + B3_ASSERT( pointCount <= B3_MAX_CLIP_POINTS ); + + B3_SWAP( output, input ); + + if ( pointCount < 3 ) + { + *cache = (b3SATCache){ 0 }; + return false; + } + + edgeIndex = nextEdgeIndex; + } + while ( edgeIndex != face->edge ); + + pointCount = b3MinInt( pointCount, B3_MAX_CLIP_POINTS ); + + b3LocalManifoldPoint points[B3_MAX_CLIP_POINTS]; + float minSeparation = FLT_MAX; + + manifold->normal = refPlane.normal; + + for ( int i = 0; i < pointCount; ++i ) + { + b3ClipVertex* clipPoint = input + i; + b3LocalManifoldPoint* pt = points + i; + *pt = (b3LocalManifoldPoint){ 0 }; + + // Using the half-way point keeps the points in the same position when swapping reference face from A to B. + b3Vec3 point = b3MulSub( clipPoint->position, 0.5f * clipPoint->separation, refPlane.normal ); + + // Old way of pushing onto the reference face. + // b3Vec3 point = clipPoint->position - clipPoint->separation * refPlane.normal; + + pt->point = point; + pt->separation = clipPoint->separation; + pt->pair = clipPoint->pair; + + minSeparation = b3MinFloat( minSeparation, clipPoint->separation ); + } + + if ( minSeparation >= B3_SPECULATIVE_DISTANCE ) + { + *cache = (b3SATCache){ 0 }; + return false; + } + + b3ReduceManifoldPoints( manifold, capacity, points, pointCount ); + + // Save cache + cache->separation = minSeparation; + cache->type = (uint8_t)b3_faceAxisA; + cache->indexA = (uint8_t)query.faceIndex; + cache->indexB = (uint8_t)query.vertexIndex; + + return true; +} + +static bool b3BuildFaceBContact( b3LocalManifold* manifold, int capacity, const b3HullData* hullA, const b3HullData* hullB, + b3Transform transformBtoA, b3FaceQuery query, b3SATCache* cache ) +{ + b3Transform transformAtoB = b3InvertTransform( transformBtoA ); + bool touching = b3BuildFaceAContact( manifold, capacity, hullB, hullA, transformAtoB, query, cache ); + if ( touching == false ) + { + return false; + } + + // Results are in frame B, need to transform them into frame A + b3Matrix3 matrix = b3MakeMatrixFromQuat( transformBtoA.q ); + + // Transform and flip normal so it points from A to B, even though the B has the reference face. + manifold->normal = b3Neg( b3MulMV( matrix, manifold->normal ) ); + cache->type = (uint8_t)b3_faceAxisB; + cache->indexA = (uint8_t)query.vertexIndex; + cache->indexB = (uint8_t)query.faceIndex; + + // Transform points from frame B to frame A. + // Also flip the pairs to ensure correct matches. + for ( int i = 0; i < manifold->pointCount; ++i ) + { + b3LocalManifoldPoint* pt = manifold->points + i; + pt->point = b3Add( b3MulMV( matrix, pt->point ), transformBtoA.p ); + pt->pair = b3FlipPair( pt->pair ); + } + + return true; +} + +static bool b3BuildEdgeContact( b3LocalManifold* manifold, const b3HullData* hullA, const b3HullData* hullB, b3Transform transformBtoA, + b3EdgeQuery query, b3SATCache* cache ) +{ + // Work in shapeA coordinates + const b3HullHalfEdge* edgesA = b3GetHullEdges( hullA ); + const b3Vec3* pointsA = b3GetHullPoints( hullA ); + + const b3HullHalfEdge* edgesB = b3GetHullEdges( hullB ); + const b3Vec3* pointsB = b3GetHullPoints( hullB ); + + // B3_VALIDATE( query.separation <= 2.0f * B3_SPECULATIVE_DISTANCE ); + + const b3HullHalfEdge* edgeA = edgesA + query.indexA; + const b3HullHalfEdge* twinA = edgesA + edgeA->twin; + b3Vec3 centerA = hullA->center; + b3Vec3 pA = pointsA[edgeA->origin]; + b3Vec3 qA = pointsA[twinA->origin]; + b3Vec3 eA = b3Sub( qA, pA ); + + const b3HullHalfEdge* edgeB = edgesB + query.indexB; + const b3HullHalfEdge* twinB = edgesB + edgeB->twin; + b3Vec3 pB = b3TransformPoint( transformBtoA, pointsB[edgeB->origin] ); + b3Vec3 qB = b3TransformPoint( transformBtoA, pointsB[twinB->origin] ); + b3Vec3 eB = b3Sub( qB, pB ); + + b3Vec3 normal = b3Cross( eA, eB ); + normal = b3Normalize( normal ); + + if ( b3Dot( normal, b3Sub( pA, centerA ) ) < 0.0f ) + { + normal = b3Neg( normal ); + } + + b3SegmentDistanceResult result = b3LineDistance( pA, eA, pB, eB ); + + if ( b3IsWithinSegments( &result ) == false ) + { + *cache = (b3SATCache){ 0 }; + return false; + } + + // This can slide off the end from caching + float separation = b3Dot( normal, b3Sub( result.point2, result.point1 ) ); + + // todo I suspect this could trip if the cache becomes invalid + // B3_VALIDATE( b3AbsFloat( separation - query.separation ) < B3_LINEAR_SLOP ); + + b3Vec3 point = b3MulSV( 0.5f, b3Add( result.point1, result.point2 ) ); + + // Result in frame A + manifold->normal = normal; + manifold->pointCount = 1; + + b3LocalManifoldPoint* pt = manifold->points + 0; + pt->point = point; + pt->separation = separation; + pt->pair = b3MakeFeaturePair( b3_featureShapeA, query.indexA, b3_featureShapeB, query.indexB ); + + // Save cache + cache->separation = separation; + cache->type = (uint8_t)b3_edgePairAxis; + cache->indexA = (uint8_t)query.indexA; + cache->indexB = (uint8_t)query.indexB; + + return true; +} + +void b3CollideHulls( b3LocalManifold* manifold, int capacity, const b3HullData* hullA, const b3HullData* hullB, b3Transform transformBtoA, + b3SATCache* cache ) +{ + manifold->pointCount = 0; + + if ( capacity < 4 ) + { + return; + } + + // Work in shapeA coordinates + float speculativeDistance = B3_SPECULATIVE_DISTANCE; + + float linearSlop = B3_LINEAR_SLOP; + const b3HullHalfEdge* edgesA = b3GetHullEdges( hullA ); + const b3Plane* planesA = b3GetHullPlanes( hullA ); + const b3Vec3* pointsA = b3GetHullPoints( hullA ); + + const b3HullHalfEdge* edgesB = b3GetHullEdges( hullB ); + const b3Plane* planesB = b3GetHullPlanes( hullB ); + const b3Vec3* pointsB = b3GetHullPoints( hullB ); + + // Attempt to use the cache to speed up collision + switch ( cache->type ) + { + case b3_invalidAxis: + *cache = (b3SATCache){ 0 }; + break; + + case b3_faceAxisA: + { + B3_ASSERT( cache->indexA < hullA->faceCount ); + + // Check for separation using cached face + b3Plane plane = planesA[cache->indexA]; + b3Vec3 searchDirectionInB = b3Neg( b3InvRotateVector( transformBtoA.q, plane.normal ) ); + int vertexIndex = b3FindHullSupportVertex( hullB, searchDirectionInB ); + b3Vec3 support = b3TransformPoint( transformBtoA, pointsB[vertexIndex] ); + float separation = b3PlaneSeparation( plane, support ); + + if ( separation >= speculativeDistance ) + { + // Cache hit, shapes are separated + return; + } + + // if ( cache->separation < speculativeDistance ) + { + // Attempt face contact using cached feature + b3FaceQuery faceQuery; + faceQuery.separation = 0.0f; + faceQuery.faceIndex = cache->indexA; + faceQuery.vertexIndex = vertexIndex; + + b3SATCache localCache = { 0 }; + bool touching = b3BuildFaceAContact( manifold, capacity, hullA, hullB, transformBtoA, faceQuery, &localCache ); + if ( touching == true && b3AbsFloat( cache->separation - localCache.separation ) < linearSlop ) + { + // Cache hit, contact points generated + return; + } + } + } + break; + + case b3_faceAxisB: + { + B3_ASSERT( cache->indexB < hullB->faceCount ); + + // Check for separation using cached face + b3Plane plane = planesB[cache->indexB]; + b3Vec3 searchDirectionInA = b3Neg( b3RotateVector( transformBtoA.q, plane.normal ) ); + int vertexIndex = b3FindHullSupportVertex( hullA, searchDirectionInA ); + b3Vec3 support = b3InvTransformPoint( transformBtoA, pointsA[vertexIndex] ); + float separation = b3PlaneSeparation( plane, support ); + + if ( separation >= speculativeDistance ) + { + // Cache hit, shapes are separated + return; + } + + // if ( cache->separation < speculativeDistance ) + { + // Attempt face contact using cached feature + b3FaceQuery faceQuery; + faceQuery.separation = 0.0f; + faceQuery.faceIndex = cache->indexB; + faceQuery.vertexIndex = vertexIndex; + + b3SATCache localCache = { 0 }; + bool touching = b3BuildFaceBContact( manifold, capacity, hullA, hullB, transformBtoA, faceQuery, &localCache ); + if ( touching == true && b3AbsFloat( cache->separation - localCache.separation ) < linearSlop ) + { + // Cache hit, contact points generated + return; + } + } + } + break; + + case b3_edgePairAxis: + { + int index1 = cache->indexA; + const b3HullHalfEdge* edge1 = edgesA + index1; + const b3HullHalfEdge* twin1 = edgesA + index1 + 1; + B3_ASSERT( edge1->twin == index1 + 1 && twin1->twin == index1 ); + + b3Vec3 p1 = pointsA[edge1->origin]; + b3Vec3 q1 = pointsA[twin1->origin]; + b3Vec3 e1 = b3Sub( q1, p1 ); + + b3Vec3 u1 = planesA[edge1->face].normal; + b3Vec3 v1 = planesA[twin1->face].normal; + + int index2 = cache->indexB; + const b3HullHalfEdge* edge2 = edgesB + index2; + const b3HullHalfEdge* twin2 = edgesB + index2 + 1; + B3_ASSERT( edge2->twin == index2 + 1 && twin2->twin == index2 ); + + b3Vec3 p2 = b3TransformPoint( transformBtoA, pointsB[edge2->origin] ); + b3Vec3 q2 = b3TransformPoint( transformBtoA, pointsB[twin2->origin] ); + b3Vec3 e2 = b3Sub( q2, p2 ); + + b3Vec3 u2 = b3RotateVector( transformBtoA.q, planesB[edge2->face].normal ); + b3Vec3 v2 = b3RotateVector( transformBtoA.q, planesB[twin2->face].normal ); + + // flipping the signs of u2 and v2 + // cross(v2, u2) == cross(-v2, -u2) + // so we still use -e2 + // but we can also use e1 = cross(u1, v1) and e2 = cross(u2, v2) + bool isMinkowski = b3IsMinkowskiFace( u1, v1, e1, b3Neg( u2 ), b3Neg( v2 ), e2 ); + if ( isMinkowski == true ) + { + // Transform reference center of the first hull into local space of the second hull + b3Vec3 c1 = hullA->center; + b3Vec3 c2 = b3TransformPoint( transformBtoA, hullB->center ); + + float separation = b3EdgeEdgeSeparation( p1, e1, c1, p2, e2, c2 ); + if ( separation > speculativeDistance ) + { + // Cache hit, shapes are separated + return; + } + + // if ( cache->separation <= speculativeDistance ) + { + // Try to rebuild contact from last features + b3EdgeQuery edgeQuery; + edgeQuery.indexA = cache->indexA; + edgeQuery.indexB = cache->indexB; + edgeQuery.separation = 0.0f; + + b3SATCache localCache = { 0 }; + bool touching = b3BuildEdgeContact( manifold, hullA, hullB, transformBtoA, edgeQuery, &localCache ); + if ( touching && b3AbsFloat( cache->separation - localCache.separation ) < linearSlop ) + { + // Cache hit, contact point generated + return; + } + } + } + } + break; + + // This case is for testing + case b3_manualFaceAxisA: + { + b3FaceQuery faceQueryA = b3QueryFaceDirections( hullA, hullB, transformBtoA ); + b3BuildFaceAContact( manifold, capacity, hullA, hullB, transformBtoA, faceQueryA, cache ); + return; + } + + // This case is for testing + case b3_manualFaceAxisB: + { + b3FaceQuery faceQueryB = b3QueryFaceDirections( hullB, hullA, b3InvertTransform( transformBtoA ) ); + b3BuildFaceBContact( manifold, capacity, hullA, hullB, transformBtoA, faceQueryB, cache ); + return; + } + + // This case is for testing + case b3_manualEdgePairAxis: + { + b3EdgeQuery edgeQuery = b3QueryEdgeDirections( hullA, hullB, transformBtoA ); + if ( edgeQuery.indexA != B3_NULL_INDEX ) + { + b3BuildEdgeContact( manifold, hullA, hullB, transformBtoA, edgeQuery, cache ); + } + return; + } + + default: + B3_ASSERT( false ); + break; + } + + manifold->pointCount = 0; + *cache = (b3SATCache){ 0 }; + + // Find axis of minimum penetration + b3FaceQuery faceQueryA = b3QueryFaceDirections( hullA, hullB, transformBtoA ); + if ( faceQueryA.separation > speculativeDistance ) + { + B3_ASSERT( faceQueryA.faceIndex < hullA->faceCount ); + B3_ASSERT( faceQueryA.vertexIndex < hullB->vertexCount ); + + // We found a separating axis + cache->separation = faceQueryA.separation; + cache->type = (uint8_t)b3_faceAxisA; + cache->indexA = (uint8_t)faceQueryA.faceIndex; + cache->indexB = (uint8_t)faceQueryA.vertexIndex; + return; + } + + b3FaceQuery faceQueryB = b3QueryFaceDirections( hullB, hullA, b3InvertTransform( transformBtoA ) ); + if ( faceQueryB.separation > speculativeDistance ) + { + B3_ASSERT( faceQueryB.faceIndex < hullB->faceCount ); + B3_ASSERT( faceQueryB.vertexIndex < hullA->vertexCount ); + + // We found a separating axis + cache->separation = faceQueryB.separation; + cache->type = (uint8_t)b3_faceAxisB; + cache->indexA = (uint8_t)faceQueryB.vertexIndex; + cache->indexB = (uint8_t)faceQueryB.faceIndex; + return; + } + + b3EdgeQuery edgeQuery = b3QueryEdgeDirections( hullA, hullB, transformBtoA ); + if ( edgeQuery.separation > speculativeDistance ) + { + // We found a separating axis + cache->separation = edgeQuery.separation; + cache->type = (uint8_t)b3_edgePairAxis; + cache->indexA = (uint8_t)edgeQuery.indexA; + cache->indexB = (uint8_t)edgeQuery.indexB; + return; + } + + // Always build a face contact (e.g. Jenga problem) + float faceSeparationA = faceQueryA.separation; + float faceSeparationB = faceQueryB.separation; + B3_VALIDATE( faceSeparationA <= speculativeDistance && faceSeparationB <= speculativeDistance ); + + if ( faceSeparationB > faceSeparationA + 0.5f * linearSlop ) + { + // Face contact B + b3BuildFaceBContact( manifold, capacity, hullA, hullB, transformBtoA, faceQueryB, cache ); + } + else + { + // Face contact A + b3BuildFaceAContact( manifold, capacity, hullA, hullB, transformBtoA, faceQueryA, cache ); + } + + if ( edgeQuery.indexA == B3_NULL_INDEX ) + { + // There are no valid edge pairs (all edges parallel) + return; + } + + float clippedFaceSeparation = cache->separation; + + B3_VALIDATE( edgeQuery.separation <= speculativeDistance ); + + // todo get rid of relative tolerance + const float kRelEdgeTolerance = 0.90f; + // const float kRelFaceTolerance = 0.98f; + const float kAbsTolerance = 0.5f * linearSlop; + + // Face contact can be empty if it does not realize the axis of minimum penetration. + // Create edge contact if face contact fails or edge contact is significantly better! + if ( manifold->pointCount == 0 || edgeQuery.separation > kRelEdgeTolerance * clippedFaceSeparation + kAbsTolerance ) + { + // Edge contact + b3LocalManifold edgeManifold = { 0 }; + b3LocalManifoldPoint edgePoint = { 0 }; + edgeManifold.points = &edgePoint; + + b3BuildEdgeContact( &edgeManifold, hullA, hullB, transformBtoA, edgeQuery, cache ); + + // It is possible with speculation to have vertex-vertex collision that is missed by SAT. + // todo I doubt this backup scheme matters because I'm using the clipped face separation. + // B3_VALIDATE( edgeManifold.pointCount == 1 ); + + if ( edgeManifold.pointCount == 1 ) + { + // Copy edge manifold out, being careful to preserve manifold point buffer. + b3LocalManifoldPoint* points = manifold->points; + *manifold = edgeManifold; + manifold->points = points; + manifold->points[0] = edgePoint; + } + } +} diff --git a/vendor/box3d/src/src/core.c b/vendor/box3d/src/src/core.c new file mode 100644 index 000000000..6747594b5 --- /dev/null +++ b/vendor/box3d/src/src/core.c @@ -0,0 +1,255 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +#if defined( B3_COMPILER_MSVC ) +// CRTDBG requires these to be included first +#define _CRTDBG_MAP_ALLOC +#include +#include +#else +#include +#endif + +#include "core.h" + +#include "box3d/constants.h" +#include "box3d/math_functions.h" + +#include +#include + +#ifdef BOX2D_PROFILE + +#include +#define b3TracyCAlloc( ptr, size ) TracyCAlloc( ptr, size ) +#define b3TracyCFree( ptr ) TracyCFree( ptr ) + +#else + +#define b3TracyCAlloc( ptr, size ) +#define b3TracyCFree( ptr ) + +#endif + +#include "platform.h" + +#include + +// This allows the user to change the length units at runtime +static float b3_lengthUnitsPerMeter = 1.0f; + +void b3SetLengthUnitsPerMeter( float lengthUnits ) +{ + B3_ASSERT( b3IsValidFloat( lengthUnits ) && lengthUnits > 0.0f ); + b3_lengthUnitsPerMeter = lengthUnits; +} + +float b3GetLengthUnitsPerMeter( void ) +{ + return b3_lengthUnitsPerMeter; +} + +static float b3_stallThreshold = FLT_MAX; + +void b3SetStallThreshold( float seconds ) +{ + B3_ASSERT( b3IsValidFloat( seconds ) && seconds > 0.0f ); + b3_stallThreshold = seconds; +} + +float b3GetStallThreshold( void ) +{ + return b3_stallThreshold; +} + +static int b3DefaultAssertFcn( const char* condition, const char* fileName, int lineNumber ) +{ + printf( "BOX3D ASSERTION: %s, %s, line %d\n", condition, fileName, lineNumber ); + + // return non-zero to break to debugger + return 1; +} + +b3AssertFcn* b3AssertHandler = b3DefaultAssertFcn; + +void b3SetAssertFcn( b3AssertFcn* assertFcn ) +{ + B3_ASSERT( assertFcn != NULL ); + b3AssertHandler = assertFcn; +} + +#if !defined( NDEBUG ) || defined( B3_ENABLE_ASSERT ) +int b3InternalAssert( const char* condition, const char* fileName, int lineNumber ) +{ + int result = b3AssertHandler( condition, fileName, lineNumber ); + if ( result ) + { + B3_BREAKPOINT; + } + return result; +} +#endif + +static void b3DefaultLogFcn( const char* message ) +{ + printf( "Box3D: %s\n", message ); +} + +b3LogFcn* b3LogHandler = b3DefaultLogFcn; + +void b3SetLogFcn( b3LogFcn* logFcn ) +{ + B3_ASSERT( logFcn != NULL ); + b3LogHandler = logFcn; +} + +void b3Log( const char* format, ... ) +{ + va_list args; + va_start( args, format ); + char buffer[512]; + vsnprintf( buffer, sizeof( buffer ), format, args ); + b3LogHandler( buffer ); + va_end( args ); +} + +b3Version b3GetVersion( void ) +{ + return (b3Version){ 0, 1, 0 }; +} + +bool b3IsDoublePrecision( void ) +{ +#if defined( BOX3D_DOUBLE_PRECISION ) + return true; +#else + return false; +#endif +} + +static b3AllocFcn* b3_allocFcn = NULL; +static b3FreeFcn* b3_freeFcn = NULL; + +b3AtomicInt b3_byteCount; + +void b3SetAllocator( b3AllocFcn* allocFcn, b3FreeFcn* freeFcn ) +{ + b3_allocFcn = allocFcn; + b3_freeFcn = freeFcn; +} + +void* b3Alloc( size_t size ) +{ + if ( size == 0 ) + { + return NULL; + } + + // This could cause some sharing issues, however Box3D rarely calls b3Alloc. + // todo this is not true, Box3D allocates a lot. + b3AtomicFetchAddInt( &b3_byteCount, (int)size ); + + // Allocation must be a multiple of B3_ALIGNMENT (required by spec). + // https://en.cppreference.com/w/c/memory/aligned_alloc + int alignedSize = ( ( (int)size - 1 ) | ( B3_ALIGNMENT - 1 ) ) + 1; + + if ( b3_allocFcn != NULL ) + { + void* ptr = b3_allocFcn( alignedSize, B3_ALIGNMENT ); + b3TracyCAlloc( ptr, size ); + + B3_ASSERT( ptr != NULL ); + B3_ASSERT( ( (uintptr_t)ptr & ( B3_ALIGNMENT - 1 ) ) == 0 ); + + return ptr; + } + +#ifdef B3_PLATFORM_WINDOWS + void* ptr = _aligned_malloc( alignedSize, B3_ALIGNMENT ); +#elif defined( B3_PLATFORM_ANDROID ) + void* ptr = NULL; + if ( posix_memalign( &ptr, B3_ALIGNMENT, alignedSize ) != 0 ) + { + // allocation failed, exit the application + exit( EXIT_FAILURE ); + } +#else + void* ptr = aligned_alloc( B3_ALIGNMENT, alignedSize ); +#endif + + b3TracyCAlloc( ptr, size ); + + B3_ASSERT( ptr != NULL ); + B3_ASSERT( ( (uintptr_t)ptr & ( B3_ALIGNMENT - 1 ) ) == 0 ); + + return ptr; +} + +void b3Free( void* mem, size_t size ) +{ + if ( mem == NULL ) + { + return; + } + + b3TracyCFree( mem ); + + if ( b3_freeFcn != NULL ) + { + b3_freeFcn( mem ); + } + else + { +#ifdef B3_PLATFORM_WINDOWS + _aligned_free( mem ); +#else + free( mem ); +#endif + } + + b3AtomicFetchAddInt( &b3_byteCount, -(int)size ); +} + +void* b3GrowAlloc( void* oldMem, int oldSize, int newSize ) +{ + B3_ASSERT( newSize > oldSize ); + void* newMem = b3Alloc( newSize ); + if ( oldSize > 0 ) + { + memcpy( newMem, oldMem, oldSize ); + b3Free( oldMem, oldSize ); + } + return newMem; +} + +int b3GetByteCount( void ) +{ + return b3AtomicLoadInt( &b3_byteCount ); +} + +void* b3AllocZeroed( size_t size ) +{ + void* mem = b3Alloc( size ); + memset( mem, 0, size ); + return mem; +} + +// Not used. Keeping around in case I need this. +void b3StrCpy( char* dst, int size, const char* src ) +{ + B3_ASSERT( size > 0 ); + + if ( src != NULL ) + { +#if defined( _MSC_VER ) + strncpy_s( dst, size, src, size - 1 ); +#else + strncpy( dst, src, size - 1 ); + dst[size - 1] = 0; +#endif + } + else + { + memset( dst, 0, size ); + } +} diff --git a/vendor/box3d/src/src/core.h b/vendor/box3d/src/src/core.h new file mode 100644 index 000000000..3aaf196b1 --- /dev/null +++ b/vendor/box3d/src/src/core.h @@ -0,0 +1,161 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +#pragma once + +#include "box3d/base.h" + +#include + +// clang-format off + +#ifdef NDEBUG + #define B3_DEBUG 0 +#else + #define B3_DEBUG 1 +#endif + +// Define platform +#if defined(_WIN32) || defined(_WIN64) + #define B3_PLATFORM_WINDOWS +#elif defined( __ANDROID__ ) + #define B3_PLATFORM_ANDROID +#elif defined( __linux__ ) + #define B3_PLATFORM_LINUX +#elif defined( __APPLE__ ) + #include + #if defined( TARGET_OS_IPHONE ) && !TARGET_OS_IPHONE + #define B3_PLATFORM_MACOS + #else + #define B3_PLATFORM_IOS + #endif +#elif defined( __EMSCRIPTEN__ ) + #define B3_PLATFORM_WASM +#else + #define B3_PLATFORM_UNKNOWN +#endif + +// Define CPU +#if defined( __x86_64__ ) || defined( _M_X64 ) || defined( __i386__ ) || defined( _M_IX86 ) + #define B3_CPU_X86_X64 +#elif defined( __aarch64__ ) || defined( _M_ARM64 ) || defined( __arm__ ) || defined( _M_ARM ) + #define B3_CPU_ARM +#elif defined( __EMSCRIPTEN__ ) + #define B3_CPU_WASM +#else + #define B3_CPU_UNKNOWN +#endif + +// Define SIMD +#if defined( BOX3D_DISABLE_SIMD ) + #define B3_SIMD_NONE + #define B3_SIMD_WIDTH 4 + //#pragma message("B3_SIMD_NONE") +#else + #if defined( B3_CPU_X86_X64 ) + #define B3_SIMD_SSE2 + #define B3_SIMD_WIDTH 4 + //#pragma message("B3_SIMD_SSE2") + #elif defined( B3_CPU_ARM ) + #define B3_SIMD_NEON + #define B3_SIMD_WIDTH 4 + //#pragma message("B3_SIMD_NEON") + #elif defined( B3_CPU_WASM ) + #define B3_CPU_WASM + #define B3_SIMD_SSE2 + #define B3_SIMD_WIDTH 4 + //#pragma message("B3_SIMD_SSE2") + #else + #define B3_SIMD_NONE + #define B3_SIMD_WIDTH 4 + //#pragma message("B3_SIMD_NONE") + #endif +#endif + +// Define compiler +#if defined( __clang__ ) + #define B3_COMPILER_CLANG +#elif defined( __GNUC__ ) + #define B3_COMPILER_GCC +#elif defined( _MSC_VER ) + #define B3_COMPILER_MSVC +#endif + +/// Tracy profiler instrumentation +/// https://github.com/wolfpld/tracy +#ifdef BOX3D_PROFILE + #include + #define b3TracyCZoneC( ctx, color, active ) TracyCZoneC( ctx, color, active ) + #define b3TracyCZoneNC( ctx, name, color, active ) TracyCZoneNC( ctx, name, color, active ) + #define b3TracyCZoneEnd( ctx ) TracyCZoneEnd( ctx ) + #define b3TracyCFrame TracyCFrameMark +#else + #define b3TracyCZoneC( ctx, color, active ) + #define b3TracyCZoneNC( ctx, name, color, active ) + #define b3TracyCZoneEnd( ctx ) + #define b3TracyCFrame +#endif + +// clang-format on + +typedef struct b3AtomicInt +{ + int value; +} b3AtomicInt; + +typedef struct b3AtomicU32 +{ + uint32_t value; +} b3AtomicU32; + +// Minimum memory alignment used for all allocations +#define B3_ALIGNMENT 16 + +// Returns the number of elements of an array +#define B3_ARRAY_COUNT( A ) (int)( sizeof( A ) / sizeof( A[0] ) ) + +// Used to prevent the compiler from warning about unused variables +#define B3_UNUSED( ... ) (void)sizeof( ( __VA_ARGS__, 0 ) ) + +// Use to validate definitions. Do not take my cookie. +#define B3_SECRET_COOKIE 1152023 + +#define B3_CHECK_DEF( DEF ) B3_ASSERT( DEF->internalValue == B3_SECRET_COOKIE ) +#define B3_CHECK_JOINT_DEF( DEF ) B3_ASSERT( DEF->base.internalValue == B3_SECRET_COOKIE ) + +// These macros help avoid sizeof bugs +#define B3_ALLOC( T, N ) (T*)b3Alloc( N * sizeof( T ) ); +#define B3_FREE( M, T, N ) b3Free( M, N * sizeof( T ) ); + +void* b3Alloc( size_t size ); +void* b3AllocZeroed( size_t size ); +void b3Free( void* mem, size_t size ); +void* b3GrowAlloc( void* oldMem, int oldSize, int newSize ); + +void b3Log( const char* format, ... ); + +// Geometry content hashes reserve zero to mean unhashed +static inline uint32_t b3NonZeroHash( uint32_t hash ) +{ + return hash != 0 ? hash : 1; +} + +typedef struct b3Mutex b3Mutex; +b3Mutex* b3CreateMutex( void ); +void b3DestroyMutex( b3Mutex* m ); +void b3LockMutex( b3Mutex* m ); +void b3UnlockMutex( b3Mutex* m ); + +typedef struct b3Semaphore b3Semaphore; +b3Semaphore* b3CreateSemaphore( int initCount ); +void b3DestroySemaphore( b3Semaphore* s ); +void b3WaitSemaphore( b3Semaphore* s ); +void b3SignalSemaphore( b3Semaphore* s ); + +typedef void b3ThreadFunction( void* context ); +typedef struct b3Thread b3Thread; +// Name may be NULL, otherwise it is copied. +b3Thread* b3CreateThread( b3ThreadFunction* function, void* context, const char* name ); +void b3JoinThread( b3Thread* t ); + +void b3StrCpy( char* dst, int size, const char* src ); diff --git a/vendor/box3d/src/src/ctz.h b/vendor/box3d/src/src/ctz.h new file mode 100644 index 000000000..483ce4bec --- /dev/null +++ b/vendor/box3d/src/src/ctz.h @@ -0,0 +1,134 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +#pragma once + +#include "box3d/base.h" + +#include +#include + +#if defined( _MSC_VER ) && !defined( __clang__ ) + +#include + +// https://en.wikipedia.org/wiki/Find_first_set + +static inline uint32_t b3CTZ32( uint32_t block ) +{ + unsigned long index; + _BitScanForward( &index, block ); + return index; +} + +// This function doesn't need to be fast, so using the Ivy Bridge fallback. +static inline uint32_t b3CLZ32( uint32_t value ) +{ +#if 1 + + // Use BSR (Bit Scan Reverse) which is available on Ivy Bridge + unsigned long index; + if ( _BitScanReverse( &index, value ) ) + { + // BSR gives the index of the most significant 1-bit + // We need to invert this to get the number of leading zeros + return 31 - index; + } + else + { + // If x is 0, BSR sets the zero flag and doesn't modify index + // LZCNT should return 32 for an input of 0 + return 32; + } + +#else + + return __lzcnt( value ); + +#endif +} + +static inline uint32_t b3CTZ64( uint64_t block ) +{ + unsigned long index; + +#ifdef _WIN64 + _BitScanForward64( &index, block ); +#else + // 32-bit fall back + if ( (uint32_t)block != 0 ) + { + _BitScanForward( &index, (uint32_t)block ); + } + else + { + _BitScanForward( &index, (uint32_t)( block >> 32 ) ); + index += 32; + } +#endif + + return index; +} + +static inline int b3PopCount64( uint64_t block ) +{ + return (int)__popcnt64( block ); +} + +#else + +static inline uint32_t b3CTZ32( uint32_t block ) +{ + return __builtin_ctz( block ); +} + +static inline uint32_t b3CLZ32( uint32_t value ) +{ + return __builtin_clz( value ); +} + +static inline uint32_t b3CTZ64( uint64_t block ) +{ + return __builtin_ctzll( block ); +} + +static inline int b3PopCount64( uint64_t block ) +{ + return __builtin_popcountll( block ); +} + +#endif + +static inline bool b3IsPowerOf2( int x ) +{ + return ( x & ( x - 1 ) ) == 0; +} + +static inline int b3BoundingPowerOf2( int x ) +{ + if ( x <= 1 ) + { + return 1; + } + + return 32 - (int)b3CLZ32( (uint32_t)x - 1 ); +} + +static inline int b3RoundUpPowerOf2( int x ) +{ + if ( x <= 1 ) + { + return 1; + } + + return 1 << ( 32 - (int)b3CLZ32( (uint32_t)x - 1 ) ); +} + +static inline int b3LowerPowerOf2Exponent( int x ) +{ + B3_ASSERT( x > 0 ); + int clz = (int)b3CLZ32( (uint32_t)x ); + + // Position of most significant bit = floor(log2(M)) + return 31 - clz; +} diff --git a/vendor/box3d/src/src/distance.c b/vendor/box3d/src/src/distance.c new file mode 100644 index 000000000..3d558ab34 --- /dev/null +++ b/vendor/box3d/src/src/distance.c @@ -0,0 +1,1904 @@ +// SPDX-FileCopyrightText: 2026 Erin Catto +// SPDX-License-Identifier: MIT + +// Dirk Gregorius contributed portions of this code + +#include "math_internal.h" + +#include "box3d/collision.h" +#include "box3d/constants.h" + +#define B3_MAX_SIMPLEX_VERTICES 4 +#define B3_MAX_GJK_ITERATIONS 32 + +int b3GetProxySupport( const b3ShapeProxy* proxy, b3Vec3 axis ) +{ + int count = proxy->count; + const b3Vec3* points = proxy->points; + + B3_ASSERT( count > 0 ); + B3_ASSERT( points != NULL ); + + // We move the first vertex into the origin for improved precision. + // This is necessary since we don't have shape transforms and + // vertices can potentially be far away from the origin (large). + b3Vec3 origin = points[0]; + int maxIndex = 0; + float maxProjection = 0.0f; + + for ( int index = 1; index < count; ++index ) + { + // We subtract the first vertex since we are shifting into the origin. + float projection = b3Dot( axis, b3Sub( points[index], origin ) ); + if ( projection > maxProjection ) + { + maxIndex = index; + maxProjection = projection; + } + } + + return maxIndex; +} + +int b3GetPointSupport( const b3Vec3* points, int count, b3Vec3 axis ) +{ + B3_ASSERT( count > 0 ); + B3_ASSERT( points != NULL ); + + // We move the first vertex into the origin for improved precision. + // This is necessary since we don't have shape transforms and + // vertices can potentially be far away from the origin (large). + b3Vec3 origin = points[0]; + int maxIndex = 0; + float maxProjection = 0.0f; + + for ( int index = 1; index < count; ++index ) + { + // We subtract the first vertex since we are shifting into the origin. + float projection = b3Dot( axis, b3Sub( points[index], origin ) ); + if ( projection > maxProjection ) + { + maxIndex = index; + maxProjection = projection; + } + } + + return maxIndex; +} + +static void b3BarycentricCoordsEdge( float out[3], b3Vec3 a, b3Vec3 b ) +{ + b3Vec3 ab = b3Sub( b, a ); + + // Last element is divisor + float divisor = b3Dot( ab, ab ); + + out[0] = b3Dot( b, ab ); + out[1] = -b3Dot( a, ab ); + out[2] = divisor; +} + +static void b3BarycentricCoordsTri( float out[4], b3Vec3 a, b3Vec3 b, b3Vec3 c ) +{ + b3Vec3 ab = b3Sub( b, a ); + b3Vec3 ac = b3Sub( c, a ); + + b3Vec3 bXC = b3Cross( b, c ); + b3Vec3 cXA = b3Cross( c, a ); + b3Vec3 aXB = b3Cross( a, b ); + + b3Vec3 abXAc = b3Cross( ab, ac ); + + // Last element is divisor + float divisor = b3Dot( abXAc, abXAc ); + + out[0] = b3Dot( bXC, abXAc ); + out[1] = b3Dot( cXA, abXAc ); + out[2] = b3Dot( aXB, abXAc ); + out[3] = divisor; +} + +static void b3BarycentricCoordsTet( float out[5], b3Vec3 a, b3Vec3 b, b3Vec3 c, b3Vec3 d ) +{ + b3Vec3 ab = b3Sub( b, a ); + b3Vec3 ac = b3Sub( c, a ); + b3Vec3 ad = b3Sub( d, a ); + + // Last element is divisor (forced to be positive) + float divisor = b3ScalarTripleProduct( ab, ac, ad ); + + float sign = divisor < 0.0f ? -1.0f : 1.0f; + out[0] = sign * b3ScalarTripleProduct( b, c, d ); + out[1] = sign * b3ScalarTripleProduct( a, d, c ); + out[2] = sign * b3ScalarTripleProduct( a, b, d ); + out[3] = sign * b3ScalarTripleProduct( a, c, b ); + out[4] = sign * divisor; +} + +static float b3GetMetric( const b3Simplex* simplex ) +{ + int count = simplex->count; + B3_ASSERT( 1 <= count && count <= 4 ); + + const b3SimplexVertex* vertices = simplex->vertices; + + switch ( count ) + { + case 1: + { + return 0.0f; + } + + case 2: + { + b3Vec3 a = vertices[0].w; + b3Vec3 b = vertices[1].w; + return b3Distance( a, b ); + } + + case 3: + { + b3Vec3 a = vertices[0].w; + b3Vec3 b = vertices[1].w; + b3Vec3 c = vertices[2].w; + return b3Length( b3Cross( b3Sub( b, a ), b3Sub( c, a ) ) ) / 2.0f; + } + + case 4: + { + b3Vec3 a = vertices[0].w; + b3Vec3 b = vertices[1].w; + b3Vec3 c = vertices[2].w; + b3Vec3 d = vertices[3].w; + return b3ScalarTripleProduct( b3Sub( b, a ), b3Sub( c, a ), b3Sub( d, a ) ) / 6.0f; + } + + default: + B3_ASSERT( !"Should never get here!" ); + break; + } + + return 0.0f; +} + +static void b3WriteCache( b3SimplexCache* cache, const b3Simplex* simplex ) +{ + int count = simplex->count; + cache->metric = b3GetMetric( simplex ); + cache->count = (uint16_t)count; + for ( int index = 0; index < count; ++index ) + { + cache->indexA[index] = (uint8_t)simplex->vertices[index].indexA; + cache->indexB[index] = (uint8_t)simplex->vertices[index].indexB; + } +} + +static bool b3SolveSimplex2( b3Simplex* simplex ) +{ + b3SimplexVertex* vs = simplex->vertices; + B3_ASSERT( simplex->count == 2 ); + + // Vertex regions + //float wAB[3]; + + b3Vec3 a = vs[0].w; + b3Vec3 b = vs[1].w; + b3Vec3 ab = b3Sub( b, a ); + + // Last element is divisor + float divisor = b3Dot( ab, ab ); + + float u = b3Dot( b, ab ); + float v = -b3Dot( a, ab ); + //wAB[2] = divisor; + + // V( A ) + if ( v <= 0.0f ) + { + // Reduce simplex + simplex->count = 1; + vs[0].a = 1.0f; + + return true; + } + + // V( B ) + if ( u <= 0.0f ) + { + // Reduce simplex + simplex->count = 1; + vs[0] = vs[1]; + vs[0].a = 1.0f; + + return true; + } + + // Edge region + if ( divisor <= 0.0f ) + { + return false; + } + + // VR( AB ) + float denominator = 1.0f / divisor; + vs[0].a = denominator * u; + vs[1].a = denominator * v; + + return true; +} + +static bool b3SolveSimplex3( b3Simplex* simplex ) +{ + b3SimplexVertex* vs = simplex->vertices; + B3_ASSERT( simplex->count == 3 ); + + // Get simplex (be aware of aliasing here!) + b3SimplexVertex v1 = vs[0]; + b3SimplexVertex v2 = vs[1]; + b3SimplexVertex v3 = vs[2]; + + // Vertex regions + float wAB[3], wBC[3], wCA[3]; + b3BarycentricCoordsEdge( wAB, v1.w, v2.w ); + b3BarycentricCoordsEdge( wBC, v2.w, v3.w ); + b3BarycentricCoordsEdge( wCA, v3.w, v1.w ); + + // VR( A ) + if ( wAB[1] <= 0.0f && wCA[0] <= 0.0f ) + { + // Reduce simplex + simplex->count = 1; + vs[0] = v1; + vs[0].a = 1.0f; + + return true; + } + + // VR( B ) + if ( wBC[1] <= 0.0f && wAB[0] <= 0.0f ) + { + // Reduce simplex + simplex->count = 1; + vs[0] = v2; + vs[0].a = 1.0f; + + return true; + } + + // VR( C ) + if ( wCA[1] <= 0.0f && wBC[0] <= 0.0f ) + { + // Reduce simplex + simplex->count = 1; + vs[0] = v3; + vs[0].a = 1.0f; + + return true; + } + + // Edge regions + float wABC[4]; + b3BarycentricCoordsTri( wABC, v1.w, v2.w, v3.w ); + + // VR( AB ) + if ( wABC[2] <= 0.0f && wAB[0] > 0.0f && wAB[1] > 0.0f ) + { + // Reduce simplex + simplex->count = 2; + vs[0] = v1; + vs[1] = v2; + + // Normalize + float divisor = wAB[2]; + if ( divisor <= 0.0f ) + { + return false; + } + + vs[0].a = wAB[0] / divisor; + vs[1].a = wAB[1] / divisor; + + return true; + } + + // VR( BC ) + if ( wABC[0] <= 0.0f && wBC[0] > 0.0f && wBC[1] > 0.0f ) + { + // Reduce simplex + simplex->count = 2; + vs[0] = v2; + vs[1] = v3; + + // Normalize + float divisor = wBC[2]; + if ( divisor <= 0.0f ) + { + return false; + } + + vs[0].a = wBC[0] / divisor; + vs[1].a = wBC[1] / divisor; + + return true; + } + + // VR( CA ) + if ( wABC[1] <= 0.0f && wCA[0] > 0.0f && wCA[1] > 0.0f ) + { + // Reduce simplex + simplex->count = 2; + vs[0] = v3; + vs[1] = v1; + + // Normalize + float divisor = wCA[2]; + if ( divisor <= 0.0f ) + { + return false; + } + + vs[0].a = wCA[0] / divisor; + vs[1].a = wCA[1] / divisor; + + return true; + } + + // Face region + float divisor = wABC[3]; + if ( divisor <= 0.0f ) + { + return false; + } + + // VR( ABC ) + vs[0].a = wABC[0] / divisor; + vs[1].a = wABC[1] / divisor; + vs[2].a = wABC[2] / divisor; + + return true; +} + +static bool b3SolveSimplex4( b3Simplex* simplex ) +{ + b3SimplexVertex* vs = simplex->vertices; + + // Get simplex (be aware of aliasing here!) + B3_ASSERT( simplex->count == 4 ); + b3SimplexVertex vertexA = vs[0]; + b3SimplexVertex vertexB = vs[1]; + b3SimplexVertex vertexC = vs[2]; + b3SimplexVertex vertexD = vs[3]; + + // Vertex region + float wAB[3], wAC[3], wAD[3], wBC[3], wCD[3], wDB[3]; + b3BarycentricCoordsEdge( wAB, vertexA.w, vertexB.w ); + b3BarycentricCoordsEdge( wAC, vertexA.w, vertexC.w ); + b3BarycentricCoordsEdge( wAD, vertexA.w, vertexD.w ); + b3BarycentricCoordsEdge( wBC, vertexB.w, vertexC.w ); + b3BarycentricCoordsEdge( wCD, vertexC.w, vertexD.w ); + b3BarycentricCoordsEdge( wDB, vertexD.w, vertexB.w ); + + // VR( A ) + if ( wAB[1] <= 0.0f && wAC[1] <= 0.0f && wAD[1] <= 0.0f ) + { + // Reduce simplex + simplex->count = 1; + vs[0] = vertexA; + + vs[0].a = 1.0f; + + return true; + } + + // VR( B ) + if ( wAB[0] <= 0.0f && wDB[0] <= 0.0f && wBC[1] <= 0.0f ) + { + // Reduce simplex + simplex->count = 1; + vs[0] = vertexB; + + vs[0].a = 1.0f; + + return true; + } + + // VR( C ) + if ( wAC[0] <= 0.0f && wBC[0] <= 0.0f && wCD[1] <= 0.0f ) + { + // Reduce simplex + simplex->count = 1; + vs[0] = vertexC; + + vs[0].a = 1.0f; + + return true; + } + + // VR( D ) + if ( wAD[0] <= 0.0f && wCD[0] <= 0.0f && wDB[1] <= 0.0f ) + { + // Reduce simplex + simplex->count = 1; + vs[0] = vertexD; + + vs[0].a = 1.0f; + + return true; + } + + // Edge region + float wACB[4], wABD[4], wADC[4], wBCD[4]; + b3BarycentricCoordsTri( wACB, vertexA.w, vertexC.w, vertexB.w ); + b3BarycentricCoordsTri( wABD, vertexA.w, vertexB.w, vertexD.w ); + b3BarycentricCoordsTri( wADC, vertexA.w, vertexD.w, vertexC.w ); + b3BarycentricCoordsTri( wBCD, vertexB.w, vertexC.w, vertexD.w ); + + // VR( AB ) + if ( wABD[2] <= 0.0f && wACB[1] <= 0.0f && wAB[0] > 0.0f && wAB[1] > 0.0f ) + { + // Reduce simplex + simplex->count = 2; + vs[0] = vertexA; + vs[1] = vertexB; + + // Normalize + float divisor = wAB[2]; + if ( divisor <= 0.0f ) + { + return false; + } + + vs[0].a = wAB[0] / divisor; + vs[1].a = wAB[1] / divisor; + + return true; + } + + // VR( AC ) + if ( wACB[2] <= 0.0f && wADC[1] <= 0.0f && wAC[0] > 0.0f && wAC[1] > 0.0f ) + { + // Reduce simplex + simplex->count = 2; + vs[0] = vertexA; + vs[1] = vertexC; + + // Normalize + float divisor = wAC[2]; + if ( divisor <= 0.0f ) + { + return false; + } + + vs[0].a = wAC[0] / divisor; + vs[1].a = wAC[1] / divisor; + + return true; + } + + // VR( AD ) + if ( wADC[2] <= 0.0f && wABD[1] <= 0.0f && wAD[0] > 0.0f && wAD[1] > 0.0f ) + { + // Reduce simplex + simplex->count = 2; + vs[0] = vertexA; + vs[1] = vertexD; + + // Normalize + float divisor = wAD[2]; + if ( divisor <= 0.0f ) + { + return false; + } + + vs[0].a = wAD[0] / divisor; + vs[1].a = wAD[1] / divisor; + + return true; + } + + // VR( BC ) + if ( wACB[0] <= 0.0f && wBCD[2] <= 0.0f && wBC[0] > 0.0f && wBC[1] > 0.0f ) + { + // Reduce simplex + simplex->count = 2; + vs[0] = vertexB; + vs[1] = vertexC; + + // Normalize + float divisor = wBC[2]; + if ( divisor <= 0.0f ) + { + return false; + } + + vs[0].a = wBC[0] / divisor; + vs[1].a = wBC[1] / divisor; + + return true; + } + + // VR( CD ) + if ( wADC[0] <= 0.0f && wBCD[0] <= 0.0f && wCD[0] > 0.0f && wCD[1] > 0.0f ) + { + // Reduce simplex + simplex->count = 2; + vs[0] = vertexC; + vs[1] = vertexD; + + // Normalize + float divisor = wCD[2]; + if ( divisor <= 0.0f ) + { + return false; + } + + vs[0].a = wCD[0] / divisor; + vs[1].a = wCD[1] / divisor; + + return true; + } + + // VR( DB ) + if ( wABD[0] <= 0.0f && wBCD[1] <= 0.0f && wDB[0] > 0.0f && wDB[1] > 0.0f ) + { + // Reduce simplex + simplex->count = 2; + vs[0] = vertexD; + vs[1] = vertexB; + + // Normalize + float divisor = wDB[2]; + if ( divisor <= 0.0f ) + { + return false; + } + + vs[0].a = wDB[0] / divisor; + vs[1].a = wDB[1] / divisor; + + return true; + } + + // Face regions + float wABCD[5]; + b3BarycentricCoordsTet( wABCD, vertexA.w, vertexB.w, vertexC.w, vertexD.w ); + + // VR( ACB ) + if ( wABCD[3] < 0.0f && wACB[0] > 0.0f && wACB[1] > 0.0f && wACB[2] > 0.0f ) + { + // Reduce simplex + simplex->count = 3; + vs[0] = vertexA; + vs[1] = vertexC; + vs[2] = vertexB; + + // Normalize + float divisor = wACB[3]; + if ( divisor <= 0.0f ) + { + return false; + } + + vs[0].a = wACB[0] / divisor; + vs[1].a = wACB[1] / divisor; + vs[2].a = wACB[2] / divisor; + + return true; + } + + // VR( ABD ) + if ( wABCD[2] < 0.0f && wABD[0] > 0.0f && wABD[1] > 0.0f && wABD[2] > 0.0f ) + { + // Reduce simplex + simplex->count = 3; + vs[0] = vertexA; + vs[1] = vertexB; + vs[2] = vertexD; + + // Normalize + float divisor = wABD[3]; + if ( divisor <= 0.0f ) + { + return false; + } + + vs[0].a = wABD[0] / divisor; + vs[1].a = wABD[1] / divisor; + vs[2].a = wABD[2] / divisor; + + return true; + } + + // VR( ADC ) + if ( wABCD[1] < 0.0f && wADC[0] > 0.0f && wADC[1] > 0.0f && wADC[2] > 0.0f ) + { + // Reduce simplex + simplex->count = 3; + vs[0] = vertexA; + vs[1] = vertexD; + vs[2] = vertexC; + + // Normalize + float divisor = wADC[3]; + if ( divisor <= 0.0f ) + { + return false; + } + + vs[0].a = wADC[0] / divisor; + vs[1].a = wADC[1] / divisor; + vs[2].a = wADC[2] / divisor; + + return true; + } + + // VR( BCD ) + if ( wABCD[0] < 0.0f && wBCD[0] > 0.0f && wBCD[1] > 0.0f && wBCD[2] > 0.0f ) + { + // Reduce simplex + simplex->count = 3; + vs[0] = vertexB; + vs[1] = vertexC; + vs[2] = vertexD; + + // Normalize + float divisor = wBCD[3]; + if ( divisor <= 0.0f ) + { + return false; + } + + vs[0].a = wBCD[0] / divisor; + vs[1].a = wBCD[1] / divisor; + vs[2].a = wBCD[2] / divisor; + + return true; + } + + // *** Inside tetrahedron *** + float divisor = wABCD[4]; + if ( divisor <= 0.0f ) + { + return false; + } + + // VR( ABCD ) + vs[0].a = wABCD[0] / divisor; + vs[1].a = wABCD[1] / divisor; + vs[2].a = wABCD[2] / divisor; + vs[3].a = wABCD[3] / divisor; + + return true; +} + +static void b3ComputeWitnessPoints( const b3Simplex* simplex, b3Vec3* vertexA, b3Vec3* vertexB ) +{ + const b3SimplexVertex* vs = simplex->vertices; + int count = simplex->count; + B3_ASSERT( 1 <= count && count <= 4 ); + + switch ( count ) + { + case 1: + *vertexA = vs[0].wA; + *vertexB = vs[0].wB; + break; + + case 2: + *vertexA = b3Blend2( vs[0].a, vs[0].wA, vs[1].a, vs[1].wA ); + *vertexB = b3Blend2( vs[0].a, vs[0].wB, vs[1].a, vs[1].wB ); + break; + + case 3: + *vertexA = b3Blend3( vs[0].a, vs[0].wA, vs[1].a, vs[1].wA, vs[2].a, vs[2].wA ); + *vertexB = b3Blend3( vs[0].a, vs[0].wB, vs[1].a, vs[1].wB, vs[2].a, vs[2].wB ); + break; + + case 4: + { + // Force identical points and *zero* distance + b3Vec3 sum = b3Add( b3Blend2( vs[0].a, vs[0].wA, vs[1].a, vs[1].wA ), + b3Blend2( vs[2].a, vs[2].wA, vs[3].a, vs[3].wA ) ); + *vertexA = sum; + *vertexB = sum; + } + break; + + default: + B3_ASSERT( !"Should never get here!" ); + break; + } +} + +b3DistanceOutput b3ShapeDistance( const b3DistanceInput* input, b3SimplexCache* cache, b3Simplex* simplexes, int simplexCapacity ) +{ + // The query runs in frame A using the relative pose of B in A. + b3Transform xf = input->transform; + + // Use matrices for faster math + b3Matrix3 m = b3MakeMatrixFromQuat( xf.q ); + b3Matrix3 mt = b3Transpose( m ); + + const b3ShapeProxy* proxyA = &input->proxyA; + const b3ShapeProxy* proxyB = &input->proxyB; + + // Compute initial simplex from cache + B3_ASSERT( cache->count <= B3_MAX_SIMPLEX_VERTICES ); + + b3Simplex simplex = { 0 }; + b3SimplexVertex* vs = simplex.vertices; + + simplex.count = cache->count; + for ( int i = 0; i < cache->count; ++i ) + { + int index1 = cache->indexA[i]; + int index2 = cache->indexB[i]; + + B3_ASSERT( 0 <= index1 && index1 < proxyA->count ); + B3_ASSERT( 0 <= index2 && index2 < proxyB->count ); + + b3Vec3 vertex1 = proxyA->points[index1]; + b3Vec3 vertex2 = b3Add( b3MulMV( m, proxyB->points[index2] ), xf.p ); + + vs[i].indexA = index1; + vs[i].indexB = index2; + vs[i].wA = vertex1; + vs[i].wB = vertex2; + vs[i].w = b3Sub( vertex2, vertex1 ); + vs[i].a = 0.0f; + } + + // Compute the new simplex metric, if it is substantially + // different than the old metric flush the simplex. + if ( simplex.count > 0 ) + { + float metric1 = cache->metric; + float metric2 = b3GetMetric( &simplex ); + + // todo the tetrahedron metric can be negative + if ( 2.0f * metric1 < metric2 || metric2 < 0.5f * metric1 || metric2 < FLT_EPSILON ) + { + // Flush the simplex + simplex.count = 0; + } + } + + // If the cache is invalid or empty + if ( simplex.count == 0 ) + { + b3Vec3 vertex1 = proxyA->points[0]; + b3Vec3 vertex2 = b3Add( b3MulMV( m, proxyB->points[0] ), xf.p ); + + simplex.count = 1; + simplex.vertices[0].indexA = 0; + simplex.vertices[0].indexB = 0; + simplex.vertices[0].wA = vertex1; + simplex.vertices[0].wB = vertex2; + simplex.vertices[0].w = b3Sub( vertex2, vertex1 ); + simplex.vertices[0].a = 0.0f; + } + + b3Simplex backup = { 0 }; + + int simplexIndex = 0; + if ( simplexes != NULL && simplexIndex < simplexCapacity ) + { + simplexes[simplexIndex] = simplex; + simplexIndex += 1; + } + + b3DistanceOutput distanceOutput = { 0 }; + + // Keep track of squared distance + float distanceSq = FLT_MAX; + + b3Vec3 normal = b3Vec3_zero; + + // Run GJK + int iteration = 0; + for ( ; iteration < B3_MAX_GJK_ITERATIONS; ++iteration ) + { + // Solve simplex + bool solved = false; + switch ( simplex.count ) + { + case 1: + simplex.vertices[0].a = 1.0f; + solved = true; + break; + + case 2: + solved = b3SolveSimplex2( &simplex ); + break; + + case 3: + solved = b3SolveSimplex3( &simplex ); + break; + + case 4: + solved = b3SolveSimplex4( &simplex ); + break; + + default: + B3_ASSERT( !"Should never get here!" ); + break; + } + + if ( solved == false ) + { + // No progress - reconstruct last simplex + B3_ASSERT( backup.count != 0 ); + simplex = backup; + break; + } + + if ( simplexes != NULL && simplexIndex < simplexCapacity ) + { + simplexes[simplexIndex] = simplex; + simplexIndex += 1; + distanceOutput.iterations = iteration; + distanceOutput.simplexCount = simplexIndex; + } + + if ( simplex.count == B3_MAX_SIMPLEX_VERTICES ) + { + // Overlap + b3Vec3 localPointA, localPointB; + b3ComputeWitnessPoints( &simplex, &localPointA, &localPointB ); + distanceOutput.pointA = localPointA; + distanceOutput.pointB = localPointB; + return distanceOutput; + } + + // Assure distance progression + float oldDistanceSq = distanceSq; + + // Compute closest point + b3Vec3 closestPoint = { 0 }; + + switch ( simplex.count ) + { + case 1: + closestPoint = vs[0].w; + break; + + case 2: + closestPoint = b3Blend2( vs[0].a, vs[0].w, vs[1].a, vs[1].w ); + break; + + case 3: + closestPoint = b3Blend3( vs[0].a, vs[0].w, vs[1].a, vs[1].w, vs[2].a, vs[2].w ); + break; + + case 4: + closestPoint = b3Add( b3Blend2( vs[0].a, vs[0].w, vs[1].a, vs[1].w ), + b3Blend2( vs[2].a, vs[2].w, vs[3].a, vs[3].w ) ); + break; + + default: + B3_ASSERT( !"Should never get here!" ); + break; + } + + distanceSq = b3Dot( closestPoint, closestPoint ); + + if ( distanceSq >= oldDistanceSq ) + { + // No progress - reconstruct last simplex + B3_ASSERT( backup.count != 0 ); + simplex = backup; + break; + } + + // Build new tentative support point + b3Vec3 searchDirection = { 0 }; + + switch ( simplex.count ) + { + case 1: + { + // v = -A + searchDirection = b3Neg( vs[0].w ); + } + break; + + case 2: + { + // v = (AB x AO) x AB + b3Vec3 a = vs[0].w; + b3Vec3 b = vs[1].w; + + b3Vec3 ab = b3Sub( b, a ); + + searchDirection = b3Cross( b3Cross( ab, b3Neg( a ) ), ab ); + } + break; + + case 3: + { + // v = AB x AC or v = AC x AB + b3Vec3 a = vs[0].w; + b3Vec3 b = vs[1].w; + b3Vec3 c = vs[2].w; + + b3Vec3 ab = b3Sub( b, a ); + b3Vec3 ac = b3Sub( c, a ); + + b3Vec3 n = b3Cross( ab, ac ); + + searchDirection = b3Dot( n, a ) < 0.0f ? n : b3Neg( n ); + } + break; + + default: + B3_ASSERT( !"Should never get here!" ); + break; + } + + if ( b3LengthSquared( searchDirection ) < 1000.0f * FLT_MIN ) + { + // The origin is probably contained by a line segment or triangle. + // Thus the shapes are overlapped. + b3Vec3 localPointA, localPointB; + b3ComputeWitnessPoints( &simplex, &localPointA, &localPointB ); + distanceOutput.pointA = localPointA; + distanceOutput.pointB = localPointB; + B3_VALIDATE( b3Distance( localPointA, localPointB ) < FLT_EPSILON ); + return distanceOutput; + } + + normal = b3Neg( searchDirection ); + + // Get new support points + b3Vec3 searchDirection1 = searchDirection; + int indexA = b3GetProxySupport( &input->proxyA, b3Neg( searchDirection1 ) ); + b3Vec3 supportA = input->proxyA.points[indexA]; + b3Vec3 searchDirection2 = b3MulMV( mt, searchDirection ); + int indexB = b3GetProxySupport( &input->proxyB, searchDirection2 ); + b3Vec3 supportB = b3Add( b3MulMV( m, input->proxyB.points[indexB] ), xf.p ); + + // Save current simplex and add new vertex - this can fail if we detect cycling + backup = simplex; + + // Check for duplicate support points. This is the main termination criteria. + bool duplicate = false; + for ( int i = 0; i < simplex.count; ++i ) + { + if ( vs[i].indexA == indexA && vs[i].indexB == indexB ) + { + duplicate = true; + break; + } + } + + if ( duplicate ) + { + break; + } + + vs[simplex.count].indexA = indexA; + vs[simplex.count].indexB = indexB; + vs[simplex.count].wA = supportA; + vs[simplex.count].wB = supportB; + vs[simplex.count].w = b3Sub( supportB, supportA ); + simplex.count += 1; + } + + normal = b3Normalize( normal ); + if ( b3IsNormalized( normal ) == false ) + { + // Treat as overlap + return distanceOutput; + } + + // Build witness points and safe cache + b3Vec3 localPointA, localPointB; + b3ComputeWitnessPoints( &simplex, &localPointA, &localPointB ); + b3WriteCache( cache, &simplex ); + + // Results stay in frame A + distanceOutput.pointA = localPointA; + distanceOutput.pointB = localPointB; + distanceOutput.distance = b3Distance( localPointA, localPointB ); + distanceOutput.normal = normal; + distanceOutput.iterations = iteration; + distanceOutput.simplexCount = simplexIndex; + + // Apply radii if requested + if ( input->useRadii ) + { + float rA = input->proxyA.radius; + float rB = input->proxyB.radius; + distanceOutput.distance = b3MaxFloat( 0.0f, distanceOutput.distance - rA - rB ); + + // Keep closest points on perimeter even if overlapped, this way the points move smoothly. + distanceOutput.pointA = b3Add( distanceOutput.pointA, b3MulSV( rA, normal ) ); + distanceOutput.pointB = b3Sub( distanceOutput.pointB, b3MulSV( rB, normal ) ); + } + + return distanceOutput; +} + + +// Separation function: +// f(t) = (c2 + t * dp2 - c1 - t * dp1 ) * n + +// Root finding : f(t) - target = 0 +// (c2 + t * dp2 - c1 - t * dp1 ) * n - target = 0 +// (c2 - c1) * n + t * (dp2 - dp1) * n - target = 0 +// t = [target - (c2 - c1) * n] / [(dp2 - dp1) * n] +// t = (target - d) / [(dp2 - dp1) * n] + +b3CastOutput b3ShapeCast( const b3ShapeCastPairInput* input ) +{ + // Compute tolerance + float linearSlop = B3_LINEAR_SLOP; + float totalRadius = input->proxyA.radius + input->proxyB.radius; + float target = b3MaxFloat( linearSlop, totalRadius - linearSlop ); + float tolerance = 0.25f * linearSlop; + + B3_ASSERT( target > tolerance ); + + // Prepare input for distance query + b3SimplexCache cache = { 0 }; + + float alpha = 0.0f; + + b3DistanceInput distanceInput = { 0 }; + distanceInput.proxyA = input->proxyA; + distanceInput.proxyB = input->proxyB; + distanceInput.useRadii = false; + + // The whole cast runs in frame A. Advance the relative pose of B in float each iteration, + // which keeps the math near the local origin and avoids re-relativizing world poses. + distanceInput.transform = input->transform; + + b3Vec3 delta2 = input->translationB; + b3DistanceOutput distanceOutput = { 0 }; + b3CastOutput output = { 0 }; + output.triangleIndex = B3_NULL_INDEX; + + int iteration = 0; + const int maxIterations = 20; + + for ( ; iteration < maxIterations; ++iteration ) + { + output.iterations += 1; + + distanceOutput = b3ShapeDistance( &distanceInput, &cache, NULL, 0 ); + + if ( distanceOutput.distance < target + tolerance ) + { + if ( iteration == 0 ) + { + if ( input->canEncroach && distanceOutput.distance > 2.0f * linearSlop ) + { + target = distanceOutput.distance - linearSlop; + } + else + { + // Initial overlap + output.hit = true; + + // Compute a common point + b3Vec3 c1 = b3MulAdd( distanceOutput.pointA, input->proxyA.radius, distanceOutput.normal ); + b3Vec3 c2 = b3MulAdd( distanceOutput.pointB, -input->proxyB.radius, distanceOutput.normal ); + output.point = b3Lerp( c1, c2, 0.5f ); + return output; + } + } + else + { + // Logging for bad input data + if ( distanceOutput.distance > 0.0f && b3IsNormalized( distanceOutput.normal ) == false ) + { + for ( int i = 0; i < input->proxyA.count; ++i ) + { + b3Vec3 p = input->proxyA.points[i]; + b3Log( "pointA[%d] = {%.9f, %.9f, %.9f}", i, p.x, p.y, p.z ); + } + b3Log( "radiusA = %.9f", input->proxyA.radius ); + + for ( int i = 0; i < input->proxyB.count; ++i ) + { + b3Vec3 p = input->proxyB.points[i]; + b3Log( "pointB[%d] = {%.9f, %.9f, %.9f}", i, p.x, p.y, p.z ); + } + b3Log( "radiusB = %.9f", input->proxyB.radius ); + + { + b3Transform xf = input->transform; + b3Log( "transform = {{%.9f, %.9f, %.9f}, {{%.9f, %.9f, %.9f}, %.9f}", xf.p.x, xf.p.y, xf.p.z, xf.q.v.x, + xf.q.v.y, xf.q.v.z, xf.q.s ); + } + + { + b3Vec3 t = input->translationB; + b3Log( "t = {%.9f, %.9f, %.9f}", t.x, t.y, t.z ); + } + + b3Log( "maxFraction = %.9f, canEncroach = %d", input->maxFraction, input->canEncroach ); + + // Numerical problem. Likely extreme input. + return output; + } + + // Hitting this assert implies that the algorithm brought the shapes too close. + // B3_ASSERT( distanceOutput.distance > 0.0f && b3IsNormalized( distanceOutput.normal ) ); + + output.fraction = alpha; + output.point = b3MulAdd( distanceOutput.pointA, input->proxyA.radius, distanceOutput.normal ); + output.normal = distanceOutput.normal; + output.hit = true; + return output; + } + } + + B3_ASSERT( distanceOutput.distance > 0.0f ); + B3_ASSERT( b3IsNormalized( distanceOutput.normal ) ); + + // Check if shapes are approaching each other + float denominator = b3Dot( delta2, distanceOutput.normal ); + if ( denominator >= 0.0f ) + { + // Miss + return output; + } + + // Advance sweep + alpha += ( target - distanceOutput.distance ) / denominator; + if ( alpha >= input->maxFraction ) + { + // Success! + return output; + } + + distanceInput.transform.p = b3MulAdd( input->transform.p, alpha, delta2 ); + } + + // Failure! + return output; +} + +b3Transform b3GetSweepTransform( const b3Sweep* sweep, float time ) +{ + b3Transform transform; + transform.q = b3NLerp( sweep->q1, sweep->q2, time ); + transform.p = b3Sub( b3Lerp( sweep->c1, sweep->c2, time ), b3RotateVector( transform.q, sweep->localCenter ) ); + return transform; +} + +static inline b3Transform b3GetFinalSweepTransform( const b3Sweep* sweep ) +{ + b3Transform transform; + transform.q = sweep->q2; + transform.p = b3Sub( sweep->c2, b3RotateVector( transform.q, sweep->localCenter ) ); + return transform; +} + +static int b3UniqueCount( int vertexCount, int vertices[3] ) +{ + B3_ASSERT( 1 <= vertexCount && vertexCount <= 3 ); + + switch ( vertexCount ) + { + case 1: + return 1; + + case 2: + return vertices[0] != vertices[1] ? 2 : 1; + + case 3: + if ( vertices[0] != vertices[1] && vertices[0] != vertices[2] && vertices[1] != vertices[2] ) + { + // All different + return 3; + } + + if ( vertices[0] == vertices[1] && vertices[0] == vertices[2] && vertices[1] == vertices[2] ) + { + // All equal + return 1; + } + + return 2; + + default: + B3_ASSERT( !"Should never get here!" ); + break; + } + + return 0; +} + +// This checks if the cross product of two edges switches direction. +static inline bool b3CheckFastEdges( b3Transform xfA, b3Vec3 localEdgeA, b3Transform xfB, b3Vec3 localEdgeB, b3Vec3 axis0 ) +{ + // By taking the local witness axes we make sure that we + // get the correct orientations (e.g. if one axis was flipped)! + b3Vec3 edgeA = b3RotateVector( xfA.q, localEdgeA ); + b3Vec3 edgeB = b3RotateVector( xfB.q, localEdgeB ); + b3Vec3 axis = b3Cross( edgeA, edgeB ); + return b3Dot( axis, axis0 ) < 0.0f; +} + +typedef enum b3SeparationType +{ + b3_separationUnknown = 0, + b3_separationVertices, + b3_separationEdges, + b3_separationFaceA, + b3_separationFaceB, +} b3SeparationType; + +typedef struct b3SeparationFunction +{ + const b3ShapeProxy* proxyA; + const b3ShapeProxy* proxyB; + b3Sweep sweepA; + b3Sweep sweepB; + + // These are associated with different bodies depending on the separation function type. + // It could be two local vectors/points on the same body (for example, both on bodyA). + b3Vec3 witness1; + b3Vec3 witness2; + + b3SeparationType type; +} b3SeparationFunction; + +static b3SeparationFunction b3MakeSeparationFunction( const b3SimplexCache cache, const b3ShapeProxy* proxyA, + const b3Sweep* sweepA, const b3ShapeProxy* proxyB, const b3Sweep* sweepB, + b3Vec3 worldNormal, float t1 ) +{ + B3_ASSERT( 1 <= cache.count && cache.count <= 3 ); + B3_VALIDATE( b3IsNormalized( worldNormal ) ); + + b3SeparationFunction fcn = { 0 }; + fcn.proxyA = proxyA; + fcn.proxyB = proxyB; + fcn.sweepA = *sweepA; + fcn.sweepB = *sweepB; + fcn.type = b3_separationUnknown; + + int indexA[3] = { cache.indexA[0], cache.indexA[1], cache.indexA[2] }; + int indexB[3] = { cache.indexB[0], cache.indexB[1], cache.indexB[2] }; + + int uniqueCountA = b3UniqueCount( cache.count, indexA ); + int uniqueCountB = b3UniqueCount( cache.count, indexB ); + + b3Transform xfA1 = b3GetSweepTransform( sweepA, t1 ); + b3Transform xfB1 = b3GetSweepTransform( sweepB, t1 ); + + b3Quat qA = xfA1.q; + b3Quat qB = xfB1.q; + + // Minimize round-off + b3Vec3 deltaP = b3Sub( xfB1.p, xfA1.p ); + + switch ( cache.count ) + { + case 1: + { + // Witness is the world space direction + fcn.type = b3_separationVertices; + fcn.witness1 = worldNormal; + } + break; + + case 2: + { + if ( uniqueCountA == 2 && uniqueCountB == 2 ) + { + // Edge/Edge + b3Vec3 vA1 = proxyA->points[indexA[0]]; + b3Vec3 localEdgeA = b3Sub( proxyA->points[indexA[1]], vA1 ); + localEdgeA = b3Normalize( localEdgeA ); + b3Vec3 edgeA = b3RotateVector( qA, localEdgeA ); + + b3Vec3 vB1 = proxyB->points[indexB[0]]; + b3Vec3 localEdgeB = b3Sub( proxyB->points[indexB[1]], vB1 ); + localEdgeB = b3Normalize( localEdgeB ); + b3Vec3 edgeB = b3RotateVector( qB, localEdgeB ); + + b3Vec3 axis = b3Cross( edgeA, edgeB ); + float lengthSquared = b3LengthSquared( axis ); + + // Skip near parallel edges: |e1 x e1| = sin(alpha) * |e1| * |e2| + const float kToleranceSquared = 0.05f * 0.05f; + if ( lengthSquared < kToleranceSquared ) + { + // The axis is not safe to normalize so we use a world axis instead! + fcn.type = b3_separationVertices; + fcn.witness1 = worldNormal; + } + else + { + b3Vec3 delta = b3Add( b3Sub( b3RotateVector( qB, vB1 ), b3RotateVector( qA, vA1 ) ), deltaP ); + if ( b3Dot( delta, axis ) < 0.0f ) + { + // Make axis point from A to B + axis = b3Neg( axis ); + localEdgeB = b3Neg( localEdgeB ); + } + + // Check for possible sign flip in edge/edge cross product + b3Transform xfA2 = b3GetFinalSweepTransform( sweepA ); + b3Transform xfB2 = b3GetFinalSweepTransform( sweepB ); + bool fastEdges = b3CheckFastEdges( xfA2, localEdgeA, xfB2, localEdgeB, axis ); + if ( fastEdges == true ) + { + // Not safe to use local edges, fall back to initial world space axis instead + fcn.type = b3_separationVertices; + fcn.witness1 = b3Normalize( axis ); + } + else + { + // Edge cross product is safe. This converges faster than a fixed axis. + fcn.type = b3_separationEdges; + fcn.witness1 = localEdgeA; + fcn.witness2 = localEdgeB; + } + } + } + else + { + B3_VALIDATE( b3IsNormalized( worldNormal ) ); + + // Vertex versus edge, use world axis witness + fcn.type = b3_separationVertices; + fcn.witness1 = worldNormal; + } + } + break; + + case 3: + { + if ( uniqueCountA == 3 ) + { + b3Vec3 vA1 = proxyA->points[indexA[0]]; + b3Vec3 vA2 = proxyA->points[indexA[1]]; + b3Vec3 vA3 = proxyA->points[indexA[2]]; + b3Vec3 localAxisA = b3Cross( b3Sub( vA2, vA1 ), b3Sub( vA3, vA1 ) ); + localAxisA = b3Normalize( localAxisA ); + b3Vec3 axisA = b3RotateVector( qA, localAxisA ); + + b3Vec3 localPointA = b3MulSV( 1.0f / 3.0f, b3Add( b3Add( vA1, vA2 ), vA3 ) ); + b3Vec3 localPointB = proxyB->points[indexB[0]]; + b3Vec3 delta = b3Add( b3Sub( b3RotateVector( qB, localPointB ), b3RotateVector( qA, localPointA ) ), deltaP ); + + if ( b3Dot( delta, axisA ) < 0.0f ) + { + // Make axis point from A to B + localAxisA = b3Neg( localAxisA ); + } + + // Witness is the local plane of faceA + fcn.type = b3_separationFaceA; + fcn.witness1 = localAxisA; + fcn.witness2 = localPointA; + } + else if ( uniqueCountB == 3 ) + { + b3Vec3 vB1 = proxyB->points[indexB[0]]; + b3Vec3 vB2 = proxyB->points[indexB[1]]; + b3Vec3 vB3 = proxyB->points[indexB[2]]; + b3Vec3 localAxisB = b3Cross( b3Sub( vB2, vB1 ), b3Sub( vB3, vB1 ) ); + localAxisB = b3Normalize( localAxisB ); + b3Vec3 axisB = b3RotateVector( qB, localAxisB ); + + b3Vec3 localPointA = proxyA->points[indexA[0]]; + b3Vec3 localPointB = b3MulSV( 1.0f / 3.0f, b3Add( b3Add( vB1, vB2 ), vB3 ) ); + b3Vec3 delta = b3Sub( b3Sub( b3RotateVector( qA, localPointA ), b3RotateVector( qB, localPointB ) ), deltaP ); + + if ( b3Dot( delta, axisB ) < 0.0f ) + { + // Make axis point from B to A + localAxisB = b3Neg( localAxisB ); + } + + // Witness is the local plane of faceB + fcn.type = b3_separationFaceB; + fcn.witness1 = localAxisB; + fcn.witness2 = localPointB; + } + else + { + B3_ASSERT( uniqueCountA == 2 && uniqueCountB == 2 ); + + if ( indexA[0] == indexA[1] ) + { + // Make first two indices are unique + indexA[1] = indexA[2]; + B3_ASSERT( indexA[0] != indexA[1] ); + } + + b3Vec3 vA1 = proxyA->points[indexA[0]]; + b3Vec3 vA2 = proxyA->points[indexA[1]]; + b3Vec3 localEdgeA = b3Normalize( b3Sub( vA2, vA1 ) ); + b3Vec3 edgeA = b3RotateVector( qA, localEdgeA ); + + if ( indexB[0] == indexB[1] ) + { + // Make first two indices are unique + indexB[1] = indexB[2]; + B3_ASSERT( indexB[0] != indexB[1] ); + } + + b3Vec3 vB1 = proxyB->points[indexB[0]]; + b3Vec3 vB2 = proxyB->points[indexB[1]]; + b3Vec3 localEdgeB = b3Normalize( b3Sub( vB2, vB1 ) ); + b3Vec3 edgeB = b3RotateVector( qB, localEdgeB ); + + b3Vec3 axis = b3Cross( edgeA, edgeB ); + float lengthSquared = b3LengthSquared( axis ); + + // Skip near parallel edges: |e1 x e1| = sin(alpha) * |e1| * |e2| + const float kToleranceSquared = 0.005f * 0.005f; + if ( lengthSquared < kToleranceSquared ) + { + // The axis is not safe to normalize so we use a world axis instead! + fcn.type = b3_separationVertices; + fcn.witness1 = worldNormal; + } + else + { + b3Vec3 delta = b3Add( b3Sub( b3RotateVector( qB, vB1 ), b3RotateVector( qA, vA1 ) ), deltaP ); + if ( b3Dot( delta, axis ) < 0.0f ) + { + // Make axis point from A to B + axis = b3Neg( axis ); + localEdgeB = b3Neg( localEdgeB ); + } + + // Check for possible sign flip in edge/edge cross product + b3Transform xfA2 = b3GetFinalSweepTransform( sweepA ); + b3Transform xfB2 = b3GetFinalSweepTransform( sweepB ); + bool fastEdges = b3CheckFastEdges( xfA2, localEdgeA, xfB2, localEdgeB, axis ); + if ( fastEdges ) + { + // Not safe to use local edges, fall back to initial world space axis instead + fcn.type = b3_separationVertices; + fcn.witness1 = b3Normalize( axis ); + } + else + { + // Edge cross product is safe. This converges faster than a fixed axis. + fcn.type = b3_separationEdges; + fcn.witness1 = localEdgeA; + fcn.witness2 = localEdgeB; + } + } + } + } + break; + + default: + B3_ASSERT( !"Should never get here!" ); + break; + } + + return fcn; +} + +static float b3FindMinSeparation( b3SeparationFunction* fcn, int* indexA, int* indexB, float t ) +{ + b3Transform xfA = b3GetSweepTransform( &fcn->sweepA, t ); + b3Transform xfB = b3GetSweepTransform( &fcn->sweepB, t ); + + switch ( fcn->type ) + { + case b3_separationVertices: + { + b3Vec3 axis = fcn->witness1; + + b3Vec3 localAxisA = b3InvRotateVector( xfA.q, axis ); + b3Vec3 localAxisB = b3InvRotateVector( xfB.q, b3Neg( axis ) ); + + *indexA = b3GetPointSupport( fcn->proxyA->points, fcn->proxyA->count, localAxisA ); + *indexB = b3GetPointSupport( fcn->proxyB->points, fcn->proxyB->count, localAxisB ); + + b3Vec3 deltaP = b3Sub( xfB.p, xfA.p ); + b3Vec3 localPointA = fcn->proxyA->points[*indexA]; + b3Vec3 localPointB = fcn->proxyB->points[*indexB]; + b3Vec3 delta = b3Add( b3Sub( b3RotateVector( xfB.q, localPointB ), b3RotateVector( xfA.q, localPointA ) ), deltaP ); + return b3Dot( delta, axis ); + } + + case b3_separationEdges: + { + b3Vec3 edgeA = b3RotateVector( xfA.q, fcn->witness1 ); + b3Vec3 edgeB = b3RotateVector( xfB.q, fcn->witness2 ); + b3Vec3 axis = b3Cross( edgeA, edgeB ); + B3_ASSERT( axis.x != 0.0f || axis.y != 0.0f || axis.z != 0.0f ); + axis = b3Normalize( axis ); + + b3Vec3 axisA = b3InvRotateVector( xfA.q, axis ); + *indexA = b3GetPointSupport( fcn->proxyA->points, fcn->proxyA->count, axisA ); + + b3Vec3 axisB = b3InvRotateVector( xfB.q, axis ); + *indexB = b3GetPointSupport( fcn->proxyB->points, fcn->proxyB->count, b3Neg( axisB ) ); + + b3Vec3 deltaP = b3Sub( xfB.p, xfA.p ); + b3Vec3 localPointA = fcn->proxyA->points[*indexA]; + b3Vec3 localPointB = fcn->proxyB->points[*indexB]; + b3Vec3 delta = b3Add( b3Sub( b3RotateVector( xfB.q, localPointB ), b3RotateVector( xfA.q, localPointA ) ), deltaP ); + + return b3Dot( delta, axis ); + } + + case b3_separationFaceA: + { + b3Vec3 normal = b3RotateVector( xfA.q, fcn->witness1 ); + *indexA = -1; + b3Vec3 pointA = b3TransformPoint( xfA, fcn->witness2 ); + + b3Vec3 axisB = b3InvRotateVector( xfB.q, normal ); + *indexB = b3GetPointSupport( fcn->proxyB->points, fcn->proxyB->count, b3Neg( axisB ) ); + b3Vec3 pointB = b3TransformPoint( xfB, fcn->proxyB->points[*indexB] ); + + return b3Dot( b3Sub( pointB, pointA ), normal ); + } + + case b3_separationFaceB: + { + b3Vec3 normal = b3RotateVector( xfB.q, fcn->witness1 ); + + b3Vec3 axisA = b3InvRotateVector( xfA.q, normal ); + *indexA = b3GetPointSupport( fcn->proxyA->points, fcn->proxyA->count, b3Neg( axisA ) ); + b3Vec3 pointA = b3TransformPoint( xfA, fcn->proxyA->points[*indexA] ); + + *indexB = -1; + b3Vec3 pointB = b3TransformPoint( xfB, fcn->witness2 ); + + return b3Dot( b3Sub( pointA, pointB ), normal ); + } + + default: + B3_ASSERT( !"Should never get here!" ); + break; + } + + return 0.0f; +} + +static float b3EvaluateSeparation( b3SeparationFunction* fcn, int index1, int index2, float beta ) +{ + b3Transform transform1 = b3GetSweepTransform( &fcn->sweepA, beta ); + b3Transform transform2 = b3GetSweepTransform( &fcn->sweepB, beta ); + + switch ( fcn->type ) + { + case b3_separationVertices: + { + b3Vec3 axis = fcn->witness1; + + b3Vec3 point1 = b3TransformPoint( transform1, fcn->proxyA->points[index1] ); + b3Vec3 point2 = b3TransformPoint( transform2, fcn->proxyB->points[index2] ); + + return b3Dot( b3Sub( point2, point1 ), axis ); + } + + case b3_separationEdges: + { + b3Vec3 edge1 = b3RotateVector( transform1.q, fcn->witness1 ); + b3Vec3 edge2 = b3RotateVector( transform2.q, fcn->witness2 ); + b3Vec3 axis = b3Cross( edge1, edge2 ); + axis = b3Normalize( axis ); + + b3Vec3 point1 = b3TransformPoint( transform1, fcn->proxyA->points[index1] ); + b3Vec3 point2 = b3TransformPoint( transform2, fcn->proxyB->points[index2] ); + + return b3Dot( b3Sub( point2, point1 ), axis ); + } + + case b3_separationFaceA: + { + b3Vec3 axis = b3RotateVector( transform1.q, fcn->witness1 ); + + b3Vec3 point1 = b3TransformPoint( transform1, fcn->witness2 ); + b3Vec3 point2 = b3TransformPoint( transform2, fcn->proxyB->points[index2] ); + + return b3Dot( b3Sub( point2, point1 ), axis ); + } + + case b3_separationFaceB: + { + b3Vec3 axis = b3RotateVector( transform2.q, fcn->witness1 ); + + b3Vec3 point1 = b3TransformPoint( transform1, fcn->proxyA->points[index1] ); + b3Vec3 point2 = b3TransformPoint( transform2, fcn->witness2 ); + + return b3Dot( b3Sub( point1, point2 ), axis ); + } + + default: + B3_ASSERT( !"Should never get here!" ); + break; + } + + return 0.0f; +} + +static void b3ForceFixedAxis( b3SeparationFunction* fcn, float beta ) +{ + B3_ASSERT( fcn->type == b3_separationEdges ); + + b3Transform transform1 = b3GetSweepTransform( &fcn->sweepA, beta ); + b3Transform transform2 = b3GetSweepTransform( &fcn->sweepB, beta ); + + b3Vec3 edge1 = b3RotateVector( transform1.q, fcn->witness1 ); + b3Vec3 edge2 = b3RotateVector( transform2.q, fcn->witness2 ); + b3Vec3 axis = b3Cross( edge1, edge2 ); + axis = b3Normalize( axis ); + + fcn->type = b3_separationVertices; + fcn->witness1 = axis; + fcn->witness2 = b3Vec3_zero; +} + +// Time of Impact using root finding +b3TOIOutput b3TimeOfImpact( const b3TOIInput* input ) +{ + b3TOIOutput output = { 0 }; + + // Set these to invalid values so they can be validated on exit + output.state = b3_toiStateUnknown; + output.fraction = -1.0f; + + b3Sweep sweepA = input->sweepA; + b3Sweep sweepB = input->sweepB; + + // Shift to origin + b3Vec3 origin = sweepA.c1; + sweepA.c1 = b3Vec3_zero; + sweepA.c2 = b3Sub( sweepA.c2, origin ); + sweepB.c1 = b3Sub( sweepB.c1, origin ); + sweepB.c2 = b3Sub( sweepB.c2, origin ); + + b3ShapeProxy proxyA = input->proxyA; + b3ShapeProxy proxyB = input->proxyB; + + int maxPushBackIterations = proxyA.count + proxyB.count; + float tMax = input->maxFraction; + + // Setup target distance and tolerance + float linearSlop = B3_LINEAR_SLOP; + float totalRadius = proxyA.radius + proxyB.radius; + float target = b3MaxFloat( linearSlop, totalRadius - linearSlop ); + float tolerance = 0.25f * linearSlop; + B3_ASSERT( target > tolerance ); + + float t1 = 0.0f; + const int maxIterations = 25; + int distanceIterations = 0; + + // Prepare input for distance query. + b3SimplexCache cache = { 0 }; + b3DistanceInput distanceInput = { 0 }; + distanceInput.proxyA = proxyA; + distanceInput.proxyB = proxyB; + distanceInput.useRadii = false; + + // The outer loop progressively attempts to compute new separating axes. + // This loop terminates when an axis is repeated (no progress is made). + for ( ;; ) + { + // Get the distance between shapes. We can also use the results to get a separating axis. + b3Transform xfA = b3GetSweepTransform( &sweepA, t1 ); + b3Transform xfB = b3GetSweepTransform( &sweepB, t1 ); + distanceInput.transform = b3InvMulTransforms( xfA, xfB ); + b3DistanceOutput distanceOutput = b3ShapeDistance( &distanceInput, &cache, NULL, 0 ); + output.distance = distanceOutput.distance; + + // The distance query runs in frame A, project the witness data back to the shifted world + b3Vec3 worldNormal = b3RotateVector( xfA.q, distanceOutput.normal ); + b3Vec3 worldPointA = b3TransformPoint( xfA, distanceOutput.pointA ); + b3Vec3 worldPointB = b3TransformPoint( xfA, distanceOutput.pointB ); + + output.distanceIterations += 1; + distanceIterations += 1; + + // If the shapes are overlapped, we give up on continuous collision. + if ( distanceOutput.distance <= 0.0f ) + { + output.state = b3_toiStateOverlapped; + output.fraction = 0.0f; + break; + } + + if ( distanceOutput.distance <= target + tolerance ) + { + // Success! + output.state = b3_toiStateHit; + + // Averaged hit point + b3Vec3 pA = b3MulAdd( worldPointA, proxyA.radius, worldNormal ); + b3Vec3 pB = b3MulAdd( worldPointB, -proxyB.radius, worldNormal ); + output.point = b3Lerp( pA, pB, 0.5f ); + output.point = b3Add( output.point, origin ); + output.normal = worldNormal; + output.fraction = t1; + break; + } + + if ( distanceIterations == maxIterations ) + { + // Progress too slow. This can happen when a capsule rotates around a + // triangle vertex. + output.state = b3_toiStateFailed; + output.fraction = t1; + + // Averaged hit point + b3Vec3 pA = b3MulAdd( worldPointA, input->proxyA.radius, worldNormal ); + b3Vec3 pB = b3MulAdd( worldPointB, -input->proxyB.radius, worldNormal ); + output.point = b3Lerp( pA, pB, 0.5f ); + output.point = b3Add( output.point, origin ); + output.normal = worldNormal; + break; + } + + // Initialize the separating axis. + b3SeparationFunction function = + b3MakeSeparationFunction( cache, &proxyA, &sweepA, &proxyB, &sweepB, worldNormal, t1 ); + +#if B3_ENABLE_VALIDATION && 0 + // todo this can give a negative value for diagonal edge contact on faces, typical GJK problem + // to fix this I think the separation function would need to identify faces + { + int index1, index2; + float minSeparation = b3FindMinSeparation( &function, &index1, &index2, t1 ); + // SAT should give a closer result than GJK + B3_VALIDATE( minSeparation > target - tolerance && minSeparation - distanceOutput.distance < 0.1f * B3_LINEAR_SLOP ); + } +#endif + + // Compute the TOI on the separating axis. We do this by successively resolving the deepest point. + bool done = false; + float t2 = tMax; + int pushBackIterations = 0; + for ( ;; ) + { + int indexA, indexB; + float s2 = b3FindMinSeparation( &function, &indexA, &indexB, t2 ); + + // Dump the function seen by the root finder + // for ( int Step = 0; Step <= 100; ++Step ) + // { + // float Alpha = 0.01f * Step; + // float Separation = Function.Evaluate( Index1, Index2, Alpha ); + // + // b3Report( "s(%4.2g) = %g\n", Alpha, Separation ); + // } + + // Is the final configuration separated? + if ( s2 - target > tolerance ) + { + // Success! + output.state = b3_toiStateSeparated; + output.fraction = input->maxFraction; + done = true; + break; + } + + // Has the separation reached tolerance? + if ( s2 >= target - tolerance ) + { + // Advance the sweeps + t1 = t2; + break; + } + + // Compute the initial separation of the witness points + float s1 = b3EvaluateSeparation( &function, indexA, indexB, t1 ); + + // Check for overlap. This might happen if the root finder runs out of iterations. + if ( s1 < target - tolerance ) + { + // Failed! + B3_VALIDATE( false ); + output.state = b3_toiStateFailed; + output.fraction = t1; + done = true; + break; + } + + // Has the separation reached tolerance? + if ( s1 <= target + tolerance ) + { + // Success! t1 should hold the TOI (could be 0.0) + output.state = b3_toiStateHit; + output.fraction = t1; + done = true; + break; + } + + // Compute 1D root of: f(x) - target = 0 + int rootIterationCount = 0; + int maxRootIterations = 50; + float a1 = t1; + float a2 = t2; + for ( ;; ) + { + // Use a mix of false position and bisection. + float t; + if ( rootIterationCount & 1 ) + { + // False position to improve convergence. + t = a1 + ( target - s1 ) * ( a2 - a1 ) / ( s2 - s1 ); + } + else + { + // Bisection to guarantee progress. + t = 0.5f * ( a1 + a2 ); + } + + output.rootIterations += 1; + rootIterationCount += 1; + + float s = b3EvaluateSeparation( &function, indexA, indexB, t ); + + // Has the separation reached tolerance? + if ( b3AbsFloat( s - target ) <= tolerance ) + { + // t2 holds a tentative value for t1 + t2 = t; + break; + } + + // Ensure we continue to bracket the root. + if ( s > target ) + { + a1 = t; + s1 = s; + } + else + { + a2 = t; + s2 = s; + } + + if ( rootIterationCount == maxRootIterations ) + { + B3_VALIDATE( false ); + break; + } + } + + // Restart the inner loop if we have a failing edge case. + if ( rootIterationCount == maxRootIterations - 1 && function.type == b3_separationEdges ) + { + B3_VALIDATE( false ); + + rootIterationCount = 0; + t2 = input->maxFraction; + b3ForceFixedAxis( &function, t1 ); + B3_ASSERT( function.type != b3_separationEdges ); + } + + output.pushBackIterations += 1; + pushBackIterations += 1; + + if ( pushBackIterations == maxPushBackIterations ) + { + break; + } + } + + if ( done ) + { + // Averaged hit point + b3Vec3 pA = b3MulAdd( worldPointA, input->proxyA.radius, worldNormal ); + b3Vec3 pB = b3MulAdd( worldPointB, -input->proxyB.radius, worldNormal ); + output.point = b3Lerp( pA, pB, 0.5f ); + output.point = b3Add( output.point, origin ); + output.normal = worldNormal; + break; + } + } + + // It is expected that the state and fraction are set before reaching this + B3_ASSERT( output.state != b3_toiStateUnknown ); + B3_ASSERT( output.fraction >= 0.0f ); + + return output; +} diff --git a/vendor/box3d/src/src/distance_joint.c b/vendor/box3d/src/src/distance_joint.c new file mode 100644 index 000000000..9185010ae --- /dev/null +++ b/vendor/box3d/src/src/distance_joint.c @@ -0,0 +1,579 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +#include "body.h" +#include "core.h" +#include "joint.h" +#include "physics_world.h" +#include "recording.h" +#include "solver.h" +#include "solver_set.h" + +// needed for dll export +#include "box3d/box3d.h" + +void b3DistanceJoint_SetLength( b3JointId jointId, float length ) +{ + b3World* world = b3GetWorld( jointId.world0 ); + B3_REC( world, DistanceJointSetLength, jointId, length ); + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_distanceJoint ); + b3DistanceJoint* joint = &base->distanceJoint; + + joint->length = b3ClampFloat( length, B3_LINEAR_SLOP, B3_HUGE ); + joint->impulse = 0.0f; + joint->lowerImpulse = 0.0f; + joint->upperImpulse = 0.0f; +} + +float b3DistanceJoint_GetLength( b3JointId jointId ) +{ + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_distanceJoint ); + b3DistanceJoint* joint = &base->distanceJoint; + return joint->length; +} + +void b3DistanceJoint_EnableLimit( b3JointId jointId, bool enableLimit ) +{ + b3World* world = b3GetWorld( jointId.world0 ); + B3_REC( world, DistanceJointEnableLimit, jointId, enableLimit ); + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_distanceJoint ); + b3DistanceJoint* joint = &base->distanceJoint; + joint->enableLimit = enableLimit; +} + +bool b3DistanceJoint_IsLimitEnabled( b3JointId jointId ) +{ + b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_distanceJoint ); + return joint->distanceJoint.enableLimit; +} + +void b3DistanceJoint_SetLengthRange( b3JointId jointId, float minLength, float maxLength ) +{ + b3World* world = b3GetWorld( jointId.world0 ); + B3_REC( world, DistanceJointSetLengthRange, jointId, minLength, maxLength ); + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_distanceJoint ); + b3DistanceJoint* joint = &base->distanceJoint; + + minLength = b3ClampFloat( minLength, B3_LINEAR_SLOP, B3_HUGE ); + maxLength = b3ClampFloat( maxLength, B3_LINEAR_SLOP, B3_HUGE ); + joint->minLength = b3MinFloat( minLength, maxLength ); + joint->maxLength = b3MaxFloat( minLength, maxLength ); + joint->impulse = 0.0f; + joint->lowerImpulse = 0.0f; + joint->upperImpulse = 0.0f; +} + +float b3DistanceJoint_GetMinLength( b3JointId jointId ) +{ + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_distanceJoint ); + b3DistanceJoint* joint = &base->distanceJoint; + return joint->minLength; +} + +float b3DistanceJoint_GetMaxLength( b3JointId jointId ) +{ + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_distanceJoint ); + b3DistanceJoint* joint = &base->distanceJoint; + return joint->maxLength; +} + +float b3DistanceJoint_GetCurrentLength( b3JointId jointId ) +{ + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_distanceJoint ); + + b3World* world = b3GetUnlockedWorld( jointId.world0 ); + if ( world == NULL ) + { + return 0.0f; + } + + b3WorldTransform transformA = b3GetBodyTransform( world, base->bodyIdA ); + b3WorldTransform transformB = b3GetBodyTransform( world, base->bodyIdB ); + + b3Pos pA = b3TransformWorldPoint( transformA, base->localFrameA.p ); + b3Pos pB = b3TransformWorldPoint( transformB, base->localFrameB.p ); + b3Vec3 d = b3SubPos( pB, pA ); + float length = b3Length( d ); + return length; +} + +void b3DistanceJoint_EnableSpring( b3JointId jointId, bool enableSpring ) +{ + b3World* world = b3GetWorld( jointId.world0 ); + B3_REC( world, DistanceJointEnableSpring, jointId, enableSpring ); + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_distanceJoint ); + base->distanceJoint.enableSpring = enableSpring; +} + +bool b3DistanceJoint_IsSpringEnabled( b3JointId jointId ) +{ + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_distanceJoint ); + return base->distanceJoint.enableSpring; +} + + +void b3DistanceJoint_SetSpringForceRange( b3JointId jointId, float lowerForce, float upperForce ) +{ + B3_ASSERT( lowerForce <= upperForce ); + b3World* world = b3GetWorld( jointId.world0 ); + B3_REC( world, DistanceJointSetSpringForceRange, jointId, lowerForce, upperForce ); + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_distanceJoint ); + base->distanceJoint.lowerSpringForce = lowerForce; + base->distanceJoint.upperSpringForce = upperForce; +} + +void b3DistanceJoint_GetSpringForceRange( b3JointId jointId, float* lowerForce, float* upperForce ) +{ + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_distanceJoint ); + *lowerForce = base->distanceJoint.lowerSpringForce; + *upperForce = base->distanceJoint.upperSpringForce; +} + +void b3DistanceJoint_SetSpringHertz( b3JointId jointId, float hertz ) +{ + b3World* world = b3GetWorld( jointId.world0 ); + B3_REC( world, DistanceJointSetSpringHertz, jointId, hertz ); + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_distanceJoint ); + base->distanceJoint.hertz = hertz; +} + +void b3DistanceJoint_SetSpringDampingRatio( b3JointId jointId, float dampingRatio ) +{ + b3World* world = b3GetWorld( jointId.world0 ); + B3_REC( world, DistanceJointSetSpringDampingRatio, jointId, dampingRatio ); + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_distanceJoint ); + base->distanceJoint.dampingRatio = dampingRatio; +} + +float b3DistanceJoint_GetSpringHertz( b3JointId jointId ) +{ + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_distanceJoint ); + b3DistanceJoint* joint = &base->distanceJoint; + return joint->hertz; +} + +float b3DistanceJoint_GetSpringDampingRatio( b3JointId jointId ) +{ + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_distanceJoint ); + b3DistanceJoint* joint = &base->distanceJoint; + return joint->dampingRatio; +} + +void b3DistanceJoint_EnableMotor( b3JointId jointId, bool enableMotor ) +{ + b3World* world = b3GetWorld( jointId.world0 ); + B3_REC( world, DistanceJointEnableMotor, jointId, enableMotor ); + b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_distanceJoint ); + if ( enableMotor != joint->distanceJoint.enableMotor ) + { + joint->distanceJoint.enableMotor = enableMotor; + joint->distanceJoint.motorImpulse = 0.0f; + } +} + +bool b3DistanceJoint_IsMotorEnabled( b3JointId jointId ) +{ + b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_distanceJoint ); + return joint->distanceJoint.enableMotor; +} + +void b3DistanceJoint_SetMotorSpeed( b3JointId jointId, float motorSpeed ) +{ + b3World* world = b3GetWorld( jointId.world0 ); + B3_REC( world, DistanceJointSetMotorSpeed, jointId, motorSpeed ); + b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_distanceJoint ); + joint->distanceJoint.motorSpeed = motorSpeed; +} + +float b3DistanceJoint_GetMotorSpeed( b3JointId jointId ) +{ + b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_distanceJoint ); + return joint->distanceJoint.motorSpeed; +} + +float b3DistanceJoint_GetMotorForce( b3JointId jointId ) +{ + b3World* world = b3GetWorld( jointId.world0 ); + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_distanceJoint ); + return world->inv_h * base->distanceJoint.motorImpulse; +} + +void b3DistanceJoint_SetMaxMotorForce( b3JointId jointId, float force ) +{ + b3World* world = b3GetWorld( jointId.world0 ); + B3_REC( world, DistanceJointSetMaxMotorForce, jointId, force ); + b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_distanceJoint ); + joint->distanceJoint.maxMotorForce = force; +} + +float b3DistanceJoint_GetMaxMotorForce( b3JointId jointId ) +{ + b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_distanceJoint ); + return joint->distanceJoint.maxMotorForce; +} + +b3Vec3 b3GetDistanceJointForce( b3World* world, b3JointSim* base ) +{ + b3DistanceJoint* joint = &base->distanceJoint; + + b3WorldTransform transformA = b3GetBodyTransform( world, base->bodyIdA ); + b3WorldTransform transformB = b3GetBodyTransform( world, base->bodyIdB ); + + b3Pos pA = b3TransformWorldPoint( transformA, base->localFrameA.p ); + b3Pos pB = b3TransformWorldPoint( transformB, base->localFrameB.p ); + b3Vec3 d = b3SubPos( pB, pA ); + b3Vec3 axis = b3Normalize( d ); + float force = ( joint->impulse + joint->lowerImpulse - joint->upperImpulse + joint->motorImpulse ) * world->inv_h; + return b3MulSV( force, axis ); +} + +// 1-D constrained system +// m (v2 - v1) = lambda +// v2 + (beta/h) * x1 + gamma * lambda = 0, gamma has units of inverse mass. +// x2 = x1 + h * v2 + +// 1-D mass-damper-spring system +// m (v2 - v1) + h * d * v2 + h * k * + +// C = norm(p2 - p1) - L +// u = (p2 - p1) / norm(p2 - p1) +// Cdot = dot(u, v2 + cross(w2, r2) - v1 - cross(w1, r1)) +// J = [-u -cross(r1, u) u cross(r2, u)] +// K = J * invM * JT +// = invMass1 + invI1 * cross(r1, u)^2 + invMass2 + invI2 * cross(r2, u)^2 + +void b3PrepareDistanceJoint( b3JointSim* base, b3StepContext* context ) +{ + B3_ASSERT( base->type == b3_distanceJoint ); + + // chase body id to the solver set where the body lives + int idA = base->bodyIdA; + int idB = base->bodyIdB; + + b3World* world = context->world; + b3Body* bodyA = b3Array_Get( world->bodies, idA ); + b3Body* bodyB = b3Array_Get( world->bodies, idB ); + + B3_ASSERT( bodyA->setIndex == b3_awakeSet || bodyB->setIndex == b3_awakeSet ); + + b3SolverSet* setA = b3Array_Get( world->solverSets, bodyA->setIndex ); + b3SolverSet* setB = b3Array_Get( world->solverSets, bodyB->setIndex ); + + int localIndexA = bodyA->localIndex; + int localIndexB = bodyB->localIndex; + + b3BodySim* bodySimA = b3Array_Get( setA->bodySims, localIndexA ); + b3BodySim* bodySimB = b3Array_Get( setB->bodySims, localIndexB ); + + float mA = bodySimA->invMass; + b3Matrix3 iA = bodySimA->invInertiaWorld; + float mB = bodySimB->invMass; + b3Matrix3 iB = bodySimB->invInertiaWorld; + + base->invMassA = mA; + base->invMassB = mB; + base->invIA = iA; + base->invIB = iB; + + b3DistanceJoint* joint = &base->distanceJoint; + + joint->indexA = bodyA->setIndex == b3_awakeSet ? localIndexA : B3_NULL_INDEX; + joint->indexB = bodyB->setIndex == b3_awakeSet ? localIndexB : B3_NULL_INDEX; + + // initial anchors in world space + joint->anchorA = b3RotateVector( bodySimA->transform.q, b3Sub( base->localFrameA.p, bodySimA->localCenter ) ); + joint->anchorB = b3RotateVector( bodySimB->transform.q, b3Sub( base->localFrameB.p, bodySimB->localCenter ) ); + joint->deltaCenter = b3SubPos( bodySimB->center, bodySimA->center ); + + b3Vec3 rA = joint->anchorA; + b3Vec3 rB = joint->anchorB; + b3Vec3 separation = b3Add( b3Sub( rB, rA ), joint->deltaCenter ); + b3Vec3 axis = b3Normalize( separation ); + + // compute effective mass + b3Vec3 crA = b3Cross( rA, axis ); + b3Vec3 crB = b3Cross( rB, axis ); + float k = mA + mB + b3Dot( crA, b3MulMV( iA, crA ) ) + b3Dot( crB, b3MulMV( iB, crB ) ); + joint->axialMass = k > 0.0f ? 1.0f / k : 0.0f; + + joint->distanceSoftness = b3MakeSoft( joint->hertz, joint->dampingRatio, context->h ); + + if ( context->enableWarmStarting == false ) + { + joint->impulse = 0.0f; + joint->lowerImpulse = 0.0f; + joint->upperImpulse = 0.0f; + joint->motorImpulse = 0.0f; + } +} + +void b3WarmStartDistanceJoint( b3JointSim* base, b3StepContext* context ) +{ + B3_ASSERT( base->type == b3_distanceJoint ); + + float mA = base->invMassA; + float mB = base->invMassB; + b3Matrix3 iA = base->invIA; + b3Matrix3 iB = base->invIB; + + // dummy state for static bodies + b3BodyState dummyState = b3_identityBodyState; + + b3DistanceJoint* joint = &base->distanceJoint; + b3BodyState* stateA = joint->indexA == B3_NULL_INDEX ? &dummyState : context->states + joint->indexA; + b3BodyState* stateB = joint->indexB == B3_NULL_INDEX ? &dummyState : context->states + joint->indexB; + + b3Vec3 rA = b3RotateVector( stateA->deltaRotation, joint->anchorA ); + b3Vec3 rB = b3RotateVector( stateB->deltaRotation, joint->anchorB ); + + b3Vec3 ds = b3Add( b3Sub( stateB->deltaPosition, stateA->deltaPosition ), b3Sub( rB, rA ) ); + b3Vec3 separation = b3Add( joint->deltaCenter, ds ); + b3Vec3 axis = b3Normalize( separation ); + + float axialImpulse = joint->impulse + joint->lowerImpulse - joint->upperImpulse + joint->motorImpulse; + b3Vec3 P = b3MulSV( axialImpulse, axis ); + + if ( stateA->flags & b3_dynamicFlag ) + { + stateA->linearVelocity = b3MulSub( stateA->linearVelocity, mA, P ); + stateA->angularVelocity = b3Sub( stateA->angularVelocity, b3MulMV( iA, b3Cross( rA, P ) ) ); + } + + if ( stateB->flags & b3_dynamicFlag ) + { + stateB->linearVelocity = b3MulAdd( stateB->linearVelocity, mB, P ); + stateB->angularVelocity = b3Add( stateB->angularVelocity, b3MulMV( iB, b3Cross( rB, P ) ) ); + } +} + +void b3SolveDistanceJoint( b3JointSim* base, b3StepContext* context, bool useBias ) +{ + B3_ASSERT( base->type == b3_distanceJoint ); + + float mA = base->invMassA; + float mB = base->invMassB; + b3Matrix3 iA = base->invIA; + b3Matrix3 iB = base->invIB; + + // dummy state for static bodies + b3BodyState dummyState = b3_identityBodyState; + + b3DistanceJoint* joint = &base->distanceJoint; + b3BodyState* stateA = joint->indexA == B3_NULL_INDEX ? &dummyState : context->states + joint->indexA; + b3BodyState* stateB = joint->indexB == B3_NULL_INDEX ? &dummyState : context->states + joint->indexB; + + b3Vec3 vA = stateA->linearVelocity; + b3Vec3 wA = stateA->angularVelocity; + b3Vec3 vB = stateB->linearVelocity; + b3Vec3 wB = stateB->angularVelocity; + + // current anchors + b3Vec3 rA = b3RotateVector( stateA->deltaRotation, joint->anchorA ); + b3Vec3 rB = b3RotateVector( stateB->deltaRotation, joint->anchorB ); + + // current separation + b3Vec3 ds = b3Add( b3Sub( stateB->deltaPosition, stateA->deltaPosition ), b3Sub( rB, rA ) ); + b3Vec3 separation = b3Add( joint->deltaCenter, ds ); + + float length = b3Length( separation ); + b3Vec3 axis = b3Normalize( separation ); + + // joint is soft if + // - spring is enabled + // - and (joint limit is disabled or limits are not equal) + if ( joint->enableSpring && ( joint->minLength < joint->maxLength || joint->enableLimit == false ) ) + { + // spring + if ( joint->hertz > 0.0f ) + { + // Cdot = dot(u, v + cross(w, r)) + b3Vec3 vr = b3Add( b3Sub( vB, vA ), b3Sub( b3Cross( wB, rB ), b3Cross( wA, rA ) ) ); + float Cdot = b3Dot( axis, vr ); + float C = length - joint->length; + float bias = joint->distanceSoftness.biasRate * C; + + float m = joint->distanceSoftness.massScale * joint->axialMass; + float oldImpulse = joint->impulse; + float impulse = -m * ( Cdot + bias ) - joint->distanceSoftness.impulseScale * oldImpulse; + float h = context->h; + joint->impulse = b3ClampFloat( joint->impulse + impulse, joint->lowerSpringForce * h, joint->upperSpringForce * h ); + impulse = joint->impulse - oldImpulse; + + b3Vec3 P = b3MulSV( impulse, axis ); + vA = b3MulSub( vA, mA, P ); + wA = b3Sub( wA, b3MulMV( iA, b3Cross( rA, P ) ) ); + vB = b3MulAdd( vB, mB, P ); + wB = b3Add( wB, b3MulMV( iB, b3Cross( rB, P ) ) ); + } + + if ( joint->enableLimit ) + { + // lower limit + { + b3Vec3 vr = b3Add( b3Sub( vB, vA ), b3Sub( b3Cross( wB, rB ), b3Cross( wA, rA ) ) ); + float Cdot = b3Dot( axis, vr ); + + float C = length - joint->minLength; + + float bias = 0.0f; + float massCoeff = 1.0f; + float impulseCoeff = 0.0f; + if ( C > 0.0f ) + { + // speculative + bias = C * context->inv_h; + } + else if ( useBias ) + { + bias = base->constraintSoftness.biasRate * C; + massCoeff = base->constraintSoftness.massScale; + impulseCoeff = base->constraintSoftness.impulseScale; + } + + float impulse = -massCoeff * joint->axialMass * ( Cdot + bias ) - impulseCoeff * joint->lowerImpulse; + float newImpulse = b3MaxFloat( 0.0f, joint->lowerImpulse + impulse ); + impulse = newImpulse - joint->lowerImpulse; + joint->lowerImpulse = newImpulse; + + b3Vec3 P = b3MulSV( impulse, axis ); + vA = b3MulSub( vA, mA, P ); + wA = b3Sub( wA, b3MulMV( iA, b3Cross( rA, P ) ) ); + vB = b3MulAdd( vB, mB, P ); + wB = b3Add( wB, b3MulMV( iB, b3Cross( rB, P ) ) ); + } + + // upper + { + b3Vec3 vr = b3Add( b3Sub( vA, vB ), b3Sub( b3Cross( wA, rA ), b3Cross( wB, rB ) ) ); + float Cdot = b3Dot( axis, vr ); + + float C = joint->maxLength - length; + + float bias = 0.0f; + float massScale = 1.0f; + float impulseScale = 0.0f; + if ( C > 0.0f ) + { + // speculative + bias = C * context->inv_h; + } + else if ( useBias ) + { + bias = base->constraintSoftness.biasRate * C; + massScale = base->constraintSoftness.massScale; + impulseScale = base->constraintSoftness.impulseScale; + } + + float impulse = -massScale * joint->axialMass * ( Cdot + bias ) - impulseScale * joint->upperImpulse; + float newImpulse = b3MaxFloat( 0.0f, joint->upperImpulse + impulse ); + impulse = newImpulse - joint->upperImpulse; + joint->upperImpulse = newImpulse; + + b3Vec3 P = b3MulSV( -impulse, axis ); + vA = b3MulSub( vA, mA, P ); + wA = b3Sub( wA, b3MulMV( iA, b3Cross( rA, P ) ) ); + vB = b3MulAdd( vB, mB, P ); + wB = b3Add( wB, b3MulMV( iB, b3Cross( rB, P ) ) ); + } + } + + if ( joint->enableMotor ) + { + b3Vec3 vr = b3Add( b3Sub( vB, vA ), b3Sub( b3Cross( wB, rB ), b3Cross( wA, rA ) ) ); + float Cdot = b3Dot( axis, vr ); + float impulse = joint->axialMass * ( joint->motorSpeed - Cdot ); + float oldImpulse = joint->motorImpulse; + float maxImpulse = context->h * joint->maxMotorForce; + joint->motorImpulse = b3ClampFloat( joint->motorImpulse + impulse, -maxImpulse, maxImpulse ); + impulse = joint->motorImpulse - oldImpulse; + + b3Vec3 P = b3MulSV( impulse, axis ); + vA = b3MulSub( vA, mA, P ); + wA = b3Sub( wA, b3MulMV( iA, b3Cross( rA, P ) ) ); + vB = b3MulAdd( vB, mB, P ); + wB = b3Add( wB, b3MulMV( iB, b3Cross( rB, P ) ) ); + } + } + else + { + // rigid constraint + b3Vec3 vr = b3Add( b3Sub( vB, vA ), b3Sub( b3Cross( wB, rB ), b3Cross( wA, rA ) ) ); + float Cdot = b3Dot( axis, vr ); + + float C = length - joint->length; + + float bias = 0.0f; + float massScale = 1.0f; + float impulseScale = 0.0f; + if ( useBias ) + { + bias = base->constraintSoftness.biasRate * C; + massScale = base->constraintSoftness.massScale; + impulseScale = base->constraintSoftness.impulseScale; + } + + float impulse = -massScale * joint->axialMass * ( Cdot + bias ) - impulseScale * joint->impulse; + joint->impulse += impulse; + + b3Vec3 P = b3MulSV( impulse, axis ); + vA = b3MulSub( vA, mA, P ); + wA = b3Sub( wA, b3MulMV( iA, b3Cross( rA, P ) ) ); + vB = b3MulAdd( vB, mB, P ); + wB = b3Add( wB, b3MulMV( iB, b3Cross( rB, P ) ) ); + } + + if ( stateA->flags & b3_dynamicFlag ) + { + stateA->linearVelocity = vA; + stateA->angularVelocity = wA; + } + + if ( stateB->flags & b3_dynamicFlag ) + { + stateB->linearVelocity = vB; + stateB->angularVelocity = wB; + } +} + +void b3DrawDistanceJoint( b3DebugDraw* draw, b3JointSim* base, b3WorldTransform transformA, b3WorldTransform transformB ) +{ + B3_ASSERT( base->type == b3_distanceJoint ); + + b3DistanceJoint* joint = &base->distanceJoint; + + b3Pos pA = b3TransformWorldPoint( transformA, base->localFrameA.p ); + b3Pos pB = b3TransformWorldPoint( transformB, base->localFrameB.p ); + + b3Vec3 axis = b3Normalize( b3SubPos( pB, pA ) ); + + if ( joint->minLength < joint->maxLength && joint->enableLimit ) + { + b3Pos pMin = b3OffsetPos( pA, b3MulSV( joint->minLength, axis ) ); + b3Pos pMax = b3OffsetPos( pA, b3MulSV( joint->maxLength, axis ) ); + + if ( joint->minLength > B3_LINEAR_SLOP ) + { + draw->DrawPointFcn( pMin, 6.0f, b3_colorLightGreen, draw->context ); + } + + if ( joint->maxLength < B3_HUGE ) + { + draw->DrawPointFcn(pMax, 6.0f, b3_colorRed, draw->context); + } + + if ( joint->minLength > B3_LINEAR_SLOP && joint->maxLength < B3_HUGE ) + { + draw->DrawSegmentFcn( pMin, pMax, b3_colorGray, draw->context ); + } + } + + draw->DrawSegmentFcn( pA, pB, b3_colorWhite, draw->context ); + draw->DrawPointFcn( pA, 4.0f, b3_colorWhite, draw->context ); + draw->DrawPointFcn( pB, 4.0f, b3_colorWhite, draw->context ); + + if ( joint->hertz > 0.0f && joint->enableSpring ) + { + b3Pos pRest = b3OffsetPos( pA, b3MulSV( joint->length, axis ) ); + draw->DrawPointFcn( pRest, 4.0f, b3_colorBlue, draw->context ); + } +} diff --git a/vendor/box3d/src/src/dynamic_tree.c b/vendor/box3d/src/src/dynamic_tree.c new file mode 100644 index 000000000..98370dc05 --- /dev/null +++ b/vendor/box3d/src/src/dynamic_tree.c @@ -0,0 +1,2186 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +#include "aabb.h" +#include "core.h" +#include "joint.h" +#include "simd.h" + +#include "box3d/collision.h" +#include "box3d/constants.h" +#include "box3d/math_functions.h" + +#include +#include +#include + +#define B3_TREE_STACK_SIZE 1024 + +static b3TreeNode b3_defaultTreeNode = { + .aabb = { { 0.0f, 0.0f, 0.0f }, { 0.0f, 0.0f, 0.0f } }, + .categoryBits = B3_DEFAULT_CATEGORY_BITS, + .children = + { + .child1 = B3_NULL_INDEX, + .child2 = B3_NULL_INDEX, + }, + .parent = B3_NULL_INDEX, + .height = 0, + .flags = b3_allocatedNode, +}; + +static inline bool b3IsLeaf( const b3TreeNode* node ) +{ + return node->flags & b3_leafNode; +} + +static inline bool b3IsAllocated( const b3TreeNode* node ) +{ + return node->flags & b3_allocatedNode; +} + +static inline uint16_t b3MaxUInt16( uint16_t a, uint16_t b ) +{ + return a > b ? a : b; +} + +b3DynamicTree b3DynamicTree_Create( int proxyCapacity ) +{ + int capacity = b3MaxInt( proxyCapacity, 16 ); + + b3DynamicTree tree; + + // memset needed for deterministic serialization + memset( &tree, 0, sizeof( b3DynamicTree ) ); + + tree.version = B3_DYNAMIC_TREE_VERSION; + tree.root = B3_NULL_INDEX; + + // maximum node count for a full binary tree is 2 * leafCount - 1 + tree.nodeCapacity = 2 * capacity - 1; + tree.nodeCount = 0; + + tree.nodes = (b3TreeNode*)b3Alloc( tree.nodeCapacity * sizeof( b3TreeNode ) ); + + memset( tree.nodes, 0, tree.nodeCapacity * sizeof( b3TreeNode ) ); + + // Build a linked list for the free list. + // todo use a bump allocator until the capacity is consumed (see b3PoolAllocator) + for ( int i = 0; i < tree.nodeCapacity - 1; ++i ) + { + tree.nodes[i].next = i + 1; + } + + tree.nodes[tree.nodeCapacity - 1].next = B3_NULL_INDEX; + tree.freeList = 0; + + tree.proxyCount = 0; + + tree.leafIndices = NULL; + tree.leafBoxes = NULL; + tree.leafCenters = NULL; + tree.binIndices = NULL; + tree.rebuildCapacity = 0; + + return tree; +} + +void b3DynamicTree_Destroy( b3DynamicTree* tree ) +{ + b3Free( tree->nodes, tree->nodeCapacity * sizeof( b3TreeNode ) ); + b3Free( tree->leafIndices, tree->rebuildCapacity * sizeof( int ) ); + b3Free( tree->leafBoxes, tree->rebuildCapacity * sizeof( b3AABB ) ); + b3Free( tree->leafCenters, tree->rebuildCapacity * sizeof( b3Vec3 ) ); + b3Free( tree->binIndices, tree->rebuildCapacity * sizeof( int ) ); + + memset( tree, 0, sizeof( b3DynamicTree ) ); +} + +// Allocate a node from the pool. Grow the pool if necessary. +static int b3AllocateNode( b3DynamicTree* tree ) +{ + // Expand the node pool as needed. + if ( tree->freeList == B3_NULL_INDEX ) + { + B3_ASSERT( tree->nodeCount == tree->nodeCapacity ); + + // The free list is empty. Rebuild a bigger pool. + b3TreeNode* oldNodes = tree->nodes; + int oldCapacity = tree->nodeCapacity; + tree->nodeCapacity += oldCapacity >> 1; + tree->nodes = (b3TreeNode*)b3Alloc( tree->nodeCapacity * sizeof( b3TreeNode ) ); + B3_ASSERT( oldNodes != NULL ); + memcpy( tree->nodes, oldNodes, tree->nodeCount * sizeof( b3TreeNode ) ); + memset( tree->nodes + tree->nodeCount, 0, ( tree->nodeCapacity - tree->nodeCount ) * sizeof( b3TreeNode ) ); + b3Free( oldNodes, oldCapacity * sizeof( b3TreeNode ) ); + + // Build a linked list for the free list. The parent pointer becomes the "next" pointer. + // todo avoid building freelist? + for ( int i = tree->nodeCount; i < tree->nodeCapacity - 1; ++i ) + { + tree->nodes[i].next = i + 1; + } + + tree->nodes[tree->nodeCapacity - 1].next = B3_NULL_INDEX; + tree->freeList = tree->nodeCount; + } + + // Peel a node off the free list. + int nodeIndex = tree->freeList; + b3TreeNode* node = tree->nodes + nodeIndex; + tree->freeList = node->next; + *node = b3_defaultTreeNode; + ++tree->nodeCount; + return nodeIndex; +} + +// Return a node to the pool. +static void b3FreeNode( b3DynamicTree* tree, int nodeId ) +{ + B3_ASSERT( 0 <= nodeId && nodeId < tree->nodeCapacity ); + B3_ASSERT( 0 < tree->nodeCount ); + tree->nodes[nodeId].next = tree->freeList; + tree->nodes[nodeId].flags = 0; + tree->freeList = nodeId; + --tree->nodeCount; +} + +// Greedy algorithm for sibling selection using the SAH +// We have three nodes A-(B,C) and want to add a leaf D, there are three choices. +// 1: make a new parent for A and D : E-(A-(B,C), D) +// 2: associate D with B +// a: B is a leaf : A-(E-(B,D), C) +// b: B is an internal node: A-(B{D},C) +// 3: associate D with C +// a: C is a leaf : A-(B, E-(C,D)) +// b: C is an internal node: A-(B, C{D}) +// All of these have a clear cost except when B or C is an internal node. Hence we need to be greedy. + +// The cost for cases 1, 2a, and 3a can be computed using the sibling cost formula. +// cost of sibling H = area(union(H, D)) + increased area of ancestors + +// Suppose B (or C) is an internal node, then the lowest cost would be one of two cases: +// case1: D becomes a sibling of B +// case2: D becomes a descendant of B along with a new internal node of area(D). +static int b3FindBestSibling( const b3DynamicTree* tree, b3AABB boxD ) +{ + b3Vec3 centerD = b3AABB_Center( boxD ); + float areaD = b3Perimeter( boxD ); + + const b3TreeNode* nodes = tree->nodes; + int rootIndex = tree->root; + + b3AABB rootBox = nodes[rootIndex].aabb; + + // Area of current node + float areaBase = b3Perimeter( rootBox ); + + // Area of inflated node + float directCost = b3Perimeter( b3AABB_Union( rootBox, boxD ) ); + float inheritedCost = 0.0f; + + int bestSibling = rootIndex; + float bestCost = directCost; + + // Descend the tree from root, following a single greedy path. + int index = rootIndex; + while ( b3IsLeaf( nodes + index ) == false ) + { + int child1 = nodes[index].children.child1; + int child2 = nodes[index].children.child2; + + // Cost of creating a new parent for this node and the new leaf + float cost = directCost + inheritedCost; + + // Sometimes there are multiple identical costs within tolerance. + // This breaks the ties using the centroid distance. + if ( cost < bestCost ) + { + bestSibling = index; + bestCost = cost; + } + + // Inheritance cost seen by children + inheritedCost += directCost - areaBase; + + bool leaf1 = b3IsLeaf( nodes + child1 ); + bool leaf2 = b3IsLeaf( nodes + child2 ); + + // Cost of descending into child 1 + float lowerCost1 = FLT_MAX; + b3AABB box1 = nodes[child1].aabb; + float directCost1 = b3Perimeter( b3AABB_Union( box1, boxD ) ); + float area1 = 0.0f; + if ( leaf1 ) + { + // Child 1 is a leaf + // Cost of creating new node and increasing area of node P + float cost1 = directCost1 + inheritedCost; + + // Need this here due to while condition above + if ( cost1 < bestCost ) + { + bestSibling = child1; + bestCost = cost1; + } + } + else + { + // Child 1 is an internal node + area1 = b3Perimeter( box1 ); + + // Lower bound cost of inserting under child 1. + lowerCost1 = inheritedCost + directCost1 + b3MinFloat( areaD - area1, 0.0f ); + } + + // Cost of descending into child 2 + float lowerCost2 = FLT_MAX; + b3AABB box2 = nodes[child2].aabb; + float directCost2 = b3Perimeter( b3AABB_Union( box2, boxD ) ); + float area2 = 0.0f; + if ( leaf2 ) + { + // Child 2 is a leaf + // Cost of creating new node and increasing area of node P + float cost2 = directCost2 + inheritedCost; + + // Need this here due to while condition above + if ( cost2 < bestCost ) + { + bestSibling = child2; + bestCost = cost2; + } + } + else + { + // Child 2 is an internal node + area2 = b3Perimeter( box2 ); + + // Lower bound cost of inserting under child 2. This is not the cost + // of child 2, it is the best we can hope for under child 2. + lowerCost2 = inheritedCost + directCost2 + b3MinFloat( areaD - area2, 0.0f ); + } + + if ( leaf1 && leaf2 ) + { + break; + } + + // Can the cost possibly be decreased? + if ( bestCost <= lowerCost1 && bestCost <= lowerCost2 ) + { + break; + } + + if ( lowerCost1 == lowerCost2 && leaf1 == false ) + { + B3_ASSERT( lowerCost1 < FLT_MAX ); + B3_ASSERT( lowerCost2 < FLT_MAX ); + + // No clear choice based on lower bound surface area. This can happen when both + // children fully contain D. Fall back to node distance. + b3Vec3 d1 = b3Sub( b3AABB_Center( box1 ), centerD ); + b3Vec3 d2 = b3Sub( b3AABB_Center( box2 ), centerD ); + lowerCost1 = b3LengthSquared( d1 ); + lowerCost2 = b3LengthSquared( d2 ); + } + + // Descend + if ( lowerCost1 < lowerCost2 && leaf1 == false ) + { + index = child1; + areaBase = area1; + directCost = directCost1; + } + else + { + index = child2; + areaBase = area2; + directCost = directCost2; + } + + B3_ASSERT( b3IsLeaf( nodes + index ) == false ); + } + + return bestSibling; +} + +enum b3RotateType +{ + b3_rotateNone, + b3_rotateBF, + b3_rotateBG, + b3_rotateCD, + b3_rotateCE +}; + +// Perform a left or right rotation if node A is imbalanced. +// Returns the new root index. +static void b3RotateNodes( b3DynamicTree* tree, int iA ) +{ + B3_ASSERT( iA != B3_NULL_INDEX ); + + b3TreeNode* nodes = tree->nodes; + + b3TreeNode* A = nodes + iA; + if ( b3IsLeaf( A ) == true ) + { + return; + } + + int iB = A->children.child1; + int iC = A->children.child2; + B3_ASSERT( 0 <= iB && iB < tree->nodeCapacity ); + B3_ASSERT( 0 <= iC && iC < tree->nodeCapacity ); + + b3TreeNode* B = nodes + iB; + b3TreeNode* C = nodes + iC; + + bool isLeafB = b3IsLeaf( B ); + bool isLeafC = b3IsLeaf( C ); + + if ( isLeafB == true && isLeafC == false ) + { + int iF = C->children.child1; + int iG = C->children.child2; + b3TreeNode* F = nodes + iF; + b3TreeNode* G = nodes + iG; + B3_ASSERT( 0 <= iF && iF < tree->nodeCapacity ); + B3_ASSERT( 0 <= iG && iG < tree->nodeCapacity ); + + // Base cost + float costBase = b3Perimeter( C->aabb ); + + // Cost of swapping B and F + b3AABB aabbBG = b3AABB_Union( B->aabb, G->aabb ); + float costBF = b3Perimeter( aabbBG ); + + // Cost of swapping B and G + b3AABB aabbBF = b3AABB_Union( B->aabb, F->aabb ); + float costBG = b3Perimeter( aabbBF ); + + if ( costBase < costBF && costBase < costBG ) + { + // Rotation does not improve cost + return; + } + + if ( costBF < costBG ) + { + // Swap B and F + A->children.child1 = iF; + C->children.child1 = iB; + + B->parent = iC; + F->parent = iA; + + C->aabb = aabbBG; + + C->height = 1 + b3MaxUInt16( B->height, G->height ); + A->height = 1 + b3MaxUInt16( C->height, F->height ); + C->categoryBits = B->categoryBits | G->categoryBits; + A->categoryBits = C->categoryBits | F->categoryBits; + C->flags |= ( B->flags | G->flags ) & b3_enlargedNode; + A->flags |= ( C->flags | F->flags ) & b3_enlargedNode; + } + else + { + // Swap B and G + A->children.child1 = iG; + C->children.child2 = iB; + + B->parent = iC; + G->parent = iA; + + C->aabb = aabbBF; + + C->height = 1 + b3MaxUInt16( B->height, F->height ); + A->height = 1 + b3MaxUInt16( C->height, G->height ); + C->categoryBits = B->categoryBits | F->categoryBits; + A->categoryBits = C->categoryBits | G->categoryBits; + C->flags |= ( B->flags | F->flags ) & b3_enlargedNode; + A->flags |= ( C->flags | G->flags ) & b3_enlargedNode; + } + } + else if ( isLeafC == true && isLeafB == false ) + { + // C is a leaf and B is internal + + int iD = B->children.child1; + int iE = B->children.child2; + b3TreeNode* D = nodes + iD; + b3TreeNode* E = nodes + iE; + B3_ASSERT( 0 <= iD && iD < tree->nodeCapacity ); + B3_ASSERT( 0 <= iE && iE < tree->nodeCapacity ); + + // Base cost + float costBase = b3Perimeter( B->aabb ); + + // Cost of swapping C and D + b3AABB aabbCE = b3AABB_Union( C->aabb, E->aabb ); + float costCD = b3Perimeter( aabbCE ); + + // Cost of swapping C and E + b3AABB aabbCD = b3AABB_Union( C->aabb, D->aabb ); + float costCE = b3Perimeter( aabbCD ); + + if ( costBase < costCD && costBase < costCE ) + { + // Rotation does not improve cost + return; + } + + if ( costCD < costCE ) + { + // Swap C and D + A->children.child2 = iD; + B->children.child1 = iC; + + C->parent = iB; + D->parent = iA; + + B->aabb = aabbCE; + + B->height = 1 + b3MaxUInt16( C->height, E->height ); + A->height = 1 + b3MaxUInt16( B->height, D->height ); + B->categoryBits = C->categoryBits | E->categoryBits; + A->categoryBits = B->categoryBits | D->categoryBits; + B->flags |= ( C->flags | E->flags ) & b3_enlargedNode; + A->flags |= ( B->flags | D->flags ) & b3_enlargedNode; + } + else + { + // Swap C and E + A->children.child2 = iE; + B->children.child2 = iC; + + C->parent = iB; + E->parent = iA; + + B->aabb = aabbCD; + + B->height = 1 + b3MaxUInt16( C->height, D->height ); + A->height = 1 + b3MaxUInt16( B->height, E->height ); + B->categoryBits = C->categoryBits | D->categoryBits; + A->categoryBits = B->categoryBits | E->categoryBits; + B->flags |= ( C->flags | D->flags ) & b3_enlargedNode; + A->flags |= ( B->flags | E->flags ) & b3_enlargedNode; + } + } + else if ( isLeafB == false && isLeafC == false ) + { + // All grand children exist so there are many options for rotation + int iD = B->children.child1; + int iE = B->children.child2; + int iF = C->children.child1; + int iG = C->children.child2; + + B3_ASSERT( 0 <= iD && iD < tree->nodeCapacity ); + B3_ASSERT( 0 <= iE && iE < tree->nodeCapacity ); + B3_ASSERT( 0 <= iF && iF < tree->nodeCapacity ); + B3_ASSERT( 0 <= iG && iG < tree->nodeCapacity ); + + b3TreeNode* D = nodes + iD; + b3TreeNode* E = nodes + iE; + b3TreeNode* F = nodes + iF; + b3TreeNode* G = nodes + iG; + + // Base cost + float areaB = b3Perimeter( B->aabb ); + float areaC = b3Perimeter( C->aabb ); + float costBase = areaB + areaC; + enum b3RotateType bestRotation = b3_rotateNone; + float bestCost = costBase; + + // Cost of swapping B and F + b3AABB aabbBG = b3AABB_Union( B->aabb, G->aabb ); + float costBF = areaB + b3Perimeter( aabbBG ); + if ( costBF < bestCost ) + { + bestRotation = b3_rotateBF; + bestCost = costBF; + } + + // Cost of swapping B and G + b3AABB aabbBF = b3AABB_Union( B->aabb, F->aabb ); + float costBG = areaB + b3Perimeter( aabbBF ); + if ( costBG < bestCost ) + { + bestRotation = b3_rotateBG; + bestCost = costBG; + } + + // Cost of swapping C and D + b3AABB aabbCE = b3AABB_Union( C->aabb, E->aabb ); + float costCD = areaC + b3Perimeter( aabbCE ); + if ( costCD < bestCost ) + { + bestRotation = b3_rotateCD; + bestCost = costCD; + } + + // Cost of swapping C and E + b3AABB aabbCD = b3AABB_Union( C->aabb, D->aabb ); + float costCE = areaC + b3Perimeter( aabbCD ); + if ( costCE < bestCost ) + { + bestRotation = b3_rotateCE; + // bestCost = costCE; + } + + switch ( bestRotation ) + { + case b3_rotateNone: + break; + + case b3_rotateBF: + A->children.child1 = iF; + C->children.child1 = iB; + + B->parent = iC; + F->parent = iA; + + C->aabb = aabbBG; + + C->height = 1 + b3MaxUInt16( B->height, G->height ); + A->height = 1 + b3MaxUInt16( C->height, F->height ); + C->categoryBits = B->categoryBits | G->categoryBits; + A->categoryBits = C->categoryBits | F->categoryBits; + C->flags |= ( B->flags | G->flags ) & b3_enlargedNode; + A->flags |= ( C->flags | F->flags ) & b3_enlargedNode; + break; + + case b3_rotateBG: + A->children.child1 = iG; + C->children.child2 = iB; + + B->parent = iC; + G->parent = iA; + + C->aabb = aabbBF; + + C->height = 1 + b3MaxUInt16( B->height, F->height ); + A->height = 1 + b3MaxUInt16( C->height, G->height ); + C->categoryBits = B->categoryBits | F->categoryBits; + A->categoryBits = C->categoryBits | G->categoryBits; + C->flags |= ( B->flags | F->flags ) & b3_enlargedNode; + A->flags |= ( C->flags | G->flags ) & b3_enlargedNode; + break; + + case b3_rotateCD: + A->children.child2 = iD; + B->children.child1 = iC; + + C->parent = iB; + D->parent = iA; + + B->aabb = aabbCE; + + B->height = 1 + b3MaxUInt16( C->height, E->height ); + A->height = 1 + b3MaxUInt16( B->height, D->height ); + B->categoryBits = C->categoryBits | E->categoryBits; + A->categoryBits = B->categoryBits | D->categoryBits; + B->flags |= ( C->flags | E->flags ) & b3_enlargedNode; + A->flags |= ( B->flags | D->flags ) & b3_enlargedNode; + break; + + case b3_rotateCE: + A->children.child2 = iE; + B->children.child2 = iC; + + C->parent = iB; + E->parent = iA; + + B->aabb = aabbCD; + + B->height = 1 + b3MaxUInt16( C->height, D->height ); + A->height = 1 + b3MaxUInt16( B->height, E->height ); + B->categoryBits = C->categoryBits | D->categoryBits; + A->categoryBits = B->categoryBits | E->categoryBits; + B->flags |= ( C->flags | D->flags ) & b3_enlargedNode; + A->flags |= ( B->flags | E->flags ) & b3_enlargedNode; + break; + + default: + B3_ASSERT( false ); + break; + } + } +} + +// It would be nicer if the root had zero height but maintaining this would drastically increase +// insertion cost because whole sub-trees would need the height to be updated. +static void b3InsertLeaf( b3DynamicTree* tree, int leaf, bool shouldRotate ) +{ + if ( tree->root == B3_NULL_INDEX ) + { + tree->root = leaf; + tree->nodes[tree->root].parent = B3_NULL_INDEX; + return; + } + + // Stage 1: find the best sibling for this node + b3AABB leafAABB = tree->nodes[leaf].aabb; + int sibling = b3FindBestSibling( tree, leafAABB ); + + // Stage 2: create a new parent for the leaf and sibling + int oldParent = tree->nodes[sibling].parent; + int newParent = b3AllocateNode( tree ); + + // warning: node pointer can change after allocation + b3TreeNode* nodes = tree->nodes; + nodes[newParent].parent = oldParent; + nodes[newParent].userData = UINT64_MAX; + nodes[newParent].aabb = b3AABB_Union( leafAABB, nodes[sibling].aabb ); + nodes[newParent].categoryBits = nodes[leaf].categoryBits | nodes[sibling].categoryBits; + nodes[newParent].height = nodes[sibling].height + 1; + + if ( oldParent != B3_NULL_INDEX ) + { + // The sibling was not the root. + if ( nodes[oldParent].children.child1 == sibling ) + { + nodes[oldParent].children.child1 = newParent; + } + else + { + nodes[oldParent].children.child2 = newParent; + } + + nodes[newParent].children.child1 = sibling; + nodes[newParent].children.child2 = leaf; + nodes[sibling].parent = newParent; + nodes[leaf].parent = newParent; + } + else + { + // The sibling was the root. + nodes[newParent].children.child1 = sibling; + nodes[newParent].children.child2 = leaf; + nodes[sibling].parent = newParent; + nodes[leaf].parent = newParent; + tree->root = newParent; + } + + // Stage 3: walk back up the tree fixing heights and AABBs + int index = nodes[leaf].parent; + while ( index != B3_NULL_INDEX ) + { + int child1 = nodes[index].children.child1; + int child2 = nodes[index].children.child2; + + B3_ASSERT( child1 != B3_NULL_INDEX ); + B3_ASSERT( child2 != B3_NULL_INDEX ); + + nodes[index].aabb = b3AABB_Union( nodes[child1].aabb, nodes[child2].aabb ); + nodes[index].categoryBits = nodes[child1].categoryBits | nodes[child2].categoryBits; + nodes[index].height = 1 + b3MaxUInt16( nodes[child1].height, nodes[child2].height ); + nodes[index].flags |= ( nodes[child1].flags | nodes[child2].flags ) & b3_enlargedNode; + + if ( shouldRotate ) + { + b3RotateNodes( tree, index ); + } + + index = nodes[index].parent; + } +} + +static void b3RemoveLeaf( b3DynamicTree* tree, int leaf ) +{ + if ( leaf == tree->root ) + { + tree->root = B3_NULL_INDEX; + return; + } + + b3TreeNode* nodes = tree->nodes; + + int parent = nodes[leaf].parent; + int grandParent = nodes[parent].parent; + int sibling; + if ( nodes[parent].children.child1 == leaf ) + { + sibling = nodes[parent].children.child2; + } + else + { + sibling = nodes[parent].children.child1; + } + + if ( grandParent != B3_NULL_INDEX ) + { + // Destroy parent and connect sibling to grandParent. + if ( nodes[grandParent].children.child1 == parent ) + { + nodes[grandParent].children.child1 = sibling; + } + else + { + nodes[grandParent].children.child2 = sibling; + } + nodes[sibling].parent = grandParent; + b3FreeNode( tree, parent ); + + // Adjust ancestor bounds. + int index = grandParent; + while ( index != B3_NULL_INDEX ) + { + b3TreeNode* node = nodes + index; + b3TreeNode* child1 = nodes + node->children.child1; + b3TreeNode* child2 = nodes + node->children.child2; + + // Fast union using SSE + //__m128 aabb1 = _mm_load_ps(&child1->aabb.lowerBound.x); + //__m128 aabb2 = _mm_load_ps(&child2->aabb.lowerBound.x); + //__m128 lower = _mm_min_ps(aabb1, aabb2); + //__m128 upper = _mm_max_ps(aabb1, aabb2); + //__m128 aabb = _mm_shuffle_ps(lower, upper, _MM_SHUFFLE(3, 2, 1, 0)); + //_mm_store_ps(&node->aabb.lowerBound.x, aabb); + + node->aabb = b3AABB_Union( child1->aabb, child2->aabb ); + node->categoryBits = child1->categoryBits | child2->categoryBits; + node->height = 1 + b3MaxUInt16( child1->height, child2->height ); + + index = node->parent; + } + } + else + { + tree->root = sibling; + tree->nodes[sibling].parent = B3_NULL_INDEX; + b3FreeNode( tree, parent ); + } +} + +// Create a proxy in the tree as a leaf node. We return the index of the node instead of a pointer so that we can grow +// the node pool. +int b3DynamicTree_CreateProxy( b3DynamicTree* tree, b3AABB aabb, uint64_t categoryBits, uint64_t userData ) +{ + B3_ASSERT( b3IsValidAABB( aabb ) ); + + int proxyId = b3AllocateNode( tree ); + b3TreeNode* node = tree->nodes + proxyId; + + node->aabb = aabb; + node->userData = userData; + node->categoryBits = categoryBits; + node->height = 0; + node->flags = b3_allocatedNode | b3_leafNode; + + bool shouldRotate = true; + b3InsertLeaf( tree, proxyId, shouldRotate ); + + tree->proxyCount += 1; + + return proxyId; +} + +void b3DynamicTree_DestroyProxy( b3DynamicTree* tree, int proxyId ) +{ + B3_ASSERT( 0 <= proxyId && proxyId < tree->nodeCapacity ); + B3_ASSERT( b3IsLeaf( tree->nodes + proxyId ) ); + + b3RemoveLeaf( tree, proxyId ); + b3FreeNode( tree, proxyId ); + + B3_ASSERT( tree->proxyCount > 0 ); + tree->proxyCount -= 1; +} + +int b3DynamicTree_GetProxyCount( const b3DynamicTree* tree ) +{ + return tree->proxyCount; +} + +void b3DynamicTree_MoveProxy( b3DynamicTree* tree, int proxyId, b3AABB aabb ) +{ + B3_ASSERT( b3IsValidAABB( aabb ) ); + B3_ASSERT( 0 <= proxyId && proxyId < tree->nodeCapacity ); + B3_ASSERT( b3IsLeaf( tree->nodes + proxyId ) ); + + b3RemoveLeaf( tree, proxyId ); + + tree->nodes[proxyId].aabb = aabb; + + bool shouldRotate = false; + b3InsertLeaf( tree, proxyId, shouldRotate ); +} + +void b3DynamicTree_EnlargeProxy( b3DynamicTree* tree, int proxyId, b3AABB aabb ) +{ + b3TreeNode* nodes = tree->nodes; + B3_VALIDATE( b3IsValidAABB( aabb ) ); + B3_ASSERT( 0 <= proxyId && proxyId < tree->nodeCapacity ); + B3_VALIDATE( b3IsLeaf( tree->nodes + proxyId ) ); + + // Caller must ensure this + B3_VALIDATE( b3AABB_Contains( nodes[proxyId].aabb, aabb ) == false ); + + b3TreeNode* node = nodes + proxyId; + node->aabb = aabb; + + int parentIndex = node->parent; + while ( parentIndex != B3_NULL_INDEX ) + { + node = nodes + parentIndex; + bool changed = b3EnlargeAABB( &node->aabb, aabb ); + + // todo not sure why this node is marked as enlarged even if it didn't change + node->flags |= b3_enlargedNode; + + parentIndex = node->parent; + + if ( changed == false ) + { + break; + } + } + + while ( parentIndex != B3_NULL_INDEX ) + { + node = nodes + parentIndex; + if ( node->flags & b3_enlargedNode ) + { + // early out because this ancestor was previously ascended and marked as enlarged + break; + } + + node->flags |= b3_enlargedNode; + parentIndex = node->parent; + } +} + +void b3DynamicTree_SetCategoryBits( b3DynamicTree* tree, int proxyId, uint64_t categoryBits ) +{ + b3TreeNode* nodes = tree->nodes; + + B3_ASSERT( b3IsLeaf( nodes + proxyId ) ); + + nodes[proxyId].categoryBits = categoryBits; + + // Fix up category bits in ancestor internal nodes + int nodeIndex = nodes[proxyId].parent; + while ( nodeIndex != B3_NULL_INDEX ) + { + b3TreeNode* node = nodes + nodeIndex; + int child1 = node->children.child1; + B3_ASSERT( child1 != B3_NULL_INDEX ); + int child2 = node->children.child2; + B3_ASSERT( child2 != B3_NULL_INDEX ); + node->categoryBits = nodes[child1].categoryBits | nodes[child2].categoryBits; + + nodeIndex = node->parent; + } +} + +uint64_t b3DynamicTree_GetCategoryBits( b3DynamicTree* tree, int proxyId ) +{ + B3_ASSERT( 0 <= proxyId && proxyId < tree->nodeCapacity ); + return tree->nodes[proxyId].categoryBits; +} + +int b3DynamicTree_GetHeight( const b3DynamicTree* tree ) +{ + if ( tree->root == B3_NULL_INDEX ) + { + return 0; + } + + return tree->nodes[tree->root].height; +} + +float b3DynamicTree_GetAreaRatio( const b3DynamicTree* tree ) +{ + if ( tree->root == B3_NULL_INDEX ) + { + return 0.0f; + } + + const b3TreeNode* root = tree->nodes + tree->root; + float rootArea = b3Perimeter( root->aabb ); + + float totalArea = 0.0f; + for ( int i = 0; i < tree->nodeCapacity; ++i ) + { + const b3TreeNode* node = tree->nodes + i; + if ( b3IsAllocated( node ) == false || b3IsLeaf( node ) || i == tree->root ) + { + continue; + } + + totalArea += b3Perimeter( node->aabb ); + } + + return totalArea / rootArea; +} + +b3AABB b3DynamicTree_GetRootBounds( const b3DynamicTree* tree ) +{ + if ( tree->root != B3_NULL_INDEX ) + { + return tree->nodes[tree->root].aabb; + } + + b3AABB empty = { b3Vec3_zero, b3Vec3_zero }; + return empty; +} + +#if B3_ENABLE_VALIDATION +// Compute the height of a sub-tree. +static int b3ComputeHeightRecurse( const b3DynamicTree* tree, int nodeId ) +{ + B3_ASSERT( 0 <= nodeId && nodeId < tree->nodeCapacity ); + b3TreeNode* node = tree->nodes + nodeId; + + if ( b3IsLeaf( node ) ) + { + return 0; + } + + int height1 = b3ComputeHeightRecurse( tree, node->children.child1 ); + int height2 = b3ComputeHeightRecurse( tree, node->children.child2 ); + return 1 + b3MaxInt( height1, height2 ); +} + +static int b3ComputeHeight( const b3DynamicTree* tree ) +{ + int height = b3ComputeHeightRecurse( tree, tree->root ); + return height; +} + +static void b3ValidateStructure( const b3DynamicTree* tree, int index ) +{ + if ( index == B3_NULL_INDEX ) + { + return; + } + + if ( index == tree->root ) + { + B3_ASSERT( tree->nodes[index].parent == B3_NULL_INDEX ); + } + + const b3TreeNode* node = tree->nodes + index; + + B3_ASSERT( node->flags == 0 || ( node->flags & b3_allocatedNode ) != 0 ); + + if ( b3IsLeaf( node ) ) + { + B3_ASSERT( node->height == 0 ); + return; + } + + int child1 = node->children.child1; + int child2 = node->children.child2; + + B3_ASSERT( 0 <= child1 && child1 < tree->nodeCapacity ); + B3_ASSERT( 0 <= child2 && child2 < tree->nodeCapacity ); + + B3_ASSERT( tree->nodes[child1].parent == index ); + B3_ASSERT( tree->nodes[child2].parent == index ); + + if ( ( tree->nodes[child1].flags | tree->nodes[child2].flags ) & b3_enlargedNode ) + { + B3_ASSERT( node->flags & b3_enlargedNode ); + } + + b3ValidateStructure( tree, child1 ); + b3ValidateStructure( tree, child2 ); +} + +static void b3ValidateMetrics( const b3DynamicTree* tree, int index ) +{ + if ( index == B3_NULL_INDEX ) + { + return; + } + + const b3TreeNode* node = tree->nodes + index; + + B3_VALIDATE( b3IsValidAABB( node->aabb ) ); + + if ( b3IsLeaf( node ) ) + { + B3_ASSERT( node->height == 0 ); + return; + } + + int child1 = node->children.child1; + int child2 = node->children.child2; + + B3_ASSERT( 0 <= child1 && child1 < tree->nodeCapacity ); + B3_ASSERT( 0 <= child2 && child2 < tree->nodeCapacity ); + + int height1 = tree->nodes[child1].height; + int height2 = tree->nodes[child2].height; + int height = 1 + b3MaxInt( height1, height2 ); + B3_ASSERT( node->height == height ); + + // b3AABB aabb = b3AABB_Union(tree->nodes[child1].aabb, tree->nodes[child2].aabb); + + B3_ASSERT( b3AABB_Contains( node->aabb, tree->nodes[child1].aabb ) ); + B3_ASSERT( b3AABB_Contains( node->aabb, tree->nodes[child2].aabb ) ); + + // B3_ASSERT(aabb.lowerBound.x == node->aabb.lowerBound.x); + // B3_ASSERT(aabb.lowerBound.y == node->aabb.lowerBound.y); + // B3_ASSERT(aabb.upperBound.x == node->aabb.upperBound.x); + // B3_ASSERT(aabb.upperBound.y == node->aabb.upperBound.y); + + uint64_t categoryBits = tree->nodes[child1].categoryBits | tree->nodes[child2].categoryBits; + B3_ASSERT( node->categoryBits == categoryBits ); + + b3ValidateMetrics( tree, child1 ); + b3ValidateMetrics( tree, child2 ); +} +#endif + +void b3DynamicTree_Validate( const b3DynamicTree* tree ) +{ +#if B3_ENABLE_VALIDATION + if ( tree->root == B3_NULL_INDEX ) + { + return; + } + + b3ValidateStructure( tree, tree->root ); + b3ValidateMetrics( tree, tree->root ); + + int freeCount = 0; + int freeIndex = tree->freeList; + while ( freeIndex != B3_NULL_INDEX ) + { + B3_ASSERT( 0 <= freeIndex && freeIndex < tree->nodeCapacity ); + freeIndex = tree->nodes[freeIndex].next; + ++freeCount; + } + + int height = b3DynamicTree_GetHeight( tree ); + int computedHeight = b3ComputeHeight( tree ); + B3_ASSERT( height == computedHeight ); + + B3_ASSERT( tree->nodeCount + freeCount == tree->nodeCapacity ); +#else + B3_UNUSED( tree ); +#endif +} + +void b3DynamicTree_ValidateNoEnlarged( const b3DynamicTree* tree ) +{ +#if B3_ENABLE_VALIDATION == 1 + int capacity = tree->nodeCapacity; + const b3TreeNode* nodes = tree->nodes; + for ( int i = 0; i < capacity; ++i ) + { + const b3TreeNode* node = nodes + i; + if ( node->flags & b3_allocatedNode ) + { + B3_ASSERT( ( node->flags & b3_enlargedNode ) == 0 ); + } + } +#else + B3_UNUSED( tree ); +#endif +} + +int b3DynamicTree_GetByteCount( const b3DynamicTree* tree ) +{ + size_t size = sizeof( b3DynamicTree ) + sizeof( b3TreeNode ) * tree->nodeCapacity + + tree->rebuildCapacity * ( sizeof( int ) + sizeof( b3AABB ) + sizeof( b3Vec3 ) + sizeof( int ) ); + + return (int)size; +} + +b3TreeStats b3DynamicTree_Query( const b3DynamicTree* tree, b3AABB aabb, uint64_t maskBits, bool requireAllBits, + b3TreeQueryCallbackFcn* callback, void* context ) +{ + b3TreeStats result = { 0 }; + + if ( tree->nodeCount == 0 ) + { + return result; + } + + int stack[B3_TREE_STACK_SIZE]; + int stackCount = 0; + stack[stackCount++] = tree->root; + + while ( stackCount > 0 ) + { + int nodeId = stack[--stackCount]; + if ( nodeId == B3_NULL_INDEX ) + { + // todo huh? + B3_ASSERT( false ); + continue; + } + + const b3TreeNode* node = tree->nodes + nodeId; + result.nodeVisits += 1; + + // Assuming branch prediction deals with requireAllBits well + uint64_t bitMatch = requireAllBits ? ( node->categoryBits & maskBits ) == maskBits : ( node->categoryBits & maskBits ); + + if ( bitMatch && b3AABB_Overlaps( node->aabb, aabb ) ) + { + if ( b3IsLeaf( node ) ) + { + // callback to user code with proxy id + bool proceed = callback( nodeId, node->userData, context ); + result.leafVisits += 1; + + if ( proceed == false ) + { + return result; + } + } + else + { + B3_ASSERT( stackCount < B3_TREE_STACK_SIZE - 1 ); + if ( stackCount < B3_TREE_STACK_SIZE - 1 ) + { + stack[stackCount++] = node->children.child1; + stack[stackCount++] = node->children.child2; + } + } + } + } + + return result; +} + +B3_FORCE_INLINE float b3DistanceToNodeSqr( b3Vec3 point, const b3TreeNode* node ) +{ + b3Vec3 r = b3Sub( point, b3Clamp( point, node->aabb.lowerBound, node->aabb.upperBound ) ); + return b3Dot( r, r ); +} + +struct b3QueryClosestItem +{ + int nodeIndex; + float distanceToNodeSqr; +}; + +b3TreeStats b3DynamicTree_QueryClosest( const b3DynamicTree* tree, b3Vec3 point, uint64_t maskBits, bool requireAllBits, + b3TreeQueryClosestCallbackFcn* callback, void* context, float* minDistanceSqr ) +{ + b3TreeStats result = { 0 }; + + if ( tree->nodeCount == 0 ) + { + return result; + } + + float minSqr = *minDistanceSqr; + struct b3QueryClosestItem stack[B3_TREE_STACK_SIZE]; + int stackCount = 0; + + float rootDistanceSqr = b3DistanceToNodeSqr( point, tree->nodes + tree->root ); + stack[stackCount++] = (struct b3QueryClosestItem){ + .nodeIndex = tree->root, + .distanceToNodeSqr = rootDistanceSqr, + }; + + while ( stackCount > 0 ) + { + struct b3QueryClosestItem item = stack[--stackCount]; + const b3TreeNode* node = tree->nodes + item.nodeIndex; + result.nodeVisits += 1; + + uint64_t bitMatch = requireAllBits ? ( node->categoryBits & maskBits ) == maskBits : ( node->categoryBits & maskBits ); + + if ( bitMatch ) + { + if ( item.distanceToNodeSqr < minSqr ) + { + if ( b3IsLeaf( node ) ) + { + // callback to user code with minimum distance squared so far and proxy id + float dd = callback( minSqr, item.nodeIndex, node->userData, context ); + + if ( dd < minSqr ) + { + minSqr = dd; + } + + result.leafVisits += 1; + } + else + { + B3_ASSERT( stackCount < B3_TREE_STACK_SIZE - 1 ); + if ( stackCount < B3_TREE_STACK_SIZE - 1 ) + { + int child1 = node->children.child1; + int child2 = node->children.child2; + + // Store the distance to node in the stack instead of recomputing after pop + struct b3QueryClosestItem item1 = { + .nodeIndex = child1, + .distanceToNodeSqr = b3DistanceToNodeSqr( point, tree->nodes + child1 ), + }; + + struct b3QueryClosestItem item2 = { + .nodeIndex = child2, + .distanceToNodeSqr = b3DistanceToNodeSqr( point, tree->nodes + child2 ), + }; + + // Ensure we iterate the closest child first as we pop off the stack + if ( item2.distanceToNodeSqr < item1.distanceToNodeSqr ) + { + stack[stackCount++] = item1; + stack[stackCount++] = item2; + } + else + { + stack[stackCount++] = item2; + stack[stackCount++] = item1; + } + } + } + } + } + } + + *minDistanceSqr = minSqr; + + return result; +} + +b3TreeStats b3DynamicTree_RayCast( const b3DynamicTree* tree, const b3RayCastInput* input, uint64_t maskBits, bool requireAllBits, + b3TreeRayCastCallbackFcn* callback, void* context ) +{ + b3TreeStats result = { 0 }; + + if ( tree->nodeCount == 0 ) + { + return result; + } + + b3Vec3 p1 = input->origin; + b3Vec3 d = input->translation; + + b3V32 pv1 = b3LoadV( &p1.x ); + b3V32 dv = b3LoadV( &d.x ); + + float maxFraction = input->maxFraction; + + b3Vec3 p2 = b3MulAdd( p1, maxFraction, d ); + + // Build a bounding box for the segment. + b3AABB segmentAABB = { b3Min( p1, p2 ), b3Max( p1, p2 ) }; + + int stack[B3_TREE_STACK_SIZE]; + int stackCount = 0; + stack[stackCount++] = tree->root; + + const b3TreeNode* nodes = tree->nodes; + + b3RayCastInput subInput = *input; + + while ( stackCount > 0 ) + { + int nodeId = stack[--stackCount]; + if ( nodeId == B3_NULL_INDEX ) + { + // todo is this possible? + B3_ASSERT( false ); + continue; + } + + const b3TreeNode* node = nodes + nodeId; + result.nodeVisits += 1; + + b3AABB nodeAABB = node->aabb; + + // todo look at disassembly + uint64_t bitMatch = requireAllBits ? ( node->categoryBits & maskBits ) == maskBits : ( node->categoryBits & maskBits ); + + if ( bitMatch == 0 || b3AABB_Overlaps( nodeAABB, segmentAABB ) == false ) + { + continue; + } + + b3V32 lower = b3LoadV( &nodeAABB.lowerBound.x ); + b3V32 upper = b3LoadV( &nodeAABB.upperBound.x ); + + bool edgeOverlap = b3TestBoundsRayOverlap( lower, upper, pv1, dv ); + if ( edgeOverlap == false ) + { + continue; + } + + if ( b3IsLeaf( node ) ) + { + subInput.maxFraction = maxFraction; + + float value = callback( &subInput, nodeId, node->userData, context ); + result.leafVisits += 1; + + // The user may return -1 to indicate this shape should be skipped + + if ( value == 0.0f ) + { + // The client has terminated the ray cast. + return result; + } + + if ( 0.0f < value && value <= maxFraction ) + { + // Update segment bounding box. + maxFraction = value; + p2 = b3MulAdd( p1, maxFraction, d ); + segmentAABB.lowerBound = b3Min( p1, p2 ); + segmentAABB.upperBound = b3Max( p1, p2 ); + } + } + else + { + B3_ASSERT( stackCount < B3_TREE_STACK_SIZE - 1 ); + if ( stackCount < B3_TREE_STACK_SIZE - 1 ) + { + b3Vec3 c1 = b3AABB_Center( nodes[node->children.child1].aabb ); + b3Vec3 c2 = b3AABB_Center( nodes[node->children.child2].aabb ); + if ( b3DistanceSquared( c1, p1 ) < b3DistanceSquared( c2, p1 ) ) + { + stack[stackCount++] = node->children.child2; + stack[stackCount++] = node->children.child1; + } + else + { + stack[stackCount++] = node->children.child1; + stack[stackCount++] = node->children.child2; + } + } + } + } + + return result; +} + +b3TreeStats b3DynamicTree_BoxCast( const b3DynamicTree* tree, const b3BoxCastInput* input, uint64_t maskBits, bool requireAllBits, + b3TreeBoxCastCallbackFcn* callback, void* context ) +{ + b3TreeStats stats = { 0 }; + + if ( tree->nodeCount == 0 ) + { + return stats; + } + + // The caller folds the shape radius and the world origin into the box + b3AABB originAABB = input->box; + + b3Vec3 p1 = b3AABB_Center( originAABB ); + b3Vec3 extension = b3AABB_Extents( originAABB ); + + b3Vec3 d = input->translation; + + b3V32 pv1 = b3LoadV( &p1.x ); + b3V32 dv = b3LoadV( &d.x ); + b3V32 ev = b3LoadV( &extension.x ); + + float maxFraction = input->maxFraction; + + // Build total box for the cast + b3Vec3 t = b3MulSV( maxFraction, input->translation ); + b3AABB totalAABB = { + b3Min( originAABB.lowerBound, b3Add( originAABB.lowerBound, t ) ), + b3Max( originAABB.upperBound, b3Add( originAABB.upperBound, t ) ), + }; + + b3BoxCastInput subInput = *input; + const b3TreeNode* nodes = tree->nodes; + + int stack[B3_TREE_STACK_SIZE]; + int stackCount = 0; + stack[stackCount++] = tree->root; + + while ( stackCount > 0 ) + { + int nodeId = stack[--stackCount]; + if ( nodeId == B3_NULL_INDEX ) + { + B3_ASSERT( false ); + continue; + } + + const b3TreeNode* node = nodes + nodeId; + stats.nodeVisits += 1; + + uint64_t bitMatch = requireAllBits ? ( node->categoryBits & maskBits ) == maskBits : ( node->categoryBits & maskBits ); + + if ( bitMatch == 0 || b3AABB_Overlaps( node->aabb, totalAABB ) == false ) + { + continue; + } + + // radius extension is added to the node in this case + b3V32 lower = b3SubV( b3LoadV( &node->aabb.lowerBound.x ), ev ); + b3V32 upper = b3AddV( b3LoadV( &node->aabb.upperBound.x ), ev ); + bool edgeOverlap = b3TestBoundsRayOverlap( lower, upper, pv1, dv ); + if ( edgeOverlap == false ) + { + continue; + } + + if ( b3IsLeaf( node ) ) + { + subInput.maxFraction = maxFraction; + + float value = callback( &subInput, nodeId, node->userData, context ); + stats.leafVisits += 1; + + if ( value == 0.0f ) + { + // The client has terminated the cast. + return stats; + } + + if ( 0.0f < value && value < maxFraction ) + { + maxFraction = value; + t = b3MulSV( maxFraction, input->translation ); + totalAABB.lowerBound = b3Min( originAABB.lowerBound, b3Add( originAABB.lowerBound, t ) ); + totalAABB.upperBound = b3Max( originAABB.upperBound, b3Add( originAABB.upperBound, t ) ); + } + } + else + { + B3_ASSERT( stackCount < B3_TREE_STACK_SIZE - 1 ); + if ( stackCount < B3_TREE_STACK_SIZE - 1 ) + { + b3Vec3 c1 = b3AABB_Center( nodes[node->children.child1].aabb ); + b3Vec3 c2 = b3AABB_Center( nodes[node->children.child2].aabb ); + if ( b3DistanceSquared( c1, p1 ) < b3DistanceSquared( c2, p1 ) ) + { + stack[stackCount++] = node->children.child2; + stack[stackCount++] = node->children.child1; + } + else + { + stack[stackCount++] = node->children.child1; + stack[stackCount++] = node->children.child2; + } + } + } + } + + return stats; +} + +// Median split == 0, Surface area heuristic == 1 +#define B3_TREE_HEURISTIC 0 + +#if B3_TREE_HEURISTIC == 0 + +// Median split heuristic +static int b3PartitionMid( int* indices, b3Vec3* centers, int count ) +{ + // Handle trivial case + if ( count <= 2 ) + { + return count / 2; + } + + b3Vec3 lowerBound = centers[0]; + b3Vec3 upperBound = centers[0]; + + for ( int i = 1; i < count; ++i ) + { + lowerBound = b3Min( lowerBound, centers[i] ); + upperBound = b3Max( upperBound, centers[i] ); + } + + b3Vec3 d = b3Sub( upperBound, lowerBound ); + b3Vec3 c = b3MulSV( 0.5f, b3Add( lowerBound, upperBound ) ); + + // Partition longest axis using the Hoare partition scheme + // https://en.wikipedia.org/wiki/Quicksort + // https://nicholasvadivelu.com/2021/01/11/array-partition/ + int i1 = 0, i2 = count; + if ( d.x >= d.y && d.x >= d.z ) + { + float pivot = c.x; + + while ( i1 < i2 ) + { + while ( i1 < i2 && centers[i1].x < pivot ) + { + i1 += 1; + }; + + while ( i1 < i2 && centers[i2 - 1].x >= pivot ) + { + i2 -= 1; + }; + + if ( i1 < i2 ) + { + // Swap indices + { + int temp = indices[i1]; + indices[i1] = indices[i2 - 1]; + indices[i2 - 1] = temp; + } + + // Swap centers + { + b3Vec3 temp = centers[i1]; + centers[i1] = centers[i2 - 1]; + centers[i2 - 1] = temp; + } + + i1 += 1; + i2 -= 1; + } + } + } + else if ( d.y >= d.z ) + { + float pivot = c.y; + + while ( i1 < i2 ) + { + while ( i1 < i2 && centers[i1].y < pivot ) + { + i1 += 1; + }; + + while ( i1 < i2 && centers[i2 - 1].y >= pivot ) + { + i2 -= 1; + }; + + if ( i1 < i2 ) + { + // Swap indices + { + int temp = indices[i1]; + indices[i1] = indices[i2 - 1]; + indices[i2 - 1] = temp; + } + + // Swap centers + { + b3Vec3 temp = centers[i1]; + centers[i1] = centers[i2 - 1]; + centers[i2 - 1] = temp; + } + + i1 += 1; + i2 -= 1; + } + } + } + else + { + float pivot = c.z; + + while ( i1 < i2 ) + { + while ( i1 < i2 && centers[i1].z < pivot ) + { + i1 += 1; + }; + + while ( i1 < i2 && centers[i2 - 1].z >= pivot ) + { + i2 -= 1; + }; + + if ( i1 < i2 ) + { + // Swap indices + { + int temp = indices[i1]; + indices[i1] = indices[i2 - 1]; + indices[i2 - 1] = temp; + } + + // Swap centers + { + b3Vec3 temp = centers[i1]; + centers[i1] = centers[i2 - 1]; + centers[i2 - 1] = temp; + } + + i1 += 1; + i2 -= 1; + } + } + } + B3_ASSERT( i1 == i2 ); + + if ( i1 > 0 && i1 < count ) + { + return i1; + } + + return count / 2; +} + +#else + +#define B3_BIN_COUNT 8 + +typedef struct b3TreeBin +{ + b3AABB aabb; + int count; +} b3TreeBin; + +typedef struct b3TreePlane +{ + b3AABB leftAABB; + b3AABB rightAABB; + int leftCount; + int rightCount; +} b3TreePlane; + +// "On Fast Construction of SAH-based Bounding Volume Hierarchies" by Ingo Wald +// Returns the left child count +static int b3PartitionSAH( int* indices, int* binIndices, b3AABB* boxes, int count ) +{ + B3_ASSERT( count > 0 ); + + b3TreeBin bins[B3_BIN_COUNT]; + b3TreePlane planes[B3_BIN_COUNT - 1]; + + b3Vec3 center = b3AABB_Center( boxes[0] ); + b3AABB centroidAABB; + centroidAABB.lowerBound = center; + centroidAABB.upperBound = center; + + for ( int i = 1; i < count; ++i ) + { + center = b3AABB_Center( boxes[i] ); + centroidAABB.lowerBound = b3Min( centroidAABB.lowerBound, center ); + centroidAABB.upperBound = b3Max( centroidAABB.upperBound, center ); + } + + b3Vec3 d = b3Sub( centroidAABB.upperBound, centroidAABB.lowerBound ); + + // Find longest axis + int axisIndex; + float invD; + if ( d.x > d.y ) + { + axisIndex = 0; + invD = d.x; + } + else + { + axisIndex = 1; + invD = d.y; + } + + invD = invD > 0.0f ? 1.0f / invD : 0.0f; + + // Initialize bin bounds and count + for ( int i = 0; i < B3_BIN_COUNT; ++i ) + { + bins[i].aabb.lowerBound = (b3Vec3){ FLT_MAX, FLT_MAX }; + bins[i].aabb.upperBound = (b3Vec3){ -FLT_MAX, -FLT_MAX }; + bins[i].count = 0; + } + + // Assign boxes to bins and compute bin boxes + // TODO_ERIN optimize + float binCount = B3_BIN_COUNT; + float lowerBoundArray[2] = { centroidAABB.lowerBound.x, centroidAABB.lowerBound.y }; + float minC = lowerBoundArray[axisIndex]; + for ( int i = 0; i < count; ++i ) + { + b3Vec3 c = b3AABB_Center( boxes[i] ); + float cArray[2] = { c.x, c.y }; + int binIndex = (int)( binCount * ( cArray[axisIndex] - minC ) * invD ); + binIndex = b3ClampInt( binIndex, 0, B3_BIN_COUNT - 1 ); + binIndices[i] = binIndex; + bins[binIndex].count += 1; + bins[binIndex].aabb = b3AABB_Union( bins[binIndex].aabb, boxes[i] ); + } + + int planeCount = B3_BIN_COUNT - 1; + + // Prepare all the left planes, candidates for left child + planes[0].leftCount = bins[0].count; + planes[0].leftAABB = bins[0].aabb; + for ( int i = 1; i < planeCount; ++i ) + { + planes[i].leftCount = planes[i - 1].leftCount + bins[i].count; + planes[i].leftAABB = b3AABB_Union( planes[i - 1].leftAABB, bins[i].aabb ); + } + + // Prepare all the right planes, candidates for right child + planes[planeCount - 1].rightCount = bins[planeCount].count; + planes[planeCount - 1].rightAABB = bins[planeCount].aabb; + for ( int i = planeCount - 2; i >= 0; --i ) + { + planes[i].rightCount = planes[i + 1].rightCount + bins[i + 1].count; + planes[i].rightAABB = b3AABB_Union( planes[i + 1].rightAABB, bins[i + 1].aabb ); + } + + // Find best split to minimize SAH + float minCost = FLT_MAX; + int bestPlane = 0; + for ( int i = 0; i < planeCount; ++i ) + { + float leftArea = b3Perimeter( planes[i].leftAABB ); + float rightArea = b3Perimeter( planes[i].rightAABB ); + int leftCount = planes[i].leftCount; + int rightCount = planes[i].rightCount; + + float cost = leftCount * leftArea + rightCount * rightArea; + if ( cost < minCost ) + { + bestPlane = i; + minCost = cost; + } + } + + // Partition node indices and boxes using the Hoare partition scheme + // https://en.wikipedia.org/wiki/Quicksort + // https://nicholasvadivelu.com/2021/01/11/array-partition/ + int i1 = 0, i2 = count; + while ( i1 < i2 ) + { + while ( i1 < i2 && binIndices[i1] < bestPlane ) + { + i1 += 1; + }; + + while ( i1 < i2 && binIndices[i2 - 1] >= bestPlane ) + { + i2 -= 1; + }; + + if ( i1 < i2 ) + { + // Swap indices + { + int temp = indices[i1]; + indices[i1] = indices[i2 - 1]; + indices[i2 - 1] = temp; + } + + // Swap boxes + { + b3AABB temp = boxes[i1]; + boxes[i1] = boxes[i2 - 1]; + boxes[i2 - 1] = temp; + } + + i1 += 1; + i2 -= 1; + } + } + B3_ASSERT( i1 == i2 ); + + if ( i1 > 0 && i1 < count ) + { + return i1; + } + else + { + return count / 2; + } +} + +#endif + +// Temporary data used to track the rebuild of a tree node +struct b3RebuildItem +{ + int nodeIndex; + int childCount; + + // Leaf indices + int startIndex; + int splitIndex; + int endIndex; +}; + +// Returns root node index +static int b3BuildTree( b3DynamicTree* tree, int leafCount ) +{ + b3TreeNode* nodes = tree->nodes; + int* leafIndices = tree->leafIndices; + + if ( leafCount == 1 ) + { + nodes[leafIndices[0]].parent = B3_NULL_INDEX; + return leafIndices[0]; + } + +#if B3_TREE_HEURISTIC == 0 + b3Vec3* leafCenters = tree->leafCenters; +#else + b3AABB* leafBoxes = tree->leafBoxes; + int* binIndices = tree->binIndices; +#endif + + // todo large stack item + struct b3RebuildItem stack[B3_TREE_STACK_SIZE]; + int top = 0; + + stack[0].nodeIndex = b3AllocateNode( tree ); + stack[0].childCount = -1; + stack[0].startIndex = 0; + stack[0].endIndex = leafCount; +#if B3_TREE_HEURISTIC == 0 + stack[0].splitIndex = b3PartitionMid( leafIndices, leafCenters, leafCount ); +#else + stack[0].splitIndex = b3PartitionSAH( leafIndices, binIndices, leafBoxes, leafCount ); +#endif + + while ( true ) + { + struct b3RebuildItem* item = stack + top; + + item->childCount += 1; + + if ( item->childCount == 2 ) + { + // This internal node has both children established + + if ( top == 0 ) + { + // all done + break; + } + + struct b3RebuildItem* parentItem = stack + ( top - 1 ); + b3TreeNode* parentNode = nodes + parentItem->nodeIndex; + + if ( parentItem->childCount == 0 ) + { + B3_ASSERT( parentNode->children.child1 == B3_NULL_INDEX ); + parentNode->children.child1 = item->nodeIndex; + } + else + { + B3_ASSERT( parentItem->childCount == 1 ); + B3_ASSERT( parentNode->children.child2 == B3_NULL_INDEX ); + parentNode->children.child2 = item->nodeIndex; + } + + b3TreeNode* node = nodes + item->nodeIndex; + + B3_ASSERT( node->parent == B3_NULL_INDEX ); + node->parent = parentItem->nodeIndex; + + B3_ASSERT( node->children.child1 != B3_NULL_INDEX ); + B3_ASSERT( node->children.child2 != B3_NULL_INDEX ); + b3TreeNode* child1 = nodes + node->children.child1; + b3TreeNode* child2 = nodes + node->children.child2; + + node->aabb = b3AABB_Union( child1->aabb, child2->aabb ); + node->height = 1 + b3MaxUInt16( child1->height, child2->height ); + node->categoryBits = child1->categoryBits | child2->categoryBits; + + // Pop stack + top -= 1; + } + else + { + int startIndex, endIndex; + if ( item->childCount == 0 ) + { + startIndex = item->startIndex; + endIndex = item->splitIndex; + } + else + { + B3_ASSERT( item->childCount == 1 ); + startIndex = item->splitIndex; + endIndex = item->endIndex; + } + + int count = endIndex - startIndex; + + if ( count == 1 ) + { + int childIndex = leafIndices[startIndex]; + b3TreeNode* node = nodes + item->nodeIndex; + + if ( item->childCount == 0 ) + { + B3_ASSERT( node->children.child1 == B3_NULL_INDEX ); + node->children.child1 = childIndex; + } + else + { + B3_ASSERT( item->childCount == 1 ); + B3_ASSERT( node->children.child2 == B3_NULL_INDEX ); + node->children.child2 = childIndex; + } + + b3TreeNode* childNode = nodes + childIndex; + B3_ASSERT( childNode->parent == B3_NULL_INDEX ); + childNode->parent = item->nodeIndex; + } + else + { + B3_ASSERT( count > 0 ); + B3_ASSERT( top < B3_TREE_STACK_SIZE ); + + top += 1; + struct b3RebuildItem* newItem = stack + top; + newItem->nodeIndex = b3AllocateNode( tree ); + newItem->childCount = -1; + newItem->startIndex = startIndex; + newItem->endIndex = endIndex; +#if B3_TREE_HEURISTIC == 0 + newItem->splitIndex = b3PartitionMid( leafIndices + startIndex, leafCenters + startIndex, count ); +#else + newItem->splitIndex = + b3PartitionSAH( leafIndices + startIndex, binIndices + startIndex, leafBoxes + startIndex, count ); +#endif + newItem->splitIndex += startIndex; + } + } + } + + b3TreeNode* rootNode = nodes + stack[0].nodeIndex; + B3_ASSERT( rootNode->parent == B3_NULL_INDEX ); + B3_ASSERT( rootNode->children.child1 != B3_NULL_INDEX ); + B3_ASSERT( rootNode->children.child2 != B3_NULL_INDEX ); + + b3TreeNode* child1 = nodes + rootNode->children.child1; + b3TreeNode* child2 = nodes + rootNode->children.child2; + + rootNode->aabb = b3AABB_Union( child1->aabb, child2->aabb ); + rootNode->height = 1 + b3MaxUInt16( child1->height, child2->height ); + rootNode->categoryBits = child1->categoryBits | child2->categoryBits; + + return stack[0].nodeIndex; +} + +// Not safe to access tree during this operation because it may grow +int b3DynamicTree_Rebuild( b3DynamicTree* tree, bool fullBuild ) +{ + int proxyCount = tree->proxyCount; + if ( proxyCount == 0 ) + { + return 0; + } + + // Ensure capacity for rebuild space + if ( proxyCount > tree->rebuildCapacity ) + { + int newCapacity = proxyCount + proxyCount / 2; + + b3Free( tree->leafIndices, tree->rebuildCapacity * sizeof( int ) ); + tree->leafIndices = (int*)b3Alloc( newCapacity * sizeof( int ) ); + +#if B3_TREE_HEURISTIC == 0 + b3Free( tree->leafCenters, tree->rebuildCapacity * sizeof( b3Vec3 ) ); + tree->leafCenters = (b3Vec3*)b3Alloc( newCapacity * sizeof( b3Vec3 ) ); +#else + b3Free( tree->leafBoxes, tree->rebuildCapacity * sizeof( b3AABB ) ); + tree->leafBoxes = (b3AABB*)b3Alloc( newCapacity * sizeof( b3AABB ) ); + b3Free( tree->binIndices, tree->rebuildCapacity * sizeof( int ) ); + tree->binIndices = (int*)b3Alloc( newCapacity * sizeof( int ) ); +#endif + tree->rebuildCapacity = newCapacity; + } + + int leafCount = 0; + int stack[B3_TREE_STACK_SIZE]; + int stackCount = 0; + + int nodeIndex = tree->root; + b3TreeNode* nodes = tree->nodes; + b3TreeNode* node = nodes + nodeIndex; + + // These are the nodes that get sorted to rebuild the tree. + // I'm using indices because the node pool may grow during the build. + int* leafIndices = tree->leafIndices; + +#if B3_TREE_HEURISTIC == 0 + b3Vec3* leafCenters = tree->leafCenters; +#else + b3AABB* leafBoxes = tree->leafBoxes; +#endif + + // Gather all proxy nodes that have grown and all internal nodes that haven't grown. Both are + // considered leaves in the tree rebuild. + // Free all internal nodes that have grown. + // todo use a node growth metric instead of simply enlarged to reduce rebuild size and frequency + // this should be weighed against B3_MAX_AABB_MARGIN + while ( true ) + { + if ( b3IsLeaf( node ) == true || ( ( node->flags & b3_enlargedNode ) == 0 && fullBuild == false ) ) + { + leafIndices[leafCount] = nodeIndex; +#if B3_TREE_HEURISTIC == 0 + leafCenters[leafCount] = b3AABB_Center( node->aabb ); +#else + leafBoxes[leafCount] = node->aabb; +#endif + leafCount += 1; + + // Detach + node->parent = B3_NULL_INDEX; + } + else + { + int doomedNodeIndex = nodeIndex; + + // Handle children + nodeIndex = node->children.child1; + + B3_ASSERT( stackCount < B3_TREE_STACK_SIZE ); + if ( stackCount < B3_TREE_STACK_SIZE ) + { + stack[stackCount++] = node->children.child2; + } + + node = nodes + nodeIndex; + + // Remove doomed node + b3FreeNode( tree, doomedNodeIndex ); + + continue; + } + + if ( stackCount == 0 ) + { + break; + } + + nodeIndex = stack[--stackCount]; + node = nodes + nodeIndex; + } + +#if B3_ENABLE_VALIDATION == 1 + int capacity = tree->nodeCapacity; + for ( int i = 0; i < capacity; ++i ) + { + if ( nodes[i].flags & b3_allocatedNode ) + { + B3_ASSERT( ( nodes[i].flags & b3_enlargedNode ) == 0 ); + } + } +#endif + + B3_ASSERT( leafCount <= proxyCount ); + + tree->root = b3BuildTree( tree, leafCount ); + + b3DynamicTree_Validate( tree ); + + return leafCount; +} + +static FILE* b3OpenTreeFile( const char* fileName, const char* mode ) +{ + FILE* file = NULL; + +#if defined( _MSC_VER ) + errno_t e = fopen_s( &file, fileName, mode ); + if ( e != 0 ) + { + return NULL; + } +#else + file = fopen( fileName, mode ); + if ( file == NULL ) + { + return NULL; + } +#endif + + return file; +} + +void b3DynamicTree_Save( const b3DynamicTree* tree, const char* fileName ) +{ + FILE* file = b3OpenTreeFile( fileName, "wb" ); + if ( file == NULL ) + { + return; + } + + // Copy to allow setting some fields to zero + b3DynamicTree temp = *tree; + + // Zero pointers and temp data + temp.nodes = NULL; + temp.leafIndices = NULL; + temp.leafBoxes = NULL; + temp.leafCenters = NULL; + temp.binIndices = NULL; + temp.rebuildCapacity = 0; + + // Write tree struct + fwrite( &temp, sizeof( b3DynamicTree ), 1, file ); + + // Write the node array, this includes the free list + if ( tree->nodeCapacity > 0 && tree->nodes != NULL ) + { + fwrite( tree->nodes, sizeof( b3TreeNode ), tree->nodeCapacity, file ); + } + + fclose( file ); +} + +b3DynamicTree b3DynamicTree_Load( const char* fileName, float scale ) +{ + b3DynamicTree tree = { 0 }; + + FILE* file = b3OpenTreeFile( fileName, "rb" ); + if ( file == NULL ) + { + return tree; + } + + int readCount = (int)fread( &tree, sizeof( b3DynamicTree ), 1, file ); + if ( readCount != 1 ) + { + fclose( file ); + return tree; + } + + if ( tree.version != B3_DYNAMIC_TREE_VERSION ) + { + fclose( file ); + memset( &tree, 0, sizeof( b3DynamicTree ) ); + return tree; + } + + if ( tree.nodeCapacity > 0 ) + { + tree.nodes = (b3TreeNode*)b3Alloc( tree.nodeCapacity * sizeof( b3TreeNode ) ); + readCount = (int)fread( tree.nodes, sizeof( b3TreeNode ), tree.nodeCapacity, file ); + if ( readCount != tree.nodeCapacity ) + { + b3Free( tree.nodes, tree.nodeCapacity * sizeof( b3TreeNode ) ); + fclose( file ); + memset( &tree, 0, sizeof( b3DynamicTree ) ); + return tree; + } + + for ( int i = 0; i < tree.nodeCapacity; ++i ) + { + b3TreeNode* node = tree.nodes + i; + node->aabb.lowerBound = b3MulSV( scale, node->aabb.lowerBound ); + node->aabb.upperBound = b3MulSV( scale, node->aabb.upperBound ); + } + } + else + { + tree.nodes = NULL; + } + + // Zero temp data fields + tree.leafIndices = NULL; + tree.leafBoxes = NULL; + tree.leafCenters = NULL; + tree.binIndices = NULL; + tree.rebuildCapacity = 0; + + fclose( file ); + + return tree; +} diff --git a/vendor/box3d/src/src/height_field.c b/vendor/box3d/src/src/height_field.c new file mode 100644 index 000000000..1b0307fe9 --- /dev/null +++ b/vendor/box3d/src/src/height_field.c @@ -0,0 +1,1583 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +#include "aabb.h" +#include "algorithm.h" +#include "body.h" +#include "core.h" +#include "shape.h" +#include "simd.h" + +#include "box3d/collision.h" +#include "box3d/constants.h" +#include "box3d/math_functions.h" + +#include +#include +#include + +/* + Convention + + index = row * columnCount + column + height = minHeight + heightScale * compressedHeights[index]; + + column = index % columnCount; + row = index / columnCount; + + x-axis : columns + z-axis : rows + + 00 --- 01 --- 02 --- 03 X + | 0 | 1 | 2 | + 04 --- 05 --- 06 --- 07 + | 3 | 4 | 5 | + 08 --- 09 --- 10 --- 11 + | 6 | 7 | 8 | + 12 --- 13 --- 14 --- 15 + Z + + The quads exist before the column and row ends: row < rowCount - 1 and column < columnCount - 1 + quadIndex = row * (columnCount - 1) + column + + Quad origin index from quad index (needs row): + index = quadIndex + row * columnCount + + Triangle index is related to the quad index + triangleIndex = 2 * quadIndex + (0/1) + quadIndex = triangleIndex / 2 + + Row and column from quad index: + row = quadIndex / (columnCount - 1) + column = quadIndex - row * (columnCount - 1) + + The triangle diagonal is fixed. + + triangle0 = {00, 04, 01} -> {11, 21, 12} + triangle1 = {04, 05, 01} -> {22, 12, 21} + + 11 12 + 00 ---- 01 + | / | + | 0 / 1 | 1 + | / | + 04 ---- 05 + 21 22 + + For adjacency we have + + NA + 00 ---- 01 ---- 02 + | / | / | + NA | 0 / 1 | 2 / 3 | + | / | / | + 04 ---- 05 ---- 06 + | / | / | + NA | 6 / 7 | 8 / 9 | + | / | / | + 08 ---- 09 ---- 10 + + 0: NA, NA, 1 + 1: 0, 6, 2 + + Triangle layouts + + 11 3 12 + 1 ---- 3 + | / + 1 | 0 / 2 + | / + 2 + 21 + + 12 + 2 + / | + 2 / 1 | 1 + / | + 3 ---- 1 + 21 3 22 + */ + +b3HeightFieldData* b3CreateHeightField( const b3HeightFieldDef* data ) +{ + int columnCount = data->countX; + int rowCount = data->countZ; + + int heightCount = columnCount * rowCount; + B3_ASSERT( heightCount >= 4 ); + + int cellCount = ( columnCount - 1 ) * ( rowCount - 1 ); + int triangleCount = 2 * cellCount; + + // Single blob: struct followed by the height, material, and flag arrays. Layout + // mirrors b3HullData/b3MeshData so the recording path can copy it with one memcpy. + size_t byteCount = b3AlignUp8( sizeof( b3HeightFieldData ) ); + int heightsOffset = (int)byteCount; + byteCount += b3AlignUp8( heightCount * sizeof( uint16_t ) ); + int materialOffset = (int)byteCount; + byteCount += b3AlignUp8( cellCount * sizeof( uint8_t ) ); + int flagsOffset = (int)byteCount; + byteCount += b3AlignUp8( triangleCount * sizeof( uint8_t ) ); + + // Zero the whole blob so alignment padding is defined. The construction-time hash + // sweeps raw bytes and would otherwise pick up uninitialized padding. + b3HeightFieldData* hf = (b3HeightFieldData*)b3Alloc( byteCount ); + memset( hf, 0, byteCount ); + + hf->version = B3_HEIGHT_FIELD_VERSION; + hf->byteCount = (int)byteCount; + hf->scale = data->scale; + hf->columnCount = columnCount; + hf->rowCount = rowCount; + hf->heightsOffset = heightsOffset; + hf->materialOffset = materialOffset; + hf->flagsOffset = flagsOffset; + hf->clockwise = data->clockwiseWinding; + + uint16_t* compressedHeights = (uint16_t*)( (intptr_t)hf + heightsOffset ); + uint8_t* materialIndices = (uint8_t*)( (intptr_t)hf + materialOffset ); + uint8_t* flags = (uint8_t*)( (intptr_t)hf + flagsOffset ); + + const float* heights = data->heights; + + B3_ASSERT( data->globalMinimumHeight <= data->globalMaximumHeight ); + hf->minHeight = data->globalMinimumHeight; + hf->maxHeight = data->globalMaximumHeight; + + float height = b3MaxFloat( hf->maxHeight - hf->minHeight, B3_LINEAR_SLOP ); + hf->heightScale = height / UINT16_MAX; + + float lowerHeightBound = hf->maxHeight; + float upperHeightBound = hf->minHeight; + + float invHeightScale = 1.0f / hf->heightScale; + for ( int i = 0; i < heightCount; ++i ) + { + float clampedHeight = b3ClampFloat( heights[i], hf->minHeight, hf->maxHeight ); + float scaledHeight = ( clampedHeight - hf->minHeight ) * invHeightScale; + compressedHeights[i] = (uint16_t)( b3MinFloat( scaledHeight, (float)UINT16_MAX ) ); + + lowerHeightBound = b3MinFloat( lowerHeightBound, clampedHeight ); + upperHeightBound = b3MaxFloat( upperHeightBound, clampedHeight ); + } + + // Use decompressed heights for accurate convexity metrics. + float* decompressedHeights = (float*)b3Alloc( heightCount * sizeof( float ) ); + for ( int i = 0; i < heightCount; ++i ) + { + decompressedHeights[i] = hf->minHeight + hf->heightScale * compressedHeights[i]; + } + heights = decompressedHeights; + + if ( data->materialIndices != NULL ) + { + for ( int i = 0; i < cellCount; ++i ) + { + materialIndices[i] = data->materialIndices[i]; + } + } + else + { + for ( int i = 0; i < cellCount; ++i ) + { + materialIndices[i] = 0; + } + } + + hf->aabb.lowerBound = (b3Vec3){ 0.0f, hf->scale.y * lowerHeightBound, 0.0f }; + hf->aabb.upperBound = + (b3Vec3){ hf->scale.x * ( hf->columnCount - 1 ), hf->scale.y * upperHeightBound, hf->scale.z * ( hf->rowCount - 1 ) }; + + float cos5Deg = 0.9962f; + b3Vec3 scale = hf->scale; + + int triangleIndex = 0; + for ( int row = 0; row < rowCount - 1; ++row ) + { + for ( int column = 0; column < columnCount - 1; ++column ) + { + // todo compute convexity flags + // This requires a couple things + // - determine all 3 adjacent triangles for each triangle + // - consider clockwise winding + // - consider borders where there is no adjacent triangle + + int triangleIndex1 = triangleIndex; + int triangleIndex2 = triangleIndex + 1; + triangleIndex += 2; + + int cellIndex = row * ( columnCount - 1 ) + column; + + if ( materialIndices[cellIndex] == B3_HEIGHT_FIELD_HOLE ) + { + continue; + } + + int flags1 = 0; + int flags2 = 0; + + b3Plane plane1, plane2; + b3Vec3 center1, center2; + + int index11 = row * columnCount + column; + int index12 = index11 + 1; + int index21 = ( row + 1 ) * columnCount + column; + int index22 = index21 + 1; + + { + float height11 = heights[index11]; + float height12 = heights[index12]; + float height21 = heights[index21]; + float height22 = heights[index22]; + + float x1 = (float)( column ); + float x2 = (float)( column + 1 ); + float z1 = (float)( row ); + float z2 = (float)( row + 1 ); + + // triangle 0 : 11, 21, 12 + b3Vec3 vs0[3]; + vs0[0] = b3Mul( scale, (b3Vec3){ x1, height11, z1 } ); + vs0[1] = b3Mul( scale, (b3Vec3){ x1, height21, z2 } ); + vs0[2] = b3Mul( scale, (b3Vec3){ x2, height12, z1 } ); + plane1 = b3MakePlaneFromPoints( vs0[0], vs0[1], vs0[2] ); + + center1 = b3MulSV( 1.0f / 3.0f, b3Add( b3Add( vs0[0], vs0[1] ), vs0[2] ) ); + + // triangle 1 : 22, 12, 21 + b3Vec3 vs1[3]; + vs1[0] = b3Mul( scale, (b3Vec3){ x2, height22, z2 } ); + vs1[1] = b3Mul( scale, (b3Vec3){ x2, height12, z1 } ); + vs1[2] = b3Mul( scale, (b3Vec3){ x1, height21, z2 } ); + plane2 = b3MakePlaneFromPoints( vs1[0], vs1[1], vs1[2] ); + + center2 = b3MulSV( 1.0f / 3.0f, b3Add( b3Add( vs1[0], vs1[1] ), vs1[2] ) ); + + float separation = b3PlaneSeparation( plane1, vs1[0] ); + float cosAngle = b3Dot( plane1.normal, plane2.normal ); + if ( separation > 0.0f || cosAngle > cos5Deg ) + { + flags1 |= b3_concaveEdge2; + flags2 |= b3_concaveEdge2; + } + if ( separation < 0.0f || cosAngle > cos5Deg ) + { + flags1 |= b3_inverseConcaveEdge2; + flags2 |= b3_inverseConcaveEdge2; + } + } + + B3_UNUSED( center1 ); + B3_UNUSED( center2 ); + + // top + int topCellIndex = ( row - 1 ) * ( columnCount - 1 ) + column; + if ( row > 0 && materialIndices[topCellIndex] != B3_HEIGHT_FIELD_HOLE ) + { + B3_ASSERT( 0 <= topCellIndex && topCellIndex < cellCount ); + + int r = row - 1; + int c = column; + + int i11 = r * columnCount + c; + int i12 = i11 + 1; + int i21 = ( r + 1 ) * columnCount + c; + int i22 = i21 + 1; + + B3_ASSERT( i21 == index11 ); + B3_ASSERT( i22 == index12 ); + + // float h11 = heights[i11]; + float h12 = heights[i12]; + float h21 = heights[i21]; + float h22 = heights[i22]; + + float x1 = (float)( c ); + float x2 = (float)( c + 1 ); + float z1 = (float)( r ); + float z2 = (float)( r + 1 ); + + // triangle 1 + b3Vec3 vs[3]; + vs[0] = b3Mul( scale, (b3Vec3){ x2, h22, z2 } ); + vs[1] = b3Mul( scale, (b3Vec3){ x2, h12, z1 } ); + vs[2] = b3Mul( scale, (b3Vec3){ x1, h21, z2 } ); + + b3Vec3 n = b3Normalize( b3Cross( b3Sub( vs[1], vs[0] ), b3Sub( vs[2], vs[0] ) ) ); + + float separation = b3PlaneSeparation( plane1, vs[1] ); + float cosAngle = b3Dot( plane1.normal, n ); + if ( separation > 0.0f || cosAngle > cos5Deg ) + { + flags1 |= b3_concaveEdge3; + } + if ( separation < 0.0f || cosAngle > cos5Deg ) + { + flags1 |= b3_inverseConcaveEdge3; + } + } + + int bottomCellIndex = ( row + 1 ) * ( columnCount - 1 ) + column; + if ( row + 1 < rowCount - 1 && materialIndices[bottomCellIndex] != B3_HEIGHT_FIELD_HOLE ) + { + B3_ASSERT( 0 <= bottomCellIndex && bottomCellIndex < cellCount ); + + int r = row + 1; + int c = column; + + int i11 = r * columnCount + c; + int i12 = i11 + 1; + int i21 = ( r + 1 ) * columnCount + c; + // int i22 = i21 + 1; + + B3_ASSERT( i11 == index21 ); + B3_ASSERT( i12 == index22 ); + + float h11 = heights[i11]; + float h12 = heights[i12]; + float h21 = heights[i21]; + // float h22 = heights[i22]; + + float x1 = (float)( c ); + float x2 = (float)( c + 1 ); + float z1 = (float)( r ); + float z2 = (float)( r + 1 ); + + // triangle 0 + b3Vec3 vs[3]; + vs[0] = b3Mul( scale, (b3Vec3){ x1, h11, z1 } ); + vs[1] = b3Mul( scale, (b3Vec3){ x1, h21, z2 } ); + vs[2] = b3Mul( scale, (b3Vec3){ x2, h12, z1 } ); + + b3Vec3 n = b3Normalize( b3Cross( b3Sub( vs[1], vs[0] ), b3Sub( vs[2], vs[0] ) ) ); + + float separation = b3PlaneSeparation( plane2, vs[1] ); + float cosAngle = b3Dot( plane2.normal, n ); + if ( separation > 0.0f || cosAngle > cos5Deg ) + { + flags2 |= b3_concaveEdge3; + } + if ( separation < 0.0f || cosAngle > cos5Deg ) + { + flags2 |= b3_inverseConcaveEdge3; + } + } + + int leftCellIndex = row * ( columnCount - 1 ) + column - 1; + if ( column - 1 >= 0 && materialIndices[leftCellIndex] != B3_HEIGHT_FIELD_HOLE ) + { + B3_ASSERT( 0 <= leftCellIndex && leftCellIndex < cellCount ); + + int r = row; + int c = column - 1; + + int i11 = r * columnCount + c; + int i12 = i11 + 1; + int i21 = ( r + 1 ) * columnCount + c; + int i22 = i21 + 1; + + B3_ASSERT( i12 == index11 ); + B3_ASSERT( i22 == index21 ); + + // float h11 = heights[i11]; + float h12 = heights[i12]; + float h21 = heights[i21]; + float h22 = heights[i22]; + + float x1 = (float)( c ); + float x2 = (float)( c + 1 ); + float z1 = (float)( r ); + float z2 = (float)( r + 1 ); + + // triangle 1 + b3Vec3 vs[3]; + vs[0] = b3Mul( scale, (b3Vec3){ x2, h22, z2 } ); + vs[1] = b3Mul( scale, (b3Vec3){ x2, h12, z1 } ); + vs[2] = b3Mul( scale, (b3Vec3){ x1, h21, z2 } ); + + b3Vec3 n = b3Normalize( b3Cross( b3Sub( vs[1], vs[0] ), b3Sub( vs[2], vs[0] ) ) ); + + float separation = b3PlaneSeparation( plane1, vs[2] ); + float cosAngle = b3Dot( plane1.normal, n ); + if ( separation > 0.0f || cosAngle > cos5Deg ) + { + flags1 |= b3_concaveEdge1; + } + if ( separation < 0.0f || cosAngle > cos5Deg ) + { + flags1 |= b3_inverseConcaveEdge1; + } + } + + int rightCellIndex = row * ( columnCount - 1 ) + column + 1; + if ( column + 1 < columnCount - 1 && materialIndices[rightCellIndex] != B3_HEIGHT_FIELD_HOLE ) + { + B3_ASSERT( 0 <= rightCellIndex && rightCellIndex < cellCount ); + + int r = row; + int c = column + 1; + + int i11 = r * columnCount + c; + int i12 = i11 + 1; + int i21 = ( r + 1 ) * columnCount + c; + // int i22 = i21 + 1; + + B3_ASSERT( i11 == index12 ); + B3_ASSERT( i21 == index22 ); + + float h11 = heights[i11]; + float h12 = heights[i12]; + float h21 = heights[i21]; + // float h22 = heights[i22]; + + float x1 = (float)( c ); + float x2 = (float)( c + 1 ); + float z1 = (float)( r ); + float z2 = (float)( r + 1 ); + + // triangle 0 + b3Vec3 vs[3]; + vs[0] = b3Mul( scale, (b3Vec3){ x1, h11, z1 } ); + vs[1] = b3Mul( scale, (b3Vec3){ x1, h21, z2 } ); + vs[2] = b3Mul( scale, (b3Vec3){ x2, h12, z1 } ); + + b3Vec3 n = b3Normalize( b3Cross( b3Sub( vs[1], vs[0] ), b3Sub( vs[2], vs[0] ) ) ); + + float separation = b3PlaneSeparation( plane2, vs[2] ); + float cosAngle = b3Dot( plane2.normal, n ); + if ( separation > 0.0f || cosAngle > cos5Deg ) + { + flags2 |= b3_concaveEdge1; + } + if ( separation < 0.0f || cosAngle > cos5Deg ) + { + flags2 |= b3_inverseConcaveEdge1; + } + } + + B3_ASSERT( 0 <= flags1 && flags1 <= UINT8_MAX ); + B3_ASSERT( 0 <= flags2 && flags2 <= UINT8_MAX ); + + flags[triangleIndex1] = (uint8_t)flags1; + flags[triangleIndex2] = (uint8_t)flags2; + } + } + + B3_ASSERT( triangleIndex == triangleCount ); + + b3Free( decompressedHeights, heightCount * sizeof( float ) ); + + // Content hash over the whole blob with the hash field zeroed, like b3HullData/b3MeshData. + hf->hash = 0; + hf->hash = b3NonZeroHash( b3Hash( B3_HASH_INIT, (const uint8_t*)hf, hf->byteCount ) ); + + return hf; +} + +_Static_assert( b3_concaveEdge3 == 4 * b3_concaveEdge1, "bit math" ); +_Static_assert( b3_inverseConcaveEdge3 == 4 * b3_inverseConcaveEdge1, "bit math" ); + +// Decode the four corner vertices of a height field cell into local space. +// Output order matches the index naming used throughout this file: +// corners[0] = (column, row), corners[1] = (column + 1, row), +// corners[2] = (column, row + 1), corners[3] = (column + 1, row + 1). +static inline void b3GetHeightFieldCellCorners( const b3HeightFieldData* hf, int row, int column, b3Vec3 corners[4] ) +{ + B3_ASSERT( 0 <= row && row < hf->rowCount - 1 && 0 <= column && column < hf->columnCount - 1 ); + + int columnCount = hf->columnCount; + int index11 = row * columnCount + column; + int index12 = index11 + 1; + int index21 = ( row + 1 ) * columnCount + column; + int index22 = index21 + 1; + + float minHeight = hf->minHeight; + float heightScale = hf->heightScale; + const uint16_t* heights = b3GetHeightFieldCompressedHeights( hf ); + + float height11 = minHeight + heightScale * heights[index11]; + float height12 = minHeight + heightScale * heights[index12]; + float height21 = minHeight + heightScale * heights[index21]; + float height22 = minHeight + heightScale * heights[index22]; + + float x1 = (float)( column ); + float x2 = (float)( column + 1 ); + float z1 = (float)( row ); + float z2 = (float)( row + 1 ); + + b3Vec3 scale = hf->scale; + corners[0] = b3Mul( scale, (b3Vec3){ x1, height11, z1 } ); + corners[1] = b3Mul( scale, (b3Vec3){ x2, height12, z1 } ); + corners[2] = b3Mul( scale, (b3Vec3){ x1, height21, z2 } ); + corners[3] = b3Mul( scale, (b3Vec3){ x2, height22, z2 } ); +} + +b3Triangle b3GetHeightFieldTriangle( const b3HeightFieldData* heightField, int triangleIndex ) +{ + B3_ASSERT( 0 <= triangleIndex ); + B3_ASSERT( triangleIndex < 2 * ( heightField->columnCount - 1 ) * ( heightField->rowCount - 1 ) ); + + b3Triangle triangle; + triangle.flags = b3GetHeightFieldFlags( heightField )[triangleIndex]; + + int columnCount = heightField->columnCount; + int quadIndex = triangleIndex >> 1; + int row = quadIndex / ( columnCount - 1 ); + int column = quadIndex - row * ( columnCount - 1 ); + + int index11 = row * columnCount + column; + int index12 = index11 + 1; + int index21 = ( row + 1 ) * columnCount + column; + int index22 = index21 + 1; + + int cellIndex = row * ( columnCount - 1 ) + column; + + B3_ASSERT( quadIndex == cellIndex ); + B3_ASSERT( b3GetHeightFieldMaterialIndices( heightField )[cellIndex] != B3_HEIGHT_FIELD_HOLE ); + B3_UNUSED( cellIndex ); + + b3Vec3 corners[4]; + b3GetHeightFieldCellCorners( heightField, row, column, corners ); + + if ( ( triangleIndex & 1 ) == 0 ) + { + triangle.vertices[0] = corners[0]; + triangle.vertices[1] = corners[2]; + triangle.vertices[2] = corners[1]; + triangle.i1 = index11; + triangle.i2 = index21; + triangle.i3 = index12; + } + else + { + triangle.vertices[0] = corners[3]; + triangle.vertices[1] = corners[1]; + triangle.vertices[2] = corners[2]; + triangle.i1 = index22; + triangle.i2 = index12; + triangle.i3 = index21; + } + + if ( heightField->clockwise ) + { + B3_SWAP( triangle.vertices[1], triangle.vertices[2] ); + B3_SWAP( triangle.i2, triangle.i3 ); + + // Reversing winding swaps edge1 and edge3; edge2 (the diagonal) is preserved. + int flags = triangle.flags; + int edge1Bits = flags & ( b3_concaveEdge1 | b3_inverseConcaveEdge1 ); + int edge3Bits = flags & ( b3_concaveEdge3 | b3_inverseConcaveEdge3 ); + flags &= ~( b3_concaveEdge1 | b3_concaveEdge3 | b3_inverseConcaveEdge1 | b3_inverseConcaveEdge3 ); + flags |= edge1Bits << 2; + flags |= edge3Bits >> 2; + triangle.flags = flags; + } + + return triangle; +} + +int b3GetHeightFieldMaterial( const b3HeightFieldData* heightField, int triangleIndex ) +{ + B3_ASSERT( 0 <= triangleIndex ); + B3_ASSERT( triangleIndex < 2 * ( heightField->columnCount - 1 ) * ( heightField->rowCount - 1 ) ); + + int cellIndex = triangleIndex >> 1; + return b3GetHeightFieldMaterialIndices( heightField )[cellIndex]; +} + +b3AABB b3ComputeHeightFieldAABB( const b3HeightFieldData* shape, b3Transform transform ) +{ + return b3AABB_Transform( transform, shape->aabb ); +} + +b3CastOutput b3RayCastHeightField( const b3HeightFieldData* heightField, const b3RayCastInput* input ) +{ + b3ShapeCastInput shapeCastInput = { 0 }; + shapeCastInput.proxy = (b3ShapeProxy){ &input->origin, 1, 0.0f }; + shapeCastInput.translation = input->translation; + shapeCastInput.maxFraction = input->maxFraction; + + return b3ShapeCastHeightField( heightField, &shapeCastInput ); +} + +// todo advance cast to the grid border immediately if it starts outside the row/column range +// todo terminate the cast immediately if it leaves the row/column range +b3CastOutput b3ShapeCastHeightField( const b3HeightFieldData* heightField, const b3ShapeCastInput* input ) +{ + b3AABB shapeBounds = b3MakeAABB( input->proxy.points, input->proxy.count, input->proxy.radius ); + b3Vec3 shapeTranslation = input->translation; + b3Vec3 scale = heightField->scale; + + b3Vec3 shapeStart = b3AABB_Center( shapeBounds ); + b3Vec3 shapeDelta = b3MulSV( input->maxFraction, shapeTranslation ); + b3Vec3 shapeEnd = b3Add( shapeStart, shapeDelta ); + + b3CastOutput result = { 0 }; + + b3Vec3 shapeExtents = b3AABB_Extents( shapeBounds ); + b3Vec3 margin = { B3_MAX_AABB_MARGIN, B3_MAX_AABB_MARGIN, B3_MAX_AABB_MARGIN }; + b3AABB combinedBounds = { b3Sub( b3Sub( heightField->aabb.lowerBound, shapeExtents ), margin ), + b3Add( b3Add( heightField->aabb.upperBound, shapeExtents ), margin ) }; + + float minFraction, maxFraction; + bool intersects = b3RayCastAABB( combinedBounds, shapeStart, shapeEnd, &minFraction, &maxFraction ); + if ( intersects == false ) + { + return result; + } + + // These are for walking the grid, not the triangle cast. + // The triangle cast uses the unclamped ray and fraction. + b3Vec3 clampedStart = b3MulAdd( shapeStart, minFraction, shapeDelta ); + b3Vec3 clampedDelta = b3MulSV( maxFraction - minFraction, shapeDelta ); + b3Vec3 clampedEnd = b3Add( clampedStart, clampedDelta ); + + // Preserve the un-shifted center sweep. clampedStart/clampedEnd get pushed out to the + // leading box corner below to drive the grid DDA, but the swept-volume AABB used to + // cull cells must stay centered on the actual shape path. + b3Vec3 centerStart = clampedStart; + b3Vec3 centerEnd = clampedEnd; + + // The grid traversal starts from the leading shape bounds corner + float signX, signZ; + if ( shapeTranslation.x >= 0.0f ) + { + clampedStart.x += shapeExtents.x; + signX = 1.0f; + } + else + { + clampedStart.x -= shapeExtents.x; + signX = -1.0f; + } + + if ( shapeTranslation.z >= 0.0f ) + { + clampedStart.z += shapeExtents.z; + signZ = 1.0f; + } + else + { + clampedStart.z -= shapeExtents.z; + signZ = -1.0f; + } + + // Shift the end as well + clampedEnd = b3Add( clampedStart, clampedDelta ); + + // Row and column range for the shape cast + int columnStart = (int)floorf( clampedStart.x / scale.x ); + int columnEnd = (int)floorf( clampedEnd.x / scale.x ); + int rowStart = (int)floorf( clampedStart.z / scale.z ); + int rowEnd = (int)floorf( clampedEnd.z / scale.z ); + + b3Vec3 absClampedDelta = b3Abs( clampedDelta ); + + // Precompute increments for row and column traversal. + // The ray can be slightly tilted yet remain within a single row or column + // once rasterized. + float deltaAlphaX; + float nextFractionX; + int deltaColumn; + + if ( columnStart < columnEnd ) + { + B3_ASSERT( absClampedDelta.x > 0.0f ); + + // Going forward on x columns + deltaAlphaX = scale.x / absClampedDelta.x; + nextFractionX = ( scale.x * ( columnStart + 1 ) - clampedStart.x ) / absClampedDelta.x; + deltaColumn = 1; + } + else if ( columnEnd < columnStart ) + { + B3_ASSERT( absClampedDelta.x > 0.0f ); + + // Going backwards on x columns + deltaAlphaX = scale.x / absClampedDelta.x; + nextFractionX = ( clampedStart.x - scale.x * columnStart ) / absClampedDelta.x; + deltaColumn = -1; + } + else + { + // Cast stays in a single column + deltaAlphaX = 0.0f; + nextFractionX = FLT_MAX; + deltaColumn = 0; + } + + float deltaAlphaZ; + float nextFractionZ; + int deltaRow; + + if ( rowStart < rowEnd ) + { + B3_ASSERT( absClampedDelta.z > 0.0f ); + + // Going forward on z rows + deltaAlphaZ = scale.z / absClampedDelta.z; + nextFractionZ = ( scale.z * ( rowStart + 1 ) - clampedStart.z ) / absClampedDelta.z; + deltaRow = 1; + } + else if ( rowEnd < rowStart ) + { + B3_ASSERT( absClampedDelta.z > 0.0f ); + + // Going backwards on z rows + deltaAlphaZ = scale.z / absClampedDelta.z; + nextFractionZ = ( clampedStart.z - scale.z * rowStart ) / absClampedDelta.z; + deltaRow = -1; + } + else + { + // Cast stays in a single row + deltaAlphaZ = 0.0f; + nextFractionZ = FLT_MAX; + deltaRow = 0; + } + + // Column and row range for 2D projected initial shape bounds + int boxColumnHead = columnStart; + int boxRowHead = rowStart; + + int boxColumnTail = (int)floorf( ( clampedStart.x - 2.0f * signX * shapeExtents.x ) / scale.x ); + int boxRowTail = (int)floorf( ( clampedStart.z - 2.0f * signZ * shapeExtents.z ) / scale.z ); + + float bestFraction = input->maxFraction; + + // nextFractionX / nextFractionZ advance in units of the clamped sweep + // [minFraction, maxFraction], but bestFraction is a fraction of the full input + // translation. Precompute the affine map from clamped space to input space so + // the loop termination test compares like with like — otherwise it can exit + // early and miss a closer hit in a later cell. + float gridFractionScale = input->maxFraction * ( maxFraction - minFraction ); + float gridFractionOffset = input->maxFraction * minFraction; + + int rowCount = heightField->rowCount; + int columnCount = heightField->columnCount; + int cellCount = ( heightField->rowCount - 1 ) * ( heightField->columnCount - 1 ); + B3_UNUSED( cellCount ); + + b3ShapeCastPairInput pairInput = { 0 }; + pairInput.proxyB = input->proxy; + pairInput.transform = b3Transform_identity; + pairInput.translationB = input->translation; + pairInput.canEncroach = input->canEncroach; + + b3AABB castBounds; + castBounds.lowerBound = b3Sub( b3Min( centerStart, centerEnd ), shapeExtents ); + castBounds.upperBound = b3Add( b3Max( centerStart, centerEnd ), shapeExtents ); + + b3V32 rayOrigin = b3LoadV( &shapeStart.x ); + b3V32 rayTranslation = b3LoadV( &shapeTranslation.x ); + + while ( true ) + { + int column1, column2; + if ( boxColumnTail < boxColumnHead ) + { + column1 = boxColumnTail; + column2 = boxColumnHead; + } + else + { + column1 = boxColumnHead; + column2 = boxColumnTail; + } + + int row1, row2; + if ( boxRowTail < boxRowHead ) + { + row1 = boxRowTail; + row2 = boxRowHead; + } + else + { + row1 = boxRowHead; + row2 = boxRowTail; + } + + for ( int row = row1; row <= row2; ++row ) + { + if ( row < 0 || rowCount - 1 <= row ) + { + continue; + } + + for ( int column = column1; column <= column2; ++column ) + { + if ( column < 0 || columnCount - 1 <= column ) + { + continue; + } + + int cellIndex = row * ( columnCount - 1 ) + column; + B3_ASSERT( cellIndex < cellCount ); + + uint8_t materialIndex = b3GetHeightFieldMaterialIndices( heightField )[cellIndex]; + if ( materialIndex == B3_HEIGHT_FIELD_HOLE ) + { + continue; + } + + b3Vec3 corners[4]; + b3GetHeightFieldCellCorners( heightField, row, column, corners ); + b3Vec3 point11 = corners[0]; + b3Vec3 point12 = corners[1]; + b3Vec3 point21 = corners[2]; + b3Vec3 point22 = corners[3]; + + // I know the min/max x and z values, but not the min/max heights. + b3AABB bounds; + bounds.lowerBound = b3Min( b3Min( point11, point12 ), b3Min( point21, point22 ) ); + bounds.upperBound = b3Max( b3Max( point11, point12 ), b3Max( point21, point22 ) ); + + if ( b3AABB_Overlaps( castBounds, bounds ) == false ) + { + continue; + } + + int quadIndex = row * ( columnCount - 1 ) + column; + int triangleIndex1 = 2 * quadIndex; + int triangleIndex2 = triangleIndex1 + 1; + + if ( input->proxy.count == 1 && input->proxy.radius == 0.0f ) + { + // Ray cast + { + b3V32 vertex1 = b3LoadV( &point11.x ); + b3V32 vertex2, vertex3; + + if ( heightField->clockwise ) + { + vertex2 = b3LoadV( &point12.x ); + vertex3 = b3LoadV( &point21.x ); + } + else + { + vertex2 = b3LoadV( &point21.x ); + vertex3 = b3LoadV( &point12.x ); + } + + float alpha = b3IntersectRayTriangle( rayOrigin, rayTranslation, vertex1, vertex2, vertex3 ); + B3_ASSERT( 0 <= alpha && alpha <= 1.0f ); + + if ( alpha < bestFraction ) + { + b3Vec3 edge1 = b3Sub( point21, point11 ); + b3Vec3 edge2 = b3Sub( point12, point11 ); + b3Vec3 normal = heightField->clockwise ? b3Cross( edge2, edge1 ) : b3Cross( edge1, edge2 ); + + result.point = b3MulAdd( shapeStart, alpha, shapeTranslation ); + result.normal = b3Normalize( normal ); + result.fraction = alpha; + result.triangleIndex = triangleIndex1; + result.materialIndex = materialIndex; + result.hit = true; + bestFraction = alpha; + } + } + + { + b3V32 vertex1 = b3LoadV( &point22.x ); + b3V32 vertex2, vertex3; + + if ( heightField->clockwise ) + { + vertex2 = b3LoadV( &point21.x ); + vertex3 = b3LoadV( &point12.x ); + } + else + { + vertex2 = b3LoadV( &point12.x ); + vertex3 = b3LoadV( &point21.x ); + } + + float alpha = b3IntersectRayTriangle( rayOrigin, rayTranslation, vertex1, vertex2, vertex3 ); + B3_ASSERT( 0 <= alpha && alpha <= 1.0f ); + + if ( alpha < bestFraction ) + { + b3Vec3 edge1 = b3Sub( point22, point21 ); + b3Vec3 edge2 = b3Sub( point12, point21 ); + b3Vec3 normal = heightField->clockwise ? b3Cross( edge2, edge1 ) : b3Cross( edge1, edge2 ); + + result.point = b3MulAdd( shapeStart, alpha, shapeTranslation ); + result.normal = b3Normalize( normal ); + result.fraction = alpha; + result.triangleIndex = triangleIndex2; + result.materialIndex = materialIndex; + result.hit = true; + bestFraction = alpha; + } + } + } + else + { + // Shape cast + // todo back-side culling + { + // Shift origin to first vertex + b3Vec3 origin = point11; + b3Vec3 triangleVertices[] = { b3Vec3_zero, b3Sub( point21, origin ), b3Sub( point12, origin ) }; + pairInput.proxyA = (b3ShapeProxy){ triangleVertices, 3, 0.0f }; + pairInput.maxFraction = bestFraction; + pairInput.transform.p = b3Neg( origin ); + + b3CastOutput pairOutput = b3ShapeCast( &pairInput ); + + if ( pairOutput.hit ) + { + bestFraction = pairOutput.fraction; + result = pairOutput; + result.point = b3Add( result.point, origin ); + result.triangleIndex = triangleIndex1; + result.materialIndex = materialIndex; + } + } + + { + // Shift origin to first vertex + b3Vec3 origin = point21; + b3Vec3 triangleVertices[] = { b3Vec3_zero, b3Sub( point22, origin ), b3Sub( point12, origin ) }; + pairInput.proxyA = (b3ShapeProxy){ triangleVertices, 3, 0.0f }; + pairInput.maxFraction = bestFraction; + pairInput.transform.p = b3Neg( origin ); + + b3CastOutput pairOutput = b3ShapeCast( &pairInput ); + + if ( pairOutput.hit ) + { + bestFraction = pairOutput.fraction; + result = pairOutput; + result.point = b3Add( result.point, origin ); + result.triangleIndex = triangleIndex2; + result.materialIndex = materialIndex; + } + } + } + } + } + + // These fractions always increase to guarantee the loop eventually exits. + // Map them from clamped-sweep space into input-translation space before + // comparing against bestFraction. + float inputFractionX = nextFractionX == FLT_MAX ? FLT_MAX : gridFractionOffset + nextFractionX * gridFractionScale; + float inputFractionZ = nextFractionZ == FLT_MAX ? FLT_MAX : gridFractionOffset + nextFractionZ * gridFractionScale; + if ( inputFractionX > bestFraction && inputFractionZ > bestFraction ) + { + break; + } + + // Advance the cast to the next column or row + if ( nextFractionX <= nextFractionZ ) + { + if ( boxColumnHead == columnEnd ) + { + // Hit the end already + break; + } + + // Advance to next column + boxColumnHead += deltaColumn; + + // Build a single column to cast + boxColumnTail = boxColumnHead; + + if ( shapeExtents.z == 0.0f ) + { + // Single row + boxRowTail = boxRowHead; + } + else + { + // Rasterize shape row + float rowIntercept = clampedStart.z + nextFractionX * clampedDelta.z; + boxRowTail = (int)floorf( ( rowIntercept - 2.0f * signZ * shapeExtents.z ) / scale.z ); + } + + nextFractionX += deltaAlphaX; + } + else + { + if ( boxRowHead == rowEnd ) + { + // Hit the end already + break; + } + + // Advance to next row + boxRowHead += deltaRow; + + // Build a single row to cast + boxRowTail = boxRowHead; + + if ( shapeExtents.x == 0.0f ) + { + // Single column + boxColumnTail = boxColumnHead; + } + else + { + // Rasterize shape column + float columnIntercept = clampedStart.x + nextFractionZ * clampedDelta.x; + boxColumnTail = (int)floorf( ( columnIntercept - 2.0f * signX * shapeExtents.x ) / scale.x ); + } + + nextFractionZ += deltaAlphaZ; + } + } + + return result; +} + +bool b3OverlapHeightField( const b3HeightFieldData* shape, b3Transform shapeTransform, const b3ShapeProxy* proxy ) +{ + b3Vec3 buffer[B3_MAX_SHAPE_CAST_POINTS]; + b3ShapeProxy localProxy = b3MakeLocalProxy( proxy, shapeTransform, buffer ); + b3AABB aabb = b3ComputeProxyAABB( &localProxy ); + + b3Vec3 scale = shape->scale; + int minRow = (int)floorf( aabb.lowerBound.z / scale.z ); + int maxRow = (int)floorf( aabb.upperBound.z / scale.z ); + int minCol = (int)floorf( aabb.lowerBound.x / scale.x ); + int maxCol = (int)floorf( aabb.upperBound.x / scale.x ); + + b3V32 boundsMin = b3LoadV( &aabb.lowerBound.x ); + b3V32 boundsMax = b3LoadV( &aabb.upperBound.x ); + b3V32 boundsCenter = b3MulV( b3_halfV, b3AddV( boundsMin, boundsMax ) ); + b3V32 boundsExtent = b3SubV( boundsMax, boundsCenter ); + + b3DistanceInput input; + input.proxyB = localProxy; + input.transform = b3Transform_identity; + input.useRadii = true; + + b3SimplexCache cache = { 0 }; + + // Outer loop on rows and inner loop on columns so that triangle indices + // increase monotonically. + for ( int row = minRow; row <= maxRow; ++row ) + { + if ( row < 0 || shape->rowCount - 1 <= row ) + { + continue; + } + + for ( int column = minCol; column <= maxCol; ++column ) + { + if ( column < 0 || shape->columnCount - 1 <= column ) + { + continue; + } + + int cellIndex = row * ( shape->columnCount - 1 ) + column; + B3_ASSERT( cellIndex < ( shape->rowCount - 1 ) * ( shape->columnCount - 1 ) ); + uint8_t material = b3GetHeightFieldMaterialIndices( shape )[cellIndex]; + if ( material == B3_HEIGHT_FIELD_HOLE ) + { + continue; + } + + b3Vec3 corners[4]; + b3GetHeightFieldCellCorners( shape, row, column, corners ); + b3Vec3 point11 = corners[0]; + b3Vec3 point12 = corners[1]; + b3Vec3 point21 = corners[2]; + b3Vec3 point22 = corners[3]; + + b3V32 v11 = b3LoadV( &point11.x ); + b3V32 v12 = b3LoadV( &point12.x ); + b3V32 v21 = b3LoadV( &point21.x ); + b3V32 v22 = b3LoadV( &point22.x ); + + if ( b3TestBoundsTriangleOverlap( boundsCenter, boundsExtent, v11, v21, v12 ) ) + { + b3Vec3 triangleVertices[] = { point11, point21, point12 }; + input.proxyA = (b3ShapeProxy){ triangleVertices, 3, 0.0f }; + + // reset the cache + cache.count = 0; + + // get distance between triangle and query shape + b3DistanceOutput output = b3ShapeDistance( &input, &cache, NULL, 0 ); + + float tolerance = 0.1f * B3_LINEAR_SLOP; + if ( output.distance < tolerance ) + { + // overlap detected + return true; + } + } + + if ( b3TestBoundsTriangleOverlap( boundsCenter, boundsExtent, v21, v22, v12 ) ) + { + b3Vec3 triangleVertices[] = { point22, point12, point21 }; + input.proxyA = (b3ShapeProxy){ triangleVertices, 3, 0.0f }; + + // reset the cache + cache.count = 0; + + // get distance between triangle and query shape + b3DistanceOutput output = b3ShapeDistance( &input, &cache, NULL, 0 ); + + float tolerance = 0.1f * B3_LINEAR_SLOP; + if ( output.distance < tolerance ) + { + // overlap detected + return true; + } + } + } + } + + return false; +} + +void b3QueryHeightField( const b3HeightFieldData* heightField, b3AABB bounds, b3MeshQueryFcn* fcn, void* context ) +{ + b3Vec3 scale = heightField->scale; + + int minRow = (int)floorf( bounds.lowerBound.z / scale.z ); + int maxRow = (int)floorf( bounds.upperBound.z / scale.z ); + int minCol = (int)floorf( bounds.lowerBound.x / scale.x ); + int maxCol = (int)floorf( bounds.upperBound.x / scale.x ); + + // Outer loop on rows and inner loop on columns so that triangle indices + // increase monotonically. + for ( int row = minRow; row <= maxRow; ++row ) + { + if ( row < 0 || heightField->rowCount - 1 <= row ) + { + continue; + } + + for ( int column = minCol; column <= maxCol; ++column ) + { + if ( column < 0 || heightField->columnCount - 1 <= column ) + { + continue; + } + + int cellIndex = row * ( heightField->columnCount - 1 ) + column; + B3_ASSERT( cellIndex < ( heightField->rowCount - 1 ) * ( heightField->columnCount - 1 ) ); + uint8_t material = b3GetHeightFieldMaterialIndices( heightField )[cellIndex]; + if ( material == B3_HEIGHT_FIELD_HOLE ) + { + continue; + } + + b3Vec3 corners[4]; + b3GetHeightFieldCellCorners( heightField, row, column, corners ); + b3Vec3 point11 = corners[0]; + b3Vec3 point12 = corners[1]; + b3Vec3 point21 = corners[2]; + b3Vec3 point22 = corners[3]; + + // I know the min/max x and z values, but not the min/max heights. + // This could be done with no branching in SIMD. + b3AABB cellBound; + cellBound.lowerBound = b3Min( b3Min( point11, point12 ), b3Min( point21, point22 ) ); + cellBound.upperBound = b3Max( b3Max( point11, point12 ), b3Max( point21, point22 ) ); + + if ( b3AABB_Overlaps( bounds, cellBound ) ) + { + int quadIndex = row * ( heightField->columnCount - 1 ) + column; + int triangleIndex = 2 * quadIndex; + + if ( heightField->clockwise ) + { + fcn( point11, point12, point21, triangleIndex, context ); + fcn( point22, point21, point12, triangleIndex + 1, context ); + } + else + { + fcn( point11, point21, point12, triangleIndex, context ); + fcn( point22, point12, point21, triangleIndex + 1, context ); + } + } + } + } +} + +int b3CollideMoverAndHeightField( b3PlaneResult* planes, int capacity, const b3HeightFieldData* shape, const b3Capsule* mover ) +{ + b3DistanceInput distanceInput = { 0 }; + distanceInput.proxyB = (b3ShapeProxy){ &mover->center1, 2, 0.0f }; + distanceInput.transform = b3Transform_identity; + distanceInput.useRadii = false; + + b3SimplexCache cache = { 0 }; + + float radius = mover->radius; + b3V32 center1 = b3LoadV( &mover->center1.x ); + b3V32 center2 = b3LoadV( &mover->center2.x ); + b3V32 r = b3SplatV( radius ); + b3V32 boundsMin = b3SubV( b3MinV( center1, center2 ), r ); + b3V32 boundsMax = b3AddV( b3MaxV( center1, center2 ), r ); + b3V32 boundsCenter = b3MulV( b3_halfV, b3AddV( boundsMin, boundsMax ) ); + b3V32 boundsExtent = b3SubV( boundsMax, boundsCenter ); + + float localMinX = b3GetXV( boundsMin ); + float localMinZ = b3GetZV( boundsMin ); + float localMaxX = b3GetXV( boundsMax ); + float localMaxZ = b3GetZV( boundsMax ); + + b3Vec3 scale = shape->scale; + int minRow = (int)floorf( localMinZ / scale.z ); + int maxRow = (int)floorf( localMaxZ / scale.z ); + int minCol = (int)floorf( localMinX / scale.x ); + int maxCol = (int)floorf( localMaxX / scale.x ); + + int planeCount = 0; + + // Outer loop on rows and inner loop on columns so that triangle indices + // increase monotonically. + for ( int row = minRow; row <= maxRow; ++row ) + { + if ( row < 0 || shape->rowCount - 1 <= row ) + { + continue; + } + + for ( int column = minCol; column <= maxCol; ++column ) + { + if ( column < 0 || shape->columnCount - 1 <= column ) + { + continue; + } + + int cellIndex = row * ( shape->columnCount - 1 ) + column; + B3_ASSERT( cellIndex < ( shape->rowCount - 1 ) * ( shape->columnCount - 1 ) ); + uint8_t material = b3GetHeightFieldMaterialIndices( shape )[cellIndex]; + if ( material == B3_HEIGHT_FIELD_HOLE ) + { + continue; + } + + b3Vec3 corners[4]; + b3GetHeightFieldCellCorners( shape, row, column, corners ); + b3Vec3 point11 = corners[0]; + b3Vec3 point12 = corners[1]; + b3Vec3 point21 = corners[2]; + b3Vec3 point22 = corners[3]; + + b3V32 v11 = b3LoadV( &point11.x ); + b3V32 v12 = b3LoadV( &point12.x ); + b3V32 v21 = b3LoadV( &point21.x ); + b3V32 v22 = b3LoadV( &point22.x ); + + if ( b3TestBoundsTriangleOverlap( boundsCenter, boundsExtent, v11, v21, v12 ) ) + { + b3Vec3 triangleVertices[] = { point11, point21, point12 }; + distanceInput.proxyA = (b3ShapeProxy){ triangleVertices, 3, 0.0f }; + + // reset the cache + cache.count = 0; + + // get distance between triangle and mover + b3DistanceOutput distanceOutput = b3ShapeDistance( &distanceInput, &cache, NULL, 0 ); + + if ( distanceOutput.distance == 0.0f ) + { + // todo SAT + } + else if ( distanceOutput.distance <= mover->radius ) + { + b3Plane plane = { distanceOutput.normal, mover->radius - distanceOutput.distance }; + planes[planeCount] = (b3PlaneResult){ plane, distanceOutput.pointA }; + planeCount += 1; + + if ( planeCount == capacity ) + { + return planeCount; + } + } + } + + if ( b3TestBoundsTriangleOverlap( boundsCenter, boundsExtent, v21, v22, v12 ) ) + { + b3Vec3 triangleVertices[] = { point22, point12, point21 }; + distanceInput.proxyA = (b3ShapeProxy){ triangleVertices, 3, 0.0f }; + + // reset the cache + cache.count = 0; + + // get distance between triangle and mover + b3DistanceOutput distanceOutput = b3ShapeDistance( &distanceInput, &cache, NULL, 0 ); + + if ( distanceOutput.distance == 0.0f ) + { + // todo SAT + } + else if ( distanceOutput.distance <= mover->radius ) + { + b3Plane plane = { distanceOutput.normal, mover->radius - distanceOutput.distance }; + planes[planeCount] = (b3PlaneResult){ plane, distanceOutput.pointA }; + planeCount += 1; + + if ( planeCount == capacity ) + { + return planeCount; + } + } + } + } + } + + return planeCount; +} + +b3HeightFieldData* b3CreateGrid( int rowCount, int columnCount, b3Vec3 scale, bool makeHoles ) +{ + int heightCount = rowCount * columnCount; + float* heights = (float*)b3Alloc( heightCount * sizeof( float ) ); + + for ( int i = 0; i < rowCount; ++i ) + { + for ( int j = 0; j < columnCount; ++j ) + { + int k = i * columnCount + j; + heights[k] = 0.0f; + } + } + + int cellCount = ( rowCount - 1 ) * ( columnCount - 1 ); + uint8_t* materialIndices = (uint8_t*)b3Alloc( cellCount * sizeof( uint8_t ) ); + + for ( int i = 0; i < rowCount - 1; ++i ) + { + for ( int j = 0; j < columnCount - 1; ++j ) + { + int k = i * ( columnCount - 1 ) + j; + + if ( makeHoles && k > 0 && k % 16 == 0 ) + { + materialIndices[k] = B3_HEIGHT_FIELD_HOLE; + } + else + { + materialIndices[k] = 0; + } + } + } + + b3HeightFieldDef data = { 0 }; + data.heights = heights; + data.materialIndices = materialIndices; + data.scale = scale; + data.countX = columnCount; + data.countZ = rowCount; + data.globalMinimumHeight = -256.0f; + data.globalMaximumHeight = 256.0f; + data.clockwiseWinding = false; + + b3HeightFieldData* heightField = b3CreateHeightField( &data ); + + b3Free( heights, heightCount * sizeof( float ) ); + b3Free( materialIndices, cellCount * sizeof( uint8_t ) ); + + return heightField; +} + +b3HeightFieldData* b3CreateWave( int rowCount, int columnCount, b3Vec3 scale, float rowFrequency, float columnFrequency, + bool makeHoles ) +{ + int heightCount = rowCount * columnCount; + float* heights = (float*)b3Alloc( heightCount * sizeof( float ) ); + + float omegaZ = 2.0f * B3_PI * rowFrequency; + float omegaX = 2.0f * B3_PI * columnFrequency; + + for ( int i = 0; i < rowCount; ++i ) + { + float rowHeight = sinf( omegaZ * i ); + + for ( int j = 0; j < columnCount; ++j ) + { + int k = i * columnCount + j; + float columnHeight = sinf( omegaX * j ); + heights[k] = rowHeight * columnHeight; + } + } + + int cellCount = ( rowCount - 1 ) * ( columnCount - 1 ); + uint8_t* materialIndices = (uint8_t*)b3Alloc( cellCount * sizeof( uint8_t ) ); + + for ( int i = 0; i < rowCount - 1; ++i ) + { + for ( int j = 0; j < columnCount - 1; ++j ) + { + int k = i * ( columnCount - 1 ) + j; + + if ( makeHoles && k > 0 && k % 16 == 0 ) + { + materialIndices[k] = B3_HEIGHT_FIELD_HOLE; + } + else + { + materialIndices[k] = 0; + } + } + } + + b3HeightFieldDef data = { 0 }; + data.heights = heights; + data.materialIndices = materialIndices; + data.scale = scale; + data.countX = columnCount; + data.countZ = rowCount; + data.globalMinimumHeight = -256.0f; + data.globalMaximumHeight = 256.0f; + data.clockwiseWinding = false; + + b3HeightFieldData* heightField = b3CreateHeightField( &data ); + + b3Free( heights, heightCount * sizeof( float ) ); + b3Free( materialIndices, cellCount * sizeof( uint8_t ) ); + + return heightField; +} + +void b3DestroyHeightField( b3HeightFieldData* heightField ) +{ + b3Free( heightField, heightField->byteCount ); +} + +void b3DumpHeightData( const b3HeightFieldDef* data, const char* fileName ) +{ + FILE* file = NULL; + +#if defined( _MSC_VER ) + errno_t e = fopen_s( &file, fileName, "w" ); + if ( e != 0 ) + { + return; + } +#else + file = fopen( fileName, "w" ); + if ( file == NULL ) + { + return; + } +#endif + + fprintf( file, "%d %d\n", data->countX, data->countZ ); + fprintf( file, "%.9f %.9f %.9f\n", data->scale.x, data->scale.y, data->scale.z ); + fprintf( file, "%.9f %.9f\n", data->globalMinimumHeight, data->globalMaximumHeight ); + fprintf( file, "%d\n", data->clockwiseWinding ); + + int heightCount = data->countX * data->countZ; + for ( int i = 0; i < heightCount; ++i ) + { + fprintf( file, "%.9f\n", data->heights[i] ); + } + + int materialCount = ( data->countX - 1 ) * ( data->countZ - 1 ); + for ( int i = 0; i < materialCount; ++i ) + { + fprintf( file, "%d\n", data->materialIndices[i] ); + } + + fclose( file ); +} + +#if defined( _MSC_VER ) +#define B3_FILE_SCAN fscanf_s +#else +#define B3_FILE_SCAN fscanf +#endif + +b3HeightFieldData* b3LoadHeightField( const char* fileName ) +{ + FILE* file = NULL; + +#if defined( _MSC_VER ) + errno_t e = fopen_s( &file, fileName, "r" ); + if ( e != 0 ) + { + return NULL; + } +#else + file = fopen( fileName, "r" ); + if ( file == NULL ) + { + return NULL; + } +#endif + + b3HeightFieldDef data = { 0 }; + + // Read dimensions + if ( B3_FILE_SCAN( file, "%d %d", &data.countX, &data.countZ ) != 2 ) + { + fclose( file ); + return NULL; + } + + // Read scale + if ( B3_FILE_SCAN( file, "%f %f %f", &data.scale.x, &data.scale.y, &data.scale.z ) != 3 ) + { + fclose( file ); + return NULL; + } + + // Read global height bounds + if ( B3_FILE_SCAN( file, "%f %f", &data.globalMinimumHeight, &data.globalMaximumHeight ) != 2 ) + { + fclose( file ); + return NULL; + } + + // Read clockwise winding + int clockwise; + if ( B3_FILE_SCAN( file, "%d", &clockwise ) != 1 ) + { + fclose( file ); + return NULL; + } + data.clockwiseWinding = clockwise != 0; + + // Allocate and read height data + int heightCount = data.countX * data.countZ; + data.heights = (float*)b3Alloc( heightCount * sizeof( float ) ); + + for ( int i = 0; i < heightCount; ++i ) + { + if ( B3_FILE_SCAN( file, "%f", &data.heights[i] ) != 1 ) + { + b3Free( data.heights, heightCount * sizeof( float ) ); + fclose( file ); + return NULL; + } + } + + // Allocate and read material indices + int materialCount = ( data.countX - 1 ) * ( data.countZ - 1 ); + data.materialIndices = (uint8_t*)b3Alloc( materialCount * sizeof( uint8_t ) ); + + for ( int i = 0; i < materialCount; ++i ) + { + int materialIndex; + if ( B3_FILE_SCAN( file, "%d", &materialIndex ) != 1 ) + { + b3Free( data.heights, heightCount * sizeof( float ) ); + b3Free( data.materialIndices, materialCount * sizeof( uint8_t ) ); + fclose( file ); + return NULL; + } + data.materialIndices[i] = (uint8_t)materialIndex; + } + + fclose( file ); + + // Create height field from loaded data + b3HeightFieldData* heightField = b3CreateHeightField( &data ); + + // Clean up temporary allocations + b3Free( data.heights, heightCount * sizeof( float ) ); + b3Free( data.materialIndices, materialCount * sizeof( uint8_t ) ); + + return heightField; +} diff --git a/vendor/box3d/src/src/hull.c b/vendor/box3d/src/src/hull.c new file mode 100644 index 000000000..75bace9f9 --- /dev/null +++ b/vendor/box3d/src/src/hull.c @@ -0,0 +1,2791 @@ +// SPDX-FileCopyrightText: 2026 Erin Catto +// SPDX-License-Identifier: MIT + +// Dirk Gregorius contributed portions of this code + +#include "algorithm.h" +#include "hull_map.h" +#include "math_internal.h" +#include "shape.h" + +#include "box3d/collision.h" +#include "box3d/constants.h" +#include "box3d/math_functions.h" + +#include +#include +#include +#include +#include +#include + +#define B3_AXIS_X 0 +#define B3_AXIS_Y 1 +#define B3_AXIS_Z 2 + +#define B3_MARK_VISIBLE 0 +#define B3_MARK_DELETE 1 + +// Final hull is index-encoded with uint8_t, so vertex/edge/face counts are capped at UINT8_MAX. +#define B3_HULL_LIMIT UINT8_MAX + +typedef struct b3QHListNode +{ + struct b3QHListNode* prev; + struct b3QHListNode* next; +} b3QHListNode; + +typedef struct b3QHFace b3QHFace; + +typedef struct b3QHVertex +{ + // Intrusive list link. Must be first so (b3QHVertex*)nodePtr is valid. + b3QHListNode link; + + b3QHFace* conflictFace; + b3Vec3 position; + + // Index in the finalized hull, stamped during emit. B3_NULL_INDEX until then. + int finalIndex; + bool reachable; +} b3QHVertex; + +typedef struct b3QHHalfEdge +{ + // Edge ring (CCW) around the owning face. Not an external list. + struct b3QHHalfEdge* prev; + struct b3QHHalfEdge* next; + + b3QHVertex* origin; + b3QHFace* face; + struct b3QHHalfEdge* twin; + + // Index in the finalized hull, stamped during emit. B3_NULL_INDEX until then. + int finalIndex; +} b3QHHalfEdge; + +struct b3QHFace +{ + // Intrusive list link. Must be first so (b3QHFace*)nodePtr is valid. + b3QHListNode link; + + b3QHHalfEdge* edge; + + int mark; + float area; + b3Plane plane; + b3Vec3 centroid; + float maxConflictDistance; + + // Sentinel head for this face's conflict list of b3QHVertex. + b3QHVertex conflictListHead; + + // Cached farthest conflict vertex (above b3HullBuilder::minOutside). + // NULL when no conflict above threshold; maxConflictDistance is then minOutside. + b3QHVertex* maxConflict; + + // Index in the finalized hull, stamped during emit. B3_NULL_INDEX until then. + int finalIndex; + bool flipped; +}; + +// One frame of the iterative horizon DFS. Replaces a recursive call to b3HullBuilder_BuildHorizon. +typedef struct b3HorizonFrame +{ + b3QHFace* face; + b3QHHalfEdge* startEdge; // ring termination sentinel + b3QHHalfEdge* edge; // next edge to process + bool started; // false until the first edge of this ring has been processed +} b3HorizonFrame; + +// All working memory for one hull build, carved from a single b3Alloc block. +typedef struct b3HullBuilder +{ + float tolerance; + float minRadius; + float minOutside; + + b3Vec3 interiorPoint; + + // List sentinels. Only the link is meaningful; other fields are unused. + b3QHVertex orphanedList; + b3QHVertex vertexList; + b3QHFace faceList; + + // Bump-allocated pools with pointer-based free lists for faces and edges. + b3QHVertex* vertexBase; + int vertexCapacity; + int vertexCount; + + b3QHHalfEdge* edgeBase; + int edgeCapacity; + int edgeCount; + b3QHHalfEdge* edgeFreeHead; // LIFO free list; overlays edge->next + + b3QHFace* faceBase; + int faceCapacity; + int faceCount; + b3QHFace* faceFreeHead; // LIFO free list; overlays face->link.next + + // Reusable scratch buffers. + b3QHHalfEdge** horizon; + int horizonCapacity; + int horizonCount; + + b3QHFace** cone; + int coneCapacity; + int coneCount; + + b3QHFace** mergedFaces; + int mergedFacesCapacity; + int mergedFacesCount; + + // DFS stack used by the iterative b3HullBuilder_BuildHorizon. Depth bounded by live faces. + b3HorizonFrame* horizonStack; + int horizonStackCapacity; + + // Final counts of the constructed hull (vertexList / faceList / half-edges around faces). + // Populated by CleanHull; zero until then. + int finalVertexCount; + int finalHalfEdgeCount; + int finalFaceCount; +} b3HullBuilder; + +static inline void b3QHList_Init( b3QHListNode* head ) +{ + head->prev = head; + head->next = head; +} + +#define B3_LIST_EMPTY( A ) ( ( A )->next == ( A ) ) + +static inline bool b3QHList_Contains( const b3QHListNode* node ) +{ + return node->prev != NULL && node->next != NULL; +} + +// Insert node before `where`. +static inline void b3QHList_Insert( b3QHListNode* node, b3QHListNode* where ) +{ + B3_ASSERT( !b3QHList_Contains( node ) && b3QHList_Contains( where ) ); + + node->prev = where->prev; + node->next = where; + + node->prev->next = node; + node->next->prev = node; +} + +static inline void b3QHList_Remove( b3QHListNode* node ) +{ + B3_ASSERT( b3QHList_Contains( node ) ); + + node->prev->next = node->next; + node->next->prev = node->prev; + + node->prev = NULL; + node->next = NULL; +} + +static inline void b3QHList_PushBack( b3QHListNode* head, b3QHListNode* node ) +{ + b3QHList_Insert( node, head->prev ); +} + +static b3QHVertex* b3HullBuilder_NewVertex( b3HullBuilder* b, b3Vec3 position ) +{ + B3_ASSERT( b->vertexCount < b->vertexCapacity ); + b3QHVertex* vertex = b->vertexBase + b->vertexCount++; + + vertex->link.prev = NULL; + vertex->link.next = NULL; + vertex->conflictFace = NULL; + vertex->position = position; + vertex->finalIndex = B3_NULL_INDEX; + vertex->reachable = false; + + return vertex; +} + +static b3QHHalfEdge* b3HullBuilder_NewEdge( b3HullBuilder* b ) +{ + b3QHHalfEdge* edge; + if ( b->edgeFreeHead != NULL ) + { + edge = b->edgeFreeHead; + b->edgeFreeHead = edge->next; + } + else + { + B3_ASSERT( b->edgeCount < b->edgeCapacity ); + edge = b->edgeBase + b->edgeCount++; + } + // All other fields (prev/next/origin/face/twin) are written by NewFace immediately after. + edge->finalIndex = B3_NULL_INDEX; + return edge; +} + +static void b3HullBuilder_RetireEdge( b3HullBuilder* b, b3QHHalfEdge* edge ) +{ + edge->next = b->edgeFreeHead; + b->edgeFreeHead = edge; +} + +static b3QHFace* b3HullBuilder_NewFace( b3HullBuilder* b, b3QHVertex* v1, b3QHVertex* v2, b3QHVertex* v3 ) +{ + b3QHFace* face; + if ( b->faceFreeHead != NULL ) + { + face = b->faceFreeHead; + // link.next was used as free-list pointer; recover next head before we clobber. + b->faceFreeHead = (b3QHFace*)face->link.next; + } + else + { + B3_ASSERT( b->faceCount < b->faceCapacity ); + face = b->faceBase + b->faceCount++; + } + + // link.prev: NULL on retired faces (cleared by Remove); NULL here so PushBack's + // !b3QHList_Contains assert holds for fresh bump slots too. + // link.next: was the free-list pointer on reused slots, now stale; PushBack overwrites. + face->link.prev = NULL; + face->link.next = NULL; + face->maxConflict = NULL; + face->maxConflictDistance = 0.0f; + face->finalIndex = B3_NULL_INDEX; + + b3QHHalfEdge* edge1 = b3HullBuilder_NewEdge( b ); + b3QHHalfEdge* edge2 = b3HullBuilder_NewEdge( b ); + b3QHHalfEdge* edge3 = b3HullBuilder_NewEdge( b ); + + b3Vec3 p1 = v1->position; + b3Vec3 p2 = v2->position; + b3Vec3 p3 = v3->position; + + b3Plane plane; + plane.normal = b3Cross( b3Sub( p2, p1 ), b3Sub( p3, p1 ) ); + float length; + plane.normal = b3GetLengthAndNormalize( &length, plane.normal ); + plane.offset = b3Dot( plane.normal, p1 ); + + float area = 0.5f * length; + + face->edge = edge1; + face->mark = B3_MARK_VISIBLE; + face->area = area; + face->centroid = b3MulSV( 1.0f / 3.0f, b3Add( v1->position, b3Add( v2->position, v3->position ) ) ); + face->plane = plane; + face->flipped = b3PlaneSeparation( plane, b->interiorPoint ) > 0.0f; + b3QHList_Init( &face->conflictListHead.link ); + + edge1->prev = edge3; + edge1->next = edge2; + edge1->origin = v1; + edge1->face = face; + edge1->twin = NULL; + + edge2->prev = edge1; + edge2->next = edge3; + edge2->origin = v2; + edge2->face = face; + edge2->twin = NULL; + + edge3->prev = edge2; + edge3->next = edge1; + edge3->origin = v3; + edge3->face = face; + edge3->twin = NULL; + + return face; +} + +// Remove face from faceList if still linked, clear its edge pointer, then push onto faceFreeHead. +// Uses face->link.next as the free-list next pointer (link.prev stays NULL, so b3QHList_Contains +// returns false on a free slot, as required by the retire-guard in ResolveFaces). +static void b3HullBuilder_RetireFace( b3HullBuilder* b, b3QHFace* face ) +{ + if ( b3QHList_Contains( &face->link ) ) + { + b3QHList_Remove( &face->link ); + } + face->edge = NULL; + // link.prev is already NULL after Remove (or was never set). link.next holds free-list ptr. + face->link.next = (b3QHListNode*)b->faceFreeHead; + b->faceFreeHead = face; +} + +static b3AABB b3BuildBounds( int vertexCount, const b3Vec3* vertices ) +{ + b3AABB bounds = B3_BOUNDS3_EMPTY; + for ( int i = 0; i < vertexCount; ++i ) + { + bounds.lowerBound = b3Min( bounds.lowerBound, vertices[i] ); + bounds.upperBound = b3Max( bounds.upperBound, vertices[i] ); + } + return bounds; +} + +static void b3FindFarthestPointsAlongCardinalAxes( int* index1Out, int* index2Out, float tolerance, int vertexCount, + const b3Vec3* vertexBase ) +{ + *index1Out = B3_NULL_INDEX; + *index2Out = B3_NULL_INDEX; + + b3Vec3 v0 = vertexBase[0]; + b3Vec3 minPt[3] = { v0, v0, v0 }; + b3Vec3 maxPt[3] = { v0, v0, v0 }; + + int minIndex[3] = { 0, 0, 0 }; + int maxIndex[3] = { 0, 0, 0 }; + + for ( int i = 1; i < vertexCount; ++i ) + { + b3Vec3 v = vertexBase[i]; + + if ( v.x < minPt[B3_AXIS_X].x ) + { + minPt[B3_AXIS_X] = v; + minIndex[B3_AXIS_X] = i; + } + else if ( v.x > maxPt[B3_AXIS_X].x ) + { + maxPt[B3_AXIS_X] = v; + maxIndex[B3_AXIS_X] = i; + } + + if ( v.y < minPt[B3_AXIS_Y].y ) + { + minPt[B3_AXIS_Y] = v; + minIndex[B3_AXIS_Y] = i; + } + else if ( v.y > maxPt[B3_AXIS_Y].y ) + { + maxPt[B3_AXIS_Y] = v; + maxIndex[B3_AXIS_Y] = i; + } + + if ( v.z < minPt[B3_AXIS_Z].z ) + { + minPt[B3_AXIS_Z] = v; + minIndex[B3_AXIS_Z] = i; + } + else if ( v.z > maxPt[B3_AXIS_Z].z ) + { + maxPt[B3_AXIS_Z] = v; + maxIndex[B3_AXIS_Z] = i; + } + } + + b3Vec3 distance; + distance.x = maxPt[B3_AXIS_X].x - minPt[B3_AXIS_X].x; + distance.y = maxPt[B3_AXIS_Y].y - minPt[B3_AXIS_Y].y; + distance.z = maxPt[B3_AXIS_Z].z - minPt[B3_AXIS_Z].z; + + float distanceArray[3] = { distance.x, distance.y, distance.z }; + int maxElement = b3MaxElementIndex( distance ); + + if ( distanceArray[maxElement] > 2.0f * tolerance ) + { + *index1Out = minIndex[maxElement]; + *index2Out = maxIndex[maxElement]; + } +} + +static int b3FindFarthestPointFromLine( int index1, int index2, float tolerance, int vertexCount, const b3Vec3* vertexBase ) +{ + b3Vec3 a = vertexBase[index1]; + b3Vec3 b = vertexBase[index2]; + + // |ap x ab|^2 / |ab|^2 is the squared perpendicular distance from p to the line. + // Compares against (2 * tolerance)^2 + b3Vec3 ab = b3Sub( b, a ); + float abLengthSqr = b3Dot( ab, ab ); + B3_ASSERT( abLengthSqr > 0.0f ); + + float invAbLengthSqr = 1.0f / abLengthSqr; + float maxDistanceSqr = 4.0f * tolerance * tolerance; + int maxIndex = B3_NULL_INDEX; + + for ( int i = 0; i < vertexCount; ++i ) + { + if ( i == index1 || i == index2 ) + { + continue; + } + + b3Vec3 ap = b3Sub( vertexBase[i], a ); + b3Vec3 cross = b3Cross( ap, ab ); + float distanceSqr = b3Dot( cross, cross ) * invAbLengthSqr; + if ( distanceSqr > maxDistanceSqr ) + { + maxDistanceSqr = distanceSqr; + maxIndex = i; + } + } + + return maxIndex; +} + +static int b3FindFarthestPointFromPlane( int index1, int index2, int index3, float tolerance, int vertexCount, + const b3Vec3* vertexBase ) +{ + b3Vec3 a = vertexBase[index1]; + b3Vec3 b = vertexBase[index2]; + b3Vec3 c = vertexBase[index3]; + + b3Plane plane = b3MakePlaneFromPoints( a, b, c ); + + float maxDistance = 2.0f * tolerance; + int maxIndex = B3_NULL_INDEX; + + for ( int i = 0; i < vertexCount; ++i ) + { + if ( i == index1 || i == index2 || i == index3 ) + { + continue; + } + + float distance = b3AbsFloat( b3PlaneSeparation( plane, vertexBase[i] ) ); + if ( distance > maxDistance ) + { + maxDistance = distance; + maxIndex = i; + } + } + + return maxIndex; +} + +static bool b3IsEdgeConvex( const b3QHHalfEdge* edge, float tolerance ) +{ + float distance = b3PlaneSeparation( edge->face->plane, edge->twin->face->centroid ); + return distance < -tolerance; +} + +static bool b3IsEdgeConcave( const b3QHHalfEdge* edge, float tolerance ) +{ + float distance = b3PlaneSeparation( edge->face->plane, edge->twin->face->centroid ); + return distance > tolerance; +} + +static int b3VertexCountOfFace( const b3QHFace* face ) +{ + int count = 0; + const b3QHHalfEdge* edge = face->edge; + do + { + count++; + edge = edge->next; + } + while ( edge != face->edge ); + + return count; +} + +static void b3LinkFace( b3QHFace* face, int index, b3QHHalfEdge* twin ) +{ + B3_ASSERT( face != twin->face ); + + b3QHHalfEdge* edge = face->edge; + while ( index-- > 0 ) + { + B3_ASSERT( edge->face == face ); + edge = edge->next; + } + + B3_ASSERT( edge != twin ); + edge->twin = twin; + twin->twin = edge; +} + +static void b3LinkFaces( b3QHFace* face1, int index1, b3QHFace* face2, int index2 ) +{ + B3_ASSERT( face1 != face2 ); + + b3QHHalfEdge* edge1 = face1->edge; + while ( index1-- > 0 ) + { + edge1 = edge1->next; + } + + b3QHHalfEdge* edge2 = face2->edge; + while ( index2-- > 0 ) + { + edge2 = edge2->next; + } + + B3_ASSERT( edge1 != edge2 ); + edge1->twin = edge2; + edge2->twin = edge1; +} + +static void b3NewellPlane( b3QHFace* face ) +{ + int count = 0; + b3Vec3 centroid = b3Vec3_zero; + b3Vec3 normal = b3Vec3_zero; + + b3QHHalfEdge* edge = face->edge; + B3_ASSERT( edge->face == face ); + + // Use the first vertex as the origin to reduce round-off + b3Vec3 origin = edge->origin->position; + + do + { + b3QHHalfEdge* twin = edge->twin; + B3_ASSERT( twin->twin == edge ); + + b3Vec3 v1 = b3Sub( edge->origin->position, origin ); + b3Vec3 v2 = b3Sub( twin->origin->position, origin ); + + count++; + centroid = b3Add( centroid, v1 ); + normal.x += ( v1.y - v2.y ) * ( v1.z + v2.z ); + normal.y += ( v1.z - v2.z ) * ( v1.x + v2.x ); + normal.z += ( v1.x - v2.x ) * ( v1.y + v2.y ); + + edge = edge->next; + } + while ( edge != face->edge ); + + B3_ASSERT( count > 0 ); + centroid = b3MulSV( 1.0f / (float)count, centroid ); + centroid = b3Add( centroid, origin ); + + float length = b3Length( normal ); + B3_VALIDATE( length > 0.0f ); + normal = b3MulSV( 1.0f / length, normal ); + + face->centroid = centroid; + face->plane = b3MakePlaneFromNormalAndPoint( normal, centroid ); + face->area = 0.5f * length; +} + +#if B3_DEBUG +static bool b3CheckConsistency( const b3QHFace* face ) +{ + if ( face->mark == B3_MARK_DELETE ) + { + return false; + } + + if ( b3VertexCountOfFace( face ) < 3 ) + { + return false; + } + + const b3QHHalfEdge* edge = face->edge; + + do + { + const b3QHHalfEdge* twin = edge->twin; + + if ( twin == NULL ) + { + return false; + } + if ( twin->face == NULL ) + { + return false; + } + if ( twin->face == face ) + { + return false; + } + if ( twin->face->mark == B3_MARK_DELETE ) + { + return false; + } + if ( twin->twin != edge ) + { + return false; + } + if ( edge->next->origin != twin->origin ) + { + return false; + } + if ( edge->origin != twin->next->origin ) + { + return false; + } + if ( edge->face != face ) + { + return false; + } + + edge = edge->next; + } + while ( edge != face->edge ); + + return true; +} +#endif + +static void b3HullBuilder_ComputeTolerance( b3HullBuilder* b, int pointCount, const b3Vec3* points ) +{ + b3AABB bounds = b3BuildBounds( pointCount, points ); + b3Vec3 maxAbs = b3Max( b3Abs( bounds.lowerBound ), b3Abs( bounds.upperBound ) ); + + float maxSum = maxAbs.x + maxAbs.y + maxAbs.z; + float maxCoord = b3MaxFloat( maxAbs.x, b3MaxFloat( maxAbs.y, maxAbs.z ) ); + float maxDistance = b3MinFloat( B3_SQRT3 * maxCoord, maxSum ); + + float tolerance = ( 3.0f * maxDistance * 1.01f + maxCoord ) * FLT_EPSILON; + + b->tolerance = tolerance; + b->minRadius = 4.0f * b->tolerance; + b->minOutside = 2.0f * b->minRadius; + B3_ASSERT( b->minRadius < b->minOutside + 3.0f * FLT_EPSILON ); +} + +static bool b3HullBuilder_BuildInitialHull( b3HullBuilder* b, int pointCount, const b3Vec3* points ) +{ + int index1, index2; + b3FindFarthestPointsAlongCardinalAxes( &index1, &index2, b->tolerance, pointCount, points ); + if ( index1 < 0 || index2 < 0 ) + { + return false; + } + + int index3 = b3FindFarthestPointFromLine( index1, index2, b->tolerance, pointCount, points ); + if ( index3 < 0 ) + { + return false; + } + + int index4 = b3FindFarthestPointFromPlane( index1, index2, index3, b->tolerance, pointCount, points ); + if ( index4 < 0 ) + { + return false; + } + + b3Vec3 v1 = b3Sub( points[index1], points[index4] ); + b3Vec3 v2 = b3Sub( points[index2], points[index4] ); + b3Vec3 v3 = b3Sub( points[index3], points[index4] ); + + if ( b3ScalarTripleProduct( v1, v2, v3 ) < 0.0f ) + { + int temp = index2; + index2 = index3; + index3 = temp; + } + + b->interiorPoint = b3Vec3_zero; + b->interiorPoint = b3Add( b->interiorPoint, points[index1] ); + b->interiorPoint = b3Add( b->interiorPoint, points[index2] ); + b->interiorPoint = b3Add( b->interiorPoint, points[index3] ); + b->interiorPoint = b3Add( b->interiorPoint, points[index4] ); + b->interiorPoint = b3MulSV( 0.25f, b->interiorPoint ); + + b3QHVertex* vertex1 = b3HullBuilder_NewVertex( b, points[index1] ); + b3QHList_PushBack( &b->vertexList.link, &vertex1->link ); + b3QHVertex* vertex2 = b3HullBuilder_NewVertex( b, points[index2] ); + b3QHList_PushBack( &b->vertexList.link, &vertex2->link ); + b3QHVertex* vertex3 = b3HullBuilder_NewVertex( b, points[index3] ); + b3QHList_PushBack( &b->vertexList.link, &vertex3->link ); + b3QHVertex* vertex4 = b3HullBuilder_NewVertex( b, points[index4] ); + b3QHList_PushBack( &b->vertexList.link, &vertex4->link ); + + b3QHFace* face1 = b3HullBuilder_NewFace( b, vertex1, vertex2, vertex3 ); + b3QHList_PushBack( &b->faceList.link, &face1->link ); + b3QHFace* face2 = b3HullBuilder_NewFace( b, vertex4, vertex2, vertex1 ); + b3QHList_PushBack( &b->faceList.link, &face2->link ); + b3QHFace* face3 = b3HullBuilder_NewFace( b, vertex4, vertex3, vertex2 ); + b3QHList_PushBack( &b->faceList.link, &face3->link ); + b3QHFace* face4 = b3HullBuilder_NewFace( b, vertex4, vertex1, vertex3 ); + b3QHList_PushBack( &b->faceList.link, &face4->link ); + + b3LinkFaces( face1, 0, face2, 1 ); + b3LinkFaces( face1, 1, face3, 1 ); + b3LinkFaces( face1, 2, face4, 1 ); + + b3LinkFaces( face2, 0, face3, 2 ); + b3LinkFaces( face3, 0, face4, 2 ); + b3LinkFaces( face4, 0, face2, 2 ); + +#if B3_DEBUG + B3_ASSERT( b3CheckConsistency( face1 ) ); + B3_ASSERT( b3CheckConsistency( face2 ) ); + B3_ASSERT( b3CheckConsistency( face3 ) ); + B3_ASSERT( b3CheckConsistency( face4 ) ); +#endif + + for ( int index = 0; index < pointCount; ++index ) + { + if ( index == index1 || index == index2 || index == index3 || index == index4 ) + { + continue; + } + + b3Vec3 point = points[index]; + + float maxDistance = b->minOutside; + b3QHFace* maxFace = NULL; + + for ( b3QHListNode* node = b->faceList.link.next; node != &b->faceList.link; node = node->next ) + { + b3QHFace* face = (b3QHFace*)node; + float distance = b3PlaneSeparation( face->plane, point ); + if ( distance > maxDistance ) + { + maxDistance = distance; + maxFace = face; + } + } + + if ( maxFace != NULL ) + { + b3QHVertex* vertex = b3HullBuilder_NewVertex( b, point ); + vertex->conflictFace = maxFace; + b3QHList_PushBack( &maxFace->conflictListHead.link, &vertex->link ); + if ( maxDistance > maxFace->maxConflictDistance ) + { + maxFace->maxConflictDistance = maxDistance; + maxFace->maxConflict = vertex; + } + } + } + + return true; +} + +// Recompute the farthest-conflict cache after a face's plane changes. +// Walks the existing conflict list once; cost is bounded by that list, not the global pool. +static void b3HullBuilder_RecacheConflicts( b3QHFace* face, float minOutside ) +{ + b3QHVertex* maxVertex = NULL; + float maxDistance = minOutside; + + for ( b3QHListNode* node = face->conflictListHead.link.next; node != &face->conflictListHead.link; node = node->next ) + { + b3QHVertex* vertex = (b3QHVertex*)node; + float distance = b3PlaneSeparation( face->plane, vertex->position ); + if ( distance > maxDistance ) + { + maxDistance = distance; + maxVertex = vertex; + } + } + + face->maxConflict = maxVertex; + face->maxConflictDistance = maxDistance; +} + +static b3QHVertex* b3HullBuilder_NextConflictVertex( const b3HullBuilder* b ) +{ + b3QHVertex* maxVertex = NULL; + float maxDistance = b->minOutside; + + for ( const b3QHListNode* faceNode = b->faceList.link.next; faceNode != &b->faceList.link; faceNode = faceNode->next ) + { + const b3QHFace* face = (const b3QHFace*)faceNode; + if ( face->maxConflict != NULL && face->maxConflictDistance > maxDistance ) + { + maxDistance = face->maxConflictDistance; + maxVertex = face->maxConflict; + } + } + + return maxVertex; +} + +// Move every conflict vertex of `face` onto the orphaned list and clear their conflictFace. +static void b3HullBuilder_DrainConflictList( b3HullBuilder* b, b3QHFace* face ) +{ + b3QHListNode* node = face->conflictListHead.link.next; + while ( node != &face->conflictListHead.link ) + { + b3QHVertex* orphan = (b3QHVertex*)node; + node = node->next; + + orphan->conflictFace = NULL; + b3QHList_Remove( &orphan->link ); + b3QHList_PushBack( &b->orphanedList.link, &orphan->link ); + } + B3_ASSERT( B3_LIST_EMPTY( &face->conflictListHead.link ) ); +} + +// Mark a face for deletion, drain its conflict list, and populate a fresh DFS frame for it. +// `entryEdge` is the half-edge in `face` whose twin lies in the just-deleted parent face, or +// NULL for the seed. The frame skips `entryEdge` on recursive entries (it would be ignored +// anyway since the parent's mark is now DELETE, but skipping saves one iteration). +static void b3HullBuilder_EnterHorizonFace( b3HullBuilder* b, b3QHFace* face, b3QHHalfEdge* entryEdge, b3HorizonFrame* frameOut ) +{ + face->mark = B3_MARK_DELETE; + b3HullBuilder_DrainConflictList( b, face ); + + frameOut->face = face; + frameOut->started = false; + if ( entryEdge != NULL ) + { + frameOut->startEdge = entryEdge; + frameOut->edge = entryEdge->next; + } + else + { + frameOut->startEdge = face->edge; + frameOut->edge = face->edge; + } +} + +static void b3HullBuilder_BuildHorizon( b3HullBuilder* b, b3QHVertex* apex, b3QHFace* seed ) +{ + b3HorizonFrame* stack = b->horizonStack; + int top = 0; + + B3_ASSERT( top < b->horizonStackCapacity ); + b3HullBuilder_EnterHorizonFace( b, seed, NULL, &stack[top++] ); + + while ( top > 0 ) + { + b3HorizonFrame* f = &stack[top - 1]; + + if ( f->started && f->edge == f->startEdge ) + { + top--; + continue; + } + f->started = true; + + b3QHHalfEdge* edge = f->edge; + b3QHHalfEdge* twin = edge->twin; + f->edge = edge->next; + + if ( twin->face->mark != B3_MARK_VISIBLE ) + { + continue; + } + + float distance = b3PlaneSeparation( twin->face->plane, apex->position ); + if ( distance > b->minRadius ) + { + B3_ASSERT( top < b->horizonStackCapacity ); + b3HullBuilder_EnterHorizonFace( b, twin->face, twin, &stack[top++] ); + } + else + { + B3_ASSERT( b->horizonCount < b->horizonCapacity ); + b->horizon[b->horizonCount++] = edge; + } + } +} + +static void b3HullBuilder_BuildCone( b3HullBuilder* b, b3QHVertex* apex ) +{ + for ( int i = 0; i < b->horizonCount; ++i ) + { + b3QHHalfEdge* edge = b->horizon[i]; + B3_ASSERT( edge->twin->twin == edge ); + + b3QHFace* face = b3HullBuilder_NewFace( b, apex, edge->origin, edge->twin->origin ); + B3_ASSERT( b->coneCount < b->coneCapacity ); + b->cone[b->coneCount++] = face; + + b3LinkFace( face, 1, edge->twin ); + } + + b3QHFace* face1 = b->cone[b->coneCount - 1]; + for ( int i = 0; i < b->coneCount; ++i ) + { + b3QHFace* face2 = b->cone[i]; + b3LinkFaces( face1, 2, face2, 0 ); + face1 = face2; + } +} + +// Retire half-edges in the half-open ring range [begin, end) by pushing each onto edgeFreeHead. +// The caller has already detached these edges from the live face ring (rewire happens before +// destroy), so they are unreachable from the live hull. +static void b3HullBuilder_DestroyEdges( b3HullBuilder* b, b3QHHalfEdge* begin, b3QHHalfEdge* end ) +{ + b3QHHalfEdge* edge = begin; + while ( edge != end ) + { + b3QHHalfEdge* next = edge->next; + b3HullBuilder_RetireEdge( b, edge ); + edge = next; + } +} + +static void b3HullBuilder_ConnectEdges( b3HullBuilder* b, b3QHHalfEdge* prev, b3QHHalfEdge* next ) +{ + B3_ASSERT( prev != next ); + B3_ASSERT( prev->face == next->face ); + + // If both shared neighbors are the same face, prev and next together would orphan that face. + if ( prev->twin->face == next->twin->face ) + { + // next is redundant. + if ( next->face->edge == next ) + { + next->face->edge = prev; + } + + b3QHHalfEdge* twin; + if ( b3VertexCountOfFace( prev->twin->face ) == 3 ) + { + // Capture all 3 half-edges of the dead triangle before the rewire overwrites prev->twin. + b3QHHalfEdge* deadEdge0 = prev->twin; // prev->twin (will be rewired below) + b3QHHalfEdge* deadEdge1 = next->twin; // next->twin + b3QHHalfEdge* deadEdge2 = next->twin->prev; // third edge of the dead triangle + + twin = deadEdge2->twin; + B3_ASSERT( twin->face->mark != B3_MARK_DELETE ); + + b3QHFace* opposingFace = prev->twin->face; + opposingFace->mark = B3_MARK_DELETE; + B3_ASSERT( b->mergedFacesCount < b->mergedFacesCapacity ); + b->mergedFaces[b->mergedFacesCount++] = opposingFace; + + prev->next = next->next; + prev->next->prev = prev; + + prev->twin = twin; + twin->twin = prev; + + // Drop the redundant vertex (slot abandoned in the bump allocator). + b3QHList_Remove( &next->origin->link ); + + // Retire the 3 half-edges of the dead triangle now that the rewire is complete. + b3HullBuilder_RetireEdge( b, deadEdge0 ); + b3HullBuilder_RetireEdge( b, deadEdge1 ); + b3HullBuilder_RetireEdge( b, deadEdge2 ); + } + else + { + twin = next->twin; + + if ( twin->face->edge == prev->twin ) + { + twin->face->edge = twin; + } + + twin->next = prev->twin->next; + twin->next->prev = twin; + // prev->twin slot is retired to the edge free list. + b3HullBuilder_RetireEdge( b, prev->twin ); + + prev->next = next->next; + prev->next->prev = prev; + + prev->twin = twin; + twin->twin = prev; + + // Drop the redundant vertex (slot abandoned in the bump allocator). + b3QHList_Remove( &next->origin->link ); + } + + // Twin->face changed shape; recompute its plane and refresh its cached max conflict. + b3NewellPlane( twin->face ); + b3HullBuilder_RecacheConflicts( twin->face, b->minOutside ); + } + else + { + prev->next = next; + next->prev = prev; + } +} + +static void b3HullBuilder_AbsorbFaces( b3HullBuilder* b, b3QHFace* face ) +{ + for ( int i = 0; i < b->mergedFacesCount; ++i ) + { + B3_ASSERT( b->mergedFaces[i]->mark == B3_MARK_DELETE ); + b3QHListNode* head = &b->mergedFaces[i]->conflictListHead.link; + + b3QHListNode* node = head->next; + while ( node != head ) + { + b3QHVertex* vertex = (b3QHVertex*)node; + node = node->next; + + b3QHList_Remove( &vertex->link ); + + float distance = b3PlaneSeparation( face->plane, vertex->position ); + if ( distance > b->minOutside ) + { + b3QHList_PushBack( &face->conflictListHead.link, &vertex->link ); + vertex->conflictFace = face; + if ( distance > face->maxConflictDistance ) + { + face->maxConflictDistance = distance; + face->maxConflict = vertex; + } + } + else + { + b3QHList_PushBack( &b->orphanedList.link, &vertex->link ); + vertex->conflictFace = NULL; + } + } + + B3_ASSERT( B3_LIST_EMPTY( head ) ); + + // Conflict list is now drained. Retire this face to the free list. + b3HullBuilder_RetireFace( b, b->mergedFaces[i] ); + } +} + +static void b3HullBuilder_ConnectFaces( b3HullBuilder* b, b3QHHalfEdge* edge ) +{ + b3QHFace* face = edge->face; + + b3QHHalfEdge* twin = edge->twin; + + b3QHHalfEdge* edgePrev = edge->prev; + b3QHHalfEdge* edgeNext = edge->next; + b3QHHalfEdge* twinPrev = twin->prev; + b3QHHalfEdge* twinNext = twin->next; + + while ( edgePrev->twin->face == twin->face ) + { + B3_ASSERT( edgePrev->twin == twinNext ); + B3_ASSERT( twinNext->twin == edgePrev ); + + edgePrev = edgePrev->prev; + twinNext = twinNext->next; + } + B3_ASSERT( edgePrev->face != twinNext->face ); + + while ( edgeNext->twin->face == twin->face ) + { + B3_ASSERT( edgeNext->twin == twinPrev ); + B3_ASSERT( twinPrev->twin == edgeNext ); + + edgeNext = edgeNext->next; + twinPrev = twinPrev->prev; + } + B3_ASSERT( edgeNext->face != twinPrev->face ); + + face->edge = edgePrev; + + // Discard opposing face. mergedFaces is single-buffered: ConnectFaces does not nest. + b->mergedFacesCount = 0; + B3_ASSERT( b->mergedFacesCount < b->mergedFacesCapacity ); + b->mergedFaces[b->mergedFacesCount++] = twin->face; + twin->face->mark = B3_MARK_DELETE; + twin->face->edge = NULL; + + for ( b3QHHalfEdge* absorbed = twinNext; absorbed != twinPrev->next; absorbed = absorbed->next ) + { + absorbed->face = face; + } + + b3HullBuilder_DestroyEdges( b, edgePrev->next, edgeNext ); + b3HullBuilder_DestroyEdges( b, twinPrev->next, twinNext ); + + b3HullBuilder_ConnectEdges( b, edgePrev, twinNext ); + b3HullBuilder_ConnectEdges( b, twinPrev, edgeNext ); + + b3NewellPlane( face ); + // Existing conflicts now have stale distances under the new plane; AbsorbFaces will then + // add more incrementally and update the cache as it goes. + b3HullBuilder_RecacheConflicts( face, b->minOutside ); +#if B3_DEBUG + B3_ASSERT( b3CheckConsistency( face ) ); +#endif + + b3HullBuilder_AbsorbFaces( b, face ); +} + +static bool b3HullBuilder_MergeConcave( b3HullBuilder* b, b3QHFace* face ) +{ + b3QHHalfEdge* edge = face->edge; + + do + { + b3QHHalfEdge* twin = edge->twin; + + if ( b3IsEdgeConcave( edge, b->minRadius ) || b3IsEdgeConcave( twin, b->minRadius ) ) + { + b3HullBuilder_ConnectFaces( b, edge ); + return true; + } + + edge = edge->next; + } + while ( edge != face->edge ); + + return false; +} + +static bool b3HullBuilder_MergeCoplanar( b3HullBuilder* b, b3QHFace* face ) +{ + b3QHHalfEdge* edge = face->edge; + + do + { + b3QHHalfEdge* twin = edge->twin; + + if ( !b3IsEdgeConvex( edge, b->minRadius ) || !b3IsEdgeConvex( twin, b->minRadius ) ) + { + b3HullBuilder_ConnectFaces( b, edge ); + return true; + } + + edge = edge->next; + } + while ( edge != face->edge ); + + return false; +} + +static void b3HullBuilder_MergeFaces( b3HullBuilder* b ) +{ + for ( int i = 0; i < b->coneCount; ++i ) + { + b3QHFace* face = b->cone[i]; + if ( face->mark == B3_MARK_VISIBLE && face->flipped ) + { + face->flipped = false; + + float bestArea = 0; + b3QHHalfEdge* bestEdge = NULL; + + b3QHHalfEdge* edge = face->edge; + do + { + b3QHHalfEdge* twin = edge->twin; + float area = twin->face->area; + if ( area > bestArea ) + { + bestArea = area; + bestEdge = edge; + } + + edge = edge->next; + } + while ( edge != face->edge ); + + B3_ASSERT( bestEdge != NULL ); + b3HullBuilder_ConnectFaces( b, bestEdge ); + } + } + + for ( int i = 0; i < b->coneCount; ++i ) + { + b3QHFace* face = b->cone[i]; + if ( face->mark == B3_MARK_VISIBLE ) + { + while ( b3HullBuilder_MergeConcave( b, face ) ) + { + } + } + } + + for ( int i = 0; i < b->coneCount; ++i ) + { + b3QHFace* face = b->cone[i]; + if ( face->mark == B3_MARK_VISIBLE ) + { + while ( b3HullBuilder_MergeCoplanar( b, face ) ) + { + } + } + } +} + +static void b3HullBuilder_ResolveVertices( b3HullBuilder* b ) +{ + b3QHListNode* node = b->orphanedList.link.next; + while ( node != &b->orphanedList.link ) + { + b3QHVertex* vertex = (b3QHVertex*)node; + node = node->next; + b3QHList_Remove( &vertex->link ); + + float maxDistance = b->minOutside; + b3QHFace* maxFace = NULL; + + for ( int i = 0; i < b->coneCount; ++i ) + { + if ( b->cone[i]->mark == B3_MARK_VISIBLE ) + { + float distance = b3PlaneSeparation( b->cone[i]->plane, vertex->position ); + if ( distance > maxDistance ) + { + maxDistance = distance; + maxFace = b->cone[i]; + } + } + } + + if ( maxFace != NULL ) + { + B3_ASSERT( maxFace->mark == B3_MARK_VISIBLE ); + b3QHList_PushBack( &maxFace->conflictListHead.link, &vertex->link ); + vertex->conflictFace = maxFace; + if ( maxDistance > maxFace->maxConflictDistance ) + { + maxFace->maxConflictDistance = maxDistance; + maxFace->maxConflict = vertex; + } + } + // Otherwise: vertex is interior to the hull. Its slot in the bump pool is abandoned. + } + + B3_ASSERT( B3_LIST_EMPTY( &b->orphanedList.link ) ); +} + +static void b3HullBuilder_ResolveFaces( b3HullBuilder* b ) +{ + // Splice deleted faces out of the face list. Faces already retired by AbsorbFaces are no + // longer on faceList, so we guard with b3QHList_Contains before removing. + b3QHListNode* node = b->faceList.link.next; + while ( node != &b->faceList.link ) + { + b3QHFace* face = (b3QHFace*)node; + node = node->next; + + if ( face->mark == B3_MARK_DELETE && b3QHList_Contains( &face->link ) ) + { + B3_ASSERT( B3_LIST_EMPTY( &face->conflictListHead.link ) ); + b3QHList_Remove( &face->link ); + } + } + + for ( int i = 0; i < b->coneCount; ++i ) + { + b3QHFace* face = b->cone[i]; + if ( face->mark == B3_MARK_DELETE ) + { + continue; + } + b3QHList_PushBack( &b->faceList.link, &face->link ); + } +} + +static void b3HullBuilder_AddVertexToHull( b3HullBuilder* b, b3QHVertex* vertex ) +{ + b3QHFace* face = vertex->conflictFace; + vertex->conflictFace = NULL; + b3QHList_Remove( &vertex->link ); + b3QHList_PushBack( &b->vertexList.link, &vertex->link ); + + b->horizonCount = 0; + b3HullBuilder_BuildHorizon( b, vertex, face ); + B3_ASSERT( b->horizonCount >= 3 ); + + b->coneCount = 0; + b3HullBuilder_BuildCone( b, vertex ); + B3_ASSERT( b->coneCount >= 3 ); + + b3HullBuilder_MergeFaces( b ); + b3HullBuilder_ResolveVertices( b ); + b3HullBuilder_ResolveFaces( b ); +} + +static void b3HullBuilder_CleanHull( b3HullBuilder* b, b3Vec3 origin ) +{ + int faceCount = 0; + int halfEdgeCount = 0; + + for ( b3QHListNode* faceNode = b->faceList.link.next; faceNode != &b->faceList.link; faceNode = faceNode->next ) + { + b3QHFace* face = (b3QHFace*)faceNode; + b3QHHalfEdge* edge = face->edge; + + do + { + edge->origin->reachable = true; + edge = edge->next; + halfEdgeCount++; + } + while ( edge != face->edge ); + + face->plane.offset += b3Dot( face->plane.normal, origin ); + face->centroid = b3Add( face->centroid, origin ); + faceCount++; + } + + int vertexCount = 0; + b3QHListNode* node = b->vertexList.link.next; + while ( node != &b->vertexList.link ) + { + b3QHVertex* vertex = (b3QHVertex*)node; + node = node->next; + + if ( !vertex->reachable ) + { + b3QHList_Remove( &vertex->link ); + } + else + { + vertex->position = b3Add( vertex->position, origin ); + vertexCount++; + } + } + + b->interiorPoint = b3Add( b->interiorPoint, origin ); + + b->finalVertexCount = vertexCount; + b->finalHalfEdgeCount = halfEdgeCount; + b->finalFaceCount = faceCount; +} + +#if B3_DEBUG +static bool b3HullBuilder_IsConsistent( const b3HullBuilder* b ) +{ + int v = b->finalVertexCount; + int e = b->finalHalfEdgeCount / 2; + int f = b->finalFaceCount; + + if ( v - e + f != 2 ) + { + return false; + } + + for ( const b3QHListNode* faceNode = b->faceList.link.next; faceNode != &b->faceList.link; faceNode = faceNode->next ) + { + const b3QHFace* face = (const b3QHFace*)faceNode; + if ( face->edge->face != face ) + { + return false; + } + + if ( !b3CheckConsistency( face ) ) + { + return false; + } + + if ( b3PlaneSeparation( face->plane, b->interiorPoint ) > 0 ) + { + return false; + } + + if ( face->mark != B3_MARK_VISIBLE ) + { + return false; + } + + const b3QHHalfEdge* edge = face->edge; + + do + { + if ( edge->next->origin != edge->twin->origin ) + { + return false; + } + if ( edge->prev->next != edge ) + { + return false; + } + if ( edge->next->prev != edge ) + { + return false; + } + if ( edge->twin->twin != edge ) + { + return false; + } + if ( edge->face != face ) + { + return false; + } + if ( b3DistanceSquared( edge->origin->position, edge->twin->origin->position ) < 1000.0f * FLT_MIN ) + { + return false; + } + + edge = edge->next; + } + while ( edge != face->edge ); + } + + return true; +} +#endif + +static bool b3HullBuilder_HasHull( const b3HullBuilder* b ) +{ + int v = b->finalVertexCount; + int e = b->finalHalfEdgeCount / 2; + int f = b->finalFaceCount; + return v - e + f == 2 && f >= 4; +} + +// Build the entire hull. Returns true iff the result satisfies Euler's identity. +static bool b3HullBuilder_Construct( b3HullBuilder* b, const b3Vec3* points, int pointCount, int maxVertexCount, b3Vec3 origin, + b3Vec3* shiftedPoints ) +{ + if ( pointCount < 4 ) + { + return false; + } + + for ( int i = 0; i < pointCount; ++i ) + { + shiftedPoints[i] = b3Sub( points[i], origin ); + } + + b3HullBuilder_ComputeTolerance( b, pointCount, shiftedPoints ); + if ( !b3HullBuilder_BuildInitialHull( b, pointCount, shiftedPoints ) ) + { + return false; + } + + int budget = b3ClampInt( maxVertexCount - 4, 0, B3_HULL_LIMIT - 4 ); + + b3QHVertex* vertex = b3HullBuilder_NextConflictVertex( b ); + while ( vertex && budget > 0 ) + { + b3HullBuilder_AddVertexToHull( b, vertex ); + vertex = b3HullBuilder_NextConflictVertex( b ); + budget -= 1; + } + + b3HullBuilder_CleanHull( b, origin ); + +#if B3_DEBUG + B3_ASSERT( b3HullBuilder_IsConsistent( b ) ); +#endif + + return b3HullBuilder_HasHull( b ); +} + +typedef struct b3HullWorkSizes +{ + int N; // pointCount + int M; // clamped maxVertexCount, in [4, B3_HULL_LIMIT] + int vertexCapacity; + int edgeCapacity; + int faceCapacity; + int horizonCapacity; + int coneCapacity; + int mergedFacesCapacity; + int horizonStackCapacity; + size_t totalBytes; + + size_t offsetVertex; + size_t offsetEdge; + size_t offsetFace; + size_t offsetHorizon; + size_t offsetCone; + size_t offsetMergedFaces; + size_t offsetHorizonStack; + size_t offsetShiftedPoints; +} b3HullWorkSizes; + +static b3HullWorkSizes b3ComputeHullWorkSizes( int pointCount, int clampedMaxCount ) +{ + b3HullWorkSizes s; + s.N = pointCount; + s.M = clampedMaxCount; + + // Vertices: 4 initial hull vertices + at most one per remaining input point. No free list. + s.vertexCapacity = pointCount + 4; + + // Edges and faces use free-list recycling; capacity is proportional to live hull size. + // edgeCapacity: peak is ~twice live edges plus cone edges; floor 48. + s.edgeCapacity = 24 * s.M - 48; + if ( s.edgeCapacity < 48 ) + { + s.edgeCapacity = 48; + } + + // faceCapacity: peak intermediate state live faces (<=2*M-4) plus full cone (<=3*M-6); floor 16. + s.faceCapacity = 5 * s.M - 10; + if ( s.faceCapacity < 16 ) + { + s.faceCapacity = 16; + } + + // Horizon/cone bounded by current half-edge count; mergedFaces by face count. + s.horizonCapacity = 3 * s.M - 6; + if ( s.horizonCapacity < 6 ) + { + s.horizonCapacity = 6; + } + s.coneCapacity = s.horizonCapacity; + s.mergedFacesCapacity = 2 * s.M - 4; + if ( s.mergedFacesCapacity < 4 ) + { + s.mergedFacesCapacity = 4; + } + + // Horizon DFS depth is bounded by the number of live faces (Euler: <=2*M-4). + s.horizonStackCapacity = 2 * s.M - 4; + if ( s.horizonStackCapacity < 4 ) + { + s.horizonStackCapacity = 4; + } + + size_t offset = 0; + + s.offsetVertex = offset; + offset = b3AlignUp8( offset + (size_t)s.vertexCapacity * sizeof( b3QHVertex ) ); + + s.offsetEdge = offset; + offset = b3AlignUp8( offset + (size_t)s.edgeCapacity * sizeof( b3QHHalfEdge ) ); + + s.offsetFace = offset; + offset = b3AlignUp8( offset + (size_t)s.faceCapacity * sizeof( b3QHFace ) ); + + s.offsetHorizon = offset; + offset = b3AlignUp8( offset + (size_t)s.horizonCapacity * sizeof( b3QHHalfEdge* ) ); + + s.offsetCone = offset; + offset = b3AlignUp8( offset + (size_t)s.coneCapacity * sizeof( b3QHFace* ) ); + + s.offsetMergedFaces = offset; + offset = b3AlignUp8( offset + (size_t)s.mergedFacesCapacity * sizeof( b3QHFace* ) ); + + s.offsetHorizonStack = offset; + offset = b3AlignUp8( offset + (size_t)s.horizonStackCapacity * sizeof( b3HorizonFrame ) ); + + s.offsetShiftedPoints = offset; + offset += (size_t)pointCount * sizeof( b3Vec3 ); + + s.totalBytes = offset; + return s; +} + +static void b3HullBuilder_Init( b3HullBuilder* b, char* mem, const b3HullWorkSizes* s ) +{ + memset( b, 0, sizeof( *b ) ); + b3QHList_Init( &b->orphanedList.link ); + b3QHList_Init( &b->vertexList.link ); + b3QHList_Init( &b->faceList.link ); + + b->vertexBase = (b3QHVertex*)( mem + s->offsetVertex ); + b->vertexCapacity = s->vertexCapacity; + + b->edgeBase = (b3QHHalfEdge*)( mem + s->offsetEdge ); + b->edgeCapacity = s->edgeCapacity; + + b->faceBase = (b3QHFace*)( mem + s->offsetFace ); + b->faceCapacity = s->faceCapacity; + + b->horizon = (b3QHHalfEdge**)( mem + s->offsetHorizon ); + b->horizonCapacity = s->horizonCapacity; + + b->cone = (b3QHFace**)( mem + s->offsetCone ); + b->coneCapacity = s->coneCapacity; + + b->mergedFaces = (b3QHFace**)( mem + s->offsetMergedFaces ); + b->mergedFacesCapacity = s->mergedFacesCapacity; + + b->horizonStack = (b3HorizonFrame*)( mem + s->offsetHorizonStack ); + b->horizonStackCapacity = s->horizonStackCapacity; +} + +static b3Vec3* b3GetHullPointsWrite( b3HullData* hull ) +{ + if ( hull->pointOffset == 0 ) + { + return NULL; + } + return (b3Vec3*)( (intptr_t)hull + hull->pointOffset ); +} + +static b3Plane* b3GetHullPlanesWrite( b3HullData* hull ) +{ + if ( hull->planeOffset == 0 ) + { + return NULL; + } + return (b3Plane*)( (intptr_t)hull + hull->planeOffset ); +} + +static b3HullVertex* b3GetHullVerticesWrite( b3HullData* hull ) +{ + if ( hull->vertexOffset == 0 ) + { + return NULL; + } + return (b3HullVertex*)( (intptr_t)hull + hull->vertexOffset ); +} + +static b3HullHalfEdge* b3GetHullEdgesWrite( b3HullData* hull ) +{ + if ( hull->edgeOffset == 0 ) + { + return NULL; + } + return (b3HullHalfEdge*)( (intptr_t)hull + hull->edgeOffset ); +} + +int b3FindHullSupportVertex( const b3HullData* hull, b3Vec3 direction ) +{ + int bestIndex = B3_NULL_INDEX; + float bestDot = -FLT_MAX; + + int vertexCount = hull->vertexCount; + const b3Vec3* points = b3GetHullPoints( hull ); + + for ( int index = 0; index < vertexCount; ++index ) + { + float dot = b3Dot( direction, points[index] ); + if ( dot > bestDot ) + { + bestIndex = index; + bestDot = dot; + } + } + B3_ASSERT( bestIndex >= 0 ); + + return bestIndex; +} + +int b3FindHullSupportFace( const b3HullData* hull, b3Vec3 direction ) +{ + int bestIndex = B3_NULL_INDEX; + float bestDot = -FLT_MAX; + + int faceCount = hull->faceCount; + const b3Plane* planes = b3GetHullPlanes( hull ); + + for ( int index = 0; index < faceCount; ++index ) + { + float dot = b3Dot( planes[index].normal, direction ); + if ( dot > bestDot ) + { + bestDot = dot; + bestIndex = index; + } + } + B3_ASSERT( bestIndex >= 0 ); + + return bestIndex; +} + +#if B3_ENABLE_VALIDATION + +bool b3IsValidHull( const b3HullData* hull ) +{ + if ( hull->version != B3_HULL_VERSION ) + { + return false; + } + + int v = hull->vertexCount; + int e = hull->edgeCount / 2; + int f = hull->faceCount; + + if ( v - e + f != 2 ) + { + return false; + } + + const b3HullVertex* vertices = b3GetHullVertices( hull ); + const b3HullHalfEdge* edges = b3GetHullEdges( hull ); + for ( int index = 0; index < hull->vertexCount; ++index ) + { + const b3HullVertex* vertex = vertices + index; + const b3HullHalfEdge* edge = edges + vertex->edge; + + if ( edge->origin != index ) + { + return false; + } + } + + for ( int index = 0; index < hull->edgeCount; index += 2 ) + { + const b3HullHalfEdge* edge = edges + index + 0; + const b3HullHalfEdge* twin = edges + index + 1; + + if ( edge->twin != index + 1 ) + { + return false; + } + + if ( twin->twin != index + 0 ) + { + return false; + } + } + + const b3HullFace* faces = b3GetHullFaces( hull ); + const b3Plane* planes = b3GetHullPlanes( hull ); + for ( int faceIndex = 0; faceIndex < hull->faceCount; ++faceIndex ) + { + const b3HullFace* face = faces + faceIndex; + + int baseEdgeIndex = face->edge; + const b3HullHalfEdge* edge = edges + baseEdgeIndex; + + b3Plane plane = planes[faceIndex]; + if ( b3PlaneSeparation( plane, hull->center ) >= 0.0f ) + { + return false; + } + + int edgeIndex = baseEdgeIndex; + do + { + edge = edges + edgeIndex; + const b3HullHalfEdge* next = edges + edge->next; + const b3HullHalfEdge* twin = edges + edge->twin; + + if ( edge->face != faceIndex ) + { + return false; + } + + if ( twin->twin != edgeIndex ) + { + return false; + } + + if ( next->origin != twin->origin ) + { + return false; + } + + edgeIndex = edge->next; + } + while ( edgeIndex != baseEdgeIndex ); + } + + if ( hull->volume <= 0.0f ) + { + return false; + } + + if ( hull->surfaceArea <= 0.0f ) + { + return false; + } + + if ( hull->innerRadius <= 0.0f ) + { + return false; + } + + return true; +} + +#else + +bool b3IsValidHull( const b3HullData* hull ) +{ + B3_UNUSED( hull ); + return true; +} + +#endif + +b3HullData* b3CreateCylinder( float height, float radius, float yOffset, int sides ) +{ + B3_ASSERT( height > 0.0f ); + B3_ASSERT( radius > 0.0f ); + B3_ASSERT( 3 <= sides && sides <= 32 ); + + int pointCount = 2 * sides; + b3Vec3* points = (b3Vec3*)b3Alloc( pointCount * sizeof( b3Vec3 ) ); + B3_ASSERT( points != NULL ); + + float alpha = 0.0f; + float deltaAlpha = 2.0f * B3_PI / sides; + + for ( int index = 0; index < sides; ++index ) + { + float sinAlpha = b3Sin( alpha ); + float cosAlpha = b3Cos( alpha ); + + points[2 * index + 0] = (b3Vec3){ radius * cosAlpha, yOffset, radius * sinAlpha }; + points[2 * index + 1] = (b3Vec3){ radius * cosAlpha, yOffset + height, radius * sinAlpha }; + + alpha += deltaAlpha; + } + + b3HullData* hull = b3CreateHull( points, pointCount, pointCount ); + B3_ASSERT( hull->vertexCount == pointCount ); + B3_ASSERT( hull->edgeCount == 6 * sides ); + B3_ASSERT( hull->faceCount == sides + 2 ); + + b3Free( points, pointCount * sizeof( b3Vec3 ) ); + + return hull; +} + +b3HullData* b3CreateCone( float height, float radius1, float radius2, int slices ) +{ + B3_ASSERT( height > 0.0f ); + B3_ASSERT( radius1 > 0.0f ); + B3_ASSERT( radius2 > 0.0f ); + B3_ASSERT( 4 <= slices && slices <= 32 ); + + int pointCount = 2 * slices; + b3Vec3* points = (b3Vec3*)b3Alloc( pointCount * sizeof( b3Vec3 ) ); + B3_ASSERT( points != NULL ); + + float alpha = 0.0f; + float deltaAlpha = 2.0f * B3_PI / slices; + + for ( int index = 0; index < slices; ++index ) + { + float sinAlpha = b3Sin( alpha ); + float cosAlpha = b3Cos( alpha ); + + points[2 * index + 0] = (b3Vec3){ radius1 * cosAlpha, 0.0f, radius1 * sinAlpha }; + points[2 * index + 1] = (b3Vec3){ radius2 * cosAlpha, height, radius2 * sinAlpha }; + + alpha += deltaAlpha; + } + + b3HullData* hull = b3CreateHull( points, pointCount, pointCount ); + B3_ASSERT( hull->vertexCount == pointCount ); + B3_ASSERT( hull->edgeCount == 6 * slices ); + B3_ASSERT( hull->faceCount == slices + 2 ); + + b3Free( points, pointCount * sizeof( b3Vec3 ) ); + + return hull; +} + +b3HullData* b3CreateRock( float radius ) +{ + int pointCount = 10; + + // Golden ratio + const float phi = ( 1.0f + sqrtf( 5.0f ) ) / 2.0f; + + // Fibonacci lattice + b3Vec3 points[10]; + + // Azimuthal angle + float theta = 2.0f * B3_PI / phi; + + b3CosSin cs = { 1.0f, 0.0 }; + b3CosSin deltaCS = b3ComputeCosSin( theta ); + + for ( int i = 0; i < pointCount; ++i ) + { + // Z coordinate + float z = 1.0f - ( 2.0f * i + 1.0f ) / pointCount; + // Radius in xy-plane + float radius_XY = sqrtf( 1.0f - z * z ); + + points[i].x = radius * radius_XY * cs.cosine; + points[i].y = radius * radius_XY * cs.sine; + points[i].z = radius * z; + + b3CosSin cs0 = cs; + cs.cosine = deltaCS.cosine * cs0.cosine - deltaCS.sine * cs0.sine; + cs.sine = deltaCS.sine * cs0.cosine + deltaCS.cosine * cs0.sine; + } + + return b3CreateHull( points, pointCount, pointCount ); +} + +static void b3UpdateHullBounds( b3HullData* hull ) +{ + const b3Vec3* points = b3GetHullPoints( hull ); + int vertexCount = hull->vertexCount; + + B3_ASSERT( vertexCount > 0 ); + b3AABB bounds; + bounds.lowerBound = points[0]; + bounds.upperBound = points[0]; + + for ( int i = 1; i < vertexCount; ++i ) + { + b3Vec3 p = points[i]; + bounds.lowerBound = b3Min( bounds.lowerBound, p ); + bounds.upperBound = b3Max( bounds.upperBound, p ); + } + + hull->aabb = bounds; +} + +// M. Kallay - "Computing the Moment of Inertia of a Solid Defined by a Triangle Mesh" +static bool b3UpdateHullBulkProperties( b3HullData* hull ) +{ + const b3Vec3* points = b3GetHullPoints( hull ); + const b3HullFace* faces = b3GetHullFaces( hull ); + const b3HullHalfEdge* edges = b3GetHullEdges( hull ); + const b3Plane* planes = b3GetHullPlanes( hull ); + + float area = 0.0f; + float volume = 0.0f; + b3Vec3 center = b3Vec3_zero; + + // Use the first vertex to reduce round-off errors. + b3Vec3 origin = points[0]; + + float xx = 0.0f; + float xy = 0.0f; + float yy = 0.0f; + float xz = 0.0f; + float zz = 0.0f; + float yz = 0.0f; + + int faceCount = hull->faceCount; + + for ( int faceIndex = 0; faceIndex < faceCount; ++faceIndex ) + { + const b3HullFace* face = faces + faceIndex; + const b3HullHalfEdge* edge1 = edges + face->edge; + const b3HullHalfEdge* edge2 = edges + edge1->next; + const b3HullHalfEdge* edge3 = edges + edge2->next; + + B3_ASSERT( edge1 != edge3 ); + B3_ASSERT( edge1->origin < hull->vertexCount ); + + b3Vec3 v1 = b3Sub( points[edge1->origin], origin ); + + do + { + B3_ASSERT( edge2->origin < hull->vertexCount ); + B3_ASSERT( edge3->origin < hull->vertexCount ); + + b3Vec3 v2 = b3Sub( points[edge2->origin], origin ); + b3Vec3 v3 = b3Sub( points[edge3->origin], origin ); + + area += b3Length( b3Cross( b3Sub( v2, v1 ), b3Sub( v3, v1 ) ) ); + + float det = b3ScalarTripleProduct( v1, v2, v3 ); + + volume += det; + + b3Vec3 v4 = b3Add( v1, b3Add( v2, v3 ) ); + center = b3Add( center, b3MulSV( det, v4 ) ); + + xx += det * ( v1.x * v1.x + v2.x * v2.x + v3.x * v3.x + v4.x * v4.x ); + yy += det * ( v1.y * v1.y + v2.y * v2.y + v3.y * v3.y + v4.y * v4.y ); + zz += det * ( v1.z * v1.z + v2.z * v2.z + v3.z * v3.z + v4.z * v4.z ); + xy += det * ( v1.x * v1.y + v2.x * v2.y + v3.x * v3.y + v4.x * v4.y ); + xz += det * ( v1.x * v1.z + v2.x * v2.z + v3.x * v3.z + v4.x * v4.z ); + yz += det * ( v1.y * v1.z + v2.y * v2.z + v3.y * v3.z + v4.y * v4.z ); + + edge2 = edge3; + edge3 = edges + edge3->next; + } + while ( edge1 != edge3 ); + } + + B3_VALIDATE( volume > 0.0f ); + + b3Vec3 localCenter = volume > 0.0f ? b3MulSV( 0.25f / volume, center ) : b3Vec3_zero; + center = b3Add( localCenter, origin ); + + float radius = FLT_MAX; + for ( int faceIndex = 0; faceIndex < faceCount; ++faceIndex ) + { + b3Plane plane = planes[faceIndex]; + float distance = b3PlaneSeparation( plane, center ); + B3_VALIDATE( distance < 0.0f ); + + radius = b3MinFloat( radius, -distance ); + } + + B3_VALIDATE( 0.0f < radius && radius < FLT_MAX ); + + b3Matrix3 inertia; + inertia.cx.x = yy + zz; + inertia.cy.x = -xy; + inertia.cz.x = -xz; + inertia.cx.y = -xy; + inertia.cy.y = xx + zz; + inertia.cz.y = -yz; + inertia.cx.z = -xz; + inertia.cy.z = -yz; + inertia.cz.z = xx + yy; + + float mass = volume / 6.0f; + + b3Matrix3 centralInertia = b3MulSM( 1.0f / 120.0f, inertia ); + centralInertia = b3SubMM( centralInertia, b3Steiner( mass, localCenter ) ); + + hull->center = center; + hull->centralInertia = centralInertia; + hull->volume = mass; + hull->surfaceArea = 0.5f * area; + hull->innerRadius = radius; + + if ( mass <= 0.0f ) + { + return false; + } + + if ( volume <= 0.0f ) + { + return false; + } + + if ( area <= 0.0f ) + { + return false; + } + + if ( radius <= 0.0f ) + { + return false; + } + + return true; +} + +b3HullData* b3CreateHull( const b3Vec3* points, int pointCount, int maxVertexCount ) +{ + if ( pointCount < 4 ) + { + return NULL; + } + + b3Vec3 origin = points[0]; + int clampedMaxCount = b3ClampInt( maxVertexCount, 4, B3_HULL_LIMIT ); + + // Single allocation for all working memory. + b3HullWorkSizes sizes = b3ComputeHullWorkSizes( pointCount, clampedMaxCount ); + char* work = b3Alloc( sizes.totalBytes ); + + b3HullBuilder builder; + b3HullBuilder_Init( &builder, work, &sizes ); + + b3Vec3* shiftedPoints = (b3Vec3*)( work + sizes.offsetShiftedPoints ); + + bool ok = b3HullBuilder_Construct( &builder, points, pointCount, clampedMaxCount, origin, shiftedPoints ); + if ( !ok ) + { + b3Free( work, sizes.totalBytes ); + return NULL; + } + + if ( builder.finalVertexCount >= B3_HULL_LIMIT ) + { + b3Log( "hull final vertex count of %d exceeds limit of %d", builder.finalVertexCount, B3_HULL_LIMIT ); + b3Free( work, sizes.totalBytes ); + return NULL; + } + + if ( builder.finalFaceCount >= B3_HULL_LIMIT ) + { + b3Log( "hull final face count of %d exceeds limit of %d", builder.finalFaceCount, B3_HULL_LIMIT ); + b3Free( work, sizes.totalBytes ); + return NULL; + } + + if ( builder.finalHalfEdgeCount >= B3_HULL_LIMIT ) + { + b3Log( "hull final half edge count of %d exceeds limit of %d", builder.finalHalfEdgeCount, B3_HULL_LIMIT ); + b3Free( work, sizes.totalBytes ); + return NULL; + } + + // Walk lists into temp arrays bounded by B3_HULL_LIMIT, stamping finalIndex on each node so + // the resolution pass below is O(E + F) instead of O(E^2 + F^2). + const b3QHVertex* tempVertices[B3_HULL_LIMIT]; + int vertexCount = 0; + for ( b3QHListNode* node = builder.vertexList.link.next; node != &builder.vertexList.link; node = node->next ) + { + B3_ASSERT( vertexCount <= B3_HULL_LIMIT - 1 ); + + b3QHVertex* vertex = (b3QHVertex*)node; + vertex->finalIndex = vertexCount; + tempVertices[vertexCount++] = vertex; + } + + // Collect edges in twin-paired order (i, i+1) by stamping each pair as we discover it. + // Replaces b3SortEdges' O(E^2) twin pairing. + const b3QHFace* tempFaces[B3_HULL_LIMIT]; + const b3QHHalfEdge* tempEdges[B3_HULL_LIMIT]; + int faceCount = 0; + int edgeCount = 0; + + for ( b3QHListNode* faceNode = builder.faceList.link.next; faceNode != &builder.faceList.link; faceNode = faceNode->next ) + { + B3_ASSERT( faceCount <= B3_HULL_LIMIT - 1 ); + + b3QHFace* face = (b3QHFace*)faceNode; + face->finalIndex = faceCount; + tempFaces[faceCount++] = face; + + b3QHHalfEdge* edge = face->edge; + do + { + if ( edge->finalIndex < 0 ) + { + B3_ASSERT( edgeCount + 1 <= B3_HULL_LIMIT - 1 ); + + edge->finalIndex = edgeCount; + tempEdges[edgeCount++] = edge; + edge->twin->finalIndex = edgeCount; + tempEdges[edgeCount++] = edge->twin; + } + edge = edge->next; + } + while ( edge != face->edge ); + } + + // Allocate the hull. Arrays hang off the end. + size_t byteCount = b3AlignUp8( sizeof( b3HullData ) ); + int vertexOffset = (int)byteCount; + byteCount += b3AlignUp8( vertexCount * (int)sizeof( b3HullVertex ) ); + int pointOffset = (int)byteCount; + byteCount += b3AlignUp8( vertexCount * (int)sizeof( b3Vec3 ) ); + int edgeOffset = (int)byteCount; + byteCount += b3AlignUp8( edgeCount * (int)sizeof( b3HullHalfEdge ) ); + int faceOffset = (int)byteCount; + byteCount += b3AlignUp8( faceCount * (int)sizeof( b3HullFace ) ); + int planeOffset = (int)byteCount; + byteCount += b3AlignUp8( faceCount * (int)sizeof( b3Plane ) ); + + b3HullData* hull = b3Alloc( byteCount ); + memset( hull, 0, byteCount ); + + hull->version = B3_HULL_VERSION; + hull->vertexOffset = vertexOffset; + hull->pointOffset = pointOffset; + hull->edgeOffset = edgeOffset; + hull->faceOffset = faceOffset; + hull->planeOffset = planeOffset; + + hull->vertexCount = vertexCount; + hull->edgeCount = edgeCount; + hull->faceCount = faceCount; + + hull->byteCount = (int)byteCount; + + b3HullVertex* vertices = b3GetHullVerticesWrite( hull ); + b3HullHalfEdge* edges = b3GetHullEdgesWrite( hull ); + b3HullFace* faces = (b3HullFace*)( (intptr_t)hull + hull->faceOffset ); + b3Vec3* finalPoints = b3GetHullPointsWrite( hull ); + b3Plane* planes = b3GetHullPlanesWrite( hull ); + + for ( int index = 0; index < vertexCount; ++index ) + { + vertices[index].edge = 0; + finalPoints[index] = tempVertices[index]->position; + } + + for ( int index = 0; index < edgeCount; ++index ) + { + const b3QHHalfEdge* edge = tempEdges[index]; + B3_ASSERT( 0 <= edge->next->finalIndex && edge->next->finalIndex <= UINT8_MAX ); + B3_ASSERT( 0 <= edge->twin->finalIndex && edge->twin->finalIndex <= UINT8_MAX ); + B3_ASSERT( 0 <= edge->face->finalIndex && edge->face->finalIndex <= UINT8_MAX ); + B3_ASSERT( 0 <= edge->origin->finalIndex && edge->origin->finalIndex <= UINT8_MAX ); + + edges[index].next = (uint8_t)edge->next->finalIndex; + edges[index].twin = (uint8_t)edge->twin->finalIndex; + edges[index].face = (uint8_t)edge->face->finalIndex; + edges[index].origin = (uint8_t)edge->origin->finalIndex; + + vertices[edge->origin->finalIndex].edge = (uint8_t)index; + } + + for ( int index = 0; index < faceCount; ++index ) + { + const b3QHFace* face = tempFaces[index]; + B3_ASSERT( 0 <= face->edge->finalIndex && face->edge->finalIndex <= UINT8_MAX ); + + faces[index].edge = (uint8_t)face->edge->finalIndex; + planes[index] = face->plane; + } + + // All builder pointers are dead from here on. + b3Free( work, sizes.totalBytes ); + + b3UpdateHullBounds( hull ); + bool success = b3UpdateHullBulkProperties( hull ); + if ( success == false ) + { + b3DestroyHull( hull ); + return NULL; + } + + if ( b3IsValidHull( hull ) == false ) + { + b3DestroyHull( hull ); + return NULL; + } + + hull->hash = 0; + hull->hash = b3NonZeroHash( b3Hash( B3_HASH_INIT, (uint8_t*)hull, hull->byteCount ) ); + + return hull; +} + +b3HullData* b3CloneHull( const b3HullData* hull ) +{ + if ( hull == NULL || b3IsValidHull( hull ) == false ) + { + return NULL; + } + + b3HullData* clone = (b3HullData*)b3Alloc( hull->byteCount ); + memcpy( clone, hull, hull->byteCount ); + + return clone; +} + +uint64_t b3HashHullData( const b3HullData* hull ) +{ + // The baked content hash already covers byteCount. Spread the 32 bits across 64 so the table + // can use the high bits for its fast reject fragment. + return (uint64_t)hull->hash * 0x9E3779B97F4A7C15ull; +} + +bool b3CompareHullData( const b3HullData* hull1, const b3HullData* hull2 ) +{ + if ( hull1 == hull2 ) + { + return true; + } + + if ( hull1->byteCount != hull2->byteCount ) + { + return false; + } + + return memcmp( hull1, hull2, hull1->byteCount ) == 0; +} + +// Hull identity covers every byte, so the structs carry explicit padding. These lock +// the layout, re-audit padding if a size changes. +_Static_assert( sizeof( b3HullData ) == 136, "unexpected hull data size" ); +_Static_assert( sizeof( b3BoxHull ) == 440, "unexpected box hull size" ); + +#define NAME b3HullMap +#define KEY_TY const b3HullData* +#define VAL_TY int +#define HASH_FN b3HashHullData +#define CMPR_FN b3CompareHullData +#define MALLOC_FN b3Alloc +#define FREE_FN b3Free +#define IMPLEMENTATION_MODE +#include "verstable.h" + +size_t b3HullMapByteCount( b3HullMap* map ) +{ + // The map owns a combined bucket and metadata allocation, valid only when buckets exist + size_t byteCount = sizeof( b3HullMap ); + if ( b3HullMap_bucket_count( map ) > 0 ) + { + byteCount += b3HullMap_total_alloc_size( map ); + } + return byteCount; +} + +b3HullData* b3CloneAndTransformHull( const b3HullData* original, b3Transform transform, b3Vec3 scale ) +{ + if ( original == NULL || b3IsValidHull( original ) == false ) + { + return NULL; + } + + b3HullData* hull = (b3HullData*)b3Alloc( original->byteCount ); + memcpy( hull, original, original->byteCount ); + + b3Vec3 safeScale = b3SafeScale( scale ); + + b3HullHalfEdge* edges = b3GetHullEdgesWrite( hull ); + const b3HullFace* faces = b3GetHullFaces( hull ); + int faceCount = hull->faceCount; + int vertexCount = hull->vertexCount; + + if ( safeScale.x * safeScale.y * safeScale.z < 0.0f ) + { + // Reflected: reverse edge winding for each face. + for ( int i = 0; i < faceCount; ++i ) + { + const b3HullFace* face = faces + i; + + uint8_t startEdgeIndex = face->edge; + uint8_t currentEdgeIndex = startEdgeIndex; + uint8_t prevEdgeIndex = UINT8_MAX; + + do + { + b3HullHalfEdge* edge = edges + currentEdgeIndex; + + if ( edge->next == startEdgeIndex ) + { + prevEdgeIndex = currentEdgeIndex; + break; + } + + currentEdgeIndex = edge->next; + } + while ( currentEdgeIndex != startEdgeIndex ); + + B3_ASSERT( prevEdgeIndex != UINT8_MAX ); + + currentEdgeIndex = startEdgeIndex; + + do + { + b3HullHalfEdge* edge = edges + currentEdgeIndex; + uint8_t nextIndex = edge->next; + edge->next = prevEdgeIndex; + + if ( currentEdgeIndex < edge->twin ) + { + b3HullHalfEdge* twin = edges + edge->twin; + B3_SWAP( edge->origin, twin->origin ); + } + + prevEdgeIndex = currentEdgeIndex; + currentEdgeIndex = nextIndex; + } + while ( currentEdgeIndex != startEdgeIndex ); + } + + b3HullVertex* vertices = b3GetHullVerticesWrite( hull ); + + for ( int i = 0; i < vertexCount; ++i ) + { + b3HullVertex* vertex = vertices + i; + const b3HullHalfEdge* edge = edges + vertex->edge; + vertex->edge = edge->twin; + } + } + + b3Matrix3 matrix = b3MakeMatrixFromQuat( transform.q ); + b3Vec3* points = b3GetHullPointsWrite( hull ); + + for ( int i = 0; i < vertexCount; ++i ) + { + points[i] = b3Add( b3MulMV( matrix, b3Mul( safeScale, points[i] ) ), transform.p ); + } + + b3Plane* planes = b3GetHullPlanesWrite( hull ); + + for ( int i = 0; i < faceCount; ++i ) + { + int count = 0; + b3Vec3 centroid = b3Vec3_zero; + b3Vec3 normal = b3Vec3_zero; + + const b3HullFace* face = faces + i; + uint8_t startEdgeIndex = face->edge; + uint8_t currentEdgeIndex = startEdgeIndex; + + const b3HullHalfEdge* startEdge = edges + currentEdgeIndex; + B3_ASSERT( startEdge->face == i ); + B3_ASSERT( startEdge->origin < vertexCount ); + + b3Vec3 origin = points[startEdge->origin]; + + do + { + b3HullHalfEdge* edge = edges + currentEdgeIndex; + b3HullHalfEdge* twin = edges + edge->twin; + B3_ASSERT( twin->twin == currentEdgeIndex ); + + b3Vec3 v1 = b3Sub( points[edge->origin], origin ); + b3Vec3 v2 = b3Sub( points[twin->origin], origin ); + + count++; + centroid = b3Add( centroid, v1 ); + normal.x += ( v1.y - v2.y ) * ( v1.z + v2.z ); + normal.y += ( v1.z - v2.z ) * ( v1.x + v2.x ); + normal.z += ( v1.x - v2.x ) * ( v1.y + v2.y ); + + currentEdgeIndex = edge->next; + } + while ( currentEdgeIndex != startEdgeIndex ); + + B3_ASSERT( count > 0 ); + centroid = b3MulSV( 1.0f / (float)count, centroid ); + centroid = b3Add( centroid, origin ); + + float area = b3Length( normal ); + B3_ASSERT( area > 0.0f ); + normal = b3MulSV( 1.0f / area, normal ); + + planes[i] = b3MakePlaneFromNormalAndPoint( normal, centroid ); + } + + b3UpdateHullBounds( hull ); + bool success = b3UpdateHullBulkProperties( hull ); + if ( success == false ) + { + b3Free( hull, original->byteCount ); + return NULL; + } + + hull->hash = 0; + hull->hash = b3NonZeroHash( b3Hash( B3_HASH_INIT, (uint8_t*)hull, hull->byteCount ) ); + + B3_VALIDATE( b3IsValidHull( hull ) ); + + return hull; +} + +void b3DestroyHull( b3HullData* hull ) +{ + b3Free( hull, hull->byteCount ); +} + +b3MassData b3ComputeHullMass( const b3HullData* shape, float density ) +{ + b3MassData out; + out.mass = density * shape->volume; + out.center = shape->center; + + // Inertia about the center of mass + out.inertia = b3MulSM( density, shape->centralInertia ); + return out; +} + +b3AABB b3ComputeHullAABB( const b3HullData* shape, b3Transform transform ) +{ + return b3AABB_Transform( transform, shape->aabb ); +} + +b3AABB b3ComputeSweptHullAABB( const b3HullData* shape, b3Transform xf1, b3Transform xf2 ) +{ + b3AABB aabb1 = b3AABB_Transform( xf1, shape->aabb ); + b3AABB aabb2 = b3AABB_Transform( xf2, shape->aabb ); + return b3AABB_Union( aabb1, aabb2 ); +} + +bool b3OverlapHull( const b3HullData* shape, b3Transform shapeTransform, const b3ShapeProxy* proxy ) +{ + const b3Vec3* points = b3GetHullPoints( shape ); + + b3DistanceInput input; + input.proxyA = (b3ShapeProxy){ points, shape->vertexCount, 0.0f }; + input.proxyB = *proxy; + input.transform = b3InvMulTransforms( shapeTransform, b3Transform_identity ); + input.useRadii = true; + + b3SimplexCache cache = { 0 }; + b3DistanceOutput output = b3ShapeDistance( &input, &cache, NULL, 0 ); + return output.distance < B3_OVERLAP_SLOP; +} + +b3CastOutput b3RayCastHull( const b3HullData* shape, const b3RayCastInput* input ) +{ + B3_ASSERT( b3IsValidRay( input ) ); + b3CastOutput output = { 0 }; + + float lower = 0.0f; + float upper = input->maxFraction; + int bestFace = B3_NULL_INDEX; + + const b3Plane* planes = b3GetHullPlanes( shape ); + + for ( int faceIndex = 0; faceIndex < shape->faceCount; ++faceIndex ) + { + b3Plane plane = planes[faceIndex]; + + float distance = plane.offset - b3Dot( plane.normal, input->origin ); + float denominator = b3Dot( plane.normal, input->translation ); + + if ( denominator == 0.0f ) + { + if ( distance < 0.0f ) + { + return output; + } + } + else + { + float fraction = distance / denominator; + + if ( denominator < 0.0f ) + { + if ( fraction > lower ) + { + bestFace = faceIndex; + lower = fraction; + } + } + else + { + if ( fraction < upper ) + { + upper = fraction; + } + } + + if ( upper < lower ) + { + return output; + } + } + } + + if ( bestFace >= 0 ) + { + output.point = b3Add( input->origin, b3MulSV( lower, input->translation ) ); + output.normal = planes[bestFace].normal; + output.fraction = lower; + output.hit = true; + } + else + { + output.point = input->origin; + output.hit = true; + } + + return output; +} + +b3CastOutput b3ShapeCastHull( const b3HullData* shape, const b3ShapeCastInput* input ) +{ + const b3Vec3* points = b3GetHullPoints( shape ); + + b3ShapeCastPairInput pairInput; + pairInput.proxyA = (b3ShapeProxy){ points, shape->vertexCount, 0.0f }; + pairInput.proxyB = input->proxy; + pairInput.transform = b3Transform_identity; + pairInput.translationB = input->translation; + pairInput.maxFraction = input->maxFraction; + pairInput.canEncroach = input->canEncroach; + + b3CastOutput output = b3ShapeCast( &pairInput ); + return output; +} + +int b3CollideMoverAndHull( b3PlaneResult* result, const b3HullData* shape, const b3Capsule* mover ) +{ + const b3Vec3* points = b3GetHullPoints( shape ); + b3DistanceInput distanceInput; + distanceInput.proxyA = (b3ShapeProxy){ points, shape->vertexCount, 0.0f }; + distanceInput.proxyB = (b3ShapeProxy){ &mover->center1, 2, mover->radius }; + distanceInput.transform = b3Transform_identity; + distanceInput.useRadii = false; + + float totalRadius = mover->radius; + + b3SimplexCache cache = { 0 }; + b3DistanceOutput distanceOutput = b3ShapeDistance( &distanceInput, &cache, NULL, 0 ); + + if ( distanceOutput.distance == 0.0f ) + { + // I could handle deep overlap on hulls, but there is no reasonable solution for + // deep overlap on meshes. So if someone converted a hull to a mesh there would be + // different behavior. So I think there is not a good reason to handle deep overlap + // on hulls. + return 0; + } + + if ( distanceOutput.distance <= totalRadius ) + { + b3Plane plane = { distanceOutput.normal, totalRadius - distanceOutput.distance }; + *result = (b3PlaneResult){ plane, distanceOutput.pointA }; + return 1; + } + + return 0; +} + +b3ShapeExtent b3ComputeHullExtent( const b3HullData* hull, b3Vec3 origin ) +{ + const b3Vec3* points = b3GetHullPoints( hull ); + + b3ShapeExtent extent; + extent.minExtent = hull->innerRadius; + extent.maxExtent = b3Vec3_zero; + for ( int index = 0; index < hull->vertexCount; ++index ) + { + b3Vec3 point = points[index]; + extent.maxExtent = b3Max( extent.maxExtent, b3Abs( b3Sub( point, origin ) ) ); + } + + return extent; +} + +float b3ComputeHullProjectedArea( const b3HullData* hull, b3Vec3 direction ) +{ + float area = 0.0f; + + int faceCount = hull->faceCount; + const b3HullFace* hullFaces = b3GetHullFaces( hull ); + const b3HullHalfEdge* hullEdges = b3GetHullEdges( hull ); + const b3Vec3* hullPoints = b3GetHullPoints( hull ); + + for ( int i = 0; i < faceCount; ++i ) + { + const b3HullFace* face = hullFaces + i; + + int baseEdge = face->edge; + const b3HullHalfEdge* edge = hullEdges + baseEdge; + b3Vec3 p1 = hullPoints[edge->origin]; + + int edgeIndex = edge->next; + edge = hullEdges + edgeIndex; + b3Vec3 p2 = hullPoints[edge->origin]; + + edgeIndex = edge->next; + + do + { + edge = hullEdges + edgeIndex; + b3Vec3 p3 = hullPoints[edge->origin]; + + b3Vec3 e1 = b3Sub( p2, p1 ); + b3Vec3 e2 = b3Sub( p3, p1 ); + b3Vec3 n = b3Cross( e1, e2 ); + float a = b3Dot( n, direction ); + area += b3MaxFloat( a, 0.0f ); + + p2 = p3; + edgeIndex = edge->next; + } + while ( edgeIndex != baseEdge ); + } + + return 0.5f * area; +} + +// Constant template box (vertex/edge/face/topology). b3MakeTransformedBoxHull copies and +// fills in the runtime-dependent fields (boxPoints, boxPlanes, aabb, mass properties, hash). +static const b3BoxHull s_boxHull = { + .base = + { + .version = B3_HULL_VERSION, + .byteCount = sizeof( b3BoxHull ), + .hash = 0, + .vertexCount = 8, + .edgeCount = 24, + .faceCount = 6, + .vertexOffset = offsetof( b3BoxHull, boxVertices ), + .pointOffset = offsetof( b3BoxHull, boxPoints ), + .edgeOffset = offsetof( b3BoxHull, boxEdges ), + .faceOffset = offsetof( b3BoxHull, boxFaces ), + .planeOffset = offsetof( b3BoxHull, boxPlanes ), + }, + .boxVertices = + { + [0] = { .edge = 8 }, + [1] = { .edge = 1 }, + [2] = { .edge = 0 }, + [3] = { .edge = 9 }, + [4] = { .edge = 13 }, + [5] = { .edge = 3 }, + [6] = { .edge = 5 }, + [7] = { .edge = 11 }, + }, + .boxEdges = + { + [0] = { 2, 1, 2, 0 }, [1] = { 17, 0, 1, 5 }, [2] = { 4, 3, 1, 0 }, [3] = { 20, 2, 5, 3 }, + [4] = { 6, 5, 5, 0 }, [5] = { 23, 4, 6, 4 }, [6] = { 0, 7, 6, 0 }, [7] = { 18, 6, 2, 2 }, + [8] = { 10, 9, 0, 1 }, [9] = { 21, 8, 3, 5 }, [10] = { 12, 11, 3, 1 }, [11] = { 16, 10, 7, 2 }, + [12] = { 14, 13, 7, 1 }, [13] = { 19, 12, 4, 4 }, [14] = { 8, 15, 4, 1 }, [15] = { 22, 14, 0, 3 }, + [16] = { 7, 17, 3, 2 }, [17] = { 9, 16, 2, 5 }, [18] = { 11, 19, 6, 2 }, [19] = { 5, 18, 7, 4 }, + [20] = { 15, 21, 1, 3 }, [21] = { 1, 20, 0, 5 }, [22] = { 3, 23, 4, 3 }, [23] = { 13, 22, 5, 4 }, + }, + .boxFaces = + { + [0] = { .edge = 0 }, + [1] = { .edge = 8 }, + [2] = { .edge = 16 }, + [3] = { .edge = 20 }, + [4] = { .edge = 19 }, + [5] = { .edge = 21 }, + }, +}; + +b3BoxHull b3MakeTransformedBoxHull( float hx, float hy, float hz, b3Transform transform ) +{ + b3BoxHull boxHull = s_boxHull; + + float minH = 0.2f * B3_LINEAR_SLOP; + b3Vec3 h = b3Max( (b3Vec3){ minH, minH, minH }, (b3Vec3){ hx, hy, hz } ); + + boxHull.base.aabb = b3AABB_Transform( transform, (b3AABB){ b3Neg( h ), h } ); + boxHull.base.surfaceArea = 8.0f * ( h.x * h.y + h.x * h.z + h.y * h.z ); + boxHull.base.volume = 8.0f * h.x * h.y * h.z; + boxHull.base.innerRadius = b3MinFloat( h.x, b3MinFloat( h.y, h.z ) ); + boxHull.base.center = transform.p; + + b3Matrix3 boxInertia = b3BoxInertia( boxHull.base.volume, b3Neg( h ), h ); + boxHull.base.centralInertia = b3RotateInertia( transform.q, boxInertia ); + + b3Vec3 lower = b3Neg( h ); + b3Vec3 upper = h; + + boxHull.boxPlanes[0] = b3TransformPlane( transform, b3MakePlaneFromNormalAndPoint( b3Neg( b3Vec3_axisX ), lower ) ); + boxHull.boxPlanes[1] = b3TransformPlane( transform, b3MakePlaneFromNormalAndPoint( b3Vec3_axisX, upper ) ); + boxHull.boxPlanes[2] = b3TransformPlane( transform, b3MakePlaneFromNormalAndPoint( b3Neg( b3Vec3_axisY ), lower ) ); + boxHull.boxPlanes[3] = b3TransformPlane( transform, b3MakePlaneFromNormalAndPoint( b3Vec3_axisY, upper ) ); + boxHull.boxPlanes[4] = b3TransformPlane( transform, b3MakePlaneFromNormalAndPoint( b3Neg( b3Vec3_axisZ ), lower ) ); + boxHull.boxPlanes[5] = b3TransformPlane( transform, b3MakePlaneFromNormalAndPoint( b3Vec3_axisZ, upper ) ); + + boxHull.boxPoints[0] = b3TransformPoint( transform, (b3Vec3){ h.x, h.y, h.z } ); + boxHull.boxPoints[1] = b3TransformPoint( transform, (b3Vec3){ -h.x, h.y, h.z } ); + boxHull.boxPoints[2] = b3TransformPoint( transform, (b3Vec3){ -h.x, -h.y, h.z } ); + boxHull.boxPoints[3] = b3TransformPoint( transform, (b3Vec3){ h.x, -h.y, h.z } ); + boxHull.boxPoints[4] = b3TransformPoint( transform, (b3Vec3){ h.x, h.y, -h.z } ); + boxHull.boxPoints[5] = b3TransformPoint( transform, (b3Vec3){ -h.x, h.y, -h.z } ); + boxHull.boxPoints[6] = b3TransformPoint( transform, (b3Vec3){ -h.x, -h.y, -h.z } ); + boxHull.boxPoints[7] = b3TransformPoint( transform, (b3Vec3){ h.x, -h.y, -h.z } ); + + boxHull.base.hash = 0; + boxHull.base.hash = b3NonZeroHash( b3Hash( B3_HASH_INIT, (uint8_t*)&boxHull, sizeof( b3BoxHull ) ) ); + + return boxHull; +} + +b3BoxHull b3MakeCubeHull( float halfWidth ) +{ + return b3MakeBoxHull( halfWidth, halfWidth, halfWidth ); +} + +b3BoxHull b3MakeOffsetBoxHull( float hx, float hy, float hz, b3Vec3 offset ) +{ + b3Transform transform = { .p = offset, .q = b3Quat_identity }; + return b3MakeTransformedBoxHull( hx, hy, hz, transform ); +} + +b3BoxHull b3MakeBoxHull( float hx, float hy, float hz ) +{ + return b3MakeTransformedBoxHull( hx, hy, hz, b3Transform_identity ); +} + +void b3ScaleBox( b3Vec3* halfWidths, b3Transform* transform, b3Vec3 postScale, float minHalfWidth ) +{ + B3_ASSERT( b3IsValidFloat( minHalfWidth ) && minHalfWidth > 0.0f ); + + b3Quat q = transform->q; + + if ( postScale.x < 0.0f || postScale.y < 0.0f || postScale.z < 0.0f ) + { + // todo this might be unnecessary if rotation is identity + // todo compare with polar decomposition (much more expensive) + b3Matrix3 m = b3MakeMatrixFromQuat( q ); + m.cx.x *= postScale.x; + m.cy.x *= postScale.x; + m.cz.x *= postScale.x; + m.cx.y *= postScale.y; + m.cy.y *= postScale.y; + m.cz.y *= postScale.y; + m.cx.z *= postScale.z; + m.cy.z *= postScale.z; + m.cz.z *= postScale.z; + m.cx = b3Normalize( m.cx ); + m.cy = b3Normalize( m.cy ); + m.cz = b3Normalize( m.cz ); + m.cx = postScale.x < 0.0f ? b3Neg( m.cx ) : m.cx; + m.cy = postScale.y < 0.0f ? b3Neg( m.cy ) : m.cy; + m.cz = postScale.z < 0.0f ? b3Neg( m.cz ) : m.cz; + q = b3MakeQuatFromMatrix( &m ); + } + + b3Vec3 absScale = b3Abs( postScale ); + + b3Vec3 h = *halfWidths; + b3Vec3 p1 = b3Mul( absScale, b3RotateVector( q, b3Neg( h ) ) ); + b3Vec3 p2 = b3Mul( absScale, b3RotateVector( q, h ) ); + + b3Vec3 localP1 = b3InvRotateVector( q, p1 ); + b3Vec3 localP2 = b3InvRotateVector( q, p2 ); + + b3Vec3 lower = b3Min( localP1, localP2 ); + b3Vec3 upper = b3Max( localP1, localP2 ); + + b3Vec3 scaledHalfWidth = b3MulSV( 0.5f, b3Sub( upper, lower ) ); + + b3Vec3 mLimit = { minHalfWidth, minHalfWidth, minHalfWidth }; + *halfWidths = b3Max( scaledHalfWidth, mLimit ); + transform->p = b3Mul( postScale, transform->p ); + transform->q = q; +} + +// todo use new hull scaling technique +b3BoxHull b3MakeScaledBoxHull( b3Vec3 halfWidths, b3Transform transform, b3Vec3 postScale ) +{ + b3Vec3 h = halfWidths; + b3Transform xf = transform; + b3ScaleBox( &h, &xf, postScale, 4.0f * B3_LINEAR_SLOP ); + return b3MakeTransformedBoxHull( h.x, h.y, h.z, xf ); +} diff --git a/vendor/box3d/src/src/hull_map.h b/vendor/box3d/src/src/hull_map.h new file mode 100644 index 000000000..5b50c56eb --- /dev/null +++ b/vendor/box3d/src/src/hull_map.h @@ -0,0 +1,24 @@ +// SPDX-FileCopyrightText: 2026 Erin Catto +// SPDX-License-Identifier: MIT + +#pragma once + +#include "box3d/types.h" + +#include + +// Content hash and equality over the whole baked hull. Shared by every hull +// de-duplication map so they agree on identity. +uint64_t b3HashHullData( const b3HullData* hull ); +bool b3CompareHullData( const b3HullData* hull1, const b3HullData* hull2 ); + +// Map keyed by hull content. The world hull database stores a reference count, +// compound baking stores a byte offset. Implementation lives in hull.c. +#define NAME b3HullMap +#define KEY_TY const b3HullData* +#define VAL_TY int +#define HEADER_MODE +#include "verstable.h" + +// Total map allocation in bytes, excluding the stored hull data +size_t b3HullMapByteCount( b3HullMap* map ); diff --git a/vendor/box3d/src/src/id_pool.c b/vendor/box3d/src/src/id_pool.c new file mode 100644 index 000000000..185b6eebc --- /dev/null +++ b/vendor/box3d/src/src/id_pool.c @@ -0,0 +1,73 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +#include "id_pool.h" + +b3IdPool b3CreateIdPool( void ) +{ + b3IdPool pool = { 0 }; + b3Array_Reserve( pool.freeArray, 32 ); + return pool; +} + +void b3DestroyIdPool( b3IdPool* pool ) +{ + b3Array_Destroy( pool->freeArray ); + *pool = (b3IdPool){ 0 }; +} + +int b3AllocId( b3IdPool* pool ) +{ + int count = pool->freeArray.count; + if ( count > 0 ) + { + int id = b3Array_Pop( pool->freeArray ); + return id; + } + + int id = pool->nextIndex; + pool->nextIndex += 1; + return id; +} + +void b3FreeId( b3IdPool* pool, int id ) +{ + B3_ASSERT( pool->nextIndex > 0 ); + B3_ASSERT( 0 <= id && id < pool->nextIndex ); + + // todo does not work with assertion above + // should probably be `id == pool->nextIndex - 1` + if ( id == pool->nextIndex ) + { + pool->nextIndex -= 1; + return; + } + + b3Array_Push( pool->freeArray, id ); +} + +#if B3_ENABLE_VALIDATION + +void b3ValidateFreeId( const b3IdPool* pool, int id ) +{ + int freeCount = pool->freeArray.count; + for ( int i = 0; i < freeCount; ++i ) + { + if ( pool->freeArray.data[i] == id ) + { + return; + } + } + + B3_ASSERT( 0 ); +} + +#else + +void b3ValidateFreeId( const b3IdPool* pool, int id ) +{ + B3_UNUSED( pool ); + B3_UNUSED( id ); +} + +#endif diff --git a/vendor/box3d/src/src/id_pool.h b/vendor/box3d/src/src/id_pool.h new file mode 100644 index 000000000..06106de20 --- /dev/null +++ b/vendor/box3d/src/src/id_pool.h @@ -0,0 +1,34 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +#pragma once + +#include "container.h" + +typedef struct b3IdPool +{ + b3Array( int ) freeArray; + int nextIndex; +} b3IdPool; + +b3IdPool b3CreateIdPool( void ); +void b3DestroyIdPool( b3IdPool* pool ); + +int b3AllocId( b3IdPool* pool ); +void b3FreeId( b3IdPool* pool, int id ); +void b3ValidateFreeId( const b3IdPool* pool, int id ); + +static inline int b3GetIdCount( const b3IdPool* pool ) +{ + return pool->nextIndex - pool->freeArray.count; +} + +static inline int b3GetIdCapacity( const b3IdPool* pool ) +{ + return pool->nextIndex; +} + +static inline int b3GetIdBytes( const b3IdPool* pool ) +{ + return b3Array_ByteCount( pool->freeArray ); +} diff --git a/vendor/box3d/src/src/island.c b/vendor/box3d/src/src/island.c new file mode 100644 index 000000000..625a1b345 --- /dev/null +++ b/vendor/box3d/src/src/island.c @@ -0,0 +1,735 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +#include "island.h" + +#include "body.h" +#include "contact.h" +#include "core.h" +#include "id_pool.h" +#include "joint.h" +#include "physics_world.h" +#include "solver_set.h" + +#include + +b3Island* b3CreateIsland( b3World* world, int setIndex ) +{ + B3_ASSERT( setIndex == b3_awakeSet || setIndex >= b3_firstSleepingSet ); + + int islandId = b3AllocId( &world->islandIdPool ); + + if ( islandId == world->islands.count ) + { + b3Island emptyIsland = { 0 }; + b3Array_Push( world->islands, emptyIsland ); + } + else + { + B3_ASSERT( world->islands.data[islandId].setIndex == B3_NULL_INDEX ); + } + + b3SolverSet* set = b3Array_Get( world->solverSets, setIndex ); + + b3Island* island = b3Array_Get( world->islands, islandId ); + island->setIndex = setIndex; + island->localIndex = set->islandSims.count; + island->islandId = islandId; + b3Array_Create( island->bodies ); + b3Array_Create( island->contacts ); + b3Array_Create( island->joints ); + island->constraintRemoveCount = 0; + + b3IslandSim* islandSim = b3Array_Emplace( set->islandSims ); + islandSim->islandId = islandId; + + return island; +} + +void b3DestroyIsland( b3World* world, int islandId ) +{ + if ( world->splitIslandId == islandId ) + { + world->splitIslandId = B3_NULL_INDEX; + } + + // assume island is empty + b3Island* island = b3Array_Get( world->islands, islandId ); + b3SolverSet* set = b3Array_Get( world->solverSets, island->setIndex ); + { + int localIndex = island->localIndex; + int lastIndex = set->islandSims.count - 1; + B3_ASSERT( 0 <= localIndex && localIndex <= lastIndex ); + int moveIslandId = set->islandSims.data[lastIndex].islandId; + set->islandSims.data[localIndex] = set->islandSims.data[lastIndex]; + world->islands.data[moveIslandId].localIndex = localIndex; + set->islandSims.count -= 1; + } + + // Free island and id (preserve island generation) + b3Array_Destroy( island->bodies ); + b3Array_Destroy( island->contacts ); + b3Array_Destroy( island->joints ); + island->constraintRemoveCount = 0; + island->localIndex = B3_NULL_INDEX; + island->islandId = B3_NULL_INDEX; + island->setIndex = B3_NULL_INDEX; + + b3FreeId( &world->islandIdPool, islandId ); +} + +static int b3MergeIslands( b3World* world, int islandIdA, int islandIdB ) +{ + if ( islandIdA == islandIdB ) + { + return islandIdA; + } + + if ( islandIdA == B3_NULL_INDEX ) + { + B3_ASSERT( islandIdB != B3_NULL_INDEX ); + return islandIdB; + } + + if ( islandIdB == B3_NULL_INDEX ) + { + B3_ASSERT( islandIdA != B3_NULL_INDEX ); + return islandIdA; + } + + b3Island* smallIsland; + b3Island* bigIsland; + { + b3Island* islandA = b3Array_Get( world->islands, islandIdA ); + b3Island* islandB = b3Array_Get( world->islands, islandIdB ); + + // Keep the biggest island to reduce cache misses + if ( islandA->bodies.count >= islandB->bodies.count ) + { + bigIsland = islandA; + smallIsland = islandB; + } + else + { + bigIsland = islandB; + smallIsland = islandA; + } + } + + int bigIslandId = bigIsland->islandId; + b3Array_Reserve( bigIsland->bodies, bigIsland->bodies.count + smallIsland->bodies.count ); + + // Move bodies from smaller island to larger island + for ( int i = 0; i < smallIsland->bodies.count; ++i ) + { + int bodyId = smallIsland->bodies.data[i]; + b3Body* body = b3Array_Get( world->bodies, bodyId ); + B3_VALIDATE( body->islandId == smallIsland->islandId ); + body->islandId = bigIslandId; + body->islandIndex = bigIsland->bodies.count; + b3Array_Push( bigIsland->bodies, bodyId ); + } + + // Migrate contacts from smaller island to larger island + if ( smallIsland->contacts.count > 0 ) + { + b3Array_Reserve( bigIsland->contacts, bigIsland->contacts.count + smallIsland->contacts.count ); + + for ( int i = 0; i < smallIsland->contacts.count; ++i ) + { + b3ContactLink* link = smallIsland->contacts.data + i; + b3Contact* contact = b3Array_Get( world->contacts, link->contactId ); + contact->islandId = bigIslandId; + contact->islandIndex = bigIsland->contacts.count; + b3Array_Push( bigIsland->contacts, *link ); + } + } + + // Migrate joints from smaller island to larger island + if ( smallIsland->joints.count > 0 ) + { + b3Array_Reserve( bigIsland->joints, bigIsland->joints.count + smallIsland->joints.count ); + + for ( int i = 0; i < smallIsland->joints.count; ++i ) + { + b3JointLink* link = smallIsland->joints.data + i; + b3Joint* joint = b3Array_Get( world->joints, link->jointId ); + joint->islandId = bigIslandId; + joint->islandIndex = bigIsland->joints.count; + b3Array_Push( bigIsland->joints, *link ); + } + } + + // Track removed constraints + bigIsland->constraintRemoveCount += smallIsland->constraintRemoveCount; + + b3DestroyIsland( world, smallIsland->islandId ); + + b3ValidateIsland( world, bigIslandId ); + + return bigIslandId; +} + +static void b3AddContactToIsland( b3World* world, int islandId, b3Contact* contact ) +{ + B3_ASSERT( contact->islandId == B3_NULL_INDEX ); + B3_ASSERT( contact->islandIndex == B3_NULL_INDEX ); + + b3Island* island = b3Array_Get( world->islands, islandId ); + + contact->islandId = islandId; + contact->islandIndex = island->contacts.count; + + b3ContactLink link; + link.contactId = contact->contactId; + link.bodyIdA = contact->edges[0].bodyId; + link.bodyIdB = contact->edges[1].bodyId; + b3Array_Push( island->contacts, link ); + + b3ValidateIsland( world, islandId ); +} + +// Link a contact into an island. +void b3LinkContact( b3World* world, b3Contact* contact ) +{ + B3_ASSERT( ( contact->flags & b3_contactTouchingFlag ) != 0 ); + + int bodyIdA = contact->edges[0].bodyId; + int bodyIdB = contact->edges[1].bodyId; + + b3Body* bodyA = b3Array_Get( world->bodies, bodyIdA ); + b3Body* bodyB = b3Array_Get( world->bodies, bodyIdB ); + + B3_ASSERT( bodyA->setIndex != b3_disabledSet && bodyB->setIndex != b3_disabledSet ); + B3_ASSERT( bodyA->setIndex != b3_staticSet || bodyB->setIndex != b3_staticSet ); + + // Wake bodyB if bodyA is awake and bodyB is sleeping + if ( bodyA->setIndex == b3_awakeSet && bodyB->setIndex >= b3_firstSleepingSet ) + { + b3WakeSolverSet( world, bodyB->setIndex ); + } + + // Wake bodyA if bodyB is awake and bodyA is sleeping + if ( bodyB->setIndex == b3_awakeSet && bodyA->setIndex >= b3_firstSleepingSet ) + { + b3WakeSolverSet( world, bodyA->setIndex ); + } + + int islandIdA = bodyA->islandId; + int islandIdB = bodyB->islandId; + + // Static bodies have null island indices. + B3_ASSERT( bodyA->setIndex != b3_staticSet || islandIdA == B3_NULL_INDEX ); + B3_ASSERT( bodyB->setIndex != b3_staticSet || islandIdB == B3_NULL_INDEX ); + B3_ASSERT( islandIdA != B3_NULL_INDEX || islandIdB != B3_NULL_INDEX ); + + // Merge islands. This will destroy one of the islands. + int finalIslandId = b3MergeIslands( world, islandIdA, islandIdB ); + + // Add contact to the island that survived + b3AddContactToIsland( world, finalIslandId, contact ); +} + +// This is called when a contact no longer has contact points or when a contact is destroyed. +void b3UnlinkContact( b3World* world, b3Contact* contact ) +{ + B3_ASSERT( contact->islandId != B3_NULL_INDEX ); + + // remove from island + int islandId = contact->islandId; + b3Island* island = b3Array_Get( world->islands, islandId ); + + int removeIndex = contact->islandIndex; + B3_ASSERT( 0 <= removeIndex && removeIndex < island->contacts.count ); + B3_ASSERT( island->contacts.data[removeIndex].contactId == contact->contactId ); + + int movedIndex = b3Array_RemoveSwap( island->contacts, removeIndex ); + if ( movedIndex != B3_NULL_INDEX ) + { + // Fix islandIndex on the contact that was swapped into removeIndex + b3ContactLink* movedLink = island->contacts.data + removeIndex; + b3Contact* movedContact = b3Array_Get( world->contacts, movedLink->contactId ); + B3_ASSERT( movedContact->islandIndex == movedIndex ); + movedContact->islandIndex = removeIndex; + } + + contact->islandId = B3_NULL_INDEX; + contact->islandIndex = B3_NULL_INDEX; + island->constraintRemoveCount += 1; + + b3ValidateIsland( world, islandId ); +} + +static void b3AddJointToIsland( b3World* world, int islandId, b3Joint* joint ) +{ + B3_ASSERT( joint->islandId == B3_NULL_INDEX ); + B3_ASSERT( joint->islandIndex == B3_NULL_INDEX ); + + b3Island* island = b3Array_Get( world->islands, islandId ); + + joint->islandId = islandId; + joint->islandIndex = island->joints.count; + + b3JointLink link; + link.jointId = joint->jointId; + link.bodyIdA = joint->edges[0].bodyId; + link.bodyIdB = joint->edges[1].bodyId; + b3Array_Push( island->joints, link ); + + b3ValidateIsland( world, islandId ); +} + +void b3LinkJoint( b3World* world, b3Joint* joint ) +{ + b3Body* bodyA = b3Array_Get( world->bodies, joint->edges[0].bodyId ); + b3Body* bodyB = b3Array_Get( world->bodies, joint->edges[1].bodyId ); + + B3_ASSERT( bodyA->type == b3_dynamicBody || bodyB->type == b3_dynamicBody ); + + if ( bodyA->setIndex == b3_awakeSet && bodyB->setIndex >= b3_firstSleepingSet ) + { + b3WakeSolverSet( world, bodyB->setIndex ); + } + else if ( bodyB->setIndex == b3_awakeSet && bodyA->setIndex >= b3_firstSleepingSet ) + { + b3WakeSolverSet( world, bodyA->setIndex ); + } + + int islandIdA = bodyA->islandId; + int islandIdB = bodyB->islandId; + + B3_ASSERT( islandIdA != B3_NULL_INDEX || islandIdB != B3_NULL_INDEX ); + + // Merge islands. This will destroy one of the islands. + int finalIslandId = b3MergeIslands( world, islandIdA, islandIdB ); + + // Add joint the island that survived + b3AddJointToIsland( world, finalIslandId, joint ); +} + +void b3UnlinkJoint( b3World* world, b3Joint* joint ) +{ + if ( joint->islandId == B3_NULL_INDEX ) + { + return; + } + + // remove from island + int islandId = joint->islandId; + b3Island* island = b3Array_Get( world->islands, islandId ); + + int removeIndex = joint->islandIndex; + B3_ASSERT( 0 <= removeIndex && removeIndex < island->joints.count ); + B3_ASSERT( island->joints.data[removeIndex].jointId == joint->jointId ); + + int movedIndex = b3Array_RemoveSwap( island->joints, removeIndex ); + if ( movedIndex != B3_NULL_INDEX ) + { + // Fix islandIndex on the joint that was swapped into removeIndex + b3JointLink* movedLink = island->joints.data + removeIndex; + b3Joint* movedJoint = b3Array_Get( world->joints, movedLink->jointId ); + B3_ASSERT( movedJoint->islandIndex == movedIndex ); + movedJoint->islandIndex = removeIndex; + } + + joint->islandId = B3_NULL_INDEX; + joint->islandIndex = B3_NULL_INDEX; + island->constraintRemoveCount += 1; + + b3ValidateIsland( world, islandId ); +} + +// Find parent of a node. Use path halving to speed up further queries. +static inline int b3IslandFindParent( int* parents, int node ) +{ + // Walk the chain of parents to find the node that is its own parent (the root) + while ( parents[node] != node ) + { + int grandParent = parents[parents[node]]; + parents[node] = grandParent; + node = grandParent; + } + + return node; +} + +// Connect the components containing node1 and node2. +// Uses rank to keep tree balanced. Tracks per-component contact and joint counts. +static inline void b3IslandUnion( int* parents, int* ranks, int node1, int node2, int* contactCounts, int* jointCounts ) +{ + int root1 = b3IslandFindParent( parents, node1 ); + int root2 = b3IslandFindParent( parents, node2 ); + if ( root1 != root2 ) + { + if ( ranks[root1] < ranks[root2] ) + { + parents[root1] = root2; + contactCounts[root2] += contactCounts[root1]; + jointCounts[root2] += jointCounts[root1]; + } + else if ( ranks[root1] > ranks[root2] ) + { + parents[root2] = root1; + contactCounts[root1] += contactCounts[root2]; + jointCounts[root1] += jointCounts[root2]; + } + else + { + parents[root2] = root1; + ranks[root1] += 1; + contactCounts[root1] += contactCounts[root2]; + jointCounts[root1] += jointCounts[root2]; + } + } +} + +// This uses union-find. +// https://en.wikipedia.org/wiki/Disjoint-set_data_structure +void b3SplitIsland( b3World* world, int baseId ) +{ + b3Island* baseIsland = b3Array_Get( world->islands, baseId ); + B3_ASSERT( baseIsland->constraintRemoveCount > 0 ); + B3_ASSERT( baseIsland->setIndex == b3_awakeSet ); + + b3ValidateIsland( world, baseId ); + + // Cache base island fields before b3CreateIsland, which may reallocate + // world->islands and invalidate the baseIsland pointer. + int baseBodyCount = baseIsland->bodies.count; + int* baseBodyIds = baseIsland->bodies.data; + int baseBodyCapacity = baseIsland->bodies.capacity; + + int baseContactCount = baseIsland->contacts.count; + b3ContactLink* baseContacts = baseIsland->contacts.data; + int baseContactCapacity = baseIsland->contacts.capacity; + + int baseJointCount = baseIsland->joints.count; + b3JointLink* baseJoints = baseIsland->joints.data; + int baseJointCapacity = baseIsland->joints.capacity; + + b3Stack* alloc = &world->stack; + + // No lock is needed because I ensure the allocator is not used while this task is active. + // Allocate contactCounts and jointCounts before ranks so ranks can be freed first (LIFO arena). + int* parents = b3StackAlloc( alloc, baseBodyCount * sizeof( int ), "parents" ); + int* contactCounts = b3StackAlloc( alloc, baseBodyCount * sizeof( int ), "contact counts" ); + int* jointCounts = b3StackAlloc( alloc, baseBodyCount * sizeof( int ), "joint counts" ); + int* ranks = b3StackAlloc( alloc, baseBodyCount * sizeof( int ), "ranks" ); + for ( int i = 0; i < baseBodyCount; ++i ) + { + parents[i] = i; + ranks[i] = 0; + contactCounts[i] = 0; + jointCounts[i] = 0; + } + + b3Body* bodies = world->bodies.data; + + // Union over contacts, tracking per-component contact counts + for ( int i = 0; i < baseContactCount; ++i ) + { + int bodyIdA = baseContacts[i].bodyIdA; + int bodyIdB = baseContacts[i].bodyIdB; + B3_VALIDATE( 0 <= bodyIdA && bodyIdA < world->bodies.count ); + B3_VALIDATE( 0 <= bodyIdB && bodyIdB < world->bodies.count ); + b3Body* bodyA = bodies + bodyIdA; + b3Body* bodyB = bodies + bodyIdB; + int islandIndexA = bodyA->islandIndex; + int islandIndexB = bodyB->islandIndex; + + // Only connect non-static bodies + if ( islandIndexA != B3_NULL_INDEX && islandIndexB != B3_NULL_INDEX ) + { + B3_VALIDATE( 0 <= islandIndexA && islandIndexA < baseBodyCount ); + B3_VALIDATE( 0 <= islandIndexB && islandIndexB < baseBodyCount ); + b3IslandUnion( parents, ranks, islandIndexA, islandIndexB, contactCounts, jointCounts ); + int root = b3IslandFindParent( parents, islandIndexA ); + contactCounts[root] += 1; + } + else + { + int islandIndex = islandIndexA != B3_NULL_INDEX ? islandIndexA : islandIndexB; + int root = b3IslandFindParent( parents, islandIndex ); + contactCounts[root] += 1; + } + } + + // Union over joints, tracking per-component joint counts + for ( int i = 0; i < baseJointCount; ++i ) + { + int bodyIdA = baseJoints[i].bodyIdA; + int bodyIdB = baseJoints[i].bodyIdB; + B3_VALIDATE( 0 <= bodyIdA && bodyIdA < world->bodies.count ); + B3_VALIDATE( 0 <= bodyIdB && bodyIdB < world->bodies.count ); + b3Body* bodyA = bodies + bodyIdA; + b3Body* bodyB = bodies + bodyIdB; + int islandIndexA = bodyA->islandIndex; + int islandIndexB = bodyB->islandIndex; + + // Only connect non-static bodies + if ( islandIndexA != B3_NULL_INDEX && islandIndexB != B3_NULL_INDEX ) + { + B3_VALIDATE( 0 <= islandIndexA && islandIndexA < baseBodyCount ); + B3_VALIDATE( 0 <= islandIndexB && islandIndexB < baseBodyCount ); + b3IslandUnion( parents, ranks, islandIndexA, islandIndexB, contactCounts, jointCounts ); + int root = b3IslandFindParent( parents, islandIndexA ); + jointCounts[root] += 1; + } + else + { + int islandIndex = islandIndexA != B3_NULL_INDEX ? islandIndexA : islandIndexB; + int root = b3IslandFindParent( parents, islandIndex ); + jointCounts[root] += 1; + } + } + + // Done with ranks + b3StackFree( alloc, ranks ); + ranks = NULL; + + // Flatten all parent indices and count connected components. + int componentCount = 0; + for ( int i = 0; i < baseBodyCount; ++i ) + { + parents[i] = b3IslandFindParent( parents, i ); + if ( parents[i] == i ) + { + componentCount += 1; + } + } + + // Early return — island is still fully connected, no split needed. + if ( componentCount == 1 ) + { + baseIsland->constraintRemoveCount = 0; + b3StackFree( alloc, jointCounts ); + b3StackFree( alloc, contactCounts ); + b3StackFree( alloc, parents ); + return; + } + + // Detach body/contact/joint arrays from base island so b3DestroyIsland won't free them + baseIsland->bodies.data = NULL; + baseIsland->bodies.count = 0; + baseIsland->bodies.capacity = 0; + + baseIsland->contacts.data = NULL; + baseIsland->contacts.count = 0; + baseIsland->contacts.capacity = 0; + + baseIsland->joints.data = NULL; + baseIsland->joints.count = 0; + baseIsland->joints.capacity = 0; + + // Null so code below doesn't accidentally use this. + baseIsland = NULL; + + // Map from body index to new island index. Only set for root bodies. + int* rootMap = b3StackAlloc( alloc, baseBodyCount * sizeof( int ), "root map" ); + for ( int i = 0; i < baseBodyCount; ++i ) + { + rootMap[i] = B3_NULL_INDEX; + } + + int* componentBodyCounts = b3StackAlloc( alloc, componentCount * sizeof( int ), "component body counts" ); + int* componentContactCounts = b3StackAlloc( alloc, componentCount * sizeof( int ), "component contact counts" ); + int* componentJointCounts = b3StackAlloc( alloc, componentCount * sizeof( int ), "component joint counts" ); + int islandCount = 0; + + // Find the root body for each body and create islands as needed. + // Extract per-component counts from the root nodes' accumulated counts. + for ( int i = 0; i < baseBodyCount; ++i ) + { + int rootIndex = parents[i]; + if ( rootMap[rootIndex] == B3_NULL_INDEX ) + { + rootMap[rootIndex] = islandCount; + componentBodyCounts[islandCount] = 0; + componentContactCounts[islandCount] = contactCounts[rootIndex]; + componentJointCounts[islandCount] = jointCounts[rootIndex]; + islandCount += 1; + } + + componentBodyCounts[rootMap[rootIndex]] += 1; + } + + B3_ASSERT( islandCount == componentCount ); + + // Map from new island index to island id + int* islandIds = b3StackAlloc( alloc, islandCount * sizeof( int ), "island ids" ); + + // Create new islands and reserve body/contact/joint arrays + for ( int i = 0; i < islandCount; ++i ) + { + // WARNING: this invalidates baseIsland pointer + b3Island* newIsland = b3CreateIsland( world, b3_awakeSet ); + islandIds[i] = newIsland->islandId; + + // Reserve arrays to avoid wasteful growth and memcpy. + b3Array_Reserve( newIsland->bodies, componentBodyCounts[i] ); + b3Array_Reserve( newIsland->contacts, componentContactCounts[i] ); + b3Array_Reserve( newIsland->joints, componentJointCounts[i] ); + } + + // Assign bodies to new islands + for ( int i = 0; i < baseBodyCount; ++i ) + { + int bodyId = baseBodyIds[i]; + int root = b3IslandFindParent( parents, i ); + int newIslandId = islandIds[rootMap[root]]; + + b3Body* body = b3Array_Get( world->bodies, bodyId ); + b3Island* newIsland = b3Array_Get( world->islands, newIslandId ); + + body->islandId = newIslandId; + body->islandIndex = newIsland->bodies.count; + + // Ensure the array has the correct capacity + B3_VALIDATE( newIsland->bodies.count < newIsland->bodies.capacity ); + b3Array_Push( newIsland->bodies, bodyId ); + } + + // Assign contacts to the island of their bodies + for ( int i = 0; i < baseContactCount; ++i ) + { + b3ContactLink* link = baseContacts + i; + b3Contact* contact = b3Array_Get( world->contacts, link->contactId ); + + // Static bodies don't have an island id. + b3Body* bodyA = b3Array_Get( world->bodies, link->bodyIdA ); + b3Body* bodyB = b3Array_Get( world->bodies, link->bodyIdB ); + int targetIslandId = bodyA->islandId != B3_NULL_INDEX ? bodyA->islandId : bodyB->islandId; + + b3Island* targetIsland = b3Array_Get( world->islands, targetIslandId ); + contact->islandId = targetIslandId; + contact->islandIndex = targetIsland->contacts.count; + + // Ensure the array has the correct capacity + B3_VALIDATE( targetIsland->contacts.count < targetIsland->contacts.capacity ); + b3Array_Push( targetIsland->contacts, *link ); + } + + // Assign joints to the island of their bodies + for ( int i = 0; i < baseJointCount; ++i ) + { + b3JointLink* link = baseJoints + i; + b3Joint* joint = b3Array_Get( world->joints, link->jointId ); + + // Static bodies don't have an island id. + b3Body* bodyA = b3Array_Get( world->bodies, link->bodyIdA ); + b3Body* bodyB = b3Array_Get( world->bodies, link->bodyIdB ); + int targetIslandId = bodyA->islandId != B3_NULL_INDEX ? bodyA->islandId : bodyB->islandId; + + b3Island* targetIsland = b3Array_Get( world->islands, targetIslandId ); + joint->islandId = targetIslandId; + joint->islandIndex = targetIsland->joints.count; + + // Ensure the array has the correct capacity + B3_VALIDATE( targetIsland->joints.count < targetIsland->joints.capacity ); + b3Array_Push( targetIsland->joints, *link ); + } + + // Destroy the base island + b3DestroyIsland( world, baseId ); + + // Free the detached arrays manually + b3Free( baseBodyIds, baseBodyCapacity * sizeof( int ) ); + b3Free( baseContacts, baseContactCapacity * sizeof( b3ContactLink ) ); + b3Free( baseJoints, baseJointCapacity * sizeof( b3JointLink ) ); + + // Free arena items in LIFO order + b3StackFree( alloc, islandIds ); + b3StackFree( alloc, componentJointCounts ); + b3StackFree( alloc, componentContactCounts ); + b3StackFree( alloc, componentBodyCounts ); + b3StackFree( alloc, rootMap ); + b3StackFree( alloc, jointCounts ); + b3StackFree( alloc, contactCounts ); + b3StackFree( alloc, parents ); +} + +// Split an island because some contacts and/or joints have been removed. +// This is called during the constraint solve while islands are not being touched. This uses union find and +// touches a lot of memory, so it can be slow. +// Note: contacts/joints connected to static bodies must belong to an island but don't affect island connectivity +// Note: static bodies are never in an island +// Note: this task interacts with some allocators without locks under the assumption that no other tasks +// are interacting with these data structures. +void b3SplitIslandTask( void* context ) +{ + b3TracyCZoneNC( split, "Split Island", b3_colorOlive, true ); + + uint64_t ticks = b3GetTicks(); + b3World* world = context; + + B3_ASSERT( world->splitIslandId != B3_NULL_INDEX ); + + b3SplitIsland( world, world->splitIslandId ); + + world->splitIslandId = B3_NULL_INDEX; + world->profile.splitIslands += b3GetMilliseconds( ticks ); + b3TracyCZoneEnd( split ); +} + +#if B3_ENABLE_VALIDATION +void b3ValidateIsland( b3World* world, int islandId ) +{ + if ( islandId == B3_NULL_INDEX ) + { + return; + } + + b3Island* island = b3Array_Get( world->islands, islandId ); + B3_ASSERT( island->islandId == islandId ); + B3_ASSERT( island->setIndex != B3_NULL_INDEX ); + + { + B3_ASSERT( island->bodies.count > 0 ); + B3_ASSERT( island->bodies.count <= b3GetIdCount( &world->bodyIdPool ) ); + + for ( int i = 0; i < island->bodies.count; ++i ) + { + b3Body* body = b3Array_Get( world->bodies, island->bodies.data[i] ); + B3_ASSERT( body->islandId == islandId ); + B3_ASSERT( body->islandIndex == i ); + B3_ASSERT( body->setIndex == island->setIndex ); + } + } + + if ( island->contacts.count > 0 ) + { + B3_ASSERT( island->contacts.count <= b3GetIdCount( &world->contactIdPool ) ); + + for ( int i = 0; i < island->contacts.count; ++i ) + { + b3ContactLink* link = island->contacts.data + i; + b3Contact* contact = b3Array_Get( world->contacts, link->contactId ); + B3_ASSERT( contact->setIndex == island->setIndex ); + B3_ASSERT( contact->islandId == islandId ); + B3_ASSERT( contact->islandIndex == i ); + } + } + + if ( island->joints.count > 0 ) + { + B3_ASSERT( island->joints.count <= b3GetIdCount( &world->jointIdPool ) ); + + for ( int i = 0; i < island->joints.count; ++i ) + { + b3JointLink* link = island->joints.data + i; + b3Joint* joint = b3Array_Get( world->joints, link->jointId ); + B3_ASSERT( joint->setIndex == island->setIndex ); + B3_ASSERT( joint->islandId == islandId ); + B3_ASSERT( joint->islandIndex == i ); + } + } +} + +#else + +void b3ValidateIsland( b3World* world, int islandId ) +{ + B3_UNUSED( world ); + B3_UNUSED( islandId ); +} +#endif diff --git a/vendor/box3d/src/src/island.h b/vendor/box3d/src/src/island.h new file mode 100644 index 000000000..297c198dd --- /dev/null +++ b/vendor/box3d/src/src/island.h @@ -0,0 +1,101 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +#pragma once + +#include "container.h" + +#include + +typedef struct b3Contact b3Contact; +typedef struct b3Joint b3Joint; +typedef struct b3World b3World; + +// Cached contact data stored in the island for fast contiguous iteration. +// Avoids touching b3Contact during union-find in b3SplitIsland. +typedef struct b3ContactLink +{ + int contactId; + int bodyIdA; + int bodyIdB; +} b3ContactLink; + +b3DeclareArray( b3ContactLink ); + +// Cached joint data stored in the island for fast contiguous iteration. +typedef struct b3JointLink +{ + int jointId; + int bodyIdA; + int bodyIdB; +} b3JointLink; + +b3DeclareArray( b3JointLink ); + +// Deterministic solver +// +// Collide all awake contacts +// Use bit array to emit start/stop touching events in defined order, per thread. Try using contact index, assuming contacts are +// created in a deterministic order. bit-wise OR together bit arrays and issue changes: +// - start touching: merge islands - temporary linked list - mark root island dirty - wake all - largest island is root +// - stop touching: increment constraintRemoveCount + +// Persistent island for awake bodies, joints, and contacts. +// Contacts are touching. +// Contacts and joints may connect to static bodies, but static bodies are not in the island. +// https://en.wikipedia.org/wiki/Component_(graph_theory) +// https://en.wikipedia.org/wiki/Dynamic_connectivity +typedef struct b3Island +{ + // index of solver set stored in b3World + // may be B3_NULL_INDEX + int setIndex; + + // island index within set + // may be B3_NULL_INDEX + int localIndex; + + int islandId; + + // Keeps track of how many contacts have been removed from this island. + // This is used to determine if an island is a candidate for splitting. + int constraintRemoveCount; + + // I tried using a stack array for this but the data pointer goes out of + // sync when the world island array grows. + b3Array( int ) bodies; + + // Contacts and joints that belong to this island. May connect to static + // bodies not in the island. + // Each link has the two body ids so that b3SplitIsland's union-find pass + // never needs to touch b3Contact/b3Joint. + b3Array( b3ContactLink ) contacts; + b3Array( b3JointLink ) joints; + +} b3Island; + +// This is used to move islands across solver sets +typedef struct b3IslandSim +{ + int islandId; +} b3IslandSim; + +b3Island* b3CreateIsland( b3World* world, int setIndex ); +void b3DestroyIsland( b3World* world, int islandId ); + +// Link contacts into the island graph when it starts having contact points +void b3LinkContact( b3World* world, b3Contact* contact ); + +// Unlink contact from the island graph when it stops having contact points +void b3UnlinkContact( b3World* world, b3Contact* contact ); + +// Link a joint into the island graph when it is created +void b3LinkJoint( b3World* world, b3Joint* joint ); + +// Unlink a joint from the island graph when it is destroyed +void b3UnlinkJoint( b3World* world, b3Joint* joint ); + +void b3SplitIsland( b3World* world, int baseId ); +void b3SplitIslandTask( void* context ); + +void b3ValidateIsland( b3World* world, int islandId ); diff --git a/vendor/box3d/src/src/joint.c b/vendor/box3d/src/src/joint.c new file mode 100644 index 000000000..5875f55f9 --- /dev/null +++ b/vendor/box3d/src/src/joint.c @@ -0,0 +1,1748 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +#include "joint.h" + +#include "body.h" +#include "contact.h" +#include "core.h" +#include "island.h" +#include "physics_world.h" +#include "recording.h" +#include "shape.h" +#include "solver.h" +#include "solver_set.h" + +// needed for dll export +#include "box3d/box3d.h" + +#include +#include +#include + +static b3JointDef b3DefaultJointDef( void ) +{ + b3JointDef def = { 0 }; + def.localFrameA.q = b3Quat_identity; + def.localFrameB.q = b3Quat_identity; + def.forceThreshold = FLT_MAX; + def.torqueThreshold = FLT_MAX; + def.constraintHertz = 60.0f; + def.constraintDampingRatio = 2.0f; + def.drawScale = b3GetLengthUnitsPerMeter(); + def.internalValue = B3_SECRET_COOKIE; + return def; +} + +b3ParallelJointDef b3DefaultParallelJointDef( void ) +{ + b3ParallelJointDef def = { 0 }; + def.base = b3DefaultJointDef(); + def.hertz = 1.0f; + def.dampingRatio = 1.0f; + def.maxTorque = FLT_MAX; + return def; +} + +b3DistanceJointDef b3DefaultDistanceJointDef( void ) +{ + b3DistanceJointDef def = { 0 }; + def.base = b3DefaultJointDef(); + def.lowerSpringForce = -FLT_MAX; + def.upperSpringForce = FLT_MAX; + def.length = 1.0f; + def.maxLength = B3_HUGE; + return def; +} + +b3MotorJointDef b3DefaultMotorJointDef( void ) +{ + b3MotorJointDef def = { 0 }; + def.base = b3DefaultJointDef(); + return def; +} + +b3FilterJointDef b3DefaultFilterJointDef( void ) +{ + b3FilterJointDef def = { 0 }; + def.base = b3DefaultJointDef(); + return def; +} + +b3PrismaticJointDef b3DefaultPrismaticJointDef( void ) +{ + b3PrismaticJointDef def = { 0 }; + def.base = b3DefaultJointDef(); + return def; +} + +b3RevoluteJointDef b3DefaultRevoluteJointDef( void ) +{ + b3RevoluteJointDef def = { 0 }; + def.base = b3DefaultJointDef(); + return def; +} + +b3SphericalJointDef b3DefaultSphericalJointDef( void ) +{ + b3SphericalJointDef def = { 0 }; + def.base = b3DefaultJointDef(); + def.targetRotation = b3Quat_identity; + return def; +} + +b3WeldJointDef b3DefaultWeldJointDef( void ) +{ + b3WeldJointDef def = { 0 }; + def.base = b3DefaultJointDef(); + return def; +} + +b3WheelJointDef b3DefaultWheelJointDef( void ) +{ + b3WheelJointDef def = { 0 }; + def.base = b3DefaultJointDef(); + def.enableSuspensionSpring = true; + def.suspensionHertz = 1.0f; + def.suspensionDampingRatio = 0.7f; + def.steeringHertz = 1.0f; + def.steeringDampingRatio = 0.7f; + return def; +} + +b3ExplosionDef b3DefaultExplosionDef( void ) +{ + b3ExplosionDef def = { 0 }; + def.maskBits = B3_DEFAULT_MASK_BITS; + return def; +} + +b3Joint* b3GetJointFullId( b3World* world, b3JointId jointId ) +{ + int id = jointId.index1 - 1; + b3Joint* joint = b3Array_Get( world->joints, id ); + B3_ASSERT( joint->jointId == id && joint->generation == jointId.generation ); + return joint; +} + +b3JointSim* b3GetJointSim( b3World* world, b3Joint* joint ) +{ + if ( joint->setIndex == b3_awakeSet ) + { + B3_ASSERT( 0 <= joint->colorIndex && joint->colorIndex < B3_GRAPH_COLOR_COUNT ); + b3GraphColor* color = world->constraintGraph.colors + joint->colorIndex; + return b3Array_Get( color->jointSims, joint->localIndex ); + } + + b3SolverSet* set = b3Array_Get( world->solverSets, joint->setIndex ); + return b3Array_Get( set->jointSims, joint->localIndex ); +} + +b3JointSim* b3GetJointSimCheckType( b3JointId jointId, b3JointType type ) +{ + B3_UNUSED( type ); + b3World* world = b3GetWorld( jointId.world0 ); + b3Joint* joint = b3GetJointFullId( world, jointId ); + B3_ASSERT( joint->type == type ); + b3JointSim* jointSim = b3GetJointSim( world, joint ); + B3_ASSERT( jointSim->type == type ); + return jointSim; +} + +typedef struct b3JointPair +{ + b3Joint* joint; + b3JointSim* jointSim; +} b3JointPair; + +static b3JointPair b3CreateJoint( b3World* world, const b3JointDef* def, b3JointType type ) +{ + B3_ASSERT( b3IsValidTransform( def->localFrameA ) ); + B3_ASSERT( b3IsValidTransform( def->localFrameB ) ); + + b3Body* bodyA = b3GetBodyFullId( world, def->bodyIdA ); + b3Body* bodyB = b3GetBodyFullId( world, def->bodyIdB ); + + int bodyIdA = bodyA->id; + int bodyIdB = bodyB->id; + int maxSetIndex = b3MaxInt( bodyA->setIndex, bodyB->setIndex ); + + // Create joint id and joint + int jointId = b3AllocId( &world->jointIdPool ); + if ( jointId == world->joints.count ) + { + b3Array_Push( world->joints, (b3Joint){ 0 } ); + } + + b3Joint* joint = b3Array_Get( world->joints, jointId ); + joint->jointId = jointId; + joint->userData = def->userData; + joint->generation += 1; + joint->setIndex = B3_NULL_INDEX; + joint->colorIndex = B3_NULL_INDEX; + joint->localIndex = B3_NULL_INDEX; + joint->islandId = B3_NULL_INDEX; + joint->islandIndex = B3_NULL_INDEX; + joint->drawScale = def->drawScale; + joint->type = type; + joint->collideConnected = def->collideConnected; + + // Doubly linked list on bodyA + joint->edges[0].bodyId = bodyIdA; + joint->edges[0].prevKey = B3_NULL_INDEX; + joint->edges[0].nextKey = bodyA->headJointKey; + + int keyA = ( jointId << 1 ) | 0; + if ( bodyA->headJointKey != B3_NULL_INDEX ) + { + b3Joint* jointA = b3Array_Get( world->joints, bodyA->headJointKey >> 1 ); + b3JointEdge* edgeA = jointA->edges + ( bodyA->headJointKey & 1 ); + edgeA->prevKey = keyA; + } + bodyA->headJointKey = keyA; + bodyA->jointCount += 1; + + // Doubly linked list on bodyB + joint->edges[1].bodyId = bodyIdB; + joint->edges[1].prevKey = B3_NULL_INDEX; + joint->edges[1].nextKey = bodyB->headJointKey; + + int keyB = ( jointId << 1 ) | 1; + if ( bodyB->headJointKey != B3_NULL_INDEX ) + { + b3Joint* jointB = b3Array_Get( world->joints, bodyB->headJointKey >> 1 ); + b3JointEdge* edgeB = jointB->edges + ( bodyB->headJointKey & 1 ); + edgeB->prevKey = keyB; + } + bodyB->headJointKey = keyB; + bodyB->jointCount += 1; + + b3JointSim* jointSim; + + if ( bodyA->setIndex == b3_disabledSet || bodyB->setIndex == b3_disabledSet ) + { + // if either body is disabled, create in disabled set + b3SolverSet* set = b3Array_Get( world->solverSets, b3_disabledSet ); + joint->setIndex = b3_disabledSet; + joint->localIndex = set->jointSims.count; + + jointSim = b3Array_Emplace( set->jointSims ); + memset( jointSim, 0, sizeof( b3JointSim ) ); + + jointSim->jointId = jointId; + jointSim->bodyIdA = bodyIdA; + jointSim->bodyIdB = bodyIdB; + } + else if ( bodyA->type != b3_dynamicBody && bodyB->type != b3_dynamicBody ) + { + // joint is not attached to a dynamic body + b3SolverSet* set = b3Array_Get( world->solverSets, b3_staticSet ); + joint->setIndex = b3_staticSet; + joint->localIndex = set->jointSims.count; + + jointSim = b3Array_Emplace( set->jointSims ); + memset( jointSim, 0, sizeof( b3JointSim ) ); + + jointSim->jointId = jointId; + jointSim->bodyIdA = bodyIdA; + jointSim->bodyIdB = bodyIdB; + } + else if ( bodyA->setIndex == b3_awakeSet || bodyB->setIndex == b3_awakeSet ) + { + // if either body is sleeping, wake it + if ( maxSetIndex >= b3_firstSleepingSet ) + { + b3WakeSolverSet( world, maxSetIndex ); + } + + joint->setIndex = b3_awakeSet; + + jointSim = b3CreateJointInGraph( world, joint ); + jointSim->jointId = jointId; + jointSim->bodyIdA = bodyIdA; + jointSim->bodyIdB = bodyIdB; + } + else + { + // joint connected between sleeping and/or static bodies + B3_ASSERT( bodyA->setIndex >= b3_firstSleepingSet || bodyB->setIndex >= b3_firstSleepingSet ); + B3_ASSERT( bodyA->setIndex != b3_staticSet || bodyB->setIndex != b3_staticSet ); + + // joint should go into the sleeping set (not static set) + int setIndex = maxSetIndex; + + b3SolverSet* set = b3Array_Get( world->solverSets, setIndex ); + joint->setIndex = setIndex; + joint->localIndex = set->jointSims.count; + + jointSim = b3Array_Emplace( set->jointSims ); + memset( jointSim, 0, sizeof( b3JointSim ) ); + + jointSim->jointId = jointId; + jointSim->bodyIdA = bodyIdA; + jointSim->bodyIdB = bodyIdB; + + if ( bodyA->setIndex != bodyB->setIndex && bodyA->setIndex >= b3_firstSleepingSet && + bodyB->setIndex >= b3_firstSleepingSet ) + { + // merge sleeping sets + b3MergeSolverSets( world, bodyA->setIndex, bodyB->setIndex ); + B3_ASSERT( bodyA->setIndex == bodyB->setIndex ); + + // fix potentially invalid set index + setIndex = bodyA->setIndex; + + b3SolverSet* mergedSet = b3Array_Get( world->solverSets, setIndex ); + + // Careful! The joint sim pointer was orphaned by the set merge. + jointSim = b3Array_Get( mergedSet->jointSims, joint->localIndex ); + } + + B3_ASSERT( joint->setIndex == setIndex ); + } + + jointSim->localFrameA = def->localFrameA; + jointSim->localFrameB = def->localFrameB; + jointSim->type = type; + jointSim->constraintHertz = def->constraintHertz; + jointSim->constraintDampingRatio = def->constraintDampingRatio; + jointSim->constraintSoftness = (b3Softness){ + .biasRate = 0.0f, + .massScale = 1.0f, + .impulseScale = 0.0f, + }; + + B3_ASSERT( b3IsValidFloat( def->forceThreshold ) && def->forceThreshold >= 0.0f ); + B3_ASSERT( b3IsValidFloat( def->torqueThreshold ) && def->torqueThreshold >= 0.0f ); + + jointSim->forceThreshold = def->forceThreshold; + jointSim->torqueThreshold = def->torqueThreshold; + + B3_ASSERT( jointSim->jointId == jointId ); + B3_ASSERT( jointSim->bodyIdA == bodyIdA ); + B3_ASSERT( jointSim->bodyIdB == bodyIdB ); + + if ( joint->setIndex > b3_disabledSet ) + { + // Add edge to island graph + b3LinkJoint( world, joint ); + } + + b3ValidateSolverSets( world ); + + return (b3JointPair){ joint, jointSim }; +} + +static void b3DestroyContactsBetweenBodies( b3World* world, b3Body* bodyA, b3Body* bodyB ) +{ + int contactKey; + int otherBodyId; + + // use the smaller of the two contact lists + if ( bodyA->contactCount < bodyB->contactCount ) + { + contactKey = bodyA->headContactKey; + otherBodyId = bodyB->id; + } + else + { + contactKey = bodyB->headContactKey; + otherBodyId = bodyA->id; + } + + // no need to wake bodies when a joint removes collision between them + bool wakeBodies = false; + + // destroy the contacts + while ( contactKey != B3_NULL_INDEX ) + { + int contactId = contactKey >> 1; + int edgeIndex = contactKey & 1; + + b3Contact* contact = b3Array_Get( world->contacts, contactId ); + contactKey = contact->edges[edgeIndex].nextKey; + + int otherEdgeIndex = edgeIndex ^ 1; + if ( contact->edges[otherEdgeIndex].bodyId == otherBodyId ) + { + // Careful, this removes the contact from the current doubly linked list + b3DestroyContact( world, contact, wakeBodies ); + } + } + + b3ValidateSolverSets( world ); +} + +void b3Joint_SetConstraintTuning( b3JointId jointId, float hertz, float dampingRatio ) +{ + B3_ASSERT( b3IsValidFloat( hertz ) && hertz >= 0.0f ); + B3_ASSERT( b3IsValidFloat( dampingRatio ) && dampingRatio >= 0.0f ); + + b3World* world = b3GetWorld( jointId.world0 ); + B3_REC( world, JointSetConstraintTuning, jointId, hertz, dampingRatio ); + b3Joint* joint = b3GetJointFullId( world, jointId ); + b3JointSim* base = b3GetJointSim( world, joint ); + base->constraintHertz = hertz; + base->constraintDampingRatio = dampingRatio; +} + +void b3Joint_GetConstraintTuning( b3JointId jointId, float* hertz, float* dampingRatio ) +{ + b3World* world = b3GetWorld( jointId.world0 ); + b3Joint* joint = b3GetJointFullId( world, jointId ); + b3JointSim* base = b3GetJointSim( world, joint ); + *hertz = base->constraintHertz; + *dampingRatio = base->constraintDampingRatio; +} + +void b3Joint_SetForceThreshold( b3JointId jointId, float threshold ) +{ + B3_ASSERT( b3IsValidFloat( threshold ) && threshold >= 0.0f ); + + b3World* world = b3GetWorld( jointId.world0 ); + B3_REC( world, JointSetForceThreshold, jointId, threshold ); + b3Joint* joint = b3GetJointFullId( world, jointId ); + b3JointSim* base = b3GetJointSim( world, joint ); + base->forceThreshold = threshold; +} + +float b3Joint_GetForceThreshold( b3JointId jointId ) +{ + b3World* world = b3GetWorld( jointId.world0 ); + b3Joint* joint = b3GetJointFullId( world, jointId ); + b3JointSim* base = b3GetJointSim( world, joint ); + return base->forceThreshold; +} + +void b3Joint_SetTorqueThreshold( b3JointId jointId, float threshold ) +{ + B3_ASSERT( b3IsValidFloat( threshold ) && threshold >= 0.0f ); + + b3World* world = b3GetWorld( jointId.world0 ); + B3_REC( world, JointSetTorqueThreshold, jointId, threshold ); + b3Joint* joint = b3GetJointFullId( world, jointId ); + b3JointSim* base = b3GetJointSim( world, joint ); + base->torqueThreshold = threshold; +} + +float b3Joint_GetTorqueThreshold( b3JointId jointId ) +{ + b3World* world = b3GetWorld( jointId.world0 ); + b3Joint* joint = b3GetJointFullId( world, jointId ); + b3JointSim* base = b3GetJointSim( world, joint ); + return base->torqueThreshold; +} + +b3JointId b3CreateDistanceJoint( b3WorldId worldId, const b3DistanceJointDef* def ) +{ + B3_CHECK_JOINT_DEF( def ); + b3World* world = b3GetUnlockedWorldFromId( worldId ); + if (world == NULL) + { + return (b3JointId){ 0 }; + } + + B3_ASSERT( b3IsValidFloat( def->length ) && def->length > 0.0f ); + B3_ASSERT( def->lowerSpringForce <= def->upperSpringForce ); + + b3JointPair pair = b3CreateJoint( world, &def->base, b3_distanceJoint ); + + b3JointSim* joint = pair.jointSim; + + joint->distanceJoint = (b3DistanceJoint){ 0 }; + joint->distanceJoint.length = b3MaxFloat( def->length, B3_LINEAR_SLOP ); + joint->distanceJoint.hertz = def->hertz; + joint->distanceJoint.dampingRatio = def->dampingRatio; + joint->distanceJoint.lowerSpringForce = def->lowerSpringForce; + joint->distanceJoint.upperSpringForce = def->upperSpringForce; + joint->distanceJoint.minLength = b3MaxFloat( def->minLength, B3_LINEAR_SLOP ); + joint->distanceJoint.maxLength = b3MaxFloat( def->minLength, def->maxLength ); + joint->distanceJoint.maxMotorForce = def->maxMotorForce; + joint->distanceJoint.motorSpeed = def->motorSpeed; + joint->distanceJoint.enableSpring = def->enableSpring; + joint->distanceJoint.enableLimit = def->enableLimit; + joint->distanceJoint.enableMotor = def->enableMotor; + joint->distanceJoint.impulse = 0.0f; + joint->distanceJoint.lowerImpulse = 0.0f; + joint->distanceJoint.upperImpulse = 0.0f; + joint->distanceJoint.motorImpulse = 0.0f; + + b3JointId jointId = { joint->jointId + 1, world->worldId, pair.joint->generation }; + B3_REC_CREATE( world, CreateDistanceJoint, jointId, worldId, *def ); + return jointId; +} + +b3JointId b3CreateMotorJoint( b3WorldId worldId, const b3MotorJointDef* def ) +{ + B3_CHECK_JOINT_DEF( def ); + b3World* world = b3GetUnlockedWorldFromId( worldId ); + if ( world == NULL ) + { + return (b3JointId){ 0 }; + } + + b3JointPair pair = b3CreateJoint( world, &def->base, b3_motorJoint ); + b3JointSim* joint = pair.jointSim; + + joint->motorJoint = (b3MotorJoint){ 0 }; + joint->motorJoint.linearVelocity = def->linearVelocity; + joint->motorJoint.maxVelocityForce = def->maxVelocityForce; + joint->motorJoint.angularVelocity = def->angularVelocity; + joint->motorJoint.maxVelocityTorque = def->maxVelocityTorque; + joint->motorJoint.linearHertz = def->linearHertz; + joint->motorJoint.linearDampingRatio = def->linearDampingRatio; + joint->motorJoint.maxSpringForce = def->maxSpringForce; + joint->motorJoint.angularHertz = def->angularHertz; + joint->motorJoint.angularDampingRatio = def->angularDampingRatio; + joint->motorJoint.maxSpringTorque = def->maxSpringTorque; + + b3JointId jointId = { joint->jointId + 1, world->worldId, pair.joint->generation }; + B3_REC_CREATE( world, CreateMotorJoint, jointId, worldId, *def ); + return jointId; +} + +b3JointId b3CreateFilterJoint( b3WorldId worldId, const b3FilterJointDef* def ) +{ + B3_CHECK_JOINT_DEF( def ); + b3World* world = b3GetUnlockedWorldFromId( worldId ); + if (world == NULL) + { + return (b3JointId){ 0 }; + } + + b3JointPair pair = b3CreateJoint( world, &def->base, b3_filterJoint ); + + b3JointSim* joint = pair.jointSim; + + b3JointId jointId = { joint->jointId + 1, world->worldId, pair.joint->generation }; + B3_REC_CREATE( world, CreateFilterJoint, jointId, worldId, *def ); + return jointId; +} + +b3JointId b3CreateParallelJoint( b3WorldId worldId, const b3ParallelJointDef* def ) +{ + B3_CHECK_JOINT_DEF( def ); + b3World* world = b3GetUnlockedWorldFromId( worldId ); + if (world == NULL) + { + return (b3JointId){ 0 }; + } + + B3_ASSERT( b3IsValidFloat( def->hertz ) && def->hertz >= 0.0f ); + B3_ASSERT( b3IsValidFloat( def->dampingRatio ) && def->dampingRatio >= 0.0f ); + B3_ASSERT( b3IsValidFloat( def->maxTorque ) && def->maxTorque >= 0.0f ); + + b3JointPair pair = b3CreateJoint( world, &def->base, b3_parallelJoint ); + + b3JointSim* joint = pair.jointSim; + + joint->parallelJoint = (b3ParallelJoint){ 0 }; + joint->parallelJoint.hertz = def->hertz; + joint->parallelJoint.dampingRatio = def->dampingRatio; + joint->parallelJoint.maxTorque = def->maxTorque; + + b3JointId jointId = { joint->jointId + 1, world->worldId, pair.joint->generation }; + B3_REC_CREATE( world, CreateParallelJoint, jointId, worldId, *def ); + return jointId; +} + +b3JointId b3CreatePrismaticJoint( b3WorldId worldId, const b3PrismaticJointDef* def ) +{ + B3_CHECK_JOINT_DEF( def ); + B3_ASSERT( def->lowerTranslation <= def->upperTranslation ); + + b3World* world = b3GetUnlockedWorldFromId( worldId ); + if (world == NULL) + { + return (b3JointId){ 0 }; + } + + b3JointPair pair = b3CreateJoint( world, &def->base, b3_prismaticJoint ); + + b3JointSim* joint = pair.jointSim; + + joint->prismaticJoint = (b3PrismaticJoint){ 0 }; + joint->prismaticJoint.hertz = def->hertz; + joint->prismaticJoint.dampingRatio = def->dampingRatio; + joint->prismaticJoint.targetTranslation = def->targetTranslation; + joint->prismaticJoint.lowerTranslation = def->lowerTranslation; + joint->prismaticJoint.upperTranslation = def->upperTranslation; + joint->prismaticJoint.maxMotorForce = def->maxMotorForce; + joint->prismaticJoint.motorSpeed = def->motorSpeed; + joint->prismaticJoint.enableSpring = def->enableSpring; + joint->prismaticJoint.enableLimit = def->enableLimit; + joint->prismaticJoint.enableMotor = def->enableMotor; + + b3JointId jointId = { joint->jointId + 1, world->worldId, pair.joint->generation }; + B3_REC_CREATE( world, CreatePrismaticJoint, jointId, worldId, *def ); + return jointId; +} + +b3JointId b3CreateRevoluteJoint( b3WorldId worldId, const b3RevoluteJointDef* def ) +{ + B3_CHECK_JOINT_DEF( def ); + + b3World* world = b3GetUnlockedWorldFromId( worldId ); + if (world == NULL) + { + return (b3JointId){ 0 }; + } + + b3JointPair pair = b3CreateJoint( world, &def->base, b3_revoluteJoint ); + + b3JointSim* joint = pair.jointSim; + + joint->revoluteJoint = (b3RevoluteJoint){ 0 }; + joint->revoluteJoint.hertz = def->hertz; + joint->revoluteJoint.dampingRatio = def->dampingRatio; + joint->revoluteJoint.targetAngle = b3ClampFloat( def->targetAngle, -B3_PI, B3_PI ); + + float lowerAngle = b3MinFloat( def->lowerAngle, def->upperAngle ); + float upperAngle = b3MaxFloat( def->lowerAngle, def->upperAngle ); + joint->revoluteJoint.lowerAngle = b3ClampFloat( lowerAngle, -0.99f * B3_PI, 0.99f * B3_PI ); + joint->revoluteJoint.upperAngle = b3ClampFloat( upperAngle, -0.99f * B3_PI, 0.99f * B3_PI ); + + joint->revoluteJoint.maxMotorTorque = def->maxMotorTorque; + joint->revoluteJoint.motorSpeed = def->motorSpeed; + joint->revoluteJoint.enableSpring = def->enableSpring; + joint->revoluteJoint.enableLimit = def->enableLimit; + joint->revoluteJoint.enableMotor = def->enableMotor; + + b3JointId jointId = { joint->jointId + 1, world->worldId, pair.joint->generation }; + B3_REC_CREATE( world, CreateRevoluteJoint, jointId, worldId, *def ); + return jointId; +} + +b3JointId b3CreateSphericalJoint( b3WorldId worldId, const b3SphericalJointDef* def ) +{ + B3_CHECK_JOINT_DEF( def ); + B3_ASSERT( 0.0f <= def->coneAngle && def->coneAngle <= 0.99f * B3_PI ); + B3_ASSERT( b3IsValidQuat( def->targetRotation ) ); + + b3World* world = b3GetUnlockedWorldFromId( worldId ); + if (world == NULL) + { + return (b3JointId){ 0 }; + } + + b3JointPair pair = b3CreateJoint( world, &def->base, b3_sphericalJoint ); + + b3JointSim* joint = pair.jointSim; + + joint->sphericalJoint = (b3SphericalJoint){ 0 }; + joint->sphericalJoint.hertz = def->hertz; + joint->sphericalJoint.dampingRatio = def->dampingRatio; + joint->sphericalJoint.targetRotation = def->targetRotation; + joint->sphericalJoint.coneAngle = b3ClampFloat( def->coneAngle, 0.0f, 0.5f * B3_PI ); + + float lowerAngle = b3MinFloat( def->lowerTwistAngle, def->upperTwistAngle ); + float upperAngle = b3MaxFloat( def->lowerTwistAngle, def->upperTwistAngle ); + joint->sphericalJoint.lowerTwistAngle = b3ClampFloat( lowerAngle, -0.99f * B3_PI, 0.99f * B3_PI ); + joint->sphericalJoint.upperTwistAngle = b3ClampFloat( upperAngle, -0.99f * B3_PI, 0.99f * B3_PI ); + + joint->sphericalJoint.maxMotorTorque = def->maxMotorTorque; + joint->sphericalJoint.motorVelocity = def->motorVelocity; + joint->sphericalJoint.enableSpring = def->enableSpring; + joint->sphericalJoint.enableConeLimit = def->enableConeLimit; + joint->sphericalJoint.enableTwistLimit = def->enableTwistLimit; + joint->sphericalJoint.enableMotor = def->enableMotor; + + b3JointId jointId = { joint->jointId + 1, world->worldId, pair.joint->generation }; + B3_REC_CREATE( world, CreateSphericalJoint, jointId, worldId, *def ); + return jointId; +} + +b3JointId b3CreateWeldJoint( b3WorldId worldId, const b3WeldJointDef* def ) +{ + B3_CHECK_JOINT_DEF( def ); + B3_ASSERT( 0.0f <= def->angularHertz ); + B3_ASSERT( 0.0f <= def->angularDampingRatio ); + B3_ASSERT( 0.0f <= def->linearHertz ); + B3_ASSERT( 0.0f <= def->linearDampingRatio ); + + b3World* world = b3GetUnlockedWorldFromId( worldId ); + if (world == NULL) + { + return (b3JointId){ 0 }; + } + + b3JointPair pair = b3CreateJoint( world, &def->base, b3_weldJoint ); + + b3JointSim* joint = pair.jointSim; + + joint->weldJoint = (b3WeldJoint){ 0 }; + joint->weldJoint.linearHertz = def->linearHertz; + joint->weldJoint.linearDampingRatio = def->linearDampingRatio; + joint->weldJoint.angularHertz = def->angularHertz; + joint->weldJoint.angularDampingRatio = def->angularDampingRatio; + + b3JointId jointId = { joint->jointId + 1, world->worldId, pair.joint->generation }; + B3_REC_CREATE( world, CreateWeldJoint, jointId, worldId, *def ); + return jointId; +} + +b3JointId b3CreateWheelJoint( b3WorldId worldId, const b3WheelJointDef* def ) +{ + B3_CHECK_JOINT_DEF( def ); + B3_ASSERT( def->lowerSuspensionLimit <= def->upperSuspensionLimit ); + + b3World* world = b3GetUnlockedWorldFromId( worldId ); + if (world == NULL) + { + return (b3JointId){ 0 }; + } + + b3JointPair pair = b3CreateJoint( world, &def->base, b3_wheelJoint ); + + b3JointSim* joint = pair.jointSim; + + joint->wheelJoint = (b3WheelJoint){ 0 }; + joint->wheelJoint.enableSuspensionSpring = def->enableSuspensionSpring; + joint->wheelJoint.suspensionHertz = def->suspensionHertz; + joint->wheelJoint.suspensionDampingRatio = def->suspensionDampingRatio; + joint->wheelJoint.enableSuspensionLimit = def->enableSuspensionLimit; + joint->wheelJoint.lowerSuspensionLimit = def->lowerSuspensionLimit; + joint->wheelJoint.upperSuspensionLimit = def->upperSuspensionLimit; + joint->wheelJoint.enableSpinMotor = def->enableSpinMotor; + joint->wheelJoint.maxSpinTorque = def->maxSpinTorque; + joint->wheelJoint.spinSpeed = def->spinSpeed; + + joint->wheelJoint.enableSteering = def->enableSteering; + joint->wheelJoint.steeringHertz = def->steeringHertz; + joint->wheelJoint.steeringDampingRatio = def->steeringDampingRatio; + joint->wheelJoint.targetSteeringAngle = def->targetSteeringAngle; + joint->wheelJoint.maxSteeringTorque = def->maxSteeringTorque; + joint->wheelJoint.enableSteeringLimit = def->enableSteeringLimit; + joint->wheelJoint.lowerSteeringLimit = def->lowerSteeringLimit; + joint->wheelJoint.upperSteeringLimit = def->upperSteeringLimit; + + b3JointId jointId = { joint->jointId + 1, world->worldId, pair.joint->generation }; + B3_REC_CREATE( world, CreateWheelJoint, jointId, worldId, *def ); + return jointId; +} + +void b3DestroyJointInternal( b3World* world, b3Joint* joint, bool wakeBodies ) +{ + int jointId = joint->jointId; + + b3JointEdge* edgeA = joint->edges + 0; + b3JointEdge* edgeB = joint->edges + 1; + + int idA = edgeA->bodyId; + int idB = edgeB->bodyId; + b3Body* bodyA = b3Array_Get( world->bodies, idA ); + b3Body* bodyB = b3Array_Get( world->bodies, idB ); + + // Remove from body A + if ( edgeA->prevKey != B3_NULL_INDEX ) + { + b3Joint* prevJoint = b3Array_Get( world->joints, edgeA->prevKey >> 1 ); + b3JointEdge* prevEdge = prevJoint->edges + ( edgeA->prevKey & 1 ); + prevEdge->nextKey = edgeA->nextKey; + } + + if ( edgeA->nextKey != B3_NULL_INDEX ) + { + b3Joint* nextJoint = b3Array_Get( world->joints, edgeA->nextKey >> 1 ); + b3JointEdge* nextEdge = nextJoint->edges + ( edgeA->nextKey & 1 ); + nextEdge->prevKey = edgeA->prevKey; + } + + int edgeKeyA = ( jointId << 1 ) | 0; + if ( bodyA->headJointKey == edgeKeyA ) + { + bodyA->headJointKey = edgeA->nextKey; + } + + bodyA->jointCount -= 1; + + // Remove from body B + if ( edgeB->prevKey != B3_NULL_INDEX ) + { + b3Joint* prevJoint = b3Array_Get( world->joints, edgeB->prevKey >> 1 ); + b3JointEdge* prevEdge = prevJoint->edges + ( edgeB->prevKey & 1 ); + prevEdge->nextKey = edgeB->nextKey; + } + + if ( edgeB->nextKey != B3_NULL_INDEX ) + { + b3Joint* nextJoint = b3Array_Get( world->joints, edgeB->nextKey >> 1 ); + b3JointEdge* nextEdge = nextJoint->edges + ( edgeB->nextKey & 1 ); + nextEdge->prevKey = edgeB->prevKey; + } + + int edgeKeyB = ( jointId << 1 ) | 1; + if ( bodyB->headJointKey == edgeKeyB ) + { + bodyB->headJointKey = edgeB->nextKey; + } + + bodyB->jointCount -= 1; + + if ( joint->islandId != B3_NULL_INDEX ) + { + B3_ASSERT( joint->setIndex > b3_disabledSet ); + b3UnlinkJoint( world, joint ); + } + else + { + B3_ASSERT( joint->setIndex <= b3_disabledSet ); + } + + // Remove joint from solver set that owns it + int setIndex = joint->setIndex; + int localIndex = joint->localIndex; + + if ( setIndex == b3_awakeSet ) + { + b3RemoveJointFromGraph( world, joint->edges[0].bodyId, joint->edges[1].bodyId, joint->colorIndex, localIndex ); + } + else + { + b3SolverSet* set = b3Array_Get( world->solverSets, setIndex ); + int movedIndex = b3Array_RemoveSwap( set->jointSims, localIndex ); + if ( movedIndex != B3_NULL_INDEX ) + { + // Fix moved joint + b3JointSim* movedJointSim = set->jointSims.data + localIndex; + int movedId = movedJointSim->jointId; + b3Joint* movedJoint = b3Array_Get( world->joints, movedId ); + B3_ASSERT( movedJoint->localIndex == movedIndex ); + movedJoint->localIndex = localIndex; + } + } + + // Free joint and id (preserve joint revision) + joint->setIndex = B3_NULL_INDEX; + joint->localIndex = B3_NULL_INDEX; + joint->colorIndex = B3_NULL_INDEX; + joint->jointId = B3_NULL_INDEX; + b3FreeId( &world->jointIdPool, jointId ); + + if ( wakeBodies ) + { + b3WakeBody( world, bodyA ); + b3WakeBody( world, bodyB ); + } + + b3ValidateSolverSets( world ); +} + +void b3DestroyJoint( b3JointId jointId, bool wakeAttached ) +{ + b3World* world = b3GetWorld( jointId.world0 ); + if ( world == NULL ) + { + return; + } + + B3_REC( world, DestroyJoint, jointId, wakeAttached ); + + b3Joint* joint = b3GetJointFullId( world, jointId ); + + b3DestroyJointInternal( world, joint, wakeAttached ); +} + +b3JointType b3Joint_GetType( b3JointId jointId ) +{ + b3World* world = b3GetWorld( jointId.world0 ); + b3Joint* joint = b3GetJointFullId( world, jointId ); + return joint->type; +} + +b3BodyId b3Joint_GetBodyA( b3JointId jointId ) +{ + b3World* world = b3GetWorld( jointId.world0 ); + b3Joint* joint = b3GetJointFullId( world, jointId ); + return b3MakeBodyId( world, joint->edges[0].bodyId ); +} + +b3BodyId b3Joint_GetBodyB( b3JointId jointId ) +{ + b3World* world = b3GetWorld( jointId.world0 ); + b3Joint* joint = b3GetJointFullId( world, jointId ); + return b3MakeBodyId( world, joint->edges[1].bodyId ); +} + +b3WorldId b3Joint_GetWorld( b3JointId jointId ) +{ + b3World* world = b3GetWorld( jointId.world0 ); + return (b3WorldId){ (uint16_t)( jointId.world0 + 1 ), world->generation }; +} + +void b3Joint_SetLocalFrameA( b3JointId jointId, b3Transform localFrame ) +{ + B3_ASSERT( b3IsValidTransform( localFrame ) ); + + b3World* world = b3GetWorld( jointId.world0 ); + B3_REC( world, JointSetLocalFrameA, jointId, localFrame ); + b3Joint* joint = b3GetJointFullId( world, jointId ); + b3JointSim* jointSim = b3GetJointSim( world, joint ); + jointSim->localFrameA = localFrame; +} + +b3Transform b3Joint_GetLocalFrameA( b3JointId jointId ) +{ + b3World* world = b3GetWorld( jointId.world0 ); + b3Joint* joint = b3GetJointFullId( world, jointId ); + b3JointSim* jointSim = b3GetJointSim( world, joint ); + return jointSim->localFrameA; +} + +void b3Joint_SetLocalFrameB( b3JointId jointId, b3Transform localFrame ) +{ + B3_ASSERT( b3IsValidTransform( localFrame ) ); + + b3World* world = b3GetWorld( jointId.world0 ); + B3_REC( world, JointSetLocalFrameB, jointId, localFrame ); + b3Joint* joint = b3GetJointFullId( world, jointId ); + b3JointSim* jointSim = b3GetJointSim( world, joint ); + jointSim->localFrameB = localFrame; +} + +b3Transform b3Joint_GetLocalFrameB( b3JointId jointId ) +{ + b3World* world = b3GetWorld( jointId.world0 ); + b3Joint* joint = b3GetJointFullId( world, jointId ); + b3JointSim* jointSim = b3GetJointSim( world, joint ); + return jointSim->localFrameB; +} + +void b3Joint_SetCollideConnected( b3JointId jointId, bool shouldCollide ) +{ + b3World* world = b3GetUnlockedWorld( jointId.world0 ); + if ( world == NULL ) + { + return; + } + + B3_REC( world, JointSetCollideConnected, jointId, shouldCollide ); + + b3Joint* joint = b3GetJointFullId( world, jointId ); + if ( joint->collideConnected == shouldCollide ) + { + return; + } + + joint->collideConnected = shouldCollide; + + b3Body* bodyA = b3Array_Get( world->bodies, joint->edges[0].bodyId ); + b3Body* bodyB = b3Array_Get( world->bodies, joint->edges[1].bodyId ); + + if ( shouldCollide ) + { + // need to tell the broad-phase to look for new pairs for one of the + // two bodies. Pick the one with the fewest shapes. + int shapeCountA = bodyA->shapeCount; + int shapeCountB = bodyB->shapeCount; + + int shapeId = shapeCountA < shapeCountB ? bodyA->headShapeId : bodyB->headShapeId; + while ( shapeId != B3_NULL_INDEX ) + { + b3Shape* shape = b3Array_Get( world->shapes, shapeId ); + + if ( shape->proxyKey != B3_NULL_INDEX ) + { + b3BufferMove( &world->broadPhase, shape->proxyKey ); + } + + shapeId = shape->nextShapeId; + } + } + else + { + b3DestroyContactsBetweenBodies( world, bodyA, bodyB ); + } +} + +bool b3Joint_GetCollideConnected( b3JointId jointId ) +{ + b3World* world = b3GetWorld( jointId.world0 ); + b3Joint* joint = b3GetJointFullId( world, jointId ); + return joint->collideConnected; +} + +void b3Joint_SetUserData( b3JointId jointId, void* userData ) +{ + b3World* world = b3GetWorld( jointId.world0 ); + b3Joint* joint = b3GetJointFullId( world, jointId ); + joint->userData = userData; +} + +void* b3Joint_GetUserData( b3JointId jointId ) +{ + b3World* world = b3GetWorld( jointId.world0 ); + b3Joint* joint = b3GetJointFullId( world, jointId ); + return joint->userData; +} + +void b3Joint_WakeBodies( b3JointId jointId ) +{ + b3World* world = b3GetUnlockedWorld( jointId.world0 ); + if ( world == NULL ) + { + return; + } + + B3_REC( world, JointWakeBodies, jointId ); + + world->locked = true; + + b3Joint* joint = b3GetJointFullId( world, jointId ); + b3Body* bodyA = b3Array_Get( world->bodies, joint->edges[0].bodyId ); + b3Body* bodyB = b3Array_Get( world->bodies, joint->edges[1].bodyId ); + + b3WakeBody( world, bodyA ); + b3WakeBody( world, bodyB ); + + world->locked = false; +} + +void b3GetJointReaction( b3World* world, b3JointSim* sim, float invTimeStep, float* force, float* torque ) +{ + float linearImpulse = 0.0f; + float angularImpulse = 0.0f; + + switch ( sim->type ) + { + case b3_parallelJoint: + { + b3ParallelJoint* joint = &sim->parallelJoint; + b3Vec3 impulse = { + .x = joint->perpImpulse.x, + .y = joint->perpImpulse.y, + .z = 0.0f, + }; + angularImpulse = b3Length( impulse ); + } + break; + + case b3_distanceJoint: + { + b3DistanceJoint* joint = &sim->distanceJoint; + linearImpulse = b3AbsFloat( joint->impulse + joint->lowerImpulse - joint->upperImpulse + joint->motorImpulse ); + } + break; + + case b3_motorJoint: + { + b3MotorJoint* joint = &sim->motorJoint; + linearImpulse = b3Length( b3Add( joint->linearVelocityImpulse, joint->linearSpringImpulse ) ); + angularImpulse = b3Length( b3Add( joint->angularVelocityImpulse, joint->angularSpringImpulse ) ); + } + break; + + case b3_prismaticJoint: + { + b3PrismaticJoint* joint = &sim->prismaticJoint; + b3Vec3 impulse = { + .x = joint->motorImpulse + joint->lowerImpulse - joint->upperImpulse, + .y = joint->perpImpulse.x, + .z = joint->perpImpulse.y, + }; + linearImpulse = b3Length( impulse ); + angularImpulse = b3Length( joint->angularImpulse ); + } + break; + + case b3_revoluteJoint: + { + b3RevoluteJoint* joint = &sim->revoluteJoint; + linearImpulse = b3Length( joint->linearImpulse ); + b3Vec3 impulse = { + .x = joint->perpImpulse.x, + .y = joint->perpImpulse.y, + .z = joint->motorImpulse + joint->lowerImpulse - joint->upperImpulse, + }; + angularImpulse = b3Length( impulse ); + } + break; + + case b3_sphericalJoint: + { + // todo improve performance + b3SphericalJoint* joint = &sim->sphericalJoint; + linearImpulse = b3Length( joint->linearImpulse ); + + b3WorldTransform xfA = b3GetBodyTransform( world, sim->bodyIdA ); + b3WorldTransform xfB = b3GetBodyTransform( world, sim->bodyIdB ); + b3Quat qA = b3MulQuat( xfA.q, sim->localFrameA.q ); + b3Quat qB = b3MulQuat( xfB.q, sim->localFrameB.q ); + + // Cone axis is the z-axis of body A. + b3Vec3 coneAxis = b3RotateVector( qA, b3Vec3_axisZ ); + b3Vec3 twistAxis = b3RotateVector( qB, b3Vec3_axisZ ); + b3Vec3 swingAxis = b3Normalize( b3Cross( coneAxis, twistAxis ) ); + + b3Vec3 impulse = b3Add( joint->springImpulse, joint->motorImpulse ); + impulse = b3MulAdd( impulse, joint->lowerTwistImpulse - joint->upperTwistImpulse, twistAxis ); + impulse = b3MulAdd( impulse, joint->swingImpulse, swingAxis ); + + angularImpulse = b3Length( impulse ); + } + break; + + case b3_weldJoint: + { + b3WeldJoint* joint = &sim->weldJoint; + linearImpulse = b3Length( joint->linearImpulse ); + angularImpulse = b3Length( joint->angularImpulse ); + } + break; + + case b3_wheelJoint: + { + // todo probably wrong + b3WheelJoint* joint = &sim->wheelJoint; + b3Vec2 perpImpulse = joint->linearImpulse; + float axialImpulse = joint->suspensionSpringImpulse + joint->lowerSuspensionImpulse - joint->upperSuspensionImpulse; + linearImpulse = sqrtf( perpImpulse.x * perpImpulse.x + perpImpulse.y * perpImpulse.y + axialImpulse * axialImpulse ); + angularImpulse = b3AbsFloat( joint->spinImpulse ); + } + break; + + default: + break; + } + + *force = linearImpulse * invTimeStep; + *torque = angularImpulse * invTimeStep; +} + +static b3Vec3 b3GetJointConstraintForce( b3World* world, b3Joint* joint ) +{ + b3JointSim* base = b3GetJointSim( world, joint ); + + switch ( joint->type ) + { + case b3_parallelJoint: + return b3Vec3_zero; + + case b3_distanceJoint: + return b3GetDistanceJointForce( world, base ); + + case b3_filterJoint: + return b3Vec3_zero; + + case b3_motorJoint: + return b3GetMotorJointForce( world, base ); + + case b3_prismaticJoint: + return b3GetPrismaticJointForce( world, base ); + + case b3_revoluteJoint: + return b3GetRevoluteJointForce( world, base ); + + case b3_sphericalJoint: + return b3GetSphericalJointForce( world, base ); + + case b3_weldJoint: + return b3GetWeldJointForce( world, base ); + + case b3_wheelJoint: + return b3GetWheelJointForce( world, base ); + + default: + B3_ASSERT( false ); + return b3Vec3_zero; + } +} + +static b3Vec3 b3GetJointConstraintTorque( b3World* world, b3Joint* joint ) +{ + b3JointSim* base = b3GetJointSim( world, joint ); + + switch ( joint->type ) + { + case b3_parallelJoint: + return b3GetParallelJointTorque( world, base ); + + case b3_distanceJoint: + return b3Vec3_zero; + + case b3_filterJoint: + return b3Vec3_zero; + + case b3_motorJoint: + return b3GetMotorJointTorque( world, base ); + + case b3_prismaticJoint: + return b3GetPrismaticJointTorque( world, base ); + + case b3_revoluteJoint: + return b3GetRevoluteJointTorque( world, base ); + + case b3_sphericalJoint: + return b3GetSphericalJointTorque( world, base ); + + case b3_weldJoint: + return b3GetWeldJointTorque( world, base ); + + case b3_wheelJoint: + return b3GetWheelJointTorque( world, base ); + + default: + B3_ASSERT( false ); + return b3Vec3_zero; + } +} + +b3Vec3 b3Joint_GetConstraintForce( b3JointId jointId ) +{ + b3World* world = b3GetWorld( jointId.world0 ); + b3Joint* joint = b3GetJointFullId( world, jointId ); + return b3GetJointConstraintForce( world, joint ); +} + +b3Vec3 b3Joint_GetConstraintTorque( b3JointId jointId ) +{ + b3World* world = b3GetWorld( jointId.world0 ); + b3Joint* joint = b3GetJointFullId( world, jointId ); + return b3GetJointConstraintTorque( world, joint ); +} + +float b3Joint_GetLinearSeparation( b3JointId jointId ) +{ + b3World* world = b3GetWorld( jointId.world0 ); + b3Joint* joint = b3GetJointFullId( world, jointId ); + b3JointSim* base = b3GetJointSim( world, joint ); + + b3WorldTransform xfA = b3GetBodyTransform( world, joint->edges[0].bodyId ); + b3WorldTransform xfB = b3GetBodyTransform( world, joint->edges[1].bodyId ); + + b3Pos pA = b3TransformWorldPoint( xfA, base->localFrameA.p ); + b3Pos pB = b3TransformWorldPoint( xfB, base->localFrameB.p ); + b3Vec3 dp = b3SubPos( pB, pA ); + + switch ( joint->type ) + { + case b3_parallelJoint: + return 0.0f; + + case b3_distanceJoint: + { + b3DistanceJoint* distanceJoint = &base->distanceJoint; + float length = b3Length( dp ); + if ( distanceJoint->enableSpring ) + { + if ( distanceJoint->enableLimit ) + { + if ( length < distanceJoint->minLength ) + { + return distanceJoint->minLength - length; + } + + if ( length > distanceJoint->maxLength ) + { + return length - distanceJoint->maxLength; + } + + return 0.0f; + } + + return 0.0f; + } + + return b3AbsFloat( length - distanceJoint->length ); + } + + case b3_motorJoint: + return 0.0f; + + case b3_filterJoint: + return 0.0f; + + case b3_prismaticJoint: + { + b3PrismaticJoint* prismaticJoint = &base->prismaticJoint; + b3Vec3 axisA = b3RotateVector( xfA.q, b3Vec3_axisX ); + b3Vec3 perpA = b3Perp( axisA ); + float perpendicularSeparation = b3AbsFloat( b3Dot( perpA, dp ) ); + float limitSeparation = 0.0f; + + if ( prismaticJoint->enableLimit ) + { + float translation = b3Dot( axisA, dp ); + if ( translation < prismaticJoint->lowerTranslation ) + { + limitSeparation = prismaticJoint->lowerTranslation - translation; + } + + if ( prismaticJoint->upperTranslation < translation ) + { + limitSeparation = translation - prismaticJoint->upperTranslation; + } + } + + return sqrtf( perpendicularSeparation * perpendicularSeparation + limitSeparation * limitSeparation ); + } + + case b3_revoluteJoint: + return b3Length( dp ); + + case b3_sphericalJoint: + return b3Length( dp ); + + case b3_weldJoint: + { + b3WeldJoint* weldJoint = &base->weldJoint; + if ( weldJoint->linearHertz == 0.0f ) + { + return b3Length( dp ); + } + + return 0.0f; + } + + case b3_wheelJoint: + { + b3WheelJoint* wheelJoint = &base->wheelJoint; + b3Vec3 axisA = b3RotateVector( xfA.q, b3Vec3_axisX ); + b3Vec3 perpA = b3Perp( axisA ); + float perpendicularSeparation = b3AbsFloat( b3Dot( perpA, dp ) ); + float limitSeparation = 0.0f; + + if ( wheelJoint->enableSuspensionLimit ) + { + float translation = b3Dot( axisA, dp ); + if ( translation < wheelJoint->lowerSuspensionLimit ) + { + limitSeparation = wheelJoint->lowerSuspensionLimit - translation; + } + + if ( wheelJoint->upperSuspensionLimit < translation ) + { + limitSeparation = translation - wheelJoint->upperSuspensionLimit; + } + } + + return sqrtf( perpendicularSeparation * perpendicularSeparation + limitSeparation * limitSeparation ); + } + + default: + B3_ASSERT( false ); + return 0.0f; + } +} + +float b3Joint_GetAngularSeparation( b3JointId jointId ) +{ + b3World* world = b3GetWorld( jointId.world0 ); + b3Joint* joint = b3GetJointFullId( world, jointId ); + b3JointSim* base = b3GetJointSim( world, joint ); + + b3WorldTransform xfA = b3GetBodyTransform( world, joint->edges[0].bodyId ); + b3WorldTransform xfB = b3GetBodyTransform( world, joint->edges[1].bodyId ); + + b3Quat relQ = b3InvMulQuat( xfA.q, xfB.q ); + + switch ( joint->type ) + { + case b3_parallelJoint: + { + // Remove hinge angle + relQ.v.z = 0.0f; + return b3GetQuatAngle( relQ ); + } + + case b3_distanceJoint: + return 0.0f; + + case b3_motorJoint: + return 0.0f; + + case b3_filterJoint: + return 0.0f; + + case b3_prismaticJoint: + return b3GetQuatAngle( relQ ); + + case b3_revoluteJoint: + { + b3RevoluteJoint* revoluteJoint = &base->revoluteJoint; + if ( revoluteJoint->enableLimit ) + { + float angle = b3GetTwistAngle( relQ ); + if ( angle < revoluteJoint->lowerAngle ) + { + return b3GetQuatAngle( relQ ); + } + + if ( revoluteJoint->upperAngle < angle ) + { + return b3GetQuatAngle( relQ ); + } + } + + // Remove hinge angle + relQ.v.z = 0.0f; + return b3GetQuatAngle( relQ ); + } + + case b3_sphericalJoint: + { + b3SphericalJoint* sphericalJoint = &base->sphericalJoint; + float sum = 0.0f; + if ( sphericalJoint->enableConeLimit ) + { + float swingAngle = b3GetSwingAngle( relQ ); + sum += b3MaxFloat( 0.0f, swingAngle - sphericalJoint->coneAngle ); + } + + if ( sphericalJoint->enableTwistLimit ) + { + float twistAngle = b3GetTwistAngle( relQ ); + sum += b3MaxFloat( 0.0f, sphericalJoint->lowerTwistAngle - twistAngle ); + sum += b3MaxFloat( 0.0f, twistAngle - sphericalJoint->upperTwistAngle ); + } + + return sum; + } + + case b3_weldJoint: + { + b3WeldJoint* weldJoint = &base->weldJoint; + if ( weldJoint->angularHertz == 0.0f ) + { + return b3GetQuatAngle( relQ ); + } + + return 0.0f; + } + + case b3_wheelJoint: + // todo + B3_ASSERT( false ); + return 0.0f; + + default: + B3_ASSERT( false ); + return 0.0f; + } +} + +#if 0 +void b3Joint_SetSpringRotationTarget( b3JointId jointId, b3Quat relativeBodyRotation, float hertz ) +{ + B3_ASSERT( b3IsValidQuat( relativeBodyRotation ) ); + B3_ASSERT( b3IsValidFloat( hertz ) && hertz > 0.0f ); + + b3World* world = b3GetWorld( jointId.world0 ); + b3Joint* joint = b3GetJointFullId( world, jointId ); + b3JointSim* base = b3GetJointSim( world, joint ); + + b3Quat qA = base->localFrameA.q; + b3Quat qB = b3MulQuat( relativeBodyRotation, base->localFrameB.q ); + + // This keeps the twist angle in the range [-pi, pi] + if ( b3DotQuat( qA, qB ) < 0.0f ) + { + qA = -qA; + } + + b3Quat relQ = b3InvMulQuat( qA, qB ); + + switch ( joint->type ) + { + case b3_revoluteJoint: + base->revoluteJoint.targetAngle = b3GetTwistAngle( relQ ); + base->revoluteJoint.hertz = hertz; + break; + + case b3_sphericalJoint: + base->sphericalJoint.targetRotation = relQ; + base->sphericalJoint.hertz = hertz; + break; + + default: + break; + } +} +#endif + +void b3PrepareJoint( b3JointSim* joint, b3StepContext* context ) +{ + // Clamp joint hertz based on the time step to reduce jitter. + float hertz = b3MinFloat( joint->constraintHertz, 0.25f * context->inv_h ); + joint->constraintSoftness = b3MakeSoft( hertz, joint->constraintDampingRatio, context->h ); + + switch ( joint->type ) + { + case b3_parallelJoint: + b3PrepareParallelJoint( joint, context ); + break; + + case b3_distanceJoint: + b3PrepareDistanceJoint( joint, context ); + break; + + case b3_filterJoint: + break; + + case b3_motorJoint: + b3PrepareMotorJoint( joint, context ); + break; + + case b3_prismaticJoint: + b3PreparePrismaticJoint( joint, context ); + break; + + case b3_revoluteJoint: + b3PrepareRevoluteJoint( joint, context ); + break; + + case b3_sphericalJoint: + b3PrepareSphericalJoint( joint, context ); + break; + + case b3_weldJoint: + b3PrepareWeldJoint( joint, context ); + break; + + case b3_wheelJoint: + b3PrepareWheelJoint( joint, context ); + break; + + default: + B3_ASSERT( false ); + } +} + +void b3WarmStartJoint( b3JointSim* joint, b3StepContext* context ) +{ + switch ( joint->type ) + { + case b3_parallelJoint: + b3WarmStartParallelJoint( joint, context ); + break; + + case b3_distanceJoint: + b3WarmStartDistanceJoint( joint, context ); + break; + + case b3_filterJoint: + break; + + case b3_motorJoint: + b3WarmStartMotorJoint( joint, context ); + break; + + case b3_prismaticJoint: + b3WarmStartPrismaticJoint( joint, context ); + break; + + case b3_revoluteJoint: + b3WarmStartRevoluteJoint( joint, context ); + break; + + case b3_sphericalJoint: + b3WarmStartSphericalJoint( joint, context ); + break; + + case b3_weldJoint: + b3WarmStartWeldJoint( joint, context ); + break; + + case b3_wheelJoint: + b3WarmStartWheelJoint( joint, context ); + break; + + default: + B3_ASSERT( false ); + } +} + +void b3SolveJoint( b3JointSim* joint, b3StepContext* context, bool useBias ) +{ + B3_UNUSED( useBias ); + + switch ( joint->type ) + { + case b3_parallelJoint: + b3SolveParallelJoint( joint, context ); + break; + + case b3_distanceJoint: + b3SolveDistanceJoint( joint, context, useBias ); + break; + + case b3_filterJoint: + break; + + case b3_motorJoint: + b3SolveMotorJoint( joint, context ); + break; + + case b3_prismaticJoint: + b3SolvePrismaticJoint( joint, context, useBias ); + break; + + case b3_revoluteJoint: + b3SolveRevoluteJoint( joint, context, useBias ); + break; + + case b3_sphericalJoint: + b3SolveSphericalJoint( joint, context, useBias ); + break; + + case b3_weldJoint: + b3SolveWeldJoint( joint, context, useBias ); + break; + + case b3_wheelJoint: + b3SolveWheelJoint( joint, context, useBias ); + break; + + default: + B3_ASSERT( false ); + } +} + +void b3PrepareJoints_Overflow( b3StepContext* context ) +{ + b3TracyCZoneNC( prepare_joints, "PrepJoints", b3_colorOldLace, true ); + + b3ConstraintGraph* graph = context->graph; + b3JointSim* joints = graph->colors[B3_OVERFLOW_INDEX].jointSims.data; + int jointCount = graph->colors[B3_OVERFLOW_INDEX].jointSims.count; + + for ( int i = 0; i < jointCount; ++i ) + { + b3JointSim* joint = joints + i; + b3PrepareJoint( joint, context ); + } + + b3TracyCZoneEnd( prepare_joints ); +} + +void b3WarmStartJoints_Overflow( b3StepContext* context ) +{ + b3TracyCZoneNC( prepare_joints, "PrepJoints", b3_colorOldLace, true ); + + b3ConstraintGraph* graph = context->graph; + b3JointSim* joints = graph->colors[B3_OVERFLOW_INDEX].jointSims.data; + int jointCount = graph->colors[B3_OVERFLOW_INDEX].jointSims.count; + + for ( int i = 0; i < jointCount; ++i ) + { + b3JointSim* joint = joints + i; + b3WarmStartJoint( joint, context ); + } + + b3TracyCZoneEnd( prepare_joints ); +} + +void b3SolveJoints_Overflow( b3StepContext* context, bool useBias ) +{ + b3TracyCZoneNC( solve_joints, "SolveJoints", b3_colorLemonChiffon, true ); + + b3ConstraintGraph* graph = context->graph; + b3JointSim* joints = graph->colors[B3_OVERFLOW_INDEX].jointSims.data; + int jointCount = graph->colors[B3_OVERFLOW_INDEX].jointSims.count; + + for ( int i = 0; i < jointCount; ++i ) + { + b3JointSim* joint = joints + i; + b3SolveJoint( joint, context, useBias ); + } + + b3TracyCZoneEnd( solve_joints ); +} + +void b3DrawJoint( b3DebugDraw* draw, b3World* world, b3Joint* joint ) +{ + b3Body* bodyA = b3Array_Get( world->bodies, joint->edges[0].bodyId ); + b3Body* bodyB = b3Array_Get( world->bodies, joint->edges[1].bodyId ); + if ( bodyA->setIndex == b3_disabledSet || bodyB->setIndex == b3_disabledSet ) + { + return; + } + + b3JointSim* jointSim = b3GetJointSim( world, joint ); + + b3WorldTransform transformA = b3GetBodyTransformQuick( world, bodyA ); + b3WorldTransform transformB = b3GetBodyTransformQuick( world, bodyB ); + b3Pos pA = b3TransformWorldPoint( transformA, jointSim->localFrameA.p ); + b3Pos pB = b3TransformWorldPoint( transformB, jointSim->localFrameB.p ); + + b3HexColor color = b3_colorDarkSeaGreen; + + float scale = b3MaxFloat( 0.0001f, draw->jointScale * joint->drawScale ); + + switch ( joint->type ) + { + case b3_parallelJoint: + b3DrawParallelJoint( draw, jointSim, transformA, transformB, scale ); + break; + + case b3_distanceJoint: + b3DrawDistanceJoint( draw, jointSim, transformA, transformB ); + break; + + case b3_filterJoint: + draw->DrawSegmentFcn( pA, pB, b3_colorGold, draw->context ); + break; + + case b3_motorJoint: + draw->DrawSegmentFcn( pA, pB, b3_colorPlum, draw->context ); + draw->DrawPointFcn( pA, 8.0f, b3_colorYellowGreen, draw->context ); + draw->DrawPointFcn( pB, 8.0f, b3_colorPlum, draw->context ); + break; + + case b3_prismaticJoint: + b3DrawPrismaticJoint( draw, jointSim, transformA, transformB, scale ); + break; + + case b3_revoluteJoint: + b3DrawRevoluteJoint( draw, jointSim, transformA, transformB, scale ); + break; + + case b3_sphericalJoint: + b3DrawSphericalJoint( draw, jointSim, transformA, transformB, scale ); + break; + + case b3_weldJoint: + b3DrawWeldJoint( draw, jointSim, transformA, transformB, scale ); + break; + + case b3_wheelJoint: + b3DrawWheelJoint( draw, jointSim, transformA, transformB, scale ); + break; + + default: + draw->DrawSegmentFcn( transformA.p, pA, color, draw->context ); + draw->DrawSegmentFcn( pA, pB, color, draw->context ); + draw->DrawSegmentFcn( transformB.p, pB, color, draw->context ); + break; + } + + if ( draw->drawGraphColors ) + { + b3HexColor graphColors[B3_GRAPH_COLOR_COUNT] = { + b3_colorRed, b3_colorOrange, b3_colorYellow, b3_colorGreen, b3_colorCyan, b3_colorBlue, + b3_colorViolet, b3_colorPink, b3_colorChocolate, b3_colorGoldenRod, b3_colorCoral, b3_colorRosyBrown, + b3_colorAqua, b3_colorPeru, b3_colorLime, b3_colorGold, b3_colorPlum, b3_colorSnow, + b3_colorTeal, b3_colorKhaki, b3_colorSalmon, b3_colorPeachPuff, b3_colorHoneyDew, b3_colorBlack, + }; + + int colorIndex = joint->colorIndex; + if ( colorIndex != B3_NULL_INDEX ) + { + b3Pos p = b3LerpPosition( pA, pB, 0.5f ); + draw->DrawPointFcn( p, 5.0f, graphColors[colorIndex], draw->context ); + } + } + + if ( draw->drawJointExtras ) + { + b3Vec3 force = b3GetJointConstraintForce( world, joint ); + b3Vec3 torque = b3GetJointConstraintTorque( world, joint ); + b3Pos p = b3LerpPosition( pA, pB, 0.5f ); + + draw->DrawSegmentFcn( p, b3OffsetPos( p, b3MulSV( 0.001f, force ) ), b3_colorAzure, draw->context ); + + char buffer[64]; + snprintf( buffer, 64, "f = %g, t = %g", b3Length( force ), b3Length( torque ) ); + draw->DrawStringFcn( p, buffer, b3_colorAzure, draw->context ); + } +} diff --git a/vendor/box3d/src/src/joint.h b/vendor/box3d/src/src/joint.h new file mode 100644 index 000000000..64171b957 --- /dev/null +++ b/vendor/box3d/src/src/joint.h @@ -0,0 +1,415 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +#pragma once + +#include "math_internal.h" +#include "solver.h" + +#include "box3d/types.h" + +typedef struct b3DebugDraw b3DebugDraw; +typedef struct b3StepContext b3StepContext; +typedef struct b3World b3World; + +/// A joint edge is used to connect bodies and joints together +/// in a joint graph where each body is a node and each joint +/// is an edge. A joint edge belongs to a doubly linked list +/// maintained in each attached body. Each joint has two joint +/// nodes, one for each attached body. +typedef struct b3JointEdge +{ + int bodyId; + int prevKey; + int nextKey; +} b3JointEdge; + +// Map from b3JointId to b3Joint in the solver sets +typedef struct b3Joint +{ + void* userData; + + // index of simulation set stored in b3World + // B3_NULL_INDEX when slot is free + int setIndex; + + // index into the constraint graph color array, may be B3_NULL_INDEX for sleeping/disabled joints + // B3_NULL_INDEX when slot is free + int colorIndex; + + // joint index within set or graph color + // B3_NULL_INDEX when slot is free + int localIndex; + + b3JointEdge edges[2]; + + int jointId; + int islandId; + + // Index into the island's joints array for O(1) swap-removal. + // B3_NULL_INDEX when not in an island. + int islandIndex; + + float drawScale; + + b3JointType type; + + // This is monotonically advanced when a body is allocated in this slot + // Used to check for invalid b3JointId + uint16_t generation; + + bool collideConnected; +} b3Joint; + +typedef struct b3DistanceJoint +{ + float length; + float hertz; + float dampingRatio; + float lowerSpringForce; + float upperSpringForce; + float minLength; + float maxLength; + + float maxMotorForce; + float motorSpeed; + + float impulse; + float lowerImpulse; + float upperImpulse; + float motorImpulse; + + int indexA; + int indexB; + b3Vec3 anchorA; + b3Vec3 anchorB; + b3Vec3 deltaCenter; + b3Softness distanceSoftness; + float axialMass; + + bool enableSpring; + bool enableLimit; + bool enableMotor; +} b3DistanceJoint; + +typedef struct b3MotorJoint +{ + b3Vec3 linearVelocity; + b3Vec3 angularVelocity; + float maxVelocityForce; + float maxVelocityTorque; + float linearHertz; + float linearDampingRatio; + float maxSpringForce; + float angularHertz; + float angularDampingRatio; + float maxSpringTorque; + + b3Vec3 linearVelocityImpulse; + b3Vec3 angularVelocityImpulse; + b3Vec3 linearSpringImpulse; + b3Vec3 angularSpringImpulse; + + b3Softness linearSpring; + b3Softness angularSpring; + + int indexA; + int indexB; + b3Transform frameA; + b3Transform frameB; + b3Vec3 deltaCenter; + b3Matrix3 angularMass; +} b3MotorJoint; + +typedef struct b3ParallelJoint +{ + float hertz; + float dampingRatio; + float maxTorque; + + b3Vec2 perpImpulse; + b3Vec3 perpAxisX; + b3Vec3 perpAxisY; + + b3Quat quatA; + b3Quat quatB; + int indexA; + int indexB; + b3Softness softness; +} b3ParallelJoint; + +typedef struct b3PrismaticJoint +{ + b3Vec2 perpImpulse; + b3Vec3 angularImpulse; + float springImpulse; + float motorImpulse; + float lowerImpulse; + float upperImpulse; + float hertz; + float dampingRatio; + float maxMotorForce; + float motorSpeed; + float targetTranslation; + float lowerTranslation; + float upperTranslation; + + int indexA; + int indexB; + b3Transform frameA; + b3Transform frameB; + b3Vec3 jointAxis; + b3Vec3 perpAxisY; + b3Vec3 perpAxisZ; + b3Vec3 deltaCenter; + float deltaAngle; + b3Matrix3 rotationMass; + b3Softness springSoftness; + + bool enableSpring; + bool enableLimit; + bool enableMotor; +} b3PrismaticJoint; + +typedef struct b3RevoluteJoint +{ + b3Vec3 linearImpulse; + b3Vec2 perpImpulse; + float springImpulse; + float motorImpulse; + float lowerImpulse; + float upperImpulse; + float hertz; + float dampingRatio; + float maxMotorTorque; + float motorSpeed; + float targetAngle; + float lowerAngle; + float upperAngle; + + int indexA; + int indexB; + b3Transform frameA; + b3Transform frameB; + b3Vec3 rotationAxisZ; + b3Vec3 perpAxisX; + b3Vec3 perpAxisY; + b3Vec3 deltaCenter; + float deltaAngle; + float axialMass; + b3Softness springSoftness; + + bool enableSpring; + bool enableMotor; + bool enableLimit; +} b3RevoluteJoint; + +typedef struct b3SphericalJoint +{ + b3Vec3 linearImpulse; + b3Vec3 springImpulse; + b3Vec3 motorImpulse; + float lowerTwistImpulse; + float upperTwistImpulse; + float swingImpulse; + float hertz; + float dampingRatio; + float maxMotorTorque; + b3Vec3 motorVelocity; + float lowerTwistAngle; + float upperTwistAngle; + float coneAngle; + b3Quat targetRotation; + + int indexA; + int indexB; + b3Transform frameA; + b3Transform frameB; + b3Vec3 deltaCenter; + b3Vec3 swingAxis; + b3Vec3 twistJacobian; + + b3Matrix3 rotationMass; + float swingMass; + float twistMass; + b3Softness springSoftness; + + bool enableSpring; + bool enableMotor; + bool enableConeLimit; + bool enableTwistLimit; +} b3SphericalJoint; + +typedef struct b3WeldJoint +{ + float linearHertz; + float linearDampingRatio; + float angularHertz; + float angularDampingRatio; + + b3Softness linearSpring; + b3Softness angularSpring; + b3Vec3 linearImpulse; + b3Vec3 angularImpulse; + + int indexA; + int indexB; + b3Transform frameA; + b3Transform frameB; + b3Vec3 deltaCenter; + + b3Matrix3 angularMass; +} b3WeldJoint; + +typedef struct b3WheelJoint +{ + b3Vec2 linearImpulse; + b3Vec2 angularImpulse; + float spinImpulse; + float maxSpinTorque; + float spinSpeed; + float suspensionSpringImpulse; + float lowerSuspensionImpulse; + float upperSuspensionImpulse; + float lowerSuspensionLimit; + float upperSuspensionLimit; + float suspensionHertz; + float suspensionDampingRatio; + float steeringSpringImpulse; + float lowerSteeringImpulse; + float upperSteeringImpulse; + float lowerSteeringLimit; + float upperSteeringLimit; + float targetSteeringAngle; + float maxSteeringTorque; + float steeringHertz; + float steeringDampingRatio; + + int indexA; + int indexB; + b3Transform frameA; + b3Transform frameB; + b3Vec3 deltaCenter; + float spinMass; + float suspensionMass; + float steeringMass; + b3Softness suspensionSoftness; + b3Softness steeringSoftness; + + bool enableSpinMotor; + bool enableSuspensionSpring; + bool enableSuspensionLimit; + bool enableSteering; + bool enableSteeringLimit; + bool enableSteeringMotor; +} b3WheelJoint; + +/// The base joint class. Joints are used to constraint two bodies together in +/// various fashions. Some joints also feature limits and motors. +typedef struct b3JointSim +{ + int jointId; + + int bodyIdA; + int bodyIdB; + + b3JointType type; + + // Joint frames local to body origin + b3Transform localFrameA; + b3Transform localFrameB; + + float invMassA, invMassB; + b3Matrix3 invIA, invIB; + + float constraintHertz; + float constraintDampingRatio; + + b3Softness constraintSoftness; + + float forceThreshold; + float torqueThreshold; + + bool fixedRotation; + + union + { + b3DistanceJoint distanceJoint; + b3MotorJoint motorJoint; + b3ParallelJoint parallelJoint; + b3RevoluteJoint revoluteJoint; + b3SphericalJoint sphericalJoint; + b3PrismaticJoint prismaticJoint; + b3WeldJoint weldJoint; + b3WheelJoint wheelJoint; + }; +} b3JointSim; + +void b3DestroyJointInternal( b3World* world, b3Joint* joint, bool wakeBodies ); + +b3Joint* b3GetJointFullId( b3World* world, b3JointId jointId ); +b3JointSim* b3GetJointSim( b3World* world, b3Joint* joint ); +b3JointSim* b3GetJointSimCheckType( b3JointId jointId, b3JointType type ); + +void b3PrepareJoint( b3JointSim* joint, b3StepContext* context ); +void b3WarmStartJoint( b3JointSim* joint, b3StepContext* context ); +void b3SolveJoint( b3JointSim* joint, b3StepContext* context, bool useBias ); + +void b3PrepareJoints_Overflow( b3StepContext* context ); +void b3WarmStartJoints_Overflow( b3StepContext* context ); +void b3SolveJoints_Overflow( b3StepContext* context, bool useBias ); + +void b3GetJointReaction( b3World* world, b3JointSim* sim, float invTimeStep, float* force, float* torque ); + +void b3DrawJoint( b3DebugDraw* draw, b3World* world, b3Joint* joint ); + +b3Vec3 b3GetDistanceJointForce( b3World* world, b3JointSim* base ); +b3Vec3 b3GetMotorJointForce( b3World* world, b3JointSim* base ); +b3Vec3 b3GetPrismaticJointForce( b3World* world, b3JointSim* base ); +b3Vec3 b3GetRevoluteJointForce( b3World* world, b3JointSim* base ); +b3Vec3 b3GetSphericalJointForce( b3World* world, b3JointSim* base ); +b3Vec3 b3GetWeldJointForce( b3World* world, b3JointSim* base ); +b3Vec3 b3GetWheelJointForce( b3World* world, b3JointSim* base ); + +b3Vec3 b3GetMotorJointTorque( b3World* world, b3JointSim* base ); +b3Vec3 b3GetParallelJointTorque( b3World* world, b3JointSim* base ); +b3Vec3 b3GetPrismaticJointTorque( b3World* world, b3JointSim* base ); +b3Vec3 b3GetRevoluteJointTorque( b3World* world, b3JointSim* base ); +b3Vec3 b3GetSphericalJointTorque( b3World* world, b3JointSim* base ); +b3Vec3 b3GetWeldJointTorque( b3World* world, b3JointSim* base ); +b3Vec3 b3GetWheelJointTorque( b3World* world, b3JointSim* base ); + +void b3PrepareDistanceJoint( b3JointSim* base, b3StepContext* context ); +void b3PrepareMotorJoint( b3JointSim* base, b3StepContext* context ); +void b3PrepareParallelJoint( b3JointSim* base, b3StepContext* context ); +void b3PreparePrismaticJoint( b3JointSim* base, b3StepContext* context ); +void b3PrepareRevoluteJoint( b3JointSim* base, b3StepContext* context ); +void b3PrepareSphericalJoint( b3JointSim* base, b3StepContext* context ); +void b3PrepareWeldJoint( b3JointSim* base, b3StepContext* context ); +void b3PrepareWheelJoint( b3JointSim* base, b3StepContext* context ); + +void b3WarmStartDistanceJoint( b3JointSim* base, b3StepContext* context ); +void b3WarmStartMotorJoint( b3JointSim* base, b3StepContext* context ); +void b3WarmStartParallelJoint( b3JointSim* base, b3StepContext* context ); +void b3WarmStartPrismaticJoint( b3JointSim* base, b3StepContext* context ); +void b3WarmStartRevoluteJoint( b3JointSim* base, b3StepContext* context ); +void b3WarmStartSphericalJoint( b3JointSim* base, b3StepContext* context ); +void b3WarmStartWeldJoint( b3JointSim* base, b3StepContext* context ); +void b3WarmStartWheelJoint( b3JointSim* base, b3StepContext* context ); + +void b3SolveDistanceJoint( b3JointSim* base, b3StepContext* context, bool useBias ); +void b3SolveMotorJoint( b3JointSim* base, b3StepContext* context ); +void b3SolveParallelJoint( b3JointSim* base, b3StepContext* context ); +void b3SolvePrismaticJoint( b3JointSim* base, b3StepContext* context, bool useBias ); +void b3SolveRevoluteJoint( b3JointSim* base, b3StepContext* context, bool useBias ); +void b3SolveSphericalJoint( b3JointSim* base, b3StepContext* context, bool useBias ); +void b3SolveWeldJoint( b3JointSim* base, b3StepContext* context, bool useBias ); +void b3SolveWheelJoint( b3JointSim* base, b3StepContext* context, bool useBias ); + +void b3DrawDistanceJoint( b3DebugDraw* draw, b3JointSim* base, b3WorldTransform transformA, b3WorldTransform transformB ); +void b3DrawParallelJoint( b3DebugDraw* draw, b3JointSim* base, b3WorldTransform transformA, b3WorldTransform transformB, float scale ); +void b3DrawPrismaticJoint( b3DebugDraw* draw, b3JointSim* base, b3WorldTransform transformA, b3WorldTransform transformB, float scale ); +void b3DrawRevoluteJoint( b3DebugDraw* draw, b3JointSim* base, b3WorldTransform transformA, b3WorldTransform transformB, float scale ); +void b3DrawSphericalJoint( b3DebugDraw* draw, b3JointSim* base, b3WorldTransform transformA, b3WorldTransform transformB, float scale ); +void b3DrawWeldJoint( b3DebugDraw* draw, b3JointSim* base, b3WorldTransform transformA, b3WorldTransform transformB, float scale ); +void b3DrawWheelJoint( b3DebugDraw* draw, b3JointSim* base, b3WorldTransform transformA, b3WorldTransform transformB, float scale ); diff --git a/vendor/box3d/src/src/manifold.c b/vendor/box3d/src/src/manifold.c new file mode 100644 index 000000000..40e60ad57 --- /dev/null +++ b/vendor/box3d/src/src/manifold.c @@ -0,0 +1,246 @@ +// SPDX-FileCopyrightText: 2026 Erin Catto +// SPDX-License-Identifier: MIT + +// Dirk Gregorius contributed portions of this code + +#include "manifold.h" + +#include "algorithm.h" +#include "shape.h" + +#include "box3d/math_functions.h" + +// p1 : origin on edge 1 +// e1 : edge 1 +// c1 : shape 1 centroid +// p2 : origin on edge 2 +// e2 : edge 2 +// c2 : shape 2 centroid +float b3EdgeEdgeSeparation( b3Vec3 p1, b3Vec3 e1, b3Vec3 c1, b3Vec3 p2, b3Vec3 e2, b3Vec3 c2 ) +{ + // Build search direction + b3Vec3 u = b3Cross( e1, e2 ); + float length = b3Length( u ); + + // Skip near parallel edges: |e1 x e1| = sin(alpha) * |e1| * |e2| + const float kTolerance = 0.005f; + if ( length < kTolerance * sqrtf( b3LengthSquared( e1 ) * b3LengthSquared( e2 ) ) ) + { + return -FLT_MAX; + } + + if ( length * length < 1000.0f * FLT_MIN ) + { + return -FLT_MAX; + } + + b3Vec3 n = b3MulSV( 1.0f / length, u ); + + // Make sure normal points away from the first shape + // For a triangle, it is possible that N is aligned with the triangle normal and the sign + // value can be close to zero and flicker between small negative and positive values, leading to + // an incorrect separation value. So we assume the other hull has some volume and pick the most + // significant sign value to orient N. + float sign1 = b3Dot( n, b3Sub( p1, c1 ) ); + float sign2 = b3Dot( n, b3Sub( p2, c2 ) ); + if ( b3AbsFloat( sign1 ) > b3AbsFloat( sign2 ) ) + { + if ( sign1 < 0.0f ) + { + n = b3Neg( n ); + } + } + else + { + if ( sign2 > 0.0f ) + { + n = b3Neg( n ); + } + } + + // s = Dot(n, p2) - d = Dot(n, p2) - Dot(n, p1) = Dot(n, p2 - p1) + return b3Dot( n, b3Sub( p2, p1 ) ); +} + +// This was extended to make the wedge shape get the correct incident face. +// Instead of looking directly for the most anti-parallel face, we first find the closest vertex (passed in). +// Then we look for all edges coming out of that vertex and look for the edge that is +// most perpendicular to the reference normal. +// Then from that edge, we select the adjacent face that is most anti-parallel to the reference normal. +int b3FindIncidentFace( const b3HullData* hull, b3Vec3 refNormal, int vertexIndex ) +{ + const b3HullVertex* vertices = b3GetHullVertices( hull ); + const b3HullHalfEdge* edges = b3GetHullEdges( hull ); + const b3Plane* planes = b3GetHullPlanes( hull ); + const b3Vec3* points = b3GetHullPoints( hull ); + + int minEdgeIndex = -1; + float minEdgeProjection = FLT_MAX; + + const b3HullVertex* vertex = vertices + vertexIndex; + B3_ASSERT( vertex ); + + int edgeIndex = vertex->edge; + const b3HullHalfEdge* edge = edges + edgeIndex; + b3Vec3 edgeOrigin = points[edge->origin]; + B3_ASSERT( edge->origin == vertexIndex ); + + do + { + const b3HullHalfEdge* twin = edges + edge->twin; + b3Vec3 twinOrigin = points[twin->origin]; + + b3Vec3 axis = b3Normalize( b3Sub( twinOrigin, edgeOrigin ) ); + float edgeProjection = b3AbsFloat( b3Dot( axis, refNormal ) ); + if ( edgeProjection < minEdgeProjection ) + { + minEdgeIndex = edgeIndex; + minEdgeProjection = edgeProjection; + } + + edgeIndex = twin->next; + edge = edges + edgeIndex; + B3_ASSERT( edge->origin == vertexIndex ); + } + while ( edge != edges + vertex->edge ); + B3_ASSERT( minEdgeIndex >= 0 ); + + const b3HullHalfEdge* minEdge = edges + minEdgeIndex; + int minFaceIndex1 = minEdge->face; + b3Plane minPlane1 = planes[minFaceIndex1]; + + const b3HullHalfEdge* minTwin = edges + minEdge->twin; + int minFaceIndex2 = minTwin->face; + b3Plane minPlane2 = planes[minFaceIndex2]; + + return b3Dot( minPlane1.normal, refNormal ) < b3Dot( minPlane2.normal, refNormal ) ? minFaceIndex1 : minFaceIndex2; +} + +b3FeaturePair b3MakeFeaturePair( b3FeatureOwner owner1, int index1, b3FeatureOwner owner2, int index2 ) +{ + B3_ASSERT( 0 <= index1 && index1 <= UINT8_MAX ); + B3_ASSERT( 0 <= index2 && index2 <= UINT8_MAX ); + + b3FeaturePair pair; + pair.index1 = (uint8_t)index1; + pair.owner1 = (uint8_t)owner1; + pair.index2 = (uint8_t)index2; + pair.owner2 = (uint8_t)owner2; + return pair; +} + +// This logic seems wrong but it is designed so that choosing +// face A or B as the reference face does not change the resulting +// feature pair. This way the contact impulses are persisted even +// if there is reference face flip-flop. This is verified in the +// HullAndHull sample using the b3SATCache with a manual feature +// specified such as b3_manualFaceAxisA. +b3FeaturePair b3FlipPair( b3FeaturePair pair ) +{ + B3_ASSERT( pair.owner1 == 0 || pair.owner1 == 1 ); + B3_ASSERT( pair.owner2 == 0 || pair.owner2 == 1 ); + B3_SWAP( pair.owner1, pair.owner2 ); + pair.owner1 = 1 - pair.owner1; + pair.owner2 = 1 - pair.owner2; + B3_SWAP( pair.index1, pair.index2 ); + return pair; +} + +#if B3_ENABLE_VALIDATION +bool b3ValidatePolygon( b3ClipVertex* polygon, int count ) +{ + // Empty polygons are valid (we can clip away all points when re-constructing manifolds from cache) + if ( count == 0 ) + { + return true; + } + + // Validate that incoming and outgoing edges match + b3ClipVertex vertex1 = polygon[count - 1]; + for ( int i = 0; i < count; ++i ) + { + b3ClipVertex vertex2 = polygon[i]; + + if ( vertex1.pair.owner2 != vertex2.pair.owner1 ) + { + return false; + } + + if ( vertex1.pair.index2 != vertex2.pair.index1 ) + { + return false; + } + + vertex1 = vertex2; + } + + return true; +} +#endif + +int b3ClipPolygon( b3ClipVertex* out, b3ClipVertex* polygon, int count, b3Plane clipPlane, int edge, b3Plane refPlane ) +{ + B3_ASSERT( count >= 3 ); + + b3ClipVertex vertex1 = polygon[count - 1]; + float distance1 = b3PlaneSeparation( clipPlane, vertex1.position ); + int outCount = 0; + + for ( int index = 0; index < count; ++index ) + { + b3ClipVertex vertex2 = polygon[index]; + float distance2 = b3PlaneSeparation( clipPlane, vertex2.position ); + + // Clip edge against plane (Sutherland-Hodgman clipping) + if ( distance1 <= 0.0f && distance2 <= 0.0f ) + { + // Both vertices are behind the plane - keep vertex2 + out[outCount] = vertex2; + outCount += 1; + } + else if ( distance1 <= 0.0f && distance2 > 0.0f ) + { + // Vertex1 is behind of the plane, vertex2 is in front -> intersection point + float fraction = distance1 / ( distance1 - distance2 ); + b3Vec3 position = b3MulAdd( vertex1.position, fraction, b3Sub( vertex2.position, vertex1.position ) ); + + // Keep intersection point and adjust outgoing edge + b3ClipVertex vertex; + vertex.position = position; + vertex.separation = b3PlaneSeparation( refPlane, position ); + vertex.pair = vertex2.pair; + vertex.pair.owner2 = b3_featureShapeA; + vertex.pair.index2 = (uint8_t)edge; + out[outCount] = vertex; + outCount += 1; + } + else if ( distance2 <= 0.0f && distance1 > 0.0f ) + { + // Vertex1 is in front, vertex2 is behind of the plane, -> intersection point + float fraction = distance1 / ( distance1 - distance2 ); + b3Vec3 position = b3MulAdd( vertex1.position, fraction, b3Sub( vertex2.position, vertex1.position ) ); + + // Keep intersection point and adjust incoming edge + b3ClipVertex vertex; + vertex.position = position; + vertex.separation = b3PlaneSeparation( refPlane, position ); + vertex.pair = vertex1.pair; + vertex.pair.owner1 = b3_featureShapeA; + vertex.pair.index1 = (uint8_t)edge; + out[outCount] = vertex; + outCount += 1; + + // And also keep vertex2 + out[outCount] = vertex2; + outCount += 1; + } + + // Keep vertex2 as starting vertex for next edge + vertex1 = vertex2; + distance1 = distance2; + } + + B3_VALIDATE( b3ValidatePolygon( out, outCount ) ); + + return outCount; +} diff --git a/vendor/box3d/src/src/manifold.h b/vendor/box3d/src/src/manifold.h new file mode 100644 index 000000000..9e529ab97 --- /dev/null +++ b/vendor/box3d/src/src/manifold.h @@ -0,0 +1,57 @@ +// SPDX-FileCopyrightText: 2026 Erin Catto +// SPDX-License-Identifier: MIT + +#pragma once + +#include "box3d/collision.h" +#include "box3d/math_functions.h" + +#define B3_MAX_CLIP_POINTS 64 + +typedef struct b3FaceQuery +{ + float separation; + int faceIndex; + int vertexIndex; +} b3FaceQuery; + +typedef struct b3EdgeQuery +{ + float separation; + int indexA; + int indexB; +} b3EdgeQuery; + +typedef struct b3ClipVertex +{ + b3Vec3 position; + float separation; + b3FeaturePair pair; +} b3ClipVertex; + +typedef enum b3FeatureOwner +{ + b3_featureShapeA = 0, + b3_featureShapeB = 1 +} b3FeatureOwner; + +float b3EdgeEdgeSeparation( b3Vec3 p1, b3Vec3 e1, b3Vec3 c1, b3Vec3 p2, b3Vec3 e2, b3Vec3 c2 ); +int b3FindIncidentFace( const b3HullData* hull, b3Vec3 refNormal, int vertexIndex ); +b3FeaturePair b3MakeFeaturePair( b3FeatureOwner owner1, int index1, b3FeatureOwner owner2, int index2 ); + +b3FeaturePair b3FlipPair( b3FeaturePair pair ); + +int b3ClipPolygon( b3ClipVertex* out, b3ClipVertex* polygon, int count, b3Plane clipPlane, int edge, b3Plane refPlane ); + +#if B3_ENABLE_VALIDATION +bool b3ValidatePolygon( b3ClipVertex* polygon, int count ); +#endif + +// For single point contact, such as sphere-sphere, sphere-capsule, sphere-triangle +static const b3FeaturePair b3FeaturePair_single = { 0 }; + +static inline uint32_t b3MakeFeatureId( b3FeaturePair pair ) +{ + return ( (uint32_t)pair.owner1 << 24 ) | ( (uint32_t)pair.index1 << 16 ) | ( (uint32_t)pair.owner2 << 8 ) | + (uint32_t)pair.index2; +} diff --git a/vendor/box3d/src/src/math_functions.c b/vendor/box3d/src/src/math_functions.c new file mode 100644 index 000000000..a339d95ff --- /dev/null +++ b/vendor/box3d/src/src/math_functions.c @@ -0,0 +1,616 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +#include "box3d/math_functions.h" + +#include "math_internal.h" + +#include "box3d/collision.h" +#include "box3d/constants.h" + +#include +#include + +bool b3IsValidFloat( float a ) +{ + if ( isnan( a ) ) + { + return false; + } + + if ( isinf( a ) ) + { + return false; + } + + return true; +} + +bool b3IsValidVec3( b3Vec3 a ) +{ + if ( isnan( a.x ) || isnan( a.y ) || isnan( a.z ) ) + { + return false; + } + + if ( isinf( a.x ) || isinf( a.y ) || isinf( a.z ) ) + { + return false; + } + + return true; +} + +bool b3IsValidQuat( b3Quat a ) +{ + if ( isnan( a.v.x ) || isnan( a.v.y ) || isnan( a.v.z ) || isnan( a.s ) ) + { + return false; + } + + if ( isinf( a.v.x ) || isinf( a.v.y ) || isinf( a.v.z ) || isinf( a.s ) ) + { + return false; + } + + return b3IsNormalizedQuat( a ); +} + +bool b3IsValidTransform( b3Transform a ) +{ + return b3IsValidVec3( a.p ) && b3IsValidQuat( a.q ); +} + +bool b3IsValidMatrix3( b3Matrix3 a ) +{ + return b3IsValidVec3( a.cx ) && b3IsValidVec3( a.cy ) && b3IsValidVec3( a.cz ); +} + +bool b3IsValidAABB( b3AABB a ) +{ + if ( b3IsValidVec3( a.lowerBound ) == false ) + { + return false; + } + + if ( b3IsValidVec3( a.upperBound ) == false ) + { + return false; + } + + if ( a.lowerBound.x > a.upperBound.x ) + { + return false; + } + + if ( a.lowerBound.y > a.upperBound.y ) + { + return false; + } + + if ( a.lowerBound.z > a.upperBound.z ) + { + return false; + } + + return true; +} + +bool b3IsBoundedAABB( b3AABB a ) +{ + if ( a.lowerBound.x < -B3_HUGE || a.lowerBound.y < -B3_HUGE || a.lowerBound.z < -B3_HUGE ) + { + return false; + } + + if ( a.upperBound.x > B3_HUGE || a.upperBound.y > B3_HUGE || a.upperBound.z > B3_HUGE ) + { + return false; + } + + return true; +} + +bool b3IsSaneAABB( b3AABB a ) +{ + if ( b3IsValidAABB( a ) == false ) + { + return false; + } + + if ( a.lowerBound.x < -B3_HUGE || a.lowerBound.y < -B3_HUGE || a.lowerBound.z < -B3_HUGE ) + { + return false; + } + + if ( a.upperBound.x > B3_HUGE || a.upperBound.y > B3_HUGE || a.upperBound.z > B3_HUGE ) + { + return false; + } + + return true; +} + +bool b3IsValidPlane( b3Plane a ) +{ + if ( b3IsValidVec3( a.normal ) == false ) + { + return false; + } + + if ( b3IsNormalized( a.normal ) == false ) + { + return false; + } + + return b3IsValidFloat( a.offset ); +} + +bool b3IsValidPosition( b3Pos p ) +{ + if ( isnan( p.x ) || isnan( p.y ) || isnan( p.z ) ) + { + return false; + } + + if ( isinf( p.x ) || isinf( p.y ) || isinf( p.z ) ) + { + return false; + } + + return true; +} + +bool b3IsValidWorldTransform( b3WorldTransform t ) +{ + return b3IsValidPosition( t.p ) && b3IsValidQuat( t.q ); +} + +// https://stackoverflow.com/questions/46210708/atan2-approximation-with-11bits-in-mantissa-on-x86with-sse2-and-armwith-vfpv4 +float b3Atan2( float y, float x ) +{ + // Added check for (0,0) to match atan2f and avoid NaN + if ( x == 0.0f && y == 0.0f ) + { + return 0.0f; + } + + float ax = b3AbsFloat( x ); + float ay = b3AbsFloat( y ); + float mx = b3MaxFloat( ay, ax ); + float mn = b3MinFloat( ay, ax ); + float a = mn / mx; + + // Minimax polynomial approximation to atan(a) on [0,1] + float s = a * a; + float c = s * a; + float q = s * s; + float r = 0.024840285f * q + 0.18681418f; + float t = -0.094097948f * q - 0.33213072f; + r = r * s + t; + r = r * c + a; + + // Map to full circle + if ( ay > ax ) + { + r = 1.57079637f - r; + } + + if ( x < 0 ) + { + r = 3.14159274f - r; + } + + if ( y < 0 ) + { + r = -r; + } + + return r; +} + +// Approximate cosine and sine for determinism. In my testing cosf and sinf produced +// the same results on x64 and ARM using MSVC, GCC, and Clang. However, I don't trust +// this result. +// https://en.wikipedia.org/wiki/Bh%C4%81skara_I%27s_sine_approximation_formula +b3CosSin b3ComputeCosSin( float radians ) +{ +#if 0 + return { + cosf( radians ), + sinf( radians ), + }; +#else + float x = b3UnwindAngle( radians ); + float pi2 = B3_PI * B3_PI; + + // cosine needs angle in [-pi/2, pi/2] + float c; + if ( x < -0.5f * B3_PI ) + { + float y = x + B3_PI; + float y2 = y * y; + c = -( pi2 - 4.0f * y2 ) / ( pi2 + y2 ); + } + else if ( x > 0.5f * B3_PI ) + { + float y = x - B3_PI; + float y2 = y * y; + c = -( pi2 - 4.0f * y2 ) / ( pi2 + y2 ); + } + else + { + float y2 = x * x; + c = ( pi2 - 4.0f * y2 ) / ( pi2 + y2 ); + } + + // sine needs angle in [0, pi] + float s; + if ( x < 0.0f ) + { + float y = x + B3_PI; + s = -16.0f * y * ( B3_PI - y ) / ( 5.0f * pi2 - 4.0f * y * ( B3_PI - y ) ); + } + else + { + s = 16.0f * x * ( B3_PI - x ) / ( 5.0f * pi2 - 4.0f * x * ( B3_PI - x ) ); + } + + float mag = sqrtf( s * s + c * c ); + float invMag = mag > 0.0f ? 1.0f / mag : 0.0f; + b3CosSin cs = { c * invMag, s * invMag }; + return cs; +#endif +} + +b3Quat b3MakeQuatFromMatrix( const b3Matrix3* m ) +{ + b3Vec3 c1 = m->cx; + b3Vec3 c2 = m->cy; + b3Vec3 c3 = m->cz; + + b3Quat q; + + float trace = m->cx.x + m->cy.y + m->cz.z; + if ( trace >= 0.0f ) + { + q.v.x = c2.z - c3.y; + q.v.y = c3.x - c1.z; + q.v.z = c1.y - c2.x; + q.s = trace + 1.0f; + } + else + { + if ( c1.x > c2.y && c1.x > c3.z ) + { + q.v.x = c1.x - c2.y - c3.z + 1.0f; + q.v.y = c2.x + c1.y; + q.v.z = c3.x + c1.z; + q.s = c2.z - c3.y; + } + else if ( c2.y > c3.z ) + { + q.v.x = c1.y + c2.x; + q.v.y = c2.y - c3.z - c1.x + 1.0f; + q.v.z = c3.y + c2.z; + q.s = c3.x - c1.z; + } + else + { + q.v.x = c1.z + c3.x; + q.v.y = c2.z + c3.y; + q.v.z = c3.z - c1.x - c2.y + 1.0f; + q.s = c1.y - c2.x; + } + } + + // The algorithm is simplified and made more accurate by normalizing at the end + return b3NormalizeQuat( q ); +} + +b3Quat b3ComputeQuatBetweenUnitVectors( b3Vec3 v1, b3Vec3 v2 ) +{ + B3_ASSERT( b3IsNormalized( v1 ) ); + B3_ASSERT( b3IsNormalized( v2 ) ); + + b3Quat out; + + b3Vec3 m = b3Lerp( v1, v2, 0.5f ); + float tolerance = 100.0f * FLT_EPSILON; + if ( b3LengthSquared( m ) > tolerance * tolerance ) + { + out.v = b3Cross( v1, m ); + out.s = b3Dot( v1, m ); + } + else + { + // Anti-parallel: Use a perpendicular vector + if ( b3AbsFloat( v1.x ) > 0.5f ) + { + out.v.x = v1.y; + out.v.y = -v1.x; + out.v.z = 0.0f; + } + else + { + out.v.x = 0.0f; + out.v.y = v1.z; + out.v.z = -v1.y; + } + + out.s = 0.0f; + } + + // The algorithm is simplified and made more accurate by normalizing at the end + return b3NormalizeQuat( out ); +} + +b3SegmentDistanceResult b3LineDistance( b3Vec3 p1, b3Vec3 d1, b3Vec3 p2, b3Vec3 d2 ) +{ + b3SegmentDistanceResult result; + + // Solve A*x = b + float a11 = b3Dot( d1, d1 ); + float a12 = -b3Dot( d1, d2 ); + float a21 = b3Dot( d2, d1 ); + float a22 = -b3Dot( d2, d2 ); + + b3Vec3 w = b3Sub( p1, p2 ); + float b1 = -b3Dot( d1, w ); + float b2 = -b3Dot( d2, w ); + + float det = a11 * a22 - a12 * a21; + if ( det * det < 1000.0f * FLT_MIN ) + { + // Lines are parallel - project p2 onto line L1: x1 = p1 + s1 * d1 + float s1 = b3Dot( b3Sub( p2, p1 ), d1 ) / b3Dot( d1, d1 ); + float s2 = 0.0f; + + result.point1 = b3MulAdd( p1, s1, d1 ); + result.fraction1 = s1; + result.point2 = b3MulAdd( p2, s2, d2 ); + result.fraction2 = s2; + + return result; + } + + float s1 = ( a22 * b1 - a12 * b2 ) / det; + float s2 = ( a11 * b2 - a21 * b1 ) / det; + + result.point1 = b3MulAdd( p1, s1, d1 ); + result.fraction1 = s1; + result.point2 = b3MulAdd( p2, s2, d2 ); + result.fraction2 = s2; + return result; +} + +b3SegmentDistanceResult b3SegmentDistance( b3Vec3 p1, b3Vec3 q1, b3Vec3 p2, b3Vec3 q2 ) +{ + b3SegmentDistanceResult result; + + b3Vec3 d1 = b3Sub( q1, p1 ); + b3Vec3 d2 = b3Sub( q2, p2 ); + b3Vec3 r = b3Sub( p1, p2 ); + + float a = b3Dot( d1, d1 ); + float b = b3Dot( d1, d2 ); + float c = b3Dot( d1, r ); + float e = b3Dot( d2, d2 ); + float f = b3Dot( d2, r ); + + // Check if one of the segments degenerates into a point + if ( a < 100.0f * FLT_EPSILON && e < 100.0f * FLT_EPSILON ) + { + // Both segments degenerate into points + result.point1 = p1; + result.fraction1 = 0.0f; + result.point2 = p2; + result.fraction2 = 0.0f; + + return result; + } + + if ( a < 100.0f * FLT_EPSILON ) + { + // First segment degenerates into a point + float s2 = b3ClampFloat( f / e, 0.0f, 1.0f ); + + result.point1 = p1; + result.fraction1 = 0.0f; + result.point2 = b3MulAdd( p2, s2, d2 ); + result.fraction2 = s2; + + return result; + } + + if ( e < 100.0f * FLT_EPSILON ) + { + // Second segment degenerates into a point + float s1 = b3ClampFloat( -c / a, 0.0f, 1.0f ); + + result.point1 = b3MulAdd( p1, s1, d1 ); + result.fraction1 = s1; + result.point2 = p2; + result.fraction2 = 0.0f; + + return result; + } + + // Non-degenerate case + float denom = a * e - b * b; + float s1 = denom > 1000.0f * FLT_MIN ? b3ClampFloat( ( b * f - c * e ) / denom, 0.0f, 1.0f ) : 0.0f; + float s2 = ( b * s1 + f ) / e; + + // Clamp lambda2 and recompute lambda1 if necessary + if ( s2 < 0.0f ) + { + s1 = b3ClampFloat( -c / a, 0.0f, 1.0f ); + s2 = 0.0f; + } + else if ( s2 > 1.0f ) + { + s1 = b3ClampFloat( ( b - c ) / a, 0.0f, 1.0f ); + s2 = 1.0f; + } + + result.point1 = b3MulAdd( p1, s1, d1 ); + result.fraction1 = s1; + result.point2 = b3MulAdd( p2, s2, d2 ); + result.fraction2 = s2; + + return result; +} + +b3Vec3 b3PointToSegmentDistance( b3Vec3 a, b3Vec3 b, b3Vec3 q ) +{ + b3Vec3 ab = b3Sub( b, a ); + b3Vec3 aq = b3Sub( q, a ); + + float alpha = b3Dot( ab, aq ); + + if ( alpha <= 0.0f ) + { + // q projects outside interval [a, b] on the side of a + return a; + } + else + { + float denominator = b3Dot( ab, ab ); + if ( alpha > denominator ) + { + // q projects outside interval [a, b] on the side of b + return b; + } + else + { + // q projects inside interval [a, b] + alpha /= denominator; + return b3MulAdd( a, alpha, ab ); + } + } +} + +b3TrianglePoint b3ClosestPointOnTriangle( b3Vec3 a, b3Vec3 b, b3Vec3 c, b3Vec3 q ) +{ + // Check if P lies in vertex region of A + b3Vec3 ab = b3Sub( b, a ); + b3Vec3 ac = b3Sub( c, a ); + b3Vec3 aq = b3Sub( q, a ); + + float d1 = b3Dot( ab, aq ); + float d2 = b3Dot( ac, aq ); + if ( d1 <= 0.0f && d2 <= 0.0f ) + { + return (b3TrianglePoint){ a, b3_featureVertex1 }; + } + + // Check if P lies in vertex region of B + b3Vec3 bq = b3Sub( q, b ); + + float d3 = b3Dot( ab, bq ); + float d4 = b3Dot( ac, bq ); + if ( d3 > 0.0f && d4 <= d3 ) + { + return (b3TrianglePoint){ b, b3_featureVertex2 }; + } + + // Check if P lies in edge region AB + float vc = d1 * d4 - d3 * d2; + if ( vc <= 0.0f && d1 >= 0.0f && d3 <= 0.0f ) + { + float t = d1 / ( d1 - d3 ); + return (b3TrianglePoint){ b3MulAdd( a, t, ab ), b3_featureEdge1 }; + } + + // Check if P lies in vertex region of C + b3Vec3 cq = b3Sub( q, c ); + + float d5 = b3Dot( ab, cq ); + float d6 = b3Dot( ac, cq ); + if ( d6 >= 0.0f && d5 <= d6 ) + { + return (b3TrianglePoint){ c, b3_featureVertex3 }; + } + + // Check if P lies in edge region AC + float vb = d5 * d2 - d1 * d6; + if ( vb <= 0.0f && d2 >= 0.0f && d6 <= 0.0f ) + { + float t = d2 / ( d2 - d6 ); + return (b3TrianglePoint){ b3MulAdd( a, t, ac ), b3_featureEdge3 }; + } + + // Check if P lies in edge region of BC + float va = d3 * d6 - d5 * d4; + if ( va <= 0.0f && d4 >= d3 && d5 >= d6 ) + { + b3Vec3 bc = b3Sub( c, b ); + + float t = ( d4 - d3 ) / ( ( d4 - d3 ) + ( d5 - d6 ) ); + return (b3TrianglePoint){ b3MulAdd( b, t, bc ), b3_featureEdge2 }; + } + + // P inside face region ABC + float t1 = vb / ( va + vb + vc ); + float t2 = vc / ( va + vb + vc ); + + b3Vec3 p = b3MulAdd( a, t1, ab ); + p = b3MulAdd( p, t2, ac ); + return (b3TrianglePoint){ p, b3_featureTriangleFace }; +} + +b3Matrix3 b3SphereInertia( float mass, float radius ) +{ + float i = 0.4f * mass * radius * radius; + return b3MakeDiagonalMatrix( i, i, i ); +} + +b3Matrix3 b3CylinderInertia( float mass, float radius, float height ) +{ + float ixx = mass * ( 3 * radius * radius + height * height ) / 12.0f; + float iyy = 0.5f * mass * radius * radius; + return b3MakeDiagonalMatrix( ixx, iyy, ixx ); +} + +b3Matrix3 b3BoxInertia( float mass, b3Vec3 min, b3Vec3 max ) +{ + b3Vec3 delta = b3Sub( max, min ); + float ixx = mass * ( delta.y * delta.y + delta.z * delta.z ) / 12.0f; + float iyy = mass * ( delta.x * delta.x + delta.z * delta.z ) / 12.0f; + float izz = mass * ( delta.x * delta.x + delta.y * delta.y ) / 12.0f; + + return b3MakeDiagonalMatrix( ixx, iyy, izz ); +} + +// https://en.wikipedia.org/wiki/Parallel_axis_theorem +b3Matrix3 b3Steiner( float mass, b3Vec3 origin ) +{ + // Usage: Io = Ic + Is and Ic = Io - Is + float ixx = mass * ( origin.y * origin.y + origin.z * origin.z ); + float iyy = mass * ( origin.x * origin.x + origin.z * origin.z ); + float izz = mass * ( origin.x * origin.x + origin.y * origin.y ); + float ixy = -mass * origin.x * origin.y; + float ixz = -mass * origin.x * origin.z; + float iyz = -mass * origin.y * origin.z; + + // Write + b3Matrix3 out; + out.cx.x = ixx; + out.cy.x = ixy; + out.cz.x = ixz; + out.cx.y = ixy; + out.cy.y = iyy; + out.cz.y = iyz; + out.cx.z = ixz; + out.cy.z = iyz; + out.cz.z = izz; + + return out; +} + +bool b3IsValidRay( const b3RayCastInput* input ) +{ + bool isValid = b3IsValidVec3( input->origin ) && b3IsValidVec3( input->translation ) && + b3IsValidFloat( input->maxFraction ) && 0.0f <= input->maxFraction && input->maxFraction < B3_HUGE; + return isValid; +} diff --git a/vendor/box3d/src/src/math_internal.h b/vendor/box3d/src/src/math_internal.h new file mode 100644 index 000000000..4d152c5fc --- /dev/null +++ b/vendor/box3d/src/src/math_internal.h @@ -0,0 +1,480 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +#pragma once + +#include "core.h" + +#include "box3d/collision.h" +#include "box3d/math_functions.h" + +struct b3Sweep; +struct b3Plane; + +#define B3_TWO_PI 6.283185307f +#define B3_PI_OVER_TWO 1.570796327f +#define B3_PI_OVER_FOUR 0.785398163f +#define B3_SQRT3 1.732050808f + +// todo eliminate this +static const b3AABB B3_BOUNDS3_EMPTY = { { FLT_MAX, FLT_MAX, FLT_MAX }, { -FLT_MAX, -FLT_MAX, -FLT_MAX } }; + +typedef struct b3Matrix2 +{ + b3Vec2 cx, cy; +} b3Matrix2; + +typedef struct b3Triangle +{ + b3Vec3 vertices[3]; + int i1, i2, i3; + int flags; +} b3Triangle; + +typedef struct b3TrianglePoint +{ + b3Vec3 point; + b3TriangleFeature feature; +} b3TrianglePoint; + +typedef struct b3ShapeExtent +{ + float minExtent; + b3Vec3 maxExtent; +} b3ShapeExtent; + +b3TrianglePoint b3ClosestPointOnTriangle( b3Vec3 a, b3Vec3 b, b3Vec3 c, b3Vec3 q ); + +float b3IntersectSegmentTriangle( b3Vec3 p, b3Vec3 q, b3Vec3 a, b3Vec3 b, b3Vec3 c ); +float b3IntersectSegmentSphere( b3Vec3 p, b3Vec3 q, b3Vec3 c, float r ); + +b3MassData b3ComputeMassProperties( int triangleCount, const int* triangles, int vertexCount, const b3Vec3* vertices, + float density ); + +bool b3IsValidMassData( const b3MassData* massData ); + +b3Matrix3 b3SphereInertia( float mass, float radius ); +b3Matrix3 b3CylinderInertia( float mass, float radius, float height ); +b3Matrix3 b3BoxInertia( float mass, b3Vec3 min, b3Vec3 max ); + +// Inertia helper (Io = Ic + Is and Ic = Io - Is) +int b3GetProxySupport( const b3ShapeProxy* proxy, b3Vec3 axis ); +int b3GetPointSupport( const b3Vec3* points, int count, b3Vec3 axis ); + +static inline size_t b3AlignUp8( size_t x ) +{ + return ( x + 7u ) & ~(size_t)7u; +} + +// https://en.wikipedia.org/wiki/Floor_and_ceiling_functions +static inline int b3CeilingInt( int numerator, int denominator ) +{ + B3_VALIDATE( denominator > 0 ); + return ( numerator + denominator - 1 ) / denominator; +} + +// Assumes denominator == 2^exponent +static inline int b3CeilingPow2( int numerator, int denominator, int exponent ) +{ + B3_VALIDATE( exponent > 0 && ( denominator == 1 << exponent ) ); + return ( numerator + denominator - 1 ) >> exponent; +} + +bool b3IsSweepNormalized( b3Sweep* sweep ); + +static inline float b3Dot2( b3Vec2 v1, b3Vec2 v2 ) +{ + return v1.x * v2.x + v1.y * v2.y; +} + +static inline float b3Length2( b3Vec2 v ) +{ + return sqrtf( b3Dot2( v, v ) ); +} + +static inline float b3LengthSquared2( b3Vec2 v ) +{ + return b3Dot2( v, v ); +} + +static inline b3Vec2 b3MinVec2( b3Vec2 v1, b3Vec2 v2 ) +{ + b3Vec2 v; + v.x = b3MinFloat( v1.x, v2.x ); + v.y = b3MinFloat( v1.y, v2.y ); + return v; +} + +static inline b3Vec2 b3MaxVec2( b3Vec2 v1, b3Vec2 v2 ) +{ + b3Vec2 v; + v.x = b3MaxFloat( v1.x, v2.x ); + v.y = b3MaxFloat( v1.y, v2.y ); + return v; +} + +static inline void b3Store( float* dst, b3Vec3 src ) +{ + dst[0] = src.x; + dst[1] = src.y; + dst[2] = src.z; +} + +static inline b3Vec3 b3ClampLength( b3Vec3 v, float maxLength ) +{ + float lengthSq = b3LengthSquared( v ); + if ( lengthSq <= maxLength * maxLength ) + { + return v; + } + + float length = sqrtf( lengthSq ); + return b3MulSV( maxLength / length, v ); +} + +// Assume v is a unit vector +static inline b3Vec3 b3ArbitraryPerp( b3Vec3 v ) +{ + // Suppose vector a has all equal components and is a unit vector: a = (s, s, s) + // Then 3*s*s = 1, s = sqrt(1/3) = 0.57735. This means that at least one component + // of a unit vector must be greater or equal to 0.57735. + b3Vec3 p; + if ( v.x < -0.5f || 0.5f < v.x ) + { + // x is non-zero and it should not go into the x component + // dot([ay + bz, cx, dx], [x, y, z]) = ayx + bzx + cxy + dzx + // for the dot product to be zero need: c = -a, d = -b + float a = 0.67f; + float b = -0.42f; + p = B3_LITERAL( b3Vec3 ){ a * v.y + b * v.z, -a * v.x, -b * v.x }; + } + else if ( v.y < -0.5f || 0.5f < v.y ) + { + // y is non-zero and it should not go into the y component + // p = [ay, bx + cz, dy] + // axy + bxy + cyz + dyz = 0 + // b = -a, d = -c + float a = 0.67f; + float c = -0.42f; + p = B3_LITERAL( b3Vec3 ){ a * v.y, -a * v.x + c * v.z, -c * v.y }; + } + else + { + // This would trip if the input is not a unit vector + B3_VALIDATE( v.z < -0.5f || 0.5f < v.z ); + + // z is non-zero and it should not go into the z component + // p = [az, bz, cx + dy] + // axz + byz + cxz + dyz = 0 + // c = -a, d = -b + float a = 0.67f; + float b = -0.42f; + p = B3_LITERAL( b3Vec3 ){ a * v.z, b * v.z, -a * v.x - b * v.y }; + } + + B3_VALIDATE( b3LengthSquared( p ) > 0.1f ); + B3_VALIDATE( b3AbsFloat( b3Dot( p, v ) ) < 100.0f * FLT_EPSILON ); + + return b3Normalize( p ); +} + +static inline b3Quat b3QuatFromExponentialMap( b3Vec3 v ) +{ + // Exponential map (Grassia) + float threshold = 0.018581361f; + + float angle = b3Length( v ); + if ( angle < threshold ) + { + // Taylor expansion + b3Quat out; + out.v = b3MulSV( 0.5f + angle * angle / 48.0f, v ); + out.s = b3Cos( 0.5f * angle ); + + return out; + } + + return b3MakeQuatFromAxisAngle( b3MulSV( 1.0f / angle, v ), angle ); +} + +/// Integrate rotation from angular velocity +/// @param q1 initial rotation +/// @param deltaRotation the angular displacement vector in radians (angular velocity multiplied by the time step) +/// q2 = q1 + 0.5 * omega * q1 +static inline b3Quat b3IntegrateRotation( b3Quat q1, b3Vec3 deltaRotation ) +{ +#if 1 + // https://fgiesen.wordpress.com/2012/08/24/quaternion-differentiation/ + b3Quat qd = { b3MulSV( 0.5f, deltaRotation ), 0.0f }; + qd = b3MulQuat( qd, q1 ); + b3Quat q2 = { b3Add( q1.v, qd.v ), qd.s + q1.s }; + q2 = b3NormalizeQuat( q2 ); + return q2; +#else + return b3NormalizeQuat( b3MulQuat(b3QuatFromExponentialMap( deltaRotation ), q1) ); +#endif +} + +// Pseudo angular velocity from a quaternion target +// w = 2 * (target - q) * conj(q) +static inline b3Vec3 b3DeltaQuatToRotation( b3Quat q, b3Quat target ) +{ + b3Quat s = q; + if ( b3DotQuat( q, target ) < 0.0f ) + { + // Correct polarity + s = b3NegateQuat( q ); + } + + b3Quat diff = { b3Sub( target.v, s.v ), target.s - s.s }; + b3Quat product = b3MulQuat( diff, b3Conjugate( s ) ); + return b3MulSV( 2.0f, product.v ); +} + +static inline float b3ScalarTripleProduct( b3Vec3 a, b3Vec3 b, b3Vec3 c ) +{ + b3Vec3 d; + d.x = b.y * c.z - b.z * c.y; + d.y = b.z * c.x - b.x * c.z; + d.z = b.x * c.y - b.y * c.x; + return a.x * d.x + a.y * d.y + a.z * d.z; +} + +// Get a value by index. Avoid undefined behavior of code like (&v.x)[2]. +static inline float b3GetByIndex( b3Vec3 v, int index ) +{ + B3_VALIDATE( 0 <= index && index < 3 ); + float temp[3] = { v.x, v.y, v.z }; + return temp[index]; +} + +static inline int b3MajorAxis( b3Vec3 v ) +{ + return v.x < v.y ? ( v.y < v.z ? 2 : 1 ) : ( v.x < v.z ? 2 : 0 ); +} + +static inline float b3MinElement( b3Vec3 v ) +{ + return b3MinFloat( v.x, b3MinFloat( v.y, v.z ) ); +} + +static inline float b3MaxElement( b3Vec3 v ) +{ + return b3MaxFloat( v.x, b3MaxFloat( v.y, v.z ) ); +} + +static inline int b3MaxElementIndex( b3Vec3 v ) +{ + return v.x < v.y ? ( v.y < v.z ? 2 : 1 ) : ( v.x < v.z ? 2 : 0 ); +} + +static inline b3Vec2 b3Add2( b3Vec2 a, b3Vec2 b ) +{ + b3Vec2 c = { a.x + b.x, a.y + b.y }; + return c; +} + +static inline b3Vec2 b3Sub2( b3Vec2 a, b3Vec2 b ) +{ + b3Vec2 c = { a.x - b.x, a.y - b.y }; + return c; +} + +static inline b3Vec2 b3Neg2( b3Vec2 v ) +{ + b3Vec2 c = { -v.x, -v.y }; + return c; +} + +static inline b3Vec2 b3MulSV2( float s, b3Vec2 v ) +{ + b3Vec2 c = { s * v.x, s * v.y }; + return c; +} + +// a + s * b +static inline b3Vec2 b3MulAdd2( b3Vec2 a, float s, b3Vec2 b ) +{ + b3Vec2 c = { a.x + s * b.x, a.y + s * b.y }; + return c; +} + +// a - s * b +static inline b3Vec2 b3MulSub2( b3Vec2 a, float s, b3Vec2 b ) +{ + b3Vec2 c = { a.x - s * b.x, a.y - s * b.y }; + return c; +} + +static inline float b3Cross2( b3Vec2 a, b3Vec2 b ) +{ + return a.x * b.y - a.y * b.x; +} + +static inline float b3DistanceSquared2( b3Vec2 a, b3Vec2 b ) +{ + float dx = b.x - a.x; + float dy = b.y - a.y; + return dx * dx + dy * dy; +} + +static inline b3Vec2 b3MulMV2( b3Matrix2 m, b3Vec2 a ) +{ + b3Vec2 b = { m.cx.x * a.x + m.cy.x * a.y, m.cx.y * a.x + m.cy.y * a.y }; + return b; +} + +static inline b3Matrix2 b3MulMM2( b3Matrix2 m1, b3Matrix2 m2 ) +{ + b3Matrix2 out; + out.cx = b3MulMV2( m1, m2.cx ); + out.cy = b3MulMV2( m1, m2.cy ); + return out; +} + +static inline float b3Det2( b3Matrix2 m ) +{ + return m.cx.x * m.cy.y - m.cx.y * m.cy.x; +} + +static inline b3Matrix2 b3Invert2( b3Matrix2 m ) +{ + float det = b3Det2( m ); + if ( b3AbsFloat( det ) > 1000.0f * FLT_MIN ) + { + float invDet = 1.0f / det; + return B3_LITERAL( b3Matrix2 ){ + { invDet * m.cy.y, -invDet * m.cx.y }, + { -invDet * m.cy.x, invDet * m.cx.x }, + }; + } + + return B3_LITERAL( b3Matrix2 ){ { 0.0f, 0.0f }, { 0.0f, 0.0f } }; +} + +// Assumes positive semi-definite +static inline b3Vec2 b3Solve2( b3Matrix2 m, b3Vec2 b ) +{ + float det = b3Det2( m ); + if ( det > 1000.0f * FLT_MIN ) + { + float invDet = 1.0f / det; + return B3_LITERAL( b3Vec2 ){ + invDet * m.cy.y * b.x - invDet * m.cy.x * b.y, + -invDet * m.cx.y * b.x + invDet * m.cx.x * b.y, + }; + } + + return B3_LITERAL( b3Vec2 ){ 0.0f, 0.0f }; +} + +// Convenience function: s * a + t * b + u * c +static inline b3Vec3 b3Blend3( float s, b3Vec3 a, float t, b3Vec3 b, float u, b3Vec3 c ) +{ + b3Vec3 d = { + s * a.x + t * b.x + u * c.x, + s * a.y + t * b.y + u * c.y, + s * a.z + t * b.z + u * c.z, + }; + return d; +} + +static inline b3Vec3 b3ModifiedCross( b3Vec3 a, b3Vec3 b ) +{ + b3Vec3 c; + c.x = a.y * b.z + a.z * b.y; + c.y = a.z * b.x + a.x * b.z; + c.z = a.x * b.y + a.y * b.x; + return c; +} + +static inline b3Matrix3 b3MakeDiagonalMatrix( float a, float b, float c ) +{ + return (b3Matrix3){ { a, 0.0f, 0.0f }, { 0.0f, b, 0.0f }, { 0.0f, 0.0f, c } }; +} + +static inline b3Matrix3 b3Skew( b3Vec3 v ) +{ + b3Matrix3 out; + out.cx = (b3Vec3){ 0, v.z, -v.y }; + out.cy = (b3Vec3){ -v.z, 0, v.x }; + out.cz = (b3Vec3){ v.y, -v.x, 0 }; + + return out; +} + +static inline b3Plane b3NormalizePlane( b3Plane plane ) +{ + float invLength = 1.0f / b3Length( plane.normal ); + return (b3Plane){ b3MulSV( invLength, plane.normal ), invLength * plane.offset }; +} + +static inline b3Plane b3MakePlaneFromNormalAndPoint( b3Vec3 normal, b3Vec3 point ) +{ + return (b3Plane){ normal, b3Dot( normal, point ) }; +} + +static inline b3Plane b3MakePlaneFromPoints( b3Vec3 point1, b3Vec3 point2, b3Vec3 point3 ) +{ + b3Plane plane; + plane.normal = b3Cross( b3Sub( point2, point1 ), b3Sub( point3, point1 ) ); + plane.normal = b3Normalize( plane.normal ); + plane.offset = b3Dot( plane.normal, point1 ); + return plane; +} + +static inline b3Vec3 b3MakeNormalFromPoints( b3Vec3 point1, b3Vec3 point2, b3Vec3 point3 ) +{ + b3Vec3 normal = b3Cross( b3Sub( point2, point1 ), b3Sub( point3, point1 ) ); + return b3Normalize( normal ); +} + +// normal2 = q * normal1 +// offset2 = dot(normal2, p) + offset1 +static inline b3Plane b3TransformPlane( b3Transform transform, b3Plane plane ) +{ + b3Vec3 normal = b3RotateVector( transform.q, plane.normal ); + return B3_LITERAL( b3Plane ){ normal, plane.offset + b3Dot( normal, transform.p ) }; +} + +/// Signed separation of a point from a plane +static inline float b3PlaneSeparation( b3Plane plane, b3Vec3 point ) +{ + return b3Dot( plane.normal, point ) - plane.offset; +} + +// Negative if p is below the triangle v1-v2-v3 +static inline float b3SignedVolume( b3Vec3 v1, b3Vec3 v2, b3Vec3 v3, b3Vec3 p ) +{ + b3Vec3 e1 = b3Sub( v2, v1 ); + b3Vec3 e2 = b3Sub( v3, v1 ); + b3Vec3 n = b3Cross( e1, e2 ); + return b3Dot( n, b3Sub( p, v1 ) ); +} + +// todo eliminate this +static inline bool b3IsWithinSegments( const b3SegmentDistanceResult* result ) +{ + return ( 0.0f <= result->fraction1 && result->fraction1 <= 1.0f ) && + ( 0.0f <= result->fraction2 && result->fraction2 <= 1.0f ); +} + +static inline b3Matrix3 b3RotateInertia( b3Quat q, b3Matrix3 centralInertia ) +{ + b3Matrix3 rotationMatrix = b3MakeMatrixFromQuat( q ); + b3Matrix3 inertia = b3MulMM( rotationMatrix, b3MulMM( centralInertia, b3Transpose( rotationMatrix ) ) ); + return inertia; +} + +static inline b3Matrix3 b3TransformInertia( b3Transform transform, b3Matrix3 centralInertia, float mass ) +{ + b3Matrix3 inertia = b3RotateInertia( transform.q, centralInertia ); + inertia = b3AddMM( inertia, b3Steiner( mass, transform.p ) ); + return inertia; +} + +// Add a point to an AABB. +static inline b3AABB b3AABB_AddPoint( b3AABB a, b3Vec3 point ) +{ + return (b3AABB){ b3Min( a.lowerBound, point ), b3Max( a.upperBound, point ) }; +} diff --git a/vendor/box3d/src/src/mesh.c b/vendor/box3d/src/src/mesh.c new file mode 100644 index 000000000..c03489955 --- /dev/null +++ b/vendor/box3d/src/src/mesh.c @@ -0,0 +1,2411 @@ +// SPDX-FileCopyrightText: 2026 Erin Catto +// SPDX-License-Identifier: MIT + +// Dirk Gregorius contributed portions of this code + +#include "container.h" +#include "math_internal.h" +#include "shape.h" +#include "simd.h" + +#include "box3d/collision.h" +#include "box3d/constants.h" + +#include + +b3DeclareArray( b3VertexNode ); +b3DeclareArray( b3MeshNode ); +b3DeclareArray( b3MeshTriangle ); +b3DeclareArray( b3Vec3 ); +b3DeclareArray( b3Primitive ); +b3DeclareArrayNative( uint8_t ); + +#define B3_BIN_COUNT 8 +#define B3_DESIRED_TRIANGLES_PER_LEAF 4 +#define B3_LEAF_NODE 3 +#define B3_MAXIMUM_TRIANGLES_PER_LEAF 8 +#define B3_MESH_STACK_SIZE 256 + +static bool b3IsLeaf( const b3MeshNode* node ) +{ + return node->data.asLeaf.type == B3_LEAF_NODE; +} + +static b3MeshNode* b3GetMeshNodesWrite( b3MeshData* mesh ) +{ + if ( mesh->nodeOffset == 0 ) + { + return NULL; + } + + return (b3MeshNode*)( (intptr_t)mesh + mesh->nodeOffset ); +} + +static b3MeshNode* b3GetLeftChildWrite( b3MeshNode* node ) +{ + // The left child follows its parent. + B3_ASSERT( !b3IsLeaf( node ) ); + return node + 1; +} + +static const b3MeshNode* b3GetLeftChild( const b3MeshNode* node ) +{ + // The left child follows its parent. + B3_ASSERT( !b3IsLeaf( node ) ); + return node + 1; +} + +static b3MeshNode* b3GetRightChildWrite( b3MeshNode* node ) +{ + // We store the offset of the right child relative to its parent + B3_ASSERT( !b3IsLeaf( node ) ); + return node + node->data.asNode.childOffset; +} + +static const b3MeshNode* b3GetRightChild( const b3MeshNode* node ) +{ + // We store the offset of the right child relative to its parent + B3_ASSERT( !b3IsLeaf( node ) ); + return node + node->data.asNode.childOffset; +} + +static const b3MeshNode* b3GetRoot( const b3MeshData* mesh ) +{ + // The first node is the root + return b3GetMeshNodes( mesh ); +} + +static b3MeshNode* b3GetRootWrite( b3MeshData* mesh ) +{ + // The first node is the root + return b3GetMeshNodesWrite( mesh ); +} + +static b3MeshTriangle* b3GetMeshTrianglesWrite( b3MeshData* mesh ) +{ + if ( mesh->triangleOffset == 0 ) + { + return NULL; + } + + return (b3MeshTriangle*)( (intptr_t)mesh + mesh->triangleOffset ); +} + +static b3Vec3* b3GetMeshVerticesWrite( b3MeshData* mesh ) +{ + if ( mesh->vertexOffset == 0 ) + { + return NULL; + } + + return (b3Vec3*)( (intptr_t)mesh + mesh->vertexOffset ); +} + +// static b3Vec3 b3GetVertex( b3MeshData& mesh, int vertexIndex ) +//{ +// B3_ASSERT( 0 <= vertexIndex && vertexIndex < mesh.vertexCount ); +// b3Vec3* vertices = b3GetMeshVertices( &mesh ); +// return vertices[vertexIndex]; +// } + +static uint8_t* b3GetMeshMaterialIndicesWrite( b3MeshData* mesh ) +{ + if ( mesh->materialOffset == 0 ) + { + return NULL; + } + + return (uint8_t*)( (intptr_t)mesh + mesh->materialOffset ); +} + +static uint8_t* b3GetMeshFlagsWrite( b3MeshData* mesh ) +{ + if ( mesh->flagsOffset == 0 ) + { + return NULL; + } + + return (uint8_t*)( (intptr_t)mesh + mesh->flagsOffset ); +} + +static int b3GetNodeHeight( const b3MeshNode* node ) +{ + if ( b3IsLeaf( node ) ) + { + return 0; + } + + const b3MeshNode* leftChild = b3GetLeftChild( node ); + int leftHeight = b3GetNodeHeight( leftChild ); + const b3MeshNode* rightChild = b3GetRightChild( node ); + int rightHeight = b3GetNodeHeight( rightChild ); + + return 1 + b3MaxInt( leftHeight, rightHeight ); +} + +int b3GetHeight( const b3MeshData* mesh ) +{ + const b3MeshNode* root = b3GetRoot( mesh ); + if ( root == NULL ) + { + return 0; + } + + return b3GetNodeHeight( root ); +} + +#if B3_ENABLE_VALIDATION == 1 +static bool b3IsDegenerate( b3Vec3 v1, b3Vec3 v2, b3Vec3 v3, float minArea ) +{ + b3Vec3 normal = b3Cross( b3Sub( v2, v1 ), b3Sub( v3, v1 ) ); + float lengthSq = b3LengthSquared( normal ); + return lengthSq < minArea * minArea; +} + +static bool b3IsNonDegenerate( const b3MeshData* mesh, float minArea ) +{ + const b3MeshTriangle* triangles = b3GetMeshTriangles( mesh ); + const b3Vec3* vertices = b3GetMeshVertices( mesh ); + + // Check triangles + for ( int index = 0; index < mesh->triangleCount; ++index ) + { + // Index range + b3MeshTriangle triangle = triangles[index]; + if ( triangle.index1 >= mesh->vertexCount ) + { + return false; + } + + if ( triangle.index2 >= mesh->vertexCount ) + { + return false; + } + + if ( triangle.index3 >= mesh->vertexCount ) + { + return false; + } + + // Degenerate topology + if ( triangle.index1 == triangle.index2 ) + { + return false; + } + if ( triangle.index1 == triangle.index3 ) + { + return false; + } + if ( triangle.index2 == triangle.index3 ) + { + return false; + } + + // Degenerate geometry + b3Vec3 vertex1 = vertices[triangle.index1]; + b3Vec3 vertex2 = vertices[triangle.index2]; + b3Vec3 vertex3 = vertices[triangle.index3]; + if ( b3IsDegenerate( vertex1, vertex2, vertex3, minArea ) ) + { + return false; + } + } + + return true; +} + +static inline b3AABB b3GetNodeAABB( const b3MeshNode* node ) +{ + return (b3AABB){ + node->lowerBound, + node->upperBound, + }; +} + +static bool b3IsConsistent( const b3MeshData* mesh ) +{ + const b3MeshTriangle* triangles = b3GetMeshTriangles( mesh ); + const b3Vec3* vertices = b3GetMeshVertices( mesh ); + + // Check nodes + int count = 0; + const b3MeshNode* stack[64]; + stack[count++] = b3GetRoot( mesh ); + + while ( count > 0 ) + { + const b3MeshNode* node = stack[--count]; + b3AABB nodeBounds = b3GetNodeAABB( node ); + + if ( b3IsLeaf( node ) == false ) + { + const b3MeshNode* child1 = b3GetLeftChild( node ); + b3AABB bounds1 = b3GetNodeAABB( child1 ); + const b3MeshNode* child2 = b3GetRightChild( node ); + b3AABB bounds2 = b3GetNodeAABB( child2 ); + + if ( !b3AABB_Contains( nodeBounds, bounds1 ) ) + { + return false; + } + + if ( !b3AABB_Contains( nodeBounds, bounds2 ) ) + { + return false; + } + + stack[count++] = child2; + stack[count++] = child1; + } + else + { + b3AABB triangleBounds = B3_BOUNDS3_EMPTY; + for ( uint32_t index = 0; index < node->data.asLeaf.triangleCount; ++index ) + { + int triangleIndex = node->triangleOffset + index; + B3_ASSERT( 0 <= triangleIndex && triangleIndex < mesh->triangleCount ); + + b3MeshTriangle triangle = triangles[triangleIndex]; + + b3AABB vertexBounds = B3_BOUNDS3_EMPTY; + vertexBounds = b3AABB_AddPoint( vertexBounds, vertices[triangle.index1] ); + vertexBounds = b3AABB_AddPoint( vertexBounds, vertices[triangle.index2] ); + vertexBounds = b3AABB_AddPoint( vertexBounds, vertices[triangle.index3] ); + + triangleBounds = b3AABB_Union( triangleBounds, vertexBounds ); + } + + if ( !b3AABB_Contains( nodeBounds, triangleBounds ) ) + { + return false; + } + } + } + + return true; +} + +bool b3IsValidMesh( const b3MeshData* meshData ) +{ + if ( meshData == NULL ) + { + return false; + } + + if ( meshData->version != B3_MESH_VERSION ) + { + return false; + } + + if ( meshData->byteCount < (int)sizeof( b3MeshData ) ) + { + return false; + } + + return b3IsConsistent( meshData ); +} + +#else + +bool b3IsValidMesh( const b3MeshData* meshData ) +{ + if ( meshData == NULL ) + { + return false; + } + + if ( meshData->version != B3_MESH_VERSION ) + { + return false; + } + + if ( meshData->byteCount < (int)sizeof( b3MeshData ) ) + { + return false; + } + + return true; +} + +#endif + +// Node for a vertex linked list +typedef struct b3VertexNode +{ + int32_t vertexIndex; + int nextNodeIndex; +} b3VertexNode; + +#define NAME b3VertexMap +#define KEY_TY uint64_t +#define VAL_TY int +#define HASH_FN vt_hash_integer +#define CMPR_FN vt_cmpr_integer +#define MALLOC_FN b3Alloc +#define FREE_FN b3Free +#include "verstable.h" + +typedef struct b3SpatialHash +{ + b3Array( b3VertexNode ) nodes; + const b3Vec3* vertices; + int vertexCount; + b3VertexMap vertexMap; + float cellSize; + float tolerance; +} b3SpatialHash; + +static void b3SpatialHash_Create( b3SpatialHash* h, const b3Vec3* vertices, int vertexCount, float tolerance ) +{ + h->vertices = vertices; + h->vertexCount = vertexCount; + h->tolerance = tolerance; + h->cellSize = 2.0f * tolerance; + b3Array_CreateN( h->nodes, vertexCount ); + + b3VertexMap_init( &h->vertexMap ); + b3VertexMap_reserve( &h->vertexMap, vertexCount ); + + B3_ASSERT( h->cellSize > 0.0f ); +} + +static void b3SpatialHash_Destroy( b3SpatialHash* h ) +{ + b3VertexMap_cleanup( &h->vertexMap ); + b3Array_Destroy( h->nodes ); +} + +// Welding works by bucketing nearby vertices into identical keys in a hash table. +// Bucketing is done manually with an array. +static int32_t b3SpatialHash_FindDuplicate( b3SpatialHash* h, int32_t currentIndex ) +{ + B3_ASSERT( currentIndex < h->vertexCount ); + b3Vec3 vertex = h->vertices[currentIndex]; + float cellSize = h->cellSize; + float tolerance = h->tolerance; + + // Get the grid coordinates for the current vertex + int32_t baseX = (int32_t)( floorf( vertex.x / cellSize ) ); + int32_t baseY = (int32_t)( floorf( vertex.y / cellSize ) ); + int32_t baseZ = (int32_t)( floorf( vertex.z / cellSize ) ); + + // Check the current cell and all 26 neighboring cells (3x3x3 - 1) + for ( int dx = -1; dx <= 1; ++dx ) + { + for ( int dy = -1; dy <= 1; ++dy ) + { + for ( int dz = -1; dz <= 1; ++dz ) + { + int32_t x = baseX + dx; + int32_t y = baseY + dy; + int32_t z = baseZ + dz; + + // Compute hash for this neighboring cell (this is the key in the map) + uint64_t key = 0; + key ^= (uint64_t)( x ) + 0x9e3779b9 + ( key << 6 ) + ( key >> 2 ); + key ^= (uint64_t)( y ) + 0x9e3779b9 + ( key << 6 ) + ( key >> 2 ); + key ^= (uint64_t)( z ) + 0x9e3779b9 + ( key << 6 ) + ( key >> 2 ); + + b3VertexMap_itr it = b3VertexMap_get( &h->vertexMap, key ); + if ( b3VertexMap_is_end( it ) == false ) + { + // Check all vertices in this key + int nodeIndex = it.data->val; + + while ( nodeIndex != B3_NULL_INDEX ) + { + b3VertexNode node = h->nodes.data[nodeIndex]; + + int32_t existingIndex = node.vertexIndex; + B3_ASSERT( existingIndex < currentIndex ); + B3_ASSERT( existingIndex < h->vertexCount ); + + b3Vec3 other = h->vertices[existingIndex]; + + // IsEqual inlined: check if vertices are within tolerance + if ( fabsf( vertex.x - other.x ) <= tolerance && fabsf( vertex.y - other.y ) <= tolerance && + fabsf( vertex.z - other.z ) <= tolerance ) + { + // Found duplicate + return existingIndex; + } + + nodeIndex = node.nextNodeIndex; + } + } + } + } + } + + // No duplicate found, add to hash table + uint64_t currentKey = 0; + currentKey ^= (uint64_t)( baseX ) + 0x9e3779b9 + ( currentKey << 6 ) + ( currentKey >> 2 ); + currentKey ^= (uint64_t)( baseY ) + 0x9e3779b9 + ( currentKey << 6 ) + ( currentKey >> 2 ); + currentKey ^= (uint64_t)( baseZ ) + 0x9e3779b9 + ( currentKey << 6 ) + ( currentKey >> 2 ); + + b3VertexMap_itr it = b3VertexMap_get( &h->vertexMap, currentKey ); + if ( b3VertexMap_is_end( it ) == false ) + { + int nodeIndex = it.data->val; + + b3VertexNode node = { + .vertexIndex = currentIndex, + .nextNodeIndex = nodeIndex, + }; + + it.data->val = h->nodes.count; + b3Array_Push( h->nodes, node ); + } + else + { + b3VertexNode node = { + .vertexIndex = currentIndex, + .nextNodeIndex = B3_NULL_INDEX, + }; + + b3VertexMap_insert( &h->vertexMap, currentKey, h->nodes.count ); + b3Array_Push( h->nodes, node ); + } + + // Not welded + return B3_NULL_INDEX; +} + +typedef struct b3WeldData +{ + const b3Vec3* srcVertices; + const int32_t* srcIndices; + + b3Vec3* dstVertices; + int32_t* dstIndices; + + int vertexCount; + int indexCount; +} b3WeldData; + +static int b3WeldVertices( b3WeldData* data, float tolerance ) +{ + int vertexCount = data->vertexCount; + int uniqueCount = 0; + + // Create spatial hash and find duplicates + b3SpatialHash spatialHash; + b3SpatialHash_Create( &spatialHash, data->srcVertices, vertexCount, tolerance ); + b3Array( int ) vertexMapping = { 0 }; + b3Array_Resize( vertexMapping, vertexCount ); + + for ( int i = 0; i < vertexCount; ++i ) + { + int32_t duplicateIndex = b3SpatialHash_FindDuplicate( &spatialHash, i ); + + if ( duplicateIndex == B3_NULL_INDEX ) + { + // New unique vertex + vertexMapping.data[i] = uniqueCount; + data->dstVertices[uniqueCount] = data->srcVertices[i]; + uniqueCount += 1; + } + else + { + // Found duplicate, map to existing vertex + vertexMapping.data[i] = vertexMapping.data[duplicateIndex]; + } + } + + // Update indices to reference the new vertex array + int indexCount = data->indexCount; + for ( int i = 0; i < indexCount; ++i ) + { + int srcIndex = data->srcIndices[i]; + B3_ASSERT( srcIndex < vertexCount ); + data->dstIndices[i] = vertexMapping.data[srcIndex]; + } + + b3SpatialHash_Destroy( &spatialHash ); + b3Array_Destroy( vertexMapping ); + + return uniqueCount; +} + +static inline void b3StoreLeaf( b3MeshNode* node, const b3AABB* aabb, int triangleCount, int triangleOffset ) +{ + node->data.asLeaf.type = B3_LEAF_NODE; + node->data.asLeaf.triangleCount = triangleCount; + node->triangleOffset = triangleOffset; + node->lowerBound = aabb->lowerBound; + node->upperBound = aabb->upperBound; +} + +typedef struct b3Primitive +{ + b3AABB aabb; + b3Vec3 center; + int triangleIndex; +} b3Primitive; + +typedef struct b3Bucket +{ + int count; + b3AABB bounds; +} b3Bucket; + +typedef struct b3Split +{ + b3AABB leftBounds; + b3AABB rightBounds; + int axis; + int index; +} b3Split; + +static b3Split b3SplitBinnedSah( int count, b3Primitive* primitives ) +{ + b3Split split; + split.axis = -1; + split.index = -1; + + // Compute bounds of primitive centroids and choose split axis + b3AABB bounds = { primitives[0].center, primitives[0].center }; + for ( int i = 1; i < count; ++i ) + { + bounds = b3AABB_AddPoint( bounds, primitives[i].center ); + } + + // Compute costs for splitting after each bucket and keep track of best split + // This is a small O(n^2) loop. This can be further optimized, but it is already + // very fast and is kept for simplicity right now. + int bestBucket = -1; + float bestCost = FLT_MAX; + + for ( int axis = 0; axis < 3; ++axis ) + { + b3Vec3 extent = b3AABB_Extents( bounds ); + if ( b3GetByIndex( extent, axis ) < B3_LINEAR_SLOP ) + { + continue; + } + + // Initialize buckets + b3Bucket buckets[B3_BIN_COUNT]; + for ( int i = 0; i < B3_BIN_COUNT; ++i ) + { + buckets[i].count = 0; + buckets[i].bounds = B3_BOUNDS3_EMPTY; + } + + // Fill buckets + float factor = B3_BIN_COUNT * ( 1.0f - FLT_EPSILON ) / + ( b3GetByIndex( bounds.upperBound, axis ) - b3GetByIndex( bounds.lowerBound, axis ) ); + for ( int i = 0; i < count; ++i ) + { + b3Vec3 center = primitives[i].center; + int index = (int)( factor * ( b3GetByIndex( center, axis ) - b3GetByIndex( bounds.lowerBound, axis ) ) ); + B3_ASSERT( 0 <= index && index < B3_BIN_COUNT ); + + buckets[index].count++; + buckets[index].bounds = b3AABB_Union( buckets[index].bounds, primitives[i].aabb ); + } + + // Evaluate splits + for ( int i = 0; i < B3_BIN_COUNT - 1; ++i ) + { + int leftCount = 0; + b3AABB leftBounds = B3_BOUNDS3_EMPTY; + for ( int k = 0; k <= i; ++k ) + { + leftCount += buckets[k].count; + leftBounds = b3AABB_Union( leftBounds, buckets[k].bounds ); + } + + int rightCount = 0; + b3AABB rightBounds = B3_BOUNDS3_EMPTY; + for ( int k = i + 1; k < B3_BIN_COUNT; ++k ) + { + rightCount += buckets[k].count; + rightBounds = b3AABB_Union( rightBounds, buckets[k].bounds ); + } + + B3_ASSERT( leftCount + rightCount == count ); + if ( leftCount > 0 && rightCount > 0 ) + { + float cost = leftCount * b3AABB_Area( leftBounds ) + rightCount * b3AABB_Area( rightBounds ); + + if ( cost < bestCost ) + { + bestBucket = i; + bestCost = cost; + + split.axis = axis; + split.index = leftCount; + split.leftBounds = leftBounds; + split.rightBounds = rightBounds; + } + } + } + } + + // Partition + if ( bestBucket >= 0 ) + { + int axis = split.axis; + float factor = B3_BIN_COUNT * ( 1.0f - FLT_EPSILON ) / + ( b3GetByIndex( bounds.upperBound, axis ) - b3GetByIndex( bounds.lowerBound, axis ) ); + + int splitIndex = 0; + for ( int i = 0; i < count; ++i ) + { + b3Vec3 center = primitives[i].center; + int index = (int)( factor * ( b3GetByIndex( center, axis ) - b3GetByIndex( bounds.lowerBound, axis ) ) ); + + if ( index <= bestBucket ) + { + b3Primitive temp = primitives[i]; + primitives[i] = primitives[splitIndex]; + primitives[splitIndex] = temp; + splitIndex++; + } + } + B3_ASSERT( splitIndex == split.index ); + } + + return split; +} + +static b3Split b3SplitHalf( int count, b3Primitive* primitives ) +{ + // Split in the middle + int splitIndex = count / 2; + + b3AABB leftBounds = B3_BOUNDS3_EMPTY; + for ( int i = 0; i < splitIndex; ++i ) + { + leftBounds = b3AABB_Union( leftBounds, primitives[i].aabb ); + } + + b3AABB rightBounds = B3_BOUNDS3_EMPTY; + for ( int i = splitIndex; i < count; ++i ) + { + + rightBounds = b3AABB_Union( rightBounds, primitives[i].aabb ); + } + + b3AABB bounds = b3AABB_Union( leftBounds, rightBounds ); + int axis = b3MajorAxis( b3AABB_Extents( bounds ) ); + + b3Split split; + split.axis = axis; + split.index = splitIndex; + split.leftBounds = leftBounds; + split.rightBounds = rightBounds; + + return split; +} + +static b3Split b3SplitMedian( int count, b3Primitive* primitives ) +{ + B3_ASSERT( count > 2 ); + + b3Vec3 lowerBound = primitives[0].center; + b3Vec3 upperBound = primitives[0].center; + + for ( int i = 1; i < count; ++i ) + { + lowerBound = b3Min( lowerBound, primitives[i].center ); + upperBound = b3Max( upperBound, primitives[i].center ); + } + + b3Vec3 d = b3Sub( upperBound, lowerBound ); + b3Vec3 c = b3MulSV( 0.5f, b3Add( lowerBound, upperBound ) ); + + b3Split split = { 0 }; + split.index = -1; + + // Partition longest axis using the Hoare partition scheme + // https://en.wikipedia.org/wiki/Quicksort + // https://nicholasvadivelu.com/2021/01/11/array-partition/ + int i1 = 0, i2 = count; + if ( d.x >= d.y && d.x >= d.z ) + { + split.axis = 0; + + float pivot = c.x; + + while ( i1 < i2 ) + { + while ( i1 < i2 && primitives[i1].center.x < pivot ) + { + i1 += 1; + }; + + while ( i1 < i2 && primitives[i2 - 1].center.x >= pivot ) + { + i2 -= 1; + }; + + if ( i1 < i2 ) + { + // Swap primitives + b3Primitive temp = primitives[i1]; + primitives[i1] = primitives[i2 - 1]; + primitives[i2 - 1] = temp; + + i1 += 1; + i2 -= 1; + } + } + } + else if ( d.y >= d.z ) + { + split.axis = 1; + + float pivot = c.y; + + while ( i1 < i2 ) + { + while ( i1 < i2 && primitives[i1].center.y < pivot ) + { + i1 += 1; + }; + + while ( i1 < i2 && primitives[i2 - 1].center.y >= pivot ) + { + i2 -= 1; + }; + + if ( i1 < i2 ) + { + // Swap primitives + b3Primitive temp = primitives[i1]; + primitives[i1] = primitives[i2 - 1]; + primitives[i2 - 1] = temp; + + i1 += 1; + i2 -= 1; + } + } + } + else + { + split.axis = 2; + + float pivot = c.z; + + while ( i1 < i2 ) + { + while ( i1 < i2 && primitives[i1].center.z < pivot ) + { + i1 += 1; + }; + + while ( i1 < i2 && primitives[i2 - 1].center.z >= pivot ) + { + i2 -= 1; + }; + + if ( i1 < i2 ) + { + // Swap primitives + b3Primitive temp = primitives[i1]; + primitives[i1] = primitives[i2 - 1]; + primitives[i2 - 1] = temp; + + i1 += 1; + i2 -= 1; + } + } + } + B3_ASSERT( i1 == i2 ); + B3_ASSERT( 0 <= i1 && i1 < count ); + + if ( i1 == 0 || i1 == count - 1 ) + { + // failed to split + i1 = count / 2; + } + + b3AABB leftBounds = B3_BOUNDS3_EMPTY; + for ( int i = 0; i < i1; ++i ) + { + leftBounds = b3AABB_Union( leftBounds, primitives[i].aabb ); + } + + b3AABB rightBounds = B3_BOUNDS3_EMPTY; + for ( int i = i1; i < count; ++i ) + { + rightBounds = b3AABB_Union( rightBounds, primitives[i].aabb ); + } + + split.index = i1; + split.leftBounds = leftBounds; + split.rightBounds = rightBounds; + return split; +} + +#if B3_ENABLE_VALIDATION == 1 + +static bool b3ValidateSplit( int count, b3Primitive* primitives, const b3Split* split ) +{ + if ( split->axis < 0 ) + { + return false; + } + + for ( int i = 0; i < split->index; ++i ) + { + if ( !b3AABB_Contains( split->leftBounds, primitives[i].aabb ) ) + { + return false; + } + } + + for ( int i = split->index; i < count; ++i ) + { + if ( !b3AABB_Contains( split->rightBounds, primitives[i].aabb ) ) + { + return false; + } + } + + return true; +} + +#endif + +static int b3BuildRecursive( b3Array( b3MeshNode ) * nodes, int count, b3Primitive* primitives, b3Primitive* base, + bool useMedianSplit, int* height ) +{ + if ( count > B3_DESIRED_TRIANGLES_PER_LEAF ) + { + // Try to split the input set using the SAH + b3Split split; + if ( useMedianSplit ) + { + split = b3SplitMedian( count, primitives ); + } + else + { + split = b3SplitBinnedSah( count, primitives ); + } + + if ( split.axis < 0 ) + { + if ( count > B3_MAXIMUM_TRIANGLES_PER_LEAF ) + { + // Re-split. This is a less optimal split and can create more false positives! + split = b3SplitHalf( count, primitives ); + } + else + { + b3AABB bounds = B3_BOUNDS3_EMPTY; + for ( int i = 0; i < count; ++i ) + { + bounds = b3AABB_Union( bounds, primitives[i].aabb ); + } + + // We have only a few triangles left. Create a leaf. + int index = b3Array_AddIndex( *nodes ); + b3StoreLeaf( &nodes->data[index], &bounds, count, (int)( primitives - base ) ); + + return index; + } + } + B3_VALIDATE( b3ValidateSplit( count, primitives, &split ) ); + + // Allocate node and recurse + int index = b3Array_AddIndex( *nodes ); + int heightLeft = 0, heightRight = 0; + int leftIndex = b3BuildRecursive( nodes, split.index, primitives, base, useMedianSplit, &heightLeft ); + int rightIndex = + b3BuildRecursive( nodes, count - split.index, primitives + split.index, base, useMedianSplit, &heightRight ); + + *height = b3MaxInt( heightLeft, heightRight ) + 1; + + B3_UNUSED( leftIndex ); + B3_ASSERT( leftIndex - index == 1 && rightIndex - index > 1 ); + + b3AABB aabb = b3AABB_Union( split.leftBounds, split.rightBounds ); + b3MeshNode* node = b3Array_Get( *nodes, index ); + node->data.asNode.axis = split.axis; + node->data.asNode.childOffset = rightIndex - index; + node->lowerBound = aabb.lowerBound; + node->upperBound = aabb.upperBound; + // triangleOffset is leaf-only, but lives outside the union — zero it so mesh->hash is deterministic + node->triangleOffset = 0; + + return index; + } + + b3AABB aabb = B3_BOUNDS3_EMPTY; + for ( int i = 0; i < count; ++i ) + { + aabb = b3AABB_Union( aabb, primitives[i].aabb ); + } + + int index = b3Array_AddIndex( *nodes ); + b3StoreLeaf( &nodes->data[index], &aabb, count, (int)( primitives - base ) ); + + *height = 1; + + return index; +} + +static bool b3SortMeshTriangles( b3MeshData* mesh ) +{ + b3MeshTriangle* triangles = b3GetMeshTrianglesWrite( mesh ); + uint8_t* materialIndices = b3GetMeshMaterialIndicesWrite( mesh ); + + // Sort triangles in depth-first-order + int offset = 0; + b3Array( b3MeshTriangle ) tempTriangles; + b3Array_CreateN( tempTriangles, mesh->triangleCount ); + + b3Array( uint8_t ) tempMaterialIndices; + b3Array_CreateN( tempMaterialIndices, mesh->triangleCount ); + + int count = 0; + b3MeshNode* stack[B3_MESH_STACK_SIZE]; + stack[count++] = b3GetRootWrite( mesh ); + + while ( count > 0 ) + { + b3MeshNode* node = stack[--count]; + + if ( b3IsLeaf( node ) == false ) + { + if ( count >= B3_MESH_STACK_SIZE - 2 ) + { + return false; + } + + stack[count++] = b3GetRightChildWrite( node ); + stack[count++] = b3GetLeftChildWrite( node ); + } + else + { + int triangleCount = node->data.asLeaf.triangleCount; + int triangleOffset = node->triangleOffset; + + for ( int triangle = 0; triangle < triangleCount; ++triangle ) + { + int index = triangleOffset + triangle; + b3Array_Push( tempTriangles, triangles[index] ); + b3Array_Push( tempMaterialIndices, materialIndices[index] ); + } + + node->triangleOffset = offset; + offset += triangleCount; + } + } + + B3_ASSERT( offset == tempTriangles.count ); + B3_ASSERT( tempTriangles.count == mesh->triangleCount ); + B3_ASSERT( tempMaterialIndices.count == mesh->triangleCount ); + + // Copy sorted triangle array back to tree + memcpy( triangles, tempTriangles.data, mesh->triangleCount * sizeof( b3MeshTriangle ) ); + memcpy( materialIndices, tempMaterialIndices.data, mesh->triangleCount * sizeof( uint8_t ) ); + + b3Array_Destroy( tempTriangles ); + b3Array_Destroy( tempMaterialIndices ); + + return true; +} + +typedef struct +{ + int vertex1; + int vertex2; + int triangle1; + int triangle2; + uint16_t triangleCount; + + // The index of an edge within the parent triangle: 0, 1, or 2. 0xFF is unset + uint8_t triangleEdgeIndex1; + uint8_t triangleEdgeIndex2; +} b3MeshEdge; + +#define NAME b3EdgeMap +#define KEY_TY uint64_t +#define VAL_TY int +#define HASH_FN vt_hash_integer +#define CMPR_FN vt_cmpr_integer +#define MALLOC_FN b3Alloc +#define FREE_FN b3Free +#include "verstable.h" + +#if 0 +// todo this is for testing other hash tables +struct b3TestHash +{ + size_t operator()( uint64_t key ) const + { + key ^= key >> 23; + key *= 0x2127599bf4325c37ull; + key ^= key >> 47; + return (size_t)key; + } +}; +#endif + +// Results for MeshCreationBenchmark +// eastl::hash_map : 4.9703 ms and 5.1445ms with FastHash function +// std::unordered_map : 4.8755 ms and 4.4780ms with FastHash function +// verstable : 3.3968 ms with default FastHash +// no edge identification : 1.7396 ms +static void b3IdentifyEdges( b3MeshData* mesh ) +{ + b3MeshTriangle* triangles = b3GetMeshTrianglesWrite( mesh ); + const b3Vec3* vertices = b3GetMeshVertices( mesh ); + uint8_t* flags = b3GetMeshFlagsWrite( mesh ); + + int triangleCount = mesh->triangleCount; + int edgeCount = 3 * triangleCount; + b3MeshEdge* edges = B3_ALLOC( b3MeshEdge, edgeCount ); + b3Vec3* normals = B3_ALLOC( b3Vec3, triangleCount ); + + for ( int i = 0; i < triangleCount; ++i ) + { + b3MeshTriangle* triangle = triangles + i; + int i1 = triangle->index1; + int i2 = triangle->index2; + int i3 = triangle->index3; + + edges[3 * i + 0].vertex1 = b3MinInt( i1, i2 ); + edges[3 * i + 0].vertex2 = b3MaxInt( i1, i2 ); + edges[3 * i + 0].triangle1 = i; + edges[3 * i + 0].triangle2 = B3_NULL_INDEX; + edges[3 * i + 0].triangleEdgeIndex1 = 0; + edges[3 * i + 0].triangleEdgeIndex2 = 0xFF; + edges[3 * i + 0].triangleCount = 1; + + edges[3 * i + 1].vertex1 = b3MinInt( i2, i3 ); + edges[3 * i + 1].vertex2 = b3MaxInt( i2, i3 ); + edges[3 * i + 1].triangle1 = i; + edges[3 * i + 1].triangle2 = B3_NULL_INDEX; + edges[3 * i + 1].triangleEdgeIndex1 = 1; + edges[3 * i + 1].triangleEdgeIndex2 = 0xFF; + edges[3 * i + 1].triangleCount = 1; + + edges[3 * i + 2].vertex1 = b3MinInt( i3, i1 ); + edges[3 * i + 2].vertex2 = b3MaxInt( i3, i1 ); + edges[3 * i + 2].triangle1 = i; + edges[3 * i + 2].triangle2 = B3_NULL_INDEX; + edges[3 * i + 2].triangleEdgeIndex1 = 2; + edges[3 * i + 2].triangleEdgeIndex2 = 0xFF; + edges[3 * i + 2].triangleCount = 1; + + b3Vec3 v1 = vertices[i1]; + b3Vec3 v2 = vertices[i2]; + b3Vec3 v3 = vertices[i3]; + + b3Vec3 e1 = b3Sub( v2, v1 ); + b3Vec3 e2 = b3Sub( v3, v1 ); + b3Vec3 n = b3Cross( e1, e2 ); + + normals[i] = b3Normalize( n ); + } + + b3EdgeMap map; + b3EdgeMap_init( &map ); + b3EdgeMap_reserve( &map, edgeCount ); + + uint64_t key = (uint64_t)edges[0].vertex1 << 32 | (uint64_t)edges[0].vertex2; + b3EdgeMap_insert( &map, key, 0 ); + + // Find unique edges and assign adjacency + for ( int i = 1; i < edgeCount; ++i ) + { + b3MeshEdge* edge = edges + i; + key = (uint64_t)edge->vertex1 << 32 | (uint64_t)edge->vertex2; + b3EdgeMap_itr itr = b3EdgeMap_get( &map, key ); + + if ( b3EdgeMap_is_end( itr ) ) + { + b3EdgeMap_insert( &map, key, i ); + } + else + { + int otherIndex = itr.data->val; + B3_ASSERT( otherIndex < i ); + + b3MeshEdge* base = edges + otherIndex; + if ( base->triangleCount == 1 ) + { + base->triangle2 = edge->triangle1; + base->triangleEdgeIndex2 = edge->triangleEdgeIndex1; + } + + base->triangleCount += 1; + } + } + + b3EdgeMap_cleanup( &map ); + + for ( int i = 0; i < edgeCount; ++i ) + { + b3MeshEdge* edge = edges + i; + if ( edge->triangleCount != 2 ) + { + continue; + } + + B3_ASSERT( edge->triangleEdgeIndex1 < 3 ); + B3_ASSERT( edge->triangleEdgeIndex2 < 3 ); + + b3MeshTriangle* triangle1 = triangles + edge->triangle1; + b3MeshTriangle* triangle2 = triangles + edge->triangle2; + uint8_t* flag1 = flags + edge->triangle1; + uint8_t* flag2 = flags + edge->triangle2; + + int j1 = triangle2->index1; + int j2 = triangle2->index2; + int j3 = triangle2->index3; + + int opposite = B3_NULL_INDEX; + + switch ( edge->triangleEdgeIndex2 ) + { + case 0: + opposite = j3; + break; + + case 1: + opposite = j1; + break; + + case 2: + opposite = j2; + break; + + default: + B3_ASSERT( false ); + } + + int i1 = triangle1->index1; + int i2 = triangle1->index2; + int i3 = triangle1->index3; + + b3Vec3 v1 = vertices[i1]; + b3Vec3 v2 = vertices[i2]; + b3Vec3 v3 = vertices[i3]; + b3Vec3 p = vertices[opposite]; + + float cos5Deg = 0.9962f; + float signedVolume = b3SignedVolume( v1, v2, v3, p ); + b3Vec3 n1 = normals[edge->triangle1]; + b3Vec3 n2 = normals[edge->triangle2]; + float cosAngle = b3Dot( n1, n2 ); + if ( signedVolume > 0.0f || cosAngle > cos5Deg ) + { + int edgeFlags[3] = { b3_concaveEdge1, b3_concaveEdge2, b3_concaveEdge3 }; + *flag1 |= edgeFlags[edge->triangleEdgeIndex1]; + *flag2 |= edgeFlags[edge->triangleEdgeIndex2]; + } + + if ( signedVolume < 0.0f || cosAngle > cos5Deg ) + { + int edgeFlags[3] = { b3_inverseConcaveEdge1, b3_inverseConcaveEdge2, b3_inverseConcaveEdge3 }; + *flag1 |= edgeFlags[edge->triangleEdgeIndex1]; + *flag2 |= edgeFlags[edge->triangleEdgeIndex2]; + } + } + + B3_FREE( normals, b3Vec3, triangleCount ); + B3_FREE( edges, b3MeshEdge, edgeCount ); +} + +b3MeshData* b3CreateGridMesh( int xCount, int zCount, float cellWidth, int materialCount, bool identifyEdges ) +{ + B3_ASSERT( 0 <= materialCount && materialCount <= UINT8_MAX ); + + // Create vertices + int vertexCount = ( xCount + 1 ) * ( zCount + 1 ); + + b3Array( b3Vec3 ) vertices = { 0 }; + b3Array_Resize( vertices, vertexCount ); + int index = 0; + + float xWidth = cellWidth * xCount; + float zWidth = cellWidth * zCount; + + float x = -0.5f * xWidth; + for ( int ix = 0; ix <= xCount; ++ix ) + { + float z = -0.5f * zWidth; + for ( int iz = 0; iz <= zCount; ++iz ) + { + vertices.data[index] = (b3Vec3){ x, 0.0f, z }; + z += cellWidth; + index += 1; + } + x += cellWidth; + } + B3_ASSERT( index == vertexCount ); + + // Triangles + int triangleCount = 2 * xCount * zCount; + + b3Array( int ) indices = { 0 }; + b3Array_Resize( indices, 3 * triangleCount ); + + b3Array( uint8_t ) materialIndices = { 0 }; + b3Array_Resize( materialIndices, triangleCount ); + + int materialIndex = 0; + index = 0; + for ( int ix = 0; ix < xCount; ++ix ) + { + for ( int iz = 0; iz < zCount; ++iz ) + { + int index1 = iz + ( zCount + 1 ) * ix; + int index2 = index1 + 1; + int index3 = index2 + ( zCount + 1 ); + int index4 = index3 - 1; + + B3_ASSERT( index1 < vertexCount ); + B3_ASSERT( index2 < vertexCount ); + B3_ASSERT( index3 < vertexCount ); + B3_ASSERT( index4 < vertexCount ); + + indices.data[index + 0] = index1; + indices.data[index + 1] = index2; + indices.data[index + 2] = index3; + + indices.data[index + 3] = index3; + indices.data[index + 4] = index4; + indices.data[index + 5] = index1; + + if ( materialCount > 0 ) + { + materialIndices.data[2 * materialIndex + 0] = (uint8_t)( materialIndex % materialCount ); + materialIndices.data[2 * materialIndex + 1] = (uint8_t)( materialIndex % materialCount ); + } + + materialIndex += 1; + index += 6; + } + } + B3_ASSERT( index == 3 * triangleCount ); + + b3MeshDef def = { 0 }; + def.vertexCount = vertices.count; + def.vertices = vertices.data; + def.triangleCount = indices.count / 3; + def.indices = indices.data; + def.materialIndices = materialCount > 0 ? materialIndices.data : NULL; + def.useMedianSplit = true; + def.identifyEdges = identifyEdges; + + b3MeshData* meshData = b3CreateMesh( &def, NULL, 0 ); + + b3Array_Destroy( indices ); + b3Array_Destroy( vertices ); + b3Array_Destroy( materialIndices ); + + return meshData; +} + +b3MeshData* b3CreateWaveMesh( int xCount, int zCount, float cellWidth, float amplitude, float rowFrequency, + float columnFrequency ) +{ + // Create vertices + int vertexCount = ( xCount + 1 ) * ( zCount + 1 ); + + b3Array( b3Vec3 ) vertices = { 0 }; + b3Array_Resize( vertices, vertexCount ); + int index = 0; + + float xWidth = cellWidth * xCount; + float zWidth = cellWidth * zCount; + + float omegaZ = 2.0f * B3_PI * rowFrequency * cellWidth; + float omegaX = 2.0f * B3_PI * columnFrequency * cellWidth; + + float x = -0.5f * xWidth; + for ( int ix = 0; ix <= xCount; ++ix ) + { + float rowHeight = sinf( omegaX * ix ); + + float z = -0.5f * zWidth; + for ( int iz = 0; iz <= zCount; ++iz ) + { + float columnHeight = sinf( omegaZ * iz ); + + float y = amplitude * rowHeight * columnHeight; + vertices.data[index] = (b3Vec3){ x, y, z }; + z += cellWidth; + index += 1; + } + x += cellWidth; + } + B3_ASSERT( index == vertexCount ); + + // Triangles + int triangleCount = 2 * xCount * zCount; + + b3Array( int ) indices = { 0 }; + b3Array_Resize( indices, 3 * triangleCount ); + + index = 0; + for ( int ix = 0; ix < xCount; ++ix ) + { + for ( int iz = 0; iz < zCount; ++iz ) + { + int index1 = iz + ( zCount + 1 ) * ix; + int index2 = index1 + 1; + int index3 = index2 + ( zCount + 1 ); + int index4 = index3 - 1; + + B3_ASSERT( index1 < vertexCount ); + B3_ASSERT( index2 < vertexCount ); + B3_ASSERT( index3 < vertexCount ); + B3_ASSERT( index4 < vertexCount ); + + indices.data[index + 0] = index1; + indices.data[index + 1] = index2; + indices.data[index + 2] = index3; + + indices.data[index + 3] = index3; + indices.data[index + 4] = index4; + indices.data[index + 5] = index1; + + index += 6; + } + } + B3_ASSERT( index == 3 * triangleCount ); + + b3MeshDef def = { 0 }; + def.vertexCount = vertices.count; + def.vertices = vertices.data; + def.triangleCount = indices.count / 3; + def.indices = indices.data; + def.useMedianSplit = true; + def.identifyEdges = true; + + b3MeshData* meshData = b3CreateMesh( &def, NULL, 0 ); + + b3Array_Destroy( indices ); + b3Array_Destroy( vertices ); + + return meshData; +} + +b3MeshData* b3CreateTorusMesh( int radialResolution, int tubularResolution, float radius, float thickness ) +{ + // Create vertices + b3Array( b3Vec3 ) vertices = { 0 }; + + for ( int radialIndex = 0; radialIndex < radialResolution; radialIndex++ ) + { + for ( int tubularIndex = 0; tubularIndex < tubularResolution; tubularIndex++ ) + { + float u = (float)tubularIndex / tubularResolution * B3_TWO_PI; + float v = (float)radialIndex / radialResolution * B3_TWO_PI; + + float x = ( radius + thickness * b3Cos( v ) ) * b3Cos( u ); + float y = ( radius + thickness * b3Cos( v ) ) * b3Sin( u ); + float z = thickness * b3Sin( v ); + + b3Vec3 vertex = { x, y, z }; + b3Array_Push( vertices, vertex ); + } + } + + // Triangles + b3Array( int ) indices = { 0 }; + for ( int radialIndex1 = 0; radialIndex1 < radialResolution; radialIndex1++ ) + { + int radialIndex2 = ( radialIndex1 + 1 ) % radialResolution; + for ( int tubularIndex1 = 0; tubularIndex1 < tubularResolution; tubularIndex1++ ) + { + int tubularIndex2 = ( tubularIndex1 + 1 ) % tubularResolution; + int index1 = radialIndex1 * tubularResolution + tubularIndex1; + int index2 = radialIndex1 * tubularResolution + tubularIndex2; + int index3 = radialIndex2 * tubularResolution + tubularIndex2; + int index4 = radialIndex2 * tubularResolution + tubularIndex1; + + b3Array_Push( indices, index1 ); + b3Array_Push( indices, index2 ); + b3Array_Push( indices, index3 ); + + b3Array_Push( indices, index3 ); + b3Array_Push( indices, index4 ); + b3Array_Push( indices, index1 ); + } + } + + b3MeshDef def = { 0 }; + def.vertexCount = vertices.count; + def.vertices = vertices.data; + def.triangleCount = indices.count / 3; + def.indices = indices.data; + def.useMedianSplit = false; + def.identifyEdges = true; + + b3MeshData* meshData = b3CreateMesh( &def, NULL, 0 ); + + b3Array_Destroy( vertices ); + b3Array_Destroy( indices ); + return meshData; +} + +b3MeshData* b3CreateBoxMesh( b3Vec3 center, b3Vec3 extent, bool identifyEdges ) +{ + float x = extent.x; + float y = extent.y; + float z = extent.z; + b3Vec3 vertices[] = { + { x, y, z }, { -x, y, z }, { -x, -y, z }, { x, -y, z }, { x, y, -z }, { -x, y, -z }, { -x, -y, -z }, { x, -y, -z }, + }; + + for ( int i = 0; i < 8; ++i ) + { + vertices[i] = b3Add( vertices[i], center ); + } + + int indices[] = { + 0, 1, 3, 1, 2, 3, // front + 0, 4, 1, 1, 4, 5, // top + 0, 3, 7, 4, 0, 7, // right + 4, 7, 5, 6, 5, 7, // back + 1, 5, 2, 6, 2, 5, // left + 3, 2, 7, 6, 7, 2, // bottom + }; + + b3MeshDef def = { 0 }; + def.vertexCount = 8; + def.vertices = vertices; + def.triangleCount = 12; + def.indices = indices; + def.useMedianSplit = false; + def.identifyEdges = identifyEdges; + + return b3CreateMesh( &def, NULL, 0 ); +} + +b3MeshData* b3CreateHollowBoxMesh(b3Vec3 center, b3Vec3 extent) +{ + float x = extent.x; + float y = extent.y; + float z = extent.z; + b3Vec3 vertices[] = { + { x, y, z }, { -x, y, z }, { -x, -y, z }, { x, -y, z }, { x, y, -z }, { -x, y, -z }, { -x, -y, -z }, { x, -y, -z }, + }; + + for ( int i = 0; i < 8; ++i ) + { + vertices[i] = b3Add( vertices[i], center ); + } + + int indices[] = { + 3, 1, 0, 3, 2, 1, // front + 1, 4, 0, 5, 4, 1, // top + 7, 3, 0, 7, 0, 4, // right + 5, 7, 4, 7, 5, 6, // back + 2, 5, 1, 5, 2, 6, // left + 7, 2, 3, 2, 7, 6, // bottom + }; + + b3MeshDef def = { 0 }; + def.vertexCount = 8; + def.vertices = vertices; + def.triangleCount = 12; + def.indices = indices; + def.useMedianSplit = false; + def.identifyEdges = true; + + return b3CreateMesh( &def, NULL, 0 ); +} + +b3MeshData* b3CreatePlatformMesh( b3Vec3 center, float height, float topWidth, float bottomWidth ) +{ + float hb = 0.5f * bottomWidth; + float ht = 0.5f * topWidth; + float hy = 0.5f * height; + b3Vec3 vertices[] = { + { ht, hy, ht }, { -ht, hy, ht }, { -hb, -hy, hb }, { hb, -hy, hb }, + { ht, hy, -ht }, { -ht, hy, -ht }, { -hb, -hy, -hb }, { hb, -hy, -hb }, + }; + + for ( int i = 0; i < 8; ++i ) + { + vertices[i] = b3Add( vertices[i], center ); + } + + int indices[] = { + 0, 1, 3, 1, 2, 3, // front + 0, 4, 1, 1, 4, 5, // top + 0, 3, 7, 4, 0, 7, // right + 4, 7, 5, 6, 5, 7, // back + 1, 5, 2, 6, 2, 5, // left + 3, 2, 7, 6, 7, 2, // bottom + }; + + b3MeshDef def = { 0 }; + def.vertexCount = 8; + def.vertices = vertices; + def.triangleCount = 12; + def.indices = indices; + def.useMedianSplit = true; + def.identifyEdges = true; + + return b3CreateMesh( &def, NULL, 0 ); +} + +// todo this should fail if the mesh has a height greater than B3_MESH_STACK_SIZE +b3MeshData* b3CreateMesh( const b3MeshDef* def, int* degenerateTriangleIndices, int degenerateCapacity ) +{ + if ( def->vertexCount < 3 || def->vertices == NULL || def->triangleCount <= 0 || def->indices == NULL ) + { + return NULL; + } + + int triangleCount = def->triangleCount; + if ( triangleCount == 0 ) + { + return NULL; + } + + int vertexCount = def->vertexCount; + + b3AABB meshBounds = B3_BOUNDS3_EMPTY; + + // Clone indices and vertices to support welding + b3Array( int ) indices; + b3Array_CreateN( indices, 3 * triangleCount ); + + b3Array( b3Vec3 ) vertices; + b3Array_CreateN( vertices, vertexCount ); + + if ( def->weldVertices && def->weldTolerance > 0.0f ) + { + b3Array_Resize( vertices, vertexCount ); + b3Array_Resize( indices, 3 * triangleCount ); + b3WeldData data = { + .srcVertices = def->vertices, + .srcIndices = def->indices, + .dstVertices = vertices.data, + .dstIndices = indices.data, + .vertexCount = vertexCount, + .indexCount = 3 * triangleCount, + }; + vertices.count = b3WeldVertices( &data, def->weldTolerance ); + vertexCount = vertices.count; + B3_ASSERT( vertexCount <= def->vertexCount ); + } + else + { + b3Array_Append( vertices, def->vertices, vertexCount ); + b3Array_Append( indices, def->indices, 3 * triangleCount ); + } + + b3Array( b3Primitive ) primitives; + b3Array_CreateN( primitives, triangleCount ); + int degenerateCount = 0; + float minArea = 0.01f * B3_LINEAR_SLOP * B3_LINEAR_SLOP; + float surfaceArea = 0.0f; + int materialCount = 1; + + for ( int index = 0; index < triangleCount; ++index ) + { + int index1 = indices.data[3 * index + 0]; + int index2 = indices.data[3 * index + 1]; + int index3 = indices.data[3 * index + 2]; + + b3Vec3 vertex1 = vertices.data[index1]; + b3Vec3 vertex2 = vertices.data[index2]; + b3Vec3 vertex3 = vertices.data[index3]; + + b3Vec3 normal = b3Cross( b3Sub( vertex2, vertex1 ), b3Sub( vertex3, vertex1 ) ); + float area = 0.5f * b3Length( normal ); + + if ( area < minArea ) + { + // b3Log( "degenerate: %d %d %d\n", index1, index2, index3 ); + + if ( index1 != index2 && index1 != index3 && index2 != index3 ) + { + degenerateCount += 1; + if ( degenerateTriangleIndices != NULL && degenerateCount < degenerateCapacity ) + { + degenerateTriangleIndices[degenerateCount - 1] = index; + } + } + + continue; + } + + surfaceArea += area; + + b3AABB box = { + b3Min( vertex1, b3Min( vertex2, vertex3 ) ), + b3Max( vertex1, b3Max( vertex2, vertex3 ) ), + }; + + b3Vec3 center = b3AABB_Center( box ); + + b3Primitive primitive = { + .aabb = box, + .center = center, + .triangleIndex = index, + }; + b3Array_Push( primitives, primitive ); + + if ( def->materialIndices != NULL ) + { + materialCount = b3MaxInt( materialCount, def->materialIndices[index] + 1 ); + } + + meshBounds = b3AABB_Union( meshBounds, box ); + } + + // Update triangle count due to degenerates being skipped + triangleCount = primitives.count; + + if ( b3IsSaneAABB( meshBounds ) == false ) + { + b3Array_Destroy( primitives ); + return NULL; + } + + // Build the tree (this reorders the builder triangles) + b3Array( b3MeshNode ) tempNodes; + b3Array_CreateN( tempNodes, 2 * triangleCount - 1 ); + + int treeHeight = 0; + b3BuildRecursive( &tempNodes, triangleCount, primitives.data, primitives.data, def->useMedianSplit, &treeHeight ); + + // Allocate the mesh + size_t byteCount = b3AlignUp8( sizeof( b3MeshData ) ); + int nodeOffset = (int)byteCount; + byteCount += b3AlignUp8( tempNodes.count * sizeof( b3MeshNode ) ); + int vertexOffset = (int)byteCount; + byteCount += b3AlignUp8( vertexCount * sizeof( b3Vec3 ) ); + int triangleOffset = (int)byteCount; + byteCount += b3AlignUp8( triangleCount * sizeof( b3MeshTriangle ) ); + int materialIndicesOffset = (int)byteCount; + byteCount += b3AlignUp8( triangleCount * sizeof( uint8_t ) ); + int flagsOffset = (int)byteCount; + byteCount += b3AlignUp8( triangleCount * sizeof( uint8_t ) ); + + b3MeshData* mesh = b3Alloc( byteCount ); + + // zero initialize for determinism + memset( mesh, 0, byteCount ); + + mesh->version = B3_MESH_VERSION; + mesh->byteCount = (int)byteCount; + mesh->bounds = meshBounds; + mesh->surfaceArea = surfaceArea; + mesh->nodeCount = tempNodes.count; + mesh->treeHeight = treeHeight; + mesh->vertexCount = vertexCount; + mesh->triangleCount = triangleCount; + mesh->degenerateCount = degenerateCount; + mesh->nodeOffset = nodeOffset; + mesh->vertexOffset = vertexOffset; + mesh->triangleOffset = triangleOffset; + mesh->materialOffset = materialIndicesOffset; + mesh->materialCount = materialCount; + mesh->flagsOffset = flagsOffset; + + b3MeshNode* nodes = b3GetMeshNodesWrite( mesh ); + b3MeshTriangle* triangles = b3GetMeshTrianglesWrite( mesh ); + b3Vec3* meshVertices = b3GetMeshVerticesWrite( mesh ); + uint8_t* materialIndices = b3GetMeshMaterialIndicesWrite( mesh ); + uint8_t* flags = b3GetMeshFlagsWrite( mesh ); + + memcpy( nodes, tempNodes.data, tempNodes.count * sizeof( b3MeshNode ) ); + memcpy( meshVertices, vertices.data, vertexCount * sizeof( b3Vec3 ) ); + + for ( int index = 0; index < triangleCount; ++index ) + { + b3Primitive primitive = primitives.data[index]; + triangles[index].index1 = indices.data[3 * primitive.triangleIndex + 0]; + triangles[index].index2 = indices.data[3 * primitive.triangleIndex + 1]; + triangles[index].index3 = indices.data[3 * primitive.triangleIndex + 2]; + flags[index] = 0; + + // Copy material indices if they exist. Otherwise the material indices are all zeroes. + if ( def->materialIndices != NULL ) + { + uint8_t materialIndex = def->materialIndices[primitive.triangleIndex]; + materialIndices[index] = materialIndex; + } + } + + // Sort triangle in DFS order. Casts and volume queries will return sorted arrays. + // This also sorts material indices, but not the materials. + // This can fail if the BVH height is too large. + bool success = b3SortMeshTriangles( mesh ); + if ( success == false ) + { + b3Array_Destroy( tempNodes ); + b3Array_Destroy( primitives ); + return NULL; + } + + if ( def->identifyEdges ) + { + b3IdentifyEdges( mesh ); + } + + B3_VALIDATE( b3IsNonDegenerate( mesh, minArea ) ); + B3_VALIDATE( b3IsConsistent( mesh ) ); + + b3Array_Destroy( tempNodes ); + b3Array_Destroy( primitives ); + b3Array_Destroy( indices ); + b3Array_Destroy( vertices ); + + mesh->hash = 0; + mesh->hash = b3NonZeroHash( b3Hash( B3_HASH_INIT, (uint8_t*)mesh, mesh->byteCount ) ); + + return mesh; +} + +void b3DestroyMesh( b3MeshData* mesh ) +{ + b3Free( mesh, mesh->byteCount ); +} + +bool b3OverlapMesh( const b3Mesh* shape, b3Transform shapeTransform, const b3ShapeProxy* proxy ) +{ + B3_ASSERT( proxy->count > 0 ); + b3SimplexCache cache = { 0 }; + + b3Vec3 buffer[B3_MAX_SHAPE_CAST_POINTS]; + b3ShapeProxy localProxy = b3MakeLocalProxy( proxy, shapeTransform, buffer ); + b3AABB aabb = b3ComputeProxyAABB( &localProxy ); + + b3Vec3 meshScale = shape->scale; + + // Scale may have reflection so min/max may become invalid when unscaled + b3V32 scale = b3LoadV( &meshScale.x ); + b3V32 invScale = b3DivV( b3_oneV, scale ); + b3V32 temp1 = b3MulV( invScale, b3LoadV( &aabb.lowerBound.x ) ); + b3V32 temp2 = b3MulV( invScale, b3LoadV( &aabb.upperBound.x ) ); + b3V32 invScaledBoundsMin = b3MinV( temp1, temp2 ); + b3V32 invScaledBoundsMax = b3MaxV( temp1, temp2 ); + b3V32 invScaledBoundsCenter = b3MulV( b3_halfV, b3AddV( invScaledBoundsMin, invScaledBoundsMax ) ); + b3V32 invScaledBoundsExtent = b3SubV( invScaledBoundsMax, invScaledBoundsCenter ); + + b3DistanceInput input; + input.proxyB = localProxy; + input.transform = b3Transform_identity; + input.useRadii = true; + + int count = 0; + const b3MeshNode* stack[B3_MESH_STACK_SIZE]; + const b3MeshNode* node = b3GetRoot( shape->data ); + const b3MeshTriangle* triangles = b3GetMeshTriangles( shape->data ); + const b3Vec3* vertices = b3GetMeshVertices( shape->data ); + + while ( true ) + { + // Test node overlap in unscaled space + b3V32 nodeMin = b3LoadV( &node->lowerBound.x ); + b3V32 nodeMax = b3LoadV( &node->upperBound.x ); + if ( b3TestBoundsOverlap( nodeMin, nodeMax, invScaledBoundsMin, invScaledBoundsMax ) ) + { + if ( b3IsLeaf( node ) ) + { + int triangleCount = node->data.asLeaf.triangleCount; + int triangleOffset = node->triangleOffset; + + for ( int index = 0; index < triangleCount; ++index ) + { + int triangleIndex = triangleOffset + index; + b3MeshTriangle triangle = triangles[triangleIndex]; + + b3Vec3 vertex1 = vertices[triangle.index1]; + b3Vec3 vertex2 = vertices[triangle.index2]; + b3Vec3 vertex3 = vertices[triangle.index3]; + b3V32 v1 = b3LoadV( &vertex1.x ); + b3V32 v2 = b3LoadV( &vertex2.x ); + b3V32 v3 = b3LoadV( &vertex3.x ); + + // Bounding box overlap test in unscaled space + if ( b3TestBoundsTriangleOverlap( invScaledBoundsCenter, invScaledBoundsExtent, v1, v2, v3 ) ) + { + // Shape-triangle overlap test in scaled space. Winding order doesn't matter. + b3Vec3 triangleVertices[] = { b3Mul( meshScale, vertex1 ), b3Mul( meshScale, vertex2 ), + b3Mul( meshScale, vertex3 ) }; + input.proxyA = (b3ShapeProxy){ triangleVertices, 3, 0.0f }; + + // reset the cache + cache.count = 0; + + // get distance between triangle and query shape + b3DistanceOutput output = b3ShapeDistance( &input, &cache, NULL, 0 ); + + float tolerance = 0.1f * B3_LINEAR_SLOP; + if ( output.distance < tolerance ) + { + // overlap detected + return true; + } + } + } + } + else + { + // Recurse + B3_ASSERT( count <= B3_MESH_STACK_SIZE - 1 ); + stack[count++] = b3GetRightChild( node ); + node = b3GetLeftChild( node ); + + continue; + } + } + + if ( count == 0 ) + { + break; + } + node = stack[--count]; + } + + return false; +} + +b3AABB b3ComputeMeshAABB( const b3MeshData* shape, b3Transform transform, b3Vec3 scale ) +{ + b3Vec3 scaledLower = b3Mul( scale, shape->bounds.lowerBound ); + b3Vec3 scaledUpper = b3Mul( scale, shape->bounds.upperBound ); + b3AABB bounds = { b3Min( scaledLower, scaledUpper ), b3Max( scaledLower, scaledUpper ) }; + return b3AABB_Transform( transform, bounds ); +} + +b3CastOutput b3RayCastMesh( const b3Mesh* mesh, const b3RayCastInput* input ) +{ + const b3MeshData* data = mesh->data; + b3Vec3 meshScale = mesh->scale; + + b3CastOutput bestOutput = { 0 }; + bestOutput.fraction = input->maxFraction; + bestOutput.triangleIndex = B3_NULL_INDEX; + + b3V32 lambda = b3SplatV( input->maxFraction ); + + b3V32 rayStart = b3LoadV( &input->origin.x ); + b3V32 rayDelta = b3LoadV( &input->translation.x ); + + b3V32 scale = b3LoadV( &meshScale.x ); + b3V32 invScale = b3DivV( b3_oneV, scale ); + bool clockwise = meshScale.x * meshScale.y * meshScale.z < 0.0f; + + // Use the inverse scaled ray for traversal of the BVH + b3V32 invScaledRayStart = b3MulV( invScale, rayStart ); + b3V32 invScaledRayDelta = b3MulV( invScale, rayDelta ); + b3V32 invScaledRayEnd = b3AddV( invScaledRayStart, b3MulV( lambda, invScaledRayDelta ) ); + b3V32 invScaledRayMin = b3MinV( invScaledRayStart, invScaledRayEnd ); + b3V32 invScaledRayMax = b3MaxV( invScaledRayStart, invScaledRayEnd ); + + int count = 0; + const b3MeshNode* stack[B3_MESH_STACK_SIZE]; + const b3MeshNode* node = b3GetRoot( data ); + const b3MeshTriangle* triangles = b3GetMeshTriangles( data ); + const b3Vec3* vertices = b3GetMeshVertices( data ); + const uint8_t* materialIndices = b3GetMeshMaterialIndices( data ); + + while ( true ) + { + // Test node/ray overlap using SAT + b3V32 nodeMin = b3LoadV( &node->lowerBound.x ); + b3V32 nodeMax = b3LoadV( &node->upperBound.x ); + if ( b3TestBoundsOverlap( nodeMin, nodeMax, invScaledRayMin, invScaledRayMax ) && + b3TestBoundsRayOverlap( nodeMin, nodeMax, invScaledRayStart, invScaledRayDelta ) ) + { + // SAT: The node and ray overlap - process leaf node or recurse + if ( b3IsLeaf( node ) ) + { + int triangleCount = node->data.asLeaf.triangleCount; + int triangleOffset = node->triangleOffset; + + for ( int index = 0; index < triangleCount; ++index ) + { + int triangleIndex = triangleOffset + index; + b3MeshTriangle triangle = triangles[triangleIndex]; + + // Collide ray with triangle in scaled space + b3Vec3 vertex1 = b3Mul( meshScale, vertices[triangle.index1] ); + b3Vec3 vertex2, vertex3; + + // The CPU should predict this branch + if ( clockwise ) + { + vertex2 = b3Mul( meshScale, vertices[triangle.index3] ); + vertex3 = b3Mul( meshScale, vertices[triangle.index2] ); + } + else + { + vertex2 = b3Mul( meshScale, vertices[triangle.index2] ); + vertex3 = b3Mul( meshScale, vertices[triangle.index3] ); + } + + // Collide ray with triangle in scaled space + b3V32 v1 = b3LoadV( &vertex1.x ); + b3V32 v2 = b3LoadV( &vertex2.x ); + b3V32 v3 = b3LoadV( &vertex3.x ); + + float alpha = b3IntersectRayTriangle( rayStart, rayDelta, v1, v2, v3 ); + B3_ASSERT( 0 <= alpha && alpha <= 1.0f ); + + if ( alpha < bestOutput.fraction ) + { + b3Vec3 edge1 = b3Sub( vertex2, vertex1 ); + b3Vec3 edge2 = b3Sub( vertex3, vertex1 ); + bestOutput.normal = b3Normalize( b3Cross( edge1, edge2 ) ); + bestOutput.point = b3Add( input->origin, b3MulSV( alpha, input->translation ) ); + bestOutput.fraction = alpha; + bestOutput.triangleIndex = triangleIndex; + bestOutput.materialIndex = materialIndices[triangleIndex]; + bestOutput.hit = true; + + // Update ray bounds in unscaled space + lambda = b3SplatV( alpha ); + invScaledRayEnd = b3AddV( invScaledRayStart, b3MulV( lambda, invScaledRayDelta ) ); + invScaledRayMin = b3MinV( invScaledRayStart, invScaledRayEnd ); + invScaledRayMax = b3MaxV( invScaledRayStart, invScaledRayEnd ); + } + } + } + else + { + // Determine traversal order (front -> back) and recurse + int axis = node->data.asNode.axis; + if ( b3GetV( invScaledRayDelta, axis ) > 0.0f ) + { + B3_ASSERT( count <= B3_MESH_STACK_SIZE - 1 ); + stack[count++] = b3GetRightChild( node ); + node = b3GetLeftChild( node ); + } + else + { + B3_ASSERT( count <= B3_MESH_STACK_SIZE - 1 ); + stack[count++] = b3GetLeftChild( node ); + node = b3GetRightChild( node ); + } + + continue; + } + } + + if ( count == 0 ) + { + break; + } + node = stack[--count]; + } + + return bestOutput; +} + +b3CastOutput b3ShapeCastMesh( const b3Mesh* mesh, const b3ShapeCastInput* input ) +{ + const b3MeshData* data = mesh->data; + b3Vec3 meshScale = mesh->scale; + + b3CastOutput bestOutput = { 0 }; + bestOutput.fraction = input->maxFraction; + bestOutput.triangleIndex = B3_NULL_INDEX; + + b3V32 lambda = b3SplatV( input->maxFraction ); + + b3AABB shapeBounds = b3MakeAABB( input->proxy.points, input->proxy.count, input->proxy.radius ); + b3Vec3 center = b3AABB_Center( shapeBounds ); + b3Vec3 extents = b3AABB_Extents( shapeBounds ); + b3V32 shapeExtent = b3LoadV( &extents.x ); + + b3V32 rayStart = b3LoadV( ¢er.x ); + b3V32 rayDelta = b3LoadV( &input->translation.x ); + b3V32 rayEnd = b3AddV( rayStart, b3MulV( lambda, rayDelta ) ); + b3V32 rayMin = b3MinV( rayStart, rayEnd ); + b3V32 rayMax = b3MaxV( rayStart, rayEnd ); + + b3V32 scale = b3LoadV( &meshScale.x ); + b3V32 invScale = b3DivV( b3_oneV, scale ); + b3V32 absInvScale = b3AbsV( invScale ); + bool clockwise = meshScale.x * meshScale.y * meshScale.z < 0.0f; + + // Use the inverse scaled shape cast for traversal of the BVH + b3V32 invScaledRayStart = b3MulV( invScale, rayStart ); + b3V32 invScaledRayDelta = b3MulV( invScale, rayDelta ); + b3V32 invScaledRayEnd = b3AddV( invScaledRayStart, b3MulV( lambda, invScaledRayDelta ) ); + b3V32 invScaledRayMin = b3MinV( invScaledRayStart, invScaledRayEnd ); + b3V32 invScaledRayMax = b3MaxV( invScaledRayStart, invScaledRayEnd ); + b3V32 invScaledShapeExtent = b3MulV( absInvScale, shapeExtent ); + + int count = 0; + const b3MeshNode* stack[B3_MESH_STACK_SIZE]; + const b3MeshNode* node = b3GetRoot( data ); + const b3MeshTriangle* triangles = b3GetMeshTriangles( data ); + const b3Vec3* vertices = b3GetMeshVertices( data ); + const uint8_t* materialIndices = b3GetMeshMaterialIndices( data ); + + while ( true ) + { + // Test node/ray overlap using SAT in unscaled space + b3V32 nodeMin = b3SubV( b3LoadV( &node->lowerBound.x ), invScaledShapeExtent ); + b3V32 nodeMax = b3AddV( b3LoadV( &node->upperBound.x ), invScaledShapeExtent ); + + if ( b3TestBoundsOverlap( nodeMin, nodeMax, invScaledRayMin, invScaledRayMax ) && + b3TestBoundsRayOverlap( nodeMin, nodeMax, invScaledRayStart, invScaledRayDelta ) ) + { + // SAT: The node and ray overlap - process leaf node or recurse + if ( b3IsLeaf( node ) ) + { + int triangleCount = node->data.asLeaf.triangleCount; + int triangleOffset = node->triangleOffset; + + for ( int index = 0; index < triangleCount; ++index ) + { + int triangleIndex = triangleOffset + index; + b3MeshTriangle triangle = triangles[triangleIndex]; + + // Collide ray with triangle in scaled space + b3Vec3 vertex1 = b3Mul( meshScale, vertices[triangle.index1] ); + b3Vec3 vertex2, vertex3; + + // The CPU should predict this branch + if ( clockwise ) + { + vertex2 = b3Mul( meshScale, vertices[triangle.index3] ); + vertex3 = b3Mul( meshScale, vertices[triangle.index2] ); + } + else + { + vertex2 = b3Mul( meshScale, vertices[triangle.index2] ); + vertex3 = b3Mul( meshScale, vertices[triangle.index3] ); + } + + b3V32 v1 = b3LoadV( &vertex1.x ); + b3V32 v2 = b3LoadV( &vertex2.x ); + b3V32 v3 = b3LoadV( &vertex3.x ); + + b3V32 triangleMin = b3SubV( b3MinV( v1, b3MinV( v2, v3 ) ), shapeExtent ); + b3V32 triangleMax = b3AddV( b3MaxV( v1, b3MaxV( v2, v3 ) ), shapeExtent ); + + // Test triangle-ray overlap in scaled space + if ( b3TestBoundsOverlap( triangleMin, triangleMax, rayMin, rayMax ) ) + { + // Collide shape with triangle in scaled space + b3Vec3 origin = vertex1; + b3Vec3 triangleVertices[] = { b3Vec3_zero, b3Sub( vertex2, origin ), b3Sub( vertex3, origin ) }; + b3Transform shiftedOrigin = { b3Neg( origin ), b3Quat_identity }; + + b3ShapeCastPairInput pairInput; + pairInput.proxyA = (b3ShapeProxy){ triangleVertices, 3, 0.0f }; + pairInput.proxyB = input->proxy; + pairInput.transform = shiftedOrigin; + pairInput.maxFraction = bestOutput.fraction; + pairInput.translationB = input->translation; + pairInput.canEncroach = input->canEncroach; + + b3CastOutput pairOutput = b3ShapeCast( &pairInput ); + + if ( pairOutput.hit ) + { + pairOutput.point = b3Add( pairOutput.point, origin ); + + bestOutput = pairOutput; + bestOutput.triangleIndex = triangleIndex; + bestOutput.materialIndex = materialIndices[triangleIndex]; + + // Update ray bounds in scaled space + lambda = b3SplatV( pairOutput.fraction ); + rayEnd = b3AddV( rayStart, b3MulV( lambda, rayDelta ) ); + rayMin = b3MinV( rayStart, rayEnd ); + rayMax = b3MaxV( rayStart, rayEnd ); + + // Ray bounds in unscaled space + invScaledRayEnd = b3AddV( invScaledRayStart, b3MulV( lambda, invScaledRayDelta ) ); + invScaledRayMin = b3MinV( invScaledRayStart, invScaledRayEnd ); + invScaledRayMax = b3MaxV( invScaledRayStart, invScaledRayEnd ); + } + } + } + } + else + { + // Determine traversal order (front -> back) and recurse + int axis = node->data.asNode.axis; + if ( b3GetV( invScaledRayDelta, axis ) > 0.0f ) + { + B3_ASSERT( count <= B3_MESH_STACK_SIZE - 1 ); + stack[count++] = b3GetRightChild( node ); + node = b3GetLeftChild( node ); + } + else + { + B3_ASSERT( count <= B3_MESH_STACK_SIZE - 1 ); + stack[count++] = b3GetLeftChild( node ); + node = b3GetRightChild( node ); + } + + continue; + } + } + + if ( count == 0 ) + { + break; + } + node = stack[--count]; + } + + return bestOutput; +} + +b3Triangle b3GetMeshTriangle( const b3Mesh* mesh, int triangleIndex ) +{ + B3_ASSERT( 0 <= triangleIndex && triangleIndex < mesh->data->triangleCount ); + + const b3MeshTriangle* triangles = b3GetMeshTriangles( mesh->data ); + const uint8_t* flags = b3GetMeshFlags( mesh->data ); + const b3Vec3* vertices = b3GetMeshVertices( mesh->data ); + + b3Triangle result; + b3MeshTriangle triangle = triangles[triangleIndex]; + uint8_t triangleFlags = flags[triangleIndex]; + + b3Vec3 scale = mesh->scale; + + result.vertices[0] = b3Mul( scale, vertices[triangle.index1] ); + result.i1 = triangle.index1; + + if ( scale.x * scale.y * scale.z < 0.0f ) + { + result.vertices[1] = b3Mul( scale, vertices[triangle.index3] ); + result.vertices[2] = b3Mul( scale, vertices[triangle.index2] ); + + result.i2 = triangle.index3; + result.i3 = triangle.index2; + + // mesh is inverted, so concave edges are now convex + result.flags = 0; + result.flags |= ( triangleFlags & b3_inverseConcaveEdge1 ) ? b3_concaveEdge1 : 0; + result.flags |= ( triangleFlags & b3_inverseConcaveEdge2 ) ? b3_concaveEdge2 : 0; + result.flags |= ( triangleFlags & b3_inverseConcaveEdge3 ) ? b3_concaveEdge3 : 0; + } + else + { + result.vertices[1] = b3Mul( scale, vertices[triangle.index2] ); + result.vertices[2] = b3Mul( scale, vertices[triangle.index3] ); + + result.i2 = triangle.index2; + result.i3 = triangle.index3; + result.flags = triangleFlags; + } + + return result; +} + +int b3CollideMoverAndMesh( b3PlaneResult* planes, int capacity, const b3Mesh* shape, const b3Capsule* mover ) +{ + if ( capacity == 0 ) + { + return 0; + } + + b3DistanceInput distanceInput = { 0 }; + distanceInput.proxyB = (b3ShapeProxy){ &mover->center1, 2, 0.0f }; + distanceInput.transform = b3Transform_identity; + distanceInput.useRadii = false; + + b3SimplexCache cache = { 0 }; + float radius = mover->radius; + + b3V32 center1 = b3LoadV( &mover->center1.x ); + b3V32 center2 = b3LoadV( &mover->center2.x ); + b3V32 r = b3SplatV( radius ); + b3V32 boundsMin = b3SubV( b3MinV( center1, center2 ), r ); + b3V32 boundsMax = b3AddV( b3MaxV( center1, center2 ), r ); + + // Scale may have reflection so min/max may become invalid when unscaled + b3Vec3 meshScale = shape->scale; + b3V32 scale = b3LoadV( &meshScale.x ); + b3V32 invScale = b3DivV( b3_oneV, scale ); + b3V32 temp1 = b3MulV( invScale, boundsMin ); + b3V32 temp2 = b3MulV( invScale, boundsMax ); + b3V32 invScaledBoundsMin = b3MinV( temp1, temp2 ); + b3V32 invScaledBoundsMax = b3MaxV( temp1, temp2 ); + b3V32 invScaledBoundsCenter = b3MulV( b3_halfV, b3AddV( invScaledBoundsMin, invScaledBoundsMax ) ); + b3V32 invScaledBoundsExtent = b3SubV( invScaledBoundsMax, invScaledBoundsCenter ); + + int count = 0; + const b3MeshNode* stack[B3_MESH_STACK_SIZE]; + const b3MeshNode* node = b3GetRoot( shape->data ); + const b3MeshTriangle* triangles = b3GetMeshTriangles( shape->data ); + const b3Vec3* vertices = b3GetMeshVertices( shape->data ); + + int planeCount = 0; + while ( planeCount < capacity ) + { + // Test node overlap in unscaled space + b3V32 nodeMin = b3LoadV( &node->lowerBound.x ); + b3V32 nodeMax = b3LoadV( &node->upperBound.x ); + if ( b3TestBoundsOverlap( nodeMin, nodeMax, invScaledBoundsMin, invScaledBoundsMax ) ) + { + if ( b3IsLeaf( node ) ) + { + int triangleCount = node->data.asLeaf.triangleCount; + int triangleOffset = node->triangleOffset; + + for ( int index = 0; index < triangleCount; ++index ) + { + int triangleIndex = triangleOffset + index; + b3MeshTriangle triangle = triangles[triangleIndex]; + + b3Vec3 vertex1 = vertices[triangle.index1]; + b3Vec3 vertex2 = vertices[triangle.index2]; + b3Vec3 vertex3 = vertices[triangle.index3]; + b3V32 v1 = b3LoadV( &vertex1.x ); + b3V32 v2 = b3LoadV( &vertex2.x ); + b3V32 v3 = b3LoadV( &vertex3.x ); + + // Test triangle bounds overlap in unscaled space + if ( b3TestBoundsTriangleOverlap( invScaledBoundsCenter, invScaledBoundsExtent, v1, v2, v3 ) ) + { + // Compute shape distance in scaled space. Winding order doesn't matter. + // todo implement one-sided collision? + b3Vec3 triangleVertices[] = { b3Mul( meshScale, vertex1 ), b3Mul( meshScale, vertex2 ), + b3Mul( meshScale, vertex3 ) }; + distanceInput.proxyA = (b3ShapeProxy){ triangleVertices, 3, 0.0f }; + + // reset the cache + cache.count = 0; + + // get distance between triangle and query shape + b3DistanceOutput distanceOutput = b3ShapeDistance( &distanceInput, &cache, NULL, 0 ); + + if ( distanceOutput.distance == 0.0f ) + { + // todo SAT + } + else if ( distanceOutput.distance <= mover->radius ) + { + b3Plane plane = { distanceOutput.normal, mover->radius - distanceOutput.distance }; + planes[planeCount] = (b3PlaneResult){ plane, distanceOutput.pointA }; + planeCount += 1; + + if ( planeCount == capacity ) + { + return planeCount; + } + } + } + } + } + else + { + // Recurse + B3_ASSERT( count <= B3_MESH_STACK_SIZE - 1 ); + stack[count++] = b3GetRightChild( node ); + node = b3GetLeftChild( node ); + + continue; + } + } + + if ( count == 0 ) + { + break; + } + node = stack[--count]; + } + + return planeCount; +} + +void b3QueryMesh( const b3Mesh* mesh, b3AABB bounds, b3MeshQueryFcn* fcn, void* context ) +{ + b3Vec3 meshScale = mesh->scale; + bool clockwise = meshScale.x * meshScale.y * meshScale.z > 0.0f; + + // Scale may have reflection so min/max may become invalid when unscaled + b3V32 scale = b3LoadV( &meshScale.x ); + b3V32 invScale = b3DivV( b3_oneV, scale ); + b3V32 temp1 = b3MulV( invScale, b3LoadV( &bounds.lowerBound.x ) ); + b3V32 temp2 = b3MulV( invScale, b3LoadV( &bounds.upperBound.x ) ); + b3V32 invScaledBoundsMin = b3MinV( temp1, temp2 ); + b3V32 invScaledBoundsMax = b3MaxV( temp1, temp2 ); + b3V32 invScaledBoundsCenter = b3MulV( b3_halfV, b3AddV( invScaledBoundsMin, invScaledBoundsMax ) ); + b3V32 invScaledBoundsExtent = b3SubV( invScaledBoundsMax, invScaledBoundsCenter ); + + const b3MeshData* data = mesh->data; + + int count = 0; + const b3MeshNode* stack[B3_MESH_STACK_SIZE]; + const b3MeshNode* node = b3GetRoot( data ); + const b3MeshTriangle* triangles = b3GetMeshTriangles( data ); + const b3Vec3* vertices = b3GetMeshVertices( data ); + + while ( true ) + { + // Test node overlap in unscaled space + b3V32 nodeMin = b3LoadV( &node->lowerBound.x ); + b3V32 nodeMax = b3LoadV( &node->upperBound.x ); + + if ( b3TestBoundsOverlap( nodeMin, nodeMax, invScaledBoundsMin, invScaledBoundsMax ) ) + { + if ( b3IsLeaf( node ) ) + { + int triangleCount = node->data.asLeaf.triangleCount; + int triangleOffset = node->triangleOffset; + + for ( int index = 0; index < triangleCount; ++index ) + { + int triangleIndex = triangleOffset + index; + b3MeshTriangle triangle = triangles[triangleIndex]; + + b3Vec3 vertex1 = vertices[triangle.index1]; + b3Vec3 vertex2 = vertices[triangle.index2]; + b3Vec3 vertex3 = vertices[triangle.index3]; + b3V32 v1 = b3LoadV( &vertex1.x ); + b3V32 v2 = b3LoadV( &vertex2.x ); + b3V32 v3 = b3LoadV( &vertex3.x ); + + // Perform triangle overlap test in unscaled space. Winding order doesn't matter. + // todo it is possible that some margins are getting scaled + if ( b3TestBoundsTriangleOverlap( invScaledBoundsCenter, invScaledBoundsExtent, v1, v2, v3 ) ) + { + b3Vec3 a = b3Mul( meshScale, vertex1 ); + b3Vec3 b, c; + if ( clockwise ) + { + b = b3Mul( meshScale, vertex2 ); + c = b3Mul( meshScale, vertex3 ); + } + else + { + b = b3Mul( meshScale, vertex3 ); + c = b3Mul( meshScale, vertex2 ); + } + + bool result = fcn( a, b, c, triangleIndex, context ); + if ( result == false ) + { + return; + } + } + } + } + else + { + // Recurse + B3_ASSERT( count <= B3_MESH_STACK_SIZE - 1 ); + stack[count++] = b3GetRightChild( node ); + node = b3GetLeftChild( node ); + + continue; + } + } + + if ( count == 0 ) + { + break; + } + node = stack[--count]; + } +} diff --git a/vendor/box3d/src/src/mesh_contact.c b/vendor/box3d/src/src/mesh_contact.c new file mode 100644 index 000000000..a7163477a --- /dev/null +++ b/vendor/box3d/src/src/mesh_contact.c @@ -0,0 +1,1186 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +#include "contact.h" +#include "manifold.h" +#include "physics_world.h" +#include "qsort.h" +#include "shape.h" + +#include "box3d/types.h" + +#include + +// This guards against excessive memory usage and complex collision +#define B3_MAX_MESH_CONTACT_TRIANGLES 256 +#define B3_MAX_POINTS_PER_TRIANGLE 32 + +#if B3_ENABLE_VALIDATION +static bool b3IsSorted( const int* array, int count ) +{ + for ( int i = 0; i < count - 1; ++i ) + { + if ( array[i] >= array[i + 1] ) + { + return false; + } + } + + return true; +} +#endif + +typedef struct b3TriangleQueryContext +{ + int* indices; + int capacity; + int count; +} b3TriangleQueryContext; + +static bool b3CollectTriangleIndicesCallback( b3Vec3 a, b3Vec3 b, b3Vec3 c, int triangleIndex, void* context ) +{ + B3_UNUSED( a, b, c ); + b3TriangleQueryContext* triangleContext = (b3TriangleQueryContext*)context; + if ( triangleContext->count == triangleContext->capacity ) + { + return false; + } + triangleContext->indices[triangleContext->count] = triangleIndex; + triangleContext->count += 1; + return triangleContext->count < triangleContext->capacity; +} + +static int b3QueryMeshTriangles( int* indices, int capacity, const b3Mesh* mesh, b3AABB bounds ) +{ + b3TriangleQueryContext context = { + .indices = indices, + .capacity = capacity, + .count = 0, + }; + + b3QueryMesh( mesh, bounds, b3CollectTriangleIndicesCallback, &context ); + return context.count; +} + +static int b3QueryHeightFieldTriangles( int* indices, int capacity, const b3HeightFieldData* heightField, b3AABB bounds ) +{ + b3TriangleQueryContext context = { + .indices = indices, + .capacity = capacity, + .count = 0, + }; + + b3QueryHeightField( heightField, bounds, b3CollectTriangleIndicesCallback, &context ); + return context.count; +} + +static void b3RefreshCache( b3Contact* contact, const b3Shape* shapeA, b3WorldTransform xfA, const b3AABB* bounds ) +{ + B3_ASSERT( shapeA->type == b3_meshShape || shapeA->type == b3_heightShape ); + + b3MeshContact* meshContact = &contact->meshContact; + + // If the dynamic body didn't move out of the cached query bounds we are done! + if ( b3AABB_Contains( meshContact->queryBounds, *bounds ) ) + { + if ( shapeA->type == b3_meshShape ) + { + for ( int i = 0; i < contact->meshContact.triangleCache.count; ++i ) + { + B3_ASSERT( 0 <= contact->meshContact.triangleCache.data[i].triangleIndex && + contact->meshContact.triangleCache.data[i].triangleIndex < shapeA->mesh.data->triangleCount ); + } + } + + return; + } + + // Enlarge to the query bounds to absorb small movement + float radius = B3_MAX_AABB_MARGIN + B3_SPECULATIVE_DISTANCE; + b3Vec3 extension = { radius, radius, radius }; + meshContact->queryBounds.lowerBound = b3Sub( bounds->lowerBound, extension ); + meshContact->queryBounds.upperBound = b3Add( bounds->upperBound, extension ); + + // Query triangles + int triangleCapacity = B3_MAX_MESH_CONTACT_TRIANGLES; + + int triangleIndices[B3_MAX_MESH_CONTACT_TRIANGLES]; + + // Bounds are in world space. Convert to the local mesh frame. The broadphase bounds are float, + // so the demoted mesh transform is the matching float world frame (exact in float mode). + b3Transform meshTransform = b3ToRelativeTransform( xfA, b3Pos_zero ); + b3AABB localBounds = b3AABB_Transform( b3InvertTransform( meshTransform ), meshContact->queryBounds ); + int triangleCount; + if ( shapeA->type == b3_meshShape ) + { + triangleCount = b3QueryMeshTriangles( triangleIndices, triangleCapacity, &shapeA->mesh, localBounds ); + } + else + { + B3_ASSERT( shapeA->type == b3_heightShape ); + triangleCount = b3QueryHeightFieldTriangles( triangleIndices, triangleCapacity, shapeA->heightField, localBounds ); + } + + if ( triangleCount == triangleCapacity ) + { + static bool s_once = false; + if ( s_once == false ) + { + b3Log( "WARNING: complex mesh detected, triangle buffer capacity of %d reached", triangleCapacity ); + s_once = true; + } + } + + // Triangle indices must be sorted to match caches. + B3_VALIDATE( b3IsSorted( triangleIndices, triangleCount ) ); + + // Create new contact cache and match with old one + b3ContactCache contactCache[B3_MAX_MESH_CONTACT_TRIANGLES]; + + int index2 = 0; + for ( int index1 = 0; index1 < triangleCount; ++index1 ) + { + contactCache[index1] = (b3ContactCache){ 0 }; + + while ( index2 < contact->meshContact.triangleCache.count && + contact->meshContact.triangleCache.data[index2].triangleIndex < triangleIndices[index1] ) + { + index2 += 1; + } + + if ( index2 < contact->meshContact.triangleCache.count && + contact->meshContact.triangleCache.data[index2].triangleIndex == triangleIndices[index1] ) + { + contactCache[index1] = contact->meshContact.triangleCache.data[index2].cache; + } + } + + // Save new cache + b3Array_Resize( contact->meshContact.triangleCache, triangleCount ); + for ( int i = 0; i < triangleCount; ++i ) + { + contact->meshContact.triangleCache.data[i] = (b3TriangleCache){ triangleIndices[i], contactCache[i] }; + + if ( shapeA->type == b3_meshShape ) + { + B3_ASSERT( 0 <= contact->meshContact.triangleCache.data[i].triangleIndex && + contact->meshContact.triangleCache.data[i].triangleIndex < shapeA->mesh.data->triangleCount ); + } + } +} + +typedef struct b3TentativeTriangle +{ + float squaredDistance; + int index; +} b3TentativeTriangle; + +#define B3_MAX_EDGE_COUNT 64 + +typedef struct b3FoundEdges +{ + uint64_t keys[B3_MAX_EDGE_COUNT]; + int count; +} b3FoundEdges; + +static inline bool b3AddEdge( b3FoundEdges* edges, int vertex1, int vertex2 ) +{ + uint64_t i1 = (uint64_t)b3MinInt( vertex1, vertex2 ); + uint64_t i2 = (uint64_t)b3MaxInt( vertex1, vertex2 ); + uint64_t key = i1 << 32 | i2; + + int count = edges->count; + for ( int i = 0; i < count; ++i ) + { + if ( edges->keys[i] == key ) + { + return false; + } + } + + if ( count == B3_MAX_EDGE_COUNT ) + { + // This will lead to a potential ghost collision + return true; + } + + edges->keys[count] = key; + edges->count += 1; + + return true; +} + +static inline bool b3FindEdge( b3FoundEdges* edges, int vertex1, int vertex2 ) +{ + uint64_t i1 = (uint64_t)b3MinInt( vertex1, vertex2 ); + uint64_t i2 = (uint64_t)b3MaxInt( vertex1, vertex2 ); + uint64_t key = i1 << 32 | i2; + + int count = edges->count; + for ( int i = 0; i < count; ++i ) + { + if ( edges->keys[i] == key ) + { + return true; + } + } + + return false; +} + +#if 0 +// Two triangles share an edge iff they share at least two vertex indices. +static inline bool b3TrianglesShareEdge( int a1, int a2, int a3, int b1, int b2, int b3 ) +{ + int matches = 0; + matches += ( a1 == b1 || a1 == b2 || a1 == b3 ); + matches += ( a2 == b1 || a2 == b2 || a2 == b3 ); + matches += ( a3 == b1 || a3 == b2 || a3 == b3 ); + return matches >= 2; +} +#endif + +#define B3_MAX_VERTEX_COUNT 64 + +typedef struct b3FoundVertices +{ + int keys[B3_MAX_VERTEX_COUNT]; + int count; +} b3FoundVertices; + +static inline bool b3AddVertex( b3FoundVertices* vertices, int vertex ) +{ + int key = vertex; + + int count = vertices->count; + for ( int i = 0; i < count; ++i ) + { + if ( vertices->keys[i] == key ) + { + return false; + } + } + + if ( count == B3_MAX_VERTEX_COUNT ) + { + // This will lead to a potential ghost collision + return true; + } + + vertices->keys[count] = key; + vertices->count += 1; + + return true; +} + +// Returns true if (score, separation) should replace (bestScore, bestSeparation). +static inline bool b3IsBetterCullCandidate( float score, float separation, float bestScore, float bestSeparation, float scoreTol, + float separationTol ) +{ + if ( score > bestScore + scoreTol ) + { + return true; + } + if ( score < bestScore - scoreTol ) + { + return false; + } + + // Break the tie using separation + return separation < bestSeparation - separationTol; +} + +typedef struct b3Point2D +{ + b3Vec2 p; + float separation; + int originalIndex; +} b3Point2D; + +static int b3CullPoints( b3Point2D* points, int count ) +{ + if ( count <= 1 ) + { + return count; + } + + float tol = 0.25f * B3_LINEAR_SLOP; + float tolSqr = tol * tol; + float separationTol = B3_LINEAR_SLOP; + + b3Point2D finalPoints[4]; + int count1 = count; + + // Step 1: the two points with the largest distance, ties broken by deepest combined separation + float bestScore = 0.0f; + float bestSeparation = FLT_MAX; + int bestIndex1 = B3_NULL_INDEX; + int bestIndex2 = B3_NULL_INDEX; + + for ( int i = 0; i < count1; ++i ) + { + b3Vec2 p1 = points[i].p; + for ( int j = i + 1; j < count1; ++j ) + { + float score = b3DistanceSquared2( p1, points[j].p ); + // Separation sum heuristic + float separation = points[i].separation + points[j].separation; + + if ( b3IsBetterCullCandidate( score, separation, bestScore, bestSeparation, tolSqr, separationTol ) ) + { + bestIndex1 = i; + bestIndex2 = j; + bestScore = score; + bestSeparation = separation; + } + } + } + + if ( bestScore < tolSqr ) + { + // Choose deepest point + int deepestIndex = 0; + for ( int i = 1; i < count1; ++i ) + { + if ( points[i].separation < points[deepestIndex].separation ) + { + deepestIndex = i; + } + } + + if ( deepestIndex != 0 ) + { + points[0] = points[deepestIndex]; + } + return 1; + } + + finalPoints[0] = points[bestIndex1]; + finalPoints[1] = points[bestIndex2]; + + // Cull + points[bestIndex2] = points[count1 - 1]; + points[bestIndex1] = points[count1 - 2]; + count1 -= 2; + + if ( count1 == 0 ) + { + points[0] = finalPoints[0]; + points[1] = finalPoints[1]; + return 2; + } + + // First anchor point + b3Vec2 a = finalPoints[0].p; + + // Second anchor point + b3Vec2 b = finalPoints[1].p; + b3Vec2 ba = b3Sub2( b, a ); + // float length = b3Length2( ba ); + // float areaTol = tol * length; + + // Step 2: find the point with the maximum triangular area, ties broken by deepest separation + bestScore = 0.0f; + bestSeparation = FLT_MAX; + int bestIndex = B3_NULL_INDEX; + float bestSignedArea = 0.0f; + for ( int i = 0; i < count1; ++i ) + { + b3Vec2 p = points[i].p; + float signedArea = b3Cross2( ba, b3Sub2( p, a ) ); + float score = b3AbsFloat( signedArea ); + + if ( b3IsBetterCullCandidate( score, points[i].separation, bestScore, bestSeparation, tolSqr, separationTol ) ) + { + bestSignedArea = signedArea; + bestScore = score; + bestSeparation = points[i].separation; + bestIndex = i; + } + } + + if ( bestIndex == B3_NULL_INDEX ) + { + // All points collinear + points[0] = finalPoints[0]; + points[1] = finalPoints[1]; + return 2; + } + + // Store best point + finalPoints[2] = points[bestIndex]; + + if ( count1 == 1 ) + { + points[0] = finalPoints[0]; + points[1] = finalPoints[1]; + points[2] = finalPoints[2]; + return 3; + } + + // Cull + points[bestIndex] = points[count1 - 1]; + count1 -= 1; + + // Step 4: get the point that adds the most area outside the current triangle + + // Third anchor + b3Vec2 c = finalPoints[2].p; + + // Ensure CCW ordering + if ( bestSignedArea < 0.0f ) + { + B3_SWAP( b, c ); + ba = b3Sub2( b, a ); + } + + b3Vec2 cb = b3Sub2( c, b ); + b3Vec2 ac = b3Sub2( a, c ); + + bestScore = 0.0f; + bestSeparation = FLT_MAX; + bestIndex = B3_NULL_INDEX; + for ( int i = 0; i < count1; ++i ) + { + b3Vec2 p = points[i].p; + float u1 = b3Cross2( b3Sub2( p, a ), ba ); + float u2 = b3Cross2( b3Sub2( p, b ), cb ); + float u3 = b3Cross2( b3Sub2( p, c ), ac ); + float score = b3MaxFloat( u1, b3MaxFloat( u2, u3 ) ); + + // Use the area tolerance for collinear points and hysteresis + if ( b3IsBetterCullCandidate( score, points[i].separation, bestScore, bestSeparation, tolSqr, separationTol ) ) + { + bestScore = score; + bestSeparation = points[i].separation; + bestIndex = i; + } + } + + if ( bestIndex == B3_NULL_INDEX ) + { + // No additional area + points[0] = finalPoints[0]; + points[1] = finalPoints[1]; + points[2] = finalPoints[2]; + return 3; + } + + // Store best point + finalPoints[3] = points[bestIndex]; + + // Full quad + points[0] = finalPoints[0]; + points[1] = finalPoints[1]; + points[2] = finalPoints[2]; + points[3] = finalPoints[3]; + return 4; +} + +static int b3ReduceCluster( b3LocalManifoldPoint* points, int count1, b3Vec3 normal, b3Arena arena ) +{ + int targetCount = 1; + if ( count1 <= targetCount ) + { + return count1; + } + + b3Point2D* pts = b3Bump( &arena, count1 * sizeof( b3Point2D ) ); + b3Vec3 u = b3Perp( normal ); + b3Vec3 v = b3Cross( normal, u ); + b3Vec3 origin = points[0].point; + + for ( int i = 0; i < count1; ++i ) + { + b3Vec3 d = b3Sub( points[i].point, origin ); + pts[i].p = (b3Vec2){ b3Dot( d, u ), b3Dot( d, v ) }; + pts[i].separation = points[i].separation; + pts[i].originalIndex = i; + } + + int count2 = b3CullPoints( pts, count1 ); + B3_ASSERT( count2 <= B3_MAX_MANIFOLD_POINTS ); + + b3LocalManifoldPoint finalPoints[B3_MAX_MANIFOLD_POINTS]; + for ( int i = 0; i < count2; ++i ) + { + int index = pts[i].originalIndex; + B3_ASSERT( 0 <= index && index < count1 ); + finalPoints[i] = points[index]; + } + + memcpy( points, finalPoints, count2 * sizeof( b3LocalManifoldPoint ) ); + return count2; +} + +typedef struct b3Cluster +{ + b3Vec3 manifoldNormal; + b3Vec3 triangleNormal; + b3LocalManifoldPoint* points; + int pointCapacity; + int pointCount; +} b3Cluster; + +bool b3ComputeMeshManifolds( b3World* world, int workerIndex, b3Contact* contact, const b3Shape* shapeA, const int* materialMap, + b3WorldTransform xfA, const b3Shape* shapeB, b3WorldTransform xfB, bool isFast, b3Arena arena ) +{ + B3_ASSERT( shapeA->type == b3_meshShape || shapeA->type == b3_heightShape ); + B3_UNUSED( workerIndex ); + B3_UNUSED( isFast ); + B3_UNUSED( materialMap ); + + b3TaskContext* context = b3Array_Get( world->taskContexts, workerIndex ); + + b3RefreshCache( contact, shapeA, xfA, &shapeB->aabb ); + + // Collide with triangles and build manifolds + b3MeshContact* meshContact = &contact->meshContact; + int triangleCount = meshContact->triangleCache.count; + + b3LocalManifold** acceptedManifolds = b3Bump( &arena, triangleCount * sizeof( b3LocalManifold* ) ); + int acceptedManifoldCount = 0; + b3LocalManifold** tentativeManifolds = b3Bump( &arena, triangleCount * sizeof( b3LocalManifold* ) ); + int tentativeManifoldCount = 0; + b3TentativeTriangle* tentativeTriangles = b3Bump( &arena, triangleCount * sizeof( b3TentativeTriangle ) ); + int tentativeTriangleCount = 0; + + b3FoundEdges foundEdges; + b3FoundVertices foundVertices; + foundEdges.count = 0; + foundVertices.count = 0; + + // This transform converts from mesh frame into the shapeB frame + b3Transform transformAtoB = b3InvMulWorldTransforms( xfB, xfA ); + b3Matrix3 relativeMatrix = b3MakeMatrixFromQuat( transformAtoB.q ); + float linearSlop = B3_LINEAR_SLOP; + + // This should push apart shapes after a time of impact event. + // In the past I've called this `polygon skin`, but PhysX and Unreal + // call it `rest offset` which seems appropriate in this case. + // It leads to a small visual gap but seems to improve the quality of mesh + // collision, especially for hull versus mesh. + float restOffset = B3_MESH_REST_OFFSET; + bool enableSpeculative = contact->flags & b3_enableSpeculativePoints; + + // Make room for clip points + int pointBufferCapacity = B3_MAX_POINTS_PER_TRIANGLE * triangleCount; + + b3LocalManifoldPoint* pointBuffer = b3Bump( &arena, pointBufferCapacity * sizeof( b3LocalManifoldPoint ) ); + int totalPointCount = 0; + + b3LocalManifold* manifoldBuffer = b3Bump( &arena, triangleCount * sizeof( b3LocalManifold ) ); + int manifoldCount = 0; + + b3TriangleCache* triangleCaches = meshContact->triangleCache.data; + + const b3HullData* hullB = shapeB->type == b3_hullShape ? shapeB->hull : NULL; + + for ( int index = 0; index < triangleCount && totalPointCount + 3 < pointBufferCapacity; ++index ) + { + int triangleIndex = triangleCaches[index].triangleIndex; + + b3Triangle triangle; + if ( shapeA->type == b3_meshShape ) + { + triangle = b3GetMeshTriangle( &shapeA->mesh, triangleIndex ); + } + else + { + B3_ASSERT( shapeA->type == b3_heightShape ); + triangle = b3GetHeightFieldTriangle( shapeA->heightField, triangleIndex ); + } + + // Transform triangle into the shape frame + b3Vec3 vertices[3]; + vertices[0] = b3Add( b3MulMV( relativeMatrix, triangle.vertices[0] ), transformAtoB.p ); + vertices[1] = b3Add( b3MulMV( relativeMatrix, triangle.vertices[1] ), transformAtoB.p ); + vertices[2] = b3Add( b3MulMV( relativeMatrix, triangle.vertices[2] ), transformAtoB.p ); + + b3ContactCache* cache = &triangleCaches[index].cache; + int pointCapacity = pointBufferCapacity - totalPointCount; + b3LocalManifold* manifold = manifoldBuffer + manifoldCount; + manifold->points = pointBuffer + totalPointCount; + manifold->pointCount = 0; + manifold->triangleFlags = triangle.flags; + manifold->feature = b3_featureNone; + + switch ( shapeB->type ) + { + case b3_capsuleShape: + b3CollideCapsuleAndTriangle( manifold, pointCapacity, &shapeB->capsule, vertices, &cache->simplexCache ); + break; + + case b3_hullShape: + // Cached edge contact is dangerous at high speed because the hull can rotate around the edge and tunnel + // through the triangle. + if ( isFast && cache->satCache.type == b3_edgePairAxis ) + { + cache->satCache = (b3SATCache){ 0 }; + } + + b3CollideHullAndTriangle( manifold, pointCapacity, hullB, vertices[0], vertices[1], vertices[2], triangle.flags, + &cache->satCache, enableSpeculative ); + context->satCallCount += 1; + context->satCacheHitCount += cache->satCache.hit; + break; + + case b3_sphereShape: + b3CollideSphereAndTriangle( manifold, pointCapacity, &shapeB->sphere, vertices ); + break; + + default: + B3_ASSERT( false ); + return false; + } + + int manifoldPointCount = manifold->pointCount; + + if ( manifoldPointCount > 0 ) + { + B3_ASSERT( manifold->feature != b3_featureNone ); + + manifoldCount += 1; + totalPointCount += manifoldPointCount; + manifold->triangleIndex = triangleIndex; + manifold->triangleNormal = b3MakeNormalFromPoints( vertices[0], vertices[1], vertices[2] ); + manifold->i1 = triangle.i1; + manifold->i2 = triangle.i2; + manifold->i3 = triangle.i3; + + if ( manifold->feature == b3_featureTriangleFace || B3_FORCE_GHOST_COLLISIONS ) + { + (void)b3AddEdge( &foundEdges, triangle.i1, triangle.i2 ); + (void)b3AddEdge( &foundEdges, triangle.i2, triangle.i3 ); + (void)b3AddEdge( &foundEdges, triangle.i3, triangle.i1 ); + (void)b3AddVertex( &foundVertices, triangle.i1 ); + (void)b3AddVertex( &foundVertices, triangle.i2 ); + (void)b3AddVertex( &foundVertices, triangle.i3 ); + + acceptedManifolds[acceptedManifoldCount++] = manifold; + } + else if ( manifold->feature == b3_featureHullFace ) + { + float cosNormalAngle = b3Dot( manifold->triangleNormal, manifold->normal ); + if ( cosNormalAngle > 0.5f ) + { + (void)b3AddEdge( &foundEdges, triangle.i1, triangle.i2 ); + (void)b3AddEdge( &foundEdges, triangle.i2, triangle.i3 ); + (void)b3AddEdge( &foundEdges, triangle.i3, triangle.i1 ); + (void)b3AddVertex( &foundVertices, triangle.i1 ); + (void)b3AddVertex( &foundVertices, triangle.i2 ); + (void)b3AddVertex( &foundVertices, triangle.i3 ); + + acceptedManifolds[acceptedManifoldCount++] = manifold; + } + else + { + float minSeparation = manifold->points[0].separation; + for ( int i = 1; i < manifoldPointCount; ++i ) + { + minSeparation = b3MinFloat( minSeparation, manifold->points[i].separation ); + } + + if ( minSeparation < -2.0f * linearSlop ) + { + // Deep overlap + (void)b3AddEdge( &foundEdges, triangle.i1, triangle.i2 ); + (void)b3AddEdge( &foundEdges, triangle.i2, triangle.i3 ); + (void)b3AddEdge( &foundEdges, triangle.i3, triangle.i1 ); + (void)b3AddVertex( &foundVertices, triangle.i1 ); + (void)b3AddVertex( &foundVertices, triangle.i2 ); + (void)b3AddVertex( &foundVertices, triangle.i3 ); + acceptedManifolds[acceptedManifoldCount++] = manifold; + } + else + { + b3TentativeTriangle tentativeTriangle = { .squaredDistance = manifold->squaredDistance, + .index = tentativeManifoldCount }; + tentativeTriangles[tentativeTriangleCount++] = tentativeTriangle; + tentativeManifolds[tentativeManifoldCount++] = manifold; + } + } + } + else + { + b3TentativeTriangle tentativeTriangle = { .squaredDistance = manifold->squaredDistance, + .index = tentativeManifoldCount }; + tentativeTriangles[tentativeTriangleCount++] = tentativeTriangle; + tentativeManifolds[tentativeManifoldCount++] = manifold; + } + } + } + + B3_ASSERT( acceptedManifoldCount <= triangleCount ); + B3_ASSERT( tentativeManifoldCount <= triangleCount ); + B3_ASSERT( tentativeTriangleCount <= triangleCount ); + + if ( shapeB->type == b3_sphereShape ) + { + // Sort triangles so the closest triangles are processed first + { +#define LESS( i, j ) tentativeTriangles[(int)i].squaredDistance < tentativeTriangles[(int)j].squaredDistance +#define SWAP( i, j ) \ + do \ + { \ + b3TentativeTriangle tmp = tentativeTriangles[(int)i]; \ + tentativeTriangles[(int)i] = tentativeTriangles[(int)j]; \ + tentativeTriangles[(int)j] = tmp; \ + } \ + while ( 0 ) + QSORT( tentativeTriangleCount, LESS, SWAP ); +#undef LESS +#undef SWAP + } + + // Add tentative manifolds in sorted order. Avoid adding manifolds that generate ghost collisions. + for ( int i = 0; i < tentativeTriangleCount; ++i ) + { + b3LocalManifold* m = tentativeManifolds[tentativeTriangles[i].index]; + + bool addedEdge1 = b3AddEdge( &foundEdges, m->i1, m->i2 ); + bool addedEdge2 = b3AddEdge( &foundEdges, m->i2, m->i3 ); + bool addedEdge3 = b3AddEdge( &foundEdges, m->i3, m->i1 ); + bool addedVertex1 = b3AddVertex( &foundVertices, m->i1 ); + bool addedVertex2 = b3AddVertex( &foundVertices, m->i2 ); + bool addedVertex3 = b3AddVertex( &foundVertices, m->i3 ); + + b3TriangleFeature feature = m->feature; + bool shouldCollide = false; + switch ( feature ) + { + case b3_featureNone: + case b3_featureTriangleFace: + B3_ASSERT( false ); + break; + + case b3_featureEdge1: + shouldCollide = addedEdge1; + break; + + case b3_featureEdge2: + shouldCollide = addedEdge2; + break; + + case b3_featureEdge3: + shouldCollide = addedEdge3; + break; + + case b3_featureVertex1: + shouldCollide = addedVertex1; + break; + + case b3_featureVertex2: + shouldCollide = addedVertex2; + break; + + case b3_featureVertex3: + shouldCollide = addedVertex3; + break; + + default: + B3_ASSERT( false ); + break; + } + + if ( shouldCollide == true ) + { + acceptedManifolds[acceptedManifoldCount++] = m; + } + } + } + else + { + // Problem: hull can tunnel if time of impact is at concave edge + // Example: flat box sliding down a ramp to a flat bottom + // Solution: only ignore flat edges + for ( int i = 0; i < tentativeManifoldCount; ++i ) + { + b3LocalManifold* m = tentativeManifolds[i]; + int triangleFlags = m->triangleFlags; + + if ( ( triangleFlags & b3_allFlatEdges ) == b3_allFlatEdges ) + { + continue; + } + + if ( ( triangleFlags & b3_flatEdge1 ) == b3_flatEdge1 ) + { + if ( b3FindEdge( &foundEdges, m->i1, m->i2 ) ) + { + continue; + } + } + + if ( ( triangleFlags & b3_flatEdge2 ) == b3_flatEdge2 ) + { + if ( b3FindEdge( &foundEdges, m->i2, m->i3 ) ) + { + continue; + } + } + + if ( ( triangleFlags & b3_flatEdge3 ) == b3_flatEdge3 ) + { + if ( b3FindEdge( &foundEdges, m->i3, m->i1 ) ) + { + continue; + } + } + + acceptedManifolds[acceptedManifoldCount++] = m; + } + } + + B3_ASSERT( acceptedManifoldCount <= triangleCount ); + + if ( acceptedManifoldCount == 0 ) + { + if ( contact->manifoldCount > 0 ) + { + b3FreeManifolds( world, contact->manifolds, contact->manifoldCount ); + contact->manifolds = NULL; + contact->manifoldCount = 0; + } + return false; + } + + b3Cluster* clusters = b3Bump( &arena, acceptedManifoldCount * sizeof( b3Cluster ) ); + int* clusterMemberships = b3Bump( &arena, acceptedManifoldCount * sizeof( int ) ); + + // Cluster tolerance is tighter than the warm starting manifold matching tolerance. These + // serve different purposes. + const float clusterThreshold = 0.996f; + int clusterCount = 0; + int clusterPointCount = 0; + for ( int i = 0; i < acceptedManifoldCount; ++i ) + { + clusterMemberships[i] = B3_NULL_INDEX; + + const b3LocalManifold* manifold = acceptedManifolds[i]; + clusterPointCount += manifold->pointCount; + + // Cluster based on the triangle normal and contact normal. + // The first cluster found is accepted because the tolerance is tight. + // todo consider requiring the triangles to be connect by an edge. + // todo consider looking for the best cluster instead of the first one within tolerance + // This bool is here to allow quick testing with and without clustering. + bool allowClustering = true; + b3Vec3 manifoldNormal = manifold->normal; + b3Vec3 triangleNormal = manifold->triangleNormal; + int clusterIndex = B3_NULL_INDEX; + for ( int j = 0; j < clusterCount && allowClustering; ++j ) + { + float cosManifoldAngle = b3Dot( clusters[j].manifoldNormal, manifoldNormal ); + float cosTriangleAngle = b3Dot( clusters[j].triangleNormal, triangleNormal ); + if ( cosManifoldAngle <= clusterThreshold || cosTriangleAngle <= clusterThreshold ) + { + continue; + } + +#if 0 + // todo there could be later triangles that create the connection + // then failure to cluster breaks greedy impulse warm starting + bool edgeConnected = false; + + for ( int k = 0; k < i; ++k ) + { + if ( clusterMemberships[k] != j ) + { + continue; + } + + const b3LocalManifold* other = acceptedManifolds[k]; + if ( b3TrianglesShareEdge( manifold->i1, manifold->i2, manifold->i3, other->i1, other->i2, other->i3 ) ) + { + edgeConnected = true; + break; + } + } + + if ( edgeConnected ) + { + clusterIndex = j; + break; + } +#else + + // Found a cluster + clusterIndex = j; + break; +#endif + } + + if ( clusterIndex != B3_NULL_INDEX ) + { + clusterMemberships[i] = clusterIndex; + clusters[clusterIndex].pointCapacity += manifold->pointCount; + } + else + { + clusters[clusterCount].manifoldNormal = manifoldNormal; + clusters[clusterCount].triangleNormal = triangleNormal; + clusters[clusterCount].pointCapacity = manifold->pointCount; + clusterMemberships[i] = clusterCount; + clusterCount += 1; + } + } + + if ( clusterPointCount == 0 ) + { + return false; + } + + // Setup clusters + b3LocalManifoldPoint* clusterPoints = b3Bump( &arena, clusterPointCount * sizeof( b3LocalManifoldPoint ) ); + int pointOffset = 0; + + for ( int i = 0; i < clusterCount; ++i ) + { + b3Cluster* cluster = clusters + i; + cluster->points = clusterPoints + pointOffset; + cluster->pointCount = 0; + pointOffset += cluster->pointCapacity; + } + + // Populate clusters + for ( int i = 0; i < acceptedManifoldCount; ++i ) + { + int clusterIndex = clusterMemberships[i]; + if ( clusterIndex == B3_NULL_INDEX ) + { + continue; + } + + B3_ASSERT( 0 <= clusterIndex && clusterIndex < clusterCount ); + + b3LocalManifold* am = acceptedManifolds[i]; + b3Cluster* cm = clusters + clusterIndex; + for ( int j = 0; j < am->pointCount; ++j ) + { + B3_ASSERT( cm->pointCount < cm->pointCapacity ); + b3LocalManifoldPoint* ap = am->points + j; + b3LocalManifoldPoint* cp = cm->points + cm->pointCount; + + cp->triangleIndex = am->triangleIndex; + cp->point = ap->point; + cp->separation = ap->separation; + cp->pair = ap->pair; + cm->pointCount += 1; + } + } + + // Simplify clusters + for ( int i = 0; i < clusterCount; ++i ) + { + b3Cluster* cm = clusters + i; + B3_ASSERT( cm->pointCount == cm->pointCapacity ); + int reducedCount = b3ReduceCluster( cm->points, cm->pointCount, cm->triangleNormal, arena ); + cm->pointCount = reducedCount; + } + + // Make a temporary copy of previous manifolds + int oldManifoldCount = contact->manifoldCount; + b3Manifold* oldManifolds = NULL; + if ( oldManifoldCount > 0 ) + { + oldManifolds = b3Bump( &arena, oldManifoldCount * sizeof( b3Manifold ) ); + memcpy( oldManifolds, contact->manifolds, oldManifoldCount * sizeof( b3Manifold ) ); + } + + // Resize manifolds if needed + if ( oldManifoldCount != clusterCount ) + { + b3FreeManifolds( world, contact->manifolds, contact->manifoldCount ); + contact->manifolds = b3AllocateManifolds( world, clusterCount ); + contact->manifoldCount = (uint16_t)clusterCount; + } + else + { + // Mem zero manifolds + memset( contact->manifolds, 0, contact->manifoldCount * sizeof( b3Manifold ) ); + } + + bool* consumed = NULL; + if ( oldManifoldCount > 0 ) + { + consumed = b3Bump( &arena, oldManifoldCount * sizeof( bool ) ); + memset( consumed, 0, oldManifoldCount * sizeof( bool ) ); + } + + b3Matrix3 matrixB = b3MakeMatrixFromQuat( xfB.q ); + b3Vec3 offsetA = b3SubPos( xfB.p, xfA.p ); + + const float normalMatchTolerance = 0.995f; + for ( int i = 0; i < clusterCount; ++i ) + { + b3Cluster* cm = clusters + i; + int pointCount = cm->pointCount; + B3_ASSERT( 0 < pointCount && pointCount <= B3_MAX_MANIFOLD_POINTS ); + + b3Manifold* manifold = contact->manifolds + i; + manifold->pointCount = pointCount; + manifold->normal = b3MulMV( matrixB, cm->manifoldNormal ); + + b3Vec3 clusterNormal = b3MulMV( matrixB, cm->manifoldNormal ); + float bestDot = normalMatchTolerance; + int bestIndex = B3_NULL_INDEX; + + for ( int j = 0; j < oldManifoldCount; ++j ) + { + if ( consumed[j] == true ) + { + continue; + } + + float dot = b3Dot( oldManifolds[j].normal, clusterNormal ); + if ( dot > bestDot ) + { + bestIndex = j; + bestDot = dot; + } + } + + b3Manifold* matchedManifold = NULL; + if ( bestIndex != B3_NULL_INDEX ) + { + matchedManifold = oldManifolds + bestIndex; + manifold->frictionImpulse = matchedManifold->frictionImpulse; + manifold->rollingImpulse = matchedManifold->rollingImpulse; + manifold->twistImpulse = matchedManifold->twistImpulse; + consumed[bestIndex] = true; + } + + for ( int j = 0; j < pointCount; ++j ) + { + const b3LocalManifoldPoint* source = cm->points + j; + b3ManifoldPoint* target = manifold->points + j; + + // Contact points are computed in frame B + target->anchorB = b3MulMV( matrixB, source->point ); + target->anchorA = b3Add( target->anchorB, offsetA ); + target->separation = source->separation - restOffset; + target->featureId = b3MakeFeatureId( source->pair ); + target->triangleIndex = source->triangleIndex; + + // Preserve normal impulse if possible + if ( matchedManifold != NULL ) + { + int oldPointCount = matchedManifold->pointCount; + for ( int k = 0; k < oldPointCount; ++k ) + { + b3ManifoldPoint* oldPt = matchedManifold->points + k; + + if ( target->featureId == oldPt->featureId && target->triangleIndex == oldPt->triangleIndex ) + { + target->normalImpulse = oldPt->normalImpulse; + target->persisted = true; + + // claimed + oldPt->triangleIndex = B3_NULL_INDEX; + break; + } + } + } + } + } + + const b3SurfaceMaterial* materialsA = b3GetShapeMaterials( shapeA ); + const b3SurfaceMaterial* materialB = b3GetShapeMaterials( shapeB ); + b3Vec3 tangentVelocityA = b3Vec3_zero; + + // Update friction and restitution if the mesh has per triangle material + if ( shapeA->materialCount > 0 ) + { + float friction = 0.0f; + float restitution = 0.0f; + float sampleCount = 0.0f; + + const uint8_t* materialIndices; + if ( shapeA->type == b3_meshShape ) + { + materialIndices = b3GetMeshMaterialIndices( shapeA->mesh.data ); + } + else + { + materialIndices = b3GetHeightFieldMaterialIndices( shapeA->heightField ); + } + + for ( int i = 0; i < clusterCount; ++i ) + { + b3Manifold* manifold = contact->manifolds + i; + int pointCount = manifold->pointCount; + for ( int j = 0; j < pointCount; ++j ) + { + int triangleIndex = manifold->points[j].triangleIndex; + int materialIndex; + if ( shapeA->type == b3_meshShape ) + { + materialIndex = materialIndices[triangleIndex]; + + if ( materialMap != NULL ) + { + materialIndex = materialMap[materialIndex]; + } + } + else + { + materialIndex = materialIndices[triangleIndex >> 1]; + } + + materialIndex = b3ClampInt( materialIndex, 0, shapeA->materialCount - 1 ); + b3SurfaceMaterial material = materialsA[materialIndex]; + friction += world->frictionCallback( material.friction, material.userMaterialId, materialB->friction, + materialB->userMaterialId ); + restitution += world->restitutionCallback( material.restitution, material.userMaterialId, materialB->restitution, + materialB->userMaterialId ); + + tangentVelocityA = b3Add( tangentVelocityA, material.tangentVelocity ); + + sampleCount += 1.0f; + } + } + + if ( sampleCount > 0.0f ) + { + float invCount = 1.0f / sampleCount; + contact->friction = invCount * friction; + contact->restitution = invCount * restitution; + tangentVelocityA = b3MulSV( invCount, tangentVelocityA ); + } + + B3_ASSERT( b3IsValidFloat( contact->friction ) && contact->friction >= 0.0f ); + B3_ASSERT( b3IsValidFloat( contact->restitution ) && contact->restitution >= 0.0f ); + } + else + { + // Keep these updated in case the values on the shapes are modified + contact->friction = world->frictionCallback( materialsA[0].friction, materialsA[0].userMaterialId, materialB->friction, + materialB->userMaterialId ); + contact->restitution = world->restitutionCallback( materialsA[0].restitution, materialsA[0].userMaterialId, + materialB->restitution, materialB->userMaterialId ); + tangentVelocityA = materialsA[0].tangentVelocity; + } + + tangentVelocityA = b3RotateVector( xfA.q, tangentVelocityA ); + + float radiusB = 0.0f; + if ( shapeB->type == b3_sphereShape ) + { + radiusB = shapeB->sphere.radius; + } + else if ( shapeB->type == b3_capsuleShape ) + { + radiusB = shapeB->capsule.radius; + } + else if ( shapeB->type == b3_hullShape ) + { + radiusB = shapeB->hull->innerRadius; + } + + contact->rollingResistance = materialB->rollingResistance * radiusB; + + b3Vec3 tangentVelocityB = b3RotateVector( xfB.q, materialB->tangentVelocity ); + contact->tangentVelocity = b3Sub( tangentVelocityA, tangentVelocityB ); + return true; +} diff --git a/vendor/box3d/src/src/motor_joint.c b/vendor/box3d/src/src/motor_joint.c new file mode 100644 index 000000000..734d49767 --- /dev/null +++ b/vendor/box3d/src/src/motor_joint.c @@ -0,0 +1,439 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +#include "body.h" +#include "core.h" +#include "joint.h" +#include "math_internal.h" +#include "physics_world.h" +#include "solver.h" +#include "solver_set.h" +#include "recording.h" + +// needed for dll export +#include "box3d/box3d.h" + +void b3MotorJoint_SetLinearVelocity( b3JointId jointId, b3Vec3 velocity ) +{ + b3World* world = b3GetWorld( jointId.world0 ); + B3_REC( world, MotorJointSetLinearVelocity, jointId, velocity ); + b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_motorJoint ); + joint->motorJoint.linearVelocity = velocity; +} + +b3Vec3 b3MotorJoint_GetLinearVelocity( b3JointId jointId ) +{ + b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_motorJoint ); + return joint->motorJoint.linearVelocity; +} + +void b3MotorJoint_SetAngularVelocity( b3JointId jointId, b3Vec3 velocity ) +{ + b3World* world = b3GetWorld( jointId.world0 ); + B3_REC( world, MotorJointSetAngularVelocity, jointId, velocity ); + b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_motorJoint ); + joint->motorJoint.angularVelocity = velocity; +} + +b3Vec3 b3MotorJoint_GetAngularVelocity( b3JointId jointId ) +{ + b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_motorJoint ); + return joint->motorJoint.angularVelocity; +} + +void b3MotorJoint_SetMaxVelocityTorque( b3JointId jointId, float maxTorque ) +{ + b3World* world = b3GetWorld( jointId.world0 ); + B3_REC( world, MotorJointSetMaxVelocityTorque, jointId, maxTorque ); + b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_motorJoint ); + joint->motorJoint.maxVelocityTorque = maxTorque; +} + +float b3MotorJoint_GetMaxVelocityTorque( b3JointId jointId ) +{ + b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_motorJoint ); + return joint->motorJoint.maxVelocityTorque; +} + +void b3MotorJoint_SetMaxVelocityForce( b3JointId jointId, float maxForce ) +{ + b3World* world = b3GetWorld( jointId.world0 ); + B3_REC( world, MotorJointSetMaxVelocityForce, jointId, maxForce ); + b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_motorJoint ); + joint->motorJoint.maxVelocityForce = maxForce; +} + +float b3MotorJoint_GetMaxVelocityForce( b3JointId jointId ) +{ + b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_motorJoint ); + return joint->motorJoint.maxVelocityForce; +} + +void b3MotorJoint_SetLinearHertz( b3JointId jointId, float hertz ) +{ + b3World* world = b3GetWorld( jointId.world0 ); + B3_REC( world, MotorJointSetLinearHertz, jointId, hertz ); + b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_motorJoint ); + joint->motorJoint.linearHertz = hertz; +} + +float b3MotorJoint_GetLinearHertz( b3JointId jointId ) +{ + b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_motorJoint ); + return joint->motorJoint.linearHertz; +} + +void b3MotorJoint_SetLinearDampingRatio( b3JointId jointId, float damping ) +{ + b3World* world = b3GetWorld( jointId.world0 ); + B3_REC( world, MotorJointSetLinearDampingRatio, jointId, damping ); + b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_motorJoint ); + joint->motorJoint.linearDampingRatio = damping; +} + +float b3MotorJoint_GetLinearDampingRatio( b3JointId jointId ) +{ + b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_motorJoint ); + return joint->motorJoint.linearDampingRatio; +} + +void b3MotorJoint_SetAngularHertz( b3JointId jointId, float hertz ) +{ + b3World* world = b3GetWorld( jointId.world0 ); + B3_REC( world, MotorJointSetAngularHertz, jointId, hertz ); + b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_motorJoint ); + joint->motorJoint.angularHertz = hertz; +} + +float b3MotorJoint_GetAngularHertz( b3JointId jointId ) +{ + b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_motorJoint ); + return joint->motorJoint.angularHertz; +} + +void b3MotorJoint_SetAngularDampingRatio( b3JointId jointId, float damping ) +{ + b3World* world = b3GetWorld( jointId.world0 ); + B3_REC( world, MotorJointSetAngularDampingRatio, jointId, damping ); + b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_motorJoint ); + joint->motorJoint.angularDampingRatio = damping; +} + +float b3MotorJoint_GetAngularDampingRatio( b3JointId jointId ) +{ + b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_motorJoint ); + return joint->motorJoint.angularDampingRatio; +} + +void b3MotorJoint_SetMaxSpringForce( b3JointId jointId, float maxForce ) +{ + b3World* world = b3GetWorld( jointId.world0 ); + B3_REC( world, MotorJointSetMaxSpringForce, jointId, maxForce ); + b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_motorJoint ); + joint->motorJoint.maxSpringForce = b3MaxFloat( 0.0f, maxForce ); +} + +float b3MotorJoint_GetMaxSpringForce( b3JointId jointId ) +{ + b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_motorJoint ); + return joint->motorJoint.maxSpringForce; +} + +void b3MotorJoint_SetMaxSpringTorque( b3JointId jointId, float maxTorque ) +{ + b3World* world = b3GetWorld( jointId.world0 ); + B3_REC( world, MotorJointSetMaxSpringTorque, jointId, maxTorque ); + b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_motorJoint ); + joint->motorJoint.maxSpringTorque = b3MaxFloat( 0.0f, maxTorque ); +} + +float b3MotorJoint_GetMaxSpringTorque( b3JointId jointId ) +{ + b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_motorJoint ); + return joint->motorJoint.maxSpringTorque; +} + +b3Vec3 b3GetMotorJointForce( b3World* world, b3JointSim* base ) +{ + b3Vec3 force = b3MulSV( world->inv_h, b3Add( base->motorJoint.linearVelocityImpulse, base->motorJoint.linearSpringImpulse ) ); + return force; +} + +b3Vec3 b3GetMotorJointTorque( b3World* world, b3JointSim* base ) +{ + return b3MulSV( world->inv_h, b3Add( base->motorJoint.angularVelocityImpulse, base->motorJoint.angularSpringImpulse ) ); +} + +// Point-to-point constraint +// C = p2 - p1 +// Cdot = v2 - v1 +// = v2 + cross(w2, r2) - v1 - cross(w1, r1) +// J = [-I -r1_skew I r2_skew ] +// Identity used: +// w k % (rx i + ry j) = w * (-ry i + rx j) + +// Angle constraint +// C = angle2 - angle1 - referenceAngle +// Cdot = w2 - w1 +// J = [0 0 -1 0 0 1] +// K = invI1 + invI2 + +void b3PrepareMotorJoint( b3JointSim* base, b3StepContext* context ) +{ + B3_ASSERT( base->type == b3_motorJoint ); + + // chase body id to the solver set where the body lives + int idA = base->bodyIdA; + int idB = base->bodyIdB; + + b3World* world = context->world; + + b3Body* bodyA = b3Array_Get( world->bodies, idA ); + b3Body* bodyB = b3Array_Get( world->bodies, idB ); + + B3_ASSERT( bodyA->setIndex == b3_awakeSet || bodyB->setIndex == b3_awakeSet ); + + b3SolverSet* setA = b3Array_Get( world->solverSets, bodyA->setIndex ); + b3SolverSet* setB = b3Array_Get( world->solverSets, bodyB->setIndex ); + + int localIndexA = bodyA->localIndex; + int localIndexB = bodyB->localIndex; + + b3BodySim* bodySimA = b3Array_Get( setA->bodySims, localIndexA ); + b3BodySim* bodySimB = b3Array_Get( setB->bodySims, localIndexB ); + + base->invMassA = bodySimA->invMass; + base->invMassB = bodySimB->invMass; + base->invIA = bodySimA->invInertiaWorld; + base->invIB = bodySimB->invInertiaWorld; + + b3Matrix3 invInertiaSum = b3AddMM( base->invIA, base->invIB ); + base->fixedRotation = b3Det( invInertiaSum ) < 1000.0f * FLT_MIN; + + b3MotorJoint* joint = &base->motorJoint; + joint->indexA = bodyA->setIndex == b3_awakeSet ? localIndexA : B3_NULL_INDEX; + joint->indexB = bodyB->setIndex == b3_awakeSet ? localIndexB : B3_NULL_INDEX; + + // Compute joint anchor frames with world space rotation, relative to center of mass + joint->frameA.q = b3MulQuat( bodySimA->transform.q, base->localFrameA.q ); + joint->frameA.p = b3RotateVector( bodySimA->transform.q, b3Sub( base->localFrameA.p, bodySimA->localCenter ) ); + joint->frameB.q = b3MulQuat( bodySimB->transform.q, base->localFrameB.q ); + joint->frameB.p = b3RotateVector( bodySimB->transform.q, b3Sub( base->localFrameB.p, bodySimB->localCenter ) ); + + // Compute the initial center delta. Incremental position updates are relative to this. + joint->deltaCenter = b3SubPos( bodySimB->center, bodySimA->center ); + + joint->linearSpring = b3MakeSoft( joint->linearHertz, joint->linearDampingRatio, context->h ); + joint->angularSpring = b3MakeSoft( joint->angularHertz, joint->angularDampingRatio, context->h ); + + joint->angularMass = b3InvertMatrix( invInertiaSum ); + + if ( context->enableWarmStarting == false ) + { + joint->linearVelocityImpulse = b3Vec3_zero; + joint->angularVelocityImpulse = b3Vec3_zero; + joint->linearSpringImpulse = b3Vec3_zero; + joint->angularSpringImpulse = b3Vec3_zero; + } +} + +void b3WarmStartMotorJoint( b3JointSim* base, b3StepContext* context ) +{ + B3_ASSERT( base->type == b3_motorJoint ); + + float mA = base->invMassA; + float mB = base->invMassB; + b3Matrix3 iA = base->invIA; + b3Matrix3 iB = base->invIB; + + b3MotorJoint* joint = &base->motorJoint; + + // dummy state for static bodies + b3BodyState dummyState = b3_identityBodyState; + + b3BodyState* stateA = joint->indexA == B3_NULL_INDEX ? &dummyState : context->states + joint->indexA; + b3BodyState* stateB = joint->indexB == B3_NULL_INDEX ? &dummyState : context->states + joint->indexB; + + b3Vec3 rA = b3RotateVector( stateA->deltaRotation, joint->frameA.p ); + b3Vec3 rB = b3RotateVector( stateB->deltaRotation, joint->frameB.p ); + + b3Vec3 linearImpulse = b3Add( joint->linearVelocityImpulse, joint->linearSpringImpulse ); + b3Vec3 angularImpulse = b3Add( joint->angularVelocityImpulse, joint->angularSpringImpulse ); + + stateA->linearVelocity = b3MulSub( stateA->linearVelocity, mA, linearImpulse ); + stateA->angularVelocity = b3Sub( stateA->angularVelocity, b3MulMV( iA, b3Add( b3Cross( rA, linearImpulse ), angularImpulse ) ) ); + stateB->linearVelocity = b3MulAdd( stateB->linearVelocity, mB, linearImpulse ); + stateB->angularVelocity = b3Add( stateB->angularVelocity, b3MulMV( iB, b3Add( b3Cross( rB, linearImpulse ), angularImpulse ) ) ); +} + +void b3SolveMotorJoint( b3JointSim* base, b3StepContext* context ) +{ + B3_ASSERT( base->type == b3_motorJoint ); + + float mA = base->invMassA; + float mB = base->invMassB; + b3Matrix3 iA = base->invIA; + b3Matrix3 iB = base->invIB; + + // dummy state for static bodies + b3BodyState dummyState = b3_identityBodyState; + + b3MotorJoint* joint = &base->motorJoint; + b3BodyState* stateA = joint->indexA == B3_NULL_INDEX ? &dummyState : context->states + joint->indexA; + b3BodyState* stateB = joint->indexB == B3_NULL_INDEX ? &dummyState : context->states + joint->indexB; + + b3Vec3 vA = stateA->linearVelocity; + b3Vec3 wA = stateA->angularVelocity; + b3Vec3 vB = stateB->linearVelocity; + b3Vec3 wB = stateB->angularVelocity; + + b3Quat quatA = b3MulQuat( stateA->deltaRotation, joint->frameA.q ); + b3Quat quatB = b3MulQuat( stateB->deltaRotation, joint->frameB.q ); + + if ( b3DotQuat( quatA, quatB ) < 0.0f ) + { + // this keeps the rotation angle in the range [-pi, pi] + quatB = b3NegateQuat( quatB ); + } + + b3Quat relQ = b3InvMulQuat( quatA, quatB ); + + // angular spring + if ( joint->maxSpringTorque > 0.0f && joint->angularHertz > 0.0f ) + { + b3Quat targetQuat = b3Quat_identity; + b3Vec3 deltaRotation = b3DeltaQuatToRotation( relQ, targetQuat ); + b3Vec3 c = b3Neg( b3RotateVector( quatA, deltaRotation ) ); + + b3Vec3 bias = b3MulSV( joint->angularSpring.biasRate, c ); + float massScale = joint->angularSpring.massScale; + float impulseScale = joint->angularSpring.impulseScale; + + b3Vec3 cdot = b3Sub( wB, wA ); + + float maxImpulse = context->h * joint->maxSpringTorque; + b3Vec3 oldImpulse = joint->angularSpringImpulse; + b3Vec3 impulse = b3MulSub( b3MulSV( -massScale, b3MulMV( joint->angularMass, b3Add( cdot, bias ) ) ), impulseScale, oldImpulse ); + joint->angularSpringImpulse = b3Add( oldImpulse, impulse ); + if ( b3LengthSquared( joint->angularSpringImpulse ) > maxImpulse * maxImpulse ) + { + joint->angularSpringImpulse = b3MulSV( maxImpulse, b3Normalize( joint->angularSpringImpulse ) ); + } + impulse = b3Sub( joint->angularSpringImpulse, oldImpulse ); + + wA = b3Sub( wA, b3MulMV( iA, impulse ) ); + wB = b3Add( wB, b3MulMV( iB, impulse ) ); + } + + // angular velocity + if ( joint->maxVelocityTorque > 0.0 ) + { + b3Vec3 cdot = b3Sub( b3Sub( wB, wA ), joint->angularVelocity ); + b3Vec3 impulse = b3Neg( b3MulMV( joint->angularMass, cdot ) ); + + float maxImpulse = context->h * joint->maxVelocityTorque; + b3Vec3 oldImpulse = joint->angularVelocityImpulse; + joint->angularVelocityImpulse = b3Add( oldImpulse, impulse ); + if ( b3LengthSquared( joint->angularVelocityImpulse ) > maxImpulse * maxImpulse ) + { + joint->angularVelocityImpulse = b3MulSV( maxImpulse, b3Normalize( joint->angularVelocityImpulse ) ); + } + impulse = b3Sub( joint->angularVelocityImpulse, oldImpulse ); + + wA = b3Sub( wA, b3MulMV( iA, impulse ) ); + wB = b3Add( wB, b3MulMV( iB, impulse ) ); + } + + b3Vec3 rA = b3RotateVector( stateA->deltaRotation, joint->frameA.p ); + b3Vec3 rB = b3RotateVector( stateB->deltaRotation, joint->frameB.p ); + + // linear spring + if ( joint->maxSpringForce > 0.0f && joint->linearHertz > 0.0f ) + { + b3Vec3 dcA = stateA->deltaPosition; + b3Vec3 dcB = stateB->deltaPosition; + b3Vec3 c = b3Add( b3Add( b3Sub( dcB, dcA ), b3Sub( rB, rA ) ), joint->deltaCenter ); + + b3Vec3 bias = b3MulSV( joint->linearSpring.biasRate, c ); + float massScale = joint->linearSpring.massScale; + float impulseScale = joint->linearSpring.impulseScale; + + b3Vec3 cdot = b3Sub( b3Add( vB, b3Cross( wB, rB ) ), b3Add( vA, b3Cross( wA, rA ) ) ); + + //// K = [(1/m1 + 1/m2) * eye(2) - skew(r1) * invI1 * skew(r1) - skew(r2) * invI2 * skew(r2)] + b3Matrix3 sA = b3Skew( rA ); + b3Matrix3 sB = b3Skew( rB ); + b3Matrix3 kA = b3MulMM( sA, b3MulMM( base->invIA, sA ) ); + b3Matrix3 kB = b3MulMM( sB, b3MulMM( base->invIB, sB ) ); + b3Matrix3 k = b3NegateMat3( b3AddMM( kA, kB ) ); + k.cx.x += mA + mB; + k.cy.y += mA + mB; + k.cz.z += mA + mB; + + b3Vec3 b = b3Solve3( k, b3Add( cdot, bias ) ); + + b3Vec3 oldImpulse = joint->linearSpringImpulse; + b3Vec3 impulse = b3MulSub( b3MulSV( -massScale, b ), impulseScale, oldImpulse ); + float maxImpulse = context->h * joint->maxSpringForce; + joint->linearSpringImpulse = b3Add( joint->linearSpringImpulse, impulse ); + + if ( b3LengthSquared( joint->linearSpringImpulse ) > maxImpulse * maxImpulse ) + { + joint->linearSpringImpulse = b3MulSV( maxImpulse, b3Normalize( joint->linearSpringImpulse ) ); + } + + impulse = b3Sub( joint->linearSpringImpulse, oldImpulse ); + + vA = b3MulSub( vA, mA, impulse ); + wA = b3Sub( wA, b3MulMV( iA, b3Cross( rA, impulse ) ) ); + vB = b3MulAdd( vB, mB, impulse ); + wB = b3Add( wB, b3MulMV( iB, b3Cross( rB, impulse ) ) ); + } + + // linear velocity + if ( joint->maxVelocityForce > 0.0f ) + { + b3Vec3 cdot = b3Sub( b3Add( vB, b3Cross( wB, rB ) ), b3Add( vA, b3Cross( wA, rA ) ) ); + cdot = b3Sub( cdot, joint->linearVelocity ); + //// K = [(1/m1 + 1/m2) * eye(2) - skew(r1) * invI1 * skew(r1) - skew(r2) * invI2 * skew(r2)] + b3Matrix3 sA = b3Skew( rA ); + b3Matrix3 sB = b3Skew( rB ); + b3Matrix3 kA = b3MulMM( sA, b3MulMM( base->invIA, sA ) ); + b3Matrix3 kB = b3MulMM( sB, b3MulMM( base->invIB, sB ) ); + b3Matrix3 k = b3NegateMat3( b3AddMM( kA, kB ) ); + k.cx.x += mA + mB; + k.cy.y += mA + mB; + k.cz.z += mA + mB; + + b3Vec3 b = b3Solve3( k, cdot ); + b3Vec3 impulse = b3Neg( b ); + + b3Vec3 oldImpulse = joint->linearVelocityImpulse; + float maxImpulse = context->h * joint->maxVelocityForce; + joint->linearVelocityImpulse = b3Add( joint->linearVelocityImpulse, impulse ); + + if ( b3LengthSquared( joint->linearVelocityImpulse ) > maxImpulse * maxImpulse ) + { + joint->linearVelocityImpulse = b3MulSV( maxImpulse, b3Normalize( joint->linearVelocityImpulse ) ); + } + + impulse = b3Sub( joint->linearVelocityImpulse, oldImpulse ); + + vA = b3MulSub( vA, mA, impulse ); + wA = b3Sub( wA, b3MulMV( iA, b3Cross( rA, impulse ) ) ); + vB = b3MulAdd( vB, mB, impulse ); + wB = b3Add( wB, b3MulMV( iB, b3Cross( rB, impulse ) ) ); + } + + if ( stateA->flags & b3_dynamicFlag ) + { + stateA->linearVelocity = vA; + stateA->angularVelocity = wA; + } + + if ( stateB->flags & b3_dynamicFlag ) + { + stateB->linearVelocity = vB; + stateB->angularVelocity = wB; + } +} diff --git a/vendor/box3d/src/src/mover.c b/vendor/box3d/src/src/mover.c new file mode 100644 index 000000000..209426e97 --- /dev/null +++ b/vendor/box3d/src/src/mover.c @@ -0,0 +1,70 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +#include "math_internal.h" + +#include "box3d/collision.h" +#include "box3d/constants.h" + +b3PlaneSolverResult b3SolvePlanes( b3Vec3 targetDelta, b3CollisionPlane* planes, int count ) +{ + for ( int i = 0; i < count; ++i ) + { + planes[i].push = 0.0f; + } + + b3Vec3 delta = targetDelta; + float tolerance = B3_LINEAR_SLOP; + + int iteration; + for ( iteration = 0; iteration < 20; ++iteration ) + { + float totalPush = 0.0f; + for ( int planeIndex = 0; planeIndex < count; ++planeIndex ) + { + b3CollisionPlane* plane = planes + planeIndex; + + // Add slop to prevent jitter + float separation = b3PlaneSeparation( plane->plane, delta ) + B3_LINEAR_SLOP; + + float push = -separation; + + // Clamp accumulated push + float accumulatedPush = plane->push; + plane->push = b3ClampFloat( plane->push + push, 0.0f, plane->pushLimit ); + push = plane->push - accumulatedPush; + delta = b3MulAdd( delta, push, plane->plane.normal ); + + // Track maximum push for convergence + totalPush += b3AbsFloat( push ); + } + + if ( totalPush < tolerance ) + { + break; + } + } + + return (b3PlaneSolverResult){ + .delta = delta, + .iterationCount = iteration, + }; +} + +b3Vec3 b3ClipVector( b3Vec3 vector, const b3CollisionPlane* planes, int count ) +{ + b3Vec3 v = vector; + + for ( int planeIndex = 0; planeIndex < count; ++planeIndex ) + { + const b3CollisionPlane* plane = planes + planeIndex; + if ( plane->push == 0.0f || plane->clipVelocity == false ) + { + continue; + } + + v = b3MulSub( v, b3MinFloat( 0.0f, b3Dot( v, plane->plane.normal ) ), plane->plane.normal ); + } + + return v; +} diff --git a/vendor/box3d/src/src/name_cache.c b/vendor/box3d/src/src/name_cache.c new file mode 100644 index 000000000..a7833df5a --- /dev/null +++ b/vendor/box3d/src/src/name_cache.c @@ -0,0 +1,155 @@ +// SPDX-FileCopyrightText: 2026 Erin Catto +// SPDX-License-Identifier: MIT + +#include "name_cache.h" + +#include "recording.h" + +#include + +#define NAME b3NameMap +#define KEY_TY uint32_t +#define VAL_TY int +#define HASH_FN vt_hash_integer +#define CMPR_FN vt_cmpr_integer +#define MALLOC_FN b3Alloc +#define FREE_FN b3Free +#include "verstable.h" + +#define FNV32_OFFSET_BASIS 0x811c9dc5u +#define FNV32_PRIME 0x01000193u + +// This is designed to hash small strings and while avoiding collisions +// for a 32-bit hash. +// Note: changing this will break recordings +uint32_t b3Hash32( const void* data, size_t length ) +{ + // Cast to unsigned to avoid sign extension + const unsigned char* p = data; + + // FNV-1a + uint32_t h = FNV32_OFFSET_BASIS; + for ( size_t i = 0; i < length; ++i ) + { + h ^= p[i]; + h *= FNV32_PRIME; + } + + // Murmur3 finalizer to help with short strings + h ^= h >> 16; + h *= 0x85ebca6bu; + h ^= h >> 13; + h *= 0xc2b2ae35u; + h ^= h >> 16; + return h; +} + +b3NameCache b3CreateNameCache( void ) +{ + b3NameCache cache = { 0 }; + b3Array_Create( cache.entries ); + + b3NameMap* map = b3Alloc( sizeof( b3NameMap ) ); + b3NameMap_init( map ); + cache.map = map; + return cache; +} + +void b3DestroyNameCache( b3NameCache* cache ) +{ + int count = cache->entries.count; + for ( int i = 0; i < count; ++i ) + { + b3NameEntry* entry = cache->entries.data + i; + b3Free( entry->name, entry->length + 1 ); + } + + b3Array_Destroy( cache->entries ); + b3NameMap_cleanup( (b3NameMap*)cache->map ); + b3Free( cache->map, sizeof( b3NameMap ) ); +} + +uint32_t b3AddName( b3NameCache* cache, const char* name ) +{ + if ( name == NULL || name[0] == 0 ) + { + return B3_NULL_NAME; + } + + size_t length = strlen( name ); + uint32_t id = b3Hash32( name, length ); + + id = id == 0 ? 1 : id; + + b3NameMap* map = cache->map; + b3NameMap_itr itr = b3NameMap_get( map, id ); + if ( b3NameMap_is_end( itr ) == false ) + { + if ( strcmp( cache->entries.data[itr.data->val].name, name ) != 0 ) + { + // Different name, same hash + b3Log( "Hash collision on %s", name ); + } + return id; + } + + char* clone = b3Alloc( length + 1 ); + memcpy( clone, name, length + 1 ); + int index = cache->entries.count; + b3NameEntry entry = { + .hash = id, + .name = clone, + .length = (int)length, + }; + + b3Array_Push( cache->entries, entry ); + + b3NameMap_insert( map, id, index ); + return id; +} + +const char* b3FindName( const b3NameCache* cache, uint32_t id ) +{ + b3NameMap* map = cache->map; + b3NameMap_itr itr = b3NameMap_get( map, id ); + if ( b3NameMap_is_end( itr ) == false ) + { + int index = itr.data->val; + const b3NameEntry* entry = b3Array_Get( cache->entries, index ); + return entry->name; + } + + return NULL; +} + +void b3LoadName( b3NameCache* cache, uint32_t id, char* name, int length ) +{ + if ( name == NULL ) + { + return; + } + + if ( name[0] == 0 || id == B3_NULL_NAME ) + { + b3Free( name, length + 1 ); + return; + } + + b3NameMap* map = cache->map; + b3NameMap_itr itr = b3NameMap_get( map, id ); + if ( b3NameMap_is_end( itr ) == false ) + { + b3Free( name, length + 1 ); + return; + } + + int index = cache->entries.count; + b3NameEntry entry = { + .hash = id, + .name = name, + .length = length, + }; + + b3Array_Push( cache->entries, entry ); + b3NameMap_insert( map, id, index ); +} diff --git a/vendor/box3d/src/src/name_cache.h b/vendor/box3d/src/src/name_cache.h new file mode 100644 index 000000000..d1535b37a --- /dev/null +++ b/vendor/box3d/src/src/name_cache.h @@ -0,0 +1,42 @@ +// SPDX-FileCopyrightText: 2026 Erin Catto +// SPDX-License-Identifier: MIT + +#pragma once + +#include "container.h" + +#define B3_NULL_NAME 0u + +typedef struct b3NameEntry +{ + // Names are used for debugging and I'm favoring simplicity and + // minimal storage. Collisions will be logged. + uint32_t hash; + int length; + char* name; +} b3NameEntry; + +b3DeclareArray( b3NameEntry ); + +typedef struct b3NameCache +{ + b3Array( b3NameEntry ) entries; + void* map; +} b3NameCache; + +b3NameCache b3CreateNameCache( void ); +void b3DestroyNameCache( b3NameCache* cache ); + +uint32_t b3AddName( b3NameCache* cache, const char* name ); +const char* b3FindName( const b3NameCache* cache, uint32_t id ); + +// Load a name from a recording. Name ownership is transferred. +void b3LoadName( b3NameCache* cache, uint32_t id, char* name, int length ); + +uint32_t b3Hash32( const void* data, size_t length ); + +static inline const char* b3FindNameWithDefault( const b3NameCache* cache, uint32_t id, const char* def ) +{ + const char* name = b3FindName( cache, id ); + return name == NULL ? def : name; +} diff --git a/vendor/box3d/src/src/parallel_for.c b/vendor/box3d/src/src/parallel_for.c new file mode 100644 index 000000000..07d1c3fdc --- /dev/null +++ b/vendor/box3d/src/src/parallel_for.c @@ -0,0 +1,135 @@ +// SPDX-FileCopyrightText: 2026 Erin Catto +// SPDX-License-Identifier: MIT + +#include "parallel_for.h" + +#include "core.h" +#include "physics_world.h" +#include "platform.h" + +#include "box3d/base.h" +#include "box3d/constants.h" + +#include + +// Shared state for one b3ParallelFor invocation. Workers race on nextBlock to +// claim work, so a slow chunk can't strand the other threads. +typedef struct b3ParallelForShared +{ + b3AtomicInt nextBlock; + int blockCount; + int blockSize; + int itemCount; + b3ParallelForCallback* callback; + void* context; +} b3ParallelForShared; + +typedef struct b3ParallelForTask +{ + b3ParallelForShared* shared; + int workerIndex; +} b3ParallelForTask; + +static void b3ParallelForTrampoline( void* taskContext ) +{ + b3ParallelForTask* task = (b3ParallelForTask*)taskContext; + b3ParallelForShared* shared = task->shared; + int workerIndex = task->workerIndex; + void* context = shared->context; + b3ParallelForCallback* callback = shared->callback; + + int blockCount = shared->blockCount; + int blockSize = shared->blockSize; + int itemCount = shared->itemCount; + + for ( ;; ) + { + int blockIndex = b3AtomicFetchAddInt( &shared->nextBlock, 1 ); + if ( blockIndex >= blockCount ) + { + break; + } + + int start = blockIndex * blockSize; + int end = start + blockSize; + if ( end > itemCount ) + { + end = itemCount; + } + + callback( start, end, workerIndex, context ); + } +} + +void b3ParallelFor( b3World* world, b3ParallelForCallback* callback, int itemCount, int minRange, void* context, + const char* name ) +{ + if ( itemCount <= 0 ) + { + return; + } + + B3_ASSERT( minRange > 0 ); + + int workerCount = world->workerCount; + B3_ASSERT( 0 < workerCount && workerCount <= B3_MAX_WORKERS ); + + // Target multiple blocks per worker to reduce thread stalls. + // block size grows once items exceed maxBlockCount * minRange + // so the block count stays bounded and per-block sync overhead stays low. + int blocksPerWorker = 4; + int maxBlockCount = blocksPerWorker * workerCount; + + int blockSize; + int blockCount; + if ( itemCount <= minRange * maxBlockCount ) + { + blockSize = minRange; + blockCount = ( itemCount + blockSize - 1 ) / blockSize; + } + else + { + blockSize = ( itemCount + maxBlockCount - 1 ) / maxBlockCount; + blockCount = ( itemCount + blockSize - 1 ) / blockSize; + } + B3_ASSERT( blockCount >= 1 ); + B3_ASSERT( blockSize * blockCount >= itemCount ); + + // No point enqueueing more tasks than blocks. + int taskCount = workerCount < blockCount ? workerCount : blockCount; + + b3ParallelForShared shared; + shared.blockCount = blockCount; + shared.blockSize = blockSize; + shared.itemCount = itemCount; + shared.callback = callback; + shared.context = context; + b3AtomicStoreInt( &shared.nextBlock, 0 ); + + b3ParallelForTask tasks[B3_MAX_WORKERS]; + void* handles[B3_MAX_WORKERS]; + for ( int i = 0; i < taskCount; ++i ) + { + tasks[i].shared = &shared; + tasks[i].workerIndex = i; + + if (world->taskCount < B3_MAX_TASKS) + { + handles[i] = world->enqueueTaskFcn( &b3ParallelForTrampoline, tasks + i, world->userTaskContext, name ); + world->taskCount += 1; + } + else + { + handles[i] = NULL; + b3ParallelForTrampoline( tasks + i ); + } + } + + for ( int i = 0; i < taskCount; ++i ) + { + if ( handles[i] != NULL ) + { + world->finishTaskFcn( handles[i], world->userTaskContext ); + } + } +} diff --git a/vendor/box3d/src/src/parallel_for.h b/vendor/box3d/src/src/parallel_for.h new file mode 100644 index 000000000..4a6d2851c --- /dev/null +++ b/vendor/box3d/src/src/parallel_for.h @@ -0,0 +1,23 @@ +// SPDX-FileCopyrightText: 2026 Erin Catto +// SPDX-License-Identifier: MIT + +#pragma once + +typedef struct b3World b3World; + +// Callback invoked by b3ParallelFor to process a range of items. May be called +// multiple times per worker: work is divided into blocks that workers claim +// atomically, so a worker that finishes early picks up the next unclaimed +// block instead of sitting idle. workerIndex is the worker identity and is +// stable across all invocations from the same worker, so it is safe to use as +// an index into per-worker state (e.g. world->taskContexts.data + workerIndex). +typedef void b3ParallelForCallback( int startIndex, int endIndex, int workerIndex, void* context ); + +// Divide [0, itemCount) into blocks and process them with cooperative claiming: +// up to world->workerCount tasks are enqueued, and each task loops, atomically +// claiming the next unclaimed block until the range is drained. Blocks the +// caller until all work is complete. minRange is the minimum block size; block +// size grows once itemCount exceeds 4 * workerCount * minRange so block count +// stays bounded. +void b3ParallelFor( b3World* world, b3ParallelForCallback* callback, int itemCount, int minRange, void* context, + const char* name ); diff --git a/vendor/box3d/src/src/parallel_joint.c b/vendor/box3d/src/src/parallel_joint.c new file mode 100644 index 000000000..7390a2131 --- /dev/null +++ b/vendor/box3d/src/src/parallel_joint.c @@ -0,0 +1,251 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +#include "body.h" +#include "core.h" +#include "joint.h" +#include "math_internal.h" +#include "physics_world.h" +#include "solver.h" +#include "solver_set.h" +#include "recording.h" + +// needed for dll export +#include "box3d/box3d.h" + +void b3ParallelJoint_SetSpringHertz( b3JointId jointId, float hertz ) +{ + B3_ASSERT( b3IsValidFloat( hertz ) && hertz >= 0.0f ); + b3World* world = b3GetWorld( jointId.world0 ); + B3_REC( world, ParallelJointSetSpringHertz, jointId, hertz ); + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_parallelJoint ); + base->parallelJoint.hertz = hertz; +} + +float b3ParallelJoint_GetSpringHertz( b3JointId jointId ) +{ + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_parallelJoint ); + return base->parallelJoint.hertz; +} + +void b3ParallelJoint_SetSpringDampingRatio( b3JointId jointId, float dampingRatio ) +{ + B3_ASSERT( b3IsValidFloat( dampingRatio ) && dampingRatio >= 0.0f ); + b3World* world = b3GetWorld( jointId.world0 ); + B3_REC( world, ParallelJointSetSpringDampingRatio, jointId, dampingRatio ); + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_parallelJoint ); + base->parallelJoint.dampingRatio = dampingRatio; +} + +float b3ParallelJoint_GetSpringDampingRatio( b3JointId jointId ) +{ + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_parallelJoint ); + return base->parallelJoint.dampingRatio; +} + +void b3ParallelJoint_SetMaxTorque( b3JointId jointId, float maxForce ) +{ + B3_ASSERT( b3IsValidFloat( maxForce ) && maxForce >= 0.0f ); + b3World* world = b3GetWorld( jointId.world0 ); + B3_REC( world, ParallelJointSetMaxTorque, jointId, maxForce ); + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_parallelJoint ); + base->parallelJoint.maxTorque = maxForce; +} + +float b3ParallelJoint_GetMaxTorque( b3JointId jointId ) +{ + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_parallelJoint ); + return base->parallelJoint.maxTorque; +} + +b3Vec3 b3GetParallelJointTorque( b3World* world, b3JointSim* base ) +{ + b3ParallelJoint* joint = &base->parallelJoint; + + b3Quat relQ = b3InvMulQuat( joint->quatA, joint->quatB ); + joint->perpAxisX = b3MulSV( 0.5f, b3RotateVector( joint->quatA, b3Add( b3MulSV( relQ.s, b3Vec3_axisX ), b3Cross( relQ.v, b3Vec3_axisX ) ) ) ); + joint->perpAxisY = b3MulSV( 0.5f, b3RotateVector( joint->quatA, b3Add( b3MulSV( relQ.s, b3Vec3_axisY ), b3Cross( relQ.v, b3Vec3_axisY ) ) ) ); + + b3Vec3 angularImpulse = b3Blend2( joint->perpImpulse.x, joint->perpAxisX, joint->perpImpulse.y, joint->perpAxisY ); + b3Vec3 torque = b3MulSV( world->inv_h, angularImpulse ); + return torque; +} + +void b3PrepareParallelJoint( b3JointSim* base, b3StepContext* context ) +{ + B3_ASSERT( base->type == b3_parallelJoint ); + + b3World* world = context->world; + + b3Body* bodyA = b3Array_Get( world->bodies, base->bodyIdA ); + b3Body* bodyB = b3Array_Get( world->bodies, base->bodyIdB ); + + B3_ASSERT( bodyA->setIndex == b3_awakeSet || bodyB->setIndex == b3_awakeSet ); + b3SolverSet* setA = b3Array_Get( world->solverSets, bodyA->setIndex ); + b3SolverSet* setB = b3Array_Get( world->solverSets, bodyB->setIndex ); + + int localIndexA = bodyA->localIndex; + int localIndexB = bodyB->localIndex; + + b3BodySim* bodySimA = b3Array_Get( setA->bodySims, localIndexA ); + b3BodySim* bodySimB = b3Array_Get( setB->bodySims, localIndexB ); + + base->invMassA = bodySimA->invMass; + base->invMassB = bodySimB->invMass; + base->invIA = bodySimA->invInertiaWorld; + base->invIB = bodySimB->invInertiaWorld; + + b3Matrix3 invInertiaSum = b3AddMM( base->invIA, base->invIB ); + base->fixedRotation = b3Det( invInertiaSum ) < 1000.0f * FLT_MIN; + + b3ParallelJoint* joint = &base->parallelJoint; + joint->indexA = bodyA->setIndex == b3_awakeSet ? localIndexA : B3_NULL_INDEX; + joint->indexB = bodyB->setIndex == b3_awakeSet ? localIndexB : B3_NULL_INDEX; + + // Compute joint anchor frames with world space rotation, relative to center of mass + joint->quatA = b3MulQuat( bodySimA->transform.q, base->localFrameA.q ); + joint->quatB = b3MulQuat( bodySimB->transform.q, base->localFrameB.q ); + + b3Quat relQ = b3InvMulQuat( joint->quatA, joint->quatB ); + + { + // These are needed for warm starting + joint->perpAxisX = b3MulSV( 0.5f, b3RotateVector( joint->quatA, b3Add( b3MulSV( relQ.s, b3Vec3_axisX ), b3Cross( relQ.v, b3Vec3_axisX ) ) ) ); + joint->perpAxisY = b3MulSV( 0.5f, b3RotateVector( joint->quatA, b3Add( b3MulSV( relQ.s, b3Vec3_axisY ), b3Cross( relQ.v, b3Vec3_axisY ) ) ) ); + } + + joint->softness = b3MakeSoft( joint->hertz, joint->dampingRatio, context->h ); + + if ( context->enableWarmStarting == false ) + { + joint->perpImpulse = (b3Vec2){ 0.0f, 0.0f }; + } +} + +void b3WarmStartParallelJoint( b3JointSim* base, b3StepContext* context ) +{ + B3_ASSERT( base->type == b3_parallelJoint ); + + b3Matrix3 iA = base->invIA; + b3Matrix3 iB = base->invIB; + + // dummy state for static bodies + b3BodyState dummyState = b3_identityBodyState; + + b3ParallelJoint* joint = &base->parallelJoint; + + b3BodyState* stateA = joint->indexA == B3_NULL_INDEX ? &dummyState : context->states + joint->indexA; + b3BodyState* stateB = joint->indexB == B3_NULL_INDEX ? &dummyState : context->states + joint->indexB; + + b3Vec3 wA = stateA->angularVelocity; + b3Vec3 wB = stateB->angularVelocity; + + b3Vec3 angularImpulse = b3Blend2( joint->perpImpulse.x, joint->perpAxisX, joint->perpImpulse.y, joint->perpAxisY ); + + wA = b3Sub( wA, b3MulMV( iA, angularImpulse ) ); + wB = b3Add( wB, b3MulMV( iB, angularImpulse ) ); + + if ( stateA->flags & b3_dynamicFlag ) + { + stateA->angularVelocity = wA; + } + + if ( stateB->flags & b3_dynamicFlag ) + { + stateB->angularVelocity = wB; + } +} + +void b3SolveParallelJoint( b3JointSim* base, b3StepContext* context ) +{ + b3Matrix3 iA = base->invIA; + b3Matrix3 iB = base->invIB; + + // dummy state for static bodies + b3BodyState dummyState = b3_identityBodyState; + + b3ParallelJoint* joint = &base->parallelJoint; + b3BodyState* stateA = joint->indexA == B3_NULL_INDEX ? &dummyState : context->states + joint->indexA; + b3BodyState* stateB = joint->indexB == B3_NULL_INDEX ? &dummyState : context->states + joint->indexB; + + b3Vec3 wA = stateA->angularVelocity; + b3Vec3 wB = stateB->angularVelocity; + + bool fixedRotation = base->fixedRotation; + b3Quat quatA = b3MulQuat( stateA->deltaRotation, joint->quatA ); + b3Quat quatB = b3MulQuat( stateB->deltaRotation, joint->quatB ); + + if ( b3DotQuat( quatA, quatB ) < 0.0f ) + { + // this keeps the rotation angle in the range [-pi, pi] + quatB = b3NegateQuat( quatB ); + } + + b3Quat relQ = b3InvMulQuat( quatA, quatB ); + + if ( fixedRotation == false && joint->maxTorque > 0.0f ) + { + b3Vec2 c = { relQ.v.x, relQ.v.y }; + b3Vec2 bias = { joint->softness.biasRate * c.x, joint->softness.biasRate * c.y }; + float massScale = joint->softness.massScale; + float impulseScale = joint->softness.impulseScale; + + // Collinearity constraint as 2-by-2 + b3Vec3 perpAxisX = b3MulSV( 0.5f, b3RotateVector( quatA, b3Add( b3MulSV( relQ.s, b3Vec3_axisX ), b3Cross( relQ.v, b3Vec3_axisX ) ) ) ); + b3Vec3 perpAxisY = b3MulSV( 0.5f, b3RotateVector( quatA, b3Add( b3MulSV( relQ.s, b3Vec3_axisY ), b3Cross( relQ.v, b3Vec3_axisY ) ) ) ); + joint->perpAxisX = perpAxisX; + joint->perpAxisY = perpAxisY; + + b3Matrix3 invInertiaSum = b3AddMM( iA, iB ); + float kxx = b3Dot( perpAxisX, b3MulMV( invInertiaSum, perpAxisX ) ); + float kyy = b3Dot( perpAxisY, b3MulMV( invInertiaSum, perpAxisY ) ); + float kxy = b3Dot( perpAxisX, b3MulMV( invInertiaSum, perpAxisY ) ); + + b3Matrix2 k = { { kxx, kxy }, { kxy, kyy } }; + + b3Vec3 wRel = b3Sub( wB, wA ); + b3Vec2 cdot = { b3Dot( wRel, perpAxisX ), b3Dot( wRel, perpAxisY ) }; + + float maxImpulse = context->h * joint->maxTorque; + b3Vec2 oldImpulse = joint->perpImpulse; + b3Vec2 cdotPlusBias = { cdot.x + bias.x, cdot.y + bias.y }; + b3Vec2 sol = b3Solve2( k, cdotPlusBias ); + b3Vec2 deltaImpulse = { + -massScale * sol.x - impulseScale * oldImpulse.x, + -massScale * sol.y - impulseScale * oldImpulse.y, + }; + joint->perpImpulse = (b3Vec2){ oldImpulse.x + deltaImpulse.x, oldImpulse.y + deltaImpulse.y }; + if ( b3LengthSquared2( joint->perpImpulse ) > maxImpulse * maxImpulse ) + { + float s = maxImpulse / b3Length2( joint->perpImpulse ); + joint->perpImpulse = (b3Vec2){ s * joint->perpImpulse.x, s * joint->perpImpulse.y }; + } + + deltaImpulse = b3Sub2( joint->perpImpulse, oldImpulse ); + + b3Vec3 angularImpulse = b3Blend2( deltaImpulse.x, perpAxisX, deltaImpulse.y, perpAxisY ); + wA = b3Sub( wA, b3MulMV( iA, angularImpulse ) ); + wB = b3Add( wB, b3MulMV( iB, angularImpulse ) ); + } + + if ( stateA->flags & b3_dynamicFlag ) + { + stateA->angularVelocity = wA; + } + + if ( stateB->flags & b3_dynamicFlag ) + { + stateB->angularVelocity = wB; + } +} + +void b3DrawParallelJoint( b3DebugDraw* draw, b3JointSim* base, b3WorldTransform transformA, b3WorldTransform transformB, float scale ) +{ + float length = 0.1f * scale; + + b3WorldTransform frameA = b3MulWorldTransforms( transformA, base->localFrameA ); + draw->DrawSegmentFcn( frameA.p, b3OffsetPos( frameA.p, b3MulSV( length, b3RotateVector( frameA.q, b3Vec3_axisZ ) ) ), b3_colorGreen, draw->context ); + + b3WorldTransform frameB = b3MulWorldTransforms( transformB, base->localFrameB ); + draw->DrawSegmentFcn( frameB.p, b3OffsetPos( frameB.p, b3MulSV( length, b3RotateVector( frameB.q, b3Vec3_axisZ ) ) ), b3_colorBlue, draw->context ); +} diff --git a/vendor/box3d/src/src/physics_world.c b/vendor/box3d/src/src/physics_world.c new file mode 100644 index 000000000..f0dc19e2d --- /dev/null +++ b/vendor/box3d/src/src/physics_world.c @@ -0,0 +1,4099 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +#include "physics_world.h" + +#include "arena_allocator.h" +#include "bitset.h" +#include "body.h" +#include "broad_phase.h" +#include "constraint_graph.h" +#include "contact.h" +#include "core.h" +#include "ctz.h" +#include "hull_map.h" +#include "island.h" +#include "joint.h" +#include "parallel_for.h" +#include "platform.h" +#include "recording.h" +#include "scheduler.h" +#include "sensor.h" +#include "shape.h" +#include "solver.h" +#include "solver_set.h" + +#include "box3d/box3d.h" +#include "box3d/constants.h" + +#include +#include +#include +#include + +_Static_assert( B3_MAX_WORLDS > 0, "must be 1 or more" ); +_Static_assert( B3_MAX_WORLDS < UINT16_MAX, "B3_MAX_WORLDS limit exceeded" ); +b3World b3_worlds[B3_MAX_WORLDS]; +b3AtomicInt b3_worldCount; +int b3_maxWorldCount; + +const b3HullData* b3AddHullToDatabase( b3World* world, const b3HullData* src ) +{ + b3HullMap* database = world->hullDatabase; + + // Compare by content so an unowned query hull finds the shared copy. + b3HullMap_itr itr = b3HullMap_get( database, src ); + if ( b3HullMap_is_end( itr ) == false ) + { + itr.data->val += 1; + return itr.data->key; + } + + b3HullData* owned = b3CloneHull( src ); + B3_ASSERT( owned != NULL ); + b3HullMap_insert( database, owned, 1 ); + return owned; +} + +const b3HullData* b3AddOwnedHullToDatabase( b3World* world, b3HullData* owned ) +{ + b3HullMap* database = world->hullDatabase; + + b3HullMap_itr itr = b3HullMap_get( database, owned ); + if ( b3HullMap_is_end( itr ) == false ) + { + itr.data->val += 1; + b3DestroyHull( owned ); + return itr.data->key; + } + + // Take ownership of input hull. + b3HullMap_insert( database, owned, 1 ); + return owned; +} + +void b3RemoveHullFromDatabase( b3World* world, const b3HullData* data ) +{ + b3HullMap* database = world->hullDatabase; + + b3HullMap_itr itr = b3HullMap_get( database, data ); + B3_ASSERT( b3HullMap_is_end( itr ) == false ); + + if ( --itr.data->val == 0 ) + { + // Erase through the iterator we already have so the lookup runs once. + b3HullData* owned = (b3HullData*)itr.data->key; + b3HullMap_erase_itr( database, itr ); + b3DestroyHull( owned ); + } +} + +b3World* b3GetUnlockedWorldFromId( b3WorldId id ) +{ + B3_ASSERT( 1 <= id.index1 && id.index1 <= B3_MAX_WORLDS ); + b3World* world = b3_worlds + ( id.index1 - 1 ); + B3_ASSERT( id.index1 == world->worldId + 1 ); + B3_ASSERT( id.generation == world->generation ); + + // A world accessed from an id should not be locked + if ( world->locked ) + { + B3_ASSERT( false ); + return NULL; + } + return world; +} + +b3World* b3GetWorldFromId( b3WorldId id ) +{ + B3_ASSERT( 1 <= id.index1 && id.index1 <= B3_MAX_WORLDS ); + b3World* world = b3_worlds + ( id.index1 - 1 ); + B3_ASSERT( id.index1 == world->worldId + 1 ); + B3_ASSERT( id.generation == world->generation ); + return world; +} + +b3World* b3GetWorld( int index ) +{ + B3_ASSERT( 0 <= index && index < B3_MAX_WORLDS ); + b3World* world = b3_worlds + index; + B3_ASSERT( world->worldId == index ); + return world; +} + +b3World* b3GetUnlockedWorld( int index ) +{ + B3_ASSERT( 0 <= index && index < B3_MAX_WORLDS ); + b3World* world = b3_worlds + index; + B3_ASSERT( world->worldId == index ); + if ( world->locked ) + { + B3_ASSERT( false ); + return NULL; + } + + return world; +} + +static void* b3DefaultAddTaskFcn( b3TaskCallback* task, void* taskContext, void* userContext, const char* name ) +{ + B3_UNUSED( userContext, name ); + task( taskContext ); + return NULL; +} + +static void b3DefaultFinishTaskFcn( void* userTask, void* userContext ) +{ + B3_UNUSED( userTask ); + B3_UNUSED( userContext ); +} + +static float b3DefaultFrictionCallback( float frictionA, uint64_t materialA, float frictionB, uint64_t materialB ) +{ + B3_UNUSED( materialA, materialB ); + return sqrtf( frictionA * frictionB ); +} + +static float b3DefaultRestitutionCallback( float restitutionA, uint64_t materialA, float restitutionB, uint64_t materialB ) +{ + B3_UNUSED( materialA, materialB ); + return b3MaxFloat( restitutionA, restitutionB ); +} + +static void b3CreateWorkerContexts( b3World* world ) +{ + b3Array_Resize( world->taskContexts, world->workerCount ); + b3Array_MemZero( world->taskContexts ); + + b3Array_Resize( world->sensorTaskContexts, world->workerCount ); + b3Array_MemZero( world->sensorTaskContexts ); + + for ( int i = 0; i < world->workerCount; ++i ) + { + world->taskContexts.data[i].arena = b3CreateArena( 128 * 1024 ); + b3Array_Reserve( world->taskContexts.data[i].sensorHits, 8 ); + world->taskContexts.data[i].contactStateBitSet = b3CreateBitSet( 1024 ); + world->taskContexts.data[i].hitEventBitSet = b3CreateBitSet( 1024 ); + world->taskContexts.data[i].hasHitEvents = false; + world->taskContexts.data[i].jointStateBitSet = b3CreateBitSet( 1024 ); + world->taskContexts.data[i].enlargedSimBitSet = b3CreateBitSet( 256 ); + world->taskContexts.data[i].awakeIslandBitSet = b3CreateBitSet( 256 ); + world->taskContexts.data[i].splitIslandId = B3_NULL_INDEX; + + world->sensorTaskContexts.data[i].eventBits = b3CreateBitSet( 128 ); + } +} + +static void b3DestroyWorkerContexts( b3World* world ) +{ + for ( int i = 0; i < world->workerCount; ++i ) + { + b3DestroyArena( &world->taskContexts.data[i].arena ); + b3Array_Destroy( world->taskContexts.data[i].sensorHits ); + b3DestroyBitSet( &world->taskContexts.data[i].contactStateBitSet ); + b3DestroyBitSet( &world->taskContexts.data[i].hitEventBitSet ); + b3DestroyBitSet( &world->taskContexts.data[i].jointStateBitSet ); + b3DestroyBitSet( &world->taskContexts.data[i].enlargedSimBitSet ); + b3DestroyBitSet( &world->taskContexts.data[i].awakeIslandBitSet ); + + b3DestroyBitSet( &world->sensorTaskContexts.data[i].eventBits ); + } + + b3Array_Destroy( world->taskContexts ); + b3Array_Destroy( world->sensorTaskContexts ); +} + +b3WorldId b3CreateWorld( const b3WorldDef* def ) +{ + B3_CHECK_DEF( def ); + + B3_ASSERT( B3_LINEAR_SLOP <= B3_MESH_REST_OFFSET ); + B3_ASSERT( B3_MESH_REST_OFFSET < B3_SPECULATIVE_DISTANCE ); + + int worldId = B3_NULL_INDEX; + for ( int i = 0; i < B3_MAX_WORLDS; ++i ) + { + if ( b3_worlds[i].inUse == false ) + { + worldId = i; + break; + } + } + + if ( worldId == B3_NULL_INDEX ) + { + b3Log( "B3_MAX_WORLDS of %d exceeded!!!", B3_MAX_WORLDS ); + B3_ASSERT( worldId != B3_NULL_INDEX ); + return (b3WorldId){ 0 }; + } + + // b3Log( "b3_lengthUnitsPerMeter = %g", b3_lengthUnitsPerMeter ); + + int oldCount = b3AtomicFetchAddInt( &b3_worldCount, 1 ); + b3_maxWorldCount = b3MaxInt( b3_maxWorldCount, oldCount + 1 ); + + // b3Log( "Created world %d", worldId ); + + b3InitializeContactRegisters(); + + b3World* world = b3_worlds + worldId; + uint16_t revision = world->generation; + + memset( world, 0, sizeof( b3World ) ); + + world->worldId = (uint16_t)worldId; + world->generation = revision; + world->inUse = true; + + world->stack = b3CreateStack( 2048 ); + + b3Array_Reserve( world->manifoldAllocators, 16 ); + world->manifoldAllocatorMutex = b3CreateMutex(); + + b3CreateBroadPhase( &world->broadPhase, &def->capacity ); + b3CreateGraph( &world->constraintGraph, 16 ); + + // pools + world->bodyIdPool = b3CreateIdPool(); + + int bodyCapacity = b3MaxInt( 16, def->capacity.staticBodyCount + def->capacity.dynamicBodyCount ); + b3Array_Reserve( world->bodies, bodyCapacity ); + b3Array_Reserve( world->solverSets, 8 ); + + // add empty static, active, and disabled body sets + world->solverSetIdPool = b3CreateIdPool(); + b3SolverSet set = { 0 }; + + // static set + set.setIndex = b3AllocId( &world->solverSetIdPool ); + b3Array_Push( world->solverSets, set ); + b3Array_Reserve( world->solverSets.data[b3_staticSet].bodySims, b3MaxInt( 16, def->capacity.staticBodyCount ) ); + B3_ASSERT( world->solverSets.data[b3_staticSet].setIndex == b3_staticSet ); + + // disabled set + set.setIndex = b3AllocId( &world->solverSetIdPool ); + b3Array_Push( world->solverSets, set ); + B3_ASSERT( world->solverSets.data[b3_disabledSet].setIndex == b3_disabledSet ); + + // awake set + set.setIndex = b3AllocId( &world->solverSetIdPool ); + b3Array_Push( world->solverSets, set ); + b3Array_Reserve( world->solverSets.data[b3_awakeSet].bodySims, b3MaxInt( 16, def->capacity.dynamicBodyCount ) ); + b3Array_Reserve( world->solverSets.data[b3_awakeSet].bodyStates, b3MaxInt( 16, def->capacity.dynamicBodyCount ) ); + b3Array_Reserve( world->solverSets.data[b3_awakeSet].contactIndices, b3MaxInt( 16, def->capacity.contactCount ) ); + B3_ASSERT( world->solverSets.data[b3_awakeSet].setIndex == b3_awakeSet ); + + world->shapeIdPool = b3CreateIdPool(); + + int shapeCapacity = b3MaxInt( 16, def->capacity.staticShapeCount + def->capacity.dynamicShapeCount ); + b3Array_Reserve( world->shapes, shapeCapacity ); + + world->hullDatabase = b3Alloc( sizeof( b3HullMap ) ); + b3HullMap_init( world->hullDatabase ); + + world->names = b3CreateNameCache(); + + world->contactIdPool = b3CreateIdPool(); + b3Array_Reserve( world->contacts, b3MaxInt( 16, def->capacity.contactCount ) ); + + world->jointIdPool = b3CreateIdPool(); + b3Array_Reserve( world->joints, 16 ); + + world->islandIdPool = b3CreateIdPool(); + b3Array_Reserve( world->islands, b3MaxInt( 16, def->capacity.dynamicBodyCount ) ); + + b3Array_Reserve( world->sensors, 4 ); + + b3Array_Reserve( world->bodyMoveEvents, 4 ); + b3Array_Reserve( world->sensorBeginEvents, 4 ); + b3Array_Reserve( world->sensorEndEvents[0], 4 ); + b3Array_Reserve( world->sensorEndEvents[1], 4 ); + b3Array_Reserve( world->contactBeginEvents, 4 ); + b3Array_Reserve( world->contactEndEvents[0], 4 ); + b3Array_Reserve( world->contactEndEvents[1], 4 ); + b3Array_Reserve( world->contactHitEvents, 4 ); + b3Array_Reserve( world->jointEvents, 4 ); + world->endEventArrayIndex = 0; + + world->stepIndex = 0; + world->splitIslandId = B3_NULL_INDEX; + world->activeTaskCount = 0; + world->taskCount = 0; + world->gravity = def->gravity; + world->hitEventThreshold = def->hitEventThreshold; + world->restitutionThreshold = def->restitutionThreshold; + world->maxLinearSpeed = def->maximumLinearSpeed; + world->contactSpeed = def->contactSpeed; + world->contactHertz = def->contactHertz; + world->contactDampingRatio = def->contactDampingRatio; + world->contactRecycleDistance = B3_CONTACT_RECYCLE_DISTANCE; + + if ( def->frictionCallback == NULL ) + { + world->frictionCallback = b3DefaultFrictionCallback; + } + else + { + world->frictionCallback = def->frictionCallback; + } + + if ( def->restitutionCallback == NULL ) + { + world->restitutionCallback = b3DefaultRestitutionCallback; + } + else + { + world->restitutionCallback = def->restitutionCallback; + } + + world->enableSleep = def->enableSleep; + world->locked = false; + world->enableWarmStarting = true; + world->enableContinuous = def->enableContinuous; + world->enableSpeculative = true; + world->userTreeTask = NULL; + world->userData = def->userData; + + if ( def->workerCount > 0 && def->enqueueTask != NULL && def->finishTask != NULL ) + { + // External task system + world->workerCount = b3MinInt( def->workerCount, B3_MAX_WORKERS ); + world->enqueueTaskFcn = def->enqueueTask; + world->finishTaskFcn = def->finishTask; + world->userTaskContext = def->userTaskContext; + world->scheduler = NULL; + } + else if ( def->workerCount > 1 ) + { + // Built-in scheduler + world->workerCount = b3MinInt( def->workerCount, B3_MAX_WORKERS ); + world->scheduler = b3CreateScheduler( world->workerCount ); + world->enqueueTaskFcn = b3SchedulerEnqueueTask; + world->finishTaskFcn = b3SchedulerFinishTask; + world->userTaskContext = world->scheduler; + } + else + { + // Serial fallback + world->workerCount = 1; + world->enqueueTaskFcn = b3DefaultAddTaskFcn; + world->finishTaskFcn = b3DefaultFinishTaskFcn; + world->userTaskContext = NULL; + world->scheduler = NULL; + } + + b3CreateWorkerContexts( world ); + + world->debugBodySet = b3CreateBitSet( 256 ); + world->debugJointSet = b3CreateBitSet( 256 ); + world->debugContactSet = b3CreateBitSet( 256 ); + world->debugIslandSet = b3CreateBitSet( 256 ); + world->createDebugShape = def->createDebugShape; + world->destroyDebugShape = def->destroyDebugShape; + world->userDebugShapeContext = def->userDebugShapeContext; + + // add one to worldId so that 0 represents a null b3WorldId + return (b3WorldId){ (uint16_t)( worldId + 1 ), world->generation }; +} + +void b3DestroyWorld( b3WorldId worldId ) +{ + b3AtomicFetchAddInt( &b3_worldCount, -1 ); + + b3World* world = b3GetUnlockedWorldFromId( worldId ); + if ( world == NULL ) + { + return; + } + + world->locked = true; + + // Detach any recording before teardown. The user owns and frees the recording buffer. + b3StopRecordingInternal( world ); + + if ( world->scheduler != NULL ) + { + b3DestroyScheduler( world->scheduler ); + world->scheduler = NULL; + } + + b3DestroyBitSet( &world->debugBodySet ); + b3DestroyBitSet( &world->debugJointSet ); + b3DestroyBitSet( &world->debugContactSet ); + b3DestroyBitSet( &world->debugIslandSet ); + + b3DestroyWorkerContexts( world ); + + b3Array_Destroy( world->bodyMoveEvents ); + b3Array_Destroy( world->sensorBeginEvents ); + b3Array_Destroy( world->sensorEndEvents[0] ); + b3Array_Destroy( world->sensorEndEvents[1] ); + b3Array_Destroy( world->contactBeginEvents ); + b3Array_Destroy( world->contactEndEvents[0] ); + b3Array_Destroy( world->contactEndEvents[1] ); + b3Array_Destroy( world->contactHitEvents ); + b3Array_Destroy( world->jointEvents ); + + int sensorCount = world->sensors.count; + for ( int i = 0; i < sensorCount; ++i ) + { + b3Array_Destroy( world->sensors.data[i].hits ); + b3Array_Destroy( world->sensors.data[i].overlaps1 ); + b3Array_Destroy( world->sensors.data[i].overlaps2 ); + } + + b3Array_Destroy( world->sensors ); + b3Array_Destroy( world->bodies ); + + int shapeCapacity = world->shapes.count; + b3Shape* shapes = world->shapes.data; + for ( int i = 0; i < shapeCapacity; ++i ) + { + b3Shape* shape = shapes + i; + if ( shape->id != B3_NULL_INDEX ) + { + b3DestroyShapeAllocations( world, shapes + i ); + } + } + + int contactCapacity = world->contacts.count; + b3Contact* contacts = world->contacts.data; + for ( int i = 0; i < contactCapacity; ++i ) + { + b3Contact* contact = contacts + i; + if ( contact->contactId != B3_NULL_INDEX ) + { + if ( contact->flags & b3_simMeshContact ) + { + b3Array_Destroy( contact->meshContact.triangleCache ); + } + } + } + + // Destroying every shape above released all hull references, so the database is empty. + B3_ASSERT( b3HullMap_size( (b3HullMap*)world->hullDatabase ) == 0 ); + b3HullMap_cleanup( world->hullDatabase ); + b3Free( world->hullDatabase, sizeof( b3HullMap ) ); + world->hullDatabase = NULL; + + b3DestroyNameCache( &world->names ); + + b3Array_Destroy( world->shapes ); + b3Array_Destroy( world->contacts ); + b3Array_Destroy( world->joints ); + + for ( int i = 0; i < world->islands.count; ++i ) + { + b3Array_Destroy( world->islands.data[i].bodies ); + b3Array_Destroy( world->islands.data[i].contacts ); + b3Array_Destroy( world->islands.data[i].joints ); + } + b3Array_Destroy( world->islands ); + + // Destroy solver sets + int setCapacity = world->solverSets.count; + for ( int i = 0; i < setCapacity; ++i ) + { + b3SolverSet* set = world->solverSets.data + i; + if ( set->setIndex != B3_NULL_INDEX ) + { + b3DestroySolverSet( world, i ); + } + } + + b3Array_Destroy( world->solverSets ); + + b3DestroyGraph( &world->constraintGraph ); + b3DestroyBroadPhase( &world->broadPhase ); + + b3DestroyIdPool( &world->bodyIdPool ); + b3DestroyIdPool( &world->shapeIdPool ); + b3DestroyIdPool( &world->contactIdPool ); + b3DestroyIdPool( &world->jointIdPool ); + b3DestroyIdPool( &world->islandIdPool ); + b3DestroyIdPool( &world->solverSetIdPool ); + + for ( int i = 0; i < world->manifoldAllocators.count; ++i ) + { + b3DestroyBlockAllocator( world->manifoldAllocators.data + i ); + } + b3Array_Destroy( world->manifoldAllocators ); + b3DestroyMutex( world->manifoldAllocatorMutex ); + + b3DestroyStack( &world->stack ); + + // Wipe world but preserve generation + uint16_t generation = world->generation; + memset( world, 0, sizeof( b3World ) ); + world->generation = generation + 1; + + // b3Log( "Destroyed world %d", worldId.index1 - 1 ); +} + +int b3GetWorldCount( void ) +{ + return b3AtomicLoadInt( &b3_worldCount ); +} + +int b3GetMaxWorldCount( void ) +{ + return b3_maxWorldCount; +} + +// Issues T0 prefetches across the cache lines of a b3Contact (216 B / 4 lines). +// Used to hide the random-access latency of contact lookups while we work on an +// earlier index. +static inline void b3PrefetchContact( const b3Contact* contact ) +{ + const char* p = (const char*)contact; + b3Prefetch( p ); + b3Prefetch( p + 64 ); + b3Prefetch( p + 128 ); + b3Prefetch( p + 192 ); +} + +static void b3CollideTask( int startIndex, int endIndex, int workerIndex, void* context ) +{ + b3TracyCZoneNC( collide_task, "Collide Task", b3_colorDodgerBlue, true ); + + b3StepContext* stepContext = (b3StepContext*)context; + b3World* world = stepContext->world; + b3ConstraintGraph* graph = &world->constraintGraph; + b3TaskContext* taskContext = world->taskContexts.data + workerIndex; + int* contactIndices = stepContext->awakeContactIndices; + b3Contact* contacts = world->contacts.data; + b3Shape* shapes = world->shapes.data; + b3Body* bodies = world->bodies.data; + b3BodySim* awakeSims = world->solverSets.data[b3_awakeSet].bodySims.data; + b3BodySim* staticSims = world->solverSets.data[b3_staticSet].bodySims.data; + + B3_ASSERT( startIndex < endIndex ); + + float recycleDistance = world->contactRecycleDistance; + float speculativeDistance = B3_SPECULATIVE_DISTANCE; + float recycleDistanceNonTouching = b3MinFloat( recycleDistance, speculativeDistance ); + + // Prefetch contact[i + contactPrefetchDistance] each iteration so the random + // 216 B contact load lands in L1 by the time we reach it. Distance picked to + // cover ~200 cycles of memory latency without overshooting the L1 working set. + const int contactPrefetchDistance = 4; + int prefetchEnd = endIndex - contactPrefetchDistance; + + for ( int i = startIndex; i < endIndex; ++i ) + { + if ( i < prefetchEnd ) + { + b3PrefetchContact( contacts + contactIndices[i + contactPrefetchDistance] ); + } + + int contactIndex = contactIndices[i]; + B3_ASSERT( contactIndex < world->contacts.count ); + + b3Contact* contact = contacts + contactIndex; + B3_VALIDATE( contact->contactId == contactIndex ); + + b3Shape* shapeA = shapes + contact->shapeIdA; + b3Shape* shapeB = shapes + contact->shapeIdB; + + // Do proxies still overlap? + bool overlap = b3AABB_Overlaps( shapeA->fatAABB, shapeB->fatAABB ); + if ( overlap == false ) + { + // This contact will be destroyed + contact->flags |= b3_simDisjoint; + contact->flags &= ~b3_simTouchingFlag; + b3SetBit( &taskContext->contactStateBitSet, contactIndex ); + continue; + } + + // Update contact respecting shape/body order (A,B). Bodies behind awake-set + // contacts are always either awake or static - inline b3GetBodySim with that + // invariant to skip the cross-TU call and per-call solverSets indirection. + b3Body* bodyA = bodies + shapeA->bodyId; + b3Body* bodyB = bodies + shapeB->bodyId; + bool isStaticA = bodyA->type == b3_staticBody; + bool isStaticB = bodyB->type == b3_staticBody; + bool wasTouching = ( contact->flags & b3_simTouchingFlag ); + bool isMeshContact = ( contact->flags & b3_simMeshContact ); + b3BodySim* bodySimA; + b3BodySim* bodySimB; + if ( wasTouching ) + { + B3_ASSERT( bodyA->setIndex == b3_awakeSet || bodyA->setIndex == b3_staticSet ); + B3_ASSERT( bodyB->setIndex == b3_awakeSet || bodyB->setIndex == b3_staticSet ); + bodySimA = ( isStaticA ? staticSims : awakeSims ) + bodyA->localIndex; + bodySimB = ( isStaticB ? staticSims : awakeSims ) + bodyB->localIndex; + } + else + { + // There can be non-touching contacts between awake bodies and sleeping bodies. + { + b3SolverSet* set = b3Array_Get( world->solverSets, bodyA->setIndex ); + bodySimA = b3Array_Get( set->bodySims, bodyA->localIndex ); + } + { + b3SolverSet* set = b3Array_Get( world->solverSets, bodyB->setIndex ); + bodySimB = b3Array_Get( set->bodySims, bodyB->localIndex ); + } + } + + b3WorldTransform transformA = bodySimA->transform; + b3WorldTransform transformB = bodySimB->transform; + + bool isFast = ( bodySimA->flags & b3_isFast ) || ( bodySimB->flags & b3_isFast ); + + // These are used by the contact solver. If the contact is between an awake body + // and a sleeping body and the contact begins to touch, the these will be invalid + // but fixed when linked in the constraint graph. + contact->bodySimIndexA = isStaticA ? B3_NULL_INDEX : bodyA->localIndex; + contact->bodySimIndexB = isStaticB ? B3_NULL_INDEX : bodyB->localIndex; + float recycleTolerance = wasTouching ? recycleDistance : recycleDistanceNonTouching; + + // Contact recycling optimization. Please cite this library if you use this optimization. + // This is inspired by persistent contact manifolds used in some physics engines, such as PhysX. + // However, this allows larger relative motion and has fewer tuning parameters (just one). + if ( ( isFast == false || isMeshContact == false ) && recycleDistance > 0.0f && + ( contact->flags & b3_relativeTransformValid ) && ( contact->flags & b3_contactRecycleFlag ) ) + { + float angleA = b3DotQuat( transformA.q, contact->cachedRotationA ); + float angleB = b3DotQuat( transformB.q, contact->cachedRotationB ); + float angularDistance = b3MinFloat( angleA * angleA, angleB * angleB ); + + b3Transform xf = b3InvMulWorldTransforms( transformA, transformB ); + b3Transform xfc = contact->cachedRelativePose; + b3Vec3 maxExtentA = isStaticA ? b3Vec3_zero : bodySimA->maxExtent; + b3Vec3 maxExtentB = isStaticB ? b3Vec3_zero : bodySimB->maxExtent; + b3Vec3 maxExtent = b3Max( maxExtentA, maxExtentB ); + + // Variation of Conservative Advancement + // distance + 2 * length(modified_cross(|qr.v|, maxExtent)) < recycleTolerance. + // 2*|qr.v| == 2*|sin(theta/2)| ~= theta for small angles. + float distSquared = b3DistanceSquared( xf.p, xfc.p ); + + if ( angularDistance > B3_CONTACT_RECYCLE_ANGULAR_DISTANCE && distSquared < recycleTolerance * recycleTolerance ) + { + float distance = sqrtf( distSquared ); + float slack = recycleTolerance - distance; + + // qr = inv( inv(qA0) * qB0 ) * inv(qA) * qB + // = inv(qB0) * qA0 * inv(qA) * qB + // Suppose A is static + // qr = inv(qB0) * qA0 * inv(qA0) * qB + // = inv(qB0) * qB + // qB = qB0 * qr + // Therefore qr is associated with the local angular velocity of body B when A is static. + b3Quat qr = b3InvMulQuat( xfc.q, xf.q ); + b3Vec3 arc = b3ModifiedCross( b3Abs( qr.v ), maxExtent ); + + float arcSq = 4.0f * b3LengthSquared( arc ); + if ( arcSq < slack * slack ) + { + b3Quat dqA = b3MulQuat( transformA.q, b3Conjugate( contact->cachedRotationA ) ); + b3Quat dqB = b3MulQuat( transformB.q, b3Conjugate( contact->cachedRotationB ) ); + b3Matrix3 matrixA = b3MakeMatrixFromQuat( dqA ); + b3Matrix3 matrixB = b3MakeMatrixFromQuat( dqB ); + + // Minimize round-off + b3Vec3 dc = b3SubPos( bodySimB->center, bodySimA->center ); + + int manifoldCount = contact->manifoldCount; + for ( int manifoldIndex = 0; manifoldIndex < manifoldCount; ++manifoldIndex ) + { + b3Manifold* manifold = contact->manifolds + manifoldIndex; + b3Vec3 normal = manifold->normal; + + int pointCount = manifold->pointCount; + for ( int pointIndex = 0; pointIndex < pointCount; ++pointIndex ) + { + // Keep anchors but update separation, same as sub-stepping. This eliminates jitter. + b3ManifoldPoint* mp = manifold->points + pointIndex; + b3Vec3 rA = b3MulMV( matrixA, mp->anchorA ); + b3Vec3 rB = b3MulMV( matrixB, mp->anchorB ); + b3Vec3 dp = b3Add( dc, b3Sub( rB, rA ) ); + mp->separation = mp->baseSeparation + b3Dot( dp, normal ); + mp->persisted = true; + } + } + + // Diagnostics + taskContext->recycledContactCount += 1; + int bucketIndex = b3MinInt( manifoldCount, B3_CONTACT_MANIFOLD_COUNT_BUCKETS - 1 ); + if ( bucketIndex > 0 ) + { + taskContext->manifoldCounts[bucketIndex - 1] += 1; + } + + // Contact is recycled. This also skips updating other aspects of the contact + // such as material parameters. + continue; + } + } + } + + // Caching for contact recycling. + contact->cachedRotationA = transformA.q; + contact->cachedRotationB = transformB.q; + contact->cachedRelativePose = b3InvMulWorldTransforms( transformA, transformB ); + contact->flags |= b3_relativeTransformValid; + + // This updates solid contacts + bool touching = b3UpdateContact( world, workerIndex, contact, shapeA, bodySimA->localCenter, transformA, shapeB, + bodySimB->localCenter, transformB, isFast, taskContext->arena ); + + int bucketIndex = b3MinInt( contact->manifoldCount, B3_CONTACT_MANIFOLD_COUNT_BUCKETS - 1 ); + if ( bucketIndex > 0 ) + { + taskContext->manifoldCounts[bucketIndex - 1] += 1; + } + + // Update the mesh contact spec + if ( touching == true && wasTouching == true && ( contact->flags & b3_simMeshContact ) ) + { + B3_ASSERT( contact->colorIndex != B3_NULL_INDEX ); + B3_ASSERT( 0 <= contact->colorIndex && contact->colorIndex < B3_GRAPH_COLOR_COUNT ); + b3GraphColor* color = graph->colors + contact->colorIndex; + b3ContactSpec* spec = b3Array_Get( color->contacts, contact->localIndex ); + spec->manifoldCount = (uint16_t)contact->manifoldCount; + } + + // State changes that affect island connectivity. Also affects contact events. + if ( touching == true && wasTouching == false ) + { + contact->flags |= b3_simStartedTouching; + b3SetBit( &taskContext->contactStateBitSet, contactIndex ); + } + else if ( touching == false && wasTouching == true ) + { + contact->flags |= b3_simStoppedTouching; + b3SetBit( &taskContext->contactStateBitSet, contactIndex ); + } + + for ( int manifoldIndex = 0; manifoldIndex < contact->manifoldCount; ++manifoldIndex ) + { + b3Manifold* manifold = contact->manifolds + manifoldIndex; + for ( int pointIndex = 0; pointIndex < manifold->pointCount; ++pointIndex ) + { + // Cache separation + b3ManifoldPoint* mp = manifold->points + pointIndex; + mp->baseSeparation = mp->separation; + } + } + } + + b3TracyCZoneEnd( collide_task ); +} + +static void b3AddNonTouchingContact( b3World* world, b3Contact* contact ) +{ + B3_ASSERT( contact->setIndex == b3_awakeSet ); + b3SolverSet* set = b3Array_Get( world->solverSets, b3_awakeSet ); + contact->colorIndex = B3_NULL_INDEX; + contact->localIndex = set->contactIndices.count; + contact->bodySimIndexA = B3_NULL_INDEX; + contact->bodySimIndexB = B3_NULL_INDEX; + b3Array_Push( set->contactIndices, contact->contactId ); +} + +static void b3RemoveNonTouchingContact( b3World* world, int setIndex, int localIndex ) +{ + b3SolverSet* set = b3Array_Get( world->solverSets, setIndex ); + int movedIndex = b3Array_RemoveSwap( set->contactIndices, localIndex ); + if ( movedIndex != B3_NULL_INDEX ) + { + int movedContactIndex = set->contactIndices.data[localIndex]; + b3Contact* movedContact = b3Array_Get( world->contacts, movedContactIndex ); + B3_ASSERT( movedContact->setIndex == setIndex ); + B3_ASSERT( movedContact->colorIndex == B3_NULL_INDEX ); + B3_ASSERT( movedContact->localIndex == movedIndex ); + movedContact->localIndex = localIndex; + } +} + +// Narrow-phase collision +static void b3Collide( b3StepContext* context ) +{ + b3World* world = context->world; + + B3_ASSERT( world->workerCount > 0 ); + + b3TracyCZoneNC( collide, "Collide", b3_colorDarkOrchid, true ); + + // Gather contacts from all the graph colors into a single array for easier parallel-for + int touchingCount = 0; + + b3GraphColor* graphColors = world->constraintGraph.colors; + for ( int i = 0; i < B3_GRAPH_COLOR_COUNT; ++i ) + { + touchingCount += graphColors[i].convexContacts.count + graphColors[i].contacts.count; + } + + b3SolverSet* awakeSet = b3Array_Get( world->solverSets, b3_awakeSet ); + int nonTouchingCount = awakeSet->contactIndices.count; + + int contactCount = touchingCount + nonTouchingCount; + + if ( contactCount == 0 ) + { + b3TracyCZoneEnd( collide ); + return; + } + + int* contactIndices = (int*)b3StackAlloc( &world->stack, contactCount * sizeof( int ), "contact indices" ); + + int contactIndex = 0; + for ( int i = 0; i < B3_GRAPH_COLOR_COUNT; ++i ) + { + b3GraphColor* color = graphColors + i; + int count = color->convexContacts.count; + for ( int j = 0; j < count; ++j ) + { + contactIndices[contactIndex] = color->convexContacts.data[j]; + contactIndex += 1; + } + + count = color->contacts.count; + for ( int j = 0; j < count; ++j ) + { + contactIndices[contactIndex] = color->contacts.data[j].contactId; + contactIndex += 1; + } + } + + B3_ASSERT( contactIndex == touchingCount ); + + if ( nonTouchingCount > 0 ) + { + int* nonTouchingIndices = awakeSet->contactIndices.data; + memcpy( contactIndices + touchingCount, nonTouchingIndices, nonTouchingCount * sizeof( int ) ); + } + + context->awakeContactIndices = contactIndices; + + // Contact bit set on ids because contact pointers are unstable as they move between touching and not touching. + int contactIdCapacity = b3GetIdCapacity( &world->contactIdPool ); + for ( int i = 0; i < world->workerCount; ++i ) + { + b3TaskContext* taskContext = world->taskContexts.data + i; + b3SetBitCountAndClear( &taskContext->contactStateBitSet, contactIdCapacity ); + taskContext->satCallCount = 0; + taskContext->satCacheHitCount = 0; + taskContext->recycledContactCount = 0; + memset( taskContext->manifoldCounts, 0, sizeof( taskContext->manifoldCounts ) ); + } + + // Task should take at least 40us on a 4GHz CPU (10K cycles) + int minRange = 20; + b3ParallelFor( world, b3CollideTask, contactCount, minRange, context, "collide" ); + + b3StackFree( &world->stack, contactIndices ); + context->awakeContactIndices = NULL; + contactIndices = NULL; + + // Serially update contact state + // todo bring this zone together with island merge + b3TracyCZoneNC( contact_state, "Contact State", b3_colorLightSlateGray, true ); + + int satMultiplier = context->dt > 0.0f ? 1 : 0; + + // Bitwise OR all contact bits + b3BitSet* bitSet = &world->taskContexts.data[0].contactStateBitSet; + world->satCallCount = satMultiplier * world->taskContexts.data[0].satCallCount; + world->satCacheHitCount = satMultiplier * world->taskContexts.data[0].satCacheHitCount; + memcpy( world->manifoldCounts, world->taskContexts.data[0].manifoldCounts, + B3_CONTACT_MANIFOLD_COUNT_BUCKETS * sizeof( int ) ); + + for ( int i = 1; i < world->workerCount; ++i ) + { + b3InPlaceUnion( bitSet, &world->taskContexts.data[i].contactStateBitSet ); + world->satCallCount += world->taskContexts.data[i].satCallCount; + world->satCacheHitCount += world->taskContexts.data[i].satCacheHitCount; + for ( int j = 0; j < B3_CONTACT_MANIFOLD_COUNT_BUCKETS; ++j ) + { + world->manifoldCounts[j] += world->taskContexts.data[i].manifoldCounts[j]; + } + } + + // Release per-step overflow blocks and grow the backing capacity if last + // step's demand exceeded it. Contact processing is the only consumer of + // these arenas and is finished by this point. + for ( int i = 0; i < world->workerCount; ++i ) + { + b3ArenaSync( &world->taskContexts.data[i].arena ); + } + + int endEventArrayIndex = world->endEventArrayIndex; + + const b3Shape* shapes = world->shapes.data; + uint16_t worldId = world->worldId; + + // Process contact state changes. Iterate over set bits + for ( uint32_t k = 0; k < bitSet->blockCount; ++k ) + { + uint64_t bits = bitSet->bits[k]; + while ( bits != 0 ) + { + uint32_t ctz = b3CTZ64( bits ); + int contactId = (int)( 64 * k + ctz ); + + b3Contact* contact = b3Array_Get( world->contacts, contactId ); + B3_ASSERT( contact->setIndex == b3_awakeSet ); + + const b3Shape* shapeA = shapes + contact->shapeIdA; + const b3Shape* shapeB = shapes + contact->shapeIdB; + b3ShapeId shapeIdA = { shapeA->id + 1, worldId, shapeA->generation }; + b3ShapeId shapeIdB = { shapeB->id + 1, worldId, shapeB->generation }; + b3ContactId contactFullId = { + .index1 = contactId + 1, + .world0 = worldId, + .padding = 0, + .generation = contact->generation, + }; + uint32_t flags = contact->flags; + + if ( flags & b3_simDisjoint ) + { + // Bounding boxes no longer overlap + b3DestroyContact( world, contact, false ); + contact = NULL; + } + else if ( flags & b3_simStartedTouching ) + { + B3_ASSERT( contact->islandId == B3_NULL_INDEX ); + + if ( flags & b3_contactEnableContactEvents ) + { + b3ContactBeginTouchEvent event = { shapeIdA, shapeIdB, contactFullId }; + b3Array_Push( world->contactBeginEvents, event ); + } + + B3_ASSERT( contact->manifoldCount > 0 ); + B3_ASSERT( contact->setIndex == b3_awakeSet ); + + // Link first because this wakes colliding bodies and ensures the body sims + // are in the correct place. + contact->flags &= ~b3_simStartedTouching; + contact->flags |= b3_contactTouchingFlag; + b3LinkContact( world, contact ); + + // Make sure these didn't change + B3_ASSERT( contact->colorIndex == B3_NULL_INDEX ); + + // Contact sim pointer may have become orphaned due to awake set growth, + // so I just need to refresh it. + + int oldLocalIndex = contact->localIndex; + + b3AddContactToGraph( world, contact ); + b3RemoveNonTouchingContact( world, b3_awakeSet, oldLocalIndex ); + } + else if ( flags & b3_simStoppedTouching ) + { + contact->flags &= ~b3_simStoppedTouching; + contact->flags &= ~b3_contactTouchingFlag; + + if ( contact->flags & b3_contactEnableContactEvents ) + { + b3ContactEndTouchEvent event = { shapeIdA, shapeIdB, contactFullId }; + b3Array_Push( world->contactEndEvents[endEventArrayIndex], event ); + } + + B3_ASSERT( contact->manifoldCount == 0 ); + + // Cache these here for the remove below + int colorIndex = contact->colorIndex; + int localIndex = contact->localIndex; + + b3UnlinkContact( world, contact ); + int bodyIdA = contact->edges[0].bodyId; + int bodyIdB = contact->edges[1].bodyId; + + b3AddNonTouchingContact( world, contact ); + + bool isMeshContact = contact->flags & b3_simMeshContact; + b3RemoveContactFromGraph( world, bodyIdA, bodyIdB, colorIndex, localIndex, isMeshContact ); + contact = NULL; + } + + // Clear the smallest set bit + bits = bits & ( bits - 1 ); + } + } + + b3ValidateSolverSets( world ); + b3ValidateContacts( world ); + + b3TracyCZoneEnd( contact_state ); + b3TracyCZoneEnd( collide ); +} + +void b3World_Step( b3WorldId worldId, float timeStep, int subStepCount ) +{ + b3World* world = b3GetUnlockedWorldFromId( worldId ); + if ( world == NULL ) + { + return; + } + + B3_REC( world, Step, worldId, timeStep, subStepCount ); + + world->locked = true; + + b3TracyCZoneNC( world_step, "Step", b3_colorBox2DGreen, true ); + + // Clear debug buffers + for ( int i = 0; i < world->workerCount; ++i ) + { + world->taskContexts.data[i].pointCount = 0; + world->taskContexts.data[i].lineCount = 0; + } + + // Prepare to capture events + // Ensure user does not access stale data if there is an early return + b3Array_Clear( world->bodyMoveEvents ); + b3Array_Clear( world->sensorBeginEvents ); + b3Array_Clear( world->contactBeginEvents ); + b3Array_Clear( world->contactHitEvents ); + b3Array_Clear( world->jointEvents ); + + world->profile = (b3Profile){ 0 }; + + world->activeTaskCount = 0; + world->taskCount = 0; + + if ( world->scheduler != NULL ) + { + b3ResetScheduler( world->scheduler ); + } + + uint64_t stepTicks = b3GetTicks(); + + { + b3Capacity* c = &world->maxCapacity; + c->staticShapeCount = b3MaxInt( c->staticShapeCount, world->broadPhase.trees[b3_staticBody].proxyCount ); + c->dynamicShapeCount = b3MaxInt( c->dynamicShapeCount, world->broadPhase.trees[b3_dynamicBody].proxyCount ); + + int staticBodyCount = world->solverSets.data[b3_staticSet].bodySims.count; + c->staticBodyCount = b3MaxInt( c->staticBodyCount, staticBodyCount ); + + // this includes kinematic bodies + int totalBodyCount = b3GetIdCount( &world->bodyIdPool ); + c->dynamicBodyCount = b3MaxInt( c->dynamicBodyCount, totalBodyCount - staticBodyCount ); + + int totalContactCount = b3GetIdCount( &world->contactIdPool ); + c->contactCount = b3MaxInt( c->contactCount, totalContactCount ); + } + + // Update collision pairs and create contacts + { + uint64_t pairTicks = b3GetTicks(); + b3UpdateBroadPhasePairs( world ); + world->profile.pairs = b3GetMilliseconds( pairTicks ); + } + + b3SolverSet* awakeSet = b3Array_Get( world->solverSets, b3_awakeSet ); + + b3StepContext context = { 0 }; + context.world = world; + context.states = awakeSet->bodyStates.data; + context.dt = timeStep; + context.subStepCount = b3MaxInt( 1, subStepCount ); + + if ( timeStep > 0.0f ) + { + context.inv_dt = 1.0f / timeStep; + context.h = timeStep / context.subStepCount; + context.inv_h = context.subStepCount * context.inv_dt; + } + else + { + context.inv_dt = 0.0f; + context.h = 0.0f; + context.inv_h = 0.0f; + } + + world->inv_h = context.inv_h; + world->inv_dt = context.inv_dt; + + // Hertz values get reduced for large time steps + float contactHertz = b3MinFloat( world->contactHertz, 0.125f * context.inv_h ); + context.contactSoftness = b3MakeSoft( contactHertz, world->contactDampingRatio, context.h ); + context.staticSoftness = b3MakeSoft( 2.0f * contactHertz, 0.5f * world->contactDampingRatio, context.h ); + + context.restitutionThreshold = world->restitutionThreshold; + context.maxLinearVelocity = world->maxLinearSpeed; + context.enableWarmStarting = world->enableWarmStarting; + + // Narrow phase : update contacts + { + uint64_t collideTicks = b3GetTicks(); + b3Collide( &context ); + world->profile.collide = b3GetMilliseconds( collideTicks ); + } + + // Integrate velocities, solve velocity constraints, and integrate positions. + if ( timeStep > 0.0f ) + { + uint64_t solveTicks = b3GetTicks(); + b3Solve( world, &context ); + world->profile.solve = b3GetMilliseconds( solveTicks ); + } + + // Finish the tree task in case b3Solve didn't finish it + if ( world->userTreeTask ) + { + world->finishTaskFcn( world->userTreeTask, world->userTaskContext ); + world->userTreeTask = NULL; + world->activeTaskCount -= 1; + } + + // Update sensors + { + uint64_t sensorTicks = b3GetTicks(); + b3OverlapSensors( world ); + world->profile.sensors = b3GetMilliseconds( sensorTicks ); + } + + world->profile.step = b3GetMilliseconds( stepTicks ); + + B3_ASSERT( world->stack.allocation == 0 ); + + // Ensure stack is large enough + b3GrowStack( &world->stack ); + + // Make sure all tasks that were started were also finished + B3_ASSERT( world->activeTaskCount == 0 ); + + // Swap end event array buffers + world->endEventArrayIndex = 1 - world->endEventArrayIndex; + b3Array_Clear( world->sensorEndEvents[world->endEventArrayIndex] ); + b3Array_Clear( world->contactEndEvents[world->endEventArrayIndex] ); + world->locked = false; + + if ( world->recording != NULL ) + { + uint64_t hash = b3HashWorldState( world ); + b3RecArgs_StateHash stateHash = { worldId, hash }; + b3RecWrite_StateHash( world->recording, &stateHash ); + + // Fold this step's world bounds into the recording so a viewer can frame the whole motion. + b3AABB worldBounds = { 0 }; + bool haveBounds = false; + for ( int i = 0; i < b3_bodyTypeCount; ++i ) + { + b3DynamicTree* tree = world->broadPhase.trees + i; + if ( b3DynamicTree_GetProxyCount( tree ) == 0 ) + { + continue; + } + b3AABB bounds = b3DynamicTree_GetRootBounds( tree ); + worldBounds = haveBounds ? b3AABB_Union( worldBounds, bounds ) : bounds; + haveBounds = true; + } + if ( haveBounds ) + { + b3RecAccumulateBounds( world->recording, worldBounds ); + } + } + + b3TracyCZoneEnd( world_step ); + b3TracyCFrame; +} + +typedef struct DrawContext +{ + b3World* world; + b3DebugDraw* draw; +} DrawContext; + +static bool DrawQueryCallback( int proxyId, uint64_t userData, void* context ) +{ + int shapeId = (int)userData; + + B3_UNUSED( proxyId ); + + struct DrawContext* drawContext = (DrawContext*)context; + b3World* world = drawContext->world; + b3DebugDraw* draw = drawContext->draw; + + b3Shape* shape = b3Array_Get( world->shapes, shapeId ); + B3_ASSERT( shape->id == shapeId ); + + b3SetBit( &world->debugBodySet, shape->bodyId ); + + if ( draw->drawShapes ) + { + b3Body* body = b3Array_Get( world->bodies, shape->bodyId ); + b3BodySim* bodySim = b3GetBodySim( world, body ); + + b3HexColor color; + + const b3SurfaceMaterial* surfaceMaterial = b3GetShapeMaterials( shape ); + if ( surfaceMaterial[0].customColor != 0 ) + { + // May already carry a packed material preset, pass through unchanged + color = (b3HexColor)surfaceMaterial[0].customColor; + } + else + { + // Hue carries the state, material carries its energy. Calm matte for the + // resting masses, glossy for fast bodies, metallic for the driven kinematic. + // Diagnostic states keep a saturated hue and the default material so they pop. + b3HexColor rgb; + b3DebugMaterial material = b3_debugMaterialDefault; + + if ( body->type == b3_dynamicBody && body->mass == 0.0f ) + { + // Bad body + rgb = b3_colorRed; + } + else if ( body->setIndex == b3_disabledSet ) + { + rgb = b3_colorSlateGray; + } + else if ( shape->sensorIndex != B3_NULL_INDEX ) + { + rgb = b3_colorWheat; + } + else if ( body->flags & b3_hadTimeOfImpact ) + { + rgb = b3_colorLime; + } + else if ( ( bodySim->flags & b3_isBullet ) && body->setIndex == b3_awakeSet ) + { + rgb = b3_colorTurquoise; + } + else if ( body->flags & b3_isSpeedCapped ) + { + rgb = b3_colorYellow; + } + else if ( bodySim->flags & b3_isFast ) + { + rgb = b3_colorOrange; + material = b3_debugMaterialGlossy; + } + else if ( body->type == b3_staticBody ) + { + rgb = b3_colorDarkGray; + material = b3_debugMaterialMatte; + } + else if ( body->type == b3_kinematicBody ) + { + if ( body->setIndex == b3_awakeSet ) + { + rgb = b3_colorSteelBlue; + material = b3_debugMaterialMetallic; + } + else + { + rgb = b3_colorLightSteelBlue; + material = b3_debugMaterialMatte; + } + } + else if ( body->setIndex == b3_awakeSet ) + { + rgb = b3_colorTan; + material = b3_debugMaterialSoft; + } + else + { + rgb = b3_colorLightSlateGray; + material = b3_debugMaterialDead; + } + + color = (b3HexColor)b3MakeDebugColor( rgb, material ); + } + + if ( shape->userShape == NULL && world->createDebugShape != NULL ) + { + b3DebugShape debugShape = { 0 }; + debugShape.shapeId = (b3ShapeId){ + .world0 = world->worldId, + .index1 = shapeId + 1, + .generation = shape->generation, + }; + debugShape.type = shape->type; + + switch ( shape->type ) + { + case b3_capsuleShape: + debugShape.capsule = &shape->capsule; + shape->userShape = world->createDebugShape( &debugShape, world->userDebugShapeContext ); + break; + case b3_compoundShape: + debugShape.compound = shape->compound; + shape->userShape = world->createDebugShape( &debugShape, world->userDebugShapeContext ); + break; + case b3_heightShape: + debugShape.heightField = shape->heightField; + shape->userShape = world->createDebugShape( &debugShape, world->userDebugShapeContext ); + break; + case b3_hullShape: + debugShape.hull = shape->hull; + shape->userShape = world->createDebugShape( &debugShape, world->userDebugShapeContext ); + break; + case b3_meshShape: + debugShape.mesh = &shape->mesh; + shape->userShape = world->createDebugShape( &debugShape, world->userDebugShapeContext ); + break; + case b3_sphereShape: + debugShape.sphere = &shape->sphere; + shape->userShape = world->createDebugShape( &debugShape, world->userDebugShapeContext ); + break; + default: + B3_ASSERT( false ); + break; + } + } + + if ( shape->userShape != NULL ) + { + draw->DrawShapeFcn( shape->userShape, bodySim->transform, color, draw->context ); + } + } + + if ( draw->drawBounds ) + { + draw->DrawBoundsFcn( shape->fatAABB, b3_colorGold, draw->context ); + } + + return true; +} + +void b3World_Draw( b3WorldId worldId, b3DebugDraw* draw, uint64_t maskBits ) +{ + b3World* world = b3GetUnlockedWorldFromId( worldId ); + if ( world == NULL ) + { + return; + } + + B3_ASSERT( b3IsValidAABB( draw->drawingBounds ) ); + + float lengthScale = b3GetLengthUnitsPerMeter(); + float axisScale = 0.3f * lengthScale; + b3HexColor farColor = b3_colorDarkBlue; + b3HexColor speculativeColor = b3_colorBlue; + b3HexColor addColor = b3_colorLimeGreen; + b3HexColor persistColor = b3_colorLightBlue; + // b3HexColor normalColors[3] = { b3_colorLightGray, b3_colorLightSalmon, b3_colorLightSeaGreen }; + b3HexColor impulseColor = b3_colorMagenta; + b3HexColor frictionColor = b3_colorYellow; + + b3HexColor graphColors[B3_GRAPH_COLOR_COUNT] = { + b3_colorRed, b3_colorOrange, b3_colorYellow, b3_colorGreen, b3_colorCyan, b3_colorBlue, + b3_colorViolet, b3_colorPink, b3_colorChocolate, b3_colorGoldenRod, b3_colorCoral, b3_colorRosyBrown, + b3_colorAqua, b3_colorPeru, b3_colorLime, b3_colorGold, b3_colorPlum, b3_colorSnow, + b3_colorTeal, b3_colorKhaki, b3_colorSalmon, b3_colorPeachPuff, b3_colorHoneyDew, b3_colorBlack, + }; + + int bodyCapacity = b3GetIdCapacity( &world->bodyIdPool ); + b3SetBitCountAndClear( &world->debugBodySet, bodyCapacity ); + + int jointCapacity = b3GetIdCapacity( &world->jointIdPool ); + b3SetBitCountAndClear( &world->debugJointSet, jointCapacity ); + + int contactCapacity = b3GetIdCapacity( &world->contactIdPool ); + b3SetBitCountAndClear( &world->debugContactSet, contactCapacity ); + + int islandCapacity = b3GetIdCapacity( &world->islandIdPool ); + b3SetBitCountAndClear( &world->debugIslandSet, islandCapacity ); + + struct DrawContext drawContext = { world, draw }; + + for ( int i = 0; i < b3_bodyTypeCount; ++i ) + { + b3DynamicTree_Query( world->broadPhase.trees + i, draw->drawingBounds, maskBits, false, DrawQueryCallback, &drawContext ); + } + + uint32_t wordCount = world->debugBodySet.blockCount; + uint64_t* bits = world->debugBodySet.bits; + for ( uint32_t wordIndex = 0; wordIndex < wordCount; ++wordIndex ) + { + uint64_t word = bits[wordIndex]; + while ( word != 0 ) + { + uint32_t ctz = b3CTZ64( word ); + int bodyId = (int)( 64 * wordIndex + ctz ); + + b3Body* body = b3Array_Get( world->bodies, bodyId ); + b3BodySim* bodySim = b3GetBodySim( world, body ); + + if ( draw->drawBodyNames && body->nameId != B3_NULL_NAME ) + { + b3Vec3 offset = { 0.05f, 0.05f, 0.05f }; + b3WorldTransform transform = { bodySim->center, bodySim->transform.q }; + b3Pos p = b3TransformWorldPoint( transform, offset ); + const char* name = b3FindName( &world->names, body->nameId ); + if ( name != NULL ) + { + draw->DrawStringFcn( p, name, b3_colorOrange, draw->context ); + } + } + + if ( draw->drawMass && body->type == b3_dynamicBody ) + { + b3Vec3 offset = { 0.1f, 0.1f, 0.1f }; + + b3WorldTransform transform = { bodySim->center, bodySim->transform.q }; + draw->DrawTransformFcn( transform, draw->context ); + b3Pos p = b3TransformWorldPoint( transform, offset ); + + char buffer[32]; + snprintf( buffer, 32, " %.2f", body->mass ); + draw->DrawStringFcn( p, buffer, b3_colorWhite, draw->context ); + } + + if ( draw->drawSleep ) + { + b3BodyState* bodyState = b3GetBodyState( world, body ); + + if ( bodyState != NULL ) + { + b3HexColor colors[4] = { b3_colorBlue, b3_colorSkyBlue, b3_colorOrange, b3_colorRed }; + + b3HexColor color = b3_colorBlack; + if ( body->sleepThreshold > 0.0f ) + { + float ratio = body->sleepVelocity / body->sleepThreshold; + int index = b3ClampInt( (int)ratio, 0, 3 ); + color = colors[index]; + } + + b3Pos center = bodySim->center; + draw->DrawPointFcn( center, 10.0f, color, draw->context ); + + b3Vec3 offset = { 0.1f, 0.1f, 0.1f }; + b3Pos p = b3OffsetPos( center, offset ); + + char buffer[32]; + snprintf( buffer, 32, " %.3f", body->sleepVelocity ); + draw->DrawStringFcn( p, buffer, color, draw->context ); + } + } + + if ( draw->drawJoints ) + { + int jointKey = body->headJointKey; + while ( jointKey != B3_NULL_INDEX ) + { + int jointId = jointKey >> 1; + int edgeIndex = jointKey & 1; + b3Joint* joint = b3Array_Get( world->joints, jointId ); + + // avoid double draw + if ( b3GetBit( &world->debugJointSet, jointId ) == false ) + { + b3DrawJoint( draw, world, joint ); + b3SetBit( &world->debugJointSet, jointId ); + } + + jointKey = joint->edges[edgeIndex].nextKey; + } + } + + const float speculativeDistance = B3_SPECULATIVE_DISTANCE; + if ( draw->drawContacts && body->type == b3_dynamicBody && body->setIndex == b3_awakeSet ) + { + int contactKey = body->headContactKey; + while ( contactKey != B3_NULL_INDEX ) + { + int contactId = contactKey >> 1; + int edgeIndex = contactKey & 1; + b3Contact* contact = b3Array_Get( world->contacts, contactId ); + contactKey = contact->edges[edgeIndex].nextKey; + + if ( contact->setIndex != b3_awakeSet || contact->colorIndex == B3_NULL_INDEX ) + { + continue; + } + + // avoid double draw + if ( b3GetBit( &world->debugContactSet, contactId ) == false ) + { + b3Body* bodyA = b3Array_Get( world->bodies, contact->edges[0].bodyId ); + b3BodySim* bodySimA = b3GetBodySim( world, bodyA ); + b3Body* bodyB = b3Array_Get( world->bodies, contact->edges[1].bodyId ); + b3BodySim* bodySimB = b3GetBodySim( world, bodyB ); + + for ( int manifoldIndex = 0; manifoldIndex < contact->manifoldCount; ++manifoldIndex ) + { + const b3Manifold* manifold = contact->manifolds + manifoldIndex; + B3_ASSERT( manifold->pointCount > 0 ); + + b3Vec3 normal = manifold->normal; + + // Average the anchors not the world points so the friction center stays exact far from the origin + b3Pos contactCenter = draw->drawAnchorA == 1 ? bodySimA->center : bodySimB->center; + b3Vec3 anchorSum = b3Vec3_zero; + + const b3ManifoldPoint* points = manifold->points; + for ( int pointIndex = 0; pointIndex < manifold->pointCount; ++pointIndex ) + { + const b3ManifoldPoint* mp = points + pointIndex; + + char buffer[32]; + + b3Vec3 anchor = draw->drawAnchorA == 1 ? mp->anchorA : mp->anchorB; + b3Pos p = b3OffsetPos( contactCenter, anchor ); + + anchorSum = b3Add( anchorSum, anchor ); + + if ( draw->drawContactNormals ) + { + b3Pos p1 = p; + b3Pos p2 = b3OffsetPos( p1, b3MulSV( axisScale, normal ) ); + draw->DrawSegmentFcn( p1, p2, b3_colorLightGray, draw->context ); + + snprintf( buffer, B3_ARRAY_COUNT( buffer ), " %.2f", 100.0f * mp->separation ); + draw->DrawStringFcn( p, buffer, b3_colorWhite, draw->context ); + } + else if ( draw->drawContactForces ) + { + // Hack inv_dt for single step debugging + float inv_dt = world->inv_dt > 0.0f ? world->inv_dt : 60.0f; + // todo validate + // multiply by one-half due to relax iteration + float force = 0.5f * mp->totalNormalImpulse * inv_dt; + b3Pos p1 = p; + b3Pos p2 = b3OffsetPos( p1, b3MulSV( draw->forceScale * force, normal ) ); + draw->DrawSegmentFcn( p1, p2, impulseColor, draw->context ); + snprintf( buffer, B3_ARRAY_COUNT( buffer ), " %.1f", force ); + draw->DrawStringFcn( p1, buffer, b3_colorWhite, draw->context ); + } + else if ( draw->drawContactFeatures ) + { + snprintf( buffer, B3_ARRAY_COUNT( buffer ), " %#X", mp->featureId ); + draw->DrawStringFcn( p, buffer, b3_colorWhite, draw->context ); + } + + if ( draw->drawGraphColors ) + { + // graph color + float pointSize = contact->colorIndex == B3_OVERFLOW_INDEX ? 15.0f : 10.0f; + draw->DrawPointFcn( p, pointSize, graphColors[contact->colorIndex], draw->context ); + // g_draw.DrawString(point->position, "%d", point->color); + } + else if ( mp->persisted == false ) + { + // Add + draw->DrawPointFcn( p, 20.0f, addColor, draw->context ); + } + else if ( mp->separation > speculativeDistance ) + { + // Speculative + draw->DrawPointFcn( p, 10.0f, farColor, draw->context ); + } + else if ( mp->separation > 0.0f ) + { + // Speculative + draw->DrawPointFcn( p, 10.0f, speculativeColor, draw->context ); + } + else + { + // Persist + draw->DrawPointFcn( p, 10.0f, persistColor, draw->context ); + } + } + + if ( draw->drawContactForces ) + { + // Hack inv_dt for single step debugging + float inv_dt = world->inv_dt > 0.0f ? world->inv_dt : 60.0f; + + b3Vec3 avgAnchor = b3MulSV( 1.0f / manifold->pointCount, anchorSum ); + b3Pos p1 = b3OffsetPos( contactCenter, avgAnchor ); + b3Vec3 frictionForce = b3MulSV( 0.5f * inv_dt, manifold->frictionImpulse ); + b3Pos p2 = b3OffsetPos( p1, b3MulSV( draw->forceScale, frictionForce ) ); + draw->DrawSegmentFcn( p1, p2, frictionColor, draw->context ); + draw->DrawPointFcn( p1, 5.0f, frictionColor, draw->context ); + + p1 = b3OffsetPos( p1, b3MulSV( 0.05f * lengthScale, normal ) ); + char buffer[32]; + snprintf( buffer, B3_ARRAY_COUNT( buffer ), "%.2f", b3Length( frictionForce ) ); + draw->DrawStringFcn( p1, buffer, b3_colorWhite, draw->context ); + } + } + + b3SetBit( &world->debugContactSet, contactId ); + } + + contactKey = contact->edges[edgeIndex].nextKey; + } + } + + if ( draw->drawIslands ) + { + int islandId = body->islandId; + if ( islandId != B3_NULL_INDEX && b3GetBit( &world->debugIslandSet, islandId ) == false ) + { + b3Island* island = world->islands.data + islandId; + if ( island->setIndex == B3_NULL_INDEX ) + { + continue; + } + + int shapeCount = 0; + b3AABB aabb = { + .lowerBound = { FLT_MAX, FLT_MAX, FLT_MAX }, + .upperBound = { -FLT_MAX, -FLT_MAX, -FLT_MAX }, + }; + + for ( int b = 0; b < island->bodies.count; ++b ) + { + int islandBodyId = island->bodies.data[b]; + b3Body* islandBody = b3Array_Get( world->bodies, islandBodyId ); + int shapeId = islandBody->headShapeId; + while ( shapeId != B3_NULL_INDEX ) + { + b3Shape* shape = b3Array_Get( world->shapes, shapeId ); + aabb = b3AABB_Union( aabb, shape->fatAABB ); + shapeCount += 1; + shapeId = shape->nextShapeId; + } + } + + if ( shapeCount > 0 ) + { + draw->DrawBoundsFcn( aabb, b3_colorOrangeRed, draw->context ); + } + + b3SetBit( &world->debugIslandSet, islandId ); + } + } + + // Clear the smallest set bit + word = word & ( word - 1 ); + } + } + + char buffer[32] = { 0 }; + float lengthUnits = b3GetLengthUnitsPerMeter(); + b3Vec3 offset = { 0.002f * lengthUnits, 0.002f * lengthUnits, 0.002f * lengthUnits }; + for ( int i = 0; i < world->workerCount; ++i ) + { + int pointCount = world->taskContexts.data[i].pointCount; + b3DebugPoint* points = world->taskContexts.data[i].points; + for ( int j = 0; j < pointCount; ++j ) + { + b3DebugPoint* point = points + j; + b3Pos p = point->p; + draw->DrawPointFcn( p, 5.0f, point->color, draw->context ); + snprintf( buffer, 32, " %d, %.2f", point->label, point->value ); + b3Pos ps = b3OffsetPos( p, offset ); + draw->DrawStringFcn( ps, buffer, point->color, draw->context ); + } + + int lineCount = world->taskContexts.data[i].lineCount; + b3DebugLine* lines = world->taskContexts.data[i].lines; + for ( int j = 0; j < lineCount; ++j ) + { + b3DebugLine* line = lines + j; + b3Pos p1 = line->p1; + b3Pos p2 = line->p2; + draw->DrawSegmentFcn( p1, p2, line->color, draw->context ); + draw->DrawPointFcn( p1, 10.0f, line->color, draw->context ); + snprintf( buffer, 32, "%d", line->label ); + b3Pos ps = b3OffsetPos( p1, offset ); + draw->DrawStringFcn( ps, buffer, line->color, draw->context ); + } + } +} + +b3AABB b3World_GetBounds( b3WorldId worldId ) +{ + b3World* world = b3GetUnlockedWorldFromId( worldId ); + if ( world == NULL ) + { + return (b3AABB){ 0 }; + } + + b3AABB worldBounds = { 0 }; + bool haveBounds = false; + + for ( int i = 0; i < b3_bodyTypeCount; ++i ) + { + b3DynamicTree* tree = world->broadPhase.trees + i; + if ( b3DynamicTree_GetProxyCount( tree ) == 0 ) + { + continue; + } + + b3AABB bounds = b3DynamicTree_GetRootBounds( world->broadPhase.trees + i ); + + if ( haveBounds ) + { + worldBounds = b3AABB_Union( worldBounds, bounds ); + } + else + { + worldBounds = bounds; + haveBounds = true; + } + } + + return worldBounds; +} + +b3BodyEvents b3World_GetBodyEvents( b3WorldId worldId ) +{ + b3World* world = b3GetUnlockedWorldFromId( worldId ); + if ( world == NULL ) + { + return (b3BodyEvents){ 0 }; + } + + int count = world->bodyMoveEvents.count; + b3BodyEvents events = { world->bodyMoveEvents.data, count }; + return events; +} + +b3SensorEvents b3World_GetSensorEvents( b3WorldId worldId ) +{ + b3World* world = b3GetUnlockedWorldFromId( worldId ); + if ( world == NULL ) + { + return (b3SensorEvents){ 0 }; + } + + // Careful to use previous buffer + int endEventArrayIndex = 1 - world->endEventArrayIndex; + + int beginCount = world->sensorBeginEvents.count; + int endCount = world->sensorEndEvents[endEventArrayIndex].count; + + b3SensorEvents events = { + .beginEvents = world->sensorBeginEvents.data, + .endEvents = world->sensorEndEvents[endEventArrayIndex].data, + .beginCount = beginCount, + .endCount = endCount, + }; + return events; +} + +b3ContactEvents b3World_GetContactEvents( b3WorldId worldId ) +{ + b3World* world = b3GetUnlockedWorldFromId( worldId ); + if ( world == NULL ) + { + return (b3ContactEvents){ 0 }; + } + + // Careful to use previous buffer + int endEventArrayIndex = 1 - world->endEventArrayIndex; + + int beginCount = world->contactBeginEvents.count; + int endCount = world->contactEndEvents[endEventArrayIndex].count; + int hitCount = world->contactHitEvents.count; + + b3ContactEvents events = { + .beginEvents = world->contactBeginEvents.data, + .endEvents = world->contactEndEvents[endEventArrayIndex].data, + .hitEvents = world->contactHitEvents.data, + .beginCount = beginCount, + .endCount = endCount, + .hitCount = hitCount, + }; + + return events; +} + +b3JointEvents b3World_GetJointEvents( b3WorldId worldId ) +{ + b3World* world = b3GetUnlockedWorldFromId( worldId ); + if ( world == NULL ) + { + return (b3JointEvents){ 0 }; + } + + int count = world->jointEvents.count; + b3JointEvents events = { world->jointEvents.data, count }; + return events; +} + +bool b3World_IsValid( b3WorldId id ) +{ + if ( id.index1 < 1 || B3_MAX_WORLDS < id.index1 ) + { + return false; + } + + b3World* world = b3_worlds + ( id.index1 - 1 ); + + if ( world->worldId != id.index1 - 1 ) + { + // world is not allocated + return false; + } + + return id.generation == world->generation; +} + +bool b3Body_IsValid( b3BodyId id ) +{ + if ( B3_MAX_WORLDS <= id.world0 ) + { + // invalid world + return false; + } + + b3World* world = b3_worlds + id.world0; + if ( world->worldId != id.world0 ) + { + // world is free + return false; + } + + if ( id.index1 < 1 || world->bodies.count < id.index1 ) + { + // invalid index + return false; + } + + b3Body* body = world->bodies.data + ( id.index1 - 1 ); + if ( body->setIndex == B3_NULL_INDEX ) + { + // this was freed + return false; + } + + B3_ASSERT( body->localIndex != B3_NULL_INDEX ); + + if ( body->generation != id.generation ) + { + // this id is orphaned + return false; + } + + return true; +} + +bool b3Shape_IsValid( b3ShapeId id ) +{ + if ( B3_MAX_WORLDS <= id.world0 ) + { + return false; + } + + b3World* world = b3_worlds + id.world0; + if ( world->worldId != id.world0 ) + { + // world is free + return false; + } + + int shapeId = id.index1 - 1; + if ( shapeId < 0 || world->shapes.count <= shapeId ) + { + return false; + } + + b3Shape* shape = world->shapes.data + shapeId; + if ( shape->id == B3_NULL_INDEX ) + { + // shape is free + return false; + } + + B3_ASSERT( shape->id == shapeId ); + + return id.generation == shape->generation; +} + +bool b3Joint_IsValid( b3JointId id ) +{ + if ( B3_MAX_WORLDS <= id.world0 ) + { + return false; + } + + b3World* world = b3_worlds + id.world0; + if ( world->worldId != id.world0 ) + { + // world is free + return false; + } + + int jointId = id.index1 - 1; + if ( jointId < 0 || world->joints.count <= jointId ) + { + return false; + } + + b3Joint* joint = world->joints.data + jointId; + if ( joint->jointId == B3_NULL_INDEX ) + { + // joint is free + return false; + } + + B3_ASSERT( joint->jointId == jointId ); + + return id.generation == joint->generation; +} + +bool b3Contact_IsValid( b3ContactId id ) +{ + if ( B3_MAX_WORLDS <= id.world0 ) + { + return false; + } + + b3World* world = b3_worlds + id.world0; + if ( world->worldId != id.world0 ) + { + // world is free + return false; + } + + int contactId = id.index1 - 1; + if ( contactId < 0 || world->contacts.count <= contactId ) + { + return false; + } + + b3Contact* contact = world->contacts.data + contactId; + if ( contact->contactId == B3_NULL_INDEX ) + { + // contact is free + return false; + } + + B3_ASSERT( contact->contactId == contactId ); + + return id.generation == contact->generation; +} + +void b3World_EnableSleeping( b3WorldId worldId, bool flag ) +{ + b3World* world = b3GetUnlockedWorldFromId( worldId ); + if ( world == NULL ) + { + return; + } + + if ( flag == world->enableSleep ) + { + return; + } + + B3_REC( world, WorldEnableSleeping, worldId, flag ); + + world->enableSleep = flag; + + if ( flag == false ) + { + int setCount = world->solverSets.count; + for ( int i = b3_firstSleepingSet; i < setCount; ++i ) + { + b3SolverSet* set = b3Array_Get( world->solverSets, i ); + if ( set->bodySims.count > 0 ) + { + b3WakeSolverSet( world, i ); + } + } + } +} + +bool b3World_IsSleepingEnabled( b3WorldId worldId ) +{ + b3World* world = b3GetWorldFromId( worldId ); + return world->enableSleep; +} + +void b3World_EnableWarmStarting( b3WorldId worldId, bool flag ) +{ + b3World* world = b3GetUnlockedWorldFromId( worldId ); + if ( world == NULL ) + { + return; + } + + B3_REC( world, WorldEnableWarmStarting, worldId, flag ); + + world->enableWarmStarting = flag; +} + +bool b3World_IsWarmStartingEnabled( b3WorldId worldId ) +{ + b3World* world = b3GetWorldFromId( worldId ); + return world->enableWarmStarting; +} + +int b3World_GetAwakeBodyCount( b3WorldId worldId ) +{ + b3World* world = b3GetUnlockedWorldFromId( worldId ); + if ( world == NULL ) + { + return 0; + } + b3SolverSet* awakeSet = b3Array_Get( world->solverSets, b3_awakeSet ); + return awakeSet->bodySims.count; +} + +void b3World_EnableContinuous( b3WorldId worldId, bool flag ) +{ + b3World* world = b3GetUnlockedWorldFromId( worldId ); + if ( world == NULL ) + { + return; + } + + B3_REC( world, WorldEnableContinuous, worldId, flag ); + + world->enableContinuous = flag; +} + +bool b3World_IsContinuousEnabled( b3WorldId worldId ) +{ + b3World* world = b3GetWorldFromId( worldId ); + return world->enableContinuous; +} + +void b3World_SetRestitutionThreshold( b3WorldId worldId, float value ) +{ + b3World* world = b3GetUnlockedWorldFromId( worldId ); + if ( world == NULL ) + { + return; + } + + B3_REC( world, WorldSetRestitutionThreshold, worldId, value ); + + world->restitutionThreshold = b3ClampFloat( value, 0.0f, FLT_MAX ); +} + +float b3World_GetRestitutionThreshold( b3WorldId worldId ) +{ + b3World* world = b3GetWorldFromId( worldId ); + return world->restitutionThreshold; +} + +void b3World_SetHitEventThreshold( b3WorldId worldId, float value ) +{ + b3World* world = b3GetUnlockedWorldFromId( worldId ); + if ( world == NULL ) + { + return; + } + + B3_REC( world, WorldSetHitEventThreshold, worldId, value ); + + world->hitEventThreshold = b3ClampFloat( value, 0.0f, FLT_MAX ); +} + +float b3World_GetHitEventThreshold( b3WorldId worldId ) +{ + b3World* world = b3GetWorldFromId( worldId ); + return world->hitEventThreshold; +} + +void b3World_SetContactTuning( b3WorldId worldId, float hertz, float dampingRatio, float contactSpeed ) +{ + b3World* world = b3GetUnlockedWorldFromId( worldId ); + if ( world == NULL ) + { + return; + } + + B3_REC( world, WorldSetContactTuning, worldId, hertz, dampingRatio, contactSpeed ); + + world->contactHertz = b3ClampFloat( hertz, 0.0f, FLT_MAX ); + world->contactDampingRatio = b3ClampFloat( dampingRatio, 0.0f, FLT_MAX ); + world->contactSpeed = b3ClampFloat( contactSpeed, 0.0f, FLT_MAX ); +} + +void b3World_SetContactRecycleDistance( b3WorldId worldId, float recycleDistance ) +{ + b3World* world = b3GetWorldFromId( worldId ); + B3_ASSERT( world->locked == false ); + if ( world->locked ) + { + return; + } + + B3_REC( world, WorldSetContactRecycleDistance, worldId, recycleDistance ); + + world->contactRecycleDistance = b3ClampFloat( recycleDistance, 0.0f, FLT_MAX ); +} + +float b3World_GetContactRecycleDistance( b3WorldId worldId ) +{ + b3World* world = b3GetWorldFromId( worldId ); + return world->contactRecycleDistance; +} + +void b3World_SetMaximumLinearSpeed( b3WorldId worldId, float maximumLinearSpeed ) +{ + B3_ASSERT( b3IsValidFloat( maximumLinearSpeed ) && maximumLinearSpeed > 0.0f ); + + b3World* world = b3GetUnlockedWorldFromId( worldId ); + if ( world == NULL ) + { + return; + } + + B3_REC( world, WorldSetMaximumLinearSpeed, worldId, maximumLinearSpeed ); + + world->maxLinearSpeed = maximumLinearSpeed; +} + +float b3World_GetMaximumLinearSpeed( b3WorldId worldId ) +{ + b3World* world = b3GetWorldFromId( worldId ); + return world->maxLinearSpeed; +} + +b3Profile b3World_GetProfile( b3WorldId worldId ) +{ + b3World* world = b3GetUnlockedWorldFromId( worldId ); + if ( world == NULL ) + { + return (b3Profile){ 0 }; + } + return world->profile; +} + +b3Counters b3World_GetCounters( b3WorldId worldId ) +{ + b3World* world = b3GetUnlockedWorldFromId( worldId ); + if ( world == NULL ) + { + return (b3Counters){ 0 }; + } + + b3Counters s = { 0 }; + s.bodyCount = b3GetIdCount( &world->bodyIdPool ); + s.shapeCount = b3GetIdCount( &world->shapeIdPool ); + s.contactCount = b3GetIdCount( &world->contactIdPool ); + s.jointCount = b3GetIdCount( &world->jointIdPool ); + s.islandCount = b3GetIdCount( &world->islandIdPool ); + + b3DynamicTree* staticTree = world->broadPhase.trees + b3_staticBody; + s.staticTreeHeight = b3DynamicTree_GetHeight( staticTree ); + + b3DynamicTree* dynamicTree = world->broadPhase.trees + b3_dynamicBody; + b3DynamicTree* kinematicTree = world->broadPhase.trees + b3_kinematicBody; + s.treeHeight = b3MaxInt( b3DynamicTree_GetHeight( dynamicTree ), b3DynamicTree_GetHeight( kinematicTree ) ); + + s.satCallCount = world->satCallCount; + s.satCacheHitCount = world->satCacheHitCount; + memcpy( s.manifoldCounts, world->manifoldCounts, B3_CONTACT_MANIFOLD_COUNT_BUCKETS * sizeof( int ) ); + s.stackUsed = world->stack.maxAllocation; + s.byteCount = b3GetByteCount(); + s.taskCount = world->taskCount; + + _Static_assert( B3_GRAPH_COLOR_COUNT == sizeof( s.colorCounts ) / sizeof( s.colorCounts[0] ), "colorCounts size mismatch" ); + + s.awakeContactCount = 0; + for ( int i = 0; i < B3_GRAPH_COLOR_COUNT; ++i ) + { + b3GraphColor* color = world->constraintGraph.colors + i; + int colorContactCount = color->convexContacts.count + color->contacts.count; + s.colorCounts[i] = colorContactCount + color->jointSims.count; + s.awakeContactCount += colorContactCount; + } + s.awakeContactCount += world->solverSets.data[b3_awakeSet].contactIndices.count; + + s.recycledContactCount = 0; + s.arenaCapacity = 0; + s.distanceIterations = 0; + s.pushBackIterations = 0; + s.rootIterations = 0; + for ( int i = 0; i < world->workerCount; ++i ) + { + s.recycledContactCount += world->taskContexts.data[i].recycledContactCount; + + s.distanceIterations = b3MaxInt( s.distanceIterations, world->taskContexts.data[i].distanceIterations ); + s.pushBackIterations = b3MaxInt( s.pushBackIterations, world->taskContexts.data[i].pushBackIterations ); + s.rootIterations = b3MaxInt( s.rootIterations, world->taskContexts.data[i].rootIterations ); + + int peak = world->taskContexts.data[i].arena.shared->peakDemand; + if ( peak > s.arenaCapacity ) + { + s.arenaCapacity = peak; + } + } + + return s; +} + +b3Capacity b3World_GetMaxCapacity( b3WorldId worldId ) +{ + b3World* world = b3GetUnlockedWorldFromId( worldId ); + if ( world == NULL ) + { + return (b3Capacity){ 0 }; + } + return world->maxCapacity; +} + +void b3World_SetUserData( b3WorldId worldId, void* userData ) +{ + b3World* world = b3GetWorldFromId( worldId ); + world->userData = userData; +} + +void* b3World_GetUserData( b3WorldId worldId ) +{ + b3World* world = b3GetWorldFromId( worldId ); + return world->userData; +} + +void b3World_SetFrictionCallback( b3WorldId worldId, b3FrictionCallback* callback ) +{ + b3World* world = b3GetUnlockedWorldFromId( worldId ); + if ( world == NULL ) + { + return; + } + + world->frictionCallback = callback != NULL ? callback : b3DefaultFrictionCallback; +} + +void b3World_SetRestitutionCallback( b3WorldId worldId, b3RestitutionCallback* callback ) +{ + b3World* world = b3GetUnlockedWorldFromId( worldId ); + if ( world == NULL ) + { + return; + } + + world->restitutionCallback = callback != NULL ? callback : b3DefaultRestitutionCallback; +} + +void b3World_SetWorkerCount( b3WorldId worldId, int count ) +{ + b3World* world = b3GetUnlockedWorldFromId( worldId ); + if ( world == NULL ) + { + return; + } + + if ( count == world->workerCount ) + { + return; + } + + b3DestroyWorkerContexts( world ); + world->workerCount = b3ClampInt( count, 1, B3_MAX_WORKERS ); + b3CreateWorkerContexts( world ); +} + +int b3World_GetWorkerCount( b3WorldId worldId ) +{ + b3World* world = b3GetUnlockedWorldFromId( worldId ); + if ( world == NULL ) + { + return 0; + } + + return world->workerCount; +} + +void b3World_StartRecording( b3WorldId worldId, b3Recording* recording ) +{ + // Must be a step boundary, so refuse a locked world + b3World* world = b3GetUnlockedWorldFromId( worldId ); + + if ( world == NULL || recording == NULL || world->recording != NULL ) + { + return; + } + + b3StartRecordingIntoBuffer( world, recording ); +} + +void b3World_StopRecording( b3WorldId worldId ) +{ + b3World* world = b3GetUnlockedWorldFromId( worldId ); + if ( world == NULL ) + { + return; + } + + b3StopRecordingInternal( world ); +} + +void b3World_DumpMemoryStats( b3WorldId worldId ) +{ + b3World* world = b3GetUnlockedWorldFromId( worldId ); + if ( world == NULL ) + { + return; + } + + // Large worlds can exceed 2GB, sum in 64 bits + uint64_t total = 0; + + // id pools + int bodyIdBytes = b3GetIdBytes( &world->bodyIdPool ); + int solverSetIdBytes = b3GetIdBytes( &world->solverSetIdPool ); + int jointIdBytes = b3GetIdBytes( &world->jointIdPool ); + int contactIdBytes = b3GetIdBytes( &world->contactIdPool ); + int islandIdBytes = b3GetIdBytes( &world->islandIdPool ); + int shapeIdBytes = b3GetIdBytes( &world->shapeIdPool ); + total += (uint64_t)bodyIdBytes + solverSetIdBytes + jointIdBytes + contactIdBytes + islandIdBytes + shapeIdBytes; + + b3Log( "id pools" ); + b3Log( "body ids: %d", bodyIdBytes ); + b3Log( "solver set ids: %d", solverSetIdBytes ); + b3Log( "joint ids: %d", jointIdBytes ); + b3Log( "contact ids: %d", contactIdBytes ); + b3Log( "island ids: %d", islandIdBytes ); + b3Log( "shape ids: %d", shapeIdBytes ); + + // Islands own per-island body/contact/joint link arrays + int islandLinkBytes = 0; + for ( int i = 0; i < world->islands.count; ++i ) + { + b3Island* island = world->islands.data + i; + islandLinkBytes += b3Array_ByteCount( island->bodies ); + islandLinkBytes += b3Array_ByteCount( island->contacts ); + islandLinkBytes += b3Array_ByteCount( island->joints ); + } + + // world arrays + int bodyArrayBytes = b3Array_ByteCount( world->bodies ); + int solverSetArrayBytes = b3Array_ByteCount( world->solverSets ); + int jointArrayBytes = b3Array_ByteCount( world->joints ); + int contactArrayBytes = b3Array_ByteCount( world->contacts ); + int islandArrayBytes = b3Array_ByteCount( world->islands ); + int shapeArrayBytes = b3Array_ByteCount( world->shapes ); + int sensorArrayBytes = b3Array_ByteCount( world->sensors ); + total += (uint64_t)bodyArrayBytes + solverSetArrayBytes + jointArrayBytes + contactArrayBytes + islandArrayBytes + + islandLinkBytes + shapeArrayBytes + sensorArrayBytes; + + b3Log( "world arrays" ); + b3Log( "bodies: %d", bodyArrayBytes ); + b3Log( "solver sets: %d", solverSetArrayBytes ); + b3Log( "joints: %d", jointArrayBytes ); + b3Log( "contacts: %d", contactArrayBytes ); + b3Log( "islands: %d", islandArrayBytes ); + b3Log( "island links: %d", islandLinkBytes ); + b3Log( "shapes: %d", shapeArrayBytes ); + b3Log( "sensors: %d", sensorArrayBytes ); + + // Sensors own overlap tracking arrays. The sensor array is dense. + int sensorOverlapBytes = 0; + for ( int i = 0; i < world->sensors.count; ++i ) + { + b3Sensor* sensor = world->sensors.data + i; + sensorOverlapBytes += b3Array_ByteCount( sensor->hits ); + sensorOverlapBytes += b3Array_ByteCount( sensor->overlaps1 ); + sensorOverlapBytes += b3Array_ByteCount( sensor->overlaps2 ); + } + total += sensorOverlapBytes; + + b3Log( "owned arrays" ); + b3Log( "sensor overlaps: %d", sensorOverlapBytes ); + + // Shared hull database. The map owns a combined bucket and metadata allocation + // plus the small map struct. Each stored key is an owned clone sized by byteCount. + b3HullMap* hullDatabase = world->hullDatabase; + int hullCount = (int)b3HullMap_size( hullDatabase ); + int hullBucketCount = (int)b3HullMap_bucket_count( hullDatabase ); + uint64_t hullMapBytes = b3HullMapByteCount( hullDatabase ); + uint64_t hullDataBytes = 0; + for ( b3HullMap_itr itr = b3HullMap_first( hullDatabase ); b3HullMap_is_end( itr ) == false; itr = b3HullMap_next( itr ) ) + { + hullDataBytes += itr.data->key->byteCount; + } + total += hullMapBytes + hullDataBytes; + + b3Log( "hulls" ); + b3Log( "database: %d (%d, %d)", (int)hullMapBytes, hullCount, hullBucketCount ); + b3Log( "hull data: %d", (int)hullDataBytes ); + + // broad-phase + int staticTreeBytes = b3DynamicTree_GetByteCount( world->broadPhase.trees + b3_staticBody ); + int kinematicTreeBytes = b3DynamicTree_GetByteCount( world->broadPhase.trees + b3_kinematicBody ); + int dynamicTreeBytes = b3DynamicTree_GetByteCount( world->broadPhase.trees + b3_dynamicBody ); + int movedBytes = 0; + for ( int i = 0; i < b3_bodyTypeCount; ++i ) + { + movedBytes += b3GetBitSetBytes( &world->broadPhase.movedProxies[i] ); + } + int moveArrayBytes = b3Array_ByteCount( world->broadPhase.moveArray ); + b3HashSet* pairSet = &world->broadPhase.pairSet; + int pairSetBytes = b3GetHashSetBytes( pairSet ); + total += (uint64_t)staticTreeBytes + kinematicTreeBytes + dynamicTreeBytes + movedBytes + moveArrayBytes + pairSetBytes; + + b3Log( "broad-phase" ); + b3Log( "static tree: %d", staticTreeBytes ); + b3Log( "kinematic tree: %d", kinematicTreeBytes ); + b3Log( "dynamic tree: %d", dynamicTreeBytes ); + b3Log( "movedProxies: %d", movedBytes ); + b3Log( "moveArray: %d", moveArrayBytes ); + b3Log( "pairSet: %d (%d, %d)", pairSetBytes, pairSet->count, pairSet->capacity ); + + // Manifold block allocators, one per manifold point count + int manifoldArrayBytes = b3Array_ByteCount( world->manifoldAllocators ); + int manifoldBlockBytes = 0; + for ( int i = 0; i < world->manifoldAllocators.count; ++i ) + { + b3BlockAllocator* allocator = world->manifoldAllocators.data + i; + manifoldBlockBytes += b3Array_ByteCount( allocator->blocks ); + manifoldBlockBytes += allocator->blocks.count * B3_BLOCK_SIZE * allocator->elementSize; + } + total += (uint64_t)manifoldArrayBytes + manifoldBlockBytes; + + b3Log( "manifold allocators" ); + b3Log( "allocator array: %d", manifoldArrayBytes ); + b3Log( "blocks: %d", manifoldBlockBytes ); + + // solver sets + int bodySimCapacity = 0; + int bodyStateCapacity = 0; + int jointSimCapacity = 0; + int contactIndexCapacity = 0; + int islandSimCapacity = 0; + int solverSetCapacity = world->solverSets.count; + for ( int i = 0; i < solverSetCapacity; ++i ) + { + b3SolverSet* set = world->solverSets.data + i; + if ( set->setIndex == B3_NULL_INDEX ) + { + continue; + } + + bodySimCapacity += set->bodySims.capacity; + bodyStateCapacity += set->bodyStates.capacity; + jointSimCapacity += set->jointSims.capacity; + contactIndexCapacity += set->contactIndices.capacity; + islandSimCapacity += set->islandSims.capacity; + } + + int setBodySimBytes = bodySimCapacity * (int)sizeof( b3BodySim ); + int setBodyStateBytes = bodyStateCapacity * (int)sizeof( b3BodyState ); + int setJointSimBytes = jointSimCapacity * (int)sizeof( b3JointSim ); + int setContactSimBytes = contactIndexCapacity * (int)sizeof( int ); + int setIslandSimBytes = islandSimCapacity * (int)sizeof( b3IslandSim ); + total += (uint64_t)setBodySimBytes + setBodyStateBytes + setJointSimBytes + setContactSimBytes + setIslandSimBytes; + + b3Log( "solver sets" ); + b3Log( "body sim: %d", setBodySimBytes ); + b3Log( "body state: %d", setBodyStateBytes ); + b3Log( "joint sim: %d", setJointSimBytes ); + b3Log( "contact sim: %d", setContactSimBytes ); + b3Log( "island sim: %d", setIslandSimBytes ); + + // constraint graph + int bodyBitSetBytes = 0; + int graphContactBytes = 0; + int graphJointSimBytes = 0; + for ( int i = 0; i < B3_GRAPH_COLOR_COUNT; ++i ) + { + b3GraphColor* c = world->constraintGraph.colors + i; + bodyBitSetBytes += b3GetBitSetBytes( &c->bodySet ); + graphContactBytes += b3Array_ByteCount( c->convexContacts ) + b3Array_ByteCount( c->contacts ); + graphJointSimBytes += b3Array_ByteCount( c->jointSims ); + } + total += (uint64_t)bodyBitSetBytes + graphJointSimBytes + graphContactBytes; + + b3Log( "constraint graph" ); + b3Log( "body bit sets: %d", bodyBitSetBytes ); + b3Log( "joint sim: %d", graphJointSimBytes ); + b3Log( "contact sim: %d", graphContactBytes ); + + // Per worker task storage and its bit sets + int taskContextBytes = b3Array_ByteCount( world->taskContexts ); + for ( int i = 0; i < world->taskContexts.count; ++i ) + { + b3TaskContext* taskContext = world->taskContexts.data + i; + taskContextBytes += b3Array_ByteCount( taskContext->sensorHits ); + taskContextBytes += b3GetBitSetBytes( &taskContext->contactStateBitSet ); + taskContextBytes += b3GetBitSetBytes( &taskContext->jointStateBitSet ); + taskContextBytes += b3GetBitSetBytes( &taskContext->hitEventBitSet ); + taskContextBytes += b3GetBitSetBytes( &taskContext->enlargedSimBitSet ); + taskContextBytes += b3GetBitSetBytes( &taskContext->awakeIslandBitSet ); + } + + int sensorTaskContextBytes = b3Array_ByteCount( world->sensorTaskContexts ); + for ( int i = 0; i < world->sensorTaskContexts.count; ++i ) + { + b3SensorTaskContext* taskContext = world->sensorTaskContexts.data + i; + sensorTaskContextBytes += b3GetBitSetBytes( &taskContext->eventBits ); + } + total += (uint64_t)taskContextBytes + sensorTaskContextBytes; + + b3Log( "task contexts" ); + b3Log( "worker: %d", taskContextBytes ); + b3Log( "sensor: %d", sensorTaskContextBytes ); + + // Double buffered event arrays + int eventBytes = 0; + eventBytes += b3Array_ByteCount( world->bodyMoveEvents ); + eventBytes += b3Array_ByteCount( world->sensorBeginEvents ); + eventBytes += b3Array_ByteCount( world->contactBeginEvents ); + eventBytes += b3Array_ByteCount( world->sensorEndEvents[0] ); + eventBytes += b3Array_ByteCount( world->sensorEndEvents[1] ); + eventBytes += b3Array_ByteCount( world->contactEndEvents[0] ); + eventBytes += b3Array_ByteCount( world->contactEndEvents[1] ); + eventBytes += b3Array_ByteCount( world->contactHitEvents ); + eventBytes += b3Array_ByteCount( world->jointEvents ); + total += eventBytes; + + b3Log( "events: %d", eventBytes ); + + // Debug draw bit sets + int debugBytes = 0; + debugBytes += b3GetBitSetBytes( &world->debugBodySet ); + debugBytes += b3GetBitSetBytes( &world->debugJointSet ); + debugBytes += b3GetBitSetBytes( &world->debugContactSet ); + debugBytes += b3GetBitSetBytes( &world->debugIslandSet ); + total += debugBytes; + + b3Log( "debug draw: %d", debugBytes ); + + // stack allocator + total += world->stack.capacity; + b3Log( "stack allocator: %d", world->stack.capacity ); + + b3Log( "total: %u KB", (uint32_t)( total / 1024 ) ); +} + +typedef struct WorldQueryContext +{ + b3World* world; + b3OverlapResultFcn* fcn; + b3QueryFilter filter; + void* userContext; +} WorldQueryContext; + +static bool TreeQueryCallback( int proxyId, uint64_t userData, void* context ) +{ + B3_UNUSED( proxyId ); + + int shapeId = (int)userData; + WorldQueryContext* worldContext = (WorldQueryContext*)context; + b3World* world = worldContext->world; + + b3Shape* shape = b3Array_Get( world->shapes, shapeId ); + + b3Filter shapeFilter = shape->filter; + b3QueryFilter queryFilter = worldContext->filter; + + if ( b3ShouldQueryCollide( &shapeFilter, &queryFilter ) == false ) + { + return true; + } + + b3ShapeId id = { shapeId + 1, world->worldId, shape->generation }; + bool result = worldContext->fcn( id, worldContext->userContext ); + return result; +} + +b3TreeStats b3World_OverlapAABB( b3WorldId worldId, b3AABB aabb, b3QueryFilter filter, b3OverlapResultFcn* fcn, void* context ) +{ + b3TreeStats treeStats = { 0 }; + + b3World* world = b3GetUnlockedWorldFromId( worldId ); + if ( world == NULL ) + { + return treeStats; + } + + B3_ASSERT( b3IsValidAABB( aabb ) ); + + b3RecQueryWriter recWriter = { 0 }; + if ( world->recording != NULL ) + { + b3RecQueryBegin( &recWriter, context, filter.id, filter.name ); + recWriter.userFcn.overlapFcn = fcn; + b3RecW_WORLDID( &recWriter.buf, worldId ); + b3RecW_AABB( &recWriter.buf, aabb ); + b3RecW_QUERYFILTER( &recWriter.buf, filter ); + recWriter.countOffset = b3RecReserveU32( &recWriter.buf ); + fcn = b3RecOverlapTrampoline; + context = &recWriter; + } + + WorldQueryContext worldContext = { world, fcn, filter, context }; + + for ( int i = 0; i < b3_bodyTypeCount; ++i ) + { + b3TreeStats treeResult = + b3DynamicTree_Query( world->broadPhase.trees + i, aabb, filter.maskBits, false, TreeQueryCallback, &worldContext ); + + treeStats.nodeVisits += treeResult.nodeVisits; + treeStats.leafVisits += treeResult.leafVisits; + } + + if ( world->recording != NULL ) + { + b3RecPatchU32( &recWriter.buf, recWriter.countOffset, recWriter.hitCount ); + b3RecW_TREESTATS( &recWriter.buf, treeStats ); + b3RecQueryCommit( world->recording, b3_recOpQueryOverlapAABB, &recWriter ); + } + + return treeStats; +} + +typedef struct WorldOverlapContext +{ + b3World* world; + b3OverlapResultFcn* fcn; + b3QueryFilter filter; + b3ShapeProxy proxy; + b3Pos origin; + void* userContext; +} WorldOverlapContext; + +static bool b3TreeOverlapCallback( int proxyId, uint64_t userData, void* context ) +{ + B3_UNUSED( proxyId ); + + int shapeId = (int)userData; + WorldOverlapContext* worldContext = (WorldOverlapContext*)context; + b3World* world = worldContext->world; + + b3Shape* shape = b3Array_Get( world->shapes, shapeId ); + + b3Filter shapeFilter = shape->filter; + b3QueryFilter queryFilter = worldContext->filter; + + if ( b3ShouldQueryCollide( &shapeFilter, &queryFilter ) == false ) + { + return true; + } + + // Re-center on the query origin so the overlap test stays in float precision far from the origin + b3Body* body = b3Array_Get( world->bodies, shape->bodyId ); + b3Transform transform = b3ToRelativeTransform( b3GetBodyTransformQuick( world, body ), worldContext->origin ); + + bool overlapping = b3OverlapShape( shape, transform, &worldContext->proxy ); + if ( overlapping == false ) + { + return true; + } + + b3ShapeId id = { shape->id + 1, world->worldId, shape->generation }; + bool result = worldContext->fcn( id, worldContext->userContext ); + return result; +} + +b3TreeStats b3World_OverlapShape( b3WorldId worldId, b3Pos origin, const b3ShapeProxy* proxy, b3QueryFilter filter, + b3OverlapResultFcn* fcn, void* context ) +{ + b3TreeStats treeStats = { 0 }; + + b3World* world = b3GetUnlockedWorldFromId( worldId ); + if ( world == NULL ) + { + return treeStats; + } + + B3_ASSERT( b3IsValidPosition( origin ) ); + + b3RecQueryWriter recWriter = { 0 }; + if ( world->recording != NULL ) + { + b3RecQueryBegin( &recWriter, context, filter.id, filter.name ); + recWriter.userFcn.overlapFcn = fcn; + b3RecW_WORLDID( &recWriter.buf, worldId ); + b3RecW_POSITION( &recWriter.buf, origin ); + b3RecW_SHAPEPROXY( &recWriter.buf, *proxy ); + b3RecW_QUERYFILTER( &recWriter.buf, filter ); + recWriter.countOffset = b3RecReserveU32( &recWriter.buf ); + fcn = b3RecOverlapTrampoline; + context = &recWriter; + } + + // Bound the proxy in origin relative space then lift to a conservative world float box + b3AABB aabb = b3OffsetAABB( b3MakeAABB( proxy->points, proxy->count, proxy->radius ), origin ); + WorldOverlapContext worldContext = { + world, fcn, filter, *proxy, origin, context, + }; + + for ( int i = 0; i < b3_bodyTypeCount; ++i ) + { + b3TreeStats treeResult = b3DynamicTree_Query( world->broadPhase.trees + i, aabb, filter.maskBits, false, + b3TreeOverlapCallback, &worldContext ); + + treeStats.nodeVisits += treeResult.nodeVisits; + treeStats.leafVisits += treeResult.leafVisits; + } + + if ( world->recording != NULL ) + { + b3RecPatchU32( &recWriter.buf, recWriter.countOffset, recWriter.hitCount ); + b3RecW_TREESTATS( &recWriter.buf, treeStats ); + b3RecQueryCommit( world->recording, b3_recOpQueryOverlapShape, &recWriter ); + } + + return treeStats; +} + +typedef struct WorldMoverContext +{ + b3World* world; + b3PlaneResultFcn* fcn; + b3QueryFilter filter; + b3Capsule mover; + b3Pos origin; + void* userContext; +} WorldMoverContext; + +static bool TreeCollideCallback( int proxyId, uint64_t userData, void* context ) +{ + B3_UNUSED( proxyId ); + + int shapeId = (int)userData; + WorldMoverContext* worldContext = (WorldMoverContext*)context; + b3World* world = worldContext->world; + + b3Shape* shape = b3Array_Get( world->shapes, shapeId ); + + b3Filter shapeFilter = shape->filter; + b3QueryFilter queryFilter = worldContext->filter; + + if ( b3ShouldQueryCollide( &shapeFilter, &queryFilter ) == false ) + { + return true; + } + + // Re-center on the query origin, the mover and the resulting planes are origin relative + b3Body* body = b3Array_Get( world->bodies, shape->bodyId ); + b3WorldTransform bodyTransform = b3GetBodyTransformQuick( world, body ); + b3Transform transform = b3ToRelativeTransform( bodyTransform, worldContext->origin ); + + b3PlaneResult buffer[64]; + int count = b3CollideMover( buffer, 64, shape, transform, &worldContext->mover ); + + if ( count > 0 ) + { + b3ShapeId id = { shape->id + 1, world->worldId, shape->generation }; + return worldContext->fcn( id, buffer, count, worldContext->userContext ); + } + + return true; +} + +// It is tempting to use a shape proxy for the mover, but this makes handling deep overlap difficult and the generality may +// not be worth it. +void b3World_CollideMover( b3WorldId worldId, b3Pos origin, const b3Capsule* mover, b3QueryFilter filter, b3PlaneResultFcn* fcn, + void* context ) +{ + b3World* world = b3GetUnlockedWorldFromId( worldId ); + if ( world == NULL ) + { + return; + } + + B3_ASSERT( b3IsValidPosition( origin ) ); + + b3RecQueryWriter recWriter = { 0 }; + if ( world->recording != NULL ) + { + b3RecQueryBegin( &recWriter, context, filter.id, filter.name ); + recWriter.userFcn.planeFcn = fcn; + b3RecW_WORLDID( &recWriter.buf, worldId ); + b3RecW_POSITION( &recWriter.buf, origin ); + b3RecW_CAPSULE( &recWriter.buf, *mover ); + b3RecW_QUERYFILTER( &recWriter.buf, filter ); + recWriter.countOffset = b3RecReserveU32( &recWriter.buf ); + fcn = b3RecPlaneTrampoline; + context = &recWriter; + } + + b3Vec3 r = { mover->radius, mover->radius, mover->radius }; + + // Relative box lifted to world float with outward rounding, conservative for the tree + b3AABB relBox; + relBox.lowerBound = b3Sub( b3Min( mover->center1, mover->center2 ), r ); + relBox.upperBound = b3Add( b3Max( mover->center1, mover->center2 ), r ); + b3AABB aabb = b3OffsetAABB( relBox, origin ); + + WorldMoverContext worldContext = { + world, fcn, filter, *mover, origin, context, + }; + + for ( int i = 0; i < b3_bodyTypeCount; ++i ) + { + b3DynamicTree_Query( world->broadPhase.trees + i, aabb, filter.maskBits, false, TreeCollideCallback, &worldContext ); + } + + if ( world->recording != NULL ) + { + // CollideMover returns void: no treestats tail, just the per-shape plane batches. + b3RecPatchU32( &recWriter.buf, recWriter.countOffset, recWriter.hitCount ); + b3RecQueryCommit( world->recording, b3_recOpQueryCollideMover, &recWriter ); + } +} + +typedef struct WorldRayCastContext +{ + b3World* world; + b3CastResultFcn* fcn; + b3QueryFilter filter; + float fraction; + b3Pos origin; + void* userContext; +} WorldRayCastContext; + +static float RayCastCallback( const b3RayCastInput* input, int proxyId, uint64_t userData, void* context ) +{ + B3_UNUSED( proxyId ); + + int shapeId = (int)userData; + WorldRayCastContext* worldContext = (WorldRayCastContext*)context; + b3World* world = worldContext->world; + + b3Shape* shape = b3Array_Get( world->shapes, shapeId ); + b3Filter shapeFilter = shape->filter; + b3QueryFilter queryFilter = worldContext->filter; + + if ( b3ShouldQueryCollide( &shapeFilter, &queryFilter ) == false ) + { + return input->maxFraction; + } + + b3Body* body = b3Array_Get( world->bodies, shape->bodyId ); + b3WorldTransform bodyTransform = b3GetBodyTransformQuick( world, body ); + b3Transform transform = b3ToRelativeTransform( bodyTransform, worldContext->origin ); + + b3RayCastInput localInput = *input; + localInput.origin = b3Vec3_zero; + b3CastOutput output = b3RayCastShape( shape, transform, &localInput ); + + if ( output.hit ) + { + B3_ASSERT( output.fraction <= input->maxFraction ); + + b3ShapeId id = { shapeId + 1, world->worldId, shape->generation }; + b3Pos point = b3OffsetPos( worldContext->origin, output.point ); + int materialIndex = b3ClampInt( output.materialIndex, 0, shape->materialCount - 1 ); + uint64_t userMaterialId = b3GetShapeMaterials( shape )[materialIndex].userMaterialId; + + int triangleIndex = output.triangleIndex; + int childIndex = output.childIndex; + float fraction = worldContext->fcn( id, point, output.normal, output.fraction, userMaterialId, triangleIndex, childIndex, + worldContext->userContext ); + + // The user may return -1 to skip this shape + if ( 0.0f <= fraction && fraction <= 1.0f ) + { + worldContext->fraction = fraction; + } + + return fraction; + } + + return input->maxFraction; +} + +b3TreeStats b3World_CastRay( b3WorldId worldId, b3Pos origin, b3Vec3 translation, b3QueryFilter filter, b3CastResultFcn* fcn, + void* context ) +{ + b3TreeStats treeStats = { 0 }; + + b3World* world = b3GetUnlockedWorldFromId( worldId ); + if ( world == NULL ) + { + return treeStats; + } + + B3_ASSERT( b3IsValidPosition( origin ) ); + B3_ASSERT( b3IsValidVec3( translation ) ); + + b3RecQueryWriter recWriter = { 0 }; + if ( world->recording != NULL ) + { + b3RecQueryBegin( &recWriter, context, filter.id, filter.name ); + recWriter.userFcn.castFcn = fcn; + b3RecW_WORLDID( &recWriter.buf, worldId ); + b3RecW_POSITION( &recWriter.buf, origin ); + b3RecW_VEC3( &recWriter.buf, translation ); + b3RecW_QUERYFILTER( &recWriter.buf, filter ); + recWriter.countOffset = b3RecReserveU32( &recWriter.buf ); + fcn = b3RecCastTrampoline; + context = &recWriter; + } + + // The tree traverses in float relative to the world origin. Each shape is then re-differenced at + // full precision against the origin, so a hit stays accurate far from the origin. + b3RayCastInput input = { b3ToVec3( origin ), translation, 1.0f }; + + WorldRayCastContext worldContext = { world, fcn, filter, 1.0f, origin, context }; + + for ( int i = 0; i < b3_bodyTypeCount; ++i ) + { + b3TreeStats treeResult = + b3DynamicTree_RayCast( world->broadPhase.trees + i, &input, filter.maskBits, false, RayCastCallback, &worldContext ); + treeStats.nodeVisits += treeResult.nodeVisits; + treeStats.leafVisits += treeResult.leafVisits; + + if ( worldContext.fraction == 0.0f ) + { + break; + } + + input.maxFraction = worldContext.fraction; + } + + if ( world->recording != NULL ) + { + b3RecPatchU32( &recWriter.buf, recWriter.countOffset, recWriter.hitCount ); + b3RecW_TREESTATS( &recWriter.buf, treeStats ); + b3RecQueryCommit( world->recording, b3_recOpQueryCastRay, &recWriter ); + } + + return treeStats; +} + +// This callback finds the closest hit. This is the most common callback used in games. +static float b3RayCastClosestFcn( b3ShapeId shapeId, b3Pos point, b3Vec3 normal, float fraction, uint64_t userMaterialId, + int triangleIndex, int childIndex, void* context ) +{ + // Ignore initial overlap + if ( fraction == 0.0f ) + { + return -1.0f; + } + + b3RayResult* rayResult = (b3RayResult*)context; + rayResult->shapeId = shapeId; + rayResult->point = point; + rayResult->normal = normal; + rayResult->fraction = fraction; + rayResult->userMaterialId = userMaterialId; + rayResult->triangleIndex = triangleIndex; + rayResult->childIndex = childIndex; + rayResult->hit = true; + return fraction; +} + +b3RayResult b3World_CastRayClosest( b3WorldId worldId, b3Pos origin, b3Vec3 translation, b3QueryFilter filter ) +{ + b3RayResult result = { 0 }; + + b3World* world = b3GetUnlockedWorldFromId( worldId ); + if ( world == NULL ) + { + return result; + } + + B3_ASSERT( b3IsValidPosition( origin ) ); + B3_ASSERT( b3IsValidVec3( translation ) ); + + // The tree traverses in float relative to the world origin. Each shape is then re-differenced at + // full precision against its body, so a hit stays accurate far from the origin. + b3RayCastInput input = { b3ToVec3( origin ), translation, 1.0f }; + WorldRayCastContext worldContext = { + .world = world, + .fcn = b3RayCastClosestFcn, + .filter = filter, + .fraction = 1.0f, + .origin = origin, + .userContext = &result, + }; + + for ( int i = 0; i < b3_bodyTypeCount; ++i ) + { + b3TreeStats treeResult = + b3DynamicTree_RayCast( world->broadPhase.trees + i, &input, filter.maskBits, false, RayCastCallback, &worldContext ); + result.nodeVisits += treeResult.nodeVisits; + result.leafVisits += treeResult.leafVisits; + + if ( worldContext.fraction == 0.0f ) + { + break; + } + + input.maxFraction = worldContext.fraction; + } + + // Closed query, no user callback: record the inputs and the single result for the replay compare. + if ( world->recording != NULL ) + { + b3RecQueryWriter recWriter = { 0 }; + b3RecQueryBegin( &recWriter, NULL, filter.id, filter.name ); + b3RecW_WORLDID( &recWriter.buf, worldId ); + b3RecW_POSITION( &recWriter.buf, origin ); + b3RecW_VEC3( &recWriter.buf, translation ); + b3RecW_QUERYFILTER( &recWriter.buf, filter ); + b3RecW_RAYRESULT( &recWriter.buf, result ); + b3RecQueryCommit( world->recording, b3_recOpQueryCastRayClosest, &recWriter ); + } + + return result; +} + +typedef struct WorldShapeCastContext +{ + b3World* world; + b3CastResultFcn* fcn; + b3QueryFilter filter; + float fraction; + b3Pos origin; + // origin relative input + b3ShapeCastInput input; + void* userContext; +} WorldShapeCastContext; + +static float b3ShapeCastCallback( const b3BoxCastInput* input, int proxyId, uint64_t userData, void* context ) +{ + B3_UNUSED( proxyId ); + + int shapeId = (int)userData; + WorldShapeCastContext* worldContext = (WorldShapeCastContext*)context; + b3World* world = worldContext->world; + + b3Shape* shape = b3Array_Get( world->shapes, shapeId ); + b3Filter shapeFilter = shape->filter; + b3QueryFilter queryFilter = worldContext->filter; + + if ( b3ShouldQueryCollide( &shapeFilter, &queryFilter ) == false ) + { + return input->maxFraction; + } + + // Rebuild from the origin relative input, taking only the advancing fraction from the tree. + // The tree box is world float and would lose the cast far from the origin. + b3ShapeCastInput localInput = worldContext->input; + localInput.maxFraction = input->maxFraction; + + // Re-center on the query origin so the per-shape cast stays in float precision far from the origin + b3Body* body = b3Array_Get( world->bodies, shape->bodyId ); + b3Transform transform = b3ToRelativeTransform( b3GetBodyTransformQuick( world, body ), worldContext->origin ); + + b3CastOutput output = b3ShapeCastShape( shape, transform, &localInput ); + + if ( output.hit ) + { + b3ShapeId id = { shapeId + 1, world->worldId, shape->generation }; + int materialIndex = b3ClampInt( output.materialIndex, 0, shape->materialCount - 1 ); + uint64_t userMaterialId = b3GetShapeMaterials( shape )[materialIndex].userMaterialId; + + int triangleIndex = output.triangleIndex; + int childIndex = output.childIndex; + float fraction = worldContext->fcn( id, b3OffsetPos( worldContext->origin, output.point ), output.normal, output.fraction, + userMaterialId, triangleIndex, childIndex, worldContext->userContext ); + + // The user may return -1 to skip this shape + if ( 0.0f <= fraction && fraction <= 1.0f ) + { + worldContext->fraction = fraction; + } + + return fraction; + } + + return input->maxFraction; +} + +b3TreeStats b3World_CastShape( b3WorldId worldId, b3Pos origin, const b3ShapeProxy* proxy, b3Vec3 translation, + b3QueryFilter filter, b3CastResultFcn* fcn, void* context ) +{ + b3TreeStats treeStats = { 0 }; + + b3World* world = b3GetUnlockedWorldFromId( worldId ); + if ( world == NULL ) + { + return treeStats; + } + + B3_ASSERT( b3IsValidPosition( origin ) ); + B3_ASSERT( b3IsValidVec3( translation ) ); + + b3RecQueryWriter recWriter = { 0 }; + if ( world->recording != NULL ) + { + b3RecQueryBegin( &recWriter, context, filter.id, filter.name ); + recWriter.userFcn.castFcn = fcn; + b3RecW_WORLDID( &recWriter.buf, worldId ); + b3RecW_POSITION( &recWriter.buf, origin ); + b3RecW_SHAPEPROXY( &recWriter.buf, *proxy ); + b3RecW_VEC3( &recWriter.buf, translation ); + b3RecW_QUERYFILTER( &recWriter.buf, filter ); + recWriter.countOffset = b3RecReserveU32( &recWriter.buf ); + fcn = b3RecCastTrampoline; + context = &recWriter; + } + + WorldShapeCastContext worldContext = { 0 }; + worldContext.world = world; + worldContext.fcn = fcn; + worldContext.filter = filter; + worldContext.fraction = 1.0f; + worldContext.origin = origin; + worldContext.input.proxy = *proxy; + worldContext.input.translation = translation; + worldContext.input.maxFraction = 1.0f; + worldContext.input.canEncroach = false; + worldContext.userContext = context; + + // Bound the proxy in origin relative space then lift to a conservative world float box. The tree + // node boxes use the same directed rounding, so the swept box never clips a shape far from the + // origin. Per shape casts re-difference at full precision against the carried origin. + b3AABB localBox = b3MakeAABB( proxy->points, proxy->count, proxy->radius ); + b3BoxCastInput treeInput = { b3OffsetAABB( localBox, origin ), translation, 1.0f }; + + for ( int i = 0; i < b3_bodyTypeCount; ++i ) + { + b3TreeStats treeResult = b3DynamicTree_BoxCast( world->broadPhase.trees + i, &treeInput, filter.maskBits, false, + b3ShapeCastCallback, &worldContext ); + treeStats.nodeVisits += treeResult.nodeVisits; + treeStats.leafVisits += treeResult.leafVisits; + + if ( worldContext.fraction == 0.0f ) + { + break; + } + + treeInput.maxFraction = worldContext.fraction; + } + + if ( world->recording != NULL ) + { + b3RecPatchU32( &recWriter.buf, recWriter.countOffset, recWriter.hitCount ); + b3RecW_TREESTATS( &recWriter.buf, treeStats ); + b3RecQueryCommit( world->recording, b3_recOpQueryCastShape, &recWriter ); + } + + return treeStats; +} + +typedef struct WorldMoverCastContext +{ + b3World* world; + b3MoverFilterFcn* fcn; + b3QueryFilter filter; + float fraction; + b3Pos origin; + // origin relative input + b3ShapeCastInput input; + void* userContext; +} WorldMoverCastContext; + +static float MoverCastCallback( const b3BoxCastInput* input, int proxyId, uint64_t userData, void* context ) +{ + B3_UNUSED( proxyId ); + + int shapeId = (int)userData; + WorldMoverCastContext* worldContext = (WorldMoverCastContext*)context; + b3World* world = worldContext->world; + + b3Shape* shape = b3Array_Get( world->shapes, shapeId ); + b3Filter shapeFilter = shape->filter; + b3QueryFilter queryFilter = worldContext->filter; + + if ( b3ShouldQueryCollide( &shapeFilter, &queryFilter ) == false ) + { + return worldContext->fraction; + } + + if ( worldContext->fcn != NULL ) + { + b3ShapeId id = { shapeId + 1, world->worldId, shape->generation }; + bool shouldCollide = worldContext->fcn( id, worldContext->userContext ); + if ( shouldCollide == false ) + { + return worldContext->fraction; + } + } + + // Rebuild from the origin relative input, taking only the advancing fraction from the tree + b3ShapeCastInput localInput = worldContext->input; + localInput.maxFraction = input->maxFraction; + + // Re-center on the query origin so the per-shape cast stays in float precision far from the origin + b3Body* body = b3Array_Get( world->bodies, shape->bodyId ); + b3Transform transform = b3ToRelativeTransform( b3GetBodyTransformQuick( world, body ), worldContext->origin ); + + b3CastOutput output = b3ShapeCastShape( shape, transform, &localInput ); + if ( output.fraction == 0.0f ) + { + // Ignore overlapping shapes + return worldContext->fraction; + } + + worldContext->fraction = output.fraction; + return output.fraction; +} + +float b3World_CastMover( b3WorldId worldId, b3Pos origin, const b3Capsule* mover, b3Vec3 translation, b3QueryFilter filter, + b3MoverFilterFcn* fcn, void* context ) +{ + B3_ASSERT( b3IsValidPosition( origin ) ); + B3_ASSERT( b3IsValidVec3( translation ) ); + + b3World* world = b3GetUnlockedWorldFromId( worldId ); + if ( world == NULL ) + { + return 1.0f; + } + + // The mover filter is a per-shape bool(shapeId, ctx) decision, the same shape as an overlap + // callback, so the overlap trampoline captures it. Installing it even when the user passed no + // filter records an accept-all stream that replays identically. + b3RecQueryWriter recWriter = { 0 }; + if ( world->recording != NULL ) + { + b3RecQueryBegin( &recWriter, context, filter.id, filter.name ); + recWriter.userFcn.moverFilterFcn = fcn; + b3RecW_WORLDID( &recWriter.buf, worldId ); + b3RecW_POSITION( &recWriter.buf, origin ); + b3RecW_CAPSULE( &recWriter.buf, *mover ); + b3RecW_VEC3( &recWriter.buf, translation ); + b3RecW_QUERYFILTER( &recWriter.buf, filter ); + recWriter.countOffset = b3RecReserveU32( &recWriter.buf ); + fcn = b3RecOverlapTrampoline; + context = &recWriter; + } + + WorldMoverCastContext worldContext = { + .world = world, + .fcn = fcn, + .filter = filter, + .fraction = 1.0f, + .origin = origin, + .userContext = context, + }; + worldContext.input.proxy = (b3ShapeProxy){ &mover->center1, 2, mover->radius }; + worldContext.input.translation = translation; + worldContext.input.maxFraction = 1.0f; + worldContext.input.canEncroach = mover->radius > 0.0f; + + // Bound the capsule in origin relative space then lift to a conservative world float box + b3Vec3 centers[2] = { mover->center1, mover->center2 }; + b3BoxCastInput treeInput = { b3OffsetAABB( b3MakeAABB( centers, 2, mover->radius ), origin ), translation, 1.0f }; + + for ( int i = 0; i < b3_bodyTypeCount; ++i ) + { + b3DynamicTree_BoxCast( world->broadPhase.trees + i, &treeInput, filter.maskBits, false, MoverCastCallback, + &worldContext ); + + if ( worldContext.fraction == 0.0f ) + { + break; + } + + treeInput.maxFraction = worldContext.fraction; + } + + if ( world->recording != NULL ) + { + // The mover filter type aliases the overlap trampoline, so the user fcn lands in the same + // union slot. Backpatch the accept count, then record the returned fraction as the tail. + b3RecPatchU32( &recWriter.buf, recWriter.countOffset, recWriter.hitCount ); + b3RecW_F32( &recWriter.buf, worldContext.fraction ); + b3RecQueryCommit( world->recording, b3_recOpQueryCastMover, &recWriter ); + } + + return worldContext.fraction; +} + +void b3World_SetCustomFilterCallback( b3WorldId worldId, b3CustomFilterFcn* fcn, void* context ) +{ + b3World* world = b3GetUnlockedWorldFromId( worldId ); + if ( world == NULL ) + { + return; + } + world->customFilterFcn = fcn; + world->customFilterContext = context; +} + +void b3World_SetPreSolveCallback( b3WorldId worldId, b3PreSolveFcn* fcn, void* context ) +{ + b3World* world = b3GetUnlockedWorldFromId( worldId ); + if ( world == NULL ) + { + return; + } + world->preSolveFcn = fcn; + world->preSolveContext = context; +} + +void b3World_SetGravity( b3WorldId worldId, b3Vec3 gravity ) +{ + b3World* world = b3GetWorldFromId( worldId ); + + B3_REC( world, WorldSetGravity, worldId, gravity ); + + world->gravity = gravity; +} + +b3Vec3 b3World_GetGravity( b3WorldId worldId ) +{ + b3World* world = b3GetWorldFromId( worldId ); + return world->gravity; +} + +typedef struct ExplosionContext +{ + b3World* world; + b3Pos position; + float radius; + float falloff; + float impulsePerArea; +} ExplosionContext; + +static bool ExplosionCallback( int proxyId, uint64_t userData, void* context ) +{ + B3_UNUSED( proxyId ); + + int shapeId = (int)userData; + ExplosionContext* explosionContext = (ExplosionContext*)context; + b3World* world = explosionContext->world; + + b3Shape* shape = b3Array_Get( world->shapes, shapeId ); + if ( shape->explosionScale == 0.0f ) + { + return true; + } + + b3Body* body = b3Array_Get( world->bodies, shape->bodyId ); + B3_ASSERT( body->type == b3_dynamicBody ); + + b3WorldTransform xf = b3GetBodyTransformQuick( world, body ); + + // Re-center the explosion into the shape local frame so distance and direction stay precise + // far from the origin. Everything below runs in that near-origin frame. + b3Vec3 localPosition = b3InvTransformWorldPoint( xf, explosionContext->position ); + + b3DistanceInput input; + input.proxyA = b3MakeShapeProxy( shape ); + input.proxyB = (b3ShapeProxy){ &localPosition, 1, 0.0f }; + input.transform = b3Transform_identity; + input.useRadii = true; + + b3SimplexCache cache = { 0 }; + b3DistanceOutput output = b3ShapeDistance( &input, &cache, NULL, 0 ); + + float radius = explosionContext->radius; + float falloff = explosionContext->falloff; + if ( output.distance > radius + falloff ) + { + return true; + } + + b3WakeBody( world, body ); + + if ( body->setIndex != b3_awakeSet ) + { + return true; + } + + // Witness point is already in the body local query frame + b3Vec3 closestPoint = output.pointA; + if ( output.distance == 0.0f ) + { + closestPoint = b3GetShapeCentroid( shape ); + } + + b3Vec3 direction = b3Sub( closestPoint, localPosition ); + if ( b3LengthSquared( direction ) > 100.0f * FLT_EPSILON * FLT_EPSILON ) + { + direction = b3Normalize( direction ); + } + else + { + direction = (b3Vec3){ 1.0f, 0.0f, 0.0f }; + } + + float area = b3GetShapeProjectedArea( shape, direction ); + float scale = 1.0f; + if ( output.distance > radius && falloff > 0.0f ) + { + scale = b3ClampFloat( ( radius + falloff - output.distance ) / falloff, 0.0f, 1.0f ); + } + + float magnitude = explosionContext->impulsePerArea * area * scale * shape->explosionScale; + b3Vec3 impulse = b3MulSV( magnitude, b3RotateVector( xf.q, direction ) ); + + int localIndex = body->localIndex; + b3SolverSet* set = b3Array_Get( world->solverSets, b3_awakeSet ); + b3BodyState* state = b3Array_Get( set->bodyStates, localIndex ); + b3BodySim* bodySim = b3Array_Get( set->bodySims, localIndex ); + state->linearVelocity = b3MulAdd( state->linearVelocity, bodySim->invMass, impulse ); + + // Lever arm from the center of mass to the closest point, rotated to world + b3Vec3 r = b3RotateVector( xf.q, b3Sub( closestPoint, bodySim->localCenter ) ); + state->angularVelocity = b3Add( state->angularVelocity, b3MulMV( bodySim->invInertiaWorld, b3Cross( r, impulse ) ) ); + + return true; +} + +void b3World_Explode( b3WorldId worldId, const b3ExplosionDef* explosionDef ) +{ + uint64_t maskBits = explosionDef->maskBits; + b3Pos position = explosionDef->position; + float radius = explosionDef->radius; + float falloff = explosionDef->falloff; + float impulsePerArea = explosionDef->impulsePerArea; + + B3_ASSERT( b3IsValidPosition( position ) ); + B3_ASSERT( b3IsValidFloat( radius ) && radius >= 0.0f ); + B3_ASSERT( b3IsValidFloat( falloff ) && falloff >= 0.0f ); + B3_ASSERT( b3IsValidFloat( impulsePerArea ) ); + + b3World* world = b3GetUnlockedWorldFromId( worldId ); + if ( world == NULL ) + { + return; + } + + B3_REC( world, WorldExplode, worldId, *explosionDef ); + + // Locked due to waking + world->locked = true; + + struct ExplosionContext explosionContext = { world, position, radius, falloff, impulsePerArea }; + + // The broad-phase tree is float, so translate a local query box out to world with outward rounding + float extent = radius + falloff; + b3AABB localBox = { { -extent, -extent, -extent }, { extent, extent, extent } }; + b3AABB aabb = b3OffsetAABB( localBox, position ); + + b3DynamicTree_Query( world->broadPhase.trees + b3_dynamicBody, aabb, maskBits, false, ExplosionCallback, &explosionContext ); + + world->locked = false; +} + +void b3World_RebuildStaticTree( b3WorldId worldId ) +{ + b3World* world = b3GetUnlockedWorldFromId( worldId ); + if ( world == NULL ) + { + return; + } + + B3_REC( world, WorldRebuildStaticTree, worldId ); + + b3DynamicTree* staticTree = world->broadPhase.trees + b3_staticBody; + b3DynamicTree_Rebuild( staticTree, true ); +} + +void b3World_EnableSpeculative( b3WorldId worldId, bool flag ) +{ + b3World* world = b3GetUnlockedWorldFromId( worldId ); + if ( world == NULL ) + { + return; + } + + B3_REC( world, WorldEnableSpeculative, worldId, flag ); + + world->enableSpeculative = flag; +} + +#if B3_ENABLE_VALIDATION +// This validates island graph connectivity for each body +void b3ValidateConnectivity( b3World* world ) +{ + b3Body* bodies = world->bodies.data; + int bodyCapacity = world->bodies.count; + + for ( int bodyIndex = 0; bodyIndex < bodyCapacity; ++bodyIndex ) + { + b3Body* body = bodies + bodyIndex; + if ( body->id == B3_NULL_INDEX ) + { + b3ValidateFreeId( &world->bodyIdPool, bodyIndex ); + continue; + } + + B3_ASSERT( bodyIndex == body->id ); + + // Need to get the root island because islands are not merged until the next time step + int bodyIslandId = body->islandId; + int bodySetIndex = body->setIndex; + + int contactKey = body->headContactKey; + while ( contactKey != B3_NULL_INDEX ) + { + int contactId = contactKey >> 1; + int edgeIndex = contactKey & 1; + + b3Contact* contact = b3Array_Get( world->contacts, contactId ); + + bool touching = ( contact->flags & b3_contactTouchingFlag ) != 0; + if ( touching ) + { + if ( bodySetIndex != b3_staticSet ) + { + int contactIslandId = contact->islandId; + B3_ASSERT( contactIslandId == bodyIslandId ); + } + } + else + { + B3_ASSERT( contact->islandId == B3_NULL_INDEX ); + } + + contactKey = contact->edges[edgeIndex].nextKey; + } + + int jointKey = body->headJointKey; + while ( jointKey != B3_NULL_INDEX ) + { + int jointId = jointKey >> 1; + int edgeIndex = jointKey & 1; + + b3Joint* joint = b3Array_Get( world->joints, jointId ); + + int otherEdgeIndex = edgeIndex ^ 1; + + b3Body* otherBody = b3Array_Get( world->bodies, joint->edges[otherEdgeIndex].bodyId ); + + if ( bodySetIndex == b3_disabledSet || otherBody->setIndex == b3_disabledSet ) + { + B3_ASSERT( joint->islandId == B3_NULL_INDEX ); + } + else if ( bodySetIndex == b3_staticSet ) + { + // Intentional nesting + if ( otherBody->setIndex == b3_staticSet ) + { + B3_ASSERT( joint->islandId == B3_NULL_INDEX ); + } + } + else if ( body->type != b3_dynamicBody && otherBody->type != b3_dynamicBody ) + { + B3_ASSERT( joint->islandId == B3_NULL_INDEX ); + } + else + { + int jointIslandId = joint->islandId; + B3_ASSERT( jointIslandId == bodyIslandId ); + } + + jointKey = joint->edges[edgeIndex].nextKey; + } + } +} + +// Validates solver sets, but not island connectivity +void b3ValidateSolverSets( b3World* world ) +{ + B3_ASSERT( b3GetIdCapacity( &world->bodyIdPool ) == world->bodies.count ); + B3_ASSERT( b3GetIdCapacity( &world->contactIdPool ) == world->contacts.count ); + B3_ASSERT( b3GetIdCapacity( &world->jointIdPool ) == world->joints.count ); + B3_ASSERT( b3GetIdCapacity( &world->islandIdPool ) == world->islands.count ); + B3_ASSERT( b3GetIdCapacity( &world->solverSetIdPool ) == world->solverSets.count ); + + int activeSetCount = 0; + int totalBodyCount = 0; + int totalJointCount = 0; + int totalContactCount = 0; + int totalIslandCount = 0; + + // Validate all solver sets + int setCount = world->solverSets.count; + for ( int setIndex = 0; setIndex < setCount; ++setIndex ) + { + b3SolverSet* set = world->solverSets.data + setIndex; + if ( set->setIndex != B3_NULL_INDEX ) + { + activeSetCount += 1; + + if ( setIndex == b3_staticSet ) + { + B3_ASSERT( set->contactIndices.count == 0 ); + B3_ASSERT( set->islandSims.count == 0 ); + B3_ASSERT( set->bodyStates.count == 0 ); + } + else if ( setIndex == b3_disabledSet ) + { + B3_ASSERT( set->islandSims.count == 0 ); + B3_ASSERT( set->bodyStates.count == 0 ); + } + else if ( setIndex == b3_awakeSet ) + { + B3_ASSERT( set->bodySims.count == set->bodyStates.count ); + B3_ASSERT( set->jointSims.count == 0 ); + } + else + { + B3_ASSERT( set->bodyStates.count == 0 ); + } + + // Validate bodies + { + b3Body* bodies = world->bodies.data; + B3_ASSERT( set->bodySims.count >= 0 ); + totalBodyCount += set->bodySims.count; + for ( int i = 0; i < set->bodySims.count; ++i ) + { + b3BodySim* bodySim = set->bodySims.data + i; + + int bodyId = bodySim->bodyId; + B3_ASSERT( 0 <= bodyId && bodyId < world->bodies.count ); + b3Body* body = bodies + bodyId; + B3_ASSERT( body->setIndex == setIndex ); + B3_ASSERT( body->localIndex == i ); + + uint32_t syncedFlags = body->flags & ~b3_bodyTransientFlags; + B3_ASSERT( ( bodySim->flags & syncedFlags ) == syncedFlags ); + + b3BodyState* bodyState = b3GetBodyState( world, body ); + if ( bodyState != NULL ) + { + B3_ASSERT( ( bodyState->flags & syncedFlags ) == syncedFlags ); + } + + if ( body->type == b3_dynamicBody ) + { + B3_ASSERT( body->flags & b3_dynamicFlag ); + } + + if ( setIndex == b3_disabledSet ) + { + B3_ASSERT( body->headContactKey == B3_NULL_INDEX ); + } + + // Validate body shapes + int prevShapeId = B3_NULL_INDEX; + int shapeId = body->headShapeId; + while ( shapeId != B3_NULL_INDEX ) + { + b3Shape* shape = b3Array_Get( world->shapes, shapeId ); + B3_ASSERT( shape->id == shapeId ); + B3_ASSERT( shape->prevShapeId == prevShapeId ); + + if ( setIndex == b3_disabledSet ) + { + B3_ASSERT( shape->proxyKey == B3_NULL_INDEX ); + } + else if ( setIndex == b3_staticSet ) + { + B3_ASSERT( B3_PROXY_TYPE( shape->proxyKey ) == b3_staticBody ); + } + else + { + b3BodyType proxyType = B3_PROXY_TYPE( shape->proxyKey ); + B3_ASSERT( proxyType == b3_kinematicBody || proxyType == b3_dynamicBody ); + } + + prevShapeId = shapeId; + shapeId = shape->nextShapeId; + } + + // Validate body contacts + int contactKey = body->headContactKey; + while ( contactKey != B3_NULL_INDEX ) + { + int contactId = contactKey >> 1; + int edgeIndex = contactKey & 1; + + b3Contact* contact = b3Array_Get( world->contacts, contactId ); + B3_ASSERT( contact->setIndex != b3_staticSet ); + B3_ASSERT( contact->edges[0].bodyId == bodyId || contact->edges[1].bodyId == bodyId ); + contactKey = contact->edges[edgeIndex].nextKey; + } + + // Validate body joints + int jointKey = body->headJointKey; + while ( jointKey != B3_NULL_INDEX ) + { + int jointId = jointKey >> 1; + int edgeIndex = jointKey & 1; + + b3Joint* joint = b3Array_Get( world->joints, jointId ); + + int otherEdgeIndex = edgeIndex ^ 1; + + b3Body* otherBody = b3Array_Get( world->bodies, joint->edges[otherEdgeIndex].bodyId ); + + if ( setIndex == b3_disabledSet || otherBody->setIndex == b3_disabledSet ) + { + B3_ASSERT( joint->setIndex == b3_disabledSet ); + } + else if ( setIndex == b3_staticSet && otherBody->setIndex == b3_staticSet ) + { + B3_ASSERT( joint->setIndex == b3_staticSet ); + } + else if ( body->type != b3_dynamicBody && otherBody->type != b3_dynamicBody ) + { + B3_ASSERT( joint->setIndex == b3_staticSet ); + } + else if ( setIndex == b3_awakeSet ) + { + B3_ASSERT( joint->setIndex == b3_awakeSet ); + } + else if ( setIndex >= b3_firstSleepingSet ) + { + B3_ASSERT( joint->setIndex == setIndex ); + } + + b3JointSim* jointSim = b3GetJointSim( world, joint ); + B3_ASSERT( jointSim->jointId == jointId ); + B3_ASSERT( jointSim->bodyIdA == joint->edges[0].bodyId ); + B3_ASSERT( jointSim->bodyIdB == joint->edges[1].bodyId ); + + jointKey = joint->edges[edgeIndex].nextKey; + } + } + } + + // Validate contacts + { + B3_ASSERT( set->contactIndices.count >= 0 ); + totalContactCount += set->contactIndices.count; + for ( int i = 0; i < set->contactIndices.count; ++i ) + { + int contactIndex = set->contactIndices.data[i]; + b3Contact* contact = b3Array_Get( world->contacts, contactIndex ); + if ( setIndex == b3_awakeSet ) + { + // contact should be non-touching if awake + // or it could be this contact hasn't been transferred yet + B3_ASSERT( contact->manifoldCount == 0 || ( contact->flags & b3_simStartedTouching ) != 0 ); + } + B3_ASSERT( contact->setIndex == setIndex ); + B3_ASSERT( contact->colorIndex == B3_NULL_INDEX ); + B3_ASSERT( contact->localIndex == i ); + } + } + + // Validate joints + { + B3_ASSERT( set->jointSims.count >= 0 ); + totalJointCount += set->jointSims.count; + for ( int i = 0; i < set->jointSims.count; ++i ) + { + b3JointSim* jointSim = set->jointSims.data + i; + b3Joint* joint = b3Array_Get( world->joints, jointSim->jointId ); + B3_ASSERT( joint->setIndex == setIndex ); + B3_ASSERT( joint->colorIndex == B3_NULL_INDEX ); + B3_ASSERT( joint->localIndex == i ); + } + } + + // Validate islands + { + B3_ASSERT( set->islandSims.count >= 0 ); + totalIslandCount += set->islandSims.count; + for ( int i = 0; i < set->islandSims.count; ++i ) + { + b3IslandSim* islandSim = set->islandSims.data + i; + b3Island* island = b3Array_Get( world->islands, islandSim->islandId ); + B3_ASSERT( island->setIndex == setIndex ); + B3_ASSERT( island->localIndex == i ); + } + } + } + else + { + B3_ASSERT( set->bodySims.count == 0 ); + B3_ASSERT( set->contactIndices.count == 0 ); + B3_ASSERT( set->jointSims.count == 0 ); + B3_ASSERT( set->islandSims.count == 0 ); + B3_ASSERT( set->bodyStates.count == 0 ); + } + } + + int setIdCount = b3GetIdCount( &world->solverSetIdPool ); + B3_ASSERT( activeSetCount == setIdCount ); + + int bodyIdCount = b3GetIdCount( &world->bodyIdPool ); + B3_ASSERT( totalBodyCount == bodyIdCount ); + + int islandIdCount = b3GetIdCount( &world->islandIdPool ); + B3_ASSERT( totalIslandCount == islandIdCount ); + + // Validate constraint graph + for ( int colorIndex = 0; colorIndex < B3_GRAPH_COLOR_COUNT; ++colorIndex ) + { + b3GraphColor* color = world->constraintGraph.colors + colorIndex; + int bitCount = 0; + + B3_ASSERT( color->convexContacts.count >= 0 ); + totalContactCount += color->convexContacts.count; + for ( int i = 0; i < color->convexContacts.count; ++i ) + { + int contactId = color->convexContacts.data[i]; + b3Contact* contact = b3Array_Get( world->contacts, contactId ); + // contact should be touching in the constraint graph or awaiting transfer to non-touching + B3_ASSERT( contact->manifoldCount > 0 || ( contact->flags & ( b3_simStoppedTouching | b3_simDisjoint ) ) != 0 ); + B3_ASSERT( contact->setIndex == b3_awakeSet ); + B3_ASSERT( contact->colorIndex == colorIndex ); + B3_ASSERT( contact->localIndex == i ); + + int bodyIdA = contact->edges[0].bodyId; + int bodyIdB = contact->edges[1].bodyId; + + if ( colorIndex < B3_OVERFLOW_INDEX ) + { + b3Body* bodyA = b3Array_Get( world->bodies, bodyIdA ); + b3Body* bodyB = b3Array_Get( world->bodies, bodyIdB ); + B3_ASSERT( b3GetBit( &color->bodySet, bodyIdA ) == ( bodyA->type == b3_dynamicBody ) ); + B3_ASSERT( b3GetBit( &color->bodySet, bodyIdB ) == ( bodyB->type == b3_dynamicBody ) ); + + bitCount += bodyA->type == b3_dynamicBody ? 1 : 0; + bitCount += bodyB->type == b3_dynamicBody ? 1 : 0; + } + } + + totalContactCount += color->contacts.count; + for ( int i = 0; i < color->contacts.count; ++i ) + { + int contactId = color->contacts.data[i].contactId; + b3Contact* contact = b3Array_Get( world->contacts, contactId ); + // contact should be touching in the constraint graph or awaiting transfer to non-touching + B3_ASSERT( contact->manifoldCount > 0 || ( contact->flags & ( b3_simStoppedTouching | b3_simDisjoint ) ) != 0 ); + B3_ASSERT( contact->setIndex == b3_awakeSet ); + B3_ASSERT( contact->colorIndex == colorIndex ); + B3_ASSERT( contact->localIndex == i ); + + int bodyIdA = contact->edges[0].bodyId; + int bodyIdB = contact->edges[1].bodyId; + + if ( colorIndex < B3_OVERFLOW_INDEX ) + { + b3Body* bodyA = b3Array_Get( world->bodies, bodyIdA ); + b3Body* bodyB = b3Array_Get( world->bodies, bodyIdB ); + B3_ASSERT( b3GetBit( &color->bodySet, bodyIdA ) == ( bodyA->type == b3_dynamicBody ) ); + B3_ASSERT( b3GetBit( &color->bodySet, bodyIdB ) == ( bodyB->type == b3_dynamicBody ) ); + + bitCount += bodyA->type == b3_dynamicBody ? 1 : 0; + bitCount += bodyB->type == b3_dynamicBody ? 1 : 0; + } + } + + B3_ASSERT( color->jointSims.count >= 0 ); + totalJointCount += color->jointSims.count; + for ( int i = 0; i < color->jointSims.count; ++i ) + { + b3JointSim* jointSim = color->jointSims.data + i; + b3Joint* joint = b3Array_Get( world->joints, jointSim->jointId ); + B3_ASSERT( joint->setIndex == b3_awakeSet ); + B3_ASSERT( joint->colorIndex == colorIndex ); + B3_ASSERT( joint->localIndex == i ); + + int bodyIdA = joint->edges[0].bodyId; + int bodyIdB = joint->edges[1].bodyId; + + if ( colorIndex < B3_OVERFLOW_INDEX ) + { + b3Body* bodyA = b3Array_Get( world->bodies, bodyIdA ); + b3Body* bodyB = b3Array_Get( world->bodies, bodyIdB ); + B3_ASSERT( b3GetBit( &color->bodySet, bodyIdA ) == ( bodyA->type == b3_dynamicBody ) ); + B3_ASSERT( b3GetBit( &color->bodySet, bodyIdB ) == ( bodyB->type == b3_dynamicBody ) ); + + bitCount += bodyA->type == b3_dynamicBody ? 1 : 0; + bitCount += bodyB->type == b3_dynamicBody ? 1 : 0; + } + } + + // Validate the bit population for this graph color + B3_ASSERT( bitCount == b3CountSetBits( &color->bodySet ) ); + } + + int contactIdCount = b3GetIdCount( &world->contactIdPool ); + B3_ASSERT( totalContactCount == contactIdCount ); + B3_ASSERT( totalContactCount == (int)world->broadPhase.pairSet.count ); + + int jointIdCount = b3GetIdCount( &world->jointIdPool ); + B3_ASSERT( totalJointCount == jointIdCount ); + +// Validate shapes +// This is very slow on compounds +#if 0 + int shapeCapacity = b3Array(world->shapeArray).count; + for (int shapeIndex = 0; shapeIndex < shapeCapacity; shapeIndex += 1) + { + b3Shape* shape = world->shapeArray + shapeIndex; + if (shape->id != shapeIndex) + { + continue; + } + + B3_ASSERT(0 <= shape->bodyId && shape->bodyId < b3Array(world->bodyArray).count); + + b3Body* body = world->bodyArray + shape->bodyId; + B3_ASSERT(0 <= body->setIndex && body->setIndex < b3Array(world->solverSetArray).count); + + b3SolverSet* set = world->solverSetArray + body->setIndex; + B3_ASSERT(0 <= body->localIndex && body->localIndex < set->sims.count); + + b3BodySim* bodySim = set->sims.mData + body->localIndex; + B3_ASSERT(bodySim->bodyId == shape->bodyId); + + bool found = false; + int shapeCount = 0; + int index = body->headShapeId; + while (index != B3_NULL_INDEX) + { + b3CheckId(world->shapeArray, index); + b3Shape* s = world->shapeArray + index; + if (index == shapeIndex) + { + found = true; + } + + index = s->nextShapeId; + shapeCount += 1; + } + + B3_ASSERT(found); + B3_ASSERT(shapeCount == body->shapeCount); + } +#endif +} + +// Validate contact touching status. +void b3ValidateContacts( b3World* world ) +{ + b3ConstraintGraph* graph = &world->constraintGraph; + int contactCount = world->contacts.count; + B3_ASSERT( contactCount == b3GetIdCapacity( &world->contactIdPool ) ); + int allocatedContactCount = 0; + + for ( int contactIndex = 0; contactIndex < contactCount; ++contactIndex ) + { + b3Contact* contact = b3Array_Get( world->contacts, contactIndex ); + if ( contact->contactId == B3_NULL_INDEX ) + { + continue; + } + + B3_ASSERT( contact->contactId == contactIndex ); + + allocatedContactCount += 1; + + bool touching = ( contact->flags & b3_contactTouchingFlag ) != 0; + + int setId = contact->setIndex; + b3SolverSet* set = b3Array_Get( world->solverSets, setId ); + + if ( setId == b3_awakeSet ) + { + if ( touching ) + { + B3_ASSERT( 0 <= contact->colorIndex && contact->colorIndex < B3_GRAPH_COLOR_COUNT ); + // Validate body sim indices + b3Shape* shapeA = b3Array_Get( world->shapes, contact->shapeIdA ); + b3Shape* shapeB = b3Array_Get( world->shapes, contact->shapeIdB ); + + b3Body* bodyA = b3Array_Get( world->bodies, shapeA->bodyId ); + b3Body* bodyB = b3Array_Get( world->bodies, shapeB->bodyId ); + + if ( bodyA->type == b3_staticBody ) + { + B3_ASSERT( contact->bodySimIndexA == B3_NULL_INDEX ); + } + else + { + B3_ASSERT( contact->bodySimIndexA == bodyA->localIndex ); + } + + if ( bodyB->type == b3_staticBody ) + { + B3_ASSERT( contact->bodySimIndexB == B3_NULL_INDEX ); + } + else + { + B3_ASSERT( contact->bodySimIndexB == bodyB->localIndex ); + } + + if ( ( contact->flags & b3_simMeshContact ) != 0 || contact->colorIndex == B3_OVERFLOW_INDEX ) + { + b3GraphColor* color = graph->colors + contact->colorIndex; + int contactId = b3Array_Get( color->contacts, contact->localIndex )->contactId; + B3_ASSERT( contactId == contactIndex ); + } + else + { + b3GraphColor* color = graph->colors + contact->colorIndex; + int contactId = *b3Array_Get( color->convexContacts, contact->localIndex ); + B3_ASSERT( contactId == contactIndex ); + } + } + else + { + B3_ASSERT( contact->colorIndex == B3_NULL_INDEX ); + B3_ASSERT( contact->manifolds == NULL ); + B3_ASSERT( contact->manifoldCount == 0 ); + + int* index = b3Array_Get( set->contactIndices, contact->localIndex ); + B3_ASSERT( *index == contactIndex ); + } + } + else if ( setId >= b3_firstSleepingSet ) + { + // Only touching contacts allowed in a sleeping set + B3_ASSERT( touching == true ); + B3_ASSERT( contact->manifolds != NULL ); + B3_ASSERT( contact->manifoldCount > 0 ); + int* index = b3Array_Get( set->contactIndices, contact->localIndex ); + B3_ASSERT( *index == contactIndex ); + } + else + { + // Sleeping and non-touching contacts belong in the disabled set + B3_ASSERT( touching == false && setId == b3_disabledSet ); + B3_ASSERT( contact->manifolds == NULL ); + B3_ASSERT( contact->manifoldCount == 0 ); + int* index = b3Array_Get( set->contactIndices, contact->localIndex ); + B3_ASSERT( *index == contactIndex ); + } + + if ( contact->flags & b3_simMeshContact ) + { + int cacheCount = contact->meshContact.triangleCache.count; + if ( cacheCount > 0 ) + { + B3_ASSERT( contact->meshContact.triangleCache.data != NULL ); + B3_ASSERT( contact->meshContact.triangleCache.capacity >= cacheCount ); + + b3Shape* shapeA = b3Array_Get( world->shapes, contact->shapeIdA ); + if ( shapeA->type == b3_meshShape ) + { + int triangleCount = shapeA->mesh.data->triangleCount; + for ( int i = 0; i < cacheCount; ++i ) + { + int triangleIndex = contact->meshContact.triangleCache.data[i].triangleIndex; + B3_ASSERT( 0 <= triangleIndex && triangleIndex < triangleCount ); + } + } + else if ( shapeA->type == b3_heightShape ) + { + int triangleCount = b3GetHeightFieldTriangleCount( shapeA->heightField ); + for ( int i = 0; i < cacheCount; ++i ) + { + int triangleIndex = contact->meshContact.triangleCache.data[i].triangleIndex; + B3_ASSERT( 0 <= triangleIndex && triangleIndex < triangleCount ); + } + } + else + { + B3_ASSERT( shapeA->type == b3_compoundShape ); + b3ChildShape child = b3GetCompoundChild( shapeA->compound, contact->childIndex ); + B3_ASSERT( child.type == b3_meshShape ); + + int triangleCount = child.mesh.data->triangleCount; + for ( int i = 0; i < cacheCount; ++i ) + { + int triangleIndex = contact->meshContact.triangleCache.data[i].triangleIndex; + B3_ASSERT( 0 <= triangleIndex && triangleIndex < triangleCount ); + } + } + } + } + } + + int contactIdCount = b3GetIdCount( &world->contactIdPool ); + B3_ASSERT( allocatedContactCount == contactIdCount ); +} + +#else + +void b3ValidateConnectivity( b3World* world ) +{ + B3_UNUSED( world ); +} + +void b3ValidateSolverSets( b3World* world ) +{ + B3_UNUSED( world ); +} + +void b3ValidateContacts( b3World* world ) +{ + B3_UNUSED( world ); +} + +#endif diff --git a/vendor/box3d/src/src/physics_world.h b/vendor/box3d/src/src/physics_world.h new file mode 100644 index 000000000..12f169561 --- /dev/null +++ b/vendor/box3d/src/src/physics_world.h @@ -0,0 +1,350 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +#pragma once + +#include "arena_allocator.h" +#include "bitset.h" +#include "block_allocator.h" +#include "broad_phase.h" +#include "constraint_graph.h" +#include "id_pool.h" +#include "name_cache.h" + +#include "box3d/types.h" + +#define B3_DEBUG_POINT_CAPACITY 64 +#define B3_DEBUG_LINE_CAPACITY 64 + +typedef struct b3Body b3Body; +typedef struct b3Recording b3Recording; +typedef struct b3Contact b3Contact; +typedef struct b3Island b3Island; +typedef struct b3Joint b3Joint; +typedef struct b3Sensor b3Sensor; +typedef struct b3SensorTaskContext b3SensorTaskContext; +typedef struct b3SensorHit b3SensorHit; +typedef struct b3Shape b3Shape; +typedef struct b3SolverSet b3SolverSet; + +b3DeclareArray( b3BlockAllocator ); +b3DeclareArray( b3Body ); +b3DeclareArray( b3SolverSet ); +b3DeclareArray( b3Joint ); +b3DeclareArray( b3Contact ); +b3DeclareArray( b3Island ); +b3DeclareArray( b3Shape ); +b3DeclareArray( b3Sensor ); +b3DeclareArray( b3SensorTaskContext ); +b3DeclareArray( b3SensorHit ); +b3DeclareArray( b3BodyMoveEvent ); +b3DeclareArray( b3SensorBeginTouchEvent ); +b3DeclareArray( b3ContactBeginTouchEvent ); +b3DeclareArray( b3SensorEndTouchEvent ); +b3DeclareArray( b3ContactEndTouchEvent ); +b3DeclareArray( b3ContactHitEvent ); +b3DeclareArray( b3JointEvent ); + +enum b3SetType +{ + b3_staticSet = 0, + b3_disabledSet = 1, + b3_awakeSet = 2, + b3_firstSleepingSet = 3, +}; + +typedef struct b3DebugPoint +{ + b3Pos p; + int label; + float value; + b3HexColor color; +} b3DebugPoint; + +typedef struct b3DebugLine +{ + b3Pos p1, p2; + int label; + b3HexColor color; +} b3DebugLine; + +// Per thread task storage +typedef struct b3TaskContext +{ + b3Arena arena; + + // Collect per thread sensor continuous hit events. + b3Array( b3SensorHit ) sensorHits; + + // These bits align with the b3ConstraintGraph::contactBlocks and signal a change in contact status + b3BitSet contactStateBitSet; + + // These bits align with the joint id capacity and signal a change in contact status + b3BitSet jointStateBitSet; + + // These bits align with the contact id capacity and signal a hit event. + b3BitSet hitEventBitSet; + + // Fast-path flag: true when this worker set at least one bit in hitEventBitSet this step. + bool hasHitEvents; + + // Used to track bodies with shapes that have enlarged AABBs. This avoids having a bit array + // that is very large when there are many static shapes. + b3BitSet enlargedSimBitSet; + + // Used to put islands to sleep + b3BitSet awakeIslandBitSet; + + // Per worker split island candidate + float splitSleepTime; + int splitIslandId; + + // Profiling + int satCallCount; + int satCacheHitCount; + int distanceIterations; + int pushBackIterations; + int rootIterations; + + // Number of contacts recycled this step (collide pass). + int recycledContactCount; + + b3DebugPoint points[B3_DEBUG_POINT_CAPACITY]; + int pointCount; + + b3DebugLine lines[B3_DEBUG_LINE_CAPACITY]; + int lineCount; + + int manifoldCounts[B3_CONTACT_MANIFOLD_COUNT_BUCKETS]; + // Prevent false sharing + char cacheLine[64]; +} b3TaskContext; + +b3DeclareArray( b3TaskContext ); + +// The world struct manages all physics entities, dynamic simulation, and asynchronous queries. +// The world also contains efficient memory management facilities. +typedef struct b3World +{ + b3Stack stack; + b3BroadPhase broadPhase; + b3ConstraintGraph constraintGraph; + + // Manifold allocators have one allocator for each manifold count. + b3Array( b3BlockAllocator ) manifoldAllocators; + b3Mutex* manifoldAllocatorMutex; + + // The body id pool is used to allocate and recycle body ids. Body ids + // provide a stable identifier for users, but incur caches misses when used + // to access body data. Aligns with b3Body. + b3IdPool bodyIdPool; + + // This is a sparse array that maps body ids to the body data + // stored in solver sets. As sims move within a set or across set. + // Indices come from id pool. + b3Array( b3Body ) bodies; + + // Provides free list for solver sets. + b3IdPool solverSetIdPool; + + // Solvers sets allow sims to be stored in contiguous arrays. The first + // set is all static sims. The second set is active sims. The third set is disabled + // sims. The remaining sets are sleeping islands. + b3Array( b3SolverSet ) solverSets; + + // Used to create stable ids for joints + b3IdPool jointIdPool; + + // This is a sparse array that maps joint ids to the joint data stored in the constraint graph + // or in the solver sets. + b3Array( b3Joint ) joints; + + // Used to create stable ids for contacts + b3IdPool contactIdPool; + + // This is a sparse array that maps contact ids to the contact data stored in the constraint graph + // or in the solver sets. + b3Array( b3Contact ) contacts; + + // Used to create stable ids for islands + b3IdPool islandIdPool; + + // This is a sparse array that maps island ids to the island data stored in the solver sets. + b3Array( b3Island ) islands; + + b3IdPool shapeIdPool; + + // These are sparse arrays that point into the pools above + b3Array( b3Shape ) shapes; + + // Reference counted store of shared hull data keyed by content. Shapes hold a + // pointer to the owned copy here. Opaque to avoid leaking the verstable map + // type into this header. + void* hullDatabase; + + // Name cache for shape and body names. This works with recording. + b3NameCache names; + + // This is a dense array of sensor data. + b3Array( b3Sensor ) sensors; + + // Per thread storage + b3Array( b3TaskContext ) taskContexts; + b3Array( b3SensorTaskContext ) sensorTaskContexts; + + b3Array( b3BodyMoveEvent ) bodyMoveEvents; + b3Array( b3SensorBeginTouchEvent ) sensorBeginEvents; + b3Array( b3ContactBeginTouchEvent ) contactBeginEvents; + + // End events are double buffered so that the user doesn't need to flush events + b3Array( b3SensorEndTouchEvent ) sensorEndEvents[2]; + b3Array( b3ContactEndTouchEvent ) contactEndEvents[2]; + int endEventArrayIndex; + + b3Array( b3ContactHitEvent ) contactHitEvents; + b3Array( b3JointEvent ) jointEvents; + + // Used to track debug draw + b3BitSet debugBodySet; + b3BitSet debugJointSet; + b3BitSet debugContactSet; + b3BitSet debugIslandSet; + b3CreateDebugShapeCallback* createDebugShape; + b3DestroyDebugShapeCallback* destroyDebugShape; + void* userDebugShapeContext; + + // Id that is incremented every time step + uint64_t stepIndex; + + // Identify islands for splitting as follows: + // - I want to split islands so smaller islands can sleep + // - when a body comes to rest and its sleep timer trips, I can look at the island and flag it for splitting + // if it has removed constraints + // - islands that have removed constraints must be put split first because I don't want to wake bodies incorrectly + // - otherwise I can use the awake islands that have bodies wanting to sleep as the splitting candidates + // - if no bodies want to sleep then there is no reason to perform island splitting + int splitIslandId; + + b3Vec3 gravity; + float hitEventThreshold; + float restitutionThreshold; + float maxLinearSpeed; + float contactSpeed; + float contactHertz; + float contactDampingRatio; + float contactRecycleDistance; + + b3FrictionCallback* frictionCallback; + b3RestitutionCallback* restitutionCallback; + + uint16_t generation; + + b3Profile profile; + int satCallCount; + int satCacheHitCount; + int manifoldCounts[B3_CONTACT_MANIFOLD_COUNT_BUCKETS]; + + b3Capacity maxCapacity; + + b3PreSolveFcn* preSolveFcn; + void* preSolveContext; + + b3CustomFilterFcn* customFilterFcn; + void* customFilterContext; + + int workerCount; + b3EnqueueTaskCallback* enqueueTaskFcn; + b3FinishTaskCallback* finishTaskFcn; + void* userTaskContext; + void* userTreeTask; + + struct b3Scheduler* scheduler; + + void* userData; + + // Non-NULL while a recording session is active. Set by b3World_StartRecording, + // cleared by b3World_StopRecording. Hooks in mutators check this before writing. + struct b3Recording* recording; + + // latest inverse sub-step + float inv_h; + + // latest inverse full-step + float inv_dt; + + int activeTaskCount; + int taskCount; + + uint16_t worldId; + + bool enableSleep; + + // This indicates there is a world write operation in progress. This is for debugging and + // not a real mutex. This should have minimal performance impact. + bool locked; + bool enableWarmStarting; + bool enableContinuous; + bool enableSpeculative; + bool inUse; +} b3World; + +b3World* b3GetUnlockedWorldFromId( b3WorldId id ); +b3World* b3GetWorldFromId( b3WorldId id ); + +b3World* b3GetUnlockedWorld( int index ); +b3World* b3GetWorld( int index ); + +void b3ValidateConnectivity( b3World* world ); +void b3ValidateSolverSets( b3World* world ); +void b3ValidateContacts( b3World* world ); + +// Register a hull in the world database, returning the owned shared copy. Identical hulls +// share one copy with a reference count. The input may be freed after this call. +const b3HullData* b3AddHullToDatabase( b3World* world, const b3HullData* src ); + +// Like b3AddHullToDatabase but takes ownership of a heap hull: inserted directly on a miss, +// freed on a hit. Avoids cloning data the caller already allocated. +const b3HullData* b3AddOwnedHullToDatabase( b3World* world, b3HullData* owned ); + +// Release a reference to a shared hull. The owned copy is freed when the count reaches zero. +void b3RemoveHullFromDatabase( b3World* world, const b3HullData* data ); + +static inline b3Manifold* b3AllocateManifolds( b3World* world, int count ) +{ + if ( count == 0 ) + { + return NULL; + } + + int index = count - 1; + + // Need lock because this is called from the parallel narrow phase + b3LockMutex( world->manifoldAllocatorMutex ); + int currentCount = world->manifoldAllocators.count; + for ( int i = currentCount; i < count; ++i ) + { + b3BlockAllocator allocator = b3CreateBlockAllocator( ( i + 1 ) * sizeof( b3Manifold ), 2 * B3_BLOCK_SIZE ); + b3Array_Push( world->manifoldAllocators, allocator ); + } + + b3BlockAllocator* allocator = b3Array_Get( world->manifoldAllocators, index ); + b3Manifold* manifolds = (b3Manifold*)b3AllocateElement( allocator ); + b3UnlockMutex( world->manifoldAllocatorMutex ); + memset( manifolds, 0, count * sizeof( b3Manifold ) ); + return manifolds; +} + +static inline void b3FreeManifolds( b3World* world, b3Manifold* manifolds, int count ) +{ + if ( count == 0 ) + { + return; + } + + int index = count - 1; + b3LockMutex( world->manifoldAllocatorMutex ); + b3BlockAllocator* allocator = b3Array_Get( world->manifoldAllocators, index ); + b3FreeElement( allocator, manifolds ); + b3UnlockMutex( world->manifoldAllocatorMutex ); +} + diff --git a/vendor/box3d/src/src/platform.h b/vendor/box3d/src/src/platform.h new file mode 100644 index 000000000..2bc37a591 --- /dev/null +++ b/vendor/box3d/src/src/platform.h @@ -0,0 +1,104 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +#pragma once + +#include "core.h" + +#include +#include + +// Software prefetch hint. T0 brings the line into all cache levels. +// On x86 MSVC exposes _mm_prefetch, ARM MSVC uses __prefetch instead. +// clang/gcc provide __builtin_prefetch on every target. +#if defined( B3_COMPILER_MSVC ) +#include +#if defined( B3_CPU_X86_X64 ) +#define b3Prefetch( addr ) _mm_prefetch( (const char*)( addr ), _MM_HINT_T0 ) +#else +#define b3Prefetch( addr ) __prefetch( (const void*)( addr ) ) +#endif +#elif defined( B3_COMPILER_CLANG ) || defined( B3_COMPILER_GCC ) +#define b3Prefetch( addr ) __builtin_prefetch( (const void*)( addr ), 0, 3 ) +#else +#define b3Prefetch( addr ) ( (void)( addr ) ) +#endif + +static inline void b3AtomicStoreInt( b3AtomicInt* a, int value ) +{ +#if defined( _MSC_VER ) + (void)_InterlockedExchange( (long*)&a->value, value ); +#elif defined( __GNUC__ ) || defined( __clang__ ) + __atomic_store_n( &a->value, value, __ATOMIC_SEQ_CST ); +#else +#error "Unsupported platform" +#endif +} + +static inline int b3AtomicLoadInt( b3AtomicInt* a ) +{ +#if defined( _MSC_VER ) && !defined( __clang__ ) + int value = __iso_volatile_load32( (volatile __int32*)&a->value ); +#if defined( _M_ARM ) || defined( _M_ARM64 ) || defined( _M_ARM64EC ) + __dmb( 0xB ); +#else + _ReadWriteBarrier(); +#endif + return value; +#elif defined( __GNUC__ ) || defined( __clang__ ) + return __atomic_load_n( &a->value, __ATOMIC_SEQ_CST ); +#else +#error "Unsupported platform" +#endif +} + +static inline int b3AtomicFetchAddInt( b3AtomicInt* a, int increment ) +{ +#if defined( _MSC_VER ) + return _InterlockedExchangeAdd( (long*)&a->value, (long)increment ); +#elif defined( __GNUC__ ) || defined( __clang__ ) + return __atomic_fetch_add( &a->value, increment, __ATOMIC_SEQ_CST ); +#else +#error "Unsupported platform" +#endif +} + +static inline bool b3AtomicCompareExchangeInt( b3AtomicInt* a, int expected, int desired ) +{ +#if defined( _MSC_VER ) + return _InterlockedCompareExchange( (long*)&a->value, (long)desired, (long)expected ) == expected; +#elif defined( __GNUC__ ) || defined( __clang__ ) + // The value written to expected is ignored + return __atomic_compare_exchange_n( &a->value, &expected, desired, false, __ATOMIC_SEQ_CST, __ATOMIC_SEQ_CST ); +#else +#error "Unsupported platform" +#endif +} + +static inline void b3AtomicStoreU32( b3AtomicU32* a, uint32_t value ) +{ +#if defined( _MSC_VER ) + (void)_InterlockedExchange( (long*)&a->value, value ); +#elif defined( __GNUC__ ) || defined( __clang__ ) + __atomic_store_n( &a->value, value, __ATOMIC_SEQ_CST ); +#else +#error "Unsupported platform" +#endif +} + +static inline uint32_t b3AtomicLoadU32( b3AtomicU32* a ) +{ +#if defined( _MSC_VER ) && !defined( __clang__ ) + uint32_t value = (uint32_t)__iso_volatile_load32( (volatile __int32*)&a->value ); +#if defined( _M_ARM ) || defined( _M_ARM64 ) || defined( _M_ARM64EC ) + __dmb( 0xB ); +#else + _ReadWriteBarrier(); +#endif + return value; +#elif defined( __GNUC__ ) || defined( __clang__ ) + return __atomic_load_n( &a->value, __ATOMIC_SEQ_CST ); +#else +#error "Unsupported platform" +#endif +} diff --git a/vendor/box3d/src/src/prismatic_joint.c b/vendor/box3d/src/src/prismatic_joint.c new file mode 100644 index 000000000..c96e6d643 --- /dev/null +++ b/vendor/box3d/src/src/prismatic_joint.c @@ -0,0 +1,725 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +#include "body.h" +#include "core.h" +#include "joint.h" +#include "math_internal.h" +#include "physics_world.h" +#include "solver.h" +#include "solver_set.h" +#include "recording.h" + +// needed for dll export +#include "box3d/box3d.h" + +// Linear constraint (point-to-line) +// joint axis is along joint frame A local z-axis +// perpX and perpY are world vectors fixed in A +// +// d = pB - pA = xB + rB - xA - rA +// Cx = dot(perpX, d) +// Cy = dot(perpY, d) + +// CdotX = dot(d, cross(wA, perpX)) + dot(perpX, vB + cross(wB, rB) - vA - cross(wA, rA)) +// = -dot(perpX, vA) - dot(cross(d + rA, perpX), wA) + dot(perpX, vB) + dot(cross(rB, perpX), vB) +// Jx = [-perpX, -cross(d + rA, perpX), perpX, cross(rB, perpX)] +// similar for perpY +// +// Simplification dropping dot(d, cross(wA, perpX)) (todo needs testing) +// CdotXs = dot(perpX, vB + cross(wB, rB) - vA - cross(wA, rA)) +// Jxs = [-perpX, -cross(rA, perpX), perpX, cross(rB, perpX)] + +// Motor/limit/spring linear constraint +// axis is the world joint axis fixed in A + +// C = dot(axis, d) +// Cdot = dot(d, cross(wA, axis)) + dot(axis, vB + cross(wB, rB) - vA - cross(wA, rA)) +// Cdot = -dot(axis, vA) - dot(cross(d + rA, axis), wA) + dot(axis, vB) + dot(cross(rB, axis), vB) +// J = [-axis -cross(d + rA, axis) axis cross(rB, axis)] +// +// Simplified (todo needs testing) +// Cdot = -dot(axis, vA) - dot(cross(rA, axis), wA) + dot(axis, vB) + dot(cross(rB, axis), vB) +// J = [-axis -cross(rA, axis) axis cross(rB, axis)] + +// Predictive limit is applied even when the limit is not active. +// Prevents a constraint speed that can lead to a constraint error in one time step. +// Want C2 = C1 + h * Cdot >= 0 +// Or: +// Cdot + C1/h >= 0 +// I do not apply a negative constraint error because that is handled in position correction. +// So: +// Cdot + max(C1, 0)/h >= 0 + +void b3PrismaticJoint_EnableLimit( b3JointId jointId, bool enableLimit ) +{ + b3World* world = b3GetWorld( jointId.world0 ); + B3_REC( world, PrismaticJointEnableLimit, jointId, enableLimit ); + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_prismaticJoint ); + if ( enableLimit != base->prismaticJoint.enableLimit ) + { + base->prismaticJoint.lowerImpulse = 0.0f; + base->prismaticJoint.upperImpulse = 0.0f; + } + base->prismaticJoint.enableLimit = enableLimit; +} + +bool b3PrismaticJoint_IsLimitEnabled( b3JointId jointId ) +{ + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_prismaticJoint ); + return base->prismaticJoint.enableLimit; +} + +float b3PrismaticJoint_GetLowerLimit( b3JointId jointId ) +{ + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_prismaticJoint ); + return base->prismaticJoint.lowerTranslation; +} + +float b3PrismaticJoint_GetUpperLimit( b3JointId jointId ) +{ + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_prismaticJoint ); + return base->prismaticJoint.upperTranslation; +} + +void b3PrismaticJoint_SetLimits( b3JointId jointId, float lower, float upper ) +{ + B3_ASSERT( b3IsValidFloat( lower ) && b3IsValidFloat( upper ) ); + b3World* world = b3GetWorld( jointId.world0 ); + B3_REC( world, PrismaticJointSetLimits, jointId, lower, upper ); + float lowerAngle = b3MinFloat( lower, upper ); + float upperAngle = b3MaxFloat( lower, upper ); + + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_prismaticJoint ); + base->prismaticJoint.lowerTranslation = lowerAngle; + base->prismaticJoint.upperTranslation = upperAngle; +} + +float b3PrismaticJoint_GetTranslation( b3JointId jointId ) +{ + b3World* world = b3GetWorld( jointId.world0 ); + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_prismaticJoint ); + b3WorldTransform transformA = b3GetBodyTransform( world, base->bodyIdA ); + b3WorldTransform transformB = b3GetBodyTransform( world, base->bodyIdB ); + + b3Vec3 jointAxis = b3RotateVector( base->localFrameA.q, b3Vec3_axisX ); + jointAxis = b3RotateVector( transformA.q, jointAxis ); + + b3Vec3 anchorA = b3RotateVector( transformA.q, base->localFrameA.p ); + b3Vec3 anchorB = b3RotateVector( transformB.q, base->localFrameB.p ); + b3Vec3 d = b3Add( b3SubPos( transformB.p, transformA.p ), b3Sub( anchorB, anchorA ) ); + float translation = b3Dot( d, jointAxis ); + return translation; +} + +void b3PrismaticJoint_EnableSpring( b3JointId jointId, bool enableSpring ) +{ + b3World* world = b3GetWorld( jointId.world0 ); + B3_REC( world, PrismaticJointEnableSpring, jointId, enableSpring ); + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_prismaticJoint ); + if ( enableSpring != base->prismaticJoint.enableSpring ) + { + base->prismaticJoint.springImpulse = 0.0f; + } + base->prismaticJoint.enableSpring = enableSpring; +} + +bool b3PrismaticJoint_IsSpringEnabled( b3JointId jointId ) +{ + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_prismaticJoint ); + return base->prismaticJoint.enableSpring; +} + +void b3PrismaticJoint_SetTargetTranslation( b3JointId jointId, float targetTranslation ) +{ + B3_ASSERT( b3IsValidFloat( targetTranslation ) ); + b3World* world = b3GetWorld( jointId.world0 ); + B3_REC( world, PrismaticJointSetTargetTranslation, jointId, targetTranslation ); + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_prismaticJoint ); + base->prismaticJoint.targetTranslation = targetTranslation; +} + +float b3PrismaticJoint_GetTargetTranslation( b3JointId jointId ) +{ + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_prismaticJoint ); + return base->prismaticJoint.targetTranslation; +} + +void b3PrismaticJoint_SetSpringHertz( b3JointId jointId, float hertz ) +{ + B3_ASSERT( b3IsValidFloat( hertz ) && hertz >= 0.0f ); + b3World* world = b3GetWorld( jointId.world0 ); + B3_REC( world, PrismaticJointSetSpringHertz, jointId, hertz ); + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_prismaticJoint ); + base->prismaticJoint.hertz = hertz; +} + +float b3PrismaticJoint_GetSpringHertz( b3JointId jointId ) +{ + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_prismaticJoint ); + return base->prismaticJoint.hertz; +} + +void b3PrismaticJoint_SetSpringDampingRatio( b3JointId jointId, float dampingRatio ) +{ + B3_ASSERT( b3IsValidFloat( dampingRatio ) && dampingRatio >= 0.0f ); + b3World* world = b3GetWorld( jointId.world0 ); + B3_REC( world, PrismaticJointSetSpringDampingRatio, jointId, dampingRatio ); + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_prismaticJoint ); + base->prismaticJoint.dampingRatio = dampingRatio; +} + +float b3PrismaticJoint_GetSpringDampingRatio( b3JointId jointId ) +{ + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_prismaticJoint ); + return base->prismaticJoint.dampingRatio; +} + +void b3PrismaticJoint_EnableMotor( b3JointId jointId, bool enableMotor ) +{ + b3World* world = b3GetWorld( jointId.world0 ); + B3_REC( world, PrismaticJointEnableMotor, jointId, enableMotor ); + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_prismaticJoint ); + if ( enableMotor != base->prismaticJoint.enableMotor ) + { + base->prismaticJoint.motorImpulse = 0.0f; + } + base->prismaticJoint.enableMotor = enableMotor; +} + +bool b3PrismaticJoint_IsMotorEnabled( b3JointId jointId ) +{ + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_prismaticJoint ); + return base->prismaticJoint.enableMotor; +} + +void b3PrismaticJoint_SetMotorSpeed( b3JointId jointId, float motorSpeed ) +{ + B3_ASSERT( b3IsValidFloat( motorSpeed ) ); + b3World* world = b3GetWorld( jointId.world0 ); + B3_REC( world, PrismaticJointSetMotorSpeed, jointId, motorSpeed ); + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_prismaticJoint ); + base->prismaticJoint.motorSpeed = motorSpeed; +} + +float b3PrismaticJoint_GetMotorSpeed( b3JointId jointId ) +{ + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_prismaticJoint ); + return base->prismaticJoint.motorSpeed; +} + +void b3PrismaticJoint_SetMaxMotorForce( b3JointId jointId, float maxForce ) +{ + B3_ASSERT( b3IsValidFloat( maxForce ) && maxForce >= 0.0f ); + b3World* world = b3GetWorld( jointId.world0 ); + B3_REC( world, PrismaticJointSetMaxMotorForce, jointId, maxForce ); + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_prismaticJoint ); + base->prismaticJoint.maxMotorForce = maxForce; +} + +float b3PrismaticJoint_GetMaxMotorForce( b3JointId jointId ) +{ + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_prismaticJoint ); + return base->prismaticJoint.maxMotorForce; +} + +float b3PrismaticJoint_GetMotorForce( b3JointId jointId ) +{ + b3World* world = b3GetWorld( jointId.world0 ); + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_prismaticJoint ); + return world->inv_h * base->prismaticJoint.motorImpulse; +} + +float b3PrismaticJoint_GetSpeed( b3JointId jointId ) +{ + b3World* world = b3GetWorld( jointId.world0 ); + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_prismaticJoint ); + + b3Body* bodyA = b3Array_Get( world->bodies, base->bodyIdA ); + b3Body* bodyB = b3Array_Get( world->bodies, base->bodyIdB ); + b3BodySim* bodySimA = b3GetBodySim( world, bodyA ); + b3BodySim* bodySimB = b3GetBodySim( world, bodyB ); + b3BodyState* stateA = b3GetBodyState( world, bodyA ); + b3BodyState* stateB = b3GetBodyState( world, bodyB ); + + b3Quat qA = bodySimA->transform.q; + b3Quat qB = bodySimB->transform.q; + + b3Vec3 axisA = b3RotateVector( qA, b3RotateVector( base->localFrameA.q, b3Vec3_axisX ) ); + b3Vec3 rA = b3RotateVector( qA, b3Sub( base->localFrameA.p, bodySimA->localCenter ) ); + b3Vec3 rB = b3RotateVector( qB, b3Sub( base->localFrameB.p, bodySimB->localCenter ) ); + + // Difference the centers in double so the speed stays exact far from the origin. + b3Vec3 d = b3Add( b3SubPos( bodySimB->center, bodySimA->center ), b3Sub( rB, rA ) ); + + b3Vec3 vA = stateA ? stateA->linearVelocity : b3Vec3_zero; + b3Vec3 vB = stateB ? stateB->linearVelocity : b3Vec3_zero; + b3Vec3 wA = stateA ? stateA->angularVelocity : b3Vec3_zero; + b3Vec3 wB = stateB ? stateB->angularVelocity : b3Vec3_zero; + + b3Vec3 vRel = b3Sub( b3Add( vB, b3Cross( wB, rB ) ), b3Add( vA, b3Cross( wA, rA ) ) ); + + // The axis moves with body A, so account for its rotation. + float speed = b3Dot( d, b3Cross( wA, axisA ) ) + b3Dot( axisA, vRel ); + return speed; +} + +b3Vec3 b3GetPrismaticJointForce( b3World* world, b3JointSim* base ) +{ + b3WorldTransform transformA = b3GetBodyTransform( world, base->bodyIdA ); + b3PrismaticJoint* joint = &base->prismaticJoint; + + // impulse in joint space + b3Vec3 impulse = { + joint->perpImpulse.x, + joint->perpImpulse.y, + joint->motorImpulse + joint->lowerImpulse + joint->upperImpulse + joint->springImpulse, + }; + + // convert impulse to force + b3Vec3 force = b3MulSV( world->inv_h, impulse ); + + // convert to body space + force = b3RotateVector( base->localFrameA.q, force ); + + // convert to world space + force = b3RotateVector( transformA.q, force ); + return force; +} + +b3Vec3 b3GetPrismaticJointTorque( b3World* world, b3JointSim* base ) +{ + b3WorldTransform transformA = b3GetBodyTransform( world, base->bodyIdA ); + b3PrismaticJoint* joint = &base->prismaticJoint; + + b3Vec3 torque = b3MulSV( world->inv_h, joint->angularImpulse ); + torque = b3RotateVector( base->localFrameA.q, torque ); + torque = b3RotateVector( transformA.q, torque ); + return torque; +} + +void b3PreparePrismaticJoint( b3JointSim* base, b3StepContext* context ) +{ + B3_ASSERT( base->type == b3_prismaticJoint ); + + b3World* world = context->world; + + b3Body* bodyA = b3Array_Get( world->bodies, base->bodyIdA ); + b3Body* bodyB = b3Array_Get( world->bodies, base->bodyIdB ); + + B3_ASSERT( bodyB->setIndex == b3_awakeSet ); + b3SolverSet* setA = b3Array_Get( world->solverSets, bodyA->setIndex ); + b3SolverSet* setB = b3Array_Get( world->solverSets, bodyB->setIndex ); + + int localIndexA = bodyA->localIndex; + int localIndexB = bodyB->localIndex; + + b3BodySim* bodySimA = b3Array_Get( setA->bodySims, localIndexA ); + b3BodySim* bodySimB = b3Array_Get( setB->bodySims, localIndexB ); + + base->invMassA = bodySimA->invMass; + base->invMassB = bodySimB->invMass; + base->invIA = bodySimA->invInertiaWorld; + base->invIB = bodySimB->invInertiaWorld; + + b3Matrix3 invInertiaSum = b3AddMM( base->invIA, base->invIB ); + base->fixedRotation = b3Det( invInertiaSum ) < 1000.0f * FLT_MIN; + + b3PrismaticJoint* joint = &base->prismaticJoint; + joint->indexA = bodyA->setIndex == b3_awakeSet ? localIndexA : B3_NULL_INDEX; + joint->indexB = bodyB->setIndex == b3_awakeSet ? localIndexB : B3_NULL_INDEX; + + // Compute joint anchor frames with world space rotation, relative to center of mass + joint->frameA.q = b3MulQuat( bodySimA->transform.q, base->localFrameA.q ); + joint->frameA.p = b3RotateVector( bodySimA->transform.q, b3Sub( base->localFrameA.p, bodySimA->localCenter ) ); + joint->frameB.q = b3MulQuat( bodySimB->transform.q, base->localFrameB.q ); + joint->frameB.p = b3RotateVector( bodySimB->transform.q, b3Sub( base->localFrameB.p, bodySimB->localCenter ) ); + + joint->deltaCenter = b3SubPos( bodySimB->center, bodySimA->center ); + joint->rotationMass = b3InvertMatrix( invInertiaSum ); + + // Initial joint axes in world space + b3Matrix3 matrixA = b3MakeMatrixFromQuat( joint->frameA.q ); + joint->jointAxis = matrixA.cx; + joint->perpAxisY = matrixA.cy; + joint->perpAxisZ = matrixA.cz; + + joint->springSoftness = b3MakeSoft( joint->hertz, joint->dampingRatio, context->h ); + + if ( context->enableWarmStarting == false ) + { + joint->perpImpulse = (b3Vec2){ 0.0f, 0.0f }; + joint->angularImpulse = (b3Vec3){ 0.0f, 0.0f, 0.0f }; + joint->motorImpulse = 0.0f; + joint->springImpulse = 0.0f; + joint->lowerImpulse = 0.0f; + joint->upperImpulse = 0.0f; + } +} + +void b3WarmStartPrismaticJoint( b3JointSim* base, b3StepContext* context ) +{ + B3_ASSERT( base->type == b3_prismaticJoint ); + + float mA = base->invMassA; + float mB = base->invMassB; + b3Matrix3 iA = base->invIA; + b3Matrix3 iB = base->invIB; + + // dummy state for static bodies + b3BodyState dummyState = b3_identityBodyState; + + b3PrismaticJoint* joint = &base->prismaticJoint; + + b3BodyState* stateA = joint->indexA == B3_NULL_INDEX ? &dummyState : context->states + joint->indexA; + b3BodyState* stateB = joint->indexB == B3_NULL_INDEX ? &dummyState : context->states + joint->indexB; + + // todo make this code and the wheel joint more similar + + b3Vec3 rA = b3RotateVector( stateA->deltaRotation, joint->frameA.p ); + b3Vec3 rB = b3RotateVector( stateB->deltaRotation, joint->frameB.p ); + b3Vec3 d = b3Add( b3Add( b3Sub( stateB->deltaPosition, stateA->deltaPosition ), joint->deltaCenter ), b3Sub( rB, rA ) ); + b3Vec3 jointAxis = b3RotateVector( stateA->deltaRotation, joint->jointAxis ); + b3Vec3 sAx = b3Cross( b3Add( rA, d ), jointAxis ); + b3Vec3 sBx = b3Cross( rB, jointAxis ); + + b3Vec3 perpY = b3RotateVector( stateA->deltaRotation, joint->perpAxisY ); + b3Vec3 perpZ = b3RotateVector( stateA->deltaRotation, joint->perpAxisZ ); + b3Vec3 sAy = b3Cross( b3Add( rA, d ), perpY ); + b3Vec3 sBy = b3Cross( rB, perpY ); + b3Vec3 sAz = b3Cross( b3Add( rA, d ), perpZ ); + b3Vec3 sBz = b3Cross( rB, perpZ ); + + float axialImpulse = joint->springImpulse + joint->motorImpulse + joint->lowerImpulse - joint->upperImpulse; + b3Vec2 perpImpulse = joint->perpImpulse; + + b3Vec3 P = b3Blend3( axialImpulse, jointAxis, perpImpulse.x, perpY, perpImpulse.y, perpZ ); + b3Vec3 LA = b3Add( b3Blend3( axialImpulse, sAx, perpImpulse.x, sAy, perpImpulse.y, sAz ), joint->angularImpulse ); + b3Vec3 LB = b3Add( b3Blend3( axialImpulse, sBx, perpImpulse.x, sBy, perpImpulse.y, sBz ), joint->angularImpulse ); + + b3Vec3 vA = stateA->linearVelocity; + b3Vec3 wA = stateA->angularVelocity; + b3Vec3 vB = stateB->linearVelocity; + b3Vec3 wB = stateB->angularVelocity; + vA = b3MulSub( vA, mA, P ); + wA = b3Sub( wA, b3MulMV( iA, LA ) ); + vB = b3MulAdd( vB, mB, P ); + wB = b3Add( wB, b3MulMV( iB, LB ) ); + + if ( stateA->flags & b3_dynamicFlag ) + { + stateA->linearVelocity = vA; + stateA->angularVelocity = wA; + } + + if ( stateB->flags & b3_dynamicFlag ) + { + stateB->linearVelocity = vB; + stateB->angularVelocity = wB; + } +} + +void b3SolvePrismaticJoint( b3JointSim* base, b3StepContext* context, bool useBias ) +{ + float mA = base->invMassA; + float mB = base->invMassB; + b3Matrix3 iA = base->invIA; + b3Matrix3 iB = base->invIB; + + // dummy state for static bodies + b3BodyState dummyState = b3_identityBodyState; + + b3PrismaticJoint* joint = &base->prismaticJoint; + b3BodyState* stateA = joint->indexA == B3_NULL_INDEX ? &dummyState : context->states + joint->indexA; + b3BodyState* stateB = joint->indexB == B3_NULL_INDEX ? &dummyState : context->states + joint->indexB; + + b3Vec3 vA = stateA->linearVelocity; + b3Vec3 wA = stateA->angularVelocity; + b3Vec3 vB = stateB->linearVelocity; + b3Vec3 wB = stateB->angularVelocity; + + bool fixedRotation = base->fixedRotation; + b3Vec3 rA = b3RotateVector( stateA->deltaRotation, joint->frameA.p ); + b3Vec3 rB = b3RotateVector( stateB->deltaRotation, joint->frameB.p ); + + b3Vec3 dcA = stateA->deltaPosition; + b3Vec3 dcB = stateB->deltaPosition; + b3Vec3 d = b3Add( b3Add( b3Sub( dcB, dcA ), joint->deltaCenter ), b3Sub( rB, rA ) ); + + b3Vec3 jointAxis = b3RotateVector( stateA->deltaRotation, joint->jointAxis ); + b3Vec3 sAx = b3Cross( b3Add( rA, d ), jointAxis ); + b3Vec3 sBx = b3Cross( rB, jointAxis ); + float jointTranslation = b3Dot( d, jointAxis ); + float targetTranslation = joint->targetTranslation; + + // The axial effective mass must be fresh to avoid divergence when the joint is stressed + float ka = mA + mB + b3Dot( sAx, b3MulMV( iA, sAx ) ) + b3Dot( sBx, b3MulMV( iB, sBx ) ); + float axialMass = ka > 0.0f ? 1.0f / ka : 0.0f; + + // Solve spring + if ( joint->enableSpring && fixedRotation == false ) + { + // Get the substep relative rotation + float c = jointTranslation - targetTranslation; + + float bias = joint->springSoftness.biasRate * c; + float massScale = joint->springSoftness.massScale; + float impulseScale = joint->springSoftness.impulseScale; + + b3Vec3 vRel = b3Sub( b3Sub( b3Add( vB, b3Cross( wB, rB ) ), vA ), b3Cross( wA, b3Add( rA, d ) ) ); + float cdot = b3Dot( vRel, jointAxis ); + float deltaImpulse = -massScale * axialMass * ( cdot + bias ) - impulseScale * joint->springImpulse; + joint->springImpulse += deltaImpulse; + + b3Vec3 P = b3MulSV( deltaImpulse, jointAxis ); + b3Vec3 LA = b3MulSV( deltaImpulse, sAx ); + b3Vec3 LB = b3MulSV( deltaImpulse, sBx ); + + vA = b3MulSub( vA, mA, P ); + wA = b3Sub( wA, b3MulMV( iA, LA ) ); + vB = b3MulAdd( vB, mB, P ); + wB = b3Add( wB, b3MulMV( iB, LB ) ); + } + + if ( joint->enableMotor && fixedRotation == false ) + { + b3Vec3 vRel = b3Sub( b3Sub( b3Add( vB, b3Cross( wB, rB ) ), vA ), b3Cross( wA, b3Add( rA, d ) ) ); + float cdot = b3Dot( vRel, jointAxis ) - joint->motorSpeed; + + float deltaImpulse = -axialMass * cdot; + float newImpulse = joint->motorImpulse + deltaImpulse; + float maxImpulse = joint->maxMotorForce * context->h; + newImpulse = b3ClampFloat( newImpulse, -maxImpulse, maxImpulse ); + deltaImpulse = newImpulse - joint->motorImpulse; + joint->motorImpulse = newImpulse; + + b3Vec3 P = b3MulSV( deltaImpulse, jointAxis ); + b3Vec3 LA = b3MulSV( deltaImpulse, sAx ); + b3Vec3 LB = b3MulSV( deltaImpulse, sBx ); + + vA = b3MulSub( vA, mA, P ); + wA = b3Sub( wA, b3MulMV( iA, LA ) ); + vB = b3MulAdd( vB, mB, P ); + wB = b3Add( wB, b3MulMV( iB, LB ) ); + } + + if ( joint->enableLimit && fixedRotation == false ) + { + float speculativeDistance = 0.25f * ( joint->upperTranslation - joint->lowerTranslation ); + + // Lower limit + { + float C = jointTranslation - joint->lowerTranslation; + + if ( C < speculativeDistance ) + { + float bias = 0.0f; + float massScale = 1.0f; + float impulseScale = 0.0f; + if ( C > 0.0f ) + { + // speculation + bias = C * context->inv_h; + } + else if ( useBias ) + { + bias = base->constraintSoftness.biasRate * C; + massScale = base->constraintSoftness.massScale; + impulseScale = base->constraintSoftness.impulseScale; + } + + b3Vec3 vRel = b3Sub( b3Sub( b3Add( vB, b3Cross( wB, rB ) ), vA ), b3Cross( wA, b3Add( rA, d ) ) ); + float cdot = b3Dot( vRel, jointAxis ); + float oldImpulse = joint->lowerImpulse; + float deltaImpulse = -massScale * axialMass * ( cdot + bias ) - impulseScale * oldImpulse; + joint->lowerImpulse = b3MaxFloat( oldImpulse + deltaImpulse, 0.0f ); + deltaImpulse = joint->lowerImpulse - oldImpulse; + + b3Vec3 P = b3MulSV( deltaImpulse, jointAxis ); + b3Vec3 LA = b3MulSV( deltaImpulse, sAx ); + b3Vec3 LB = b3MulSV( deltaImpulse, sBx ); + + vA = b3MulSub( vA, mA, P ); + wA = b3Sub( wA, b3MulMV( iA, LA ) ); + vB = b3MulAdd( vB, mB, P ); + wB = b3Add( wB, b3MulMV( iB, LB ) ); + } + else + { + joint->lowerImpulse = 0.0f; + } + } + + // Upper limit + { + float C = joint->upperTranslation - jointTranslation; + + if ( C < speculativeDistance ) + { + float bias = 0.0f; + float massScale = 1.0f; + float impulseScale = 0.0f; + if ( C > 0.0f ) + { + // speculation + bias = C * context->inv_h; + } + else if ( useBias ) + { + bias = base->constraintSoftness.biasRate * C; + massScale = base->constraintSoftness.massScale; + impulseScale = base->constraintSoftness.impulseScale; + } + + // sign flipped on Cdot + b3Vec3 vRel = b3Sub( b3Sub( b3Add( vB, b3Cross( wB, rB ) ), vA ), b3Cross( wA, b3Add( rA, d ) ) ); + float cdot = -b3Dot( vRel, jointAxis ); + float oldImpulse = joint->upperImpulse; + float deltaImpulse = -massScale * axialMass * ( cdot + bias ) - impulseScale * oldImpulse; + joint->upperImpulse = b3MaxFloat( oldImpulse + deltaImpulse, 0.0f ); + + // sign flipped on applied impulse + float negDeltaImpulse = oldImpulse - joint->upperImpulse; + b3Vec3 P = b3MulSV( negDeltaImpulse, jointAxis ); + b3Vec3 LA = b3MulSV( negDeltaImpulse, sAx ); + b3Vec3 LB = b3MulSV( negDeltaImpulse, sBx ); + + vA = b3MulSub( vA, mA, P ); + wA = b3Sub( wA, b3MulMV( iA, LA ) ); + vB = b3MulAdd( vB, mB, P ); + wB = b3Add( wB, b3MulMV( iB, LB ) ); + } + else + { + joint->upperImpulse = 0.0f; + } + } + } + + // Rotation constraint + if ( fixedRotation == false ) + { + b3Vec3 bias = { 0.0f, 0.0f, 0.0f }; + float massScale = 1.0f; + float impulseScale = 0.0f; + + if ( useBias ) + { + b3Quat quatA = b3MulQuat( stateA->deltaRotation, joint->frameA.q ); + b3Quat quatB = b3MulQuat( stateB->deltaRotation, joint->frameB.q ); + + b3Quat relQ = b3InvMulQuat( quatA, quatB ); + b3Quat targetQuat = b3Quat_identity; + b3Vec3 deltaRotation = b3DeltaQuatToRotation( relQ, targetQuat ); + b3Vec3 c = b3Neg( b3RotateVector( quatA, deltaRotation ) ); + + bias = b3MulSV( base->constraintSoftness.biasRate, c ); + massScale = base->constraintSoftness.massScale; + impulseScale = base->constraintSoftness.impulseScale; + } + + b3Vec3 cdot = b3Sub( wB, wA ); + b3Vec3 impulse = b3Sub( + b3MulSV( -massScale, b3MulMV( joint->rotationMass, b3Add( cdot, bias ) ) ), + b3MulSV( impulseScale, joint->angularImpulse ) ); + joint->angularImpulse = b3Add( joint->angularImpulse, impulse ); + + wA = b3Sub( wA, b3MulMV( iA, impulse ) ); + wB = b3Add( wB, b3MulMV( iB, impulse ) ); + } + + // Solve point-to-line constraint + { + b3Vec3 perpY = b3RotateVector( stateA->deltaRotation, joint->perpAxisY ); + b3Vec3 perpZ = b3RotateVector( stateA->deltaRotation, joint->perpAxisZ ); + + b3Vec2 bias = { 0.0f, 0.0f }; + float massScale = 1.0f; + float impulseScale = 0.0f; + if ( useBias ) + { + b3Vec2 c = { b3Dot( perpY, d ), b3Dot( perpZ, d ) }; + bias = b3MulSV2( base->constraintSoftness.biasRate, c ); + massScale = base->constraintSoftness.massScale; + impulseScale = base->constraintSoftness.impulseScale; + } + + b3Vec3 vRel = b3Sub( b3Sub( b3Add( vB, b3Cross( wB, rB ) ), vA ), b3Cross( wA, b3Add( rA, d ) ) ); + b3Vec2 cdot = { b3Dot( perpY, vRel ), b3Dot( perpZ, vRel ) }; + + // K = [(1/mA + 1/mB) * eye(2) - skew(rA) * invIA * skew(rA) - skew(rB) * invIB * skew(rB)] + // Jx = [-perpX, -cross(d + rA, perpX), perpX, cross(rB, perpX)] + b3Vec3 sAy = b3Cross( b3Add( rA, d ), perpY ); + b3Vec3 sBy = b3Cross( rB, perpY ); + b3Vec3 sAz = b3Cross( b3Add( rA, d ), perpZ ); + b3Vec3 sBz = b3Cross( rB, perpZ ); + + float kyy = mA + mB + b3Dot( sAy, b3MulMV( iA, sAy ) ) + b3Dot( sBy, b3MulMV( iB, sBy ) ); + float kyz = b3Dot( sAy, b3MulMV( iA, sAz ) ) + b3Dot( sBy, b3MulMV( iB, sBz ) ); + float kzz = mA + mB + b3Dot( sAz, b3MulMV( iA, sAz ) ) + b3Dot( sBz, b3MulMV( iB, sBz ) ); + + b3Matrix2 K = { { kyy, kyz }, { kyz, kzz } }; + + b3Vec2 oldImpulse = joint->perpImpulse; + b3Vec2 sol = b3Solve2( K, b3Add2( cdot, bias ) ); + b3Vec2 deltaImpulse = b3Sub2( b3MulSV2( -massScale, sol ), b3MulSV2( impulseScale, oldImpulse ) ); + joint->perpImpulse = b3Add2( oldImpulse, deltaImpulse ); + + b3Vec3 P = b3Blend2( deltaImpulse.x, perpY, deltaImpulse.y, perpZ ); + + vA = b3MulSub( vA, mA, P ); + wA = b3Sub( wA, b3MulMV( iA, b3Blend2( deltaImpulse.x, sAy, deltaImpulse.y, sAz ) ) ); + vB = b3MulAdd( vB, mB, P ); + wB = b3Add( wB, b3MulMV( iB, b3Blend2( deltaImpulse.x, sBy, deltaImpulse.y, sBz ) ) ); + } + + B3_ASSERT( b3IsValidVec3( vA ) ); + B3_ASSERT( b3IsValidVec3( wA ) ); + B3_ASSERT( b3IsValidVec3( vB ) ); + B3_ASSERT( b3IsValidVec3( wB ) ); + + if ( stateA->flags & b3_dynamicFlag ) + { + stateA->linearVelocity = vA; + stateA->angularVelocity = wA; + } + + if ( stateB->flags & b3_dynamicFlag ) + { + stateB->linearVelocity = vB; + stateB->angularVelocity = wB; + } +} + +void b3DrawPrismaticJoint( b3DebugDraw* draw, b3JointSim* base, b3WorldTransform transformA, b3WorldTransform transformB, float scale ) +{ + b3WorldTransform frameA = b3MulWorldTransforms( transformA, base->localFrameA ); + b3WorldTransform frameB = b3MulWorldTransforms( transformB, base->localFrameB ); + + b3Matrix3 R = b3MakeMatrixFromQuat( frameA.q ); + b3Vec3 axis = R.cx; + b3Vec3 perpY = R.cy; + b3Vec3 perpZ = R.cz; + + float s = 0.2f * scale; + draw->DrawSegmentFcn( frameA.p, b3OffsetPos( frameA.p, b3MulSV( s, perpY ) ), b3_colorGreen, draw->context ); + draw->DrawSegmentFcn( frameA.p, b3OffsetPos( frameA.p, b3MulSV( s, perpZ ) ), b3_colorBlue, draw->context ); + + b3PrismaticJoint* joint = &base->prismaticJoint; + if ( joint->enableLimit ) + { + b3Pos p1 = b3OffsetPos( frameA.p, b3MulSV( joint->lowerTranslation, axis ) ); + b3Pos p2 = b3OffsetPos( frameA.p, b3MulSV( joint->upperTranslation, axis ) ); + draw->DrawSegmentFcn( p1, p2, b3_colorOrange, draw->context ); + draw->DrawPointFcn( p1, 10.0f, b3_colorGreen, draw->context ); + draw->DrawPointFcn( p2, 10.0f, b3_colorRed, draw->context ); + } + else + { + b3Pos p1 = b3OffsetPos( frameA.p, b3MulSV( -0.5f * scale, axis ) ); + b3Pos p2 = b3OffsetPos( frameA.p, b3MulSV( 0.5f * scale, axis ) ); + draw->DrawSegmentFcn( p1, p2, b3_colorOrange, draw->context ); + } + + draw->DrawPointFcn( frameB.p, 8.0f, b3_colorViolet, draw->context ); +} diff --git a/vendor/box3d/src/src/qsort.h b/vendor/box3d/src/src/qsort.h new file mode 100644 index 000000000..9ab426bac --- /dev/null +++ b/vendor/box3d/src/src/qsort.h @@ -0,0 +1,186 @@ +/* + * Copyright (c) 2013, 2017 Alexey Tourbin + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/* + * This is a traditional Quicksort implementation which mostly follows + * [Sedgewick 1978]. Sorting is performed entirely on array indices, + * while actual access to the array elements is abstracted out with the + * user-defined `LESS` and `SWAP` primitives. + * + * Synopsis: + * QSORT(N, LESS, SWAP); + * where + * N - the number of elements in A[]; + * LESS(i, j) - compares A[i] to A[j]; + * SWAP(i, j) - exchanges A[i] with A[j]. + */ + +#ifndef QSORT_H +#define QSORT_H + +/* Sort 3 elements. */ +#define Q_SORT3(q_a1, q_a2, q_a3, Q_LESS, Q_SWAP) \ +do { \ + if (Q_LESS(q_a2, q_a1)) { \ + if (Q_LESS(q_a3, q_a2)) \ + Q_SWAP(q_a1, q_a3); \ + else { \ + Q_SWAP(q_a1, q_a2); \ + if (Q_LESS(q_a3, q_a2)) \ + Q_SWAP(q_a2, q_a3); \ + } \ + } \ + else if (Q_LESS(q_a3, q_a2)) { \ + Q_SWAP(q_a2, q_a3); \ + if (Q_LESS(q_a2, q_a1)) \ + Q_SWAP(q_a1, q_a2); \ + } \ +} while (0) + +/* Partition [q_l,q_r] around a pivot. After partitioning, + * [q_l,q_j] are the elements that are less than or equal to the pivot, + * while [q_i,q_r] are the elements greater than or equal to the pivot. */ +#define Q_PARTITION(q_l, q_r, q_i, q_j, Q_UINT, Q_LESS, Q_SWAP) \ +do { \ + /* The middle element, not to be confused with the median. */ \ + Q_UINT q_m = q_l + ((q_r - q_l) >> 1); \ + /* Reorder the second, the middle, and the last items. \ + * As [Edelkamp Weiss 2016] explain, using the second element \ + * instead of the first one helps avoid bad behaviour for \ + * decreasingly sorted arrays. This method is used in recent \ + * versions of gcc's std::sort, see gcc bug 58437#c13, although \ + * the details are somewhat different (cf. #c14). */ \ + Q_SORT3(q_l + 1, q_m, q_r, Q_LESS, Q_SWAP); \ + /* Place the median at the beginning. */ \ + Q_SWAP(q_l, q_m); \ + /* Partition [q_l+2, q_r-1] around the median which is in q_l. \ + * q_i and q_j are initially off by one, they get decremented \ + * in the do-while loops. */ \ + q_i = q_l + 1; q_j = q_r; \ + while (1) { \ + do q_i++; while (Q_LESS(q_i, q_l)); \ + do q_j--; while (Q_LESS(q_l, q_j)); \ + if (q_i >= q_j) break; /* Sedgewick says "until j < i" */ \ + Q_SWAP(q_i, q_j); \ + } \ + /* Compensate for the i==j case. */ \ + q_i = q_j + 1; \ + /* Put the median to its final place. */ \ + Q_SWAP(q_l, q_j); \ + /* The median is not part of the left subfile. */ \ + q_j--; \ +} while (0) + +/* Insertion sort is applied to small subfiles - this is contrary to + * Sedgewick's suggestion to run a separate insertion sort pass after + * the partitioning is done. The reason I don't like a separate pass + * is that it triggers extra comparisons, because it can't see that the + * medians are already in their final positions and need not be rechecked. + * Since I do not assume that comparisons are cheap, I also do not try + * to eliminate the (q_j > q_l) boundary check. */ +#define Q_INSERTION_SORT(q_l, q_r, Q_UINT, Q_LESS, Q_SWAP) \ +do { \ + Q_UINT q_i, q_j; \ + /* For each item starting with the second... */ \ + for (q_i = q_l + 1; q_i <= q_r; q_i++) \ + /* move it down the array so that the first part is sorted. */ \ + for (q_j = q_i; q_j > q_l && (Q_LESS(q_j, q_j - 1)); q_j--) \ + Q_SWAP(q_j, q_j - 1); \ +} while (0) + +/* When the size of [q_l,q_r], i.e. q_r-q_l+1, is greater than or equal to + * Q_THRESH, the algorithm performs recursive partitioning. When the size + * drops below Q_THRESH, the algorithm switches to insertion sort. + * The minimum valid value is probably 5 (with 5 items, the second and + * the middle items, the middle itself being rounded down, are distinct). */ +#define Q_THRESH 16 + +/* The main loop. */ +#define Q_LOOP(Q_UINT, Q_N, Q_LESS, Q_SWAP) \ +do { \ + Q_UINT q_l = 0; \ + Q_UINT q_r = (Q_N) - 1; \ + Q_UINT q_sp = 0; /* the number of frames pushed to the stack */ \ + struct { Q_UINT q_l, q_r; } \ + /* On 32-bit platforms, to sort a "char[3GB+]" array, \ + * it may take full 32 stack frames. On 64-bit CPUs, \ + * though, the address space is limited to 48 bits. \ + * The usage is further reduced if Q_N has a 32-bit type. */ \ + q_st[sizeof(Q_UINT) > 4 && sizeof(Q_N) > 4 ? 48 : 32]; \ + while (1) { \ + if (q_r - q_l + 1 >= Q_THRESH) { \ + Q_UINT q_i, q_j; \ + Q_PARTITION(q_l, q_r, q_i, q_j, Q_UINT, Q_LESS, Q_SWAP); \ + /* Now have two subfiles: [q_l,q_j] and [q_i,q_r]. \ + * Dealing with them depends on which one is bigger. */ \ + if (q_j - q_l >= q_r - q_i) \ + Q_SUBFILES(q_l, q_j, q_i, q_r); \ + else \ + Q_SUBFILES(q_i, q_r, q_l, q_j); \ + } \ + else { \ + Q_INSERTION_SORT(q_l, q_r, Q_UINT, Q_LESS, Q_SWAP); \ + /* Pop subfiles from the stack, until it gets empty. */ \ + if (q_sp == 0) break; \ + q_sp--; \ + q_l = q_st[q_sp].q_l; \ + q_r = q_st[q_sp].q_r; \ + } \ + } \ +} while (0) + +/* The missing part: dealing with subfiles. + * Assumes that the first subfile is not smaller than the second. */ +#define Q_SUBFILES(q_l1, q_r1, q_l2, q_r2) \ +do { \ + /* If the second subfile is only a single element, it needs \ + * no further processing. The first subfile will be processed \ + * on the next iteration (both subfiles cannot be only a single \ + * element, due to Q_THRESH). */ \ + if (q_l2 == q_r2) { \ + q_l = q_l1; \ + q_r = q_r1; \ + } \ + else { \ + /* Otherwise, both subfiles need processing. \ + * Push the larger subfile onto the stack. */ \ + q_st[q_sp].q_l = q_l1; \ + q_st[q_sp].q_r = q_r1; \ + q_sp++; \ + /* Process the smaller subfile on the next iteration. */ \ + q_l = q_l2; \ + q_r = q_r2; \ + } \ +} while (0) + +/* And now, ladies and gentlemen, may I proudly present to you... */ +#define QSORT(Q_N, Q_LESS, Q_SWAP) \ +do { \ + if ((Q_N) > 1) \ + /* We could check sizeof(Q_N) and use "unsigned", but at least \ + * on x86_64, this has the performance penalty of up to 5%. */ \ + Q_LOOP(unsigned long, Q_N, Q_LESS, Q_SWAP); \ +} while (0) + +#endif + +/* ex:set ts=8 sts=4 sw=4 noet: */ diff --git a/vendor/box3d/src/src/recording.c b/vendor/box3d/src/src/recording.c new file mode 100644 index 000000000..4d5c5995d --- /dev/null +++ b/vendor/box3d/src/src/recording.c @@ -0,0 +1,1279 @@ +// SPDX-FileCopyrightText: 2026 Erin Catto +// SPDX-License-Identifier: MIT + +#if defined( _MSC_VER ) && !defined( _CRT_SECURE_NO_WARNINGS ) +#define _CRT_SECURE_NO_WARNINGS +#endif + +#include "recording.h" + +#include "body.h" +#include "compound.h" +#include "physics_world.h" +#include "world_snapshot.h" + +#include "box3d/box3d.h" +#include "box3d/constants.h" + +#include +#include + +// Buffer helpers + +void b3RecBufAppend( b3RecBuffer* buf, const void* data, int size ) +{ + if ( size <= 0 ) + { + return; + } + + if ( buf->countOnly ) + { + buf->size += size; + return; + } + + if ( buf->size + size > buf->capacity ) + { + int newCap = buf->capacity * 2; + if ( newCap < buf->size + size + 64 ) + { + newCap = buf->size + size + 64; + } + if ( buf->data == NULL ) + { + buf->data = b3Alloc( (size_t)newCap ); + } + else + { + buf->data = b3GrowAlloc( buf->data, buf->capacity, newCap ); + } + buf->capacity = newCap; + } + + memcpy( buf->data + buf->size, data, (size_t)size ); + buf->size += size; +} + +void b3RecBufFree( b3RecBuffer* buf ) +{ + if ( buf->data != NULL ) + { + b3Free( buf->data, (size_t)buf->capacity ); + buf->data = NULL; + buf->capacity = 0; + buf->size = 0; + } +} + +// Write primitives + +void b3RecW_U8( b3RecBuffer* buf, uint8_t v ) +{ + b3RecBufAppend( buf, &v, 1 ); +} + +void b3RecW_U16( b3RecBuffer* buf, uint16_t v ) +{ + uint8_t b[2] = { (uint8_t)v, (uint8_t)( v >> 8 ) }; + b3RecBufAppend( buf, b, 2 ); +} + +void b3RecW_U32( b3RecBuffer* buf, uint32_t v ) +{ + uint8_t b[4] = { (uint8_t)v, (uint8_t)( v >> 8 ), (uint8_t)( v >> 16 ), (uint8_t)( v >> 24 ) }; + b3RecBufAppend( buf, b, 4 ); +} + +void b3RecW_U64( b3RecBuffer* buf, uint64_t v ) +{ + uint8_t b[8] = { (uint8_t)v, (uint8_t)( v >> 8 ), (uint8_t)( v >> 16 ), (uint8_t)( v >> 24 ), + (uint8_t)( v >> 32 ), (uint8_t)( v >> 40 ), (uint8_t)( v >> 48 ), (uint8_t)( v >> 56 ) }; + b3RecBufAppend( buf, b, 8 ); +} + +void b3RecW_I32( b3RecBuffer* buf, int32_t v ) +{ + b3RecW_U32( buf, (uint32_t)v ); +} + +void b3RecW_F32( b3RecBuffer* buf, float v ) +{ + uint32_t bits; + memcpy( &bits, &v, 4 ); + b3RecW_U32( buf, bits ); +} + +void b3RecW_F64( b3RecBuffer* buf, double v ) +{ + uint64_t bits; + memcpy( &bits, &v, 8 ); + b3RecW_U64( buf, bits ); +} + +void b3RecW_BOOL( b3RecBuffer* buf, bool v ) +{ + b3RecW_U8( buf, v ? 1u : 0u ); +} + +void b3RecW_VEC3( b3RecBuffer* buf, b3Vec3 v ) +{ + b3RecW_F32( buf, v.x ); + b3RecW_F32( buf, v.y ); + b3RecW_F32( buf, v.z ); +} + +void b3RecW_QUAT( b3RecBuffer* buf, b3Quat v ) +{ + b3RecW_F32( buf, v.v.x ); + b3RecW_F32( buf, v.v.y ); + b3RecW_F32( buf, v.v.z ); + b3RecW_F32( buf, v.s ); +} + +void b3RecW_TRANSFORM( b3RecBuffer* buf, b3Transform v ) +{ + b3RecW_VEC3( buf, v.p ); + b3RecW_QUAT( buf, v.q ); +} + +// World position at full precision so recordings reproduce the simulation far from the origin. +// In the float build this is three floats, wire-identical to VEC3. +void b3RecW_POSITION( b3RecBuffer* buf, b3Pos v ) +{ +#if defined( BOX3D_DOUBLE_PRECISION ) + b3RecW_F64( buf, v.x ); + b3RecW_F64( buf, v.y ); + b3RecW_F64( buf, v.z ); +#else + b3RecW_F32( buf, v.x ); + b3RecW_F32( buf, v.y ); + b3RecW_F32( buf, v.z ); +#endif +} + +void b3RecW_WORLDXF( b3RecBuffer* buf, b3WorldTransform v ) +{ + b3RecW_POSITION( buf, v.p ); + b3RecW_QUAT( buf, v.q ); +} + +void b3RecW_MATRIX3( b3RecBuffer* buf, b3Matrix3 v ) +{ + b3RecW_VEC3( buf, v.cx ); + b3RecW_VEC3( buf, v.cy ); + b3RecW_VEC3( buf, v.cz ); +} + +void b3RecW_AABB( b3RecBuffer* buf, b3AABB v ) +{ + b3RecW_VEC3( buf, v.lowerBound ); + b3RecW_VEC3( buf, v.upperBound ); +} + +void b3RecW_QUERYFILTER( b3RecBuffer* buf, b3QueryFilter v ) +{ + b3RecW_U64( buf, v.categoryBits ); + b3RecW_U64( buf, v.maskBits ); +} + +// Variable length: count, then count points, then radius. The point cloud lives behind a pointer so +// it cannot ride along as POD. +void b3RecW_SHAPEPROXY( b3RecBuffer* buf, b3ShapeProxy v ) +{ + int count = v.count; + if ( count < 0 ) + count = 0; + if ( count > B3_MAX_SHAPE_CAST_POINTS ) + count = B3_MAX_SHAPE_CAST_POINTS; + b3RecW_I32( buf, count ); + for ( int i = 0; i < count; ++i ) + { + b3RecW_VEC3( buf, v.points[i] ); + } + b3RecW_F32( buf, v.radius ); +} + +void b3RecW_TREESTATS( b3RecBuffer* buf, b3TreeStats v ) +{ + b3RecW_I32( buf, v.nodeVisits ); + b3RecW_I32( buf, v.leafVisits ); +} + +void b3RecW_RAYRESULT( b3RecBuffer* buf, b3RayResult v ) +{ + b3RecW_SHAPEID( buf, v.shapeId ); + b3RecW_POSITION( buf, v.point ); + b3RecW_VEC3( buf, v.normal ); + b3RecW_U64( buf, v.userMaterialId ); + b3RecW_F32( buf, v.fraction ); + b3RecW_I32( buf, v.triangleIndex ); + b3RecW_I32( buf, v.childIndex ); + b3RecW_BOOL( buf, v.hit ); +} + +void b3RecW_PLANERESULT( b3RecBuffer* buf, b3PlaneResult v ) +{ + b3RecW_VEC3( buf, v.plane.normal ); + b3RecW_F32( buf, v.plane.offset ); + b3RecW_VEC3( buf, v.point ); +} + +void b3RecW_WORLDID( b3RecBuffer* buf, b3WorldId v ) +{ + b3RecW_U32( buf, b3StoreWorldId( v ) ); +} + +void b3RecW_BODYID( b3RecBuffer* buf, b3BodyId v ) +{ + b3RecW_U64( buf, b3StoreBodyId( v ) ); +} + +void b3RecW_SHAPEID( b3RecBuffer* buf, b3ShapeId v ) +{ + b3RecW_U64( buf, b3StoreShapeId( v ) ); +} + +void b3RecW_JOINTID( b3RecBuffer* buf, b3JointId v ) +{ + b3RecW_U64( buf, b3StoreJointId( v ) ); +} + +// Pointer-free POD; pointerWidth in the header gates the layout on replay +void b3RecW_SPHERE( b3RecBuffer* buf, b3Sphere v ) +{ + b3RecBufAppend( buf, &v, (int)sizeof( b3Sphere ) ); +} + +void b3RecW_CAPSULE( b3RecBuffer* buf, b3Capsule v ) +{ + b3RecBufAppend( buf, &v, (int)sizeof( b3Capsule ) ); +} + +void b3RecW_GEOMID( b3RecBuffer* buf, uint32_t v ) +{ + b3RecW_U32( buf, v ); +} + +void b3RecW_FILTER( b3RecBuffer* buf, b3Filter v ) +{ + b3RecW_U64( buf, v.categoryBits ); + b3RecW_U64( buf, v.maskBits ); + b3RecW_I32( buf, v.groupIndex ); +} + +void b3RecW_MATERIAL( b3RecBuffer* buf, b3SurfaceMaterial v ) +{ + b3RecW_F32( buf, v.friction ); + b3RecW_F32( buf, v.restitution ); + b3RecW_F32( buf, v.rollingResistance ); + b3RecW_VEC3( buf, v.tangentVelocity ); + b3RecW_U64( buf, v.userMaterialId ); + b3RecW_U32( buf, v.customColor ); +} + +void b3RecW_MASSDATA( b3RecBuffer* buf, b3MassData v ) +{ + b3RecW_F32( buf, v.mass ); + b3RecW_VEC3( buf, v.center ); + b3RecW_MATRIX3( buf, v.inertia ); +} + +void b3RecW_LOCKS( b3RecBuffer* buf, b3MotionLocks v ) +{ + b3RecW_BOOL( buf, v.linearX ); + b3RecW_BOOL( buf, v.linearY ); + b3RecW_BOOL( buf, v.linearZ ); + b3RecW_BOOL( buf, v.angularX ); + b3RecW_BOOL( buf, v.angularY ); + b3RecW_BOOL( buf, v.angularZ ); +} + +static void b3RecW_STR( b3RecBuffer* buf, const char* s ) +{ + if ( s == NULL ) + { + b3RecW_U16( buf, 0xFFFFu ); + return; + } + int len = 0; + while ( s[len] != '\0' && len < 65534 ) + { + len++; + } + b3RecW_U16( buf, (uint16_t)len ); + if ( len > 0 ) + { + b3RecBufAppend( buf, s, len ); + } +} + +// Hand-written def helpers. Zero pointer and cookie fields before serializing. +// Readers call b3Default*Def() first to get the cookie, then overwrite fields. + +// Tripwire: each def serializer below is paired with a reader in recording_replay.c, and the two must +// stay field-for-field in sync. Add a field to a def and the size changes, firing the matching assert +// so the writer and reader both get updated. Only enforced on the 64-bit target; each def lists the +// single-precision and double-precision sizes (equal for most), so either build configuration passes. +_Static_assert( sizeof( void* ) != 8 || sizeof( b3ExplosionDef ) == 32 || sizeof( b3ExplosionDef ) == 48, + "b3ExplosionDef changed: update b3RecW_EXPLOSIONDEF and b3RecR_EXPLOSIONDEF together" ); +_Static_assert( sizeof( void* ) != 8 || sizeof( b3BodyDef ) == 104 || sizeof( b3BodyDef ) == 120, + "b3BodyDef changed: update b3RecW_BODYDEF and b3RecR_BODYDEF together" ); +_Static_assert( sizeof( void* ) != 8 || sizeof( b3ShapeDef ) == 120, + "b3ShapeDef changed: update b3RecW_SHAPEDEF and b3RecR_SHAPEDEF together" ); +_Static_assert( sizeof( void* ) != 8 || sizeof( b3ParallelJointDef ) == 128, + "b3ParallelJointDef changed: update b3RecW_PARALLELJOINTDEF and its reader together" ); +_Static_assert( sizeof( void* ) != 8 || sizeof( b3DistanceJointDef ) == 160, + "b3DistanceJointDef changed: update b3RecW_DISTANCEJOINTDEF and its reader together" ); +_Static_assert( sizeof( void* ) != 8 || sizeof( b3FilterJointDef ) == 112, + "b3FilterJointDef changed: update b3RecW_FILTERJOINTDEF and its reader together" ); +_Static_assert( sizeof( void* ) != 8 || sizeof( b3MotorJointDef ) == 168, + "b3MotorJointDef changed: update b3RecW_MOTORJOINTDEF and its reader together" ); +_Static_assert( sizeof( void* ) != 8 || sizeof( b3PrismaticJointDef ) == 152, + "b3PrismaticJointDef changed: update b3RecW_PRISMATICJOINTDEF and its reader together" ); +_Static_assert( sizeof( void* ) != 8 || sizeof( b3RevoluteJointDef ) == 152, + "b3RevoluteJointDef changed: update b3RecW_REVOLUTEJOINTDEF and its reader together" ); +_Static_assert( sizeof( void* ) != 8 || sizeof( b3SphericalJointDef ) == 184, + "b3SphericalJointDef changed: update b3RecW_SPHERICALJOINTDEF and its reader together" ); +_Static_assert( sizeof( void* ) != 8 || sizeof( b3WeldJointDef ) == 128, + "b3WeldJointDef changed: update b3RecW_WELDJOINTDEF and its reader together" ); +_Static_assert( sizeof( void* ) != 8 || sizeof( b3WheelJointDef ) == 184, + "b3WheelJointDef changed: update b3RecW_WHEELJOINTDEF and its reader together" ); + +void b3RecW_EXPLOSIONDEF( b3RecBuffer* buf, b3ExplosionDef v ) +{ + b3RecW_U64( buf, v.maskBits ); + b3RecW_POSITION( buf, v.position ); + b3RecW_F32( buf, v.radius ); + b3RecW_F32( buf, v.falloff ); + b3RecW_F32( buf, v.impulsePerArea ); +} + +void b3RecW_BODYDEF( b3RecBuffer* buf, b3BodyDef v ) +{ + b3RecW_I32( buf, (int32_t)v.type ); + b3RecW_POSITION( buf, v.position ); + b3RecW_QUAT( buf, v.rotation ); + b3RecW_VEC3( buf, v.linearVelocity ); + b3RecW_VEC3( buf, v.angularVelocity ); + b3RecW_F32( buf, v.linearDamping ); + b3RecW_F32( buf, v.angularDamping ); + b3RecW_F32( buf, v.gravityScale ); + b3RecW_F32( buf, v.sleepThreshold ); + b3RecW_STR( buf, v.name ); + // userData: not preserved + b3RecW_U64( buf, 0u ); + b3RecW_LOCKS( buf, v.motionLocks ); + b3RecW_BOOL( buf, v.enableSleep ); + b3RecW_BOOL( buf, v.isAwake ); + b3RecW_BOOL( buf, v.isBullet ); + b3RecW_BOOL( buf, v.isEnabled ); + b3RecW_BOOL( buf, v.allowFastRotation ); + b3RecW_BOOL( buf, v.enableContactRecycling ); + // internalValue omitted +} + +void b3RecW_SHAPEDEF( b3RecBuffer* buf, b3ShapeDef v ) +{ + b3RecW_STR( buf, v.name ); + + // userData: not preserved + b3RecW_U64( buf, 0u ); + // Per-triangle materials: length-prefixed so the reader can rebuild the array. + // Guard NULL so a default def (materialCount=0, materials=NULL) round-trips cleanly. + int matCount = ( v.materials != NULL ) ? v.materialCount : 0; + b3RecW_I32( buf, matCount ); + for ( int i = 0; i < matCount; ++i ) + { + b3RecW_MATERIAL( buf, v.materials[i] ); + } + b3RecW_MATERIAL( buf, v.baseMaterial ); + b3RecW_F32( buf, v.density ); + b3RecW_F32( buf, v.explosionScale ); + b3RecW_FILTER( buf, v.filter ); + b3RecW_BOOL( buf, v.enableCustomFiltering ); + b3RecW_BOOL( buf, v.isSensor ); + b3RecW_BOOL( buf, v.enableSensorEvents ); + b3RecW_BOOL( buf, v.enableContactEvents ); + b3RecW_BOOL( buf, v.enableHitEvents ); + b3RecW_BOOL( buf, v.enablePreSolveEvents ); + b3RecW_BOOL( buf, v.invokeContactCreation ); + b3RecW_BOOL( buf, v.updateBodyMass ); + b3RecW_BOOL( buf, v.enableSpeculativeContact ); + // internalValue omitted +} + +// Joint defs share a base. Body ids are written as packed ids for replay remapping. +static void b3RecW_JointBase( b3RecBuffer* buf, const b3JointDef* base ) +{ + // userData: not preserved + b3RecW_U64( buf, 0u ); + b3RecW_BODYID( buf, base->bodyIdA ); + b3RecW_BODYID( buf, base->bodyIdB ); + b3RecW_TRANSFORM( buf, base->localFrameA ); + b3RecW_TRANSFORM( buf, base->localFrameB ); + b3RecW_F32( buf, base->forceThreshold ); + b3RecW_F32( buf, base->torqueThreshold ); + b3RecW_F32( buf, base->constraintHertz ); + b3RecW_F32( buf, base->constraintDampingRatio ); + b3RecW_F32( buf, base->drawScale ); + b3RecW_BOOL( buf, base->collideConnected ); + // internalValue omitted +} + +void b3RecW_PARALLELJOINTDEF( b3RecBuffer* buf, b3ParallelJointDef v ) +{ + b3RecW_JointBase( buf, &v.base ); + b3RecW_F32( buf, v.hertz ); + b3RecW_F32( buf, v.dampingRatio ); + b3RecW_F32( buf, v.maxTorque ); +} + +void b3RecW_DISTANCEJOINTDEF( b3RecBuffer* buf, b3DistanceJointDef v ) +{ + b3RecW_JointBase( buf, &v.base ); + b3RecW_F32( buf, v.length ); + b3RecW_BOOL( buf, v.enableSpring ); + b3RecW_F32( buf, v.lowerSpringForce ); + b3RecW_F32( buf, v.upperSpringForce ); + b3RecW_F32( buf, v.hertz ); + b3RecW_F32( buf, v.dampingRatio ); + b3RecW_BOOL( buf, v.enableLimit ); + b3RecW_F32( buf, v.minLength ); + b3RecW_F32( buf, v.maxLength ); + b3RecW_BOOL( buf, v.enableMotor ); + b3RecW_F32( buf, v.maxMotorForce ); + b3RecW_F32( buf, v.motorSpeed ); +} + +void b3RecW_FILTERJOINTDEF( b3RecBuffer* buf, b3FilterJointDef v ) +{ + b3RecW_JointBase( buf, &v.base ); +} + +void b3RecW_MOTORJOINTDEF( b3RecBuffer* buf, b3MotorJointDef v ) +{ + b3RecW_JointBase( buf, &v.base ); + b3RecW_VEC3( buf, v.linearVelocity ); + b3RecW_F32( buf, v.maxVelocityForce ); + b3RecW_VEC3( buf, v.angularVelocity ); + b3RecW_F32( buf, v.maxVelocityTorque ); + b3RecW_F32( buf, v.linearHertz ); + b3RecW_F32( buf, v.linearDampingRatio ); + b3RecW_F32( buf, v.maxSpringForce ); + b3RecW_F32( buf, v.angularHertz ); + b3RecW_F32( buf, v.angularDampingRatio ); + b3RecW_F32( buf, v.maxSpringTorque ); +} + +void b3RecW_PRISMATICJOINTDEF( b3RecBuffer* buf, b3PrismaticJointDef v ) +{ + b3RecW_JointBase( buf, &v.base ); + b3RecW_BOOL( buf, v.enableSpring ); + b3RecW_F32( buf, v.hertz ); + b3RecW_F32( buf, v.dampingRatio ); + b3RecW_F32( buf, v.targetTranslation ); + b3RecW_BOOL( buf, v.enableLimit ); + b3RecW_F32( buf, v.lowerTranslation ); + b3RecW_F32( buf, v.upperTranslation ); + b3RecW_BOOL( buf, v.enableMotor ); + b3RecW_F32( buf, v.maxMotorForce ); + b3RecW_F32( buf, v.motorSpeed ); +} + +void b3RecW_REVOLUTEJOINTDEF( b3RecBuffer* buf, b3RevoluteJointDef v ) +{ + b3RecW_JointBase( buf, &v.base ); + b3RecW_F32( buf, v.targetAngle ); + b3RecW_BOOL( buf, v.enableSpring ); + b3RecW_F32( buf, v.hertz ); + b3RecW_F32( buf, v.dampingRatio ); + b3RecW_BOOL( buf, v.enableLimit ); + b3RecW_F32( buf, v.lowerAngle ); + b3RecW_F32( buf, v.upperAngle ); + b3RecW_BOOL( buf, v.enableMotor ); + b3RecW_F32( buf, v.maxMotorTorque ); + b3RecW_F32( buf, v.motorSpeed ); +} + +void b3RecW_SPHERICALJOINTDEF( b3RecBuffer* buf, b3SphericalJointDef v ) +{ + b3RecW_JointBase( buf, &v.base ); + b3RecW_BOOL( buf, v.enableSpring ); + b3RecW_F32( buf, v.hertz ); + b3RecW_F32( buf, v.dampingRatio ); + b3RecW_QUAT( buf, v.targetRotation ); + b3RecW_BOOL( buf, v.enableConeLimit ); + b3RecW_F32( buf, v.coneAngle ); + b3RecW_BOOL( buf, v.enableTwistLimit ); + b3RecW_F32( buf, v.lowerTwistAngle ); + b3RecW_F32( buf, v.upperTwistAngle ); + b3RecW_BOOL( buf, v.enableMotor ); + b3RecW_F32( buf, v.maxMotorTorque ); + b3RecW_VEC3( buf, v.motorVelocity ); +} + +void b3RecW_WELDJOINTDEF( b3RecBuffer* buf, b3WeldJointDef v ) +{ + b3RecW_JointBase( buf, &v.base ); + b3RecW_F32( buf, v.linearHertz ); + b3RecW_F32( buf, v.angularHertz ); + b3RecW_F32( buf, v.linearDampingRatio ); + b3RecW_F32( buf, v.angularDampingRatio ); +} + +void b3RecW_WHEELJOINTDEF( b3RecBuffer* buf, b3WheelJointDef v ) +{ + b3RecW_JointBase( buf, &v.base ); + b3RecW_BOOL( buf, v.enableSuspensionSpring ); + b3RecW_F32( buf, v.suspensionHertz ); + b3RecW_F32( buf, v.suspensionDampingRatio ); + b3RecW_BOOL( buf, v.enableSuspensionLimit ); + b3RecW_F32( buf, v.lowerSuspensionLimit ); + b3RecW_F32( buf, v.upperSuspensionLimit ); + b3RecW_BOOL( buf, v.enableSpinMotor ); + b3RecW_F32( buf, v.maxSpinTorque ); + b3RecW_F32( buf, v.spinSpeed ); + b3RecW_BOOL( buf, v.enableSteering ); + b3RecW_F32( buf, v.steeringHertz ); + b3RecW_F32( buf, v.steeringDampingRatio ); + b3RecW_F32( buf, v.targetSteeringAngle ); + b3RecW_F32( buf, v.maxSteeringTorque ); + b3RecW_BOOL( buf, v.enableSteeringLimit ); + b3RecW_F32( buf, v.lowerSteeringLimit ); + b3RecW_F32( buf, v.upperSteeringLimit ); +} + +// Query recording. A query collects a variable number of hits through a user callback, so the count +// is not known until the callback stops firing. The record is built in a local buffer with a +// reserved hit-count slot, then committed whole under the lock so concurrent query threads never +// interleave records in the shared buffer. + +int b3RecReserveU32( b3RecBuffer* buf ) +{ + int offset = buf->size; + uint8_t zero[4] = { 0, 0, 0, 0 }; + b3RecBufAppend( buf, zero, 4 ); + return offset; +} + +void b3RecPatchU32( b3RecBuffer* buf, int offset, uint32_t v ) +{ + B3_ASSERT( offset >= 0 && offset + 4 <= buf->size ); + uint8_t* p = buf->data + offset; + p[0] = (uint8_t)v; + p[1] = (uint8_t)( v >> 8 ); + p[2] = (uint8_t)( v >> 16 ); + p[3] = (uint8_t)( v >> 24 ); +} + +// Frame and append one record into the buffer. Caller holds rec->lock. +static void b3RecCommitRecordLocked( b3Recording* rec, uint8_t opcode, const uint8_t* payload, int payloadSize ) +{ + B3_ASSERT( payloadSize >= 0 && payloadSize < ( 1 << 24 ) ); + b3RecW_U8( &rec->buffer, opcode ); + uint8_t sz[3] = { (uint8_t)payloadSize, (uint8_t)( payloadSize >> 8 ), (uint8_t)( payloadSize >> 16 ) }; + b3RecBufAppend( &rec->buffer, sz, 3 ); + b3RecBufAppend( &rec->buffer, payload, payloadSize ); +} + +void b3RecCommitRecord( b3Recording* rec, uint8_t opcode, const uint8_t* payload, int payloadSize ) +{ + b3LockMutex( rec->lock ); + b3RecCommitRecordLocked( rec, opcode, payload, payloadSize ); + b3UnlockMutex( rec->lock ); +} + +void b3RecQueryBegin( b3RecQueryWriter* w, void* context, uint64_t tagId, const char* tagName ) +{ + w->buf = (b3RecBuffer){ 0 }; + w->userFcn.overlapFcn = NULL; + w->userContext = context; + w->hitCount = 0; + w->countOffset = 0; + w->tagId = tagId; + w->tagName = tagName; +} + +void b3RecQueryCommit( b3Recording* rec, uint8_t opcode, b3RecQueryWriter* w ) +{ + b3LockMutex( rec->lock ); + // A tagged query writes its identity key right before the query record, under one lock so the pair + // stays adjacent even with concurrent queries. The key is the hash of the caller (id, name), which + // are interned once into the trailing tag table so the viewer can show them. + bool tagged = w->tagId != 0 || ( w->tagName != NULL && w->tagName[0] != '\0' ); + if ( tagged ) + { + uint64_t key = b3HashQueryTag( w->tagId, w->tagName ); + b3RecInternTag( rec, key, w->tagId, w->tagName ); + b3RecBuffer tagBuf = { 0 }; + b3RecW_U64( &tagBuf, key ); + b3RecCommitRecordLocked( rec, b3_recOpQueryTag, tagBuf.data, tagBuf.size ); + b3RecBufFree( &tagBuf ); + } + b3RecCommitRecordLocked( rec, opcode, w->buf.data, w->buf.size ); + b3UnlockMutex( rec->lock ); + b3RecBufFree( &w->buf ); +} + +bool b3RecOverlapTrampoline( b3ShapeId id, void* ctx ) +{ + b3RecQueryWriter* w = (b3RecQueryWriter*)ctx; + // The user fcn is NULL for an unfiltered mover cast: accept all, still record the decision so + // replay reproduces the same per-shape accept stream. + bool ret = w->userFcn.overlapFcn != NULL ? w->userFcn.overlapFcn( id, w->userContext ) : true; + b3RecW_SHAPEID( &w->buf, id ); + b3RecW_BOOL( &w->buf, ret ); + w->hitCount++; + return ret; +} + +float b3RecCastTrampoline( b3ShapeId id, b3Pos point, b3Vec3 normal, float fraction, uint64_t userMaterialId, int triangleIndex, + int childIndex, void* ctx ) +{ + b3RecQueryWriter* w = (b3RecQueryWriter*)ctx; + float ret = w->userFcn.castFcn( id, point, normal, fraction, userMaterialId, triangleIndex, childIndex, w->userContext ); + b3RecW_SHAPEID( &w->buf, id ); + b3RecW_POSITION( &w->buf, point ); + b3RecW_VEC3( &w->buf, normal ); + b3RecW_F32( &w->buf, fraction ); + b3RecW_U64( &w->buf, userMaterialId ); + b3RecW_I32( &w->buf, triangleIndex ); + b3RecW_I32( &w->buf, childIndex ); + b3RecW_F32( &w->buf, ret ); + w->hitCount++; + return ret; +} + +// 3D delivers every plane for one shape in a single call. Record the shape, its plane count, each +// plane, then the user return so replay reproduces the same per-shape batch. One hit per shape. +bool b3RecPlaneTrampoline( b3ShapeId id, const b3PlaneResult* planes, int planeCount, void* ctx ) +{ + b3RecQueryWriter* w = (b3RecQueryWriter*)ctx; + bool ret = w->userFcn.planeFcn( id, planes, planeCount, w->userContext ); + b3RecW_SHAPEID( &w->buf, id ); + b3RecW_I32( &w->buf, planeCount ); + for ( int i = 0; i < planeCount; ++i ) + { + b3RecW_PLANERESULT( &w->buf, planes[i] ); + } + b3RecW_BOOL( &w->buf, ret ); + w->hitCount++; + return ret; +} + +// Record framing + +void b3RecBeginRecord( b3Recording* rec, uint8_t opcode ) +{ + b3RecW_U8( &rec->buffer, opcode ); + rec->recordStart = rec->buffer.size; + // Reserve 3 bytes for the u24 payload size, backpatched in b3RecEndRecord. + uint8_t zero[3] = { 0, 0, 0 }; + b3RecBufAppend( &rec->buffer, zero, 3 ); +} + +void b3RecEndRecord( b3Recording* rec ) +{ + int payloadSize = rec->buffer.size - rec->recordStart - 3; + B3_ASSERT( payloadSize >= 0 && payloadSize < ( 1 << 24 ) ); + uint8_t* p = rec->buffer.data + rec->recordStart; + p[0] = (uint8_t)payloadSize; + p[1] = (uint8_t)( payloadSize >> 8 ); + p[2] = (uint8_t)( payloadSize >> 16 ); +} + +// Codegen pass 1b: arg writers +#define ARG( TAG, field ) b3RecW_##TAG( &rec->buffer, a->field ); +#define B3_REC_OP( op, Name, RET, ... ) \ + void b3RecWriteArgs_##Name( b3Recording* rec, const b3RecArgs_##Name* a ) \ + { \ + __VA_ARGS__ \ + } +#include "recording_ops.inl" +#undef B3_REC_OP +#undef ARG + +// Codegen: full writers. Setters may run on threads that each own a distinct object, +// so hold the lock across the whole record. Without it a concurrent writer splices its bytes between +// our begin and end and the record desyncs replay. Same lock the query commit path takes. +#define B3_REC_OP( op, Name, RET, ... ) \ + void b3RecWrite_##Name( b3Recording* rec, const b3RecArgs_##Name* a ) \ + { \ + b3LockMutex( rec->lock ); \ + b3RecBeginRecord( rec, (uint8_t)( op ) ); \ + b3RecWriteArgs_##Name( rec, a ); \ + b3RecEndRecord( rec ); \ + b3UnlockMutex( rec->lock ); \ + } +#include "recording_ops.inl" +#undef B3_REC_OP + +// Codegen: create-op writers that append the returned id inside the record +#define B3_REC_RETWRITE( op, Name, idType, idW ) \ + void b3RecWriteRet_##Name( b3Recording* rec, const b3RecArgs_##Name* a, idType id ) \ + { \ + b3LockMutex( rec->lock ); \ + b3RecBeginRecord( rec, (uint8_t)( op ) ); \ + b3RecWriteArgs_##Name( rec, a ); \ + idW( &rec->buffer, id ); \ + b3RecEndRecord( rec ); \ + b3UnlockMutex( rec->lock ); \ + } +#define B3_REC_RETWRITE_RET_NONE( op, Name ) +#define B3_REC_RETWRITE_RET_BODYID( op, Name ) B3_REC_RETWRITE( op, Name, b3BodyId, b3RecW_BODYID ) +#define B3_REC_RETWRITE_RET_SHAPEID( op, Name ) B3_REC_RETWRITE( op, Name, b3ShapeId, b3RecW_SHAPEID ) +#define B3_REC_RETWRITE_RET_JOINTID( op, Name ) B3_REC_RETWRITE( op, Name, b3JointId, b3RecW_JOINTID ) +#define B3_REC_OP( op, Name, RET, ... ) B3_REC_RETWRITE_##RET( op, Name ) +#include "recording_ops.inl" +#undef B3_REC_OP +#undef B3_REC_RETWRITE_RET_NONE +#undef B3_REC_RETWRITE_RET_BODYID +#undef B3_REC_RETWRITE_RET_SHAPEID +#undef B3_REC_RETWRITE_RET_JOINTID +#undef B3_REC_RETWRITE + +// Geometry registry + +// Full 64-bit content hash, so distinct blobs of the same length get independent bits. A reseeded +// 32-bit djb2 cannot: djb2 is affine in its seed, so a same-length collision survives every seed and +// the high word would just track the low one. Word folded for speed, byte order normalized on +// big-endian to match b3Hash, then a splitmix64 finalizer so tiny inputs still spread across all bits. +// From Fowler/Noll/Vo FNV-1a salted by length, then the splitmix64 mix. +uint64_t b3Hash64Blob( const uint8_t* bytes, int n ) +{ + uint64_t h = 0xcbf29ce484222325ull ^ (uint64_t)(uint32_t)n; + const uint64_t prime = 0x100000001b3ull; + int i = 0; + + while ( i + 8 <= n ) + { + uint64_t word; + memcpy( &word, bytes + i, sizeof( word ) ); +#if defined( __BYTE_ORDER__ ) && __BYTE_ORDER__ == __ORDER_BIG_ENDIAN__ + word = ( ( word & 0x00000000000000FFULL ) << 56 ) | ( ( word & 0x000000000000FF00ULL ) << 40 ) | + ( ( word & 0x0000000000FF0000ULL ) << 24 ) | ( ( word & 0x00000000FF000000ULL ) << 8 ) | + ( ( word & 0x000000FF00000000ULL ) >> 8 ) | ( ( word & 0x0000FF0000000000ULL ) >> 24 ) | + ( ( word & 0x00FF000000000000ULL ) >> 40 ) | ( ( word & 0xFF00000000000000ULL ) >> 56 ); +#endif + h = ( h ^ word ) * prime; + i += 8; + } + + while ( i < n ) + { + h = ( h ^ (uint64_t)bytes[i] ) * prime; + i += 1; + } + + h ^= h >> 30; + h *= 0xbf58476d1ce4e5b9ull; + h ^= h >> 27; + h *= 0x94d049bb133111ebull; + h ^= h >> 31; + return h; +} + +// Content hash to chain head, so dedup is near O(1). Colliding hashes share a head and are walked +// through b3GeometryEntry::hashNext, so the byteCount + memcmp check always finds an existing blob. +#define NAME b3GeometryHashMap +#define KEY_TY uint64_t +#define VAL_TY uint32_t +#define HASH_FN vt_hash_integer +#define CMPR_FN vt_cmpr_integer +#define MALLOC_FN b3Alloc +#define FREE_FN b3Free +#include "verstable.h" + +// Tag key to tag index, so interning a query tag is O(1) rather than a linear scan over the tag table. +#define NAME b3RecTagMap +#define KEY_TY uint64_t +#define VAL_TY uint32_t +#define HASH_FN vt_hash_integer +#define CMPR_FN vt_cmpr_integer +#define MALLOC_FN b3Alloc +#define FREE_FN b3Free +#include "verstable.h" + +// Append a fresh entry and splice it onto the front of its hash chain. The map value is the chain head. +static uint32_t b3RegistryPush( b3GeometryRegistry* reg, b3GeometryHashMap* map, b3GeometryHashMap_itr itr, bool hashPresent, + b3GeometryKind kind, uint64_t contentHash, uint8_t* bytes, int byteCount ) +{ + if ( reg->count >= reg->capacity ) + { + int newCap = reg->capacity < 8 ? 8 : reg->capacity * 2; + reg->entries = (b3GeometryEntry*)b3GrowAlloc( reg->entries, reg->capacity * (int)sizeof( b3GeometryEntry ), + newCap * (int)sizeof( b3GeometryEntry ) ); + reg->capacity = newCap; + } + + uint32_t id = (uint32_t)reg->count; + b3GeometryEntry* entry = reg->entries + reg->count; + entry->contentHash = contentHash; + entry->id = id; + entry->kind = kind; + entry->byteCount = byteCount; + entry->bytes = bytes; // take ownership + entry->hashNext = hashPresent ? (int)itr.data->val : B3_NULL_INDEX; + reg->count++; + + if ( hashPresent ) + { + itr.data->val = id; + } + else + { + b3GeometryHashMap_insert( map, contentHash, id ); + } + return id; +} + +static b3GeometryHashMap* b3RegistryMap( b3GeometryRegistry* reg ) +{ + if ( reg->dedupMap == NULL ) + { + b3GeometryHashMap* fresh = (b3GeometryHashMap*)b3Alloc( sizeof( b3GeometryHashMap ) ); + b3GeometryHashMap_init( fresh ); + reg->dedupMap = fresh; + } + return (b3GeometryHashMap*)reg->dedupMap; +} + +uint32_t b3InternGeometry( b3GeometryRegistry* reg, b3GeometryKind kind, uint64_t contentHash, uint8_t* bytes, int byteCount ) +{ + b3GeometryHashMap* map = b3RegistryMap( reg ); + + b3GeometryHashMap_itr itr = b3GeometryHashMap_get( map, contentHash ); + bool hashPresent = b3GeometryHashMap_is_end( itr ) == false; + if ( hashPresent ) + { + // Walk every entry sharing this hash so a collision still finds the identical blob. + for ( int idx = (int)itr.data->val; idx != B3_NULL_INDEX; idx = reg->entries[idx].hashNext ) + { + b3GeometryEntry* e = reg->entries + idx; + if ( e->byteCount == byteCount && memcmp( e->bytes, bytes, (size_t)byteCount ) == 0 ) + { + // Duplicate: the caller transferred ownership; return existing id + b3Free( bytes, (size_t)byteCount ); + return e->id; + } + } + } + + return b3RegistryPush( reg, map, itr, hashPresent, kind, contentHash, bytes, byteCount ); +} + +uint32_t b3AppendGeometry( b3GeometryRegistry* reg, b3GeometryKind kind, uint64_t contentHash, uint8_t* bytes, int byteCount ) +{ + b3GeometryHashMap* map = b3RegistryMap( reg ); + b3GeometryHashMap_itr itr = b3GeometryHashMap_get( map, contentHash ); + bool hashPresent = b3GeometryHashMap_is_end( itr ) == false; + return b3RegistryPush( reg, map, itr, hashPresent, kind, contentHash, bytes, byteCount ); +} + +void b3FreeRegistry( b3GeometryRegistry* reg ) +{ + for ( int i = 0; i < reg->count; ++i ) + { + b3Free( reg->entries[i].bytes, (size_t)reg->entries[i].byteCount ); + } + if ( reg->entries != NULL ) + { + b3Free( reg->entries, (size_t)( reg->capacity * (int)sizeof( b3GeometryEntry ) ) ); + } + if ( reg->dedupMap != NULL ) + { + b3GeometryHashMap_cleanup( (b3GeometryHashMap*)reg->dedupMap ); + b3Free( reg->dedupMap, sizeof( b3GeometryHashMap ) ); + } + reg->entries = NULL; + reg->count = 0; + reg->capacity = 0; + reg->dedupMap = NULL; +} + +uint64_t b3HashQueryTag( uint64_t id, const char* name ) +{ + uint64_t h = B3_SNAP_FNV_INIT; + for ( int i = 0; i < 8; ++i ) + { + h = ( h ^ ( ( id >> ( 8 * i ) ) & 0xFFu ) ) * B3_SNAP_FNV_PRIME; + } + if ( name != NULL ) + { + for ( int i = 0; name[i] != '\0'; ++i ) + { + h = ( h ^ (uint8_t)name[i] ) * B3_SNAP_FNV_PRIME; + } + } + // Never 0 so the key doubles as the tagged flag. + return h != 0 ? h : 1; +} + +static b3RecTagMap* b3RecTags( b3Recording* rec ) +{ + if ( rec->tagMap == NULL ) + { + b3RecTagMap* fresh = b3Alloc( sizeof( b3RecTagMap ) ); + b3RecTagMap_init( fresh ); + rec->tagMap = fresh; + } + return rec->tagMap; +} + +void b3RecInternTag( b3Recording* rec, uint64_t key, uint64_t id, const char* name ) +{ + b3RecTagMap* map = b3RecTags( rec ); + if ( b3RecTagMap_is_end( b3RecTagMap_get( map, key ) ) == false ) + { + return; // first id/name for a key wins + } + + if ( rec->tagCount == rec->tagCapacity ) + { + int newCap = rec->tagCapacity == 0 ? 8 : 2 * rec->tagCapacity; + rec->tags = b3GrowAlloc( rec->tags, rec->tagCapacity * (int)sizeof( b3RecTag ), newCap * (int)sizeof( b3RecTag ) ); + rec->tagCapacity = newCap; + } + + uint32_t index = (uint32_t)rec->tagCount; + b3RecTag* tag = &rec->tags[rec->tagCount++]; + tag->key = key; + tag->id = id; + int n = 0; + while ( name != NULL && name[n] != '\0' && n < B3_MAX_QUERY_NAME_LENGTH ) + { + tag->queryName[n] = name[n]; + n++; + } + tag->queryName[n] = '\0'; + b3RecTagMap_insert( map, key, index ); +} + +// Write the trailing registry block: u32 entryCount then per-entry { u8 kind, u32 byteCount, bytes }, +// followed by the query-tag table { u32 tagCount, per-tag uu64 id, STR name }. A reader built before +// the tag table stops after the geometry entries and ignores the trailing tag bytes. +void b3RecWriteRegistry( b3Recording* rec ) +{ + b3RecW_U32( &rec->buffer, (uint32_t)rec->registry.count ); + for ( int i = 0; i < rec->registry.count; ++i ) + { + b3GeometryEntry* e = rec->registry.entries + i; + b3RecW_U8( &rec->buffer, (uint8_t)e->kind ); + b3RecW_U32( &rec->buffer, (uint32_t)e->byteCount ); + b3RecBufAppend( &rec->buffer, e->bytes, e->byteCount ); + } + + b3RecW_U32( &rec->buffer, (uint32_t)rec->tagCount ); + for ( int i = 0; i < rec->tagCount; ++i ) + { + b3RecW_U64( &rec->buffer, rec->tags[i].key ); + b3RecW_U64( &rec->buffer, rec->tags[i].id ); + b3RecW_STR( &rec->buffer, rec->tags[i].queryName ); + } +} + +// Lifecycle + +b3Recording* b3CreateRecording( int byteCapacity ) +{ + b3Recording* rec = (b3Recording*)b3Alloc( sizeof( b3Recording ) ); + *rec = (b3Recording){ 0 }; + + int initCap = byteCapacity > 0 ? byteCapacity : 65536; + rec->buffer.data = (uint8_t*)b3Alloc( (size_t)initCap ); + rec->buffer.capacity = initCap; + rec->buffer.size = 0; + rec->lock = b3CreateMutex(); + return rec; +} + +void b3DestroyRecording( b3Recording* recording ) +{ + if ( recording == NULL ) + { + return; + } + + b3RecBufFree( &recording->buffer ); + b3FreeRegistry( &recording->registry ); + if ( recording->tags != NULL ) + { + b3Free( recording->tags, (size_t)recording->tagCapacity * sizeof( b3RecTag ) ); + } + if ( recording->tagMap != NULL ) + { + b3RecTagMap_cleanup( (b3RecTagMap*)recording->tagMap ); + b3Free( recording->tagMap, sizeof( b3RecTagMap ) ); + } + b3DestroyMutex( recording->lock ); + b3Free( recording, sizeof( b3Recording ) ); +} + +const uint8_t* b3Recording_GetData( const b3Recording* recording ) +{ + return recording->buffer.data; +} + +int b3Recording_GetSize( const b3Recording* recording ) +{ + return recording->buffer.size; +} + +void b3RecAccumulateBounds( b3Recording* rec, b3AABB bounds ) +{ + rec->accumulatedBounds = rec->haveBounds ? b3AABB_Union( rec->accumulatedBounds, bounds ) : bounds; + rec->haveBounds = true; +} + +void b3StartRecordingIntoBuffer( b3World* world, b3Recording* recording ) +{ + // Reset so a recording handle can be reused for a fresh session + recording->buffer.size = 0; + recording->recordStart = 0; + recording->haveBounds = false; + b3FreeRegistry( &recording->registry ); + if ( recording->tags != NULL ) + { + b3Free( recording->tags, (size_t)recording->tagCapacity * sizeof( b3RecTag ) ); + recording->tags = NULL; + } + if ( recording->tagMap != NULL ) + { + b3RecTagMap_cleanup( (b3RecTagMap*)recording->tagMap ); + b3Free( recording->tagMap, sizeof( b3RecTagMap ) ); + recording->tagMap = NULL; + } + recording->tagCount = 0; + recording->tagCapacity = 0; + + b3RecHeader hdr = { 0 }; + hdr.magic = B3_REC_MAGIC; + hdr.versionMajor = B3_REC_VERSION_MAJOR; + hdr.versionMinor = B3_REC_VERSION_MINOR; + hdr.pointerWidth = (uint8_t)sizeof( void* ); + hdr.bigEndian = 0; + hdr.validationEnabled = B3_ENABLE_VALIDATION ? 1u : 0u; + hdr.lengthScale = b3GetLengthUnitsPerMeter(); + hdr.registryOffset = 0; // backpatched in b3StopRecordingInternal + hdr.registryByteCount = 0; + + world->recording = recording; + + // Every recording is snapshot-seeded. The seed blob follows the header so replay restores in + // place and the world id stays stable across a restart or backward scrub. An empty world still + // serializes a valid blob, so there is no from-creation special case. + b3RecBuffer snapBuf = { 0 }; + b3SerializeWorld( world, &snapBuf, recording ); + hdr.snapshotSize = (uint64_t)snapBuf.size; + + b3RecBufAppend( &recording->buffer, &hdr, (int)sizeof( hdr ) ); + b3RecBufAppend( &recording->buffer, snapBuf.data, snapBuf.size ); + b3RecBufFree( &snapBuf ); + + // Anchor the recording with the current world state hash so replay can assert + // determinism from the very first step. + b3WorldId worldId = { (uint16_t)( world->worldId + 1 ), world->generation }; + b3RecArgs_StateHash stateHash = { worldId, b3HashWorldState( world ) }; + b3RecWrite_StateHash( recording, &stateHash ); +} + +void b3StopRecordingInternal( b3World* world ) +{ + if ( world->recording == NULL ) + { + return; + } + + b3Recording* rec = world->recording; + world->recording = NULL; + + // Write accumulated bounds so a viewer can frame the whole recorded motion + b3RecArgs_RecordingBounds rb = { 0 }; + if ( rec->haveBounds ) + { + rb.bounds = rec->accumulatedBounds; + } + b3RecWrite_RecordingBounds( rec, &rb ); + + // End-of-stream marker; the buffer is now self-contained + b3WorldId wid = { (uint16_t)( world->worldId + 1 ), world->generation }; + b3RecArgs_DestroyWorld a = { wid }; + b3RecWrite_DestroyWorld( rec, &a ); + + // Write the trailing registry block + int registryOffset = rec->buffer.size; + b3RecWriteRegistry( rec ); + int registryByteCount = rec->buffer.size - registryOffset; + + // Backpatch registryOffset and registryByteCount into the header + uint8_t* hdrBytes = rec->buffer.data; + uint64_t regOff = (uint64_t)registryOffset; + uint64_t regSz = (uint64_t)registryByteCount; + // Little-endian backpatch in place; offsetof keeps this correct if the header layout shifts + uint8_t* pOff = hdrBytes + offsetof( b3RecHeader, registryOffset ); + uint8_t* pSz = hdrBytes + offsetof( b3RecHeader, registryByteCount ); + for ( int i = 0; i < 8; ++i ) + { + pOff[i] = (uint8_t)( regOff >> ( 8 * i ) ); + pSz[i] = (uint8_t)( regSz >> ( 8 * i ) ); + } +} + +// Convenience file I/O + +bool b3SaveRecordingToFile( const b3Recording* recording, const char* path ) +{ + if ( recording == NULL || path == NULL ) + { + return false; + } + + FILE* f = fopen( path, "wb" ); + if ( f == NULL ) + { + return false; + } + + size_t written = fwrite( recording->buffer.data, 1, (size_t)recording->buffer.size, f ); + fclose( f ); + return (int)written == recording->buffer.size; +} + +b3Recording* b3LoadRecordingFromFile( const char* path ) +{ + if ( path == NULL ) + { + return NULL; + } + + FILE* f = fopen( path, "rb" ); + if ( f == NULL ) + { + return NULL; + } + + if ( fseek( f, 0, SEEK_END ) != 0 ) + { + fclose( f ); + return NULL; + } + + long fileSize = ftell( f ); + // Anything smaller than the fixed header can't be a recording + if ( fileSize < (long)sizeof( b3RecHeader ) || fileSize > INT_MAX ) + { + fclose( f ); + return NULL; + } + fseek( f, 0, SEEK_SET ); + + b3Recording* rec = b3CreateRecording( (int)fileSize ); + size_t readSize = fread( rec->buffer.data, 1, (size_t)fileSize, f ); + fclose( f ); + + if ( (long)readSize != fileSize ) + { + b3DestroyRecording( rec ); + return NULL; + } + + // Validate magic so a wrong file fails at load rather than deep in the player + b3RecHeader hdr; + memcpy( &hdr, rec->buffer.data, sizeof( hdr ) ); + if ( hdr.magic != B3_REC_MAGIC ) + { + b3DestroyRecording( rec ); + return NULL; + } + + rec->buffer.size = (int)fileSize; + return rec; +} + +// Geometry interning helpers + +uint32_t b3RecInternHull( b3Recording* rec, const b3HullData* hull ) +{ + int byteCount = hull->byteCount; + uint8_t* bytes = (uint8_t*)b3Alloc( (size_t)byteCount ); + memcpy( bytes, hull, (size_t)byteCount ); + uint64_t h = b3Hash64Blob( bytes, byteCount ); + return b3InternGeometry( &rec->registry, b3_geometryHull, h, bytes, byteCount ); +} + +uint32_t b3RecInternMesh( b3Recording* rec, const b3MeshData* mesh ) +{ + int byteCount = mesh->byteCount; + uint8_t* bytes = (uint8_t*)b3Alloc( (size_t)byteCount ); + memcpy( bytes, mesh, (size_t)byteCount ); + uint64_t h = b3Hash64Blob( bytes, byteCount ); + return b3InternGeometry( &rec->registry, b3_geometryMesh, h, bytes, byteCount ); +} + +uint32_t b3RecInternHeightField( b3Recording* rec, const b3HeightFieldData* hf ) +{ + int byteCount = hf->byteCount; + uint8_t* bytes = (uint8_t*)b3Alloc( (size_t)byteCount ); + memcpy( bytes, hf, (size_t)byteCount ); + uint64_t h = b3Hash64Blob( bytes, byteCount ); + return b3InternGeometry( &rec->registry, b3_geometryHeightField, h, bytes, byteCount ); +} + +uint32_t b3RecInternCompound( b3Recording* rec, const b3CompoundData* compound ) +{ + int byteCount = compound->byteCount; + uint8_t* bytes = (uint8_t*)b3Alloc( (size_t)byteCount ); + memcpy( bytes, compound, (size_t)byteCount ); + // Null the tree node pointer in the copy so the canonical bytes are pointer-free. + // b3ConvertBytesToCompound fixes it back on load via nodeOffset. + ( (b3CompoundData*)bytes )->tree.nodes = NULL; + uint64_t h = b3Hash64Blob( bytes, byteCount ); + return b3InternGeometry( &rec->registry, b3_geometryCompound, h, bytes, byteCount ); +} + +uint64_t b3HashWorldState( b3World* world ) +{ + uint64_t hash = B3_SNAP_FNV_INIT; + const uint64_t prime = B3_SNAP_FNV_PRIME; + + int bodyCount = world->bodies.count; + for ( int i = 0; i < bodyCount; ++i ) + { + b3Body* body = world->bodies.data + i; + if ( body->id != i ) + { + // Free or never-used slot + continue; + } + + b3BodySim* sim = b3GetBodySim( world, body ); + + uint32_t bits; + +#define B3_HASH_FLOAT( f ) \ + memcpy( &bits, &( f ), 4 ); \ + hash = ( hash ^ (uint64_t)bits ) * prime; + + hash = b3FnvMixPosition( hash, sim->transform.p ); + B3_HASH_FLOAT( sim->transform.q.v.x ) + B3_HASH_FLOAT( sim->transform.q.v.y ) + B3_HASH_FLOAT( sim->transform.q.v.z ) + B3_HASH_FLOAT( sim->transform.q.s ) + + b3BodyState* state = b3GetBodyState( world, body ); + if ( state != NULL ) + { + B3_HASH_FLOAT( state->linearVelocity.x ) + B3_HASH_FLOAT( state->linearVelocity.y ) + B3_HASH_FLOAT( state->linearVelocity.z ) + B3_HASH_FLOAT( state->angularVelocity.x ) + B3_HASH_FLOAT( state->angularVelocity.y ) + B3_HASH_FLOAT( state->angularVelocity.z ) + } + +#undef B3_HASH_FLOAT + } + + return hash; +} diff --git a/vendor/box3d/src/src/recording.h b/vendor/box3d/src/src/recording.h new file mode 100644 index 000000000..3ba2fd3bb --- /dev/null +++ b/vendor/box3d/src/src/recording.h @@ -0,0 +1,386 @@ +// SPDX-FileCopyrightText: 2026 Erin Catto +// SPDX-License-Identifier: MIT + +#pragma once + +#include "core.h" + +#include "box3d/id.h" +#include "box3d/math_functions.h" +#include "box3d/types.h" + +#include +#include +#include +#include + +// FNV-1a 64-bit constants +#define B3_SNAP_FNV_INIT 14695981039346656037ull +#define B3_SNAP_FNV_PRIME 1099511628211ull + +// Mix a world position at full width so the determinism gate validates past float precision +// when the body is far from the origin. +static inline uint64_t b3FnvMixPosition( uint64_t hash, b3Pos p ) +{ +#if defined( BOX3D_DOUBLE_PRECISION ) + uint64_t bx, by, bz; + memcpy( &bx, &p.x, 8 ); + memcpy( &by, &p.y, 8 ); + memcpy( &bz, &p.z, 8 ); +#else + uint32_t fx, fy, fz; + memcpy( &fx, &p.x, 4 ); + memcpy( &fy, &p.y, 4 ); + memcpy( &fz, &p.z, 4 ); + uint64_t bx = fx, by = fy, bz = fz; +#endif + hash = ( hash ^ bx ) * B3_SNAP_FNV_PRIME; + hash = ( hash ^ by ) * B3_SNAP_FNV_PRIME; + hash = ( hash ^ bz ) * B3_SNAP_FNV_PRIME; + return hash; +} + +typedef struct b3World b3World; + +// Magic 'B3RC' in little-endian: bytes B(0x42) 3(0x33) R(0x52) C(0x43) +#define B3_REC_MAGIC 0x43523342u + +// Major recording version is bumped when writers change. +// Major version 4 added b3ShapeDef::enableSpeculativeContact +#define B3_REC_VERSION_MAJOR 4 + +// Minor tracks op-stream additions that keep the 48 byte header shape. +// Minor version 3 added name cache. +#define B3_REC_VERSION_MINOR 3 + +// File header, fixed 48 bytes, little-endian. Contains the registry locator so the player +// can load geometry before replaying any ops. +typedef struct b3RecHeader +{ + uint32_t magic; // 'B3RC' = 0x43523342 + uint16_t versionMajor; // B3_REC_VERSION_MAJOR + uint16_t versionMinor; // B3_REC_VERSION_MINOR + uint8_t pointerWidth; // sizeof(void*), gates POD-def struct layout + uint8_t bigEndian; // 0 on all supported targets + uint8_t validationEnabled; // 1 if built with BOX3D_VALIDATE, diagnostic only + uint8_t reserved; + float lengthScale; // b3GetLengthUnitsPerMeter() + uint32_t reserved2; + uint32_t reserved3; // explicit pad so the 64-bit fields align with no implicit gap + uint64_t snapshotSize; // bytes of snapshot blob after the header (0 in Phase 1) + uint64_t registryOffset; // absolute offset to trailing registry block, backpatched at stop + uint64_t registryByteCount; // size of the registry block +} b3RecHeader; + +_Static_assert( sizeof( b3RecHeader ) == 48, "recording header must be 48 bytes" ); + +// Growable append-only byte buffer. Doubles on demand. countOnly mode tallies size without +// allocating, used to size a buffer cheaply before a second filling pass. +typedef struct b3RecBuffer +{ + uint8_t* data; + int capacity; + int size; + bool countOnly; +} b3RecBuffer; + +// Geometry kinds for the trailing registry section +typedef enum b3GeometryKind +{ + b3_geometryHull, + b3_geometryMesh, + b3_geometryHeightField, + b3_geometryCompound, +} b3GeometryKind; + +// One entry per unique geometry blob. id == index in the entries array. +// hashNext chains entries that share a content hash so dedup stays exact under a hash collision, +// which the keyframe registry depends on to never grow during capture. B3_NULL_INDEX ends the chain. +typedef struct b3GeometryEntry +{ + uint64_t contentHash; + uint32_t id; + b3GeometryKind kind; + int byteCount; + uint8_t* bytes; + int hashNext; +} b3GeometryEntry; + +// Growable array of geometry entries. Ids are array indices, so the array is serialized in order. +// dedupMap maps content hash to entry id for O(1) dedup; it is opaque here and owned by recording.c. +typedef struct b3GeometryRegistry +{ + b3GeometryEntry* entries; + int count; + int capacity; + void* dedupMap; +} b3GeometryRegistry; + +// Limit the maximum query name length to make recording simpler. Query names longer than this +// probably indicate a bug in user code. +#define B3_MAX_QUERY_NAME_LENGTH 64 + +// Query tag from b3QueryFilter. +// Stored once per key in the trailing block so a tagged query carries only the 8 byte key on the wire. +// Shared by the recorder (accumulate) and the player (load). +typedef struct b3RecTag +{ + // hash of (id, queryName) + uint64_t key; + uint64_t id; + char queryName[B3_MAX_QUERY_NAME_LENGTH + 1]; +} b3RecTag; + +// User-owned recording buffer. The world appends into it while active; the host saves and +// destroys it. Opaque across the public API. +typedef struct b3Recording +{ + b3RecBuffer buffer; + int recordStart; // offset of the 3-byte size field for u24 backpatch + b3Mutex* lock; // serializes record writes from concurrent threads + b3GeometryRegistry registry; + + // Interned query tags accumulated during capture, written to the tail of the registry block at stop. + // tagMap maps a tag key to its index for O(1) dedup. + b3RecTag* tags; + int tagCount; + int tagCapacity; + void* tagMap; + + // Union of world bounds over every recorded step, written at stop. + b3AABB accumulatedBounds; + bool haveBounds; +} b3Recording; + +// C type aliases per TAG, used in the X-macro codegen arg structs +typedef bool b3RecCType_BOOL; +typedef int32_t b3RecCType_I32; +typedef uint8_t b3RecCType_U8; +typedef uint16_t b3RecCType_U16; +typedef uint32_t b3RecCType_U32; +typedef uint64_t b3RecCType_U64; +typedef float b3RecCType_F32; +typedef double b3RecCType_F64; +typedef b3Vec3 b3RecCType_VEC3; +typedef b3Quat b3RecCType_QUAT; +typedef b3Transform b3RecCType_TRANSFORM; +typedef b3Pos b3RecCType_POSITION; +typedef b3WorldTransform b3RecCType_WORLDXF; +typedef b3Matrix3 b3RecCType_MATRIX3; +typedef b3AABB b3RecCType_AABB; +typedef b3Sphere b3RecCType_SPHERE; +typedef b3Capsule b3RecCType_CAPSULE; +typedef b3QueryFilter b3RecCType_QUERYFILTER; +typedef b3ShapeProxy b3RecCType_SHAPEPROXY; +// Geometry reference: a plain u32 id into the trailing registry +typedef uint32_t b3RecCType_GEOMID; +typedef b3Filter b3RecCType_FILTER; +typedef b3SurfaceMaterial b3RecCType_MATERIAL; +typedef b3MassData b3RecCType_MASSDATA; +typedef b3MotionLocks b3RecCType_LOCKS; +typedef const char* b3RecCType_STR; +typedef b3WorldId b3RecCType_WORLDID; +typedef b3BodyId b3RecCType_BODYID; +typedef b3ShapeId b3RecCType_SHAPEID; +typedef b3JointId b3RecCType_JOINTID; +typedef b3BodyDef b3RecCType_BODYDEF; +typedef b3ShapeDef b3RecCType_SHAPEDEF; +typedef b3ExplosionDef b3RecCType_EXPLOSIONDEF; +typedef b3ParallelJointDef b3RecCType_PARALLELJOINTDEF; +typedef b3DistanceJointDef b3RecCType_DISTANCEJOINTDEF; +typedef b3FilterJointDef b3RecCType_FILTERJOINTDEF; +typedef b3MotorJointDef b3RecCType_MOTORJOINTDEF; +typedef b3PrismaticJointDef b3RecCType_PRISMATICJOINTDEF; +typedef b3RevoluteJointDef b3RecCType_REVOLUTEJOINTDEF; +typedef b3SphericalJointDef b3RecCType_SPHERICALJOINTDEF; +typedef b3WeldJointDef b3RecCType_WELDJOINTDEF; +typedef b3WheelJointDef b3RecCType_WHEELJOINTDEF; + +// Codegen pass 1a: arg structs. Generated here so call sites (body.c, shape.c, etc.) can see them. +#define ARG( TAG, field ) b3RecCType_##TAG field; +#define B3_REC_OP( op, Name, RET, ... ) \ + typedef struct \ + { \ + __VA_ARGS__ \ + } b3RecArgs_##Name; +#include "recording_ops.inl" +#undef B3_REC_OP +#undef ARG + +// Opcode constants generated from the manifest, so call sites name an op instead of a raw byte and +// can't drift from the manifest if an op is renumbered. +enum +{ +#define B3_REC_OP( op, Name, RET, ... ) b3_recOp##Name = ( op ), +#include "recording_ops.inl" +#undef B3_REC_OP +}; + +// Low-level buffer helpers +void b3RecBufAppend( b3RecBuffer* buf, const void* data, int size ); +void b3RecBufFree( b3RecBuffer* buf ); + +// Write primitives +void b3RecW_U8( b3RecBuffer* buf, uint8_t v ); +void b3RecW_U16( b3RecBuffer* buf, uint16_t v ); +void b3RecW_U32( b3RecBuffer* buf, uint32_t v ); +void b3RecW_U64( b3RecBuffer* buf, uint64_t v ); +void b3RecW_I32( b3RecBuffer* buf, int32_t v ); +void b3RecW_F32( b3RecBuffer* buf, float v ); +void b3RecW_F64( b3RecBuffer* buf, double v ); +void b3RecW_BOOL( b3RecBuffer* buf, bool v ); +void b3RecW_VEC3( b3RecBuffer* buf, b3Vec3 v ); +void b3RecW_QUAT( b3RecBuffer* buf, b3Quat v ); +void b3RecW_TRANSFORM( b3RecBuffer* buf, b3Transform v ); +// World position: doubles in large-world mode, floats otherwise (wire-identical to VEC3 in float build) +void b3RecW_POSITION( b3RecBuffer* buf, b3Pos v ); +void b3RecW_WORLDXF( b3RecBuffer* buf, b3WorldTransform v ); +void b3RecW_MATRIX3( b3RecBuffer* buf, b3Matrix3 v ); +void b3RecW_AABB( b3RecBuffer* buf, b3AABB v ); +void b3RecW_QUERYFILTER( b3RecBuffer* buf, b3QueryFilter v ); +void b3RecW_SHAPEPROXY( b3RecBuffer* buf, b3ShapeProxy v ); +void b3RecW_TREESTATS( b3RecBuffer* buf, b3TreeStats v ); +void b3RecW_RAYRESULT( b3RecBuffer* buf, b3RayResult v ); +void b3RecW_PLANERESULT( b3RecBuffer* buf, b3PlaneResult v ); +void b3RecW_WORLDID( b3RecBuffer* buf, b3WorldId v ); +void b3RecW_BODYID( b3RecBuffer* buf, b3BodyId v ); +void b3RecW_SHAPEID( b3RecBuffer* buf, b3ShapeId v ); +void b3RecW_JOINTID( b3RecBuffer* buf, b3JointId v ); +void b3RecW_SPHERE( b3RecBuffer* buf, b3Sphere v ); +void b3RecW_CAPSULE( b3RecBuffer* buf, b3Capsule v ); +void b3RecW_GEOMID( b3RecBuffer* buf, uint32_t v ); +void b3RecW_FILTER( b3RecBuffer* buf, b3Filter v ); +void b3RecW_MATERIAL( b3RecBuffer* buf, b3SurfaceMaterial v ); +void b3RecW_MASSDATA( b3RecBuffer* buf, b3MassData v ); +void b3RecW_LOCKS( b3RecBuffer* buf, b3MotionLocks v ); +void b3RecW_EXPLOSIONDEF( b3RecBuffer* buf, b3ExplosionDef v ); +void b3RecW_BODYDEF( b3RecBuffer* buf, b3BodyDef v ); +void b3RecW_SHAPEDEF( b3RecBuffer* buf, b3ShapeDef v ); +void b3RecW_PARALLELJOINTDEF( b3RecBuffer* buf, b3ParallelJointDef v ); +void b3RecW_DISTANCEJOINTDEF( b3RecBuffer* buf, b3DistanceJointDef v ); +void b3RecW_FILTERJOINTDEF( b3RecBuffer* buf, b3FilterJointDef v ); +void b3RecW_MOTORJOINTDEF( b3RecBuffer* buf, b3MotorJointDef v ); +void b3RecW_PRISMATICJOINTDEF( b3RecBuffer* buf, b3PrismaticJointDef v ); +void b3RecW_REVOLUTEJOINTDEF( b3RecBuffer* buf, b3RevoluteJointDef v ); +void b3RecW_SPHERICALJOINTDEF( b3RecBuffer* buf, b3SphericalJointDef v ); +void b3RecW_WELDJOINTDEF( b3RecBuffer* buf, b3WeldJointDef v ); +void b3RecW_WHEELJOINTDEF( b3RecBuffer* buf, b3WheelJointDef v ); + +// Record framing +void b3RecBeginRecord( b3Recording* rec, uint8_t opcode ); +void b3RecEndRecord( b3Recording* rec ); + +// Per-op arg writers (no framing) and full writers (framing + args), generated from the manifest. +#define B3_REC_OP( op, Name, RET, ... ) \ + void b3RecWriteArgs_##Name( b3Recording* rec, const b3RecArgs_##Name* a ); \ + void b3RecWrite_##Name( b3Recording* rec, const b3RecArgs_##Name* a ); +#include "recording_ops.inl" +#undef B3_REC_OP + +// Create ops: declare writers that also append the returned id inside the record. +#define B3_REC_RETDECL_RET_NONE( Name ) +#define B3_REC_RETDECL_RET_BODYID( Name ) void b3RecWriteRet_##Name( b3Recording* rec, const b3RecArgs_##Name* a, b3BodyId id ); +#define B3_REC_RETDECL_RET_SHAPEID( Name ) void b3RecWriteRet_##Name( b3Recording* rec, const b3RecArgs_##Name* a, b3ShapeId id ); +#define B3_REC_RETDECL_RET_JOINTID( Name ) void b3RecWriteRet_##Name( b3Recording* rec, const b3RecArgs_##Name* a, b3JointId id ); +#define B3_REC_OP( op, Name, RET, ... ) B3_REC_RETDECL_##RET( Name ) +#include "recording_ops.inl" +#undef B3_REC_OP +#undef B3_REC_RETDECL_RET_NONE +#undef B3_REC_RETDECL_RET_BODYID +#undef B3_REC_RETDECL_RET_SHAPEID +#undef B3_REC_RETDECL_RET_JOINTID + +// Record a void op. The branch is free when recording is off. +#define B3_REC( world, Name, ... ) \ + do \ + { \ + if ( ( world )->recording != NULL ) \ + { \ + b3RecArgs_##Name recArgs = { __VA_ARGS__ }; \ + b3RecWrite_##Name( ( world )->recording, &recArgs ); \ + } \ + } \ + while ( 0 ) + +// Record a create op and its returned id in one framed record. Place after the real create call. +#define B3_REC_CREATE( world, Name, id, ... ) \ + do \ + { \ + if ( ( world )->recording != NULL ) \ + { \ + b3RecArgs_##Name recCreateArgs = { __VA_ARGS__ }; \ + b3RecWriteRet_##Name( ( world )->recording, &recCreateArgs, id ); \ + } \ + } \ + while ( 0 ) + +// Patch helpers for the query hit-count backfill +int b3RecReserveU32( b3RecBuffer* buf ); +void b3RecPatchU32( b3RecBuffer* buf, int offset, uint32_t v ); + +// Commit a finished query record under the lock. The payload buffer stays owned by the caller. +void b3RecCommitRecord( b3Recording* rec, uint8_t opcode, const uint8_t* payload, int payloadSize ); + +// Per-query writer context: holds the user fcn+ctx, the local payload buffer, and the hit counter +typedef struct b3RecQueryWriter +{ + union + { + b3OverlapResultFcn* overlapFcn; + b3CastResultFcn* castFcn; + b3PlaneResultFcn* planeFcn; + b3MoverFilterFcn* moverFilterFcn; + } userFcn; + void* userContext; + b3RecBuffer buf; // per-call local payload, heap-backed + int countOffset; // offset of the reserved u32 hit-count slot + uint32_t hitCount; + uint64_t tagId; // caller query id, 0 = untagged. Emitted as a QueryTag before the record. + const char* tagName; // caller query name, interned by id. NULL = none. +} b3RecQueryWriter; + +void b3RecQueryBegin( b3RecQueryWriter* w, void* context, uint64_t tagId, const char* tagName ); +void b3RecQueryCommit( b3Recording* rec, uint8_t opcode, b3RecQueryWriter* w ); + +// Recording trampolines: replace the user fcn so hits are captured before dispatch. The overlap +// trampoline doubles for the mover filter, which has the same bool(shapeId, ctx) shape. +bool b3RecOverlapTrampoline( b3ShapeId id, void* ctx ); +float b3RecCastTrampoline( b3ShapeId id, b3Pos point, b3Vec3 normal, float fraction, uint64_t userMaterialId, int triangleIndex, + int childIndex, void* ctx ); +bool b3RecPlaneTrampoline( b3ShapeId id, const b3PlaneResult* planes, int planeCount, void* ctx ); + +// Geometry registry +uint32_t b3InternGeometry( b3GeometryRegistry* reg, b3GeometryKind kind, uint64_t contentHash, uint8_t* bytes, int byteCount ); +// Append an entry unconditionally and return its id, which equals its array index. Unlike +// b3InternGeometry it never deduplicates, so the keyframe seed can mirror slots 1:1 even when an +// already-recorded file carries byte-identical duplicate slots (a hash collision wrote them apart). +uint32_t b3AppendGeometry( b3GeometryRegistry* reg, b3GeometryKind kind, uint64_t contentHash, uint8_t* bytes, int byteCount ); +void b3FreeRegistry( b3GeometryRegistry* reg ); +void b3RecWriteRegistry( b3Recording* rec ); + +// Hash a query (id, name) pair into the stable key the viewer tracks the query by. Never returns 0, +// so the key doubles as a tagged/untagged flag. +uint64_t b3HashQueryTag( uint64_t id, const char* name ); + +// Record a key->(id, name) mapping once, deduped by key (a repeated key keeps its first id/name). +void b3RecInternTag( b3Recording* rec, uint64_t key, uint64_t id, const char* name ); + +// Intern each large geometry kind and return a stable u32 id for use in create ops. +// Caller does NOT free bytes; b3InternGeometry takes ownership (frees on duplicate). +uint32_t b3RecInternHull( b3Recording* rec, const b3HullData* hull ); +uint32_t b3RecInternMesh( b3Recording* rec, const b3MeshData* mesh ); +uint32_t b3RecInternHeightField( b3Recording* rec, const b3HeightFieldData* hf ); +uint32_t b3RecInternCompound( b3Recording* rec, const b3CompoundData* compound ); + +uint64_t b3Hash64Blob( const uint8_t* bytes, int n ); + +// Lifecycle engine-side hooks +void b3StartRecordingIntoBuffer( b3World* world, b3Recording* recording ); +void b3StopRecordingInternal( b3World* world ); + +// Fold one step's world bounds into the running union. +void b3RecAccumulateBounds( b3Recording* rec, b3AABB bounds ); + +// Deterministic hash over all body transforms and velocities. +// Called by both recorder and replayer to verify simulation reproduces exactly. +uint64_t b3HashWorldState( b3World* world ); diff --git a/vendor/box3d/src/src/recording_ops.inl b/vendor/box3d/src/src/recording_ops.inl new file mode 100644 index 000000000..b8a3625da --- /dev/null +++ b/vendor/box3d/src/src/recording_ops.inl @@ -0,0 +1,232 @@ +// SPDX-FileCopyrightText: 2026 Erin Catto +// SPDX-License-Identifier: MIT + +// X-macro manifest for the recording system. +// Include with B3_REC_OP and ARG defined. No commas between ARG tokens. +// +// B3_REC_OP( opcode, Name, RET, ARGS ) +// RET in { RET_NONE, RET_BODYID, RET_SHAPEID, RET_JOINTID } +// ARGS = zero or more ARG( TAG, fieldName ) tokens, NO commas between them +// +// Opcode ranges: +// 0x00-0x0F world lifecycle and config +// 0x10-0x1F body create/destroy +// 0x20-0x3F body mutators +// 0x40-0x4F shape create/destroy +// 0x50-0x6F shape mutators +// 0x80 step +// 0x90-0xE7 joints (create, generic, per-type) +// 0xE8-0xEF spatial queries +// 0xF0-0xFF markers + +// Recordings are seeded with a world snapshot, not a CreateWorld op. DestroyWorld stays as the +// end-of-session marker the viewer reads. +B3_REC_OP( 0x01, DestroyWorld, RET_NONE, ARG( WORLDID, world ) ) +B3_REC_OP( 0x80, Step, RET_NONE, ARG( WORLDID, world ) ARG( F32, dt ) ARG( I32, subStepCount ) ) + +// World config. The world arg is informational; replay always targets its own world. +B3_REC_OP( 0x02, WorldEnableSleeping, RET_NONE, ARG( WORLDID, world ) ARG( BOOL, flag ) ) +B3_REC_OP( 0x03, WorldEnableContinuous, RET_NONE, ARG( WORLDID, world ) ARG( BOOL, flag ) ) +B3_REC_OP( 0x04, WorldSetRestitutionThreshold, RET_NONE, ARG( WORLDID, world ) ARG( F32, value ) ) +B3_REC_OP( 0x05, WorldSetHitEventThreshold, RET_NONE, ARG( WORLDID, world ) ARG( F32, value ) ) +B3_REC_OP( 0x06, WorldSetGravity, RET_NONE, ARG( WORLDID, world ) ARG( VEC3, gravity ) ) +B3_REC_OP( 0x07, WorldExplode, RET_NONE, ARG( WORLDID, world ) ARG( EXPLOSIONDEF, def ) ) +B3_REC_OP( 0x08, WorldSetContactTuning, RET_NONE, + ARG( WORLDID, world ) ARG( F32, hertz ) ARG( F32, dampingRatio ) ARG( F32, contactSpeed ) ) +B3_REC_OP( 0x09, WorldSetContactRecycleDistance, RET_NONE, ARG( WORLDID, world ) ARG( F32, recycleDistance ) ) +B3_REC_OP( 0x0A, WorldSetMaximumLinearSpeed, RET_NONE, ARG( WORLDID, world ) ARG( F32, maximumLinearSpeed ) ) +B3_REC_OP( 0x0B, WorldEnableWarmStarting, RET_NONE, ARG( WORLDID, world ) ARG( BOOL, flag ) ) +B3_REC_OP( 0x0C, WorldRebuildStaticTree, RET_NONE, ARG( WORLDID, world ) ) +B3_REC_OP( 0x0D, WorldEnableSpeculative, RET_NONE, ARG( WORLDID, world ) ARG( BOOL, flag ) ) + +// Body +B3_REC_OP( 0x10, CreateBody, RET_BODYID, ARG( WORLDID, world ) ARG( BODYDEF, def ) ) +B3_REC_OP( 0x11, DestroyBody, RET_NONE, ARG( BODYID, body ) ) +B3_REC_OP( 0x20, BodySetTransform, RET_NONE, ARG( BODYID, body ) ARG( POSITION, position ) ARG( QUAT, rotation ) ) +B3_REC_OP( 0x21, BodySetLinearVelocity, RET_NONE, ARG( BODYID, body ) ARG( VEC3, v ) ) +B3_REC_OP( 0x22, BodySetType, RET_NONE, ARG( BODYID, body ) ARG( I32, type ) ) +B3_REC_OP( 0x23, BodySetName, RET_NONE, ARG( BODYID, body ) ARG( STR, name ) ) +B3_REC_OP( 0x24, BodySetAngularVelocity, RET_NONE, ARG( BODYID, body ) ARG( VEC3, w ) ) +B3_REC_OP( 0x25, BodySetTargetTransform, RET_NONE, ARG( BODYID, body ) ARG( WORLDXF, target ) ARG( F32, timeStep ) ARG( BOOL, wake ) ) +B3_REC_OP( 0x26, BodyApplyForce, RET_NONE, ARG( BODYID, body ) ARG( VEC3, force ) ARG( POSITION, point ) ARG( BOOL, wake ) ) +B3_REC_OP( 0x27, BodyApplyForceToCenter, RET_NONE, ARG( BODYID, body ) ARG( VEC3, force ) ARG( BOOL, wake ) ) +B3_REC_OP( 0x28, BodyApplyTorque, RET_NONE, ARG( BODYID, body ) ARG( VEC3, torque ) ARG( BOOL, wake ) ) +B3_REC_OP( 0x29, BodyApplyLinearImpulse, RET_NONE, ARG( BODYID, body ) ARG( VEC3, impulse ) ARG( POSITION, point ) ARG( BOOL, wake ) ) +B3_REC_OP( 0x2A, BodyApplyLinearImpulseToCenter, RET_NONE, ARG( BODYID, body ) ARG( VEC3, impulse ) ARG( BOOL, wake ) ) +B3_REC_OP( 0x2B, BodyApplyAngularImpulse, RET_NONE, ARG( BODYID, body ) ARG( VEC3, impulse ) ARG( BOOL, wake ) ) +B3_REC_OP( 0x2C, BodySetMassData, RET_NONE, ARG( BODYID, body ) ARG( MASSDATA, massData ) ) +B3_REC_OP( 0x2D, BodyApplyMassFromShapes, RET_NONE, ARG( BODYID, body ) ) +B3_REC_OP( 0x2E, BodySetLinearDamping, RET_NONE, ARG( BODYID, body ) ARG( F32, damping ) ) +B3_REC_OP( 0x2F, BodySetAngularDamping, RET_NONE, ARG( BODYID, body ) ARG( F32, damping ) ) +B3_REC_OP( 0x30, BodySetGravityScale, RET_NONE, ARG( BODYID, body ) ARG( F32, scale ) ) +B3_REC_OP( 0x31, BodySetAwake, RET_NONE, ARG( BODYID, body ) ARG( BOOL, awake ) ) +B3_REC_OP( 0x32, BodyEnableSleep, RET_NONE, ARG( BODYID, body ) ARG( BOOL, flag ) ) +B3_REC_OP( 0x33, BodySetSleepThreshold, RET_NONE, ARG( BODYID, body ) ARG( F32, threshold ) ) +B3_REC_OP( 0x34, BodyDisable, RET_NONE, ARG( BODYID, body ) ) +B3_REC_OP( 0x35, BodyEnable, RET_NONE, ARG( BODYID, body ) ) +B3_REC_OP( 0x36, BodySetMotionLocks, RET_NONE, ARG( BODYID, body ) ARG( LOCKS, locks ) ) +B3_REC_OP( 0x37, BodySetBullet, RET_NONE, ARG( BODYID, body ) ARG( BOOL, flag ) ) +B3_REC_OP( 0x38, BodyEnableContactRecycling, RET_NONE, ARG( BODYID, body ) ARG( BOOL, flag ) ) +B3_REC_OP( 0x39, BodyEnableHitEvents, RET_NONE, ARG( BODYID, body ) ARG( BOOL, flag ) ) + +// Shape create/destroy +B3_REC_OP( 0x40, CreateSphereShape, RET_SHAPEID, ARG( BODYID, body ) ARG( SHAPEDEF, def ) ARG( SPHERE, sphere ) ) +B3_REC_OP( 0x41, CreateCapsuleShape, RET_SHAPEID, ARG( BODYID, body ) ARG( SHAPEDEF, def ) ARG( CAPSULE, capsule ) ) +B3_REC_OP( 0x42, CreateHullShape, RET_SHAPEID, ARG( BODYID, body ) ARG( SHAPEDEF, def ) ARG( GEOMID, geometryId ) ) +B3_REC_OP( 0x43, CreateMeshShape, RET_SHAPEID, ARG( BODYID, body ) ARG( SHAPEDEF, def ) ARG( GEOMID, geometryId ) ARG( VEC3, scale ) ) +B3_REC_OP( 0x44, CreateHeightFieldShape, RET_SHAPEID, ARG( BODYID, body ) ARG( SHAPEDEF, def ) ARG( GEOMID, geometryId ) ) +B3_REC_OP( 0x45, CreateCompoundShape, RET_SHAPEID, ARG( BODYID, body ) ARG( SHAPEDEF, def ) ARG( GEOMID, geometryId ) ) +B3_REC_OP( 0x46, DestroyShape, RET_NONE, ARG( SHAPEID, shape ) ARG( BOOL, updateBodyMass ) ) + +// Shape mutators +B3_REC_OP( 0x50, ShapeSetDensity, RET_NONE, ARG( SHAPEID, shape ) ARG( F32, density ) ARG( BOOL, updateBodyMass ) ) +B3_REC_OP( 0x51, ShapeSetFriction, RET_NONE, ARG( SHAPEID, shape ) ARG( F32, friction ) ) +B3_REC_OP( 0x52, ShapeSetRestitution, RET_NONE, ARG( SHAPEID, shape ) ARG( F32, restitution ) ) +B3_REC_OP( 0x53, ShapeSetSurfaceMaterial, RET_NONE, ARG( SHAPEID, shape ) ARG( MATERIAL, material ) ) +B3_REC_OP( 0x54, ShapeSetFilter, RET_NONE, ARG( SHAPEID, shape ) ARG( FILTER, filter ) ARG( BOOL, invokeContacts ) ) +B3_REC_OP( 0x55, ShapeEnableSensorEvents, RET_NONE, ARG( SHAPEID, shape ) ARG( BOOL, flag ) ) +B3_REC_OP( 0x56, ShapeEnableContactEvents, RET_NONE, ARG( SHAPEID, shape ) ARG( BOOL, flag ) ) +B3_REC_OP( 0x57, ShapeEnablePreSolveEvents, RET_NONE, ARG( SHAPEID, shape ) ARG( BOOL, flag ) ) +B3_REC_OP( 0x58, ShapeEnableHitEvents, RET_NONE, ARG( SHAPEID, shape ) ARG( BOOL, flag ) ) +B3_REC_OP( 0x59, ShapeSetSphere, RET_NONE, ARG( SHAPEID, shape ) ARG( SPHERE, sphere ) ) +B3_REC_OP( 0x5A, ShapeSetCapsule, RET_NONE, ARG( SHAPEID, shape ) ARG( CAPSULE, capsule ) ) +B3_REC_OP( 0x5B, ShapeApplyWind, RET_NONE, + ARG( SHAPEID, shape ) ARG( VEC3, wind ) ARG( F32, drag ) ARG( F32, lift ) ARG( F32, maxSpeed ) ARG( BOOL, wake ) ) +B3_REC_OP( 0x5C, ShapeSetName, RET_NONE, ARG( SHAPEID, shape ) ARG( STR, name ) ) + +// Joint create and destroy +B3_REC_OP( 0x90, CreateParallelJoint, RET_JOINTID, ARG( WORLDID, world ) ARG( PARALLELJOINTDEF, def ) ) +B3_REC_OP( 0x91, CreateDistanceJoint, RET_JOINTID, ARG( WORLDID, world ) ARG( DISTANCEJOINTDEF, def ) ) +B3_REC_OP( 0x92, CreateFilterJoint, RET_JOINTID, ARG( WORLDID, world ) ARG( FILTERJOINTDEF, def ) ) +B3_REC_OP( 0x93, CreateMotorJoint, RET_JOINTID, ARG( WORLDID, world ) ARG( MOTORJOINTDEF, def ) ) +B3_REC_OP( 0x94, CreatePrismaticJoint, RET_JOINTID, ARG( WORLDID, world ) ARG( PRISMATICJOINTDEF, def ) ) +B3_REC_OP( 0x95, CreateRevoluteJoint, RET_JOINTID, ARG( WORLDID, world ) ARG( REVOLUTEJOINTDEF, def ) ) +B3_REC_OP( 0x96, CreateSphericalJoint, RET_JOINTID, ARG( WORLDID, world ) ARG( SPHERICALJOINTDEF, def ) ) +B3_REC_OP( 0x97, CreateWeldJoint, RET_JOINTID, ARG( WORLDID, world ) ARG( WELDJOINTDEF, def ) ) +B3_REC_OP( 0x98, CreateWheelJoint, RET_JOINTID, ARG( WORLDID, world ) ARG( WHEELJOINTDEF, def ) ) +B3_REC_OP( 0x99, DestroyJoint, RET_NONE, ARG( JOINTID, joint ) ARG( BOOL, wakeAttached ) ) + +// Generic joint mutators +B3_REC_OP( 0x9A, JointSetLocalFrameA, RET_NONE, ARG( JOINTID, joint ) ARG( TRANSFORM, localFrame ) ) +B3_REC_OP( 0x9B, JointSetLocalFrameB, RET_NONE, ARG( JOINTID, joint ) ARG( TRANSFORM, localFrame ) ) +B3_REC_OP( 0x9C, JointSetCollideConnected, RET_NONE, ARG( JOINTID, joint ) ARG( BOOL, shouldCollide ) ) +B3_REC_OP( 0x9D, JointWakeBodies, RET_NONE, ARG( JOINTID, joint ) ) +B3_REC_OP( 0x9E, JointSetConstraintTuning, RET_NONE, ARG( JOINTID, joint ) ARG( F32, hertz ) ARG( F32, dampingRatio ) ) +B3_REC_OP( 0x9F, JointSetForceThreshold, RET_NONE, ARG( JOINTID, joint ) ARG( F32, threshold ) ) +B3_REC_OP( 0xA0, JointSetTorqueThreshold, RET_NONE, ARG( JOINTID, joint ) ARG( F32, threshold ) ) + +// Parallel joint +B3_REC_OP( 0xA1, ParallelJointSetSpringHertz, RET_NONE, ARG( JOINTID, joint ) ARG( F32, hertz ) ) +B3_REC_OP( 0xA2, ParallelJointSetSpringDampingRatio, RET_NONE, ARG( JOINTID, joint ) ARG( F32, dampingRatio ) ) +B3_REC_OP( 0xA3, ParallelJointSetMaxTorque, RET_NONE, ARG( JOINTID, joint ) ARG( F32, maxTorque ) ) + +// Distance joint +B3_REC_OP( 0xA4, DistanceJointSetLength, RET_NONE, ARG( JOINTID, joint ) ARG( F32, length ) ) +B3_REC_OP( 0xA5, DistanceJointEnableSpring, RET_NONE, ARG( JOINTID, joint ) ARG( BOOL, enableSpring ) ) +B3_REC_OP( 0xA6, DistanceJointSetSpringForceRange, RET_NONE, ARG( JOINTID, joint ) ARG( F32, lowerForce ) ARG( F32, upperForce ) ) +B3_REC_OP( 0xA7, DistanceJointSetSpringHertz, RET_NONE, ARG( JOINTID, joint ) ARG( F32, hertz ) ) +B3_REC_OP( 0xA8, DistanceJointSetSpringDampingRatio, RET_NONE, ARG( JOINTID, joint ) ARG( F32, dampingRatio ) ) +B3_REC_OP( 0xA9, DistanceJointEnableLimit, RET_NONE, ARG( JOINTID, joint ) ARG( BOOL, enableLimit ) ) +B3_REC_OP( 0xAA, DistanceJointSetLengthRange, RET_NONE, ARG( JOINTID, joint ) ARG( F32, minLength ) ARG( F32, maxLength ) ) +B3_REC_OP( 0xAB, DistanceJointEnableMotor, RET_NONE, ARG( JOINTID, joint ) ARG( BOOL, enableMotor ) ) +B3_REC_OP( 0xAC, DistanceJointSetMotorSpeed, RET_NONE, ARG( JOINTID, joint ) ARG( F32, motorSpeed ) ) +B3_REC_OP( 0xAD, DistanceJointSetMaxMotorForce, RET_NONE, ARG( JOINTID, joint ) ARG( F32, force ) ) + +// Motor joint +B3_REC_OP( 0xAE, MotorJointSetLinearVelocity, RET_NONE, ARG( JOINTID, joint ) ARG( VEC3, velocity ) ) +B3_REC_OP( 0xAF, MotorJointSetAngularVelocity, RET_NONE, ARG( JOINTID, joint ) ARG( VEC3, velocity ) ) +B3_REC_OP( 0xB0, MotorJointSetMaxVelocityForce, RET_NONE, ARG( JOINTID, joint ) ARG( F32, maxForce ) ) +B3_REC_OP( 0xB1, MotorJointSetMaxVelocityTorque, RET_NONE, ARG( JOINTID, joint ) ARG( F32, maxTorque ) ) +B3_REC_OP( 0xB2, MotorJointSetLinearHertz, RET_NONE, ARG( JOINTID, joint ) ARG( F32, hertz ) ) +B3_REC_OP( 0xB3, MotorJointSetLinearDampingRatio, RET_NONE, ARG( JOINTID, joint ) ARG( F32, damping ) ) +B3_REC_OP( 0xB4, MotorJointSetAngularHertz, RET_NONE, ARG( JOINTID, joint ) ARG( F32, hertz ) ) +B3_REC_OP( 0xB5, MotorJointSetAngularDampingRatio, RET_NONE, ARG( JOINTID, joint ) ARG( F32, damping ) ) +B3_REC_OP( 0xB6, MotorJointSetMaxSpringForce, RET_NONE, ARG( JOINTID, joint ) ARG( F32, maxForce ) ) +B3_REC_OP( 0xB7, MotorJointSetMaxSpringTorque, RET_NONE, ARG( JOINTID, joint ) ARG( F32, maxTorque ) ) + +// Prismatic joint +B3_REC_OP( 0xB8, PrismaticJointEnableSpring, RET_NONE, ARG( JOINTID, joint ) ARG( BOOL, enableSpring ) ) +B3_REC_OP( 0xB9, PrismaticJointSetSpringHertz, RET_NONE, ARG( JOINTID, joint ) ARG( F32, hertz ) ) +B3_REC_OP( 0xBA, PrismaticJointSetSpringDampingRatio, RET_NONE, ARG( JOINTID, joint ) ARG( F32, dampingRatio ) ) +B3_REC_OP( 0xBB, PrismaticJointSetTargetTranslation, RET_NONE, ARG( JOINTID, joint ) ARG( F32, translation ) ) +B3_REC_OP( 0xBC, PrismaticJointEnableLimit, RET_NONE, ARG( JOINTID, joint ) ARG( BOOL, enableLimit ) ) +B3_REC_OP( 0xBD, PrismaticJointSetLimits, RET_NONE, ARG( JOINTID, joint ) ARG( F32, lower ) ARG( F32, upper ) ) +B3_REC_OP( 0xBE, PrismaticJointEnableMotor, RET_NONE, ARG( JOINTID, joint ) ARG( BOOL, enableMotor ) ) +B3_REC_OP( 0xBF, PrismaticJointSetMotorSpeed, RET_NONE, ARG( JOINTID, joint ) ARG( F32, motorSpeed ) ) +B3_REC_OP( 0xC0, PrismaticJointSetMaxMotorForce, RET_NONE, ARG( JOINTID, joint ) ARG( F32, force ) ) + +// Revolute joint +B3_REC_OP( 0xC1, RevoluteJointEnableSpring, RET_NONE, ARG( JOINTID, joint ) ARG( BOOL, enableSpring ) ) +B3_REC_OP( 0xC2, RevoluteJointSetSpringHertz, RET_NONE, ARG( JOINTID, joint ) ARG( F32, hertz ) ) +B3_REC_OP( 0xC3, RevoluteJointSetSpringDampingRatio, RET_NONE, ARG( JOINTID, joint ) ARG( F32, dampingRatio ) ) +B3_REC_OP( 0xC4, RevoluteJointSetTargetAngle, RET_NONE, ARG( JOINTID, joint ) ARG( F32, angle ) ) +B3_REC_OP( 0xC5, RevoluteJointEnableLimit, RET_NONE, ARG( JOINTID, joint ) ARG( BOOL, enableLimit ) ) +B3_REC_OP( 0xC6, RevoluteJointSetLimits, RET_NONE, ARG( JOINTID, joint ) ARG( F32, lower ) ARG( F32, upper ) ) +B3_REC_OP( 0xC7, RevoluteJointEnableMotor, RET_NONE, ARG( JOINTID, joint ) ARG( BOOL, enableMotor ) ) +B3_REC_OP( 0xC8, RevoluteJointSetMotorSpeed, RET_NONE, ARG( JOINTID, joint ) ARG( F32, motorSpeed ) ) +B3_REC_OP( 0xC9, RevoluteJointSetMaxMotorTorque, RET_NONE, ARG( JOINTID, joint ) ARG( F32, torque ) ) + +// Spherical joint +B3_REC_OP( 0xCA, SphericalJointEnableConeLimit, RET_NONE, ARG( JOINTID, joint ) ARG( BOOL, enableLimit ) ) +B3_REC_OP( 0xCB, SphericalJointSetConeLimit, RET_NONE, ARG( JOINTID, joint ) ARG( F32, angleRadians ) ) +B3_REC_OP( 0xCC, SphericalJointEnableTwistLimit, RET_NONE, ARG( JOINTID, joint ) ARG( BOOL, enableLimit ) ) +B3_REC_OP( 0xCD, SphericalJointSetTwistLimits, RET_NONE, ARG( JOINTID, joint ) ARG( F32, lower ) ARG( F32, upper ) ) +B3_REC_OP( 0xCE, SphericalJointEnableSpring, RET_NONE, ARG( JOINTID, joint ) ARG( BOOL, enableSpring ) ) +B3_REC_OP( 0xCF, SphericalJointSetSpringHertz, RET_NONE, ARG( JOINTID, joint ) ARG( F32, hertz ) ) +B3_REC_OP( 0xD0, SphericalJointSetSpringDampingRatio, RET_NONE, ARG( JOINTID, joint ) ARG( F32, dampingRatio ) ) +B3_REC_OP( 0xD1, SphericalJointSetTargetRotation, RET_NONE, ARG( JOINTID, joint ) ARG( QUAT, targetRotation ) ) +B3_REC_OP( 0xD2, SphericalJointEnableMotor, RET_NONE, ARG( JOINTID, joint ) ARG( BOOL, enableMotor ) ) +B3_REC_OP( 0xD3, SphericalJointSetMotorVelocity, RET_NONE, ARG( JOINTID, joint ) ARG( VEC3, motorVelocity ) ) +B3_REC_OP( 0xD4, SphericalJointSetMaxMotorTorque, RET_NONE, ARG( JOINTID, joint ) ARG( F32, torque ) ) + +// Weld joint +B3_REC_OP( 0xD5, WeldJointSetLinearHertz, RET_NONE, ARG( JOINTID, joint ) ARG( F32, hertz ) ) +B3_REC_OP( 0xD6, WeldJointSetLinearDampingRatio, RET_NONE, ARG( JOINTID, joint ) ARG( F32, dampingRatio ) ) +B3_REC_OP( 0xD7, WeldJointSetAngularHertz, RET_NONE, ARG( JOINTID, joint ) ARG( F32, hertz ) ) +B3_REC_OP( 0xD8, WeldJointSetAngularDampingRatio, RET_NONE, ARG( JOINTID, joint ) ARG( F32, dampingRatio ) ) + +// Wheel joint +B3_REC_OP( 0xD9, WheelJointEnableSuspension, RET_NONE, ARG( JOINTID, joint ) ARG( BOOL, flag ) ) +B3_REC_OP( 0xDA, WheelJointSetSuspensionHertz, RET_NONE, ARG( JOINTID, joint ) ARG( F32, hertz ) ) +B3_REC_OP( 0xDB, WheelJointSetSuspensionDampingRatio, RET_NONE, ARG( JOINTID, joint ) ARG( F32, dampingRatio ) ) +B3_REC_OP( 0xDC, WheelJointEnableSuspensionLimit, RET_NONE, ARG( JOINTID, joint ) ARG( BOOL, flag ) ) +B3_REC_OP( 0xDD, WheelJointSetSuspensionLimits, RET_NONE, ARG( JOINTID, joint ) ARG( F32, lower ) ARG( F32, upper ) ) +B3_REC_OP( 0xDE, WheelJointEnableSpinMotor, RET_NONE, ARG( JOINTID, joint ) ARG( BOOL, flag ) ) +B3_REC_OP( 0xDF, WheelJointSetSpinMotorSpeed, RET_NONE, ARG( JOINTID, joint ) ARG( F32, speed ) ) + +// Wheel joint continued, overflow past the 0xDF range. +B3_REC_OP( 0xE0, WheelJointSetMaxSpinTorque, RET_NONE, ARG( JOINTID, joint ) ARG( F32, torque ) ) +B3_REC_OP( 0xE1, WheelJointEnableSteering, RET_NONE, ARG( JOINTID, joint ) ARG( BOOL, flag ) ) +B3_REC_OP( 0xE2, WheelJointSetSteeringHertz, RET_NONE, ARG( JOINTID, joint ) ARG( F32, hertz ) ) +B3_REC_OP( 0xE3, WheelJointSetSteeringDampingRatio, RET_NONE, ARG( JOINTID, joint ) ARG( F32, dampingRatio ) ) +B3_REC_OP( 0xE4, WheelJointSetMaxSteeringTorque, RET_NONE, ARG( JOINTID, joint ) ARG( F32, torque ) ) +B3_REC_OP( 0xE5, WheelJointEnableSteeringLimit, RET_NONE, ARG( JOINTID, joint ) ARG( BOOL, flag ) ) +B3_REC_OP( 0xE6, WheelJointSetSteeringLimits, RET_NONE, ARG( JOINTID, joint ) ARG( F32, lower ) ARG( F32, upper ) ) +B3_REC_OP( 0xE7, WheelJointSetTargetSteeringAngle, RET_NONE, ARG( JOINTID, joint ) ARG( F32, radians ) ) + +// Spatial queries. Inputs flow through the manifest (reader side). The hit tail and result are +// hand-written in recording.c / recording_replay.c since they are variable length. +B3_REC_OP( 0xE8, QueryOverlapAABB, RET_NONE, ARG( WORLDID, world ) ARG( AABB, aabb ) ARG( QUERYFILTER, filter ) ) +B3_REC_OP( 0xE9, QueryOverlapShape, RET_NONE, + ARG( WORLDID, world ) ARG( POSITION, origin ) ARG( SHAPEPROXY, proxy ) ARG( QUERYFILTER, filter ) ) +B3_REC_OP( 0xEA, QueryCastRay, RET_NONE, + ARG( WORLDID, world ) ARG( POSITION, origin ) ARG( VEC3, translation ) ARG( QUERYFILTER, filter ) ) +B3_REC_OP( 0xEB, QueryCastShape, RET_NONE, + ARG( WORLDID, world ) ARG( POSITION, origin ) ARG( SHAPEPROXY, proxy ) ARG( VEC3, translation ) + ARG( QUERYFILTER, filter ) ) +B3_REC_OP( 0xEC, QueryCastRayClosest, RET_NONE, + ARG( WORLDID, world ) ARG( POSITION, origin ) ARG( VEC3, translation ) ARG( QUERYFILTER, filter ) ) +B3_REC_OP( 0xED, QueryCastMover, RET_NONE, + ARG( WORLDID, world ) ARG( POSITION, origin ) ARG( CAPSULE, mover ) ARG( VEC3, translation ) ARG( QUERYFILTER, filter ) ) +B3_REC_OP( 0xEE, QueryCollideMover, RET_NONE, + ARG( WORLDID, world ) ARG( POSITION, origin ) ARG( CAPSULE, mover ) ARG( QUERYFILTER, filter ) ) + +// Identity key (hash of the caller id + label) for the query that immediately follows. Emitted only +// for a tagged query. The id and label are interned in the trailing tag table, so only the 8 byte key +// rides the stream. +B3_REC_OP( 0xEF, QueryTag, RET_NONE, ARG( U64, key ) ) + +B3_REC_OP( 0xF1, StateHash, RET_NONE, ARG( WORLDID, world ) ARG( U64, hash ) ) + +// Accumulated world bounds over the whole recording, written once at stop. +B3_REC_OP( 0xF2, RecordingBounds, RET_NONE, ARG( AABB, bounds ) ) diff --git a/vendor/box3d/src/src/recording_replay.c b/vendor/box3d/src/src/recording_replay.c new file mode 100644 index 000000000..584912d59 --- /dev/null +++ b/vendor/box3d/src/src/recording_replay.c @@ -0,0 +1,3634 @@ +// SPDX-FileCopyrightText: 2026 Erin Catto +// SPDX-License-Identifier: MIT + +#if defined( _MSC_VER ) && !defined( _CRT_SECURE_NO_WARNINGS ) +#define _CRT_SECURE_NO_WARNINGS +#endif + +#include "recording_replay.h" + +#include "body.h" +#include "compound.h" +#include "physics_world.h" +#include "world_snapshot.h" + +#include "box3d/box3d.h" + +#include +#include +#include +#include + +// Read primitives + +static void b3RecRdrCheck( b3RecReader* rdr, int size ) +{ + if ( size < 0 || (int64_t)rdr->cursor + (int64_t)size > (int64_t)rdr->size ) + { + rdr->ok = false; + } +} + +static void b3RecRdrBlob( b3RecReader* rdr, void* out, int size ) +{ + b3RecRdrCheck( rdr, size ); + if ( !rdr->ok ) + { + memset( out, 0, (size_t)size ); + return; + } + memcpy( out, rdr->data + rdr->cursor, (size_t)size ); + rdr->cursor += size; +} + +uint8_t b3RecR_U8( b3RecReader* rdr ) +{ + b3RecRdrCheck( rdr, 1 ); + if ( !rdr->ok ) + { + return 0; + } + return rdr->data[rdr->cursor++]; +} + +uint16_t b3RecR_U16( b3RecReader* rdr ) +{ + b3RecRdrCheck( rdr, 2 ); + if ( !rdr->ok ) + { + return 0; + } + uint16_t v = (uint16_t)rdr->data[rdr->cursor] | ( (uint16_t)rdr->data[rdr->cursor + 1] << 8 ); + rdr->cursor += 2; + return v; +} + +uint32_t b3RecR_U24( b3RecReader* rdr ) +{ + b3RecRdrCheck( rdr, 3 ); + if ( !rdr->ok ) + { + return 0; + } + uint32_t v = (uint32_t)rdr->data[rdr->cursor] | ( (uint32_t)rdr->data[rdr->cursor + 1] << 8 ) | + ( (uint32_t)rdr->data[rdr->cursor + 2] << 16 ); + rdr->cursor += 3; + return v; +} + +uint32_t b3RecR_U32( b3RecReader* rdr ) +{ + b3RecRdrCheck( rdr, 4 ); + if ( !rdr->ok ) + { + return 0; + } + uint32_t v = (uint32_t)rdr->data[rdr->cursor] | ( (uint32_t)rdr->data[rdr->cursor + 1] << 8 ) | + ( (uint32_t)rdr->data[rdr->cursor + 2] << 16 ) | ( (uint32_t)rdr->data[rdr->cursor + 3] << 24 ); + rdr->cursor += 4; + return v; +} + +uint64_t b3RecR_U64( b3RecReader* rdr ) +{ + b3RecRdrCheck( rdr, 8 ); + if ( !rdr->ok ) + { + return 0; + } + uint64_t v = (uint64_t)rdr->data[rdr->cursor] | ( (uint64_t)rdr->data[rdr->cursor + 1] << 8 ) | + ( (uint64_t)rdr->data[rdr->cursor + 2] << 16 ) | ( (uint64_t)rdr->data[rdr->cursor + 3] << 24 ) | + ( (uint64_t)rdr->data[rdr->cursor + 4] << 32 ) | ( (uint64_t)rdr->data[rdr->cursor + 5] << 40 ) | + ( (uint64_t)rdr->data[rdr->cursor + 6] << 48 ) | ( (uint64_t)rdr->data[rdr->cursor + 7] << 56 ); + rdr->cursor += 8; + return v; +} + +int32_t b3RecR_I32( b3RecReader* rdr ) +{ + return (int32_t)b3RecR_U32( rdr ); +} + +float b3RecR_F32( b3RecReader* rdr ) +{ + uint32_t bits = b3RecR_U32( rdr ); + float v; + memcpy( &v, &bits, 4 ); + return v; +} + +double b3RecR_F64( b3RecReader* rdr ) +{ + uint64_t bits = b3RecR_U64( rdr ); + double v; + memcpy( &v, &bits, 8 ); + return v; +} + +bool b3RecR_BOOL( b3RecReader* rdr ) +{ + return b3RecR_U8( rdr ) != 0u; +} + +b3Vec3 b3RecR_VEC3( b3RecReader* rdr ) +{ + b3Vec3 v; + v.x = b3RecR_F32( rdr ); + v.y = b3RecR_F32( rdr ); + v.z = b3RecR_F32( rdr ); + return v; +} + +b3Quat b3RecR_QUAT( b3RecReader* rdr ) +{ + b3Quat q; + q.v.x = b3RecR_F32( rdr ); + q.v.y = b3RecR_F32( rdr ); + q.v.z = b3RecR_F32( rdr ); + q.s = b3RecR_F32( rdr ); + return q; +} + +b3Transform b3RecR_TRANSFORM( b3RecReader* rdr ) +{ + b3Transform t; + t.p = b3RecR_VEC3( rdr ); + t.q = b3RecR_QUAT( rdr ); + return t; +} + +b3Pos b3RecR_POSITION( b3RecReader* rdr ) +{ + b3Pos p; +#if defined( BOX3D_DOUBLE_PRECISION ) + p.x = b3RecR_F64( rdr ); + p.y = b3RecR_F64( rdr ); + p.z = b3RecR_F64( rdr ); +#else + p.x = b3RecR_F32( rdr ); + p.y = b3RecR_F32( rdr ); + p.z = b3RecR_F32( rdr ); +#endif + return p; +} + +b3WorldTransform b3RecR_WORLDXF( b3RecReader* rdr ) +{ + b3WorldTransform t; + t.p = b3RecR_POSITION( rdr ); + t.q = b3RecR_QUAT( rdr ); + return t; +} + +b3Matrix3 b3RecR_MATRIX3( b3RecReader* rdr ) +{ + b3Matrix3 m; + m.cx = b3RecR_VEC3( rdr ); + m.cy = b3RecR_VEC3( rdr ); + m.cz = b3RecR_VEC3( rdr ); + return m; +} + +b3AABB b3RecR_AABB( b3RecReader* rdr ) +{ + b3AABB v; + v.lowerBound = b3RecR_VEC3( rdr ); + v.upperBound = b3RecR_VEC3( rdr ); + return v; +} + +b3QueryFilter b3RecR_QUERYFILTER( b3RecReader* rdr ) +{ + // id and name are not on the wire here, they ride the separate QueryTag op. Start from the default + // so they keep the untagged sentinel instead of garbage. + b3QueryFilter f = b3DefaultQueryFilter(); + f.categoryBits = b3RecR_U64( rdr ); + f.maskBits = b3RecR_U64( rdr ); + return f; +} + +// Reserve reader scratch for a count taken from an untrusted file. Every recorded element +// consumes at least one byte, so a valid count can never exceed the bytes left in the file. +// Reject anything larger (or negative, or that would overflow the byte size) by failing the read +// rather than allocating wildly. A grow keeps the old contents so callers can accumulate across +// reserves, as the collide-mover dispatcher does one shape group at a time. +static bool b3RecReserveScratch( b3RecReader* rdr, void** data, int* cap, int need, int elemSize ) +{ + int remaining = rdr->size - rdr->cursor; + if ( need < 0 || remaining < 0 || need > remaining || need > INT_MAX / elemSize ) + { + rdr->ok = false; + return false; + } + if ( need <= *cap ) + { + return true; + } + int newCap = need <= INT_MAX / elemSize - 8 ? need + 8 : need; + void* grown = b3Alloc( (size_t)newCap * (size_t)elemSize ); + if ( *data != NULL ) + { + memcpy( grown, *data, (size_t)*cap * (size_t)elemSize ); + b3Free( *data, (size_t)*cap * (size_t)elemSize ); + } + *data = grown; + *cap = newCap; + return true; +} + +// Variable length, mirrors b3RecW_SHAPEPROXY: count, count points, radius. The decoded proxy borrows +// the reader's scratch for its point cloud, valid until the next proxy read. +b3ShapeProxy b3RecR_SHAPEPROXY( b3RecReader* rdr ) +{ + b3ShapeProxy p = { 0 }; + int count = b3RecR_I32( rdr ); + if ( count < 0 ) + count = 0; + if ( count > B3_MAX_SHAPE_CAST_POINTS ) + count = B3_MAX_SHAPE_CAST_POINTS; + if ( count > 0 && + b3RecReserveScratch( rdr, (void**)&rdr->proxyScratch, &rdr->proxyScratchCap, count, (int)sizeof( b3Vec3 ) ) ) + { + for ( int i = 0; i < count; ++i ) + { + rdr->proxyScratch[i] = b3RecR_VEC3( rdr ); + } + p.points = rdr->proxyScratch; + p.count = count; + } + p.radius = b3RecR_F32( rdr ); + return p; +} + +b3TreeStats b3RecR_TREESTATS( b3RecReader* rdr ) +{ + b3TreeStats v; + v.nodeVisits = b3RecR_I32( rdr ); + v.leafVisits = b3RecR_I32( rdr ); + return v; +} + +b3RayResult b3RecR_RAYRESULT( b3RecReader* rdr ) +{ + b3RayResult v = { 0 }; + // shapeId keeps the recorded world0; b3RecMakeShapeId is applied at compare time + v.shapeId = b3RecR_SHAPEID( rdr ); + v.point = b3RecR_POSITION( rdr ); + v.normal = b3RecR_VEC3( rdr ); + v.userMaterialId = b3RecR_U64( rdr ); + v.fraction = b3RecR_F32( rdr ); + v.triangleIndex = b3RecR_I32( rdr ); + v.childIndex = b3RecR_I32( rdr ); + v.hit = b3RecR_BOOL( rdr ); + return v; +} + +b3PlaneResult b3RecR_PLANERESULT( b3RecReader* rdr ) +{ + b3PlaneResult v; + v.plane.normal = b3RecR_VEC3( rdr ); + v.plane.offset = b3RecR_F32( rdr ); + v.point = b3RecR_VEC3( rdr ); + return v; +} + +b3WorldId b3RecR_WORLDID( b3RecReader* rdr ) +{ + return b3LoadWorldId( b3RecR_U32( rdr ) ); +} + +b3BodyId b3RecR_BODYID( b3RecReader* rdr ) +{ + return b3LoadBodyId( b3RecR_U64( rdr ) ); +} + +b3ShapeId b3RecR_SHAPEID( b3RecReader* rdr ) +{ + return b3LoadShapeId( b3RecR_U64( rdr ) ); +} + +b3JointId b3RecR_JOINTID( b3RecReader* rdr ) +{ + return b3LoadJointId( b3RecR_U64( rdr ) ); +} + +b3Sphere b3RecR_SPHERE( b3RecReader* rdr ) +{ + b3Sphere s; + b3RecRdrBlob( rdr, &s, (int)sizeof( s ) ); + return s; +} + +b3Capsule b3RecR_CAPSULE( b3RecReader* rdr ) +{ + b3Capsule c; + b3RecRdrBlob( rdr, &c, (int)sizeof( c ) ); + return c; +} + +uint32_t b3RecR_GEOMID( b3RecReader* rdr ) +{ + return b3RecR_U32( rdr ); +} + +b3Filter b3RecR_FILTER( b3RecReader* rdr ) +{ + b3Filter f; + f.categoryBits = b3RecR_U64( rdr ); + f.maskBits = b3RecR_U64( rdr ); + f.groupIndex = b3RecR_I32( rdr ); + return f; +} + +b3SurfaceMaterial b3RecR_MATERIAL( b3RecReader* rdr ) +{ + b3SurfaceMaterial m = b3DefaultSurfaceMaterial(); + m.friction = b3RecR_F32( rdr ); + m.restitution = b3RecR_F32( rdr ); + m.rollingResistance = b3RecR_F32( rdr ); + m.tangentVelocity = b3RecR_VEC3( rdr ); + m.userMaterialId = b3RecR_U64( rdr ); + m.customColor = b3RecR_U32( rdr ); + return m; +} + +b3MassData b3RecR_MASSDATA( b3RecReader* rdr ) +{ + b3MassData md; + md.mass = b3RecR_F32( rdr ); + md.center = b3RecR_VEC3( rdr ); + md.inertia = b3RecR_MATRIX3( rdr ); + return md; +} + +b3MotionLocks b3RecR_LOCKS( b3RecReader* rdr ) +{ + b3MotionLocks locks; + locks.linearX = b3RecR_BOOL( rdr ); + locks.linearY = b3RecR_BOOL( rdr ); + locks.linearZ = b3RecR_BOOL( rdr ); + locks.angularX = b3RecR_BOOL( rdr ); + locks.angularY = b3RecR_BOOL( rdr ); + locks.angularZ = b3RecR_BOOL( rdr ); + return locks; +} + +// Rotating set of static string buffers, valid until the next 4 STR reads. +const char* b3RecR_STR( b3RecReader* rdr ) +{ + char* buf = rdr->stringBuffers[rdr->nextString]; + rdr->nextString = ( rdr->nextString + 1 ) & 3; + + uint16_t len = b3RecR_U16( rdr ); + if ( len == 0xFFFFu ) + { + return NULL; + } + + int n = (int)len; + if ( n > B3_MAX_NAME_LENGTH ) + { + n = B3_MAX_NAME_LENGTH; + } + b3RecRdrCheck( rdr, (int)len ); + if ( rdr->ok && n > 0 ) + { + memcpy( buf, rdr->data + rdr->cursor, (size_t)n ); + } + rdr->cursor += (int)len; + buf[n] = '\0'; + return buf; +} + +// Def readers: start from b3Default*Def() then overlay each serialized field +// in the exact order the writer produced them. + +b3ExplosionDef b3RecR_EXPLOSIONDEF( b3RecReader* rdr ) +{ + b3ExplosionDef def = b3DefaultExplosionDef(); + def.maskBits = b3RecR_U64( rdr ); + def.position = b3RecR_POSITION( rdr ); + def.radius = b3RecR_F32( rdr ); + def.falloff = b3RecR_F32( rdr ); + def.impulsePerArea = b3RecR_F32( rdr ); + return def; +} + +b3BodyDef b3RecR_BODYDEF( b3RecReader* rdr ) +{ + b3BodyDef def = b3DefaultBodyDef(); + def.type = (b3BodyType)b3RecR_I32( rdr ); + def.position = b3RecR_POSITION( rdr ); + def.rotation = b3RecR_QUAT( rdr ); + def.linearVelocity = b3RecR_VEC3( rdr ); + def.angularVelocity = b3RecR_VEC3( rdr ); + def.linearDamping = b3RecR_F32( rdr ); + def.angularDamping = b3RecR_F32( rdr ); + def.gravityScale = b3RecR_F32( rdr ); + def.sleepThreshold = b3RecR_F32( rdr ); + def.name = b3RecR_STR( rdr ); + (void)b3RecR_U64( rdr ); // userData placeholder + def.motionLocks = b3RecR_LOCKS( rdr ); + def.enableSleep = b3RecR_BOOL( rdr ); + def.isAwake = b3RecR_BOOL( rdr ); + def.isBullet = b3RecR_BOOL( rdr ); + def.isEnabled = b3RecR_BOOL( rdr ); + def.allowFastRotation = b3RecR_BOOL( rdr ); + def.enableContactRecycling = b3RecR_BOOL( rdr ); + def.userData = NULL; + return def; +} + +b3ShapeDef b3RecR_SHAPEDEF( b3RecReader* rdr ) +{ + b3ShapeDef def = b3DefaultShapeDef(); + + def.name = b3RecR_STR( rdr ); + (void)b3RecR_U64( rdr ); // userData placeholder + + int matCount = b3RecR_I32( rdr ); + if ( matCount < 0 ) + { + matCount = 0; + } + if ( matCount > 0 && + b3RecReserveScratch( rdr, (void**)&rdr->matScratch, &rdr->matScratchCap, matCount, (int)sizeof( b3SurfaceMaterial ) ) ) + { + for ( int i = 0; i < matCount; ++i ) + { + rdr->matScratch[i] = b3RecR_MATERIAL( rdr ); + } + def.materials = rdr->matScratch; + def.materialCount = matCount; + } + else + { + for ( int i = 0; i < matCount; ++i ) + { + (void)b3RecR_MATERIAL( rdr ); + } + def.materials = NULL; + def.materialCount = 0; + } + + def.baseMaterial = b3RecR_MATERIAL( rdr ); + def.density = b3RecR_F32( rdr ); + def.explosionScale = b3RecR_F32( rdr ); + def.filter = b3RecR_FILTER( rdr ); + def.enableCustomFiltering = b3RecR_BOOL( rdr ); + def.isSensor = b3RecR_BOOL( rdr ); + def.enableSensorEvents = b3RecR_BOOL( rdr ); + def.enableContactEvents = b3RecR_BOOL( rdr ); + def.enableHitEvents = b3RecR_BOOL( rdr ); + def.enablePreSolveEvents = b3RecR_BOOL( rdr ); + def.invokeContactCreation = b3RecR_BOOL( rdr ); + def.updateBodyMass = b3RecR_BOOL( rdr ); + def.enableSpeculativeContact = b3RecR_BOOL( rdr ); + def.userData = NULL; + return def; +} + +// Shared base for all joint defs. Body ids come in with recorded world0; callers remap them. +static void b3RecR_JointBase( b3RecReader* rdr, b3JointDef* base ) +{ + (void)b3RecR_U64( rdr ); // userData + base->bodyIdA = b3RecR_BODYID( rdr ); + base->bodyIdB = b3RecR_BODYID( rdr ); + base->localFrameA = b3RecR_TRANSFORM( rdr ); + base->localFrameB = b3RecR_TRANSFORM( rdr ); + base->forceThreshold = b3RecR_F32( rdr ); + base->torqueThreshold = b3RecR_F32( rdr ); + base->constraintHertz = b3RecR_F32( rdr ); + base->constraintDampingRatio = b3RecR_F32( rdr ); + base->drawScale = b3RecR_F32( rdr ); + base->collideConnected = b3RecR_BOOL( rdr ); + base->userData = NULL; +} + +b3ParallelJointDef b3RecR_PARALLELJOINTDEF( b3RecReader* rdr ) +{ + b3ParallelJointDef def = b3DefaultParallelJointDef(); + b3RecR_JointBase( rdr, &def.base ); + def.hertz = b3RecR_F32( rdr ); + def.dampingRatio = b3RecR_F32( rdr ); + def.maxTorque = b3RecR_F32( rdr ); + return def; +} + +b3DistanceJointDef b3RecR_DISTANCEJOINTDEF( b3RecReader* rdr ) +{ + b3DistanceJointDef def = b3DefaultDistanceJointDef(); + b3RecR_JointBase( rdr, &def.base ); + def.length = b3RecR_F32( rdr ); + def.enableSpring = b3RecR_BOOL( rdr ); + def.lowerSpringForce = b3RecR_F32( rdr ); + def.upperSpringForce = b3RecR_F32( rdr ); + def.hertz = b3RecR_F32( rdr ); + def.dampingRatio = b3RecR_F32( rdr ); + def.enableLimit = b3RecR_BOOL( rdr ); + def.minLength = b3RecR_F32( rdr ); + def.maxLength = b3RecR_F32( rdr ); + def.enableMotor = b3RecR_BOOL( rdr ); + def.maxMotorForce = b3RecR_F32( rdr ); + def.motorSpeed = b3RecR_F32( rdr ); + return def; +} + +b3FilterJointDef b3RecR_FILTERJOINTDEF( b3RecReader* rdr ) +{ + b3FilterJointDef def = b3DefaultFilterJointDef(); + b3RecR_JointBase( rdr, &def.base ); + return def; +} + +b3MotorJointDef b3RecR_MOTORJOINTDEF( b3RecReader* rdr ) +{ + b3MotorJointDef def = b3DefaultMotorJointDef(); + b3RecR_JointBase( rdr, &def.base ); + def.linearVelocity = b3RecR_VEC3( rdr ); + def.maxVelocityForce = b3RecR_F32( rdr ); + def.angularVelocity = b3RecR_VEC3( rdr ); + def.maxVelocityTorque = b3RecR_F32( rdr ); + def.linearHertz = b3RecR_F32( rdr ); + def.linearDampingRatio = b3RecR_F32( rdr ); + def.maxSpringForce = b3RecR_F32( rdr ); + def.angularHertz = b3RecR_F32( rdr ); + def.angularDampingRatio = b3RecR_F32( rdr ); + def.maxSpringTorque = b3RecR_F32( rdr ); + return def; +} + +b3PrismaticJointDef b3RecR_PRISMATICJOINTDEF( b3RecReader* rdr ) +{ + b3PrismaticJointDef def = b3DefaultPrismaticJointDef(); + b3RecR_JointBase( rdr, &def.base ); + def.enableSpring = b3RecR_BOOL( rdr ); + def.hertz = b3RecR_F32( rdr ); + def.dampingRatio = b3RecR_F32( rdr ); + def.targetTranslation = b3RecR_F32( rdr ); + def.enableLimit = b3RecR_BOOL( rdr ); + def.lowerTranslation = b3RecR_F32( rdr ); + def.upperTranslation = b3RecR_F32( rdr ); + def.enableMotor = b3RecR_BOOL( rdr ); + def.maxMotorForce = b3RecR_F32( rdr ); + def.motorSpeed = b3RecR_F32( rdr ); + return def; +} + +b3RevoluteJointDef b3RecR_REVOLUTEJOINTDEF( b3RecReader* rdr ) +{ + b3RevoluteJointDef def = b3DefaultRevoluteJointDef(); + b3RecR_JointBase( rdr, &def.base ); + def.targetAngle = b3RecR_F32( rdr ); + def.enableSpring = b3RecR_BOOL( rdr ); + def.hertz = b3RecR_F32( rdr ); + def.dampingRatio = b3RecR_F32( rdr ); + def.enableLimit = b3RecR_BOOL( rdr ); + def.lowerAngle = b3RecR_F32( rdr ); + def.upperAngle = b3RecR_F32( rdr ); + def.enableMotor = b3RecR_BOOL( rdr ); + def.maxMotorTorque = b3RecR_F32( rdr ); + def.motorSpeed = b3RecR_F32( rdr ); + return def; +} + +b3SphericalJointDef b3RecR_SPHERICALJOINTDEF( b3RecReader* rdr ) +{ + b3SphericalJointDef def = b3DefaultSphericalJointDef(); + b3RecR_JointBase( rdr, &def.base ); + def.enableSpring = b3RecR_BOOL( rdr ); + def.hertz = b3RecR_F32( rdr ); + def.dampingRatio = b3RecR_F32( rdr ); + def.targetRotation = b3RecR_QUAT( rdr ); + def.enableConeLimit = b3RecR_BOOL( rdr ); + def.coneAngle = b3RecR_F32( rdr ); + def.enableTwistLimit = b3RecR_BOOL( rdr ); + def.lowerTwistAngle = b3RecR_F32( rdr ); + def.upperTwistAngle = b3RecR_F32( rdr ); + def.enableMotor = b3RecR_BOOL( rdr ); + def.maxMotorTorque = b3RecR_F32( rdr ); + def.motorVelocity = b3RecR_VEC3( rdr ); + return def; +} + +b3WeldJointDef b3RecR_WELDJOINTDEF( b3RecReader* rdr ) +{ + b3WeldJointDef def = b3DefaultWeldJointDef(); + b3RecR_JointBase( rdr, &def.base ); + def.linearHertz = b3RecR_F32( rdr ); + def.angularHertz = b3RecR_F32( rdr ); + def.linearDampingRatio = b3RecR_F32( rdr ); + def.angularDampingRatio = b3RecR_F32( rdr ); + return def; +} + +b3WheelJointDef b3RecR_WHEELJOINTDEF( b3RecReader* rdr ) +{ + b3WheelJointDef def = b3DefaultWheelJointDef(); + b3RecR_JointBase( rdr, &def.base ); + def.enableSuspensionSpring = b3RecR_BOOL( rdr ); + def.suspensionHertz = b3RecR_F32( rdr ); + def.suspensionDampingRatio = b3RecR_F32( rdr ); + def.enableSuspensionLimit = b3RecR_BOOL( rdr ); + def.lowerSuspensionLimit = b3RecR_F32( rdr ); + def.upperSuspensionLimit = b3RecR_F32( rdr ); + def.enableSpinMotor = b3RecR_BOOL( rdr ); + def.maxSpinTorque = b3RecR_F32( rdr ); + def.spinSpeed = b3RecR_F32( rdr ); + def.enableSteering = b3RecR_BOOL( rdr ); + def.steeringHertz = b3RecR_F32( rdr ); + def.steeringDampingRatio = b3RecR_F32( rdr ); + def.targetSteeringAngle = b3RecR_F32( rdr ); + def.maxSteeringTorque = b3RecR_F32( rdr ); + def.enableSteeringLimit = b3RecR_BOOL( rdr ); + def.lowerSteeringLimit = b3RecR_F32( rdr ); + def.upperSteeringLimit = b3RecR_F32( rdr ); + return def; +} + +// Outliner body tracking. Defined after the player struct; forward declared here because the +// create/destroy dispatch sits above that struct. +static void b3RecTrackBodyCreate( b3RecPlayer* player, b3BodyId id ); +static void b3RecTrackBodyDestroy( b3RecPlayer* player, b3BodyId id ); + +// Id retargeting: replace world0 with the replay world's slot index. + +static b3BodyId b3RecMakeBodyId( b3RecReader* rdr, b3BodyId recorded ) +{ + b3BodyId id; + id.index1 = recorded.index1; + id.world0 = (uint16_t)( rdr->replayWorldId.index1 - 1u ); + id.generation = recorded.generation; + return id; +} + +static b3ShapeId b3RecMakeShapeId( b3RecReader* rdr, b3ShapeId recorded ) +{ + b3ShapeId id; + id.index1 = recorded.index1; + id.world0 = (uint16_t)( rdr->replayWorldId.index1 - 1u ); + id.generation = recorded.generation; + return id; +} + +static b3JointId b3RecMakeJointId( b3RecReader* rdr, b3JointId recorded ) +{ + b3JointId id; + id.index1 = recorded.index1; + id.world0 = (uint16_t)( rdr->replayWorldId.index1 - 1u ); + id.generation = recorded.generation; + return id; +} + +// A create op appends the returned id after args. index1 and generation must match; +// world0 always differs so we ignore it. +static void b3RecCheckId( b3RecReader* rdr, const char* kind, int gotIndex, unsigned gotGen, int recIndex, unsigned recGen ) +{ + if ( gotIndex != recIndex || gotGen != recGen ) + { + printf( "b3ReplayFile: %s id mismatch (rec index1=%d gen=%u, got index1=%d gen=%u)\n", kind, recIndex, recGen, gotIndex, + gotGen ); + rdr->ok = false; + } +} + +static void b3RecCheckBodyId( b3RecReader* rdr, b3BodyId got, b3BodyId rec ) +{ + b3RecCheckId( rdr, "body", got.index1, got.generation, rec.index1, rec.generation ); +} + +static void b3RecCheckShapeId( b3RecReader* rdr, b3ShapeId got, b3ShapeId rec ) +{ + b3RecCheckId( rdr, "shape", got.index1, got.generation, rec.index1, rec.generation ); +} + +static void b3RecCheckJointId( b3RecReader* rdr, b3JointId got, b3JointId rec ) +{ + b3RecCheckId( rdr, "joint", got.index1, got.generation, rec.index1, rec.generation ); +} + +// Registry slot reconstruction. Returns the live pointer for the given slot, building it +// on first use. The hull case is handled inline at the call site since it doesn't cache. + +static void* b3RecGetLiveMesh( b3RegistrySlot* slot ) +{ + // Mesh is a self-contained blob used by reference, with no pointer fixup. Hand back the pristine + // bytes directly: they already outlive the world and are freed at teardown, so a copy would just + // double the memory. Compound can't do this, see b3RecGetLiveCompound. + return slot->bytes; +} + +static void* b3RecGetLiveHeightField( b3RegistrySlot* slot ) +{ + // Self-contained blob used by reference, like b3RecGetLiveMesh. The bytes already are a + // valid b3HeightFieldData with no pointer fixup, so hand them back directly. + return slot->bytes; +} + +static void* b3RecGetLiveCompound( b3RegistrySlot* slot ) +{ + if ( slot->live != NULL ) + { + return slot->live; + } + // The copy is unavoidable here: b3ConvertBytesToCompound rewrites its input in place, while the + // pristine bytes must survive for keyframe registry seeding (b3RecSeedKeyframeRegistry). So we + // keep both the serialized bytes and a separate converted live object. + slot->live = b3Alloc( (size_t)slot->byteCount ); + memcpy( slot->live, slot->bytes, (size_t)slot->byteCount ); + b3ConvertBytesToCompound( (uint8_t*)slot->live, slot->byteCount ); + return slot->live; +} + +// Dispatch functions, one per op + +static void b3RecDispatch_DestroyWorld( const b3RecArgs_DestroyWorld* a, b3RecReader* rdr ) +{ + (void)a; + (void)rdr; + // End-of-session marker. The replay world is torn down in b3ValidateReplay, not here. +} + +static void b3RecDispatch_Step( const b3RecArgs_Step* a, b3RecReader* rdr ) +{ + (void)a; + b3World_Step( rdr->replayWorldId, a->dt, a->subStepCount ); +} + +static void b3RecDispatch_WorldEnableSleeping( const b3RecArgs_WorldEnableSleeping* a, b3RecReader* rdr ) +{ + b3World_EnableSleeping( rdr->replayWorldId, a->flag ); +} + +static void b3RecDispatch_WorldEnableContinuous( const b3RecArgs_WorldEnableContinuous* a, b3RecReader* rdr ) +{ + b3World_EnableContinuous( rdr->replayWorldId, a->flag ); +} + +static void b3RecDispatch_WorldSetRestitutionThreshold( const b3RecArgs_WorldSetRestitutionThreshold* a, b3RecReader* rdr ) +{ + b3World_SetRestitutionThreshold( rdr->replayWorldId, a->value ); +} + +static void b3RecDispatch_WorldSetHitEventThreshold( const b3RecArgs_WorldSetHitEventThreshold* a, b3RecReader* rdr ) +{ + b3World_SetHitEventThreshold( rdr->replayWorldId, a->value ); +} + +static void b3RecDispatch_WorldSetGravity( const b3RecArgs_WorldSetGravity* a, b3RecReader* rdr ) +{ + b3World_SetGravity( rdr->replayWorldId, a->gravity ); +} + +static void b3RecDispatch_WorldExplode( const b3RecArgs_WorldExplode* a, b3RecReader* rdr ) +{ + b3World_Explode( rdr->replayWorldId, &a->def ); +} + +static void b3RecDispatch_WorldSetContactTuning( const b3RecArgs_WorldSetContactTuning* a, b3RecReader* rdr ) +{ + b3World_SetContactTuning( rdr->replayWorldId, a->hertz, a->dampingRatio, a->contactSpeed ); +} + +static void b3RecDispatch_WorldSetContactRecycleDistance( const b3RecArgs_WorldSetContactRecycleDistance* a, b3RecReader* rdr ) +{ + b3World_SetContactRecycleDistance( rdr->replayWorldId, a->recycleDistance ); +} + +static void b3RecDispatch_WorldSetMaximumLinearSpeed( const b3RecArgs_WorldSetMaximumLinearSpeed* a, b3RecReader* rdr ) +{ + b3World_SetMaximumLinearSpeed( rdr->replayWorldId, a->maximumLinearSpeed ); +} + +static void b3RecDispatch_WorldEnableWarmStarting( const b3RecArgs_WorldEnableWarmStarting* a, b3RecReader* rdr ) +{ + b3World_EnableWarmStarting( rdr->replayWorldId, a->flag ); +} + +static void b3RecDispatch_WorldRebuildStaticTree( const b3RecArgs_WorldRebuildStaticTree* a, b3RecReader* rdr ) +{ + (void)a; + b3World_RebuildStaticTree( rdr->replayWorldId ); +} + +static void b3RecDispatch_WorldEnableSpeculative( const b3RecArgs_WorldEnableSpeculative* a, b3RecReader* rdr ) +{ + b3World_EnableSpeculative( rdr->replayWorldId, a->flag ); +} + +static void b3RecDispatch_CreateBody( const b3RecArgs_CreateBody* a, b3RecReader* rdr ) +{ + b3BodyId recId = b3RecR_BODYID( rdr ); + b3BodyId gotId = b3CreateBody( rdr->replayWorldId, &a->def ); + b3RecCheckBodyId( rdr, gotId, recId ); + if ( rdr->owner != NULL ) + { + b3RecTrackBodyCreate( rdr->owner, gotId ); + } +} + +static void b3RecDispatch_DestroyBody( const b3RecArgs_DestroyBody* a, b3RecReader* rdr ) +{ + b3BodyId id = b3RecMakeBodyId( rdr, a->body ); + if ( rdr->owner != NULL ) + { + b3RecTrackBodyDestroy( rdr->owner, id ); + } + b3DestroyBody( id ); +} + +static void b3RecDispatch_BodySetTransform( const b3RecArgs_BodySetTransform* a, b3RecReader* rdr ) +{ + b3Body_SetTransform( b3RecMakeBodyId( rdr, a->body ), a->position, a->rotation ); +} + +static void b3RecDispatch_BodySetLinearVelocity( const b3RecArgs_BodySetLinearVelocity* a, b3RecReader* rdr ) +{ + b3Body_SetLinearVelocity( b3RecMakeBodyId( rdr, a->body ), a->v ); +} + +static void b3RecDispatch_BodySetType( const b3RecArgs_BodySetType* a, b3RecReader* rdr ) +{ + b3Body_SetType( b3RecMakeBodyId( rdr, a->body ), (b3BodyType)a->type ); +} + +static void b3RecDispatch_BodySetName( const b3RecArgs_BodySetName* a, b3RecReader* rdr ) +{ + b3Body_SetName( b3RecMakeBodyId( rdr, a->body ), a->name ); +} + +static void b3RecDispatch_BodySetAngularVelocity( const b3RecArgs_BodySetAngularVelocity* a, b3RecReader* rdr ) +{ + b3Body_SetAngularVelocity( b3RecMakeBodyId( rdr, a->body ), a->w ); +} + +static void b3RecDispatch_BodySetTargetTransform( const b3RecArgs_BodySetTargetTransform* a, b3RecReader* rdr ) +{ + b3Body_SetTargetTransform( b3RecMakeBodyId( rdr, a->body ), a->target, a->timeStep, a->wake ); +} + +static void b3RecDispatch_BodyApplyForce( const b3RecArgs_BodyApplyForce* a, b3RecReader* rdr ) +{ + b3Body_ApplyForce( b3RecMakeBodyId( rdr, a->body ), a->force, a->point, a->wake ); +} + +static void b3RecDispatch_BodyApplyForceToCenter( const b3RecArgs_BodyApplyForceToCenter* a, b3RecReader* rdr ) +{ + b3Body_ApplyForceToCenter( b3RecMakeBodyId( rdr, a->body ), a->force, a->wake ); +} + +static void b3RecDispatch_BodyApplyTorque( const b3RecArgs_BodyApplyTorque* a, b3RecReader* rdr ) +{ + b3Body_ApplyTorque( b3RecMakeBodyId( rdr, a->body ), a->torque, a->wake ); +} + +static void b3RecDispatch_BodyApplyLinearImpulse( const b3RecArgs_BodyApplyLinearImpulse* a, b3RecReader* rdr ) +{ + b3Body_ApplyLinearImpulse( b3RecMakeBodyId( rdr, a->body ), a->impulse, a->point, a->wake ); +} + +static void b3RecDispatch_BodyApplyLinearImpulseToCenter( const b3RecArgs_BodyApplyLinearImpulseToCenter* a, b3RecReader* rdr ) +{ + b3Body_ApplyLinearImpulseToCenter( b3RecMakeBodyId( rdr, a->body ), a->impulse, a->wake ); +} + +static void b3RecDispatch_BodyApplyAngularImpulse( const b3RecArgs_BodyApplyAngularImpulse* a, b3RecReader* rdr ) +{ + b3Body_ApplyAngularImpulse( b3RecMakeBodyId( rdr, a->body ), a->impulse, a->wake ); +} + +static void b3RecDispatch_BodySetMassData( const b3RecArgs_BodySetMassData* a, b3RecReader* rdr ) +{ + b3Body_SetMassData( b3RecMakeBodyId( rdr, a->body ), a->massData ); +} + +static void b3RecDispatch_BodyApplyMassFromShapes( const b3RecArgs_BodyApplyMassFromShapes* a, b3RecReader* rdr ) +{ + b3Body_ApplyMassFromShapes( b3RecMakeBodyId( rdr, a->body ) ); +} + +static void b3RecDispatch_BodySetLinearDamping( const b3RecArgs_BodySetLinearDamping* a, b3RecReader* rdr ) +{ + b3Body_SetLinearDamping( b3RecMakeBodyId( rdr, a->body ), a->damping ); +} + +static void b3RecDispatch_BodySetAngularDamping( const b3RecArgs_BodySetAngularDamping* a, b3RecReader* rdr ) +{ + b3Body_SetAngularDamping( b3RecMakeBodyId( rdr, a->body ), a->damping ); +} + +static void b3RecDispatch_BodySetGravityScale( const b3RecArgs_BodySetGravityScale* a, b3RecReader* rdr ) +{ + b3Body_SetGravityScale( b3RecMakeBodyId( rdr, a->body ), a->scale ); +} + +static void b3RecDispatch_BodySetAwake( const b3RecArgs_BodySetAwake* a, b3RecReader* rdr ) +{ + b3Body_SetAwake( b3RecMakeBodyId( rdr, a->body ), a->awake ); +} + +static void b3RecDispatch_BodyEnableSleep( const b3RecArgs_BodyEnableSleep* a, b3RecReader* rdr ) +{ + b3Body_EnableSleep( b3RecMakeBodyId( rdr, a->body ), a->flag ); +} + +static void b3RecDispatch_BodySetSleepThreshold( const b3RecArgs_BodySetSleepThreshold* a, b3RecReader* rdr ) +{ + b3Body_SetSleepThreshold( b3RecMakeBodyId( rdr, a->body ), a->threshold ); +} + +static void b3RecDispatch_BodyDisable( const b3RecArgs_BodyDisable* a, b3RecReader* rdr ) +{ + b3Body_Disable( b3RecMakeBodyId( rdr, a->body ) ); +} + +static void b3RecDispatch_BodyEnable( const b3RecArgs_BodyEnable* a, b3RecReader* rdr ) +{ + b3Body_Enable( b3RecMakeBodyId( rdr, a->body ) ); +} + +static void b3RecDispatch_BodySetMotionLocks( const b3RecArgs_BodySetMotionLocks* a, b3RecReader* rdr ) +{ + b3Body_SetMotionLocks( b3RecMakeBodyId( rdr, a->body ), a->locks ); +} + +static void b3RecDispatch_BodySetBullet( const b3RecArgs_BodySetBullet* a, b3RecReader* rdr ) +{ + b3Body_SetBullet( b3RecMakeBodyId( rdr, a->body ), a->flag ); +} + +static void b3RecDispatch_BodyEnableContactRecycling( const b3RecArgs_BodyEnableContactRecycling* a, b3RecReader* rdr ) +{ + b3Body_EnableContactRecycling( b3RecMakeBodyId( rdr, a->body ), a->flag ); +} + +static void b3RecDispatch_BodyEnableHitEvents( const b3RecArgs_BodyEnableHitEvents* a, b3RecReader* rdr ) +{ + b3Body_EnableHitEvents( b3RecMakeBodyId( rdr, a->body ), a->flag ); +} + +static void b3RecDispatch_CreateSphereShape( const b3RecArgs_CreateSphereShape* a, b3RecReader* rdr ) +{ + b3ShapeId recId = b3RecR_SHAPEID( rdr ); + b3BodyId bodyId = b3RecMakeBodyId( rdr, a->body ); + b3ShapeId gotId = b3CreateSphereShape( bodyId, &a->def, &a->sphere ); + b3RecCheckShapeId( rdr, gotId, recId ); +} + +static void b3RecDispatch_CreateCapsuleShape( const b3RecArgs_CreateCapsuleShape* a, b3RecReader* rdr ) +{ + b3ShapeId recId = b3RecR_SHAPEID( rdr ); + b3BodyId bodyId = b3RecMakeBodyId( rdr, a->body ); + b3ShapeId gotId = b3CreateCapsuleShape( bodyId, &a->def, &a->capsule ); + b3RecCheckShapeId( rdr, gotId, recId ); +} + +static void b3RecDispatch_CreateHullShape( const b3RecArgs_CreateHullShape* a, b3RecReader* rdr ) +{ + b3ShapeId recId = b3RecR_SHAPEID( rdr ); + if ( !rdr->ok ) + { + return; + } + uint32_t id = a->geometryId; + if ( id >= (uint32_t)rdr->slotCount ) + { + printf( "b3ReplayFile: hull geometryId %u out of range\n", id ); + rdr->ok = false; + return; + } + b3RegistrySlot* slot = rdr->slots + id; + b3BodyId bodyId = b3RecMakeBodyId( rdr, a->body ); + // Hull is cloned by b3CreateHullShape into the world DB; no caching needed. + b3ShapeId gotId = b3CreateHullShape( bodyId, &a->def, (const b3HullData*)slot->bytes ); + b3RecCheckShapeId( rdr, gotId, recId ); +} + +static void b3RecDispatch_CreateMeshShape( const b3RecArgs_CreateMeshShape* a, b3RecReader* rdr ) +{ + b3ShapeId recId = b3RecR_SHAPEID( rdr ); + if ( !rdr->ok ) + { + return; + } + uint32_t id = a->geometryId; + if ( id >= (uint32_t)rdr->slotCount ) + { + printf( "b3ReplayFile: mesh geometryId %u out of range\n", id ); + rdr->ok = false; + return; + } + b3RegistrySlot* slot = rdr->slots + id; + const b3MeshData* mesh = (const b3MeshData*)b3RecGetLiveMesh( slot ); + b3BodyId bodyId = b3RecMakeBodyId( rdr, a->body ); + b3ShapeId gotId = b3CreateMeshShape( bodyId, &a->def, mesh, a->scale ); + b3RecCheckShapeId( rdr, gotId, recId ); +} + +static void b3RecDispatch_CreateHeightFieldShape( const b3RecArgs_CreateHeightFieldShape* a, b3RecReader* rdr ) +{ + b3ShapeId recId = b3RecR_SHAPEID( rdr ); + if ( !rdr->ok ) + { + return; + } + uint32_t id = a->geometryId; + if ( id >= (uint32_t)rdr->slotCount ) + { + printf( "b3ReplayFile: heightfield geometryId %u out of range\n", id ); + rdr->ok = false; + return; + } + b3RegistrySlot* slot = rdr->slots + id; + const b3HeightFieldData* hf = (const b3HeightFieldData*)b3RecGetLiveHeightField( slot ); + if ( hf == NULL ) + { + printf( "b3ReplayFile: heightfield geometry %u is corrupt\n", id ); + rdr->ok = false; + return; + } + b3BodyId bodyId = b3RecMakeBodyId( rdr, a->body ); + b3ShapeId gotId = b3CreateHeightFieldShape( bodyId, &a->def, hf ); + b3RecCheckShapeId( rdr, gotId, recId ); +} + +static void b3RecDispatch_CreateCompoundShape( const b3RecArgs_CreateCompoundShape* a, b3RecReader* rdr ) +{ + b3ShapeId recId = b3RecR_SHAPEID( rdr ); + if ( !rdr->ok ) + { + return; + } + uint32_t id = a->geometryId; + if ( id >= (uint32_t)rdr->slotCount ) + { + printf( "b3ReplayFile: compound geometryId %u out of range\n", id ); + rdr->ok = false; + return; + } + b3RegistrySlot* slot = rdr->slots + id; + const b3CompoundData* compound = (const b3CompoundData*)b3RecGetLiveCompound( slot ); + b3BodyId bodyId = b3RecMakeBodyId( rdr, a->body ); + // b3CreateCompoundShape takes a non-const def pointer; cast away const for the scratch def + b3ShapeDef shapeDef = a->def; + b3ShapeId gotId = b3CreateCompoundShape( bodyId, &shapeDef, compound ); + b3RecCheckShapeId( rdr, gotId, recId ); +} + +static void b3RecDispatch_DestroyShape( const b3RecArgs_DestroyShape* a, b3RecReader* rdr ) +{ + b3DestroyShape( b3RecMakeShapeId( rdr, a->shape ), a->updateBodyMass ); +} + +static void b3RecDispatch_ShapeSetName( const b3RecArgs_ShapeSetName* a, b3RecReader* rdr ) +{ + b3Shape_SetName( b3RecMakeShapeId( rdr, a->shape ), a->name ); +} + +static void b3RecDispatch_ShapeSetDensity( const b3RecArgs_ShapeSetDensity* a, b3RecReader* rdr ) +{ + b3Shape_SetDensity( b3RecMakeShapeId( rdr, a->shape ), a->density, a->updateBodyMass ); +} + +static void b3RecDispatch_ShapeSetFriction( const b3RecArgs_ShapeSetFriction* a, b3RecReader* rdr ) +{ + b3Shape_SetFriction( b3RecMakeShapeId( rdr, a->shape ), a->friction ); +} + +static void b3RecDispatch_ShapeSetRestitution( const b3RecArgs_ShapeSetRestitution* a, b3RecReader* rdr ) +{ + b3Shape_SetRestitution( b3RecMakeShapeId( rdr, a->shape ), a->restitution ); +} + +static void b3RecDispatch_ShapeSetSurfaceMaterial( const b3RecArgs_ShapeSetSurfaceMaterial* a, b3RecReader* rdr ) +{ + b3Shape_SetSurfaceMaterial( b3RecMakeShapeId( rdr, a->shape ), a->material ); +} + +static void b3RecDispatch_ShapeSetFilter( const b3RecArgs_ShapeSetFilter* a, b3RecReader* rdr ) +{ + b3Shape_SetFilter( b3RecMakeShapeId( rdr, a->shape ), a->filter, a->invokeContacts ); +} + +static void b3RecDispatch_ShapeEnableSensorEvents( const b3RecArgs_ShapeEnableSensorEvents* a, b3RecReader* rdr ) +{ + b3Shape_EnableSensorEvents( b3RecMakeShapeId( rdr, a->shape ), a->flag ); +} + +static void b3RecDispatch_ShapeEnableContactEvents( const b3RecArgs_ShapeEnableContactEvents* a, b3RecReader* rdr ) +{ + b3Shape_EnableContactEvents( b3RecMakeShapeId( rdr, a->shape ), a->flag ); +} + +static void b3RecDispatch_ShapeEnablePreSolveEvents( const b3RecArgs_ShapeEnablePreSolveEvents* a, b3RecReader* rdr ) +{ + b3Shape_EnablePreSolveEvents( b3RecMakeShapeId( rdr, a->shape ), a->flag ); +} + +static void b3RecDispatch_ShapeEnableHitEvents( const b3RecArgs_ShapeEnableHitEvents* a, b3RecReader* rdr ) +{ + b3Shape_EnableHitEvents( b3RecMakeShapeId( rdr, a->shape ), a->flag ); +} + +static void b3RecDispatch_ShapeSetSphere( const b3RecArgs_ShapeSetSphere* a, b3RecReader* rdr ) +{ + b3Shape_SetSphere( b3RecMakeShapeId( rdr, a->shape ), &a->sphere ); +} + +static void b3RecDispatch_ShapeSetCapsule( const b3RecArgs_ShapeSetCapsule* a, b3RecReader* rdr ) +{ + b3Shape_SetCapsule( b3RecMakeShapeId( rdr, a->shape ), &a->capsule ); +} + +static void b3RecDispatch_ShapeApplyWind( const b3RecArgs_ShapeApplyWind* a, b3RecReader* rdr ) +{ + b3Shape_ApplyWind( b3RecMakeShapeId( rdr, a->shape ), a->wind, a->drag, a->lift, a->maxSpeed, a->wake ); +} + +// Joint creates: remap body ids in the def before calling the API. + +static void b3RecDispatch_CreateParallelJoint( const b3RecArgs_CreateParallelJoint* a, b3RecReader* rdr ) +{ + b3JointId recId = b3RecR_JOINTID( rdr ); + b3ParallelJointDef def = a->def; + def.base.bodyIdA = b3RecMakeBodyId( rdr, def.base.bodyIdA ); + def.base.bodyIdB = b3RecMakeBodyId( rdr, def.base.bodyIdB ); + b3RecCheckJointId( rdr, b3CreateParallelJoint( rdr->replayWorldId, &def ), recId ); +} + +static void b3RecDispatch_CreateDistanceJoint( const b3RecArgs_CreateDistanceJoint* a, b3RecReader* rdr ) +{ + b3JointId recId = b3RecR_JOINTID( rdr ); + b3DistanceJointDef def = a->def; + def.base.bodyIdA = b3RecMakeBodyId( rdr, def.base.bodyIdA ); + def.base.bodyIdB = b3RecMakeBodyId( rdr, def.base.bodyIdB ); + b3RecCheckJointId( rdr, b3CreateDistanceJoint( rdr->replayWorldId, &def ), recId ); +} + +static void b3RecDispatch_CreateFilterJoint( const b3RecArgs_CreateFilterJoint* a, b3RecReader* rdr ) +{ + b3JointId recId = b3RecR_JOINTID( rdr ); + b3FilterJointDef def = a->def; + def.base.bodyIdA = b3RecMakeBodyId( rdr, def.base.bodyIdA ); + def.base.bodyIdB = b3RecMakeBodyId( rdr, def.base.bodyIdB ); + b3RecCheckJointId( rdr, b3CreateFilterJoint( rdr->replayWorldId, &def ), recId ); +} + +static void b3RecDispatch_CreateMotorJoint( const b3RecArgs_CreateMotorJoint* a, b3RecReader* rdr ) +{ + b3JointId recId = b3RecR_JOINTID( rdr ); + b3MotorJointDef def = a->def; + def.base.bodyIdA = b3RecMakeBodyId( rdr, def.base.bodyIdA ); + def.base.bodyIdB = b3RecMakeBodyId( rdr, def.base.bodyIdB ); + b3RecCheckJointId( rdr, b3CreateMotorJoint( rdr->replayWorldId, &def ), recId ); +} + +static void b3RecDispatch_CreatePrismaticJoint( const b3RecArgs_CreatePrismaticJoint* a, b3RecReader* rdr ) +{ + b3JointId recId = b3RecR_JOINTID( rdr ); + b3PrismaticJointDef def = a->def; + def.base.bodyIdA = b3RecMakeBodyId( rdr, def.base.bodyIdA ); + def.base.bodyIdB = b3RecMakeBodyId( rdr, def.base.bodyIdB ); + b3RecCheckJointId( rdr, b3CreatePrismaticJoint( rdr->replayWorldId, &def ), recId ); +} + +static void b3RecDispatch_CreateRevoluteJoint( const b3RecArgs_CreateRevoluteJoint* a, b3RecReader* rdr ) +{ + b3JointId recId = b3RecR_JOINTID( rdr ); + b3RevoluteJointDef def = a->def; + def.base.bodyIdA = b3RecMakeBodyId( rdr, def.base.bodyIdA ); + def.base.bodyIdB = b3RecMakeBodyId( rdr, def.base.bodyIdB ); + b3RecCheckJointId( rdr, b3CreateRevoluteJoint( rdr->replayWorldId, &def ), recId ); +} + +static void b3RecDispatch_CreateSphericalJoint( const b3RecArgs_CreateSphericalJoint* a, b3RecReader* rdr ) +{ + b3JointId recId = b3RecR_JOINTID( rdr ); + b3SphericalJointDef def = a->def; + def.base.bodyIdA = b3RecMakeBodyId( rdr, def.base.bodyIdA ); + def.base.bodyIdB = b3RecMakeBodyId( rdr, def.base.bodyIdB ); + b3RecCheckJointId( rdr, b3CreateSphericalJoint( rdr->replayWorldId, &def ), recId ); +} + +static void b3RecDispatch_CreateWeldJoint( const b3RecArgs_CreateWeldJoint* a, b3RecReader* rdr ) +{ + b3JointId recId = b3RecR_JOINTID( rdr ); + b3WeldJointDef def = a->def; + def.base.bodyIdA = b3RecMakeBodyId( rdr, def.base.bodyIdA ); + def.base.bodyIdB = b3RecMakeBodyId( rdr, def.base.bodyIdB ); + b3RecCheckJointId( rdr, b3CreateWeldJoint( rdr->replayWorldId, &def ), recId ); +} + +static void b3RecDispatch_CreateWheelJoint( const b3RecArgs_CreateWheelJoint* a, b3RecReader* rdr ) +{ + b3JointId recId = b3RecR_JOINTID( rdr ); + b3WheelJointDef def = a->def; + def.base.bodyIdA = b3RecMakeBodyId( rdr, def.base.bodyIdA ); + def.base.bodyIdB = b3RecMakeBodyId( rdr, def.base.bodyIdB ); + b3RecCheckJointId( rdr, b3CreateWheelJoint( rdr->replayWorldId, &def ), recId ); +} + +static void b3RecDispatch_DestroyJoint( const b3RecArgs_DestroyJoint* a, b3RecReader* rdr ) +{ + b3DestroyJoint( b3RecMakeJointId( rdr, a->joint ), a->wakeAttached ); +} + +static void b3RecDispatch_JointSetLocalFrameA( const b3RecArgs_JointSetLocalFrameA* a, b3RecReader* rdr ) +{ + b3Joint_SetLocalFrameA( b3RecMakeJointId( rdr, a->joint ), a->localFrame ); +} + +static void b3RecDispatch_JointSetLocalFrameB( const b3RecArgs_JointSetLocalFrameB* a, b3RecReader* rdr ) +{ + b3Joint_SetLocalFrameB( b3RecMakeJointId( rdr, a->joint ), a->localFrame ); +} + +static void b3RecDispatch_JointSetCollideConnected( const b3RecArgs_JointSetCollideConnected* a, b3RecReader* rdr ) +{ + b3Joint_SetCollideConnected( b3RecMakeJointId( rdr, a->joint ), a->shouldCollide ); +} + +static void b3RecDispatch_JointWakeBodies( const b3RecArgs_JointWakeBodies* a, b3RecReader* rdr ) +{ + b3Joint_WakeBodies( b3RecMakeJointId( rdr, a->joint ) ); +} + +static void b3RecDispatch_JointSetConstraintTuning( const b3RecArgs_JointSetConstraintTuning* a, b3RecReader* rdr ) +{ + b3Joint_SetConstraintTuning( b3RecMakeJointId( rdr, a->joint ), a->hertz, a->dampingRatio ); +} + +static void b3RecDispatch_JointSetForceThreshold( const b3RecArgs_JointSetForceThreshold* a, b3RecReader* rdr ) +{ + b3Joint_SetForceThreshold( b3RecMakeJointId( rdr, a->joint ), a->threshold ); +} + +static void b3RecDispatch_JointSetTorqueThreshold( const b3RecArgs_JointSetTorqueThreshold* a, b3RecReader* rdr ) +{ + b3Joint_SetTorqueThreshold( b3RecMakeJointId( rdr, a->joint ), a->threshold ); +} + +static void b3RecDispatch_ParallelJointSetSpringHertz( const b3RecArgs_ParallelJointSetSpringHertz* a, b3RecReader* rdr ) +{ + b3ParallelJoint_SetSpringHertz( b3RecMakeJointId( rdr, a->joint ), a->hertz ); +} + +static void b3RecDispatch_ParallelJointSetSpringDampingRatio( const b3RecArgs_ParallelJointSetSpringDampingRatio* a, + b3RecReader* rdr ) +{ + b3ParallelJoint_SetSpringDampingRatio( b3RecMakeJointId( rdr, a->joint ), a->dampingRatio ); +} + +static void b3RecDispatch_ParallelJointSetMaxTorque( const b3RecArgs_ParallelJointSetMaxTorque* a, b3RecReader* rdr ) +{ + b3ParallelJoint_SetMaxTorque( b3RecMakeJointId( rdr, a->joint ), a->maxTorque ); +} + +static void b3RecDispatch_DistanceJointSetLength( const b3RecArgs_DistanceJointSetLength* a, b3RecReader* rdr ) +{ + b3DistanceJoint_SetLength( b3RecMakeJointId( rdr, a->joint ), a->length ); +} + +static void b3RecDispatch_DistanceJointEnableSpring( const b3RecArgs_DistanceJointEnableSpring* a, b3RecReader* rdr ) +{ + b3DistanceJoint_EnableSpring( b3RecMakeJointId( rdr, a->joint ), a->enableSpring ); +} + +static void b3RecDispatch_DistanceJointSetSpringForceRange( const b3RecArgs_DistanceJointSetSpringForceRange* a, + b3RecReader* rdr ) +{ + b3DistanceJoint_SetSpringForceRange( b3RecMakeJointId( rdr, a->joint ), a->lowerForce, a->upperForce ); +} + +static void b3RecDispatch_DistanceJointSetSpringHertz( const b3RecArgs_DistanceJointSetSpringHertz* a, b3RecReader* rdr ) +{ + b3DistanceJoint_SetSpringHertz( b3RecMakeJointId( rdr, a->joint ), a->hertz ); +} + +static void b3RecDispatch_DistanceJointSetSpringDampingRatio( const b3RecArgs_DistanceJointSetSpringDampingRatio* a, + b3RecReader* rdr ) +{ + b3DistanceJoint_SetSpringDampingRatio( b3RecMakeJointId( rdr, a->joint ), a->dampingRatio ); +} + +static void b3RecDispatch_DistanceJointEnableLimit( const b3RecArgs_DistanceJointEnableLimit* a, b3RecReader* rdr ) +{ + b3DistanceJoint_EnableLimit( b3RecMakeJointId( rdr, a->joint ), a->enableLimit ); +} + +static void b3RecDispatch_DistanceJointSetLengthRange( const b3RecArgs_DistanceJointSetLengthRange* a, b3RecReader* rdr ) +{ + b3DistanceJoint_SetLengthRange( b3RecMakeJointId( rdr, a->joint ), a->minLength, a->maxLength ); +} + +static void b3RecDispatch_DistanceJointEnableMotor( const b3RecArgs_DistanceJointEnableMotor* a, b3RecReader* rdr ) +{ + b3DistanceJoint_EnableMotor( b3RecMakeJointId( rdr, a->joint ), a->enableMotor ); +} + +static void b3RecDispatch_DistanceJointSetMotorSpeed( const b3RecArgs_DistanceJointSetMotorSpeed* a, b3RecReader* rdr ) +{ + b3DistanceJoint_SetMotorSpeed( b3RecMakeJointId( rdr, a->joint ), a->motorSpeed ); +} + +static void b3RecDispatch_DistanceJointSetMaxMotorForce( const b3RecArgs_DistanceJointSetMaxMotorForce* a, b3RecReader* rdr ) +{ + b3DistanceJoint_SetMaxMotorForce( b3RecMakeJointId( rdr, a->joint ), a->force ); +} + +static void b3RecDispatch_MotorJointSetLinearVelocity( const b3RecArgs_MotorJointSetLinearVelocity* a, b3RecReader* rdr ) +{ + b3MotorJoint_SetLinearVelocity( b3RecMakeJointId( rdr, a->joint ), a->velocity ); +} + +static void b3RecDispatch_MotorJointSetAngularVelocity( const b3RecArgs_MotorJointSetAngularVelocity* a, b3RecReader* rdr ) +{ + b3MotorJoint_SetAngularVelocity( b3RecMakeJointId( rdr, a->joint ), a->velocity ); +} + +static void b3RecDispatch_MotorJointSetMaxVelocityForce( const b3RecArgs_MotorJointSetMaxVelocityForce* a, b3RecReader* rdr ) +{ + b3MotorJoint_SetMaxVelocityForce( b3RecMakeJointId( rdr, a->joint ), a->maxForce ); +} + +static void b3RecDispatch_MotorJointSetMaxVelocityTorque( const b3RecArgs_MotorJointSetMaxVelocityTorque* a, b3RecReader* rdr ) +{ + b3MotorJoint_SetMaxVelocityTorque( b3RecMakeJointId( rdr, a->joint ), a->maxTorque ); +} + +static void b3RecDispatch_MotorJointSetLinearHertz( const b3RecArgs_MotorJointSetLinearHertz* a, b3RecReader* rdr ) +{ + b3MotorJoint_SetLinearHertz( b3RecMakeJointId( rdr, a->joint ), a->hertz ); +} + +static void b3RecDispatch_MotorJointSetLinearDampingRatio( const b3RecArgs_MotorJointSetLinearDampingRatio* a, b3RecReader* rdr ) +{ + b3MotorJoint_SetLinearDampingRatio( b3RecMakeJointId( rdr, a->joint ), a->damping ); +} + +static void b3RecDispatch_MotorJointSetAngularHertz( const b3RecArgs_MotorJointSetAngularHertz* a, b3RecReader* rdr ) +{ + b3MotorJoint_SetAngularHertz( b3RecMakeJointId( rdr, a->joint ), a->hertz ); +} + +static void b3RecDispatch_MotorJointSetAngularDampingRatio( const b3RecArgs_MotorJointSetAngularDampingRatio* a, + b3RecReader* rdr ) +{ + b3MotorJoint_SetAngularDampingRatio( b3RecMakeJointId( rdr, a->joint ), a->damping ); +} + +static void b3RecDispatch_MotorJointSetMaxSpringForce( const b3RecArgs_MotorJointSetMaxSpringForce* a, b3RecReader* rdr ) +{ + b3MotorJoint_SetMaxSpringForce( b3RecMakeJointId( rdr, a->joint ), a->maxForce ); +} + +static void b3RecDispatch_MotorJointSetMaxSpringTorque( const b3RecArgs_MotorJointSetMaxSpringTorque* a, b3RecReader* rdr ) +{ + b3MotorJoint_SetMaxSpringTorque( b3RecMakeJointId( rdr, a->joint ), a->maxTorque ); +} + +static void b3RecDispatch_PrismaticJointEnableSpring( const b3RecArgs_PrismaticJointEnableSpring* a, b3RecReader* rdr ) +{ + b3PrismaticJoint_EnableSpring( b3RecMakeJointId( rdr, a->joint ), a->enableSpring ); +} + +static void b3RecDispatch_PrismaticJointSetSpringHertz( const b3RecArgs_PrismaticJointSetSpringHertz* a, b3RecReader* rdr ) +{ + b3PrismaticJoint_SetSpringHertz( b3RecMakeJointId( rdr, a->joint ), a->hertz ); +} + +static void b3RecDispatch_PrismaticJointSetSpringDampingRatio( const b3RecArgs_PrismaticJointSetSpringDampingRatio* a, + b3RecReader* rdr ) +{ + b3PrismaticJoint_SetSpringDampingRatio( b3RecMakeJointId( rdr, a->joint ), a->dampingRatio ); +} + +static void b3RecDispatch_PrismaticJointSetTargetTranslation( const b3RecArgs_PrismaticJointSetTargetTranslation* a, + b3RecReader* rdr ) +{ + b3PrismaticJoint_SetTargetTranslation( b3RecMakeJointId( rdr, a->joint ), a->translation ); +} + +static void b3RecDispatch_PrismaticJointEnableLimit( const b3RecArgs_PrismaticJointEnableLimit* a, b3RecReader* rdr ) +{ + b3PrismaticJoint_EnableLimit( b3RecMakeJointId( rdr, a->joint ), a->enableLimit ); +} + +static void b3RecDispatch_PrismaticJointSetLimits( const b3RecArgs_PrismaticJointSetLimits* a, b3RecReader* rdr ) +{ + b3PrismaticJoint_SetLimits( b3RecMakeJointId( rdr, a->joint ), a->lower, a->upper ); +} + +static void b3RecDispatch_PrismaticJointEnableMotor( const b3RecArgs_PrismaticJointEnableMotor* a, b3RecReader* rdr ) +{ + b3PrismaticJoint_EnableMotor( b3RecMakeJointId( rdr, a->joint ), a->enableMotor ); +} + +static void b3RecDispatch_PrismaticJointSetMotorSpeed( const b3RecArgs_PrismaticJointSetMotorSpeed* a, b3RecReader* rdr ) +{ + b3PrismaticJoint_SetMotorSpeed( b3RecMakeJointId( rdr, a->joint ), a->motorSpeed ); +} + +static void b3RecDispatch_PrismaticJointSetMaxMotorForce( const b3RecArgs_PrismaticJointSetMaxMotorForce* a, b3RecReader* rdr ) +{ + b3PrismaticJoint_SetMaxMotorForce( b3RecMakeJointId( rdr, a->joint ), a->force ); +} + +static void b3RecDispatch_RevoluteJointEnableSpring( const b3RecArgs_RevoluteJointEnableSpring* a, b3RecReader* rdr ) +{ + b3RevoluteJoint_EnableSpring( b3RecMakeJointId( rdr, a->joint ), a->enableSpring ); +} + +static void b3RecDispatch_RevoluteJointSetSpringHertz( const b3RecArgs_RevoluteJointSetSpringHertz* a, b3RecReader* rdr ) +{ + b3RevoluteJoint_SetSpringHertz( b3RecMakeJointId( rdr, a->joint ), a->hertz ); +} + +static void b3RecDispatch_RevoluteJointSetSpringDampingRatio( const b3RecArgs_RevoluteJointSetSpringDampingRatio* a, + b3RecReader* rdr ) +{ + b3RevoluteJoint_SetSpringDampingRatio( b3RecMakeJointId( rdr, a->joint ), a->dampingRatio ); +} + +static void b3RecDispatch_RevoluteJointSetTargetAngle( const b3RecArgs_RevoluteJointSetTargetAngle* a, b3RecReader* rdr ) +{ + b3RevoluteJoint_SetTargetAngle( b3RecMakeJointId( rdr, a->joint ), a->angle ); +} + +static void b3RecDispatch_RevoluteJointEnableLimit( const b3RecArgs_RevoluteJointEnableLimit* a, b3RecReader* rdr ) +{ + b3RevoluteJoint_EnableLimit( b3RecMakeJointId( rdr, a->joint ), a->enableLimit ); +} + +static void b3RecDispatch_RevoluteJointSetLimits( const b3RecArgs_RevoluteJointSetLimits* a, b3RecReader* rdr ) +{ + b3RevoluteJoint_SetLimits( b3RecMakeJointId( rdr, a->joint ), a->lower, a->upper ); +} + +static void b3RecDispatch_RevoluteJointEnableMotor( const b3RecArgs_RevoluteJointEnableMotor* a, b3RecReader* rdr ) +{ + b3RevoluteJoint_EnableMotor( b3RecMakeJointId( rdr, a->joint ), a->enableMotor ); +} + +static void b3RecDispatch_RevoluteJointSetMotorSpeed( const b3RecArgs_RevoluteJointSetMotorSpeed* a, b3RecReader* rdr ) +{ + b3RevoluteJoint_SetMotorSpeed( b3RecMakeJointId( rdr, a->joint ), a->motorSpeed ); +} + +static void b3RecDispatch_RevoluteJointSetMaxMotorTorque( const b3RecArgs_RevoluteJointSetMaxMotorTorque* a, b3RecReader* rdr ) +{ + b3RevoluteJoint_SetMaxMotorTorque( b3RecMakeJointId( rdr, a->joint ), a->torque ); +} + +static void b3RecDispatch_SphericalJointEnableConeLimit( const b3RecArgs_SphericalJointEnableConeLimit* a, b3RecReader* rdr ) +{ + b3SphericalJoint_EnableConeLimit( b3RecMakeJointId( rdr, a->joint ), a->enableLimit ); +} + +static void b3RecDispatch_SphericalJointSetConeLimit( const b3RecArgs_SphericalJointSetConeLimit* a, b3RecReader* rdr ) +{ + b3SphericalJoint_SetConeLimit( b3RecMakeJointId( rdr, a->joint ), a->angleRadians ); +} + +static void b3RecDispatch_SphericalJointEnableTwistLimit( const b3RecArgs_SphericalJointEnableTwistLimit* a, b3RecReader* rdr ) +{ + b3SphericalJoint_EnableTwistLimit( b3RecMakeJointId( rdr, a->joint ), a->enableLimit ); +} + +static void b3RecDispatch_SphericalJointSetTwistLimits( const b3RecArgs_SphericalJointSetTwistLimits* a, b3RecReader* rdr ) +{ + b3SphericalJoint_SetTwistLimits( b3RecMakeJointId( rdr, a->joint ), a->lower, a->upper ); +} + +static void b3RecDispatch_SphericalJointEnableSpring( const b3RecArgs_SphericalJointEnableSpring* a, b3RecReader* rdr ) +{ + b3SphericalJoint_EnableSpring( b3RecMakeJointId( rdr, a->joint ), a->enableSpring ); +} + +static void b3RecDispatch_SphericalJointSetSpringHertz( const b3RecArgs_SphericalJointSetSpringHertz* a, b3RecReader* rdr ) +{ + b3SphericalJoint_SetSpringHertz( b3RecMakeJointId( rdr, a->joint ), a->hertz ); +} + +static void b3RecDispatch_SphericalJointSetSpringDampingRatio( const b3RecArgs_SphericalJointSetSpringDampingRatio* a, + b3RecReader* rdr ) +{ + b3SphericalJoint_SetSpringDampingRatio( b3RecMakeJointId( rdr, a->joint ), a->dampingRatio ); +} + +static void b3RecDispatch_SphericalJointSetTargetRotation( const b3RecArgs_SphericalJointSetTargetRotation* a, b3RecReader* rdr ) +{ + b3SphericalJoint_SetTargetRotation( b3RecMakeJointId( rdr, a->joint ), a->targetRotation ); +} + +static void b3RecDispatch_SphericalJointEnableMotor( const b3RecArgs_SphericalJointEnableMotor* a, b3RecReader* rdr ) +{ + b3SphericalJoint_EnableMotor( b3RecMakeJointId( rdr, a->joint ), a->enableMotor ); +} + +static void b3RecDispatch_SphericalJointSetMotorVelocity( const b3RecArgs_SphericalJointSetMotorVelocity* a, b3RecReader* rdr ) +{ + b3SphericalJoint_SetMotorVelocity( b3RecMakeJointId( rdr, a->joint ), a->motorVelocity ); +} + +static void b3RecDispatch_SphericalJointSetMaxMotorTorque( const b3RecArgs_SphericalJointSetMaxMotorTorque* a, b3RecReader* rdr ) +{ + b3SphericalJoint_SetMaxMotorTorque( b3RecMakeJointId( rdr, a->joint ), a->torque ); +} + +static void b3RecDispatch_WeldJointSetLinearHertz( const b3RecArgs_WeldJointSetLinearHertz* a, b3RecReader* rdr ) +{ + b3WeldJoint_SetLinearHertz( b3RecMakeJointId( rdr, a->joint ), a->hertz ); +} + +static void b3RecDispatch_WeldJointSetLinearDampingRatio( const b3RecArgs_WeldJointSetLinearDampingRatio* a, b3RecReader* rdr ) +{ + b3WeldJoint_SetLinearDampingRatio( b3RecMakeJointId( rdr, a->joint ), a->dampingRatio ); +} + +static void b3RecDispatch_WeldJointSetAngularHertz( const b3RecArgs_WeldJointSetAngularHertz* a, b3RecReader* rdr ) +{ + b3WeldJoint_SetAngularHertz( b3RecMakeJointId( rdr, a->joint ), a->hertz ); +} + +static void b3RecDispatch_WeldJointSetAngularDampingRatio( const b3RecArgs_WeldJointSetAngularDampingRatio* a, b3RecReader* rdr ) +{ + b3WeldJoint_SetAngularDampingRatio( b3RecMakeJointId( rdr, a->joint ), a->dampingRatio ); +} + +static void b3RecDispatch_WheelJointEnableSuspension( const b3RecArgs_WheelJointEnableSuspension* a, b3RecReader* rdr ) +{ + b3WheelJoint_EnableSuspension( b3RecMakeJointId( rdr, a->joint ), a->flag ); +} + +static void b3RecDispatch_WheelJointSetSuspensionHertz( const b3RecArgs_WheelJointSetSuspensionHertz* a, b3RecReader* rdr ) +{ + b3WheelJoint_SetSuspensionHertz( b3RecMakeJointId( rdr, a->joint ), a->hertz ); +} + +static void b3RecDispatch_WheelJointSetSuspensionDampingRatio( const b3RecArgs_WheelJointSetSuspensionDampingRatio* a, + b3RecReader* rdr ) +{ + b3WheelJoint_SetSuspensionDampingRatio( b3RecMakeJointId( rdr, a->joint ), a->dampingRatio ); +} + +static void b3RecDispatch_WheelJointEnableSuspensionLimit( const b3RecArgs_WheelJointEnableSuspensionLimit* a, b3RecReader* rdr ) +{ + b3WheelJoint_EnableSuspensionLimit( b3RecMakeJointId( rdr, a->joint ), a->flag ); +} + +static void b3RecDispatch_WheelJointSetSuspensionLimits( const b3RecArgs_WheelJointSetSuspensionLimits* a, b3RecReader* rdr ) +{ + b3WheelJoint_SetSuspensionLimits( b3RecMakeJointId( rdr, a->joint ), a->lower, a->upper ); +} + +static void b3RecDispatch_WheelJointEnableSpinMotor( const b3RecArgs_WheelJointEnableSpinMotor* a, b3RecReader* rdr ) +{ + b3WheelJoint_EnableSpinMotor( b3RecMakeJointId( rdr, a->joint ), a->flag ); +} + +static void b3RecDispatch_WheelJointSetSpinMotorSpeed( const b3RecArgs_WheelJointSetSpinMotorSpeed* a, b3RecReader* rdr ) +{ + b3WheelJoint_SetSpinMotorSpeed( b3RecMakeJointId( rdr, a->joint ), a->speed ); +} + +static void b3RecDispatch_WheelJointSetMaxSpinTorque( const b3RecArgs_WheelJointSetMaxSpinTorque* a, b3RecReader* rdr ) +{ + b3WheelJoint_SetMaxSpinTorque( b3RecMakeJointId( rdr, a->joint ), a->torque ); +} + +static void b3RecDispatch_WheelJointEnableSteering( const b3RecArgs_WheelJointEnableSteering* a, b3RecReader* rdr ) +{ + b3WheelJoint_EnableSteering( b3RecMakeJointId( rdr, a->joint ), a->flag ); +} + +static void b3RecDispatch_WheelJointSetSteeringHertz( const b3RecArgs_WheelJointSetSteeringHertz* a, b3RecReader* rdr ) +{ + b3WheelJoint_SetSteeringHertz( b3RecMakeJointId( rdr, a->joint ), a->hertz ); +} + +static void b3RecDispatch_WheelJointSetSteeringDampingRatio( const b3RecArgs_WheelJointSetSteeringDampingRatio* a, + b3RecReader* rdr ) +{ + b3WheelJoint_SetSteeringDampingRatio( b3RecMakeJointId( rdr, a->joint ), a->dampingRatio ); +} + +static void b3RecDispatch_WheelJointSetMaxSteeringTorque( const b3RecArgs_WheelJointSetMaxSteeringTorque* a, b3RecReader* rdr ) +{ + b3WheelJoint_SetMaxSteeringTorque( b3RecMakeJointId( rdr, a->joint ), a->torque ); +} + +static void b3RecDispatch_WheelJointEnableSteeringLimit( const b3RecArgs_WheelJointEnableSteeringLimit* a, b3RecReader* rdr ) +{ + b3WheelJoint_EnableSteeringLimit( b3RecMakeJointId( rdr, a->joint ), a->flag ); +} + +static void b3RecDispatch_WheelJointSetSteeringLimits( const b3RecArgs_WheelJointSetSteeringLimits* a, b3RecReader* rdr ) +{ + b3WheelJoint_SetSteeringLimits( b3RecMakeJointId( rdr, a->joint ), a->lower, a->upper ); +} + +static void b3RecDispatch_WheelJointSetTargetSteeringAngle( const b3RecArgs_WheelJointSetTargetSteeringAngle* a, + b3RecReader* rdr ) +{ + b3WheelJoint_SetTargetSteeringAngle( b3RecMakeJointId( rdr, a->joint ), a->radians ); +} + +static void b3RecDispatch_StateHash( const b3RecArgs_StateHash* a, b3RecReader* rdr ) +{ + b3World* world = b3GetWorldFromId( rdr->replayWorldId ); + uint64_t computed = b3HashWorldState( world ); + if ( computed != a->hash ) + { + printf( "b3ReplayFile: StateHash mismatch (recorded=0x%" PRIx64 ", computed=0x%" PRIx64 ")\n", a->hash, computed ); + rdr->diverged = true; + } +} + +static void b3RecDispatch_RecordingBounds( const b3RecArgs_RecordingBounds* a, b3RecReader* rdr ) +{ + // Primary resolve is the open-time scan, this keeps the value right if it ever moves earlier + if ( rdr->owner != NULL ) + { + rdr->owner->bounds = a->bounds; + } +} + +// Spatial query replay. The recorded inputs come through the manifest; here the variable-length hit +// tail is read back, the query re-issued against the replay world, and each callback hit compared to +// what was recorded. Any mismatch latches rdr->diverged. When a player owns the reader the hits are +// also stashed for the viewer overlay. The stash helpers dereference the player struct, defined later +// in this file, so they are forward declared and implemented in Block B below. + +static void b3RecGrow( void** data, int* capacity, int need, int keep, int elemSize ); +static b3RecDrawQuery* b3RecStashQueryBegin( b3RecPlayer* player, int kind, const b3RecRecordedHit* hits, int hitCount ); + +// Grow the reader's hit scratch to at least n entries, preserving contents. n is bounded by the file +// size since every recorded hit consumes at least one byte, so a corrupt count fails the read. +void b3RecEnsureHits( b3RecReader* rdr, int n ) +{ + b3RecReserveScratch( rdr, (void**)&rdr->hits, &rdr->hitCap, n, (int)sizeof( b3RecRecordedHit ) ); +} + +// Bitwise float compare so the determinism check is exact, not within a tolerance. +static bool b3RecF32Differs( float a, float b ) +{ + uint32_t ua, ub; + memcpy( &ua, &a, 4 ); + memcpy( &ub, &b, 4 ); + return ua != ub; +} + +static bool b3RecVec3Differs( b3Vec3 a, b3Vec3 b ) +{ + return b3RecF32Differs( a.x, b.x ) || b3RecF32Differs( a.y, b.y ) || b3RecF32Differs( a.z, b.z ); +} + +// Shared context for the replay trampolines: walks recorded hits in order, flagging any divergence +// from the re-issued query. +typedef struct b3RecReplayQueryCtx +{ + b3RecReader* rdr; + const b3RecRecordedHit* hits; + int count; + int cursor; +} b3RecReplayQueryCtx; + +static bool b3RecReplayOverlapTrampoline( b3ShapeId id, void* ctx ) +{ + b3RecReplayQueryCtx* rc = ctx; + if ( rc->cursor >= rc->count ) + { + rc->rdr->diverged = true; + return false; + } + const b3RecRecordedHit* h = &rc->hits[rc->cursor++]; + if ( id.index1 != h->id.index1 || id.generation != h->id.generation ) + { + rc->rdr->diverged = true; + } + return h->userReturnB; +} + +// The mover filter has the same bool(shapeId, ctx) shape as an overlap callback, so it replays the +// same way. A distinct typed wrapper keeps the function-pointer types clean. +static bool b3RecReplayMoverFilterTrampoline( b3ShapeId id, void* ctx ) +{ + return b3RecReplayOverlapTrampoline( id, ctx ); +} + +static float b3RecReplayCastTrampoline( b3ShapeId id, b3Pos point, b3Vec3 normal, float fraction, uint64_t userMaterialId, + int triangleIndex, int childIndex, void* ctx ) +{ + b3RecReplayQueryCtx* rc = ctx; + if ( rc->cursor >= rc->count ) + { + rc->rdr->diverged = true; + return 0.0f; + } + const b3RecRecordedHit* h = &rc->hits[rc->cursor++]; + // Positions compared through a full-width delta, truncating both sides would pass vacuously far + // from the origin. + if ( id.index1 != h->id.index1 || id.generation != h->id.generation || + b3RecVec3Differs( b3SubPos( point, h->point ), b3Vec3_zero ) || b3RecVec3Differs( normal, h->normal ) || + b3RecF32Differs( fraction, h->fraction ) || userMaterialId != h->userMaterialId || triangleIndex != h->triangleIndex || + childIndex != h->childIndex ) + { + rc->rdr->diverged = true; + } + return h->userReturnF; +} + +// 3D delivers a whole shape's planes in one call. The recorded group starts at the cursor with its +// plane count and user return replicated on each hit, so compare the batch and advance by the +// recorded count to stay aligned with the stream even when the count itself diverged. +static bool b3RecReplayPlaneTrampoline( b3ShapeId id, const b3PlaneResult* planes, int planeCount, void* ctx ) +{ + b3RecReplayQueryCtx* rc = ctx; + if ( rc->cursor >= rc->count ) + { + rc->rdr->diverged = true; + return true; + } + const b3RecRecordedHit* head = &rc->hits[rc->cursor]; + int recordedCount = head->planeCount; + bool ret = head->userReturnB; + if ( id.index1 != head->id.index1 || id.generation != head->id.generation || recordedCount != planeCount ) + { + rc->rdr->diverged = true; + } + int n = recordedCount < planeCount ? recordedCount : planeCount; + for ( int i = 0; i < n; ++i ) + { + const b3RecRecordedHit* h = &rc->hits[rc->cursor + i]; + if ( b3RecVec3Differs( h->plane.plane.normal, planes[i].plane.normal ) || + b3RecF32Differs( h->plane.plane.offset, planes[i].plane.offset ) || + b3RecVec3Differs( h->plane.point, planes[i].point ) ) + { + rc->rdr->diverged = true; + } + } + rc->cursor += recordedCount; + return ret; +} + +// Copy a decoded proxy's points into a draw record so the overlay does not depend on reader scratch. +static void b3RecStashProxy( b3RecDrawQuery* q, const b3ShapeProxy* proxy ) +{ + int count = proxy->count; + if ( count > B3_MAX_SHAPE_CAST_POINTS ) + count = B3_MAX_SHAPE_CAST_POINTS; + q->proxyCount = count; + q->proxyRadius = proxy->radius; + for ( int i = 0; i < count; ++i ) + { + q->proxyPoints[i] = proxy->points[i]; + } +} + +// Tight world-space bounds of a query's swept geometry, so the viewer can frame any query and not +// just the overlap AABB. Mover and proxy points are origin relative. A cast sweeps the shape from the +// origin to origin plus translation. The overlap AABB is already a world-space box. +static void b3RecComputeQueryBounds( b3RecDrawQuery* q ) +{ + if ( q->kind == B3_RECQ_OVERLAP_AABB ) + { + return; + } + + // Shape points relative to the origin, plus the fattening radius. A ray has no shape, so it falls + // through to a single point at the origin. + b3Vec3 local[B3_MAX_SHAPE_CAST_POINTS]; + int count = 0; + float radius = 0.0f; + switch ( q->kind ) + { + case B3_RECQ_CAST_MOVER: + case B3_RECQ_COLLIDE_MOVER: + local[0] = q->mover.center1; + local[1] = q->mover.center2; + count = 2; + radius = q->mover.radius; + break; + + case B3_RECQ_OVERLAP_SHAPE: + case B3_RECQ_CAST_SHAPE: + count = q->proxyCount; + for ( int i = 0; i < count; ++i ) + { + local[i] = q->proxyPoints[i]; + } + radius = q->proxyRadius; + break; + + default: + break; + } + if ( count == 0 ) + { + local[0] = b3Vec3_zero; + count = 1; + } + + // Sweep each point across the translation. A non-cast query has zero translation, so both ends + // coincide and the duplicates fold away. + b3Pos end = b3OffsetPos( q->origin, q->translation ); + b3Vec3 world[2 * B3_MAX_SHAPE_CAST_POINTS]; + int n = 0; + for ( int i = 0; i < count; ++i ) + { + world[n++] = b3ToVec3( b3OffsetPos( q->origin, local[i] ) ); + world[n++] = b3ToVec3( b3OffsetPos( end, local[i] ) ); + } + q->aabb = b3MakeAABB( world, n, radius ); +} + +static void b3RecDispatch_QueryOverlapAABB( const b3RecArgs_QueryOverlapAABB* a, b3RecReader* rdr ) +{ + uint32_t n = b3RecR_U32( rdr ); + b3RecEnsureHits( rdr, (int)n ); + if ( !rdr->ok ) + return; + for ( uint32_t i = 0; i < n; ++i ) + { + rdr->hits[i].id = b3RecMakeShapeId( rdr, b3RecR_SHAPEID( rdr ) ); + rdr->hits[i].userReturnB = b3RecR_BOOL( rdr ); + } + (void)b3RecR_TREESTATS( rdr ); + if ( !rdr->ok ) + return; + b3RecReplayQueryCtx rc = { rdr, rdr->hits, (int)n, 0 }; + b3World_OverlapAABB( rdr->replayWorldId, a->aabb, a->filter, b3RecReplayOverlapTrampoline, &rc ); + if ( rc.cursor != (int)n ) + rdr->diverged = true; + if ( rdr->owner ) + { + b3RecDrawQuery* q = b3RecStashQueryBegin( rdr->owner, B3_RECQ_OVERLAP_AABB, rdr->hits, (int)n ); + q->filter = a->filter; + q->aabb = a->aabb; + b3RecComputeQueryBounds( q ); + } +} + +static void b3RecDispatch_QueryOverlapShape( const b3RecArgs_QueryOverlapShape* a, b3RecReader* rdr ) +{ + uint32_t n = b3RecR_U32( rdr ); + b3RecEnsureHits( rdr, (int)n ); + if ( !rdr->ok ) + return; + for ( uint32_t i = 0; i < n; ++i ) + { + rdr->hits[i].id = b3RecMakeShapeId( rdr, b3RecR_SHAPEID( rdr ) ); + rdr->hits[i].userReturnB = b3RecR_BOOL( rdr ); + } + (void)b3RecR_TREESTATS( rdr ); + if ( !rdr->ok ) + return; + b3RecReplayQueryCtx rc = { rdr, rdr->hits, (int)n, 0 }; + b3World_OverlapShape( rdr->replayWorldId, a->origin, &a->proxy, a->filter, b3RecReplayOverlapTrampoline, &rc ); + if ( rc.cursor != (int)n ) + rdr->diverged = true; + if ( rdr->owner ) + { + b3RecDrawQuery* q = b3RecStashQueryBegin( rdr->owner, B3_RECQ_OVERLAP_SHAPE, rdr->hits, (int)n ); + q->filter = a->filter; + q->origin = a->origin; + b3RecStashProxy( q, &a->proxy ); + b3RecComputeQueryBounds( q ); + } +} + +static void b3RecDispatch_QueryCastRay( const b3RecArgs_QueryCastRay* a, b3RecReader* rdr ) +{ + uint32_t n = b3RecR_U32( rdr ); + b3RecEnsureHits( rdr, (int)n ); + if ( !rdr->ok ) + return; + for ( uint32_t i = 0; i < n; ++i ) + { + rdr->hits[i].id = b3RecMakeShapeId( rdr, b3RecR_SHAPEID( rdr ) ); + rdr->hits[i].point = b3RecR_POSITION( rdr ); + rdr->hits[i].normal = b3RecR_VEC3( rdr ); + rdr->hits[i].fraction = b3RecR_F32( rdr ); + rdr->hits[i].userMaterialId = b3RecR_U64( rdr ); + rdr->hits[i].triangleIndex = b3RecR_I32( rdr ); + rdr->hits[i].childIndex = b3RecR_I32( rdr ); + rdr->hits[i].userReturnF = b3RecR_F32( rdr ); + } + (void)b3RecR_TREESTATS( rdr ); + if ( !rdr->ok ) + return; + b3RecReplayQueryCtx rc = { rdr, rdr->hits, (int)n, 0 }; + b3World_CastRay( rdr->replayWorldId, a->origin, a->translation, a->filter, b3RecReplayCastTrampoline, &rc ); + if ( rc.cursor != (int)n ) + rdr->diverged = true; + if ( rdr->owner ) + { + b3RecDrawQuery* q = b3RecStashQueryBegin( rdr->owner, B3_RECQ_CAST_RAY, rdr->hits, (int)n ); + q->filter = a->filter; + q->origin = a->origin; + q->translation = a->translation; + b3RecComputeQueryBounds( q ); + } +} + +static void b3RecDispatch_QueryCastShape( const b3RecArgs_QueryCastShape* a, b3RecReader* rdr ) +{ + uint32_t n = b3RecR_U32( rdr ); + b3RecEnsureHits( rdr, (int)n ); + if ( !rdr->ok ) + return; + for ( uint32_t i = 0; i < n; ++i ) + { + rdr->hits[i].id = b3RecMakeShapeId( rdr, b3RecR_SHAPEID( rdr ) ); + rdr->hits[i].point = b3RecR_POSITION( rdr ); + rdr->hits[i].normal = b3RecR_VEC3( rdr ); + rdr->hits[i].fraction = b3RecR_F32( rdr ); + rdr->hits[i].userMaterialId = b3RecR_U64( rdr ); + rdr->hits[i].triangleIndex = b3RecR_I32( rdr ); + rdr->hits[i].childIndex = b3RecR_I32( rdr ); + rdr->hits[i].userReturnF = b3RecR_F32( rdr ); + } + (void)b3RecR_TREESTATS( rdr ); + if ( !rdr->ok ) + return; + b3RecReplayQueryCtx rc = { rdr, rdr->hits, (int)n, 0 }; + b3World_CastShape( rdr->replayWorldId, a->origin, &a->proxy, a->translation, a->filter, b3RecReplayCastTrampoline, &rc ); + if ( rc.cursor != (int)n ) + rdr->diverged = true; + if ( rdr->owner ) + { + b3RecDrawQuery* q = b3RecStashQueryBegin( rdr->owner, B3_RECQ_CAST_SHAPE, rdr->hits, (int)n ); + q->filter = a->filter; + q->origin = a->origin; + q->translation = a->translation; + b3RecStashProxy( q, &a->proxy ); + b3RecComputeQueryBounds( q ); + } +} + +static void b3RecDispatch_QueryCastRayClosest( const b3RecArgs_QueryCastRayClosest* a, b3RecReader* rdr ) +{ + b3RayResult rec = b3RecR_RAYRESULT( rdr ); + if ( !rdr->ok ) + return; + b3RayResult got = b3World_CastRayClosest( rdr->replayWorldId, a->origin, a->translation, a->filter ); + b3ShapeId recId = b3RecMakeShapeId( rdr, rec.shapeId ); + if ( got.hit != rec.hit || + ( got.hit && + ( got.shapeId.index1 != recId.index1 || got.shapeId.generation != recId.generation || + b3RecVec3Differs( b3SubPos( got.point, rec.point ), b3Vec3_zero ) || b3RecVec3Differs( got.normal, rec.normal ) || + b3RecF32Differs( got.fraction, rec.fraction ) || got.userMaterialId != rec.userMaterialId ) ) ) + { + rdr->diverged = true; + } + if ( rdr->owner ) + { + // Stash the closest result as a single pooled hit so the shared draw loop renders its point. + b3RecRecordedHit h = { 0 }; + h.id = recId; + h.point = rec.point; + h.normal = rec.normal; + h.fraction = rec.fraction; + b3RecDrawQuery* q = b3RecStashQueryBegin( rdr->owner, B3_RECQ_CAST_RAY_CLOSEST, &h, rec.hit ? 1 : 0 ); + q->filter = a->filter; + q->origin = a->origin; + q->translation = a->translation; + q->rayResult = rec; + b3RecComputeQueryBounds( q ); + } +} + +static void b3RecDispatch_QueryCastMover( const b3RecArgs_QueryCastMover* a, b3RecReader* rdr ) +{ + uint32_t n = b3RecR_U32( rdr ); + b3RecEnsureHits( rdr, (int)n ); + if ( !rdr->ok ) + return; + for ( uint32_t i = 0; i < n; ++i ) + { + rdr->hits[i].id = b3RecMakeShapeId( rdr, b3RecR_SHAPEID( rdr ) ); + rdr->hits[i].userReturnB = b3RecR_BOOL( rdr ); + } + float recFraction = b3RecR_F32( rdr ); + if ( !rdr->ok ) + return; + b3RecReplayQueryCtx rc = { rdr, rdr->hits, (int)n, 0 }; + float got = b3World_CastMover( rdr->replayWorldId, a->origin, &a->mover, a->translation, a->filter, + b3RecReplayMoverFilterTrampoline, &rc ); + if ( rc.cursor != (int)n || b3RecF32Differs( got, recFraction ) ) + rdr->diverged = true; + if ( rdr->owner ) + { + b3RecDrawQuery* q = b3RecStashQueryBegin( rdr->owner, B3_RECQ_CAST_MOVER, NULL, 0 ); + q->filter = a->filter; + q->origin = a->origin; + q->mover = a->mover; + q->translation = a->translation; + q->castFraction = recFraction; + b3RecComputeQueryBounds( q ); + } +} + +static void b3RecDispatch_QueryCollideMover( const b3RecArgs_QueryCollideMover* a, b3RecReader* rdr ) +{ + // Recorded as shapeCount groups, each: shapeId, planeCount, planeCount planes, user return. Flatten + // into one hit per plane with the group's count and return replicated, so the replay walker can + // re-group per shape. + uint32_t shapeCount = b3RecR_U32( rdr ); + int total = 0; + for ( uint32_t s = 0; s < shapeCount; ++s ) + { + b3ShapeId id = b3RecMakeShapeId( rdr, b3RecR_SHAPEID( rdr ) ); + int planeCount = b3RecR_I32( rdr ); + if ( planeCount < 0 ) + planeCount = 0; + b3RecEnsureHits( rdr, total + planeCount ); + if ( !rdr->ok ) + return; + for ( int i = 0; i < planeCount; ++i ) + { + rdr->hits[total + i].plane = b3RecR_PLANERESULT( rdr ); + } + bool ret = b3RecR_BOOL( rdr ); + for ( int i = 0; i < planeCount; ++i ) + { + rdr->hits[total + i].id = id; + rdr->hits[total + i].planeCount = planeCount; + rdr->hits[total + i].userReturnB = ret; + } + total += planeCount; + } + if ( !rdr->ok ) + return; + b3RecReplayQueryCtx rc = { rdr, rdr->hits, total, 0 }; + b3World_CollideMover( rdr->replayWorldId, a->origin, &a->mover, a->filter, b3RecReplayPlaneTrampoline, &rc ); + if ( rc.cursor != total ) + rdr->diverged = true; + if ( rdr->owner ) + { + b3RecDrawQuery* q = b3RecStashQueryBegin( rdr->owner, B3_RECQ_COLLIDE_MOVER, rdr->hits, total ); + q->filter = a->filter; + q->origin = a->origin; + q->mover = a->mover; + b3RecComputeQueryBounds( q ); + } +} + +// Stash the identity key of the query that immediately follows. Consumed by the next stash. +static void b3RecDispatch_QueryTag( const b3RecArgs_QueryTag* a, b3RecReader* rdr ) +{ + rdr->pendingQueryKey = a->key; +} + +// X-macro dispatch switch: read opcode+u24 payloadSize, dispatch, skip unknown ops. +// Returns the opcode dispatched, or -1 when the stream is exhausted or broken. + +static int b3RecDispatchOne( b3RecReader* rdr ) +{ + if ( rdr->cursor >= rdr->size || !rdr->ok ) + { + return -1; + } + uint8_t opcode = b3RecR_U8( rdr ); + uint32_t payloadSize = b3RecR_U24( rdr ); + if ( !rdr->ok ) + { + return -1; + } + + int payloadStart = rdr->cursor; + + switch ( opcode ) + { +#define ARG( TAG, field ) a.field = b3RecR_##TAG( rdr ); +#define B3_REC_OP( op, Name, RET, ... ) \ + case op: \ + { \ + b3RecArgs_##Name a; \ + memset( &a, 0, sizeof( a ) ); \ + __VA_ARGS__ \ + if ( rdr->ok ) \ + { \ + b3RecDispatch_##Name( &a, rdr ); \ + } \ + break; \ + } +#include "recording_ops.inl" +#undef B3_REC_OP +#undef ARG + default: + printf( "b3ReplayFile: unknown opcode 0x%02X, skipping %u bytes\n", opcode, payloadSize ); + // payloadStart is in bounds, so size - payloadStart is the bytes left to skip over + if ( payloadSize > (uint32_t)( rdr->size - payloadStart ) ) + { + rdr->ok = false; + } + else + { + rdr->cursor = payloadStart + (int)payloadSize; + } + break; + } + return (int)(unsigned)opcode; +} + +// Public entry point + +bool b3ValidateReplay( const void* data, int size, int workerCount ) +{ + b3RecPlayer* player = b3RecPlayer_Create( data, size, workerCount ); + if ( player == NULL ) + { + return false; + } + + while ( b3RecPlayer_StepFrame( player ) ) + { + if ( player->rdr.diverged ) + { + break; + } + } + + bool ok = player->rdr.ok && player->rdr.diverged == false; + b3RecPlayer_Destroy( player ); + return ok; +} + +// b3RecPlayer implementation + +#define B3_REC_KEYFRAME_INTERVAL_DEFAULT 16 +#define B3_REC_KEYFRAME_BUDGET_DEFAULT ( (size_t)512 * 1024 * 1024 ) + +// Overflow-safe growth for the player's accumulating arrays. Counts come from the replay itself, +// not the file, so this only guards the byte-size multiply. Preserves keep elements. +static void b3RecGrow( void** data, int* capacity, int need, int keep, int elemSize ) +{ + if ( need <= *capacity ) + { + return; + } + int newCap = *capacity == 0 ? 8 : 2 * *capacity; + if ( newCap < need ) + { + newCap = need; + } + void* grown = b3Alloc( (size_t)newCap * (size_t)elemSize ); + if ( *data != NULL ) + { + if ( keep > 0 ) + { + memcpy( grown, *data, (size_t)keep * (size_t)elemSize ); + } + b3Free( *data, (size_t)*capacity * (size_t)elemSize ); + } + *data = grown; + *capacity = newCap; +} + +// Block B: per-frame query store helpers. Forward declared above the query dispatchers, which run +// before the player struct is defined, so the player-dereferencing code lives here. + +static void b3RecGrowFrameQueries( b3RecPlayer* player ) +{ + b3RecGrow( (void**)&player->frameQueries, &player->frameQueryCap, player->frameQueryCount + 1, player->frameQueryCount, + (int)sizeof( b3RecDrawQuery ) ); +} + +static void b3RecGrowFrameHits( b3RecPlayer* player, int need ) +{ + b3RecGrow( (void**)&player->frameHits, &player->frameHitCap, player->frameHitCount + need, player->frameHitCount, + (int)sizeof( b3RecRecordedHit ) ); +} + +// Push a draw record for one query and copy its hits into the per-frame store. Ids in hits[] are +// already remapped to the replay world by the dispatcher. +static b3RecDrawQuery* b3RecStashQueryBegin( b3RecPlayer* player, int kind, const b3RecRecordedHit* hits, int hitCount ) +{ + b3RecGrowFrameQueries( player ); + b3RecDrawQuery* q = &player->frameQueries[player->frameQueryCount]; + memset( q, 0, sizeof( *q ) ); + q->kind = kind; + // Pair the query with the key from its preceding QueryTag op, if any, then clear it so the next + // untagged query reads 0. + q->key = player->rdr.pendingQueryKey; + player->rdr.pendingQueryKey = 0; + q->hitStart = player->frameHitCount; + q->hitCount = hitCount; + b3RecGrowFrameHits( player, hitCount ); + for ( int i = 0; i < hitCount; ++i ) + { + player->frameHits[player->frameHitCount + i] = hits[i]; + } + player->frameHitCount += hitCount; + player->frameQueryCount++; + return q; +} + +// Append a created body to the outliner list. Ordinals are creation order and never reused. +static void b3RecTrackBodyCreate( b3RecPlayer* player, b3BodyId id ) +{ + b3RecGrow( (void**)&player->bodyIds, &player->bodyIdCap, player->bodyIdCount + 1, player->bodyIdCount, + (int)sizeof( b3BodyId ) ); + player->bodyIds[player->bodyIdCount] = id; + player->bodyIdCount += 1; +} + +// Leave a hole so later ordinals do not shift, keeping a stored selection stable. +static void b3RecTrackBodyDestroy( b3RecPlayer* player, b3BodyId id ) +{ + for ( int i = 0; i < player->bodyIdCount; ++i ) + { + if ( B3_ID_EQUALS( player->bodyIds[i], id ) ) + { + player->bodyIds[i] = b3_nullBodyId; + return; + } + } +} + +// Snapshot bodies are restored as a struct image and never hit the CreateBody hook the tracker keys +// on, so the seed world must be walked once to populate the outliner list. Slot order is stable. +static void b3RecSeedBodyIds( b3RecPlayer* player ) +{ + b3World* world = b3GetWorldFromId( player->rdr.replayWorldId ); + player->bodyIdCount = 0; + int count = world->bodies.count; + for ( int i = 0; i < count; ++i ) + { + if ( world->bodies.data[i].id != i ) + { + continue; // free slot + } + b3RecTrackBodyCreate( player, b3MakeBodyId( world, i ) ); + } +} + +// Seed the outliner list from the current world and save it as the frame-0 restore copy. +static void b3RecSeedFrame0BodyIds( b3RecPlayer* player ) +{ + b3RecSeedBodyIds( player ); + if ( player->frame0BodyIds != NULL ) + { + b3Free( player->frame0BodyIds, (size_t)player->frame0BodyIdCount * sizeof( b3BodyId ) ); + player->frame0BodyIds = NULL; + } + player->frame0BodyIdCount = player->bodyIdCount; + if ( player->bodyIdCount > 0 ) + { + player->frame0BodyIds = (b3BodyId*)b3Alloc( (size_t)player->bodyIdCount * sizeof( b3BodyId ) ); + memcpy( player->frame0BodyIds, player->bodyIds, (size_t)player->bodyIdCount * sizeof( b3BodyId ) ); + } +} + +// Tag key to tag index, so the viewer resolves a query's caller id and label in O(1) instead of a +// linear scan over the tag table. +#define NAME b3RecTagLookup +#define KEY_TY uint64_t +#define VAL_TY uint32_t +#define HASH_FN vt_hash_integer +#define CMPR_FN vt_cmpr_integer +#define MALLOC_FN b3Alloc +#define FREE_FN b3Free +#include "verstable.h" + +// Read the optional query-tag table trailing the geometry entries: u32 tagCount then per tag +// { u64 key, u64 id, u16 len, name bytes }. A recording written before the tag table leaves rp at +// dataEnd, so nothing loads. Bounds-checked; tagCount reflects only the tags that fully fit, so a +// truncated tail loads what it can and reports the real count. +static void b3RecLoadTags( b3RecReader* rdr, const uint8_t* rp, const uint8_t* dataEnd ) +{ + b3RecReader sub = { 0 }; + sub.data = rp; + sub.size = (int)( dataEnd - rp ); + sub.ok = true; + + uint32_t count = b3RecR_U32( &sub ); + if ( sub.ok == false || count == 0 ) + { + return; + } + + // Each tag is at least 18 bytes (8 key + 8 id + 2 length). Reject a count that cannot fit the + // remaining bytes so a corrupt table cannot request a wild allocation. + if ( (size_t)count > (size_t)( sub.size - sub.cursor ) / 18 ) + { + return; + } + + b3RecTag* tags = (b3RecTag*)b3Alloc( (size_t)count * sizeof( b3RecTag ) ); + memset( tags, 0, (size_t)count * sizeof( b3RecTag ) ); + + b3RecTagLookup* map = (b3RecTagLookup*)b3Alloc( sizeof( b3RecTagLookup ) ); + b3RecTagLookup_init( map ); + + uint32_t loaded = 0; + for ( uint32_t i = 0; i < count; ++i ) + { + uint64_t key = b3RecR_U64( &sub ); + uint64_t id = b3RecR_U64( &sub ); + uint16_t len = b3RecR_U16( &sub ); + if ( sub.ok == false ) + { + break; + } + if ( len == 0xFFFFu ) + { + len = 0; // a null name is written as 0xFFFF + } + if ( (int64_t)sub.cursor + (int64_t)len > (int64_t)sub.size ) + { + break; + } + int n = len > B3_MAX_QUERY_NAME_LENGTH ? B3_MAX_QUERY_NAME_LENGTH : (int)len; + tags[loaded].key = key; + tags[loaded].id = id; + if ( n > 0 ) + { + memcpy( tags[loaded].queryName, sub.data + sub.cursor, (size_t)n ); + } + tags[loaded].queryName[n] = '\0'; + sub.cursor += len; + b3RecTagLookup_insert( map, key, loaded ); + loaded += 1; + } + + rdr->tags = tags; + rdr->tagCount = (int)loaded; + rdr->tagCapacity = (int)count; + rdr->tagMap = map; +} + +// Load the trailing registry block and fill rdr->slots/slotCount, then the optional tag table. +// Returns true on success. On failure sets rdr->ok = false and returns false. +static bool b3RecLoadSlots( b3RecReader* rdr, const void* data, int size, uint64_t registryOffset, uint64_t registryByteCount ) +{ + if ( registryOffset == 0 || registryByteCount == 0 ) + { + rdr->slots = NULL; + rdr->slotCount = 0; + return true; + } + + int regStart = (int)registryOffset; + int regEnd = regStart + (int)registryByteCount; + if ( regEnd > size ) + { + printf( "b3ReplayFile: registry block out of bounds\n" ); + return false; + } + if ( regStart + 4 > size ) + { + printf( "b3ReplayFile: registry too small\n" ); + return false; + } + + const uint8_t* dataEnd = (const uint8_t*)data + regEnd; + const uint8_t* rp = (const uint8_t*)data + regStart; + uint32_t count = (uint32_t)rp[0] | ( (uint32_t)rp[1] << 8 ) | ( (uint32_t)rp[2] << 16 ) | ( (uint32_t)rp[3] << 24 ); + rp += 4; + + if ( count == 0 ) + { + rdr->slots = NULL; + rdr->slotCount = 0; + b3RecLoadTags( rdr, rp, dataEnd ); + return true; + } + + // Each entry is at least 5 bytes (kind + 4-byte length). A count that cannot fit the remaining + // registry bytes is a corrupt header, so reject it before allocating. + if ( rp > dataEnd || (size_t)count > (size_t)( dataEnd - rp ) / 5 ) + { + printf( "b3ReplayFile: registry count out of range\n" ); + return false; + } + + b3RegistrySlot* slots = (b3RegistrySlot*)b3Alloc( (size_t)count * sizeof( b3RegistrySlot ) ); + memset( slots, 0, (size_t)count * sizeof( b3RegistrySlot ) ); + + for ( uint32_t i = 0; i < count; ++i ) + { + if ( rp + 5 > dataEnd ) + { + printf( "b3ReplayFile: registry truncated at entry %u\n", i ); + for ( uint32_t j = 0; j < i; ++j ) + { + if ( slots[j].bytes != NULL ) + { + b3Free( slots[j].bytes, (size_t)slots[j].byteCount ); + } + } + b3Free( slots, (size_t)count * sizeof( b3RegistrySlot ) ); + return false; + } + uint8_t kind = rp[0]; + uint32_t byteCount = (uint32_t)rp[1] | ( (uint32_t)rp[2] << 8 ) | ( (uint32_t)rp[3] << 16 ) | ( (uint32_t)rp[4] << 24 ); + rp += 5; + if ( rp + byteCount > dataEnd ) + { + printf( "b3ReplayFile: registry entry %u bytes out of bounds\n", i ); + for ( uint32_t j = 0; j < i; ++j ) + { + if ( slots[j].bytes != NULL ) + { + b3Free( slots[j].bytes, (size_t)slots[j].byteCount ); + } + } + b3Free( slots, (size_t)count * sizeof( b3RegistrySlot ) ); + return false; + } + uint8_t* bytes = (uint8_t*)b3Alloc( byteCount > 0 ? (size_t)byteCount : 1u ); + if ( byteCount > 0 ) + { + memcpy( bytes, rp, (size_t)byteCount ); + } + rp += byteCount; + slots[i].kind = (b3GeometryKind)kind; + slots[i].byteCount = (int)byteCount; + slots[i].bytes = bytes; + slots[i].live = NULL; + } + + rdr->slots = slots; + rdr->slotCount = (int)count; + b3RecLoadTags( rdr, rp, dataEnd ); + return true; +} + +// Free slots loaded by b3RecLoadSlots. +static void b3RecFreeSlots( b3RegistrySlot* slots, int slotCount ) +{ + if ( slots == NULL ) + { + return; + } + for ( int i = 0; i < slotCount; ++i ) + { + b3RegistrySlot* slot = slots + i; + if ( slot->live != NULL ) + { + switch ( slot->kind ) + { + // Mesh and height field have no separate live object; they borrow the bytes freed below. + case b3_geometryCompound: + b3Free( slot->live, (size_t)slot->byteCount ); + break; + default: + break; + } + } + if ( slot->bytes != NULL ) + { + b3Free( slot->bytes, slot->byteCount > 0 ? (size_t)slot->byteCount : 1u ); + } + } + b3Free( slots, (size_t)slotCount * sizeof( b3RegistrySlot ) ); +} + +// Walk the op stream once without dispatching: count Step ops and grab the first step's tuning. +static void b3RecScanFile( b3RecPlayer* player ) +{ + const uint8_t* data = player->data; + int size = player->registryEnd; + int cursor = player->headerEnd; + int frameCount = 0; + bool gotStep = false; + + while ( cursor + 4 <= size ) + { + uint8_t opcode = data[cursor]; + uint32_t payloadSize = + (uint32_t)data[cursor + 1] | ( (uint32_t)data[cursor + 2] << 8 ) | ( (uint32_t)data[cursor + 3] << 16 ); + int payloadStart = cursor + 4; + if ( payloadStart + (int)payloadSize > size ) + { + break; + } + if ( opcode == b3_recOpStep ) + { + frameCount += 1; + if ( !gotStep && payloadSize >= 12 ) + { + uint32_t dtBits = (uint32_t)data[payloadStart + 4] | ( (uint32_t)data[payloadStart + 5] << 8 ) | + ( (uint32_t)data[payloadStart + 6] << 16 ) | ( (uint32_t)data[payloadStart + 7] << 24 ); + memcpy( &player->recordedDt, &dtBits, 4 ); + player->recordedSubStepCount = + (int)( (uint32_t)data[payloadStart + 8] | ( (uint32_t)data[payloadStart + 9] << 8 ) | + ( (uint32_t)data[payloadStart + 10] << 16 ) | ( (uint32_t)data[payloadStart + 11] << 24 ) ); + gotStep = true; + } + } + else if ( opcode == 0xF2 && payloadSize >= (uint32_t)sizeof( b3AABB ) ) // RecordingBounds + { + // Payload is a single b3AABB (lower xyz, upper xyz as f32), written at stop so the + // viewer can frame the whole recorded motion without playing to the end. + memcpy( &player->bounds, data + payloadStart, sizeof( b3AABB ) ); + } + cursor = payloadStart + (int)payloadSize; + } + player->frameCount = frameCount; +} + +// Free one keyframe's heap. +static void b3FreeKeyframe( b3RecKeyframe* kf ) +{ + if ( kf->image != NULL ) + { + b3Free( kf->image, (size_t)kf->imageCapacity ); + } + if ( kf->bodyIds != NULL ) + { + b3Free( kf->bodyIds, (size_t)kf->bodyIdCount * sizeof( b3BodyId ) ); + } +} + +// Pre-populate keyframeRec's registry to mirror rdr.slots so geometry ids stay stable during +// b3SerializeWorld. Each slot becomes one entry with id == slot index, even byte-identical slots that +// a hash collision left undeduplicated in an already-recorded file. b3SerializeWorld then resolves a +// live blob back to a valid slot index via the registry's exact dedup, so capture never grows it. +static void b3RecSeedKeyframeRegistry( b3RecPlayer* player ) +{ + b3GeometryRegistry* reg = &player->keyframeRec->registry; + for ( int i = 0; i < player->rdr.slotCount; ++i ) + { + b3RegistrySlot* slot = player->rdr.slots + i; + // Copy so the registry can take ownership. + int n = slot->byteCount > 0 ? slot->byteCount : 1; + uint8_t* copy = (uint8_t*)b3Alloc( (size_t)n ); + if ( slot->byteCount > 0 ) + { + memcpy( copy, slot->bytes, (size_t)slot->byteCount ); + } + uint64_t h = b3Hash64Blob( slot->bytes, slot->byteCount ); + uint32_t id = b3AppendGeometry( reg, slot->kind, h, copy, slot->byteCount ); + // Seeding in order without dedup keeps id == slot index. + B3_ASSERT( id == (uint32_t)i ); + (void)id; + } +} + +// Capture a restore-point keyframe for the just-completed frame. rdr.cursor already points to +// the next frame's Step op. +static void b3RecCaptureKeyframe( b3RecPlayer* player ) +{ + b3World* world = b3GetWorldFromId( player->rdr.replayWorldId ); + b3RecBuffer buf = { 0 }; + + int regCountBefore = player->keyframeRec->registry.count; + B3_UNUSED( regCountBefore ); + + b3SerializeWorld( world, &buf, player->keyframeRec ); + // Registry must not grow: all geometry was pre-seeded and the registry dedups exactly. + B3_ASSERT( player->keyframeRec->registry.count == regCountBefore ); + + size_t bodyBytes = (size_t)player->bodyIdCount * sizeof( b3BodyId ); + size_t newBytes = (size_t)buf.capacity + bodyBytes; + + // Make room under the budget by doubling the spacing and evicting off-grid keyframes. + while ( player->keyframeCount > 0 && player->keyframeBytes + newBytes > player->keyframeBudget ) + { + player->keyframeInterval *= 2; + int kept = 0; + size_t keptBytes = 0; + for ( int i = 0; i < player->keyframeCount; ++i ) + { + b3RecKeyframe* kf = player->keyframes + i; + if ( kf->frame % player->keyframeInterval == 0 ) + { + player->keyframes[kept] = *kf; + keptBytes += (size_t)kf->imageCapacity + (size_t)kf->bodyIdCount * sizeof( b3BodyId ); + kept += 1; + } + else + { + b3FreeKeyframe( kf ); + } + } + bool progress = ( kept < player->keyframeCount ); + player->keyframeCount = kept; + player->keyframeBytes = keptBytes; + if ( !progress ) + { + break; + } + } + + // Grow the keyframe ring if needed. + if ( player->keyframeCount >= player->keyframeCapacity ) + { + int newCap = player->keyframeCapacity < 8 ? 8 : player->keyframeCapacity * 2; + player->keyframes = (b3RecKeyframe*)b3GrowAlloc( + player->keyframes, player->keyframeCapacity * (int)sizeof( b3RecKeyframe ), newCap * (int)sizeof( b3RecKeyframe ) ); + player->keyframeCapacity = newCap; + } + + b3RecKeyframe* kf = player->keyframes + player->keyframeCount; + kf->image = buf.data; + kf->imageSize = buf.size; + kf->imageCapacity = buf.capacity; + kf->frame = player->frame; + kf->cursor = player->rdr.cursor; + kf->divergeFrame = player->divergeFrame; + kf->diverged = player->rdr.diverged; + kf->bodyIdCount = player->bodyIdCount; + kf->bodyIds = NULL; + if ( bodyBytes > 0 ) + { + kf->bodyIds = (b3BodyId*)b3Alloc( bodyBytes ); + memcpy( kf->bodyIds, player->bodyIds, bodyBytes ); + } + + player->keyframeBytes += newBytes; + player->keyframeCount += 1; + player->lastKeyframeFrame = player->frame; +} + +// Restore the world in-place from a keyframe image. +static void b3RecPlayerRestoreKeyframe( b3RecPlayer* player, const b3RecKeyframe* kf ) +{ + b3World* world = b3GetWorldFromId( player->rdr.replayWorldId ); + if ( b3DeserializeIntoShell( kf->image, kf->imageSize, world, &player->rdr ) == false ) + { + player->rdr.ok = false; + return; + } + player->rdr.cursor = kf->cursor; + player->rdr.ok = true; + player->rdr.diverged = kf->diverged; + player->frame = kf->frame; + player->divergeFrame = kf->divergeFrame; + player->atEnd = false; + player->atPreStep = false; + + // Restore the outliner list verbatim so ordinals match this frame. + b3RecGrow( (void**)&player->bodyIds, &player->bodyIdCap, kf->bodyIdCount, 0, (int)sizeof( b3BodyId ) ); + player->bodyIdCount = kf->bodyIdCount; + if ( kf->bodyIdCount > 0 ) + { + memcpy( player->bodyIds, kf->bodyIds, (size_t)kf->bodyIdCount * sizeof( b3BodyId ) ); + } +} + +// Create a replay world carrying the host debug-shape callbacks. Every world the player +// stands up funnels through here so the sample renderer can draw replayed shapes. +static b3WorldId b3RecPlayerCreateWorld( const b3RecPlayer* player ) +{ + b3WorldDef worldDef = b3DefaultWorldDef(); + worldDef.createDebugShape = player->createDebugShape; + worldDef.destroyDebugShape = player->destroyDebugShape; + worldDef.userDebugShapeContext = player->debugShapeContext; + // Carry the requested worker count so a rebuild on Restart or backward seek keeps the same + // graph partitioning. Replaying at a different count than recorded is a determinism check. + worldDef.workerCount = b3MaxInt( 1, player->recordedWorkerCount ); + return b3CreateWorld( &worldDef ); +} + +b3RecPlayer* b3RecPlayer_Create( const void* data, int size, int workerCount ) +{ + if ( data == NULL || size < (int)sizeof( b3RecHeader ) ) + { + printf( "b3RecPlayer_Create: recording too small\n" ); + return NULL; + } + + b3RecHeader hdr; + memcpy( &hdr, data, sizeof( hdr ) ); + + if ( hdr.magic != B3_REC_MAGIC ) + { + printf( "b3RecPlayer_Create: bad magic 0x%08X\n", hdr.magic ); + return NULL; + } + // Only the major version is breaking. Minor bumps are additive op-stream changes that keep the + // header shape, and the dispatcher skips opcodes it doesn't know, so a minor mismatch still loads. + if ( hdr.versionMajor != B3_REC_VERSION_MAJOR ) + { + printf( "b3RecPlayer_Create: version mismatch %u.%u vs %u.%u\n", hdr.versionMajor, hdr.versionMinor, B3_REC_VERSION_MAJOR, + B3_REC_VERSION_MINOR ); + return NULL; + } + if ( hdr.pointerWidth != (uint8_t)sizeof( void* ) ) + { + printf( "b3RecPlayer_Create: pointer width mismatch %u vs %u\n", hdr.pointerWidth, (unsigned)sizeof( void* ) ); + return NULL; + } + if ( hdr.bigEndian != 0 ) + { + printf( "b3RecPlayer_Create: big-endian recording not supported\n" ); + return NULL; + } + + // Every recording is snapshot-seeded: the seed blob sits between the header and the op stream. + if ( hdr.snapshotSize == 0 ) + { + printf( "b3RecPlayer_Create: missing snapshot seed\n" ); + return NULL; + } + + // snapshotSize and registryOffset are 64-bit and come from the file. Validate in 64-bit so a + // hostile value can't wrap when narrowed to int, then narrow once the bounds are known good. + uint64_t headerEnd64 = (uint64_t)sizeof( b3RecHeader ) + hdr.snapshotSize; + uint64_t registryEnd64 = ( hdr.registryOffset != 0 ) ? hdr.registryOffset : (uint64_t)size; + + if ( headerEnd64 < sizeof( b3RecHeader ) || headerEnd64 > registryEnd64 || registryEnd64 > (uint64_t)size ) + { + printf( "b3RecPlayer_Create: corrupt offsets\n" ); + return NULL; + } + + int headerEnd = (int)headerEnd64; + int registryEnd = (int)registryEnd64; + + // Own a private copy so the caller can free their buffer right away. + uint8_t* copy = b3Alloc( (size_t)size ); + memcpy( copy, data, (size_t)size ); + + b3RecPlayer* player = b3Alloc( sizeof( b3RecPlayer ) ); + memset( player, 0, sizeof( b3RecPlayer ) ); + + player->data = copy; + player->size = size; + player->headerEnd = headerEnd; + player->registryEnd = registryEnd; + player->lengthScale = hdr.lengthScale; + player->previousLengthScale = b3GetLengthUnitsPerMeter(); + player->frame = 0; + player->frameCount = 0; + player->recordedDt = 0.0f; + player->recordedSubStepCount = 0; + player->recordedWorkerCount = workerCount; + player->atEnd = false; + player->atPreStep = false; + player->divergeFrame = -1; + player->keyframeMinInterval = B3_REC_KEYFRAME_INTERVAL_DEFAULT; + player->keyframeInterval = B3_REC_KEYFRAME_INTERVAL_DEFAULT; + player->keyframeBudget = B3_REC_KEYFRAME_BUDGET_DEFAULT; + player->lastKeyframeFrame = 0; + + // Set length scale so replay reproduces the same tuning constants. + if ( hdr.lengthScale > 0.0f ) + { + b3SetLengthUnitsPerMeter( hdr.lengthScale ); + } + + // Count frames and read first step's dt so the viewer can show hz up front. + b3RecScanFile( player ); + + // Create the replay world. Debug-shape callbacks are NULL here; the sample wires + // them right after Create via b3RecPlayer_SetDebugShapeCallbacks, which rebuilds. + b3WorldId worldId = b3RecPlayerCreateWorld( player ); + + // Initialize the reader. + player->rdr.data = copy; + player->rdr.size = size; + player->rdr.cursor = headerEnd; + player->rdr.replayWorldId = worldId; + player->rdr.ok = true; + player->rdr.diverged = false; + player->rdr.owner = player; + + // Load the trailing geometry registry. + if ( !b3RecLoadSlots( &player->rdr, copy, size, hdr.registryOffset, hdr.registryByteCount ) ) + { + b3DestroyWorld( worldId ); + b3Free( copy, (size_t)size ); + b3Free( player, sizeof( b3RecPlayer ) ); + return NULL; + } + + // Restore the seed snapshot to stand up the replay world. The blob doubles as the frame-0 + // restore image, owned by the copy held above. + { + int snapStart = (int)sizeof( b3RecHeader ); + int snapSize = (int)hdr.snapshotSize; + b3World* replayWorld = b3GetWorldFromId( worldId ); + if ( b3DeserializeIntoShell( copy + snapStart, snapSize, replayWorld, &player->rdr ) == false ) + { + printf( "b3RecPlayer_Create: snapshot deserialization failed\n" ); + b3DestroyWorld( worldId ); + b3RecFreeSlots( player->rdr.slots, player->rdr.slotCount ); + if ( player->rdr.tags != NULL ) + { + b3Free( player->rdr.tags, (size_t)player->rdr.tagCapacity * sizeof( b3RecTag ) ); + } + if ( player->rdr.tagMap != NULL ) + { + b3RecTagLookup_cleanup( (b3RecTagLookup*)player->rdr.tagMap ); + b3Free( player->rdr.tagMap, sizeof( b3RecTagLookup ) ); + } + b3Free( copy, (size_t)size ); + b3Free( player, sizeof( b3RecPlayer ) ); + return NULL; + } + player->rdr.cursor = headerEnd; + player->frame0Image = copy + snapStart; + player->frame0Size = snapSize; + } + + // Seed the outliner from the restored world (snapshot bodies bypass the create hook) and save + // the frame-0 restore copy. + b3RecSeedFrame0BodyIds( player ); + + // Build the keyframe recording with a pre-seeded registry that mirrors rdr.slots, + // so b3SerializeWorld geometry ids stay stable across captures. + player->keyframeRec = b3CreateRecording( 0 ); + b3RecSeedKeyframeRegistry( player ); + + return player; +} + +void b3RecPlayer_Destroy( b3RecPlayer* player ) +{ + if ( player == NULL ) + { + return; + } + + if ( b3World_IsValid( player->rdr.replayWorldId ) ) + { + b3DestroyWorld( player->rdr.replayWorldId ); + } + + // Free live geometry after destroying the world (slot->live may be used by the world). + b3RecFreeSlots( player->rdr.slots, player->rdr.slotCount ); + + // Free reader scratch. + if ( player->rdr.matScratch != NULL ) + { + b3Free( player->rdr.matScratch, (size_t)player->rdr.matScratchCap * sizeof( b3SurfaceMaterial ) ); + } + if ( player->rdr.proxyScratch != NULL ) + { + b3Free( player->rdr.proxyScratch, (size_t)player->rdr.proxyScratchCap * sizeof( b3Vec3 ) ); + } + if ( player->rdr.hits != NULL ) + { + b3Free( player->rdr.hits, (size_t)player->rdr.hitCap * sizeof( b3RecRecordedHit ) ); + } + if ( player->rdr.tags != NULL ) + { + b3Free( player->rdr.tags, (size_t)player->rdr.tagCapacity * sizeof( b3RecTag ) ); + } + if ( player->rdr.tagMap != NULL ) + { + b3RecTagLookup_cleanup( (b3RecTagLookup*)player->rdr.tagMap ); + b3Free( player->rdr.tagMap, sizeof( b3RecTagLookup ) ); + } + + // Free the per-frame query store. + if ( player->frameQueries != NULL ) + { + b3Free( player->frameQueries, (size_t)player->frameQueryCap * sizeof( b3RecDrawQuery ) ); + } + if ( player->frameHits != NULL ) + { + b3Free( player->frameHits, (size_t)player->frameHitCap * sizeof( b3RecRecordedHit ) ); + } + + // Free keyframe ring. + for ( int i = 0; i < player->keyframeCount; ++i ) + { + b3FreeKeyframe( player->keyframes + i ); + } + if ( player->keyframes != NULL ) + { + b3Free( player->keyframes, (size_t)player->keyframeCapacity * sizeof( b3RecKeyframe ) ); + } + + // The keyframe recording owns only its buffer and registry; b3DestroyRecording frees both. + if ( player->keyframeRec != NULL ) + { + b3DestroyRecording( player->keyframeRec ); + } + + // Free the outliner body lists. + if ( player->bodyIds != NULL ) + { + b3Free( player->bodyIds, (size_t)player->bodyIdCap * sizeof( b3BodyId ) ); + } + if ( player->frame0BodyIds != NULL ) + { + b3Free( player->frame0BodyIds, (size_t)player->frame0BodyIdCount * sizeof( b3BodyId ) ); + } + + // frame0Image points into the owned data copy, not separately allocated. + + b3Free( player->data, (size_t)player->size ); + + // Restore the global length scale. + b3SetLengthUnitsPerMeter( player->previousLengthScale ); + + b3Free( player, sizeof( b3RecPlayer ) ); +} + +bool b3RecPlayer_StepFrame( b3RecPlayer* player ) +{ + // This is never true when full stepping + player->atPreStep = false; + + if ( player->atEnd ) + { + return false; + } + + // Reset the per-frame query store before this frame's records are dispatched. + player->frameQueryCount = 0; + player->frameHitCount = 0; + + // A frame is its leading inputs (queries and between-step mutators), one Step, and the Step's + // trailing StateHash. Queries are recorded before the Step they belong to, so they stash here + // against the world state they were computed for. + bool stepped = false; + for ( ;; ) + { + if ( player->rdr.cursor >= player->registryEnd || !player->rdr.ok ) + { + player->atEnd = true; + return stepped; + } + + // Once stepped, the StateHash is the only record still belonging to this frame. Anything else + // begins the next frame, so stop and let the next StepFrame consume it. Capture a keyframe at + // the boundary. + if ( stepped && player->rdr.data[player->rdr.cursor] != b3_recOpStateHash ) + { + if ( player->frame > player->lastKeyframeFrame && player->frame % player->keyframeInterval == 0 ) + { + b3RecCaptureKeyframe( player ); + } + return true; + } + + int op = b3RecDispatchOne( &player->rdr ); + if ( op < 0 ) + { + player->atEnd = true; + return stepped; + } + if ( op == b3_recOpDestroyWorld ) // end of recording + { + player->atEnd = true; + return stepped; + } + if ( op == b3_recOpStep ) + { + player->frame += 1; + stepped = true; + } + else if ( op == b3_recOpStateHash ) // trailing record of the frame just stepped + { + // Latch the first frame whose state hash diverged. The hash belongs to the frame Step just + // advanced, so latch against the current frame, not the next Step which would be one late. + if ( player->divergeFrame < 0 && player->rdr.diverged ) + { + player->divergeFrame = player->frame; + } + } + } +} + +void b3RecPlayer_SubStepFrame( b3RecPlayer* player ) +{ + if ( player->atEnd ) + { + return; + } + + // Reset the per-frame query store before this frame's records are dispatched. + if ( player->atPreStep == false ) + { + player->frameQueryCount = 0; + player->frameHitCount = 0; + } + + // A frame is its leading inputs (queries and between-step mutators), one Step, and the Step's + // trailing StateHash. Queries are recorded before the Step they belong to, so they stash here + // against the world state they were computed for. + bool stepped = false; + bool haveCreateBodyOp = false; + for ( ;; ) + { + if ( player->rdr.cursor >= player->registryEnd || !player->rdr.ok ) + { + player->atEnd = true; + player->atPreStep = false; + return; + } + + // Once stepped, the StateHash is the only record still belonging to this frame. Anything else + // begins the next frame, so stop and let the next StepFrame consume it. Capture a keyframe at + // the boundary. + uint8_t currentOpCode = player->rdr.data[player->rdr.cursor]; + if ( stepped && currentOpCode != b3_recOpStateHash ) + { + if ( player->frame > player->lastKeyframeFrame && player->frame % player->keyframeInterval == 0 ) + { + b3RecCaptureKeyframe( player ); + } + return; + } + + if ( player->atPreStep == false && haveCreateBodyOp == true && currentOpCode == b3_recOpStep ) + { + player->atPreStep = true; + return; + } + + int op = b3RecDispatchOne( &player->rdr ); + if ( op < 0 ) + { + player->atEnd = true; + player->atPreStep = false; + return; + } + if ( op == b3_recOpDestroyWorld ) // end of recording + { + player->atEnd = true; + player->atPreStep = false; + return; + } + + if ( op == b3_recOpCreateBody ) + { + B3_ASSERT( player->atPreStep == false ); + haveCreateBodyOp = true; + } + + if ( op == b3_recOpStep ) + { + player->atPreStep = false; + player->frame += 1; + stepped = true; + } + else if ( op == b3_recOpStateHash ) // trailing record of the frame just stepped + { + // Latch the first frame whose state hash diverged. The hash belongs to the frame Step just + // advanced, so latch against the current frame, not the next Step which would be one late. + if ( player->divergeFrame < 0 && player->rdr.diverged ) + { + player->divergeFrame = player->frame; + } + } + } +} + +void b3RecPlayer_Restart( b3RecPlayer* player ) +{ + // Restore the frame-0 image in place so the replay world id stays stable across a restart or + // backward scrub. Stepping resumes at the first Step, which rebuilds the body list deterministically. + b3World* world = b3GetWorldFromId( player->rdr.replayWorldId ); + if ( b3DeserializeIntoShell( player->frame0Image, player->frame0Size, world, &player->rdr ) == false ) + { + player->rdr.ok = false; + return; + } + player->rdr.cursor = player->headerEnd; + player->rdr.ok = true; + player->rdr.diverged = false; + player->frame = 0; + player->divergeFrame = -1; + player->atEnd = false; + player->atPreStep = false; + + // Frame 0 is the pre-step snapshot with no recorded queries, so clear the per-frame store. This + // keeps the last stepped frame's queries from lingering on a restart or a backward scrub to 0. + player->frameQueryCount = 0; + player->frameHitCount = 0; + + // Roll the outliner body list back to its frame-0 contents. + b3RecGrow( (void**)&player->bodyIds, &player->bodyIdCap, player->frame0BodyIdCount, 0, (int)sizeof( b3BodyId ) ); + player->bodyIdCount = player->frame0BodyIdCount; + if ( player->frame0BodyIdCount > 0 ) + { + memcpy( player->bodyIds, player->frame0BodyIds, (size_t)player->frame0BodyIdCount * sizeof( b3BodyId ) ); + } +} + +void b3RecPlayer_SeekFrame( b3RecPlayer* player, int targetFrame ) +{ + if ( player == NULL ) + { + return; + } + + player->atPreStep = false; + + if ( targetFrame < 0 ) + { + targetFrame = 0; + } + + // Find the best keyframe strictly before the target. + const b3RecKeyframe* best = NULL; + for ( int i = 0; i < player->keyframeCount; ++i ) + { + const b3RecKeyframe* kf = player->keyframes + i; + if ( kf->frame < targetFrame && ( best == NULL || kf->frame > best->frame ) ) + { + best = kf; + } + } + + if ( targetFrame < player->frame ) + { + // Backward seek: restore keyframe or restart from frame 0. + if ( best != NULL ) + { + b3RecPlayerRestoreKeyframe( player, best ); + } + else + { + b3RecPlayer_Restart( player ); + } + } + else if ( best != NULL && best->frame > player->frame ) + { + // Forward seek that can skip ahead via a keyframe. + b3RecPlayerRestoreKeyframe( player, best ); + } + + while ( player->frame < targetFrame && b3RecPlayer_StepFrame( player ) ) + { + } +} + +b3WorldId b3RecPlayer_GetWorldId( const b3RecPlayer* player ) +{ + return player != NULL ? player->rdr.replayWorldId : b3_nullWorldId; +} + +int b3RecPlayer_GetFrame( const b3RecPlayer* player ) +{ + return player != NULL ? player->frame : 0; +} + +int b3RecPlayer_GetFrameCount( const b3RecPlayer* player ) +{ + return player != NULL ? player->frameCount : 0; +} + +bool b3RecPlayer_IsAtEnd( const b3RecPlayer* player ) +{ + return player != NULL ? player->atEnd : true; +} + +bool b3RecPlayer_IsAtPreStep( const b3RecPlayer* player ) +{ + return player != NULL ? player->atPreStep : false; +} + +bool b3RecPlayer_HasDiverged( const b3RecPlayer* player ) +{ + return player != NULL ? player->rdr.diverged : false; +} + +b3RecPlayerInfo b3RecPlayer_GetInfo( const b3RecPlayer* player ) +{ + b3RecPlayerInfo info = { 0 }; + if ( player != NULL ) + { + info.frameCount = player->frameCount; + info.workerCount = player->recordedWorkerCount; + info.timeStep = player->recordedDt; + info.subStepCount = player->recordedSubStepCount; + info.lengthScale = player->lengthScale; + info.bounds = player->bounds; + } + return info; +} + +int b3RecPlayer_GetDivergeFrame( const b3RecPlayer* player ) +{ + return player != NULL ? player->divergeFrame : -1; +} + +void b3RecPlayer_SetWorkerCount( b3RecPlayer* player, int count ) +{ + if ( player == NULL ) + { + return; + } + + player->recordedWorkerCount = b3ClampInt( count, 1, B3_MAX_WORKERS ); + + // Apply to the live world now so the next steps re-partition without a rebuild. Worker count is + // host state, not part of a keyframe image, so it survives an in-place restore. A rebuild on + // Restart or deep backward seek picks the count back up through b3RecPlayerCreateWorld. + if ( b3World_IsValid( player->rdr.replayWorldId ) ) + { + b3World_SetWorkerCount( player->rdr.replayWorldId, player->recordedWorkerCount ); + } +} + +void b3RecPlayer_SetKeyframePolicy( b3RecPlayer* player, size_t budgetBytes, int minIntervalFrames ) +{ + if ( player == NULL ) + { + return; + } + if ( budgetBytes > 0 ) + { + player->keyframeBudget = budgetBytes; + } + if ( minIntervalFrames > 0 ) + { + player->keyframeMinInterval = minIntervalFrames; + } + + // Drop the ring so it repopulates under the new policy on the next replay. + for ( int i = 0; i < player->keyframeCount; ++i ) + { + b3FreeKeyframe( player->keyframes + i ); + } + player->keyframeCount = 0; + player->keyframeBytes = 0; + player->keyframeInterval = player->keyframeMinInterval; + player->lastKeyframeFrame = 0; +} + +size_t b3RecPlayer_GetKeyframeBudget( const b3RecPlayer* player ) +{ + return player != NULL ? player->keyframeBudget : 0; +} + +int b3RecPlayer_GetKeyframeMinInterval( const b3RecPlayer* player ) +{ + return player != NULL ? player->keyframeMinInterval : 0; +} + +int b3RecPlayer_GetKeyframeInterval( const b3RecPlayer* player ) +{ + return player != NULL ? player->keyframeInterval : 0; +} + +size_t b3RecPlayer_GetKeyframeBytes( const b3RecPlayer* player ) +{ + return player != NULL ? player->keyframeBytes : 0; +} + +int b3RecPlayer_GetBodyCount( const b3RecPlayer* player ) +{ + return player != NULL ? player->bodyIdCount : 0; +} + +b3BodyId b3RecPlayer_GetBodyId( const b3RecPlayer* player, int index ) +{ + if ( player == NULL || index < 0 || index >= player->bodyIdCount ) + { + return b3_nullBodyId; + } + return player->bodyIds[index]; +} + +// A selected query draws in one reserved color so it stands out when every query is drawn at once. +static b3HexColor b3RecQuerySelColor( bool selected, b3HexColor base ) +{ + return selected ? b3_colorPlum : base; +} + +// Highlight each reported overlap shape by its AABB. Skip any destroyed since the query, per the +// b3Shape_GetAABB contract that overlap results may contain stale shapes. +static void b3RecDrawHitBounds( const b3RecPlayer* player, const b3RecDrawQuery* q, b3DebugDraw* draw, b3HexColor color ) +{ + if ( draw->DrawBoundsFcn == NULL ) + { + return; + } + for ( int hi = q->hitStart; hi < q->hitStart + q->hitCount; ++hi ) + { + b3ShapeId id = player->frameHits[hi].id; + if ( b3Shape_IsValid( id ) == false ) + { + continue; + } + draw->DrawBoundsFcn( b3Shape_GetAABB( id ), color, draw->context ); + } +} + +// Draw a recorded shape proxy at basePos. A lone point with no radius draws a fat point, a lone point +// with a radius draws a translucent sphere, and a multi-point cloud draws its points. Capsule and hull +// proxies are rare and fall through to the point cloud for now. +static void b3RecDrawProxy( b3DebugDraw* draw, b3Pos basePos, const b3RecDrawQuery* q, b3HexColor color ) +{ + if ( q->proxyCount == 1 ) + { + b3Pos p = b3OffsetPos( basePos, q->proxyPoints[0] ); + if ( q->proxyRadius > 0.0f ) + { + if ( draw->DrawSphereFcn ) + { + draw->DrawSphereFcn( p, q->proxyRadius, color, 0.5f, draw->context ); + } + } + else if ( draw->DrawPointFcn ) + { + draw->DrawPointFcn( p, 10.0f, color, draw->context ); + } + } + else if ( q->proxyCount == 2 && q->proxyRadius > 0.0f ) + { + if ( draw->DrawCapsuleFcn ) + { + b3Pos p1 = b3OffsetPos( basePos, q->proxyPoints[0] ); + b3Pos p2 = b3OffsetPos( basePos, q->proxyPoints[1] ); + draw->DrawCapsuleFcn( p1, p2, q->proxyRadius, color, 0.5f, draw->context ); + } + } + else if ( q->proxyCount >= 2 && draw->DrawPointFcn ) + { + for ( int i = 0; i < q->proxyCount; ++i ) + { + draw->DrawPointFcn( b3OffsetPos( basePos, q->proxyPoints[i] ), 6.0f, color, draw->context ); + } + } +} + +void b3RecPlayer_DrawFrameQueries( b3RecPlayer* player, b3DebugDraw* draw, int queryIndex, int selectedIndex ) +{ + if ( player == NULL || draw == NULL ) + { + return; + } + + // queryIndex < 0 draws every query, otherwise just the one selected in the viewer. The query at + // selectedIndex draws in one reserved color and is labeled, so it stands out among the rest. + for ( int qi = 0; qi < player->frameQueryCount; ++qi ) + { + if ( queryIndex >= 0 && qi != queryIndex ) + { + continue; + } + + const b3RecDrawQuery* q = &player->frameQueries[qi]; + bool selected = ( qi == selectedIndex ); + + switch ( q->kind ) + { + case B3_RECQ_CAST_RAY: + case B3_RECQ_CAST_RAY_CLOSEST: + { + b3Pos origin = q->origin; + b3Pos end = b3OffsetPos( origin, q->translation ); + if ( draw->DrawSegmentFcn ) + { + draw->DrawSegmentFcn( origin, end, b3RecQuerySelColor( selected, b3_colorYellow ), draw->context ); + } + for ( int hi = q->hitStart; hi < q->hitStart + q->hitCount; ++hi ) + { + const b3RecRecordedHit* h = &player->frameHits[hi]; + if ( draw->DrawPointFcn ) + { + draw->DrawPointFcn( h->point, 4.0f, b3RecQuerySelColor( selected, b3_colorYellow ), draw->context ); + } + if ( draw->DrawSegmentFcn ) + { + draw->DrawSegmentFcn( h->point, b3OffsetPos( h->point, b3MulSV( 0.2f, h->normal ) ), + b3RecQuerySelColor( selected, b3_colorYellowGreen ), draw->context ); + } + } + break; + } + case B3_RECQ_CAST_SHAPE: + { + // Draw the cast line and the proxy at its start, then each hit point and normal. + if ( draw->DrawSegmentFcn ) + { + draw->DrawSegmentFcn( q->origin, b3OffsetPos( q->origin, q->translation ), + b3RecQuerySelColor( selected, b3_colorSkyBlue ), draw->context ); + } + b3RecDrawProxy( draw, q->origin, q, b3RecQuerySelColor( selected, b3_colorLightGreen ) ); + for ( int hi = q->hitStart; hi < q->hitStart + q->hitCount; ++hi ) + { + const b3RecRecordedHit* h = &player->frameHits[hi]; + if ( draw->DrawPointFcn ) + { + draw->DrawPointFcn( h->point, 4.0f, b3RecQuerySelColor( selected, b3_colorSkyBlue ), draw->context ); + } + if ( draw->DrawSegmentFcn ) + { + draw->DrawSegmentFcn( h->point, b3OffsetPos( h->point, b3MulSV( 0.2f, h->normal ) ), + b3RecQuerySelColor( selected, b3_colorLightSkyBlue ), draw->context ); + } + if ( draw->DrawSphereFcn ) + { + b3Pos p = b3OffsetPos( q->origin, b3MulSV( h->fraction, q->translation ) ); + b3RecDrawProxy( draw, p, q, b3RecQuerySelColor( selected, b3_colorSkyBlue ) ); + } + } + break; + } + case B3_RECQ_CAST_MOVER: + { + b3Pos c1 = b3OffsetPos( q->origin, q->mover.center1 ); + b3Pos c2 = b3OffsetPos( q->origin, q->mover.center2 ); + b3HexColor c = b3_colorLightSkyBlue; + if ( draw->DrawCapsuleFcn ) + { + draw->DrawCapsuleFcn( c1, c2, q->mover.radius, b3RecQuerySelColor( selected, c ), 0.6f, draw->context ); + + if ( q->castFraction > 0.01f ) + { + b3Vec3 d = b3MulSV( q->castFraction, q->translation ); + c1 = b3OffsetPos( c1, d ); + c2 = b3OffsetPos( c2, d ); + draw->DrawCapsuleFcn( c1, c2, q->mover.radius, c, 0.3f, draw->context ); + } + } + break; + } + + case B3_RECQ_COLLIDE_MOVER: + { + b3Pos c1 = b3OffsetPos( q->origin, q->mover.center1 ); + b3Pos c2 = b3OffsetPos( q->origin, q->mover.center2 ); + b3HexColor c = b3_colorTan; + if ( draw->DrawCapsuleFcn ) + { + draw->DrawCapsuleFcn( c1, c2, q->mover.radius, b3RecQuerySelColor( selected, c ), 0.6f, draw->context ); + } + + for ( int hi = q->hitStart; hi < q->hitStart + q->hitCount; ++hi ) + { + const b3RecRecordedHit* h = &player->frameHits[hi]; + b3Pos point = b3OffsetPos( q->origin, h->plane.point ); + if ( draw->DrawSegmentFcn ) + { + draw->DrawSegmentFcn( point, b3OffsetPos( point, b3MulSV( 0.2f, h->plane.plane.normal ) ), + b3RecQuerySelColor( selected, b3_colorOrange ), draw->context ); + } + } + break; + } + + case B3_RECQ_OVERLAP_AABB: + { + if ( draw->DrawBoundsFcn ) + { + draw->DrawBoundsFcn( q->aabb, b3RecQuerySelColor( selected, b3_colorLimeGreen ), draw->context ); + } + b3RecDrawHitBounds( player, q, draw, b3RecQuerySelColor( selected, b3_colorMagenta ) ); + break; + } + case B3_RECQ_OVERLAP_SHAPE: + { + // The overlap proxy sits at the origin; draw it, then the overlapping shape bounds. + b3RecDrawProxy( draw, q->origin, q, b3RecQuerySelColor( selected, b3_colorLimeGreen ) ); + b3RecDrawHitBounds( player, q, draw, b3RecQuerySelColor( selected, b3_colorMagenta ) ); + break; + } + default: + break; + } + + // Label the selected query at its origin so it reads by name among the others. The overlap AABB + // has no origin, so anchor at the box center. Untagged queries (no key) rely on the color alone. + if ( selected && q->key != 0 && draw->DrawStringFcn != NULL ) + { + const char* name = NULL; + uint64_t id = 0; + if ( player->rdr.tagMap != NULL ) + { + b3RecTagLookup_itr it = b3RecTagLookup_get( (b3RecTagLookup*)player->rdr.tagMap, q->key ); + if ( b3RecTagLookup_is_end( it ) == false ) + { + const b3RecTag* tag = &player->rdr.tags[it.data->val]; + name = tag->queryName; + id = tag->id; + } + } + char label[64]; + if ( name != NULL && name[0] != '\0' && id != 0 ) + { + snprintf( label, sizeof( label ), "%.40s (%" PRIu64 ")", name, id ); + } + else if ( name != NULL && name[0] != '\0' ) + { + snprintf( label, sizeof( label ), "%.40s", name ); + } + else + { + snprintf( label, sizeof( label ), "#%" PRIu64, id ); + } + b3Pos labelPos = q->origin; + if ( q->kind == B3_RECQ_OVERLAP_AABB ) + { + labelPos = b3ToPos( b3AABB_Center( q->aabb ) ); + } + else if ( q->kind == B3_RECQ_CAST_MOVER || q->kind == B3_RECQ_COLLIDE_MOVER ) + { + // Sit the label just past the center2 end cap, which for an upright mover is above it. + b3Pos c1 = b3OffsetPos( q->origin, q->mover.center1 ); + b3Pos c2 = b3OffsetPos( q->origin, q->mover.center2 ); + b3Vec3 dir = b3Normalize( b3SubPos( c2, c1 ) ); + labelPos = b3OffsetPos( c2, b3MulSV( 1.25f * q->mover.radius, dir ) ); + } + draw->DrawStringFcn( labelPos, label, b3_colorWhite, draw->context ); + } + } +} + +// The internal b3RecQueryKind values match the public b3RecQueryType, so the kind copies across as a +// plain cast. Pin the first and last kinds to catch enum drift. +_Static_assert( b3_recQueryOverlapAABB == 0 && B3_RECQ_OVERLAP_AABB == 0, "query type enum drift" ); +_Static_assert( b3_recQueryCollideMover == 6 && B3_RECQ_COLLIDE_MOVER == 6, "query type enum drift" ); + +int b3RecPlayer_GetFrameQueryCount( const b3RecPlayer* player ) +{ + return player != NULL ? player->frameQueryCount : 0; +} + +b3RecQueryInfo b3RecPlayer_GetFrameQuery( const b3RecPlayer* player, int index ) +{ + b3RecQueryInfo info = { 0 }; + if ( player == NULL || index < 0 || index >= player->frameQueryCount ) + { + return info; + } + + const b3RecDrawQuery* q = &player->frameQueries[index]; + info.type = (b3RecQueryType)q->kind; + info.filter = q->filter; + info.aabb = q->aabb; + info.origin = q->origin; + info.translation = q->translation; + info.hitCount = q->hitCount; + info.key = q->key; + info.id = 0; + info.name = NULL; + if ( q->key != 0 && player->rdr.tagMap != NULL ) + { + b3RecTagLookup_itr it = b3RecTagLookup_get( (b3RecTagLookup*)player->rdr.tagMap, q->key ); + if ( b3RecTagLookup_is_end( it ) == false ) + { + const b3RecTag* tag = &player->rdr.tags[it.data->val]; + info.id = tag->id; + // An id-only tag interns an empty name; report it as none so the viewer shows the id alone. + info.name = tag->queryName[0] != '\0' ? tag->queryName : NULL; + } + } + return info; +} + +b3RecQueryHit b3RecPlayer_GetFrameQueryHit( const b3RecPlayer* player, int queryIndex, int hitIndex ) +{ + b3RecQueryHit hit = { 0 }; + if ( player == NULL || queryIndex < 0 || queryIndex >= player->frameQueryCount ) + { + return hit; + } + + const b3RecDrawQuery* q = &player->frameQueries[queryIndex]; + if ( hitIndex < 0 || hitIndex >= q->hitCount ) + { + return hit; + } + + const b3RecRecordedHit* h = &player->frameHits[q->hitStart + hitIndex]; + hit.shape = h->id; + hit.point = h->point; + hit.normal = h->normal; + hit.fraction = h->fraction; + return hit; +} + +void b3RecPlayer_SetDebugShapeCallbacks( b3RecPlayer* player, b3CreateDebugShapeCallback* createDebugShape, + b3DestroyDebugShapeCallback* destroyDebugShape, void* context ) +{ + if ( player == NULL ) + { + return; + } + + player->createDebugShape = createDebugShape; + player->destroyDebugShape = destroyDebugShape; + player->debugShapeContext = context; + + // A world fixes its debug-shape callbacks at creation, so rebuild frame 0 under the new + // wiring. The old world held no adapter shapes (its callbacks were NULL), so the tear-down + // is balanced. Geometry slots are byte blobs reused as-is, the same path Restart relies on. + if ( b3World_IsValid( player->rdr.replayWorldId ) ) + { + b3DestroyWorld( player->rdr.replayWorldId ); + } + player->rdr.replayWorldId = b3RecPlayerCreateWorld( player ); + player->rdr.cursor = player->headerEnd; + player->rdr.ok = true; + player->rdr.diverged = false; + player->frame = 0; + player->divergeFrame = -1; + player->atEnd = false; + + // Re-seed the world so its shapes are recreated through the new callbacks. + b3World* world = b3GetWorldFromId( player->rdr.replayWorldId ); + if ( b3DeserializeIntoShell( player->frame0Image, player->frame0Size, world, &player->rdr ) == false ) + { + player->rdr.ok = false; + return; + } + player->rdr.cursor = player->headerEnd; + + // Rebuild the outliner from the frame-0 world that was just stood up under the new callbacks. + b3RecSeedFrame0BodyIds( player ); +} diff --git a/vendor/box3d/src/src/recording_replay.h b/vendor/box3d/src/src/recording_replay.h new file mode 100644 index 000000000..68760f6d0 --- /dev/null +++ b/vendor/box3d/src/src/recording_replay.h @@ -0,0 +1,258 @@ +// SPDX-FileCopyrightText: 2026 Erin Catto +// SPDX-License-Identifier: MIT + +#pragma once + +#include "recording.h" + +#include +#include + +typedef struct b3RecPlayer b3RecPlayer; + +// A single recorded callback hit, used both as reader scratch during a query replay and as the +// per-frame draw store. For collide-mover, one hit is one plane, with planeCount and userReturnB +// replicated across a shape's planes so the replay walker can re-group and compare per shape. +typedef struct b3RecRecordedHit +{ + b3ShapeId id; + b3Pos point; + b3Vec3 normal; + float fraction; + uint64_t userMaterialId; + int triangleIndex; + int childIndex; + b3PlaneResult plane; // collide-mover: this plane + int planeCount; // collide-mover: planes in this hit's shape group (replicated) + float userReturnF; // cast queries + bool userReturnB; // overlap / collide-mover (per shape, replicated) +} b3RecRecordedHit; + +// Recorded query kind, matching the public b3RecQueryType order. +typedef enum b3RecQueryKind +{ + B3_RECQ_OVERLAP_AABB, + B3_RECQ_OVERLAP_SHAPE, + B3_RECQ_CAST_RAY, + B3_RECQ_CAST_SHAPE, + B3_RECQ_CAST_RAY_CLOSEST, + B3_RECQ_CAST_MOVER, + B3_RECQ_COLLIDE_MOVER, +} b3RecQueryKind; + +// Per-frame draw record for one query call. Self-contained (no aliased pointers) so the player's +// frameQueries array can grow with a plain memcpy. Geometry is origin relative except the overlap +// AABB, which is world space in 3D. +typedef struct b3RecDrawQuery +{ + int kind; + uint64_t key; // identity key (hash of caller id+name), 0 = untagged + b3QueryFilter filter; + b3AABB aabb; // world-space bounds of the query, swept for casts + b3Vec3 proxyPoints[B3_MAX_SHAPE_CAST_POINTS]; // overlap/cast shape proxy, origin relative + int proxyCount; + float proxyRadius; + b3Capsule mover; // cast/collide mover, origin relative + b3Pos origin; + b3Vec3 translation; + float castFraction; // cast-mover result fraction + b3RayResult rayResult; // cast-ray-closest result + b3ShapeId shape; + int hitStart; // first hit in the player's frameHits store + int hitCount; +} b3RecDrawQuery; + +// One slot in the preloaded geometry registry. Loaded from the trailing block before any +// ops run; live pointer built lazily on first shape create that references the id. +typedef struct b3RegistrySlot +{ + b3GeometryKind kind; + int byteCount; + uint8_t* bytes; // raw bytes from the file (always freed at teardown) + void* live; // reconstructed live object, freed after b3DestroyWorld +} b3RegistrySlot; + +// This is used to simplify the scratch buffer lifetime. Names longer than this probably +// indicate a bug. +#define B3_MAX_NAME_LENGTH 256 + +// Reader state threaded through the replay loop and all dispatch functions +typedef struct b3RecReader +{ + const uint8_t* data; + int size; + int cursor; + b3WorldId replayWorldId; + bool ok; // false on read overrun or id mismatch, fatal stop + bool diverged; // a StateHash failed, non-fatal + + // Player that owns this reader, or NULL during a headless b3ValidateReplay. Body + // create/destroy and the bounds record fold back into it for the outliner and camera framing. + b3RecPlayer* owner; + + // Scratch for per-triangle materials in shape defs (grown on demand, freed at teardown) + b3SurfaceMaterial* matScratch; + int matScratchCap; + + // Scratch for string reads. Used by b3RecR_STR to pass names to b3BodyDef and b3ShapeDef. + char stringBuffers[4][B3_MAX_NAME_LENGTH + 1]; + int nextString; + + // Preloaded geometry registry + b3RegistrySlot* slots; + int slotCount; + + // Preloaded query-tag table (key -> id, name), loaded with the registry. Resolves the caller id and + // label for the viewer. tagMap maps a key to its tag index for O(1) lookup; opaque, owned by the player. + b3RecTag* tags; + int tagCount; // tags that loaded; a truncated tail loads fewer + int tagCapacity; // tags allocated, used to free the array + void* tagMap; + + // Key from the QueryTag op preceding the next query, consumed by the next stash. 0 = untagged. + uint64_t pendingQueryKey; + + // Scratch for recorded query hits; grown on demand, freed with the player. + b3RecRecordedHit* hits; + int hitCap; + + // Scratch for a shape-proxy point cloud read from the stream. b3ShapeProxy holds the points + // behind a pointer, so a decoded proxy borrows this until the next proxy read or teardown. + b3Vec3* proxyScratch; + int proxyScratchCap; +} b3RecReader; + +// Stored snapshot for fast backward seek. +typedef struct b3RecKeyframe +{ + uint8_t* image; // serialized world image at the end of this frame + int imageSize; + int imageCapacity; // allocation size (may exceed imageSize) + int frame; // frame index this restores to + int cursor; // op-stream cursor for the frame AFTER this one + int divergeFrame; // divergeFrame state at capture + bool diverged; // rdr.diverged state at capture + + // Outliner body list as it stood at this frame, restored verbatim so ordinals are stable. + b3BodyId* bodyIds; + int bodyIdCount; +} b3RecKeyframe; + +typedef struct b3RecPlayer +{ + uint8_t* data; // owned copy of recording bytes + int size; + int headerEnd; // first byte of op stream (past header + snapshot blob) + int registryEnd; // end of op stream = start of registry block (or size) + float lengthScale; + float previousLengthScale; + int frame; + int frameCount; + float recordedDt; + int recordedSubStepCount; + int recordedWorkerCount; // worker count requested for the replay world + b3AABB bounds; // accumulated world bounds, decoded from the trailing record + bool atEnd; + // Indicates all ops for the step have been consumed up to the world step op. The next sub-step + // will clear this and perform the world step. + bool atPreStep; + int divergeFrame; // first frame that diverged, -1 until then + + // Outliner body list, indexed by creation ordinal. Holes (null ids) mark destroyed bodies so + // later ordinals never shift. Snapshotted into each keyframe and the frame-0 copy, not rebuilt + // from the world, so a stored selection survives backward seeks. + b3BodyId* bodyIds; + int bodyIdCount; + int bodyIdCap; + b3BodyId* frame0BodyIds; + int frame0BodyIdCount; + + // Per-frame query store, reset at the top of each StepFrame and filled by the query dispatchers. + // Drawn by b3RecPlayer_DrawFrameQueries and inspected via the public GetFrameQuery API. + b3RecDrawQuery* frameQueries; + int frameQueryCount; + int frameQueryCap; + b3RecRecordedHit* frameHits; + int frameHitCount; + int frameHitCap; + + // Host debug-shape callbacks applied to every world the player creates. The 3D + // sample renderer builds GPU meshes here, so a replay world without them draws + // nothing. Set once via b3RecPlayer_SetDebugShapeCallbacks; persisted so a world + // rebuilt under new callbacks keeps drawing. + b3CreateDebugShapeCallback* createDebugShape; + b3DestroyDebugShapeCallback* destroyDebugShape; + void* debugShapeContext; + + b3RecReader rdr; + + // Frame-0 restore image, points into the owned data copy. Restart and backward seek + // deserialize this in place so the replay world id stays stable. + const uint8_t* frame0Image; + int frame0Size; + + // Keyframe ring + b3RecKeyframe* keyframes; + int keyframeCount; + int keyframeCapacity; + size_t keyframeBudget; + size_t keyframeBytes; + int keyframeMinInterval; + int keyframeInterval; + int lastKeyframeFrame; + + // Pre-populated recording used by b3SerializeWorld during keyframe capture. + // Its registry mirrors rdr.slots so geometry ids stay stable. + b3Recording* keyframeRec; +} b3RecPlayer; + +// Read primitives +uint8_t b3RecR_U8( b3RecReader* rdr ); +uint16_t b3RecR_U16( b3RecReader* rdr ); +uint32_t b3RecR_U24( b3RecReader* rdr ); +uint32_t b3RecR_U32( b3RecReader* rdr ); +uint64_t b3RecR_U64( b3RecReader* rdr ); +int32_t b3RecR_I32( b3RecReader* rdr ); +float b3RecR_F32( b3RecReader* rdr ); +double b3RecR_F64( b3RecReader* rdr ); +bool b3RecR_BOOL( b3RecReader* rdr ); +b3Vec3 b3RecR_VEC3( b3RecReader* rdr ); +b3Quat b3RecR_QUAT( b3RecReader* rdr ); +b3Transform b3RecR_TRANSFORM( b3RecReader* rdr ); +b3Pos b3RecR_POSITION( b3RecReader* rdr ); +b3WorldTransform b3RecR_WORLDXF( b3RecReader* rdr ); +b3Matrix3 b3RecR_MATRIX3( b3RecReader* rdr ); +b3AABB b3RecR_AABB( b3RecReader* rdr ); +b3WorldId b3RecR_WORLDID( b3RecReader* rdr ); +b3BodyId b3RecR_BODYID( b3RecReader* rdr ); +b3ShapeId b3RecR_SHAPEID( b3RecReader* rdr ); +b3JointId b3RecR_JOINTID( b3RecReader* rdr ); +b3Sphere b3RecR_SPHERE( b3RecReader* rdr ); +b3Capsule b3RecR_CAPSULE( b3RecReader* rdr ); +uint32_t b3RecR_GEOMID( b3RecReader* rdr ); +b3Filter b3RecR_FILTER( b3RecReader* rdr ); +b3SurfaceMaterial b3RecR_MATERIAL( b3RecReader* rdr ); +b3MassData b3RecR_MASSDATA( b3RecReader* rdr ); +b3MotionLocks b3RecR_LOCKS( b3RecReader* rdr ); +const char* b3RecR_STR( b3RecReader* rdr ); +b3ExplosionDef b3RecR_EXPLOSIONDEF( b3RecReader* rdr ); +b3BodyDef b3RecR_BODYDEF( b3RecReader* rdr ); +b3ShapeDef b3RecR_SHAPEDEF( b3RecReader* rdr ); +b3ParallelJointDef b3RecR_PARALLELJOINTDEF( b3RecReader* rdr ); +b3DistanceJointDef b3RecR_DISTANCEJOINTDEF( b3RecReader* rdr ); +b3FilterJointDef b3RecR_FILTERJOINTDEF( b3RecReader* rdr ); +b3MotorJointDef b3RecR_MOTORJOINTDEF( b3RecReader* rdr ); +b3PrismaticJointDef b3RecR_PRISMATICJOINTDEF( b3RecReader* rdr ); +b3RevoluteJointDef b3RecR_REVOLUTEJOINTDEF( b3RecReader* rdr ); +b3SphericalJointDef b3RecR_SPHERICALJOINTDEF( b3RecReader* rdr ); +b3WeldJointDef b3RecR_WELDJOINTDEF( b3RecReader* rdr ); +b3WheelJointDef b3RecR_WHEELJOINTDEF( b3RecReader* rdr ); +b3QueryFilter b3RecR_QUERYFILTER( b3RecReader* rdr ); +b3ShapeProxy b3RecR_SHAPEPROXY( b3RecReader* rdr ); +b3TreeStats b3RecR_TREESTATS( b3RecReader* rdr ); +b3RayResult b3RecR_RAYRESULT( b3RecReader* rdr ); +b3PlaneResult b3RecR_PLANERESULT( b3RecReader* rdr ); + +// Grow the reader's hit scratch to at least n entries, preserving contents. n is bounded by the +// file size since every recorded hit consumes at least one byte. +void b3RecEnsureHits( b3RecReader* rdr, int n ); diff --git a/vendor/box3d/src/src/revolute_joint.c b/vendor/box3d/src/src/revolute_joint.c new file mode 100644 index 000000000..7856a83d8 --- /dev/null +++ b/vendor/box3d/src/src/revolute_joint.c @@ -0,0 +1,673 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +#include "body.h" +#include "joint.h" +#include "math_internal.h" +#include "physics_world.h" +#include "solver.h" +#include "solver_set.h" +#include "recording.h" + +// needed for dll export +#include "box3d/box3d.h" + +// Point-to-point linear constraint +// C = pB - pA +// Cdot = vB - vA +// = vB + cross(wB, rB) - vA - cross(wA, rA) +// Cdot = J * v +// J = [-E -skew(rA) E skew(rB) ] +// +// K = J * invM * JT +// = [(1/mA + 1/mB) * E - skew(rA) * invIA * skew(rA) - skew(rB) * invIB * skew(rB)] + +// Perpendicularity constraint +// frameA = qA * localFrameA +// frameB = qB * localFrameB +// qRel = conj(frameA) * frameB +// C = [qRel.x; qRel.y] +// qRelDot = 0.5 * conj(frameA) * (wB - wA) * frameB +// Cdot = [qRelDot.x, qRelDot.y] +// Pulling out wB and wA +// sr = qRel.s +// vr = qRel.v +// Jx = 0.5 * rotate(frameA, sr * ex + cross(vr, ex)) +// Jy = 0.5 * rotate(frameA, sr * ey + cross(vr, ey)) + +// Motor constraint +// Cdot = wB - wA +// J = [0 0 -E 0 0 E] +// K = invIA + invIB + +void b3RevoluteJoint_EnableLimit( b3JointId jointId, bool enableLimit ) +{ + b3World* world = b3GetWorld( jointId.world0 ); + B3_REC( world, RevoluteJointEnableLimit, jointId, enableLimit ); + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_revoluteJoint ); + if ( enableLimit != base->revoluteJoint.enableLimit ) + { + base->revoluteJoint.lowerImpulse = 0.0f; + base->revoluteJoint.upperImpulse = 0.0f; + } + base->revoluteJoint.enableLimit = enableLimit; +} + +bool b3RevoluteJoint_IsLimitEnabled( b3JointId jointId ) +{ + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_revoluteJoint ); + return base->revoluteJoint.enableLimit; +} + +float b3RevoluteJoint_GetLowerLimit( b3JointId jointId ) +{ + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_revoluteJoint ); + return base->revoluteJoint.lowerAngle; +} + +float b3RevoluteJoint_GetUpperLimit( b3JointId jointId ) +{ + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_revoluteJoint ); + return base->revoluteJoint.upperAngle; +} + +void b3RevoluteJoint_SetLimits( b3JointId jointId, float lowerLimitRadians, float upperLimitRadians ) +{ + B3_ASSERT( b3IsValidFloat( lowerLimitRadians ) && b3IsValidFloat( upperLimitRadians ) ); + b3World* world = b3GetWorld( jointId.world0 ); + B3_REC( world, RevoluteJointSetLimits, jointId, lowerLimitRadians, upperLimitRadians ); + + float lowerAngle = b3MinFloat( lowerLimitRadians, upperLimitRadians ); + float upperAngle = b3MaxFloat( lowerLimitRadians, upperLimitRadians ); + + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_revoluteJoint ); + base->revoluteJoint.lowerAngle = b3ClampFloat( lowerAngle, -0.99f * B3_PI, 0.99f * B3_PI ); + base->revoluteJoint.upperAngle = b3ClampFloat( upperAngle, -0.99f * B3_PI, 0.99f * B3_PI ); +} + +float b3RevoluteJoint_GetAngle( b3JointId jointId ) +{ + b3World* world = b3GetWorld( jointId.world0 ); + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_revoluteJoint ); + b3WorldTransform transformA = b3GetBodyTransform( world, base->bodyIdA ); + b3WorldTransform transformB = b3GetBodyTransform( world, base->bodyIdB ); + + b3Quat quatA = b3MulQuat( transformA.q, base->localFrameA.q ); + b3Quat quatB = b3MulQuat( transformB.q, base->localFrameB.q ); + + if ( b3DotQuat( quatA, quatB ) < 0.0f ) + { + // this keeps the twist angle in the range [-pi, pi] + quatB = b3NegateQuat( quatB ); + } + + b3Quat relQ = b3InvMulQuat( quatA, quatB ); + + return b3GetTwistAngle( relQ ); +} + +void b3RevoluteJoint_EnableSpring( b3JointId jointId, bool enableSpring ) +{ + b3World* world = b3GetWorld( jointId.world0 ); + B3_REC( world, RevoluteJointEnableSpring, jointId, enableSpring ); + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_revoluteJoint ); + if ( enableSpring != base->revoluteJoint.enableSpring ) + { + base->revoluteJoint.springImpulse = 0.0f; + } + base->revoluteJoint.enableSpring = enableSpring; +} + +bool b3RevoluteJoint_IsSpringEnabled( b3JointId jointId ) +{ + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_revoluteJoint ); + return base->revoluteJoint.enableSpring; +} + +void b3RevoluteJoint_SetTargetAngle( b3JointId jointId, float targetRadians ) +{ + B3_ASSERT( b3IsValidFloat( targetRadians ) && -B3_PI <= targetRadians && targetRadians <= B3_PI ); + b3World* world = b3GetWorld( jointId.world0 ); + B3_REC( world, RevoluteJointSetTargetAngle, jointId, targetRadians ); + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_revoluteJoint ); + base->revoluteJoint.targetAngle = targetRadians; +} + +float b3RevoluteJoint_GetTargetAngle( b3JointId jointId ) +{ + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_revoluteJoint ); + return base->revoluteJoint.targetAngle; +} + +void b3RevoluteJoint_SetSpringHertz( b3JointId jointId, float hertz ) +{ + B3_ASSERT( b3IsValidFloat( hertz ) && hertz >= 0.0f ); + b3World* world = b3GetWorld( jointId.world0 ); + B3_REC( world, RevoluteJointSetSpringHertz, jointId, hertz ); + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_revoluteJoint ); + base->revoluteJoint.hertz = hertz; +} + +float b3RevoluteJoint_GetSpringHertz( b3JointId jointId ) +{ + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_revoluteJoint ); + return base->revoluteJoint.hertz; +} + +void b3RevoluteJoint_SetSpringDampingRatio( b3JointId jointId, float dampingRatio ) +{ + B3_ASSERT( b3IsValidFloat( dampingRatio ) && dampingRatio >= 0.0f ); + b3World* world = b3GetWorld( jointId.world0 ); + B3_REC( world, RevoluteJointSetSpringDampingRatio, jointId, dampingRatio ); + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_revoluteJoint ); + base->revoluteJoint.dampingRatio = dampingRatio; +} + +float b3RevoluteJoint_GetSpringDampingRatio( b3JointId jointId ) +{ + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_revoluteJoint ); + return base->revoluteJoint.dampingRatio; +} + +void b3RevoluteJoint_EnableMotor( b3JointId jointId, bool enableMotor ) +{ + b3World* world = b3GetWorld( jointId.world0 ); + B3_REC( world, RevoluteJointEnableMotor, jointId, enableMotor ); + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_revoluteJoint ); + if ( enableMotor != base->revoluteJoint.enableMotor ) + { + base->revoluteJoint.motorImpulse = 0.0f; + } + base->revoluteJoint.enableMotor = enableMotor; +} + +bool b3RevoluteJoint_IsMotorEnabled( b3JointId jointId ) +{ + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_revoluteJoint ); + return base->revoluteJoint.enableMotor; +} + +void b3RevoluteJoint_SetMotorSpeed( b3JointId jointId, float motorSpeed ) +{ + B3_ASSERT( b3IsValidFloat( motorSpeed ) ); + b3World* world = b3GetWorld( jointId.world0 ); + B3_REC( world, RevoluteJointSetMotorSpeed, jointId, motorSpeed ); + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_revoluteJoint ); + base->revoluteJoint.motorSpeed = motorSpeed; +} + +float b3RevoluteJoint_GetMotorSpeed( b3JointId jointId ) +{ + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_revoluteJoint ); + return base->revoluteJoint.motorSpeed; +} + +void b3RevoluteJoint_SetMaxMotorTorque( b3JointId jointId, float maxForce ) +{ + B3_ASSERT( b3IsValidFloat( maxForce ) && maxForce >= 0.0f ); + b3World* world = b3GetWorld( jointId.world0 ); + B3_REC( world, RevoluteJointSetMaxMotorTorque, jointId, maxForce ); + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_revoluteJoint ); + base->revoluteJoint.maxMotorTorque = maxForce; +} + +float b3RevoluteJoint_GetMaxMotorTorque( b3JointId jointId ) +{ + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_revoluteJoint ); + return base->revoluteJoint.maxMotorTorque; +} + +float b3RevoluteJoint_GetMotorTorque( b3JointId jointId ) +{ + b3World* world = b3GetWorld( jointId.world0 ); + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_revoluteJoint ); + return world->inv_h * base->revoluteJoint.motorImpulse; +} + +b3Vec3 b3GetRevoluteJointForce( b3World* world, b3JointSim* base ) +{ + b3Vec3 force = b3MulSV( world->inv_h, base->revoluteJoint.linearImpulse ); + return force; +} + +b3Vec3 b3GetRevoluteJointTorque( b3World* world, b3JointSim* base ) +{ + b3WorldTransform transformA = b3GetBodyTransform( world, base->bodyIdA ); + b3RevoluteJoint* joint = &base->revoluteJoint; + b3Vec3 axis = b3RotateVector( base->localFrameA.q, b3Vec3_axisZ ); + axis = b3RotateVector( transformA.q, axis ); + + b3Quat relQ = b3InvMulQuat( joint->frameA.q, joint->frameB.q ); + + // These are needed for warm starting + joint->perpAxisX = b3MulSV( + 0.5f, b3RotateVector( joint->frameA.q, b3Add( b3MulSV( relQ.s, b3Vec3_axisX ), b3Cross( relQ.v, b3Vec3_axisX ) ) ) ); + joint->perpAxisY = b3MulSV( + 0.5f, b3RotateVector( joint->frameA.q, b3Add( b3MulSV( relQ.s, b3Vec3_axisY ), b3Cross( relQ.v, b3Vec3_axisY ) ) ) ); + + float axialImpulse = joint->springImpulse + joint->motorImpulse + joint->lowerImpulse - joint->upperImpulse; + b3Vec3 angularImpulse = + b3Add( b3MulSV( joint->perpImpulse.x, joint->perpAxisX ), b3MulSV( joint->perpImpulse.y, joint->perpAxisY ) ); + angularImpulse = b3MulAdd( angularImpulse, axialImpulse, joint->rotationAxisZ ); + + // todo add pivot torque + b3Vec3 impulse = b3MulAdd( angularImpulse, + joint->springImpulse + joint->motorImpulse + joint->lowerImpulse - joint->upperImpulse, axis ); + b3Vec3 torque = b3MulSV( world->inv_h, impulse ); + return torque; +} + +void b3PrepareRevoluteJoint( b3JointSim* base, b3StepContext* context ) +{ + B3_ASSERT( base->type == b3_revoluteJoint ); + + b3World* world = context->world; + + b3Body* bodyA = b3Array_Get( world->bodies, base->bodyIdA ); + b3Body* bodyB = b3Array_Get( world->bodies, base->bodyIdB ); + + B3_ASSERT( bodyA->setIndex == b3_awakeSet || bodyB->setIndex == b3_awakeSet ); + b3SolverSet* setA = b3Array_Get( world->solverSets, bodyA->setIndex ); + b3SolverSet* setB = b3Array_Get( world->solverSets, bodyB->setIndex ); + + int localIndexA = bodyA->localIndex; + int localIndexB = bodyB->localIndex; + + b3BodySim* bodySimA = b3Array_Get( setA->bodySims, localIndexA ); + b3BodySim* bodySimB = b3Array_Get( setB->bodySims, localIndexB ); + + base->invMassA = bodySimA->invMass; + base->invMassB = bodySimB->invMass; + base->invIA = bodySimA->invInertiaWorld; + base->invIB = bodySimB->invInertiaWorld; + + b3Matrix3 invInertiaSum = b3AddMM( base->invIA, base->invIB ); + base->fixedRotation = b3Det( invInertiaSum ) < 1000.0f * FLT_MIN; + + b3RevoluteJoint* joint = &base->revoluteJoint; + joint->indexA = bodyA->setIndex == b3_awakeSet ? localIndexA : B3_NULL_INDEX; + joint->indexB = bodyB->setIndex == b3_awakeSet ? localIndexB : B3_NULL_INDEX; + + // Compute joint anchor frames with world space rotation, relative to center of mass + // Avoid round-off here as much as possible. + // b3Vec3 pf = (xf.p - c) + rot(xf.q, f.p) + // pf = xf.p - (xf.p + rot(xf.q, lc)) + rot(xf.q, f.p) + // pf = rot(xf.q, f.p - lc) + joint->frameA.q = b3MulQuat( bodySimA->transform.q, base->localFrameA.q ); + joint->frameA.p = b3RotateVector( bodySimA->transform.q, b3Sub( base->localFrameA.p, bodySimA->localCenter ) ); + joint->frameB.q = b3MulQuat( bodySimB->transform.q, base->localFrameB.q ); + joint->frameB.p = b3RotateVector( bodySimB->transform.q, b3Sub( base->localFrameB.p, bodySimB->localCenter ) ); + + joint->deltaCenter = b3SubPos( bodySimB->center, bodySimA->center ); + + { + // Rotation axis is the z-axis of body A. + b3Vec3 rotationAxisZ = b3RotateVector( joint->frameA.q, b3Vec3_axisZ ); + float k = b3Dot( rotationAxisZ, b3MulMV( invInertiaSum, rotationAxisZ ) ); + joint->axialMass = k > 0.0f ? 1.0f / k : 0.0f; + joint->rotationAxisZ = rotationAxisZ; + } + + b3Quat relQ = b3InvMulQuat( joint->frameA.q, joint->frameB.q ); + + { + // These are needed for warm starting + joint->perpAxisX = b3MulSV( + 0.5f, b3RotateVector( joint->frameA.q, b3Add( b3MulSV( relQ.s, b3Vec3_axisX ), b3Cross( relQ.v, b3Vec3_axisX ) ) ) ); + joint->perpAxisY = b3MulSV( + 0.5f, b3RotateVector( joint->frameA.q, b3Add( b3MulSV( relQ.s, b3Vec3_axisY ), b3Cross( relQ.v, b3Vec3_axisY ) ) ) ); + } + + joint->springSoftness = b3MakeSoft( joint->hertz, joint->dampingRatio, context->h ); + + if ( context->enableWarmStarting == false ) + { + joint->linearImpulse = b3Vec3_zero; + joint->perpImpulse = (b3Vec2){ 0.0f, 0.0f }; + joint->motorImpulse = 0.0f; + joint->springImpulse = 0.0f; + joint->lowerImpulse = 0.0f; + joint->upperImpulse = 0.0f; + } +} + +void b3WarmStartRevoluteJoint( b3JointSim* base, b3StepContext* context ) +{ + B3_ASSERT( base->type == b3_revoluteJoint ); + + float mA = base->invMassA; + float mB = base->invMassB; + b3Matrix3 iA = base->invIA; + b3Matrix3 iB = base->invIB; + + // dummy state for static bodies + b3BodyState dummyState = b3_identityBodyState; + + b3RevoluteJoint* joint = &base->revoluteJoint; + + b3BodyState* stateA = joint->indexA == B3_NULL_INDEX ? &dummyState : context->states + joint->indexA; + b3BodyState* stateB = joint->indexB == B3_NULL_INDEX ? &dummyState : context->states + joint->indexB; + + b3Vec3 vA = stateA->linearVelocity; + b3Vec3 wA = stateA->angularVelocity; + b3Vec3 vB = stateB->linearVelocity; + b3Vec3 wB = stateB->angularVelocity; + + b3Vec3 rA = b3RotateVector( stateA->deltaRotation, joint->frameA.p ); + b3Vec3 rB = b3RotateVector( stateB->deltaRotation, joint->frameB.p ); + + float axialImpulse = joint->springImpulse + joint->motorImpulse + joint->lowerImpulse - joint->upperImpulse; + b3Vec3 angularImpulse = + b3Add( b3MulSV( joint->perpImpulse.x, joint->perpAxisX ), b3MulSV( joint->perpImpulse.y, joint->perpAxisY ) ); + angularImpulse = b3MulAdd( angularImpulse, axialImpulse, joint->rotationAxisZ ); + + vA = b3MulSub( vA, mA, joint->linearImpulse ); + wA = b3Sub( wA, b3MulMV( iA, b3Add( b3Cross( rA, joint->linearImpulse ), angularImpulse ) ) ); + + vB = b3MulAdd( vB, mB, joint->linearImpulse ); + wB = b3Add( wB, b3MulMV( iB, b3Add( b3Cross( rB, joint->linearImpulse ), angularImpulse ) ) ); + + if ( stateA->flags & b3_dynamicFlag ) + { + stateA->linearVelocity = vA; + stateA->angularVelocity = wA; + } + + if ( stateB->flags & b3_dynamicFlag ) + { + stateB->linearVelocity = vB; + stateB->angularVelocity = wB; + } +} + +void b3SolveRevoluteJoint( b3JointSim* base, b3StepContext* context, bool useBias ) +{ + float mA = base->invMassA; + float mB = base->invMassB; + b3Matrix3 iA = base->invIA; + b3Matrix3 iB = base->invIB; + + // dummy state for static bodies + b3BodyState dummyState = b3_identityBodyState; + + b3RevoluteJoint* joint = &base->revoluteJoint; + b3BodyState* stateA = joint->indexA == B3_NULL_INDEX ? &dummyState : context->states + joint->indexA; + b3BodyState* stateB = joint->indexB == B3_NULL_INDEX ? &dummyState : context->states + joint->indexB; + + b3Vec3 vA = stateA->linearVelocity; + b3Vec3 wA = stateA->angularVelocity; + b3Vec3 vB = stateB->linearVelocity; + b3Vec3 wB = stateB->angularVelocity; + + bool fixedRotation = base->fixedRotation; + b3Quat quatA = b3MulQuat( stateA->deltaRotation, joint->frameA.q ); + b3Quat quatB = b3MulQuat( stateB->deltaRotation, joint->frameB.q ); + + if ( b3DotQuat( quatA, quatB ) < 0.0f ) + { + // this keeps the rotation angle in the range [-pi, pi] + quatB = b3NegateQuat( quatB ); + } + + b3Quat relQ = b3InvMulQuat( quatA, quatB ); + + // Solve spring + if ( joint->enableSpring && fixedRotation == false ) + { + // Get the substep relative rotation + float targetAngle = joint->targetAngle; + float angle = b3GetTwistAngle( relQ ); + float c = angle - targetAngle; + + float bias = joint->springSoftness.biasRate * c; + float massScale = joint->springSoftness.massScale; + float impulseScale = joint->springSoftness.impulseScale; + float cdot = b3Dot( b3Sub( wB, wA ), joint->rotationAxisZ ); + + float deltaImpulse = -massScale * joint->axialMass * ( cdot + bias ) - impulseScale * joint->springImpulse; + joint->springImpulse += deltaImpulse; + + wA = b3MulSub( wA, deltaImpulse, b3MulMV( iA, joint->rotationAxisZ ) ); + wB = b3MulAdd( wB, deltaImpulse, b3MulMV( iB, joint->rotationAxisZ ) ); + } + + if ( joint->enableMotor && fixedRotation == false ) + { + float cdot = b3Dot( b3Sub( wB, wA ), joint->rotationAxisZ ) - joint->motorSpeed; + + float deltaImpulse = -joint->axialMass * cdot; + float newImpulse = joint->motorImpulse + deltaImpulse; + float maxImpulse = joint->maxMotorTorque * context->h; + newImpulse = b3ClampFloat( newImpulse, -maxImpulse, maxImpulse ); + deltaImpulse = newImpulse - joint->motorImpulse; + joint->motorImpulse = newImpulse; + + wA = b3MulSub( wA, deltaImpulse, b3MulMV( iA, joint->rotationAxisZ ) ); + wB = b3MulAdd( wB, deltaImpulse, b3MulMV( iB, joint->rotationAxisZ ) ); + } + + if ( joint->enableLimit && fixedRotation == false ) + { + float angle = b3GetTwistAngle( relQ ); + + // todo does an updated twist axis help? + + b3Vec3 axis = joint->rotationAxisZ; + + // Lower limit + { + float c = angle - joint->lowerAngle; + float bias = 0.0f; + float massScale = 1.0f; + float impulseScale = 0.0f; + if ( c > 0.0f ) + { + // speculation + bias = c * context->inv_h; + } + else if ( useBias ) + { + bias = base->constraintSoftness.biasRate * c; + massScale = base->constraintSoftness.massScale; + impulseScale = base->constraintSoftness.impulseScale; + } + + float cdot = b3Dot( b3Sub( wB, wA ), axis ); + float oldImpulse = joint->lowerImpulse; + float deltaImpulse = -massScale * joint->axialMass * ( cdot + bias ) - impulseScale * oldImpulse; + joint->lowerImpulse = b3MaxFloat( oldImpulse + deltaImpulse, 0.0f ); + deltaImpulse = joint->lowerImpulse - oldImpulse; + + wA = b3MulSub( wA, deltaImpulse, b3MulMV( iA, axis ) ); + wB = b3MulAdd( wB, deltaImpulse, b3MulMV( iB, axis ) ); + } + + // Upper limit + { + float c = joint->upperAngle - angle; + float bias = 0.0f; + float massScale = 1.0f; + float impulseScale = 0.0f; + if ( c > 0.0f ) + { + // speculation + bias = c * context->inv_h; + } + else if ( useBias ) + { + bias = base->constraintSoftness.biasRate * c; + massScale = base->constraintSoftness.massScale; + impulseScale = base->constraintSoftness.impulseScale; + } + + // sign flipped on Cdot + float cdot = b3Dot( b3Sub( wA, wB ), axis ); + float oldImpulse = joint->upperImpulse; + float deltaImpulse = -massScale * joint->axialMass * ( cdot + bias ) - impulseScale * oldImpulse; + joint->upperImpulse = b3MaxFloat( oldImpulse + deltaImpulse, 0.0f ); + deltaImpulse = joint->upperImpulse - oldImpulse; + + // sign flipped on applied impulse + wA = b3MulAdd( wA, deltaImpulse, b3MulMV( iA, axis ) ); + wB = b3MulSub( wB, deltaImpulse, b3MulMV( iB, axis ) ); + } + } + + // Collinearity constraint + if ( fixedRotation == false ) + { + b3Vec2 bias = { 0.0f, 0.0f }; + float massScale = 1.0f; + float impulseScale = 0.0f; + + if ( useBias ) + { + b3Vec2 c = { relQ.v.x, relQ.v.y }; + bias = b3MulSV2( base->constraintSoftness.biasRate, c ); + massScale = base->constraintSoftness.massScale; + impulseScale = base->constraintSoftness.impulseScale; + } + + // Collinearity constraint as 2-by-2 + b3Vec3 perpAxisX = + b3MulSV( 0.5f, b3RotateVector( quatA, b3Add( b3MulSV( relQ.s, b3Vec3_axisX ), b3Cross( relQ.v, b3Vec3_axisX ) ) ) ); + b3Vec3 perpAxisY = + b3MulSV( 0.5f, b3RotateVector( quatA, b3Add( b3MulSV( relQ.s, b3Vec3_axisY ), b3Cross( relQ.v, b3Vec3_axisY ) ) ) ); + joint->perpAxisX = perpAxisX; + joint->perpAxisY = perpAxisY; + + b3Matrix3 invInertiaSum = b3AddMM( iA, iB ); + float kxx = b3Dot( perpAxisX, b3MulMV( invInertiaSum, perpAxisX ) ); + float kyy = b3Dot( perpAxisY, b3MulMV( invInertiaSum, perpAxisY ) ); + float kxy = b3Dot( perpAxisX, b3MulMV( invInertiaSum, perpAxisY ) ); + + b3Matrix2 k = { { kxx, kxy }, { kxy, kyy } }; + + b3Vec3 wRel = b3Sub( wB, wA ); + b3Vec2 cdot = { b3Dot( wRel, perpAxisX ), b3Dot( wRel, perpAxisY ) }; + b3Vec2 oldImpulse = joint->perpImpulse; + b3Vec2 sol = b3Solve2( k, b3Add2( cdot, bias ) ); + b3Vec2 deltaImpulse = b3Sub2( b3MulSV2( -massScale, sol ), b3MulSV2( impulseScale, oldImpulse ) ); + joint->perpImpulse = b3Add2( joint->perpImpulse, deltaImpulse ); + + b3Vec3 angularImpulse = b3Add( b3MulSV( deltaImpulse.x, perpAxisX ), b3MulSV( deltaImpulse.y, perpAxisY ) ); + wA = b3Sub( wA, b3MulMV( iA, angularImpulse ) ); + wB = b3Add( wB, b3MulMV( iB, angularImpulse ) ); + } + + // Solve point-to-point constraint + { + b3Vec3 rA = b3RotateVector( stateA->deltaRotation, joint->frameA.p ); + b3Vec3 rB = b3RotateVector( stateB->deltaRotation, joint->frameB.p ); + + b3Vec3 cdot = b3Sub( b3Sub( b3Add( vB, b3Cross( wB, rB ) ), vA ), b3Cross( wA, rA ) ); + + b3Vec3 bias = b3Vec3_zero; + float massScale = 1.0f; + float impulseScale = 0.0f; + if ( useBias ) + { + b3Vec3 dcA = stateA->deltaPosition; + b3Vec3 dcB = stateB->deltaPosition; + + b3Vec3 separation = b3Add( b3Add( b3Sub( dcB, dcA ), b3Sub( rB, rA ) ), joint->deltaCenter ); + + bias = b3MulSV( base->constraintSoftness.biasRate, separation ); + massScale = base->constraintSoftness.massScale; + impulseScale = base->constraintSoftness.impulseScale; + } + + //// K = [(1/m1 + 1/m2) * eye(2) - skew(r1) * invI1 * skew(r1) - skew(r2) * invI2 * skew(r2)] + b3Matrix3 sA = b3Skew( rA ); + b3Matrix3 sB = b3Skew( rB ); + b3Matrix3 kA = b3MulMM( sA, b3MulMM( base->invIA, sA ) ); + b3Matrix3 kB = b3MulMM( sB, b3MulMM( base->invIB, sB ) ); + b3Matrix3 k = b3NegateMat3( b3AddMM( kA, kB ) ); + k.cx.x += mA + mB; + k.cy.y += mA + mB; + k.cz.z += mA + mB; + + b3Vec3 b = b3Solve3( k, b3Add( cdot, bias ) ); + + b3Vec3 impulse = b3Sub( b3MulSV( -massScale, b ), b3MulSV( impulseScale, joint->linearImpulse ) ); + joint->linearImpulse = b3Add( joint->linearImpulse, impulse ); + + vA = b3MulSub( vA, mA, impulse ); + wA = b3Sub( wA, b3MulMV( iA, b3Cross( rA, impulse ) ) ); + vB = b3MulAdd( vB, mB, impulse ); + wB = b3Add( wB, b3MulMV( iB, b3Cross( rB, impulse ) ) ); + } + + if ( stateA->flags & b3_dynamicFlag ) + { + stateA->linearVelocity = vA; + stateA->angularVelocity = wA; + } + + if ( stateB->flags & b3_dynamicFlag ) + { + stateB->linearVelocity = vB; + stateB->angularVelocity = wB; + } +} + +void b3DrawRevoluteJoint( b3DebugDraw* draw, b3JointSim* base, b3WorldTransform transformA, b3WorldTransform transformB, float scale ) +{ + b3WorldTransform frameA = b3MulWorldTransforms( transformA, base->localFrameA ); + + float length1 = 0.1f * scale; + draw->DrawSegmentFcn( frameA.p, b3OffsetPos( frameA.p, b3MulSV( length1, b3RotateVector( frameA.q, b3Vec3_axisX ) ) ), b3_colorRed, + draw->context ); + draw->DrawSegmentFcn( frameA.p, b3OffsetPos( frameA.p, b3MulSV( length1, b3RotateVector( frameA.q, b3Vec3_axisY ) ) ), b3_colorGreen, + draw->context ); + draw->DrawSegmentFcn( frameA.p, b3OffsetPos( frameA.p, b3MulSV( length1, b3RotateVector( frameA.q, b3Vec3_axisZ ) ) ), b3_colorBlue, + draw->context ); + + b3WorldTransform frameB = b3MulWorldTransforms( transformB, base->localFrameB ); + + b3RevoluteJoint* joint = &base->revoluteJoint; + enum { kSliceCount = 16 }; + + // Twist limit + if ( joint->enableLimit ) + { + b3Quat quatA = frameA.q; + b3Quat quatB = frameB.q; + + if ( b3DotQuat( quatA, quatB ) < 0.0f ) + { + // this keeps the twist angle in the range [-pi, pi] + quatB = b3NegateQuat( quatB ); + } + + b3Quat relQ = b3InvMulQuat( quatA, quatB ); + + const float wedgeRadius = 0.2f * scale; + for ( int index = 0; index < kSliceCount; ++index ) + { + float t1 = (float)( index + 0 ) / kSliceCount; + float alpha1 = ( 1.0f - t1 ) * joint->lowerAngle + t1 * joint->upperAngle; + float t2 = (float)( index + 1 ) / kSliceCount; + float alpha2 = ( 1.0f - t2 ) * joint->lowerAngle + t2 * joint->upperAngle; + + b3Vec3 vertex1 = { wedgeRadius * b3Cos( alpha1 ), wedgeRadius * b3Sin( alpha1 ), 0.0f }; + b3Vec3 vertex2 = { wedgeRadius * b3Cos( alpha2 ), wedgeRadius * b3Sin( alpha2 ), 0.0f }; + + if ( index == 0 ) + { + draw->DrawSegmentFcn( frameA.p, b3TransformWorldPoint( frameA, vertex1 ), b3_colorCyan, draw->context ); + } + + if ( index == kSliceCount - 1 ) + { + draw->DrawSegmentFcn( b3TransformWorldPoint( frameA, vertex2 ), frameA.p, b3_colorCyan, draw->context ); + } + draw->DrawSegmentFcn( b3TransformWorldPoint( frameA, vertex1 ), b3TransformWorldPoint( frameA, vertex2 ), b3_colorCyan, + draw->context ); + } + + float twistAngle = b3GetTwistAngle( relQ ); + b3Vec3 p2 = { wedgeRadius * b3Cos( twistAngle ), wedgeRadius * b3Sin( twistAngle ), 0.0f }; + draw->DrawSegmentFcn( frameA.p, b3TransformWorldPoint( frameA, p2 ), b3_colorYellow, draw->context ); + } +} diff --git a/vendor/box3d/src/src/scheduler.c b/vendor/box3d/src/src/scheduler.c new file mode 100644 index 000000000..7bcde42f1 --- /dev/null +++ b/vendor/box3d/src/src/scheduler.c @@ -0,0 +1,198 @@ +// SPDX-FileCopyrightText: 2026 Erin Catto +// SPDX-License-Identifier: MIT + +#include "platform.h" +#include "core.h" +#include "scheduler.h" + +#include "box3d/base.h" +#include "box3d/constants.h" + +#include +#include +#include + +enum b3SchedulerTaskStatus +{ + b3_schedulerFree = 0, + b3_schedulerPending = 1, + b3_schedulerClaimed = 2, + b3_schedulerComplete = 3, +}; + +typedef struct b3SchedulerTask +{ + b3TaskCallback* callback; + void* taskContext; + b3AtomicInt status; +} b3SchedulerTask; + +typedef struct b3SchedulerWorkerContext +{ + struct b3Scheduler* scheduler; + int threadIndex; +} b3SchedulerWorkerContext; + +typedef struct b3Scheduler +{ + b3Thread* threads[B3_MAX_WORKERS]; + b3SchedulerWorkerContext workerContexts[B3_MAX_WORKERS]; + + // total workers including main thread + int workerCount; + + // threads created = workerCount - 1 + int threadCount; + + b3SchedulerTask tasks[B3_MAX_TASKS]; + b3AtomicInt nextSlot; + + b3Semaphore* taskSemaphore; + b3AtomicInt shutdown; +} b3Scheduler; + +// Try to claim and execute one pending task. +// Returns true if work was performed, false otherwise. +static bool b3SchedulerExecuteOne( b3Scheduler* scheduler ) +{ + int taskCount = b3AtomicLoadInt( &scheduler->nextSlot ); + for ( int t = 0; t < taskCount; ++t ) + { + b3SchedulerTask* task = scheduler->tasks + t; + if ( b3AtomicLoadInt( &task->status ) != b3_schedulerPending ) + { + continue; + } + + if ( b3AtomicCompareExchangeInt( &task->status, b3_schedulerPending, b3_schedulerClaimed ) == false ) + { + continue; + } + + task->callback( task->taskContext ); + + b3AtomicStoreInt( &task->status, b3_schedulerComplete ); + return true; + } + + return false; +} + +// Background worker thread entry point. +static void b3SchedulerWorkerMain( void* context ) +{ + b3SchedulerWorkerContext* workerContext = context; + b3Scheduler* scheduler = workerContext->scheduler; + + while ( true ) + { + b3WaitSemaphore( scheduler->taskSemaphore ); + + if ( b3AtomicLoadInt( &scheduler->shutdown ) != 0 ) + { + break; + } + + // Claim and execute all available work + while ( b3SchedulerExecuteOne( scheduler ) ) + { + } + } +} + +b3Scheduler* b3CreateScheduler( int workerCount ) +{ + B3_ASSERT( 0 < workerCount && workerCount <= B3_MAX_WORKERS ); + + b3Scheduler* scheduler = b3Alloc( sizeof( b3Scheduler ) ); + memset( scheduler, 0, sizeof( b3Scheduler ) ); + + scheduler->workerCount = workerCount; + int threadCount = workerCount - 1; + scheduler->threadCount = threadCount; + scheduler->taskSemaphore = b3CreateSemaphore( 0 ); + b3AtomicStoreInt( &scheduler->shutdown, 0 ); + b3AtomicStoreInt( &scheduler->nextSlot, 0 ); + + // Background threads use indices 1..workerCount-1. + // Main thread uses index 0. + for ( int i = 0; i < threadCount; ++i ) + { + scheduler->workerContexts[i].scheduler = scheduler; + scheduler->workerContexts[i].threadIndex = i + 1; + + char name[16]; + snprintf( name, sizeof( name ), "box2d_worker_%02d", i + 1 ); + scheduler->threads[i] = b3CreateThread( b3SchedulerWorkerMain, scheduler->workerContexts + i, name ); + } + + return scheduler; +} + +void b3DestroyScheduler( b3Scheduler* scheduler ) +{ + b3AtomicStoreInt( &scheduler->shutdown, 1 ); + + // Wake all background threads so they see the shutdown flag + for ( int i = 0; i < scheduler->threadCount; ++i ) + { + b3SignalSemaphore( scheduler->taskSemaphore ); + } + + for ( int i = 0; i < scheduler->threadCount; ++i ) + { + b3JoinThread( scheduler->threads[i] ); + scheduler->threads[i] = NULL; + } + + b3DestroySemaphore( scheduler->taskSemaphore ); + b3Free( scheduler, sizeof( b3Scheduler ) ); +} + +void b3ResetScheduler( b3Scheduler* scheduler ) +{ + b3AtomicStoreInt( &scheduler->nextSlot, 0 ); +} + +void* b3SchedulerEnqueueTask( b3TaskCallback* task, void* taskContext, void* userContext, const char* name ) +{ + B3_UNUSED( name ); + b3Scheduler* scheduler = userContext; + + int slot = b3AtomicFetchAddInt( &scheduler->nextSlot, 1 ); + B3_ASSERT( slot < B3_MAX_TASKS ); + + b3SchedulerTask* schedulerTask = scheduler->tasks + slot; + schedulerTask->callback = task; + schedulerTask->taskContext = taskContext; + + // Memory fence: status must be published after callback and context are written + b3AtomicStoreInt( &schedulerTask->status, b3_schedulerPending ); + + // One wake per enqueue is enough: at most one worker picks up each task. + b3SignalSemaphore( scheduler->taskSemaphore ); + + return schedulerTask; +} + +void b3SchedulerFinishTask( void* userTask, void* userContext ) +{ + if ( userTask == NULL ) + { + return; + } + + b3Scheduler* scheduler = userContext; + b3SchedulerTask* waitTask = userTask; + + // Main thread helps execute any available work while waiting for the + // target task to complete. This keeps the main thread from idling when + // background threads are busy on other tasks from the same phase. + while ( b3AtomicLoadInt( &waitTask->status ) != b3_schedulerComplete ) + { + if ( b3SchedulerExecuteOne( scheduler ) == false ) + { + b3Yield(); + } + } +} diff --git a/vendor/box3d/src/src/scheduler.h b/vendor/box3d/src/src/scheduler.h new file mode 100644 index 000000000..f9aeff7b2 --- /dev/null +++ b/vendor/box3d/src/src/scheduler.h @@ -0,0 +1,15 @@ +// SPDX-FileCopyrightText: 2026 Erin Catto +// SPDX-License-Identifier: MIT + +#pragma once + +typedef void b3TaskCallback( void* taskContext ); +typedef struct b3Scheduler b3Scheduler; + +b3Scheduler* b3CreateScheduler( int workerCount ); +void b3DestroyScheduler( b3Scheduler* scheduler ); +void b3ResetScheduler( b3Scheduler* scheduler ); + +// See b3EnqueueTaskCallback and b3FinishTaskCallback +void* b3SchedulerEnqueueTask( b3TaskCallback* task, void* taskContext, void* userContext, const char* name ); +void b3SchedulerFinishTask( void* userTask, void* userContext ); diff --git a/vendor/box3d/src/src/sensor.c b/vendor/box3d/src/src/sensor.c new file mode 100644 index 000000000..0cd23a460 --- /dev/null +++ b/vendor/box3d/src/src/sensor.c @@ -0,0 +1,452 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +#include "sensor.h" + +#include "body.h" +#include "contact.h" +#include "ctz.h" +#include "physics_world.h" +#include "shape.h" + +#include "box3d/collision.h" + +// for qsort +#include "parallel_for.h" + +#include + +typedef struct b3SensorQueryContext +{ + b3World* world; + b3SensorTaskContext* taskContext; + b3Sensor* sensor; + b3Shape* sensorShape; + b3Transform transform; +} b3SensorQueryContext; + +static bool b3OverlapSensor( b3Shape* sensorShape, b3Transform sensorTransform, b3Shape* visitorShape, + b3Transform visitorTransform ) +{ + b3ShapeType type = sensorShape->type; + + b3ShapeProxy proxy = b3MakeShapeProxy( visitorShape ); + + // Get the visitor shape in the frame of the sensor + b3Transform relativeTransform = b3InvMulTransforms( sensorTransform, visitorTransform ); + + b3Vec3 localPoints[B3_MAX_SHAPE_CAST_POINTS]; + b3ShapeProxy localProxy; + + localProxy.count = b3MinInt( proxy.count, B3_MAX_SHAPE_CAST_POINTS ); + for ( int i = 0; i < localProxy.count; ++i ) + { + localPoints[i] = b3TransformPoint( relativeTransform, proxy.points[i] ); + } + + localProxy.points = localPoints; + localProxy.radius = proxy.radius; + + switch ( type ) + { + case b3_capsuleShape: + return b3OverlapCapsule( &sensorShape->capsule, b3Transform_identity, &localProxy ); + + case b3_compoundShape: + return b3OverlapCompound( sensorShape->compound, b3Transform_identity, &localProxy ); + + case b3_heightShape: + return b3OverlapHeightField( sensorShape->heightField, b3Transform_identity, &localProxy ); + + case b3_hullShape: + return b3OverlapHull( sensorShape->hull, b3Transform_identity, &localProxy ); + + case b3_meshShape: + return b3OverlapMesh( &sensorShape->mesh, b3Transform_identity, &localProxy ); + + case b3_sphereShape: + return b3OverlapSphere( &sensorShape->sphere, b3Transform_identity, &localProxy ); + + default: + B3_ASSERT( false ); + return false; + } +} + +// Sensor shapes need to +// - detect begin and end overlap events +// - events must be reported in deterministic order +// - maintain an active list of overlaps for query + +// Assumption +// - sensors don't detect shapes on the same body + +// Algorithm +// Query all sensors for overlaps +// Check against previous overlaps + +// Data structures +// Each sensor has an double buffered array of overlaps +// These overlaps use a shape reference with index and generation + +static bool b3SensorQueryCallback( int proxyId, uint64_t userData, void* context ) +{ + B3_UNUSED( proxyId ); + + int shapeId = (int)userData; + + b3SensorQueryContext* queryContext = (b3SensorQueryContext*)context; + b3Shape* sensorShape = queryContext->sensorShape; + int sensorShapeId = sensorShape->id; + + if ( shapeId == sensorShapeId ) + { + return true; + } + + b3World* world = queryContext->world; + b3Shape* otherShape = b3Array_Get( world->shapes, shapeId ); + + // Mesh vs mesh is not supported + if ( ( otherShape->type == b3_meshShape || otherShape->type == b3_heightShape ) && + ( sensorShape->type == b3_meshShape || sensorShape->type == b3_heightShape ) ) + { + return true; + } + + // Are sensor events enabled on the other shape? + if ( ( otherShape->flags & b3_enableSensorEvents ) == 0 ) + { + return true; + } + + // Skip shapes on the same body + if ( otherShape->bodyId == sensorShape->bodyId ) + { + return true; + } + + // Check filter + if ( b3ShouldShapesCollide( sensorShape->filter, otherShape->filter ) == false ) + { + return true; + } + + // Custom user filter + if ( ( sensorShape->flags & b3_enableCustomFiltering ) || ( otherShape->flags & b3_enableCustomFiltering ) ) + { + b3CustomFilterFcn* customFilterFcn = queryContext->world->customFilterFcn; + if ( customFilterFcn != NULL ) + { + b3ShapeId idA = { sensorShapeId + 1, world->worldId, sensorShape->generation }; + b3ShapeId idB = { shapeId + 1, world->worldId, otherShape->generation }; + bool shouldCollide = customFilterFcn( idA, idB, queryContext->world->customFilterContext ); + if ( shouldCollide == false ) + { + return true; + } + } + } + + b3Transform otherTransform = b3ToRelativeTransform( b3GetBodyTransform( world, otherShape->bodyId ), b3Pos_zero ); + + bool overlap = b3OverlapSensor( sensorShape, queryContext->transform, otherShape, otherTransform ); + if ( overlap == false ) + { + return true; + } + + // Record the overlap + b3Sensor* sensor = queryContext->sensor; + b3Visitor* shapeRef = b3Array_Emplace( sensor->overlaps2 ); + shapeRef->shapeId = shapeId; + shapeRef->generation = otherShape->generation; + + return true; +} + +static int b3CompareVisitors( const void* a, const void* b ) +{ + const b3Visitor* sa = (const b3Visitor*)a; + const b3Visitor* sb = (const b3Visitor*)b; + + if ( sa->shapeId < sb->shapeId ) + { + return -1; + } + + return 1; +} + +static void b3SensorTask( int startIndex, int endIndex, int workerIndex, void* context ) +{ + b3TracyCZoneNC( sensor_task, "Overlap", b3_colorBrown, true ); + + b3World* world = (b3World*)context; + B3_ASSERT( (int)workerIndex < world->workerCount ); + b3SensorTaskContext* taskContext = world->sensorTaskContexts.data + workerIndex; + + B3_ASSERT( startIndex < endIndex ); + + b3DynamicTree* trees = world->broadPhase.trees; + for ( int sensorIndex = startIndex; sensorIndex < endIndex; ++sensorIndex ) + { + b3Sensor* sensor = b3Array_Get( world->sensors, sensorIndex ); + b3Shape* sensorShape = b3Array_Get( world->shapes, sensor->shapeId ); + + // Swap overlap arrays + b3Array( b3Visitor ) temp = sensor->overlaps1; + sensor->overlaps1 = sensor->overlaps2; + sensor->overlaps2 = temp; + b3Array_Clear( sensor->overlaps2 ); + + // Append sensor hits + b3Array_Append( sensor->overlaps2, sensor->hits.data, sensor->hits.count ); + + // Clear the hits + b3Array_Clear( sensor->hits ); + + b3Body* body = b3Array_Get( world->bodies, sensorShape->bodyId ); + if ( body->setIndex == b3_disabledSet || ( sensorShape->flags & b3_enableSensorEvents ) == 0 ) + { + if ( sensor->overlaps1.count != 0 ) + { + // This sensor is dropping all overlaps because it has been disabled. + b3SetBit( &taskContext->eventBits, sensorIndex ); + } + continue; + } + + b3Transform transform = b3ToRelativeTransform( b3GetBodyTransformQuick( world, body ), b3Pos_zero ); + + b3SensorQueryContext queryContext = { + .world = world, + .taskContext = taskContext, + .sensor = sensor, + .sensorShape = sensorShape, + .transform = transform, + }; + + B3_ASSERT( sensorShape->sensorIndex == sensorIndex ); + b3AABB queryBounds = sensorShape->aabb; + + // Query all trees + b3DynamicTree_Query( trees + 0, queryBounds, sensorShape->filter.maskBits, false, b3SensorQueryCallback, &queryContext ); + b3DynamicTree_Query( trees + 1, queryBounds, sensorShape->filter.maskBits, false, b3SensorQueryCallback, &queryContext ); + b3DynamicTree_Query( trees + 2, queryBounds, sensorShape->filter.maskBits, false, b3SensorQueryCallback, &queryContext ); + + // Sort the overlaps to enable finding begin and end events. + qsort( sensor->overlaps2.data, sensor->overlaps2.count, sizeof( b3Visitor ), b3CompareVisitors ); + + // Remove duplicates from overlaps2 (sorted). Duplicates are possible due to the hit events appended earlier. + int uniqueCount = 0; + int overlapCount = sensor->overlaps2.count; + b3Visitor* overlapData = sensor->overlaps2.data; + for ( int i = 0; i < overlapCount; ++i ) + { + if ( uniqueCount == 0 || overlapData[i].shapeId != overlapData[uniqueCount - 1].shapeId ) + { + overlapData[uniqueCount] = overlapData[i]; + uniqueCount += 1; + } + } + sensor->overlaps2.count = uniqueCount; + + int count1 = sensor->overlaps1.count; + int count2 = sensor->overlaps2.count; + if ( count1 != count2 ) + { + // something changed + b3SetBit( &taskContext->eventBits, sensorIndex ); + } + else + { + for ( int i = 0; i < count1; ++i ) + { + b3Visitor* s1 = sensor->overlaps1.data + i; + b3Visitor* s2 = sensor->overlaps2.data + i; + + if ( s1->shapeId != s2->shapeId || s1->generation != s2->generation ) + { + // something changed + b3SetBit( &taskContext->eventBits, sensorIndex ); + break; + } + } + } + } + + b3TracyCZoneEnd( sensor_task ); +} + +void b3OverlapSensors( b3World* world ) +{ + int sensorCount = world->sensors.count; + if ( sensorCount == 0 ) + { + return; + } + + B3_ASSERT( world->workerCount > 0 ); + + b3TracyCZoneNC( overlap_sensors, "Sensors", b3_colorMediumPurple, true ); + + for ( int i = 0; i < world->workerCount; ++i ) + { + b3SetBitCountAndClear( &world->sensorTaskContexts.data[i].eventBits, sensorCount ); + } + + // Parallel-for sensors overlaps + int minRange = 16; + b3ParallelFor( world, b3SensorTask, sensorCount, minRange, world, "sensors" ); + + b3TracyCZoneNC( sensor_state, "Events", b3_colorLightSlateGray, true ); + + b3BitSet* bitSet = &world->sensorTaskContexts.data[0].eventBits; + for ( int i = 1; i < world->workerCount; ++i ) + { + b3InPlaceUnion( bitSet, &world->sensorTaskContexts.data[i].eventBits ); + } + + // Iterate sensors bits and publish events + // Process sensor state changes. Iterate over set bits + uint64_t* bits = bitSet->bits; + uint32_t blockCount = bitSet->blockCount; + + for ( uint32_t k = 0; k < blockCount; ++k ) + { + uint64_t word = bits[k]; + while ( word != 0 ) + { + uint32_t ctz = b3CTZ64( word ); + int sensorIndex = (int)( 64 * k + ctz ); + + b3Sensor* sensor = b3Array_Get( world->sensors, sensorIndex ); + b3Shape* sensorShape = b3Array_Get( world->shapes, sensor->shapeId ); + b3ShapeId sensorId = { sensor->shapeId + 1, world->worldId, sensorShape->generation }; + + int count1 = sensor->overlaps1.count; + int count2 = sensor->overlaps2.count; + const b3Visitor* refs1 = sensor->overlaps1.data; + const b3Visitor* refs2 = sensor->overlaps2.data; + + // overlaps1 can have overlaps that end + // overlaps2 can have overlaps that begin + int index1 = 0, index2 = 0; + while ( index1 < count1 && index2 < count2 ) + { + const b3Visitor* r1 = refs1 + index1; + const b3Visitor* r2 = refs2 + index2; + if ( r1->shapeId == r2->shapeId ) + { + if ( r1->generation < r2->generation ) + { + // end + b3ShapeId visitorId = { r1->shapeId + 1, world->worldId, r1->generation }; + b3SensorEndTouchEvent event = { + .sensorShapeId = sensorId, + .visitorShapeId = visitorId, + }; + b3Array_Push( world->sensorEndEvents[world->endEventArrayIndex], event ); + index1 += 1; + } + else if ( r1->generation > r2->generation ) + { + // begin + b3ShapeId visitorId = { r2->shapeId + 1, world->worldId, r2->generation }; + b3SensorBeginTouchEvent event = { sensorId, visitorId }; + b3Array_Push( world->sensorBeginEvents, event ); + index2 += 1; + } + else + { + // persisted + index1 += 1; + index2 += 1; + } + } + else if ( r1->shapeId < r2->shapeId ) + { + // end + b3ShapeId visitorId = { r1->shapeId + 1, world->worldId, r1->generation }; + b3SensorEndTouchEvent event = { sensorId, visitorId }; + b3Array_Push( world->sensorEndEvents[world->endEventArrayIndex], event ); + index1 += 1; + } + else + { + // begin + b3ShapeId visitorId = { r2->shapeId + 1, world->worldId, r2->generation }; + b3SensorBeginTouchEvent event = { sensorId, visitorId }; + b3Array_Push( world->sensorBeginEvents, event ); + index2 += 1; + } + } + + while ( index1 < count1 ) + { + // end + const b3Visitor* r1 = refs1 + index1; + b3ShapeId visitorId = { r1->shapeId + 1, world->worldId, r1->generation }; + b3SensorEndTouchEvent event = { sensorId, visitorId }; + b3Array_Push( world->sensorEndEvents[world->endEventArrayIndex], event ); + index1 += 1; + } + + while ( index2 < count2 ) + { + // begin + const b3Visitor* r2 = refs2 + index2; + b3ShapeId visitorId = { r2->shapeId + 1, world->worldId, r2->generation }; + b3SensorBeginTouchEvent event = { sensorId, visitorId }; + b3Array_Push( world->sensorBeginEvents, event ); + index2 += 1; + } + + // Clear the smallest set bit + word = word & ( word - 1 ); + } + } + + b3TracyCZoneEnd( sensor_state ); + b3TracyCZoneEnd( overlap_sensors ); +} + +void b3DestroySensor( b3World* world, b3Shape* sensorShape ) +{ + b3Sensor* sensor = b3Array_Get( world->sensors, sensorShape->sensorIndex ); + for ( int i = 0; i < sensor->overlaps2.count; ++i ) + { + b3Visitor* ref = sensor->overlaps2.data + i; + b3SensorEndTouchEvent event = { + .sensorShapeId = + { + .index1 = sensorShape->id + 1, + .world0 = world->worldId, + .generation = sensorShape->generation, + }, + .visitorShapeId = + { + .index1 = ref->shapeId + 1, + .world0 = world->worldId, + .generation = ref->generation, + }, + }; + + b3Array_Push( world->sensorEndEvents[world->endEventArrayIndex], event ); + } + + // Destroy sensor + b3Array_Destroy( sensor->hits ); + b3Array_Destroy( sensor->overlaps1 ); + b3Array_Destroy( sensor->overlaps2 ); + + int movedIndex = b3Array_RemoveSwap( world->sensors, sensorShape->sensorIndex ); + if ( movedIndex != B3_NULL_INDEX ) + { + // Fixup moved sensor + b3Sensor* movedSensor = b3Array_Get( world->sensors, sensorShape->sensorIndex ); + b3Shape* otherSensorShape = b3Array_Get( world->shapes, movedSensor->shapeId ); + otherSensorShape->sensorIndex = sensorShape->sensorIndex; + } +} diff --git a/vendor/box3d/src/src/sensor.h b/vendor/box3d/src/src/sensor.h new file mode 100644 index 000000000..1a17366b7 --- /dev/null +++ b/vendor/box3d/src/src/sensor.h @@ -0,0 +1,41 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +#pragma once + +#include "bitset.h" +#include "container.h" + +typedef struct b3Shape b3Shape; +typedef struct b3World b3World; + +// Used to track shapes that hit sensors using time of impact +typedef struct b3SensorHit +{ + int sensorId; + int visitorId; +} b3SensorHit; + +typedef struct b3Visitor +{ + int shapeId; + uint16_t generation; +} b3Visitor; + +b3DeclareArray( b3Visitor ); + +typedef struct b3Sensor +{ + b3Array( b3Visitor ) hits; + b3Array( b3Visitor ) overlaps1; + b3Array( b3Visitor ) overlaps2; + int shapeId; +} b3Sensor; + +typedef struct b3SensorTaskContext +{ + b3BitSet eventBits; +} b3SensorTaskContext; + +void b3OverlapSensors( b3World* world ); +void b3DestroySensor( b3World* world, b3Shape* sensorShape ); diff --git a/vendor/box3d/src/src/shape.c b/vendor/box3d/src/src/shape.c new file mode 100644 index 000000000..e4d9d9e0c --- /dev/null +++ b/vendor/box3d/src/src/shape.c @@ -0,0 +1,2393 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +#include "shape.h" + +#include "body.h" +#include "broad_phase.h" +#include "contact.h" +#include "physics_world.h" +#include "recording.h" +#include "sensor.h" + +// needed for dll export +#include "aabb.h" +#include "compound.h" + +#include "box3d/box3d.h" + +static b3Shape* b3GetShape( b3World* world, b3ShapeId shapeId ) +{ + int id = shapeId.index1 - 1; + b3Shape* shape = b3Array_Get( world->shapes, id ); + B3_ASSERT( shape->id == id && shape->generation == shapeId.generation ); + return shape; +} + +static float b3ComputeShapeMargin( b3Shape* shape ) +{ + float margin = 0.0f; + + switch ( shape->type ) + { + case b3_sphereShape: + { + margin = shape->sphere.radius; + } + break; + + case b3_capsuleShape: + { + margin = 0.5f * b3Distance( shape->capsule.center2, shape->capsule.center1 ) + shape->capsule.radius; + } + break; + + case b3_hullShape: + { + const b3HullData* hull = shape->hull; + const b3Vec3* points = b3GetHullPoints( hull ); + float maxExtentSqr = 0.0f; + int count = hull->vertexCount; + for ( int i = 0; i < count; ++i ) + { + float distSqr = b3DistanceSquared( points[i], hull->center ); + maxExtentSqr = b3MaxFloat( maxExtentSqr, distSqr ); + } + margin = sqrtf( maxExtentSqr ); + } + break; + + case b3_meshShape: + case b3_heightShape: + case b3_compoundShape: + { + // Static-only shapes: broadphase uses speculative distance for static + // proxies, so the per-shape margin is never consumed in practice. + // Return the cap so any incidental use is generous. + return B3_MAX_AABB_MARGIN; + } + + default: + B3_VALIDATE( false ); + return B3_MAX_AABB_MARGIN; + } + + return b3MinFloat( B3_MAX_AABB_MARGIN, B3_AABB_MARGIN_FRACTION * margin ); +} + +static void b3UpdateShapeAABBs( b3Shape* shape, b3WorldTransform transform, b3BodyType proxyType ) +{ + // Compute a bounding box with a speculative margin + const float speculativeDistance = B3_SPECULATIVE_DISTANCE; + const float aabbMargin = shape->aabbMargin; + + b3AABB aabb = b3ComputeFatShapeAABB( shape, transform, speculativeDistance ); + shape->aabb = aabb; + + // Smaller margin for static bodies. Cannot be zero due to TOI tolerance. + float margin = proxyType == b3_staticBody ? speculativeDistance : aabbMargin; + b3AABB fatAABB; + fatAABB.lowerBound.x = aabb.lowerBound.x - margin; + fatAABB.lowerBound.y = aabb.lowerBound.y - margin; + fatAABB.lowerBound.z = aabb.lowerBound.z - margin; + fatAABB.upperBound.x = aabb.upperBound.x + margin; + fatAABB.upperBound.y = aabb.upperBound.y + margin; + fatAABB.upperBound.z = aabb.upperBound.z + margin; + shape->fatAABB = fatAABB; +} + +static b3Shape* b3CreateShapeInternal( b3World* world, b3Body* body, b3WorldTransform bodyTransform, const b3ShapeDef* def, + const void* geometry, b3ShapeType shapeType, b3Transform shapeTransform, b3Vec3 scale, + bool haveShapeTransform ) +{ + int shapeId = b3AllocId( &world->shapeIdPool ); + + if ( shapeId == world->shapes.count ) + { + b3Array_Push( world->shapes, (b3Shape){ 0 } ); + } + else + { + B3_ASSERT( world->shapes.data[shapeId].id == B3_NULL_INDEX ); + } + + b3Shape* shape = b3Array_Get( world->shapes, shapeId ); + + switch ( shapeType ) + { + case b3_capsuleShape: + shape->capsule = *(b3Capsule*)geometry; + break; + + case b3_compoundShape: + // Compounds must be a static and not a sensor + B3_ASSERT( body->type == b3_staticBody ); + B3_ASSERT( def->isSensor == false ); + shape->compound = (b3CompoundData*)geometry; + break; + + case b3_sphereShape: + shape->sphere = *(b3Sphere*)geometry; + break; + + case b3_hullShape: + if ( haveShapeTransform ) + { + // The transform and non-uniform scale are baked into fresh data, then shared. + b3HullData* baked = b3CloneAndTransformHull( (b3HullData*)geometry, shapeTransform, scale ); + if ( baked == NULL ) + { + // This can fail to produce a valid hull in extreme cases + b3FreeId( &world->shapeIdPool, shapeId ); + shape->id = B3_NULL_INDEX; + return NULL; + } + + shape->hull = b3AddOwnedHullToDatabase( world, baked ); + } + else + { + shape->hull = b3AddHullToDatabase( world, (const b3HullData*)geometry ); + } + break; + + case b3_meshShape: + { + shape->mesh.data = (b3MeshData*)geometry; + shape->mesh.scale = b3SafeScale( scale ); + } + break; + + case b3_heightShape: + shape->heightField = (b3HeightFieldData*)geometry; + break; + + default: + B3_ASSERT( false ); + break; + } + + shape->id = shapeId; + shape->bodyId = body->id; + shape->type = shapeType; + shape->density = def->density; + shape->explosionScale = def->explosionScale; + shape->filter = def->filter; + shape->userData = def->userData; + shape->userShape = NULL; + shape->flags = 0; + shape->flags |= def->enableSensorEvents ? b3_enableSensorEvents : 0; + shape->flags |= def->enableContactEvents ? b3_enableContactEvents : 0; + shape->flags |= def->enableCustomFiltering ? b3_enableCustomFiltering : 0; + shape->flags |= def->enableHitEvents ? b3_enableHitEvents : 0; + shape->flags |= def->enablePreSolveEvents ? b3_enablePreSolveEvents : 0; + shape->flags |= def->enableSpeculativeContact ? b3_enableSpeculative : 0; + shape->proxyKey = B3_NULL_INDEX; + shape->localCentroid = b3GetShapeCentroid( shape ); + shape->aabbMargin = b3ComputeShapeMargin( shape ); + shape->aabb = (b3AABB){ b3Vec3_zero, b3Vec3_zero }; + shape->fatAABB = (b3AABB){ b3Vec3_zero, b3Vec3_zero }; + shape->nameId = b3AddName( &world->names, def->name ); + shape->generation += 1; + + if ( shape->type == b3_compoundShape ) + { + // Own a copy of the compound materials so every shape frees its array the same way. Compounds + // are few, so the copy is cheap and avoids aliasing the geometry blob. + int materialCount = shape->compound->materialCount; + shape->materialCount = materialCount; + shape->materials = b3Alloc( materialCount * sizeof( b3SurfaceMaterial ) ); + memcpy( shape->materials, b3GetCompoundMaterials( shape->compound ), materialCount * sizeof( b3SurfaceMaterial ) ); + } + else if ( def->materialCount > 1 && def->materials != NULL ) + { + // Per triangle materials need a heap array. + shape->materialCount = def->materialCount; + shape->materials = b3Alloc( def->materialCount * sizeof( b3SurfaceMaterial ) ); + memcpy( shape->materials, def->materials, def->materialCount * sizeof( b3SurfaceMaterial ) ); + } + else + { + // The common case is one material, stored inline with no allocation. + shape->material = ( def->materialCount == 1 && def->materials != NULL ) ? def->materials[0] : def->baseMaterial; + shape->materialCount = 1; + shape->materials = NULL; + } + + if ( body->setIndex != b3_disabledSet ) + { + b3BodyType proxyType = body->type; + bool forcePairCreation = def->invokeContactCreation && shape->type != b3_compoundShape; + b3CreateShapeProxy( shape, &world->broadPhase, proxyType, bodyTransform, forcePairCreation ); + } + + // Add to shape doubly linked list + if ( body->headShapeId != B3_NULL_INDEX ) + { + b3Shape* headShape = b3Array_Get( world->shapes, body->headShapeId ); + headShape->prevShapeId = shapeId; + } + + shape->prevShapeId = B3_NULL_INDEX; + shape->nextShapeId = body->headShapeId; + body->headShapeId = shapeId; + body->shapeCount += 1; + + if ( def->isSensor ) + { + shape->sensorIndex = world->sensors.count; + b3Sensor* sensor = b3Array_Emplace( world->sensors ); + b3Array_CreateN( sensor->hits, 4 ); + b3Array_CreateN( sensor->overlaps1, 16 ); + b3Array_CreateN( sensor->overlaps2, 16 ); + sensor->shapeId = shapeId; + } + else + { + shape->sensorIndex = B3_NULL_INDEX; + } + + b3ValidateSolverSets( world ); + + return shape; +} + +static b3ShapeId b3CreateShape( b3BodyId bodyId, const b3ShapeDef* def, const void* geometry, b3ShapeType shapeType, + b3Transform transform, b3Vec3 scale, bool haveTransform ) +{ + B3_CHECK_DEF( def ); + B3_ASSERT( b3IsValidFloat( def->density ) && def->density >= 0.0f ); + B3_ASSERT( b3IsValidFloat( def->baseMaterial.friction ) && def->baseMaterial.friction >= 0.0f ); + B3_ASSERT( b3IsValidFloat( def->baseMaterial.restitution ) && def->baseMaterial.restitution >= 0.0f ); + + b3World* world = b3GetUnlockedWorld( bodyId.world0 ); + if ( world == NULL ) + { + return (b3ShapeId){ 0 }; + } + + if ( world->shapes.count == B3_MAX_SHAPES && world->shapeIdPool.freeArray.count == 0 ) + { + B3_ASSERT( false ); + return b3_nullShapeId; + } + + b3Body* body = b3GetBodyFullId( world, bodyId ); + if ( body->type != b3_staticBody && ( shapeType == b3_compoundShape || shapeType == b3_heightShape ) ) + { + // Compound and height shapes must be on static bodies. + return b3_nullShapeId; + } + + world->locked = true; + + b3WorldTransform bodyTransform = b3GetBodyTransformQuick( world, body ); + + b3Shape* shape = + b3CreateShapeInternal( world, body, bodyTransform, def, geometry, shapeType, transform, scale, haveTransform ); + + if ( shape == NULL ) + { + world->locked = false; + return b3_nullShapeId; + } + + if ( def->updateBodyMass == true ) + { + b3UpdateBodyMassData( world, body ); + } + else if ( ( body->flags & b3_dirtyMass ) == 0 ) + { + body->flags |= b3_dirtyMass; + b3SyncBodyFlags( world, body ); + } + + b3ValidateSolverSets( world ); + + b3ShapeId id = { shape->id + 1, bodyId.world0, shape->generation }; + + world->locked = false; + + return id; +} + +b3ShapeId b3CreateSphereShape( b3BodyId bodyId, const b3ShapeDef* def, const b3Sphere* sphere ) +{ + b3ShapeId shapeId = b3CreateShape( bodyId, def, sphere, b3_sphereShape, b3Transform_identity, b3Vec3_one, false ); + if ( shapeId.index1 != 0 ) + { + b3World* world = b3GetUnlockedWorld( bodyId.world0 ); + if ( world != NULL ) + { + B3_REC_CREATE( world, CreateSphereShape, shapeId, bodyId, *def, *sphere ); + } + } + return shapeId; +} + +b3ShapeId b3CreateCapsuleShape( b3BodyId bodyId, const b3ShapeDef* def, const b3Capsule* capsule ) +{ + float lengthSqr = b3DistanceSquared( capsule->center1, capsule->center2 ); + b3ShapeId shapeId; + if ( lengthSqr <= B3_LINEAR_SLOP * B3_LINEAR_SLOP ) + { + b3Sphere sphere = { b3Lerp( capsule->center1, capsule->center2, 0.5f ), capsule->radius }; + shapeId = b3CreateShape( bodyId, def, &sphere, b3_sphereShape, b3Transform_identity, b3Vec3_one, false ); + } + else + { + shapeId = b3CreateShape( bodyId, def, capsule, b3_capsuleShape, b3Transform_identity, b3Vec3_one, false ); + } + if ( shapeId.index1 != 0 ) + { + b3World* world = b3GetUnlockedWorld( bodyId.world0 ); + if ( world != NULL ) + { + B3_REC_CREATE( world, CreateCapsuleShape, shapeId, bodyId, *def, *capsule ); + } + } + return shapeId; +} + +b3ShapeId b3CreateHullShape( b3BodyId bodyId, const b3ShapeDef* def, const b3HullData* hull ) +{ + B3_VALIDATE( b3IsValidHull( hull ) ); + B3_VALIDATE( hull->hash != 0 ); + b3ShapeId shapeId = b3CreateShape( bodyId, def, hull, b3_hullShape, b3Transform_identity, b3Vec3_one, false ); + if ( shapeId.index1 != 0 ) + { + b3World* world = b3GetUnlockedWorld( bodyId.world0 ); + if ( world != NULL && world->recording != NULL ) + { + uint32_t geometryId = b3RecInternHull( world->recording, hull ); + b3RecArgs_CreateHullShape createArgs = { bodyId, *def, geometryId }; + b3RecWriteRet_CreateHullShape( world->recording, &createArgs, shapeId ); + } + } + return shapeId; +} + +b3ShapeId b3CreateTransformedHullShape( b3BodyId bodyId, const b3ShapeDef* def, const b3HullData* hull, b3Transform transform, + b3Vec3 scale ) +{ + B3_VALIDATE( b3IsValidHull( hull ) ); + b3ShapeId shapeId = b3CreateShape( bodyId, def, hull, b3_hullShape, transform, scale, true ); + if ( shapeId.index1 != 0 ) + { + b3World* world = b3GetUnlockedWorld( bodyId.world0 ); + if ( world != NULL && world->recording != NULL ) + { + // The transform and scale are baked into fresh hull data at create time. Record the baked hull + // as a plain hull shape so replay rebuilds identical geometry with no rebake, and the keyframe + // registry, which interns the live baked hull, stays seeded. + b3Shape* shape = b3Array_Get( world->shapes, shapeId.index1 - 1 ); + uint32_t geometryId = b3RecInternHull( world->recording, shape->hull ); + b3RecArgs_CreateHullShape createArgs = { bodyId, *def, geometryId }; + b3RecWriteRet_CreateHullShape( world->recording, &createArgs, shapeId ); + } + } + return shapeId; +} + +b3ShapeId b3CreateMeshShape( b3BodyId bodyId, const b3ShapeDef* def, const b3MeshData* mesh, b3Vec3 scale ) +{ + B3_VALIDATE( b3IsValidMesh( mesh ) ); + B3_VALIDATE( mesh->hash != 0 ); + b3ShapeId shapeId = b3CreateShape( bodyId, def, mesh, b3_meshShape, b3Transform_identity, scale, true ); + if ( shapeId.index1 != 0 ) + { + b3World* world = b3GetUnlockedWorld( bodyId.world0 ); + if ( world != NULL && world->recording != NULL ) + { + uint32_t geometryId = b3RecInternMesh( world->recording, mesh ); + b3RecArgs_CreateMeshShape createArgs = { bodyId, *def, geometryId, scale }; + b3RecWriteRet_CreateMeshShape( world->recording, &createArgs, shapeId ); + } + } + return shapeId; +} + +b3ShapeId b3CreateHeightFieldShape( b3BodyId bodyId, const b3ShapeDef* def, const b3HeightFieldData* heightField ) +{ + B3_VALIDATE( heightField->hash != 0 ); + b3ShapeId shapeId = b3CreateShape( bodyId, def, heightField, b3_heightShape, b3Transform_identity, b3Vec3_one, false ); + if ( shapeId.index1 != 0 ) + { + b3World* world = b3GetUnlockedWorld( bodyId.world0 ); + if ( world != NULL && world->recording != NULL ) + { + uint32_t geometryId = b3RecInternHeightField( world->recording, heightField ); + b3RecArgs_CreateHeightFieldShape createArgs = { bodyId, *def, geometryId }; + b3RecWriteRet_CreateHeightFieldShape( world->recording, &createArgs, shapeId ); + } + } + return shapeId; +} + +b3ShapeId b3CreateCompoundShape( b3BodyId bodyId, b3ShapeDef* def, const b3CompoundData* compound ) +{ + b3ShapeId shapeId = b3CreateShape( bodyId, def, compound, b3_compoundShape, b3Transform_identity, b3Vec3_one, false ); + if ( shapeId.index1 != 0 ) + { + b3World* world = b3GetUnlockedWorld( bodyId.world0 ); + if ( world != NULL && world->recording != NULL ) + { + uint32_t geometryId = b3RecInternCompound( world->recording, compound ); + b3RecArgs_CreateCompoundShape createArgs = { bodyId, *def, geometryId }; + b3RecWriteRet_CreateCompoundShape( world->recording, &createArgs, shapeId ); + } + } + return shapeId; +} + +// Destroy a shape on a body. This doesn't need to be called when destroying a body. +static void b3DestroyShapeInternal( b3World* world, b3Shape* shape, b3Body* body, bool wakeBodies ) +{ + int shapeId = shape->id; + + // Remove the shape from the body's doubly linked list. + if ( shape->prevShapeId != B3_NULL_INDEX ) + { + b3Shape* prevShape = b3Array_Get( world->shapes, shape->prevShapeId ); + prevShape->nextShapeId = shape->nextShapeId; + } + + if ( shape->nextShapeId != B3_NULL_INDEX ) + { + b3Shape* nextShape = b3Array_Get( world->shapes, shape->nextShapeId ); + nextShape->prevShapeId = shape->prevShapeId; + } + + if ( shapeId == body->headShapeId ) + { + body->headShapeId = shape->nextShapeId; + } + + body->shapeCount -= 1; + + // Remove from broad-phase. + b3DestroyShapeProxy( shape, &world->broadPhase ); + + // Destroy any contacts associated with the shape. + int contactKey = body->headContactKey; + while ( contactKey != B3_NULL_INDEX ) + { + int contactId = contactKey >> 1; + int edgeIndex = contactKey & 1; + + b3Contact* contact = b3Array_Get( world->contacts, contactId ); + contactKey = contact->edges[edgeIndex].nextKey; + + if ( contact->shapeIdA == shapeId || contact->shapeIdB == shapeId ) + { + b3DestroyContact( world, contact, wakeBodies ); + } + } + + if ( shape->sensorIndex != B3_NULL_INDEX ) + { + b3Sensor* sensor = b3Array_Get( world->sensors, shape->sensorIndex ); + for ( int i = 0; i < sensor->overlaps2.count; ++i ) + { + b3Visitor* ref = sensor->overlaps2.data + i; + b3SensorEndTouchEvent event = { + .sensorShapeId = + { + .index1 = shapeId + 1, + .world0 = world->worldId, + .generation = shape->generation, + }, + .visitorShapeId = + { + .index1 = ref->shapeId + 1, + .world0 = world->worldId, + .generation = ref->generation, + }, + }; + + b3Array_Push( world->sensorEndEvents[world->endEventArrayIndex], event ); + } + + // Destroy sensor + b3Array_Destroy( sensor->hits ); + b3Array_Destroy( sensor->overlaps1 ); + b3Array_Destroy( sensor->overlaps2 ); + + int movedIndex = b3Array_RemoveSwap( world->sensors, shape->sensorIndex ); + if ( movedIndex != B3_NULL_INDEX ) + { + // Fixup moved sensor + b3Sensor* movedSensor = b3Array_Get( world->sensors, shape->sensorIndex ); + b3Shape* otherSensorShape = b3Array_Get( world->shapes, movedSensor->shapeId ); + otherSensorShape->sensorIndex = shape->sensorIndex; + } + } + + // Destroy every shape member from b3Alloc + b3DestroyShapeAllocations( world, shape ); + + // Return shape to free list. + b3FreeId( &world->shapeIdPool, shapeId ); + shape->id = B3_NULL_INDEX; + + b3ValidateSolverSets( world ); +} + +void b3DestroyShape( b3ShapeId shapeId, bool updateBodyMass ) +{ + b3World* world = b3GetUnlockedWorld( shapeId.world0 ); + if ( world == NULL ) + { + return; + } + + B3_REC( world, DestroyShape, shapeId, updateBodyMass ); + + world->locked = true; + + b3Shape* shape = b3GetShape( world, shapeId ); + + // need to wake bodies because this might be a static body + bool wakeBodies = true; + + b3Body* body = b3Array_Get( world->bodies, shape->bodyId ); + b3DestroyShapeInternal( world, shape, body, wakeBodies ); + + if ( updateBodyMass == true ) + { + b3UpdateBodyMassData( world, body ); + } + + world->locked = false; +} + +b3AABB b3ComputeShapeAABB( const b3Shape* shape, b3Transform transform ) +{ + switch ( shape->type ) + { + case b3_capsuleShape: + return b3ComputeCapsuleAABB( &shape->capsule, transform ); + + case b3_compoundShape: + return b3ComputeCompoundAABB( shape->compound, transform ); + + case b3_heightShape: + return b3ComputeHeightFieldAABB( shape->heightField, transform ); + + case b3_hullShape: + return b3ComputeHullAABB( shape->hull, transform ); + + case b3_meshShape: + return b3ComputeMeshAABB( shape->mesh.data, transform, shape->mesh.scale ); + + case b3_sphereShape: + return b3ComputeSphereAABB( &shape->sphere, transform ); + + default: + { + B3_ASSERT( false ); + b3AABB empty = { transform.p, transform.p }; + return empty; + } + } +} + +b3AABB b3ComputeFatShapeAABB( const b3Shape* shape, b3WorldTransform transform, float extra ) +{ + b3Vec3 r = { extra, extra, extra }; +#if defined( BOX3D_DOUBLE_PRECISION ) + // Build the box in the body local frame, inflate, then translate by the double origin and + // round outward. Inflating before the single rounding matters far from the origin where the + // float margin would otherwise vanish. + b3Transform rotation = { b3Vec3_zero, transform.q }; + b3AABB localBox = b3ComputeShapeAABB( shape, rotation ); + localBox.lowerBound = b3Sub( localBox.lowerBound, r ); + localBox.upperBound = b3Add( localBox.upperBound, r ); + return b3OffsetAABB( localBox, transform.p ); +#else + b3AABB aabb = b3ComputeShapeAABB( shape, transform ); + aabb.lowerBound = b3Sub( aabb.lowerBound, r ); + aabb.upperBound = b3Add( aabb.upperBound, r ); + return aabb; +#endif +} + +b3AABB b3ComputeSweptShapeAABB( const b3Shape* shape, const b3Sweep* sweep, float time ) +{ + B3_ASSERT( 0.0f <= time && time <= 1.0f ); + b3Transform xf1 = { b3Sub( sweep->c1, b3RotateVector( sweep->q1, sweep->localCenter ) ), sweep->q1 }; + b3Transform xf2 = b3GetSweepTransform( sweep, time ); + + switch ( shape->type ) + { + case b3_capsuleShape: + return b3ComputeSweptCapsuleAABB( &shape->capsule, xf1, xf2 ); + + case b3_hullShape: + return b3ComputeSweptHullAABB( shape->hull, xf1, xf2 ); + + case b3_sphereShape: + return b3ComputeSweptSphereAABB( &shape->sphere, xf1, xf2 ); + + default: + B3_ASSERT( false ); + return (b3AABB){ xf1.p, xf1.p }; + } +} + +b3Vec3 b3GetShapeCentroid( const b3Shape* shape ) +{ + switch ( shape->type ) + { + case b3_capsuleShape: + return b3Lerp( shape->capsule.center1, shape->capsule.center2, 0.5f ); + case b3_compoundShape: + { + b3AABB aabb = b3ComputeCompoundAABB( shape->compound, b3Transform_identity ); + return b3AABB_Center( aabb ); + } + case b3_sphereShape: + return shape->sphere.center; + case b3_hullShape: + return shape->hull->center; + case b3_meshShape: + { + b3AABB aabb = b3ComputeMeshAABB( shape->mesh.data, b3Transform_identity, shape->mesh.scale ); + return b3AABB_Center( aabb ); + } + case b3_heightShape: + { + b3AABB aabb = b3ComputeHeightFieldAABB( shape->heightField, b3Transform_identity ); + return b3AABB_Center( aabb ); + } + default: + return b3Vec3_zero; + } +} + +float b3GetShapeArea( const b3Shape* shape ) +{ + // todo_erin fix these + switch ( shape->type ) + { + case b3_capsuleShape: + return 2.0f * b3Length( b3Sub( shape->capsule.center1, shape->capsule.center2 ) ) + + 2.0f * B3_PI * shape->capsule.radius; + + case b3_hullShape: + return shape->hull->surfaceArea; + + case b3_sphereShape: + return 2.0f * B3_PI * shape->sphere.radius; + + default: + return 0.0f; + } +} + +// This projects the shape surface area onto a plane +float b3GetShapeProjectedArea( const b3Shape* shape, b3Vec3 planeNormal ) +{ + switch ( shape->type ) + { + case b3_capsuleShape: + { + float radius = shape->capsule.radius; + b3Vec3 axis = b3Sub( shape->capsule.center2, shape->capsule.center1 ); + float projectedLength = b3Length( b3Cross( axis, planeNormal ) ); + float cylinderArea = 2.0f * radius * projectedLength; + float sphereArea = B3_PI * radius * radius; + return sphereArea + cylinderArea; + } + + case b3_hullShape: + return b3ComputeHullProjectedArea( shape->hull, planeNormal ); + + case b3_sphereShape: + return B3_PI * shape->sphere.radius * shape->sphere.radius; + + default: + return 0.0f; + } +} + +b3MassData b3ComputeShapeMass( const b3Shape* shape ) +{ + switch ( shape->type ) + { + case b3_capsuleShape: + return b3ComputeCapsuleMass( &shape->capsule, shape->density ); + + case b3_hullShape: + return b3ComputeHullMass( shape->hull, shape->density ); + + case b3_sphereShape: + return b3ComputeSphereMass( &shape->sphere, shape->density ); + + default: + return (b3MassData){ 0 }; + } +} + +b3ShapeExtent b3ComputeShapeExtent( const b3Shape* shape, b3Vec3 localCenter ) +{ + b3ShapeExtent extent = { 0 }; + + switch ( shape->type ) + { + case b3_capsuleShape: + { + float radius = shape->capsule.radius; + extent.minExtent = radius; + b3Vec3 c1 = b3Sub( shape->capsule.center1, localCenter ); + b3Vec3 c2 = b3Sub( shape->capsule.center2, localCenter ); + b3Vec3 r = { radius, radius, radius }; + extent.maxExtent = b3Add( b3Max( c1, c2 ), r ); + } + break; + + case b3_compoundShape: + { + // This is shouldn't be needed but here for completeness + b3AABB aabb = b3ComputeCompoundAABB( shape->compound, b3Transform_identity ); + float r1 = b3Length( b3Sub( aabb.lowerBound, localCenter ) ); + float r2 = b3Length( b3Sub( aabb.upperBound, localCenter ) ); + extent.minExtent = b3MinFloat( r1, r2 ); + b3Vec3 p = b3FarthestPointOnAABB( aabb, localCenter ); + extent.maxExtent = b3Abs( b3Sub( p, localCenter ) ); + } + break; + + case b3_sphereShape: + { + float radius = shape->sphere.radius; + extent.minExtent = radius; + b3Vec3 r = { radius, radius, radius }; + b3Vec3 p = b3Add( b3Sub( shape->sphere.center, localCenter ), r ); + extent.maxExtent = b3Abs( b3Sub( p, localCenter ) ); + } + break; + + case b3_hullShape: + extent = b3ComputeHullExtent( shape->hull, localCenter ); + break; + + case b3_meshShape: + { + // This is needed for kinematic mesh sleeping + b3AABB aabb = b3ComputeMeshAABB( shape->mesh.data, b3Transform_identity, shape->mesh.scale ); + float r1 = b3Length( b3Sub( aabb.lowerBound, localCenter ) ); + float r2 = b3Length( b3Sub( aabb.upperBound, localCenter ) ); + extent.minExtent = b3MinFloat( r1, r2 ); + b3Vec3 p = b3FarthestPointOnAABB( aabb, localCenter ); + extent.maxExtent = b3Abs( p ); + } + break; + + default: + break; + } + + return extent; +} + +b3CastOutput b3RayCastShape( const b3Shape* shape, b3Transform transform, const b3RayCastInput* input ) +{ + b3RayCastInput localInput = *input; + localInput.origin = b3InvTransformPoint( transform, input->origin ); + localInput.translation = b3InvRotateVector( transform.q, input->translation ); + + b3CastOutput output = { 0 }; + switch ( shape->type ) + { + case b3_capsuleShape: + output = b3RayCastCapsule( &shape->capsule, &localInput ); + break; + case b3_compoundShape: + output = b3RayCastCompound( shape->compound, &localInput ); + break; + case b3_sphereShape: + output = b3RayCastSphere( &shape->sphere, &localInput ); + break; + case b3_hullShape: + output = b3RayCastHull( shape->hull, &localInput ); + break; + case b3_meshShape: + output = b3RayCastMesh( &shape->mesh, &localInput ); + break; + case b3_heightShape: + output = b3RayCastHeightField( shape->heightField, &localInput ); + break; + default: + return output; + } + + output.point = b3TransformPoint( transform, output.point ); + output.normal = b3RotateVector( transform.q, output.normal ); + return output; +} + +b3CastOutput b3ShapeCastShape( const b3Shape* shape, b3Transform transform, const b3ShapeCastInput* input ) +{ + b3ShapeCastInput localInput = *input; + b3Vec3 localPoints[B3_MAX_SHAPE_CAST_POINTS]; + + localInput.proxy.count = b3MinInt( input->proxy.count, B3_MAX_SHAPE_CAST_POINTS ); + for ( int i = 0; i < localInput.proxy.count; ++i ) + { + localPoints[i] = b3InvTransformPoint( transform, input->proxy.points[i] ); + } + + localInput.proxy.points = localPoints; + localInput.translation = b3InvRotateVector( transform.q, input->translation ); + + b3CastOutput output = { 0 }; + switch ( shape->type ) + { + case b3_capsuleShape: + output = b3ShapeCastCapsule( &shape->capsule, &localInput ); + break; + + case b3_compoundShape: + output = b3ShapeCastCompound( shape->compound, &localInput ); + break; + + case b3_heightShape: + output = b3ShapeCastHeightField( shape->heightField, &localInput ); + break; + + case b3_hullShape: + output = b3ShapeCastHull( shape->hull, &localInput ); + break; + + case b3_meshShape: + output = b3ShapeCastMesh( &shape->mesh, &localInput ); + break; + + case b3_sphereShape: + output = b3ShapeCastSphere( &shape->sphere, &localInput ); + break; + default: + return output; + } + + output.point = b3TransformPoint( transform, output.point ); + output.normal = b3RotateVector( transform.q, output.normal ); + return output; +} + +bool b3OverlapShape( const b3Shape* shape, b3Transform transform, const b3ShapeProxy* proxy ) +{ + b3ShapeType type = shape->type; + switch ( type ) + { + case b3_capsuleShape: + return b3OverlapCapsule( &shape->capsule, transform, proxy ); + + case b3_compoundShape: + return b3OverlapCompound( shape->compound, transform, proxy ); + + case b3_heightShape: + return b3OverlapHeightField( shape->heightField, transform, proxy ); + + case b3_hullShape: + return b3OverlapHull( shape->hull, transform, proxy ); + + case b3_meshShape: + return b3OverlapMesh( &shape->mesh, transform, proxy ); + + case b3_sphereShape: + return b3OverlapSphere( &shape->sphere, transform, proxy ); + + default: + B3_ASSERT( false ); + return false; + } + +#if 0 + b3Vec3 localPoints[B3_MAX_SHAPE_CAST_POINTS]; + b3ShapeProxy localProxy; + + b3Transform invTransform = b3InvertTransform( transform ); + b3Matrix3 R = b3MakeMatrixFromQuat( invTransform.q ); + + localProxy.count = b3MinInt( proxy->count, B3_MAX_SHAPE_CAST_POINTS ); + for ( int i = 0; i < localProxy.count; ++i ) + { + localPoints[i] = b3Add( b3MulMV( R, proxy->points[i] ), invTransform.p ); + } + + localProxy.points = localPoints; + localProxy.radius = proxy->radius; + + if ( type == b3_meshShape ) + { + return b3OverlapMesh( &localProxy, shape->mesh.data, shape->mesh.scale ); + } + + B3_ASSERT( type == b3_heightShape ); + + return b3OverlapHeightField( &localProxy, shape->heightField ); +#endif +} + +int b3CollideMover( b3PlaneResult* planes, int planeCapacity, const b3Shape* shape, b3Transform transform, + const b3Capsule* mover ) +{ + if ( planeCapacity == 0 ) + { + return 0; + } + + b3Capsule localMover; + localMover.center1 = b3InvTransformPoint( transform, mover->center1 ); + localMover.center2 = b3InvTransformPoint( transform, mover->center2 ); + localMover.radius = mover->radius; + + int planeCount = 0; + switch ( shape->type ) + { + case b3_capsuleShape: + planeCount = b3CollideMoverAndCapsule( planes, &shape->capsule, &localMover ); + break; + + case b3_compoundShape: + planeCount = b3CollideMoverAndCompound( planes, planeCapacity, shape->compound, &localMover ); + break; + + case b3_sphereShape: + planeCount = b3CollideMoverAndSphere( planes, &shape->sphere, &localMover ); + break; + + case b3_hullShape: + planeCount = b3CollideMoverAndHull( planes, shape->hull, &localMover ); + break; + + case b3_meshShape: + planeCount = b3CollideMoverAndMesh( planes, planeCapacity, &shape->mesh, &localMover ); + break; + + case b3_heightShape: + planeCount = b3CollideMoverAndHeightField( planes, planeCapacity, shape->heightField, &localMover ); + break; + + default: + B3_ASSERT( false ); + break; + } + + for ( int i = 0; i < planeCount; ++i ) + { + planes[i].plane.normal = b3RotateVector( transform.q, planes[i].plane.normal ); + planes[i].point = b3TransformPoint( transform, planes[i].point ); + } + + return planeCount; +} + +void b3CreateShapeProxy( b3Shape* shape, b3BroadPhase* bp, b3BodyType type, b3WorldTransform transform, bool forcePairCreation ) +{ + B3_ASSERT( shape->proxyKey == B3_NULL_INDEX ); + + b3UpdateShapeAABBs( shape, transform, type ); + + // Create proxies in the broad-phase. + shape->proxyKey = + b3BroadPhase_CreateProxy( bp, type, shape->fatAABB, shape->filter.categoryBits, shape->id, forcePairCreation ); + B3_ASSERT( B3_PROXY_TYPE( shape->proxyKey ) < b3_bodyTypeCount ); +} + +void b3DestroyShapeProxy( b3Shape* shape, b3BroadPhase* bp ) +{ + if ( shape->proxyKey != B3_NULL_INDEX ) + { + b3BroadPhase_DestroyProxy( bp, shape->proxyKey ); + shape->proxyKey = B3_NULL_INDEX; + } +} + +static void b3DestroyShapeAllocationForShapeChange( b3World* world, b3Shape* shape ) +{ + b3ShapeType type = shape->type; + switch ( type ) + { + case b3_hullShape: + b3RemoveHullFromDatabase( world, shape->hull ); + shape->hull = NULL; + break; + + default: + break; + } + + if ( shape->userShape != NULL ) + { + world->destroyDebugShape( shape->userShape, world->userDebugShapeContext ); + shape->userShape = NULL; + } +} + +void b3DestroyShapeAllocations( b3World* world, b3Shape* shape ) +{ + b3DestroyShapeAllocationForShapeChange( world, shape ); + + if ( shape->materials != NULL ) + { + B3_ASSERT( shape->materialCount > 0 ); + b3Free( shape->materials, shape->materialCount * sizeof( b3SurfaceMaterial ) ); + shape->materials = NULL; + shape->materialCount = 0; + } + + // Name is stored inline. Sensor data is destroyed elsewhere +} + +b3ShapeProxy b3MakeShapeProxy( const b3Shape* shape ) +{ + switch ( shape->type ) + { + case b3_capsuleShape: + return (b3ShapeProxy){ &shape->capsule.center1, 2, shape->capsule.radius }; + + case b3_sphereShape: + return (b3ShapeProxy){ &shape->sphere.center, 1, shape->sphere.radius }; + + case b3_hullShape: + { + const b3HullData* hull = shape->hull; + const b3Vec3* points = b3GetHullPoints( hull ); + return (b3ShapeProxy){ points, hull->vertexCount, 0.0f }; + } + + default: + { + B3_ASSERT( false ); + return (b3ShapeProxy){ 0 }; + } + } +} + +b3ShapeProxy b3MakeLocalProxy( const b3ShapeProxy* proxy, b3Transform transform, b3Vec3* buffer ) +{ + b3Transform invTransform = b3InvertTransform( transform ); + b3Matrix3 R = b3MakeMatrixFromQuat( invTransform.q ); + + int count = b3MinInt( proxy->count, B3_MAX_SHAPE_CAST_POINTS ); + for ( int i = 0; i < count; ++i ) + { + buffer[i] = b3Add( b3MulMV( R, proxy->points[i] ), invTransform.p ); + } + + return (b3ShapeProxy){ + .points = buffer, + .count = count, + .radius = proxy->radius, + }; +} + +b3AABB b3ComputeProxyAABB( const b3ShapeProxy* proxy ) +{ + const b3Vec3* points = proxy->points; + b3AABB aabb = { + .lowerBound = points[0], + .upperBound = points[0], + }; + + for ( int i = 1; i < proxy->count; ++i ) + { + aabb.lowerBound = b3Min( aabb.lowerBound, points[i] ); + aabb.upperBound = b3Max( aabb.upperBound, points[i] ); + } + + b3Vec3 r = { proxy->radius, proxy->radius, proxy->radius }; + aabb.lowerBound = b3Sub( aabb.lowerBound, r ); + aabb.upperBound = b3Add( aabb.upperBound, r ); + return aabb; +} + +b3BodyId b3Shape_GetBody( b3ShapeId shapeId ) +{ + b3World* world = b3GetWorld( shapeId.world0 ); + b3Shape* shape = b3GetShape( world, shapeId ); + return b3MakeBodyId( world, shape->bodyId ); +} + +b3WorldId b3Shape_GetWorld( b3ShapeId shapeId ) +{ + b3World* world = b3GetWorld( shapeId.world0 ); + return (b3WorldId){ (uint16_t)( shapeId.world0 + 1 ), world->generation }; +} + +void b3Shape_SetUserData( b3ShapeId shapeId, void* userData ) +{ + b3World* world = b3GetWorld( shapeId.world0 ); + b3Shape* shape = b3GetShape( world, shapeId ); + shape->userData = userData; +} + +void* b3Shape_GetUserData( b3ShapeId shapeId ) +{ + b3World* world = b3GetWorld( shapeId.world0 ); + b3Shape* shape = b3GetShape( world, shapeId ); + return shape->userData; +} + +void b3Shape_SetName( b3ShapeId shapeId, const char* name ) +{ + b3World* world = b3GetWorld( shapeId.world0 ); + + B3_REC( world, ShapeSetName, shapeId, name ); + + b3Shape* shape = b3GetShape( world, shapeId ); + shape->nameId = b3AddName( &world->names, name ); +} + +const char* b3Shape_GetName( b3ShapeId shapeId ) +{ + b3World* world = b3GetWorld( shapeId.world0 ); + b3Shape* shape = b3GetShape( world, shapeId ); + return b3FindNameWithDefault( &world->names, shape->nameId, "" ); +} + +bool b3Shape_IsSensor( b3ShapeId shapeId ) +{ + b3World* world = b3GetWorld( shapeId.world0 ); + b3Shape* shape = b3GetShape( world, shapeId ); + return shape->sensorIndex != B3_NULL_INDEX; +} + +// todo no tests +b3WorldCastOutput b3Shape_RayCast( b3ShapeId shapeId, b3Pos origin, b3Vec3 translation ) +{ + B3_ASSERT( b3IsValidPosition( origin ) ); + B3_ASSERT( b3IsValidVec3( translation ) ); + + b3World* world = b3GetWorld( shapeId.world0 ); + b3Shape* shape = b3GetShape( world, shapeId ); + + // Re-center on the origin so the cast runs in float precision far from the world origin + b3Transform transform = b3ToRelativeTransform( b3GetBodyTransform( world, shape->bodyId ), origin ); + + // The ray starts at the origin, so its origin in the re-centered frame is zero + b3RayCastInput input = { b3Vec3_zero, translation, 1.0f }; + + // Lift the re-centered float result back to a world position + b3CastOutput local = b3RayCastShape( shape, transform, &input ); + b3WorldCastOutput output; + output.normal = local.normal; + output.point = b3OffsetPos( origin, local.point ); + output.fraction = local.fraction; + output.iterations = local.iterations; + output.triangleIndex = local.triangleIndex; + output.childIndex = local.childIndex; + output.materialIndex = local.materialIndex; + output.hit = local.hit; + + return output; +} + +void b3Shape_SetDensity( b3ShapeId shapeId, float density, bool updateBodyMass ) +{ + B3_ASSERT( b3IsValidFloat( density ) && density >= 0.0f ); + + b3World* world = b3GetUnlockedWorld( shapeId.world0 ); + if ( world == NULL ) + { + return; + } + + B3_REC( world, ShapeSetDensity, shapeId, density, updateBodyMass ); + + b3Shape* shape = b3GetShape( world, shapeId ); + if ( density == shape->density ) + { + // early return to avoid expensive function + return; + } + + shape->density = density; + + if ( updateBodyMass == true ) + { + b3Body* body = b3Array_Get( world->bodies, shape->bodyId ); + b3UpdateBodyMassData( world, body ); + } +} + +float b3Shape_GetDensity( b3ShapeId shapeId ) +{ + b3World* world = b3GetWorld( shapeId.world0 ); + b3Shape* shape = b3GetShape( world, shapeId ); + return shape->density; +} + +void b3Shape_SetFriction( b3ShapeId shapeId, float friction ) +{ + B3_ASSERT( b3IsValidFloat( friction ) && friction >= 0.0f ); + b3World* world = b3GetWorld( shapeId.world0 ); + B3_REC( world, ShapeSetFriction, shapeId, friction ); + b3Shape* shape = b3GetShape( world, shapeId ); + B3_ASSERT( shape->type != b3_compoundShape ); + b3GetShapeMaterials( shape )[0].friction = friction; +} + +float b3Shape_GetFriction( b3ShapeId shapeId ) +{ + b3World* world = b3GetWorld( shapeId.world0 ); + b3Shape* shape = b3GetShape( world, shapeId ); + return b3GetShapeMaterials( shape )[0].friction; +} + +void b3Shape_SetRestitution( b3ShapeId shapeId, float restitution ) +{ + B3_ASSERT( b3IsValidFloat( restitution ) && restitution >= 0.0f ); + b3World* world = b3GetWorld( shapeId.world0 ); + B3_REC( world, ShapeSetRestitution, shapeId, restitution ); + b3Shape* shape = b3GetShape( world, shapeId ); + B3_ASSERT( shape->type != b3_compoundShape ); + b3GetShapeMaterials( shape )[0].restitution = restitution; +} + +float b3Shape_GetRestitution( b3ShapeId shapeId ) +{ + b3World* world = b3GetWorld( shapeId.world0 ); + b3Shape* shape = b3GetShape( world, shapeId ); + return b3GetShapeMaterials( shape )[0].restitution; +} + +void b3Shape_SetSurfaceMaterial( b3ShapeId shapeId, b3SurfaceMaterial surfaceMaterial ) +{ + B3_ASSERT( b3IsValidFloat( surfaceMaterial.friction ) && surfaceMaterial.friction >= 0.0f ); + B3_ASSERT( b3IsValidFloat( surfaceMaterial.restitution ) && surfaceMaterial.restitution >= 0.0f ); + B3_ASSERT( b3IsValidFloat( surfaceMaterial.rollingResistance ) && surfaceMaterial.rollingResistance >= 0.0f ); + B3_ASSERT( b3IsValidVec3( surfaceMaterial.tangentVelocity ) ); + + b3World* world = b3GetWorld( shapeId.world0 ); + B3_REC( world, ShapeSetSurfaceMaterial, shapeId, surfaceMaterial ); + b3Shape* shape = b3GetShape( world, shapeId ); + B3_ASSERT( shape->type != b3_compoundShape ); + b3GetShapeMaterials( shape )[0] = surfaceMaterial; +} + +b3SurfaceMaterial b3Shape_GetSurfaceMaterial( b3ShapeId shapeId ) +{ + b3World* world = b3GetWorld( shapeId.world0 ); + b3Shape* shape = b3GetShape( world, shapeId ); + return b3GetShapeMaterials( shape )[0]; +} + +int b3Shape_GetMeshMaterialCount( b3ShapeId shapeId ) +{ + b3World* world = b3GetWorld( shapeId.world0 ); + b3Shape* shape = b3GetShape( world, shapeId ); + return shape->materialCount; +} + +void b3Shape_SetMeshMaterial( b3ShapeId shapeId, b3SurfaceMaterial surfaceMaterial, int index ) +{ + B3_ASSERT( b3IsValidFloat( surfaceMaterial.friction ) && surfaceMaterial.friction >= 0.0f ); + B3_ASSERT( b3IsValidFloat( surfaceMaterial.restitution ) && surfaceMaterial.restitution >= 0.0f ); + B3_ASSERT( b3IsValidFloat( surfaceMaterial.rollingResistance ) && surfaceMaterial.rollingResistance >= 0.0f ); + B3_ASSERT( b3IsValidVec3( surfaceMaterial.tangentVelocity ) ); + + b3World* world = b3GetWorld( shapeId.world0 ); + b3Shape* shape = b3GetShape( world, shapeId ); + + B3_ASSERT( 0 <= index && index < shape->materialCount ); + B3_ASSERT( shape->type != b3_compoundShape ); + b3GetShapeMaterials( shape )[index] = surfaceMaterial; +} + +b3SurfaceMaterial b3Shape_GetMeshSurfaceMaterial( b3ShapeId shapeId, int index ) +{ + b3World* world = b3GetWorld( shapeId.world0 ); + b3Shape* shape = b3GetShape( world, shapeId ); + B3_ASSERT( 0 <= index && index < shape->materialCount ); + return b3GetShapeMaterials( shape )[index]; +} + +b3Filter b3Shape_GetFilter( b3ShapeId shapeId ) +{ + b3World* world = b3GetWorld( shapeId.world0 ); + b3Shape* shape = b3GetShape( world, shapeId ); + return shape->filter; +} + +static void b3ResetProxy( b3World* world, b3Shape* shape, bool wakeBodies, bool destroyProxy ) +{ + b3Body* body = b3Array_Get( world->bodies, shape->bodyId ); + + int shapeId = shape->id; + + // destroy all contacts associated with this shape + int contactKey = body->headContactKey; + while ( contactKey != B3_NULL_INDEX ) + { + int contactId = contactKey >> 1; + int edgeIndex = contactKey & 1; + + b3Contact* contact = b3Array_Get( world->contacts, contactId ); + contactKey = contact->edges[edgeIndex].nextKey; + + if ( contact->shapeIdA == shapeId || contact->shapeIdB == shapeId ) + { + b3DestroyContact( world, contact, wakeBodies ); + } + } + + b3WorldTransform transform = b3GetBodyTransformQuick( world, body ); + if ( shape->proxyKey != B3_NULL_INDEX ) + { + b3BodyType proxyType = B3_PROXY_TYPE( shape->proxyKey ); + b3UpdateShapeAABBs( shape, transform, proxyType ); + + if ( destroyProxy ) + { + b3BroadPhase_DestroyProxy( &world->broadPhase, shape->proxyKey ); + + bool forcePairCreation = true; + shape->proxyKey = b3BroadPhase_CreateProxy( &world->broadPhase, proxyType, shape->fatAABB, shape->filter.categoryBits, + shapeId, forcePairCreation ); + } + else + { + b3BroadPhase_MoveProxy( &world->broadPhase, shape->proxyKey, shape->fatAABB ); + } + } + else + { + b3BodyType proxyType = body->type; + b3UpdateShapeAABBs( shape, transform, proxyType ); + } + + b3ValidateSolverSets( world ); +} + +void b3Shape_SetFilter( b3ShapeId shapeId, b3Filter filter, bool invokeContacts ) +{ + b3World* world = b3GetUnlockedWorld( shapeId.world0 ); + if ( world == NULL ) + { + return; + } + + B3_REC( world, ShapeSetFilter, shapeId, filter, invokeContacts ); + + b3Shape* shape = b3GetShape( world, shapeId ); + if ( filter.maskBits == shape->filter.maskBits && filter.categoryBits == shape->filter.categoryBits && + filter.groupIndex == shape->filter.groupIndex ) + { + return; + } + + shape->filter = filter; + + if ( invokeContacts ) + { + world->locked = true; + bool wakeBodies = true; + + // If the category bits change, I need to destroy the proxy because it affects the tree sorting. + bool destroyProxy = filter.categoryBits == shape->filter.categoryBits; + + // need to wake bodies because a filter change may destroy contacts + b3ResetProxy( world, shape, wakeBodies, destroyProxy ); + world->locked = false; + } + + // note: this does not immediately update sensor overlaps. Instead sensor + // overlaps are updated the next time step +} + +void b3Shape_EnableSensorEvents( b3ShapeId shapeId, bool flag ) +{ + b3World* world = b3GetUnlockedWorld( shapeId.world0 ); + if ( world == NULL ) + { + return; + } + + B3_REC( world, ShapeEnableSensorEvents, shapeId, flag ); + + b3Shape* shape = b3GetShape( world, shapeId ); + shape->flags = flag ? shape->flags | b3_enableSensorEvents : shape->flags & ~b3_enableSensorEvents; +} + +bool b3Shape_AreSensorEventsEnabled( b3ShapeId shapeId ) +{ + b3World* world = b3GetWorld( shapeId.world0 ); + b3Shape* shape = b3GetShape( world, shapeId ); + return shape->flags & b3_enableSensorEvents; +} + +void b3Shape_EnableContactEvents( b3ShapeId shapeId, bool flag ) +{ + b3World* world = b3GetUnlockedWorld( shapeId.world0 ); + if ( world == NULL ) + { + return; + } + + B3_REC( world, ShapeEnableContactEvents, shapeId, flag ); + + b3Shape* shape = b3GetShape( world, shapeId ); + shape->flags = flag ? shape->flags | b3_enableContactEvents : shape->flags & ~b3_enableContactEvents; +} + +bool b3Shape_AreContactEventsEnabled( b3ShapeId shapeId ) +{ + b3World* world = b3GetWorld( shapeId.world0 ); + b3Shape* shape = b3GetShape( world, shapeId ); + return shape->flags & b3_enableContactEvents; +} + +void b3Shape_EnablePreSolveEvents( b3ShapeId shapeId, bool flag ) +{ + b3World* world = b3GetUnlockedWorld( shapeId.world0 ); + if ( world == NULL ) + { + return; + } + + B3_REC( world, ShapeEnablePreSolveEvents, shapeId, flag ); + + b3Shape* shape = b3GetShape( world, shapeId ); + shape->flags = flag ? shape->flags | b3_enablePreSolveEvents : shape->flags & ~b3_enablePreSolveEvents; +} + +bool b3Shape_ArePreSolveEventsEnabled( b3ShapeId shapeId ) +{ + b3World* world = b3GetWorld( shapeId.world0 ); + b3Shape* shape = b3GetShape( world, shapeId ); + return shape->flags & b3_enablePreSolveEvents; +} + +void b3Shape_EnableHitEvents( b3ShapeId shapeId, bool flag ) +{ + b3World* world = b3GetUnlockedWorld( shapeId.world0 ); + if ( world == NULL ) + { + return; + } + + B3_REC( world, ShapeEnableHitEvents, shapeId, flag ); + + b3Shape* shape = b3GetShape( world, shapeId ); + shape->flags = flag ? shape->flags | b3_enableHitEvents : shape->flags & ~b3_enableHitEvents; +} + +bool b3Shape_AreHitEventsEnabled( b3ShapeId shapeId ) +{ + b3World* world = b3GetWorld( shapeId.world0 ); + b3Shape* shape = b3GetShape( world, shapeId ); + return shape->flags & b3_enableHitEvents; +} + +b3ShapeType b3Shape_GetType( b3ShapeId shapeId ) +{ + b3World* world = b3GetWorld( shapeId.world0 ); + b3Shape* shape = b3GetShape( world, shapeId ); + return shape->type; +} + +b3Sphere b3Shape_GetSphere( b3ShapeId shapeId ) +{ + b3World* world = b3GetWorld( shapeId.world0 ); + b3Shape* shape = b3GetShape( world, shapeId ); + B3_ASSERT( shape->type == b3_sphereShape ); + return shape->sphere; +} + +b3Capsule b3Shape_GetCapsule( b3ShapeId shapeId ) +{ + b3World* world = b3GetWorld( shapeId.world0 ); + b3Shape* shape = b3GetShape( world, shapeId ); + B3_ASSERT( shape->type == b3_capsuleShape ); + return shape->capsule; +} + +const b3HullData* b3Shape_GetHull( b3ShapeId shapeId ) +{ + b3World* world = b3GetWorld( shapeId.world0 ); + b3Shape* shape = b3GetShape( world, shapeId ); + B3_ASSERT( shape->type == b3_hullShape ); + return shape->hull; +} + +b3Mesh b3Shape_GetMesh( b3ShapeId shapeId ) +{ + b3World* world = b3GetWorld( shapeId.world0 ); + b3Shape* shape = b3GetShape( world, shapeId ); + B3_ASSERT( shape->type == b3_meshShape ); + return shape->mesh; +} + +const b3HeightFieldData* b3Shape_GetHeightField( b3ShapeId shapeId ) +{ + b3World* world = b3GetWorld( shapeId.world0 ); + b3Shape* shape = b3GetShape( world, shapeId ); + B3_ASSERT( shape->type == b3_heightShape ); + return shape->heightField; +} + +void b3Shape_SetSphere( b3ShapeId shapeId, const b3Sphere* sphere ) +{ + b3World* world = b3GetUnlockedWorld( shapeId.world0 ); + if ( world == NULL ) + { + return; + } + + B3_REC( world, ShapeSetSphere, shapeId, *sphere ); + + world->locked = true; + + b3Shape* shape = b3GetShape( world, shapeId ); + + b3DestroyShapeAllocationForShapeChange( world, shape ); + + shape->sphere = *sphere; + shape->type = b3_sphereShape; + shape->aabbMargin = b3ComputeShapeMargin( shape ); + + // need to wake bodies so they can react to the shape change + bool wakeBodies = true; + bool destroyProxy = true; + b3ResetProxy( world, shape, wakeBodies, destroyProxy ); + + world->locked = false; +} + +void b3Shape_SetCapsule( b3ShapeId shapeId, const b3Capsule* capsule ) +{ + b3World* world = b3GetUnlockedWorld( shapeId.world0 ); + if ( world == NULL ) + { + return; + } + + B3_REC( world, ShapeSetCapsule, shapeId, *capsule ); + + world->locked = true; + + b3Shape* shape = b3GetShape( world, shapeId ); + + b3DestroyShapeAllocationForShapeChange( world, shape ); + + shape->capsule = *capsule; + shape->type = b3_capsuleShape; + shape->aabbMargin = b3ComputeShapeMargin( shape ); + + // need to wake bodies so they can react to the shape change + bool wakeBodies = true; + bool destroyProxy = true; + b3ResetProxy( world, shape, wakeBodies, destroyProxy ); + + world->locked = false; +} + +void b3Shape_SetHull( b3ShapeId shapeId, const b3HullData* hull ) +{ + B3_VALIDATE( b3IsValidHull( hull ) ); + B3_VALIDATE( hull->hash != 0 ); + + b3World* world = b3GetUnlockedWorld( shapeId.world0 ); + if ( world == NULL ) + { + return; + } + + world->locked = true; + + b3Shape* shape = b3GetShape( world, shapeId ); + + // Acquire the new hull before releasing the old so the input may safely alias + // the shape's current shared data. + const b3HullData* data = b3AddHullToDatabase( world, hull ); + + // Same shared hull, avoid destroying contacts and recreating the proxy + if ( shape->type == b3_hullShape && data == shape->hull ) + { + b3RemoveHullFromDatabase( world, data ); + world->locked = false; + return; + } + + b3DestroyShapeAllocationForShapeChange( world, shape ); + + shape->hull = data; + shape->type = b3_hullShape; + shape->aabbMargin = b3ComputeShapeMargin( shape ); + + // need to wake bodies so they can react to the shape change + bool wakeBodies = true; + bool destroyProxy = true; + b3ResetProxy( world, shape, wakeBodies, destroyProxy ); + + world->locked = false; +} + +void b3Shape_SetMesh( b3ShapeId shapeId, const b3MeshData* meshData, b3Vec3 scale ) +{ + B3_ASSERT( b3IsValidVec3( scale ) ); + B3_ASSERT( meshData != NULL && b3IsValidMesh( meshData ) ); + + b3World* world = b3GetUnlockedWorld( shapeId.world0 ); + if ( world == NULL ) + { + return; + } + + world->locked = true; + + b3Shape* shape = b3GetShape( world, shapeId ); + + b3DestroyShapeAllocationForShapeChange( world, shape ); + + shape->mesh.data = meshData; + shape->mesh.scale = b3SafeScale( scale ); + shape->type = b3_meshShape; + shape->aabbMargin = b3ComputeShapeMargin( shape ); + + // need to wake bodies so they can react to the shape change + bool wakeBodies = true; + bool destroyProxy = true; + b3ResetProxy( world, shape, wakeBodies, destroyProxy ); + + world->locked = false; +} + +int b3Shape_GetContactCapacity( b3ShapeId shapeId ) +{ + b3World* world = b3GetUnlockedWorld( shapeId.world0 ); + if ( world == NULL ) + { + return 0; + } + + b3Shape* shape = b3GetShape( world, shapeId ); + if ( shape->sensorIndex != B3_NULL_INDEX ) + { + return 0; + } + + b3Body* body = b3Array_Get( world->bodies, shape->bodyId ); + + // Conservative and fast + return body->contactCount; +} + +int b3Shape_GetContactData( b3ShapeId shapeId, b3ContactData* contactData, int capacity ) +{ + b3World* world = b3GetUnlockedWorld( shapeId.world0 ); + if ( world == NULL ) + { + return 0; + } + + b3Shape* shape = b3GetShape( world, shapeId ); + if ( shape->sensorIndex != B3_NULL_INDEX ) + { + return 0; + } + + b3Body* body = b3Array_Get( world->bodies, shape->bodyId ); + int contactKey = body->headContactKey; + int index = 0; + while ( contactKey != B3_NULL_INDEX && index < capacity ) + { + int contactId = contactKey >> 1; + int edgeIndex = contactKey & 1; + + b3Contact* contact = b3Array_Get( world->contacts, contactId ); + + // Does contact involve this shape and is it touching? + if ( ( contact->shapeIdA == shapeId.index1 - 1 || contact->shapeIdB == shapeId.index1 - 1 ) && + ( contact->flags & b3_contactTouchingFlag ) != 0 ) + { + b3Shape* shapeA = world->shapes.data + contact->shapeIdA; + b3Shape* shapeB = world->shapes.data + contact->shapeIdB; + + contactData[index].contactId = (b3ContactId){ contact->contactId + 1, shapeId.world0, 0, contact->generation }; + contactData[index].shapeIdA = (b3ShapeId){ shapeA->id + 1, shapeId.world0, shapeA->generation }; + contactData[index].shapeIdB = (b3ShapeId){ shapeB->id + 1, shapeId.world0, shapeB->generation }; + contactData[index].manifolds = contact->manifolds; + contactData[index].manifoldCount = contact->manifoldCount; + index += 1; + } + + contactKey = contact->edges[edgeIndex].nextKey; + } + + B3_ASSERT( index <= capacity ); + + return index; +} + +int b3Shape_GetSensorCapacity( b3ShapeId shapeId ) +{ + b3World* world = b3GetUnlockedWorld( shapeId.world0 ); + if ( world == NULL ) + { + return 0; + } + + b3Shape* shape = b3GetShape( world, shapeId ); + if ( shape->sensorIndex == B3_NULL_INDEX ) + { + return 0; + } + + b3Sensor* sensor = b3Array_Get( world->sensors, shape->sensorIndex ); + return sensor->overlaps2.count; +} + +int b3Shape_GetSensorData( b3ShapeId shapeId, b3ShapeId* visitorIds, int capacity ) +{ + b3World* world = b3GetUnlockedWorld( shapeId.world0 ); + if ( world == NULL ) + { + return 0; + } + + b3Shape* shape = b3GetShape( world, shapeId ); + if ( shape->sensorIndex == B3_NULL_INDEX ) + { + return 0; + } + + b3Sensor* sensor = b3Array_Get( world->sensors, shape->sensorIndex ); + + int count = b3MinInt( sensor->overlaps2.count, capacity ); + b3Visitor* refs = sensor->overlaps2.data; + for ( int i = 0; i < count; ++i ) + { + b3ShapeId visitorId = { + .index1 = refs[i].shapeId + 1, + .world0 = shapeId.world0, + .generation = refs[i].generation, + }; + + visitorIds[i] = visitorId; + } + + return count; +} + +b3AABB b3Shape_GetAABB( b3ShapeId shapeId ) +{ + b3World* world = b3GetWorld( shapeId.world0 ); + if ( world == NULL ) + { + return (b3AABB){ 0 }; + } + + b3Shape* shape = b3GetShape( world, shapeId ); + return shape->aabb; +} + +b3MassData b3Shape_ComputeMassData( b3ShapeId shapeId ) +{ + b3World* world = b3GetWorld( shapeId.world0 ); + if ( world == NULL ) + { + return (b3MassData){ 0 }; + } + + b3Shape* shape = b3GetShape( world, shapeId ); + return b3ComputeShapeMass( shape ); +} + +b3Vec3 b3Shape_GetClosestPoint( b3ShapeId shapeId, b3Vec3 target ) +{ + b3World* world = b3GetWorld( shapeId.world0 ); + if ( world == NULL ) + { + return b3Vec3_zero; + } + + b3Shape* shape = b3GetShape( world, shapeId ); + b3Body* body = b3Array_Get( world->bodies, shape->bodyId ); + // Low level closest point query is a documented float carve-out far from the origin + b3Transform transform = b3ToRelativeTransform( b3GetBodyTransformQuick( world, body ), b3Pos_zero ); + + b3DistanceInput input; + input.proxyA = b3MakeShapeProxy( shape ); + input.proxyB = (b3ShapeProxy){ &target, 1, 0.0f }; + input.transform = b3InvMulTransforms( transform, b3Transform_identity ); + input.useRadii = true; + + b3SimplexCache cache = { 0 }; + b3DistanceOutput output = b3ShapeDistance( &input, &cache, NULL, 0 ); + + // Witness point comes back in frame A, lift it back to the query frame + return b3TransformPoint( transform, output.pointA ); +} + +#define B3_DEBUG_WIND 0 + +// https://en.wikipedia.org/wiki/Density_of_air +// https://www.engineeringtoolbox.com/wind-load-d_1775.html +// force = 0.5 * air_density * velocity^2 * area +// https://en.wikipedia.org/wiki/Lift_(force) +void b3Shape_ApplyWind( b3ShapeId shapeId, b3Vec3 wind, float drag, float lift, float maxSpeed, bool wake ) +{ + b3World* world = b3GetWorld( shapeId.world0 ); + if ( world == NULL ) + { + return; + } + + B3_REC( world, ShapeApplyWind, shapeId, wind, drag, lift, maxSpeed, wake ); + + b3Shape* shape = b3GetShape( world, shapeId ); + + b3ShapeType shapeType = shape->type; + if ( shapeType != b3_sphereShape && shapeType != b3_capsuleShape && shapeType != b3_hullShape ) + { + return; + } + + b3Body* body = b3Array_Get( world->bodies, shape->bodyId ); + + if ( body->type != b3_dynamicBody ) + { + return; + } + + if ( body->setIndex == b3_disabledSet ) + { + return; + } + + if ( body->setIndex >= b3_firstSleepingSet && wake == false ) + { + return; + } + + b3BodySim* sim = b3GetBodySim( world, body ); + + if ( body->setIndex != b3_awakeSet ) + { + // Must wake for state to exist + b3WakeBodyWithLock( world, body ); + } + + B3_ASSERT( body->setIndex == b3_awakeSet ); + + b3BodyState* state = b3GetBodyState( world, body ); + // Only the rotation is used below, so the demoted world transform is exact + b3Transform transform = b3ToRelativeTransform( sim->transform, b3Pos_zero ); + + float lengthUnits = b3GetLengthUnitsPerMeter(); + float volumeUnits = lengthUnits * lengthUnits * lengthUnits; + + float airDensity = 1.2250f / ( volumeUnits ); + + b3Vec3 force = { 0 }; + b3Vec3 torque = { 0 }; + + switch ( shape->type ) + { + case b3_sphereShape: + { + float radius = shape->sphere.radius; + b3Vec3 centroid = shape->localCentroid; + b3Vec3 lever = b3RotateVector( transform.q, b3Sub( centroid, sim->localCenter ) ); + b3Vec3 shapeVelocity = b3Add( state->linearVelocity, b3Cross( state->angularVelocity, lever ) ); + b3Vec3 relativeVelocity = b3MulSub( wind, drag, shapeVelocity ); + float speed; + b3Vec3 direction = b3GetLengthAndNormalize( &speed, relativeVelocity ); + speed = b3MinFloat( speed, maxSpeed ); + float projectedArea = B3_PI * radius * radius; + force = b3MulSV( 0.5f * airDensity * projectedArea * speed * speed, direction ); + torque = b3Cross( lever, force ); + } + break; + + case b3_capsuleShape: + { + b3Vec3 centroid = shape->localCentroid; + b3Vec3 lever = b3RotateVector( transform.q, b3Sub( centroid, sim->localCenter ) ); + b3Vec3 shapeVelocity = b3Add( state->linearVelocity, b3Cross( state->angularVelocity, lever ) ); + b3Vec3 relativeVelocity = b3MulSub( wind, drag, shapeVelocity ); + float speed; + b3Vec3 direction = b3GetLengthAndNormalize( &speed, relativeVelocity ); + speed = b3MinFloat( speed, maxSpeed ); + + b3Vec3 d = b3Sub( shape->capsule.center2, shape->capsule.center1 ); + d = b3RotateVector( transform.q, d ); + + float radius = shape->capsule.radius; + float projectedArea = B3_PI * radius * radius + 2.0f * radius * b3Length( b3Cross( d, direction ) ); + + // Normal that opposes the wind + b3Vec3 e = b3Normalize( d ); + b3Vec3 normal = b3Sub( b3MulSV( b3Dot( direction, e ), e ), direction ); + + // portion of wind that is perpendicular to surface + b3Vec3 liftDirection = b3Cross( b3Cross( normal, direction ), direction ); + + float forceMagnitude = 0.5f * airDensity * projectedArea * speed * speed; + force = b3MulSV( forceMagnitude, b3MulAdd( direction, lift, liftDirection ) ); + + b3Vec3 edgeLever = b3MulAdd( lever, radius, normal ); + torque = b3Cross( edgeLever, force ); + } + break; + + case b3_hullShape: + { + b3Matrix3 matrix = b3MakeMatrixFromQuat( transform.q ); + + int faceCount = shape->hull->faceCount; + const b3Vec3* points = b3GetHullPoints( shape->hull ); + const b3HullFace* faces = b3GetHullFaces( shape->hull ); + const b3HullHalfEdge* edges = b3GetHullEdges( shape->hull ); + const b3Plane* planes = b3GetHullPlanes( shape->hull ); + + b3Vec3 linearVelocity = state->linearVelocity; + b3Vec3 angularVelocity = state->angularVelocity; + b3Vec3 localCenterOfMass = sim->localCenter; + + for ( int i = 0; i < faceCount; ++i ) + { + const b3HullFace* face = faces + i; + const b3HullHalfEdge* edge1 = edges + face->edge; + const b3HullHalfEdge* edge2 = edges + edge1->next; + const b3HullHalfEdge* edge3 = edges + edge2->next; + + B3_ASSERT( edge1 != edge3 ); + B3_ASSERT( edge1->origin < shape->hull->vertexCount ); + B3_ASSERT( edge2->origin < shape->hull->vertexCount ); + + b3Vec3 localPoint1 = points[edge1->origin]; + b3Vec3 localPoint2 = points[edge2->origin]; + b3Vec3 v1 = b3MulMV( matrix, localPoint1 ); + b3Vec3 v2 = b3MulMV( matrix, localPoint2 ); + b3Vec3 normal = b3MulMV( matrix, planes[i].normal ); + + do + { + B3_ASSERT( edge3->origin < shape->hull->vertexCount ); + b3Vec3 localPoint3 = points[edge3->origin]; + b3Vec3 v3 = b3MulMV( matrix, localPoint3 ); + + // Triangle center + b3Vec3 localCenter = b3MulSV( 0.333333f, b3Add( localPoint1, b3Add( localPoint2, localPoint3 ) ) ); + + // Lever arm from center of mass to triangle center in world space + b3Vec3 lever = b3MulMV( matrix, b3Sub( localCenter, localCenterOfMass ) ); + + // Velocity of the triangle center in world space + b3Vec3 centerVelocity = b3Add( linearVelocity, b3Cross( angularVelocity, lever ) ); + + b3Vec3 relativeVelocity = b3MulSub( wind, drag, centerVelocity ); + float speed; + b3Vec3 direction = b3GetLengthAndNormalize( &speed, relativeVelocity ); + + // Check for back-side + if ( b3Dot( normal, direction ) < -FLT_EPSILON ) + { + float projectedArea = -0.5f * b3Dot( b3Cross( b3Sub( v2, v1 ), b3Sub( v3, v1 ) ), direction ); + B3_VALIDATE( projectedArea >= -FLT_EPSILON ); + + b3Vec3 liftDirection = b3Cross( b3Cross( normal, direction ), direction ); + + speed = b3MinFloat( speed, maxSpeed ); + + float forceMagnitude = 0.5f * airDensity * projectedArea * speed * speed; + b3Vec3 deltaForce = b3MulSV( forceMagnitude, b3MulAdd( direction, lift, liftDirection ) ); + b3Vec3 deltaTorque = b3Cross( lever, deltaForce ); + + force = b3Add( force, deltaForce ); + torque = b3Add( torque, deltaTorque ); + +#if B3_DEBUG_WIND + int lineIndex = world->taskContexts.data[0].lineCount; + if ( lineIndex < B3_DEBUG_LINE_CAPACITY ) + { + b3DebugLine* line = world->taskContexts.data[0].lines + lineIndex; + line->p1 = b3OffsetPos( sim->transform.p, b3MulMV( matrix, localCenter ) ); + line->p2 = b3OffsetPos( line->p1, deltaForce ); + line->label = i; + line->color = b3_colorBlanchedAlmond; + world->taskContexts.data[0].lineCount += 1; + } +#endif + } + + edge2 = edge3; + edge3 = edges + edge3->next; + v2 = v3; + localPoint2 = localPoint3; + } + while ( edge1 != edge3 ); + } + } + break; + + default: + break; + } + + sim->force = b3Add( sim->force, force ); + sim->torque = b3Add( sim->torque, torque ); +} + +typedef struct b3MeshImpactContext +{ + b3TOIInput toiInput; + b3TOIOutput toiOutput; + // Centroid of shape in body B local space + b3Vec3 localCentroidB; + // Centroid of shape at beginning and end of sweep in mesh local space. Used for early out. + b3Vec3 meshLocalCentroidB1, meshLocalCentroidB2; + float fallbackRadius; + bool isSensor; + + int visitCount; +} b3MeshImpactContext; + +static bool b3MeshTimeOfImpactFcn( b3Vec3 a, b3Vec3 b, b3Vec3 c, int triangleIndex, void* context ) +{ + B3_UNUSED( triangleIndex ); + + b3MeshImpactContext* toiContext = context; + + toiContext->visitCount += 1; + + // Early out for parallel movement + b3Vec3 c1 = toiContext->meshLocalCentroidB1; + b3Vec3 c2 = toiContext->meshLocalCentroidB2; + + b3Vec3 n = b3Normalize( b3Cross( b3Sub( b, a ), b3Sub( c, a ) ) ); + float offset1 = b3Dot( n, b3Sub( c1, a ) ); + float offset2 = b3Dot( n, b3Sub( c2, a ) ); + + if ( offset1 < 0.0f ) + { + // Started behind or finished in front + return true; + } + + if ( toiContext->isSensor == false && offset1 - offset2 < toiContext->fallbackRadius && offset2 > toiContext->fallbackRadius ) + { + // Finished in front + return true; + } + + b3Vec3 triangle[3] = { a, b, c }; + toiContext->toiInput.proxyA.points = triangle; + toiContext->toiInput.proxyA.count = 3; + + b3TOIOutput output = b3TimeOfImpact( &toiContext->toiInput ); + + // It is possible for a hit at fraction == 0 + + if ( 0.0f < output.fraction && output.fraction < toiContext->toiInput.maxFraction ) + { + toiContext->toiOutput = output; + toiContext->toiInput.maxFraction = output.fraction; + } + else if ( 0.0f == output.fraction ) + { + // fallback to TOI of a small circle around the fast shape centroid + b3TOIInput fallbackInput = toiContext->toiInput; + fallbackInput.proxyB = (b3ShapeProxy){ &toiContext->localCentroidB, 1, toiContext->fallbackRadius + B3_LINEAR_SLOP }; + output = b3TimeOfImpact( &fallbackInput ); + + if ( 0.0f < output.fraction && output.fraction < toiContext->toiInput.maxFraction ) + { + toiContext->toiOutput = output; + toiContext->toiInput.maxFraction = output.fraction; + toiContext->toiOutput.usedFallback = true; + } + } + + // Continue the query + return true; +} + +typedef struct b3CompoundImpactContext +{ + b3TOIInput toiInput; + b3TOIOutput toiOutput; + b3Transform compoundTransform; + + // Bounds local to compound + b3AABB localSweepBoundsB; + + // Centroid of shape in body B local space + b3Vec3 localCentroidB; + float fallbackRadius; +} b3CompoundImpactContext; + +// Implements b3CompoundQueryFcn +static bool b3CompoundTimeOfImpactFcn( const b3CompoundData* compound, int childIndex, void* context ) +{ + b3CompoundImpactContext* toiContext = (b3CompoundImpactContext*)context; + + b3ChildShape child = b3GetCompoundChild( compound, childIndex ); + + b3TOIOutput output = { 0 }; + toiContext->toiInput.sweepA = b3MakeCompoundChildSweep( toiContext->compoundTransform, child.transform ); + + switch ( child.type ) + { + case b3_capsuleShape: + { + toiContext->toiInput.proxyA.points = &child.capsule.center1; + toiContext->toiInput.proxyA.count = 2; + toiContext->toiInput.proxyA.radius = child.capsule.radius; + output = b3TimeOfImpact( &toiContext->toiInput ); + } + break; + + case b3_hullShape: + { + toiContext->toiInput.proxyA.points = b3GetHullPoints( child.hull ); + toiContext->toiInput.proxyA.count = child.hull->vertexCount; + toiContext->toiInput.proxyA.radius = 0.0f; + output = b3TimeOfImpact( &toiContext->toiInput ); + } + break; + + case b3_meshShape: + { + b3MeshImpactContext meshContext = { 0 }; + meshContext.toiInput = toiContext->toiInput; + meshContext.isSensor = false; + meshContext.localCentroidB = toiContext->localCentroidB; + meshContext.fallbackRadius = toiContext->fallbackRadius; + + b3Transform meshWorldTransform = b3MulTransforms( toiContext->compoundTransform, child.transform ); + + const b3Sweep* sweepB = &toiContext->toiInput.sweepB; + b3Transform xfB1 = { + .p = b3Sub( sweepB->c1, b3RotateVector( sweepB->q1, sweepB->localCenter ) ), + .q = sweepB->q1, + }; + + b3Transform xfB2 = { + .p = b3Sub( sweepB->c2, b3RotateVector( sweepB->q2, sweepB->localCenter ) ), + .q = sweepB->q2, + }; + + meshContext.meshLocalCentroidB1 = + b3InvTransformPoint( meshWorldTransform, b3TransformPoint( xfB1, meshContext.localCentroidB ) ); + meshContext.meshLocalCentroidB2 = + b3InvTransformPoint( meshWorldTransform, b3TransformPoint( xfB2, meshContext.localCentroidB ) ); + + // Bounds local to mesh + b3AABB localBounds = b3AABB_Transform( b3InvertTransform( child.transform ), toiContext->localSweepBoundsB ); + + b3QueryMesh( &child.mesh, localBounds, b3MeshTimeOfImpactFcn, &meshContext ); + + output = meshContext.toiOutput; + } + break; + + case b3_sphereShape: + { + toiContext->toiInput.proxyA.points = &child.sphere.center; + toiContext->toiInput.proxyA.count = 1; + toiContext->toiInput.proxyA.radius = child.sphere.radius; + output = b3TimeOfImpact( &toiContext->toiInput ); + } + break; + + default: + B3_ASSERT( false ); + break; + } + + if ( 0.0f < output.fraction && output.fraction < toiContext->toiInput.maxFraction ) + { + toiContext->toiOutput = output; + toiContext->toiInput.maxFraction = output.fraction; + } + + // Clear this to be safe + toiContext->toiInput.proxyA = (b3ShapeProxy){ 0 }; + + // Continue the query + return true; +} + +b3TOIOutput b3ShapeTimeOfImpact( b3Shape* shapeA, b3Shape* shapeB, b3Sweep* sweepA, b3Sweep* sweepB, float maxFraction ) +{ + bool isSensor = shapeA->sensorIndex != B3_NULL_INDEX; + + b3ShapeType typeA = shapeA->type; + if ( typeA == b3_compoundShape ) + { + // todo implement b3CompoundTimeOfImpact + b3CompoundImpactContext context = { 0 }; + context.toiInput.proxyB = b3MakeShapeProxy( shapeB ); + context.toiInput.sweepB = *sweepB; + context.toiInput.maxFraction = maxFraction; + + context.compoundTransform = (b3Transform){ + .p = sweepA->c1, + .q = sweepA->q1, + }; + + b3Vec3 localCentroidB = b3GetShapeCentroid( shapeB ); + context.localCentroidB = localCentroidB; + + b3ShapeExtent extents = b3ComputeShapeExtent( shapeB, context.localCentroidB ); + context.fallbackRadius = b3MaxFloat( 0.75f * extents.minExtent, B3_SPECULATIVE_DISTANCE ); + + // Swept bounds of shapeB + b3AABB bounds = b3ComputeSweptShapeAABB( shapeB, sweepB, maxFraction ); + + // Bounds local to mesh + b3AABB localBounds = b3AABB_Transform( b3InvertTransform( context.compoundTransform ), bounds ); + context.localSweepBoundsB = localBounds; + + b3QueryCompound( shapeA->compound, localBounds, b3CompoundTimeOfImpactFcn, &context ); + + return context.toiOutput; + } + + if ( typeA == b3_heightShape || typeA == b3_meshShape ) + { + // todo implement b3MeshTimeOfImpact and b3HeightFieldTimeOfImpact + // Note: assuming mesh is static + + uint64_t ticks = b3GetTicks(); + + b3MeshImpactContext context = { 0 }; + context.toiInput.sweepA = *sweepA; + context.toiInput.proxyA.count = 3; + context.toiInput.proxyB = b3MakeShapeProxy( shapeB ); + context.toiInput.sweepB = *sweepB; + context.toiInput.maxFraction = maxFraction; + context.isSensor = isSensor; + + b3Vec3 localCentroidB = b3GetShapeCentroid( shapeB ); + context.localCentroidB = localCentroidB; + + // Assume mesh is static + b3Transform xfA = { + .p = b3Sub( sweepA->c1, b3RotateVector( sweepA->q1, sweepA->localCenter ) ), + .q = sweepA->q1, + }; + + b3Transform xfB1 = { + .p = b3Sub( sweepB->c1, b3RotateVector( sweepB->q1, sweepB->localCenter ) ), + .q = sweepB->q1, + }; + + b3Transform xfB2 = { + .p = b3Sub( sweepB->c2, b3RotateVector( sweepB->q2, sweepB->localCenter ) ), + .q = sweepB->q2, + }; + + context.meshLocalCentroidB1 = b3InvTransformPoint( xfA, b3TransformPoint( xfB1, localCentroidB ) ); + context.meshLocalCentroidB2 = b3InvTransformPoint( xfA, b3TransformPoint( xfB2, localCentroidB ) ); + + b3ShapeExtent extents = b3ComputeShapeExtent( shapeB, context.localCentroidB ); + context.fallbackRadius = b3MaxFloat( 0.5f * extents.minExtent, B3_LINEAR_SLOP ); + + // Swept bounds of shapeB + // todo pass in xfA to get local bounds directly + b3AABB bounds = b3ComputeSweptShapeAABB( shapeB, sweepB, maxFraction ); + + // Bounds local to mesh + b3AABB localBounds = b3AABB_Transform( b3InvertTransform( xfA ), bounds ); + + if ( typeA == b3_meshShape ) + { + b3QueryMesh( &shapeA->mesh, localBounds, b3MeshTimeOfImpactFcn, &context ); + } + else if ( typeA == b3_heightShape ) + { + b3QueryHeightField( shapeA->heightField, localBounds, b3MeshTimeOfImpactFcn, &context ); + } + + float ms = b3GetMilliseconds( ticks ); + if ( ms > 1000.0f * b3GetStallThreshold() ) + { + b3Log( "CCD stall: visited %d triangles", context.visitCount ); + } + + return context.toiOutput; + } + + B3_ASSERT( shapeB->type != b3_compoundShape && shapeB->type != b3_meshShape && shapeB->type != b3_heightShape ); + + b3TOIInput input; + input.proxyA = b3MakeShapeProxy( shapeA ); + input.proxyB = b3MakeShapeProxy( shapeB ); + input.sweepA = *sweepA; + input.sweepB = *sweepB; + input.maxFraction = maxFraction; + + b3TOIOutput output = b3TimeOfImpact( &input ); + +#if 0 + // todo I'm not sure this is worth it for convex vs convex. + if (0.0f < output.fraction && output.fraction < maxFraction) + { + return output; + } + + if (0.0f == output.fraction) + { + // fallback to TOI of a small circle around the fast shape centroid + b3Vec3 centroid = b3GetShapeCentroid( shapeB ); + input.proxyB = ( b3ShapeProxy ){ ¢roid, 1, B3_SPECULATIVE_DISTANCE }; + output = b3TimeOfImpact( &input ); + return output; + } +#endif + + return output; +} + +// Resolve the user material id for a hit point on the given shape. Mesh/heightfield shapes +// use the manifold-point triangleIndex to pick a per-triangle material. Compound shapes use +// the contact's childIndex to find the participating child, then for a mesh child apply the +// child's materialIndices indirection on top of the per-triangle index. Convex shapes fall +// back to materials[0]. childIndex is unused for non-compound shapes. +uint64_t b3GetShapeUserMaterialId( const b3Shape* shape, int childIndex, int triangleIndex ) +{ + if ( shape->materialCount == 0 ) + { + return 0; + } + + int materialIndex = 0; + if ( shape->type == b3_meshShape ) + { + const uint8_t* indices = b3GetMeshMaterialIndices( shape->mesh.data ); + if ( indices != NULL ) + { + materialIndex = indices[triangleIndex]; + } + } + else if ( shape->type == b3_heightShape ) + { + materialIndex = b3GetHeightFieldMaterial( shape->heightField, triangleIndex ); + } + else if ( shape->type == b3_compoundShape ) + { + b3ChildShape child = b3GetCompoundChild( shape->compound, childIndex ); + if ( child.type == b3_meshShape ) + { + const uint8_t* indices = b3GetMeshMaterialIndices( child.mesh.data ); + int meshMaterialIndex = indices != NULL ? indices[triangleIndex] : 0; + meshMaterialIndex = b3ClampInt( meshMaterialIndex, 0, B3_MAX_COMPOUND_MESH_MATERIALS - 1 ); + materialIndex = child.materialIndices[meshMaterialIndex]; + } + else + { + materialIndex = child.materialIndices[0]; + } + } + + materialIndex = b3ClampInt( materialIndex, 0, shape->materialCount - 1 ); + return b3GetShapeMaterials( shape )[materialIndex].userMaterialId; +} diff --git a/vendor/box3d/src/src/shape.h b/vendor/box3d/src/src/shape.h new file mode 100644 index 000000000..519ce8694 --- /dev/null +++ b/vendor/box3d/src/src/shape.h @@ -0,0 +1,155 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +#pragma once + +#include "math_internal.h" + +#include "box3d/types.h" + +#include + +typedef struct b3BroadPhase b3BroadPhase; +typedef struct b3World b3World; + +typedef enum b3ShapeFlags +{ + b3_enableSensorEvents = 0x01, + b3_enableContactEvents = 0x02, + b3_enableCustomFiltering = 0x04, + b3_enableHitEvents = 0x08, + b3_enablePreSolveEvents = 0x10, + b3_enlargedAABB = 0x20, + b3_enableSpeculative = 0x40, +} b3ShapeFlags; + +typedef struct b3Shape +{ + int id; + int bodyId; + int prevShapeId; + int nextShapeId; + int sensorIndex; + int proxyKey; + b3ShapeType type; + float density; + float explosionScale; + float aabbMargin; + + b3AABB aabb; + b3AABB fatAABB; + b3Vec3 localCentroid; + + int materialCount; + b3SurfaceMaterial material; + b3SurfaceMaterial* materials; + + b3Filter filter; + void* userData; + void* userShape; + + uint32_t nameId; + uint16_t generation; + + // b3ShapeFlags + uint8_t flags; + + union + { + b3Capsule capsule; + b3Sphere sphere; + const b3HullData* hull; + b3Mesh mesh; + const b3HeightFieldData* heightField; + const b3CompoundData* compound; + }; + +} b3Shape; + +// A single material shape keeps its material inline. Multi material meshes and compounds own a heap +// array. Reach the materials the same way for both: a single material shape presents its inline +// material as a one element array. Do not cache the pointer, the shapes array can move. +static inline b3SurfaceMaterial* b3GetShapeMaterials( const b3Shape* shape ) +{ + return shape->materials != NULL ? shape->materials : (b3SurfaceMaterial*)&shape->material; +} + +void b3CreateShapeProxy( b3Shape* shape, b3BroadPhase* bp, b3BodyType type, b3WorldTransform transform, bool forcePairCreation ); +void b3DestroyShapeProxy( b3Shape* shape, b3BroadPhase* bp ); + +void b3DestroyShapeAllocations( b3World* world, b3Shape* shape ); + +b3MassData b3ComputeShapeMass( const b3Shape* shape ); +b3ShapeExtent b3ComputeShapeExtent( const b3Shape* shape, b3Vec3 localCenter ); + +b3AABB b3ComputeSweptSphereAABB( const b3Sphere* shape, b3Transform xf1, b3Transform xf2 ); +b3AABB b3ComputeSweptCapsuleAABB( const b3Capsule* shape, b3Transform xf1, b3Transform xf2 ); + +b3AABB b3ComputeShapeAABB( const b3Shape* shape, b3Transform transform ); + +// Conservative world AABB for a shape inflated by extra margin. In double precision mode the +// box is built in the body local frame, translated by the double origin, and rounded outward. +b3AABB b3ComputeFatShapeAABB( const b3Shape* shape, b3WorldTransform transform, float extra ); +b3AABB b3ComputeSweptShapeAABB( const b3Shape* shape, const b3Sweep* sweep, float time ); +b3Vec3 b3GetShapeCentroid( const b3Shape* shape ); +float b3GetShapeArea( const b3Shape* shape ); +float b3GetShapeProjectedArea( const b3Shape* shape, b3Vec3 planeNormal ); +uint64_t b3GetShapeUserMaterialId( const b3Shape* shape, int childIndex, int triangleIndex ); + +b3ShapeProxy b3MakeShapeProxy( const b3Shape* shape ); +b3ShapeProxy b3MakeLocalProxy( const b3ShapeProxy* proxy, b3Transform transform, b3Vec3* buffer ); +b3AABB b3ComputeProxyAABB( const b3ShapeProxy* proxy ); + +b3CastOutput b3RayCastShape( const b3Shape* shape, b3Transform transform, const b3RayCastInput* input ); +b3CastOutput b3ShapeCastShape( const b3Shape* shape, b3Transform transform, const b3ShapeCastInput* input ); +bool b3OverlapShape( const b3Shape* shape, b3Transform transform, const b3ShapeProxy* proxy ); + +float b3GetShapeArea( const b3Shape* shape ); +float b3GetShapeProjectedArea( const b3Shape* shape, b3Vec3 planeNormal ); +b3TOIOutput b3ShapeTimeOfImpact( b3Shape* shapeA, b3Shape* shapeB, b3Sweep* sweepA, b3Sweep* sweepB, float maxFraction ); + +int b3CollideMoverAndSphere( b3PlaneResult* result, const b3Sphere* shape, const b3Capsule* mover ); +int b3CollideMoverAndCapsule( b3PlaneResult* result, const b3Capsule* shape, const b3Capsule* mover ); +int b3CollideMoverAndHull( b3PlaneResult* result, const b3HullData* shape, const b3Capsule* mover ); +int b3CollideMoverAndMesh( b3PlaneResult* planes, int capacity, const b3Mesh* shape, const b3Capsule* mover ); +int b3CollideMoverAndHeightField( b3PlaneResult* results, int capacity, const b3HeightFieldData* shape, const b3Capsule* mover ); +int b3CollideMover( b3PlaneResult* planes, int planeCapacity, const b3Shape* shape, b3Transform transform, + const b3Capsule* mover ); + +// Hull +int b3FindHullSupportVertex( const b3HullData* hull, b3Vec3 direction ); +int b3FindHullSupportFace( const b3HullData* hull, b3Vec3 direction ); +bool b3IsValidHull( const b3HullData* hull ); +b3AABB b3ComputeSweptHullAABB( const b3HullData* shape, b3Transform xf1, b3Transform xf2 ); +b3ShapeExtent b3ComputeHullExtent( const b3HullData* hull, b3Vec3 origin ); +float b3ComputeHullProjectedArea( const b3HullData* hull, b3Vec3 direction ); + +// Height field +b3Triangle b3GetHeightFieldTriangle( const b3HeightFieldData* heightField, int triangleIndex ); +int b3GetHeightFieldMaterial( const b3HeightFieldData* heightField, int triangleIndex ); + +static inline int b3GetHeightFieldTriangleCount( const b3HeightFieldData* heightField ) +{ + int cellCount = ( heightField->rowCount - 1 ) * ( heightField->columnCount - 1 ); + return 2 * cellCount; +} + +// Mesh +b3Triangle b3GetMeshTriangle( const b3Mesh* mesh, int triangleIndex ); +bool b3IsValidMesh( const b3MeshData* meshData ); + +static inline bool b3ShouldShapesCollide( b3Filter filterA, b3Filter filterB ) +{ + if ( filterA.groupIndex == filterB.groupIndex && filterA.groupIndex != 0 ) + { + return filterA.groupIndex > 0; + } + + return ( filterA.maskBits & filterB.categoryBits ) != 0 && ( filterA.categoryBits & filterB.maskBits ) != 0; +} + +static inline bool b3ShouldQueryCollide( const b3Filter* shapeFilter, const b3QueryFilter* queryFilter ) +{ + return ( shapeFilter->categoryBits & queryFilter->maskBits ) != 0 && + ( shapeFilter->maskBits & queryFilter->categoryBits ) != 0; +} diff --git a/vendor/box3d/src/src/simd.c b/vendor/box3d/src/src/simd.c new file mode 100644 index 000000000..6a9faace0 --- /dev/null +++ b/vendor/box3d/src/src/simd.c @@ -0,0 +1,202 @@ +// SPDX-FileCopyrightText: 2026 Erin Catto +// SPDX-License-Identifier: MIT + +// Dirk Gregorius contributed portions of this code + +#include "simd.h" + +#if defined( B3_SIMD_SSE2 ) + +#define B3_TRANSPOSE3( C1, C2, C3 ) \ + { \ + b3V32 T1 = _mm_unpacklo_ps( ( C1 ), ( C2 ) ); \ + b3V32 T2 = _mm_unpackhi_ps( ( C1 ), ( C2 ) ); \ + ( C1 ) = _mm_shuffle_ps( ( T1 ), ( C3 ), _MM_SHUFFLE( 0, 0, 1, 0 ) ); \ + ( C2 ) = _mm_shuffle_ps( ( T1 ), ( C3 ), _MM_SHUFFLE( 1, 1, 3, 2 ) ); \ + ( C3 ) = _mm_shuffle_ps( ( T2 ), ( C3 ), _MM_SHUFFLE( 2, 2, 1, 0 ) ); \ + } + +static inline b3V32 b3SplatXV( b3V32 v ) +{ + return _mm_shuffle_ps( v, v, _MM_SHUFFLE( 0, 0, 0, 0 ) ); +} + +static inline b3V32 b3SplatYV( b3V32 v ) +{ + return _mm_shuffle_ps( v, v, _MM_SHUFFLE( 1, 1, 1, 1 ) ); +} + +static inline b3V32 b3SplatZV( b3V32 v ) +{ + return _mm_shuffle_ps( v, v, _MM_SHUFFLE( 2, 2, 2, 2 ) ); +} + +static inline bool b3AnyGreaterEq3V( b3V32 a, b3V32 b ) +{ + b3V32 v = _mm_cmpge_ps( a, b ); + return ( _mm_movemask_ps( v ) & 0x07 ) != 0; +} + +static inline b3V32 b3Dot3V( b3V32 a, b3V32 b ) +{ + b3V32 m = _mm_mul_ps( a, b ); + b3V32 x = _mm_shuffle_ps( m, m, _MM_SHUFFLE( 0, 0, 0, 0 ) ); + b3V32 y = _mm_shuffle_ps( m, m, _MM_SHUFFLE( 1, 1, 1, 1 ) ); + b3V32 z = _mm_shuffle_ps( m, m, _MM_SHUFFLE( 2, 2, 2, 2 ) ); + + return _mm_add_ps( _mm_add_ps( x, y ), z ); +} + +#else + +#define B3_TRANSPOSE3( C1, C2, C3 ) \ + { \ + float temp1 = C1.y; \ + float temp2 = C1.z; \ + float temp3 = C2.z; \ + \ + C1.y = C2.x; \ + C1.z = C3.x; \ + C2.z = C3.y; \ + \ + C2.x = temp1; \ + C3.x = temp2; \ + C3.y = temp3; \ + } + +static inline b3V32 b3SplatXV( b3V32 a ) +{ + return B3_LITERAL( b3V32 ){ a.x, a.x, a.x }; +} + +static inline b3V32 b3SplatYV( b3V32 a ) +{ + return B3_LITERAL( b3V32 ){ a.y, a.y, a.y }; +} + +static inline b3V32 b3SplatZV( b3V32 a ) +{ + return B3_LITERAL( b3V32 ){ a.z, a.z, a.z }; +} + +static inline bool b3AnyGreaterEq3V( b3V32 a, b3V32 b ) +{ + return a.x >= b.x || a.y >= b.y || a.z >= b.z; +} + +static inline b3V32 b3Dot3V( b3V32 a, b3V32 b ) +{ + float d = a.x * b.x + a.y * b.y + a.z * b.z; + return B3_LITERAL( b3V32 ){ d, d, d }; +} + +#endif + +bool b3TestBoundsTriangleOverlap( b3V32 nodeCenter, b3V32 nodeExtent, b3V32 vertex1, b3V32 vertex2, b3V32 vertex3 ) +{ + b3V32 two = b3SplatV( 2.0f ); + + // Setup triangle + vertex1 = b3SubV( vertex1, nodeCenter ); + vertex2 = b3SubV( vertex2, nodeCenter ); + vertex3 = b3SubV( vertex3, nodeCenter ); + + // Face separation + b3V32 triangleMin = b3MinV( vertex1, b3MinV( vertex2, vertex3 ) ); + b3V32 triangleMax = b3MaxV( vertex1, b3MaxV( vertex2, vertex3 ) ); + + b3V32 separation1 = b3SubV( triangleMin, nodeExtent ); + b3V32 separation2 = b3AddV( triangleMax, nodeExtent ); + + b3V32 faceSeparation = b3MaxV( separation1, b3NegV( separation2 ) ); + if ( b3AnyGreater3V( faceSeparation, b3_zeroV ) ) + { + return false; + } + + // SAT: Face separation + b3V32 edge1 = b3SubV( vertex2, vertex1 ); + b3V32 edge2 = b3SubV( vertex3, vertex2 ); + b3V32 edge3 = b3SubV( vertex1, vertex3 ); + + b3V32 normal = b3CrossV( edge1, edge2 ); + + b3V32 triangleSeparation = b3SubV( b3AbsV( b3Dot3V( normal, vertex1 ) ), b3Dot3V( b3AbsV( normal ), nodeExtent ) ); + if ( b3AnyGreater3V( triangleSeparation, b3_zeroV ) ) + { + return false; + } + + // SAT: Edge separation + b3V32 edgeSeparation1 = b3SubV( b3SubV( b3AbsV( b3CrossV( edge1, b3AddV( vertex1, vertex3 ) ) ), b3AbsV( b3CrossV( edge1, edge3 ) ) ), + b3MulV( two, b3ModifiedCrossV( b3AbsV( edge1 ), nodeExtent ) ) ); + if ( b3AnyGreater3V( edgeSeparation1, b3_zeroV ) ) + { + return false; + } + + b3V32 edgeSeparation2 = b3SubV( b3SubV( b3AbsV( b3CrossV( edge2, b3AddV( vertex1, vertex2 ) ) ), b3AbsV( b3CrossV( edge2, edge1 ) ) ), + b3MulV( two, b3ModifiedCrossV( b3AbsV( edge2 ), nodeExtent ) ) ); + if ( b3AnyGreater3V( edgeSeparation2, b3_zeroV ) ) + { + return false; + } + + b3V32 edgeSeparation3 = b3SubV( b3SubV( b3AbsV( b3CrossV( edge3, b3AddV( vertex2, vertex3 ) ) ), b3AbsV( b3CrossV( edge3, edge2 ) ) ), + b3MulV( two, b3ModifiedCrossV( b3AbsV( edge3 ), nodeExtent ) ) ); + if ( b3AnyGreater3V( edgeSeparation3, b3_zeroV ) ) + { + return false; + } + + return true; +} + +float b3IntersectRayTriangle( b3V32 rayStart, b3V32 rayDelta, b3V32 vertex1, b3V32 vertex2, b3V32 vertex3 ) +{ + // Test if ray intersects this triangle sharing same calculations for each triangle + { + b3V32 edge1 = b3SubV( vertex3, vertex2 ); + b3V32 edge2 = b3SubV( vertex1, vertex3 ); + b3V32 edge3 = b3SubV( vertex2, vertex1 ); + + b3V32 midPoint1 = b3MulV( b3_halfV, b3AddV( vertex2, vertex3 ) ); + b3V32 midPoint2 = b3MulV( b3_halfV, b3AddV( vertex3, vertex1 ) ); + b3V32 midPoint3 = b3MulV( b3_halfV, b3AddV( vertex1, vertex2 ) ); + + b3V32 normal1 = b3CrossV( edge1, b3SubV( midPoint1, rayStart ) ); + b3V32 normal2 = b3CrossV( edge2, b3SubV( midPoint2, rayStart ) ); + b3V32 normal3 = b3CrossV( edge3, b3SubV( midPoint3, rayStart ) ); + B3_TRANSPOSE3( normal1, normal2, normal3 ); + + b3V32 rayDeltaX = b3SplatXV( rayDelta ); + b3V32 rayDeltaY = b3SplatYV( rayDelta ); + b3V32 rayDeltaZ = b3SplatZV( rayDelta ); + + b3V32 volumes = b3AddV( b3AddV( b3MulV( normal1, rayDeltaX ), b3MulV( normal2, rayDeltaY ) ), b3MulV( normal3, rayDeltaZ ) ); + if ( b3AnyLess3V( volumes, b3_zeroV ) ) + { + return 1.0f; + } + } + + // Compute intersection with triangle plane + b3V32 edge1 = b3SubV( vertex2, vertex1 ); + b3V32 edge2 = b3SubV( vertex3, vertex1 ); + b3V32 normal = b3CrossV( edge1, edge2 ); + + b3V32 denominator = b3Dot3V( normal, rayDelta ); + if ( b3AnyGreaterEq3V( denominator, b3_zeroV ) ) + { + return 1.0f; + } + + b3V32 lambda = b3DivV( b3Dot3V( normal, b3SubV( vertex1, rayStart ) ), denominator ); + if ( b3AnyLessEq3V( lambda, b3_zeroV ) ) + { + return 1.0f; + } + + lambda = b3MinV( lambda, b3_oneV ); + return b3GetXV( lambda ); +} diff --git a/vendor/box3d/src/src/simd.h b/vendor/box3d/src/src/simd.h new file mode 100644 index 000000000..ac7e9e8b5 --- /dev/null +++ b/vendor/box3d/src/src/simd.h @@ -0,0 +1,357 @@ +// SPDX-FileCopyrightText: 2026 Erin Catto +// SPDX-License-Identifier: MIT + +#pragma once + +#include "core.h" + +#include + +#if defined( B3_SIMD_SSE2 ) + +#include + +// wide float holds 4 numbers +typedef __m128 b3V32; + +typedef union b3128 +{ + b3V32 v; + float f[4]; +} b3128; + +#if defined( _MSC_VER ) && !defined( __clang__ ) + +static const b3V32 b3_zeroV = { { 0.0f, 0.0f, 0.0f, 0.0f } }; +static const b3V32 b3_halfV = { { 0.5f, 0.5f, 0.5f, 0.5f } }; +static const b3V32 b3_oneV = { { 1.0f, 1.0f, 1.0f, 1.0f } }; + +#else + +static const b3V32 b3_zeroV = { 0.0f, 0.0f, 0.0f, 0.0f }; +static const b3V32 b3_halfV = { 0.5f, 0.5f, 0.5f, 0.5f }; +static const b3V32 b3_oneV = { 1.0f, 1.0f, 1.0f, 1.0f }; + +#endif + +static inline b3V32 b3AddV( b3V32 a, b3V32 b ) +{ + return _mm_add_ps( a, b ); +} + +static inline b3V32 b3SubV( b3V32 a, b3V32 b ) +{ + return _mm_sub_ps( a, b ); +} + +static inline b3V32 b3MulV( b3V32 a, b3V32 b ) +{ + return _mm_mul_ps( a, b ); +} + +static inline b3V32 b3DivV( b3V32 a, b3V32 b ) +{ + return _mm_div_ps( a, b ); +} + +static inline b3V32 b3NegV( b3V32 a ) +{ + return _mm_sub_ps( _mm_setzero_ps(), a ); +} + +static inline b3V32 b3LoadV( const float* src ) +{ + // Loads exactly 12 bytes: 8 via movsd, 4 via movss. + // Result lane 3 is implicitly zero from the partial loads. + __m128 xy = _mm_castpd_ps( _mm_load_sd( (const double*)( src ) ) ); + __m128 z = _mm_load_ss( src + 2 ); + return _mm_movelh_ps( xy, z ); // { src[0], src[1], src[2], 0.0f } +} + +static inline b3V32 b3ZeroV( void ) +{ + return _mm_setzero_ps(); +} + +static inline float b3GetXV( b3V32 a ) +{ + return _mm_cvtss_f32( a ); +} + +static inline float b3GetYV( b3V32 a ) +{ + return _mm_cvtss_f32( _mm_shuffle_ps( a, a, _MM_SHUFFLE( 1, 1, 1, 1 ) ) ); +} + +static inline float b3GetZV( b3V32 a ) +{ + return _mm_cvtss_f32( _mm_shuffle_ps( a, a, _MM_SHUFFLE( 2, 2, 2, 2 ) ) ); +} + +static inline float b3GetV( b3V32 a, int index ) +{ + b3128 b; + b.v = a; + return b.f[index]; +} + +static inline b3V32 b3SplatV( float x ) +{ + return _mm_set_ps1( x ); +} + +static inline b3V32 b3AbsV( b3V32 a ) +{ + // Abs( V ) = Max( -V, V ) + b3V32 zero = _mm_setzero_ps(); + return _mm_max_ps( _mm_sub_ps( zero, a ), a ); +} + +static inline b3V32 b3MinV( b3V32 a, b3V32 b ) +{ + return _mm_min_ps( a, b ); +} + +static inline b3V32 b3MaxV( b3V32 a, b3V32 b ) +{ + return _mm_max_ps( a, b ); +} + +static inline b3V32 b3CrossV( b3V32 a, b3V32 b ) +{ + b3V32 yzX1 = _mm_shuffle_ps( a, a, _MM_SHUFFLE( 3, 0, 2, 1 ) ); + b3V32 zxY1 = _mm_shuffle_ps( a, a, _MM_SHUFFLE( 3, 1, 0, 2 ) ); + b3V32 yzX2 = _mm_shuffle_ps( b, b, _MM_SHUFFLE( 3, 0, 2, 1 ) ); + b3V32 zxY2 = _mm_shuffle_ps( b, b, _MM_SHUFFLE( 3, 1, 0, 2 ) ); + + return _mm_sub_ps( _mm_mul_ps( yzX1, zxY2 ), _mm_mul_ps( zxY1, yzX2 ) ); +} + +static inline b3V32 b3ModifiedCrossV( b3V32 a, b3V32 b ) +{ + b3V32 yzX1 = _mm_shuffle_ps( a, a, _MM_SHUFFLE( 3, 0, 2, 1 ) ); + b3V32 zxY1 = _mm_shuffle_ps( a, a, _MM_SHUFFLE( 3, 1, 0, 2 ) ); + b3V32 yzX2 = _mm_shuffle_ps( b, b, _MM_SHUFFLE( 3, 0, 2, 1 ) ); + b3V32 zxY2 = _mm_shuffle_ps( b, b, _MM_SHUFFLE( 3, 1, 0, 2 ) ); + + return _mm_add_ps( _mm_mul_ps( yzX1, zxY2 ), _mm_mul_ps( zxY1, yzX2 ) ); +} + +static inline bool b3AnyLess3V( b3V32 a, b3V32 b ) +{ + b3V32 v = _mm_cmplt_ps( a, b ); + return ( _mm_movemask_ps( v ) & 0x07 ) != 0; +} + +static inline bool b3AnyLessEq3V( b3V32 a, b3V32 b ) +{ + b3V32 v = _mm_cmple_ps( a, b ); + return ( _mm_movemask_ps( v ) & 0x07 ) != 0; +} + +static inline bool b3AnyGreater3V( b3V32 a, b3V32 b ) +{ + b3V32 v = _mm_cmpgt_ps( a, b ); + return ( _mm_movemask_ps( v ) & 0x07 ) != 0; +} + +static inline bool b3AllLessEq3V( b3V32 a, b3V32 b ) +{ + b3V32 v = _mm_cmple_ps( a, b ); + return ( _mm_movemask_ps( v ) & 0x07 ) == 0x07; +} + +#else + +// I don't expect the use case of b3V32 to benefit from Neon code. +// In particular the cross product is very complex in Neon. + +// scalar math +typedef struct b3V32 +{ + float x, y, z; +} b3V32; + +typedef union b3128 +{ + b3V32 v; + float f[3]; +} b3128; + +static const b3V32 b3_zeroV = { 0.0f, 0.0f, 0.0f }; +static const b3V32 b3_halfV = { 0.5f, 0.5f, 0.5f }; +static const b3V32 b3_oneV = { 1.0f, 1.0f, 1.0f }; + +static inline b3V32 b3AddV( b3V32 a, b3V32 b ) +{ + return B3_LITERAL( b3V32 ){ + a.x + b.x, + a.y + b.y, + a.z + b.z, + }; +} + +static inline b3V32 b3SubV( b3V32 a, b3V32 b ) +{ + return B3_LITERAL( b3V32 ){ + a.x - b.x, + a.y - b.y, + a.z - b.z, + }; +} + +static inline b3V32 b3MulV( b3V32 a, b3V32 b ) +{ + return B3_LITERAL( b3V32 ){ + a.x * b.x, + a.y * b.y, + a.z * b.z, + }; +} + +static inline b3V32 b3DivV( b3V32 a, b3V32 b ) +{ + return B3_LITERAL( b3V32 ){ + a.x / b.x, + a.y / b.y, + a.z / b.z, + }; +} + +static inline b3V32 b3NegV( b3V32 a ) +{ + return B3_LITERAL( b3V32 ){ + -a.x, + -a.y, + -a.z, + }; +} + +// Unaligned loads are much faster on recent hardware with little to no penalty +static inline b3V32 b3LoadV( const float* src ) +{ + return B3_LITERAL( b3V32 ){ src[0], src[1], src[2] }; +} + +static inline b3V32 b3ZeroV( void ) +{ + return B3_LITERAL( b3V32 ){ 0.0f, 0.0f, 0.0f }; +} + +static inline float b3GetXV( b3V32 a ) +{ + return a.x; +} + +static inline float b3GetYV( b3V32 a ) +{ + return a.y; +} + +static inline float b3GetZV( b3V32 a ) +{ + return a.z; +} + +static inline float b3GetV( b3V32 a, int index ) +{ + b3128 b; + b.v = a; + return b.f[index]; +} + +static inline b3V32 b3SplatV( float x ) +{ + return B3_LITERAL( b3V32 ){ x, x, x }; +} + +static inline b3V32 b3AbsV( b3V32 a ) +{ + return B3_LITERAL( b3V32 ){ + a.x < 0.0f ? -a.x : a.x, + a.y < 0.0f ? -a.y : a.y, + a.z < 0.0f ? -a.z : a.z, + }; +} + +static inline b3V32 b3MinV( b3V32 a, b3V32 b ) +{ + return B3_LITERAL( b3V32 ){ + a.x < b.x ? a.x : b.x, + a.y < b.y ? a.y : b.y, + a.z < b.z ? a.z : b.z, + }; +} + +static inline b3V32 b3MaxV( b3V32 a, b3V32 b ) +{ + return B3_LITERAL( b3V32 ){ + a.x > b.x ? a.x : b.x, + a.y > b.y ? a.y : b.y, + a.z > b.z ? a.z : b.z, + }; +} + +static inline b3V32 b3CrossV( b3V32 a, b3V32 b ) +{ + b3V32 c; + c.x = a.y * b.z - a.z * b.y; + c.y = a.z * b.x - a.x * b.z; + c.z = a.x * b.y - a.y * b.x; + return c; +} + +static inline b3V32 b3ModifiedCrossV( b3V32 a, b3V32 b ) +{ + b3V32 c; + c.x = a.y * b.z + a.z * b.y; + c.y = a.z * b.x + a.x * b.z; + c.z = a.x * b.y + a.y * b.x; + return c; +} + +static inline bool b3AnyLess3V( b3V32 a, b3V32 b ) +{ + return a.x < b.x || a.y < b.y || a.z < b.z; +} + +static inline bool b3AnyLessEq3V( b3V32 a, b3V32 b ) +{ + return a.x <= b.x || a.y <= b.y || a.z <= b.z; +} + +static inline bool b3AnyGreater3V( b3V32 a, b3V32 b ) +{ + return a.x > b.x || a.y > b.y || a.z > b.z; +} + +static inline bool b3AllLessEq3V( b3V32 a, b3V32 b ) +{ + return a.x <= b.x && a.y <= b.y && a.z <= b.z; +} + +#endif + +static inline bool b3TestBoundsOverlap( b3V32 nodeMin1, b3V32 nodeMax1, b3V32 nodeMin2, b3V32 nodeMax2 ) +{ + b3V32 separation = b3MaxV( b3SubV( nodeMin2, nodeMax1 ), b3SubV( nodeMin1, nodeMax2 ) ); + return b3AllLessEq3V( separation, b3_zeroV ); +} + +// Test a ray for edge separation with an AABB (Gino, p80). +static inline bool b3TestBoundsRayOverlap( b3V32 nodeMin, b3V32 nodeMax, b3V32 rayStart, b3V32 rayDelta ) +{ + // Setup node + b3V32 nodeCenter = b3MulV( b3_halfV, b3AddV( nodeMin, nodeMax ) ); + b3V32 nodeExtent = b3SubV( nodeMax, nodeCenter ); + + // Setup ray + rayStart = b3SubV( rayStart, nodeCenter ); + + // SAT: Edge separation + b3V32 edgeSeparation = b3SubV( b3AbsV( b3CrossV( rayDelta, rayStart ) ), b3ModifiedCrossV( b3AbsV( rayDelta ), nodeExtent ) ); + return b3AllLessEq3V( edgeSeparation, b3_zeroV ); +} + +bool b3TestBoundsTriangleOverlap( b3V32 nodeCenter, b3V32 nodeExtent, b3V32 vertex1, b3V32 vertex2, b3V32 vertex3 ); +float b3IntersectRayTriangle( b3V32 rayStart, b3V32 rayDelta, b3V32 vertex1, b3V32 vertex2, b3V32 vertex3 ); diff --git a/vendor/box3d/src/src/solver.c b/vendor/box3d/src/src/solver.c new file mode 100644 index 000000000..a2b1be977 --- /dev/null +++ b/vendor/box3d/src/src/solver.c @@ -0,0 +1,2328 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +#include "solver.h" + +#include "arena_allocator.h" +#include "bitset.h" +#include "body.h" +#include "compound.h" +#include "contact.h" +#include "contact_solver.h" +#include "core.h" +#include "ctz.h" +#include "island.h" +#include "joint.h" +#include "parallel_for.h" +#include "physics_world.h" +#include "platform.h" +#include "sensor.h" +#include "shape.h" +#include "solver_set.h" + +#include +#include +#include + +// these are useful for solver testing +#define ITERATIONS 1 +#define RELAX_ITERATIONS 1 + +#if ( defined( __GNUC__ ) || defined( __clang__ ) ) && ( defined( __i386__ ) || defined( __x86_64__ ) ) +static void b3Pause( void ) +{ + __asm__ __volatile__( "pause\n" ); +} +#elif ( defined( __arm__ ) && defined( __ARM_ARCH ) && __ARM_ARCH >= 7 ) || defined( __aarch64__ ) +static void b3Pause( void ) +{ + __asm__ __volatile__( "yield" ::: "memory" ); +} +#elif defined( _MSC_VER ) && ( defined( _M_IX86 ) || defined( _M_X64 ) ) +static void b3Pause( void ) +{ + _mm_pause(); +} +#elif defined( _MSC_VER ) && ( defined( _M_ARM ) || defined( _M_ARM64 ) ) +static void b3Pause( void ) +{ + __yield(); +} +#else +static void b3Pause( void ) +{ +} +#endif + +typedef struct b3WorkerContext +{ + b3StepContext* context; + int workerIndex; + void* userTask; +} b3WorkerContext; + +// Integrate velocities, apply damping, and gyroscopic torque +static void b3IntegrateVelocitiesTask( b3SolverBlock block, b3StepContext* context ) +{ + b3TracyCZoneNC( integrate_velocity, "IntVel", b3_colorDeepPink, true ); + + B3_VALIDATE( block.startIndex + block.count <= context->world->solverSets.data[b3_awakeSet].bodyStates.count ); + + b3BodyState* states = context->states; + b3BodySim* sims = context->sims; + + b3Vec3 gravity = context->world->gravity; + float h = context->h; + + for ( int i = block.startIndex; i < block.startIndex + block.count; ++i ) + { + b3BodySim* sim = sims + i; + b3BodyState* state = states + i; + + b3Vec3 v = state->linearVelocity; + b3Vec3 w = state->angularVelocity; + + // Damping math + // Differential equation: dv/dt + c * v = 0 + // Solution: v(t) = v0 * exp(-c * t) + // Time step: v(t + dt) = v0 * exp(-c * (t + dt)) = v0 * exp(-c * t) * exp(-c * dt) = v(t) * exp(-c * dt) + // v2 = exp(-c * dt) * v1 + // Pade approximation: + // v2 = v1 * 1 / (1 + c * dt) + float linearDamping = 1.0f / ( 1.0f + h * sim->linearDamping ); + float angularDamping = 1.0f / ( 1.0f + h * sim->angularDamping ); + + // Gravity scale will be zero for kinematic bodies + float gravityScale = sim->invMass > 0.0f ? sim->gravityScale : 0.0f; + + b3Vec3 linearVelocityDelta = b3Blend2( h * sim->invMass, sim->force, h * gravityScale, gravity ); + v = b3MulAdd( linearVelocityDelta, linearDamping, v ); + + b3Vec3 angularVelocityDelta = b3MulSV( h, b3MulMV( sim->invInertiaWorld, sim->torque ) ); + w = b3MulAdd( angularVelocityDelta, angularDamping, w ); + + // Gyroscopic torque by solving this nonlinear equation using Newton-Raphson. + // I * (w2 - w1) + h * cross(w2, I * w2) = 0 + // This is all done in local coordinates where the Jacobian is easier to compute. + // This improves the simulation of long skinny bodies. + { + // Get current rotation. + b3Quat q0 = sim->transform.q; + b3Quat q = b3MulQuat( state->deltaRotation, q0 ); + + // todo wasteful computation + b3Matrix3 inertiaLocal = b3InvertMatrix( sim->invInertiaLocal ); + + // Compute local angular velocity + b3Vec3 omega1 = b3InvRotateVector( q, w ); + b3Vec3 omega2 = omega1; + + // Symmetric inertia tensor: 6 unique entries (column-major) + const float i00 = inertiaLocal.cx.x; + const float i01 = inertiaLocal.cy.x; + const float i02 = inertiaLocal.cz.x; + const float i11 = inertiaLocal.cy.y; + const float i12 = inertiaLocal.cz.y; + const float i22 = inertiaLocal.cz.z; + + for ( int gyroIteration = 0; gyroIteration < 1; ++gyroIteration ) + { + const float w1 = omega2.x; + const float w2 = omega2.y; + const float w3 = omega2.z; + + // Iw = I * omega2 (shared between residual and Jacobian) + const float Iw1 = i00 * w1 + i01 * w2 + i02 * w3; + const float Iw2 = i01 * w1 + i11 * w2 + i12 * w3; + const float Iw3 = i02 * w1 + i12 * w2 + i22 * w3; + + // Residual: b = I*(omega2 - omega1) + h * (omega2 × I*omega2) + const b3Vec3 dw = b3Sub( omega2, omega1 ); + b3Vec3 b = { + i00 * dw.x + i01 * dw.y + i02 * dw.z + h * ( w2 * Iw3 - w3 * Iw2 ), + i01 * dw.x + i11 * dw.y + i12 * dw.z + h * ( w3 * Iw1 - w1 * Iw3 ), + i02 * dw.x + i12 * dw.y + i22 * dw.z + h * ( w1 * Iw2 - w2 * Iw1 ), + }; + + // Jacobian J = I + h * (skew(omega2) * I - skew(I*omega2)) + // Jacobian derived by Erin Catto, Ph.D. Do not attempt to do this without a Ph.D. + // Doubled inertia terms above fold into Iw, e.g. row 2 col 1: i00*w3 - i02*w1 - Iw3. + b3Matrix3 J = { + { i00 + h * ( w2 * i02 - w3 * i01 ), i01 + h * ( w3 * i00 - w1 * i02 - Iw3 ), + i02 + h * ( w1 * i01 - w2 * i00 + Iw2 ) }, + { i01 + h * ( w2 * i12 - w3 * i11 + Iw3 ), i11 + h * ( w3 * i01 - w1 * i12 ), + i12 + h * ( w1 * i11 - w2 * i01 - Iw1 ) }, + { i02 + h * ( w2 * i22 - w3 * i12 - Iw2 ), i12 + h * ( w3 * i02 - w1 * i22 + Iw1 ), + i22 + h * ( w1 * i12 - w2 * i02 ) }, + }; + + omega2 = b3Sub( omega2, b3Solve3( J, b ) ); + } + + w = b3RotateVector( q, omega2 ); + } + + state->linearVelocity = v; + state->angularVelocity = w; + } + + b3TracyCZoneEnd( integrate_velocity ); +} + +static void b3IntegratePositionsTask( b3SolverBlock block, b3StepContext* context ) +{ + b3TracyCZoneNC( integrate_positions, "IntPos", b3_colorDarkSeaGreen, true ); + + B3_VALIDATE( block.startIndex + block.count <= context->world->solverSets.data[b3_awakeSet].bodyStates.count ); + + b3BodyState* states = context->states; + float h = context->h; + float maxLinearSpeed = context->maxLinearVelocity; + float maxAngularSpeed = B3_MAX_ROTATION * context->inv_dt; + float maxLinearSpeedSquared = maxLinearSpeed * maxLinearSpeed; + float maxAngularSpeedSquared = maxAngularSpeed * maxAngularSpeed; + + for ( int i = block.startIndex; i < block.startIndex + block.count; ++i ) + { + b3BodyState* state = states + i; + + b3Vec3 v = state->linearVelocity; + b3Vec3 w = state->angularVelocity; + + // Motion locks - these can be viewed as a constraint that come last + v.x = ( state->flags & b3_lockLinearX ) ? 0.0f : v.x; + v.y = ( state->flags & b3_lockLinearY ) ? 0.0f : v.y; + v.z = ( state->flags & b3_lockLinearZ ) ? 0.0f : v.z; + w.x = ( state->flags & b3_lockAngularX ) ? 0.0f : w.x; + w.y = ( state->flags & b3_lockAngularY ) ? 0.0f : w.y; + w.z = ( state->flags & b3_lockAngularZ ) ? 0.0f : w.z; + + // Clamp to max linear speed + if ( b3Dot( v, v ) > maxLinearSpeedSquared ) + { + float ratio = maxLinearSpeed / b3Length( v ); + v = b3MulSV( ratio, v ); + state->flags |= b3_isSpeedCapped; + } + + // Clamp to max angular speed + if ( b3Dot( w, w ) > maxAngularSpeedSquared && ( state->flags & b3_allowFastRotation ) == 0 ) + { + float ratio = maxAngularSpeed / b3Length( w ); + w = b3MulSV( ratio, w ); + state->flags |= b3_isSpeedCapped; + } + + state->linearVelocity = v; + state->angularVelocity = w; + state->deltaPosition = b3MulAdd( state->deltaPosition, h, v ); + state->deltaRotation = b3IntegrateRotation( state->deltaRotation, b3MulSV( h, w ) ); + } + + b3TracyCZoneEnd( integrate_positions ); +} + +static void b3PrepareJointsTask( b3SolverBlock block, b3StepContext* context ) +{ + b3TracyCZoneNC( prepare_joints, "PrepJoints", b3_colorOldLace, true ); + + b3JointPrepareSpan* spans = context->jointPrepareSpans; + + int index = block.startIndex; + int endIndex = block.startIndex + block.count; + + // Find color for start index. Linear search but fast. + int colorIndex = 0; + while ( spans[colorIndex + 1].start <= index ) + { + colorIndex += 1; + } + + // Loop over block + while ( index < endIndex ) + { + int colorStart = spans[colorIndex].start; + int colorEndIndex = b3MinInt( spans[colorIndex + 1].start, endIndex ); + b3JointSim* joints = spans[colorIndex].joints; + + // Loop over color + for ( ; index < colorEndIndex; ++index ) + { + B3_ASSERT( 0 <= index - colorStart && index - colorStart < spans[colorIndex].count ); + b3JointSim* joint = joints + ( index - colorStart ); + b3PrepareJoint( joint, context ); + } + + // Advance to next color + colorIndex += 1; + } + + b3TracyCZoneEnd( prepare_joints ); +} + +static void b3WarmStartJointsTask( b3SolverBlock block, b3StepContext* context ) +{ + b3TracyCZoneNC( warm_joints, "WarmJoints", b3_colorGold, true ); + + b3GraphColor* color = context->graph->colors + block.colorIndex; + b3JointSim* joints = color->jointSims.data; + + for ( int i = block.startIndex; i < block.startIndex + block.count; ++i ) + { + b3JointSim* joint = joints + i; + b3WarmStartJoint( joint, context ); + } + + b3TracyCZoneEnd( warm_joints ); +} + +static void b3SolveJointsTask( b3SolverBlock block, b3StepContext* context, bool useBias, int workerIndex ) +{ + b3TracyCZoneNC( solve_joints, "SolveJoints", b3_colorLemonChiffon, true ); + + b3GraphColor* color = context->graph->colors + block.colorIndex; + b3JointSim* joints = color->jointSims.data; + + B3_ASSERT( 0 <= block.startIndex && block.startIndex + block.count <= color->jointSims.count ); + + b3BitSet* jointStateBitSet = &context->world->taskContexts.data[workerIndex].jointStateBitSet; + + for ( int i = block.startIndex; i < block.startIndex + block.count; ++i ) + { + b3JointSim* joint = joints + i; + b3SolveJoint( joint, context, useBias ); + + if ( useBias && ( joint->forceThreshold < FLT_MAX || joint->torqueThreshold < FLT_MAX ) && + b3GetBit( jointStateBitSet, joint->jointId ) == false ) + { + float force, torque; + b3GetJointReaction( context->world, joint, context->inv_h, &force, &torque ); + + // Check thresholds. A zero threshold means all awake joints get reported. + if ( force >= joint->forceThreshold || torque >= joint->torqueThreshold ) + { + // Flag this joint for processing. + b3SetBit( jointStateBitSet, joint->jointId ); + } + } + } + + b3TracyCZoneEnd( solve_joints ); +} + +#define B2_MAX_CONTINUOUS_SENSOR_HITS 8 + +typedef struct b3ContinuousContext +{ + b3World* world; + b3BodySim* fastBodySim; + b3Shape* fastShape; + b3Vec3 centroid1, centroid2; + b3Sweep sweep; + // World base for re-centering sweeps. Keeps TOI in float precision far from the origin. + b3Pos base; + float fraction; + b3SensorHit sensorHits[B2_MAX_CONTINUOUS_SENSOR_HITS]; + float sensorFractions[B2_MAX_CONTINUOUS_SENSOR_HITS]; + int sensorCount; + + int visitCount; + + int distanceIterations; + int pushBackIterations; + int rootIterations; +} b3ContinuousContext; + +// This is called from b3DynamicTree_Query for continuous collision +static bool b3ContinuousQueryCallback( int proxyId, uint64_t userData, void* context ) +{ + B3_UNUSED( proxyId ); + + int shapeId = (int)userData; + b3ContinuousContext* continuousContext = context; + continuousContext->visitCount += 1; + + b3Shape* fastShape = continuousContext->fastShape; + b3BodySim* fastBodySim = continuousContext->fastBodySim; + + B3_ASSERT( fastShape->sensorIndex == B3_NULL_INDEX ); + + // Skip same shape + if ( shapeId == fastShape->id ) + { + return true; + } + + b3World* world = continuousContext->world; + b3Shape* shape = b3Array_Get( world->shapes, shapeId ); + + // Skip same body + if ( shape->bodyId == fastShape->bodyId ) + { + return true; + } + + // Skip sensors unless both shapes want sensor events + bool isSensor = shape->sensorIndex != B3_NULL_INDEX; + if ( isSensor && ( ( shape->flags & b3_enableSensorEvents ) == 0 || ( fastShape->flags & b3_enableSensorEvents ) == 0 ) ) + { + return true; + } + + // Skip filtered shapes + bool canCollide = b3ShouldShapesCollide( fastShape->filter, shape->filter ); + if ( canCollide == false ) + { + return true; + } + + b3Body* body = b3Array_Get( world->bodies, shape->bodyId ); + + b3BodySim* bodySim = b3GetBodySim( world, body ); + B3_ASSERT( body->type == b3_staticBody || ( fastBodySim->flags & b3_isBullet ) ); + + // Skip bullets + if ( bodySim->flags & b3_isBullet ) + { + return true; + } + + // Skip filtered bodies + b3Body* fastBody = b3Array_Get( world->bodies, fastBodySim->bodyId ); + canCollide = b3ShouldBodiesCollide( world, fastBody, body ); + if ( canCollide == false ) + { + return true; + } + + // Custom user filtering + if ( ( shape->flags & b3_enableCustomFiltering ) != 0 || ( fastShape->flags & b3_enableCustomFiltering ) != 0 ) + { + b3CustomFilterFcn* customFilterFcn = world->customFilterFcn; + if ( customFilterFcn != NULL ) + { + b3ShapeId idA = { shape->id + 1, world->worldId, shape->generation }; + b3ShapeId idB = { fastShape->id + 1, world->worldId, fastShape->generation }; + canCollide = customFilterFcn( idA, idB, world->customFilterContext ); + if ( canCollide == false ) + { + return true; + } + } + } + + uint64_t ticks = b3GetTicks(); + + // todo does having a sweep on shapeA help with bullets? + b3Sweep sweepA = b3MakeRelativeSweep( bodySim, continuousContext->base ); + + // Time of impact versus shape. Supports all shape types + b3TOIOutput output = b3ShapeTimeOfImpact( shape, fastShape, &sweepA, &continuousContext->sweep, continuousContext->fraction ); + if ( isSensor ) + { + // Only accept a sensor hit that is sooner than the current solid hit. + if ( output.fraction <= continuousContext->fraction && continuousContext->sensorCount < B2_MAX_CONTINUOUS_SENSOR_HITS ) + { + int index = continuousContext->sensorCount; + + // The hit shape is a sensor + b3SensorHit sensorHit = { + .sensorId = shape->id, + .visitorId = fastShape->id, + }; + + continuousContext->sensorHits[index] = sensorHit; + continuousContext->sensorFractions[index] = output.fraction; + continuousContext->sensorCount += 1; + } + } + else if ( 0.0f < output.fraction && output.fraction < continuousContext->fraction ) + { + bool didHit = true; + + if ( didHit && ( ( shape->flags & b3_enablePreSolveEvents ) || ( fastShape->flags & b3_enablePreSolveEvents ) ) ) + { + b3ShapeId shapeIdA = { shape->id + 1, world->worldId, shape->generation }; + b3ShapeId shapeIdB = { fastShape->id + 1, world->worldId, fastShape->generation }; + b3Pos point = b3OffsetPos( continuousContext->base, output.point ); + didHit = world->preSolveFcn( shapeIdA, shapeIdB, point, output.normal, world->preSolveContext ); + } + + if ( didHit ) + { + fastBodySim->flags |= b3_hadTimeOfImpact; + continuousContext->fraction = output.fraction; + continuousContext->distanceIterations = b3MaxInt( continuousContext->distanceIterations, output.distanceIterations ); + continuousContext->pushBackIterations = b3MaxInt( continuousContext->pushBackIterations, output.pushBackIterations ); + continuousContext->rootIterations = b3MaxInt( continuousContext->rootIterations, output.rootIterations ); + } + } + + float ms = b3GetMilliseconds( ticks ); + if ( ms > 1000.0f * b3GetStallThreshold() ) + { + const char* nameFast = b3FindNameWithDefault( &world->names, fastBody->nameId, "NULL" ); + const char* name = b3FindNameWithDefault( &world->names, body->nameId, "NULL" ); + b3Log( "CCD stall: duration %.1f ms for %s versus %s", ms, nameFast, name ); + } + + // Continue query + return true; +} + +// Continuous collision of dynamic versus static +static void b3SolveContinuous( b3World* world, int bodySimIndex, b3TaskContext* taskContext ) +{ + b3TracyCZoneNC( ccd, "CCD", b3_colorDarkGoldenRod, true ); + + uint64_t ticks = b3GetTicks(); + + b3SolverSet* awakeSet = b3Array_Get( world->solverSets, b3_awakeSet ); + b3BodySim* fastBodySim = b3Array_Get( awakeSet->bodySims, bodySimIndex ); + B3_ASSERT( fastBodySim->flags & b3_isFast ); + + // Re-center the sweep on the fast body so the TOI and the swept query stay in float precision + b3Pos base = fastBodySim->center0; + + b3Sweep sweep = b3MakeRelativeSweep( fastBodySim, base ); + + b3Transform xf1; + xf1.q = sweep.q1; + xf1.p = b3Sub( sweep.c1, b3RotateVector( sweep.q1, sweep.localCenter ) ); + + b3Transform xf2; + xf2.q = sweep.q2; + xf2.p = b3Sub( sweep.c2, b3RotateVector( sweep.q2, sweep.localCenter ) ); + + b3DynamicTree* staticTree = world->broadPhase.trees + b3_staticBody; + b3DynamicTree* kinematicTree = world->broadPhase.trees + b3_kinematicBody; + b3DynamicTree* dynamicTree = world->broadPhase.trees + b3_dynamicBody; + b3Body* fastBody = b3Array_Get( world->bodies, fastBodySim->bodyId ); + + b3ContinuousContext context = { 0 }; + context.world = world; + context.sweep = sweep; + context.base = base; + context.fastBodySim = fastBodySim; + context.fraction = 1.0f; + + bool isBullet = ( fastBodySim->flags & b3_isBullet ) != 0; + + int shapeId = fastBody->headShapeId; + while ( shapeId != B3_NULL_INDEX ) + { + b3Shape* fastShape = b3Array_Get( world->shapes, shapeId ); + shapeId = fastShape->nextShapeId; + + context.fastShape = fastShape; + context.centroid1 = b3TransformPoint( xf1, fastShape->localCentroid ); + context.centroid2 = b3TransformPoint( xf2, fastShape->localCentroid ); + + b3AABB box1 = fastShape->aabb; + // xf2 is relative to the base, so translate the box back to world space, rounding outward + b3AABB box2 = b3OffsetAABB( b3ComputeShapeAABB( fastShape, xf2 ), base ); + + // Store this to avoid double computation in the case there is no impact event + fastShape->aabb = box2; + + // No continuous collision for meshes + if ( fastShape->type == b3_meshShape || fastShape->type == b3_heightShape ) + { + continue; + } + + // No continuous collision for sensors + if ( fastShape->sensorIndex != B3_NULL_INDEX ) + { + continue; + } + + b3AABB sweptBox = b3AABB_Union( box1, box2 ); + b3DynamicTree_Query( staticTree, sweptBox, B3_DEFAULT_MASK_BITS, false, b3ContinuousQueryCallback, &context ); + + if ( isBullet ) + { + b3DynamicTree_Query( kinematicTree, sweptBox, B3_DEFAULT_MASK_BITS, false, b3ContinuousQueryCallback, &context ); + b3DynamicTree_Query( dynamicTree, sweptBox, B3_DEFAULT_MASK_BITS, false, b3ContinuousQueryCallback, &context ); + } + } + + const float speculativeScalar = B3_SPECULATIVE_DISTANCE; + + if ( context.fraction < 1.0f ) + { + // Handle time of impact event. The sweep is relative to the base, so re-add the base + // to return the advanced pose to world space. + b3Quat q = b3NLerp( sweep.q1, sweep.q2, context.fraction ); + b3Vec3 c = b3Lerp( sweep.c1, sweep.c2, context.fraction ); + b3Vec3 origin = b3Sub( c, b3RotateVector( q, sweep.localCenter ) ); + + // Advance body + b3WorldTransform transform = { b3OffsetPos( base, origin ), q }; + b3Pos center = b3OffsetPos( base, c ); + fastBodySim->transform = transform; + fastBodySim->center = center; + fastBodySim->rotation0 = q; + fastBodySim->center0 = center; + + // The move event was written before CCD, so correct it with the impact pose + b3BodyMoveEvent* event = b3Array_Get( world->bodyMoveEvents, bodySimIndex ); + event->transform = fastBodySim->transform; + + // Prepare AABBs for broad-phase. + // Even though a body is fast, it may not move much. So the AABB may not need enlargement. + + shapeId = fastBody->headShapeId; + while ( shapeId != B3_NULL_INDEX ) + { + b3Shape* shape = b3Array_Get( world->shapes, shapeId ); + + // Must recompute aabb at the interpolated transform + b3AABB aabb = b3ComputeFatShapeAABB( shape, transform, speculativeScalar ); + shape->aabb = aabb; + + if ( b3AABB_Contains( shape->fatAABB, aabb ) == false ) + { + float marginScalar = shape->aabbMargin; + b3Vec3 aabbMargin = { marginScalar, marginScalar, marginScalar }; + shape->fatAABB = (b3AABB){ b3Sub( aabb.lowerBound, aabbMargin ), b3Add( aabb.upperBound, aabbMargin ) }; + + shape->flags |= b3_enlargedAABB; + fastBodySim->flags |= b3_enlargeBounds; + } + + shapeId = shape->nextShapeId; + } + } + else + { + // No time of impact event + + // Advance body + fastBodySim->rotation0 = fastBodySim->transform.q; + fastBodySim->center0 = fastBodySim->center; + + // Prepare AABBs for broad-phase + shapeId = fastBody->headShapeId; + while ( shapeId != B3_NULL_INDEX ) + { + b3Shape* shape = b3Array_Get( world->shapes, shapeId ); + + // shape->aabb is still valid from above + + if ( b3AABB_Contains( shape->fatAABB, shape->aabb ) == false ) + { + float marginScalar = shape->aabbMargin; + b3Vec3 aabbMargin = { marginScalar, marginScalar, marginScalar }; + shape->fatAABB = (b3AABB){ + .lowerBound = b3Sub( shape->aabb.lowerBound, aabbMargin ), + .upperBound = b3Add( shape->aabb.upperBound, aabbMargin ), + }; + + shape->flags |= b3_enlargedAABB; + fastBodySim->flags |= b3_enlargeBounds; + } + + shapeId = shape->nextShapeId; + } + } + + // Push sensor hits on the the task context for serial processing. + for ( int i = 0; i < context.sensorCount; ++i ) + { + // Skip any sensor hits that occurred after a solid hit + if ( context.sensorFractions[i] < context.fraction ) + { + b3Array_Push( taskContext->sensorHits, context.sensorHits[i] ); + } + } + + taskContext->distanceIterations = b3MaxInt( taskContext->distanceIterations, context.distanceIterations ); + taskContext->pushBackIterations = b3MaxInt( taskContext->pushBackIterations, context.pushBackIterations ); + taskContext->rootIterations = b3MaxInt( taskContext->rootIterations, context.rootIterations ); + + float ms = b3GetMilliseconds( ticks ); + if ( ms > 1000.0f * b3GetStallThreshold() ) + { + const char* nameFast = b3FindNameWithDefault( &world->names, fastBody->nameId, "NULL" ); + b3Vec3 c1 = sweep.c1; + b3Vec3 c2 = sweep.c2; + int vc = context.visitCount; + b3Log( "CCD stall: duration %.1f ms and visit count %d for %s: c1 = (%g, %g, %g), c2 = (%g, %g, %g)", ms, vc, nameFast, + c1.x, c1.y, c1.z, c2.x, c2.y, c2.z ); + } + + b3TracyCZoneEnd( ccd ); +} + +// Implements b3ParallelForCallback +static void b3FinalizeBodiesTask( int startIndex, int endIndex, int workerIndex, void* context ) +{ + b3TracyCZoneNC( finalize_bodies, "Finalize", b3_colorMediumSeaGreen, true ); + + b3StepContext* stepContext = (b3StepContext*)context; + b3World* world = stepContext->world; + b3Body* bodies = world->bodies.data; + b3BodySim* sims = stepContext->sims; + b3BodyState* states = stepContext->states; + + B3_ASSERT( endIndex <= world->bodyMoveEvents.count ); + + bool enableSleep = world->enableSleep; + bool enableContinuous = world->enableContinuous; + float timeStep = stepContext->dt; + float invTimeStep = stepContext->inv_dt; + uint16_t worldId = world->worldId; + + // The body move event array has should already have the correct size + b3BodyMoveEvent* moveEvents = world->bodyMoveEvents.data; + + b3TaskContext* taskContext = world->taskContexts.data + workerIndex; + b3BitSet* enlargedSimBitSet = &taskContext->enlargedSimBitSet; + b3BitSet* awakeIslandBitSet = &taskContext->awakeIslandBitSet; + + const float speculativeScalar = B3_SPECULATIVE_DISTANCE; + + for ( int simIndex = startIndex; simIndex < endIndex; ++simIndex ) + { + b3BodyState* state = states + simIndex; + b3BodySim* sim = sims + simIndex; + + b3Vec3 v = state->linearVelocity; + b3Vec3 w = state->angularVelocity; + b3Vec3 localOmega = b3InvRotateVector( sim->transform.q, w ); + b3Vec3 localDeltaRotation = b3InvRotateVector( sim->transform.q, state->deltaRotation.v ); + + if ( b3IsValidVec3( v ) == false || b3IsValidVec3( w ) == false ) + { + const char* name = b3FindNameWithDefault( &world->names, bodies[sim->bodyId].nameId, "NULL" ); + b3Log( "unstable: %s", name ); + } + + B3_ASSERT( b3IsValidVec3( v ) ); + B3_ASSERT( b3IsValidVec3( w ) ); + + sim->center = b3OffsetPos( sim->center, state->deltaPosition ); + sim->transform.q = b3NormalizeQuat( b3MulQuat( state->deltaRotation, sim->transform.q ) ); + + // Use the velocity of the farthest point on the body to account for rotation. + b3Vec3 velocityArc = b3ModifiedCross( b3Abs( localOmega ), sim->maxExtent ); + float maxVelocity = b3Length( v ) + b3Length( velocityArc ); + + // Sleep needs to observe position correction as well as true velocity. + // q = [sin(theta/2) * v, cos(theta/2)] + // for small angles abs(theta) ~= 2 * length(sin(theta/2) * v) + b3Vec3 rotationArc = b3ModifiedCross( b3Abs( localDeltaRotation ), sim->maxExtent ); + float maxDeltaPosition = b3Length( state->deltaPosition ) + 2.0f * b3Length( rotationArc ); + + // Position correction is not as important for sleep as true velocity. + float positionSleepFactor = 0.5f; + float sleepVelocity = b3MaxFloat( maxVelocity, positionSleepFactor * invTimeStep * maxDeltaPosition ); + + // reset state deltas + state->deltaPosition = b3Vec3_zero; + state->deltaRotation = b3Quat_identity; + + sim->transform.p = b3OffsetPos( sim->center, b3Neg( b3RotateVector( sim->transform.q, sim->localCenter ) ) ); + + // cache miss here, however I need the shape list below + b3Body* body = bodies + sim->bodyId; + body->bodyMoveIndex = simIndex; + body->sleepVelocity = sleepVelocity; + + moveEvents[simIndex].userData = body->userData; + moveEvents[simIndex].transform = sim->transform; + moveEvents[simIndex].bodyId = (b3BodyId){ sim->bodyId + 1, worldId, body->generation }; + moveEvents[simIndex].fellAsleep = false; + + // reset applied force and torque + sim->force = b3Vec3_zero; + sim->torque = b3Vec3_zero; + + // If you hit this then it means you deferred mass computation but never called b3Body_ApplyMassFromShapes + // or b3Body_SetMassData. + B3_ASSERT( ( body->flags & b3_dirtyMass ) == 0 ); + + body->flags &= ~b3_bodyTransientFlags; + body->flags |= ( sim->flags & ( b3_isSpeedCapped | b3_hadTimeOfImpact ) ); + body->flags |= ( state->flags & ( b3_isSpeedCapped | b3_hadTimeOfImpact ) ); + sim->flags &= ~b3_bodyTransientFlags; + state->flags &= ~b3_bodyTransientFlags; + + if ( enableSleep == false || ( body->flags & b3_enableSleep ) == 0 || sleepVelocity > body->sleepThreshold ) + { + // Body is not sleepy + body->sleepTime = 0.0f; + + const float safetyFactor = 0.5f; + float maxMotion = b3MaxFloat( maxDeltaPosition, maxVelocity * timeStep ); + if ( body->type == b3_dynamicBody && enableContinuous && maxMotion > safetyFactor * sim->minExtent ) + { + // This flag is only retained for debug draw + sim->flags |= b3_isFast; + + // Store in fast array for the continuous collision stage + // This is deterministic because the order of TOI sweeps doesn't matter + if ( sim->flags & b3_isBullet ) + { + int bulletIndex = b3AtomicFetchAddInt( &stepContext->bulletBodyCount, 1 ); + stepContext->bulletBodies[bulletIndex] = simIndex; + } + else + { + b3SolveContinuous( world, simIndex, taskContext ); + } + } + else + { + // Body is safe to advance + sim->center0 = sim->center; + sim->rotation0 = sim->transform.q; + } + } + else + { + // Body is safe to advance and is falling asleep + sim->center0 = sim->center; + sim->rotation0 = sim->transform.q; + body->sleepTime += timeStep; + } + + // Update world space inverse inertia tensor. + b3Matrix3 rotationMatrix = b3MakeMatrixFromQuat( sim->transform.q ); + sim->invInertiaWorld = b3MulMM( b3MulMM( rotationMatrix, sim->invInertiaLocal ), b3Transpose( rotationMatrix ) ); + + // Any single body in an island can keep it awake + b3Island* island = b3Array_Get( world->islands, body->islandId ); + if ( body->sleepTime < B3_TIME_TO_SLEEP ) + { + // keep island awake + int islandIndex = island->localIndex; + b3SetBit( awakeIslandBitSet, islandIndex ); + } + else if ( island->constraintRemoveCount > 0 ) + { + // Body wants to sleep but its island needs splitting first. Track the sleepiest candidate. + // Break sleep time ties using the island id to ensure determinism. The cross worker reduction + // breaks ties the same way. + if ( body->sleepTime > taskContext->splitSleepTime || + ( body->sleepTime == taskContext->splitSleepTime && body->islandId > taskContext->splitIslandId ) ) + { + // pick the sleepiest candidate + taskContext->splitIslandId = body->islandId; + taskContext->splitSleepTime = body->sleepTime; + } + } + + // Update shapes AABBs + b3WorldTransform transform = sim->transform; + bool isFast = ( sim->flags & b3_isFast ) != 0; + int shapeId = body->headShapeId; + while ( shapeId != B3_NULL_INDEX ) + { + b3Shape* shape = b3Array_Get( world->shapes, shapeId ); + + if ( isFast ) + { + // For fast non-bullet bodies the AABB has already been updated in b3SolveContinuous + // For fast bullet bodies the AABB will be updated at a later stage + + // Add to enlarged shapes regardless of AABB changes. + // Bit-set to keep the move array sorted + b3SetBit( enlargedSimBitSet, simIndex ); + } + else + { + b3AABB aabb = b3ComputeFatShapeAABB( shape, transform, speculativeScalar ); + shape->aabb = aabb; + + B3_ASSERT( ( shape->flags & b3_enlargedAABB ) == 0 ); + + if ( b3AABB_Contains( shape->fatAABB, aabb ) == false ) + { + float marginScalar = shape->aabbMargin; + b3Vec3 aabbMargin = { marginScalar, marginScalar, marginScalar }; + shape->fatAABB = (b3AABB){ b3Sub( aabb.lowerBound, aabbMargin ), b3Add( aabb.upperBound, aabbMargin ) }; + shape->flags |= b3_enlargedAABB; + + // Bit-set to keep the move array sorted + b3SetBit( enlargedSimBitSet, simIndex ); + } + } + + shapeId = shape->nextShapeId; + } + } + + b3TracyCZoneEnd( finalize_bodies ); +} + +typedef struct b3BlockDim +{ + // number of items per block (except last block) + int size; + + // total number of blocks + int count; +} b3BlockDim; + +// A block is a range of tasks, a start index and count as a sub-array. Each worker receives at +// most M blocks of work. The workers may receive less blocks if there is not sufficient work. +// Each block of work has a minimum number of elements (block size). This in turn may limit the +// number of blocks. If there are many elements then the block size is increased so there are +// still at most M blocks of work per worker. M is a tunable number that has two goals: +// 1. keep M small to reduce overhead +// 2. keep M large enough for other workers to be able to steal work +// The block size is a power of two to make math efficient. +static inline b3BlockDim b3ComputeBlockCount( int itemCount, int minSize, int maxBlockCount ) +{ + b3BlockDim dim = { 0 }; + if ( itemCount == 0 ) + { + return dim; + } + + if ( itemCount <= minSize * maxBlockCount ) + { + dim.size = minSize; + } + else + { + dim.size = ( itemCount + maxBlockCount - 1 ) / maxBlockCount; + } + + dim.count = ( itemCount + dim.size - 1 ) / dim.size; + + B3_ASSERT( dim.count >= 1 ); + B3_ASSERT( dim.size * dim.count >= itemCount ); + + return dim; +} + +// Initialize solver blocks for a contiguous range of items. Computes block size internally +// from the same parameters used by b3ComputeBlockCount. The atomic claim counter is zeroed +// so workers can CAS (0, 1) on the first stage that owns these blocks. +static void b3InitBlocks( b3SyncBlock* blocks, b3BlockDim dim, int itemCount, uint8_t blockType, uint8_t colorIndex ) +{ + if ( dim.count == 0 ) + { + return; + } + + B3_ASSERT( itemCount >= dim.count ); + + // Compute the number of elements per block + int blockSize = dim.size; + + // Simulation too big + B3_ASSERT( blockSize <= UINT16_MAX ); + + for ( int i = 0; i < dim.count; ++i ) + { + blocks[i].block.startIndex = i * blockSize; + blocks[i].block.count = (uint16_t)blockSize; + blocks[i].block.blockType = blockType; + blocks[i].block.colorIndex = colorIndex; + b3AtomicStoreInt( &blocks[i].syncIndex, 0 ); + } + + // The last block may not be full + blocks[dim.count - 1].block.count = (uint16_t)( itemCount - ( dim.count - 1 ) * blockSize ); + + B3_VALIDATE( blocks[dim.count - 1].block.count <= blockSize ); + B3_VALIDATE( ( dim.count - 1 ) * dim.size + blocks[dim.count - 1].block.count == itemCount ); +} + +static inline b3SolverStage* b3InitStage( b3SolverStage* stage, b3SolverStageType type, b3SyncBlock* blocks, int blockCount, + uint8_t colorIndex ) +{ + stage->type = type; + stage->blocks = blocks; + stage->blockCount = blockCount; + stage->colorIndex = colorIndex; + b3AtomicStoreInt( &stage->completionCount, 0 ); + return stage + 1; +} + +// Initialize one stage per color for each iteration. Used for warm start, solve, relax, and restitution. +// All iterations of a given color share the same b3SyncBlock array so the per-block syncIndex +// grows monotonically across stages within that color. +static b3SolverStage* b3InitColorStages( b3SolverStage* stage, b3SolverStageType type, int iterations, int activeColorCount, + b3SyncBlock** colorBlocks, int* colorBlockCounts, int* activeColorIndices ) +{ + for ( int j = 0; j < iterations; ++j ) + { + for ( int i = 0; i < activeColorCount; ++i ) + { + stage = b3InitStage( stage, type, colorBlocks[i], colorBlockCounts[i], (uint8_t)activeColorIndices[i] ); + } + } + return stage; +} + +static void b3ExecuteBlock( b3SolverStage* stage, b3StepContext* context, b3SolverBlock block, int workerIndex ) +{ + b3SolverStageType stageType = stage->type; + b3SolverBlockType blockType = (b3SolverBlockType)block.blockType; + + switch ( stageType ) + { + case b3_stagePrepareJoints: + b3PrepareJointsTask( block, context ); + break; + + case b3_stagePrepareWideContacts: + b3PrepareContacts_Convex( block, context ); + break; + + case b3_stagePrepareContacts: + b3PrepareContacts_Mesh( block, context ); + break; + + case b3_stageIntegrateVelocities: + b3IntegrateVelocitiesTask( block, context ); + break; + + case b3_stageWarmStart: + if ( blockType == b3_graphJointBlock ) + { + b3WarmStartJointsTask( block, context ); + } + else if ( blockType == b3_graphWideContactBlock ) + { + b3WarmStartContacts_Convex( block, context ); + } + else + { + b3WarmStartContacts_Mesh( block, context ); + } + break; + + case b3_stageSolve: + if ( blockType == b3_graphJointBlock ) + { + bool useBias = true; + b3SolveJointsTask( block, context, useBias, workerIndex ); + } + else if ( blockType == b3_graphWideContactBlock ) + { + bool useBias = true; + b3SolveContacts_Convex( block, context, useBias ); + } + else + { + bool useBias = true; + b3SolveContacts_Mesh( block, context, useBias ); + } + break; + + case b3_stageIntegratePositions: + b3IntegratePositionsTask( block, context ); + break; + + case b3_stageRelax: + if ( blockType == b3_graphJointBlock ) + { + bool useBias = false; + b3SolveJointsTask( block, context, useBias, workerIndex ); + } + else if ( blockType == b3_graphWideContactBlock ) + { + bool useBias = false; + b3SolveContacts_Convex( block, context, useBias ); + } + else + { + bool useBias = false; + b3SolveContacts_Mesh( block, context, useBias ); + } + break; + + case b3_stageRestitution: + if ( blockType == b3_graphWideContactBlock ) + { + b3ApplyRestitution_Convex( block, context ); + } + else if ( blockType == b3_graphContactBlock ) + { + b3ApplyRestitution_Mesh( block, context ); + } + break; + + case b3_stageStoreWideImpulses: + b3StoreImpulses_Convex( block, context, workerIndex ); + break; + + case b3_stageStoreImpulses: + b3StoreImpulses_Mesh( block, context, workerIndex ); + break; + } +} + +// This staggers the worker start indices so they avoid touching the same solver blocks +static inline int GetWorkerStartIndex( int workerIndex, int blockCount, int workerCount ) +{ + if ( blockCount <= workerCount ) + { + return workerIndex < blockCount ? workerIndex : B3_NULL_INDEX; + } + + int blocksPerWorker = blockCount / workerCount; + int remainder = blockCount - blocksPerWorker * workerCount; + return blocksPerWorker * workerIndex + b3MinInt( remainder, workerIndex ); +} + +// Execute a stage, which is an array of solver blocks, each controlled with an atomic sync index. +// Each worker starts at its home index and sweeps the ring, CAS-claiming any unclaimed blocks. +static void b3ExecuteStage( b3SolverStage* stage, b3StepContext* context, int previousSyncIndex, int syncIndex, int workerIndex ) +{ + int completedCount = 0; + b3SyncBlock* blocks = stage->blocks; + int blockCount = stage->blockCount; + + int startIndex = GetWorkerStartIndex( workerIndex, blockCount, context->workerCount ); + if ( startIndex == B3_NULL_INDEX ) + { + return; + } + + B3_ASSERT( 0 <= startIndex && startIndex < blockCount ); + + int blockIndex = startIndex; + for ( int i = 0; i < blockCount; ++i ) + { + if ( b3AtomicCompareExchangeInt( &blocks[blockIndex].syncIndex, previousSyncIndex, syncIndex ) ) + { + B3_ASSERT( completedCount < blockCount ); + + // Pass the descriptor by value -- the wrapping b3SyncBlock holds the atomic + // syncIndex but we only copy .block, so the struct copy never aliases the CAS target. + b3ExecuteBlock( stage, context, blocks[blockIndex].block, workerIndex ); + completedCount += 1; + } + + blockIndex += 1; + if ( blockIndex >= blockCount ) + { + blockIndex = 0; + } + } + + (void)b3AtomicFetchAddInt( &stage->completionCount, completedCount ); +} + +// Execute a stage on worker 0 (main thread). +static void b3ExecuteMainStage( b3SolverStage* stage, b3StepContext* context, uint32_t syncBits ) +{ + int blockCount = stage->blockCount; + if ( blockCount == 0 ) + { + return; + } + + const int workerIndex = 0; + + if ( blockCount == 1 ) + { + b3ExecuteBlock( stage, context, stage->blocks[0].block, workerIndex ); + } + else + { + b3AtomicStoreU32( &context->atomicSyncBits, syncBits ); + + int syncIndex = ( syncBits >> 16 ) & 0xFFFF; + B3_ASSERT( syncIndex > 0 ); + int previousSyncIndex = syncIndex - 1; + + b3ExecuteStage( stage, context, previousSyncIndex, syncIndex, workerIndex ); + + // Spin waiting for thieves to finish + while ( b3AtomicLoadInt( &stage->completionCount ) != blockCount ) + { + b3Pause(); + } + + b3AtomicStoreInt( &stage->completionCount, 0 ); + } +} + +// Parallel solver task +static void b3SolverTask( void* taskContext ) +{ + b3WorkerContext* workerContext = (b3WorkerContext*)taskContext; + int workerIndex = workerContext->workerIndex; + b3StepContext* context = workerContext->context; + int activeColorCount = context->activeColorCount; + b3SolverStage* stages = context->stages; + b3Profile* profile = &context->world->profile; + + if ( workerIndex == 0 ) + { + // The orchestrator slot is a race. The calling thread of b3World_Step also enters here + // as worker 0, so progress is guaranteed even if the user's task system schedules tasks + // out of order, has fewer threads than workerCount, or runs the task synchronously + // inside enqueueTaskFcn. Whoever wins the CAS becomes the orchestrator; the loser + // returns and lets the spinner-only path handle workers >0. + if ( b3AtomicCompareExchangeInt( &context->mainClaimed, 0, 1 ) == false ) + { + return; + } + + // Main thread synchronizes the workers and does work itself. + // + // This needs to be a task for the main thread because the user's task system may execute + // the tasks serially and this is the first task. This single task is able to fully + // complete all work even if all other workers are blocked. + + // Stages are re-used by loops so that I don't need more stages for large substep counts. + // The sync indices grow monotonically for the body/graph/constraint groupings because they share solver blocks. + // The stage index and sync indices are combined in to sync bits for atomic synchronization. + // The workers need to compute the previous sync index for a given stage so that CAS works correctly. This + // setup makes this easy to do. + + /* + b3_stagePrepareJoints, + b3_stagePrepareContacts, + b3_stageIntegrateVelocities, + b3_stageWarmStart, + b3_stageSolve, + b3_stageIntegratePositions, + b3_stageRelax, + b3_stageRestitution, + b3_stageStoreImpulses + */ + + uint64_t ticks = b3GetTicks(); + + int bodySyncIndex = 1; + int stageIndex = 0; + + // Prepare joint constraints + uint32_t jointSyncIndex = 1; + uint32_t syncBits = ( jointSyncIndex << 16 ) | stageIndex; + B3_ASSERT( stages[stageIndex].type == b3_stagePrepareJoints ); + b3ExecuteMainStage( stages + stageIndex, context, syncBits ); + stageIndex += 1; + jointSyncIndex += 1; + + // Prepare convex contact constraints + uint32_t convexSyncIndex = 1; + syncBits = ( convexSyncIndex << 16 ) | stageIndex; + B3_ASSERT( stages[stageIndex].type == b3_stagePrepareWideContacts ); + b3ExecuteMainStage( stages + stageIndex, context, syncBits ); + stageIndex += 1; + convexSyncIndex += 1; + + // Prepare mesh contact constraints + uint32_t meshSyncIndex = 1; + syncBits = ( meshSyncIndex << 16 ) | stageIndex; + B3_ASSERT( stages[stageIndex].type == b3_stagePrepareContacts ); + b3ExecuteMainStage( stages + stageIndex, context, syncBits ); + stageIndex += 1; + meshSyncIndex += 1; + + // Single-threaded overflow work. These constraints don't fit in the graph coloring. + b3PrepareJoints_Overflow( context ); + b3PrepareContacts_Overflow( context ); + + profile->prepareConstraints += b3GetMillisecondsAndReset( &ticks ); + + int graphSyncIndex = 1; + int subStepCount = context->subStepCount; + for ( int subStepIndex = 0; subStepIndex < subStepCount; ++subStepIndex ) + { + // stageIndex restarted each iteration + // syncBits still increases monotonically because the upper bits increase each iteration + int iterationStageIndex = stageIndex; + + // Integrate velocities + syncBits = ( bodySyncIndex << 16 ) | iterationStageIndex; + B3_ASSERT( stages[iterationStageIndex].type == b3_stageIntegrateVelocities ); + b3ExecuteMainStage( stages + iterationStageIndex, context, syncBits ); + iterationStageIndex += 1; + bodySyncIndex += 1; + + profile->integrateVelocities += b3GetMillisecondsAndReset( &ticks ); + + // Warm start constraints + b3WarmStartJoints_Overflow( context ); + b3WarmStartContacts_Overflow( context ); + + for ( int colorIndex = 0; colorIndex < activeColorCount; ++colorIndex ) + { + syncBits = ( graphSyncIndex << 16 ) | iterationStageIndex; + B3_ASSERT( stages[iterationStageIndex].type == b3_stageWarmStart ); + b3ExecuteMainStage( stages + iterationStageIndex, context, syncBits ); + iterationStageIndex += 1; + } + graphSyncIndex += 1; + + profile->warmStart += b3GetMillisecondsAndReset( &ticks ); + + // Solve constraints + bool useBias = true; + for ( int j = 0; j < ITERATIONS; ++j ) + { + // Overflow constraints have lower priority. Typically these are dynamic-vs-dynamic. + b3SolveJoints_Overflow( context, useBias ); + b3SolveContacts_Overflow( context, useBias ); + + for ( int colorIndex = 0; colorIndex < activeColorCount; ++colorIndex ) + { + syncBits = ( graphSyncIndex << 16 ) | iterationStageIndex; + B3_ASSERT( stages[iterationStageIndex].type == b3_stageSolve ); + b3ExecuteMainStage( stages + iterationStageIndex, context, syncBits ); + iterationStageIndex += 1; + } + graphSyncIndex += 1; + } + + profile->solveImpulses += b3GetMillisecondsAndReset( &ticks ); + + // Integrate positions + B3_ASSERT( stages[iterationStageIndex].type == b3_stageIntegratePositions ); + syncBits = ( bodySyncIndex << 16 ) | iterationStageIndex; + b3ExecuteMainStage( stages + iterationStageIndex, context, syncBits ); + iterationStageIndex += 1; + bodySyncIndex += 1; + + profile->integratePositions += b3GetMillisecondsAndReset( &ticks ); + + // Relax constraints + useBias = false; + for ( int j = 0; j < RELAX_ITERATIONS; ++j ) + { + b3SolveJoints_Overflow( context, useBias ); + b3SolveContacts_Overflow( context, useBias ); + + for ( int colorIndex = 0; colorIndex < activeColorCount; ++colorIndex ) + { + syncBits = ( graphSyncIndex << 16 ) | iterationStageIndex; + B3_ASSERT( stages[iterationStageIndex].type == b3_stageRelax ); + b3ExecuteMainStage( stages + iterationStageIndex, context, syncBits ); + iterationStageIndex += 1; + } + graphSyncIndex += 1; + } + + profile->relaxImpulses += b3GetMillisecondsAndReset( &ticks ); + } + + // Advance the stage according to the sub-stepping tasks just completed + // integrate velocities / warm start / solve / integrate positions / relax + stageIndex += 1 + activeColorCount + ITERATIONS * activeColorCount + 1 + RELAX_ITERATIONS * activeColorCount; + + // Restitution + { + b3ApplyRestitution_Overflow( context ); + + int iterStageIndex = stageIndex; + for ( int colorIndex = 0; colorIndex < activeColorCount; ++colorIndex ) + { + syncBits = ( graphSyncIndex << 16 ) | iterStageIndex; + B3_ASSERT( stages[iterStageIndex].type == b3_stageRestitution ); + b3ExecuteMainStage( stages + iterStageIndex, context, syncBits ); + iterStageIndex += 1; + } + // graphSyncIndex += 1; + stageIndex += activeColorCount; + } + + profile->applyRestitution += b3GetMillisecondsAndReset( &ticks ); + + // Store impulses + b3StoreImpulses_Overflow( context ); + + syncBits = ( convexSyncIndex << 16 ) | stageIndex; + B3_ASSERT( stages[stageIndex].type == b3_stageStoreWideImpulses ); + b3ExecuteMainStage( stages + stageIndex, context, syncBits ); + stageIndex += 1; + + syncBits = ( meshSyncIndex << 16 ) | stageIndex; + B3_ASSERT( stages[stageIndex].type == b3_stageStoreImpulses ); + b3ExecuteMainStage( stages + stageIndex, context, syncBits ); + stageIndex += 1; + + profile->storeImpulses += b3GetMillisecondsAndReset( &ticks ); + + // Signal workers to finish + b3AtomicStoreU32( &context->atomicSyncBits, UINT_MAX ); + + B3_ASSERT( stageIndex == context->stageCount ); + return; + } + + // Worker spins and waits for work + uint32_t lastSyncBits = 0; + // uint64_t maxSpinTime = 10; + while ( true ) + { + // Spin until main thread bumps changes the sync bits. This can waste significant time overall, but it is necessary for + // parallel simulation with graph coloring. + // todo improve this spinner + uint32_t syncBits; + int spinCount = 0; + while ( ( syncBits = b3AtomicLoadU32( &context->atomicSyncBits ) ) == lastSyncBits ) + { + if ( spinCount > 5 ) + { + b3Yield(); + spinCount = 0; + } + else + { + // Using the cycle counter helps to account for variation in mm_pause timing across different + // CPUs. However, this is X64 only. + // uint64_t prev = __rdtsc(); + // do + //{ + // b3Pause(); + //} + // while ((__rdtsc() - prev) < maxSpinTime); + // maxSpinTime += 10; + b3Pause(); + b3Pause(); + spinCount += 1; + } + } + + if ( syncBits == UINT_MAX ) + { + // sentinel hit + break; + } + + int stageIndex = syncBits & 0xFFFF; + B3_ASSERT( stageIndex < context->stageCount ); + + int syncIndex = ( syncBits >> 16 ) & 0xFFFF; + B3_ASSERT( syncIndex > 0 ); + + int previousSyncIndex = syncIndex - 1; + + b3SolverStage* stage = stages + stageIndex; + b3ExecuteStage( stage, context, previousSyncIndex, syncIndex, workerIndex ); + + lastSyncBits = syncBits; + } +} + +static void b3BulletBodyTask( int startIndex, int endIndex, int workerIndex, void* context ) +{ + b3TracyCZoneNC( bullet_body_task, "Bullet Body Task", b3_colorLightSkyBlue, true ); + + b3StepContext* stepContext = (b3StepContext*)context; + b3TaskContext* taskContext = b3Array_Get( stepContext->world->taskContexts, workerIndex ); + + B3_ASSERT( startIndex <= endIndex ); + + for ( int i = startIndex; i < endIndex; ++i ) + { + int simIndex = stepContext->bulletBodies[i]; + b3SolveContinuous( stepContext->world, simIndex, taskContext ); + } + + b3TracyCZoneEnd( bullet_body_task ); +} + +#if B3_SIMD_WIDTH == 4 +#define B3_SIMD_SHIFT 2 +#else +#define B3_SIMD_SHIFT 0 +#endif + +// Solve with graph coloring +void b3Solve( b3World* world, b3StepContext* stepContext ) +{ + // Only count steps that advance the simulation + world->stepIndex += 1; + + b3SolverSet* awakeSet = b3Array_Get( world->solverSets, b3_awakeSet ); + int awakeBodyCount = awakeSet->bodySims.count; + if ( awakeBodyCount == 0 ) + { + b3ValidateNoEnlarged( &world->broadPhase ); + return; + } + + // Solve constraints using graph coloring + { + b3TracyCZoneNC( solver_setup, "Solver Setup", b3_colorDarkOrange, true ); + uint64_t setupTicks = b3GetTicks(); + + // Prepare buffers for continuous collision (fast bodies) + b3AtomicStoreInt( &stepContext->bulletBodyCount, 0 ); + stepContext->bulletBodies = (int*)b3StackAlloc( &world->stack, awakeBodyCount * sizeof( int ), "bullet bodies" ); + + b3ConstraintGraph* graph = &world->constraintGraph; + b3GraphColor* colors = graph->colors; + + stepContext->sims = awakeSet->bodySims.data; + stepContext->states = awakeSet->bodyStates.data; + + // count contacts, joints, and colors + int activeColorCount = 0; + for ( int i = 0; i < B3_GRAPH_COLOR_COUNT - 1; ++i ) + { + int perColorContactCount = colors[i].convexContacts.count + colors[i].contacts.count; + int perColorJointCount = colors[i].jointSims.count; + int occupancyCount = perColorContactCount + perColorJointCount; + activeColorCount += occupancyCount > 0 ? 1 : 0; + } + + // prepare for move events + b3Array_Resize( world->bodyMoveEvents, awakeBodyCount ); + + int workerCount = world->workerCount; + + // Target 4 blocks per worker to allow work stealing + const int maxBlockCount = 4 * workerCount; + + // Body blocks are for parallel iteration over bodies directly (integration, update transforms) + int minBodiesPerBlock = 32; + b3BlockDim bodyDim = b3ComputeBlockCount( awakeBodyCount, minBodiesPerBlock, maxBlockCount ); + + const int minContactsPerBlock = 4; + const int minJointsPerBlock = 4; + + // Configure blocks for tasks parallel-for each active graph color + // The blocks are a mix of convex contact, mesh contact, and joint blocks + int activeColorIndices[B3_GRAPH_COLOR_COUNT]; + int colorWideContactCounts[B3_GRAPH_COLOR_COUNT]; + int colorContactCounts[B3_GRAPH_COLOR_COUNT]; + // int colorManifoldCounts[B3_GRAPH_COLOR_COUNT]; + int colorJointCounts[B3_GRAPH_COLOR_COUNT]; + b3BlockDim graphWideContactDims[B3_GRAPH_COLOR_COUNT]; + b3BlockDim graphContactDims[B3_GRAPH_COLOR_COUNT]; + b3BlockDim graphJointDims[B3_GRAPH_COLOR_COUNT]; + int graphBlockCount = 0; + + // c is the active color index + int wideContactCount = 0; + int contactCount = 0; + int manifoldCount = 0; + int jointCount = 0; + int c = 0; + for ( int i = 0; i < B3_GRAPH_COLOR_COUNT - 1; ++i ) + { + b3GraphColor* color = colors + i; + int colorConvexContactCount = color->convexContacts.count; + int colorContactCount = color->contacts.count; + int colorJointCount = color->jointSims.count; + + if ( colorConvexContactCount + colorContactCount + colorJointCount == 0 ) + { + continue; + } + + activeColorIndices[c] = i; + + // Ceiling for wide constraint count + int colorWideConstraintCount = + colorConvexContactCount > 0 ? ( ( colorConvexContactCount - 1 ) >> B3_SIMD_SHIFT ) + 1 : 0; + wideContactCount += colorWideConstraintCount; + colorWideContactCounts[c] = colorWideConstraintCount; + + colorContactCounts[c] = colorContactCount; + contactCount += colorContactCount; + + // Compute manifold starts and accumulate manifold count + for ( int j = 0; j < colorContactCount; ++j ) + { + color->contacts.data[j].manifoldStart = manifoldCount; + manifoldCount += color->contacts.data[j].manifoldCount; + } + + colorJointCounts[c] = colorJointCount; + jointCount += colorJointCount; + + // Solver block dimensions + graphWideContactDims[c] = b3ComputeBlockCount( colorWideConstraintCount, minContactsPerBlock, maxBlockCount ); + graphContactDims[c] = b3ComputeBlockCount( colorContactCount, minContactsPerBlock, maxBlockCount ); + graphJointDims[c] = b3ComputeBlockCount( colorJointCount, minJointsPerBlock, maxBlockCount ); + graphBlockCount += graphWideContactDims[c].count + graphContactDims[c].count + graphJointDims[c].count; + + c += 1; + } + activeColorCount = c; + + // Prepare and store run as one flat parallel-for over the entire wide constraint range, + // partitioned into uniformly sized blocks. Color info is consulted inside the task via + // a small span array, so blocks do not need to honor color boundaries here. + b3BlockDim convexPrepareDim = b3ComputeBlockCount( wideContactCount, minContactsPerBlock, maxBlockCount ); + b3BlockDim meshPrepareDim = b3ComputeBlockCount( contactCount, minContactsPerBlock, maxBlockCount ); + b3BlockDim jointPrepareDim = b3ComputeBlockCount( jointCount, minJointsPerBlock, maxBlockCount ); + + int wideContactByteCount = b3GetWideContactConstraintByteCount(); + b3ContactConstraintWide* wideConstraints = + (b3ContactConstraintWide*)b3StackAlloc( &world->stack, wideContactCount * wideContactByteCount, "wide contacts" ); + b3ContactConstraint* contactConstraints = + (b3ContactConstraint*)b3StackAlloc( &world->stack, contactCount * sizeof( b3ContactConstraint ), "contacts" ); + b3ManifoldConstraint* manifoldConstraints = (b3ManifoldConstraint*)b3StackAlloc( + &world->stack, manifoldCount * sizeof( b3ManifoldConstraint ), "manifold constraints" ); + + b3GraphColor* overflow = colors + B3_OVERFLOW_INDEX; + int overflowCount = overflow->contacts.count; + int overflowManifoldCount = 0; + for ( int i = 0; i < overflowCount; ++i ) + { + overflow->contacts.data[i].manifoldStart = overflowManifoldCount; + overflowManifoldCount += overflow->contacts.data[i].manifoldCount; + } + + overflow->contactConstraints = (b3ContactConstraint*)b3StackAlloc( + &world->stack, overflowCount * sizeof( b3ContactConstraint ), "overflow contacts" ); + overflow->manifoldConstraints = (b3ManifoldConstraint*)b3StackAlloc( + &world->stack, overflowManifoldCount * sizeof( b3ManifoldConstraint ), "overflow manifolds" ); + + // Build the span table for the flat prepare/store parallel-for while I slice the + // wide constraint buffer across colors. One entry per active color plus a sentinel + // at wideContactCount. + b3WidePrepareSpan widePrepareSpans[B3_GRAPH_COLOR_COUNT + 1]; + b3ContactPrepareSpan contactPrepareSpans[B3_GRAPH_COLOR_COUNT + 1]; + b3JointPrepareSpan jointPrepareSpans[B3_GRAPH_COLOR_COUNT + 1]; + + // Distribute transient constraints to each graph color and prepare spans + // todo it might be simpler for solver blocks to index into the global arrays + { + int wideBase = 0; + int contactBase = 0; + int jointBase = 0; + for ( int i = 0; i < activeColorCount; ++i ) + { + int j = activeColorIndices[i]; + b3GraphColor* color = colors + j; + + int colorConvexContactCount = color->convexContacts.count; + widePrepareSpans[i].start = wideBase; + widePrepareSpans[i].count = colorConvexContactCount; + widePrepareSpans[i].contacts = color->convexContacts.data; + + if ( colorConvexContactCount == 0 ) + { + color->wideConstraints = NULL; + color->wideConstraintCount = 0; + } + else + { + color->wideConstraints = + (b3ContactConstraintWide*)( (uint8_t*)wideConstraints + wideBase * wideContactByteCount ); + + int colorContactCountW = ( ( colorConvexContactCount - 1 ) >> B3_SIMD_SHIFT ) + 1; + color->wideConstraintCount = colorContactCountW; + + // Zero remainder lanes in the tail wide slot so prepare workers don't need to + // initialize them. + if ( ( colorConvexContactCount & ( B3_SIMD_WIDTH - 1 ) ) != 0 ) + { + memset( (uint8_t*)color->wideConstraints + ( colorContactCountW - 1 ) * wideContactByteCount, 0, + wideContactByteCount ); + } + + wideBase += colorContactCountW; + } + + int colorContactCount = color->contacts.count; + contactPrepareSpans[i].start = contactBase; + contactPrepareSpans[i].count = colorContactCount; + contactPrepareSpans[i].contacts = color->contacts.data; + + if ( colorContactCount == 0 ) + { + color->contactConstraints = NULL; + color->contactConstraintCount = 0; + } + else + { + color->contactConstraints = contactConstraints + contactBase; + color->contactConstraintCount = colorContactCount; + contactBase += colorContactCount; + } + + jointPrepareSpans[i].start = jointBase; + jointPrepareSpans[i].count = color->jointSims.count; + jointPrepareSpans[i].joints = color->jointSims.data; + jointBase += color->jointSims.count; + } + + // Sentinels + widePrepareSpans[activeColorCount].start = wideContactCount; + widePrepareSpans[activeColorCount].count = 0; + widePrepareSpans[activeColorCount].contacts = NULL; + B3_ASSERT( wideBase == wideContactCount ); + + contactPrepareSpans[activeColorCount].start = contactCount; + contactPrepareSpans[activeColorCount].count = 0; + contactPrepareSpans[activeColorCount].contacts = NULL; + B3_ASSERT( contactBase == contactCount ); + + jointPrepareSpans[activeColorCount].start = jointCount; + jointPrepareSpans[activeColorCount].count = 0; + jointPrepareSpans[activeColorCount].joints = NULL; + B3_ASSERT( jointBase == jointCount ); + } + + //// Special span for overflow to allow for function re-use + b3ContactPrepareSpan overflowSpans[2] = { 0 }; + overflowSpans[0].start = 0; + overflowSpans[0].count = overflow->contacts.count; + overflowSpans[0].contacts = overflow->contacts.data; + overflowSpans[1].start = overflow->contacts.count; + overflowSpans[1].count = 0; + overflowSpans[1].contacts = NULL; + + int stageCount = 0; + + // b3_stagePrepareJoints + stageCount += 1; + // b3_stagePrepareWideContacts + stageCount += 1; + // b3_stagePrepareContacts + stageCount += 1; + // b3_stageIntegrateVelocities + stageCount += 1; + // b3_stageWarmStart + stageCount += activeColorCount; + // b3_stageSolve + stageCount += ITERATIONS * activeColorCount; + // b3_stageIntegratePositions + stageCount += 1; + // b3_stageRelax + stageCount += RELAX_ITERATIONS * activeColorCount; + // b3_stageRestitution + stageCount += activeColorCount; + // b3_stageStoreWideImpulses + stageCount += 1; + // b3_stageStoreImpulses + stageCount += 1; + + b3SolverStage* stages = (b3SolverStage*)b3StackAlloc( &world->stack, stageCount * sizeof( b3SolverStage ), "stages" ); + b3SyncBlock* bodyBlocks = + (b3SyncBlock*)b3StackAlloc( &world->stack, bodyDim.count * sizeof( b3SyncBlock ), "body blocks" ); + b3SyncBlock* convexBlocks = + (b3SyncBlock*)b3StackAlloc( &world->stack, convexPrepareDim.count * sizeof( b3SyncBlock ), "convex blocks" ); + b3SyncBlock* meshBlocks = + (b3SyncBlock*)b3StackAlloc( &world->stack, meshPrepareDim.count * sizeof( b3SyncBlock ), "mesh blocks" ); + b3SyncBlock* jointBlocks = + (b3SyncBlock*)b3StackAlloc( &world->stack, jointPrepareDim.count * sizeof( b3SyncBlock ), "joint blocks" ); + b3SyncBlock* graphBlocks = + (b3SyncBlock*)b3StackAlloc( &world->stack, graphBlockCount * sizeof( b3SyncBlock ), "graph blocks" ); + + // Split an awake island. This modifies: + // - stack allocator + // - world island array and solver set + // - island indices on bodies, contacts, and joints + // I'm squeezing this task in here because it may be expensive and this is a safe place to put it. + // Note: cannot split islands in parallel with FinalizeBodies + void* splitIslandTask = NULL; + if ( world->splitIslandId != B3_NULL_INDEX ) + { + if ( world->taskCount < B3_MAX_TASKS ) + { + splitIslandTask = world->enqueueTaskFcn( &b3SplitIslandTask, world, world->userTaskContext, "split" ); + world->taskCount += 1; + world->activeTaskCount += splitIslandTask == NULL ? 0 : 1; + } + else + { + b3SplitIslandTask( world ); + } + } + + // Prepare body blocks + b3InitBlocks( bodyBlocks, bodyDim, awakeBodyCount, b3_bodyBlock, UINT8_MAX ); + + // Prepare blocks as a single flat parallel-for over the whole constraint range. + // The task walks spans to decode flat slot indices back to per-color arrays. + b3InitBlocks( convexBlocks, convexPrepareDim, wideContactCount, b3_wideContactBlock, UINT8_MAX ); + b3InitBlocks( meshBlocks, meshPrepareDim, contactCount, b3_contactBlock, UINT8_MAX ); + b3InitBlocks( jointBlocks, jointPrepareDim, jointCount, b3_jointBlock, UINT8_MAX ); + + // Prepare graph work blocks. Each color gets joint blocks followed by contact blocks. + b3SyncBlock* graphColorBlocks[B3_GRAPH_COLOR_COUNT] = { 0 }; + b3SyncBlock* baseGraphBlock = graphBlocks; + int graphBlockCounts[B3_GRAPH_COLOR_COUNT] = { 0 }; + for ( int i = 0; i < activeColorCount; ++i ) + { + graphColorBlocks[i] = baseGraphBlock; + + uint8_t colorIndex = (uint8_t)activeColorIndices[i]; + b3InitBlocks( baseGraphBlock, graphJointDims[i], colorJointCounts[i], b3_graphJointBlock, colorIndex ); + baseGraphBlock += graphJointDims[i].count; + + b3InitBlocks( baseGraphBlock, graphWideContactDims[i], colorWideContactCounts[i], b3_graphWideContactBlock, + colorIndex ); + baseGraphBlock += graphWideContactDims[i].count; + + b3InitBlocks( baseGraphBlock, graphContactDims[i], colorContactCounts[i], b3_graphContactBlock, colorIndex ); + baseGraphBlock += graphContactDims[i].count; + + graphBlockCounts[i] = graphJointDims[i].count + graphWideContactDims[i].count + graphContactDims[i].count; + } + + B3_ASSERT( (ptrdiff_t)( baseGraphBlock - graphBlocks ) == graphBlockCount ); + + b3SolverStage* stage = stages; + stage = b3InitStage( stage, b3_stagePrepareJoints, jointBlocks, jointPrepareDim.count, UINT8_MAX ); + stage = b3InitStage( stage, b3_stagePrepareWideContacts, convexBlocks, convexPrepareDim.count, UINT8_MAX ); + stage = b3InitStage( stage, b3_stagePrepareContacts, meshBlocks, meshPrepareDim.count, UINT8_MAX ); + stage = b3InitStage( stage, b3_stageIntegrateVelocities, bodyBlocks, bodyDim.count, UINT8_MAX ); + stage = b3InitColorStages( stage, b3_stageWarmStart, 1, activeColorCount, graphColorBlocks, graphBlockCounts, + activeColorIndices ); + stage = b3InitColorStages( stage, b3_stageSolve, ITERATIONS, activeColorCount, graphColorBlocks, graphBlockCounts, + activeColorIndices ); + stage = b3InitStage( stage, b3_stageIntegratePositions, bodyBlocks, bodyDim.count, UINT8_MAX ); + stage = b3InitColorStages( stage, b3_stageRelax, RELAX_ITERATIONS, activeColorCount, graphColorBlocks, graphBlockCounts, + activeColorIndices ); + // Note: joint blocks mixed in, could have joint limit restitution + stage = b3InitColorStages( stage, b3_stageRestitution, 1, activeColorCount, graphColorBlocks, graphBlockCounts, + activeColorIndices ); + stage = b3InitStage( stage, b3_stageStoreWideImpulses, convexBlocks, convexPrepareDim.count, UINT8_MAX ); + stage = b3InitStage( stage, b3_stageStoreImpulses, meshBlocks, meshPrepareDim.count, UINT8_MAX ); + + B3_ASSERT( (int)( stage - stages ) == stageCount ); + + B3_ASSERT( workerCount <= B3_MAX_WORKERS ); + b3WorkerContext workerContext[B3_MAX_WORKERS]; + + stepContext->graph = graph; + stepContext->activeColorCount = activeColorCount; + stepContext->workerCount = workerCount; + stepContext->stageCount = stageCount; + stepContext->stages = stages; + stepContext->wideConstraints = wideConstraints; + stepContext->widePrepareSpans = widePrepareSpans; + stepContext->wideContactCount = wideContactCount; + stepContext->manifoldConstraints = manifoldConstraints; + stepContext->contactConstraints = contactConstraints; + stepContext->contactPrepareSpans = contactPrepareSpans; + stepContext->overflowSpans = overflowSpans; + stepContext->jointPrepareSpans = jointPrepareSpans; + b3AtomicStoreU32( &stepContext->atomicSyncBits, 0 ); + b3AtomicStoreInt( &stepContext->mainClaimed, 0 ); + + world->profile.solverSetup = b3GetMillisecondsAndReset( &setupTicks ); + b3TracyCZoneEnd( solver_setup ); + + b3TracyCZoneNC( solve_constraints, "Solve Constraints", b3_colorIndigo, true ); + uint64_t constraintTicks = b3GetTicks(); + + int jointIdCapacity = b3GetIdCapacity( &world->jointIdPool ); + int contactIdCapacity = b3GetIdCapacity( &world->contactIdPool ); + for ( int i = 0; i < workerCount; ++i ) + { + b3TaskContext* taskContext = b3Array_Get( world->taskContexts, i ); + b3SetBitCountAndClear( &taskContext->jointStateBitSet, jointIdCapacity ); + b3SetBitCountAndClear( &taskContext->hitEventBitSet, contactIdCapacity ); + taskContext->hasHitEvents = false; + + workerContext[i].context = stepContext; + workerContext[i].workerIndex = i; + + if ( world->taskCount < B3_MAX_TASKS ) + { + char buffer[16]; + snprintf( buffer, sizeof( buffer ), "solve[%d]", i ); + workerContext[i].userTask = + world->enqueueTaskFcn( &b3SolverTask, workerContext + i, world->userTaskContext, buffer ); + world->taskCount += 1; + world->activeTaskCount += workerContext[i].userTask == NULL ? 0 : 1; + } + else + { + workerContext[i].userTask = NULL; + b3SolverTask( workerContext + i ); + } + } + + // The calling thread of b3World_Step also enters b3SolverTask as worker 0 and races for the + // orchestrator slot via the CAS inside. This guarantees progress even when the user's task + // system can't run the queued worker 0 promptly: it might schedule out of order, have fewer + // threads than workerCount, or invert priority by parking the calling thread in finishTaskFcn. + // Whoever wins the CAS becomes the orchestrator; the loser returns and lets the spinner-only + // path handle workers >0. + b3WorkerContext callerContext = { stepContext, 0, NULL }; + b3SolverTask( &callerContext ); + + // Finish constraint solve + for ( int i = 0; i < workerCount; ++i ) + { + if ( workerContext[i].userTask != NULL ) + { + world->finishTaskFcn( workerContext[i].userTask, world->userTaskContext ); + world->activeTaskCount -= 1; + } + } + + // Finish island split + if ( splitIslandTask != NULL ) + { + world->finishTaskFcn( splitIslandTask, world->userTaskContext ); + world->activeTaskCount -= 1; + } + world->splitIslandId = B3_NULL_INDEX; + + world->profile.constraints = b3GetMillisecondsAndReset( &constraintTicks ); + b3TracyCZoneEnd( solve_constraints ); + + b3TracyCZoneNC( update_transforms, "Update Transforms", b3_colorMediumSeaGreen, true ); + uint64_t transformTicks = b3GetTicks(); + + // Prepare contact, enlarged body, and island bit sets used in body finalization. + int awakeIslandCount = awakeSet->islandSims.count; + for ( int i = 0; i < world->workerCount; ++i ) + { + b3TaskContext* taskContext = world->taskContexts.data + i; + b3Array_Clear( taskContext->sensorHits ); + b3SetBitCountAndClear( &taskContext->enlargedSimBitSet, awakeBodyCount ); + b3SetBitCountAndClear( &taskContext->awakeIslandBitSet, awakeIslandCount ); + taskContext->splitIslandId = B3_NULL_INDEX; + taskContext->splitSleepTime = 0.0f; + } + + // Finalize bodies. Must happen after the constraint solver and after island splitting. + b3ParallelFor( world, &b3FinalizeBodiesTask, awakeBodyCount, 16, stepContext, "ccd" ); + + // Free in reverse order + b3StackFree( &world->stack, graphBlocks ); + b3StackFree( &world->stack, jointBlocks ); + b3StackFree( &world->stack, meshBlocks ); + b3StackFree( &world->stack, convexBlocks ); + b3StackFree( &world->stack, bodyBlocks ); + b3StackFree( &world->stack, stages ); + b3StackFree( &world->stack, overflow->manifoldConstraints ); + b3StackFree( &world->stack, overflow->contactConstraints ); + b3StackFree( &world->stack, manifoldConstraints ); + b3StackFree( &world->stack, contactConstraints ); + b3StackFree( &world->stack, wideConstraints ); + + world->profile.transforms = b3GetMilliseconds( transformTicks ); + b3TracyCZoneEnd( update_transforms ); + } + + // Report joint events + { + b3TracyCZoneNC( joint_events, "Joint Events", b3_colorPeru, true ); + uint64_t jointEventTicks = b3GetTicks(); + + // Gather bits for all joints that have force/torque events + b3BitSet* jointStateBitSet = &world->taskContexts.data[0].jointStateBitSet; + for ( int i = 1; i < world->workerCount; ++i ) + { + b3InPlaceUnion( jointStateBitSet, &world->taskContexts.data[i].jointStateBitSet ); + } + + { + uint32_t wordCount = jointStateBitSet->blockCount; + uint64_t* bits = jointStateBitSet->bits; + + b3Joint* jointArray = world->joints.data; + uint16_t worldIndex0 = world->worldId; + + for ( uint32_t k = 0; k < wordCount; ++k ) + { + uint64_t word = bits[k]; + while ( word != 0 ) + { + uint32_t ctz = b3CTZ64( word ); + int jointId = (int)( 64 * k + ctz ); + + B3_ASSERT( jointId < world->joints.capacity ); + + b3Joint* joint = jointArray + jointId; + + B3_ASSERT( joint->setIndex == b3_awakeSet ); + + b3JointEvent event = { + .jointId = + { + .index1 = jointId + 1, + .world0 = worldIndex0, + .generation = joint->generation, + }, + .userData = joint->userData, + }; + + b3Array_Push( world->jointEvents, event ); + + // Clear the smallest set bit + word = word & ( word - 1 ); + } + } + } + + world->profile.jointEvents = b3GetMilliseconds( jointEventTicks ); + b3TracyCZoneEnd( joint_events ); + } + + // Report hit events + { + b3TracyCZoneNC( hit_events, "Hit Events", b3_colorRosyBrown, true ); + uint64_t hitTicks = b3GetTicks(); + + B3_ASSERT( world->contactHitEvents.count == 0 ); + + // Fast path: if no worker flagged any hit-event candidates during b2StoreImpulsesTask, skip entirely. + bool anyHitEvents = false; + for ( int i = 0; i < world->workerCount; ++i ) + { + if ( world->taskContexts.data[i].hasHitEvents ) + { + anyHitEvents = true; + break; + } + } + + if ( anyHitEvents ) + { + // Union per-worker bits into worker 0's bit set. + b3BitSet* hitEventBitSet = &world->taskContexts.data[0].hitEventBitSet; + for ( int i = 1; i < world->workerCount; ++i ) + { + if ( world->taskContexts.data[i].hasHitEvents ) + { + b3InPlaceUnion( hitEventBitSet, &world->taskContexts.data[i].hitEventBitSet ); + } + } + + float threshold = world->hitEventThreshold; + b3Contact* contactArray = world->contacts.data; + uint16_t worldId = world->worldId; + + uint32_t wordCount = hitEventBitSet->blockCount; + uint64_t* bits = hitEventBitSet->bits; + for ( uint32_t k = 0; k < wordCount; ++k ) + { + uint64_t word = bits[k]; + while ( word != 0 ) + { + uint32_t ctz = b3CTZ64( word ); + int contactId = (int)( 64 * k + ctz ); + + b3Contact* contact = contactArray + contactId; + B3_ASSERT( contact->setIndex == b3_awakeSet && contact->colorIndex != B3_NULL_INDEX ); + + b3Shape* shapeA = b3Array_Get( world->shapes, contact->shapeIdA ); + b3Shape* shapeB = b3Array_Get( world->shapes, contact->shapeIdB ); + b3Body* bodyA = b3Array_Get( world->bodies, shapeA->bodyId ); + b3Body* bodyB = b3Array_Get( world->bodies, shapeB->bodyId ); + b3BodySim* simA = b3GetBodySim( world, bodyA ); + b3BodySim* simB = b3GetBodySim( world, bodyB ); + b3Pos midCenter = b3LerpPosition( simA->center, simB->center, 0.5f ); + + b3ContactHitEvent event = { 0 }; + event.approachSpeed = threshold; + + bool found = false; + int triangleIndex = 0; + int manifoldCount = contact->manifoldCount; + for ( int i = 0; i < manifoldCount; ++i ) + { + b3Manifold* manifold = contact->manifolds + i; + int pointCount = manifold->pointCount; + for ( int p = 0; p < pointCount; ++p ) + { + b3ManifoldPoint* mp = manifold->points + p; + float approachSpeed = -mp->normalVelocity; + + // Need to check total impulse because the point may be speculative and not colliding + if ( approachSpeed > event.approachSpeed && mp->totalNormalImpulse > 0.0f ) + { + event.approachSpeed = approachSpeed; + event.point = b3OffsetPos( midCenter, b3Lerp( mp->anchorA, mp->anchorB, 0.5f ) ); + event.normal = manifold->normal; + triangleIndex = mp->triangleIndex; + found = true; + } + } + } + + if ( found == true ) + { + event.shapeIdA = (b3ShapeId){ shapeA->id + 1, worldId, shapeA->generation }; + event.shapeIdB = (b3ShapeId){ shapeB->id + 1, worldId, shapeB->generation }; + + event.contactId = (b3ContactId){ + .index1 = contact->contactId + 1, + .world0 = worldId, + .padding = 0, + .generation = contact->generation, + }; + + // shapeB is never a compound today (asserted in b3CreateContact), so the + // childIndex argument is irrelevant for it. shapeA carries the compound. + event.userMaterialIdA = b3GetShapeUserMaterialId( shapeA, contact->childIndex, triangleIndex ); + event.userMaterialIdB = b3GetShapeUserMaterialId( shapeB, 0, triangleIndex ); + + b3Array_Push( world->contactHitEvents, event ); + } + + // Clear the smallest set bit + word = word & ( word - 1 ); + } + } + } + + world->profile.hitEvents = b3GetMilliseconds( hitTicks ); + b3TracyCZoneEnd( hit_events ); + } + + { + b3TracyCZoneNC( refit_bvh, "Refit BVH", b3_colorFireBrick, true ); + uint64_t refitTicks = b3GetTicks(); + + // Finish the user tree task that was queued earlier in the time step. This must be complete before touching the + // broad-phase. + if ( world->userTreeTask != NULL ) + { + world->finishTaskFcn( world->userTreeTask, world->userTaskContext ); + world->userTreeTask = NULL; + world->activeTaskCount -= 1; + } + + b3ValidateNoEnlarged( &world->broadPhase ); + + // Gather bits for all sim bodies that have enlarged AABBs + b3BitSet* enlargedBodyBitSet = &world->taskContexts.data[0].enlargedSimBitSet; + for ( int i = 1; i < world->workerCount; ++i ) + { + b3InPlaceUnion( enlargedBodyBitSet, &world->taskContexts.data[i].enlargedSimBitSet ); + } + + // Enlarge broad-phase proxies and build move array + // Apply shape AABB changes to broad-phase. This also create the move array which must be + // in deterministic order. I'm tracking sim bodies because the number of shape ids can be huge. + // This has to happen before bullets are processed. + { + b3BroadPhase* broadPhase = &world->broadPhase; + uint32_t wordCount = enlargedBodyBitSet->blockCount; + uint64_t* bits = enlargedBodyBitSet->bits; + + // Fast array access is important here + b3Body* bodyArray = world->bodies.data; + b3BodySim* bodySimArray = awakeSet->bodySims.data; + b3Shape* shapeArray = world->shapes.data; + + for ( uint32_t k = 0; k < wordCount; ++k ) + { + uint64_t word = bits[k]; + while ( word != 0 ) + { + uint32_t ctz = b3CTZ64( word ); + uint32_t bodySimIndex = 64 * k + ctz; + + b3BodySim* bodySim = bodySimArray + bodySimIndex; + + b3Body* body = bodyArray + bodySim->bodyId; + + int shapeId = body->headShapeId; + if ( ( bodySim->flags & ( b3_isBullet | b3_isFast ) ) == ( b3_isBullet | b3_isFast ) ) + { + // Fast bullet bodies don't have their final AABB yet + while ( shapeId != B3_NULL_INDEX ) + { + b3Shape* shape = shapeArray + shapeId; + + // Shape is fast. It's aabb will be enlarged in continuous collision. + // Update the move array here for determinism because bullets are processed + // below in non-deterministic order. + b3BufferMove( broadPhase, shape->proxyKey ); + + shapeId = shape->nextShapeId; + } + } + else + { + while ( shapeId != B3_NULL_INDEX ) + { + b3Shape* shape = shapeArray + shapeId; + + // The AABB may not have been enlarged, despite the body being flagged as enlarged. + // For example, a body with multiple shapes may have not have all shapes enlarged. + // A fast body may have been flagged as enlarged despite having no shapes enlarged. + if ( shape->flags & b3_enlargedAABB ) + { + b3BroadPhase_EnlargeProxy( broadPhase, shape->proxyKey, shape->fatAABB ); + shape->flags &= ~b3_enlargedAABB; + } + + shapeId = shape->nextShapeId; + } + } + + // Clear the smallest set bit + word = word & ( word - 1 ); + } + } + } + + b3ValidateBroadPhase( &world->broadPhase ); + + world->profile.refit = b3GetMilliseconds( refitTicks ); + b3TracyCZoneEnd( refit_bvh ); + } + + int bulletBodyCount = b3AtomicLoadInt( &stepContext->bulletBodyCount ); + if ( bulletBodyCount > 0 ) + { + b3TracyCZoneNC( bullets, "Bullets", b3_colorDarkGoldenRod, true ); + uint64_t bulletTicks = b3GetTicks(); + + // Fast bullet bodies + // Note: a bullet body may be moving slow + int minRange = 8; + b3ParallelFor( world, &b3BulletBodyTask, bulletBodyCount, minRange, stepContext, "bullets" ); + + // Serially enlarge broad-phase proxies for bullet shapes + b3BroadPhase* broadPhase = &world->broadPhase; + b3DynamicTree* dynamicTree = broadPhase->trees + b3_dynamicBody; + + // Fast array access is important here + b3Body* bodyArray = world->bodies.data; + b3BodySim* bodySimArray = awakeSet->bodySims.data; + b3Shape* shapeArray = world->shapes.data; + + // Serially enlarge broad-phase proxies for bullet shapes + int* bulletBodySimIndices = stepContext->bulletBodies; + + // This loop has non-deterministic order but it shouldn't affect the result + for ( int i = 0; i < bulletBodyCount; ++i ) + { + b3BodySim* bulletBodySim = bodySimArray + bulletBodySimIndices[i]; + if ( ( bulletBodySim->flags & b3_enlargeBounds ) == 0 ) + { + continue; + } + + // Clear flag + bulletBodySim->flags &= ~b3_enlargeBounds; + + int bodyId = bulletBodySim->bodyId; + B3_ASSERT( 0 <= bodyId && bodyId < world->bodies.count ); + b3Body* bulletBody = bodyArray + bodyId; + + int shapeId = bulletBody->headShapeId; + while ( shapeId != B3_NULL_INDEX ) + { + b3Shape* shape = shapeArray + shapeId; + if ( ( shape->flags & b3_enlargedAABB ) == 0 ) + { + shapeId = shape->nextShapeId; + continue; + } + + // clear flag + shape->flags &= ~b3_enlargedAABB; + + int proxyKey = shape->proxyKey; + int proxyId = B3_PROXY_ID( proxyKey ); + B3_ASSERT( B3_PROXY_TYPE( proxyKey ) == b3_dynamicBody ); + + // all fast bullet shapes should already be in the move buffer + B3_ASSERT( b3GetBit( &broadPhase->movedProxies[b3_dynamicBody], proxyId ) ); + + b3DynamicTree_EnlargeProxy( dynamicTree, proxyId, shape->fatAABB ); + + shapeId = shape->nextShapeId; + } + } + + world->profile.bullets = b3GetMilliseconds( bulletTicks ); + b3TracyCZoneEnd( bullets ); + } + + b3StackFree( &world->stack, stepContext->bulletBodies ); + stepContext->bulletBodies = NULL; + b3AtomicStoreInt( &stepContext->bulletBodyCount, 0 ); + + // Report sensor hits. This may include bullets sensor hits. + { + b3TracyCZoneNC( sensor_hits, "Sensor Hits", b3_colorPowderBlue, true ); + uint64_t sensorHitTicks = b3GetTicks(); + + int workerCount = world->workerCount; + B3_ASSERT( workerCount == world->taskContexts.count ); + + for ( int i = 0; i < workerCount; ++i ) + { + b3TaskContext* taskContext = world->taskContexts.data + i; + int hitCount = taskContext->sensorHits.count; + b3SensorHit* hits = taskContext->sensorHits.data; + + for ( int j = 0; j < hitCount; ++j ) + { + b3SensorHit hit = hits[j]; + b3Shape* sensorShape = b3Array_Get( world->shapes, hit.sensorId ); + b3Shape* visitor = b3Array_Get( world->shapes, hit.visitorId ); + + b3Sensor* sensor = b3Array_Get( world->sensors, sensorShape->sensorIndex ); + b3Visitor shapeRef = { + .shapeId = hit.visitorId, + .generation = visitor->generation, + }; + b3Array_Push( sensor->hits, shapeRef ); + } + } + + world->profile.sensorHits = b3GetMilliseconds( sensorHitTicks ); + b3TracyCZoneEnd( sensor_hits ); + } + + // Island sleeping + // This must be done last because putting islands to sleep invalidates the enlarged body bits. + // todo_erin figure out how to do this in parallel with tree refit + if ( world->enableSleep == true ) + { + b3TracyCZoneNC( sleep_islands, "Island Sleep", b3_colorLightSlateGray, true ); + uint64_t sleepTicks = b3GetTicks(); + + // Collect split island candidate for the next time step. No need to split if sleeping is disabled. + B3_ASSERT( world->splitIslandId == B3_NULL_INDEX ); + float splitSleepTimer = 0.0f; + for ( int i = 0; i < world->workerCount; ++i ) + { + b3TaskContext* taskContext = world->taskContexts.data + i; + if ( taskContext->splitIslandId != B3_NULL_INDEX && taskContext->splitSleepTime >= splitSleepTimer ) + { + B3_ASSERT( taskContext->splitSleepTime > 0.0f ); + + // Tie breaking for determinism. Largest island id wins. Needed due to work stealing. + if ( taskContext->splitSleepTime == splitSleepTimer && taskContext->splitIslandId < world->splitIslandId ) + { + continue; + } + + world->splitIslandId = taskContext->splitIslandId; + splitSleepTimer = taskContext->splitSleepTime; + } + } + + b3BitSet* awakeIslandBitSet = &world->taskContexts.data[0].awakeIslandBitSet; + for ( int i = 1; i < world->workerCount; ++i ) + { + b3InPlaceUnion( awakeIslandBitSet, &world->taskContexts.data[i].awakeIslandBitSet ); + } + + // Need to process in reverse because this moves islands to sleeping solver sets. + b3IslandSim* islands = awakeSet->islandSims.data; + int count = awakeSet->islandSims.count; + for ( int islandIndex = count - 1; islandIndex >= 0; islandIndex -= 1 ) + { + if ( b3GetBit( awakeIslandBitSet, islandIndex ) == true ) + { + // this island is still awake + continue; + } + + b3IslandSim* island = islands + islandIndex; + int islandId = island->islandId; + + b3TrySleepIsland( world, islandId ); + } + + b3ValidateSolverSets( world ); + + world->profile.sleepIslands = b3GetMilliseconds( sleepTicks ); + b3TracyCZoneEnd( sleep_islands ); + } +} diff --git a/vendor/box3d/src/src/solver.h b/vendor/box3d/src/src/solver.h new file mode 100644 index 000000000..7c8a1c71b --- /dev/null +++ b/vendor/box3d/src/src/solver.h @@ -0,0 +1,306 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +// Solver work is partitioned into fixed-size blocks that worker threads claim +// in parallel via atomic CAS on a per-block syncIndex. The descriptor (b3SolverBlock) +// and the atomic counter sit in a wrapping b3SyncBlock so the CAS-winner can +// pass the descriptor by value into stage tasks without aliasing the atomic +// memory other threads are CAS-writing. Three properties of this design +// matter for performance: +// +// 1. Distributed contention. Per-block atomic syncIndex avoids the cache line stampede +// that a single shared fetch_add counter would cause. Once a worker +// settles into a block range, its CAS targets live in its own L1. +// +// 2. Monotonic syncIndex across iterations. Iterative stages (warm start, +// solve, relax) reuse the same block array every sub-step iteration. +// syncIndex grows each iteration; workers CAS (prev, prev+1), so the +// main thread never touches any per-block state between iterations. +// Non-iterative stages simply use syncIndex 1. +// +// 3. L2 affinity across iterations. Each worker picks a start offset from +// its workerIndex, then scans forward and (after wrap) backward: +// +// blocks: [0] [1] [2] [3] [4] [5] [6] [7] +// ^ ^ ^ ^ +// W0 W1 W2 W3 <- start offsets +// +// W0 claims 0,1,2,3 (forward), W1 claims 4,5, etc. Under balanced load +// each worker re-hits the same block range every iteration, keeping that +// range's hot data resident in its L2. A failed CAS means a neighbour +// already claimed the block, so the stealing worker stops -- preserving +// locality under mild imbalance while still draining the queue. +// +// A graph color stage lays out joint blocks first, then contact blocks: +// +// stage->blocks -> +// +------+------+------+------+------+------+------+ +// | J0 | J1 | J2 | C0 | C1 | C2 | C3 | +// +------+------+------+------+------+------+------+ +// <-- graphJointBlocks --><---- graphContactBlocks ----> +// +// Each block carries its type so the dispatcher routes J-blocks to the joint +// solver and C-blocks to the SIMD contact solver; both kinds run concurrently +// within the stage -- no barrier between them. The type tag lives on the +// block (not the stage) so that mixed-type stages can keep the concurrency. +// +// The solver threading model is inspired by https://github.com/bepu/bepuphysics2 + +#pragma once + +#include "container.h" +#include "core.h" + +#include "box3d/math_functions.h" + +#include + +typedef struct b3BodySim b3BodySim; +typedef struct b3BodyState b3BodyState; +typedef struct b3ContactConstraint b3ContactConstraint; +typedef struct b3ContactConstraintWide b3ContactConstraintWide; +typedef struct b3ContactSpec b3ContactSpec; +typedef struct b3JointSim b3JointSim; +typedef struct b3Manifold b3Manifold; +typedef struct b3ManifoldConstraint b3ManifoldConstraint; +typedef struct b3World b3World; + +// Solver stages +typedef enum b3SolverStageType +{ + b3_stagePrepareJoints, + b3_stagePrepareWideContacts, + b3_stagePrepareContacts, + b3_stageIntegrateVelocities, + b3_stageWarmStart, + b3_stageSolve, + b3_stageIntegratePositions, + b3_stageRelax, + b3_stageRestitution, + b3_stageStoreWideImpulses, + b3_stageStoreImpulses, +} b3SolverStageType; + +typedef enum b3SolverBlockType +{ + // Block for iterating across bodies. + b3_bodyBlock, + + // Block for iterating across joints. For prepare. + b3_jointBlock, + + // Block for iterating across wide contacts. For prepare and store. + b3_wideContactBlock, + + // Block for iterating across contacts. For prepare and store. + b3_contactBlock, + + // Block for iterating across joints of a single graph color. + b3_graphJointBlock, + + // Block for iterating across wide contacts of a single graph color. + b3_graphWideContactBlock, + + // Block for iterating across contacts of a single graph color. + b3_graphContactBlock, + + // Block for processing overflow constraints + b3_overflowBlock, +} b3SolverBlockType; + +// Solver block describes a multithreaded unit of work. +typedef struct b3SolverBlock +{ + int startIndex; + uint16_t count; + // b3SolverBlockType + uint8_t blockType; + uint8_t colorIndex; +} b3SolverBlock; + +// A unit of multithreaded work along with atomic synchronization. The syncIndex grows +// monotonically allowing the solver block to be re-used across sub-steps. +typedef struct b3SyncBlock +{ + b3SolverBlock block; + b3AtomicInt syncIndex; +} b3SyncBlock; + +// Each stage must be completed before going to the next stage. +// Non-iterative stages use a stage instance once while iterative stages re-use the same instance each iteration. +typedef struct b3SolverStage +{ + b3SyncBlock* blocks; + b3SolverStageType type; + int blockCount; + uint8_t colorIndex; + b3AtomicInt completionCount; +} b3SolverStage; + +// Constraint softness +typedef struct b3Softness +{ + float biasRate; + float massScale; + float impulseScale; +} b3Softness; + +// Prepare/store run as a flat parallel-for over the whole wide-constraint +// range. Each span maps a slice of that range back to the owning color's +// contacts so workers can decode flat wide-slot indices without touching +// graph state. The spans array has one entry per active color plus a sentinel +// whose start == wideContactCount. +typedef struct b3WidePrepareSpan +{ + int start; + int count; + int* contacts; +} b3WidePrepareSpan; + +typedef struct b3ContactPrepareSpan +{ + int start; + int count; + b3ContactSpec* contacts; +} b3ContactPrepareSpan; + +typedef struct b3JointPrepareSpan +{ + int start; + int count; + b3JointSim* joints; +} b3JointPrepareSpan; + +// Context for a time step. Recreated each time step. +typedef struct b3StepContext +{ + // time step + float dt; + + // inverse time step (0 if dt == 0). + float inv_dt; + + // sub-step + float h; + float inv_h; + + int subStepCount; + + b3Softness contactSoftness; + b3Softness staticSoftness; + + float restitutionThreshold; + float maxLinearVelocity; + + struct b3World* world; + struct b3ConstraintGraph* graph; + + // shortcut to body states from awake set + b3BodyState* states; + + // shortcut to body sims from awake set + b3BodySim* sims; + + // array of all shape ids for shapes that have enlarged AABBs + int* enlargedShapes; + int enlargedShapeCount; + + // Array of bullet bodies that need continuous collision handling + int* bulletBodies; + b3AtomicInt bulletBodyCount; + + // Contact ids for simplified parallel-for access. Used in narrow-phase. + // These contacts may or may not be touching. They are associated with awake bodies. + int* awakeContactIndices; + + // Flat view of the wide contact constraint array used by prepare and store. + // prepareSpans has activeColorCount + 1 entries, the last being a sentinel + // at wideContactCount. wideContactConstraints is the contiguous base + // pointer; per-color slices live at colors[i].wideConstraints. + struct b3ContactConstraintWide* wideConstraints; + b3WidePrepareSpan* widePrepareSpans; + int wideContactCount; + + // Similar for mesh/overflow contact constraints + struct b3ManifoldConstraint* manifoldConstraints; + struct b3ContactConstraint* contactConstraints; + b3ContactPrepareSpan* contactPrepareSpans; + b3ContactPrepareSpan* overflowSpans; + b3JointPrepareSpan* jointPrepareSpans; + + int activeColorCount; + int workerCount; + + b3SolverStage* stages; + int stageCount; + bool enableWarmStarting; + + // padding to prevent false sharing + char padding1[64]; + + // This atomic is central to multi-threaded solver task synchronization. + // It prevents ABA problems by monotonically growing as the solver advances. + // This means a delayed worker thread will catch up without repeating already completed + // work (causing a race condition). + // sync index (16-bits) | stage type (16-bits) + b3AtomicU32 atomicSyncBits; + + // padding to prevent false sharing + char padding2[64]; + + // Race flag claimed by whichever runner reaches b3SolverTask with workerIndex 0 first. + // The calling thread of b3World_Step also races for this slot so the orchestrator can + // always make progress, regardless of how the user's task system schedules tasks (out + // of order, fewer threads than workers, or synchronously inside enqueueTaskFcn). The + // loser of the race no-ops as workerIndex 0. + b3AtomicInt mainClaimed; + + // padding to prevent false sharing + char padding3[64]; +} b3StepContext; + +void b3Solve( b3World* world, b3StepContext* stepContext ); + +static inline b3Softness b3MakeSoft( float hertz, float zeta, float h ) +{ + if ( hertz == 0.0f ) + { + return B3_LITERAL( b3Softness ){ + .biasRate = 0.0f, + .massScale = 0.0f, + .impulseScale = 0.0f, + }; + } + + float omega = 2.0f * B3_PI * hertz; + float a1 = 2.0f * zeta + h * omega; + float a2 = h * omega * a1; + float a3 = 1.0f / ( 1.0f + a2 ); + + // bias = w / (2 * z + hw) + // massScale = hw * (2 * z + hw) / (1 + hw * (2 * z + hw)) + // impulseScale = 1 / (1 + hw * (2 * z + hw)) + + // If z == 0 + // bias = 1/h + // massScale = hw^2 / (1 + hw^2) + // impulseScale = 1 / (1 + hw^2) + + // w -> inf + // bias = 1/h + // massScale = 1 + // impulseScale = 0 + + // if w = pi / 4 * inv_h + // massScale = (pi/4)^2 / (1 + (pi/4)^2) = pi^2 / (16 + pi^2) ~= 0.38 + // impulseScale = 1 / (1 + (pi/4)^2) = 16 / (16 + pi^2) ~= 0.62 + + // In all cases: + // massScale + impulseScale == 1 + + return ( b3Softness ){ + .biasRate = omega / a1, + .massScale = a2 * a3, + .impulseScale = a3, + }; +} diff --git a/vendor/box3d/src/src/solver_set.c b/vendor/box3d/src/src/solver_set.c new file mode 100644 index 000000000..f050a869d --- /dev/null +++ b/vendor/box3d/src/src/solver_set.c @@ -0,0 +1,633 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +#include "solver_set.h" + +#include "body.h" +#include "constraint_graph.h" +#include "contact.h" +#include "island.h" +#include "joint.h" +#include "physics_world.h" + +#include + +void b3DestroySolverSet( b3World* world, int setIndex ) +{ + b3SolverSet* set = b3Array_Get( world->solverSets, setIndex ); + + b3Array_Destroy( set->bodySims ); + b3Array_Destroy( set->bodyStates ); + b3Array_Destroy( set->contactIndices ); + b3Array_Destroy( set->jointSims ); + b3Array_Destroy( set->islandSims ); + + b3FreeId( &world->solverSetIdPool, setIndex ); + *set = (b3SolverSet){ 0 }; + set->setIndex = B3_NULL_INDEX; +} + +// Wake a solver set. Does not merge islands. +// Contacts can be in several places: +// 1. non-touching contacts in the disabled set +// 2. non-touching contacts already in the awake set +// 3. touching contacts in the sleeping set +// This handles contact types 1 and 3. Type 2 doesn't need any action. +void b3WakeSolverSet( b3World* world, int setIndex ) +{ + B3_ASSERT( setIndex >= b3_firstSleepingSet ); + b3SolverSet* set = b3Array_Get( world->solverSets, setIndex ); + b3SolverSet* awakeSet = b3Array_Get( world->solverSets, b3_awakeSet ); + b3SolverSet* disabledSet = b3Array_Get( world->solverSets, b3_disabledSet ); + + b3Body* bodies = world->bodies.data; + + int bodyCount = set->bodySims.count; + for ( int i = 0; i < bodyCount; ++i ) + { + b3BodySim* simSrc = set->bodySims.data + i; + + b3Body* body = bodies + simSrc->bodyId; + B3_ASSERT( body->setIndex == setIndex ); + body->setIndex = b3_awakeSet; + body->localIndex = awakeSet->bodySims.count; + + // Reset sleep timer + body->sleepTime = 0.0f; + + b3BodySim* simDst = b3Array_Emplace( awakeSet->bodySims ); + memcpy( simDst, simSrc, sizeof( b3BodySim ) ); + + b3BodyState* state = b3Array_Emplace( awakeSet->bodyStates ); + *state = b3_identityBodyState; + state->flags = body->flags; + + // move non-touching contacts from disabled set to awake set + int contactKey = body->headContactKey; + while ( contactKey != B3_NULL_INDEX ) + { + int edgeIndex = contactKey & 1; + int contactId = contactKey >> 1; + + b3Contact* contact = b3Array_Get( world->contacts, contactId ); + contactKey = contact->edges[edgeIndex].nextKey; + + if ( contact->setIndex != b3_disabledSet ) + { + B3_ASSERT( contact->setIndex == b3_awakeSet || contact->setIndex == setIndex ); + continue; + } + + int localIndex = contact->localIndex; + + B3_ASSERT( 0 <= localIndex && localIndex < disabledSet->contactIndices.count ); + B3_ASSERT( disabledSet->contactIndices.data[localIndex] == contactId ); + + B3_ASSERT( ( contact->flags & b3_contactTouchingFlag ) == 0 && contact->manifoldCount == 0 ); + + contact->setIndex = b3_awakeSet; + contact->localIndex = awakeSet->contactIndices.count; + b3Array_Push( awakeSet->contactIndices, contactId ); + + int movedLocalIndex = b3Array_RemoveSwap( disabledSet->contactIndices, localIndex ); + if ( movedLocalIndex != B3_NULL_INDEX ) + { + // fix moved element + int movedContactIndex = disabledSet->contactIndices.data[localIndex]; + b3Contact* movedContact = b3Array_Get( world->contacts, movedContactIndex ); + B3_ASSERT( movedContact->localIndex == movedLocalIndex ); + movedContact->localIndex = localIndex; + } + } + } + + // Transfer touching contacts from sleeping set to constraint graph. + { + int contactCount = set->contactIndices.count; + for ( int i = 0; i < contactCount; ++i ) + { + int contactIndex = set->contactIndices.data[i]; + b3Contact* contact = b3Array_Get( world->contacts, contactIndex ); + B3_ASSERT( contact->flags & b3_contactTouchingFlag ); + B3_ASSERT( contact->flags & b3_simTouchingFlag ); + B3_ASSERT( contact->setIndex == setIndex ); + b3AddContactToGraph( world, contact ); + contact->setIndex = b3_awakeSet; + } + } + + // transfer joints from sleeping set to awake set + { + int jointCount = set->jointSims.count; + for ( int i = 0; i < jointCount; ++i ) + { + b3JointSim* jointSim = set->jointSims.data + i; + b3Joint* joint = b3Array_Get( world->joints, jointSim->jointId ); + B3_ASSERT( joint->setIndex == setIndex ); + b3AddJointToGraph( world, jointSim, joint ); + joint->setIndex = b3_awakeSet; + } + } + + // transfer island from sleeping set to awake set + // Usually a sleeping set has only one island, but it is possible + // that joints are created between sleeping islands and they + // are moved to the same sleeping set. + { + int islandCount = set->islandSims.count; + for ( int i = 0; i < islandCount; ++i ) + { + b3IslandSim* islandSrc = set->islandSims.data + i; + b3Island* island = b3Array_Get( world->islands, islandSrc->islandId ); + island->setIndex = b3_awakeSet; + island->localIndex = awakeSet->islandSims.count; + b3IslandSim* islandDst = b3Array_Emplace( awakeSet->islandSims ); + memcpy( islandDst, islandSrc, sizeof( b3IslandSim ) ); + } + } + + // destroy the sleeping set + b3DestroySolverSet( world, setIndex ); +} + +void b3TrySleepIsland( b3World* world, int islandId ) +{ + b3Island* island = b3Array_Get( world->islands, islandId ); + B3_ASSERT( island->setIndex == b3_awakeSet ); + + // Cannot put an island to sleep while it has a pending split and more than one body. + if ( island->constraintRemoveCount > 0 && island->bodies.count > 1 ) + { + return; + } + + // island is sleeping + // - create new sleeping solver set + // - move island to sleeping solver set + // - identify non-touching contacts that should move to sleeping solver set or disabled set + // - remove old island + // - fix island + int sleepSetId = b3AllocId( &world->solverSetIdPool ); + if ( sleepSetId == world->solverSets.count ) + { + b3SolverSet set = { 0 }; + set.setIndex = B3_NULL_INDEX; + b3Array_Push( world->solverSets, set ); + } + + b3SolverSet* sleepSet = b3Array_Get( world->solverSets, sleepSetId ); + *sleepSet = (b3SolverSet){ 0 }; + + // grab awake set after creating the sleep set because the solver set array may have been resized + b3SolverSet* awakeSet = b3Array_Get( world->solverSets, b3_awakeSet ); + b3SolverSet* disabledSet = b3Array_Get( world->solverSets, b3_disabledSet ); + B3_ASSERT( 0 <= island->localIndex && island->localIndex < awakeSet->islandSims.count ); + + sleepSet->setIndex = sleepSetId; + b3Array_Reserve( sleepSet->bodySims, island->bodies.count ); + b3Array_Reserve( sleepSet->contactIndices, island->contacts.count ); + b3Array_Reserve( sleepSet->jointSims, island->joints.count ); + + // move awake bodies to sleeping set + // this shuffles around bodies in the awake set + { + for ( int i = 0; i < island->bodies.count; ++i ) + { + int bodyId = island->bodies.data[i]; + b3Body* body = b3Array_Get( world->bodies, bodyId ); + B3_ASSERT( body->setIndex == b3_awakeSet ); + B3_ASSERT( body->islandId == islandId ); + B3_ASSERT( body->islandIndex == i ); + + // Update the body move event to indicate this body fell asleep + // It could happen the body is forced asleep before it ever moves. + if ( body->bodyMoveIndex != B3_NULL_INDEX ) + { + b3BodyMoveEvent* moveEvent = b3Array_Get( world->bodyMoveEvents, body->bodyMoveIndex ); + B3_ASSERT( moveEvent->bodyId.index1 - 1 == bodyId ); + B3_ASSERT( moveEvent->bodyId.generation == body->generation ); + moveEvent->fellAsleep = true; + body->bodyMoveIndex = B3_NULL_INDEX; + } + + int awakeBodyIndex = body->localIndex; + b3BodySim* awakeSim = b3Array_Get( awakeSet->bodySims, awakeBodyIndex ); + + // move body sim to sleep set + int sleepBodyIndex = sleepSet->bodySims.count; + b3BodySim* sleepBodySim = b3Array_Emplace( sleepSet->bodySims ); + memcpy( sleepBodySim, awakeSim, sizeof( b3BodySim ) ); + + int movedIndex = b3Array_RemoveSwap( awakeSet->bodySims, awakeBodyIndex ); + if ( movedIndex != B3_NULL_INDEX ) + { + // fix local index on moved element + b3BodySim* movedSim = awakeSet->bodySims.data + awakeBodyIndex; + int movedId = movedSim->bodyId; + b3Body* movedBody = b3Array_Get( world->bodies, movedId ); + B3_ASSERT( movedBody->localIndex == movedIndex ); + movedBody->localIndex = awakeBodyIndex; + } + + // destroy state, no need to clone + b3Array_RemoveSwap( awakeSet->bodyStates, awakeBodyIndex ); + + body->setIndex = sleepSetId; + body->localIndex = sleepBodyIndex; + + // Move non-touching contacts to the disabled set. + // Non-touching contacts may exist between sleeping islands and there is no clear ownership. + int contactKey = body->headContactKey; + while ( contactKey != B3_NULL_INDEX ) + { + int contactId = contactKey >> 1; + int edgeIndex = contactKey & 1; + + b3Contact* contact = b3Array_Get( world->contacts, contactId ); + + B3_ASSERT( contact->setIndex == b3_awakeSet || contact->setIndex == b3_disabledSet ); + contactKey = contact->edges[edgeIndex].nextKey; + + if ( contact->setIndex == b3_disabledSet ) + { + // already moved to disabled set by another body in the island + continue; + } + + if ( contact->colorIndex != B3_NULL_INDEX ) + { + // contact is touching and will be moved separately + B3_ASSERT( ( contact->flags & b3_contactTouchingFlag ) != 0 ); + continue; + } + + // the other body may still be awake, it still may go to sleep and then it will be responsible + // for moving this contact to the disabled set. + int otherEdgeIndex = edgeIndex ^ 1; + int otherBodyId = contact->edges[otherEdgeIndex].bodyId; + b3Body* otherBody = b3Array_Get( world->bodies, otherBodyId ); + if ( otherBody->setIndex == b3_awakeSet ) + { + continue; + } + + int localIndex = contact->localIndex; + B3_ASSERT( awakeSet->contactIndices.data[localIndex] == contactId ); + + B3_ASSERT( contact->manifoldCount == 0 ); + B3_ASSERT( ( contact->flags & b3_contactTouchingFlag ) == 0 ); + + // Move the non-touching contact to the disabled set. + contact->setIndex = b3_disabledSet; + + // This is mandatory for validation to work correctly + contact->localIndex = disabledSet->contactIndices.count; + b3Array_Push( disabledSet->contactIndices, contact->contactId ); + + int movedLocalIndex = b3Array_RemoveSwap( awakeSet->contactIndices, localIndex ); + if ( movedLocalIndex != B3_NULL_INDEX ) + { + // fix moved element + int movedContactIndex = awakeSet->contactIndices.data[localIndex]; + b3Contact* movedContact = b3Array_Get( world->contacts, movedContactIndex ); + B3_ASSERT( movedContact->localIndex == movedLocalIndex ); + movedContact->localIndex = localIndex; + } + } + } + } + + // move touching contacts to sleeping set + // this shuffles contacts in the awake set + { + for ( int i = 0; i < island->contacts.count; ++i ) + { + int contactId = island->contacts.data[i].contactId; + b3Contact* contact = b3Array_Get( world->contacts, contactId ); + B3_ASSERT( contact->setIndex == b3_awakeSet ); + B3_ASSERT( contact->islandId == islandId ); + B3_ASSERT( contact->islandIndex == i ); + int colorIndex = contact->colorIndex; + B3_ASSERT( 0 <= colorIndex && colorIndex < B3_GRAPH_COLOR_COUNT ); + + b3GraphColor* color = world->constraintGraph.colors + colorIndex; + + // Remove bodies from graph coloring associated with this constraint + if ( colorIndex != B3_OVERFLOW_INDEX ) + { + // might clear a bit for a static body, but this has no effect + b3ClearBit( &color->bodySet, contact->edges[0].bodyId ); + b3ClearBit( &color->bodySet, contact->edges[1].bodyId ); + } + + int sleepContactIndex = sleepSet->contactIndices.count; + b3Array_Push( sleepSet->contactIndices, contactId ); + + int localIndex = contact->localIndex; + if ( ( contact->flags & b3_simMeshContact ) || colorIndex == B3_OVERFLOW_INDEX ) + { + int movedLocalIndex = b3Array_RemoveSwap( color->contacts, localIndex ); + if ( movedLocalIndex != B3_NULL_INDEX ) + { + // fix moved element + int movedContactId = color->contacts.data[localIndex].contactId; + b3Contact* movedContact = b3Array_Get( world->contacts, movedContactId ); + B3_ASSERT( movedContact->localIndex == movedLocalIndex ); + movedContact->localIndex = localIndex; + } + } + else + { + int movedLocalIndex = b3Array_RemoveSwap( color->convexContacts, localIndex ); + if ( movedLocalIndex != B3_NULL_INDEX ) + { + // fix moved element + int movedContactId = color->convexContacts.data[localIndex]; + b3Contact* movedContact = b3Array_Get( world->contacts, movedContactId ); + B3_ASSERT( movedContact->localIndex == movedLocalIndex ); + movedContact->localIndex = localIndex; + } + } + + contact->setIndex = sleepSetId; + contact->colorIndex = B3_NULL_INDEX; + contact->localIndex = sleepContactIndex; + } + } + + // move joints + // this shuffles joints in the awake set + { + for ( int i = 0; i < island->joints.count; ++i ) + { + int jointId = island->joints.data[i].jointId; + b3Joint* joint = b3Array_Get( world->joints, jointId ); + B3_ASSERT( joint->setIndex == b3_awakeSet ); + B3_ASSERT( joint->islandId == islandId ); + B3_ASSERT( joint->islandIndex == i ); + int colorIndex = joint->colorIndex; + int localIndex = joint->localIndex; + + B3_ASSERT( 0 <= colorIndex && colorIndex < B3_GRAPH_COLOR_COUNT ); + + b3GraphColor* color = world->constraintGraph.colors + colorIndex; + + b3JointSim* awakeJointSim = b3Array_Get( color->jointSims, localIndex ); + + if ( colorIndex != B3_OVERFLOW_INDEX ) + { + // might clear a bit for a static body, but this has no effect + b3ClearBit( &color->bodySet, joint->edges[0].bodyId ); + b3ClearBit( &color->bodySet, joint->edges[1].bodyId ); + } + + int sleepJointIndex = sleepSet->jointSims.count; + b3JointSim* sleepJointSim = b3Array_Emplace( sleepSet->jointSims ); + memcpy( sleepJointSim, awakeJointSim, sizeof( b3JointSim ) ); + + int movedIndex = b3Array_RemoveSwap( color->jointSims, localIndex ); + if ( movedIndex != B3_NULL_INDEX ) + { + // fix moved element + b3JointSim* movedJointSim = color->jointSims.data + localIndex; + int movedId = movedJointSim->jointId; + b3Joint* movedJoint = b3Array_Get( world->joints, movedId ); + B3_ASSERT( movedJoint->localIndex == movedIndex ); + movedJoint->localIndex = localIndex; + } + + joint->setIndex = sleepSetId; + joint->colorIndex = B3_NULL_INDEX; + joint->localIndex = sleepJointIndex; + } + } + + // move island struct + { + B3_ASSERT( island->setIndex == b3_awakeSet ); + + int islandIndex = island->localIndex; + b3IslandSim* sleepIsland = b3Array_Emplace( sleepSet->islandSims ); + sleepIsland->islandId = islandId; + + int movedIslandIndex = b3Array_RemoveSwap( awakeSet->islandSims, islandIndex ); + if ( movedIslandIndex != B3_NULL_INDEX ) + { + // fix index on moved element + b3IslandSim* movedIslandSim = awakeSet->islandSims.data + islandIndex; + int movedIslandId = movedIslandSim->islandId; + b3Island* movedIsland = b3Array_Get( world->islands, movedIslandId ); + B3_ASSERT( movedIsland->localIndex == movedIslandIndex ); + movedIsland->localIndex = islandIndex; + } + + island->setIndex = sleepSetId; + island->localIndex = 0; + } + + if ( world->splitIslandId == islandId ) + { + world->splitIslandId = B3_NULL_INDEX; + } + + b3ValidateSolverSets( world ); +} + +// This is called when joints are created between sets. I want to allow the sets +// to continue sleeping if both are asleep. Otherwise one set is waked. +// Islands will get merge when the set is woke. +void b3MergeSolverSets( b3World* world, int setId1, int setId2 ) +{ + B3_ASSERT( setId1 >= b3_firstSleepingSet ); + B3_ASSERT( setId2 >= b3_firstSleepingSet ); + b3SolverSet* set1 = b3Array_Get( world->solverSets, setId1 ); + b3SolverSet* set2 = b3Array_Get( world->solverSets, setId2 ); + + // Move the fewest number of bodies + if ( set1->bodySims.count < set2->bodySims.count ) + { + b3SolverSet* tempSet = set1; + set1 = set2; + set2 = tempSet; + + int tempId = setId1; + setId1 = setId2; + setId2 = tempId; + } + + // transfer bodies + { + b3Body* bodies = world->bodies.data; + int bodyCount = set2->bodySims.count; + for ( int i = 0; i < bodyCount; ++i ) + { + b3BodySim* simSrc = set2->bodySims.data + i; + + b3Body* body = bodies + simSrc->bodyId; + B3_ASSERT( body->setIndex == setId2 ); + body->setIndex = setId1; + body->localIndex = set1->bodySims.count; + + b3BodySim* simDst = b3Array_Emplace( set1->bodySims ); + memcpy( simDst, simSrc, sizeof( b3BodySim ) ); + } + } + + // transfer contacts + { + int contactCount = set2->contactIndices.count; + for ( int i = 0; i < contactCount; ++i ) + { + int contactIndex = set2->contactIndices.data[i]; + b3Contact* contact = b3Array_Get( world->contacts, contactIndex ); + B3_ASSERT( contact->setIndex == setId2 ); + contact->setIndex = setId1; + contact->localIndex = set1->contactIndices.count; + b3Array_Push( set1->contactIndices, contactIndex ); + } + } + + // transfer joints + { + int jointCount = set2->jointSims.count; + for ( int i = 0; i < jointCount; ++i ) + { + b3JointSim* jointSrc = set2->jointSims.data + i; + + b3Joint* joint = b3Array_Get( world->joints, jointSrc->jointId ); + B3_ASSERT( joint->setIndex == setId2 ); + joint->setIndex = setId1; + joint->localIndex = set1->jointSims.count; + + b3JointSim* jointDst = b3Array_Emplace( set1->jointSims ); + memcpy( jointDst, jointSrc, sizeof( b3JointSim ) ); + } + } + + // transfer islands + { + int islandCount = set2->islandSims.count; + for ( int i = 0; i < islandCount; ++i ) + { + b3IslandSim* islandSrc = set2->islandSims.data + i; + int islandId = islandSrc->islandId; + + b3Island* island = b3Array_Get( world->islands, islandId ); + island->setIndex = setId1; + island->localIndex = set1->islandSims.count; + + b3IslandSim* islandDst = b3Array_Emplace( set1->islandSims ); + memcpy( islandDst, islandSrc, sizeof( b3IslandSim ) ); + } + } + + // destroy the merged set + // Warning: need to be careful not to destroy things that got transferred, like triangle caches. + b3DestroySolverSet( world, setId2 ); + + b3ValidateSolverSets( world ); +} + +void b3TransferBody( b3World* world, b3SolverSet* targetSet, b3SolverSet* sourceSet, b3Body* body ) +{ + if ( targetSet == sourceSet ) + { + return; + } + + int sourceIndex = body->localIndex; + b3BodySim* sourceSim = b3Array_Get( sourceSet->bodySims, sourceIndex ); + + int targetIndex = targetSet->bodySims.count; + b3BodySim* targetSim = b3Array_Emplace( targetSet->bodySims ); + memcpy( targetSim, sourceSim, sizeof( b3BodySim ) ); + + // Clear transient body flags + targetSim->flags &= ~( b3_isFast | b3_isSpeedCapped | b3_hadTimeOfImpact ); + + // Remove body sim from solver set that owns it + int movedIndex = b3Array_RemoveSwap( sourceSet->bodySims, sourceIndex ); + if ( movedIndex != B3_NULL_INDEX ) + { + // Fix moved body index + b3BodySim* movedSim = sourceSet->bodySims.data + sourceIndex; + int movedId = movedSim->bodyId; + b3Body* movedBody = b3Array_Get( world->bodies, movedId ); + B3_ASSERT( movedBody->localIndex == movedIndex ); + movedBody->localIndex = sourceIndex; + } + + if ( sourceSet->setIndex == b3_awakeSet ) + { + b3Array_RemoveSwap( sourceSet->bodyStates, sourceIndex ); + } + else if ( targetSet->setIndex == b3_awakeSet ) + { + b3BodyState* state = b3Array_Emplace( targetSet->bodyStates ); + *state = b3_identityBodyState; + state->flags = body->flags; + } + + body->setIndex = targetSet->setIndex; + body->localIndex = targetIndex; +} + +void b3TransferJoint( b3World* world, b3SolverSet* targetSet, b3SolverSet* sourceSet, b3Joint* joint ) +{ + if ( targetSet == sourceSet ) + { + return; + } + + int localIndex = joint->localIndex; + int colorIndex = joint->colorIndex; + + // Retrieve source. + b3JointSim* sourceSim; + if ( sourceSet->setIndex == b3_awakeSet ) + { + B3_ASSERT( 0 <= colorIndex && colorIndex < B3_GRAPH_COLOR_COUNT ); + b3GraphColor* color = world->constraintGraph.colors + colorIndex; + + sourceSim = b3Array_Get( color->jointSims, localIndex ); + } + else + { + B3_ASSERT( colorIndex == B3_NULL_INDEX ); + sourceSim = b3Array_Get( sourceSet->jointSims, localIndex ); + } + + // Create target and copy. Fix joint. + if ( targetSet->setIndex == b3_awakeSet ) + { + b3AddJointToGraph( world, sourceSim, joint ); + joint->setIndex = b3_awakeSet; + } + else + { + joint->setIndex = targetSet->setIndex; + joint->localIndex = targetSet->jointSims.count; + joint->colorIndex = B3_NULL_INDEX; + + b3JointSim* targetSim = b3Array_Emplace( targetSet->jointSims ); + memcpy( targetSim, sourceSim, sizeof( b3JointSim ) ); + } + + // Destroy source. + if ( sourceSet->setIndex == b3_awakeSet ) + { + b3RemoveJointFromGraph( world, joint->edges[0].bodyId, joint->edges[1].bodyId, colorIndex, localIndex ); + } + else + { + int movedIndex = b3Array_RemoveSwap( sourceSet->jointSims, localIndex ); + if ( movedIndex != B3_NULL_INDEX ) + { + // fix swapped element + b3JointSim* movedJointSim = sourceSet->jointSims.data + localIndex; + int movedId = movedJointSim->jointId; + b3Joint* movedJoint = b3Array_Get( world->joints, movedId ); + movedJoint->localIndex = localIndex; + } + } +} diff --git a/vendor/box3d/src/src/solver_set.h b/vendor/box3d/src/src/solver_set.h new file mode 100644 index 000000000..bf78ab04c --- /dev/null +++ b/vendor/box3d/src/src/solver_set.h @@ -0,0 +1,67 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +#pragma once + +#include "container.h" +#include "contact.h" + +typedef struct b3Body b3Body; +typedef struct b3BodySim b3BodySim; +typedef struct b3BodyState b3BodyState; +typedef struct b3IslandSim b3IslandSim; +typedef struct b3Joint b3Joint; +typedef struct b3JointSim b3JointSim; +typedef struct b3World b3World; + +b3DeclareArray( b3BodySim ); +b3DeclareArray( b3BodyState ); +b3DeclareArray( b3JointSim ); +b3DeclareArray( b3IslandSim ); + +// This holds solver set data. The following sets are used: +// - static set for all static bodies and joints between static bodies +// - active set for all active bodies with body states (no +// contacts or joints) +// - disabled set for disabled bodies and their joints +// - all further sets are sleeping island sets along with their contacts and joints +// The purpose of solver sets is to achieve high memory locality. +// https://www.youtube.com/watch?v=nZNd5FjSquk +typedef struct b3SolverSet +{ + // Body array. Empty for unused set. + b3Array( b3BodySim ) bodySims; + + // Body state only exists for active set + b3Array( b3BodyState ) bodyStates; + + // This holds sleeping/disabled joints. Empty for static/active set. + b3Array( b3JointSim ) jointSims; + + // This holds all contacts for sleeping sets. + // This holds non-touching contacts for the awake set. + // This should be empty for the static and disabled sets. + b3Array( int ) contactIndices; + + // The awake set has an array of islands. Sleeping sets normally have a single islands. However, joints + // created between sleeping sets causes the sets to merge, leaving them with multiple islands. These sleeping + // islands will be naturally merged with the set is woken. + // The static and disabled sets have no islands. + // Islands live in the solver sets to limit the number of islands that need to be considered for sleeping. + b3Array( b3IslandSim ) islandSims; + + // Aligns with b3World::solverSetIdPool. Used to create a stable id for body/contact/joint/islands. + int setIndex; +} b3SolverSet; + +void b3DestroySolverSet( b3World* world, int setIndex ); + +void b3WakeSolverSet( b3World* world, int setIndex ); +void b3TrySleepIsland( b3World* world, int islandId ); + +// Merge set 2 into set 1 then destroy set 2. +// Warning: any pointers into these sets will be orphaned. +void b3MergeSolverSets( b3World* world, int setIndex1, int setIndex2 ); + +void b3TransferBody( b3World* world, b3SolverSet* targetSet, b3SolverSet* sourceSet, b3Body* body ); +void b3TransferJoint( b3World* world, b3SolverSet* targetSet, b3SolverSet* sourceSet, b3Joint* joint ); diff --git a/vendor/box3d/src/src/sphere.c b/vendor/box3d/src/src/sphere.c new file mode 100644 index 000000000..9e4bdaac0 --- /dev/null +++ b/vendor/box3d/src/src/sphere.c @@ -0,0 +1,247 @@ +// SPDX-FileCopyrightText: 2026 Erin Catto +// SPDX-License-Identifier: MIT + +// Dirk Gregorius contributed portions of this code + +#include "math_internal.h" +#include "shape.h" + +#include "box3d/base.h" +#include "box3d/collision.h" +#include "box3d/constants.h" + +b3MassData b3ComputeSphereMass( const b3Sphere* shape, float density ) +{ + b3Vec3 center = shape->center; + float radius = shape->radius; + + float volume = 4.0f / 3.0f * B3_PI * radius * radius * radius; + float mass = volume * density; + float ixx = 0.4f * mass * radius * radius; + + b3MassData out; + out.mass = mass; + out.center = center; + + // Inertia about the center of mass + out.inertia = b3MakeDiagonalMatrix( ixx, ixx, ixx ); + return out; +} + +b3AABB b3ComputeSphereAABB( const b3Sphere* shape, b3Transform transform ) +{ + b3Vec3 center = b3TransformPoint( transform, shape->center ); + float radius = shape->radius; + b3Vec3 extent = { radius, radius, radius }; + return ( b3AABB ){ b3Sub( center, extent ), b3Add( center, extent ) }; +} + +b3AABB b3ComputeSweptSphereAABB( const b3Sphere* shape, b3Transform xf1, b3Transform xf2 ) +{ + b3Vec3 r = { shape->radius, shape->radius, shape->radius }; + b3Vec3 center1 = b3TransformPoint( xf1, shape->center ); + b3Vec3 center2 = b3TransformPoint( xf2, shape->center ); + return ( b3AABB ){ b3Sub( b3Min( center1, center2 ), r ), b3Add( b3Max( center1, center2 ), r ) }; +} + +bool b3OverlapSphere( const b3Sphere* shape, b3Transform shapeTransform, const b3ShapeProxy* proxy ) +{ + b3DistanceInput input; + input.proxyA = ( b3ShapeProxy ){ &shape->center, 1, shape->radius }; + input.proxyB = *proxy; + input.transform = b3InvMulTransforms( shapeTransform, b3Transform_identity ); + input.useRadii = true; + + b3SimplexCache cache = { 0 }; + b3DistanceOutput output = b3ShapeDistance( &input, &cache, NULL, 0 ); + return output.distance < B3_OVERLAP_SLOP; +} + +// Precision Improvements for Ray / Sphere Intersection - Ray Tracing Gems 2019 +// http://www.codercorner.com/blog/?p=321 +b3CastOutput b3RayCastSphere(const b3Sphere* shape, const b3RayCastInput* input ) +{ + B3_ASSERT( b3IsValidRay( input ) ); + b3CastOutput output = { 0 }; + + b3Vec3 p = shape->center; + + // Shift ray so sphere center is the origin + b3Vec3 s = b3Sub( input->origin, p ); + + float r = shape->radius; + float rr = r * r; + + float length; + b3Vec3 d = b3GetLengthAndNormalize( &length, input->translation ); + if ( length == 0.0f ) + { + // zero length ray + + if ( b3LengthSquared( s ) < rr ) + { + // initial overlap + output.point = input->origin; + output.hit = true; + } + + return output; + } + + // Find closest point on ray to origin + + // solve: dot(s + t * d, d) = 0 + float t = -b3Dot( s, d ); + + // c is the closest point on the line to the origin + b3Vec3 c = b3MulAdd( s, t, d ); + + float cc = b3Dot( c, c ); + + if ( cc > rr ) + { + // closest point is outside the sphere + return output; + } + + // Pythagoras + float h = sqrtf( rr - cc ); + + float fraction = t - h; + + if ( fraction < 0.0f || input->maxFraction * length < fraction ) + { + // intersection is point outside the range of the ray segment + + if ( b3LengthSquared( s ) < rr ) + { + // initial overlap + output.point = input->origin; + output.hit = true; + } + + return output; + } + + b3Vec3 hitPoint = b3MulAdd( s, fraction, d ); + + output.fraction = fraction / length; + + if ( output.fraction > input->maxFraction ) + { + b3Log( "sphere input fraction = %g, output fraction = %g", input->maxFraction, output.fraction ); + output.fraction = input->maxFraction; + } + + output.normal = b3Normalize( hitPoint ); + output.point = b3MulAdd( p, shape->radius, output.normal ); + output.hit = true; + + return output; +} + +// Precision Improvements for Ray / Sphere Intersection - Ray Tracing Gems 2019 +// http://www.codercorner.com/blog/?p=321 +// This will do interior hits. +b3CastOutput b3RayCastHollowSphere( const b3Sphere* sphere, const b3RayCastInput* input ) +{ + b3Vec3 p = sphere->center; + + b3CastOutput output = { 0 }; + + // Shift ray so sphere center is the origin + b3Vec3 s = b3Sub( input->origin, p ); + b3Vec3 d = b3Normalize( input->translation ); + + // Find closest point on ray to origin + + // solve: dot(s + t * d, d) = 0 + float t = -b3Dot( s, d ); + + // c is the closest point on the line to the origin + b3Vec3 c = b3MulAdd( s, t, d ); + + float cc = b3Dot( c, c ); + float r = sphere->radius; + float rr = r * r; + + if ( cc > rr ) + { + // closest point is outside the sphere + return output; + } + + // Pythagoras + float h = sqrtf( rr - cc ); + + float fraction = t - h; + + if ( fraction < 0.0f ) + { + fraction = t + h; + } + + if ( fraction < 0.0f ) + { + // behind the ray + return output; + } + + if (fraction > input->maxFraction) + { + return output; + } + + b3Vec3 hitPoint = b3MulAdd( s, fraction, d ); + + output.fraction = fraction; + output.normal = b3Normalize( hitPoint ); + output.point = b3MulAdd( p, sphere->radius, output.normal ); + output.hit = true; + + return output; +} + +b3CastOutput b3ShapeCastSphere( const b3Sphere* sphere, const b3ShapeCastInput* input ) +{ + b3ShapeCastPairInput pairInput; + pairInput.proxyA = ( b3ShapeProxy ){ &sphere->center, 1, sphere->radius }; + pairInput.proxyB = input->proxy; + pairInput.transform = b3Transform_identity; + pairInput.translationB = input->translation; + pairInput.maxFraction = input->maxFraction; + pairInput.canEncroach = input->canEncroach; + + b3CastOutput output = b3ShapeCast( &pairInput ); + return output; +} + +int b3CollideMoverAndSphere( b3PlaneResult* result, const b3Sphere* shape, const b3Capsule* mover ) +{ + float totalRadius = mover->radius + shape->radius; + b3Vec3 closest = b3PointToSegmentDistance( mover->center1, mover->center2, shape->center ); + + // The normal points from the sphere toward the mover. + float distance; + b3Vec3 normal = b3GetLengthAndNormalize( &distance, b3Sub( closest, shape->center ) ); + + if ( distance > totalRadius ) + { + return 0; + } + + float linearSlop = B3_LINEAR_SLOP; + if ( distance < linearSlop ) + { + // Deep overlap: the mover axis passes through the sphere center, so no + // direction is preferred. Push perpendicular to the mover axis. + float length; + b3Vec3 axis = b3GetLengthAndNormalize( &length, b3Sub( mover->center2, mover->center1 ) ); + normal = length > linearSlop ? b3Perp( axis ) : b3Vec3_axisY; + distance = 0.0f; + } + + b3Plane plane = { normal, totalRadius - distance }; + *result = ( b3PlaneResult ){ plane, shape->center }; + return 1; +} diff --git a/vendor/box3d/src/src/spherical_joint.c b/vendor/box3d/src/src/spherical_joint.c new file mode 100644 index 000000000..3f699ac18 --- /dev/null +++ b/vendor/box3d/src/src/spherical_joint.c @@ -0,0 +1,745 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +#include "body.h" +#include "core.h" +#include "joint.h" +#include "math_internal.h" +#include "physics_world.h" +#include "solver.h" +#include "solver_set.h" +#include "recording.h" + +// needed for dll export +#include "box3d/box3d.h" + +void b3SphericalJoint_EnableConeLimit( b3JointId jointId, bool enableLimit ) +{ + b3World* world = b3GetWorld( jointId.world0 ); + B3_REC( world, SphericalJointEnableConeLimit, jointId, enableLimit ); + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_sphericalJoint ); + if ( enableLimit != base->sphericalJoint.enableConeLimit ) + { + base->sphericalJoint.swingImpulse = 0.0f; + } + base->sphericalJoint.enableConeLimit = enableLimit; +} + +bool b3SphericalJoint_IsConeLimitEnabled( b3JointId jointId ) +{ + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_sphericalJoint ); + return base->sphericalJoint.enableConeLimit; +} + +float b3SphericalJoint_GetConeLimit( b3JointId jointId ) +{ + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_sphericalJoint ); + return base->sphericalJoint.coneAngle; +} + +void b3SphericalJoint_SetConeLimit( b3JointId jointId, float angleRadians ) +{ + B3_ASSERT( b3IsValidFloat( angleRadians ) && 0 <= angleRadians && angleRadians <= 0.5f * B3_PI ); + b3World* world = b3GetWorld( jointId.world0 ); + B3_REC( world, SphericalJointSetConeLimit, jointId, angleRadians ); + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_sphericalJoint ); + base->sphericalJoint.coneAngle = angleRadians; +} + +float b3SphericalJoint_GetConeAngle( b3JointId jointId ) +{ + b3World* world = b3GetWorld( jointId.world0 ); + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_sphericalJoint ); + b3WorldTransform transformA = b3GetBodyTransform( world, base->bodyIdA ); + b3WorldTransform transformB = b3GetBodyTransform( world, base->bodyIdB ); + + b3Quat quatA = b3MulQuat( transformA.q, base->localFrameA.q ); + b3Quat quatB = b3MulQuat( transformB.q, base->localFrameB.q ); + + if ( b3DotQuat( quatA, quatB ) < 0.0f ) + { + // this keeps the swing angle in the range [0, pi] + quatB = b3NegateQuat( quatB ); + } + + b3Quat relQ = b3InvMulQuat( quatA, quatB ); + + return b3GetSwingAngle( relQ ); +} + +void b3SphericalJoint_EnableTwistLimit( b3JointId jointId, bool enableLimit ) +{ + b3World* world = b3GetWorld( jointId.world0 ); + B3_REC( world, SphericalJointEnableTwistLimit, jointId, enableLimit ); + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_sphericalJoint ); + if ( enableLimit != base->sphericalJoint.enableTwistLimit ) + { + base->sphericalJoint.lowerTwistImpulse = 0.0f; + base->sphericalJoint.upperTwistImpulse = 0.0f; + } + base->sphericalJoint.enableTwistLimit = enableLimit; +} + +bool b3SphericalJoint_IsTwistLimitEnabled( b3JointId jointId ) +{ + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_sphericalJoint ); + return base->sphericalJoint.enableTwistLimit; +} + +float b3SphericalJoint_GetLowerTwistLimit( b3JointId jointId ) +{ + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_sphericalJoint ); + return base->sphericalJoint.lowerTwistAngle; +} + +float b3SphericalJoint_GetUpperTwistLimit( b3JointId jointId ) +{ + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_sphericalJoint ); + return base->sphericalJoint.upperTwistAngle; +} + +void b3SphericalJoint_SetTwistLimits( b3JointId jointId, float lowerLimitRadians, float upperLimitRadians ) +{ + B3_ASSERT( b3IsValidFloat( lowerLimitRadians ) && b3IsValidFloat( upperLimitRadians ) ); + b3World* world = b3GetWorld( jointId.world0 ); + B3_REC( world, SphericalJointSetTwistLimits, jointId, lowerLimitRadians, upperLimitRadians ); + + float lowerAngle = b3MinFloat( lowerLimitRadians, upperLimitRadians ); + float upperAngle = b3MaxFloat( lowerLimitRadians, upperLimitRadians ); + + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_sphericalJoint ); + base->sphericalJoint.lowerTwistAngle = b3ClampFloat( lowerAngle, -0.99f * B3_PI, 0.99f * B3_PI ); + base->sphericalJoint.upperTwistAngle = b3ClampFloat( upperAngle, -0.99f * B3_PI, 0.99f * B3_PI ); +} + +float b3SphericalJoint_GetTwistAngle( b3JointId jointId ) +{ + b3World* world = b3GetWorld( jointId.world0 ); + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_sphericalJoint ); + b3WorldTransform transformA = b3GetBodyTransform( world, base->bodyIdA ); + b3WorldTransform transformB = b3GetBodyTransform( world, base->bodyIdB ); + + b3Quat quatA = b3MulQuat( transformA.q, base->localFrameA.q ); + b3Quat quatB = b3MulQuat( transformB.q, base->localFrameB.q ); + + if ( b3DotQuat( quatA, quatB ) < 0.0f ) + { + // this keeps the twist angle in the range [-pi, pi] + quatB = b3NegateQuat( quatB ); + } + + b3Quat relQ = b3InvMulQuat( quatA, quatB ); + + return b3GetTwistAngle( relQ ); +} + +void b3SphericalJoint_EnableSpring( b3JointId jointId, bool enableSpring ) +{ + b3World* world = b3GetWorld( jointId.world0 ); + B3_REC( world, SphericalJointEnableSpring, jointId, enableSpring ); + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_sphericalJoint ); + if ( enableSpring != base->sphericalJoint.enableSpring ) + { + base->sphericalJoint.springImpulse = b3Vec3_zero; + } + base->sphericalJoint.enableSpring = enableSpring; +} + +bool b3SphericalJoint_IsSpringEnabled( b3JointId jointId ) +{ + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_sphericalJoint ); + return base->sphericalJoint.enableSpring; +} + +void b3SphericalJoint_SetTargetRotation( b3JointId jointId, b3Quat targetRotation ) +{ + B3_ASSERT( b3IsValidQuat( targetRotation ) ); + b3World* world = b3GetWorld( jointId.world0 ); + B3_REC( world, SphericalJointSetTargetRotation, jointId, targetRotation ); + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_sphericalJoint ); + base->sphericalJoint.targetRotation = targetRotation; +} + +b3Quat b3SphericalJoint_GetTargetRotation( b3JointId jointId ) +{ + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_sphericalJoint ); + return base->sphericalJoint.targetRotation; +} + +void b3SphericalJoint_SetSpringHertz( b3JointId jointId, float hertz ) +{ + B3_ASSERT( b3IsValidFloat( hertz ) && hertz >= 0.0f ); + b3World* world = b3GetWorld( jointId.world0 ); + B3_REC( world, SphericalJointSetSpringHertz, jointId, hertz ); + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_sphericalJoint ); + base->sphericalJoint.hertz = hertz; +} + +float b3SphericalJoint_GetSpringHertz( b3JointId jointId ) +{ + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_sphericalJoint ); + return base->sphericalJoint.hertz; +} + +void b3SphericalJoint_SetSpringDampingRatio( b3JointId jointId, float dampingRatio ) +{ + B3_ASSERT( b3IsValidFloat( dampingRatio ) && dampingRatio >= 0.0f ); + b3World* world = b3GetWorld( jointId.world0 ); + B3_REC( world, SphericalJointSetSpringDampingRatio, jointId, dampingRatio ); + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_sphericalJoint ); + base->sphericalJoint.dampingRatio = dampingRatio; +} + +float b3SphericalJoint_GetSpringDampingRatio( b3JointId jointId ) +{ + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_sphericalJoint ); + return base->sphericalJoint.dampingRatio; +} + +void b3SphericalJoint_EnableMotor( b3JointId jointId, bool enableMotor ) +{ + b3World* world = b3GetWorld( jointId.world0 ); + B3_REC( world, SphericalJointEnableMotor, jointId, enableMotor ); + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_sphericalJoint ); + if ( enableMotor != base->sphericalJoint.enableMotor ) + { + base->sphericalJoint.motorImpulse = b3Vec3_zero; + } + base->sphericalJoint.enableMotor = enableMotor; +} + +bool b3SphericalJoint_IsMotorEnabled( b3JointId jointId ) +{ + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_sphericalJoint ); + return base->sphericalJoint.enableMotor; +} + +void b3SphericalJoint_SetMotorVelocity( b3JointId jointId, b3Vec3 motorVelocity ) +{ + B3_ASSERT( b3IsValidVec3( motorVelocity ) ); + b3World* world = b3GetWorld( jointId.world0 ); + B3_REC( world, SphericalJointSetMotorVelocity, jointId, motorVelocity ); + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_sphericalJoint ); + base->sphericalJoint.motorVelocity = motorVelocity; +} + +b3Vec3 b3SphericalJoint_GetMotorVelocity( b3JointId jointId ) +{ + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_sphericalJoint ); + return base->sphericalJoint.motorVelocity; +} + +void b3SphericalJoint_SetMaxMotorTorque( b3JointId jointId, float maxForce ) +{ + B3_ASSERT( b3IsValidFloat( maxForce ) && maxForce >= 0.0f ); + b3World* world = b3GetWorld( jointId.world0 ); + B3_REC( world, SphericalJointSetMaxMotorTorque, jointId, maxForce ); + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_sphericalJoint ); + base->sphericalJoint.maxMotorTorque = maxForce; +} + +float b3SphericalJoint_GetMaxMotorTorque( b3JointId jointId ) +{ + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_sphericalJoint ); + return base->sphericalJoint.maxMotorTorque; +} + +b3Vec3 b3SphericalJoint_GetMotorTorque( b3JointId jointId ) +{ + b3World* world = b3GetWorld( jointId.world0 ); + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_sphericalJoint ); + return b3MulSV( world->inv_h, base->sphericalJoint.motorImpulse ); +} + +b3Vec3 b3GetSphericalJointForce( b3World* world, b3JointSim* base ) +{ + b3Vec3 force = b3MulSV( world->inv_h, base->sphericalJoint.linearImpulse ); + return force; +} + +b3Vec3 b3GetSphericalJointTorque( b3World* world, b3JointSim* base ) +{ + b3WorldTransform xfA = b3GetBodyTransform( world, base->bodyIdA ); + b3WorldTransform xfB = b3GetBodyTransform( world, base->bodyIdB ); + b3Quat qA = b3MulQuat( xfA.q, base->localFrameA.q ); + b3Quat qB = b3MulQuat( xfB.q, base->localFrameB.q ); + + // Cone axis is the z-axis of body A. + b3Vec3 coneAxis = b3RotateVector( qA, b3Vec3_axisZ ); + b3Vec3 twistAxis = b3RotateVector( qB, b3Vec3_axisZ ); + b3Vec3 swingAxis = b3Normalize( b3Cross( coneAxis, twistAxis ) ); + + b3SphericalJoint* joint = &base->sphericalJoint; + b3Vec3 impulse = b3Add( joint->springImpulse, joint->motorImpulse ); + impulse = b3MulAdd( impulse, joint->lowerTwistImpulse - joint->upperTwistImpulse, twistAxis ); + impulse = b3MulAdd( impulse, joint->swingImpulse, swingAxis ); + b3Vec3 torque = b3MulSV( world->inv_h, impulse ); + return torque; +} + +// Point-to-point constraint +// C = p2 - p1 +// Cdot = v2 - v1 +// = v2 + cross(w2, r2) - v1 - cross(w1, r1) +// J = [-I r1_skew I -r2_skew ] +// K = J * invM * transpose(J) +// transpose(skew(r)) = -skew(r) +// K = diag(1/m1 + 1/m2) - r1_skew * invI1 * r1_skew - r2_skew * invI2 * r2_skew + +// r_skew = R * skew(r_local) * RT +// invI = R * invI_local * RT +// r_skew * invI * r_skew = R * skew(r_local) * RT * R * invI_local * RT * R * r_skew * RT +// = R * ( skew(r_local) * invI_local * skew(r_local) ) * RT + +void b3PrepareSphericalJoint( b3JointSim* base, b3StepContext* context ) +{ + B3_ASSERT( base->type == b3_sphericalJoint ); + + b3World* world = context->world; + + b3Body* bodyA = b3Array_Get( world->bodies, base->bodyIdA ); + b3Body* bodyB = b3Array_Get( world->bodies, base->bodyIdB ); + + B3_ASSERT( bodyA->setIndex == b3_awakeSet || bodyB->setIndex == b3_awakeSet ); + b3SolverSet* setA = b3Array_Get( world->solverSets, bodyA->setIndex ); + b3SolverSet* setB = b3Array_Get( world->solverSets, bodyB->setIndex ); + + int localIndexA = bodyA->localIndex; + int localIndexB = bodyB->localIndex; + + b3BodySim* bodySimA = b3Array_Get( setA->bodySims, localIndexA ); + b3BodySim* bodySimB = b3Array_Get( setB->bodySims, localIndexB ); + + base->invMassA = bodySimA->invMass; + base->invMassB = bodySimB->invMass; + base->invIA = bodySimA->invInertiaWorld; + base->invIB = bodySimB->invInertiaWorld; + + b3Matrix3 invInertiaSum = b3AddMM( base->invIA, base->invIB ); + base->fixedRotation = b3Det( invInertiaSum ) < 1000.0f * FLT_MIN; + + b3SphericalJoint* joint = &base->sphericalJoint; + joint->indexA = bodyA->setIndex == b3_awakeSet ? localIndexA : B3_NULL_INDEX; + joint->indexB = bodyB->setIndex == b3_awakeSet ? localIndexB : B3_NULL_INDEX; + + // Compute joint anchor frames with world space rotation, relative to center of mass + joint->frameA.q = b3MulQuat( bodySimA->transform.q, base->localFrameA.q ); + joint->frameA.p = b3RotateVector( bodySimA->transform.q, b3Sub( base->localFrameA.p, bodySimA->localCenter ) ); + joint->frameB.q = b3MulQuat( bodySimB->transform.q, base->localFrameB.q ); + joint->frameB.p = b3RotateVector( bodySimB->transform.q, b3Sub( base->localFrameB.p, bodySimB->localCenter ) ); + + joint->deltaCenter = b3SubPos( bodySimB->center, bodySimA->center ); + + // Cone axis is the z-axis of body A. + b3Vec3 coneAxis = b3RotateVector( joint->frameA.q, b3Vec3_axisZ ); + + // Twist axis is the z-axis of body B. + b3Vec3 twistAxis = b3RotateVector( joint->frameB.q, b3Vec3_axisZ ); + + if ( joint->enableConeLimit ) + { + // Swing axis may be zero + b3Vec3 swingAxis = b3Normalize( b3Cross( coneAxis, twistAxis ) ); + float k = b3Dot( swingAxis, b3MulMV( invInertiaSum, swingAxis ) ); + joint->swingMass = k > 0.0f ? 1.0f / k : 0.0f; + joint->swingAxis = swingAxis; + } + + if ( joint->enableTwistLimit ) + { + b3Quat relQ = b3InvMulQuat( joint->frameA.q, joint->frameB.q ); + float tanThetaOver2 = sqrtf( ( relQ.v.x * relQ.v.x + relQ.v.y * relQ.v.y ) / ( relQ.v.z * relQ.v.z + relQ.s * relQ.s ) ); + + // todo verify this Jacobian using a finite difference, unit test? + b3Vec3 swingAxis = b3Normalize( b3Cross( coneAxis, twistAxis ) ); + b3Vec3 perpAxis = b3Cross( swingAxis, coneAxis ); + b3Vec3 twistJacobian = b3MulAdd( coneAxis, tanThetaOver2, perpAxis ); + float k = b3Dot( twistJacobian, b3MulMV( invInertiaSum, twistJacobian ) ); + joint->twistMass = k > 0.0f ? 1.0f / k : 0.0f; + joint->twistJacobian = twistJacobian; + } + + if ( base->fixedRotation == false ) + { + joint->rotationMass = b3InvertMatrix( invInertiaSum ); + } + else + { + joint->rotationMass = b3Mat3_zero; + } + + joint->springSoftness = b3MakeSoft( joint->hertz, joint->dampingRatio, context->h ); + + if ( context->enableWarmStarting == false ) + { + joint->linearImpulse = b3Vec3_zero; + joint->motorImpulse = b3Vec3_zero; + joint->springImpulse = b3Vec3_zero; + joint->swingImpulse = 0.0f; + joint->lowerTwistImpulse = 0.0f; + joint->upperTwistImpulse = 0.0f; + } +} + +void b3WarmStartSphericalJoint( b3JointSim* base, b3StepContext* context ) +{ + B3_ASSERT( base->type == b3_sphericalJoint ); + + float mA = base->invMassA; + float mB = base->invMassB; + b3Matrix3 iA = base->invIA; + b3Matrix3 iB = base->invIB; + + // dummy state for static bodies + b3BodyState dummyState = b3_identityBodyState; + + b3SphericalJoint* joint = &base->sphericalJoint; + + b3BodyState* stateA = joint->indexA == B3_NULL_INDEX ? &dummyState : context->states + joint->indexA; + b3BodyState* stateB = joint->indexB == B3_NULL_INDEX ? &dummyState : context->states + joint->indexB; + + b3Vec3 vA = stateA->linearVelocity; + b3Vec3 wA = stateA->angularVelocity; + b3Vec3 vB = stateB->linearVelocity; + b3Vec3 wB = stateB->angularVelocity; + + b3Vec3 rA = b3RotateVector( stateA->deltaRotation, joint->frameA.p ); + b3Vec3 rB = b3RotateVector( stateB->deltaRotation, joint->frameB.p ); + + b3Vec3 angularImpulse = b3Add( joint->springImpulse, joint->motorImpulse ); + angularImpulse = b3MulSub( angularImpulse, joint->swingImpulse, joint->swingAxis ); + angularImpulse = b3MulAdd( angularImpulse, joint->lowerTwistImpulse - joint->upperTwistImpulse, joint->twistJacobian ); + + vA = b3MulSub( vA, mA, joint->linearImpulse ); + wA = b3Sub( wA, b3MulMV( iA, b3Add( b3Cross( rA, joint->linearImpulse ), angularImpulse ) ) ); + + vB = b3MulAdd( vB, mB, joint->linearImpulse ); + wB = b3Add( wB, b3MulMV( iB, b3Add( b3Cross( rB, joint->linearImpulse ), angularImpulse ) ) ); + + if ( stateA->flags & b3_dynamicFlag ) + { + stateA->linearVelocity = vA; + stateA->angularVelocity = wA; + } + + if ( stateB->flags & b3_dynamicFlag ) + { + stateB->linearVelocity = vB; + stateB->angularVelocity = wB; + } +} + +void b3SolveSphericalJoint( b3JointSim* base, b3StepContext* context, bool useBias ) +{ + float mA = base->invMassA; + float mB = base->invMassB; + b3Matrix3 iA = base->invIA; + b3Matrix3 iB = base->invIB; + + // dummy state for static bodies + b3BodyState dummyState = b3_identityBodyState; + + b3SphericalJoint* joint = &base->sphericalJoint; + b3BodyState* stateA = joint->indexA == B3_NULL_INDEX ? &dummyState : context->states + joint->indexA; + b3BodyState* stateB = joint->indexB == B3_NULL_INDEX ? &dummyState : context->states + joint->indexB; + + b3Vec3 vA = stateA->linearVelocity; + b3Vec3 wA = stateA->angularVelocity; + b3Vec3 vB = stateB->linearVelocity; + b3Vec3 wB = stateB->angularVelocity; + + bool fixedRotation = base->fixedRotation; + b3Quat quatA = b3MulQuat( stateA->deltaRotation, joint->frameA.q ); + b3Quat quatB = b3MulQuat( stateB->deltaRotation, joint->frameB.q ); + + b3Quat relQ = b3InvMulQuat( quatA, quatB ); + + // Solve spring + if ( joint->enableSpring && fixedRotation == false ) + { + // Rotation constraint error + b3Vec3 deltaRotation = b3DeltaQuatToRotation( relQ, joint->targetRotation ); + b3Vec3 c = b3Neg( b3RotateVector( quatA, deltaRotation ) ); + + b3Vec3 bias = b3MulSV( joint->springSoftness.biasRate, c ); + float massScale = joint->springSoftness.massScale; + float impulseScale = joint->springSoftness.impulseScale; + b3Vec3 cdot = b3Sub( wB, wA ); + + b3Vec3 impulse = b3MulSub( b3MulSV( -massScale, b3MulMV( joint->rotationMass, b3Add( cdot, bias ) ) ), + impulseScale, joint->springImpulse ); + joint->springImpulse = b3Add( joint->springImpulse, impulse ); + + wA = b3Sub( wA, b3MulMV( iA, impulse ) ); + wB = b3Add( wB, b3MulMV( iB, impulse ) ); + } + + if ( joint->enableMotor && fixedRotation == false ) + { + b3Vec3 cdot = b3Sub( wB, wA ); + + b3Vec3 lambda = b3Neg( b3MulMV( joint->rotationMass, b3Sub( cdot, joint->motorVelocity ) ) ); + b3Vec3 newImpulse = b3Add( joint->motorImpulse, lambda ); + float length = b3Length( newImpulse ); + float maxImpulse = joint->maxMotorTorque * context->h; + if ( length > maxImpulse ) + { + newImpulse = b3MulSV( maxImpulse / length, newImpulse ); + } + + lambda = b3Sub( newImpulse, joint->motorImpulse ); + joint->motorImpulse = newImpulse; + + wA = b3Sub( wA, b3MulMV( iA, lambda ) ); + wB = b3Add( wB, b3MulMV( iB, lambda ) ); + } + + if ( joint->enableTwistLimit && fixedRotation == false ) + { + float twistAngle = b3GetTwistAngle( relQ ); + + // todo does an updated twist axis help? + + b3Vec3 twistJacobian = joint->twistJacobian; + + // Lower limit + { + float c = twistAngle - joint->lowerTwistAngle; + float bias = 0.0f; + float massScale = 1.0f; + float impulseScale = 0.0f; + if ( c > 0.0f ) + { + // speculation + bias = c * context->inv_h; + } + else if ( useBias ) + { + bias = base->constraintSoftness.biasRate * c; + massScale = base->constraintSoftness.massScale; + impulseScale = base->constraintSoftness.impulseScale; + } + + float cdot = b3Dot( b3Sub( wB, wA ), twistJacobian ); + float oldImpulse = joint->lowerTwistImpulse; + float deltaImpulse = -massScale * joint->twistMass * ( cdot + bias ) - impulseScale * oldImpulse; + joint->lowerTwistImpulse = b3MaxFloat( oldImpulse + deltaImpulse, 0.0f ); + deltaImpulse = joint->lowerTwistImpulse - oldImpulse; + + wA = b3MulSub( wA, deltaImpulse, b3MulMV( iA, twistJacobian ) ); + wB = b3MulAdd( wB, deltaImpulse, b3MulMV( iB, twistJacobian ) ); + } + + // Upper limit + { + float c = joint->upperTwistAngle - twistAngle; + float bias = 0.0f; + float massScale = 1.0f; + float impulseScale = 0.0f; + if ( c > 0.0f ) + { + // speculation + bias = c * context->inv_h; + } + else if ( useBias ) + { + bias = base->constraintSoftness.biasRate * c; + massScale = base->constraintSoftness.massScale; + impulseScale = base->constraintSoftness.impulseScale; + } + + // sign flipped on Cdot + float cdot = b3Dot( b3Sub( wA, wB ), twistJacobian ); + float oldImpulse = joint->upperTwistImpulse; + float deltaImpulse = -massScale * joint->twistMass * ( cdot + bias ) - impulseScale * oldImpulse; + joint->upperTwistImpulse = b3MaxFloat( oldImpulse + deltaImpulse, 0.0f ); + deltaImpulse = joint->upperTwistImpulse - oldImpulse; + + // sign flipped on applied impulse + wA = b3MulAdd( wA, deltaImpulse, b3MulMV( iA, twistJacobian ) ); + wB = b3MulSub( wB, deltaImpulse, b3MulMV( iB, twistJacobian ) ); + } + } + + if ( joint->enableConeLimit && fixedRotation == false ) + { + float swingAngle = b3GetSwingAngle( relQ ); + + // todo does an updated swing axis help? + // b3Vec3 axisA = b3RotateVector( quatA, b3Vec3_axisZ ); + // b3Vec3 axisB = b3RotateVector( quatB, b3Vec3_axisZ ); + // b3Vec3 swingAxis = b3Normalize( b3Cross( axisA, axisB ) ); + // joint->swingAxis = swingAxis; + + b3Vec3 swingAxis = joint->swingAxis; + + float c = joint->coneAngle - swingAngle; + float bias = 0.0f; + float massScale = 1.0f; + float impulseScale = 0.0f; + if ( c > 0.0f ) + { + // speculation + bias = c * context->inv_h; + } + else if ( useBias ) + { + bias = base->constraintSoftness.biasRate * c; + massScale = base->constraintSoftness.massScale; + impulseScale = base->constraintSoftness.impulseScale; + } + + // sign flipped on Cdot + float cdot = b3Dot( b3Sub( wA, wB ), swingAxis ); + float oldImpulse = joint->swingImpulse; + float deltaImpulse = -massScale * joint->swingMass * ( cdot + bias ) - impulseScale * oldImpulse; + joint->swingImpulse = b3MaxFloat( oldImpulse + deltaImpulse, 0.0f ); + deltaImpulse = joint->swingImpulse - oldImpulse; + + // sign flipped on applied impulse + wA = b3MulAdd( wA, deltaImpulse, b3MulMV( iA, swingAxis ) ); + wB = b3MulSub( wB, deltaImpulse, b3MulMV( iB, swingAxis ) ); + } + + // Solve point-to-point constraint + { + b3Vec3 rA = b3RotateVector( stateA->deltaRotation, joint->frameA.p ); + b3Vec3 rB = b3RotateVector( stateB->deltaRotation, joint->frameB.p ); + + b3Vec3 cdot = b3Sub( b3Sub( b3Add( vB, b3Cross( wB, rB ) ), vA ), b3Cross( wA, rA ) ); + + b3Vec3 bias = b3Vec3_zero; + float massScale = 1.0f; + float impulseScale = 0.0f; + if ( useBias ) + { + b3Vec3 dcA = stateA->deltaPosition; + b3Vec3 dcB = stateB->deltaPosition; + + b3Vec3 separation = b3Add( b3Sub( dcB, dcA ), b3Sub( rB, rA ) ); + separation = b3Add( separation, joint->deltaCenter ); + + bias = b3MulSV( base->constraintSoftness.biasRate, separation ); + massScale = base->constraintSoftness.massScale; + impulseScale = base->constraintSoftness.impulseScale; + } + + //// K = [(1/m1 + 1/m2) * eye(2) - skew(r1) * invI1 * skew(r1) - skew(r2) * invI2 * skew(r2)] + b3Matrix3 sA = b3Skew( rA ); + b3Matrix3 sB = b3Skew( rB ); + b3Matrix3 kA = b3MulMM( sA, b3MulMM( base->invIA, sA ) ); + b3Matrix3 kB = b3MulMM( sB, b3MulMM( base->invIB, sB ) ); + b3Matrix3 k = b3NegateMat3( b3AddMM( kA, kB ) ); + k.cx.x += mA + mB; + k.cy.y += mA + mB; + k.cz.z += mA + mB; + + b3Vec3 b = b3Solve3( k, b3Add( cdot, bias ) ); + + b3Vec3 impulse = b3MulSub( b3MulSV( -massScale, b ), impulseScale, joint->linearImpulse ); + joint->linearImpulse = b3Add( joint->linearImpulse, impulse ); + + vA = b3MulSub( vA, mA, impulse ); + wA = b3Sub( wA, b3MulMV( iA, b3Cross( rA, impulse ) ) ); + vB = b3MulAdd( vB, mB, impulse ); + wB = b3Add( wB, b3MulMV( iB, b3Cross( rB, impulse ) ) ); + } + + if ( stateA->flags & b3_dynamicFlag ) + { + stateA->linearVelocity = vA; + stateA->angularVelocity = wA; + } + + if ( stateB->flags & b3_dynamicFlag ) + { + stateB->linearVelocity = vB; + stateB->angularVelocity = wB; + } +} + +void b3DrawSphericalJoint( b3DebugDraw* draw, b3JointSim* base, b3WorldTransform transformA, b3WorldTransform transformB, float scale ) +{ + b3WorldTransform frameA = b3MulWorldTransforms( transformA, base->localFrameA ); + + float length1 = 0.1f * scale; + draw->DrawSegmentFcn( frameA.p, b3OffsetPos( frameA.p, b3MulSV( length1, b3RotateVector( frameA.q, b3Vec3_axisX ) ) ), b3_colorRed, + draw->context ); + draw->DrawSegmentFcn( frameA.p, b3OffsetPos( frameA.p, b3MulSV( length1, b3RotateVector( frameA.q, b3Vec3_axisY ) ) ), b3_colorGreen, + draw->context ); + draw->DrawSegmentFcn( frameA.p, b3OffsetPos( frameA.p, b3MulSV( length1, b3RotateVector( frameA.q, b3Vec3_axisZ ) ) ), b3_colorBlue, + draw->context ); + + b3WorldTransform frameB = b3MulWorldTransforms( transformB, base->localFrameB ); + + float length2 = 0.2f * scale; + draw->DrawSegmentFcn( frameB.p, b3OffsetPos( frameB.p, b3MulSV( length2, b3RotateVector( frameB.q, b3Vec3_axisZ ) ) ), b3_colorOrange, + draw->context ); + + b3SphericalJoint* joint = &base->sphericalJoint; + enum { kSliceCount = 16 }; + + // Twist limit + if ( joint->enableTwistLimit ) + { + b3Quat quatA = frameA.q; + b3Quat quatB = frameB.q; + + if ( b3DotQuat( quatA, quatB ) < 0.0f ) + { + // this keeps the twist angle in the range [-pi, pi] + quatB = b3NegateQuat( quatB ); + } + + b3Quat relQ = b3InvMulQuat( quatA, quatB ); + + const float wedgeRadius = 0.1f * scale; + for ( int index = 0; index < kSliceCount; ++index ) + { + float t1 = (float)( index + 0 ) / kSliceCount; + float alpha1 = ( 1.0f - t1 ) * joint->lowerTwistAngle + t1 * joint->upperTwistAngle; + float t2 = (float)( index + 1 ) / kSliceCount; + float alpha2 = ( 1.0f - t2 ) * joint->lowerTwistAngle + t2 * joint->upperTwistAngle; + + b3Vec3 vertex1 = { wedgeRadius * b3Cos( alpha1 ), wedgeRadius * b3Sin( alpha1 ), 0.0f }; + b3Vec3 vertex2 = { wedgeRadius * b3Cos( alpha2 ), wedgeRadius * b3Sin( alpha2 ), 0.0f }; + + if ( index == 0 ) + { + draw->DrawSegmentFcn( frameA.p, b3TransformWorldPoint( frameA, vertex1 ), b3_colorCyan, draw->context ); + } + + if ( index == kSliceCount - 1 ) + { + draw->DrawSegmentFcn( b3TransformWorldPoint( frameA, vertex2 ), frameA.p, b3_colorCyan, draw->context ); + } + draw->DrawSegmentFcn( b3TransformWorldPoint( frameA, vertex1 ), b3TransformWorldPoint( frameA, vertex2 ), b3_colorCyan, + draw->context ); + } + + float twistAngle = b3GetTwistAngle( relQ ); + b3Vec3 p2 = { wedgeRadius * b3Cos( twistAngle ), wedgeRadius * b3Sin( twistAngle ), 0.0f }; + draw->DrawSegmentFcn( frameA.p, b3TransformWorldPoint( frameA, p2 ), b3_colorYellow, draw->context ); + } + + // Swing limit + if ( joint->enableConeLimit ) + { + const float radius = 0.1f * scale; + float coneRadius = radius * b3Sin( joint->coneAngle ); + float coneHeight = radius * b3Cos( joint->coneAngle ); + + for ( int index = 0; index < kSliceCount; ++index ) + { + float phi1 = 2.0f * ( index + 0 ) / kSliceCount * B3_PI; + float phi2 = 2.0f * ( index + 1 ) / kSliceCount * B3_PI; + + b3Vec3 vertex1 = { coneRadius * b3Cos( phi1 ), coneRadius * b3Sin( phi1 ), coneHeight }; + b3Vec3 vertex2 = { coneRadius * b3Cos( phi2 ), coneRadius * b3Sin( phi2 ), coneHeight }; + + draw->DrawSegmentFcn( frameA.p, b3TransformWorldPoint( frameA, vertex1 ), b3_colorCyan, draw->context ); + draw->DrawSegmentFcn( b3TransformWorldPoint( frameA, vertex1 ), b3TransformWorldPoint( frameA, vertex2 ), b3_colorCyan, + draw->context ); + } + } +} diff --git a/vendor/box3d/src/src/table.c b/vendor/box3d/src/src/table.c new file mode 100644 index 000000000..fb066b807 --- /dev/null +++ b/vendor/box3d/src/src/table.c @@ -0,0 +1,248 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +#include "table.h" + +#include "bitset.h" +#include "core.h" +#include "ctz.h" +#include "platform.h" + +#include + +#if B3_DEBUG +b3AtomicInt b3_probeCount; +#endif + +_Static_assert( 2 * B3_SHAPE_POWER + B3_CHILD_POWER == 64, "compound power" ); +_Static_assert( B3_CHILD_POWER > 8, "compound child power" ); + +b3HashSet b3CreateSet( int32_t capacity ) +{ + b3HashSet set = { 0 }; + + // Capacity must be a power of 2 + if ( capacity > 16 ) + { + set.capacity = b3RoundUpPowerOf2( capacity ); + } + else + { + set.capacity = 16; + } + + set.count = 0; + set.items = (b3SetItem*)b3Alloc( set.capacity * sizeof( b3SetItem ) ); + memset( set.items, 0, set.capacity * sizeof( b3SetItem ) ); + + return set; +} + +void b3DestroySet( b3HashSet* set ) +{ + b3Free( set->items, set->capacity * sizeof( b3SetItem ) ); + set->items = NULL; + set->count = 0; + set->capacity = 0; +} + +void b3ClearSet( b3HashSet* set ) +{ + set->count = 0; + memset( set->items, 0, set->capacity * sizeof( b3SetItem ) ); +} + +// I need a good hash because the keys are built from pairs of increasing integers. +// A simple hash like hash = (integer1 XOR integer2) has many collisions. +// https://lemire.me/blog/2018/08/15/fast-strongly-universal-64-bit-hashing-everywhere/ +// https://preshing.com/20130107/this-hash-set-is-faster-than-a-judy-array/ +// todo try: https://www.jandrewrogers.com/2019/02/12/fast-perfect-hashing/ +// todo try: +// https://probablydance.com/2018/06/16/fibonacci-hashing-the-optimization-that-the-world-forgot-or-a-better-alternative-to-integer-modulo/ +static inline uint32_t b3KeyHash( uint64_t key ) +{ + uint64_t h = key; + h ^= h >> 33; + h *= 0xff51afd7ed558ccdL; + h ^= h >> 33; + h *= 0xc4ceb9fe1a85ec53L; + h ^= h >> 33; + + return (uint32_t)h; +} + +static int32_t b3FindSlot( const b3HashSet* set, uint64_t key, uint32_t hash ) +{ + uint32_t capacity = set->capacity; + int32_t index = hash & ( capacity - 1 ); + const b3SetItem* items = set->items; + while ( items[index].hash != 0 && items[index].key != key ) + { +#if B3_DEBUG + b3AtomicFetchAddInt( &b3_probeCount, 1 ); +#endif + index = ( index + 1 ) & ( capacity - 1 ); + } + + return index; +} + +static void b3AddKeyHaveCapacity( b3HashSet* set, uint64_t key, uint32_t hash ) +{ + int32_t index = b3FindSlot( set, key, hash ); + b3SetItem* items = set->items; + B3_ASSERT( items[index].hash == 0 ); + + items[index].key = key; + items[index].hash = hash; + set->count += 1; +} + +static void b3GrowTable( b3HashSet* set ) +{ + uint32_t oldCount = set->count; + B3_UNUSED( oldCount ); + + uint32_t oldCapacity = set->capacity; + b3SetItem* oldItems = set->items; + + set->count = 0; + // Capacity must be a power of 2 + set->capacity = 2 * oldCapacity; + set->items = (b3SetItem*)b3Alloc( set->capacity * sizeof( b3SetItem ) ); + memset( set->items, 0, set->capacity * sizeof( b3SetItem ) ); + + // Transfer items into new array + for ( uint32_t i = 0; i < oldCapacity; ++i ) + { + b3SetItem* item = oldItems + i; + if ( item->hash == 0 ) + { + // this item was empty + continue; + } + + b3AddKeyHaveCapacity( set, item->key, item->hash ); + } + + B3_ASSERT( set->count == oldCount ); + + b3Free( oldItems, oldCapacity * sizeof( b3SetItem ) ); +} + +bool b3ContainsKey( const b3HashSet* set, uint64_t key ) +{ + // key of zero is a sentinel + B3_ASSERT( key != 0 ); + uint32_t hash = b3KeyHash( key ); + int32_t index = b3FindSlot( set, key, hash ); + return set->items[index].key == key; +} + +int b3GetHashSetBytes( b3HashSet* set ) +{ + return set->capacity * (int)sizeof( b3SetItem ); +} + +bool b3AddKey( b3HashSet* set, uint64_t key ) +{ + // key of zero is a sentinel + B3_ASSERT( key != 0 ); + + uint32_t hash = b3KeyHash( key ); + B3_ASSERT( hash != 0 ); + + int32_t index = b3FindSlot( set, key, hash ); + if ( set->items[index].hash != 0 ) + { + // Already in set + B3_ASSERT( set->items[index].hash == hash && set->items[index].key == key ); + return true; + } + + if ( 2 * set->count >= set->capacity ) + { + b3GrowTable( set ); + } + + b3AddKeyHaveCapacity( set, key, hash ); + return false; +} + +// See https://en.wikipedia.org/wiki/Open_addressing +bool b3RemoveKey( b3HashSet* set, uint64_t key ) +{ + uint32_t hash = b3KeyHash( key ); + int32_t i = b3FindSlot( set, key, hash ); + b3SetItem* items = set->items; + if ( items[i].hash == 0 ) + { + // Not in set + return false; + } + + // Mark item i as unoccupied + items[i].key = 0; + items[i].hash = 0; + + B3_ASSERT( set->count > 0 ); + set->count -= 1; + + // Attempt to fill item i + int32_t j = i; + uint32_t capacity = set->capacity; + for ( ;; ) + { + j = ( j + 1 ) & ( capacity - 1 ); + if ( items[j].hash == 0 ) + { + break; + } + + // k is the first item for the hash of j + int32_t k = items[j].hash & ( capacity - 1 ); + + // determine if k lies cyclically in (i,j] + // i <= j: | i..k..j | + // i > j: |.k..j i....| or |....j i..k.| + if ( i <= j ) + { + if ( i < k && k <= j ) + { + continue; + } + } + else + { + if ( i < k || k <= j ) + { + continue; + } + } + + // Move j into i + items[i] = items[j]; + + // Mark item j as unoccupied + items[j].key = 0; + items[j].hash = 0; + + i = j; + } + + return true; +} + +// This function is here because ctz.h is included by +// this file but not in bitset.c +int b3CountSetBits( b3BitSet* bitSet ) +{ + int popCount = 0; + uint32_t blockCount = bitSet->blockCount; + for ( uint32_t i = 0; i < blockCount; ++i ) + { + popCount += b3PopCount64( bitSet->bits[i] ); + } + + return popCount; +} diff --git a/vendor/box3d/src/src/table.h b/vendor/box3d/src/src/table.h new file mode 100644 index 000000000..ec8f39782 --- /dev/null +++ b/vendor/box3d/src/src/table.h @@ -0,0 +1,53 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +#pragma once + +#include "box3d/constants.h" + +#include +#include + +typedef struct b3SetItem +{ + uint64_t key; + uint32_t hash; +} b3SetItem; + +typedef struct b3HashSet +{ + b3SetItem* items; + uint32_t capacity; + uint32_t count; +} b3HashSet; + +#define B3_SHAPE_MASK ( B3_MAX_SHAPES - 1 ) +#define B3_CHILD_MASK ( B3_MAX_CHILD_SHAPES - 1 ) + +static inline uint64_t b3ShapePairKey( int s1, int s2, int c ) +{ + if (s1 < s2) + { + return ( (uint64_t)( B3_SHAPE_MASK & s1 ) << ( 64 - B3_SHAPE_POWER ) ) | + ( (uint64_t)( B3_SHAPE_MASK & s2 ) << ( 64 - 2 * B3_SHAPE_POWER ) ) | ( (uint64_t)( B3_CHILD_MASK & c ) ); + } + + return ( (uint64_t)( B3_SHAPE_MASK & s2 ) << ( 64 - B3_SHAPE_POWER ) ) | + ( (uint64_t)( B3_SHAPE_MASK & s1 ) << ( 64 - 2 * B3_SHAPE_POWER ) ) | + ( (uint64_t)( B3_CHILD_MASK & c ) ); +} + +b3HashSet b3CreateSet( int32_t capacity ); +void b3DestroySet( b3HashSet* set ); + +void b3ClearSet( b3HashSet* set ); + +// Returns true if key was already in set +bool b3AddKey( b3HashSet* set, uint64_t key ); + +// Returns true if the key was found +bool b3RemoveKey( b3HashSet* set, uint64_t key ); + +bool b3ContainsKey( const b3HashSet* set, uint64_t key ); + +int b3GetHashSetBytes( b3HashSet* set ); diff --git a/vendor/box3d/src/src/timer.c b/vendor/box3d/src/src/timer.c new file mode 100644 index 000000000..826b1495c --- /dev/null +++ b/vendor/box3d/src/src/timer.c @@ -0,0 +1,700 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +// Required on Linux to expose pthread_setname_np. Must be defined before any +// system header is included. +#if defined( __linux__ ) && !defined( _GNU_SOURCE ) +#define _GNU_SOURCE +#endif + +#include "core.h" + +#include "box3d/base.h" + +#include +#include +#include + +#define NAME_LENGTH 16 + +#if defined( _WIN32 ) + +#ifndef WIN32_LEAN_AND_MEAN +#define WIN32_LEAN_AND_MEAN 1 +#endif + +#include +#include + +static double s_invFrequency = 0.0; + +uint64_t b3GetTicks( void ) +{ + LARGE_INTEGER counter; + QueryPerformanceCounter( &counter ); + return (uint64_t)counter.QuadPart; +} + +float b3GetMilliseconds( uint64_t ticks ) +{ + if ( s_invFrequency == 0.0 ) + { + LARGE_INTEGER frequency; + QueryPerformanceFrequency( &frequency ); + + s_invFrequency = (double)frequency.QuadPart; + if ( s_invFrequency > 0.0 ) + { + s_invFrequency = 1000.0 / s_invFrequency; + } + } + + uint64_t ticksNow = b3GetTicks(); + return (float)( s_invFrequency * ( ticksNow - ticks ) ); +} + +float b3GetMillisecondsAndReset( uint64_t* ticks ) +{ + if ( s_invFrequency == 0.0 ) + { + LARGE_INTEGER frequency; + QueryPerformanceFrequency( &frequency ); + + s_invFrequency = (double)frequency.QuadPart; + if ( s_invFrequency > 0.0 ) + { + s_invFrequency = 1000.0 / s_invFrequency; + } + } + + uint64_t ticksNow = b3GetTicks(); + float ms = (float)( s_invFrequency * ( ticksNow - *ticks ) ); + *ticks = ticksNow; + return ms; +} + +void b3Yield( void ) +{ + SwitchToThread(); +} + +void b3Sleep( int milliseconds ) +{ + Sleep( (DWORD)milliseconds ); +} + +typedef struct b3Mutex +{ + CRITICAL_SECTION cs; +} b3Mutex; + +b3Mutex* b3CreateMutex( void ) +{ + b3Mutex* m = b3Alloc( sizeof( b3Mutex ) ); + InitializeCriticalSection( &m->cs ); + return m; +} + +void b3DestroyMutex( b3Mutex* m ) +{ + DeleteCriticalSection( &m->cs ); + *m = (b3Mutex){ 0 }; + b3Free( m, sizeof( b3Mutex ) ); +} + +void b3LockMutex( b3Mutex* m ) +{ + EnterCriticalSection( &m->cs ); +} + +void b3UnlockMutex( b3Mutex* m ) +{ + LeaveCriticalSection( &m->cs ); +} + +typedef struct b3Semaphore +{ + HANDLE semaphore; +} b3Semaphore; + +b3Semaphore* b3CreateSemaphore( int initCount ) +{ + b3Semaphore* s = b3Alloc( sizeof( b3Semaphore ) ); + s->semaphore = CreateSemaphoreExW( NULL, initCount, INT_MAX, NULL, 0, SEMAPHORE_ALL_ACCESS ); + return s; +} + +void b3DestroySemaphore( b3Semaphore* s ) +{ + CloseHandle( s->semaphore ); + *s = (b3Semaphore){ 0 }; + b3Free( s, sizeof( b3Semaphore ) ); +} + +void b3WaitSemaphore( b3Semaphore* s ) +{ + WaitForSingleObjectEx( s->semaphore, INFINITE, FALSE ); +} + +void b3SignalSemaphore( b3Semaphore* s ) +{ + ReleaseSemaphore( s->semaphore, 1, NULL ); +} + +typedef struct b3Thread +{ + HANDLE thread; + b3ThreadFunction* function; + void* context; + char name[NAME_LENGTH]; +} b3Thread; + +typedef HRESULT( WINAPI* b3SetThreadDescriptionFn )( HANDLE, PCWSTR ); + +// SetThreadDescription exists on Windows 10 1607+. Resolve it dynamically so +// older Windows versions still link. Resolved once, cached for subsequent calls. +static void b3SetCurrentThreadName( const char* name ) +{ + if ( name == NULL || name[0] == 0 ) + { + return; + } + + static b3SetThreadDescriptionFn pfn = NULL; + static int resolved = 0; + + if ( resolved == 0 ) + { + HMODULE kernel = GetModuleHandleW( L"kernel32.dll" ); + if ( kernel != NULL ) + { + // MSVC /Wall warns C4191 on every FARPROC function-pointer cast. + // This is the intended use of GetProcAddress, so suppress locally. +#pragma warning( push ) +#pragma warning( disable : 4191 ) + pfn = (b3SetThreadDescriptionFn)GetProcAddress( kernel, "SetThreadDescription" ); +#pragma warning( pop ) + } + resolved = 1; + } + + if ( pfn == NULL ) + { + return; + } + + wchar_t wide[NAME_LENGTH]; + int n = MultiByteToWideChar( CP_UTF8, 0, name, -1, wide, (int)( sizeof( wide ) / sizeof( wide[0] ) ) ); + if ( n > 0 ) + { + pfn( GetCurrentThread(), wide ); + } +} + +static DWORD WINAPI b3ThreadStart( LPVOID param ) +{ + b3Thread* t = (b3Thread*)param; + b3SetCurrentThreadName( t->name ); + t->function( t->context ); + return 0; +} + +b3Thread* b3CreateThread( b3ThreadFunction* function, void* context, const char* name ) +{ + b3Thread* t = b3Alloc( sizeof( b3Thread ) ); + t->function = function; + t->context = context; + if ( name != NULL ) + { + snprintf( t->name, sizeof( t->name ), "%s", name ); + } + else + { + t->name[0] = 0; + } + t->thread = CreateThread( NULL, 0, b3ThreadStart, t, 0, NULL ); + return t; +} + +void b3JoinThread( b3Thread* t ) +{ + WaitForSingleObject( t->thread, INFINITE ); + CloseHandle( t->thread ); + *t = (b3Thread){ 0 }; + b3Free( t, sizeof( b3Thread ) ); +} + +#elif defined( __linux__ ) || defined( __EMSCRIPTEN__ ) + +#include +#include + +uint64_t b3GetTicks( void ) +{ + struct timespec ts; + clock_gettime( CLOCK_MONOTONIC, &ts ); + return ts.tv_sec * 1000000000LL + ts.tv_nsec; +} + +float b3GetMilliseconds( uint64_t ticks ) +{ + uint64_t ticksNow = b3GetTicks(); + return (float)( ( ticksNow - ticks ) / 1000000.0 ); +} + +float b3GetMillisecondsAndReset( uint64_t* ticks ) +{ + uint64_t ticksNow = b3GetTicks(); + float ms = (float)( ( ticksNow - *ticks ) / 1000000.0 ); + *ticks = ticksNow; + return ms; +} + +void b3Yield( void ) +{ + sched_yield(); +} + +void b3Sleep( int milliseconds ) +{ + struct timespec ts; + ts.tv_sec = milliseconds / 1000; + ts.tv_nsec = ( milliseconds % 1000 ) * 1000000L; + nanosleep( &ts, NULL ); +} + +#include +typedef struct b3Mutex +{ + pthread_mutex_t mtx; +} b3Mutex; + +b3Mutex* b3CreateMutex( void ) +{ + b3Mutex* m = b3Alloc( sizeof( b3Mutex ) ); + pthread_mutex_init( &m->mtx, NULL ); + return m; +} + +void b3DestroyMutex( b3Mutex* m ) +{ + pthread_mutex_destroy( &m->mtx ); + *m = (b3Mutex){ 0 }; + b3Free( m, sizeof( b3Mutex ) ); +} + +void b3LockMutex( b3Mutex* m ) +{ + pthread_mutex_lock( &m->mtx ); +} + +void b3UnlockMutex( b3Mutex* m ) +{ + pthread_mutex_unlock( &m->mtx ); +} + +#include + +typedef struct b3Semaphore +{ + sem_t semaphore; +} b3Semaphore; + +b3Semaphore* b3CreateSemaphore( int initCount ) +{ + b3Semaphore* s = b3Alloc( sizeof( b3Semaphore ) ); + sem_init( &s->semaphore, 0, (unsigned int)initCount ); + return s; +} + +void b3DestroySemaphore( b3Semaphore* s ) +{ + sem_destroy( &s->semaphore ); + *s = (b3Semaphore){ 0 }; + b3Free( s, sizeof( b3Semaphore ) ); +} + +void b3WaitSemaphore( b3Semaphore* s ) +{ + sem_wait( &s->semaphore ); +} + +void b3SignalSemaphore( b3Semaphore* s ) +{ + sem_post( &s->semaphore ); +} + +typedef struct b3Thread +{ + pthread_t thread; + b3ThreadFunction* function; + void* context; + char name[NAME_LENGTH]; +} b3Thread; + +static void b3SetCurrentThreadName( const char* name ) +{ + if ( name == NULL || name[0] == 0 ) + { + return; + } + +#if defined( __linux__ ) + // Linux caps thread names at 15 chars + null terminator. + char truncated[NAME_LENGTH]; + snprintf( truncated, sizeof( truncated ), "%s", name ); + pthread_setname_np( pthread_self(), truncated ); +#else + (void)name; +#endif +} + +static void* b3ThreadStart( void* param ) +{ + b3Thread* t = (b3Thread*)param; + b3SetCurrentThreadName( t->name ); + t->function( t->context ); + return NULL; +} + +b3Thread* b3CreateThread( b3ThreadFunction* function, void* context, const char* name ) +{ + b3Thread* t = b3Alloc( sizeof( b3Thread ) ); + t->function = function; + t->context = context; + if ( name != NULL ) + { + snprintf( t->name, sizeof( t->name ), "%s", name ); + } + else + { + t->name[0] = 0; + } + pthread_create( &t->thread, NULL, b3ThreadStart, t ); + return t; +} + +void b3JoinThread( b3Thread* t ) +{ + pthread_join( t->thread, NULL ); + *t = (b3Thread){ 0 }; + b3Free( t, sizeof( b3Thread ) ); +} + +#elif defined( __APPLE__ ) + +#include +#include +#include +#include + +static double s_invFrequency = 0.0; + +uint64_t b3GetTicks( void ) +{ + return mach_absolute_time(); +} + +float b3GetMilliseconds( uint64_t ticks ) +{ + if ( s_invFrequency == 0 ) + { + mach_timebase_info_data_t timebase; + mach_timebase_info( &timebase ); + + // convert to ns then to ms + s_invFrequency = 1e-6 * (double)timebase.numer / (double)timebase.denom; + } + + uint64_t ticksNow = b3GetTicks(); + return (float)( s_invFrequency * ( ticksNow - ticks ) ); +} + +float b3GetMillisecondsAndReset( uint64_t* ticks ) +{ + if ( s_invFrequency == 0 ) + { + mach_timebase_info_data_t timebase; + mach_timebase_info( &timebase ); + + // convert to ns then to ms + s_invFrequency = 1e-6 * (double)timebase.numer / (double)timebase.denom; + } + + uint64_t ticksNow = b3GetTicks(); + float ms = (float)( s_invFrequency * ( ticksNow - *ticks ) ); + *ticks = ticksNow; + return ms; +} + +void b3Yield( void ) +{ + sched_yield(); +} + +void b3Sleep( int milliseconds ) +{ + struct timespec ts; + ts.tv_sec = milliseconds / 1000; + ts.tv_nsec = ( milliseconds % 1000 ) * 1000000L; + nanosleep( &ts, NULL ); +} + +#include +typedef struct b3Mutex +{ + pthread_mutex_t mtx; +} b3Mutex; + +b3Mutex* b3CreateMutex( void ) +{ + b3Mutex* m = b3Alloc( sizeof( b3Mutex ) ); + pthread_mutex_init( &m->mtx, NULL ); + return m; +} + +void b3DestroyMutex( b3Mutex* m ) +{ + pthread_mutex_destroy( &m->mtx ); + *m = (b3Mutex){ 0 }; + b3Free( m, sizeof( b3Mutex ) ); +} + +void b3LockMutex( b3Mutex* m ) +{ + pthread_mutex_lock( &m->mtx ); +} + +void b3UnlockMutex( b3Mutex* m ) +{ + pthread_mutex_unlock( &m->mtx ); +} + +#include + +typedef struct b3Semaphore +{ + dispatch_semaphore_t semaphore; + int initialCount; +} b3Semaphore; + +b3Semaphore* b3CreateSemaphore( int initCount ) +{ + b3Semaphore* s = b3Alloc( sizeof( b3Semaphore ) ); + s->semaphore = dispatch_semaphore_create( (long)initCount ); + s->initialCount = initCount; + return s; +} + +void b3DestroySemaphore( b3Semaphore* s ) +{ + // libdispatch aborts if the current count is less than the initial count at release time. + // Pad with signals so the invariant always holds; no one is waiting at this point. + for ( int i = 0; i < s->initialCount; ++i ) + { + dispatch_semaphore_signal( s->semaphore ); + } + dispatch_release( s->semaphore ); + *s = (b3Semaphore){ 0 }; + b3Free( s, sizeof( b3Semaphore ) ); +} + +void b3WaitSemaphore( b3Semaphore* s ) +{ + dispatch_semaphore_wait( s->semaphore, DISPATCH_TIME_FOREVER ); +} + +void b3SignalSemaphore( b3Semaphore* s ) +{ + dispatch_semaphore_signal( s->semaphore ); +} + +typedef struct b3Thread +{ + pthread_t thread; + b3ThreadFunction* function; + void* context; + char name[NAME_LENGTH]; +} b3Thread; + +// macOS pthread_setname_np takes only the name — it always names the calling thread. +static void b3SetCurrentThreadName( const char* name ) +{ + if ( name == NULL || name[0] == 0 ) + { + return; + } + pthread_setname_np( name ); +} + +static void* b3ThreadStart( void* param ) +{ + b3Thread* t = (b3Thread*)param; + b3SetCurrentThreadName( t->name ); + t->function( t->context ); + return NULL; +} + +b3Thread* b3CreateThread( b3ThreadFunction* function, void* context, const char* name ) +{ + b3Thread* t = b3Alloc( sizeof( b3Thread ) ); + t->function = function; + t->context = context; + if ( name != NULL ) + { + snprintf( t->name, sizeof( t->name ), "%s", name ); + } + else + { + t->name[0] = 0; + } + pthread_create( &t->thread, NULL, b3ThreadStart, t ); + return t; +} + +void b3JoinThread( b3Thread* t ) +{ + pthread_join( t->thread, NULL ); + *t = (b3Thread){ 0 }; + b3Free( t, sizeof( b3Thread ) ); +} + +#else + +uint64_t b3GetTicks( void ) +{ + return 0; +} + +float b3GetMilliseconds( uint64_t ticks ) +{ + ( (void)( ticks ) ); + return 0.0f; +} + +float b3GetMillisecondsAndReset( uint64_t* ticks ) +{ + ( (void)( ticks ) ); + return 0.0f; +} + +void b3Yield( void ) +{ +} + +void b3Sleep( int milliseconds ) +{ + ( (void)( milliseconds ) ); +} + +typedef struct b3Mutex +{ + int dummy; +} b3Mutex; + +b3Mutex* b3CreateMutex( void ) +{ + b3Mutex* m = b3Alloc( sizeof( b3Mutex ) ); + m->dummy = 42; + return m; +} + +void b3DestroyMutex( b3Mutex* m ) +{ + *m = (b3Mutex){ 0 }; + b3Free( m, sizeof( b3Mutex ) ); +} + +void b3LockMutex( b3Mutex* m ) +{ + (void)m; +} + +void b3UnlockMutex( b3Mutex* m ) +{ + (void)m; +} + +typedef struct b3Semaphore +{ + int dummy; +} b3Semaphore; + +b3Semaphore* b3CreateSemaphore( int initCount ) +{ + b3Semaphore* s = b3Alloc( sizeof( b3Semaphore ) ); + (void)initCount; + s->dummy = 42; + return s; +} + +void b3DestroySemaphore( b3Semaphore* s ) +{ + *s = (b3Semaphore){ 0 }; + b3Free( s, sizeof( b3Semaphore ) ); +} + +void b3WaitSemaphore( b3Semaphore* s ) +{ + (void)s; +} + +void b3SignalSemaphore( b3Semaphore* s ) +{ + (void)s; +} + +typedef struct b3Thread +{ + int dummy; +} b3Thread; + +b3Thread* b3CreateThread( b3ThreadFunction* function, void* context, const char* name ) +{ + (void)name; + function( context ); + b3Thread* t = b3Alloc( sizeof( b3Thread ) ); + t->dummy = 42; + return t; +} + +void b3JoinThread( b3Thread* t ) +{ + *t = (b3Thread){ 0 }; + b3Free( t, sizeof( b3Thread ) ); +} + +#endif + +// djb2 hash, folded 8 bytes per iteration to shorten the dependency chain. +// memcpy lowers to a single load on most targets; on big-endian we byte-swap so +// the hash value is identical across endianness (preserving cross-platform determinism). +// Equivalent to byte-wise djb2 only in spirit; values differ from the original recurrence. +uint32_t b3Hash( uint32_t hash, const uint8_t* data, int count ) +{ + uint32_t result = hash; + int i = 0; + + while ( i + 8 <= count ) + { + uint64_t word; + memcpy( &word, data + i, sizeof( word ) ); +#if defined( __BYTE_ORDER__ ) && __BYTE_ORDER__ == __ORDER_BIG_ENDIAN__ + word = ( ( word & 0x00000000000000FFULL ) << 56 ) | ( ( word & 0x000000000000FF00ULL ) << 40 ) | + ( ( word & 0x0000000000FF0000ULL ) << 24 ) | ( ( word & 0x00000000FF000000ULL ) << 8 ) | + ( ( word & 0x000000FF00000000ULL ) >> 8 ) | ( ( word & 0x0000FF0000000000ULL ) >> 24 ) | + ( ( word & 0x00FF000000000000ULL ) >> 40 ) | ( ( word & 0xFF00000000000000ULL ) >> 56 ); +#endif + result = ( result << 5 ) + result + (uint32_t)word; + result = ( result << 5 ) + result + (uint32_t)( word >> 32 ); + i += 8; + } + + while ( i < count ) + { + result = ( result << 5 ) + result + data[i]; + i++; + } + + return result; +} diff --git a/vendor/box3d/src/src/triangle_manifold.c b/vendor/box3d/src/src/triangle_manifold.c new file mode 100644 index 000000000..b87973add --- /dev/null +++ b/vendor/box3d/src/src/triangle_manifold.c @@ -0,0 +1,1274 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +#include "algorithm.h" +#include "contact.h" +#include "core.h" +#include "manifold.h" +#include "shape.h" + +#include "box3d/base.h" +#include "box3d/collision.h" +#include "box3d/constants.h" + +#include +#include + +typedef struct b3TriangleData +{ + b3Vec3 v1, v2, v3; + b3Vec3 e1, e2, e3; + b3Vec3 center; + b3Plane plane; + int flags; +} b3TriangleData; + +// Indexed by the 3-bit vertex mask +static const b3TriangleFeature s_triangleFeatures[8] = { + b3_featureNone, // 000 (unreachable) + b3_featureVertex1, // 001 + b3_featureVertex2, // 010 + b3_featureEdge1, // 011 v1,v2 + b3_featureVertex3, // 100 + b3_featureEdge3, // 101 v1,v3 + b3_featureEdge2, // 110 v2,v3 + b3_featureTriangleFace, // 111 +}; + +static b3TriangleFeature b3GetTriangleFeature( const b3SimplexCache* cache ) +{ + int count = cache->count; + B3_ASSERT( 0 < count && count < 4 ); + + // Bit i set means triangle vertex i participates in the simplex. + int mask = 0; + for ( int i = 0; i < count; ++i ) + { + B3_ASSERT( cache->indexA[i] < 3 ); + mask |= 1 << cache->indexA[i]; + } + + return s_triangleFeatures[mask]; +} + +void b3CollideSphereAndTriangle( b3LocalManifold* manifold, int capacity, const b3Sphere* sphereA, const b3Vec3* triangleB ) +{ + manifold->pointCount = 0; + + if ( capacity == 0 ) + { + return; + } + + b3Vec3 center = sphereA->center; + b3Vec3 v1 = triangleB[0], v2 = triangleB[1], v3 = triangleB[2]; + b3Plane plane = b3MakePlaneFromPoints( v1, v2, v3 ); + + float offset = b3PlaneSeparation( plane, center ); + if ( offset < 0.0f ) + { + // Cull back side collision + return; + } + + // Closest point on triangle to sphere center + b3TrianglePoint closest = b3ClosestPointOnTriangle( v1, v2, v3, center ); + + // Test separating axis + float squaredDistance = b3DistanceSquared( closest.point, center ); + float speculativeDistance = B3_SPECULATIVE_DISTANCE; + float maxDistance = sphereA->radius + speculativeDistance; + if ( squaredDistance > maxDistance * maxDistance ) + { + return; + } + + float distance = sqrtf( squaredDistance ); + b3Vec3 normal; + if ( distance * distance > 1000.0f * FLT_MIN ) + { + normal = b3MulSV( 1.0f / distance, b3Sub( center, closest.point ) ); + } + else + { + normal = b3Normalize( b3Cross( b3Sub( v2, v1 ), b3Sub( v3, v1 ) ) ); + } + + // contact point mid-way + b3Vec3 contactPoint = b3MulSV( 0.5f, b3Add( b3Sub( center, b3MulSV( sphereA->radius, normal ) ), closest.point ) ); + + manifold->normal = normal; + manifold->pointCount = 1; + manifold->feature = closest.feature; + manifold->squaredDistance = squaredDistance; + + b3LocalManifoldPoint* mp = manifold->points + 0; + mp->point = contactPoint; + mp->separation = distance - sphereA->radius; + mp->pair = b3FeaturePair_single; +} + +static bool b3ClipSegmentToTriangleFace( b3ClipVertex segment[2], const b3Vec3* points, b3Plane plane ) +{ + b3Vec3 vertex1 = points[2]; + for ( int i = 0; i < 3; ++i ) + { + b3Vec3 vertex2 = points[i]; + b3Vec3 tangent = b3Normalize( b3Sub( vertex2, vertex1 ) ); + b3Vec3 binormal = b3Cross( tangent, plane.normal ); + + b3Plane clipPlane = b3MakePlaneFromNormalAndPoint( binormal, vertex1 ); + + int vertexCount = 0; + b3ClipVertex p1 = segment[0]; + b3ClipVertex p2 = segment[1]; + + float distance1 = b3PlaneSeparation( clipPlane, p1.position ); + float distance2 = b3PlaneSeparation( clipPlane, p2.position ); + + // If the points are behind the plane + if ( distance1 <= 0.0f ) + { + segment[vertexCount++] = p1; + } + if ( distance2 <= 0.0f ) + { + segment[vertexCount++] = p2; + } + + // If the points are on different sides of the plane + if ( distance1 * distance2 < 0.0f ) + { + // Find intersection point of edge and plane + float t = distance1 / ( distance1 - distance2 ); + segment[vertexCount].position = b3Lerp( p1.position, p2.position, t ); + segment[vertexCount].pair = distance1 > 0.0f ? p1.pair : p2.pair; + vertexCount++; + } + + if ( vertexCount != 2 ) + { + return false; + } + + vertex1 = vertex2; + } + + return true; +} + +static b3FaceQuery b3QueryTriangleFaceAndCapsule( b3Plane plane, const b3Capsule* capsule ) +{ + float separation1 = b3PlaneSeparation( plane, capsule->center1 ); + float separation2 = b3PlaneSeparation( plane, capsule->center2 ); + + if ( separation1 < separation2 ) + { + return (b3FaceQuery){ + .separation = separation1, + .faceIndex = 0, + .vertexIndex = 0, + }; + } + + return (b3FaceQuery){ + .separation = separation2, + .faceIndex = 0, + .vertexIndex = 1, + }; +} + +static b3EdgeQuery b3QueryTriangleAndCapsuleEdges( const b3Vec3* vertices, const b3Capsule* capsule ) +{ + // Work in the local space of the capsule + b3Vec3 p1 = capsule->center1; + b3Vec3 p2 = capsule->center2; + b3Vec3 capsuleEdge = b3Sub( p2, p1 ); + + b3Vec3 capsuleCenter = b3Lerp( p1, p2, 0.5f ); + + b3Vec3 triangleCenter = b3MulSV( 1.0f / 3.0f, b3Add( vertices[0], b3Add( vertices[1], vertices[2] ) ) ); + + // Find axis of minimum penetration + float maxSeparation = -FLT_MAX; + int maxIndex1 = UINT8_MAX; + int maxIndex2 = UINT8_MAX; + + int edgeIndex = 2; + b3Vec3 v1 = vertices[2]; + for ( int index = 0; index < 3; ++index ) + { + b3Vec3 v2 = vertices[index]; + + b3Vec3 triangleEdge = b3Sub( v2, v1 ); + + float separation = b3EdgeEdgeSeparation( p1, capsuleEdge, capsuleCenter, v1, triangleEdge, triangleCenter ); + if ( separation > maxSeparation ) + { + // Note: We don't exit early if we find a separating axis here since we want to + // find the best one for caching and account for the convex radius later. + maxSeparation = separation; + maxIndex1 = edgeIndex; + maxIndex2 = 0; + } + + v1 = v2; + edgeIndex = index; + } + + // Save result + return (b3EdgeQuery){ + .separation = maxSeparation, + .indexA = (uint8_t)maxIndex1, + .indexB = (uint8_t)maxIndex2, + }; +} + +static void b3BuildTriangleAndCapsuleFaceContact( b3LocalManifold* manifold, const b3Vec3* triangle, b3Plane plane, + const b3Capsule* capsule ) +{ + B3_ASSERT( manifold->pointCount == 0 ); + + b3ClipVertex segment[2]; + segment[0].position = capsule->center1; + segment[0].separation = 0.0f; + segment[0].pair = b3MakeFeaturePair( b3_featureShapeA, 0, b3_featureShapeA, 0 ); + segment[1].position = capsule->center2; + segment[1].separation = 0.0f; + segment[1].pair = b3MakeFeaturePair( b3_featureShapeA, 1, b3_featureShapeA, 1 ); + + bool havePoints = b3ClipSegmentToTriangleFace( segment, triangle, plane ); + if ( havePoints == false ) + { + return; + } + + float radius = capsule->radius; + float distance1 = b3PlaneSeparation( plane, segment[0].position ); + float distance2 = b3PlaneSeparation( plane, segment[1].position ); + + float speculativeDistance = B3_SPECULATIVE_DISTANCE; + if ( distance1 > speculativeDistance + radius && distance2 > speculativeDistance + radius ) + { + return; + } + + // Average points. Half-way between capsule bottom and triangle plane. + b3Vec3 point1 = b3MulSub( segment[0].position, 0.5f * ( distance1 + capsule->radius ), plane.normal ); + b3Vec3 point2 = b3MulSub( segment[1].position, 0.5f * ( distance2 + capsule->radius ), plane.normal ); + + manifold->normal = plane.normal; + manifold->feature = b3_featureTriangleFace; + manifold->pointCount = 2; + + b3LocalManifoldPoint* pt = manifold->points + 0; + pt->point = point1; + pt->separation = distance1 - capsule->radius; + pt->pair = segment[0].pair; + + pt = manifold->points + 1; + pt->point = point2; + pt->separation = distance2 - capsule->radius; + pt->pair = segment[1].pair; +} + +static void b3BuildTriangleAndCapsuleEdgeContact( b3LocalManifold* manifold, const b3Vec3* triangle, const b3Capsule* capsule, + b3EdgeQuery query ) +{ + B3_ASSERT( 0 <= query.indexA && query.indexA < 3 ); + + b3Vec3 p1 = capsule->center1; + b3Vec3 p2 = capsule->center2; + b3Vec3 capsuleEdge = b3Sub( p2, p1 ); + + const b3Vec3* vs = triangle; + + b3Vec3 triangleCenter = b3MulSV( 1.0f / 3.0f, b3Add( vs[0], b3Add( vs[1], vs[2] ) ) ); + b3Vec3 v1 = vs[query.indexA]; + b3Vec3 v2 = vs[( query.indexA + 1 ) % 3]; + b3Vec3 triangleEdge = b3Sub( v2, v1 ); + + b3Vec3 normal = b3Cross( capsuleEdge, triangleEdge ); + normal = b3Normalize( normal ); + + // Normal should point away from triangle center + if ( b3Dot( normal, b3Sub( v1, triangleCenter ) ) < 0.0f ) + { + normal = b3Neg( normal ); + } + + b3SegmentDistanceResult result = b3LineDistance( v1, triangleEdge, p1, capsuleEdge ); + + if ( result.fraction1 < 0.0f || 1.0f < result.fraction1 || result.fraction2 < 0.0f || 1.0f < result.fraction2 ) + { + // closest point beyond end points + return; + } + + b3Vec3 point = b3Lerp( b3MulSub( result.point1, capsule->radius, normal ), result.point2, 0.5f ); + + float separation = b3Dot( normal, b3Sub( result.point2, result.point1 ) ); + B3_VALIDATE( b3AbsFloat( separation - query.separation ) < B3_LINEAR_SLOP ); + + manifold->normal = normal; + manifold->pointCount = 1; + + b3TriangleFeature edgesFeatures[] = { b3_featureEdge1, b3_featureEdge2, b3_featureEdge3 }; + manifold->feature = edgesFeatures[query.indexA]; + + b3LocalManifoldPoint* pt = manifold->points + 0; + pt->point = point; + pt->separation = separation - capsule->radius; + pt->pair = b3MakeFeaturePair( b3_featureShapeA, query.indexA, b3_featureShapeB, query.indexB ); +} + +void b3CollideCapsuleAndTriangle( b3LocalManifold* manifold, int capacity, const b3Capsule* capsuleA, const b3Vec3* triangleB, + b3SimplexCache* cache ) +{ + manifold->pointCount = 0; + + if ( capacity < 2 ) + { + return; + } + + b3Vec3 v1 = triangleB[0], v2 = triangleB[1], v3 = triangleB[2]; + b3Plane plane = b3MakePlaneFromPoints( v1, v2, v3 ); + b3Vec3 capsuleCenter = b3Lerp( capsuleA->center1, capsuleA->center2, 0.5f ); + + float offset = b3PlaneSeparation( plane, capsuleCenter ); + if ( offset < 0.0f ) + { + // Cull back side collision + return; + } + + b3DistanceInput distanceInput; + distanceInput.proxyA = (b3ShapeProxy){ triangleB, 3, 0.0f }; + distanceInput.proxyB = (b3ShapeProxy){ &capsuleA->center1, 2, 0.0f }; + distanceInput.transform = b3Transform_identity; + distanceInput.useRadii = false; + + b3DistanceOutput distanceOutput = b3ShapeDistance( &distanceInput, cache, NULL, 0 ); + + float radius = capsuleA->radius; + if ( distanceOutput.distance > radius + B3_SPECULATIVE_DISTANCE ) + { + // Shapes are separated, persist the cache + return; + } + + if ( distanceOutput.distance > 100.0f * FLT_EPSILON ) + { + // Shallow penetration + b3Vec3 delta = b3Normalize( b3Sub( distanceOutput.pointB, distanceOutput.pointA ) ); + + // Try to create two contact points if closest points difference is nearly parallel to face normal + const float kTolerance = 0.2f; + float cosAngle = b3AbsFloat( b3Dot( plane.normal, delta ) ); + if ( cosAngle > kTolerance ) + { + // Clip capsule segment against side planes of reference face + b3ClipVertex segment[2]; + segment[0].position = capsuleA->center1; + segment[0].separation = 0.0f; + segment[0].pair = b3MakeFeaturePair( b3_featureShapeA, 0, b3_featureShapeA, 0 ); + segment[1].position = capsuleA->center2; + segment[1].separation = 0.0f; + segment[1].pair = b3MakeFeaturePair( b3_featureShapeA, 1, b3_featureShapeA, 1 ); + + bool havePoints = b3ClipSegmentToTriangleFace( segment, triangleB, plane ); + + if ( havePoints == true ) + { + float distance1 = b3PlaneSeparation( plane, segment[0].position ); + float distance2 = b3PlaneSeparation( plane, segment[1].position ); + + b3Vec3 normal = plane.normal; + b3Vec3 point1 = b3MulSub( segment[0].position, 0.5f * ( radius + distance1 ), normal ); + b3Vec3 point2 = b3MulSub( segment[1].position, 0.5f * ( radius + distance2 ), normal ); + + manifold->normal = normal; + manifold->feature = b3_featureTriangleFace; + manifold->pointCount = 2; + + b3LocalManifoldPoint* mp = manifold->points + 0; + mp->point = point1; + mp->separation = distance1 - radius; + mp->pair = segment[0].pair; + + mp = manifold->points + 1; + mp->point = point2; + mp->separation = distance2 - radius; + mp->pair = segment[1].pair; + + return; + } + } + + // Create contact from closest points + b3Vec3 point = b3MulSV( 0.5f, b3Add( b3Sub( distanceOutput.pointA, b3MulSV( radius, delta ) ), distanceOutput.pointB ) ); + + manifold->normal = delta; + manifold->pointCount = 1; + manifold->feature = b3GetTriangleFeature( cache ); + + b3LocalManifoldPoint* mp = manifold->points + 0; + mp->point = point; + mp->separation = distanceOutput.distance - radius; + mp->pair = b3FeaturePair_single; + + return; + } + + // Deep penetration + + b3FaceQuery faceQuery = b3QueryTriangleFaceAndCapsule( plane, capsuleA ); + if ( faceQuery.separation > radius ) + { + // Shapes are separated + return; + } + + b3EdgeQuery edgeQuery = b3QueryTriangleAndCapsuleEdges( triangleB, capsuleA ); + if ( edgeQuery.separation > radius ) + { + // Shapes are separated + return; + } + + // Create face contact + float faceSeparation = faceQuery.separation - radius; + b3BuildTriangleAndCapsuleFaceContact( manifold, triangleB, plane, capsuleA ); + if ( manifold->pointCount == 2 ) + { + faceSeparation = b3MinFloat( manifold->points[0].separation, manifold->points[1].separation ); + } + B3_VALIDATE( faceSeparation <= 0.0f ); + + // Face contact can be empty if it does not realize the axis of minimum penetration. + // Create edge contact if face contact fails or edge contact is significantly better! + const float kRelEdgeTolerance = 0.50f; + const float kAbsTolerance = 1.0f * B3_LINEAR_SLOP; + float edgeSeparation = edgeQuery.separation - radius; + if ( manifold->pointCount == 0 || edgeSeparation > kRelEdgeTolerance * faceSeparation + kAbsTolerance ) + { + // Edge contact + b3BuildTriangleAndCapsuleEdgeContact( manifold, triangleB, capsuleA, edgeQuery ); + } +} + +static inline int b3GetTriangleSupport( b3Vec3* points, b3Vec3 direction ) +{ + int index = 0; + float distance = b3Dot( points[0], direction ); + + float d = b3Dot( points[1], direction ); + if ( d > distance ) + { + distance = d; + index = 1; + } + + d = b3Dot( points[2], direction ); + if ( d > distance ) + { + return 2; + } + + return index; +} + +static b3FaceQuery b3QueryTriangleFace( const b3TriangleData* triangle, const b3HullData* hull ) +{ + const b3Vec3* hullPoints = b3GetHullPoints( hull ); + b3Plane plane = triangle->plane; + int vertexIndex = b3FindHullSupportVertex( hull, b3Neg( plane.normal ) ); + b3Vec3 support = hullPoints[vertexIndex]; + float separation = b3PlaneSeparation( plane, support ); + + return (b3FaceQuery){ + .separation = separation, + .faceIndex = 0, + .vertexIndex = (uint8_t)vertexIndex, + }; +} + +static b3FaceQuery b3QueryHullFace( const b3TriangleData* triangle, const b3HullData* hull ) +{ + const b3Plane* hullPlanes = b3GetHullPlanes( hull ); + int faceCount = hull->faceCount; + + b3Vec3 trianglePoints[] = { triangle->v1, triangle->v2, triangle->v3 }; + + int maxFaceIndex = -1; + int maxVertexIndex = -1; + float maxFaceSeparation = -FLT_MAX; + + for ( int faceIndex = 0; faceIndex < faceCount; ++faceIndex ) + { + b3Plane plane = hullPlanes[faceIndex]; + + int vertexIndex = b3GetTriangleSupport( trianglePoints, b3Neg( plane.normal ) ); + b3Vec3 support = trianglePoints[vertexIndex]; + float separation = b3PlaneSeparation( plane, support ); + if ( separation > maxFaceSeparation ) + { + maxFaceIndex = faceIndex; + maxVertexIndex = vertexIndex; + maxFaceSeparation = separation; + } + } + + return (b3FaceQuery){ + .separation = maxFaceSeparation, + .faceIndex = maxFaceIndex, + .vertexIndex = maxVertexIndex, + }; +} + +static b3EdgeQuery b3TestEdgePairs( const b3TriangleData* triangle, const b3HullData* hull ) +{ + b3EdgeQuery result = { + .separation = -FLT_MAX, + .indexA = B3_NULL_INDEX, + .indexB = B3_NULL_INDEX, + }; + + b3Vec3 trianglePoints[] = { triangle->v1, triangle->v2, triangle->v3 }; + b3Vec3 triangleEdges[] = { triangle->e1, triangle->e2, triangle->e3 }; + // int edgeFlags[] = { b3_concaveEdge1, b3_concaveEdge1, b3_concaveEdge3 }; + +#if B3_FORCE_GHOST_COLLISIONS + int triangleFlags = 0xFF; +#else + int triangleFlags = triangle->flags; +#endif + (void)triangleFlags; + + b3Vec3 triNormal = triangle->plane.normal; + + const b3HullHalfEdge* hullEdges = b3GetHullEdges( hull ); + const b3Vec3* hullPoints = b3GetHullPoints( hull ); + const b3Plane* hullPlanes = b3GetHullPlanes( hull ); + int edgeCount = hull->edgeCount; + + for ( int i = 0; i < edgeCount; i += 2 ) + { + const b3HullHalfEdge* edge = hullEdges + i; + const b3HullHalfEdge* twin = hullEdges + i + 1; + B3_ASSERT( edge->twin == i + 1 && twin->twin == i ); + + b3Vec3 hullPoint = hullPoints[edge->origin]; + b3Vec3 hullEdge = b3Sub( hullPoints[twin->origin], hullPoint ); + + b3Vec3 hullNormal1 = hullPlanes[edge->face].normal; + b3Vec3 hullNormal2 = hullPlanes[twin->face].normal; + + for ( int j = 0; j < 3; ++j ) + { + b3Vec3 triEdge = triangleEdges[j]; + + float cab = b3Dot( hullNormal1, triEdge ); + float dab = b3Dot( hullNormal2, triEdge ); + float bcd = b3Dot( triNormal, hullEdge ); + if ( cab * dab >= 0.0f || cab * bcd <= 0.0f ) + { + continue; + } + + b3Vec3 triPoint = trianglePoints[j]; + float separation = b3EdgeEdgeSeparation( triPoint, triEdge, triangle->center, hullPoint, hullEdge, hull->center ); + + // if ( separation > result.separation && ( edgeFlags[j] & triangleFlags ) == 0 ) + if ( separation > result.separation ) + { + // Note: We don't exit early if we find a separating axis here since we want to + // find the best one for caching. + result.separation = separation; + result.indexA = j; + result.indexB = i; + } + } + } + + return result; +} + +static float b3CollideHullFace( b3LocalManifold* manifold, int pointCapacity, const b3TriangleData* triangle, + const b3HullData* hull, b3FaceQuery query, b3SATCache* cache, bool enableSpeculative ) +{ + manifold->pointCount = 0; + + const b3HullFace* hullFaces = b3GetHullFaces( hull ); + const b3HullHalfEdge* hullEdges = b3GetHullEdges( hull ); + const b3Plane* hullPlanes = b3GetHullPlanes( hull ); + const b3Vec3* hullPoints = b3GetHullPoints( hull ); + + // Reference hull face + int refFace = query.faceIndex; + b3Plane refPlane = hullPlanes[refFace]; + + // Build clip polygon from triangle face (the incident face) + b3ClipVertex buffer1[B3_MAX_CLIP_POINTS], buffer2[B3_MAX_CLIP_POINTS]; + + b3Vec3 v1 = triangle->v1; + b3Vec3 v2 = triangle->v2; + b3Vec3 v3 = triangle->v3; + buffer1[0].position = v1; + buffer1[0].separation = b3PlaneSeparation( refPlane, v1 ); + buffer1[0].pair = b3MakeFeaturePair( b3_featureShapeB, 2, b3_featureShapeB, 0 ); + buffer1[1].position = v2; + buffer1[1].separation = b3PlaneSeparation( refPlane, v2 ); + buffer1[1].pair = b3MakeFeaturePair( b3_featureShapeB, 0, b3_featureShapeB, 1 ); + buffer1[2].position = v3; + buffer1[2].separation = b3PlaneSeparation( refPlane, v3 ); + buffer1[2].pair = b3MakeFeaturePair( b3_featureShapeB, 1, b3_featureShapeB, 2 ); + int pointCount = 3; + + // Clip triangle face against side planes of reference face + b3ClipVertex* input = buffer1; + b3ClipVertex* output = buffer2; + + const b3HullFace* face = hullFaces + refFace; + int edgeIndex = face->edge; + + do + { + const b3HullHalfEdge* edge = hullEdges + edgeIndex; + int nextEdgeIndex = edge->next; + const b3HullHalfEdge* next = hullEdges + nextEdgeIndex; + b3Vec3 vertex1 = hullPoints[edge->origin]; + b3Vec3 vertex2 = hullPoints[next->origin]; + b3Vec3 tangent = b3Normalize( b3Sub( vertex2, vertex1 ) ); + b3Vec3 binormal = b3Cross( tangent, refPlane.normal ); + + b3Plane clipPlane = b3MakePlaneFromNormalAndPoint( binormal, vertex1 ); + + pointCount = b3ClipPolygon( output, input, pointCount, clipPlane, edgeIndex, refPlane ); + B3_ASSERT( pointCount <= B3_MAX_CLIP_POINTS ); + + if ( pointCount < 3 ) + { + // Using a stale cache + *cache = (b3SATCache){ 0 }; + return query.separation; + } + + // Swap buffers, output becomes input for the next clipping plane + B3_SWAP( output, input ); + edgeIndex = nextEdgeIndex; + } + while ( edgeIndex != face->edge ); + + pointCount = b3MinInt( pointCount, pointCapacity ); + float minSeparation = FLT_MAX; + int finalPointCount = 0; + + for ( int i = 0; i < pointCount; ++i ) + { + b3ClipVertex* clipPoint = input + i; + minSeparation = b3MinFloat( minSeparation, clipPoint->separation ); + + if ( enableSpeculative == false && clipPoint->separation > 0.0f ) + { + continue; + } + + // Move point onto hull face improved culling + b3Vec3 point = b3MulSub( clipPoint->position, clipPoint->separation, refPlane.normal ); + + b3LocalManifoldPoint* pt = manifold->points + finalPointCount; + pt->point = point; + pt->separation = clipPoint->separation; + pt->pair = b3FlipPair( clipPoint->pair ); + + finalPointCount += 1; + } + + float speculativeDistance = enableSpeculative ? B3_SPECULATIVE_DISTANCE : 0.0f; + if ( minSeparation > speculativeDistance ) + { + // This can occur with a stale SAT cache + manifold->pointCount = 0; + *cache = (b3SATCache){ 0 }; + return minSeparation; + } + + manifold->pointCount = finalPointCount; + manifold->normal = b3Neg( refPlane.normal ); + manifold->feature = b3_featureHullFace; + + // Save cache + cache->separation = minSeparation; + cache->type = b3_faceAxisB; + cache->indexA = (uint8_t)query.vertexIndex; + cache->indexB = (uint8_t)query.faceIndex; + return minSeparation; +} + +static float b3CollideTriangleFace( b3LocalManifold* manifold, int pointCapacity, const b3TriangleData* triangle, + const b3HullData* hull, b3FaceQuery query, b3SATCache* cache, bool enableSpeculative ) +{ + B3_VALIDATE( manifold->pointCount == 0 ); + + const b3HullFace* hullFaces = b3GetHullFaces( hull ); + const b3HullHalfEdge* hullEdges = b3GetHullEdges( hull ); + const b3Vec3* hullPoints = b3GetHullPoints( hull ); + + // Find incident face + B3_ASSERT( query.faceIndex == 0 ); + b3Plane refPlane = triangle->plane; + + int incFace = b3FindIncidentFace( hull, refPlane.normal, query.vertexIndex ); + + // Build clip polygon from incident face + b3ClipVertex buffer1[2 * B3_MAX_CLIP_POINTS], buffer2[2 * B3_MAX_CLIP_POINTS]; + int pointCount = 0; + const b3HullFace* face = hullFaces + incFace; + int hullEdgeIndex = face->edge; + + do + { + const b3HullHalfEdge* edge = hullEdges + hullEdgeIndex; + + int nextEdgeIndex = edge->next; + const b3HullHalfEdge* next = hullEdges + nextEdgeIndex; + + b3Vec3 hullPoint = hullPoints[next->origin]; + buffer1[pointCount].position = hullPoint; + buffer1[pointCount].separation = b3PlaneSeparation( refPlane, hullPoint ); + buffer1[pointCount].pair = b3MakeFeaturePair( b3_featureShapeB, hullEdgeIndex, b3_featureShapeB, nextEdgeIndex ); + + pointCount += 1; + hullEdgeIndex = nextEdgeIndex; + } + while ( hullEdgeIndex != face->edge && pointCount < 2 * B3_MAX_CLIP_POINTS ); + + B3_ASSERT( pointCount >= 3 ); + + // Clip incident face against side planes of reference face (triangle) + b3ClipVertex* input = buffer1; + b3ClipVertex* output = buffer2; + + b3Vec3 trianglePoints[] = { triangle->v1, triangle->v2, triangle->v3 }; + b3Vec3 triangleEdges[] = { triangle->e1, triangle->e2, triangle->e3 }; + + for ( int i = 0; i < 3 && pointCount > 0; ++i ) + { + b3Vec3 sideNormal = b3Cross( triangleEdges[i], refPlane.normal ); + sideNormal = b3Normalize( sideNormal ); + + b3Plane clipPlane = b3MakePlaneFromNormalAndPoint( sideNormal, trianglePoints[i] ); + + pointCount = b3ClipPolygon( output, input, pointCount, clipPlane, i, refPlane ); + B3_ASSERT( pointCount <= 2 * B3_MAX_CLIP_POINTS ); + + B3_SWAP( output, input ); + } + + if ( pointCount == 0 ) + { + // Triangle face clipped away. Invalidate cache. + *cache = (b3SATCache){ 0 }; + return FLT_MAX; + } + + pointCount = b3MinInt( pointCount, pointCapacity ); + + float minSeparation = FLT_MAX; + + int finalPointCount = 0; + for ( int i = 0; i < pointCount; ++i ) + { + b3ClipVertex* clipPoint = input + i; + minSeparation = b3MinFloat( minSeparation, clipPoint->separation ); + + if ( enableSpeculative == false && clipPoint->separation > 0.0f ) + { + continue; + } + + // Move point onto triangle surface for improved culling + // b3Vec3 point = b3MulSub( clipPoint->position, clipPoint->separation, refPlane.normal ); + b3Vec3 point = clipPoint->position; + + b3LocalManifoldPoint* pt = manifold->points + finalPointCount; + pt->point = point; + pt->separation = clipPoint->separation; + pt->pair = clipPoint->pair; + + finalPointCount += 1; + } + + float speculativeDistance = enableSpeculative ? B3_SPECULATIVE_DISTANCE : 0.0f; + if ( minSeparation >= speculativeDistance ) + { + // This can happens if the objects move a part while re-using a cached axis + *cache = (b3SATCache){ 0 }; + return minSeparation; + } + + manifold->pointCount = finalPointCount; + manifold->normal = refPlane.normal; + manifold->feature = b3_featureTriangleFace; + + // Save cache + cache->separation = minSeparation; + cache->type = b3_faceAxisA; + cache->indexA = (uint8_t)query.faceIndex; + cache->indexB = (uint8_t)query.vertexIndex; + return minSeparation; +} + +static void b3CollideHullAndTriangleEdges( b3LocalManifold* manifold, int capacity, b3Vec3 trianglePoint, b3Vec3 triangleEdge, + b3Vec3 triangleCenter, const b3HullData* hull, b3EdgeQuery query, b3SATCache* cache ) +{ + B3_VALIDATE( query.separation <= 2.0f * B3_SPECULATIVE_DISTANCE ); + B3_ASSERT( query.indexA < 3 ); + + b3Vec3 cA = triangleCenter; + b3Vec3 pA = trianglePoint; + b3Vec3 eA = triangleEdge; + + const b3HullHalfEdge* edgesB = b3GetHullEdges( hull ); + const b3Vec3* pointsB = b3GetHullPoints( hull ); + const b3HullHalfEdge* edgeB = edgesB + query.indexB; + const b3HullHalfEdge* twinB = edgesB + edgeB->twin; + b3Vec3 pB = pointsB[edgeB->origin]; + b3Vec3 qB = pointsB[twinB->origin]; + b3Vec3 eB = b3Sub( qB, pB ); + + b3Vec3 normal = b3Cross( eA, eB ); + normal = b3Normalize( normal ); + + // Ensure normal points outward from triangle center + float outwardA = b3Dot( normal, b3Sub( pA, cA ) ); + + // Ensure normal points towards hull center + float outwardB = b3Dot( normal, b3Sub( hull->center, pB ) ); + + // Use the largest magnitude. The triangle outward value + // may be unreliable as some angles. + if ( b3AbsFloat( outwardA ) > b3AbsFloat( outwardB ) ) + { + if ( outwardA < 0.0f ) + { + normal = b3Neg( normal ); + } + } + else + { + if ( outwardB < 0.0f ) + { + normal = b3Neg( normal ); + } + } + + // Get the closest points between the infinite edge lines + b3SegmentDistanceResult result = b3LineDistance( pA, eA, pB, eB ); + + // Is one of the closest points outside of the associated edge segment? + if ( capacity == 0 || result.fraction1 < 0.0f || 1.0f < result.fraction1 || result.fraction2 < 0.0f || + 1.0f < result.fraction2 ) + { + // Invalid edge pair, no points generated + B3_ASSERT( manifold->pointCount == 0 ); + *cache = (b3SATCache){ 0 }; + return; + } + + // This can slide off the end from caching + float separation = b3Dot( normal, b3Sub( result.point2, result.point1 ) ); + B3_VALIDATE( b3AbsFloat( separation - query.separation ) < B3_LINEAR_SLOP ); + + b3Vec3 point = b3MulSV( 0.5f, b3Add( result.point1, result.point2 ) ); + + b3LocalManifoldPoint* pt = manifold->points + 0; + pt->point = point; + pt->separation = separation; + pt->pair = b3MakeFeaturePair( b3_featureShapeA, query.indexA, b3_featureShapeB, query.indexB ); + + // Save cache + cache->separation = separation; + cache->type = b3_edgePairAxis; + cache->indexA = (uint8_t)query.indexA; + cache->indexB = (uint8_t)query.indexB; + + manifold->normal = normal; + manifold->pointCount = 1; + + b3TriangleFeature edgesFeatures[] = { b3_featureEdge1, b3_featureEdge2, b3_featureEdge3 }; + manifold->feature = edgesFeatures[query.indexA]; +} + +// See "Collision Detection of Convex Polyhedra Based on Duality Transformation" +// Simplified for triangle versus hull +static inline bool b3IsTriangleMinkowskiFace( b3Vec3 triNormal, b3Vec3 triEdge, b3Vec3 hullNormal1, b3Vec3 hullNormal2, + b3Vec3 hullEdge ) +{ + float cab = b3Dot( hullNormal1, triEdge ); + float dab = b3Dot( hullNormal2, triEdge ); + float bcd = b3Dot( triNormal, hullEdge ); + return cab * dab < 0.0f && cab * bcd > 0.0f; +} + +b3AtomicInt b3_triangleConvexCalls; +b3AtomicInt b3_triangleCacheHits; + +// Computes the manifold in the local space of the hull +void b3CollideHullAndTriangle( b3LocalManifold* manifold, int capacity, const b3HullData* hullA, b3Vec3 v1, b3Vec3 v2, b3Vec3 v3, + int triangleFlags, b3SATCache* cache, bool enableSpeculative ) +{ + manifold->pointCount = 0; + manifold->feature = b3_featureNone; + + if ( capacity < 4 ) + { + return; + } + + b3Plane trianglePlane = b3MakePlaneFromPoints( v1, v2, v3 ); + float linearSlop = B3_LINEAR_SLOP; + + float offset = b3PlaneSeparation( trianglePlane, hullA->center ); + if ( cache->type == b3_backsideAxis ) + { + // Use hysteresis to avoid jitter on wavy meshes + if ( b3AbsFloat( cache->separation - offset ) < linearSlop ) + { + return; + } + + cache->type = b3_invalidAxis; + } + + if ( offset < -linearSlop ) + { + // Cull back side collision. Cache offset to add hysteresis. + cache->type = b3_backsideAxis; + cache->separation = offset; + return; + } + + b3Vec3 triangleCenter = b3MulSV( 1.0f / 3.0f, b3Add( v1, b3Add( v2, v3 ) ) ); + b3Vec3 trianglePoints[] = { v1, v2, v3 }; + b3Vec3 triangleEdges[] = { b3Sub( v2, v1 ), b3Sub( v3, v2 ), b3Sub( v1, v3 ) }; + + b3TriangleData triangle = { + .v1 = v1, + .v2 = v2, + .v3 = v3, + .e1 = triangleEdges[0], + .e2 = triangleEdges[1], + .e3 = triangleEdges[2], + .center = triangleCenter, + .plane = trianglePlane, + .flags = triangleFlags, + }; + + const b3HullHalfEdge* edges = b3GetHullEdges( hullA ); + const b3Plane* hullPlanes = b3GetHullPlanes( hullA ); + const b3Vec3* hullPoints = b3GetHullPoints( hullA ); + + float speculativeDistance = enableSpeculative ? B3_SPECULATIVE_DISTANCE : 0.0f; + cache->hit = 1; + + // Attempt to use the cache to speed up collision + switch ( cache->type ) + { + case b3_faceAxisA: + { + B3_ASSERT( cache->indexA == 0 ); + + int vertexIndex = b3FindHullSupportVertex( hullA, b3Neg( trianglePlane.normal ) ); + b3Vec3 support = hullPoints[vertexIndex]; + float separation = b3PlaneSeparation( trianglePlane, support ); + if ( separation > speculativeDistance ) + { + // Cache hit, shapes are separated + return; + } + + b3FaceQuery faceQuery; + faceQuery.separation = separation; + faceQuery.faceIndex = cache->indexA; + faceQuery.vertexIndex = vertexIndex; + + // Read cache but don't modify it + b3SATCache localCache = *cache; + float clippedSeparation = + b3CollideTriangleFace( manifold, capacity, &triangle, hullA, faceQuery, &localCache, enableSpeculative ); + + if ( manifold->pointCount > 0 && b3AbsFloat( cache->separation - clippedSeparation ) < linearSlop ) + { + // Cache hit, contact points generated + return; + } + + // Invalidate cache and fall through + manifold->pointCount = 0; + *cache = (b3SATCache){ 0 }; + } + break; + + case b3_faceAxisB: + { + B3_ASSERT( cache->indexB < hullA->faceCount ); + + b3Plane plane = hullPlanes[cache->indexB]; + + // Get triangle support point + int vertexIndex = 0; + float distance = -b3Dot( v1, plane.normal ); + for ( int i = 1; i < 3; ++i ) + { + float d = -b3Dot( trianglePoints[i], plane.normal ); + if ( d > distance ) + { + distance = d; + vertexIndex = i; + } + } + + b3Vec3 support = trianglePoints[vertexIndex]; + + // Separation of triangle support point with hull plane + float separation = b3PlaneSeparation( plane, support ); + if ( separation > speculativeDistance ) + { + // Cache hit, shapes are separated + return; + } + + // Deep overlap may lead to an invalid cache + // todo confirm + bool isDeep = separation < -2.0f * linearSlop; + + // Don't persist deep cache or allow separation to change too much + if ( isDeep == false ) + { + // Try to rebuild contact from last features + b3FaceQuery faceQuery; + faceQuery.separation = separation; + faceQuery.faceIndex = cache->indexB; + faceQuery.vertexIndex = vertexIndex; + + // Read cache but don't modify it + b3SATCache localCache = *cache; + float clippedSeparation = + b3CollideHullFace( manifold, capacity, &triangle, hullA, faceQuery, &localCache, enableSpeculative ); + + // Cache reuse is only successful if it creates contact points and the clipped separation didn't change much. + if ( manifold->pointCount > 0 && b3AbsFloat( cache->separation - clippedSeparation ) < linearSlop ) + { + // Cache hit, contact points generated + return; + } + } + + // Invalidate cache and fall through + manifold->pointCount = 0; + *cache = (b3SATCache){ 0 }; + } + break; + + case b3_edgePairAxis: + { + B3_ASSERT( cache->indexA < 3 ); + int indexA = cache->indexA; + + b3Vec3 triPoint = trianglePoints[indexA]; + b3Vec3 triEdge = triangleEdges[indexA]; + + B3_ASSERT( cache->indexB < hullA->edgeCount - 1 ); + int indexB = cache->indexB; + + const b3HullHalfEdge* edge2 = edges + indexB; + const b3HullHalfEdge* twin2 = edges + indexB + 1; + B3_ASSERT( edge2->twin == indexB + 1 && twin2->twin == indexB ); + + b3Vec3 hullPoint = hullPoints[edge2->origin]; + b3Vec3 hullEdge = b3Sub( hullPoints[twin2->origin], hullPoint ); + b3Vec3 hullNormal1 = hullPlanes[edge2->face].normal; + b3Vec3 hullNormal2 = hullPlanes[twin2->face].normal; + + // Confirm the edge pair is still a Minkowski face + bool isMinkowski = b3IsTriangleMinkowskiFace( trianglePlane.normal, triEdge, hullNormal1, hullNormal2, hullEdge ); + if ( isMinkowski ) + { + // Transform reference center of the first hull into local space of the second hull + float separation = b3EdgeEdgeSeparation( triPoint, triEdge, triangleCenter, hullPoint, hullEdge, hullA->center ); + if ( separation > speculativeDistance ) + { + // Cache hit, shapes are separated + return; + } + + if ( b3AbsFloat( cache->separation - separation ) < linearSlop ) + { + // Try to rebuild contact from last features + b3EdgeQuery edgeQuery; + edgeQuery.indexA = indexA; + edgeQuery.indexB = indexB; + edgeQuery.separation = separation; + + // Read cache but don't modify it + b3SATCache localCache = *cache; + b3CollideHullAndTriangleEdges( manifold, capacity, triPoint, triEdge, triangleCenter, hullA, edgeQuery, + &localCache ); + + if ( manifold->pointCount > 0 ) + { + // Cache hit, contact point generated + return; + } + } + } + + // Invalidate cache and fall through + *cache = (b3SATCache){ 0 }; + } + break; + + // This case is for testing + case b3_manualFaceAxisA: + { + b3FaceQuery faceQueryA = b3QueryTriangleFace( &triangle, hullA ); + b3CollideTriangleFace( manifold, capacity, &triangle, hullA, faceQueryA, cache, enableSpeculative ); + return; + } + + // This case is for testing + case b3_manualFaceAxisB: + { + b3FaceQuery faceQueryB = b3QueryHullFace( &triangle, hullA ); + b3CollideHullFace( manifold, capacity, &triangle, hullA, faceQueryB, cache, enableSpeculative ); + return; + } + + // This case is for testing + case b3_manualEdgePairAxis: + { + b3EdgeQuery edgeQuery = b3TestEdgePairs( &triangle, hullA ); + if ( edgeQuery.indexA != B3_NULL_INDEX ) + { + b3Vec3 trianglePoint = trianglePoints[edgeQuery.indexA]; + b3Vec3 triangleEdge = triangleEdges[edgeQuery.indexA]; + b3CollideHullAndTriangleEdges( manifold, capacity, trianglePoint, triangleEdge, triangleCenter, hullA, edgeQuery, + cache ); + } + return; + } + + default: + B3_ASSERT( cache->type == b3_invalidAxis ); + break; + } + + // Cache miss + cache->hit = 0; + + // Find axis of minimum penetration + b3FaceQuery faceQueryA = b3QueryTriangleFace( &triangle, hullA ); + if ( faceQueryA.separation > speculativeDistance ) + { + // Separating axis found + cache->separation = faceQueryA.separation; + cache->type = b3_faceAxisA; + cache->indexA = 0; + cache->indexB = UINT8_MAX; + return; + } + + b3FaceQuery faceQueryB = b3QueryHullFace( &triangle, hullA ); + if ( faceQueryB.separation > speculativeDistance ) + { + // Separating axis found + cache->separation = faceQueryB.separation; + cache->type = b3_faceAxisB; + cache->indexA = UINT8_MAX; + cache->indexB = (uint8_t)faceQueryB.faceIndex; + return; + } + + b3EdgeQuery edgeQuery = b3TestEdgePairs( &triangle, hullA ); + if ( edgeQuery.separation > speculativeDistance ) + { + // Separating axis found + cache->separation = edgeQuery.separation; + cache->type = b3_edgePairAxis; + cache->indexA = (uint8_t)edgeQuery.indexA; + cache->indexB = (uint8_t)edgeQuery.indexB; + return; + } + + float clippedFaceSeparation; + + // Don't admit a hull face significantly opposed to the triangle face. + // Need a tolerance to avoid ghost collisions. + // todo hull query skips faces that point along the triangle normal + b3Vec3 hullNormal = hullPlanes[faceQueryB.faceIndex].normal; + bool pushingDown = b3Dot( hullNormal, trianglePlane.normal ) > 0.25f; + if ( faceQueryB.separation > faceQueryA.separation + linearSlop && pushingDown == false ) + { + clippedFaceSeparation = b3CollideHullFace( manifold, capacity, &triangle, hullA, faceQueryB, cache, enableSpeculative ); + } + else + { + clippedFaceSeparation = + b3CollideTriangleFace( manifold, capacity, &triangle, hullA, faceQueryA, cache, enableSpeculative ); + } + + // Does an edge axis exist? + if ( edgeQuery.indexA != B3_NULL_INDEX ) + { + // When axes are aligned the edge separation can be garbage. + // If a face axis has positive separation there may be no points. + float maxFaceSeparation = b3MaxFloat( faceQueryA.separation, faceQueryB.separation ); + + if ( ( manifold->pointCount == 0 && edgeQuery.separation > maxFaceSeparation ) || + ( manifold->pointCount == 1 && edgeQuery.separation > clippedFaceSeparation + linearSlop ) ) + { + B3_ASSERT( 0 <= edgeQuery.indexA && edgeQuery.indexA < 3 ); + b3Vec3 trianglePoint = trianglePoints[edgeQuery.indexA]; + b3Vec3 triangleEdge = triangleEdges[edgeQuery.indexA]; + manifold->pointCount = 0; + b3CollideHullAndTriangleEdges( manifold, capacity, trianglePoint, triangleEdge, triangleCenter, hullA, edgeQuery, + cache ); + } + } + + // Using the speculative distance means that sometimes there are no valid contact points from SAT. + // In this fall back to GJK. This is important to prevent tunneling in rare cases. + if ( manifold->pointCount == 0 ) + { + b3Vec3 triangleB[] = { v1, v2, v3 }; + b3DistanceInput input = { 0 }; + input.proxyA = (b3ShapeProxy){ + .points = triangleB, + .count = 3, + .radius = 0.0f, + }; + input.proxyB = (b3ShapeProxy){ .points = hullPoints, .count = hullA->vertexCount, .radius = 0.0f }; + input.transform = b3Transform_identity; + input.useRadii = false; + + b3SimplexCache simplexCache = { 0 }; + b3DistanceOutput output = b3ShapeDistance( &input, &simplexCache, NULL, 0 ); + + if ( output.distance > 0.0f ) + { + B3_ASSERT( 0 < simplexCache.count && simplexCache.count <= 3 ); + + manifold->pointCount = 1; + manifold->feature = b3GetTriangleFeature( &simplexCache ); + manifold->normal = output.normal; + manifold->points[0].point = output.pointB; + manifold->points[0].separation = output.distance; + + // This feature pair not accurate but maybe it doesn't matter + manifold->points[0].pair = b3FeaturePair_single; + } + } +} diff --git a/vendor/box3d/src/src/types.c b/vendor/box3d/src/src/types.c new file mode 100644 index 000000000..b199cd15d --- /dev/null +++ b/vendor/box3d/src/src/types.c @@ -0,0 +1,155 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +#include "box3d/types.h" + +#include "core.h" + +#include "box3d/constants.h" + +b3WorldDef b3DefaultWorldDef( void ) +{ + float lengthUnits = b3GetLengthUnitsPerMeter(); + + b3WorldDef def = { 0 }; + def.gravity.x = 0.0f; + def.gravity.y = -10.0f; + def.hitEventThreshold = 1.0f * lengthUnits; + def.restitutionThreshold = 1.0f * lengthUnits; + def.contactSpeed = 3.0f * lengthUnits; + def.contactHertz = 30.0f; + def.contactDampingRatio = 10.0f; + + // 400 meters per second, faster than the speed of sound + def.maximumLinearSpeed = 400.0f * lengthUnits; + + def.enableSleep = true; + def.enableContinuous = true; + def.internalValue = B3_SECRET_COOKIE; + return def; +} + +b3BodyDef b3DefaultBodyDef( void ) +{ + b3BodyDef def = { 0 }; + def.type = b3_staticBody; + def.rotation = b3Quat_identity; + def.sleepThreshold = 0.05f * b3GetLengthUnitsPerMeter(); + def.gravityScale = 1.0f; + def.enableSleep = true; + def.isAwake = true; + def.isEnabled = true; + def.enableContactRecycling = true; + def.internalValue = B3_SECRET_COOKIE; + return def; +} + +b3Filter b3DefaultFilter( void ) +{ + b3Filter filter = { B3_DEFAULT_CATEGORY_BITS, B3_DEFAULT_MASK_BITS, 0 }; + return filter; +} + +b3QueryFilter b3DefaultQueryFilter( void ) +{ + b3QueryFilter filter = { B3_DEFAULT_CATEGORY_BITS, B3_DEFAULT_MASK_BITS, 0, NULL }; + return filter; +} + +b3SurfaceMaterial b3DefaultSurfaceMaterial( void ) +{ + b3SurfaceMaterial surfaceMaterial = { 0 }; + surfaceMaterial.friction = 0.6f; + return surfaceMaterial; +} + +b3ShapeDef b3DefaultShapeDef( void ) +{ + float lengthUnits = b3GetLengthUnitsPerMeter(); + + b3ShapeDef def = { 0 }; + def.baseMaterial = b3DefaultSurfaceMaterial(); + // density of water + def.density = 1000.0f / ( lengthUnits * lengthUnits * lengthUnits ); + def.explosionScale = 1.0f; + def.filter = b3DefaultFilter(); + def.updateBodyMass = true; + def.invokeContactCreation = true; + def.enableSpeculativeContact = true; + def.internalValue = B3_SECRET_COOKIE; + return def; +} + +static bool b3EmptyDrawShape( void* userShape, b3WorldTransform transform, b3HexColor color, void* context ) +{ + B3_UNUSED( userShape, transform, color, context ); + return false; +} + +static void b3EmptyDrawSegment( b3Pos p1, b3Pos p2, b3HexColor color, void* context ) +{ + B3_UNUSED( p1, p2, color, context ); +} + +static void b3EmptyDrawTransform( b3WorldTransform transform, void* context ) +{ + B3_UNUSED( transform, context ); +} + +static void b3EmptyDrawPoint( b3Pos p, float size, b3HexColor color, void* context ) +{ + B3_UNUSED( p, size, color, context ); +} + +static void b3EmptyDrawSphere( b3Pos p, float radius, b3HexColor color, float alpha, void* context ) +{ + B3_UNUSED( p, radius, color, alpha, context ); +} + +static void b3EmptyDrawCapsule( b3Pos p1, b3Pos p2, float radius, b3HexColor color, float alpha, void* context ) +{ + B3_UNUSED( p1, p2, radius, color, alpha, context ); +} + +static void b3EmptyDrawBounds( b3AABB aabb, b3HexColor color, void* context ) +{ + B3_UNUSED( aabb, color, context ); +} + +static void b3EmptyDrawBox( b3Vec3 extents, b3WorldTransform transform, b3HexColor color, void* context ) +{ + B3_UNUSED( extents, transform, color, context ); +} + +static void b3EmptyDrawString( b3Pos p, const char* s, b3HexColor color, void* context ) +{ + B3_UNUSED( p, s, color, context ); +} + +b3DebugDraw b3DefaultDebugDraw( void ) +{ + b3DebugDraw draw = { 0 }; + + // These allow the user to skip some implementations and not hit null exceptions. + draw.DrawShapeFcn = b3EmptyDrawShape; + draw.DrawSegmentFcn = b3EmptyDrawSegment; + draw.DrawTransformFcn = b3EmptyDrawTransform; + draw.DrawPointFcn = b3EmptyDrawPoint; + draw.DrawSphereFcn = b3EmptyDrawSphere; + draw.DrawCapsuleFcn = b3EmptyDrawCapsule; + draw.DrawBoundsFcn = b3EmptyDrawBounds; + draw.DrawBoxFcn = b3EmptyDrawBox; + draw.DrawStringFcn = b3EmptyDrawString; + + // Not too small, not too big. + float h = 100.0f * b3GetLengthUnitsPerMeter(); + draw.drawingBounds = (b3AABB){ + .lowerBound = { -h, -h, -h }, + .upperBound = { h, h, h }, + }; + + draw.jointScale = 1.0f; + draw.forceScale = 1.0f; + + return draw; +} diff --git a/vendor/box3d/src/src/verstable.h b/vendor/box3d/src/src/verstable.h new file mode 100644 index 000000000..c8601e6d4 --- /dev/null +++ b/vendor/box3d/src/src/verstable.h @@ -0,0 +1,2069 @@ +/*------------------------------------------------- VERSTABLE v2.2.1 --------------------------------------------------- + +Verstable is a C99-compatible, open-addressing hash table using quadratic probing and the following additions: + +* All keys that hash (i.e. "belong") to the same bucket (their "home bucket") are linked together by an 11-bit integer + specifying the quadratic displacement, relative to that bucket, of the next key in the chain. + +* If a chain of keys exists for a given bucket, then it always begins at that bucket. To maintain this policy, a 1-bit + flag is used to mark whether the key occupying a bucket belongs there. When inserting a new key, if the bucket it + belongs to is occupied by a key that does not belong there, then the occupying key is evicted and the new key takes + the bucket. + +* A 4-bit fragment of each key's hash code is also stored. + +* The aforementioned metadata associated with each bucket (the 4-bit hash fragment, the 1-bit flag, and the 11-bit link + to the next key in the chain) are stored together in a uint16_t array rather than in the bucket alongside the key and + (optionally) the value. + +One way to conceptualize this scheme is as a chained hash table in which overflowing keys are stored not in separate +memory allocations but in otherwise unused buckets. In this regard, it shares similarities with Malte Skarupke's Bytell +hash table (https://www.youtube.com/watch?v=M2fKMP47slQ) and traditional "coalesced hashing". + +Advantages of this scheme include: + +* Fast lookups impervious to load factor: If the table contains any key belonging to the lookup key's home bucket, then + that bucket contains the first in a traversable chain of all keys belonging to it. Hence, only the home bucket and + other buckets containing keys belonging to it are ever probed. Moreover, the stored hash fragments allow skipping most + non-matching keys in the chain without accessing the actual buckets array or calling the (potentially expensive) key + comparison function. + +* Fast insertions: Insertions are faster than they are in other schemes that move keys around (e.g. Robin Hood) because + they only move, at most, one existing key. + +* Fast, tombstone-free deletions: Deletions, which usually require tombstones in quadratic-probing hash tables, are + tombstone-free and only move, at most, one existing key. + +* Fast iteration: The separate metadata array allows keys in sparsely populated tables to be found without incurring the + frequent cache misses that would result from traversing the buckets array. + +Usage example: + + +---------------------------------------------------------+----------------------------------------------------------+ + | Using the generic macro API (C11 and later): | Using the prefixed functions API (C99 and later): | + +---------------------------------------------------------+----------------------------------------------------------+ + | #include | #include | + | | | + | // Instantiating a set template. | // Instantiating a set template. | + | #define NAME int_set | #define NAME int_set | + | #define KEY_TY int | #define KEY_TY int | + | #include "verstable.h" | #define HASH_FN vt_hash_integer | + | | #define CMPR_FN vt_cmpr_integer | + | // Instantiating a map template. | #include "verstable.h" | + | #define NAME int_int_map | | + | #define KEY_TY int | // Instantiating a map template. | + | #define VAL_TY int | #define NAME int_int_map | + | #include "verstable.h" | #define KEY_TY int | + | | #define VAL_TY int | + | int main( void ) | #define HASH_FN vt_hash_integer | + | { | #define CMPR_FN vt_cmpr_integer | + | // Set. | #include "verstable.h" | + | | | + | int_set our_set; | int main( void ) | + | vt_init( &our_set ); | { | + | | // Set. | + | // Inserting keys. | | + | for( int i = 0; i < 10; ++i ) | int_set our_set; | + | { | int_set_init( &our_set ); | + | int_set_itr itr = vt_insert( &our_set, i ); | | + | if( vt_is_end( itr ) ) | // Inserting keys. | + | { | for( int i = 0; i < 10; ++i ) | + | // Out of memory, so abort. | { | + | vt_cleanup( &our_set ); | int_set_itr itr = | + | return 1; | int_set_insert( &our_set, i ); | + | } | if( int_set_is_end( itr ) ) | + | } | { | + | | // Out of memory, so abort. | + | // Erasing keys. | int_set_cleanup( &our_set ); | + | for( int i = 0; i < 10; i += 3 ) | return 1; | + | vt_erase( &our_set, i ); | } | + | | } | + | // Retrieving keys. | | + | for( int i = 0; i < 10; ++i ) | // Erasing keys. | + | { | for( int i = 0; i < 10; i += 3 ) | + | int_set_itr itr = vt_get( &our_set, i ); | int_set_erase( &our_set, i ); | + | if( !vt_is_end( itr ) ) | | + | printf( "%d ", itr.data->key ); | // Retrieving keys. | + | } | for( int i = 0; i < 10; ++i ) | + | // Printed: 1 2 4 5 7 8 | { | + | | int_set_itr itr = int_set_get( &our_set, i ); | + | // Iteration. | if( !int_set_is_end( itr ) ) | + | for( | printf( "%d ", itr.data->key ); | + | int_set_itr itr = vt_first( &our_set ); | } | + | !vt_is_end( itr ); | // Printed: 1 2 4 5 7 8 | + | itr = vt_next( itr ) | | + | ) | // Iteration. | + | printf( "%d ", itr.data->key ); | for( | + | // Printed: 2 4 7 1 5 8 | int_set_itr itr = | + | | int_set_first( &our_set ); | + | vt_cleanup( &our_set ); | !int_set_is_end( itr ); | + | | itr = int_set_next( itr ) | + | // Map. | ) | + | | printf( "%d ", itr.data->key ); | + | int_int_map our_map; | // Printed: 2 4 7 1 5 8 | + | vt_init( &our_map ); | | + | | int_set_cleanup( &our_set ); | + | // Inserting keys and values. | | + | for( int i = 0; i < 10; ++i ) | // Map. | + | { | | + | int_int_map_itr itr = | int_int_map our_map; | + | vt_insert( &our_map, i, i + 1 ); | int_int_map_init( &our_map ); | + | if( vt_is_end( itr ) ) | | + | { | // Inserting keys and values. | + | // Out of memory, so abort. | for( int i = 0; i < 10; ++i ) | + | vt_cleanup( &our_map ); | { | + | return 1; | int_int_map_itr itr = | + | } | int_int_map_insert( &our_map, i, i + 1 ); | + | } | if( int_int_map_is_end( itr ) ) | + | | { | + | // Erasing keys and values. | // Out of memory, so abort. | + | for( int i = 0; i < 10; i += 3 ) | int_int_map_cleanup( &our_map ); | + | vt_erase( &our_map, i ); | return 1; | + | | } | + | // Retrieving keys and values. | } | + | for( int i = 0; i < 10; ++i ) | | + | { | // Erasing keys and values. | + | int_int_map_itr itr = vt_get( &our_map, i ); | for( int i = 0; i < 10; i += 3 ) | + | if( !vt_is_end( itr ) ) | int_int_map_erase( &our_map, i ); | + | printf( | | + | "%d:%d ", | // Retrieving keys and values. | + | itr.data->key, | for( int i = 0; i < 10; ++i ) | + | itr.data->val | { | + | ); | int_int_map_itr itr = | + | } | int_int_map_get( &our_map, i ); | + | // Printed: 1:2 2:3 4:5 5:6 7:8 8:9 | if( !int_int_map_is_end( itr ) ) | + | | printf( | + | // Iteration. | "%d:%d ", | + | for( | itr.data->key, | + | int_int_map_itr itr = vt_first( &our_map ); | itr.data->val | + | !vt_is_end( itr ); | ); | + | itr = vt_next( itr ) | } | + | ) | // Printed: 1:2 2:3 4:5 5:6 7:8 8:9 | + | printf( | | + | "%d:%d ", | // Iteration. | + | itr.data->key, | for( | + | itr.data->val | int_int_map_itr itr = | + | ); | int_int_map_first( &our_map ); | + | // Printed: 2:3 4:5 7:8 1:2 5:6 8:9 | !int_int_map_is_end( itr ); | + | | itr = int_int_map_next( itr ) | + | vt_cleanup( &our_map ); | ) | + | } | printf( | + | | "%d:%d ", | + | | itr.data->key, | + | | itr.data->val | + | | ); | + | | // Printed: 2:3 4:5 7:8 1:2 5:6 8:9 | + | | | + | | int_int_map_cleanup( &our_map ); | + | | } | + | | | + +---------------------------------------------------------+----------------------------------------------------------+ + +API: + + Instantiating a hash table template: + + Create a new hash table type in the following manner: + + #define NAME + #define KEY_TY + #include "verstable.h" + + The NAME macro specifies the name of hash table type that the library will declare, the prefix for the functions + associated with it, and the prefix for the associated iterator type. + + The KEY_TY macro specifies the key type. + + In C99, it is also always necessary to define HASH_FN and CMPR_FN (see below) before including the header. + + The following macros may also be defined before including the header: + + #define VAL_TY + + The type of the value associated with each key. + If this macro is defined, the hash table acts as a map associating keys with values. + Otherwise, it acts as a set containing only keys. + + #define HASH_FN + + The name of the existing function used to hash each key. + The function should have the signature uint64_t ( KEY_TY key ) and return a 64-bit hash code. + For best performance, the hash function should provide a high level of entropy across all bits. + There are two default hash functions: vt_hash_integer for all integer types up to 64 bits in size, and + vt_hash_string for NULL-terminated strings (i.e. char *). + When KEY_TY is one of such types and the compiler is in C11 mode or later, HASH_FN may be left undefined, in + which case the appropriate default function is inferred from KEY_TY. + Otherwise, HASH_FN must be defined. + + #define CMPR_FN + + The name of the existing function used to compare two keys. + The function should have the signature bool ( KEY_TY key_1, KEY_TY key_2 ) and return true if the two keys are + equal. + There are two default comparison functions: vt_cmpr_integer for all integer types up to 64 bits in size, and + vt_cmpr_string for NULL-terminated strings (i.e. char *). + As with the default hash functions, in C11 or later the appropriate default comparison function is inferred if + KEY_TY is one of such types and CMPR_FN is left undefined. + Otherwise, CMPR_FN must be defined. + + #define MAX_LOAD + + The floating-point load factor at which the hash table automatically doubles the size of its internal buckets + array. + The default is 0.9, i.e. 90%. + + #define KEY_DTOR_FN + + The name of the existing destructor function, with the signature void ( KEY_TY key ), called on a key when it is + erased from the table or replaced by a newly inserted key. + The API functions that may call the key destructor are NAME_insert, NAME_erase, NAME_erase_itr, NAME_clear, + and NAME_cleanup. + + #define VAL_DTOR_FN + + The name of the existing destructor function, with the signature void ( VAL_TY val ), called on a value when it + is erased from the table or replaced by a newly inserted value. + The API functions that may call the value destructor are NAME_insert, NAME_erase, NAME_erase_itr, NAME_clear, + and NAME_cleanup. + + #define CTX_TY + + The type of the hash table type's ctx (context) member. + This member only exists if CTX_TY was defined. + It is intended to be used in conjunction with MALLOC_FN and FREE_FN (see below). + + #define MALLOC_FN + + The name of the existing function used to allocate memory. + If CTX_TY was defined, the signature should be void *( size_t size, CTX_TY *ctx ), where size is the number of + bytes to allocate and ctx points to the table's ctx member. + Otherwise, the signature should be void *( size_t size ). + The default wraps stdlib.h's malloc. + + #define FREE_FN + + The name of the existing function used to free memory. + If CTX_TY was defined, the signature should be void ( void *ptr, size_t size, CTX_TY *ctx ), where ptr points to + the memory to free, size is the number of bytes that were allocated, and ctx points to the table's ctx member. + Otherwise, the signature should be void ( void *ptr, size_t size ). + The default wraps stdlib.h's free. + + #define HEADER_MODE + #define IMPLEMENTATION_MODE + + By default, all hash table functions are defined as static inline functions, the intent being that a given hash + table template should be instantiated once per translation unit; for best performance, this is the recommended + way to use the library. + However, it is also possible to separate the struct definitions and function declarations from the function + definitions such that one implementation can be shared across all translation units (as in a traditional header + and source file pair). + In that case, instantiate a template wherever it is needed by defining HEADER_MODE, along with only NAME, + KEY_TY, and (optionally) VAL_TY, CTX_TY, and header guards, and including the library, e.g.: + + #ifndef INT_INT_MAP_H + #define INT_INT_MAP_H + #define NAME int_int_map + #define KEY_TY int + #define VAL_TY int + #define HEADER_MODE + #include "verstable.h" + #endif + + In one source file, define IMPLEMENTATION_MODE, along with NAME, KEY_TY, and any of the aforementioned optional + macros, and include the library, e.g.: + + #define NAME int_int_map + #define KEY_TY int + #define VAL_TY int + #define HASH_FN vt_hash_integer // C99. + #define CMPR_FN vt_cmpr_integer // C99. + #define MAX_LOAD 0.8 + #define IMPLEMENTATION_MODE + #include "verstable.h" + + Including the library automatically undefines all the aforementioned macros after they have been used to instantiate + the template. + + Functions: + + The functions associated with a hash table type are all prefixed with the name the user supplied via the NAME macro. + In C11 and later, the generic "vt_"-prefixed macros may be used to automatically select the correct version of the + specified function based on the arguments. + + void NAME_init( NAME *table ) + void NAME_init( NAME *table, CTX_TY ctx ) + // C11 generic macro: vt_init. + + Initializes the table for use. + If CTX_TY was defined, ctx sets the table's ctx member. + + bool NAME_init_clone( NAME *table, NAME *source ) + bool NAME_init_clone( NAME *table, NAME *source, CTX_TY ctx ) + // C11 generic macro: vt_init_clone. + + Initializes the table as a shallow copy of the specified source table. + If CTX_TY was defined, ctx sets the table's ctx member. + Returns false in the case of memory allocation failure. + + size_t NAME_size( NAME *table ) // C11 generic macro: vt_size. + + Returns the number of keys currently in the table. + + size_t NAME_bucket_count( NAME *table ) // C11 generic macro: vt_bucket_count. + + Returns the table's current bucket count. + + NAME_itr NAME_insert( NAME *table, KEY_TY key ) + NAME_itr NAME_insert( NAME *table, KEY_TY key, VAL_TY val ) + // C11 generic macro: vt_insert. + + Inserts the specified key (and value, if VAL_TY was defined) into the hash table. + If the same key already exists, then the new key (and value) replaces the existing key (and value). + Returns an iterator to the new key, or an end iterator in the case of memory allocation failure. + + NAME_itr NAME_get_or_insert( NAME *table, KEY_TY key ) + NAME_itr NAME_get_or_insert( NAME *table, KEY_TY key, VAL_TY val ) + // C11 generic macro: vt_get_or_insert. + + Inserts the specified key (and value, if VAL_TY was defined) if it does not already exist in the table. + Returns an iterator to the new key if it was inserted, or an iterator to the existing key, or an end iterator if + the key did not exist but the new key could not be inserted because of memory allocation failure. + Determine whether the key was inserted by comparing the table's size before and after the call. + + NAME_itr NAME_get( NAME *table, KEY_TY key ) // C11 generic macro: vt_get. + + Returns a iterator to the specified key, or an end iterator if no such key exists. + + bool NAME_erase( NAME *table, KEY_TY key ) // C11 generic macro: vt_erase. + + Erases the specified key (and associated value, if VAL_TY was defined), if it exists. + Returns true if a key was erased. + + NAME_itr NAME_erase_itr( NAME *table, NAME_itr itr ) // C11 generic macro: vt_erase_itr. + + Erases the key (and associated value, if VAL_TY was defined) pointed to by the specified iterator. + Returns an iterator to the next key in the table, or an end iterator if the erased key was the last one. + + bool NAME_reserve( NAME *table, size_t size ) // C11 generic macro: vt_reserve. + + Ensures that the bucket count is large enough to support the specified key count (i.e. size) without rehashing. + Returns false if unsuccessful due to memory allocation failure. + + bool NAME_shrink( NAME *table ) // C11 generic macro: vt_shrink. + + Shrinks the bucket count to best accommodate the current size. + Returns false if unsuccessful due to memory allocation failure. + + NAME_itr NAME_first( NAME *table ) // C11 generic macro: vt_first. + + Returns an iterator to the first key in the table, or an end iterator if the table is empty. + + bool NAME_is_end( NAME_itr itr ) // C11 generic macro: vt_is_end. + + Returns true if the iterator is an end iterator. + + NAME_itr NAME_next( NAME_itr itr ) // C11 generic macro: vt_next. + + Returns an iterator to the key after the one pointed to by the specified iterator, or an end iterator if the + specified iterator points to the last key in the table. + + void NAME_clear( NAME *table ) // C11 generic macro: vt_clear. + + Erases all keys (and values, if VAL_TY was defined) in the table. + + void NAME_cleanup( NAME *table ) // C11 generic macro: vt_cleanup. + + Erases all keys (and values, if VAL_TY was defined) in the table, frees all memory associated with it, and + initializes it for reuse. + + Iterators: + + Access the key (and value, if VAL_TY was defined) that an iterator points to using the NAME_itr struct's data + member: + + itr.data->key + itr.data->val + + Functions that may insert new keys (NAME_insert and NAME_get_or_insert), erase keys (NAME_erase and NAME_erase_itr), + or reallocate the internal bucket array (NAME_reserve and NAME_shrink) invalidate all existing iterators. + To delete keys during iteration and resume iterating, use the return value of NAME_erase_itr. + +Version history: + + 06/05/2025 2.2.1: Fixed incorrect signature of NAME_is_end in documentation. + 18/04/2025 2.2.0: Added const qualifier to the table parameter of NAME_size, NAME_bucket_count, NAME_get, and + NAME_first and to the source parameter of NAME_init_clone. + Added default support for const char * strings. + Added support for -Wextra. + Replaced FNV-1a with Wyhash as the default string hash function. + 18/06/2024 2.1.1: Fixed a bug affecting iteration on big-endian platforms under MSVC. + 27/05/2024 2.1.0: Replaced the Murmur3 mixer with the fast-hash mixer as the default integer hash function. + Fixed a bug that could theoretically cause a crash on rehash (triggerable in testing using + NAME_shrink with a maximum load factor significantly higher than 1.0). + 06/02/2024 2.0.0: Improved custom allocator support by introducing the CTX_TY option and allowing user-supplied free + functions to receive the allocation size. + Improved documentation. + Introduced various optimizations, including storing the buckets-array size mask instead of the + bucket count, eliminating empty-table checks, combining the buckets memory and metadata memory into + one allocation, and adding branch prediction macros. + Fixed a bug that caused a key to be used after destruction during erasure. + 12/12/2023 1.0.0: Initial release. + +License (MIT): + + Copyright (c) 2023-2025 Jackson L. Allan + + Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated + documentation files (the "Software"), to deal in the Software without restriction, including without limitation the + rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit + persons to whom the Software is furnished to do so, subject to the following conditions: + + The above copyright notice and this permission notice shall be included in all copies or substantial portions of the + Software. + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE + WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR + COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR + OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. + +*/ + +/*--------------------------------------------------------------------------------------------------------------------*/ +/* Common header section */ +/*--------------------------------------------------------------------------------------------------------------------*/ + +#ifndef VERSTABLE_H +#define VERSTABLE_H + +#include +#include +#include +#include +#include +#include + +// Two-way concatenation macro. +#define VT_CAT_( a, b ) a##b +#define VT_CAT( a, b ) VT_CAT_( a, b ) + +// Branch optimization macros. +#ifdef __GNUC__ +#define VT_LIKELY( expression ) __builtin_expect( (bool)( expression ), true ) +#define VT_UNLIKELY( expression ) __builtin_expect( (bool)( expression ), false ) +#else +#define VT_LIKELY( expression ) ( expression ) +#define VT_UNLIKELY( expression ) ( expression ) +#endif + +// Masks for manipulating and extracting data from a bucket's uint16_t metadatum. +#define VT_EMPTY 0x0000 +#define VT_HASH_FRAG_MASK 0xF000 // 0b1111000000000000. +#define VT_IN_HOME_BUCKET_MASK 0x0800 // 0b0000100000000000. +#define VT_DISPLACEMENT_MASK 0x07FF // 0b0000011111111111, also denotes the displacement limit. Set to VT_LOAD to 1.0 + // to test proper handling of encroachment on the displacement limit during + // inserts. + +// Extracts a hash fragment from a uint64_t hash code. +// We take the highest four bits so that keys that map (via modulo) to the same bucket have distinct hash fragments. +static inline uint16_t vt_hashfrag( uint64_t hash ) +{ + return ( hash >> 48 ) & VT_HASH_FRAG_MASK; +} + +// Standard quadratic probing formula that guarantees that all buckets are visited when the bucket count is a power of +// two (at least in theory, because the displacement limit could terminate the search early when the bucket count is +// high). +static inline size_t vt_quadratic( uint16_t displacement ) +{ + return ( (size_t)displacement * displacement + displacement ) / 2; +} + +#define VT_MIN_NONZERO_BUCKET_COUNT 8 // Must be a power of two. + +// Function to find the left-most non-zero uint16_t in a uint64_t. +// This function is used when we scan four buckets at a time while iterating and relies on compiler intrinsics wherever +// possible. + +#if defined( __GNUC__ ) && ULLONG_MAX == 0xFFFFFFFFFFFFFFFF + +static inline int vt_first_nonzero_uint16( uint64_t val ) +{ + const uint16_t endian_checker = 0x0001; + if( *(const char *)&endian_checker ) // Little-endian (the compiler will optimize away the check at -O1 and above). + return __builtin_ctzll( val ) / 16; + + return __builtin_clzll( val ) / 16; +} + +#elif defined( _MSC_VER ) && ( defined( _M_X64 ) || defined( _M_ARM64 ) ) + +#include +#pragma intrinsic(_BitScanForward64) +#pragma intrinsic(_BitScanReverse64) + +static inline int vt_first_nonzero_uint16( uint64_t val ) +{ + unsigned long result; + + const uint16_t endian_checker = 0x0001; + if( *(const char *)&endian_checker ) + _BitScanForward64( &result, val ); + else + { + _BitScanReverse64( &result, val ); + result = 63 - result; + } + + return result / 16; +} + +#else + +static inline int vt_first_nonzero_uint16( uint64_t val ) +{ + int result = 0; + + uint32_t half; + memcpy( &half, &val, sizeof( uint32_t ) ); + if( !half ) + result += 2; + + uint16_t quarter; + memcpy( &quarter, (char *)&val + result * sizeof( uint16_t ), sizeof( uint16_t ) ); + if( !quarter ) + result += 1; + + return result; +} + +#endif + +// When the bucket count is zero, setting the metadata pointer to point to a VT_EMPTY placeholder, rather than NULL, +// allows us to avoid checking for a zero bucket count during insertion and lookup. +static const uint16_t vt_empty_placeholder_metadatum = VT_EMPTY; + +// Default hash and comparison functions. + +// Fast-hash, as described by https://jonkagstrom.com/bit-mixer-construction and +// https://code.google.com/archive/p/fast-hash. +// In testing, this hash function provided slightly better performance than the Murmur3 mixer. +static inline uint64_t vt_hash_integer( uint64_t key ) +{ + key ^= key >> 23; + key *= 0x2127599bf4325c37ull; + key ^= key >> 47; + return key; +} + +// For hashing strings, we use the public-domain Wyhash +// (https://github.com/wangyi-fudan/wyhash) with the following modifications: +// * We use a fixed seed and secret (the defaults suggested in the Wyhash repository). +// * We do not handle endianness, so the result will differ depending on the platform. +// * We omit the code optimized for 32-bit platforms. + +static inline void vt_wymum( uint64_t *a, uint64_t *b ) +{ +#if defined( __SIZEOF_INT128__ ) + __uint128_t r = *a; + r *= *b; + *a = (uint64_t)r; + *b = (uint64_t)( r >> 64 ); +#elif defined( _MSC_VER ) && defined( _M_X64 ) + *a = _umul128( *a, *b, b ); +#else + uint64_t ha = *a >> 32; + uint64_t hb = *b >> 32; + uint64_t la = (uint32_t)*a; + uint64_t lb = (uint32_t)*b; + uint64_t rh = ha * hb; + uint64_t rm0 = ha * lb; + uint64_t rm1 = hb * la; + uint64_t rl = la * lb; + uint64_t t = rl + ( rm0 << 32 ); + uint64_t c = t < rl; + uint64_t lo = t + ( rm1 << 32 ); + c += lo < t; + uint64_t hi = rh + ( rm0 >> 32 ) + ( rm1 >> 32 ) + c; + *a = lo; + *b = hi; +#endif +} + +static inline uint64_t vt_wymix( uint64_t a, uint64_t b ) +{ + vt_wymum( &a, &b ); + return a ^ b; +} + +static inline uint64_t vt_wyr8( const unsigned char *p ) +{ + uint64_t v; + memcpy( &v, p, 8 ); + return v; +} + +static inline uint64_t vt_wyr4( const unsigned char *p ) +{ + uint32_t v; + memcpy( &v, p, 4 ); + return v; +} + +static inline uint64_t vt_wyr3( const unsigned char *p, size_t k ) +{ + return ( ( (uint64_t)p[ 0 ] ) << 16 ) | ( ( (uint64_t)p[ k >> 1 ] ) << 8 ) | p[ k - 1 ]; +} + +static inline size_t vt_wyhash( const void *key, size_t len ) +{ + const unsigned char *p = (const unsigned char *)key; + uint64_t seed = 0xca813bf4c7abf0a9ull; + uint64_t a; + uint64_t b; + if( VT_LIKELY( len <= 16 ) ) + { + if( VT_LIKELY( len >= 4 ) ) + { + a = ( vt_wyr4( p ) << 32 ) | vt_wyr4( p + ( ( len >> 3 ) << 2 ) ); + b = ( vt_wyr4( p + len - 4 ) << 32 ) | vt_wyr4( p + len - 4 - ( ( len >> 3 ) << 2 ) ); + } + else if( VT_LIKELY( len > 0 ) ) + { + a = vt_wyr3( p, len ); + b = 0; + } + else + { + a = 0; + b = 0; + } + } + else + { + size_t i = len; + if( VT_UNLIKELY( i >= 48 ) ) + { + uint64_t see1 = seed; + uint64_t see2 = seed; + do{ + seed = vt_wymix( vt_wyr8( p ) ^ 0x8bb84b93962eacc9ull, vt_wyr8( p + 8 ) ^ seed ); + see1 = vt_wymix( vt_wyr8( p + 16 ) ^ 0x4b33a62ed433d4a3ull, vt_wyr8( p + 24 ) ^ see1 ); + see2 = vt_wymix( vt_wyr8( p + 32 ) ^ 0x4d5a2da51de1aa47ull, vt_wyr8( p + 40 ) ^ see2 ); + p += 48; + i -= 48; + } + while( VT_LIKELY( i >= 48 ) ); + seed ^= see1 ^ see2; + } + + while( VT_UNLIKELY( i > 16 ) ) + { + seed = vt_wymix( vt_wyr8( p ) ^ 0x8bb84b93962eacc9ull, vt_wyr8( p + 8 ) ^ seed ); + i -= 16; + p += 16; + } + + a = vt_wyr8( p + i - 16 ); + b = vt_wyr8( p + i - 8 ); + } + + a ^= 0x8bb84b93962eacc9ull; + b ^= seed; + vt_wymum( &a, &b ); + return (size_t)vt_wymix( a ^ 0x2d358dccaa6c78a5ull ^ len, b ^ 0x8bb84b93962eacc9ull ); +} + +static inline uint64_t vt_hash_string( const char *key ) +{ + return vt_wyhash( key, strlen( key ) ); +} + +static inline bool vt_cmpr_integer( uint64_t key_1, uint64_t key_2 ) +{ + return key_1 == key_2; +} + +static inline bool vt_cmpr_string( const char *key_1, const char *key_2 ) +{ + return strcmp( key_1, key_2 ) == 0; +} + +// Default allocation and free functions. + +static inline void *vt_malloc( size_t size ) +{ + return malloc( size ); +} + +static inline void vt_free( void *ptr, size_t size ) +{ + (void)size; + free( ptr ); +} + +static inline void *vt_malloc_with_ctx( size_t size, void *ctx ) +{ + (void)ctx; + return malloc( size ); +} + +static inline void vt_free_with_ctx( void *ptr, size_t size, void *ctx ) +{ + (void)size; + (void)ctx; + free( ptr ); +} + +// The rest of the common header section pertains to the C11 generic macro API. +// This interface is based on the extendible-_Generic mechanism documented in detail at +// https://github.com/JacksonAllan/CC/blob/main/articles/Better_C_Generics_Part_1_The_Extendible_Generic.md. +// In summary, instantiating a template also defines wrappers for the template's types and functions with names in the +// pattern of vt_table_NNNN and vt_init_NNNN, where NNNN is an automatically generated integer unique to the template +// instance in the current translation unit. +// These wrappers plug into _Generic-based API macros, which use preprocessor magic to automatically generate _Generic +// slots for every existing template instance. +#if defined(__STDC_VERSION__) && __STDC_VERSION__ >= 201112L && !defined( VT_NO_C11_GENERIC_API ) + +// Octal counter that supports up to 511 hash table templates. +#define VT_TEMPLATE_COUNT_D1 0 // Digit 1, i.e. least significant digit. +#define VT_TEMPLATE_COUNT_D2 0 +#define VT_TEMPLATE_COUNT_D3 0 + +// Four-way concatenation macro. +#define VT_CAT_4_( a, b, c, d ) a##b##c##d +#define VT_CAT_4( a, b, c, d ) VT_CAT_4_( a, b, c, d ) + +// Provides the current value of the counter as a three-digit octal number preceded by 0. +#define VT_TEMPLATE_COUNT VT_CAT_4( 0, VT_TEMPLATE_COUNT_D3, VT_TEMPLATE_COUNT_D2, VT_TEMPLATE_COUNT_D1 ) + +// _Generic-slot generation macros. + +#define VT_GENERIC_SLOT( ty, fn, n ) , VT_CAT( ty, n ): VT_CAT( fn, n ) +#define VT_R1_0( ty, fn, d3, d2 ) +#define VT_R1_1( ty, fn, d3, d2 ) VT_GENERIC_SLOT( ty, fn, VT_CAT_4( 0, d3, d2, 0 ) ) +#define VT_R1_2( ty, fn, d3, d2 ) VT_GENERIC_SLOT( ty, fn, VT_CAT_4( 0, d3, d2, 1 ) ) VT_R1_1( ty, fn, d3, d2 ) +#define VT_R1_3( ty, fn, d3, d2 ) VT_GENERIC_SLOT( ty, fn, VT_CAT_4( 0, d3, d2, 2 ) ) VT_R1_2( ty, fn, d3, d2 ) +#define VT_R1_4( ty, fn, d3, d2 ) VT_GENERIC_SLOT( ty, fn, VT_CAT_4( 0, d3, d2, 3 ) ) VT_R1_3( ty, fn, d3, d2 ) +#define VT_R1_5( ty, fn, d3, d2 ) VT_GENERIC_SLOT( ty, fn, VT_CAT_4( 0, d3, d2, 4 ) ) VT_R1_4( ty, fn, d3, d2 ) +#define VT_R1_6( ty, fn, d3, d2 ) VT_GENERIC_SLOT( ty, fn, VT_CAT_4( 0, d3, d2, 5 ) ) VT_R1_5( ty, fn, d3, d2 ) +#define VT_R1_7( ty, fn, d3, d2 ) VT_GENERIC_SLOT( ty, fn, VT_CAT_4( 0, d3, d2, 6 ) ) VT_R1_6( ty, fn, d3, d2 ) +#define VT_R1_8( ty, fn, d3, d2 ) VT_GENERIC_SLOT( ty, fn, VT_CAT_4( 0, d3, d2, 7 ) ) VT_R1_7( ty, fn, d3, d2 ) +#define VT_R2_0( ty, fn, d3 ) +#define VT_R2_1( ty, fn, d3 ) VT_R1_8( ty, fn, d3, 0 ) +#define VT_R2_2( ty, fn, d3 ) VT_R1_8( ty, fn, d3, 1 ) VT_R2_1( ty, fn, d3 ) +#define VT_R2_3( ty, fn, d3 ) VT_R1_8( ty, fn, d3, 2 ) VT_R2_2( ty, fn, d3 ) +#define VT_R2_4( ty, fn, d3 ) VT_R1_8( ty, fn, d3, 3 ) VT_R2_3( ty, fn, d3 ) +#define VT_R2_5( ty, fn, d3 ) VT_R1_8( ty, fn, d3, 4 ) VT_R2_4( ty, fn, d3 ) +#define VT_R2_6( ty, fn, d3 ) VT_R1_8( ty, fn, d3, 5 ) VT_R2_5( ty, fn, d3 ) +#define VT_R2_7( ty, fn, d3 ) VT_R1_8( ty, fn, d3, 6 ) VT_R2_6( ty, fn, d3 ) +#define VT_R2_8( ty, fn, d3 ) VT_R1_8( ty, fn, d3, 7 ) VT_R2_7( ty, fn, d3 ) +#define VT_R3_0( ty, fn ) +#define VT_R3_1( ty, fn ) VT_R2_8( ty, fn, 0 ) +#define VT_R3_2( ty, fn ) VT_R2_8( ty, fn, 1 ) VT_R3_1( ty, fn ) +#define VT_R3_3( ty, fn ) VT_R2_8( ty, fn, 2 ) VT_R3_2( ty, fn ) +#define VT_R3_4( ty, fn ) VT_R2_8( ty, fn, 3 ) VT_R3_3( ty, fn ) +#define VT_R3_5( ty, fn ) VT_R2_8( ty, fn, 4 ) VT_R3_4( ty, fn ) +#define VT_R3_6( ty, fn ) VT_R2_8( ty, fn, 5 ) VT_R3_5( ty, fn ) +#define VT_R3_7( ty, fn ) VT_R2_8( ty, fn, 6 ) VT_R3_6( ty, fn ) + +#define VT_GENERIC_SLOTS( ty, fn ) \ +VT_CAT( VT_R1_, VT_TEMPLATE_COUNT_D1 )( ty, fn, VT_TEMPLATE_COUNT_D3, VT_TEMPLATE_COUNT_D2 ) \ +VT_CAT( VT_R2_, VT_TEMPLATE_COUNT_D2 )( ty, fn, VT_TEMPLATE_COUNT_D3 ) \ +VT_CAT( VT_R3_, VT_TEMPLATE_COUNT_D3 )( ty, fn ) \ + +// Actual generic API macros. + +// vt_init must be handled as a special case because it could take one or two arguments, depending on whether CTX_TY +// was defined. +#define VT_ARG_3( _1, _2, _3, ... ) _3 +#define vt_init( ... ) VT_ARG_3( __VA_ARGS__, vt_init_with_ctx, vt_init_without_ctx, )( __VA_ARGS__ ) +#define vt_init_without_ctx( table ) _Generic( *( table ) VT_GENERIC_SLOTS( vt_table_, vt_init_ ) )( table ) +#define vt_init_with_ctx( table, ... ) _Generic( *( table ) \ + VT_GENERIC_SLOTS( vt_table_, vt_init_ ) \ +)( table, __VA_ARGS__ ) \ + +#define vt_init_clone( table, ... ) _Generic( *( table ) \ + VT_GENERIC_SLOTS( vt_table_, vt_init_clone_ ) \ +)( table, __VA_ARGS__ ) \ + +#define vt_size( table )_Generic( *( table ) VT_GENERIC_SLOTS( vt_table_, vt_size_ ) )( table ) + +#define vt_bucket_count( table ) _Generic( *( table ) VT_GENERIC_SLOTS( vt_table_, vt_bucket_count_ ) )( table ) + +#define vt_is_end( itr ) _Generic( itr VT_GENERIC_SLOTS( vt_table_itr_, vt_is_end_ ) )( itr ) + +#define vt_insert( table, ... ) _Generic( *( table ) VT_GENERIC_SLOTS( vt_table_, vt_insert_ ) )( table, __VA_ARGS__ ) + +#define vt_get_or_insert( table, ... ) _Generic( *( table ) \ + VT_GENERIC_SLOTS( vt_table_, vt_get_or_insert_ ) \ +)( table, __VA_ARGS__ ) \ + +#define vt_get( table, ... ) _Generic( *( table ) VT_GENERIC_SLOTS( vt_table_, vt_get_ ) )( table, __VA_ARGS__ ) + +#define vt_erase( table, ... ) _Generic( *( table ) VT_GENERIC_SLOTS( vt_table_, vt_erase_ ) )( table, __VA_ARGS__ ) + +#define vt_next( itr ) _Generic( itr VT_GENERIC_SLOTS( vt_table_itr_, vt_next_ ) )( itr ) + +#define vt_erase_itr( table, ... ) _Generic( *( table ) \ + VT_GENERIC_SLOTS( vt_table_, vt_erase_itr_ ) \ +)( table, __VA_ARGS__ ) \ + +#define vt_reserve( table, ... ) _Generic( *( table ) VT_GENERIC_SLOTS( vt_table_, vt_reserve_ ) )( table, __VA_ARGS__ ) + +#define vt_shrink( table ) _Generic( *( table ) VT_GENERIC_SLOTS( vt_table_, vt_shrink_ ) )( table ) + +#define vt_first( table ) _Generic( *( table ) VT_GENERIC_SLOTS( vt_table_, vt_first_ ) )( table ) + +#define vt_clear( table ) _Generic( *( table ) VT_GENERIC_SLOTS( vt_table_, vt_clear_ ) )( table ) + +#define vt_cleanup( table ) _Generic( *( table ) VT_GENERIC_SLOTS( vt_table_, vt_cleanup_ ) )( table ) + +#endif + +#endif + +/*--------------------------------------------------------------------------------------------------------------------*/ +/* Prefixed structs */ +/*--------------------------------------------------------------------------------------------------------------------*/ + +#ifndef IMPLEMENTATION_MODE + +typedef struct +{ + KEY_TY key; + #ifdef VAL_TY + VAL_TY val; + #endif +} VT_CAT( NAME, _bucket ); + +typedef struct +{ + VT_CAT( NAME, _bucket ) *data; + uint16_t *metadatum; + uint16_t *metadata_end; // Iterators carry an internal end pointer so that NAME_is_end does not need the table to be + // passed in as an argument. + // This also allows for the zero-bucket-count check to occur once in NAME_first, rather than + // repeatedly in NAME_is_end. + size_t home_bucket; // SIZE_MAX if home bucket is unknown. +} VT_CAT( NAME, _itr ); + +typedef struct +{ + size_t key_count; + size_t buckets_mask; // Rather than storing the bucket count directly, we store the bit mask used to reduce a hash + // code or displacement-derived bucket index to the buckets array, i.e. the bucket count minus + // one. + // Consequently, a zero bucket count (i.e. when .metadata points to the placeholder) constitutes + // a special case, represented by all bits unset (i.e. zero). + VT_CAT( NAME, _bucket ) *buckets; + uint16_t *metadata; // As described above, each metadatum consists of a 4-bit hash-code fragment (X), a 1-bit flag + // indicating whether the key in this bucket begins a chain associated with the bucket (Y), and + // an 11-bit value indicating the quadratic displacement of the next key in the chain (Z): + // XXXXYZZZZZZZZZZZ. + #ifdef CTX_TY + CTX_TY ctx; + #endif +} NAME; + +#endif + +/*--------------------------------------------------------------------------------------------------------------------*/ +/* Function prototypes */ +/*--------------------------------------------------------------------------------------------------------------------*/ + +#if defined( HEADER_MODE ) || defined( IMPLEMENTATION_MODE ) +#define VT_API_FN_QUALIFIERS +#else +#define VT_API_FN_QUALIFIERS static inline +#endif + +#ifndef IMPLEMENTATION_MODE + +VT_API_FN_QUALIFIERS void VT_CAT( NAME, _init )( + NAME * + #ifdef CTX_TY + , CTX_TY + #endif +); + +VT_API_FN_QUALIFIERS bool VT_CAT( NAME, _init_clone )( + NAME *, + const NAME * + #ifdef CTX_TY + , CTX_TY + #endif +); + +VT_API_FN_QUALIFIERS size_t VT_CAT( NAME, _size )( const NAME * ); + +VT_API_FN_QUALIFIERS size_t VT_CAT( NAME, _bucket_count )( const NAME * ); + +VT_API_FN_QUALIFIERS bool VT_CAT( NAME, _is_end )( VT_CAT( NAME, _itr ) ); + +VT_API_FN_QUALIFIERS VT_CAT( NAME, _itr ) VT_CAT( NAME, _insert )( + NAME *, + KEY_TY + #ifdef VAL_TY + , VAL_TY + #endif +); + +VT_API_FN_QUALIFIERS VT_CAT( NAME, _itr ) VT_CAT( NAME, _get_or_insert )( + NAME *, + KEY_TY + #ifdef VAL_TY + , VAL_TY + #endif +); + +VT_API_FN_QUALIFIERS VT_CAT( NAME, _itr ) VT_CAT( NAME, _get )( + const NAME *table, + KEY_TY key +); + +VT_API_FN_QUALIFIERS bool VT_CAT( NAME, _erase )( NAME *, KEY_TY ); + +VT_API_FN_QUALIFIERS VT_CAT( NAME, _itr ) VT_CAT( NAME, _next )( VT_CAT( NAME, _itr ) ); + +VT_API_FN_QUALIFIERS bool VT_CAT( NAME, _reserve )( NAME *, size_t ); + +VT_API_FN_QUALIFIERS bool VT_CAT( NAME, _shrink )( NAME * ); + +VT_API_FN_QUALIFIERS VT_CAT( NAME, _itr ) VT_CAT( NAME, _first )( const NAME * ); + +VT_API_FN_QUALIFIERS void VT_CAT( NAME, _clear )( NAME * ); + +VT_API_FN_QUALIFIERS void VT_CAT( NAME, _cleanup )( NAME * ); + +// Not an API function, but must be prototyped anyway because it is called by the inline NAME_erase_itr below. +VT_API_FN_QUALIFIERS bool VT_CAT( NAME, _erase_itr_raw ) ( NAME *, VT_CAT( NAME, _itr ) ); + +// Erases the key pointed to by itr and returns an iterator to the next key in the table. +// This function must be inlined to ensure that the compiler optimizes away the NAME_fast_forward call if the returned +// iterator is discarded. +#ifdef __GNUC__ +static inline __attribute__((always_inline)) +#elif defined( _MSC_VER ) +static __forceinline +#else +static inline +#endif +VT_CAT( NAME, _itr ) VT_CAT( NAME, _erase_itr )( NAME *table, VT_CAT( NAME, _itr ) itr ) +{ + if( VT_CAT( NAME, _erase_itr_raw )( table, itr ) ) + return VT_CAT( NAME, _next )( itr ); + + return itr; +} + +#endif + +/*--------------------------------------------------------------------------------------------------------------------*/ +/* Function implementations */ +/*--------------------------------------------------------------------------------------------------------------------*/ + +#ifndef HEADER_MODE + +// Default settings. + +#ifndef MAX_LOAD +#define MAX_LOAD 0.9 +#endif + +#ifndef MALLOC_FN +#ifdef CTX_TY +#define MALLOC_FN vt_malloc_with_ctx +#else +#define MALLOC_FN vt_malloc +#endif +#endif + +#ifndef FREE_FN +#ifdef CTX_TY +#define FREE_FN vt_free_with_ctx +#else +#define FREE_FN vt_free +#endif +#endif + +#ifndef HASH_FN +#if defined(__STDC_VERSION__) && __STDC_VERSION__ >= 201112L +#ifdef _MSC_VER // In MSVC, the compound literal in the _Generic triggers a warning about unused local variables at /W4. +#define HASH_FN \ +_Pragma( "warning( push )" ) \ +_Pragma( "warning( disable: 4189 )" ) \ +_Generic( ( KEY_TY ){ 0 }, char *: vt_hash_string, const char*: vt_hash_string, default: vt_hash_integer ) \ +_Pragma( "warning( pop )" ) +#else +#define HASH_FN _Generic( ( KEY_TY ){ 0 }, \ + char *: vt_hash_string, \ + const char*: vt_hash_string, \ + default: vt_hash_integer \ +) +#endif +#else +#error Hash function inference is only available in C11 and later. In C99, you need to define HASH_FN manually to \ +vt_hash_integer, vt_hash_string, or your own custom function with the signature uint64_t ( KEY_TY ). +#endif +#endif + +#ifndef CMPR_FN +#if defined(__STDC_VERSION__) && __STDC_VERSION__ >= 201112L +#ifdef _MSC_VER +#define CMPR_FN \ +_Pragma( "warning( push )" ) \ +_Pragma( "warning( disable: 4189 )" ) \ +_Generic( ( KEY_TY ){ 0 }, char *: vt_cmpr_string, const char*: vt_cmpr_string, default: vt_cmpr_integer ) \ +_Pragma( "warning( pop )" ) +#else +#define CMPR_FN _Generic( ( KEY_TY ){ 0 }, \ + char *: vt_cmpr_string, \ + const char*: vt_cmpr_string, \ + default: vt_cmpr_integer \ +) +#endif +#else +#error Comparison function inference is only available in C11 and later. In C99, you need to define CMPR_FN manually \ +to vt_cmpr_integer, vt_cmpr_string, or your own custom function with the signature bool ( KEY_TY, KEY_TY ). +#endif +#endif + +VT_API_FN_QUALIFIERS void VT_CAT( NAME, _init )( + NAME *table + #ifdef CTX_TY + , CTX_TY ctx + #endif +) +{ + table->key_count = 0; + table->buckets_mask = 0x0000000000000000ull; + table->buckets = NULL; + table->metadata = (uint16_t *)&vt_empty_placeholder_metadatum; + #ifdef CTX_TY + table->ctx = ctx; + #endif +} + +// For efficiency, especially in the case of a small table, the buckets array and metadata share the same dynamic memory +// allocation: +// +-----------------------------+-----+----------------+--------+ +// | Buckets | Pad | Metadata | Excess | +// +-----------------------------+-----+----------------+--------+ +// Any allocated metadata array requires four excess elements to ensure that iteration functions, which read four +// metadata at a time, never read beyond the end of it. +// This function returns the offset of the beginning of the metadata, i.e. the size of the buckets array plus the +// (usually zero) padding. +// It assumes that the bucket count is not zero. +static inline size_t VT_CAT( NAME, _metadata_offset )( NAME *table ) +{ + // Use sizeof, rather than alignof, for C99 compatibility. + return ( ( ( table->buckets_mask + 1 ) * sizeof( VT_CAT( NAME, _bucket ) ) + sizeof( uint16_t ) - 1 ) / + sizeof( uint16_t ) ) * sizeof( uint16_t ); +} + +// Returns the total allocation size, including the buckets array, padding, metadata, and excess metadata. +// As above, this function assumes that the bucket count is not zero. +static inline size_t VT_CAT( NAME, _total_alloc_size )( NAME *table ) +{ + return VT_CAT( NAME, _metadata_offset )( table ) + ( table->buckets_mask + 1 + 4 ) * sizeof( uint16_t ); +} + +VT_API_FN_QUALIFIERS bool VT_CAT( NAME, _init_clone )( + NAME *table, + const NAME *source + #ifdef CTX_TY + , CTX_TY ctx + #endif +) +{ + table->key_count = source->key_count; + table->buckets_mask = source->buckets_mask; + #ifdef CTX_TY + table->ctx = ctx; + #endif + + if( !source->buckets_mask ) + { + table->metadata = (uint16_t *)&vt_empty_placeholder_metadatum; + table->buckets = NULL; + return true; + } + + void *allocation = MALLOC_FN( + VT_CAT( NAME, _total_alloc_size )( table ) + #ifdef CTX_TY + , &table->ctx + #endif + ); + + if( VT_UNLIKELY( !allocation ) ) + return false; + + table->buckets = (VT_CAT( NAME, _bucket ) *)allocation; + table->metadata = (uint16_t *)( (unsigned char *)allocation + VT_CAT( NAME, _metadata_offset )( table ) ); + memcpy( allocation, source->buckets, VT_CAT( NAME, _total_alloc_size )( table ) ); + + return true; +} + +VT_API_FN_QUALIFIERS size_t VT_CAT( NAME, _size )( const NAME *table ) +{ + return table->key_count; +} + +VT_API_FN_QUALIFIERS size_t VT_CAT( NAME, _bucket_count )( const NAME *table ) +{ + // If the bucket count is zero, buckets_mask will be zero, not the bucket count minus one. + // We account for this special case by adding (bool)buckets_mask rather than one. + return table->buckets_mask + (bool)table->buckets_mask; +} + +VT_API_FN_QUALIFIERS bool VT_CAT( NAME, _is_end )( VT_CAT( NAME, _itr ) itr ) +{ + return itr.metadatum == itr.metadata_end; +} + +// Finds the earliest empty bucket in which a key belonging to home_bucket can be placed, assuming that home_bucket +// is already occupied. +// The reason to begin the search at home_bucket, rather than the end of the existing chain, is that keys deleted from +// other chains might have freed up buckets that could fall in this chain before the final key. +// Returns true if an empty bucket within the range of the displacement limit was found, in which case the final two +// pointer arguments contain the index of the empty bucket and its quadratic displacement from home_bucket. +static inline bool VT_CAT( NAME, _find_first_empty )( + NAME *table, + size_t home_bucket, + size_t *empty, + uint16_t *displacement +) +{ + *displacement = 1; + size_t linear_dispacement = 1; + + while( true ) + { + *empty = ( home_bucket + linear_dispacement ) & table->buckets_mask; + if( table->metadata[ *empty ] == VT_EMPTY ) + return true; + + if( VT_UNLIKELY( ++*displacement == VT_DISPLACEMENT_MASK ) ) + return false; + + linear_dispacement += *displacement; + } +} + +// Finds the key in the chain beginning in home_bucket after which to link a new key with displacement_to_empty +// quadratic displacement and returns the index of the bucket containing that key. +// Although the new key could simply be linked to the end of the chain, keeping the chain ordered by displacement +// theoretically improves cache locality during lookups. +static inline size_t VT_CAT( NAME, _find_insert_location_in_chain )( + NAME *table, + size_t home_bucket, + uint16_t displacement_to_empty +) +{ + size_t candidate = home_bucket; + while( true ) + { + uint16_t displacement = table->metadata[ candidate ] & VT_DISPLACEMENT_MASK; + + if( displacement > displacement_to_empty ) + return candidate; + + candidate = ( home_bucket + vt_quadratic( displacement ) ) & table->buckets_mask; + } +} + +// Frees up a bucket occupied by a key not belonging there so that a new key belonging there can be placed there as the +// beginning of a new chain. +// This requires: +// * Finding the previous key in the chain to which the occupying key belongs by rehashing it and then traversing the +// chain. +// * Disconnecting the key from the chain. +// * Finding the appropriate empty bucket to which to move the key. +// * Moving the key (and value) data to the empty bucket. +// * Re-linking the key to the chain. +// Returns true if the eviction succeeded, or false if no empty bucket to which to evict the occupying key could be +// found within the displacement limit. +static inline bool VT_CAT( NAME, _evict )( NAME *table, size_t bucket ) +{ + // Find the previous key in chain. + size_t home_bucket = HASH_FN( table->buckets[ bucket ].key ) & table->buckets_mask; + size_t prev = home_bucket; + while( true ) + { + size_t next = ( home_bucket + vt_quadratic( table->metadata[ prev ] & VT_DISPLACEMENT_MASK ) ) & + table->buckets_mask; + + if( next == bucket ) + break; + + prev = next; + } + + // Disconnect the key from chain. + table->metadata[ prev ] = ( table->metadata[ prev ] & ~VT_DISPLACEMENT_MASK ) | ( table->metadata[ bucket ] & + VT_DISPLACEMENT_MASK ); + + // Find the empty bucket to which to move the key. + size_t empty; + uint16_t displacement; + if( VT_UNLIKELY( !VT_CAT( NAME, _find_first_empty )( table, home_bucket, &empty, &displacement ) ) ) + return false; + + // Find the key in the chain after which to link the moved key. + prev = VT_CAT( NAME, _find_insert_location_in_chain )( table, home_bucket, displacement ); + + // Move the key (and value) data. + table->buckets[ empty ] = table->buckets[ bucket ]; + + // Re-link the key to the chain from its new bucket. + table->metadata[ empty ] = ( table->metadata[ bucket ] & VT_HASH_FRAG_MASK ) | ( table->metadata[ prev ] & + VT_DISPLACEMENT_MASK ); + table->metadata[ prev ] = ( table->metadata[ prev ] & ~VT_DISPLACEMENT_MASK ) | displacement; + + return true; +} + +// Returns an end iterator, i.e. any iterator for which .metadatum == .metadata_end. +// This function just cleans up the library code in functions that return an end iterator as a failure indicator. +static inline VT_CAT( NAME, _itr ) VT_CAT( NAME, _end_itr )( void ) +{ + VT_CAT( NAME, _itr ) itr = { NULL, NULL, NULL, 0 }; + return itr; +} + +// Inserts a key, optionally replacing the existing key if it already exists. +// There are two main cases that must be handled: +// * If the key's home bucket is empty or occupied by a key that does not belong there, then the key is inserted there, +// evicting the occupying key if there is one. +// * Otherwise, the chain of keys beginning at the home bucket is (if unique is false) traversed in search of a matching +// key. +// If none is found, then the new key is inserted at the earliest available bucket, per quadratic probing from the +// home bucket, and then linked to the chain in a manner that maintains its quadratic order. +// The unique argument tells the function whether to skip searching for the key before inserting it (on rehashing, this +// step is unnecessary). +// The replace argument tells the function whether to replace an existing key. +// If replace is true, the function returns an iterator to the inserted key, or an end iterator if the key was not +// inserted because of the maximum load factor or displacement limit constraints. +// If replace is false, then the return value is as described above, except that if the key already exists, the function +// returns an iterator to the existing key. +static inline VT_CAT( NAME, _itr ) VT_CAT( NAME, _insert_raw )( + NAME *table, + KEY_TY key, + #ifdef VAL_TY + VAL_TY *val, + #endif + bool unique, + bool replace +) +{ + uint64_t hash = HASH_FN( key ); + uint16_t hashfrag = vt_hashfrag( hash ); + size_t home_bucket = hash & table->buckets_mask; + + // Case 1: The home bucket is empty or contains a key that doesn't belong there. + // This case also implicitly handles the case of a zero bucket count, since home_bucket will be zero and metadata[ 0 ] + // will be the empty placeholder. + // In that scenario, the zero buckets_mask triggers the below load-factor check. + if( !( table->metadata[ home_bucket ] & VT_IN_HOME_BUCKET_MASK ) ) + { + if( + // Load-factor check. + VT_UNLIKELY( table->key_count + 1 > VT_CAT( NAME, _bucket_count )( table ) * MAX_LOAD ) || + // Vacate the home bucket if it contains a key. + ( table->metadata[ home_bucket ] != VT_EMPTY && VT_UNLIKELY( !VT_CAT( NAME, _evict )( table, home_bucket ) ) ) + ) + return VT_CAT( NAME, _end_itr )(); + + table->buckets[ home_bucket ].key = key; + #ifdef VAL_TY + table->buckets[ home_bucket ].val = *val; + #endif + table->metadata[ home_bucket ] = hashfrag | VT_IN_HOME_BUCKET_MASK | VT_DISPLACEMENT_MASK; + + ++table->key_count; + + VT_CAT( NAME, _itr ) itr = { + table->buckets + home_bucket, + table->metadata + home_bucket, + table->metadata + table->buckets_mask + 1, // Iteration stopper (i.e. the first of the four excess metadata). + home_bucket + }; + return itr; + } + + // Case 2: The home bucket contains the beginning of a chain. + + // Optionally, check the existing chain. + if( !unique ) + { + size_t bucket = home_bucket; + while( true ) + { + if( + ( table->metadata[ bucket ] & VT_HASH_FRAG_MASK ) == hashfrag && + VT_LIKELY( CMPR_FN( table->buckets[ bucket ].key, key ) ) + ) + { + if( replace ) + { + #ifdef KEY_DTOR_FN + KEY_DTOR_FN( table->buckets[ bucket ].key ); + #endif + table->buckets[ bucket ].key = key; + + #ifdef VAL_TY + #ifdef VAL_DTOR_FN + VAL_DTOR_FN( table->buckets[ bucket ].val ); + #endif + table->buckets[ bucket ].val = *val; + #endif + } + + VT_CAT( NAME, _itr ) itr = { + table->buckets + bucket, + table->metadata + bucket, + table->metadata + table->buckets_mask + 1, + home_bucket + }; + return itr; + } + + uint16_t displacement = table->metadata[ bucket ] & VT_DISPLACEMENT_MASK; + if( displacement == VT_DISPLACEMENT_MASK ) + break; + + bucket = ( home_bucket + vt_quadratic( displacement ) ) & table->buckets_mask; + } + } + + size_t empty; + uint16_t displacement; + if( + VT_UNLIKELY( + // Load-factor check. + table->key_count + 1 > VT_CAT( NAME, _bucket_count )( table ) * MAX_LOAD || + // Find the earliest empty bucket, per quadratic probing. + !VT_CAT( NAME, _find_first_empty )( table, home_bucket, &empty, &displacement ) + ) + ) + return VT_CAT( NAME, _end_itr )(); + + // Insert the new key (and value) in the empty bucket and link it to the chain. + + size_t prev = VT_CAT( NAME, _find_insert_location_in_chain )( table, home_bucket, displacement ); + + table->buckets[ empty ].key = key; + #ifdef VAL_TY + table->buckets[ empty ].val = *val; + #endif + table->metadata[ empty ] = hashfrag | ( table->metadata[ prev ] & VT_DISPLACEMENT_MASK ); + table->metadata[ prev ] = ( table->metadata[ prev ] & ~VT_DISPLACEMENT_MASK ) | displacement; + + ++table->key_count; + + VT_CAT( NAME, _itr ) itr = { + table->buckets + empty, + table->metadata + empty, + table->metadata + table->buckets_mask + 1, + home_bucket + }; + return itr; +} + +// Resizes the bucket array. +// This function assumes that bucket_count is a power of two and large enough to accommodate all keys without violating +// the maximum load factor. +// Returns false in the case of allocation failure. +// As this function is called very rarely in _insert and _get_or_insert, ideally it should not be inlined into those +// functions. +// In testing, the no-inline approach showed a performance benefit when inserting existing keys (i.e. replacing). +#ifdef __GNUC__ +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wattributes" // Silence warning about combining noinline with static inline. +__attribute__((noinline)) static inline +#elif defined( _MSC_VER ) +__declspec(noinline) static inline +#else +static inline +#endif +bool VT_CAT( NAME, _rehash )( NAME *table, size_t bucket_count ) +{ + // The attempt to resize the bucket array and rehash the keys must occur inside a loop that incrementally doubles the + // target bucket count because a failure could theoretically occur at any load factor due to the displacement limit. + while( true ) + { + NAME new_table = { + 0, + bucket_count - 1, + NULL, + NULL + #ifdef CTX_TY + , table->ctx + #endif + }; + + void *allocation = MALLOC_FN( + VT_CAT( NAME, _total_alloc_size )( &new_table ) + #ifdef CTX_TY + , &new_table.ctx + #endif + ); + + if( VT_UNLIKELY( !allocation ) ) + return false; + + new_table.buckets = (VT_CAT( NAME, _bucket ) *)allocation; + new_table.metadata = (uint16_t *)( (unsigned char *)allocation + VT_CAT( NAME, _metadata_offset )( &new_table ) ); + + memset( new_table.metadata, 0x00, ( bucket_count + 4 ) * sizeof( uint16_t ) ); + + // Iteration stopper at the end of the actual metadata array (i.e. the first of the four excess metadata). + new_table.metadata[ bucket_count ] = 0x01; + + for( size_t bucket = 0; bucket < VT_CAT( NAME, _bucket_count )( table ); ++bucket ) + if( table->metadata[ bucket ] != VT_EMPTY ) + { + VT_CAT( NAME, _itr ) itr = VT_CAT( NAME, _insert_raw )( + &new_table, + table->buckets[ bucket ].key, + #ifdef VAL_TY + &table->buckets[ bucket ].val, + #endif + true, + false + ); + + if( VT_UNLIKELY( VT_CAT( NAME, _is_end )( itr ) ) ) + break; + } + + // If a key could not be reinserted due to the displacement limit, double the bucket count and retry. + if( VT_UNLIKELY( new_table.key_count < table->key_count ) ) + { + FREE_FN( + new_table.buckets, + VT_CAT( NAME, _total_alloc_size )( &new_table ) + #ifdef CTX_TY + , &new_table.ctx + #endif + ); + + bucket_count *= 2; + continue; + } + + if( table->buckets_mask ) + FREE_FN( + table->buckets, + VT_CAT( NAME, _total_alloc_size )( table ) + #ifdef CTX_TY + , &table->ctx + #endif + ); + + *table = new_table; + return true; + } +} +#ifdef __GNUC__ +#pragma GCC diagnostic pop +#endif + +// Inserts a key, replacing the existing key if it already exists. +// This function wraps insert_raw in a loop that handles growing and rehashing the table if a new key cannot be inserted +// because of the maximum load factor or displacement limit constraints. +// Returns an iterator to the inserted key, or an end iterator in the case of allocation failure. +VT_API_FN_QUALIFIERS VT_CAT( NAME, _itr ) VT_CAT( NAME, _insert )( + NAME *table, + KEY_TY key + #ifdef VAL_TY + , VAL_TY val + #endif +) +{ + while( true ) + { + VT_CAT( NAME, _itr ) itr = VT_CAT( NAME, _insert_raw )( + table, + key, + #ifdef VAL_TY + &val, + #endif + false, + true + ); + + if( + // Lookup succeeded, in which case itr points to the found key. + VT_LIKELY( !VT_CAT( NAME, _is_end )( itr ) ) || + // Lookup failed and rehash also fails, in which case itr is an end iterator. + VT_UNLIKELY( + !VT_CAT( NAME, _rehash )( + table, table->buckets_mask ? VT_CAT( NAME, _bucket_count )( table ) * 2 : VT_MIN_NONZERO_BUCKET_COUNT + ) + ) + ) + return itr; + } +} + +// Same as NAME_insert, except that if the key already exists, no insertion occurs and the function returns an iterator +// to the existing key. +VT_API_FN_QUALIFIERS VT_CAT( NAME, _itr ) VT_CAT( NAME, _get_or_insert )( + NAME *table, + KEY_TY key + #ifdef VAL_TY + , VAL_TY val + #endif +) +{ + while( true ) + { + VT_CAT( NAME, _itr ) itr = VT_CAT( NAME, _insert_raw )( + table, + key, + #ifdef VAL_TY + &val, + #endif + false, + false + ); + + if( + // Lookup succeeded, in which case itr points to the found key. + VT_LIKELY( !VT_CAT( NAME, _is_end )( itr ) ) || + // Lookup failed and rehash also fails, in which case itr is an end iterator. + VT_UNLIKELY( + !VT_CAT( NAME, _rehash )( + table, table->buckets_mask ? VT_CAT( NAME, _bucket_count )( table ) * 2 : VT_MIN_NONZERO_BUCKET_COUNT + ) + ) + ) + return itr; + } +} + +// Returns an iterator pointing to the specified key, or an end iterator if the key does not exist. +VT_API_FN_QUALIFIERS VT_CAT( NAME, _itr ) VT_CAT( NAME, _get )( const NAME *table, KEY_TY key ) +{ + uint64_t hash = HASH_FN( key ); + size_t home_bucket = hash & table->buckets_mask; + + // If the home bucket is empty or contains a key that does not belong there, then our key does not exist. + // This check also implicitly handles the case of a zero bucket count, since home_bucket will be zero and + // metadata[ 0 ] will be the empty placeholder. + if( !( table->metadata[ home_bucket ] & VT_IN_HOME_BUCKET_MASK ) ) + return VT_CAT( NAME, _end_itr )(); + + // Traverse the chain of keys belonging to the home bucket. + uint16_t hashfrag = vt_hashfrag( hash ); + size_t bucket = home_bucket; + while( true ) + { + if( + ( table->metadata[ bucket ] & VT_HASH_FRAG_MASK ) == hashfrag && + VT_LIKELY( CMPR_FN( table->buckets[ bucket ].key, key ) ) + ) + { + VT_CAT( NAME, _itr ) itr = { + table->buckets + bucket, + table->metadata + bucket, + table->metadata + table->buckets_mask + 1, + home_bucket + }; + return itr; + } + + uint16_t displacement = table->metadata[ bucket ] & VT_DISPLACEMENT_MASK; + if( displacement == VT_DISPLACEMENT_MASK ) + return VT_CAT( NAME, _end_itr )(); + + bucket = ( home_bucket + vt_quadratic( displacement ) ) & table->buckets_mask; + } +} + +// Erases the key pointed to by the specified iterator. +// The erasure always occurs at the end of the chain to which the key belongs. +// If the key to be erased is not the last in the chain, it is swapped with the last so that erasure occurs at the end. +// This helps keep a chain's keys close to their home bucket for the sake of cache locality. +// Returns true if, in the case of iteration from first to end, NAME_next should now be called on the iterator to find +// the next key. +// This return value is necessary because at the iterator location, the erasure could result in an empty bucket, a +// bucket containing a moved key already visited during the iteration, or a bucket containing a moved key not yet +// visited. +VT_API_FN_QUALIFIERS bool VT_CAT( NAME, _erase_itr_raw )( NAME *table, VT_CAT( NAME, _itr ) itr ) +{ + --table->key_count; + size_t itr_bucket = itr.metadatum - table->metadata; + + // For now, we only call the value's destructor because the key may need to be hashed below to determine the home + // bucket. + #ifdef VAL_DTOR_FN + VAL_DTOR_FN( table->buckets[ itr_bucket ].val ); + #endif + + // Case 1: The key is the only one in its chain, so just remove it. + if( + table->metadata[ itr_bucket ] & VT_IN_HOME_BUCKET_MASK && + ( table->metadata[ itr_bucket ] & VT_DISPLACEMENT_MASK ) == VT_DISPLACEMENT_MASK + ) + { + #ifdef KEY_DTOR_FN + KEY_DTOR_FN( table->buckets[ itr_bucket ].key ); + #endif + table->metadata[ itr_bucket ] = VT_EMPTY; + return true; + } + + // Case 2 and 3 require that we know the key's home bucket, which the iterator may not have recorded. + if( itr.home_bucket == SIZE_MAX ) + { + if( table->metadata[ itr_bucket ] & VT_IN_HOME_BUCKET_MASK ) + itr.home_bucket = itr_bucket; + else + itr.home_bucket = HASH_FN( table->buckets[ itr_bucket ].key ) & table->buckets_mask; + } + + // The key can now be safely destructed for cases 2 and 3. + #ifdef KEY_DTOR_FN + KEY_DTOR_FN( table->buckets[ itr_bucket ].key ); + #endif + + // Case 2: The key is the last in a multi-key chain. + // Traverse the chain from the beginning and find the penultimate key. + // Then disconnect the key and erase. + if( ( table->metadata[ itr_bucket ] & VT_DISPLACEMENT_MASK ) == VT_DISPLACEMENT_MASK ) + { + size_t bucket = itr.home_bucket; + while( true ) + { + uint16_t displacement = table->metadata[ bucket ] & VT_DISPLACEMENT_MASK; + size_t next = ( itr.home_bucket + vt_quadratic( displacement ) ) & table->buckets_mask; + if( next == itr_bucket ) + { + table->metadata[ bucket ] |= VT_DISPLACEMENT_MASK; + table->metadata[ itr_bucket ] = VT_EMPTY; + return true; + } + + bucket = next; + } + } + + // Case 3: The chain has multiple keys, and the key is not the last one. + // Traverse the chain from the key to be erased and find the last and penultimate keys. + // Disconnect the last key from the chain, and swap it with the key to erase. + size_t bucket = itr_bucket; + while( true ) + { + size_t prev = bucket; + bucket = ( itr.home_bucket + vt_quadratic( table->metadata[ bucket ] & VT_DISPLACEMENT_MASK ) ) & + table->buckets_mask; + + if( ( table->metadata[ bucket ] & VT_DISPLACEMENT_MASK ) == VT_DISPLACEMENT_MASK ) + { + table->buckets[ itr_bucket ] = table->buckets[ bucket ]; + + table->metadata[ itr_bucket ] = ( table->metadata[ itr_bucket ] & ~VT_HASH_FRAG_MASK ) | ( + table->metadata[ bucket ] & VT_HASH_FRAG_MASK ); + + table->metadata[ prev ] |= VT_DISPLACEMENT_MASK; + table->metadata[ bucket ] = VT_EMPTY; + + // Whether the iterator should be advanced depends on whether the key moved to the iterator bucket came from + // before or after that bucket. + // In the former case, the iteration would already have hit the moved key, so the iterator should still be + // advanced. + if( bucket > itr_bucket ) + return false; + + return true; + } + } +} + +// Erases the specified key, if it exists. +// Returns true if a key was erased. +VT_API_FN_QUALIFIERS bool VT_CAT( NAME, _erase )( NAME *table, KEY_TY key ) +{ + VT_CAT( NAME, _itr ) itr = VT_CAT( NAME, _get)( table, key ); + if( VT_CAT( NAME, _is_end )( itr ) ) + return false; + + VT_CAT( NAME, _erase_itr_raw )( table, itr ); + return true; +} + +// Finds the first occupied bucket at or after the bucket pointed to by itr. +// This function scans four buckets at a time, ideally using intrinsics. +static inline void VT_CAT( NAME, _fast_forward )( VT_CAT( NAME, _itr ) *itr ) +{ + while( true ) + { + uint64_t metadata; + memcpy( &metadata, itr->metadatum, sizeof( uint64_t ) ); + if( metadata ) + { + int offset = vt_first_nonzero_uint16( metadata ); + itr->data += offset; + itr->metadatum += offset; + itr->home_bucket = SIZE_MAX; + return; + } + + itr->data += 4; + itr->metadatum += 4; + } +} + +VT_API_FN_QUALIFIERS VT_CAT( NAME, _itr ) VT_CAT( NAME, _next )( VT_CAT( NAME, _itr ) itr ) +{ + ++itr.data; + ++itr.metadatum; + VT_CAT( NAME, _fast_forward )( &itr ); + return itr; +} + +// Returns the minimum bucket count required to accommodate a certain number of keys, which is governed by the maximum +// load factor. +static inline size_t VT_CAT( NAME, _min_bucket_count_for_size )( size_t size ) +{ + if( size == 0 ) + return 0; + + // Round up to a power of two. + size_t bucket_count = VT_MIN_NONZERO_BUCKET_COUNT; + while( size > bucket_count * MAX_LOAD ) + bucket_count *= 2; + + return bucket_count; +} + +VT_API_FN_QUALIFIERS bool VT_CAT( NAME, _reserve )( NAME *table, size_t size ) +{ + size_t bucket_count = VT_CAT( NAME, _min_bucket_count_for_size )( size ); + + if( bucket_count <= VT_CAT( NAME, _bucket_count )( table ) ) + return true; + + return VT_CAT( NAME, _rehash )( table, bucket_count ); +} + +VT_API_FN_QUALIFIERS bool VT_CAT( NAME, _shrink )( NAME *table ) +{ + size_t bucket_count = VT_CAT( NAME, _min_bucket_count_for_size )( table->key_count ); + + if( bucket_count == VT_CAT( NAME, _bucket_count )( table ) ) // Shrink unnecessary. + return true; + + if( bucket_count == 0 ) + { + FREE_FN( + table->buckets, + VT_CAT( NAME, _total_alloc_size )( table ) + #ifdef CTX_TY + , &table->ctx + #endif + ); + + table->buckets_mask = 0x0000000000000000ull; + table->metadata = (uint16_t *)&vt_empty_placeholder_metadatum; + return true; + } + + return VT_CAT( NAME, _rehash )( table, bucket_count ); +} + +VT_API_FN_QUALIFIERS VT_CAT( NAME, _itr ) VT_CAT( NAME, _first )( const NAME *table ) +{ + if( !table->key_count ) + return VT_CAT( NAME, _end_itr )(); + + VT_CAT( NAME, _itr ) itr = { table->buckets, table->metadata, table->metadata + table->buckets_mask + 1, SIZE_MAX }; + VT_CAT( NAME, _fast_forward )( &itr ); + return itr; +} + +VT_API_FN_QUALIFIERS void VT_CAT( NAME, _clear )( NAME *table ) +{ + if( !table->key_count ) + return; + + for( size_t i = 0; i < VT_CAT( NAME, _bucket_count )( table ); ++i ) + { + if( table->metadata[ i ] != VT_EMPTY ) + { + #ifdef KEY_DTOR_FN + KEY_DTOR_FN( table->buckets[ i ].key ); + #endif + #ifdef VAL_DTOR_FN + VAL_DTOR_FN( table->buckets[ i ].val ); + #endif + } + + table->metadata[ i ] = VT_EMPTY; + } + + table->key_count = 0; +} + +VT_API_FN_QUALIFIERS void VT_CAT( NAME, _cleanup )( NAME *table ) +{ + if( !table->buckets_mask ) + return; + + #if defined( KEY_DTOR_FN ) || defined( VAL_DTOR_FN ) + VT_CAT( NAME, _clear )( table ); + #endif + + FREE_FN( + table->buckets, + VT_CAT( NAME, _total_alloc_size )( table ) + #ifdef CTX_TY + , &table->ctx + #endif + ); + + VT_CAT( NAME, _init )( + table + #ifdef CTX_TY + , table->ctx + #endif + ); +} + +#endif + +/*--------------------------------------------------------------------------------------------------------------------*/ +/* Wrapper types and functions for the C11 generic API */ +/*--------------------------------------------------------------------------------------------------------------------*/ + +#if defined(__STDC_VERSION__) && \ + __STDC_VERSION__ >= 201112L && \ + !defined( IMPLEMENTATION_MODE ) && \ + !defined( VT_NO_C11_GENERIC_API ) \ + +typedef NAME VT_CAT( vt_table_, VT_TEMPLATE_COUNT ); +typedef VT_CAT( NAME, _itr ) VT_CAT( vt_table_itr_, VT_TEMPLATE_COUNT ); + +static inline void VT_CAT( vt_init_, VT_TEMPLATE_COUNT )( + NAME *table + #ifdef CTX_TY + , CTX_TY ctx + #endif +) +{ + VT_CAT( NAME, _init )( + table + #ifdef CTX_TY + , ctx + #endif + ); +} + +static inline bool VT_CAT( vt_init_clone_, VT_TEMPLATE_COUNT )( + NAME *table, + const NAME* source + #ifdef CTX_TY + , CTX_TY ctx + #endif +) +{ + return VT_CAT( NAME, _init_clone )( + table, + source + #ifdef CTX_TY + , ctx + #endif + ); +} + +static inline size_t VT_CAT( vt_size_, VT_TEMPLATE_COUNT )( const NAME *table ) +{ + return VT_CAT( NAME, _size )( table ); +} + +static inline size_t VT_CAT( vt_bucket_count_, VT_TEMPLATE_COUNT )( const NAME *table ) +{ + return VT_CAT( NAME, _bucket_count )( table ); +} + +static inline bool VT_CAT( vt_is_end_, VT_TEMPLATE_COUNT )( VT_CAT( NAME, _itr ) itr ) +{ + return VT_CAT( NAME, _is_end )( itr ); +} + +static inline VT_CAT( NAME, _itr ) VT_CAT( vt_insert_, VT_TEMPLATE_COUNT )( + NAME *table, + KEY_TY key + #ifdef VAL_TY + , VAL_TY val + #endif +) +{ + return VT_CAT( NAME, _insert )( + table, + key + #ifdef VAL_TY + , val + #endif + ); +} + +static inline VT_CAT( NAME, _itr ) VT_CAT( vt_get_or_insert_, VT_TEMPLATE_COUNT )( + NAME *table, + KEY_TY key + #ifdef VAL_TY + , VAL_TY val + #endif +) +{ + return VT_CAT( NAME, _get_or_insert )( + table, + key + #ifdef VAL_TY + , val + #endif + ); +} + +static inline VT_CAT( NAME, _itr ) VT_CAT( vt_get_, VT_TEMPLATE_COUNT )( const NAME *table, KEY_TY key ) +{ + return VT_CAT( NAME, _get )( table, key ); +} + +static inline bool VT_CAT( vt_erase_, VT_TEMPLATE_COUNT )( NAME *table, KEY_TY key ) +{ + return VT_CAT( NAME, _erase )( table, key ); +} + +static inline VT_CAT( NAME, _itr ) VT_CAT( vt_next_, VT_TEMPLATE_COUNT )( VT_CAT( NAME, _itr ) itr ) +{ + return VT_CAT( NAME, _next )( itr ); +} + +static inline VT_CAT( NAME, _itr ) VT_CAT( vt_erase_itr_, VT_TEMPLATE_COUNT )( NAME *table, VT_CAT( NAME, _itr ) itr ) +{ + return VT_CAT( NAME, _erase_itr )( table, itr ); +} + +static inline bool VT_CAT( vt_reserve_, VT_TEMPLATE_COUNT )( NAME *table, size_t bucket_count ) +{ + return VT_CAT( NAME, _reserve )( table, bucket_count ); +} + +static inline bool VT_CAT( vt_shrink_, VT_TEMPLATE_COUNT )( NAME *table ) +{ + return VT_CAT( NAME, _shrink )( table ); +} + +static inline VT_CAT( NAME, _itr ) VT_CAT( vt_first_, VT_TEMPLATE_COUNT )( const NAME *table ) +{ + return VT_CAT( NAME, _first )( table ); +} + +static inline void VT_CAT( vt_clear_, VT_TEMPLATE_COUNT )( NAME *table ) +{ + VT_CAT( NAME, _clear )( table ); +} + +static inline void VT_CAT( vt_cleanup_, VT_TEMPLATE_COUNT )( NAME *table ) +{ + VT_CAT( NAME, _cleanup )( table ); +} + +// Increment the template counter. +#if VT_TEMPLATE_COUNT_D1 == 0 +#undef VT_TEMPLATE_COUNT_D1 +#define VT_TEMPLATE_COUNT_D1 1 +#elif VT_TEMPLATE_COUNT_D1 == 1 +#undef VT_TEMPLATE_COUNT_D1 +#define VT_TEMPLATE_COUNT_D1 2 +#elif VT_TEMPLATE_COUNT_D1 == 2 +#undef VT_TEMPLATE_COUNT_D1 +#define VT_TEMPLATE_COUNT_D1 3 +#elif VT_TEMPLATE_COUNT_D1 == 3 +#undef VT_TEMPLATE_COUNT_D1 +#define VT_TEMPLATE_COUNT_D1 4 +#elif VT_TEMPLATE_COUNT_D1 == 4 +#undef VT_TEMPLATE_COUNT_D1 +#define VT_TEMPLATE_COUNT_D1 5 +#elif VT_TEMPLATE_COUNT_D1 == 5 +#undef VT_TEMPLATE_COUNT_D1 +#define VT_TEMPLATE_COUNT_D1 6 +#elif VT_TEMPLATE_COUNT_D1 == 6 +#undef VT_TEMPLATE_COUNT_D1 +#define VT_TEMPLATE_COUNT_D1 7 +#elif VT_TEMPLATE_COUNT_D1 == 7 +#undef VT_TEMPLATE_COUNT_D1 +#define VT_TEMPLATE_COUNT_D1 0 +#if VT_TEMPLATE_COUNT_D2 == 0 +#undef VT_TEMPLATE_COUNT_D2 +#define VT_TEMPLATE_COUNT_D2 1 +#elif VT_TEMPLATE_COUNT_D2 == 1 +#undef VT_TEMPLATE_COUNT_D2 +#define VT_TEMPLATE_COUNT_D2 2 +#elif VT_TEMPLATE_COUNT_D2 == 2 +#undef VT_TEMPLATE_COUNT_D2 +#define VT_TEMPLATE_COUNT_D2 3 +#elif VT_TEMPLATE_COUNT_D2 == 3 +#undef VT_TEMPLATE_COUNT_D2 +#define VT_TEMPLATE_COUNT_D2 4 +#elif VT_TEMPLATE_COUNT_D2 == 4 +#undef VT_TEMPLATE_COUNT_D2 +#define VT_TEMPLATE_COUNT_D2 5 +#elif VT_TEMPLATE_COUNT_D2 == 5 +#undef VT_TEMPLATE_COUNT_D2 +#define VT_TEMPLATE_COUNT_D2 6 +#elif VT_TEMPLATE_COUNT_D2 == 6 +#undef VT_TEMPLATE_COUNT_D2 +#define VT_TEMPLATE_COUNT_D2 7 +#elif VT_TEMPLATE_COUNT_D2 == 7 +#undef VT_TEMPLATE_COUNT_D2 +#define VT_TEMPLATE_COUNT_D2 0 +#if VT_TEMPLATE_COUNT_D3 == 0 +#undef VT_TEMPLATE_COUNT_D3 +#define VT_TEMPLATE_COUNT_D3 1 +#elif VT_TEMPLATE_COUNT_D3 == 1 +#undef VT_TEMPLATE_COUNT_D3 +#define VT_TEMPLATE_COUNT_D3 2 +#elif VT_TEMPLATE_COUNT_D3 == 2 +#undef VT_TEMPLATE_COUNT_D3 +#define VT_TEMPLATE_COUNT_D3 3 +#elif VT_TEMPLATE_COUNT_D3 == 3 +#undef VT_TEMPLATE_COUNT_D3 +#define VT_TEMPLATE_COUNT_D3 4 +#elif VT_TEMPLATE_COUNT_D3 == 4 +#undef VT_TEMPLATE_COUNT_D3 +#define VT_TEMPLATE_COUNT_D3 5 +#elif VT_TEMPLATE_COUNT_D3 == 5 +#undef VT_TEMPLATE_COUNT_D3 +#define VT_TEMPLATE_COUNT_D3 6 +#elif VT_TEMPLATE_COUNT_D3 == 6 +#undef VT_TEMPLATE_COUNT_D3 +#define VT_TEMPLATE_COUNT_D3 7 +#elif VT_TEMPLATE_COUNT_D3 == 7 +#error Sorry, the number of template instances is limited to 511. Define VT_NO_C11_GENERIC_API globally and use the \ +C99 prefixed function API to circumvent this restriction. +#endif +#endif +#endif + +#endif + +#undef NAME +#undef KEY_TY +#undef VAL_TY +#undef HASH_FN +#undef CMPR_FN +#undef MAX_LOAD +#undef KEY_DTOR_FN +#undef VAL_DTOR_FN +#undef CTX_TY +#undef MALLOC_FN +#undef FREE_FN +#undef HEADER_MODE +#undef IMPLEMENTATION_MODE +#undef VT_API_FN_QUALIFIERS diff --git a/vendor/box3d/src/src/weld_joint.c b/vendor/box3d/src/src/weld_joint.c new file mode 100644 index 000000000..611b714fa --- /dev/null +++ b/vendor/box3d/src/src/weld_joint.c @@ -0,0 +1,314 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +#include "body.h" +#include "joint.h" +#include "physics_world.h" +#include "solver.h" +#include "solver_set.h" +#include "recording.h" + +// needed for dll export +#include "box3d/box3d.h" + +void b3WeldJoint_SetLinearHertz( b3JointId jointId, float hertz ) +{ + B3_ASSERT( b3IsValidFloat( hertz ) && hertz >= 0.0f ); + b3World* world = b3GetWorld( jointId.world0 ); + B3_REC( world, WeldJointSetLinearHertz, jointId, hertz ); + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_weldJoint ); + base->weldJoint.linearHertz = hertz; +} + +float b3WeldJoint_GetLinearHertz( b3JointId jointId ) +{ + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_weldJoint ); + return base->weldJoint.linearHertz; +} + +void b3WeldJoint_SetLinearDampingRatio( b3JointId jointId, float dampingRatio ) +{ + B3_ASSERT( b3IsValidFloat( dampingRatio ) && dampingRatio >= 0.0f ); + b3World* world = b3GetWorld( jointId.world0 ); + B3_REC( world, WeldJointSetLinearDampingRatio, jointId, dampingRatio ); + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_weldJoint ); + base->weldJoint.linearDampingRatio = dampingRatio; +} + +float b3WeldJoint_GetLinearDampingRatio( b3JointId jointId ) +{ + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_weldJoint ); + return base->weldJoint.linearDampingRatio; +} + +void b3WeldJoint_SetAngularHertz( b3JointId jointId, float hertz ) +{ + B3_ASSERT( b3IsValidFloat( hertz ) && hertz >= 0.0f ); + b3World* world = b3GetWorld( jointId.world0 ); + B3_REC( world, WeldJointSetAngularHertz, jointId, hertz ); + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_weldJoint ); + base->weldJoint.angularHertz = hertz; +} + +float b3WeldJoint_GetAngularHertz( b3JointId jointId ) +{ + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_weldJoint ); + return base->weldJoint.angularHertz; +} + +void b3WeldJoint_SetAngularDampingRatio( b3JointId jointId, float dampingRatio ) +{ + B3_ASSERT( b3IsValidFloat( dampingRatio ) && dampingRatio >= 0.0f ); + b3World* world = b3GetWorld( jointId.world0 ); + B3_REC( world, WeldJointSetAngularDampingRatio, jointId, dampingRatio ); + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_weldJoint ); + base->weldJoint.angularDampingRatio = dampingRatio; +} + +float b3WeldJoint_GetAngularDampingRatio( b3JointId jointId ) +{ + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_weldJoint ); + return base->weldJoint.angularDampingRatio; +} + +b3Vec3 b3GetWeldJointForce( b3World* world, b3JointSim* base ) +{ + b3Vec3 force = b3MulSV( world->inv_h, base->weldJoint.linearImpulse ); + return force; +} + +b3Vec3 b3GetWeldJointTorque( b3World* world, b3JointSim* base ) +{ + return b3MulSV( world->inv_h, base->weldJoint.angularImpulse ); +} + +void b3PrepareWeldJoint( b3JointSim* base, b3StepContext* context ) +{ + B3_ASSERT( base->type == b3_weldJoint ); + + b3World* world = context->world; + + b3Body* bodyA = b3Array_Get( world->bodies, base->bodyIdA ); + b3Body* bodyB = b3Array_Get( world->bodies, base->bodyIdB ); + + B3_ASSERT( bodyB->setIndex == b3_awakeSet ); + b3SolverSet* setA = b3Array_Get( world->solverSets, bodyA->setIndex ); + b3SolverSet* setB = b3Array_Get( world->solverSets, bodyB->setIndex ); + + int localIndexA = bodyA->localIndex; + int localIndexB = bodyB->localIndex; + + b3BodySim* bodySimA = b3Array_Get( setA->bodySims, localIndexA ); + b3BodySim* bodySimB = b3Array_Get( setB->bodySims, localIndexB ); + + base->invMassA = bodySimA->invMass; + base->invMassB = bodySimB->invMass; + base->invIA = bodySimA->invInertiaWorld; + base->invIB = bodySimB->invInertiaWorld; + + b3Matrix3 invInertiaSum = b3AddMM( base->invIA, base->invIB ); + base->fixedRotation = b3Det( invInertiaSum ) < 1000.0f * FLT_MIN; + + b3WeldJoint* joint = &base->weldJoint; + joint->indexA = bodyA->setIndex == b3_awakeSet ? localIndexA : B3_NULL_INDEX; + joint->indexB = bodyB->setIndex == b3_awakeSet ? localIndexB : B3_NULL_INDEX; + + // Compute joint anchor frames with world space rotation, relative to center of mass + joint->frameA.q = b3MulQuat( bodySimA->transform.q, base->localFrameA.q ); + joint->frameA.p = b3RotateVector( bodySimA->transform.q, b3Sub( base->localFrameA.p, bodySimA->localCenter ) ); + joint->frameB.q = b3MulQuat( bodySimB->transform.q, base->localFrameB.q ); + joint->frameB.p = b3RotateVector( bodySimB->transform.q, b3Sub( base->localFrameB.p, bodySimB->localCenter ) ); + + joint->deltaCenter = b3SubPos( bodySimB->center, bodySimA->center ); + joint->angularMass = b3InvertMatrix( invInertiaSum ); + + if ( joint->linearHertz == 0.0f ) + { + joint->linearSpring = base->constraintSoftness; + } + else + { + joint->linearSpring = b3MakeSoft( joint->linearHertz, joint->linearDampingRatio, context->h ); + } + + if ( joint->angularHertz == 0.0f ) + { + joint->angularSpring = base->constraintSoftness; + } + else + { + joint->angularSpring = b3MakeSoft( joint->angularHertz, joint->angularDampingRatio, context->h ); + } + + if ( context->enableWarmStarting == false ) + { + joint->linearImpulse = b3Vec3_zero; + joint->angularImpulse = b3Vec3_zero; + } +} + +void b3WarmStartWeldJoint( b3JointSim* base, b3StepContext* context ) +{ + B3_ASSERT( base->type == b3_weldJoint ); + + float mA = base->invMassA; + float mB = base->invMassB; + b3Matrix3 iA = base->invIA; + b3Matrix3 iB = base->invIB; + + // dummy state for static bodies + b3BodyState dummyState = b3_identityBodyState; + + b3WeldJoint* joint = &base->weldJoint; + + b3BodyState* stateA = joint->indexA == B3_NULL_INDEX ? &dummyState : context->states + joint->indexA; + b3BodyState* stateB = joint->indexB == B3_NULL_INDEX ? &dummyState : context->states + joint->indexB; + + b3Vec3 vA = stateA->linearVelocity; + b3Vec3 wA = stateA->angularVelocity; + b3Vec3 vB = stateB->linearVelocity; + b3Vec3 wB = stateB->angularVelocity; + + b3Vec3 rA = b3RotateVector( stateA->deltaRotation, joint->frameA.p ); + b3Vec3 rB = b3RotateVector( stateB->deltaRotation, joint->frameB.p ); + + vA = b3MulSub( vA, mA, joint->linearImpulse ); + wA = b3Sub( wA, b3MulMV( iA, b3Add( b3Cross( rA, joint->linearImpulse ), joint->angularImpulse ) ) ); + + vB = b3MulAdd( vB, mB, joint->linearImpulse ); + wB = b3Add( wB, b3MulMV( iB, b3Add( b3Cross( rB, joint->linearImpulse ), joint->angularImpulse ) ) ); + + if ( stateA->flags & b3_dynamicFlag ) + { + stateA->linearVelocity = vA; + stateA->angularVelocity = wA; + } + + if ( stateB->flags & b3_dynamicFlag ) + { + stateB->linearVelocity = vB; + stateB->angularVelocity = wB; + } +} + +void b3SolveWeldJoint( b3JointSim* base, b3StepContext* context, bool useBias ) +{ + float mA = base->invMassA; + float mB = base->invMassB; + b3Matrix3 iA = base->invIA; + b3Matrix3 iB = base->invIB; + + // dummy state for static bodies + b3BodyState dummyState = b3_identityBodyState; + + b3WeldJoint* joint = &base->weldJoint; + b3BodyState* stateA = joint->indexA == B3_NULL_INDEX ? &dummyState : context->states + joint->indexA; + b3BodyState* stateB = joint->indexB == B3_NULL_INDEX ? &dummyState : context->states + joint->indexB; + + b3Vec3 vA = stateA->linearVelocity; + b3Vec3 wA = stateA->angularVelocity; + b3Vec3 vB = stateB->linearVelocity; + b3Vec3 wB = stateB->angularVelocity; + + bool fixedRotation = base->fixedRotation; + b3Quat quatA = b3MulQuat( stateA->deltaRotation, joint->frameA.q ); + b3Quat quatB = b3MulQuat( stateB->deltaRotation, joint->frameB.q ); + + if ( b3DotQuat( quatA, quatB ) < 0.0f ) + { + // this keeps the rotation angle in the range [-pi, pi] + quatB = b3NegateQuat( quatB ); + } + + b3Quat relQ = b3InvMulQuat( quatA, quatB ); + + // angular constraint + if ( fixedRotation == false ) + { + b3Vec3 bias = b3Vec3_zero; + float massScale = 1.0f; + float impulseScale = 0.0f; + if ( useBias || joint->angularHertz > 0.0f ) + { + b3Quat targetQuat = b3Quat_identity; + b3Vec3 deltaRotation = b3DeltaQuatToRotation( relQ, targetQuat ); + b3Vec3 c = b3Neg( b3RotateVector( quatA, deltaRotation ) ); + + bias = b3MulSV( joint->angularSpring.biasRate, c ); + massScale = joint->angularSpring.massScale; + impulseScale = joint->angularSpring.impulseScale; + } + + b3Vec3 cdot = b3Sub( wB, wA ); + b3Vec3 impulse = b3MulSub( b3MulSV( -massScale, b3MulMV( joint->angularMass, b3Add( cdot, bias ) ) ), impulseScale, joint->angularImpulse ); + joint->angularImpulse = b3Add( joint->angularImpulse, impulse ); + + wA = b3Sub( wA, b3MulMV( iA, impulse ) ); + wB = b3Add( wB, b3MulMV( iB, impulse ) ); + } + + // linear constraint + { + b3Vec3 rA = b3RotateVector( stateA->deltaRotation, joint->frameA.p ); + b3Vec3 rB = b3RotateVector( stateB->deltaRotation, joint->frameB.p ); + + b3Vec3 cdot = b3Sub( b3Add( vB, b3Cross( wB, rB ) ), b3Add( vA, b3Cross( wA, rA ) ) ); + + b3Vec3 bias = b3Vec3_zero; + float massScale = 1.0f; + float impulseScale = 0.0f; + if ( useBias || joint->linearHertz > 0.0f ) + { + b3Vec3 dcA = stateA->deltaPosition; + b3Vec3 dcB = stateB->deltaPosition; + + b3Vec3 separation = b3Add( b3Add( b3Sub( dcB, dcA ), b3Sub( rB, rA ) ), joint->deltaCenter ); + + bias = b3MulSV( joint->linearSpring.biasRate, separation ); + massScale = joint->linearSpring.massScale; + impulseScale = joint->linearSpring.impulseScale; + } + + //// K = [(1/m1 + 1/m2) * eye(2) - skew(r1) * invI1 * skew(r1) - skew(r2) * invI2 * skew(r2)] + b3Matrix3 sA = b3Skew( rA ); + b3Matrix3 sB = b3Skew( rB ); + b3Matrix3 kA = b3MulMM( sA, b3MulMM( base->invIA, sA ) ); + b3Matrix3 kB = b3MulMM( sB, b3MulMM( base->invIB, sB ) ); + b3Matrix3 k = b3NegateMat3( b3AddMM( kA, kB ) ); + k.cx.x += mA + mB; + k.cy.y += mA + mB; + k.cz.z += mA + mB; + + b3Vec3 b = b3Solve3( k, b3Add( cdot, bias ) ); + + b3Vec3 impulse = b3MulSub( b3MulSV( -massScale, b ), impulseScale, joint->linearImpulse ); + joint->linearImpulse = b3Add( joint->linearImpulse, impulse ); + + vA = b3MulSub( vA, mA, impulse ); + wA = b3Sub( wA, b3MulMV( iA, b3Cross( rA, impulse ) ) ); + vB = b3MulAdd( vB, mB, impulse ); + wB = b3Add( wB, b3MulMV( iB, b3Cross( rB, impulse ) ) ); + } + + if ( stateA->flags & b3_dynamicFlag ) + { + stateA->linearVelocity = vA; + stateA->angularVelocity = wA; + } + + if ( stateB->flags & b3_dynamicFlag ) + { + stateB->linearVelocity = vB; + stateB->angularVelocity = wB; + } +} + +void b3DrawWeldJoint( b3DebugDraw* draw, b3JointSim* base, b3WorldTransform transformA, b3WorldTransform transformB, float scale ) +{ + b3WorldTransform frameA = b3MulWorldTransforms( transformA, base->localFrameA ); + b3WorldTransform frameB = b3MulWorldTransforms( transformB, base->localFrameB ); + + b3Vec3 extents = { 0.1f * scale, 0.05f * scale, 0.025f * scale }; + draw->DrawBoxFcn( extents, frameA, b3_colorDarkOrange, draw->context ); + draw->DrawBoxFcn( extents, frameB, b3_colorDarkCyan, draw->context ); +} diff --git a/vendor/box3d/src/src/wheel_joint.c b/vendor/box3d/src/src/wheel_joint.c new file mode 100644 index 000000000..05665e9f0 --- /dev/null +++ b/vendor/box3d/src/src/wheel_joint.c @@ -0,0 +1,1105 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +#include "body.h" +#include "core.h" +#include "joint.h" +#include "math_internal.h" +#include "physics_world.h" +#include "solver.h" +#include "solver_set.h" +#include "recording.h" + +// needed for dll export +#include "box3d/box3d.h" + +void b3WheelJoint_EnableSuspension( b3JointId jointId, bool enableSpring ) +{ + b3World* world = b3GetWorld( jointId.world0 ); + B3_REC( world, WheelJointEnableSuspension, jointId, enableSpring ); + b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_wheelJoint ); + + if ( enableSpring != joint->wheelJoint.enableSuspensionSpring ) + { + joint->wheelJoint.enableSuspensionSpring = enableSpring; + joint->wheelJoint.suspensionSpringImpulse = 0.0f; + } +} + +bool b3WheelJoint_IsSuspensionEnabled( b3JointId jointId ) +{ + b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_wheelJoint ); + return joint->wheelJoint.enableSuspensionSpring; +} + +void b3WheelJoint_SetSuspensionHertz( b3JointId jointId, float hertz ) +{ + b3World* world = b3GetWorld( jointId.world0 ); + B3_REC( world, WheelJointSetSuspensionHertz, jointId, hertz ); + b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_wheelJoint ); + joint->wheelJoint.suspensionHertz = hertz; +} + +float b3WheelJoint_GetSuspensionHertz( b3JointId jointId ) +{ + b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_wheelJoint ); + return joint->wheelJoint.suspensionHertz; +} + +void b3WheelJoint_SetSuspensionDampingRatio( b3JointId jointId, float dampingRatio ) +{ + b3World* world = b3GetWorld( jointId.world0 ); + B3_REC( world, WheelJointSetSuspensionDampingRatio, jointId, dampingRatio ); + b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_wheelJoint ); + joint->wheelJoint.suspensionDampingRatio = dampingRatio; +} + +float b3WheelJoint_GetSuspensionDampingRatio( b3JointId jointId ) +{ + b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_wheelJoint ); + return joint->wheelJoint.suspensionDampingRatio; +} + +void b3WheelJoint_EnableSuspensionLimit( b3JointId jointId, bool enableLimit ) +{ + b3World* world = b3GetWorld( jointId.world0 ); + B3_REC( world, WheelJointEnableSuspensionLimit, jointId, enableLimit ); + b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_wheelJoint ); + if ( joint->wheelJoint.enableSuspensionLimit != enableLimit ) + { + joint->wheelJoint.lowerSuspensionImpulse = 0.0f; + joint->wheelJoint.upperSuspensionImpulse = 0.0f; + joint->wheelJoint.enableSuspensionLimit = enableLimit; + } +} + +bool b3WheelJoint_IsSuspensionLimitEnabled( b3JointId jointId ) +{ + b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_wheelJoint ); + return joint->wheelJoint.enableSuspensionLimit; +} + +float b3WheelJoint_GetLowerSuspensionLimit( b3JointId jointId ) +{ + b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_wheelJoint ); + return joint->wheelJoint.lowerSuspensionLimit; +} + +float b3WheelJoint_GetUpperSuspensionLimit( b3JointId jointId ) +{ + b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_wheelJoint ); + return joint->wheelJoint.upperSuspensionLimit; +} + +void b3WheelJoint_SetSuspensionLimits( b3JointId jointId, float lower, float upper ) +{ + B3_ASSERT( lower <= upper ); + b3World* world = b3GetWorld( jointId.world0 ); + B3_REC( world, WheelJointSetSuspensionLimits, jointId, lower, upper ); + b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_wheelJoint ); + if ( lower != joint->wheelJoint.lowerSuspensionLimit || upper != joint->wheelJoint.upperSuspensionLimit ) + { + joint->wheelJoint.lowerSuspensionLimit = lower; + joint->wheelJoint.upperSuspensionLimit = upper; + joint->wheelJoint.lowerSuspensionImpulse = 0.0f; + joint->wheelJoint.upperSuspensionImpulse = 0.0f; + } +} + +void b3WheelJoint_EnableSpinMotor( b3JointId jointId, bool enableMotor ) +{ + b3World* world = b3GetWorld( jointId.world0 ); + B3_REC( world, WheelJointEnableSpinMotor, jointId, enableMotor ); + b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_wheelJoint ); + if ( joint->wheelJoint.enableSpinMotor != enableMotor ) + { + joint->wheelJoint.spinImpulse = 0.0f; + joint->wheelJoint.enableSpinMotor = enableMotor; + } +} + +bool b3WheelJoint_IsSpinMotorEnabled( b3JointId jointId ) +{ + b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_wheelJoint ); + return joint->wheelJoint.enableSpinMotor; +} + +void b3WheelJoint_SetSpinMotorSpeed( b3JointId jointId, float motorSpeed ) +{ + b3World* world = b3GetWorld( jointId.world0 ); + B3_REC( world, WheelJointSetSpinMotorSpeed, jointId, motorSpeed ); + b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_wheelJoint ); + joint->wheelJoint.spinSpeed = motorSpeed; +} + +float b3WheelJoint_GetSpinMotorSpeed( b3JointId jointId ) +{ + b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_wheelJoint ); + return joint->wheelJoint.spinSpeed; +} + +void b3WheelJoint_SetMaxSpinTorque( b3JointId jointId, float torque ) +{ + b3World* world = b3GetWorld( jointId.world0 ); + B3_REC( world, WheelJointSetMaxSpinTorque, jointId, torque ); + b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_wheelJoint ); + joint->wheelJoint.maxSpinTorque = torque; +} + +float b3WheelJoint_GetMaxSpinTorque( b3JointId jointId ) +{ + b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_wheelJoint ); + return joint->wheelJoint.maxSpinTorque; +} + +void b3WheelJoint_EnableSteering( b3JointId jointId, bool flag ) +{ + b3World* world = b3GetWorld( jointId.world0 ); + B3_REC( world, WheelJointEnableSteering, jointId, flag ); + b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_wheelJoint ); + if ( joint->wheelJoint.enableSteering != flag ) + { + joint->wheelJoint.angularImpulse = (b3Vec2){ 0.0f, 0.0f }; + joint->wheelJoint.enableSteering = flag; + } +} + +bool b3WheelJoint_IsSteeringEnabled( b3JointId jointId ) +{ + b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_wheelJoint ); + return joint->wheelJoint.enableSteering; +} + +void b3WheelJoint_SetSteeringHertz( b3JointId jointId, float hertz ) +{ + b3World* world = b3GetWorld( jointId.world0 ); + B3_REC( world, WheelJointSetSteeringHertz, jointId, hertz ); + b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_wheelJoint ); + joint->wheelJoint.steeringHertz = hertz; +} + +float b3WheelJoint_GetSteeringHertz( b3JointId jointId ) +{ + b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_wheelJoint ); + return joint->wheelJoint.steeringHertz; +} + +void b3WheelJoint_SetSteeringDampingRatio( b3JointId jointId, float dampingRatio ) +{ + b3World* world = b3GetWorld( jointId.world0 ); + B3_REC( world, WheelJointSetSteeringDampingRatio, jointId, dampingRatio ); + b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_wheelJoint ); + joint->wheelJoint.steeringDampingRatio = dampingRatio; +} + +float b3WheelJoint_GetSteeringDampingRatio( b3JointId jointId ) +{ + b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_wheelJoint ); + return joint->wheelJoint.steeringDampingRatio; +} + +void b3WheelJoint_SetMaxSteeringTorque( b3JointId jointId, float maxTorque ) +{ + b3World* world = b3GetWorld( jointId.world0 ); + B3_REC( world, WheelJointSetMaxSteeringTorque, jointId, maxTorque ); + b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_wheelJoint ); + joint->wheelJoint.maxSteeringTorque = maxTorque; +} + +float b3WheelJoint_GetMaxSteeringTorque( b3JointId jointId ) +{ + b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_wheelJoint ); + return joint->wheelJoint.maxSteeringTorque; +} + +void b3WheelJoint_EnableSteeringLimit( b3JointId jointId, bool flag ) +{ + b3World* world = b3GetWorld( jointId.world0 ); + B3_REC( world, WheelJointEnableSteeringLimit, jointId, flag ); + b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_wheelJoint ); + if ( joint->wheelJoint.enableSteeringLimit != flag ) + { + joint->wheelJoint.lowerSteeringImpulse = 0.0f; + joint->wheelJoint.upperSteeringImpulse = 0.0f; + joint->wheelJoint.enableSteeringLimit = flag; + } +} + +bool b3WheelJoint_IsSteeringLimitEnabled( b3JointId jointId ) +{ + b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_wheelJoint ); + return joint->wheelJoint.enableSteeringLimit; +} + +float b3WheelJoint_GetLowerSteeringLimit( b3JointId jointId ) +{ + b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_wheelJoint ); + return joint->wheelJoint.lowerSteeringLimit; +} + +float b3WheelJoint_GetUpperSteeringLimit( b3JointId jointId ) +{ + b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_wheelJoint ); + return joint->wheelJoint.upperSteeringLimit; +} + +void b3WheelJoint_SetSteeringLimits( b3JointId jointId, float lowerRadians, float upperRadians ) +{ + B3_ASSERT( lowerRadians <= upperRadians ); + b3World* world = b3GetWorld( jointId.world0 ); + B3_REC( world, WheelJointSetSteeringLimits, jointId, lowerRadians, upperRadians ); + b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_wheelJoint ); + joint->wheelJoint.lowerSteeringLimit = lowerRadians; + joint->wheelJoint.upperSteeringLimit = upperRadians; +} + +void b3WheelJoint_SetTargetSteeringAngle( b3JointId jointId, float radians ) +{ + b3World* world = b3GetWorld( jointId.world0 ); + B3_REC( world, WheelJointSetTargetSteeringAngle, jointId, radians ); + b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_wheelJoint ); + joint->wheelJoint.targetSteeringAngle = radians; +} + +float b3WheelJoint_GetTargetSteeringAngle( b3JointId jointId ) +{ + b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_wheelJoint ); + return joint->wheelJoint.targetSteeringAngle; +} + +float b3WheelJoint_GetSpinSpeed( b3JointId jointId ) +{ + b3World* world = b3GetWorld( jointId.world0 ); + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_wheelJoint ); + + int idA = base->bodyIdA; + int idB = base->bodyIdB; + + b3Body* bodyA = b3Array_Get( world->bodies, idA ); + b3Body* bodyB = b3Array_Get( world->bodies, idB ); + + b3SolverSet* setB = b3Array_Get( world->solverSets, bodyB->setIndex ); + int localIndexB = bodyB->localIndex; + b3BodySim* bodySimB = b3Array_Get( setB->bodySims, localIndexB ); + + b3Quat quatB = b3MulQuat( bodySimB->transform.q, base->localFrameB.q ); + b3Vec3 spinAxis = b3RotateVector( quatB, b3Vec3_axisZ ); + + b3Vec3 wA = b3Vec3_zero; + b3BodyState* stateA = b3GetBodyState( world, bodyA ); + if ( stateA != NULL ) + { + wA = stateA->angularVelocity; + } + + b3Vec3 wB = b3Vec3_zero; + b3BodyState* stateB = b3GetBodyState( world, bodyB ); + if ( stateB != NULL ) + { + wB = stateB->angularVelocity; + } + + float speed = b3Dot( b3Sub( wB, wA ), spinAxis ); + return speed; +} + +float b3WheelJoint_GetSpinTorque( b3JointId jointId ) +{ + b3World* world = b3GetWorld( jointId.world0 ); + b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_wheelJoint ); + return world->inv_h * joint->wheelJoint.spinImpulse; +} + +float b3WheelJoint_GetSteeringAngle( b3JointId jointId ) +{ + b3World* world = b3GetWorld( jointId.world0 ); + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_wheelJoint ); + + int idA = base->bodyIdA; + int idB = base->bodyIdB; + + b3Body* bodyA = b3Array_Get( world->bodies, idA ); + b3Body* bodyB = b3Array_Get( world->bodies, idB ); + + b3SolverSet* setA = b3Array_Get( world->solverSets, bodyA->setIndex ); + b3SolverSet* setB = b3Array_Get( world->solverSets, bodyB->setIndex ); + + int localIndexA = bodyA->localIndex; + int localIndexB = bodyB->localIndex; + + b3BodySim* bodySimA = b3Array_Get( setA->bodySims, localIndexA ); + b3BodySim* bodySimB = b3Array_Get( setB->bodySims, localIndexB ); + + b3Quat quatA = b3MulQuat( bodySimA->transform.q, base->localFrameA.q ); + b3Quat quatB = b3MulQuat( bodySimB->transform.q, base->localFrameB.q ); + + b3Matrix3 matrixA = b3MakeMatrixFromQuat( quatA ); + b3Matrix3 matrixB = b3MakeMatrixFromQuat( quatB ); + + // Twist around x-axis + float cs = b3Dot( matrixB.cz, matrixA.cz ); + float ss = -b3Dot( matrixB.cz, matrixA.cy ); + + return b3Atan2( ss, cs ); +} + +float b3WheelJoint_GetSteeringTorque( b3JointId jointId ) +{ + b3World* world = b3GetWorld( jointId.world0 ); + b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_wheelJoint ); + return world->inv_h * joint->wheelJoint.steeringSpringImpulse; +} + +b3Vec3 b3GetWheelJointForce( b3World* world, b3JointSim* base ) +{ + b3WorldTransform transformA = b3GetBodyTransform( world, base->bodyIdA ); + b3WheelJoint* joint = &base->wheelJoint; + + // impulse in joint space + b3Vec3 impulse = { + joint->linearImpulse.x, + joint->linearImpulse.y, + joint->lowerSuspensionLimit + joint->upperSuspensionImpulse + joint->suspensionSpringImpulse, + }; + + // convert impulse to force + b3Vec3 force = b3MulSV( world->inv_h, impulse ); + + // convert to body space + force = b3RotateVector( base->localFrameA.q, force ); + + // convert to world space + force = b3RotateVector( transformA.q, force ); + return force; +} + +b3Vec3 b3GetWheelJointTorque( b3World* world, b3JointSim* base ) +{ + B3_ASSERT( base->type == b3_wheelJoint ); + + // chase body id to the solver set where the body lives + int idA = base->bodyIdA; + // int idB = base->bodyIdB; + + b3Body* bodyA = b3Array_Get( world->bodies, idA ); + // b3Body* bodyB = b3Array_Get( world->bodies, idB ); + + b3SolverSet* setA = b3Array_Get( world->solverSets, bodyA->setIndex ); + // b3SolverSet* setB = b3Array_Get( world->solverSets, bodyB->setIndex ); + + int localIndexA = bodyA->localIndex; + // int localIndexB = bodyB->localIndex; + + b3BodySim* bodySimA = b3Array_Get( setA->bodySims, localIndexA ); + // b3BodySim* bodySimB = b3Array_Get( setB->bodySims, localIndexB ); + + b3Quat qA = b3MulQuat( bodySimA->transform.q, base->localFrameA.q ); + + b3Matrix3 matrixA = b3MakeMatrixFromQuat( qA ); + + return b3MulSV( world->inv_h * base->wheelJoint.spinImpulse, matrixA.cz ); +} + +// See constraints.pdf + +void b3PrepareWheelJoint( b3JointSim* base, b3StepContext* context ) +{ + B3_ASSERT( base->type == b3_wheelJoint ); + + // chase body id to the solver set where the body lives + int idA = base->bodyIdA; + int idB = base->bodyIdB; + + b3World* world = context->world; + + b3Body* bodyA = b3Array_Get( world->bodies, idA ); + b3Body* bodyB = b3Array_Get( world->bodies, idB ); + + B3_ASSERT( bodyA->setIndex == b3_awakeSet || bodyB->setIndex == b3_awakeSet ); + b3SolverSet* setA = b3Array_Get( world->solverSets, bodyA->setIndex ); + b3SolverSet* setB = b3Array_Get( world->solverSets, bodyB->setIndex ); + + int localIndexA = bodyA->localIndex; + int localIndexB = bodyB->localIndex; + + b3BodySim* bodySimA = b3Array_Get( setA->bodySims, localIndexA ); + b3BodySim* bodySimB = b3Array_Get( setB->bodySims, localIndexB ); + + base->invMassA = bodySimA->invMass; + base->invMassB = bodySimB->invMass; + base->invIA = bodySimA->invInertiaWorld; + base->invIB = bodySimB->invInertiaWorld; + + b3Matrix3 invInertiaSum = b3AddMM( base->invIA, base->invIB ); + base->fixedRotation = b3Det( invInertiaSum ) < 1000.0f * FLT_MIN; + + b3WheelJoint* joint = &base->wheelJoint; + + joint->indexA = bodyA->setIndex == b3_awakeSet ? localIndexA : B3_NULL_INDEX; + joint->indexB = bodyB->setIndex == b3_awakeSet ? localIndexB : B3_NULL_INDEX; + + // Compute joint anchor frames with world space rotation, relative to center of mass + joint->frameA.q = b3MulQuat( bodySimA->transform.q, base->localFrameA.q ); + joint->frameA.p = b3RotateVector( bodySimA->transform.q, b3Sub( base->localFrameA.p, bodySimA->localCenter ) ); + joint->frameB.q = b3MulQuat( bodySimB->transform.q, base->localFrameB.q ); + joint->frameB.p = b3RotateVector( bodySimB->transform.q, b3Sub( base->localFrameB.p, bodySimB->localCenter ) ); + + // Compute the initial center delta. Incremental position updates are relative to this. + joint->deltaCenter = b3SubPos( bodySimB->center, bodySimA->center ); + + b3Vec3 rA = joint->frameA.p; + b3Vec3 rB = joint->frameB.p; + + b3Matrix3 matrixA = b3MakeMatrixFromQuat( joint->frameA.q ); + b3Matrix3 matrixB = b3MakeMatrixFromQuat( joint->frameB.q ); + + // todo use fresh effective masses in the sub-step to avoid divergence like I saw for the prismatic joint + + { + b3Vec3 suspensionAxis = matrixA.cx; + b3Vec3 rAn = b3Cross( rA, suspensionAxis ); + b3Vec3 rBn = b3Cross( rB, suspensionAxis ); + + float k = base->invMassA + base->invMassB + b3Dot( rAn, b3MulMV( base->invIA, rAn ) ) + + b3Dot( rBn, b3MulMV( base->invIB, rBn ) ); + joint->suspensionMass = k > 0.0f ? 1.0f / k : 0.0f; + } + + joint->suspensionSoftness = b3MakeSoft( joint->suspensionHertz, joint->suspensionDampingRatio, context->h ); + joint->steeringSoftness = b3MakeSoft( joint->steeringHertz, joint->steeringDampingRatio, context->h ); + + { + // Rotation axis is the z-axis of body A. + b3Vec3 spinAxis = matrixB.cz; + float k = b3Dot( spinAxis, b3MulMV( invInertiaSum, spinAxis ) ); + joint->spinMass = k > 0.0f ? 1.0f / k : 0.0f; + } + + { + // Twist constraint around x-axis + float cs = b3Dot( matrixB.cz, matrixA.cz ); + float ss = -b3Dot( matrixB.cz, matrixA.cy ); + float den = cs * cs + ss * ss; + den = den > 0.0f ? 1.0f / den : 0.0f; + b3Vec3 steeringAxis = + b3MulSV( den, b3Cross( matrixB.cz, b3Sub( b3MulSV( -cs, matrixA.cy ), b3MulSV( ss, matrixA.cz ) ) ) ); + + float k = b3Dot( steeringAxis, b3MulMV( invInertiaSum, steeringAxis ) ); + joint->steeringMass = k > 0.0f ? 1.0f / k : 0.0f; + } + + if ( context->enableWarmStarting == false ) + { + joint->linearImpulse = (b3Vec2){ 0.0f, 0.0f }; + joint->angularImpulse = (b3Vec2){ 0.0f, 0.0f }; + joint->spinImpulse = 0.0f; + joint->suspensionSpringImpulse = 0.0f; + joint->lowerSuspensionImpulse = 0.0f; + joint->upperSuspensionImpulse = 0.0f; + joint->steeringSpringImpulse = 0.0f; + joint->lowerSteeringImpulse = 0.0f; + joint->upperSteeringImpulse = 0.0f; + } +} + +void b3WarmStartWheelJoint( b3JointSim* base, b3StepContext* context ) +{ + B3_ASSERT( base->type == b3_wheelJoint ); + + float mA = base->invMassA; + float mB = base->invMassB; + b3Matrix3 iA = base->invIA; + b3Matrix3 iB = base->invIB; + + // dummy state for static bodies + b3BodyState dummyState = b3_identityBodyState; + + b3WheelJoint* joint = &base->wheelJoint; + + b3BodyState* stateA = joint->indexA == B3_NULL_INDEX ? &dummyState : context->states + joint->indexA; + b3BodyState* stateB = joint->indexB == B3_NULL_INDEX ? &dummyState : context->states + joint->indexB; + + b3Vec3 rA = b3RotateVector( stateA->deltaRotation, joint->frameA.p ); + b3Vec3 rB = b3RotateVector( stateB->deltaRotation, joint->frameB.p ); + + b3Vec3 d = b3Add( b3Add( b3Sub( stateB->deltaPosition, stateA->deltaPosition ), joint->deltaCenter ), b3Sub( rB, rA ) ); + + b3Quat quatA = b3MulQuat( stateA->deltaRotation, joint->frameA.q ); + b3Quat quatB = b3MulQuat( stateB->deltaRotation, joint->frameB.q ); + if ( b3DotQuat( quatA, quatB ) < 0.0f ) + { + // this keeps the rotation angle in the range [-pi, pi] + quatB = b3NegateQuat( quatB ); + } + + b3Matrix3 matrixA = b3MakeMatrixFromQuat( quatA ); + b3Matrix3 matrixB = b3MakeMatrixFromQuat( quatB ); + + b3Vec3 sAx = b3Cross( b3Add( d, rA ), matrixA.cx ); + b3Vec3 sBx = b3Cross( rB, matrixA.cx ); + b3Vec3 sAy = b3Cross( b3Add( d, rA ), matrixA.cy ); + b3Vec3 sBy = b3Cross( rB, matrixA.cy ); + b3Vec3 sAz = b3Cross( b3Add( d, rA ), matrixA.cz ); + b3Vec3 sBz = b3Cross( rB, matrixA.cz ); + + float suspensionImpulse = joint->suspensionSpringImpulse + joint->lowerSuspensionImpulse - joint->upperSuspensionImpulse; + + float linearImpulseY = joint->linearImpulse.x; + float linearImpulseZ = joint->linearImpulse.y; + float angularImpulseX = joint->angularImpulse.x; + float angularImpulseY = joint->angularImpulse.y; + + b3Vec3 linearImpulse = b3Blend3( suspensionImpulse, matrixA.cx, linearImpulseY, matrixA.cy, linearImpulseZ, matrixA.cz ); + b3Vec3 angularImpulseA = b3Blend3( suspensionImpulse, sAx, linearImpulseY, sAy, linearImpulseZ, sAz ); + b3Vec3 angularImpulseB = b3Blend3( suspensionImpulse, sBx, linearImpulseY, sBy, linearImpulseZ, sBz ); + b3Vec3 angularImpulse = b3MulSV( joint->spinImpulse, matrixA.cz ); + + b3Vec3 spinAxis = matrixB.cz; + + if ( joint->enableSteering ) + { + // Twist constraint around x-axis + float cs = b3Dot( matrixB.cz, matrixA.cz ); + float ss = -b3Dot( matrixB.cz, matrixA.cy ); + float den = cs * cs + ss * ss; + den = den > 0.0f ? 1.0f / den : 0.0f; + b3Vec3 steeringAxis = + b3MulSV( den, b3Cross( matrixB.cz, b3Sub( b3MulSV( -cs, matrixA.cy ), b3MulSV( ss, matrixA.cz ) ) ) ); + + b3Vec3 perpAxis = b3Cross( spinAxis, matrixA.cx ); + float steeringImpulse = joint->steeringSpringImpulse + joint->lowerSteeringImpulse - joint->upperSteeringImpulse; + angularImpulse = b3Blend3( angularImpulseX, perpAxis, joint->spinImpulse, spinAxis, steeringImpulse, steeringAxis ); + } + else + { + b3Quat relQ = b3InvMulQuat( quatA, quatB ); + b3Vec3 perpAxisX = b3MulSV( + 0.5f, b3RotateVector( quatA, b3Add( b3MulSV( relQ.s, b3Vec3_axisX ), b3Cross( relQ.v, b3Vec3_axisX ) ) ) ); + b3Vec3 perpAxisY = b3MulSV( + 0.5f, b3RotateVector( quatA, b3Add( b3MulSV( relQ.s, b3Vec3_axisY ), b3Cross( relQ.v, b3Vec3_axisY ) ) ) ); + angularImpulse = b3Add( + angularImpulse, + b3Blend3( angularImpulseX, perpAxisX, angularImpulseY, perpAxisY, joint->spinImpulse, spinAxis ) ); + } + + if ( stateA->flags & b3_dynamicFlag ) + { + stateA->linearVelocity = b3MulSub( stateA->linearVelocity, mA, linearImpulse ); + stateA->angularVelocity = b3Sub( stateA->angularVelocity, b3MulMV( iA, b3Add( angularImpulseA, angularImpulse ) ) ); + } + + if ( stateB->flags & b3_dynamicFlag ) + { + stateB->linearVelocity = b3MulAdd( stateB->linearVelocity, mB, linearImpulse ); + stateB->angularVelocity = b3Add( stateB->angularVelocity, b3MulMV( iB, b3Add( angularImpulseB, angularImpulse ) ) ); + } +} + +void b3SolveWheelJoint( b3JointSim* base, b3StepContext* context, bool useBias ) +{ + B3_ASSERT( base->type == b3_wheelJoint ); + + float mA = base->invMassA; + float mB = base->invMassB; + b3Matrix3 iA = base->invIA; + b3Matrix3 iB = base->invIB; + + // dummy state for static bodies + b3BodyState dummyState = b3_identityBodyState; + + b3WheelJoint* joint = &base->wheelJoint; + + b3BodyState* stateA = joint->indexA == B3_NULL_INDEX ? &dummyState : context->states + joint->indexA; + b3BodyState* stateB = joint->indexB == B3_NULL_INDEX ? &dummyState : context->states + joint->indexB; + + b3Vec3 vA = stateA->linearVelocity; + b3Vec3 wA = stateA->angularVelocity; + b3Vec3 vB = stateB->linearVelocity; + b3Vec3 wB = stateB->angularVelocity; + + bool fixedRotation = base->fixedRotation; + + // current anchors + b3Vec3 rA = b3RotateVector( stateA->deltaRotation, joint->frameA.p ); + b3Vec3 rB = b3RotateVector( stateB->deltaRotation, joint->frameB.p ); + + b3Quat quatA = b3MulQuat( stateA->deltaRotation, joint->frameA.q ); + b3Quat quatB = b3MulQuat( stateB->deltaRotation, joint->frameB.q ); + + if ( b3DotQuat( quatA, quatB ) < 0.0f ) + { + // this keeps the rotation angle in the range [-pi, pi] + quatB = b3NegateQuat( quatB ); + } + + b3Quat relQ = b3InvMulQuat( quatA, quatB ); + b3Matrix3 matrixA = b3MakeMatrixFromQuat( quatA ); + b3Matrix3 matrixB = b3MakeMatrixFromQuat( quatB ); + + // b3Vec3 spinAxis = b3RotateVector( quatB, b3Vec3_axisZ ); + + b3Vec3 d = b3Add( b3Add( b3Sub( stateB->deltaPosition, stateA->deltaPosition ), joint->deltaCenter ), b3Sub( rB, rA ) ); + b3Vec3 sAx = b3Cross( b3Add( d, rA ), matrixA.cx ); + b3Vec3 sBx = b3Cross( rB, matrixA.cx ); + b3Vec3 sAy = b3Cross( b3Add( d, rA ), matrixA.cy ); + b3Vec3 sBy = b3Cross( rB, matrixA.cy ); + b3Vec3 sAz = b3Cross( b3Add( d, rA ), matrixA.cz ); + b3Vec3 sBz = b3Cross( rB, matrixA.cz ); + + float translation = b3Dot( matrixA.cx, d ); + + // Steering param ib = cz_b, ia = cz_a, ja = -cy_a + float cs = b3Dot( matrixB.cz, matrixA.cz ); + float ss = -b3Dot( matrixB.cz, matrixA.cy ); + float den = cs * cs + ss * ss; + den = den > 0.0f ? 1.0f / den : 0.0f; + b3Vec3 steeringAxis = + b3MulSV( den, b3Cross( matrixB.cz, b3Sub( b3MulSV( -cs, matrixA.cy ), b3MulSV( ss, matrixA.cz ) ) ) ); + + // motor constraint + if ( joint->enableSpinMotor && fixedRotation == false ) + { + b3Vec3 spinAxis = matrixB.cz; + float cdot = b3Dot( b3Sub( wB, wA ), spinAxis ) - joint->spinSpeed; + float impulse = -joint->spinMass * cdot; + float oldImpulse = joint->spinImpulse; + float maxImpulse = context->h * joint->maxSpinTorque; + joint->spinImpulse = b3ClampFloat( joint->spinImpulse + impulse, -maxImpulse, maxImpulse ); + impulse = joint->spinImpulse - oldImpulse; + + wA = b3Sub( wA, b3MulMV( iA, b3MulSV( impulse, spinAxis ) ) ); + wB = b3Add( wB, b3MulMV( iB, b3MulSV( impulse, spinAxis ) ) ); + } + + // suspension + if ( joint->enableSuspensionSpring ) + { + // This is a real spring and should be applied even during relax + float c = translation; + float bias = joint->suspensionSoftness.biasRate * c; + float massScale = joint->suspensionSoftness.massScale; + float impulseScale = joint->suspensionSoftness.impulseScale; + + float cdot = b3Dot( matrixA.cx, b3Sub( vB, vA ) ) + b3Dot( sBx, wB ) - b3Dot( sAx, wA ); + float impulse = -massScale * joint->suspensionMass * ( cdot + bias ) - impulseScale * joint->suspensionSpringImpulse; + joint->suspensionSpringImpulse += impulse; + + b3Vec3 linearImpulse = b3MulSV( impulse, matrixA.cx ); + b3Vec3 angularImpulseA = b3MulSV( impulse, sAx ); + b3Vec3 angularImpulseB = b3MulSV( impulse, sBx ); + + vA = b3MulSub( vA, mA, linearImpulse ); + wA = b3Sub( wA, b3MulMV( iA, angularImpulseA ) ); + vB = b3MulAdd( vB, mB, linearImpulse ); + wB = b3Add( wB, b3MulMV( iB, angularImpulseB ) ); + } + + // steering + if ( joint->enableSteering && fixedRotation == false ) + { + float steeringAngle = b3Atan2( ss, cs ); + + { + // This is a real spring and should be applied even during relax + float c = steeringAngle - joint->targetSteeringAngle; + float bias = joint->steeringSoftness.biasRate * c; + float massScale = joint->steeringSoftness.massScale; + float impulseScale = joint->steeringSoftness.impulseScale; + + float cdot = b3Dot( steeringAxis, b3Sub( wB, wA ) ); + float oldImpulse = joint->steeringSpringImpulse; + float impulse = -massScale * joint->steeringMass * ( cdot + bias ) - impulseScale * oldImpulse; + float maxImpulse = context->h * joint->maxSteeringTorque; + joint->steeringSpringImpulse = b3ClampFloat( oldImpulse + impulse, -maxImpulse, maxImpulse ); + impulse = joint->steeringSpringImpulse - oldImpulse; + + wA = b3Sub( wA, b3MulMV( iA, b3MulSV( impulse, steeringAxis ) ) ); + wB = b3Add( wB, b3MulMV( iB, b3MulSV( impulse, steeringAxis ) ) ); + } + + if ( joint->enableSteeringLimit ) + { + // Lower limit + { + float c = steeringAngle - joint->lowerSteeringLimit; + float bias = 0.0f; + float massScale = 1.0f; + float impulseScale = 0.0f; + + if ( c > 0.0f ) + { + // speculation + bias = c * context->inv_h; + } + else if ( useBias ) + { + bias = base->constraintSoftness.biasRate * c; + massScale = base->constraintSoftness.massScale; + impulseScale = base->constraintSoftness.impulseScale; + } + + float cdot = b3Dot( steeringAxis, b3Sub( wB, wA ) ); + float oldImpulse = joint->lowerSteeringImpulse; + float impulse = -massScale * joint->steeringMass * ( cdot + bias ) - impulseScale * oldImpulse; + joint->lowerSteeringImpulse = b3MaxFloat( oldImpulse + impulse, 0.0f ); + impulse = joint->lowerSteeringImpulse - oldImpulse; + + wA = b3Sub( wA, b3MulMV( iA, b3MulSV( impulse, steeringAxis ) ) ); + wB = b3Add( wB, b3MulMV( iB, b3MulSV( impulse, steeringAxis ) ) ); + } + + // Upper limit + // Note: signs are flipped to keep c positive when the constraint is satisfied. + // This also keeps the impulse positive when the limit is active. + { + // sign flipped + float c = joint->upperSteeringLimit - steeringAngle; + float bias = 0.0f; + float massScale = 1.0f; + float impulseScale = 0.0f; + + if ( c > 0.0f ) + { + // speculation + bias = c * context->inv_h; + } + else if ( useBias ) + { + bias = base->constraintSoftness.biasRate * c; + massScale = base->constraintSoftness.massScale; + impulseScale = base->constraintSoftness.impulseScale; + } + + // sign flipped on cdot + float cdot = b3Dot( steeringAxis, b3Sub( wA, wB ) ); + float oldImpulse = joint->upperSteeringImpulse; + float impulse = -massScale * joint->steeringMass * ( cdot + bias ) - impulseScale * oldImpulse; + joint->upperSteeringImpulse = b3MaxFloat( oldImpulse + impulse, 0.0f ); + impulse = joint->upperSteeringImpulse - oldImpulse; + + // sign flipped on applied impulse + wA = b3Add( wA, b3MulMV( iA, b3MulSV( impulse, steeringAxis ) ) ); + wB = b3Sub( wB, b3MulMV( iB, b3MulSV( impulse, steeringAxis ) ) ); + } + } + } + + if ( joint->enableSuspensionLimit ) + { + // Lower limit + { + float c = translation - joint->lowerSuspensionLimit; + float bias = 0.0f; + float massScale = 1.0f; + float impulseScale = 0.0f; + + if ( c > 0.0f ) + { + // speculation + bias = c * context->inv_h; + } + else if ( useBias ) + { + bias = base->constraintSoftness.biasRate * c; + massScale = base->constraintSoftness.massScale; + impulseScale = base->constraintSoftness.impulseScale; + } + + float cdot = b3Dot( matrixA.cx, b3Sub( vB, vA ) ) + b3Dot( sBx, wB ) - b3Dot( sAx, wA ); + float impulse = -massScale * joint->suspensionMass * ( cdot + bias ) - impulseScale * joint->lowerSuspensionImpulse; + float oldImpulse = joint->lowerSuspensionImpulse; + joint->lowerSuspensionImpulse = b3MaxFloat( oldImpulse + impulse, 0.0f ); + impulse = joint->lowerSuspensionImpulse - oldImpulse; + + b3Vec3 linearImpulse = b3MulSV( impulse, matrixA.cx ); + b3Vec3 angularImpulseA = b3MulSV( impulse, sAx ); + b3Vec3 angularImpulseB = b3MulSV( impulse, sBx ); + + vA = b3MulSub( vA, mA, linearImpulse ); + wA = b3Sub( wA, b3MulMV( iA, angularImpulseA ) ); + vB = b3MulAdd( vB, mB, linearImpulse ); + wB = b3Add( wB, b3MulMV( iB, angularImpulseB ) ); + } + + // Upper limit + // Note: signs are flipped to keep c positive when the constraint is satisfied. + // This also keeps the impulse positive when the limit is active. + { + // sign flipped + float c = joint->upperSuspensionLimit - translation; + float bias = 0.0f; + float massScale = 1.0f; + float impulseScale = 0.0f; + + if ( c > 0.0f ) + { + // speculation + bias = c * context->inv_h; + } + else if ( useBias ) + { + bias = base->constraintSoftness.biasRate * c; + massScale = base->constraintSoftness.massScale; + impulseScale = base->constraintSoftness.impulseScale; + } + + // sign flipped on cdot + float cdot = b3Dot( matrixA.cx, b3Sub( vA, vB ) ) + b3Dot( sAx, wA ) - b3Dot( sBx, wB ); + float impulse = -massScale * joint->suspensionMass * ( cdot + bias ) - impulseScale * joint->upperSuspensionImpulse; + float oldImpulse = joint->upperSuspensionImpulse; + joint->upperSuspensionImpulse = b3MaxFloat( oldImpulse + impulse, 0.0f ); + impulse = joint->upperSuspensionImpulse - oldImpulse; + + b3Vec3 linearImpulse = b3MulSV( impulse, matrixA.cx ); + b3Vec3 angularImpulseA = b3MulSV( impulse, sAx ); + b3Vec3 angularImpulseB = b3MulSV( impulse, sBx ); + + // sign flipped on applied impulse + vA = b3MulAdd( vA, mA, linearImpulse ); + wA = b3Add( wA, b3MulMV( iA, angularImpulseA ) ); + vB = b3MulSub( vB, mB, linearImpulse ); + wB = b3Sub( wB, b3MulMV( iB, angularImpulseB ) ); + } + } + + // Collinearity constraint + if ( fixedRotation == false ) + { + if ( joint->enableSteering == true ) + { + float bias = 0.0f; + float massScale = 1.0f; + float impulseScale = 0.0f; + if ( useBias ) + { + float c = b3Dot( matrixA.cx, matrixB.cz ); + + bias = base->constraintSoftness.biasRate * c; + massScale = base->constraintSoftness.massScale; + impulseScale = base->constraintSoftness.impulseScale; + } + + b3Vec3 u = b3Cross( matrixB.cz, matrixA.cx ); + float cdot = b3Dot( b3Sub( wB, wA ), u ); + + b3Matrix3 invInertiaSum = b3AddMM( iA, iB ); + float k = b3Dot( u, b3MulMV( invInertiaSum, u ) ); + float perpMass = k > 0.0f ? 1.0f / k : 0.0f; + + float deltaImpulse = -massScale * perpMass * ( cdot + bias ) - impulseScale * joint->angularImpulse.x; + joint->angularImpulse.x += deltaImpulse; + + wA = b3MulSub( wA, deltaImpulse, b3MulMV( iA, u ) ); + wB = b3MulAdd( wB, deltaImpulse, b3MulMV( iB, u ) ); + } + else + { + b3Vec2 bias = { 0.0f, 0.0f }; + float massScale = 1.0f; + float impulseScale = 0.0f; + + if ( useBias ) + { + b3Vec2 c = { relQ.v.x, relQ.v.y }; + bias = (b3Vec2){ base->constraintSoftness.biasRate * c.x, base->constraintSoftness.biasRate * c.y }; + massScale = base->constraintSoftness.massScale; + impulseScale = base->constraintSoftness.impulseScale; + } + + // Collinearity constraint as 2-by-2 + b3Vec3 perpAxisX = b3MulSV( + 0.5f, b3RotateVector( quatA, b3Add( b3MulSV( relQ.s, b3Vec3_axisX ), b3Cross( relQ.v, b3Vec3_axisX ) ) ) ); + b3Vec3 perpAxisY = b3MulSV( + 0.5f, b3RotateVector( quatA, b3Add( b3MulSV( relQ.s, b3Vec3_axisY ), b3Cross( relQ.v, b3Vec3_axisY ) ) ) ); + + b3Matrix3 invInertiaSum = b3AddMM( iA, iB ); + float kxx = b3Dot( perpAxisX, b3MulMV( invInertiaSum, perpAxisX ) ); + float kyy = b3Dot( perpAxisY, b3MulMV( invInertiaSum, perpAxisY ) ); + float kxy = b3Dot( perpAxisX, b3MulMV( invInertiaSum, perpAxisY ) ); + + b3Matrix2 k = { { kxx, kxy }, { kxy, kyy } }; + + b3Vec3 wRel = b3Sub( wB, wA ); + b3Vec2 cdot = { b3Dot( wRel, perpAxisX ), b3Dot( wRel, perpAxisY ) }; + b3Vec2 oldImpulse = joint->angularImpulse; + b3Vec2 cdotPlusBias = { cdot.x + bias.x, cdot.y + bias.y }; + b3Vec2 sol = b3Solve2( k, cdotPlusBias ); + b3Vec2 deltaImpulse = { + -massScale * sol.x - impulseScale * oldImpulse.x, + -massScale * sol.y - impulseScale * oldImpulse.y, + }; + joint->angularImpulse = (b3Vec2){ oldImpulse.x + deltaImpulse.x, oldImpulse.y + deltaImpulse.y }; + + b3Vec3 angularImpulse = b3Blend2( deltaImpulse.x, perpAxisX, deltaImpulse.y, perpAxisY ); + wA = b3Sub( wA, b3MulMV( iA, angularImpulse ) ); + wB = b3Add( wB, b3MulMV( iB, angularImpulse ) ); + } + } + + // Solve point-to-line constraint + { + b3Vec3 perpY = matrixA.cy; + b3Vec3 perpZ = matrixA.cz; + + b3Vec2 bias = { 0.0f, 0.0f }; + float massScale = 1.0f; + float impulseScale = 0.0f; + if ( useBias ) + { + b3Vec2 c = { b3Dot( perpY, d ), b3Dot( perpZ, d ) }; + bias = (b3Vec2){ base->constraintSoftness.biasRate * c.x, base->constraintSoftness.biasRate * c.y }; + massScale = base->constraintSoftness.massScale; + impulseScale = base->constraintSoftness.impulseScale; + } + + b3Vec3 vRel = b3Sub( b3Sub( b3Add( vB, b3Cross( wB, rB ) ), vA ), b3Cross( wA, b3Add( rA, d ) ) ); + b3Vec2 cdot = { b3Dot( perpY, vRel ), b3Dot( perpZ, vRel ) }; + + //// K = [(1/m1 + 1/m2) * eye(2) - skew(r1) * invI1 * skew(r1) - skew(r2) * invI2 * skew(r2)] + ///// Jx = [-perpX, -cross(d + rA, perpX), perpX, cross(rB, perpX)] + + float kyy = mA + mB + b3Dot( sAy, b3MulMV( iA, sAy ) ) + b3Dot( sBy, b3MulMV( iB, sBy ) ); + float kyz = b3Dot( sAy, b3MulMV( iA, sAz ) ) + b3Dot( sBy, b3MulMV( iB, sBz ) ); + float kzz = mA + mB + b3Dot( sAz, b3MulMV( iA, sAz ) ) + b3Dot( sBz, b3MulMV( iB, sBz ) ); + + b3Matrix2 k = { { kyy, kyz }, { kyz, kzz } }; + + b3Vec2 oldImpulse = joint->linearImpulse; + b3Vec2 cdotPlusBias = { cdot.x + bias.x, cdot.y + bias.y }; + b3Vec2 sol = b3Solve2( k, cdotPlusBias ); + b3Vec2 deltaImpulse = { + -massScale * sol.x - impulseScale * oldImpulse.x, + -massScale * sol.y - impulseScale * oldImpulse.y, + }; + joint->linearImpulse = (b3Vec2){ oldImpulse.x + deltaImpulse.x, oldImpulse.y + deltaImpulse.y }; + + b3Vec3 linearImpulse = b3Blend2( deltaImpulse.x, perpY, deltaImpulse.y, perpZ ); + + vA = b3MulSub( vA, mA, linearImpulse ); + wA = b3Sub( wA, b3MulMV( iA, b3Blend2( deltaImpulse.x, sAy, deltaImpulse.y, sAz ) ) ); + vB = b3MulAdd( vB, mB, linearImpulse ); + wB = b3Add( wB, b3MulMV( iB, b3Blend2( deltaImpulse.x, sBy, deltaImpulse.y, sBz ) ) ); + } + + if ( stateA->flags & b3_dynamicFlag ) + { + stateA->linearVelocity = vA; + stateA->angularVelocity = wA; + } + + if ( stateB->flags & b3_dynamicFlag ) + { + stateB->linearVelocity = vB; + stateB->angularVelocity = wB; + } +} + +#if 0 +void b3WheelJoint_Dump() +{ + int32 indexA = joint->bodyA->joint->islandIndex; + int32 indexB = joint->bodyB->joint->islandIndex; + + b3Dump(" b3WheelJointDef jd;\n"); + b3Dump(" jd.bodyA = sims[%d];\n", indexA); + b3Dump(" jd.bodyB = sims[%d];\n", indexB); + b3Dump(" jd.collideConnected = bool(%d);\n", joint->collideConnected); + b3Dump(" jd.localAnchorA.Set(%.9g, %.9g);\n", joint->localAnchorA.x, joint->localAnchorA.y); + b3Dump(" jd.localAnchorB.Set(%.9g, %.9g);\n", joint->localAnchorB.x, joint->localAnchorB.y); + b3Dump(" jd.referenceAngle = %.9g;\n", joint->referenceAngle); + b3Dump(" jd.enableLimit = bool(%d);\n", joint->enableLimit); + b3Dump(" jd.lowerAngle = %.9g;\n", joint->lowerAngle); + b3Dump(" jd.upperAngle = %.9g;\n", joint->upperAngle); + b3Dump(" jd.enableMotor = bool(%d);\n", joint->enableMotor); + b3Dump(" jd.motorSpeed = %.9g;\n", joint->motorSpeed); + b3Dump(" jd.maxMotorTorque = %.9g;\n", joint->maxMotorTorque); + b3Dump(" joints[%d] = joint->world->CreateJoint(&jd);\n", joint->index); +} +#endif + +void b3DrawWheelJoint( b3DebugDraw* draw, b3JointSim* base, b3WorldTransform transformA, b3WorldTransform transformB, float scale ) +{ + B3_ASSERT( base->type == b3_wheelJoint ); + + b3WheelJoint* joint = &base->wheelJoint; + + b3WorldTransform frameA = b3MulWorldTransforms( transformA, base->localFrameA ); + b3WorldTransform frameB = b3MulWorldTransforms( transformB, base->localFrameB ); + + b3Matrix3 matrixA = b3MakeMatrixFromQuat( frameA.q ); + b3Matrix3 matrixB = b3MakeMatrixFromQuat( frameB.q ); + + draw->DrawSegmentFcn( frameA.p, frameB.p, b3_colorBlue, draw->context ); + + if ( joint->enableSuspensionLimit ) + { + b3Pos lower = b3OffsetPos( frameA.p, b3MulSV( joint->lowerSuspensionLimit, matrixA.cx ) ); + b3Pos upper = b3OffsetPos( frameA.p, b3MulSV( joint->upperSuspensionLimit, matrixA.cx ) ); + b3Vec3 perp = matrixA.cy; + draw->DrawSegmentFcn( lower, upper, b3_colorGray, draw->context ); + draw->DrawSegmentFcn( b3OffsetPos( lower, b3MulSV( -0.1f * scale, perp ) ), b3OffsetPos( lower, b3MulSV( 0.1f * scale, perp ) ), + b3_colorGreen, draw->context ); + draw->DrawSegmentFcn( b3OffsetPos( upper, b3MulSV( -0.1f * scale, perp ) ), b3OffsetPos( upper, b3MulSV( 0.1f * scale, perp ) ), + b3_colorRed, draw->context ); + } + else + { + draw->DrawSegmentFcn( b3OffsetPos( frameA.p, b3MulSV( -1.0f * scale, matrixA.cx ) ), + b3OffsetPos( frameA.p, b3MulSV( 1.0f * scale, matrixA.cx ) ), b3_colorGray, draw->context ); + } + + if ( joint->enableSteering && joint->enableSteeringLimit ) + { + // b3Quat quatA = frameA.q; + // b3Quat quatB = frameB.q; + + // if ( b3DotQuat( quatA, quatB ) < 0.0f ) + //{ + // // this keeps the twist angle in the range [-pi, pi] + // quatB = -quatB; + // } + + // b3Quat relQ = b3InvMulQuat( quatA, quatB ); + + b3WorldTransform frame = { + .p = frameB.p, + .q = frameA.q, + }; + + const float radius = 0.5f * scale; + const int sliceCount = 16; + float lower = joint->lowerSteeringLimit; + float upper = joint->upperSteeringLimit; + + b3CosSin cs = b3ComputeCosSin( lower ); + b3Pos vertex1 = b3TransformWorldPoint( frame, (b3Vec3){ 0.0f, -radius * cs.sine, radius * cs.cosine } ); + + for ( int index = 0; index < sliceCount; ++index ) + { + float t2 = ( index + 1.0f ) / sliceCount; + float phi = b3LerpFloat( lower, upper, t2 ); + + cs = b3ComputeCosSin( phi ); + b3Pos vertex2 = b3TransformWorldPoint( frame, (b3Vec3){ 0.0f, -radius * cs.sine, radius * cs.cosine } ); + + if ( index == 0 ) + { + draw->DrawSegmentFcn( frame.p, vertex1, b3_colorCyan, draw->context ); + } + + if ( index == sliceCount - 1 ) + { + draw->DrawSegmentFcn( vertex2, frame.p, b3_colorCyan, draw->context ); + } + draw->DrawSegmentFcn( vertex1, vertex2, b3_colorCyan, draw->context ); + + vertex1 = vertex2; + } + } + + draw->DrawSegmentFcn( b3OffsetPos( frameB.p, b3MulSV( -0.5f * scale, matrixB.cz ) ), + b3OffsetPos( frameB.p, b3MulSV( 0.5f * scale, matrixB.cz ) ), b3_colorMagenta, draw->context ); + + draw->DrawPointFcn( frameA.p, 5.0f, b3_colorGray, draw->context ); + draw->DrawPointFcn( frameB.p, 5.0f, b3_colorDimGray, draw->context ); +} diff --git a/vendor/box3d/src/src/world_snapshot.c b/vendor/box3d/src/src/world_snapshot.c new file mode 100644 index 000000000..9efaee9fd --- /dev/null +++ b/vendor/box3d/src/src/world_snapshot.c @@ -0,0 +1,1335 @@ +// SPDX-FileCopyrightText: 2026 Erin Catto +// SPDX-License-Identifier: MIT + +#if defined( _MSC_VER ) && !defined( _CRT_SECURE_NO_WARNINGS ) +#define _CRT_SECURE_NO_WARNINGS +#endif + +#include "world_snapshot.h" + +#include "bitset.h" +#include "body.h" +#include "broad_phase.h" +#include "compound.h" +#include "constraint_graph.h" +#include "contact.h" +#include "container.h" +#include "core.h" +#include "id_pool.h" +#include "island.h" +#include "joint.h" +#include "physics_world.h" +#include "recording.h" +#include "sensor.h" +#include "shape.h" +#include "solver_set.h" +#include "table.h" + +#include "box3d/box3d.h" +#include "box3d/collision.h" +#include "box3d/types.h" + +#include + +// Snapshot image magic 'BNS3' and version +#define B3_SNAP_MAGIC 0x33534E42u +#define B3_SNAP_VERSION 2u + +#define B3_SNAP_FLAG_VALIDATION 0x1u +#define B3_SNAP_FLAG_DOUBLE_PRECISION 0x2u + +// Layout hash over all POD-copied structs + key constants. +// Changing a struct size updates this, catching ABI drift early. +static uint32_t b3ComputeLayoutHash( void ) +{ + uint32_t h = 2166136261u; +#define MIX( x ) \ + h ^= (uint32_t)( x ); \ + h *= 16777619u; + MIX( sizeof( b3Body ) ) + MIX( sizeof( b3BodySim ) ) + MIX( sizeof( b3BodyState ) ) + MIX( sizeof( b3Shape ) ) + MIX( sizeof( b3Contact ) ) + MIX( sizeof( b3Manifold ) ) + MIX( sizeof( b3Joint ) ) + MIX( sizeof( b3JointSim ) ) + MIX( sizeof( b3Island ) ) + MIX( sizeof( b3IslandSim ) ) + MIX( sizeof( b3ContactLink ) ) + MIX( sizeof( b3JointLink ) ) + MIX( sizeof( b3Sensor ) ) + MIX( sizeof( b3Visitor ) ) + MIX( sizeof( b3SolverSet ) ) + MIX( sizeof( b3GraphColor ) ) + MIX( sizeof( b3DynamicTree ) ) + MIX( sizeof( b3TreeNode ) ) + MIX( sizeof( b3SetItem ) ) + MIX( sizeof( b3IdPool ) ) + MIX( sizeof( b3SurfaceMaterial ) ) + MIX( sizeof( b3ContactSpec ) ) + MIX( sizeof( b3TriangleCache ) ) + MIX( B3_GRAPH_COLOR_COUNT ) + MIX( b3_bodyTypeCount ) + MIX( sizeof( void* ) ) +#undef MIX + return h; +} + +typedef struct b3SnapHeader +{ + uint32_t magic; + uint32_t version; + uint32_t layoutHash; + uint32_t flags; +} b3SnapHeader; + +// Bounds-checked read cursor +typedef struct b3SnapReader +{ + const uint8_t* data; + int cursor; + int size; + bool ok; +} b3SnapReader; + +static void b3SnapRCheck( b3SnapReader* r, int need ) +{ + if ( need < 0 || (int64_t)r->cursor + (int64_t)need > (int64_t)r->size ) + { + r->ok = false; + } +} + +static void b3SnapR_Bytes( b3SnapReader* r, void* dst, int n ) +{ + b3SnapRCheck( r, n ); + if ( !r->ok ) + { + return; + } + memcpy( dst, r->data + r->cursor, n ); + r->cursor += n; +} + +static int b3SnapR_I32( b3SnapReader* r ) +{ + int32_t v = 0; + b3SnapR_Bytes( r, &v, 4 ); + return (int)v; +} + +static uint32_t b3SnapR_U32( b3SnapReader* r ) +{ + uint32_t v = 0; + b3SnapR_Bytes( r, &v, 4 ); + return v; +} + +static void b3SnapW_I32( b3RecBuffer* buf, int v ) +{ + int32_t w = (int32_t)v; + b3RecBufAppend( buf, &w, 4 ); +} + +static void b3SnapW_U32( b3RecBuffer* buf, uint32_t v ) +{ + b3RecBufAppend( buf, &v, 4 ); +} + +static void b3SnapW_Bytes( b3RecBuffer* buf, const void* src, int n ) +{ + b3RecBufAppend( buf, src, n ); +} + +// Bounds check before allocating from image +static bool b3SnapCheckCount( const b3SnapReader* r, int count, int memSize, int minStreamBytes ) +{ + if ( count < 0 || memSize < 0 || minStreamBytes < 0 ) + { + return false; + } + if ( memSize > 0 && count > 0x7FFFFFFF / memSize ) + { + return false; + } + int64_t remaining = (int64_t)r->size - (int64_t)r->cursor; + return (int64_t)count * (int64_t)minStreamBytes <= remaining; +} + +// POD array: count + raw bytes +#define b3SerPodArray( buf, arr ) \ + do \ + { \ + b3SnapW_I32( buf, ( arr ).count ); \ + if ( ( arr ).count > 0 ) \ + { \ + b3SnapW_Bytes( buf, ( arr ).data, ( arr ).count * (int)sizeof( *( arr ).data ) ); \ + } \ + } \ + while ( 0 ) + +#define b3DesPodArray( r, arr ) \ + do \ + { \ + int cnt = b3SnapR_I32( r ); \ + int elemSize = (int)sizeof( *( arr ).data ); \ + if ( ( r )->ok && b3SnapCheckCount( r, cnt, elemSize, elemSize ) == false ) \ + { \ + ( r )->ok = false; \ + } \ + if ( ( r )->ok && cnt > 0 ) \ + { \ + b3Array_Resize( arr, cnt ); \ + b3SnapR_Bytes( r, ( arr ).data, cnt * elemSize ); \ + } \ + else if ( ( r )->ok ) \ + { \ + ( arr ).count = 0; \ + } \ + } \ + while ( 0 ) + +// Id pool: nextIndex + freeArray +static void b3SerIdPool( b3RecBuffer* buf, const b3IdPool* pool ) +{ + b3SnapW_I32( buf, pool->nextIndex ); + b3SerPodArray( buf, pool->freeArray ); +} + +static void b3DesIdPool( b3SnapReader* r, b3IdPool* pool ) +{ + pool->nextIndex = b3SnapR_I32( r ); + b3DesPodArray( r, pool->freeArray ); +} + +// BitSet: blockCount + raw words +static void b3SerBitSet( b3RecBuffer* buf, const b3BitSet* bs ) +{ + b3SnapW_U32( buf, bs->blockCount ); + if ( bs->blockCount > 0 ) + { + b3SnapW_Bytes( buf, bs->bits, (int)( bs->blockCount * sizeof( uint64_t ) ) ); + } +} + +static void b3DesBitSet( b3SnapReader* r, b3BitSet* bs ) +{ + uint32_t blockCount = b3SnapR_U32( r ); + if ( r->ok && b3SnapCheckCount( r, (int)blockCount, (int)sizeof( uint64_t ), (int)sizeof( uint64_t ) ) == false ) + { + r->ok = false; + } + b3DestroyBitSet( bs ); + if ( !r->ok ) + { + return; + } + uint32_t blockCapacity = blockCount > 0 ? blockCount : 1; + bs->bits = (uint64_t*)b3Alloc( blockCapacity * sizeof( uint64_t ) ); + memset( bs->bits, 0, blockCapacity * sizeof( uint64_t ) ); + bs->blockCapacity = blockCapacity; + bs->blockCount = blockCount; + if ( blockCount > 0 ) + { + b3SnapR_Bytes( r, bs->bits, (int)( blockCount * sizeof( uint64_t ) ) ); + } +} + +// HashSet: capacity + count + raw items (probe order depends on layout) +static void b3SerHashSet( b3RecBuffer* buf, const b3HashSet* hs ) +{ + b3SnapW_U32( buf, hs->capacity ); + b3SnapW_U32( buf, hs->count ); + if ( hs->capacity > 0 ) + { + b3SnapW_Bytes( buf, hs->items, (int)( hs->capacity * sizeof( b3SetItem ) ) ); + } +} + +static void b3DesHashSet( b3SnapReader* r, b3HashSet* hs ) +{ + uint32_t cap = b3SnapR_U32( r ); + uint32_t cnt = b3SnapR_U32( r ); + bool valid = b3SnapCheckCount( r, (int)cap, (int)sizeof( b3SetItem ), (int)sizeof( b3SetItem ) ) && + ( cap & ( cap - 1 ) ) == 0 && cnt <= cap; + if ( r->ok && valid == false && ( cap != 0 || cnt != 0 ) ) + { + r->ok = false; + } + b3DestroySet( hs ); + if ( !r->ok ) + { + return; + } + if ( cap > 0 ) + { + hs->items = (b3SetItem*)b3Alloc( cap * sizeof( b3SetItem ) ); + hs->capacity = cap; + hs->count = cnt; + b3SnapR_Bytes( r, hs->items, (int)( cap * sizeof( b3SetItem ) ) ); + } + else + { + hs->items = NULL; + hs->capacity = 0; + hs->count = 0; + } +} + +// DynamicTree: version, scalars, full nodeCapacity nodes (rebuild scratch excluded) +static void b3SerTree( b3RecBuffer* buf, const b3DynamicTree* tree ) +{ + b3SnapW_Bytes( buf, &tree->version, sizeof( uint64_t ) ); + b3SnapW_I32( buf, tree->root ); + b3SnapW_I32( buf, tree->nodeCount ); + b3SnapW_I32( buf, tree->nodeCapacity ); + b3SnapW_I32( buf, tree->freeList ); + b3SnapW_I32( buf, tree->proxyCount ); + if ( tree->nodeCapacity > 0 ) + { + b3SnapW_Bytes( buf, tree->nodes, tree->nodeCapacity * (int)sizeof( b3TreeNode ) ); + } +} + +static void b3DesTree( b3SnapReader* r, b3DynamicTree* tree ) +{ + uint64_t version; + b3SnapR_Bytes( r, &version, sizeof( uint64_t ) ); + int root = b3SnapR_I32( r ); + int nodeCount = b3SnapR_I32( r ); + int nodeCapacity = b3SnapR_I32( r ); + int freeList = b3SnapR_I32( r ); + int proxyCount = b3SnapR_I32( r ); + + if ( r->ok && b3SnapCheckCount( r, nodeCapacity, (int)sizeof( b3TreeNode ), (int)sizeof( b3TreeNode ) ) == false ) + { + r->ok = false; + } + + // Free existing allocation including any rebuild scratch + b3Free( tree->nodes, tree->nodeCapacity * (int)sizeof( b3TreeNode ) ); + b3Free( tree->leafIndices, tree->rebuildCapacity * (int)sizeof( int ) ); + b3Free( tree->leafBoxes, tree->rebuildCapacity * (int)sizeof( b3AABB ) ); + b3Free( tree->leafCenters, tree->rebuildCapacity * (int)sizeof( b3Vec3 ) ); + b3Free( tree->binIndices, tree->rebuildCapacity * (int)sizeof( int ) ); + tree->nodes = NULL; + tree->leafIndices = NULL; + tree->leafBoxes = NULL; + tree->leafCenters = NULL; + tree->binIndices = NULL; + tree->nodeCapacity = 0; + tree->rebuildCapacity = 0; + + if ( !r->ok ) + { + return; + } + + tree->version = version; + tree->root = root; + tree->nodeCount = nodeCount; + tree->nodeCapacity = nodeCapacity; + tree->freeList = freeList; + tree->proxyCount = proxyCount; + + if ( nodeCapacity > 0 ) + { + tree->nodes = (b3TreeNode*)b3Alloc( nodeCapacity * (int)sizeof( b3TreeNode ) ); + b3SnapR_Bytes( r, tree->nodes, nodeCapacity * (int)sizeof( b3TreeNode ) ); + } +} + +// Solver set: setIndex + 4 arrays (note: contactIndices is int array, not contactSims) +static void b3SerSolverSet( b3RecBuffer* buf, const b3SolverSet* set ) +{ + b3SnapW_I32( buf, set->setIndex ); + b3SerPodArray( buf, set->bodySims ); + b3SerPodArray( buf, set->bodyStates ); + b3SerPodArray( buf, set->jointSims ); + b3SerPodArray( buf, set->contactIndices ); + b3SerPodArray( buf, set->islandSims ); +} + +static void b3DesSolverSet( b3SnapReader* r, b3SolverSet* set ) +{ + set->setIndex = b3SnapR_I32( r ); + b3DesPodArray( r, set->bodySims ); + b3DesPodArray( r, set->bodyStates ); + b3DesPodArray( r, set->jointSims ); + b3DesPodArray( r, set->contactIndices ); + b3DesPodArray( r, set->islandSims ); +} + +static void b3SerNames( b3RecBuffer* buf, const b3NameCache* cache ) +{ + b3SnapW_I32( buf, cache->entries.count ); + int count = cache->entries.count; + for ( int i = 0; i < count; ++i ) + { + const b3NameEntry* entry = cache->entries.data + i; + b3SnapW_U32( buf, entry->hash ); + b3SnapW_I32( buf, entry->length ); + b3RecBufAppend( buf, entry->name, entry->length ); + } +} + +static void b3DesNames( b3SnapReader* r, b3NameCache* cache ) +{ + int count = b3SnapR_I32( r ); + + if ( r->ok && b3SnapCheckCount( r, count, (int)sizeof( b3NameEntry ), 8 ) == false ) + { + r->ok = false; + } + + if ( r->ok == false ) + { + return; + } + + b3Array_Reserve( cache->entries, count ); + for ( int i = 0; i < count; ++i ) + { + uint32_t hash = b3SnapR_U32( r ); + int length = b3SnapR_I32( r ); + if ( r->ok == false || length < 0 || length > r->size - r->cursor ) + { + r->ok = false; + return; + } + char* name = b3Alloc( length + 1 ); + b3SnapR_Bytes( r, name, length ); + name[length] = 0; + b3LoadName( cache, hash, name, length ); + } +} + +// Graph color: bodySet (non-overflow only) + jointSims + convexContacts + contacts +static void b3SerGraphColor( b3RecBuffer* buf, const b3GraphColor* color, bool isOverflow ) +{ + if ( !isOverflow ) + { + b3SerBitSet( buf, &color->bodySet ); + } + b3SerPodArray( buf, color->jointSims ); + b3SerPodArray( buf, color->convexContacts ); + b3SerPodArray( buf, color->contacts ); + // wideConstraints / manifoldConstraints / contactConstraints are transient, not serialized +} + +static void b3DesGraphColor( b3SnapReader* r, b3GraphColor* color, bool isOverflow ) +{ + if ( !isOverflow ) + { + b3DesBitSet( r, &color->bodySet ); + } + b3DesPodArray( r, color->jointSims ); + b3DesPodArray( r, color->convexContacts ); + b3DesPodArray( r, color->contacts ); + // Transient pointers left at NULL/0 from shell +} + +// World simulation scalars (never host/callback/worker state) +static void b3SerWorldConfig( b3RecBuffer* buf, const b3World* world ) +{ + b3SnapW_Bytes( buf, &world->gravity, sizeof( b3Vec3 ) ); + b3SnapW_Bytes( buf, &world->hitEventThreshold, sizeof( float ) ); + b3SnapW_Bytes( buf, &world->restitutionThreshold, sizeof( float ) ); + b3SnapW_Bytes( buf, &world->maxLinearSpeed, sizeof( float ) ); + b3SnapW_Bytes( buf, &world->contactSpeed, sizeof( float ) ); + b3SnapW_Bytes( buf, &world->contactHertz, sizeof( float ) ); + b3SnapW_Bytes( buf, &world->contactDampingRatio, sizeof( float ) ); + b3SnapW_Bytes( buf, &world->contactRecycleDistance, sizeof( float ) ); + b3SnapW_Bytes( buf, &world->stepIndex, sizeof( uint64_t ) ); + b3SnapW_I32( buf, world->splitIslandId ); + b3SnapW_Bytes( buf, &world->inv_h, sizeof( float ) ); + b3SnapW_Bytes( buf, &world->inv_dt, sizeof( float ) ); + b3SnapW_I32( buf, world->endEventArrayIndex ); + b3SnapW_Bytes( buf, &world->maxCapacity, sizeof( b3Capacity ) ); + uint8_t flags = 0; + flags |= world->enableSleep ? 0x01u : 0u; + flags |= world->enableWarmStarting ? 0x02u : 0u; + flags |= world->enableContinuous ? 0x04u : 0u; + flags |= world->enableSpeculative ? 0x08u : 0u; + b3RecBufAppend( buf, &flags, 1 ); +} + +static void b3DesWorldConfig( b3SnapReader* r, b3World* world ) +{ + b3SnapR_Bytes( r, &world->gravity, sizeof( b3Vec3 ) ); + b3SnapR_Bytes( r, &world->hitEventThreshold, sizeof( float ) ); + b3SnapR_Bytes( r, &world->restitutionThreshold, sizeof( float ) ); + b3SnapR_Bytes( r, &world->maxLinearSpeed, sizeof( float ) ); + b3SnapR_Bytes( r, &world->contactSpeed, sizeof( float ) ); + b3SnapR_Bytes( r, &world->contactHertz, sizeof( float ) ); + b3SnapR_Bytes( r, &world->contactDampingRatio, sizeof( float ) ); + b3SnapR_Bytes( r, &world->contactRecycleDistance, sizeof( float ) ); + b3SnapR_Bytes( r, &world->stepIndex, sizeof( uint64_t ) ); + world->splitIslandId = b3SnapR_I32( r ); + b3SnapR_Bytes( r, &world->inv_h, sizeof( float ) ); + b3SnapR_Bytes( r, &world->inv_dt, sizeof( float ) ); + world->endEventArrayIndex = b3SnapR_I32( r ); + b3SnapR_Bytes( r, &world->maxCapacity, sizeof( b3Capacity ) ); + uint8_t flags = 0; + b3SnapR_Bytes( r, &flags, 1 ); + world->enableSleep = ( flags & 0x01u ) != 0; + world->enableWarmStarting = ( flags & 0x02u ) != 0; + world->enableContinuous = ( flags & 0x04u ) != 0; + world->enableSpeculative = ( flags & 0x08u ) != 0; +} + +// Shapes carry pointer fields: materials, userData, userShape, and the geometry union. +// Serialize the POD scalars with pointers nulled, then the owned materials array, then geometry. +// A single material lives inline in the struct image. +// Hull/mesh/heightField/compound are interned into the recording registry; sphere/capsule inline. +static void b3SerShapes( b3RecBuffer* buf, b3World* world, b3Recording* rec ) +{ + int count = world->shapes.count; + b3SnapW_I32( buf, count ); + + for ( int i = 0; i < count; ++i ) + { + b3Shape shape = world->shapes.data[i]; + bool isLive = ( shape.id == i ); + + // Null out pointer fields before writing the raw struct + shape.materials = NULL; + shape.userData = NULL; + shape.userShape = NULL; + // Zero the geometry union so free-slot images have deterministic bytes + if ( !isLive ) + { + memset( &shape.capsule, 0, sizeof( shape.capsule ) ); + } + b3SnapW_Bytes( buf, &shape, sizeof( b3Shape ) ); + + if ( !isLive ) + { + // Free slot: no materials or geometry + b3SnapW_I32( buf, 0 ); // materialCount + b3SnapW_I32( buf, -1 ); // geometry kind sentinel + continue; + } + + // Owned material array. Only multi material meshes and compounds have one. A single material + // already rode along inline in the struct image, so write a zero length for it. + const b3Shape* src = world->shapes.data + i; + if ( src->materials != NULL ) + { + b3SnapW_I32( buf, src->materialCount ); + b3SnapW_Bytes( buf, src->materials, src->materialCount * (int)sizeof( b3SurfaceMaterial ) ); + } + else + { + b3SnapW_I32( buf, 0 ); + } + + // Geometry + switch ( src->type ) + { + case b3_sphereShape: + b3SnapW_I32( buf, (int)b3_sphereShape ); + b3SnapW_Bytes( buf, &src->sphere, sizeof( b3Sphere ) ); + break; + case b3_capsuleShape: + b3SnapW_I32( buf, (int)b3_capsuleShape ); + b3SnapW_Bytes( buf, &src->capsule, sizeof( b3Capsule ) ); + break; + case b3_hullShape: + { + b3SnapW_I32( buf, (int)b3_hullShape ); + uint32_t gid = b3RecInternHull( rec, src->hull ); + b3SnapW_U32( buf, gid ); + break; + } + case b3_meshShape: + { + b3SnapW_I32( buf, (int)b3_meshShape ); + uint32_t gid = b3RecInternMesh( rec, src->mesh.data ); + b3SnapW_U32( buf, gid ); + b3SnapW_Bytes( buf, &src->mesh.scale, sizeof( b3Vec3 ) ); + break; + } + case b3_heightShape: + { + b3SnapW_I32( buf, (int)b3_heightShape ); + uint32_t gid = b3RecInternHeightField( rec, src->heightField ); + b3SnapW_U32( buf, gid ); + break; + } + case b3_compoundShape: + { + b3SnapW_I32( buf, (int)b3_compoundShape ); + uint32_t gid = b3RecInternCompound( rec, src->compound ); + b3SnapW_U32( buf, gid ); + break; + } + default: + // A live shape must have a known geometry type. Fail loudly rather than emit a shape + // with no geometry that would silently lose its collision on restore. + B3_ASSERT( false ); + b3SnapW_I32( buf, -1 ); + break; + } + } +} + +static void b3DesShapes( b3SnapReader* r, b3World* world, b3RecReader* rdr ) +{ + int count = b3SnapR_I32( r ); + if ( r->ok && b3SnapCheckCount( r, count, (int)sizeof( b3Shape ), (int)sizeof( b3Shape ) ) == false ) + { + r->ok = false; + } + if ( !r->ok ) + { + return; + } + + // Save renderer handles before the array is wiped. A keyframe restore is a deterministic replay + // state, so a live shape that still occupies the same slot with the same generation is the same + // shape with the same geometry. Carrying its handle over avoids tearing down and rebuilding every + // GPU mesh on each seek, which the host (a 3D renderer) would otherwise pay for. Handles that are + // not reclaimed below belong to shapes that are gone or were replaced, and get released so the host + // pool does not leak across seeks. Box2D has no such handles, so its restore skips all of this. + int oldShapeCount = world->shapes.count; + void** savedUserShape = NULL; + uint16_t* savedGeneration = NULL; + if ( oldShapeCount > 0 ) + { + savedUserShape = (void**)b3Alloc( (size_t)oldShapeCount * sizeof( void* ) ); + savedGeneration = (uint16_t*)b3Alloc( (size_t)oldShapeCount * sizeof( uint16_t ) ); + for ( int i = 0; i < oldShapeCount; ++i ) + { + b3Shape* old = world->shapes.data + i; + bool oldLive = ( old->id == i ); + savedUserShape[i] = oldLive ? old->userShape : NULL; + savedGeneration[i] = old->generation; + } + } + + b3Array_Resize( world->shapes, count ); + memset( world->shapes.data, 0, (size_t)count * sizeof( b3Shape ) ); + + for ( int i = 0; i < count && r->ok; ++i ) + { + b3Shape* dst = world->shapes.data + i; + b3SnapR_Bytes( r, dst, sizeof( b3Shape ) ); + // Pointer fields were written as NULL; set them cleanly + dst->materials = NULL; + dst->userData = NULL; + dst->userShape = NULL; + memset( &dst->capsule, 0, sizeof( dst->capsule ) ); + + bool isLive = ( dst->id == i ); + + // Carry the renderer handle over when the same shape still occupies this slot. Consumed + // handles are nulled so the teardown sweep only releases the ones that vanished. + if ( isLive && i < oldShapeCount && savedUserShape != NULL && savedUserShape[i] != NULL && + savedGeneration[i] == dst->generation ) + { + dst->userShape = savedUserShape[i]; + savedUserShape[i] = NULL; + } + + // Serializer writes: matCount, matData, geoKind, geoData + int matCount = b3SnapR_I32( r ); + + if ( !r->ok ) + { + break; + } + + if ( !isLive ) + { + // Free slot: matCount=0, geoKind=-1 + (void)matCount; + b3SnapR_I32( r ); // consume the geoKind sentinel + continue; + } + + // Owned material array (written before geoKind in serializer). A zero length means the single + // material is already inline in the restored struct image, so leave materialCount as restored. + if ( matCount > 0 ) + { + if ( b3SnapCheckCount( r, matCount, (int)sizeof( b3SurfaceMaterial ), (int)sizeof( b3SurfaceMaterial ) ) == false ) + { + r->ok = false; + break; + } + dst->materialCount = matCount; + dst->materials = (b3SurfaceMaterial*)b3Alloc( (size_t)matCount * sizeof( b3SurfaceMaterial ) ); + b3SnapR_Bytes( r, dst->materials, matCount * (int)sizeof( b3SurfaceMaterial ) ); + } + else + { + dst->materials = NULL; + } + + int geoKind = b3SnapR_I32( r ); + + // Geometry + switch ( (b3ShapeType)geoKind ) + { + case b3_sphereShape: + b3SnapR_Bytes( r, &dst->sphere, sizeof( b3Sphere ) ); + break; + case b3_capsuleShape: + b3SnapR_Bytes( r, &dst->capsule, sizeof( b3Capsule ) ); + break; + case b3_hullShape: + { + uint32_t gid = b3SnapR_U32( r ); + if ( !r->ok ) + { + break; + } + if ( rdr == NULL || gid >= (uint32_t)rdr->slotCount ) + { + r->ok = false; + break; + } + // Hull is cloned into the world DB; pass raw bytes directly + b3RegistrySlot* slot = rdr->slots + gid; + dst->hull = b3AddHullToDatabase( world, (const b3HullData*)slot->bytes ); + break; + } + case b3_meshShape: + { + uint32_t gid = b3SnapR_U32( r ); + b3Vec3 scale; + b3SnapR_Bytes( r, &scale, sizeof( b3Vec3 ) ); + if ( !r->ok ) + { + break; + } + if ( rdr == NULL || gid >= (uint32_t)rdr->slotCount ) + { + r->ok = false; + break; + } + b3RegistrySlot* slot = rdr->slots + gid; + // Mesh is a self-contained blob used by reference; point straight at the pristine bytes. + dst->mesh.data = (const b3MeshData*)slot->bytes; + dst->mesh.scale = scale; + break; + } + case b3_heightShape: + { + uint32_t gid = b3SnapR_U32( r ); + if ( !r->ok ) + { + break; + } + if ( rdr == NULL || gid >= (uint32_t)rdr->slotCount ) + { + r->ok = false; + break; + } + b3RegistrySlot* slot = rdr->slots + gid; + // Self-contained blob used by reference; point straight at the pristine bytes. + dst->heightField = (const b3HeightFieldData*)slot->bytes; + break; + } + case b3_compoundShape: + { + uint32_t gid = b3SnapR_U32( r ); + if ( !r->ok ) + { + break; + } + if ( rdr == NULL || gid >= (uint32_t)rdr->slotCount ) + { + r->ok = false; + break; + } + b3RegistrySlot* slot = rdr->slots + gid; + if ( slot->live == NULL ) + { + slot->live = b3Alloc( (size_t)slot->byteCount ); + memcpy( slot->live, slot->bytes, (size_t)slot->byteCount ); + b3ConvertBytesToCompound( (uint8_t*)slot->live, slot->byteCount ); + } + dst->compound = (const b3CompoundData*)slot->live; + break; + } + default: + // Unknown geometry kind means a corrupt or unsupported snapshot. Fail the load instead + // of leaving a shape with no geometry. + r->ok = false; + break; + } + } + + // Release handles for shapes that are gone or were replaced this restore, so the host pool and any + // GPU resources they pinned do not leak across seeks. + if ( savedUserShape != NULL ) + { + for ( int i = 0; i < oldShapeCount; ++i ) + { + if ( savedUserShape[i] != NULL && world->destroyDebugShape != NULL ) + { + world->destroyDebugShape( savedUserShape[i], world->userDebugShapeContext ); + } + } + b3Free( savedUserShape, (size_t)oldShapeCount * sizeof( void* ) ); + b3Free( savedGeneration, (size_t)oldShapeCount * sizeof( uint16_t ) ); + } +} + +// Contact serialization. b3Contact is not fully POD: +// - manifolds: heap array of b3Manifold, allocated via b3AllocateManifolds +// - meshContact.triangleCache: heap b3Array, active when b3_simMeshContact flag is set +// Serialize: raw struct (with nulled manifolds/triangleCache), then manifolds, then triangleCache. +static void b3SerContacts( b3RecBuffer* buf, b3World* world ) +{ + int count = world->contacts.count; + b3SnapW_I32( buf, count ); + + for ( int i = 0; i < count; ++i ) + { + const b3Contact* c = world->contacts.data + i; + bool isLive = ( c->contactId == i ); + + // Write raw struct with pointer fields zeroed + b3Contact copy = *c; + copy.manifolds = NULL; + copy.bodySimIndexA = B3_NULL_INDEX; + copy.bodySimIndexB = B3_NULL_INDEX; + if ( copy.flags & b3_simMeshContact ) + { + copy.meshContact.triangleCache.data = NULL; + copy.meshContact.triangleCache.count = 0; + copy.meshContact.triangleCache.capacity = 0; + } + b3SnapW_Bytes( buf, ©, sizeof( b3Contact ) ); + + if ( !isLive ) + { + // Free slot: no heap data + b3SnapW_I32( buf, 0 ); // manifoldCount + // No triangleCache + continue; + } + + // Manifolds + b3SnapW_I32( buf, c->manifoldCount ); + if ( c->manifoldCount > 0 && c->manifolds != NULL ) + { + b3SnapW_Bytes( buf, c->manifolds, c->manifoldCount * (int)sizeof( b3Manifold ) ); + } + + // Mesh triangleCache + if ( c->flags & b3_simMeshContact ) + { + b3SnapW_I32( buf, c->meshContact.triangleCache.count ); + if ( c->meshContact.triangleCache.count > 0 ) + { + b3SnapW_Bytes( buf, c->meshContact.triangleCache.data, + c->meshContact.triangleCache.count * (int)sizeof( b3TriangleCache ) ); + } + } + } +} + +static void b3DesContacts( b3SnapReader* r, b3World* world ) +{ + int count = b3SnapR_I32( r ); + if ( r->ok && b3SnapCheckCount( r, count, (int)sizeof( b3Contact ), (int)sizeof( b3Contact ) ) == false ) + { + r->ok = false; + } + if ( !r->ok ) + { + return; + } + + b3Array_Resize( world->contacts, count ); + memset( world->contacts.data, 0, (size_t)count * sizeof( b3Contact ) ); + + for ( int i = 0; i < count && r->ok; ++i ) + { + b3Contact* dst = world->contacts.data + i; + b3SnapR_Bytes( r, dst, sizeof( b3Contact ) ); + dst->manifolds = NULL; + dst->bodySimIndexA = B3_NULL_INDEX; + dst->bodySimIndexB = B3_NULL_INDEX; + if ( dst->flags & b3_simMeshContact ) + { + dst->meshContact.triangleCache.data = NULL; + dst->meshContact.triangleCache.count = 0; + dst->meshContact.triangleCache.capacity = 0; + } + + bool isLive = ( dst->contactId == i ); + + int manifoldCount = b3SnapR_I32( r ); + + if ( !r->ok ) + { + break; + } + + if ( isLive && manifoldCount > 0 ) + { + if ( b3SnapCheckCount( r, manifoldCount, (int)sizeof( b3Manifold ), (int)sizeof( b3Manifold ) ) == false ) + { + r->ok = false; + break; + } + dst->manifolds = b3AllocateManifolds( world, manifoldCount ); + dst->manifoldCount = manifoldCount; + b3SnapR_Bytes( r, dst->manifolds, manifoldCount * (int)sizeof( b3Manifold ) ); + } + else + { + dst->manifolds = NULL; + dst->manifoldCount = 0; + } + + // Mesh triangleCache + if ( isLive && ( dst->flags & b3_simMeshContact ) ) + { + int cacheCount = b3SnapR_I32( r ); + if ( !r->ok ) + { + break; + } + if ( cacheCount > 0 ) + { + if ( b3SnapCheckCount( r, cacheCount, (int)sizeof( b3TriangleCache ), (int)sizeof( b3TriangleCache ) ) == false ) + { + r->ok = false; + break; + } + b3Array_Resize( dst->meshContact.triangleCache, cacheCount ); + b3SnapR_Bytes( r, dst->meshContact.triangleCache.data, cacheCount * (int)sizeof( b3TriangleCache ) ); + } + } + } +} + +// Free per-object heap that b3DeserializeIntoShell will overwrite, +// so restoring over a populated world doesn't leak. +static void b3FreeLiveSimElements( b3World* world ) +{ + // Shape heap: materials and hull DB references + for ( int i = 0; i < world->shapes.count; ++i ) + { + b3Shape* s = world->shapes.data + i; + if ( s->id != i ) + { + continue; + } + // A single material lives inline (materials == NULL). Multi material meshes and compounds own + // the array, so free it exactly as b3DestroyShapeAllocations does. + if ( s->materials != NULL ) + { + b3Free( s->materials, (size_t)s->materialCount * sizeof( b3SurfaceMaterial ) ); + s->materials = NULL; + s->materialCount = 0; + } + // Hull is ref-counted in the world DB; release before overwrite so re-adding is ref-neutral. + if ( s->type == b3_hullShape && s->hull != NULL ) + { + b3RemoveHullFromDatabase( world, s->hull ); + s->hull = NULL; + } + // name / userData / userShape are host-owned; do not free + } + + // Contact heap: manifolds + mesh triangleCache + for ( int i = 0; i < world->contacts.count; ++i ) + { + b3Contact* c = world->contacts.data + i; + if ( c->contactId == i ) + { + if ( c->manifolds != NULL ) + { + b3FreeManifolds( world, c->manifolds, c->manifoldCount ); + c->manifolds = NULL; + c->manifoldCount = 0; + } + if ( c->flags & b3_simMeshContact ) + { + b3Array_Destroy( c->meshContact.triangleCache ); + } + } + } + + // Sensor heap: inner arrays + for ( int i = 0; i < world->sensors.count; ++i ) + { + b3Sensor* sensor = world->sensors.data + i; + b3Array_Destroy( sensor->hits ); + b3Array_Destroy( sensor->overlaps1 ); + b3Array_Destroy( sensor->overlaps2 ); + } + + // Island heap: inner arrays + for ( int i = 0; i < world->islands.count; ++i ) + { + b3Island* island = world->islands.data + i; + b3Array_Destroy( island->bodies ); + b3Array_Destroy( island->contacts ); + b3Array_Destroy( island->joints ); + } +} + +int b3SerializeWorld( b3World* world, b3RecBuffer* buf, b3Recording* rec ) +{ + int startSize = buf->size; + + // Snapshot header + b3SnapHeader hdr; + hdr.magic = B3_SNAP_MAGIC; + hdr.version = B3_SNAP_VERSION; + hdr.layoutHash = b3ComputeLayoutHash(); + hdr.flags = B3_ENABLE_VALIDATION ? B3_SNAP_FLAG_VALIDATION : 0u; +#if defined( BOX3D_DOUBLE_PRECISION ) + hdr.flags |= B3_SNAP_FLAG_DOUBLE_PRECISION; +#endif + b3SnapW_Bytes( buf, &hdr, (int)sizeof( hdr ) ); + + // World scalars + b3SerWorldConfig( buf, world ); + + // 6 id pools (Box3D has no chainIdPool) + b3SerIdPool( buf, &world->bodyIdPool ); + b3SerIdPool( buf, &world->shapeIdPool ); + b3SerIdPool( buf, &world->contactIdPool ); + b3SerIdPool( buf, &world->jointIdPool ); + b3SerIdPool( buf, &world->islandIdPool ); + b3SerIdPool( buf, &world->solverSetIdPool ); + + // Solver sets + int setCount = world->solverSets.count; + b3SnapW_I32( buf, setCount ); + for ( int i = 0; i < setCount; ++i ) + { + b3SerSolverSet( buf, world->solverSets.data + i ); + } + + // Sparse body array (userData is host wiring, zero it on the copy) + { + int bodyCount = world->bodies.count; + b3SnapW_I32( buf, bodyCount ); + for ( int i = 0; i < bodyCount; ++i ) + { + b3Body elem = world->bodies.data[i]; + elem.userData = NULL; + b3SnapW_Bytes( buf, &elem, sizeof( b3Body ) ); + } + } + + // Shape sparse array with geometry interning + b3SerShapes( buf, world, rec ); + + // Contact sparse array with manifold and mesh triangleCache + b3SerContacts( buf, world ); + + // Joint sparse array (userData scrubbed) + { + int jointCount = world->joints.count; + b3SnapW_I32( buf, jointCount ); + for ( int i = 0; i < jointCount; ++i ) + { + b3Joint elem = world->joints.data[i]; + elem.userData = NULL; + b3SnapW_Bytes( buf, &elem, sizeof( b3Joint ) ); + } + } + + // Sensors: shapeId + 3 inner arrays each + { + int sensorCount = world->sensors.count; + b3SnapW_I32( buf, sensorCount ); + for ( int i = 0; i < sensorCount; ++i ) + { + b3Sensor* s = world->sensors.data + i; + b3SnapW_I32( buf, s->shapeId ); + b3SerPodArray( buf, s->hits ); + b3SerPodArray( buf, s->overlaps1 ); + b3SerPodArray( buf, s->overlaps2 ); + } + } + + // Islands: 4 scalars + 3 inner arrays each + { + int islandCount = world->islands.count; + b3SnapW_I32( buf, islandCount ); + for ( int i = 0; i < islandCount; ++i ) + { + b3Island* island = world->islands.data + i; + b3SnapW_I32( buf, island->setIndex ); + b3SnapW_I32( buf, island->localIndex ); + b3SnapW_I32( buf, island->islandId ); + b3SnapW_I32( buf, island->constraintRemoveCount ); + b3SerPodArray( buf, island->bodies ); + b3SerPodArray( buf, island->contacts ); + b3SerPodArray( buf, island->joints ); + } + } + + // Broad phase + b3BroadPhase* bp = &world->broadPhase; + for ( int t = 0; t < b3_bodyTypeCount; ++t ) + { + b3SerTree( buf, &bp->trees[t] ); + } + for ( int t = 0; t < b3_bodyTypeCount; ++t ) + { + b3SerBitSet( buf, &bp->movedProxies[t] ); + } + b3SerPodArray( buf, bp->moveArray ); + b3SerHashSet( buf, &bp->pairSet ); + + // Constraint graph + b3ConstraintGraph* graph = &world->constraintGraph; + for ( int c = 0; c < B3_GRAPH_COLOR_COUNT; ++c ) + { + b3SerGraphColor( buf, &graph->colors[c], c == B3_OVERFLOW_INDEX ); + } + + b3SerNames( buf, &world->names ); + + return buf->size - startSize; +} + +bool b3DeserializeIntoShell( const uint8_t* data, int size, b3World* world, b3RecReader* rdr ) +{ + if ( data == NULL || size < (int)sizeof( b3SnapHeader ) ) + { + return false; + } + + // Validate header + b3SnapHeader hdr; + memcpy( &hdr, data, sizeof( hdr ) ); + if ( hdr.magic != B3_SNAP_MAGIC || hdr.version != B3_SNAP_VERSION ) + { + printf( "b3DeserializeIntoShell: bad magic/version\n" ); + return false; + } + bool imageDouble = ( hdr.flags & B3_SNAP_FLAG_DOUBLE_PRECISION ) != 0; +#if defined( BOX3D_DOUBLE_PRECISION ) + bool buildDouble = true; +#else + bool buildDouble = false; +#endif + if ( imageDouble != buildDouble ) + { + printf( "b3DeserializeIntoShell: precision mismatch\n" ); + return false; + } + if ( hdr.layoutHash != b3ComputeLayoutHash() ) + { + printf( "b3DeserializeIntoShell: layout hash mismatch\n" ); + return false; + } + + b3SnapReader readerStorage; + b3SnapReader* r = &readerStorage; + r->data = data; + r->cursor = (int)sizeof( b3SnapHeader ); + r->size = size; + r->ok = true; + + // Free existing per-object heap before overwriting + b3FreeLiveSimElements( world ); + + // 1. World scalars + b3DesWorldConfig( r, world ); + + // 2. 6 id pools; destroy the pre-created sets' pool state first + b3DesIdPool( r, &world->bodyIdPool ); + b3DesIdPool( r, &world->shapeIdPool ); + b3DesIdPool( r, &world->contactIdPool ); + b3DesIdPool( r, &world->jointIdPool ); + b3DesIdPool( r, &world->islandIdPool ); + b3DesIdPool( r, &world->solverSetIdPool ); + + // 3. Solver sets: destroy inner arrays of existing sets first + for ( int i = 0; i < world->solverSets.count; ++i ) + { + b3SolverSet* set = world->solverSets.data + i; + b3Array_Destroy( set->bodySims ); + b3Array_Destroy( set->bodyStates ); + b3Array_Destroy( set->jointSims ); + b3Array_Destroy( set->contactIndices ); + b3Array_Destroy( set->islandSims ); + } + + int setCount = b3SnapR_I32( r ); + if ( r->ok && b3SnapCheckCount( r, setCount, (int)sizeof( b3SolverSet ), 6 * (int)sizeof( int ) ) == false ) + { + r->ok = false; + } + if ( r->ok ) + { + b3Array_Resize( world->solverSets, setCount ); + memset( world->solverSets.data, 0, (size_t)setCount * sizeof( b3SolverSet ) ); + for ( int i = 0; i < setCount; ++i ) + { + b3DesSolverSet( r, world->solverSets.data + i ); + } + } + + if ( !r->ok ) + { + return false; + } + + // 4. Body sparse array + { + int bodyCount = b3SnapR_I32( r ); + if ( r->ok && b3SnapCheckCount( r, bodyCount, (int)sizeof( b3Body ), (int)sizeof( b3Body ) ) == false ) + { + r->ok = false; + } + if ( r->ok ) + { + b3Array_Resize( world->bodies, bodyCount ); + for ( int i = 0; i < bodyCount; ++i ) + { + b3SnapR_Bytes( r, world->bodies.data + i, sizeof( b3Body ) ); + world->bodies.data[i].userData = NULL; + } + } + } + + if ( !r->ok ) + { + return false; + } + + // 5. Shape sparse array + b3DesShapes( r, world, rdr ); + + if ( !r->ok ) + { + return false; + } + + // 6. Contact sparse array + b3DesContacts( r, world ); + + if ( !r->ok ) + { + return false; + } + + // 7. Joint sparse array + { + int jointCount = b3SnapR_I32( r ); + if ( r->ok && b3SnapCheckCount( r, jointCount, (int)sizeof( b3Joint ), (int)sizeof( b3Joint ) ) == false ) + { + r->ok = false; + } + if ( r->ok ) + { + b3Array_Resize( world->joints, jointCount ); + for ( int i = 0; i < jointCount; ++i ) + { + b3SnapR_Bytes( r, world->joints.data + i, sizeof( b3Joint ) ); + world->joints.data[i].userData = NULL; + } + } + } + + // 8. Sensors + { + b3Array_Destroy( world->sensors ); + b3Array_Create( world->sensors ); + + int sensorCount = b3SnapR_I32( r ); + if ( r->ok && b3SnapCheckCount( r, sensorCount, (int)sizeof( b3Sensor ), 4 * (int)sizeof( int ) ) == false ) + { + r->ok = false; + } + if ( r->ok ) + { + b3Array_Resize( world->sensors, sensorCount ); + memset( world->sensors.data, 0, (size_t)sensorCount * sizeof( b3Sensor ) ); + } + + for ( int i = 0; i < sensorCount && r->ok; ++i ) + { + b3Sensor* s = world->sensors.data + i; + s->shapeId = b3SnapR_I32( r ); + b3Array_Create( s->hits ); + b3Array_Create( s->overlaps1 ); + b3Array_Create( s->overlaps2 ); + b3DesPodArray( r, s->hits ); + b3DesPodArray( r, s->overlaps1 ); + b3DesPodArray( r, s->overlaps2 ); + } + } + + // 9. Islands + { + b3Array_Destroy( world->islands ); + b3Array_Create( world->islands ); + + int islandCount = b3SnapR_I32( r ); + if ( r->ok && b3SnapCheckCount( r, islandCount, (int)sizeof( b3Island ), 7 * (int)sizeof( int ) ) == false ) + { + r->ok = false; + } + if ( r->ok ) + { + b3Array_Resize( world->islands, islandCount ); + memset( world->islands.data, 0, (size_t)islandCount * sizeof( b3Island ) ); + } + + for ( int i = 0; i < islandCount && r->ok; ++i ) + { + b3Island* island = world->islands.data + i; + island->setIndex = b3SnapR_I32( r ); + island->localIndex = b3SnapR_I32( r ); + island->islandId = b3SnapR_I32( r ); + island->constraintRemoveCount = b3SnapR_I32( r ); + b3Array_Create( island->bodies ); + b3Array_Create( island->contacts ); + b3Array_Create( island->joints ); + b3DesPodArray( r, island->bodies ); + b3DesPodArray( r, island->contacts ); + b3DesPodArray( r, island->joints ); + } + } + + // 10. Broad phase + { + b3BroadPhase* bp = &world->broadPhase; + + for ( int t = 0; t < b3_bodyTypeCount; ++t ) + { + b3DesTree( r, &bp->trees[t] ); + } + for ( int t = 0; t < b3_bodyTypeCount; ++t ) + { + b3DesBitSet( r, &bp->movedProxies[t] ); + } + + b3Array_Destroy( bp->moveArray ); + b3Array_Create( bp->moveArray ); + b3DesPodArray( r, bp->moveArray ); + + b3DesHashSet( r, &bp->pairSet ); + // Transient moveResults/movePairs stay at shell's NULL/0 + } + + // 11. Constraint graph + { + b3ConstraintGraph* graph = &world->constraintGraph; + for ( int c = 0; c < B3_GRAPH_COLOR_COUNT; ++c ) + { + b3DesGraphColor( r, &graph->colors[c], c == B3_OVERFLOW_INDEX ); + } + } + + b3DesNames( r, &world->names ); + + return r->ok; +} diff --git a/vendor/box3d/src/src/world_snapshot.h b/vendor/box3d/src/src/world_snapshot.h new file mode 100644 index 000000000..7bf0065a1 --- /dev/null +++ b/vendor/box3d/src/src/world_snapshot.h @@ -0,0 +1,21 @@ +// SPDX-FileCopyrightText: 2026 Erin Catto +// SPDX-License-Identifier: MIT + +#pragma once + +#include "recording.h" +#include "recording_replay.h" + +#include +#include + +typedef struct b3World b3World; + +// Serialize the live world into buf, interning shape geometry into rec->registry. +// On success buf holds a self-contained snapshot image. Returns the byte count. +int b3SerializeWorld( b3World* world, b3RecBuffer* buf, b3Recording* rec ); + +// Overwrite a freshly-created (shell) world with the simulation state held in the +// snapshot image [data, size). Geometry references are resolved via the shared +// registry slots in rdr. Returns false on a corrupt or incompatible image. +bool b3DeserializeIntoShell( const uint8_t* data, int size, b3World* world, b3RecReader* rdr ); diff --git a/vendor/kb_text_shape/kb_text_shape_procs.odin b/vendor/kb_text_shape/kb_text_shape_procs.odin index caf7c4b9b..aa5fb0562 100644 --- a/vendor/kb_text_shape/kb_text_shape_procs.odin +++ b/vendor/kb_text_shape/kb_text_shape_procs.odin @@ -20,7 +20,9 @@ import "core:c" @(default_calling_convention="c", link_prefix="kbts_", require_results) foreign lib { SizeOfShapeContext :: proc() -> c.int --- + PlaceShapeContext2 :: proc(Allocator: allocator_function, AllocatorData: rawptr, Memory: rawptr, Flags: shape_context_flags) -> ^shape_context --- PlaceShapeContext :: proc(Allocator: allocator_function, AllocatorData: rawptr, Memory: rawptr) -> ^shape_context --- + CreateShapeContext2 :: proc(Allocator: allocator_function, AllocatorData: rawptr, Flags: shape_context_flags) -> ^shape_context --- CreateShapeContext :: proc(Allocator: allocator_function, AllocatorData: rawptr) -> ^shape_context --- DestroyShapeContext :: proc(Context: ^shape_context) --- ShapePushFont :: proc(Context: ^shape_context, Font: ^font) -> ^font --- @@ -38,6 +40,14 @@ foreign lib { ShapeManualBreak :: proc(Context: ^shape_context) --- } +@(require_results) +PlaceShapeContextFixedMemory2 :: proc "c" (Memory: []byte, Flags: shape_context_flags) -> ^shape_context { + @(default_calling_convention="c", require_results) + foreign lib { + kbts_PlaceShapeContextFixedMemory2 :: proc(Memory: rawptr, Size: c.int, Flags: shape_context_flags) -> ^shape_context --- + } + return kbts_PlaceShapeContextFixedMemory2(raw_data(Memory), c.int(len(Memory)), Flags) +} @(require_results) PlaceShapeContextFixedMemory :: proc "c" (Memory: []byte) -> ^shape_context { @(default_calling_convention="c", require_results) @@ -120,10 +130,11 @@ ShapeCodepointIteratorNext :: proc "contextless" (It: ^shape_codepoint_iterator) // @(default_calling_convention="c", link_prefix="kbts_", require_results) foreign lib { - FreeFont :: proc(Font: ^font) --- - FontIsValid :: proc(Font: ^font) -> b32 --- - PlaceBlob :: proc(Font: ^font, State: ^load_font_state, ScratchMemory: rawptr, OutputMemory: rawptr) -> load_font_error --- - GetFontInfo :: proc(Font: ^font, Info: ^font_info) --- + FreeFont :: proc(Font: ^font) --- + FontIsValid :: proc(Font: ^font) -> b32 --- + PlaceBlob :: proc(Font: ^font, State: ^load_font_state, ScratchMemory: rawptr, OutputMemory: rawptr) -> load_font_error --- + GetFontInfo :: proc(Font: ^font, Info: ^font_info) --- + GetFontInfo2 :: proc(Font: ^font, Info: ^font_info2) --- // A shape_config is a bag of pre-computed data for a specific shaping setup. SizeOfShapeConfig :: proc(Font: ^font, Script: script, Language: language) -> b32 --- @@ -145,6 +156,14 @@ foreign lib { // A single glyph_config can be shared by multiple glyphs. DestroyGlyphConfig :: proc(Config: ^glyph_config) --- + + // A shape_scratchpad holds all transient runtime shaping data. + // While the shape_config is immutable and can be trivially shared among threads, a shape_scratchpad is mutable and needs to be per-thread. + SizeOfShapeScratchpad :: proc(Config: ^shape_config) -> un --- + PlaceShapeScratchpad :: proc(Config: ^shape_config, Memory: rawptr, Allocator: allocator_function, AllocatorData: rawptr) -> shape_scratchpad --- + PlaceShapeScratchpadFixedMemory :: proc(Config: ^shape_config, Memory: rawptr, Size: c.int) -> shape_scratchpad --- + CreateShapeScratchpad :: proc(Config: ^shape_config, Allocator: allocator_function, AllocatorData: rawptr) -> shape_scratchpad --- + DestroyShapeScratchpad :: proc(Scratchpad: ^shape_scratchpad) --- } @(require_results) @@ -177,51 +196,41 @@ FontFromMemory :: proc "c" (FileData: []byte, FontIndex: c.int, Allocator: alloc @(require_results) -ShapeDirect :: proc "contextless" (Config: ^shape_config, Storage: ^glyph_storage, RunDirection: direction, Allocator: allocator_function, AllocatorData: rawptr) -> (Output: glyph_iterator, Err: shape_error) { +ShapeDirect :: proc "contextless" (Scratchpad: ^shape_scratchpad, Storage: ^glyph_storage, RunDirection: direction) -> (Output: glyph_iterator, Err: shape_error) { @(default_calling_convention="c", require_results) foreign lib { - kbts_ShapeDirect :: proc(Config: ^shape_config, Storage: ^glyph_storage, RunDirection: direction, Allocator: allocator_function, AllocatorData: rawptr, Output: ^glyph_iterator) -> shape_error --- + kbts_ShapeDirect :: proc(Scratchpad: ^shape_scratchpad, Storage: ^glyph_storage, RunDirection: direction, Output: ^glyph_iterator) -> shape_error --- } - Err = kbts_ShapeDirect(Config, Storage, RunDirection, Allocator, AllocatorData, &Output) - return -} - -@(require_results) -ShapeDirectFixedMemory :: proc "contextless" (Config: ^shape_config, Storage: ^glyph_storage, RunDirection: direction, Memory: rawptr, MemorySize: c.int) -> (Output: glyph_iterator, Err: shape_error) { - @(default_calling_convention="c", require_results) - foreign lib { - kbts_ShapeDirectFixedMemory :: proc(Config: ^shape_config, Storage: ^glyph_storage, RunDirection: direction, Memory: rawptr, MemorySize: c.int, Output: ^glyph_iterator) -> shape_error --- - } - Err = kbts_ShapeDirectFixedMemory(Config, Storage, RunDirection, Memory, MemorySize, &Output) + Err = kbts_ShapeDirect(Scratchpad, Storage, RunDirection, &Output) return } @(require_results) -SizeOfGlyphConfig :: proc "c" (Overrides: []feature_override) -> c.int { +SizeOfGlyphConfig :: proc "c" (ShapeConfig: ^shape_config, Overrides: []feature_override) -> c.int { @(default_calling_convention="c", require_results) foreign lib { - kbts_SizeOfGlyphConfig :: proc(Overrides: [^]feature_override, OverrideCount: c.int) -> c.int --- + kbts_SizeOfGlyphConfig :: proc(ShapeConfig: ^shape_config, Overrides: [^]feature_override, OverrideCount: c.int) -> c.int --- } - return kbts_SizeOfGlyphConfig(raw_data(Overrides), c.int(len(Overrides))) + return kbts_SizeOfGlyphConfig(ShapeConfig, raw_data(Overrides), c.int(len(Overrides))) } @(require_results) -PlaceGlyphConfig :: proc "c" (Overrides: []feature_override, Memory: rawptr) -> ^glyph_config { +PlaceGlyphConfig :: proc "c" (ShapeConfig: ^shape_config, Overrides: []feature_override, Memory: rawptr) -> ^glyph_config { @(default_calling_convention="c", require_results) foreign lib { - kbts_PlaceGlyphConfig :: proc(Overrides: [^]feature_override, OverrideCount: c.int, Memory: rawptr) -> ^glyph_config --- + kbts_PlaceGlyphConfig :: proc(ShapeConfig: ^shape_config, Overrides: [^]feature_override, OverrideCount: c.int, Memory: rawptr) -> ^glyph_config --- } - return kbts_PlaceGlyphConfig(raw_data(Overrides), c.int(len(Overrides)), Memory) + return kbts_PlaceGlyphConfig(ShapeConfig, raw_data(Overrides), c.int(len(Overrides)), Memory) } @(require_results) -CreateGlyphConfig :: proc(Overrides: []feature_override, Allocator: allocator_function, AllocatorData: rawptr) -> ^glyph_config { +CreateGlyphConfig :: proc(ShapeConfig: ^shape_config, Overrides: []feature_override, Allocator: allocator_function, AllocatorData: rawptr) -> ^glyph_config { @(default_calling_convention="c", require_results) foreign lib { - kbts_CreateGlyphConfig :: proc(Overrides: [^]feature_override, OverrideCount: c.int, Allocator: allocator_function, AllocatorData: rawptr) -> ^glyph_config --- + kbts_CreateGlyphConfig :: proc(ShapeConfig: ^shape_config, Overrides: [^]feature_override, OverrideCount: c.int, Allocator: allocator_function, AllocatorData: rawptr) -> ^glyph_config --- } - return kbts_CreateGlyphConfig(raw_data(Overrides), c.int(len(Overrides)), Allocator, AllocatorData) + return kbts_CreateGlyphConfig(ShapeConfig, raw_data(Overrides), c.int(len(Overrides)), Allocator, AllocatorData) } @(default_calling_convention="c", link_prefix="kbts_", require_results) @@ -265,12 +274,13 @@ BreakEntireString :: proc "c" (Direction: direction, JapaneseLineBreakStyle: jap Input: []byte, InputFormat: text_format, Breaks: []break_type, BreakCount: ^c.int, BreakFlags: []break_flags, BreakFlagCount: ^c.int) { + @(default_calling_convention="c", require_results) foreign lib { kbts_BreakEntireString :: proc(Direction: direction, JapaneseLineBreakStyle: japanese_line_break_style, ConfigFlags: break_config_flags, - Input: rawptr, InputSizeInBytes: c.int, InputFormat: text_format, - Breaks: [^]break_type, BreakCapacity: c.int, BreakCount: ^c.int, - BreakFlags: [^]break_flags, BreakFlagCapacity: c.int, BreakFlagCount: ^c.int) --- + Input: rawptr, InputSizeInBytes: c.int, InputFormat: text_format, + Breaks: [^]break_type, BreakCapacity: c.int, BreakCount: ^c.int, + BreakFlags: [^]break_flags, BreakFlagCapacity: c.int, BreakFlagCount: ^c.int) --- } kbts_BreakEntireString(Direction, JapaneseLineBreakStyle, ConfigFlags, raw_data(Input), c.int(len(Input)), InputFormat, raw_data(Breaks), c.int(len(Breaks)), BreakCount, raw_data(BreakFlags), c.int(len(BreakFlags)), BreakFlagCount) } @@ -279,12 +289,13 @@ BreakEntireStringUtf32 :: proc "c" (Direction: direction, JapaneseLineBreakStyle Utf32: []rune, Breaks: []break_type, BreakCount: ^c.int, BreakFlags: []break_flags, BreakFlagCount: ^c.int) { + @(default_calling_convention="c", require_results) foreign lib { kbts_BreakEntireStringUtf32 :: proc(Direction: direction, JapaneseLineBreakStyle: japanese_line_break_style, ConfigFlags: break_config_flags, - Utf32: [^]rune, Utf32Count: c.int, - Breaks: [^]break_type, BreakCapacity: c.int, BreakCount: ^c.int, - BreakFlags: [^]break_flags, BreakFlagCapacity: c.int, BreakFlagCount: ^c.int) --- + Utf32: [^]rune, Utf32Count: c.int, + Breaks: [^]break_type, BreakCapacity: c.int, BreakCount: ^c.int, + BreakFlags: [^]break_flags, BreakFlagCapacity: c.int, BreakFlagCount: ^c.int) --- } kbts_BreakEntireStringUtf32(Direction, JapaneseLineBreakStyle, ConfigFlags, raw_data(Utf32), c.int(len(Utf32)), raw_data(Breaks), c.int(len(Breaks)), BreakCount, raw_data(BreakFlags), c.int(len(BreakFlags)), BreakFlagCount) } @@ -293,12 +304,13 @@ BreakEntireStringUtf8 :: proc "c" (Direction: direction, JapaneseLineBreakStyle: Utf8: string, Breaks: []break_type, BreakCount: ^c.int, BreakFlags: []break_flags, BreakFlagCount: ^c.int) { + @(default_calling_convention="c", require_results) foreign lib { kbts_BreakEntireStringUtf8 :: proc(Direction: direction, JapaneseLineBreakStyle: japanese_line_break_style, ConfigFlags: break_config_flags, - Utf8: [^]byte, Utf8Length: c.int, - Breaks: [^]break_type, BreakCapacity: c.int, BreakCount: ^c.int, - BreakFlags: [^]break_flags, BreakFlagCapacity: c.int, BreakFlagCount: ^c.int) --- + Utf8: [^]byte, Utf8Length: c.int, + Breaks: [^]break_type, BreakCapacity: c.int, BreakCount: ^c.int, + BreakFlags: [^]break_flags, BreakFlagCapacity: c.int, BreakFlagCount: ^c.int) --- } kbts_BreakEntireStringUtf8(Direction, JapaneseLineBreakStyle, ConfigFlags, raw_data(Utf8), c.int(len(Utf8)), raw_data(Breaks), c.int(len(Breaks)), BreakCount, raw_data(BreakFlags), c.int(len(BreakFlags)), BreakFlagCount) } @@ -390,7 +402,7 @@ AllocatorFromOdinAllocator :: proc "contextless" (allocator: ^runtime.Allocator) case .ALLOCATE: res, _ := runtime.mem_alloc(int(Op.Allocate.Size), runtime.DEFAULT_ALIGNMENT) Op.Allocate.Pointer = raw_data(res) - Op.Allocate.Size = u32(len(res)) + Op.Allocate.Size = u32(len(res)) case .FREE: _ = runtime.mem_free(Op.Free.Pointer) } diff --git a/vendor/kb_text_shape/kb_text_shape_types.odin b/vendor/kb_text_shape/kb_text_shape_types.odin index fa3c47f85..3d8c3ab4e 100644 --- a/vendor/kb_text_shape/kb_text_shape_types.odin +++ b/vendor/kb_text_shape/kb_text_shape_types.odin @@ -12,9 +12,9 @@ un :: distinct ( u64 ) // sn :: distinct ( -// int when (size_of(uintptr) == size_of(int)) else -// i32 when size_of(uintptr) == 4 else -// i64 +// int when (size_of(uintptr) == size_of(int)) else +// i32 when size_of(uintptr) == 4 else +// i64 // ) @@ -781,6 +781,12 @@ break_state_flag :: enum u32 { SAW_AL_AFTER_LR = 4, LAST_WAS_BRACKET = 5, } +// TODO: Was this supposed to be a bitset or regular enum? +shape_context_flag :: enum u32 { + KBTS_SHAPE_CONTEXT_FLAG_NONE, + KBTS_SHAPE_CONTEXT_FLAG_FONT_PRIORITY_BOTTOM_TO_TOP, +} +shape_context_flags :: distinct bit_set[shape_context_flag; u32] @@ -1750,18 +1756,20 @@ feature_tag :: enum u32 { zero = 'z' | 'e'<<8 | 'r'<<16 | 'o'<<24, // Slashed Zero } -_gdef :: struct {} -_cmap_14 :: struct {} -_gsub_gpos :: struct {} -_maxp :: struct {} -_hea :: struct {} -shaper_properties :: struct {} -_feature :: struct {} -_head :: struct {} -_langsys :: struct {} -shape_config :: struct {} -glyph_config :: struct {} -shape_context :: struct {} +_gdef :: struct {} +_cmap_14 :: struct {} +_gsub_gpos :: struct {} +_maxp :: struct {} +_hea :: struct {} +shaper_properties :: struct {} +_feature :: struct {} +_head :: struct {} +_langsys :: struct {} +shape_config :: struct {} +glyph_config :: struct {} +bucketed_glyph :: struct {} +shape_context :: struct {} +shape_scratchpad :: struct {} allocator_op_allocate :: struct { Pointer: rawptr, @@ -1849,6 +1857,23 @@ font_info :: struct { Weight: font_weight, Width: font_width, } +font_info2 :: struct { + Size: u32, + using FontInfo: font_info, +} +font_info2_1 :: struct { + using FontInfo2: font_info2, + UnitsPerEm: u16, + XMin, YMin, XMax, YMax: i16, + Ascent, Descent, LineGap: i16, +} +font_info2_2 :: struct { + using FontInfo2_1: font_info2_1, + + // For now, this is just OS2.sCapHeight. + // If/when this is zero, we might consider trying to communicate a useful height instead of simply passing the zero along. + CapitalHeight: i16, +} feature_override :: struct { Tag: feature_tag, @@ -2033,6 +2058,11 @@ glyph :: struct { ParentInfo: u32, + Bucketed: ^bucketed_glyph, + SortKey: u32, + SortKeyInterval: u32, + BucketedBucketIndex: u16, + // This is set by GSUB and used by GPOS. // A 0-index means that we should attach to the last component in the ligature. // @@ -2064,7 +2094,9 @@ glyph :: struct { shape_codepoint :: struct { Font: ^font, // Only set when (.GRAPHEME in BreakFlags) - Config: ^glyph_config, + + FeatureOverrides: [^]feature_override `fmt:"v,FeatureOverrideCount"`, + FeatureOverrideCount: c.int, Codepoint: rune, UserId: c.int, @@ -2121,8 +2153,8 @@ glyph_storage :: struct { } glyph_parent :: struct { - Decomposition: u64, - Codepoint: rune, + Codepoint: rune, + Codepoint1: rune, } font_coverage_test :: struct { @@ -2144,4 +2176,5 @@ run :: struct { Flags: break_flags, Glyphs: glyph_iterator, -} \ No newline at end of file +} + diff --git a/vendor/kb_text_shape/lib/kb_text_shape.lib b/vendor/kb_text_shape/lib/kb_text_shape.lib index 6a48e9f87..6de3c2ab1 100644 Binary files a/vendor/kb_text_shape/lib/kb_text_shape.lib and b/vendor/kb_text_shape/lib/kb_text_shape.lib differ diff --git a/vendor/kb_text_shape/src/build_unix.sh b/vendor/kb_text_shape/src/build_unix.sh index 159aecb52..a76ca408f 100755 --- a/vendor/kb_text_shape/src/build_unix.sh +++ b/vendor/kb_text_shape/src/build_unix.sh @@ -2,6 +2,6 @@ set -e mkdir -p "../lib" -cc -O2 -c kb_text_shape.c +cc -O2 -fPIC -c kb_text_shape.c ar -rcs ../lib/kb_text_shape.a kb_text_shape.o rm *.o diff --git a/vendor/kb_text_shape/src/kb_text_shape.h b/vendor/kb_text_shape/src/kb_text_shape.h index 986bf3be6..ef2a0b178 100644 --- a/vendor/kb_text_shape/src/kb_text_shape.h +++ b/vendor/kb_text_shape/src/kb_text_shape.h @@ -1,4 +1,4 @@ -/* kb_text_shape - v2.05 - text segmentation and shaping +/* kb_text_shape - v2.21 - text segmentation and shaping by Jimmy Lefevre SECURITY @@ -243,22 +243,53 @@ int kbts_SizeOfShapeContext() Tells you how big of a buffer you need to provide to kbts_PlaceShapeContext. + :kbts_PlaceShapeContext2 + :PlaceShapeContext2 + kbts_shape_context *kbts_PlaceShapeContext2(kbts_allocator_function *Allocator, void *AllocatorData, void *Memory, kbts_shape_context_flags Flags) + Places a context at Memory and initializes it. + [Allocator] will be used for subsequent allocations. + + [Flags] can be any combination of: + KBTS_SHAPE_CONTEXT_FLAG_NONE + No-op; default behavior. + KBTS_SHAPE_CONTEXT_FLAG_FONT_PRIORITY_BOTTOM_TO_TOP = (1 << 0) + The default priority order for the font stack is top-to-bottom, i.e. + fonts that were pushed later have higher priority. + If this flag is set, then this priority is reversed: fonts that are + pushed earlier, and are thus closer to the bottom of the stack, have + higher priority. + For more details on the font stack, see CONTEXT:FONT HANDLING. + :kbts_PlaceShapeContext :PlaceShapeContext kbts_shape_context *kbts_PlaceShapeContext(kbts_allocator_function *Allocator, void *AllocatorData, void *Memory) + Equivalent to calling kbts_PlaceShapeContext2 with Flags = KBTS_SHAPE_CONTEXT_FLAG_NONE. + + :kbts_PlaceShapeContextFixedMemory2 + :PlaceShapeContextFixedMemory2 + kbts_shape_context *kbts_PlaceShapeContextFixedMemory(void *Memory, int Size, kbts_shape_context_flags Flags) Places a context at Memory and initializes it. - [Allocator] will be used for subsequent allocations. + This context will only use the [Size] bytes located at [Memory] for its allocations. + + For more details on [Flags], see :kbts_PlaceShapeContext2. :kbts_PlaceShapeContextFixedMemory :PlaceShapeContextFixedMemory kbts_shape_context *kbts_PlaceShapeContextFixedMemory(void *Memory, int Size) - Places a context at Memory and initializes it. - This context will only use the [Size] bytes located at [Memory] for its allocations. + Equivalent to calling kbts_PlaceShapeContextFixedMemory2 with + Flags = KBTS_SHAPE_CONTEXT_FLAG_NONE. + + :kbts_CreateShapeContext2 + :CreateShapeContext2 + kbts_shape_context *kbts_CreateShapeContext2(kbts_allocator_function *Allocator, void *AllocatorData, kbts_shape_context_flags Flags) + Allocates a context using [Allocator] and initializes it. + + For more information on [Flags], see :kbts_PlaceShapeContext2. :kbts_CreateShapeContext :CreateShapeContext kbts_shape_context *kbts_CreateShapeContext(kbts_allocator_function *Allocator, void *AllocatorData) - Allocates a context using [Allocator] and initializes it. + Equivalent to calling kbts_CreateShapeContext2 with Flags = KBTS_SHAPE_CONTEXT_FLAG_NONE. :kbts_DestroyShapeContext :DestroyShapeContext @@ -423,7 +454,7 @@ User IDs for the corresponding codepoints start at [UserId]. If [UserIdGenerationMode] is KBTS_USER_ID_GENERATION_MODE_CODEPOINT_INDEX, each codepoint will increment the user ID by 1. - If [UserIdGenerationMode] is KBTS_USER_ID_GENERATION_MODE_CODEPOINT_INDEX, + If [UserIdGenerationMode] is KBTS_USER_ID_GENERATION_MODE_SOURCE_INDEX, each codepoint will increment the user ID by the length of its encoding in UTF-8. @@ -439,7 +470,7 @@ user ID counter. If [UserIdGenerationMode] is KBTS_USER_ID_GENERATION_MODE_CODEPOINT_INDEX, each codepoint will increment the user ID by 1. - If [UserIdGenerationMode] is KBTS_USER_ID_GENERATION_MODE_CODEPOINT_INDEX, + If [UserIdGenerationMode] is KBTS_USER_ID_GENERATION_MODE_SOURCE_INDEX, each codepoint will increment the user ID by the length of its encoding in bytes in UTF-8. @@ -458,7 +489,7 @@ Call kbts_ShapeCodepointIteratorNext repeatedly to loop through the corresponding codepoints. - + :kbts_ShapeCodepointIteratorIsValid :ShapeCodepointIteratorIsValid int kbts_ShapeCodepointIteratorIsValid(kbts_shape_codepoint_iterator *It) @@ -553,7 +584,7 @@ a set of active glyphs. The active glyph set part is used by the library. As a user, you only need to care about the memory allocation part. - - Scratch memory + - Scratch memory (kbts_shape_scratchpad) Unfortunately, shaping can have a very unpredictable memory footprint, so all shaping operations require some amount of scratch space that we cannot compute beforehand. @@ -561,10 +592,8 @@ :kbts_ShapeDirect :ShapeDirect - kbts_shape_error kbts_ShapeDirect(kbts_shape_config *Config, kbts_glyph_storage *Storage, - kbts_direction RunDirection, - kbts_allocator_function *Allocator, void *AllocatorData, - kbts_glyph_iterator *Output) + kbts_shape_error kbts_ShapeDirect(kbts_shape_scratchpad *Scratchpad, kbts_glyph_storage *Storage, + kbts_direction RunDirection, kbts_glyph_iterator *Output) [RunDirection] is the direction of the specific run being shaped. If the [return value] is KBTS_SHAPE_ERROR_NONE, then the shaping operation completed successfully. @@ -576,15 +605,6 @@ Glyphs are always returned in left-to-right order. In other words, RTL runs are flipped so that visual order is consistent. - - :kbts_ShapeDirectFixedMemory - :ShapeDirectFixedMemory - kbts_shape_error kbts_ShapeDirectFixedMemory(kbts_shape_config *Config, kbts_glyph_storage *Storage, - kbts_direction RunDirection, - void *Memory, int Size, - kbts_glyph_iterator *Output) - Same as kbts_ShapeDirect, but only uses the [Size] bytes at [Memory] for allocations. - The rest of the direct API is more or less about preparing the data you need to call kbts_ShapeDirect. @@ -673,16 +693,21 @@ returned by kbts_FontFromFile), frees all of [Font]'s buffers. Otherwise, does nothing. - :kbts_GetFontInfo - :GetFontInfo - void kbts_GetFontInfo(kbts_font *Font, kbts_font_info *Info) + :kbts_GetFontInfo2 + :GetFontInfo2 + void kbts_GetFontInfo2(kbts_font *Font, kbts_font_info2 *Info) Writes a bunch of useful metadata about [Font] into [Info]. - You can use this function to extract styling, name and licensing information - from a font. - We use a simplified representation for font weight and width that is fine for - classic font selection, e.g. "I need a bold font". OpenType fonts may feature - finer-grained metrics, and we currently do not expose/support those. + Before calling this function, you must fill out [Info].Size to be + sizeof([Info]). + + [Info] can be one of several types: + - kbts_font_info2 describes styling, name and licensing information. + We use a simplified representation for font weight and width that is fine for + classic font selection, e.g. "I need a bold font". OpenType fonts may feature + finer-grained metrics, and we currently do not expose/support those. + - kbts_font_info2_1 also includes metrics and bounding box information. + - kbts_font_info2_2 also includes capital height. :kbts_font_style_flags :font_style_flags @@ -694,7 +719,14 @@ A given font can be bold and italic at the same time, but probably not regular and bold and probably not regular and italic. - If [Font] is not a valid font, then [Info] will be zeroed. + If [Font] is not a valid font, or some information could not be found in the + font, then the respective members will be zeroed (except Size). + + :kbts_GetFontInfo + :GetFontInfo + void kbts_GetFontInfo(kbts_font *Font, kbts_font_info *Info) + Equivalent to calling kbts_GetFontInfo2 with an Info struct of type + kbts_font_info2. DIRECT:SHAPE CONFIG :kbts_SizeOfShapeConfig @@ -721,6 +753,51 @@ If [Config] was allocated in kbts_CreateShapeConfig, frees all of [Config]'s data. Otherwise, nothing is done. + DIRECT:SHAPE SCRATCHPAD + :kbts_SizeOfShapeScratchpad + :SizeOfShapeScratchpad + kbts_un kbts_SizeOfShapeScratchpad(kbts_shape_config *Config) + Returns how large a scratchpad for [Config] will be initially. + This is the size of the initial memory footprint, used to hold basic bookkeeping data. + A scratchpad can always dynamically allocate memory during shaping. + + :kbts_PlaceShapeScratchpad + :PlaceShapeScratchpad + kbts_shape_scratchpad *kbts_PlaceShapeScratchpad(kbts_shape_config *Config, + void *Memory, + kbts_allocator_function *Allocator, void *AllocatorData) + Initializes a scratchpad for [Config] at [Memory], and returns a pointer to it. + [Memory] should be kbts_SizeOfShapeScratchpad(Config) big. + [Allocator] will be used by the scratchpad during shaping. + + If [Memory] is null, then the [return value] is null. + + :kbts_PlaceShapeScratchpadFixedMemory + :PlaceShapeScratchpadFixedMemory + kbts_shape_scratchpad *kbts_PlaceShapeScratchpadFixedMemory(kbts_shape_config *Config, + void *Memory, int Size) + Same as kbts_PlaceShapeScratchpad, except the buffer [Memory] of size [Size] + is used for both initialization and dynamic allocation. + + If [Size] is not large enough, then the [return value] is null. + + :kbts_CreateShapeScratchpad + :CreateShapeScratchpad + kbts_shape_scratchpad *kbts_CreateShapeScratchpad(kbts_shape_config *Config, + kbts_allocator_function *Allocator, void *AllocatorData) + Same as kbts_PlaceShapeScratchpad, except [Allocator] is used both for + initialization and for dynamic allocation. + + If [Allocator] is null, then the default allocator is used. + + :kbts_DestroyShapeScratchpad + :DestroyShapeScratchpad + void kbts_DestroyShapeScratchpad(kbts_shape_scratchpad *Scratchpad) + Frees all memory associated with [Scratchpad]. + + If [Scratchpad] itself was allocated with kbts_CreateShapeScratchpad, then + it will also free itself. + DIRECT:GLYPH STORAGE kbts_glyph_storage is a public struct: @@ -729,7 +806,7 @@ typedef struct kbts_glyph_storage { kbts_arena Arena; - + kbts_glyph GlyphSentinel; kbts_glyph FreeGlyphSentinel; } kbts_glyph_storage; @@ -825,21 +902,19 @@ is 0 or 1, but a few features actually care about the exact value. (You can think of a feature that is like "when I am enabled, change this letter to one of these alternatives". In that case, the value you provide in the feature override is used - as an index into the array of alternatives.) + as a one-based index into the array of alternatives.) :kbts_SizeOfGlyphConfig :SizeOfGlyphConfig - int kbts_SizeOfGlyphConfig(kbts_feature_override *Overrides, int OverrideCount) + int kbts_SizeOfGlyphConfig(kbts_shape_config *ShapeConfig, kbts_feature_override *Overrides, int OverrideCount) Returns the buffer size needed to hold a kbts_glyph_config that describes [Overrides]. This size can vary a lot depending on the kind of feature overrides you specify. - Built-in OpenType features with values of 0 or 1 are "free"; they are packed in a - fixed-size representation which does not change the config's memory footprint. - On the other hand, if you need non-binary values, or non-standard features, then - we need to store a description of the override itself, which requires memory. + Overrides with values of 0 or 1 are stored in a compact format, while other values + will be stored explicitly. :kbts_PlaceGlyphConfig :PlaceGlyphConfig - kbts_glyph_config *kbts_PlaceGlyphConfig(kbts_feature_override *Overrides, int OverrideCount, void *Memory) + kbts_glyph_config *kbts_PlaceGlyphConfig(kbts_shape_config *ShapeConfig, kbts_feature_override *Overrides, int OverrideCount, void *Memory) Writes a kbts_glyph_config that describes [Overrides] into [Memory], and returns a pointer to it. The kbts_glyph_config uses its own representation for overrides, so you can modify @@ -847,7 +922,7 @@ :kbts_CreateGlyphConfig :CreateGlyphConfig - kbts_glyph_config *kbts_CreateGlyphConfig(kbts_feature_override *Overrides, int OverrideCount, kbts_allocator_function *Allocator, void *AllocatorData) + kbts_glyph_config *kbts_CreateGlyphConfig(kbts_shape_config *ShapeConfig, kbts_feature_override *Overrides, int OverrideCount, kbts_allocator_function *Allocator, void *AllocatorData) Allocates a kbts_glyph_config that describes [Overrides] and returns a pointer to it. The kbts_glyph_config uses its own representation for overrides, so you can modify @@ -954,7 +1029,7 @@ value. If not, returns 0. kbts_break looks like this: - + typedef struct kbts_break { int Position; @@ -1245,6 +1320,101 @@ See https://unicode.org/reports/tr9 for more information. VERSION HISTORY + 2.21 - Eliminate redundant typedefs for C99 compatibility. + 2.20 - Properly check kbts__InputCodepoint return values. + Handle null shape configs in kbts_PlaceGlyphConfig. + 2.19 - Fix the glyph config cache not taking shape_configs into account. + 2.18 - Improved handling of default-ignorable codepoints. + 2.17 - New function: kbts_PlaceShapeContextFixedMemory2. + 2.16 - New type: kbts_shape_context_flags. + New functions: kbts_PlaceShapeContext2, kbts_CreateShapeContext2. + Fix a bug where ShapeCodepointIteratorNext() returned a null terminator when + the text ended on a block boundary. + Fix inconsistent break count/break flag count reporting in BreakEntireString. + 2.15a - Fix GCC warnings + 2.15 - Handle edge case when decomposing Thai/Lao Am vowels. + 2.14 - Fix direction resolution for neutral characters surrounding digits. + 2.13 - Extend NO_BREAK flag to include attached glyphs. + 2.12 - Support fonts that use traditionally-GPOS features in GSUB. + 2.11 - Reduce the size of Unicode lookup tables from ~577KiB to ~423KiB. + Fix a memory leak when recomposing glyphs in the shaper. + Remember shape and glyph configs in the shape context. + 2.10 - Properly zero extended font_info2 types in GetFontInfo2. + Properly reset the glyph config cache in ShapeBegin. + 2.09 - Fix use-after-free when a shape_scratchpad was freed after its respective shape_config. + Extended the GetFontInfo API to include metrics and bounding box information. + New types: kbts_font_info2, kbts_font_info2_1, kbts_font_info2_2. + New function: kbts_GetFontInfo2(). + 2.08 - Fix some UB. + 2.07 - Performance improvements. + API CHANGES: + Struct layout changes for internal use: kbts_glyph, kbts_glyph_parent. + + CONTEXT API + - kbts_shape_codepoint now holds bespoke feature overrides instead of a glyph config. + BEFORE: + typedef struct kbts_shape_codepoint + { + kbts_font *Font; // Only set when (BreakFlags & KBTS_BREAK_FLAG_GRAPHEME) != 0. + + kbts_glyph_config *Config; + + int Codepoint; + int UserId; + + kbts_break_flags BreakFlags; + kbts_script Script; // Only set when (BreakFlags & KBTS_BREAK_FLAG_SCRIPT) != 0. + kbts_direction Direction; // Only set when (BreakFlags & KBTS_BREAK_FLAG_DIRECTION) != 0. + kbts_direction ParagraphDirection; // Only set when (BreakFlags & KBTS_BREAK_FLAG_PARAGRAPH_DIRECTION) != 0. + } kbts_shape_codepoint; + AFTER: + typedef struct kbts_shape_codepoint + { + kbts_font *Font; // Only set when (BreakFlags & KBTS_BREAK_FLAG_GRAPHEME) != 0. + + kbts_feature_override *FeatureOverrides; + int FeatureOverrideCount; + + int Codepoint; + int UserId; + + kbts_break_flags BreakFlags; + kbts_script Script; // Only set when (BreakFlags & KBTS_BREAK_FLAG_SCRIPT) != 0. + kbts_direction Direction; // Only set when (BreakFlags & KBTS_BREAK_FLAG_DIRECTION) != 0. + kbts_direction ParagraphDirection; // Only set when (BreakFlags & KBTS_BREAK_FLAG_PARAGRAPH_DIRECTION) != 0. + } kbts_shape_codepoint; + + DIRECT API + - Added a new (opaque pointer) type: kbts_shape_scratchpad. + This type contains all the runtime data needed for shaping according to a specific kbts_shape_config. + Unlike the kbts_shape_config, it is mutable, and so cannot be trivially shared across threads. + It can be reused across different shaping calls as long as they all use the same shape_config. + - Added functions to manage scratchpads: + kbts_un kbts_SizeOfShapeScratchpad(kbts_shape_config *Config) + kbts_shape_scratchpad *kbts_PlaceShapeScratchpad(kbts_shape_config *Config, void *Memory, kbts_allocator_function *Allocator, void *AllocatorData) + kbts_shape_scratchpad *kbts_PlaceShapeScratchpadFixedMemory(kbts_shape_config *Config, void *Memory, int Size) + kbts_shape_scratchpad *kbts_CreateShapeScratchpad(kbts_shape_config *Config, kbts_allocator_function *Allocator, void *AllocatorData) + void kbts_DestroyShapeScratchpad(kbts_shape_scratchpad *Scratchpad) + - kbts_ShapeDirect now takes a kbts_shape_scratchpad instead of a kbts_shape_config and an allocator. + BEFORE: + kbts_shape_error kbts_ShapeDirect(kbts_shape_config *Config, kbts_glyph_storage *Storage, + kbts_direction RunDirection, + kbts_allocator_function *Allocator, void *AllocatorData, + kbts_glyph_iterator *Output) + AFTER: + kbts_shape_error kbts_ShapeDirect(kbts_shape_scratchpad *Scratchpad, kbts_glyph_storage *Storage, + kbts_direction RunDirection, kbts_glyph_iterator *Output) + - Removed kbts_ShapeDirectFixedMemory. (Use kbts_PlaceShapeScratchpadFixedMemory instead.) + - Glyph configs now correspond to exactly one shape config. + BEFORE: + int kbts_SizeOfGlyphConfig(kbts_feature_override *Overrides, int OverrideCount) + kbts_glyph_config *kbts_PlaceGlyphConfig(kbts_feature_override *Overrides, int OverrideCount, void *Memory) + kbts_glyph_config *kbts_CreateGlyphConfig(kbts_feature_override *Overrides, int OverrideCount, kbts_allocator_function *Allocator, void *AllocatorData) + AFTER: + int kbts_SizeOfGlyphConfig(kbts_shape_config *ShapeConfig, kbts_feature_override *Overrides, int OverrideCount) + kbts_glyph_config *kbts_PlaceGlyphConfig(kbts_shape_config *ShapeConfig, kbts_feature_override *Overrides, int OverrideCount, void *Memory) + kbts_glyph_config *kbts_CreateGlyphConfig(kbts_shape_config *ShapeConfig, kbts_feature_override *Overrides, int OverrideCount, kbts_allocator_function *Allocator, void *AllocatorData) + 2.06 - Faster GSUB and GPOS feature culling. 2.05 - Fix custom allocator initialization for kbts_shape_context.PermanentArena. 2.04 - Fix Indic syllable logic for small/single-character syllables. Fix wrong indirection in pointer code in Indic syllable logic. @@ -1276,17 +1446,17 @@ LICENSE zlib License - + (C) Copyright 2024-2025 Jimmy Lefevre - + This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software. - + Permission is granted to anyone to use this software for any purpose, including commercial applications, and to alter it and redistribute it freely, subject to the following restrictions: - + 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be @@ -1331,6 +1501,9 @@ # ifndef kbts_s8 # define kbts_s8 signed char # endif +# ifndef kbts_b32 +# define kbts_b32 int +# endif # ifndef KB_TEXT_SHAPE_POINTER_SIZE # if defined(i386) || defined(__i386__) || defined(_M_IX86) || defined(_M_ARM) || defined(__arm__) || defined(__x86) || (defined(__APPLE__) && defined(__ppc)) || \ @@ -1344,18 +1517,20 @@ # if KB_TEXT_SHAPE_POINTER_SIZE == 4 # define kbts_un kbts_u32 # define kbts_sn kbts_s32 +# define kbts_uptr kbts_u32 # else # define kbts_un kbts_u64 # define kbts_sn kbts_s64 +# define kbts_uptr kbts_u64 # endif # ifdef __has_attribute # if __has_attribute(fallthrough) -# define KBTS_FALLTHROUGH __attribute__((fallthrough)) +# define KBTS__FALLTHROUGH __attribute__((fallthrough)) # endif # endif -# ifndef KBTS_FALLTHROUGH -# define KBTS_FALLTHROUGH +# ifndef KBTS__FALLTHROUGH +# define KBTS__FALLTHROUGH # endif # ifndef KBTS_EXPORT @@ -1389,7 +1564,7 @@ # define KBTS_INLINE static inline # endif -# define KBTS_FOURCC(A, B, C, D) ((A) | ((B) << 8) | ((C) << 16) | ((D) << 24)) +# define KBTS_FOURCC(A, B, C, D) ((kbts_u32)(A) | ((kbts_u32)(B) << 8) | ((kbts_u32)(C) << 16) | ((kbts_u32)(D) << 24)) typedef kbts_u32 kbts_language; enum kbts_language_enum @@ -2151,6 +2326,14 @@ enum kbts_break_state_flags_enum KBTS_BREAK_STATE_FLAG_LAST_WAS_BRACKET = 0x20, }; +typedef kbts_u32 kbts_shape_context_flags; +enum kbts_shape_context_flags_enum +{ + KBTS_SHAPE_CONTEXT_FLAG_NONE, + + KBTS_SHAPE_CONTEXT_FLAG_FONT_PRIORITY_BOTTOM_TO_TOP = (1 << 0), // Prioritize fonts that are pushed earlier, instead of the default which prioritizes fonts that are pushed later. +}; + typedef kbts_u32 kbts_text_format; enum kbts_text_format_enum { @@ -2257,8 +2440,9 @@ enum kbts_blob_version_enum { KBTS_BLOB_VERSION_INVALID, KBTS_BLOB_VERSION_INITIAL, + KBTS_BLOB_VERSION_REMOVED_SUBTABLE_INFOS_ALIGNED_TABLES, - KBTS_BLOB_VERSION_CURRENT = KBTS_BLOB_VERSION_INITIAL, + KBTS_BLOB_VERSION_CURRENT = KBTS_BLOB_VERSION_REMOVED_SUBTABLE_INFOS_ALIGNED_TABLES, }; typedef kbts_u32 kbts_font_style_flags; @@ -2335,18 +2519,18 @@ enum kbts_glyph_flags_enum KBTS_GLYPH_FLAG_CFAR = (1 << 19), // These can be anything. - KBTS_GLYPH_FLAG_DO_NOT_DECOMPOSE = (1 << 21), - KBTS_GLYPH_FLAG_FIRST_IN_MULTIPLE_SUBSTITUTION = (1 << 22), - KBTS_GLYPH_FLAG_NO_BREAK = (1 << 23), - KBTS_GLYPH_FLAG_CURSIVE = (1 << 24), - KBTS_GLYPH_FLAG_GENERATED_BY_GSUB = (1 << 25), - KBTS_GLYPH_FLAG_USED_IN_GPOS = (1 << 26), + KBTS_GLYPH_FLAG_DO_NOT_DECOMPOSE = (1 << 20), + KBTS_GLYPH_FLAG_FIRST_IN_MULTIPLE_SUBSTITUTION = (1 << 21), + KBTS_GLYPH_FLAG_NO_BREAK = (1 << 22), + KBTS_GLYPH_FLAG_CURSIVE = (1 << 23), + KBTS_GLYPH_FLAG_GENERATED_BY_GSUB = (1 << 24), + KBTS_GLYPH_FLAG_USED_IN_GPOS = (1 << 25), - KBTS_GLYPH_FLAG_STCH_ENDPOINT = (1 << 27), - KBTS_GLYPH_FLAG_STCH_EXTENSION = (1 << 28), + KBTS_GLYPH_FLAG_STCH_ENDPOINT = (1 << 26), + KBTS_GLYPH_FLAG_STCH_EXTENSION = (1 << 27), - KBTS_GLYPH_FLAG_LIGATURE = (1 << 29), - KBTS_GLYPH_FLAG_MULTIPLE_SUBSTITUTION = (1 << 30), + KBTS_GLYPH_FLAG_LIGATURE = (1 << 28), + KBTS_GLYPH_FLAG_MULTIPLE_SUBSTITUTION = (1 << 29), }; typedef kbts_u8 kbts_joining_feature; @@ -2379,7 +2563,7 @@ typedef kbts_u32 kbts_break_config_flags; enum kbts_break_config_flags_enum { KBTS_BREAK_CONFIG_FLAG_NONE, - + KBTS_BREAK_CONFIG_FLAG_END_OF_TEXT_GENERATES_HARD_LINE_BREAK = 1, }; @@ -2522,7 +2706,7 @@ enum kbts_line_break_class_enum // NS is strict line breaking, used for long lines. // ID is normal line breaking, used for normal body text. /* 65 */ KBTS_LINE_BREAK_CLASS_CJ, - + /* 66 */ KBTS_LINE_BREAK_CLASS_SOT, /* 67 */ KBTS_LINE_BREAK_CLASS_EOT, }; @@ -3206,6 +3390,7 @@ typedef struct kbts_glyph kbts_glyph; typedef struct kbts_glyph_config kbts_glyph_config; typedef struct kbts_shape_context kbts_shape_context; typedef struct kbts_glyph_storage kbts_glyph_storage; +typedef struct kbts_shape_scratchpad kbts_shape_scratchpad; typedef struct kbts_allocator_op_allocate { @@ -3231,12 +3416,6 @@ typedef struct kbts_allocator_op typedef void kbts_allocator_function(void *Data, kbts_allocator_op *Op); -typedef struct kbts_lookup_subtable_info -{ - kbts_u16 MinimumBacktrackPlusOne; - kbts_u16 MinimumFollowupPlusOne; -} kbts_lookup_subtable_info; - typedef struct kbts_blob_table { kbts_u32 OffsetFromStartOfFile; @@ -3253,7 +3432,7 @@ typedef struct kbts_load_font_state kbts_u32 LookupSubtableCount; kbts_u32 GlyphCount; kbts_u32 ScratchSize; - + kbts_u32 GlyphLookupMatrixSizeInBytes; kbts_u32 GlyphLookupSubtableMatrixSizeInBytes; kbts_u32 TotalSize; @@ -3304,6 +3483,57 @@ typedef struct kbts_font_info kbts_font_width Width; } kbts_font_info; +typedef struct kbts_font_info2 +{ + kbts_u32 Size; + + char *Strings[KBTS_FONT_INFO_STRING_ID_COUNT]; + kbts_u16 StringLengths[KBTS_FONT_INFO_STRING_ID_COUNT]; + + kbts_font_style_flags StyleFlags; + kbts_font_weight Weight; + kbts_font_width Width; +} kbts_font_info2; + +typedef struct kbts_font_info2_1 +{ + kbts_font_info2 Base; + + kbts_u16 UnitsPerEm; + + kbts_s16 XMin; + kbts_s16 YMin; + kbts_s16 XMax; + kbts_s16 YMax; + + kbts_s16 Ascent; + kbts_s16 Descent; + kbts_s16 LineGap; +} kbts_font_info2_1; + +typedef struct kbts_font_info2_2 +{ + kbts_font_info2 Base; + + // _1 + kbts_u16 UnitsPerEm; + + kbts_s16 XMin; + kbts_s16 YMin; + kbts_s16 XMax; + kbts_s16 YMax; + + kbts_s16 Ascent; + kbts_s16 Descent; + kbts_s16 LineGap; + + // _2 + + // For now, this is just OS2.sCapHeight. + // If/when this is zero, we might consider trying to communicate a useful height instead of simply passing the zero along. + kbts_s16 CapitalHeight; +} kbts_font_info2_2; + typedef struct kbts_feature_override { kbts_feature_tag Tag; @@ -3411,7 +3641,9 @@ typedef struct kbts_glyph_classes kbts_u16 MarkAttachmentClass; } kbts_glyph_classes; -typedef struct kbts_glyph + +typedef struct kbts__bucketed_glyph kbts__bucketed_glyph; +struct kbts_glyph { kbts_glyph *Prev; kbts_glyph *Next; @@ -3446,6 +3678,7 @@ typedef struct kbts_glyph kbts_glyph_config *Config; + // @Memory: We definitely don't need to carry this around for the entire shaping process. kbts_u64 Decomposition; kbts_glyph_classes Classes; @@ -3454,6 +3687,11 @@ typedef struct kbts_glyph kbts_u32 ParentInfo; + kbts__bucketed_glyph *Bucketed; + kbts_u32 SortKey; + kbts_u32 SortKeyInterval; + kbts_u16 BucketedBucketIndex; + // This is set by GSUB and used by GPOS. // A 0-index means that we should attach to the last component in the ligature. // @@ -3481,12 +3719,14 @@ typedef struct kbts_glyph kbts_u8 CombiningClass; kbts_u8 MarkOrdering; // Only used temporarily in NORMALIZE for Arabic mark reordering. -} kbts_glyph; +}; typedef struct kbts_shape_codepoint { kbts_font *Font; // Only set when (BreakFlags & KBTS_BREAK_FLAG_GRAPHEME) != 0. - kbts_glyph_config *Config; + + kbts_feature_override *FeatureOverrides; + int FeatureOverrideCount; int Codepoint; int UserId; @@ -3537,7 +3777,7 @@ typedef struct kbts_arena int Error; } kbts_arena; -typedef struct kbts_glyph_storage +struct kbts_glyph_storage { kbts_arena Arena; @@ -3545,12 +3785,12 @@ typedef struct kbts_glyph_storage kbts_glyph FreeGlyphSentinel; int Error; -} kbts_glyph_storage; +}; typedef struct kbts_glyph_parent { - kbts_u64 Decomposition; kbts_u32 Codepoint; + kbts_u32 Codepoint1; } kbts_glyph_parent; typedef struct kbts_font_coverage_test @@ -3582,8 +3822,11 @@ typedef struct kbts_run // KBTS_EXPORT int kbts_SizeOfShapeContext(void); +KBTS_EXPORT kbts_shape_context *kbts_PlaceShapeContext2(kbts_allocator_function *Allocator, void *AllocatorData, void *Memory, kbts_shape_context_flags Flags); KBTS_EXPORT kbts_shape_context *kbts_PlaceShapeContext(kbts_allocator_function *Allocator, void *AllocatorData, void *Memory); +KBTS_EXPORT kbts_shape_context *kbts_PlaceShapeContextFixedMemory2(void *Memory, int Size, kbts_shape_context_flags Flags); KBTS_EXPORT kbts_shape_context *kbts_PlaceShapeContextFixedMemory(void *Memory, int Size); +KBTS_EXPORT kbts_shape_context *kbts_CreateShapeContext2(kbts_allocator_function *Allocator, void *AllocatorData, kbts_shape_context_flags Flags); KBTS_EXPORT kbts_shape_context *kbts_CreateShapeContext(kbts_allocator_function *Allocator, void *AllocatorData); KBTS_EXPORT void kbts_DestroyShapeContext(kbts_shape_context *Context); #ifndef KB_TEXT_SHAPE_NO_CRT @@ -3617,8 +3860,7 @@ KBTS_EXPORT int kbts_ShapeGetShapeCodepoint(kbts_shape_context *Context, int Cod // Direct API // -KBTS_EXPORT kbts_shape_error kbts_ShapeDirect(kbts_shape_config *Config, kbts_glyph_storage *Storage, kbts_direction RunDirection, kbts_allocator_function *Allocator, void *AllocatorData, kbts_glyph_iterator *Output); -KBTS_EXPORT kbts_shape_error kbts_ShapeDirectFixedMemory(kbts_shape_config *Config, kbts_glyph_storage *Storage, kbts_direction RunDirection, void *Memory, int MemorySize, kbts_glyph_iterator *Output); +KBTS_EXPORT kbts_shape_error kbts_ShapeDirect(kbts_shape_scratchpad *Scratchpad, kbts_glyph_storage *Storage, kbts_direction RunDirection, kbts_glyph_iterator *Output); // A font holds all data that corresponds to a given font file. #ifndef KB_TEXT_SHAPE_NO_CRT @@ -3631,6 +3873,7 @@ KBTS_EXPORT int kbts_FontIsValid(kbts_font *Font); KBTS_EXPORT kbts_load_font_error kbts_LoadFont(kbts_font *Font, kbts_load_font_state *State, void *FontData, int FontDataSize, int FontIndex, int *ScratchSize_, int *OutputSize_); KBTS_EXPORT kbts_load_font_error kbts_PlaceBlob(kbts_font *Font, kbts_load_font_state *State, void *ScratchMemory, void *OutputMemory); KBTS_EXPORT void kbts_GetFontInfo(kbts_font *Font, kbts_font_info *Info); +KBTS_EXPORT void kbts_GetFontInfo2(kbts_font *Font, kbts_font_info2 *Info); // A shape_config is a bag of pre-computed data for a specific shaping setup. KBTS_EXPORT int kbts_SizeOfShapeConfig(kbts_font *Font, kbts_script Script, kbts_language Language); @@ -3650,11 +3893,20 @@ KBTS_EXPORT kbts_glyph_iterator kbts_ActiveGlyphIterator(kbts_glyph_storage *Sto // A glyph_config specifies glyph-specific shaping parameters. // A single glyph_config can be shared by multiple glyphs. -KBTS_EXPORT int kbts_SizeOfGlyphConfig(kbts_feature_override *Overrides, int OverrideCount); -KBTS_EXPORT kbts_glyph_config *kbts_PlaceGlyphConfig(kbts_feature_override *Overrides, int OverrideCount, void *Memory); -KBTS_EXPORT kbts_glyph_config *kbts_CreateGlyphConfig(kbts_feature_override *Overrides, int OverrideCount, kbts_allocator_function *Allocator, void *AllocatorData); +KBTS_EXPORT int kbts_SizeOfGlyphConfig(kbts_shape_config *ShapeConfig, kbts_feature_override *Overrides, int OverrideCount); +KBTS_EXPORT kbts_glyph_config *kbts_PlaceGlyphConfig(kbts_shape_config *ShapeConfig, kbts_feature_override *Overrides, int OverrideCount, void *Memory); +KBTS_EXPORT kbts_glyph_config *kbts_CreateGlyphConfig(kbts_shape_config *ShapeConfig, kbts_feature_override *Overrides, int OverrideCount, kbts_allocator_function *Allocator, void *AllocatorData); KBTS_EXPORT void kbts_DestroyGlyphConfig(kbts_glyph_config *Config); +// A shape_scratchpad holds all transient runtime shaping data. +// While the shape_config is immutable and can be trivially shared among threads, a +// shape_scratchpad is mutable and needs to be per-thread. +KBTS_EXPORT kbts_un kbts_SizeOfShapeScratchpad(kbts_shape_config *Config); +KBTS_EXPORT kbts_shape_scratchpad *kbts_PlaceShapeScratchpad(kbts_shape_config *Config, void *Memory, kbts_allocator_function *Allocator, void *AllocatorData); +KBTS_EXPORT kbts_shape_scratchpad *kbts_PlaceShapeScratchpadFixedMemory(kbts_shape_config *Config, void *Memory, int Size); +KBTS_EXPORT kbts_shape_scratchpad *kbts_CreateShapeScratchpad(kbts_shape_config *Config, kbts_allocator_function *Allocator, void *AllocatorData); +KBTS_EXPORT void kbts_DestroyShapeScratchpad(kbts_shape_scratchpad *Scratchpad); + // // Glyph iterator // @@ -3700,8 +3952,10 @@ KBTS_EXPORT kbts_script kbts_ScriptTagToScript(kbts_script_tag Tag); #ifdef KB_TEXT_SHAPE_IMPLEMENTATION #ifdef _MSC_VER #define KBTS__UNUSED(X) (void)sizeof((X)) +#define KBTS__RESTRICT __restrict #else #define KBTS__UNUSED(X) (void)(X) +#define KBTS__RESTRICT __restrict__ #endif # define KBTS__FOR(I, Start, End) for(kbts_un I = (Start); I < (End); ++I) # ifdef __cplusplus @@ -3713,6 +3967,8 @@ KBTS_EXPORT kbts_script kbts_ScriptTagToScript(kbts_script_tag Tag); # endif # define KBTS__MAX(A, B) (((A) < (B)) ? (B) : (A)) # define KBTS__MIN(A, B) (((A) < (B)) ? (A) : (B)) +# define KBTS__IDIV_ROUND_UP(A, B) (((A) + ((B) - 1)) / (B)) +# define KBTS__ROUND_UP_POW2(A, B) (((A) + ((B) - 1)) & ~((B) - 1)) # define KBTS__ARRAY_LENGTH(A) (sizeof(A)/sizeof(*(A))) # define KBTS__POINTER_AFTER(Type, X) ((Type *)((X) + 1)) # define KBTS__POINTER_OFFSET(Type, Base, Offset) ((Type *)((char *)(Base) + (Offset))) @@ -3736,8 +3992,12 @@ KBTS_EXPORT kbts_script kbts_ScriptTagToScript(kbts_script_tag Tag); # define KBTS__SET64(Args) (0u KBTS__PASTE(KBTS__SET64_0 Args, End)) #define KBTS__U32BE(X) kbts__ByteSwap32((X)) #define KBTS__U32LE(X) (X) +#define KBTS__BIT_WIDTH(Type) (sizeof(Type)*8) -#define KBTS_LOOKUP_STACK_SIZE 64 +#define KBTS__DELETED_SORT_KEY 0xFFFFFFFF + +#define KBTS__BUCKETED_GLYPHS_PER_BLOCK 64 +#define KBTS_LOOKUP_STACK_SIZE 32 # ifndef KBTS_ASSERT #ifndef KB_TEXT_SHAPE_NO_CRT @@ -3814,6 +4074,15 @@ KBTS_INLINE kbts_u32 kbts__MsbPositionOrZero32(kbts_u32 X) } return (kbts_u32)Result; } +KBTS_INLINE kbts_u32 kbts__LsbPositionOrBitWidth32(kbts_u32 X) +{ + unsigned long Result; + if(!_BitScanForward(&Result, X)) + { + Result = 32; + } + return (kbts_u32)Result; +} # elif defined(__clang__) || defined(__GNUC__) KBTS_INLINE kbts_u32 kbts__MsbPositionOrZero32(kbts_u32 X) { @@ -3824,6 +4093,15 @@ KBTS_INLINE kbts_u32 kbts__MsbPositionOrZero32(kbts_u32 X) } return Result; } +KBTS_INLINE kbts_u32 kbts__LsbPositionOrBitWidth32(kbts_u32 X) +{ + kbts_u32 Result = 32; + if(X) + { + Result = __builtin_ctz(X); + } + return Result; +} # else # error Unsupported compiler! # endif @@ -4177,6 +4455,7 @@ enum kbts__op_kind_enum KBTS__OP_KIND_NORMALIZE, KBTS__OP_KIND_NORMALIZE_HANGUL, KBTS__OP_KIND_FLAG_JOINING_LETTERS, + KBTS__OP_KIND_BEGIN_GSUB, KBTS__OP_KIND_GSUB_FEATURES, KBTS__OP_KIND_GSUB_FEATURES_WITH_USER, KBTS__OP_KIND_GPOS_METRICS, @@ -4203,6 +4482,7 @@ typedef struct kbts__op_list } kbts__op_list; static kbts__op_kind kbts__Ops_Default[] = { KBTS__OP_KIND_NORMALIZE, + KBTS__OP_KIND_BEGIN_GSUB, KBTS__OP_KIND_GSUB_FEATURES, KBTS__OP_KIND_GSUB_FEATURES_WITH_USER, KBTS__OP_KIND_GPOS_METRICS, @@ -4211,13 +4491,14 @@ static kbts__op_kind kbts__Ops_Default[] = { }; static kbts__feature_stage kbts__FeatureStages_Default[] = { {1, {{0ull, 0ull, 0ull, 0ull | KBTS__FEATURE_FLAG3(rvrn)}}}, - {12, {{0ull | KBTS__FEATURE_FLAG0(frac) | KBTS__FEATURE_FLAG0(numr) | KBTS__FEATURE_FLAG0(dnom) | KBTS__FEATURE_FLAG0(ccmp) | KBTS__FEATURE_FLAG0(clig) | KBTS__FEATURE_FLAG0(calt), 0ull, 0ull | KBTS__FEATURE_FLAG2(ltra) | KBTS__FEATURE_FLAG2(ltrm) | KBTS__FEATURE_FLAG2(locl) | KBTS__FEATURE_FLAG2(rlig) | KBTS__FEATURE_FLAG2(liga) | KBTS__FEATURE_FLAG2(rclt), 0ull}}}, + {19, {{0ull | KBTS__FEATURE_FLAG0(frac) | KBTS__FEATURE_FLAG0(numr) | KBTS__FEATURE_FLAG0(dnom) | KBTS__FEATURE_FLAG0(ccmp) | KBTS__FEATURE_FLAG0(clig) | KBTS__FEATURE_FLAG0(calt) | KBTS__FEATURE_FLAG0(abvm) | KBTS__FEATURE_FLAG0(blwm) | KBTS__FEATURE_FLAG0(curs), 0ull, 0ull | KBTS__FEATURE_FLAG2(ltra) | KBTS__FEATURE_FLAG2(ltrm) | KBTS__FEATURE_FLAG2(locl) | KBTS__FEATURE_FLAG2(rlig) | KBTS__FEATURE_FLAG2(liga) | KBTS__FEATURE_FLAG2(rclt) | KBTS__FEATURE_FLAG2(mark) | KBTS__FEATURE_FLAG2(mkmk) | KBTS__FEATURE_FLAG2(dist) | KBTS__FEATURE_FLAG2(kern), 0ull}}}, {7, {{0ull | KBTS__FEATURE_FLAG0(abvm) | KBTS__FEATURE_FLAG0(blwm) | KBTS__FEATURE_FLAG0(curs), 0ull, 0ull | KBTS__FEATURE_FLAG2(mark) | KBTS__FEATURE_FLAG2(mkmk) | KBTS__FEATURE_FLAG2(dist) | KBTS__FEATURE_FLAG2(kern), 0ull}}}, }; -static kbts__op_list kbts__OpList_Default = {20, KBTS__ARRAY_LENGTH(kbts__FeatureStages_Default), kbts__FeatureStages_Default, KBTS__ARRAY_LENGTH(kbts__Ops_Default), kbts__Ops_Default}; +static kbts__op_list kbts__OpList_Default = {27, KBTS__ARRAY_LENGTH(kbts__FeatureStages_Default), kbts__FeatureStages_Default, KBTS__ARRAY_LENGTH(kbts__Ops_Default), kbts__Ops_Default}; static kbts__op_kind kbts__Ops_Hangul[] = { KBTS__OP_KIND_NORMALIZE, KBTS__OP_KIND_NORMALIZE_HANGUL, + KBTS__OP_KIND_BEGIN_GSUB, KBTS__OP_KIND_GSUB_FEATURES, KBTS__OP_KIND_GSUB_FEATURES_WITH_USER, KBTS__OP_KIND_GPOS_METRICS, @@ -4226,13 +4507,14 @@ static kbts__op_kind kbts__Ops_Hangul[] = { }; static kbts__feature_stage kbts__FeatureStages_Hangul[] = { {1, {{0ull, 0ull, 0ull, 0ull | KBTS__FEATURE_FLAG3(rvrn)}}}, - {14, {{0ull | KBTS__FEATURE_FLAG0(frac) | KBTS__FEATURE_FLAG0(numr) | KBTS__FEATURE_FLAG0(dnom) | KBTS__FEATURE_FLAG0(ljmo) | KBTS__FEATURE_FLAG0(vjmo) | KBTS__FEATURE_FLAG0(tjmo) | KBTS__FEATURE_FLAG0(ccmp) | KBTS__FEATURE_FLAG0(clig), 0ull, 0ull | KBTS__FEATURE_FLAG2(ltra) | KBTS__FEATURE_FLAG2(ltrm) | KBTS__FEATURE_FLAG2(rlig) | KBTS__FEATURE_FLAG2(liga) | KBTS__FEATURE_FLAG2(locl) | KBTS__FEATURE_FLAG2(rclt), 0ull}}}, + {21, {{0ull | KBTS__FEATURE_FLAG0(frac) | KBTS__FEATURE_FLAG0(numr) | KBTS__FEATURE_FLAG0(dnom) | KBTS__FEATURE_FLAG0(ljmo) | KBTS__FEATURE_FLAG0(vjmo) | KBTS__FEATURE_FLAG0(tjmo) | KBTS__FEATURE_FLAG0(ccmp) | KBTS__FEATURE_FLAG0(clig) | KBTS__FEATURE_FLAG0(abvm) | KBTS__FEATURE_FLAG0(blwm) | KBTS__FEATURE_FLAG0(curs), 0ull, 0ull | KBTS__FEATURE_FLAG2(ltra) | KBTS__FEATURE_FLAG2(ltrm) | KBTS__FEATURE_FLAG2(rlig) | KBTS__FEATURE_FLAG2(liga) | KBTS__FEATURE_FLAG2(locl) | KBTS__FEATURE_FLAG2(rclt) | KBTS__FEATURE_FLAG2(mkmk) | KBTS__FEATURE_FLAG2(mark) | KBTS__FEATURE_FLAG2(dist) | KBTS__FEATURE_FLAG2(kern), 0ull}}}, {7, {{0ull | KBTS__FEATURE_FLAG0(abvm) | KBTS__FEATURE_FLAG0(blwm) | KBTS__FEATURE_FLAG0(curs), 0ull, 0ull | KBTS__FEATURE_FLAG2(mkmk) | KBTS__FEATURE_FLAG2(mark) | KBTS__FEATURE_FLAG2(dist) | KBTS__FEATURE_FLAG2(kern), 0ull}}}, }; -static kbts__op_list kbts__OpList_Hangul = {22, KBTS__ARRAY_LENGTH(kbts__FeatureStages_Hangul), kbts__FeatureStages_Hangul, KBTS__ARRAY_LENGTH(kbts__Ops_Hangul), kbts__Ops_Hangul}; +static kbts__op_list kbts__OpList_Hangul = {29, KBTS__ARRAY_LENGTH(kbts__FeatureStages_Hangul), kbts__FeatureStages_Hangul, KBTS__ARRAY_LENGTH(kbts__Ops_Hangul), kbts__Ops_Hangul}; static kbts__op_kind kbts__Ops_ArabicRclt[] = { KBTS__OP_KIND_NORMALIZE, KBTS__OP_KIND_FLAG_JOINING_LETTERS, + KBTS__OP_KIND_BEGIN_GSUB, KBTS__OP_KIND_GSUB_FEATURES, KBTS__OP_KIND_GSUB_FEATURES, KBTS__OP_KIND_GSUB_FEATURES, @@ -4264,13 +4546,14 @@ static kbts__feature_stage kbts__FeatureStages_ArabicRclt[] = { {1, {{0ull | KBTS__FEATURE_FLAG0(med2), 0ull, 0ull, 0ull}}}, {1, {{0ull | KBTS__FEATURE_FLAG0(init), 0ull, 0ull, 0ull}}}, {1, {{0ull, 0ull, 0ull | KBTS__FEATURE_FLAG2(rlig), 0ull}}}, - {5, {{0ull | KBTS__FEATURE_FLAG0(calt) | KBTS__FEATURE_FLAG0(clig), 0ull, 0ull | KBTS__FEATURE_FLAG2(liga) | KBTS__FEATURE_FLAG2(mset) | KBTS__FEATURE_FLAG2(rclt), 0ull}}}, + {12, {{0ull | KBTS__FEATURE_FLAG0(calt) | KBTS__FEATURE_FLAG0(clig) | KBTS__FEATURE_FLAG0(abvm) | KBTS__FEATURE_FLAG0(blwm) | KBTS__FEATURE_FLAG0(curs), 0ull, 0ull | KBTS__FEATURE_FLAG2(liga) | KBTS__FEATURE_FLAG2(mset) | KBTS__FEATURE_FLAG2(rclt) | KBTS__FEATURE_FLAG2(mark) | KBTS__FEATURE_FLAG2(mkmk) | KBTS__FEATURE_FLAG2(dist) | KBTS__FEATURE_FLAG2(kern), 0ull}}}, {7, {{0ull | KBTS__FEATURE_FLAG0(abvm) | KBTS__FEATURE_FLAG0(blwm) | KBTS__FEATURE_FLAG0(curs), 0ull, 0ull | KBTS__FEATURE_FLAG2(mark) | KBTS__FEATURE_FLAG2(mkmk) | KBTS__FEATURE_FLAG2(dist) | KBTS__FEATURE_FLAG2(kern), 0ull}}}, }; -static kbts__op_list kbts__OpList_ArabicRclt = {29, KBTS__ARRAY_LENGTH(kbts__FeatureStages_ArabicRclt), kbts__FeatureStages_ArabicRclt, KBTS__ARRAY_LENGTH(kbts__Ops_ArabicRclt), kbts__Ops_ArabicRclt}; +static kbts__op_list kbts__OpList_ArabicRclt = {36, KBTS__ARRAY_LENGTH(kbts__FeatureStages_ArabicRclt), kbts__FeatureStages_ArabicRclt, KBTS__ARRAY_LENGTH(kbts__Ops_ArabicRclt), kbts__Ops_ArabicRclt}; static kbts__op_kind kbts__Ops_ArabicNoRclt[] = { KBTS__OP_KIND_NORMALIZE, KBTS__OP_KIND_FLAG_JOINING_LETTERS, + KBTS__OP_KIND_BEGIN_GSUB, KBTS__OP_KIND_GSUB_FEATURES, KBTS__OP_KIND_GSUB_FEATURES, KBTS__OP_KIND_GSUB_FEATURES, @@ -4304,13 +4587,14 @@ static kbts__feature_stage kbts__FeatureStages_ArabicNoRclt[] = { {1, {{0ull | KBTS__FEATURE_FLAG0(init), 0ull, 0ull, 0ull}}}, {1, {{0ull, 0ull, 0ull | KBTS__FEATURE_FLAG2(rlig), 0ull}}}, {1, {{0ull | KBTS__FEATURE_FLAG0(calt), 0ull, 0ull, 0ull}}}, - {3, {{0ull | KBTS__FEATURE_FLAG0(clig), 0ull, 0ull | KBTS__FEATURE_FLAG2(liga) | KBTS__FEATURE_FLAG2(mset), 0ull}}}, + {10, {{0ull | KBTS__FEATURE_FLAG0(clig) | KBTS__FEATURE_FLAG0(abvm) | KBTS__FEATURE_FLAG0(blwm) | KBTS__FEATURE_FLAG0(curs), 0ull, 0ull | KBTS__FEATURE_FLAG2(liga) | KBTS__FEATURE_FLAG2(mset) | KBTS__FEATURE_FLAG2(mark) | KBTS__FEATURE_FLAG2(mkmk) | KBTS__FEATURE_FLAG2(dist) | KBTS__FEATURE_FLAG2(kern), 0ull}}}, {7, {{0ull | KBTS__FEATURE_FLAG0(abvm) | KBTS__FEATURE_FLAG0(blwm) | KBTS__FEATURE_FLAG0(curs), 0ull, 0ull | KBTS__FEATURE_FLAG2(mark) | KBTS__FEATURE_FLAG2(mkmk) | KBTS__FEATURE_FLAG2(dist) | KBTS__FEATURE_FLAG2(kern), 0ull}}}, }; -static kbts__op_list kbts__OpList_ArabicNoRclt = {28, KBTS__ARRAY_LENGTH(kbts__FeatureStages_ArabicNoRclt), kbts__FeatureStages_ArabicNoRclt, KBTS__ARRAY_LENGTH(kbts__Ops_ArabicNoRclt), kbts__Ops_ArabicNoRclt}; +static kbts__op_list kbts__OpList_ArabicNoRclt = {35, KBTS__ARRAY_LENGTH(kbts__FeatureStages_ArabicNoRclt), kbts__FeatureStages_ArabicNoRclt, KBTS__ARRAY_LENGTH(kbts__Ops_ArabicNoRclt), kbts__Ops_ArabicNoRclt}; static kbts__op_kind kbts__Ops_Indic[] = { KBTS__OP_KIND_PRE_NORMALIZE_DOTTED_CIRCLES, KBTS__OP_KIND_NORMALIZE, + KBTS__OP_KIND_BEGIN_GSUB, KBTS__OP_KIND_GSUB_FEATURES, KBTS__OP_KIND_GSUB_FEATURES, KBTS__OP_KIND_BEGIN_CLUSTER, @@ -4348,13 +4632,14 @@ static kbts__feature_stage kbts__FeatureStages_Indic[] = { {1, {{0ull, 0ull, 0ull, 0ull | KBTS__FEATURE_FLAG3(vatu)}}}, {1, {{0ull | KBTS__FEATURE_FLAG0(cjct), 0ull, 0ull, 0ull}}}, {6, {{0ull | KBTS__FEATURE_FLAG0(abvs) | KBTS__FEATURE_FLAG0(blws) | KBTS__FEATURE_FLAG0(init), 0ull, 0ull | KBTS__FEATURE_FLAG2(haln) | KBTS__FEATURE_FLAG2(pres) | KBTS__FEATURE_FLAG2(psts), 0ull}}}, - {5, {{0ull | KBTS__FEATURE_FLAG0(calt) | KBTS__FEATURE_FLAG0(clig), 0ull, 0ull | KBTS__FEATURE_FLAG2(locl) | KBTS__FEATURE_FLAG2(rlig) | KBTS__FEATURE_FLAG2(rclt), 0ull}}}, + {12, {{0ull | KBTS__FEATURE_FLAG0(calt) | KBTS__FEATURE_FLAG0(clig) | KBTS__FEATURE_FLAG0(abvm) | KBTS__FEATURE_FLAG0(blwm) | KBTS__FEATURE_FLAG0(curs), 0ull, 0ull | KBTS__FEATURE_FLAG2(locl) | KBTS__FEATURE_FLAG2(rlig) | KBTS__FEATURE_FLAG2(rclt) | KBTS__FEATURE_FLAG2(mark) | KBTS__FEATURE_FLAG2(mkmk) | KBTS__FEATURE_FLAG2(dist) | KBTS__FEATURE_FLAG2(kern), 0ull}}}, {7, {{0ull | KBTS__FEATURE_FLAG0(abvm) | KBTS__FEATURE_FLAG0(blwm) | KBTS__FEATURE_FLAG0(curs), 0ull, 0ull | KBTS__FEATURE_FLAG2(mark) | KBTS__FEATURE_FLAG2(mkmk) | KBTS__FEATURE_FLAG2(dist) | KBTS__FEATURE_FLAG2(kern), 0ull}}}, }; -static kbts__op_list kbts__OpList_Indic = {35, KBTS__ARRAY_LENGTH(kbts__FeatureStages_Indic), kbts__FeatureStages_Indic, KBTS__ARRAY_LENGTH(kbts__Ops_Indic), kbts__Ops_Indic}; +static kbts__op_list kbts__OpList_Indic = {42, KBTS__ARRAY_LENGTH(kbts__FeatureStages_Indic), kbts__FeatureStages_Indic, KBTS__ARRAY_LENGTH(kbts__Ops_Indic), kbts__Ops_Indic}; static kbts__op_kind kbts__Ops_Khmer[] = { KBTS__OP_KIND_PRE_NORMALIZE_DOTTED_CIRCLES, KBTS__OP_KIND_NORMALIZE, + KBTS__OP_KIND_BEGIN_GSUB, KBTS__OP_KIND_GSUB_FEATURES, KBTS__OP_KIND_GSUB_FEATURES, KBTS__OP_KIND_BEGIN_CLUSTER, @@ -4370,13 +4655,14 @@ static kbts__feature_stage kbts__FeatureStages_Khmer[] = { {1, {{0ull, 0ull, 0ull, 0ull | KBTS__FEATURE_FLAG3(rvrn)}}}, {5, {{0ull | KBTS__FEATURE_FLAG0(frac) | KBTS__FEATURE_FLAG0(numr) | KBTS__FEATURE_FLAG0(dnom), 0ull, 0ull | KBTS__FEATURE_FLAG2(ltra) | KBTS__FEATURE_FLAG2(ltrm), 0ull}}}, {7, {{0ull | KBTS__FEATURE_FLAG0(ccmp) | KBTS__FEATURE_FLAG0(pref) | KBTS__FEATURE_FLAG0(blwf) | KBTS__FEATURE_FLAG0(abvf) | KBTS__FEATURE_FLAG0(pstf) | KBTS__FEATURE_FLAG0(cfar), 0ull, 0ull | KBTS__FEATURE_FLAG2(locl), 0ull}}}, - {8, {{0ull | KBTS__FEATURE_FLAG0(abvs) | KBTS__FEATURE_FLAG0(blws) | KBTS__FEATURE_FLAG0(calt) | KBTS__FEATURE_FLAG0(clig), 0ull, 0ull | KBTS__FEATURE_FLAG2(pres) | KBTS__FEATURE_FLAG2(psts) | KBTS__FEATURE_FLAG2(rclt) | KBTS__FEATURE_FLAG2(rlig), 0ull}}}, + {15, {{0ull | KBTS__FEATURE_FLAG0(abvs) | KBTS__FEATURE_FLAG0(blws) | KBTS__FEATURE_FLAG0(calt) | KBTS__FEATURE_FLAG0(clig) | KBTS__FEATURE_FLAG0(abvm) | KBTS__FEATURE_FLAG0(blwm) | KBTS__FEATURE_FLAG0(curs), 0ull, 0ull | KBTS__FEATURE_FLAG2(pres) | KBTS__FEATURE_FLAG2(psts) | KBTS__FEATURE_FLAG2(rclt) | KBTS__FEATURE_FLAG2(rlig) | KBTS__FEATURE_FLAG2(dist) | KBTS__FEATURE_FLAG2(kern) | KBTS__FEATURE_FLAG2(mark) | KBTS__FEATURE_FLAG2(mkmk), 0ull}}}, {7, {{0ull | KBTS__FEATURE_FLAG0(abvm) | KBTS__FEATURE_FLAG0(blwm) | KBTS__FEATURE_FLAG0(curs), 0ull, 0ull | KBTS__FEATURE_FLAG2(dist) | KBTS__FEATURE_FLAG2(kern) | KBTS__FEATURE_FLAG2(mark) | KBTS__FEATURE_FLAG2(mkmk), 0ull}}}, }; -static kbts__op_list kbts__OpList_Khmer = {28, KBTS__ARRAY_LENGTH(kbts__FeatureStages_Khmer), kbts__FeatureStages_Khmer, KBTS__ARRAY_LENGTH(kbts__Ops_Khmer), kbts__Ops_Khmer}; +static kbts__op_list kbts__OpList_Khmer = {35, KBTS__ARRAY_LENGTH(kbts__FeatureStages_Khmer), kbts__FeatureStages_Khmer, KBTS__ARRAY_LENGTH(kbts__Ops_Khmer), kbts__Ops_Khmer}; static kbts__op_kind kbts__Ops_Myanmar[] = { KBTS__OP_KIND_PRE_NORMALIZE_DOTTED_CIRCLES, KBTS__OP_KIND_NORMALIZE, + KBTS__OP_KIND_BEGIN_GSUB, KBTS__OP_KIND_GSUB_FEATURES, KBTS__OP_KIND_GSUB_FEATURES, KBTS__OP_KIND_BEGIN_CLUSTER, @@ -4400,11 +4686,12 @@ static kbts__feature_stage kbts__FeatureStages_Myanmar[] = { {1, {{0ull | KBTS__FEATURE_FLAG0(pref), 0ull, 0ull, 0ull}}}, {1, {{0ull | KBTS__FEATURE_FLAG0(blwf), 0ull, 0ull, 0ull}}}, {1, {{0ull | KBTS__FEATURE_FLAG0(pstf), 0ull, 0ull, 0ull}}}, - {9, {{0ull | KBTS__FEATURE_FLAG0(abvs) | KBTS__FEATURE_FLAG0(blws) | KBTS__FEATURE_FLAG0(calt) | KBTS__FEATURE_FLAG0(clig), 0ull, 0ull | KBTS__FEATURE_FLAG2(pres) | KBTS__FEATURE_FLAG2(psts) | KBTS__FEATURE_FLAG2(rlig) | KBTS__FEATURE_FLAG2(liga) | KBTS__FEATURE_FLAG2(rclt), 0ull}}}, + {16, {{0ull | KBTS__FEATURE_FLAG0(abvs) | KBTS__FEATURE_FLAG0(blws) | KBTS__FEATURE_FLAG0(calt) | KBTS__FEATURE_FLAG0(clig) | KBTS__FEATURE_FLAG0(abvm) | KBTS__FEATURE_FLAG0(blwm) | KBTS__FEATURE_FLAG0(curs), 0ull, 0ull | KBTS__FEATURE_FLAG2(pres) | KBTS__FEATURE_FLAG2(psts) | KBTS__FEATURE_FLAG2(rlig) | KBTS__FEATURE_FLAG2(liga) | KBTS__FEATURE_FLAG2(rclt) | KBTS__FEATURE_FLAG2(dist) | KBTS__FEATURE_FLAG2(kern) | KBTS__FEATURE_FLAG2(mark) | KBTS__FEATURE_FLAG2(mkmk), 0ull}}}, {7, {{0ull | KBTS__FEATURE_FLAG0(abvm) | KBTS__FEATURE_FLAG0(blwm) | KBTS__FEATURE_FLAG0(curs), 0ull, 0ull | KBTS__FEATURE_FLAG2(dist) | KBTS__FEATURE_FLAG2(kern) | KBTS__FEATURE_FLAG2(mark) | KBTS__FEATURE_FLAG2(mkmk), 0ull}}}, }; -static kbts__op_list kbts__OpList_Myanmar = {28, KBTS__ARRAY_LENGTH(kbts__FeatureStages_Myanmar), kbts__FeatureStages_Myanmar, KBTS__ARRAY_LENGTH(kbts__Ops_Myanmar), kbts__Ops_Myanmar}; +static kbts__op_list kbts__OpList_Myanmar = {35, KBTS__ARRAY_LENGTH(kbts__FeatureStages_Myanmar), kbts__FeatureStages_Myanmar, KBTS__ARRAY_LENGTH(kbts__Ops_Myanmar), kbts__Ops_Myanmar}; static kbts__op_kind kbts__Ops_Tibetan[] = { + KBTS__OP_KIND_BEGIN_GSUB, KBTS__OP_KIND_GSUB_FEATURES, KBTS__OP_KIND_GSUB_FEATURES, KBTS__OP_KIND_GSUB_FEATURES_WITH_USER, @@ -4415,14 +4702,15 @@ static kbts__op_kind kbts__Ops_Tibetan[] = { static kbts__feature_stage kbts__FeatureStages_Tibetan[] = { {1, {{0ull, 0ull, 0ull | KBTS__FEATURE_FLAG2(locl), 0ull}}}, {1, {{0ull | KBTS__FEATURE_FLAG0(ccmp), 0ull, 0ull, 0ull}}}, - {4, {{0ull | KBTS__FEATURE_FLAG0(abvs) | KBTS__FEATURE_FLAG0(blws) | KBTS__FEATURE_FLAG0(calt), 0ull, 0ull | KBTS__FEATURE_FLAG2(liga), 0ull}}}, + {8, {{0ull | KBTS__FEATURE_FLAG0(abvs) | KBTS__FEATURE_FLAG0(blws) | KBTS__FEATURE_FLAG0(calt) | KBTS__FEATURE_FLAG0(abvm) | KBTS__FEATURE_FLAG0(blwm), 0ull, 0ull | KBTS__FEATURE_FLAG2(liga) | KBTS__FEATURE_FLAG2(kern) | KBTS__FEATURE_FLAG2(mkmk), 0ull}}}, {4, {{0ull | KBTS__FEATURE_FLAG0(abvm) | KBTS__FEATURE_FLAG0(blwm), 0ull, 0ull | KBTS__FEATURE_FLAG2(kern) | KBTS__FEATURE_FLAG2(mkmk), 0ull}}}, }; -static kbts__op_list kbts__OpList_Tibetan = {10, KBTS__ARRAY_LENGTH(kbts__FeatureStages_Tibetan), kbts__FeatureStages_Tibetan, KBTS__ARRAY_LENGTH(kbts__Ops_Tibetan), kbts__Ops_Tibetan}; +static kbts__op_list kbts__OpList_Tibetan = {14, KBTS__ARRAY_LENGTH(kbts__FeatureStages_Tibetan), kbts__FeatureStages_Tibetan, KBTS__ARRAY_LENGTH(kbts__Ops_Tibetan), kbts__Ops_Tibetan}; static kbts__op_kind kbts__Ops_Use[] = { KBTS__OP_KIND_PRE_NORMALIZE_DOTTED_CIRCLES, KBTS__OP_KIND_NORMALIZE, KBTS__OP_KIND_FLAG_JOINING_LETTERS, + KBTS__OP_KIND_BEGIN_GSUB, KBTS__OP_KIND_GSUB_FEATURES, KBTS__OP_KIND_GSUB_FEATURES, KBTS__OP_KIND_BEGIN_CLUSTER, @@ -4446,10 +4734,10 @@ static kbts__feature_stage kbts__FeatureStages_Use[] = { {1, {{0ull | KBTS__FEATURE_FLAG0(pref), 0ull, 0ull, 0ull}}}, {7, {{0ull | KBTS__FEATURE_FLAG0(abvf) | KBTS__FEATURE_FLAG0(blwf) | KBTS__FEATURE_FLAG0(cjct) | KBTS__FEATURE_FLAG0(half) | KBTS__FEATURE_FLAG0(pstf), 0ull, 0ull | KBTS__FEATURE_FLAG2(rkrf), 0ull | KBTS__FEATURE_FLAG3(vatu)}}}, {4, {{0ull | KBTS__FEATURE_FLAG0(fina) | KBTS__FEATURE_FLAG0(init) | KBTS__FEATURE_FLAG0(isol) | KBTS__FEATURE_FLAG0(medi), 0ull, 0ull, 0ull}}}, - {10, {{0ull | KBTS__FEATURE_FLAG0(abvs) | KBTS__FEATURE_FLAG0(blws) | KBTS__FEATURE_FLAG0(calt) | KBTS__FEATURE_FLAG0(clig), 0ull, 0ull | KBTS__FEATURE_FLAG2(haln) | KBTS__FEATURE_FLAG2(pres) | KBTS__FEATURE_FLAG2(psts) | KBTS__FEATURE_FLAG2(liga) | KBTS__FEATURE_FLAG2(rclt) | KBTS__FEATURE_FLAG2(rlig), 0ull}}}, + {24, {{0ull | KBTS__FEATURE_FLAG0(abvs) | KBTS__FEATURE_FLAG0(blws) | KBTS__FEATURE_FLAG0(calt) | KBTS__FEATURE_FLAG0(clig) | KBTS__FEATURE_FLAG0(abvm) | KBTS__FEATURE_FLAG0(blwm) | KBTS__FEATURE_FLAG0(curs) | KBTS__FEATURE_FLAG0(abvm) | KBTS__FEATURE_FLAG0(blwm) | KBTS__FEATURE_FLAG0(curs), 0ull, 0ull | KBTS__FEATURE_FLAG2(haln) | KBTS__FEATURE_FLAG2(pres) | KBTS__FEATURE_FLAG2(psts) | KBTS__FEATURE_FLAG2(liga) | KBTS__FEATURE_FLAG2(rclt) | KBTS__FEATURE_FLAG2(rlig) | KBTS__FEATURE_FLAG2(dist) | KBTS__FEATURE_FLAG2(kern) | KBTS__FEATURE_FLAG2(mark) | KBTS__FEATURE_FLAG2(mkmk) | KBTS__FEATURE_FLAG2(dist) | KBTS__FEATURE_FLAG2(kern) | KBTS__FEATURE_FLAG2(mark) | KBTS__FEATURE_FLAG2(mkmk), 0ull}}}, {7, {{0ull | KBTS__FEATURE_FLAG0(abvm) | KBTS__FEATURE_FLAG0(blwm) | KBTS__FEATURE_FLAG0(curs), 0ull, 0ull | KBTS__FEATURE_FLAG2(dist) | KBTS__FEATURE_FLAG2(kern) | KBTS__FEATURE_FLAG2(mark) | KBTS__FEATURE_FLAG2(mkmk), 0ull}}}, }; -static kbts__op_list kbts__OpList_Use = {40, KBTS__ARRAY_LENGTH(kbts__FeatureStages_Use), kbts__FeatureStages_Use, KBTS__ARRAY_LENGTH(kbts__Ops_Use), kbts__Ops_Use}; +static kbts__op_list kbts__OpList_Use = {54, KBTS__ARRAY_LENGTH(kbts__FeatureStages_Use), kbts__FeatureStages_Use, KBTS__ARRAY_LENGTH(kbts__Ops_Use), kbts__Ops_Use}; #define KBTS__MAXIMUM_DECOMPOSITION_CODEPOINTS 6 typedef struct kbts__script_properties { kbts_u32 Tag; @@ -5134,23 +5422,32 @@ static kbts_u8 kbts__ScriptExtensions[447] = { 48,60,68,69,80,93,131,149,158,11,32,161,32,150,64,72,97,15,59,4,103,26,27,76,26,76,26,75,76,4,25, }; -static kbts_u8 kbts__UnicodeDecomposition_PageIndices[17407] = { - 0,1,1,2,3,4,5,6,7,1,1,1,1,8,9,10,11,12,1,13,1,1,1,1,14,1,1,15,1,1,1,1, - 1,1,1,1,16,17,1,18,19,20,1,1,1,21,1,22,1,23,1,24,1,25,1,26,1,1,1,1,1,27,28,1, - 29,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,30,31, - 1,1,1,1,1,1,1,1,1,1,1,1,32,33,1,1,1,1,1,1,1,1,1,1,34,35,36,37,38,39,40,41, - 42,1,1,1,43,1,44,45,46,47,48,49,50,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,51,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,52,53,54,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, +static kbts_u8 kbts__UnicodeDecomposition_PageIndices[12194] = { + 0,1,1,1,1,1,1,1,1,1,1,1,2,3,4,5,6,7,8,9,10,11,12,13,1,1,14,15,16,17,18,19, + 20,21,22,23,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,24,1,1,25,26,27,28,29,30,31,1,1, + 32,33,1,34,1,35,1,36,1,1,1,1,37,38,39,40,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, + 1,1,41,1,1,1,1,1,1,1,1,1,42,43,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, + 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,44,45,1,46,1,1,1,1,1,1,47,48,1,1, + 1,1,1,49,1,50,1,1,1,1,1,1,1,1,1,1,1,1,1,1,51,52,1,1,1,1,1,1,53,1,1,1, + 1,1,1,1,54,1,1,1,1,1,1,1,55,1,1,1,1,1,1,1,56,1,1,1,1,1,1,1,1,57,1,1, + 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,58,59,60,61,62,63,64,65,1,1,1,1, + 1,1,66,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, + 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,67,68,1,1,1, 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, + 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,69,70,1,71,72,1,1,1,1,1,1,1,1,1,1,1, 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, + 73,74,75,76,77,78,79,80,81,82,83,84,85,86,87,88,89,90,91,92,93,94,95,96,97,98,99,100,101,102,103,104, + 105,106,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,107,1,1,1,1,1,1,108,109,1,110,1,1,1, + 111,1,112,1,113,1,114,115,116,1,117,1,1,1,118,1,1,1,119,1,1,1,1,1,1,1,1,1,1,1,1,1, 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, + 1,1,1,1,1,1,1,1,1,1,1,1,1,120,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, + 1,1,1,1,121,122,123,124,1,125,126,127,128,129,1,130,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, @@ -5166,11 +5463,7 @@ static kbts_u8 kbts__UnicodeDecomposition_PageIndices[17407] = { 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,55,56,57,58,59,60,61,62,63,64,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,65,1,1,1,1,1,1,1,1, 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,66,1,67,1,1,1,1,1,1,1,1,68,69,70,1,1,71,1,1,1,72,1,1,1,1,1,1,1,1,1, - 1,1,1,1,73,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, @@ -5179,8 +5472,6 @@ static kbts_u8 kbts__UnicodeDecomposition_PageIndices[17407] = { 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,74,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,75,1,1,1,1,1,1,1,1,1,1, 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, @@ -5193,7 +5484,6 @@ static kbts_u8 kbts__UnicodeDecomposition_PageIndices[17407] = { 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,76,77,78,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, @@ -5230,7 +5520,6 @@ static kbts_u8 kbts__UnicodeDecomposition_PageIndices[17407] = { 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 79,80,81,82,83,84,85,86,87,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, @@ -5258,16 +5547,23 @@ static kbts_u8 kbts__UnicodeDecomposition_PageIndices[17407] = { 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, + 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,131,132,133,134,135,136,137,138,139,140,141,142,143,144,145,146, + 147,148,149,150,151,152,153,154,155,156,157,158,159,160,1,1,1,161,162,163,164,1,1,1,1,1,1,1,1,1,1,1, 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, + 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,165,1,166,1, 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, + 1,1,1,1,1,1,1,1,1,167,168,1,1,1,1,1,1,1,169,1,1,1,1,1,1,1,1,1,1,1,1,1, + 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,170,1,1,1,171,172,1,1,173,1,1,1, + 1,1,1,1,1,1,1,1,1,1,1,174,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,175,1,1,1,1, 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, + 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,176,1,1,1,1,1,1,1,1,1,1,1,1, 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, @@ -5303,11 +5599,13 @@ static kbts_u8 kbts__UnicodeDecomposition_PageIndices[17407] = { 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, + 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,177,1,1,1,1,1,1,1,1,1,1,1,1,1, 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, + 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,178,1,1,1,1,1,1,1,1,1, 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, @@ -5357,6 +5655,7 @@ static kbts_u8 kbts__UnicodeDecomposition_PageIndices[17407] = { 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, + 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,179,180,1,1,1,1,181,182,1,1,1, 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, @@ -5504,368 +5803,128 @@ static kbts_u8 kbts__UnicodeDecomposition_PageIndices[17407] = { 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, + 183,184,185,186,187,188,189,190,191,192,193,194,195,196,197,198,199,200,201,202,203,204,205,206,207,208,209,210,211,212,213,214, + 215,216, }; -static kbts_u64 kbts__UnicodeDecomposition_Data[5632] = { +static kbts_u64 kbts__UnicodeDecomposition_Data[3472] = { 52776558133248ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull, - 0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull, - 0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull, - 0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull, 6442451206ull,6450839814ull,6459228422ull,6467617030ull,6509560070ull,6526337286ull,0ull,6769606926ull,6442451222ull,6450839830ull,6459228438ull,6509560086ull,6442451238ull,6450839846ull,6459228454ull,6509560102ull,0ull,6467617082ull,6442451262ull,6450839870ull,6459228478ull,6467617086ull,6509560126ull,0ull,0ull,6442451286ull,6450839894ull,6459228502ull,6509560150ull,6450839910ull,0ull,0ull, 6442451334ull,6450839942ull,6459228550ull,6467617158ull,6509560198ull,6526337414ull,0ull,6769607054ull,6442451350ull,6450839958ull,6459228566ull,6509560214ull,6442451366ull,6450839974ull,6459228582ull,6509560230ull,0ull,6467617210ull,6442451390ull,6450839998ull,6459228606ull,6467617214ull,6509560254ull,0ull,0ull,6442451414ull,6450840022ull,6459228630ull,6509560278ull,6450840038ull,0ull,6509560294ull, 6476005638ull,6476005766ull,6492782854ull,6492782982ull,6777995526ull,6777995654ull,6450839822ull,6450839950ull,6459228430ull,6459228558ull,6501171470ull,6501171598ull,6543114510ull,6543114638ull,6543114514ull,6543114642ull,0ull,0ull,6476005654ull,6476005782ull,6492782870ull,6492782998ull,6501171478ull,6501171606ull,6777995542ull,6777995670ull,6543114518ull,6543114646ull,6459228446ull,6459228574ull,6492782878ull,6492783006ull, 6501171486ull,6501171614ull,6769606942ull,6769607070ull,6459228450ull,6459228578ull,0ull,0ull,6467617062ull,6467617190ull,6476005670ull,6476005798ull,6492782886ull,6492783014ull,6777995558ull,6777995686ull,6501171494ull,0ull,0ull,0ull,6459228458ull,6459228586ull,6769606958ull,6769607086ull,0ull,6450839858ull,6450839986ull,6769606962ull,6769607090ull,6543114546ull,6543114674ull,0ull, 0ull,0ull,0ull,6450839866ull,6450839994ull,6769606970ull,6769607098ull,6543114554ull,6543114682ull,0ull,0ull,0ull,6476005694ull,6476005822ull,6492782910ull,6492783038ull,6534725950ull,6534726078ull,0ull,0ull,6450839882ull,6450840010ull,6769606986ull,6769607114ull,6543114570ull,6543114698ull,6450839886ull,6450840014ull,6459228494ull,6459228622ull,6769606990ull,6769607118ull, 6543114574ull,6543114702ull,6769606994ull,6769607122ull,6543114578ull,6543114706ull,0ull,0ull,6467617110ull,6467617238ull,6476005718ull,6476005846ull,6492782934ull,6492783062ull,6526337366ull,6526337494ull,6534725974ull,6534726102ull,6777995606ull,6777995734ull,6459228510ull,6459228638ull,6459228518ull,6459228646ull,6509560166ull,6450839914ull,6450840042ull,6501171562ull,6501171690ull,6543114602ull,6543114730ull,0ull, - 0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull, 6668943678ull,6668943806ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,6668943702ull,6668943830ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull, 0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,6543114502ull,6543114630ull,6543114534ull,6543114662ull,6543114558ull,6543114686ull,6543114582ull,6543114710ull,6476006258ull,6476006386ull,6450840434ull,6450840562ull,6543115122ull,6543115250ull,6442451826ull,6442451954ull,0ull,6476006162ull,6476006290ull, 6476007578ull,6476007582ull,6476006170ull,6476006298ull,0ull,0ull,6543114526ull,6543114654ull,6543114542ull,6543114670ull,6777995582ull,6777995710ull,6476007338ull,6476007342ull,6543115998ull,6543116874ull,6543114666ull,0ull,0ull,0ull,6450839838ull,6450839966ull,0ull,0ull,6442451258ull,6442451386ull,6450840342ull,6450840470ull,6450840346ull,6450840474ull,6450840418ull,6450840546ull, 6568280326ull,6568280454ull,6585057542ull,6585057670ull,6568280342ull,6568280470ull,6585057558ull,6585057686ull,6568280358ull,6568280486ull,6585057574ull,6585057702ull,6568280382ull,6568280510ull,6585057598ull,6585057726ull,6568280394ull,6568280522ull,6585057610ull,6585057738ull,6568280406ull,6568280534ull,6585057622ull,6585057750ull,6761218382ull,6761218510ull,6761218386ull,6761218514ull,0ull,0ull,6543114530ull,6543114658ull, 0ull,0ull,0ull,0ull,0ull,0ull,6501171462ull,6501171590ull,6769606934ull,6769607062ull,6476006234ull,6476006362ull,6476006230ull,6476006358ull,6501171518ull,6501171646ull,6476007610ull,6476007614ull,6476005734ull,6476005862ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull, - 3073ull,3077ull,0ull,3149ull,6450842658ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull, - 0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,2789ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,237ull,0ull, + 3073ull,3077ull,0ull,3149ull,6450842658ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,2789ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,237ull,0ull, 0ull,0ull,0ull,0ull,0ull,6450840226ull,6450843206ull,733ull,6450843222ull,6450843230ull,6450843238ull,0ull,6450843262ull,0ull,6450843286ull,6450843302ull,6450843434ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull, 0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,6509563494ull,6509563542ull,6450843334ull,6450843350ull,6450843358ull,6450843366ull,6450843438ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull, 0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,6509563622ull,6509563670ull,6450843390ull,6450843414ull,6450843430ull,0ull,0ull,0ull,0ull,6450843466ull,6509563722ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull, - 0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull, 6442455126ull,6509563990ull,0ull,6450843726ull,0ull,0ull,0ull,6509563930ull,0ull,0ull,0ull,0ull,6450843754ull,6442455138ull,6492786830ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,6492786786ull,0ull,0ull,0ull,0ull,0ull,0ull, - 0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,6492786914ull,0ull,0ull,0ull,0ull,0ull,0ull, - 0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,6442455254ull,6509564118ull,0ull,6450843854ull,0ull,0ull,0ull,6509564250ull,0ull,0ull,0ull,0ull,6450843882ull,6442455266ull,6492786958ull,0ull, - 0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,6568284626ull,6568284630ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull, - 0ull,6492786778ull,6492786906ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,6492786754ull,6492786882ull,6509563970ull,6509564098ull,0ull,0ull,6492786774ull,6492786902ull,0ull,0ull,6509564770ull,6509564774ull,6509563994ull,6509564122ull,6509563998ull,6509564126ull, - 0ull,0ull,6476009570ull,6476009698ull,6509564002ull,6509564130ull,6509564026ull,6509564154ull,0ull,0ull,6509564834ull,6509564838ull,6509564086ull,6509564214ull,6476009614ull,6476009742ull,6509564046ull,6509564174ull,6534729870ull,6534729998ull,6509564062ull,6509564190ull,0ull,0ull,6509564078ull,6509564206ull,0ull,0ull,0ull,0ull,0ull,0ull, - 0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull, - 0ull,0ull,13581162654ull,13589551262ull,13589551394ull,13597939870ull,13589551402ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull, + 0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,6492786914ull,0ull,0ull,0ull,0ull,0ull,0ull,6442455254ull,6509564118ull,0ull,6450843854ull,0ull,0ull,0ull,6509564250ull,0ull,0ull,0ull,0ull,6450843882ull,6442455266ull,6492786958ull,0ull, + 0ull,0ull,0ull,0ull,0ull,0ull,6568284626ull,6568284630ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,6492786778ull,6492786906ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull, + 6492786754ull,6492786882ull,6509563970ull,6509564098ull,0ull,0ull,6492786774ull,6492786902ull,0ull,0ull,6509564770ull,6509564774ull,6509563994ull,6509564122ull,6509563998ull,6509564126ull,0ull,0ull,6476009570ull,6476009698ull,6509564002ull,6509564130ull,6509564026ull,6509564154ull,0ull,0ull,6509564834ull,6509564838ull,6509564086ull,6509564214ull,6476009614ull,6476009742ull, + 6509564046ull,6509564174ull,6534729870ull,6534729998ull,6509564062ull,6509564190ull,0ull,0ull,6509564078ull,6509564206ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,13581162654ull,13589551262ull,13589551394ull,13597939870ull,13589551402ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull, 13589551958ull,0ull,13589551878ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,13589551946ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull, - 0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull, - 0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull, 0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,19830678690ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,19830678734ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull, - 0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,19830678614ull,19830678618ull,19830678622ull,19830678642ull,19830678662ull,19830678666ull,19830678702ull,19830678718ull, - 0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull, - 0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,20921198366ull,21130913566ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,20904421054ull, - 0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull, - 0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull, - 0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,21978163402ull,0ull,0ull,21978163426ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull, - 0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,21978163290ull,21978163294ull,21978163314ull,0ull,0ull,21978163374ull,0ull, - 0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull, - 0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,24343751966ull,0ull,0ull,24142425374ull,24352140574ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,24125648006ull,24125648010ull,0ull,0ull, - 0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull, - 0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,25216167706ull,25216167710ull,25425882906ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull, - 0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull, - 0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,26491236634ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull, - 0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull, - 27556590334ull,0ull,0ull,0ull,0ull,0ull,0ull,27556590362ull,27564978970ull,0ull,27397206810ull,27556590378ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull, - 0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull, - 0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,28437394714ull,28437394718ull,28647109914ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull, - 0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull, - 0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,29611800422ull,0ull,29653743462ull,29611800434ull,29787961190ull,0ull, - 0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull, + 0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,19830678614ull,19830678618ull,19830678622ull,19830678642ull,19830678662ull,19830678666ull,19830678702ull,19830678718ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,20921198366ull,21130913566ull,0ull,0ull,0ull, + 0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,20904421054ull,0ull,0ull,0ull,21978163402ull,0ull,0ull,21978163426ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull, + 0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,21978163290ull,21978163294ull,21978163314ull,0ull,0ull,21978163374ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,24343751966ull,0ull,0ull,24142425374ull,24352140574ull,0ull,0ull,0ull, + 0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,24125648006ull,24125648010ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,25216167706ull,25216167710ull,25425882906ull,0ull,0ull,0ull, + 0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,26491236634ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,27556590334ull,0ull,0ull,0ull,0ull,0ull,0ull,27556590362ull,27564978970ull,0ull,27397206810ull,27556590378ull,0ull,0ull,0ull,0ull, + 0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,28437394714ull,28437394718ull,28647109914ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,29611800422ull,0ull,29653743462ull,29611800434ull,29787961190ull,0ull, 0ull,0ull,0ull,33747385610ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,33747385650ull,0ull,0ull,0ull,0ull,33747385670ull,0ull,0ull,0ull,0ull,33747385690ull,0ull,0ull,0ull,0ull,33747385710ull,0ull,0ull,0ull, 0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,33730608386ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,33168571846ull,0ull,33185349062ull,33286012618ull,0ull,33286012622ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull, 0ull,33286012358ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,33747385930ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,33747385970ull,0ull,0ull, 0ull,0ull,33747385990ull,0ull,0ull,0ull,0ull,33747386010ull,0ull,0ull,0ull,0ull,33747386030ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,33730608706ull,0ull,0ull,0ull,0ull,0ull,0ull, - 0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull, - 0ull,0ull,0ull,0ull,0ull,0ull,34745630870ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull, - 0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull, - 0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,35235358072582ull,35235366461190ull, - 35235374849798ull,0ull,0ull,0ull,35235408404230ull,35235416792838ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull, - 0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull, - 0ull,0ull,0ull,0ull,0ull,0ull,58426682390ull,0ull,58426682398ull,0ull,58426682406ull,0ull,58426682414ull,0ull,58426682422ull,0ull,0ull,0ull,58426682438ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull, - 0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,58426682602ull,0ull,58426682610ull,0ull,0ull, - 58426682618ull,58426682622ull,0ull,58426682634ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull, - 0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull, - 6752829702ull,6752829830ull,6501171466ull,6501171594ull,6736052490ull,6736052618ull,6853493002ull,6853493130ull,6450840350ull,6450840478ull,6501171474ull,6501171602ull,6736052498ull,6736052626ull,6853493010ull,6853493138ull,6769606930ull,6769607058ull,6819938578ull,6819938706ull,6442452042ull,6442452046ull,6450840650ull,6450840654ull,6819938582ull,6819938710ull,6845104406ull,6845104534ull,6492784802ull,6492784806ull,6501171482ull,6501171610ull, - 6476005662ull,6476005790ull,6501171490ull,6501171618ull,6736052514ull,6736052642ull,6509560098ull,6509560226ull,6769606946ull,6769607074ull,6828327202ull,6828327330ull,6845104422ull,6845104550ull,6450840382ull,6450840510ull,6450839854ull,6450839982ull,6736052526ull,6736052654ull,6853493038ull,6853493166ull,6736052530ull,6736052658ull,6476036314ull,6476036318ull,6853493042ull,6853493170ull,6819938610ull,6819938738ull,6450839862ull,6450839990ull, - 6501171510ull,6501171638ull,6736052534ull,6736052662ull,6501171514ull,6501171642ull,6736052538ull,6736052666ull,6853493050ull,6853493178ull,6819938618ull,6819938746ull,6450840406ull,6450840534ull,6509560662ull,6509560790ull,6442452274ull,6442452278ull,6450840882ull,6450840886ull,6450839874ull,6450840002ull,6501171522ull,6501171650ull,6501171530ull,6501171658ull,6736052554ull,6736052682ull,6476036458ull,6476036462ull,6853493066ull,6853493194ull, - 6501171534ull,6501171662ull,6736052558ull,6736052686ull,6501172586ull,6501172590ull,6501172610ull,6501172614ull,6501202314ull,6501202318ull,6501171538ull,6501171666ull,6736052562ull,6736052690ull,6853493074ull,6853493202ull,6819938642ull,6819938770ull,6744441174ull,6744441302ull,6845104470ull,6845104598ull,6819938646ull,6819938774ull,6450840994ull,6450840998ull,6509561258ull,6509561262ull,6467617114ull,6467617242ull,6736052570ull,6736052698ull, - 6442451294ull,6442451422ull,6450839902ull,6450840030ull,6509560158ull,6509560286ull,6501171550ull,6501171678ull,6736052574ull,6736052702ull,6501171554ull,6501171682ull,6509560162ull,6509560290ull,6501171558ull,6501171686ull,6459228522ull,6459228650ull,6736052586ull,6736052714ull,6853493098ull,6853493226ull,6853493154ull,6509560274ull,6526337502ull,6526337510ull,0ull,6501172734ull,0ull,0ull,0ull,0ull, - 6736052486ull,6736052614ull,6517948678ull,6517948806ull,6450840330ull,6450840458ull,6442451722ull,6442451850ull,6517949194ull,6517949322ull,6467617546ull,6467617674ull,6459259522ull,6459259526ull,6450840586ull,6450840590ull,6442451978ull,6442451982ull,6517949450ull,6517949454ull,6467617802ull,6467617806ull,6492813954ull,6492813958ull,6736052502ull,6736052630ull,6517948694ull,6517948822ull,6467617046ull,6467617174ull,6450840362ull,6450840490ull, - 6442451754ull,6442451882ull,6517949226ull,6517949354ull,6467617578ull,6467617706ull,6459259618ull,6459259622ull,6517948710ull,6517948838ull,6736052518ull,6736052646ull,6736052542ull,6736052670ull,6517948734ull,6517948862ull,6450840402ull,6450840530ull,6442451794ull,6442451922ull,6517949266ull,6517949394ull,6467617618ull,6467617746ull,6459259698ull,6459259702ull,6450841218ull,6450841222ull,6442452610ull,6442452614ull,6517950082ull,6517950086ull, - 6467618434ull,6467618438ull,6736053890ull,6736053894ull,6736052566ull,6736052694ull,6517948758ull,6517948886ull,6450841278ull,6450841282ull,6442452670ull,6442452674ull,6517950142ull,6517950146ull,6467618494ull,6467618498ull,6736053950ull,6736053954ull,6442451302ull,6442451430ull,6736052582ull,6736052710ull,6517948774ull,6517948902ull,6467617126ull,6467617254ull,0ull,0ull,0ull,0ull,0ull,0ull, - 6601838278ull,6610226886ull,6442482690ull,6442482694ull,6450871298ull,6450871302ull,6996130818ull,6996130822ull,6601838150ull,6610226758ull,6442482722ull,6442482726ull,6450871330ull,6450871334ull,6996130850ull,6996130854ull,6601838294ull,6610226902ull,6442482754ull,6442482758ull,6450871362ull,6450871366ull,0ull,0ull,6601838166ull,6610226774ull,6442482786ull,6442482790ull,6450871394ull,6450871398ull,0ull,0ull, - 6601838302ull,6610226910ull,6442482818ull,6442482822ull,6450871426ull,6450871430ull,6996130946ull,6996130950ull,6601838174ull,6610226782ull,6442482850ull,6442482854ull,6450871458ull,6450871462ull,6996130978ull,6996130982ull,6601838310ull,6610226918ull,6442482882ull,6442482886ull,6450871490ull,6450871494ull,6996131010ull,6996131014ull,6601838182ull,6610226790ull,6442482914ull,6442482918ull,6450871522ull,6450871526ull,6996131042ull,6996131046ull, - 6601838334ull,6610226942ull,6442482946ull,6442482950ull,6450871554ull,6450871558ull,0ull,0ull,6601838206ull,6610226814ull,6442482978ull,6442482982ull,6450871586ull,6450871590ull,0ull,0ull,6601838358ull,6610226966ull,6442483010ull,6442483014ull,6450871618ull,6450871622ull,6996131138ull,6996131142ull,0ull,6610226838ull,0ull,6442483046ull,0ull,6450871654ull,0ull,6996131174ull, - 6601838374ull,6610226982ull,6442483074ull,6442483078ull,6450871682ull,6450871686ull,6996131202ull,6996131206ull,6601838246ull,6610226854ull,6442483106ull,6442483110ull,6450871714ull,6450871718ull,6996131234ull,6996131238ull,6442454726ull,3761ull,6442454742ull,3765ull,6442454750ull,3769ull,6442454758ull,3773ull,6442454782ull,3889ull,6442454806ull,3893ull,6442454822ull,3897ull,0ull,0ull, - 7021296642ull,7021296646ull,7021296650ull,7021296654ull,7021296658ull,7021296662ull,7021296666ull,7021296670ull,7021296674ull,7021296678ull,7021296682ull,7021296686ull,7021296690ull,7021296694ull,7021296698ull,7021296702ull,7021296770ull,7021296774ull,7021296778ull,7021296782ull,7021296786ull,7021296790ull,7021296794ull,7021296798ull,7021296802ull,7021296806ull,7021296810ull,7021296814ull,7021296818ull,7021296822ull,7021296826ull,7021296830ull, - 7021297026ull,7021297030ull,7021297034ull,7021297038ull,7021297042ull,7021297046ull,7021297050ull,7021297054ull,7021297058ull,7021297062ull,7021297066ull,7021297070ull,7021297074ull,7021297078ull,7021297082ull,7021297086ull,6492786374ull,6476009158ull,7021297090ull,7021268678ull,7021268658ull,0ull,6996102854ull,7021297370ull,6492786246ull,6476009030ull,6442454598ull,3609ull,7021268550ull,0ull,3813ull,0ull, - 0ull,6996099746ull,7021297106ull,7021268702ull,7021268666ull,0ull,6996102878ull,7021297434ull,6442454614ull,3617ull,6442454622ull,3621ull,7021268574ull,6442483454ull,6450872062ull,6996131582ull,6492786406ull,6476009190ull,6442454826ull,3649ull,0ull,0ull,6996102886ull,6996102954ull,6492786278ull,6476009062ull,6442454630ull,3625ull,0ull,6442483706ull,6450872314ull,6996131834ull, - 6492786454ull,6476009238ull,6442454830ull,3777ull,6601838342ull,6610226950ull,6996102934ull,6996102958ull,6492786326ull,6476009110ull,6442454678ull,3641ull,6610226822ull,6442451618ull,3605ull,385ull,0ull,0ull,7021297138ull,7021268774ull,7021268794ull,0ull,6996102950ull,7021297626ull,6442454654ull,3633ull,6442454694ull,3645ull,7021268646ull,721ull,0ull,0ull, - 32777ull,32781ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,32833ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull, - 0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull, - 0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull, - 0ull,0ull,0ull,0ull,0ull,0ull,3749ull,0ull,0ull,0ull,301ull,789ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull, - 0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,6912247362ull,6912247370ull,0ull,0ull,0ull,0ull, - 0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,6912247378ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull, - 0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,6912247618ull,6912247634ull,6912247626ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull, - 0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull, - 0ull,0ull,0ull,0ull,6912247822ull,0ull,0ull,0ull,0ull,6912247842ull,0ull,0ull,6912247854ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull, - 0ull,0ull,0ull,0ull,6912247950ull,0ull,6912247958ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull, - 0ull,6912248050ull,0ull,0ull,6912248078ull,0ull,0ull,6912248086ull,0ull,6912248098ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull, + 0ull,0ull,0ull,0ull,0ull,0ull,34745630870ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,35235358072582ull,35235366461190ull, + 35235374849798ull,0ull,0ull,0ull,35235408404230ull,35235416792838ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,58426682390ull,0ull,58426682398ull,0ull,58426682406ull,0ull,58426682414ull,0ull,58426682422ull,0ull, + 0ull,0ull,58426682438ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,58426682602ull,0ull,58426682610ull,0ull,0ull, + 58426682618ull,58426682622ull,0ull,58426682634ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,6752829702ull,6752829830ull,6501171466ull,6501171594ull,6736052490ull,6736052618ull,6853493002ull,6853493130ull,6450840350ull,6450840478ull,6501171474ull,6501171602ull,6736052498ull,6736052626ull,6853493010ull,6853493138ull, + 6769606930ull,6769607058ull,6819938578ull,6819938706ull,6442452042ull,6442452046ull,6450840650ull,6450840654ull,6819938582ull,6819938710ull,6845104406ull,6845104534ull,6492784802ull,6492784806ull,6501171482ull,6501171610ull,6476005662ull,6476005790ull,6501171490ull,6501171618ull,6736052514ull,6736052642ull,6509560098ull,6509560226ull,6769606946ull,6769607074ull,6828327202ull,6828327330ull,6845104422ull,6845104550ull,6450840382ull,6450840510ull, + 6450839854ull,6450839982ull,6736052526ull,6736052654ull,6853493038ull,6853493166ull,6736052530ull,6736052658ull,6476036314ull,6476036318ull,6853493042ull,6853493170ull,6819938610ull,6819938738ull,6450839862ull,6450839990ull,6501171510ull,6501171638ull,6736052534ull,6736052662ull,6501171514ull,6501171642ull,6736052538ull,6736052666ull,6853493050ull,6853493178ull,6819938618ull,6819938746ull,6450840406ull,6450840534ull,6509560662ull,6509560790ull, + 6442452274ull,6442452278ull,6450840882ull,6450840886ull,6450839874ull,6450840002ull,6501171522ull,6501171650ull,6501171530ull,6501171658ull,6736052554ull,6736052682ull,6476036458ull,6476036462ull,6853493066ull,6853493194ull,6501171534ull,6501171662ull,6736052558ull,6736052686ull,6501172586ull,6501172590ull,6501172610ull,6501172614ull,6501202314ull,6501202318ull,6501171538ull,6501171666ull,6736052562ull,6736052690ull,6853493074ull,6853493202ull, + 6819938642ull,6819938770ull,6744441174ull,6744441302ull,6845104470ull,6845104598ull,6819938646ull,6819938774ull,6450840994ull,6450840998ull,6509561258ull,6509561262ull,6467617114ull,6467617242ull,6736052570ull,6736052698ull,6442451294ull,6442451422ull,6450839902ull,6450840030ull,6509560158ull,6509560286ull,6501171550ull,6501171678ull,6736052574ull,6736052702ull,6501171554ull,6501171682ull,6509560162ull,6509560290ull,6501171558ull,6501171686ull, + 6459228522ull,6459228650ull,6736052586ull,6736052714ull,6853493098ull,6853493226ull,6853493154ull,6509560274ull,6526337502ull,6526337510ull,0ull,6501172734ull,0ull,0ull,0ull,0ull,6736052486ull,6736052614ull,6517948678ull,6517948806ull,6450840330ull,6450840458ull,6442451722ull,6442451850ull,6517949194ull,6517949322ull,6467617546ull,6467617674ull,6459259522ull,6459259526ull,6450840586ull,6450840590ull, + 6442451978ull,6442451982ull,6517949450ull,6517949454ull,6467617802ull,6467617806ull,6492813954ull,6492813958ull,6736052502ull,6736052630ull,6517948694ull,6517948822ull,6467617046ull,6467617174ull,6450840362ull,6450840490ull,6442451754ull,6442451882ull,6517949226ull,6517949354ull,6467617578ull,6467617706ull,6459259618ull,6459259622ull,6517948710ull,6517948838ull,6736052518ull,6736052646ull,6736052542ull,6736052670ull,6517948734ull,6517948862ull, + 6450840402ull,6450840530ull,6442451794ull,6442451922ull,6517949266ull,6517949394ull,6467617618ull,6467617746ull,6459259698ull,6459259702ull,6450841218ull,6450841222ull,6442452610ull,6442452614ull,6517950082ull,6517950086ull,6467618434ull,6467618438ull,6736053890ull,6736053894ull,6736052566ull,6736052694ull,6517948758ull,6517948886ull,6450841278ull,6450841282ull,6442452670ull,6442452674ull,6517950142ull,6517950146ull,6467618494ull,6467618498ull, + 6736053950ull,6736053954ull,6442451302ull,6442451430ull,6736052582ull,6736052710ull,6517948774ull,6517948902ull,6467617126ull,6467617254ull,0ull,0ull,0ull,0ull,0ull,0ull,6601838278ull,6610226886ull,6442482690ull,6442482694ull,6450871298ull,6450871302ull,6996130818ull,6996130822ull,6601838150ull,6610226758ull,6442482722ull,6442482726ull,6450871330ull,6450871334ull,6996130850ull,6996130854ull, + 6601838294ull,6610226902ull,6442482754ull,6442482758ull,6450871362ull,6450871366ull,0ull,0ull,6601838166ull,6610226774ull,6442482786ull,6442482790ull,6450871394ull,6450871398ull,0ull,0ull,6601838302ull,6610226910ull,6442482818ull,6442482822ull,6450871426ull,6450871430ull,6996130946ull,6996130950ull,6601838174ull,6610226782ull,6442482850ull,6442482854ull,6450871458ull,6450871462ull,6996130978ull,6996130982ull, + 6601838310ull,6610226918ull,6442482882ull,6442482886ull,6450871490ull,6450871494ull,6996131010ull,6996131014ull,6601838182ull,6610226790ull,6442482914ull,6442482918ull,6450871522ull,6450871526ull,6996131042ull,6996131046ull,6601838334ull,6610226942ull,6442482946ull,6442482950ull,6450871554ull,6450871558ull,0ull,0ull,6601838206ull,6610226814ull,6442482978ull,6442482982ull,6450871586ull,6450871590ull,0ull,0ull, + 6601838358ull,6610226966ull,6442483010ull,6442483014ull,6450871618ull,6450871622ull,6996131138ull,6996131142ull,0ull,6610226838ull,0ull,6442483046ull,0ull,6450871654ull,0ull,6996131174ull,6601838374ull,6610226982ull,6442483074ull,6442483078ull,6450871682ull,6450871686ull,6996131202ull,6996131206ull,6601838246ull,6610226854ull,6442483106ull,6442483110ull,6450871714ull,6450871718ull,6996131234ull,6996131238ull, + 6442454726ull,3761ull,6442454742ull,3765ull,6442454750ull,3769ull,6442454758ull,3773ull,6442454782ull,3889ull,6442454806ull,3893ull,6442454822ull,3897ull,0ull,0ull,7021296642ull,7021296646ull,7021296650ull,7021296654ull,7021296658ull,7021296662ull,7021296666ull,7021296670ull,7021296674ull,7021296678ull,7021296682ull,7021296686ull,7021296690ull,7021296694ull,7021296698ull,7021296702ull, + 7021296770ull,7021296774ull,7021296778ull,7021296782ull,7021296786ull,7021296790ull,7021296794ull,7021296798ull,7021296802ull,7021296806ull,7021296810ull,7021296814ull,7021296818ull,7021296822ull,7021296826ull,7021296830ull,7021297026ull,7021297030ull,7021297034ull,7021297038ull,7021297042ull,7021297046ull,7021297050ull,7021297054ull,7021297058ull,7021297062ull,7021297066ull,7021297070ull,7021297074ull,7021297078ull,7021297082ull,7021297086ull, + 6492786374ull,6476009158ull,7021297090ull,7021268678ull,7021268658ull,0ull,6996102854ull,7021297370ull,6492786246ull,6476009030ull,6442454598ull,3609ull,7021268550ull,0ull,3813ull,0ull,0ull,6996099746ull,7021297106ull,7021268702ull,7021268666ull,0ull,6996102878ull,7021297434ull,6442454614ull,3617ull,6442454622ull,3621ull,7021268574ull,6442483454ull,6450872062ull,6996131582ull, + 6492786406ull,6476009190ull,6442454826ull,3649ull,0ull,0ull,6996102886ull,6996102954ull,6492786278ull,6476009062ull,6442454630ull,3625ull,0ull,6442483706ull,6450872314ull,6996131834ull,6492786454ull,6476009238ull,6442454830ull,3777ull,6601838342ull,6610226950ull,6996102934ull,6996102958ull,6492786326ull,6476009110ull,6442454678ull,3641ull,6610226822ull,6442451618ull,3605ull,385ull, + 0ull,0ull,7021297138ull,7021268774ull,7021268794ull,0ull,6996102950ull,7021297626ull,6442454654ull,3633ull,6442454694ull,3645ull,7021268646ull,721ull,0ull,0ull,32777ull,32781ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull, + 0ull,32833ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,3749ull,0ull,0ull,0ull,301ull,789ull,0ull,0ull,0ull,0ull, + 0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,6912247362ull,6912247370ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,6912247378ull,0ull, + 0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,6912247618ull,6912247634ull,6912247626ull,0ull,0ull,0ull,0ull,6912247822ull,0ull,0ull,0ull,0ull,6912247842ull,0ull,0ull,6912247854ull,0ull,0ull,0ull, + 0ull,0ull,0ull,0ull,6912247950ull,0ull,6912247958ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,6912248050ull,0ull,0ull,6912248078ull,0ull,0ull,6912248086ull,0ull,6912248098ull,0ull,0ull,0ull,0ull,0ull,0ull, 6912213238ull,0ull,6912248198ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,6912248118ull,6912213234ull,6912213242ull,6912248210ull,6912248214ull,0ull,0ull,6912248266ull,6912248270ull,0ull,0ull,6912248282ull,6912248286ull,0ull,0ull,0ull,0ull,0ull,0ull, - 6912248298ull,6912248302ull,0ull,0ull,6912248330ull,6912248334ull,0ull,0ull,6912248346ull,6912248350ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull, - 0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,6912248458ull,6912248482ull,6912248486ull,6912248494ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull, - 0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull, - 6912248306ull,6912248310ull,6912248390ull,6912248394ull,0ull,0ull,0ull,0ull,0ull,0ull,6912248522ull,6912248526ull,6912248530ull,6912248534ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull, - 0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull, - 0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,49185ull,49189ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull, - 0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,6912256886ull,0ull,0ull,0ull, - 0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull, - 0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,104362721582ull,0ull,104362721590ull,0ull,104362721598ull,0ull,104362721606ull,0ull,104362721614ull,0ull,104362721622ull,0ull,104362721630ull,0ull,104362721638ull,0ull,104362721646ull,0ull,104362721654ull,0ull, - 104362721662ull,0ull,104362721670ull,0ull,0ull,104362721682ull,0ull,104362721690ull,0ull,104362721698ull,0ull,0ull,0ull,0ull,0ull,0ull,104362721726ull,104371110334ull,0ull,104362721738ull,104371110346ull,0ull,104362721750ull,104371110358ull,0ull,104362721762ull,104371110370ull,0ull,104362721774ull,104371110382ull,0ull,0ull, - 0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,104362721562ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,104362721910ull,0ull, + 6912248298ull,6912248302ull,0ull,0ull,6912248330ull,6912248334ull,0ull,0ull,6912248346ull,6912248350ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,6912248458ull,6912248482ull,6912248486ull,6912248494ull, + 6912248306ull,6912248310ull,6912248390ull,6912248394ull,0ull,0ull,0ull,0ull,0ull,0ull,6912248522ull,6912248526ull,6912248530ull,6912248534ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,49185ull,49189ull,0ull,0ull,0ull,0ull,0ull, + 0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,6912256886ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,104362721582ull,0ull,104362721590ull,0ull, + 104362721598ull,0ull,104362721606ull,0ull,104362721614ull,0ull,104362721622ull,0ull,104362721630ull,0ull,104362721638ull,0ull,104362721646ull,0ull,104362721654ull,0ull,104362721662ull,0ull,104362721670ull,0ull,0ull,104362721682ull,0ull,104362721690ull,0ull,104362721698ull,0ull,0ull,0ull,0ull,0ull,0ull, + 104362721726ull,104371110334ull,0ull,104362721738ull,104371110346ull,0ull,104362721750ull,104371110358ull,0ull,104362721762ull,104371110370ull,0ull,104362721774ull,104371110382ull,0ull,0ull,0ull,0ull,0ull,0ull,104362721562ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,104362721910ull,0ull, 0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,104362721966ull,0ull,104362721974ull,0ull,104362721982ull,0ull,104362721990ull,0ull,104362721998ull,0ull,104362722006ull,0ull,104362722014ull,0ull,104362722022ull,0ull,104362722030ull,0ull,104362722038ull,0ull, 104362722046ull,0ull,104362722054ull,0ull,0ull,104362722066ull,0ull,104362722074ull,0ull,104362722082ull,0ull,0ull,0ull,0ull,0ull,0ull,104362722110ull,104371110718ull,0ull,104362722122ull,104371110730ull,0ull,104362722134ull,104371110742ull,0ull,104362722146ull,104371110754ull,0ull,104362722158ull,104371110766ull,0ull,0ull, - 0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,104362721946ull,0ull,0ull,104362722238ull,104362722242ull,104362722246ull,104362722250ull,0ull,0ull,0ull,104362722294ull,0ull, - 143649ull,105425ull,146217ull,144161ull,113477ull,80073ull,85909ull,163441ull,163441ull,91461ull,149317ull,87581ull,91425ull,100313ull,121253ull,130581ull,137469ull,138985ull,140257ull,148029ull,108553ull,111725ull,115557ull,118649ull,135413ull,148905ull,157637ull,80393ull,85461ull,109585ull,116845ull,137397ull, - 161913ull,95553ull,114605ull,137013ull,140689ull,101157ull,132961ull,139389ull,97065ull,105565ull,112041ull,117745ull,148281ull,81433ull,83677ull,84857ull,103185ull,109389ull,116801ull,121757ull,131077ull,137241ull,137585ull,145341ull,154825ull,159165ull,161769ull,123441ull,124413ull,128641ull,134949ull,150545ull, - 162301ull,142169ull,91005ull,97297ull,127361ull,131577ull,117129ull,123689ull,144137ull,154589ull,90977ull,94601ull,108621ull,112489ull,113725ull,128189ull,129245ull,153901ull,84809ull,131629ull,83825ull,83761ull,125041ull,128761ull,135109ull,154069ull,142849ull,101181ull,108553ull,142329ull,80101ull,94109ull, - 98377ull,118301ull,120257ull,85085ull,123885ull,81661ull,97957ull,79925ull,111409ull,103905ull,128137ull,85773ull,90489ull,121861ull,135461ull,141993ull,110313ull,147137ull,111137ull,101369ull,134037ull,102017ull,120213ull,80569ull,83365ull,83749ull,107013ull,127901ull,133565ull,142153ull,149309ull,84949ull, - 86281ull,91597ull,97201ull,104213ull,114681ull,124073ull,153269ull,158121ull,162397ull,162617ull,84589ull,105241ull,110045ull,146825ull,96721ull,99905ull,100353ull,103017ull,113805ull,116005ull,119333ull,124713ull,128977ull,131517ull,146585ull,136121ull,147597ull,150825ull,84061ull,84621ull,86773ull,115489ull, - 140041ull,141993ull,97061ull,98261ull,101869ull,110265ull,127225ull,118229ull,80785ull,89061ull,94109ull,95977ull,98417ull,118473ull,119205ull,130665ull,131353ull,149713ull,154585ull,154913ull,155745ull,81453ull,124601ull,149201ull,154337ull,99205ull,80409ull,82793ull,94137ull,94461ull,104037ull,108553ull, - 116537ull,121097ull,136177ull,147953ull,163381ull,104993ull,153785ull,84517ull,105965ull,106445ull,111877ull,113265ull,118821ull,120165ull,123309ull,128065ull,156025ull,83381ull,100537ull,154081ull,82093ull,95333ull,112553ull,146601ull,97837ull,99601ull,106589ull,118301ull,154137ull,84133ull,86077ull,94613ull, - 104525ull,105785ull,107169ull,111509ull,118809ull,120713ull,130533ull,140093ull,140165ull,149297ull,154505ull,85245ull,113385ull,86133ull,116545ull,119393ull,137193ull,154253ull,160093ull,162429ull,106077ull,112429ull,133025ull,125741ull,126081ull,127561ull,117505ull,115301ull,142689ull,80641ull,134361ull,84201ull, - 83997ull,96921ull,101197ull,127833ull,93717ull,111737ull,105169ull,146669ull,139569ull,153909ull,140845ull,97101ull,83201ull,87809ull,0ull,0ull,90473ull,0ull,104913ull,0ull,0ull,83833ull,117929ull,121641ull,124145ull,124281ull,124309ull,124477ull,154969ull,127737ull,130805ull,0ull, - 137289ull,0ull,142305ull,0ull,0ull,147681ull,148469ull,0ull,0ull,0ull,156605ull,156657ull,156833ull,161489ull,148345ull,154333ull,81593ull,82845ull,83253ull,84773ull,84881ull,85317ull,87669ull,88089ull,88481ull,90369ull,90785ull,94609ull,94649ull,98897ull,99745ull,99897ull, - 100297ull,103741ull,104329ull,105029ull,107029ull,112093ull,112745ull,113801ull,116153ull,116909ull,118921ull,123461ull,124153ull,124197ull,124193ull,124225ull,124249ull,124277ull,124469ull,124473ull,125185ull,125445ull,126721ull,128977ull,129061ull,129285ull,130505ull,131093ull,133045ull,133605ull,133605ull,135517ull, - 140353ull,140889ull,142341ull,142565ull,144205ull,144417ull,147161ull,147681ull,154509ull,155645ull,155885ull,98773ull,592825ull,133217ull,0ull,0ull,80025ull,83669ull,83361ull,81409ull,83221ull,83457ull,84765ull,84969ull,87669ull,87381ull,87653ull,87945ull,90473ull,90829ull,91409ull,91473ull, - 92553ull,93345ull,97097ull,97125ull,97701ull,97973ull,99169ull,99641ull,99361ull,99897ull,99713ull,100297ull,100561ull,102161ull,102513ull,102729ull,103769ull,104913ull,105565ull,105581ull,105817ull,110053ull,110313ull,111877ull,113517ull,113453ull,113801ull,114809ull,116153ull,122525ull,116949ull,117437ull, - 117929ull,119237ull,119833ull,120045ull,120949ull,120957ull,121641ull,121709ull,121809ull,122153ull,122113ull,123697ull,125637ull,126721ull,127469ull,128365ull,128977ull,130297ull,131093ull,134473ull,135101ull,138725ull,140549ull,140825ull,140889ull,142077ull,142305ull,142125ull,142341ull,142329ull,142261ull,142565ull, - 142889ull,144417ull,146657ull,147913ull,149093ull,149977ull,154097ull,154509ull,154969ull,155501ull,155645ull,155693ull,155885ull,158793ull,163441ull,565545ull,565521ull,577365ull,61045ull,65633ull,65765ull,608549ull,619329ull,654157ull,163085ull,163385ull,0ull,0ull,0ull,0ull,0ull,0ull, - 0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull, - 0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,12247373670ull,0ull,12272539594ull, + 0ull,0ull,0ull,0ull,104362721946ull,0ull,0ull,104362722238ull,104362722242ull,104362722246ull,104362722250ull,0ull,0ull,0ull,104362722294ull,0ull,143649ull,105425ull,146217ull,144161ull,113477ull,80073ull,85909ull,163441ull,163441ull,91461ull,149317ull,87581ull,91425ull,100313ull,121253ull,130581ull, + 137469ull,138985ull,140257ull,148029ull,108553ull,111725ull,115557ull,118649ull,135413ull,148905ull,157637ull,80393ull,85461ull,109585ull,116845ull,137397ull,161913ull,95553ull,114605ull,137013ull,140689ull,101157ull,132961ull,139389ull,97065ull,105565ull,112041ull,117745ull,148281ull,81433ull,83677ull,84857ull, + 103185ull,109389ull,116801ull,121757ull,131077ull,137241ull,137585ull,145341ull,154825ull,159165ull,161769ull,123441ull,124413ull,128641ull,134949ull,150545ull,162301ull,142169ull,91005ull,97297ull,127361ull,131577ull,117129ull,123689ull,144137ull,154589ull,90977ull,94601ull,108621ull,112489ull,113725ull,128189ull, + 129245ull,153901ull,84809ull,131629ull,83825ull,83761ull,125041ull,128761ull,135109ull,154069ull,142849ull,101181ull,108553ull,142329ull,80101ull,94109ull,98377ull,118301ull,120257ull,85085ull,123885ull,81661ull,97957ull,79925ull,111409ull,103905ull,128137ull,85773ull,90489ull,121861ull,135461ull,141993ull, + 110313ull,147137ull,111137ull,101369ull,134037ull,102017ull,120213ull,80569ull,83365ull,83749ull,107013ull,127901ull,133565ull,142153ull,149309ull,84949ull,86281ull,91597ull,97201ull,104213ull,114681ull,124073ull,153269ull,158121ull,162397ull,162617ull,84589ull,105241ull,110045ull,146825ull,96721ull,99905ull, + 100353ull,103017ull,113805ull,116005ull,119333ull,124713ull,128977ull,131517ull,146585ull,136121ull,147597ull,150825ull,84061ull,84621ull,86773ull,115489ull,140041ull,141993ull,97061ull,98261ull,101869ull,110265ull,127225ull,118229ull,80785ull,89061ull,94109ull,95977ull,98417ull,118473ull,119205ull,130665ull, + 131353ull,149713ull,154585ull,154913ull,155745ull,81453ull,124601ull,149201ull,154337ull,99205ull,80409ull,82793ull,94137ull,94461ull,104037ull,108553ull,116537ull,121097ull,136177ull,147953ull,163381ull,104993ull,153785ull,84517ull,105965ull,106445ull,111877ull,113265ull,118821ull,120165ull,123309ull,128065ull, + 156025ull,83381ull,100537ull,154081ull,82093ull,95333ull,112553ull,146601ull,97837ull,99601ull,106589ull,118301ull,154137ull,84133ull,86077ull,94613ull,104525ull,105785ull,107169ull,111509ull,118809ull,120713ull,130533ull,140093ull,140165ull,149297ull,154505ull,85245ull,113385ull,86133ull,116545ull,119393ull, + 137193ull,154253ull,160093ull,162429ull,106077ull,112429ull,133025ull,125741ull,126081ull,127561ull,117505ull,115301ull,142689ull,80641ull,134361ull,84201ull,83997ull,96921ull,101197ull,127833ull,93717ull,111737ull,105169ull,146669ull,139569ull,153909ull,140845ull,97101ull,83201ull,87809ull,0ull,0ull, + 90473ull,0ull,104913ull,0ull,0ull,83833ull,117929ull,121641ull,124145ull,124281ull,124309ull,124477ull,154969ull,127737ull,130805ull,0ull,137289ull,0ull,142305ull,0ull,0ull,147681ull,148469ull,0ull,0ull,0ull,156605ull,156657ull,156833ull,161489ull,148345ull,154333ull, + 81593ull,82845ull,83253ull,84773ull,84881ull,85317ull,87669ull,88089ull,88481ull,90369ull,90785ull,94609ull,94649ull,98897ull,99745ull,99897ull,100297ull,103741ull,104329ull,105029ull,107029ull,112093ull,112745ull,113801ull,116153ull,116909ull,118921ull,123461ull,124153ull,124197ull,124193ull,124225ull, + 124249ull,124277ull,124469ull,124473ull,125185ull,125445ull,126721ull,128977ull,129061ull,129285ull,130505ull,131093ull,133045ull,133605ull,133605ull,135517ull,140353ull,140889ull,142341ull,142565ull,144205ull,144417ull,147161ull,147681ull,154509ull,155645ull,155885ull,98773ull,592825ull,133217ull,0ull,0ull, + 80025ull,83669ull,83361ull,81409ull,83221ull,83457ull,84765ull,84969ull,87669ull,87381ull,87653ull,87945ull,90473ull,90829ull,91409ull,91473ull,92553ull,93345ull,97097ull,97125ull,97701ull,97973ull,99169ull,99641ull,99361ull,99897ull,99713ull,100297ull,100561ull,102161ull,102513ull,102729ull, + 103769ull,104913ull,105565ull,105581ull,105817ull,110053ull,110313ull,111877ull,113517ull,113453ull,113801ull,114809ull,116153ull,122525ull,116949ull,117437ull,117929ull,119237ull,119833ull,120045ull,120949ull,120957ull,121641ull,121709ull,121809ull,122153ull,122113ull,123697ull,125637ull,126721ull,127469ull,128365ull, + 128977ull,130297ull,131093ull,134473ull,135101ull,138725ull,140549ull,140825ull,140889ull,142077ull,142305ull,142125ull,142341ull,142329ull,142261ull,142565ull,142889ull,144417ull,146657ull,147913ull,149093ull,149977ull,154097ull,154509ull,154969ull,155501ull,155645ull,155693ull,155885ull,158793ull,163441ull,565545ull, + 565521ull,577365ull,61045ull,65633ull,65765ull,608549ull,619329ull,654157ull,163085ull,163385ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,12247373670ull,0ull,12272539594ull, 0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,12356425638ull,12364814246ull,12356676902ull,12365065510ull,12272539458ull,12280928066ull,12314482498ull,12314482502ull,12314482506ull,12314482510ull,12314482514ull,12314482518ull,12314482522ull,0ull,12314482530ull,12314482534ull,12314482538ull,12314482542ull,12314482546ull,0ull,12314482554ull,0ull, - 12314482562ull,12314482566ull,0ull,12314482574ull,12314482578ull,0ull,12314482586ull,12314482590ull,12314482594ull,12314482598ull,12314482602ull,12289316694ull,12339648326ull,12339648366ull,12339648402ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull, - 0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull, - 0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,6501439306ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull, - 0ull,0ull,0ull,0ull,6501439338ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull, - 0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,585676112486ull,0ull,585676112494ull,0ull,0ull,0ull, - 0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,585676112534ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull, - 0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull, - 0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,586590471366ull,586590471370ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull, - 0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,591078378782ull,591288093982ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull, - 0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull, - 0ull,0ull,0ull,592244395530ull,0ull,592126955026ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,592185675310ull,0ull,0ull,592244395586ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull, - 0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull, - 0ull,0ull,0ull,0ull,0ull,592185675530ull,0ull,592101789450ull,592244395786ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull, - 0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull, - 0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull, - 0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,594266051302ull,594182165222ull,0ull,594291217126ull,0ull, - 0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull, - 0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,596321261282ull,596321261286ull,0ull,0ull,0ull,0ull, - 0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull, - 0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,603845846230ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull, - 0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull, - 0ull,758313747578ull,758406022266ull,758322136186ull,758322136230ull,758330524794ull,758322136198ull,758322136202ull,758330524806ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull, - 0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull, - 0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,784695932318ull,784695932302ull,784695932326ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull, - 0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,999427622238ull,999427622242ull, - 999503119742ull,999511508350ull,999519896958ull,999528285566ull,999536674174ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull, - 0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull, - 0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,999427622630ull,999427622634ull,999503120110ull,999503120114ull,999511508718ull, - 999511508722ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull, - 0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull, - 80117ull,80097ull,80133ull,525449ull,81281ull,81593ull,81645ull,81929ull,82409ull,82533ull,82845ull,82749ull,53881ull,530665ull,83253ull,83281ull,83345ull,83421ull,529521ull,53989ull,83357ull,83509ull,529709ull,83549ull,83601ull,80689ull,83633ull,83669ull,673661ull,83925ull,83981ull,54141ull, - 84205ull,84249ull,84425ull,84445ull,54357ull,84765ull,84773ull,84881ull,84969ull,85013ull,85017ull,85085ull,85285ull,85317ull,85353ull,85453ull,85493ull,85501ull,85501ull,85501ull,534705ull,115137ull,85801ull,85885ull,535949ull,85933ull,85957ull,86041ull,86649ull,86241ull,86305ull,86433ull, - 86665ull,87001ull,87105ull,87373ull,87437ull,87569ull,87569ull,87653ull,87725ull,87757ull,87817ull,89177ull,88089ull,89181ull,88389ull,88529ull,83997ull,91065ull,89913ull,90065ull,90165ull,89645ull,90313ull,90309ull,90801ull,545681ull,91081ull,91101ull,91161ull,91241ull,91273ull,91529ull, - 547489ull,547753ull,92081ull,92269ull,92317ull,92001ull,92569ull,56249ull,56305ull,93217ull,93433ull,93433ull,550689ull,93965ull,94049ull,94109ull,94157ull,552033ull,94205ull,94233ull,97613ull,94345ull,56837ull,94593ull,94649ull,94977ull,94773ull,554897ull,95501ull,554905ull,95673ull,95661ull, - 95729ull,96133ull,96137ull,57533ull,96245ull,96417ull,96501ull,96677ull,57737ull,558605ull,57841ull,96961ull,96973ull,96985ull,97065ull,691785ull,97273ull,560325ull,560325ull,133125ull,97417ull,97417ull,58141ull,576225ull,624489ull,97673ull,97709ull,58253ull,97897ull,98101ull,98141ull,98277ull, - 98821ull,58601ull,58481ull,98897ull,564049ull,99101ull,99617ull,99633ull,99641ull,99633ull,99817ull,99897ull,100041ull,99985ull,100029ull,100217ull,100297ull,100313ull,100417ull,100461ull,100725ull,101061ull,101201ull,101697ull,568369ull,101621ull,101361ull,101793ull,101901ull,102289ull,569285ull,102537ull, - 102165ull,102053ull,59577ull,102821ull,102905ull,103029ull,102877ull,59825ull,103741ull,103857ull,573481ull,104333ull,105441ull,104741ull,60517ull,105029ull,60449ull,60305ull,83529ull,83541ull,105473ull,105073ull,131765ull,69477ull,105565ull,105581ull,105605ull,105849ull,105805ull,577293ull,60709ull,106473ull, - 106005ull,106825ull,107029ull,577973ull,107065ull,106621ull,107601ull,61045ull,107785ull,108173ull,108457ull,109217ull,580237ull,109421ull,61537ull,109701ull,582301ull,109905ull,61753ull,110025ull,110205ull,110313ull,110317ull,584245ull,554029ull,584681ull,110905ull,586481ull,111357ull,111413ull,111005ull,111705ull, - 111865ull,112093ull,111877ull,112037ull,112097ull,112149ull,586873ull,111825ull,112829ull,113081ull,62669ull,113453ull,113437ull,588613ull,112613ull,114105ull,589177ull,589369ull,114457ull,114917ull,114809ull,114797ull,63065ull,114985ull,115189ull,115165ull,115381ull,529557ull,115989ull,592269ull,116337ull,593581ull, - 116897ull,116949ull,117057ull,596001ull,117249ull,117333ull,597205ull,598097ull,118249ull,118317ull,64177ull,118421ull,64225ull,64225ull,119069ull,119153ull,119237ull,119317ull,119593ull,64621ull,119953ull,602329ull,120057ull,602697ull,120257ull,558717ull,120897ull,605829ull,605921ull,606481ull,65521ull,65569ull, - 121809ull,607181ull,607177ull,607333ull,607437ull,121977ull,121981ull,121981ull,122153ull,65765ull,122413ull,65817ull,66137ull,610421ull,123193ull,123441ull,123697ull,66445ull,612505ull,124249ull,612969ull,613141ull,124477ull,124845ull,66749ull,125185ull,125225ull,125245ull,615921ull,617117ull,617117ull,125881ull, - 67593ull,618157ull,126745ull,126757ull,67741ull,619009ull,127817ull,68225ull,127905ull,127885ull,128001ull,622105ull,128397ull,68613ull,128797ull,129033ull,129301ull,68817ull,624801ull,624925ull,68965ull,625509ull,130537ull,625913ull,130645ull,131049ull,131093ull,627561ull,627853ull,131457ull,628385ull,131521ull, - 576893ull,69461ull,131785ull,132109ull,69677ull,132345ull,92885ull,630429ull,630485ull,577101ull,577137ull,133125ull,133137ull,147065ull,70061ull,133701ull,133677ull,133749ull,84685ull,133829ull,133837ull,133877ull,134041ull,634097ull,134037ull,134261ull,134541ull,134837ull,134285ull,134901ull,135069ull,135517ull, - 134477ull,134953ull,134961ull,135025ull,635097ull,636333ull,635733ull,70829ull,136133ull,136141ull,136281ull,642857ull,136593ull,638129ull,71029ull,71045ull,638661ull,639817ull,71085ull,137537ull,137585ull,137629ull,137637ull,137893ull,137761ull,138297ull,138121ull,138725ull,138401ull,138669ull,138777ull,71517ull, - 139141ull,139269ull,71653ull,139649ull,139661ull,645533ull,140125ull,140153ull,71893ull,140265ull,53997ull,647865ull,648601ull,72441ull,72477ull,141953ull,142261ull,142889ull,143701ull,651937ull,144045ull,144133ull,144493ull,144861ull,654525ull,532497ull,145197ull,145137ull,145345ull,533369ull,146257ull,146657ull, - 661321ull,661429ull,148049ull,148421ull,148549ull,662713ull,148589ull,149729ull,150365ull,150369ull,150001ull,151525ull,151637ull,667625ull,153133ull,75349ull,153309ull,669149ull,75673ull,154381ull,95945ull,154765ull,673045ull,673897ull,76217ull,76249ull,155521ull,675881ull,76489ull,676441ull,155693ull,155693ull, - 155813ull,677593ull,156553ull,77005ull,156837ull,157341ull,157449ull,157689ull,77625ull,683201ull,158793ull,160001ull,160757ull,78649ull,78773ull,161181ull,688953ull,78817ull,689173ull,690233ull,690757ull,162541ull,79193ull,162789ull,162809ull,162837ull,162877ull,162905ull,163053ull,694273ull,0ull,0ull, - 0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull, + 12314482562ull,12314482566ull,0ull,12314482574ull,12314482578ull,0ull,12314482586ull,12314482590ull,12314482594ull,12314482598ull,12314482602ull,12289316694ull,12339648326ull,12339648366ull,12339648402ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,6501439306ull,0ull,0ull,0ull,0ull,0ull,0ull, + 0ull,0ull,0ull,0ull,6501439338ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,585676112486ull,0ull,585676112494ull,0ull,0ull,0ull, + 0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,585676112534ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,586590471366ull,586590471370ull, + 0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,591078378782ull,591288093982ull,0ull,0ull,0ull,0ull,0ull,0ull,592244395530ull,0ull,592126955026ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,592185675310ull,0ull, + 0ull,592244395586ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,592185675530ull,0ull,592101789450ull,592244395786ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull, + 0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,594266051302ull,594182165222ull,0ull,594291217126ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,596321261282ull,596321261286ull,0ull,0ull,0ull,0ull, + 0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,603845846230ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,758313747578ull,758406022266ull,758322136186ull,758322136230ull,758330524794ull,758322136198ull,758322136202ull,758330524806ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull, + 0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,784695932318ull,784695932302ull,784695932326ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,999427622238ull,999427622242ull, + 999503119742ull,999511508350ull,999519896958ull,999528285566ull,999536674174ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,999427622630ull,999427622634ull,999503120110ull,999503120114ull,999511508718ull, + 999511508722ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,0ull,80117ull,80097ull,80133ull,525449ull,81281ull,81593ull,81645ull,81929ull,82409ull,82533ull,82845ull,82749ull,53881ull,530665ull,83253ull,83281ull, + 83345ull,83421ull,529521ull,53989ull,83357ull,83509ull,529709ull,83549ull,83601ull,80689ull,83633ull,83669ull,673661ull,83925ull,83981ull,54141ull,84205ull,84249ull,84425ull,84445ull,54357ull,84765ull,84773ull,84881ull,84969ull,85013ull,85017ull,85085ull,85285ull,85317ull,85353ull,85453ull, + 85493ull,85501ull,85501ull,85501ull,534705ull,115137ull,85801ull,85885ull,535949ull,85933ull,85957ull,86041ull,86649ull,86241ull,86305ull,86433ull,86665ull,87001ull,87105ull,87373ull,87437ull,87569ull,87569ull,87653ull,87725ull,87757ull,87817ull,89177ull,88089ull,89181ull,88389ull,88529ull, + 83997ull,91065ull,89913ull,90065ull,90165ull,89645ull,90313ull,90309ull,90801ull,545681ull,91081ull,91101ull,91161ull,91241ull,91273ull,91529ull,547489ull,547753ull,92081ull,92269ull,92317ull,92001ull,92569ull,56249ull,56305ull,93217ull,93433ull,93433ull,550689ull,93965ull,94049ull,94109ull, + 94157ull,552033ull,94205ull,94233ull,97613ull,94345ull,56837ull,94593ull,94649ull,94977ull,94773ull,554897ull,95501ull,554905ull,95673ull,95661ull,95729ull,96133ull,96137ull,57533ull,96245ull,96417ull,96501ull,96677ull,57737ull,558605ull,57841ull,96961ull,96973ull,96985ull,97065ull,691785ull, + 97273ull,560325ull,560325ull,133125ull,97417ull,97417ull,58141ull,576225ull,624489ull,97673ull,97709ull,58253ull,97897ull,98101ull,98141ull,98277ull,98821ull,58601ull,58481ull,98897ull,564049ull,99101ull,99617ull,99633ull,99641ull,99633ull,99817ull,99897ull,100041ull,99985ull,100029ull,100217ull, + 100297ull,100313ull,100417ull,100461ull,100725ull,101061ull,101201ull,101697ull,568369ull,101621ull,101361ull,101793ull,101901ull,102289ull,569285ull,102537ull,102165ull,102053ull,59577ull,102821ull,102905ull,103029ull,102877ull,59825ull,103741ull,103857ull,573481ull,104333ull,105441ull,104741ull,60517ull,105029ull, + 60449ull,60305ull,83529ull,83541ull,105473ull,105073ull,131765ull,69477ull,105565ull,105581ull,105605ull,105849ull,105805ull,577293ull,60709ull,106473ull,106005ull,106825ull,107029ull,577973ull,107065ull,106621ull,107601ull,61045ull,107785ull,108173ull,108457ull,109217ull,580237ull,109421ull,61537ull,109701ull, + 582301ull,109905ull,61753ull,110025ull,110205ull,110313ull,110317ull,584245ull,554029ull,584681ull,110905ull,586481ull,111357ull,111413ull,111005ull,111705ull,111865ull,112093ull,111877ull,112037ull,112097ull,112149ull,586873ull,111825ull,112829ull,113081ull,62669ull,113453ull,113437ull,588613ull,112613ull,114105ull, + 589177ull,589369ull,114457ull,114917ull,114809ull,114797ull,63065ull,114985ull,115189ull,115165ull,115381ull,529557ull,115989ull,592269ull,116337ull,593581ull,116897ull,116949ull,117057ull,596001ull,117249ull,117333ull,597205ull,598097ull,118249ull,118317ull,64177ull,118421ull,64225ull,64225ull,119069ull,119153ull, + 119237ull,119317ull,119593ull,64621ull,119953ull,602329ull,120057ull,602697ull,120257ull,558717ull,120897ull,605829ull,605921ull,606481ull,65521ull,65569ull,121809ull,607181ull,607177ull,607333ull,607437ull,121977ull,121981ull,121981ull,122153ull,65765ull,122413ull,65817ull,66137ull,610421ull,123193ull,123441ull, + 123697ull,66445ull,612505ull,124249ull,612969ull,613141ull,124477ull,124845ull,66749ull,125185ull,125225ull,125245ull,615921ull,617117ull,617117ull,125881ull,67593ull,618157ull,126745ull,126757ull,67741ull,619009ull,127817ull,68225ull,127905ull,127885ull,128001ull,622105ull,128397ull,68613ull,128797ull,129033ull, + 129301ull,68817ull,624801ull,624925ull,68965ull,625509ull,130537ull,625913ull,130645ull,131049ull,131093ull,627561ull,627853ull,131457ull,628385ull,131521ull,576893ull,69461ull,131785ull,132109ull,69677ull,132345ull,92885ull,630429ull,630485ull,577101ull,577137ull,133125ull,133137ull,147065ull,70061ull,133701ull, + 133677ull,133749ull,84685ull,133829ull,133837ull,133877ull,134041ull,634097ull,134037ull,134261ull,134541ull,134837ull,134285ull,134901ull,135069ull,135517ull,134477ull,134953ull,134961ull,135025ull,635097ull,636333ull,635733ull,70829ull,136133ull,136141ull,136281ull,642857ull,136593ull,638129ull,71029ull,71045ull, + 638661ull,639817ull,71085ull,137537ull,137585ull,137629ull,137637ull,137893ull,137761ull,138297ull,138121ull,138725ull,138401ull,138669ull,138777ull,71517ull,139141ull,139269ull,71653ull,139649ull,139661ull,645533ull,140125ull,140153ull,71893ull,140265ull,53997ull,647865ull,648601ull,72441ull,72477ull,141953ull, + 142261ull,142889ull,143701ull,651937ull,144045ull,144133ull,144493ull,144861ull,654525ull,532497ull,145197ull,145137ull,145345ull,533369ull,146257ull,146657ull,661321ull,661429ull,148049ull,148421ull,148549ull,662713ull,148589ull,149729ull,150365ull,150369ull,150001ull,151525ull,151637ull,667625ull,153133ull,75349ull, + 153309ull,669149ull,75673ull,154381ull,95945ull,154765ull,673045ull,673897ull,76217ull,76249ull,155521ull,675881ull,76489ull,676441ull,155693ull,155693ull,155813ull,677593ull,156553ull,77005ull,156837ull,157341ull,157449ull,157689ull,77625ull,683201ull,158793ull,160001ull,160757ull,78649ull,78773ull,161181ull, + 688953ull,78817ull,689173ull,690233ull,690757ull,162541ull,79193ull,162789ull,162809ull,162837ull,162877ull,162905ull,163053ull,694273ull,0ull,0ull, }; KBTS_INLINE kbts_u64 kbts__GetUnicodeDecomposition(kbts_u32 Codepoint) { - return (Codepoint < 1114110) ? kbts__UnicodeDecomposition_Data[((kbts_un)kbts__UnicodeDecomposition_PageIndices[Codepoint/64] * 64) | (Codepoint & 63)] : 0; + return (Codepoint < 1114110) ? kbts__UnicodeDecomposition_Data[((Codepoint < 195104) ? ((kbts_un)kbts__UnicodeDecomposition_PageIndices[Codepoint / 16] * 16) : 16) | (Codepoint & 15)] : 0; } -static kbts_u8 kbts__UnicodeWordBreakClass_PageIndices[8703] = { +static kbts_u8 kbts__UnicodeWordBreakClass_PageIndices[7172] = { 0,1,2,2,2,3,4,5,2,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,2,2,31,32,33,34,35,2,2,2,36,37,38,39,40,41,42,43,44,45,46,47,48,49,2,50,2,2,51,52, 53,54,55,56,57,57,58,59,57,60,57,61,62,63,64,65,57,57,66,57,57,57,67,57,2,68,69,70,71,57,57,57, @@ -6090,54 +6149,7 @@ static kbts_u8 kbts__UnicodeWordBreakClass_PageIndices[8703] = { 57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57, 57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57, 57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57, - 213,57,214,215,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57, - 57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57, - 57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57, - 57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57, - 57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57, - 57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57, - 57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57, - 57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57, - 57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57, - 57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57, - 57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57, - 57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57, - 57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57, - 57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57, - 57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57, - 57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57, - 57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57, - 57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57, - 57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57, - 57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57, - 57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57, - 57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57, - 57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57, - 57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57, - 57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57, - 57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57, - 57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57, - 57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57, - 57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57, - 57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57, - 57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57, - 57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57, - 57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57, - 57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57, - 57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57, - 57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57, - 57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57, - 57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57, - 57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57, - 57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57, - 57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57, - 57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57, - 57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57, - 57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57, - 57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57, - 57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57, - 57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57, - 57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57, + 213,57,214,215, }; static kbts_u8 kbts__UnicodeWordBreakClass_Data[27648] = { @@ -6577,10 +6589,10 @@ static kbts_u8 kbts__UnicodeWordBreakClass_Data[27648] = { KBTS_INLINE kbts_u8 kbts__GetUnicodeWordBreakClass(kbts_u32 Codepoint) { - return (Codepoint < 1114110) ? kbts__UnicodeWordBreakClass_Data[((kbts_un)kbts__UnicodeWordBreakClass_PageIndices[Codepoint/128] * 128) | (Codepoint & 127)] : 0; + return (Codepoint < 1114110) ? kbts__UnicodeWordBreakClass_Data[((Codepoint < 918016) ? ((kbts_un)kbts__UnicodeWordBreakClass_PageIndices[Codepoint / 128] * 128) : 7296) | (Codepoint & 127)] : 0; } -static kbts_u8 kbts__UnicodeLineBreakClass_PageIndices[8703] = { +static kbts_u8 kbts__UnicodeLineBreakClass_PageIndices[7172] = { 0,1,2,2,2,3,4,2,2,5,2,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,2,2,31,2,32,2,2,2,2,33,34,35,36,37,38,39,40,41,42,43,44,45,2,46,2,2,2,47, 48,49,50,2,51,52,53,54,2,2,2,55,56,57,58,59,2,2,2,60,2,2,61,2,2,62,63,64,65,66,67,68, @@ -6805,54 +6817,7 @@ static kbts_u8 kbts__UnicodeLineBreakClass_PageIndices[8703] = { 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 206,2,207,208,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, + 206,2,207,208, }; static kbts_u8 kbts__UnicodeLineBreakClass_Data[26752] = { @@ -7278,10 +7243,10 @@ static kbts_u8 kbts__UnicodeLineBreakClass_Data[26752] = { KBTS_INLINE kbts_u8 kbts__GetUnicodeLineBreakClass(kbts_u32 Codepoint) { - return (Codepoint < 1114110) ? kbts__UnicodeLineBreakClass_Data[((kbts_un)kbts__UnicodeLineBreakClass_PageIndices[Codepoint/128] * 128) | (Codepoint & 127)] : 0; + return (Codepoint < 1114110) ? kbts__UnicodeLineBreakClass_Data[((Codepoint < 918016) ? ((kbts_un)kbts__UnicodeLineBreakClass_PageIndices[Codepoint / 128] * 128) : 256) | (Codepoint & 127)] : 0; } -static kbts_u8 kbts__UnicodeGraphemeBreakClass_PageIndices[8703] = { +static kbts_u8 kbts__UnicodeGraphemeBreakClass_PageIndices[7200] = { 0,1,2,2,2,2,3,2,2,4,2,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,2,2,30,2,2,2,2,2,2,2,31,32,33,34,35,2,36,37,38,39,40,41,2,42,2,2,2,2, 43,44,45,46,2,2,47,48,2,49,2,50,51,52,53,54,2,2,55,2,2,2,56,2,2,57,58,59,2,2,2,2, @@ -7507,53 +7472,6 @@ static kbts_u8 kbts__UnicodeGraphemeBreakClass_PageIndices[8703] = { 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, 153,154,155,156,154,154,154,154,154,154,154,154,154,154,154,154,154,154,154,154,154,154,154,154,154,154,154,154,154,154,154,154, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, }; static kbts_u8 kbts__UnicodeGraphemeBreakClass_Data[20096] = { @@ -7875,305 +7793,143 @@ static kbts_u8 kbts__UnicodeGraphemeBreakClass_Data[20096] = { KBTS_INLINE kbts_u8 kbts__GetUnicodeGraphemeBreakClass(kbts_u32 Codepoint) { - return (Codepoint < 1114110) ? kbts__UnicodeGraphemeBreakClass_Data[((kbts_un)kbts__UnicodeGraphemeBreakClass_PageIndices[Codepoint/128] * 128) | (Codepoint & 127)] : 0; + return (Codepoint < 1114110) ? kbts__UnicodeGraphemeBreakClass_Data[((Codepoint < 921600) ? ((kbts_un)kbts__UnicodeGraphemeBreakClass_PageIndices[Codepoint / 128] * 128) : 256) | (Codepoint & 127)] : 0; } -static kbts_u8 kbts__UnicodeSyllabicInfo_PageIndices[8703] = { - 0,1,2,2,2,2,2,2,2,2,2,2,2,2,2,3,4,2,5,6,7,8,9,10,11,12,13,14,15,16,17,18, - 19,20,2,2,2,2,2,2,2,2,2,2,2,2,21,22,23,24,25,26,27,28,29,30,31,32,2,33,2,2,2,2, - 34,35,2,2,2,2,2,2,2,2,2,36,2,2,2,2,2,2,2,2,2,2,2,2,2,2,37,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,38,39,40,41,42,43,2,44,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,45,2,2,2, - 2,2,2,2,2,2,2,2,2,2,46,47,2,2,2,2,2,2,2,2,48,49,2,2,2,2,50,51,2,52,53,54, - 55,56,57,58,59,60,61,62,63,64,2,65,66,67,68,2,69,2,70,71,72,73,2,2,74,75,76,77,2,78,79,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,80,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,81,82,2,2,2,83,2,2,2,84,85, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,86,86,86,87,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,88,89,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,90,2,2,91,2,2,2,92,2,93,2,2,2,2,2,2,94,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, +static kbts_u8 kbts__UnicodeSyllabicInfo_PageIndices[3915] = { + 0,1,0,0,0,2,3,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,4, + 5,0,6,0,0,0,0,0,7,8,9,10,11,12,13,14,15,16,17,18,19,20,21,22,23,24,25,26,27,28,29,30, + 31,32,33,34,35,36,37,38,39,40,41,42,43,44,45,46,47,48,49,0,50,51,52,0,53,54,55,56,57,58,59,0, + 60,61,62,63,64,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,65,66,67,68,69,70,71,72, + 73,74,74,75,76,77,0,0,78,79,80,81,74,82,83,0,84,74,85,86,87,0,0,0,88,89,90,0,91,92,74,93, + 74,94,95,0,0,0,96,97,0,0,0,0,0,0,0,98,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 99,100,0,101,102,0,0,103,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,104,105,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,106,74,107,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 108,109,110,111,112,113,114,115,116,117,118,0,119,120,121,122,123,124,125,126,74,127,128,129,0,0,0,0,0,0,130,131, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,132,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,133,134,135,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,136,137,138,0,0,0,55,139,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,74,140,141,142,143,0,0,0,0,0,0,0,74,144,0,0,0,106,145,106,146,0,0,0, + 147,148,149,150,151,152,153,0,154,155,156,157,158,159,160,161,162,163,164,0,165,166,167,168,169,170,171,172,173,174,175,176, + 177,178,179,180,181,182,183,0,0,0,0,0,177,184,185,0,177,186,187,0,188,189,190,191,192,193,194,0,0,0,0,0, + 188,195,0,0,0,0,0,0,196,197,198,0,0,199,200,201,202,203,204,74,205,0,0,0,0,0,0,0,0,0,0,0, + 206,207,208,209,210,211,0,0,212,213,214,215,216,72,0,0,0,0,0,0,0,0,0,217,218,219,220,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,221,222,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,72,0,74,223,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,224,225,0,0,0,0,0,0,0,0,0,0,0,0,74,74,226,227,228,0,0,229, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,74,74,74,74,74,74,74,74, + 74,74,74,74,74,74,5,230,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 74,74,74,231,232,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,74,233,234,0,0,0,0,0,0,0,0,0,106,235,74,236,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,106,237,0,0,0,0,0,0,106,238,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,74,74,239, }; -static kbts_u16 kbts__UnicodeSyllabicInfo_Data[12160] = { +static kbts_u16 kbts__UnicodeSyllabicInfo_Data[7680] = { 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,1034,0,0,1034,1034,1034,1034,1034,1034,1034,1034,1034,1034,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 1034,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,3592,3592,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1034,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,3,3,3,3,3,3,3,3,3,0,0,0,0,0,0,0,0,0,3,0,0, 1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,3,3,3,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 3592,3592,3592,3592,1026,1026,1026,1026,1026,1026,1026,1026,1026,1026,1026,1026,1026,1026,1026,1026,1026,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025, 1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1039,1025,1025,1025,1025,1025,1025,1025,1025,1025,2311,2311,2307,3601,2311,519, 2311,2311,2311,2311,2311,2311,2311,2311,2311,2311,2311,2311,2311,2308,519,2311,0,3593,3593,3592,3592,2311,2311,2311,1025,1025,1025,1025,1025,1025,1025,1025, @@ -8217,11 +7973,9 @@ static kbts_u16 kbts__UnicodeSyllabicInfo_Data[12160] = { 0,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025, 1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,0,7,7,7,7,7,7,7,7,7,7,7,0,0,0,0,0, 519,519,519,519,519,7,0,7,3,3,3,3,7,3592,7,0,1034,1034,1034,1034,1034,1034,1034,1034,1034,1034,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,1025,1025,0,1025,0,1025,1025,1025,1025,1025,0,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025, 1025,1025,1025,1025,0,1025,0,1025,1025,1025,1025,1025,1025,1025,1025,0,7,7,7,7,7,7,7,7,7,7,7,7,1040,1040,0,0, 519,519,519,519,519,0,0,0,3,3,3,3,0,3592,3592,0,1034,1034,1034,1034,1034,1034,1034,1034,1034,1034,0,0,1025,1025,1025,1025, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 1025,1025,0,0,1025,1025,1025,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 1034,1034,1034,1034,1034,1034,1034,1034,1034,1034,1034,1034,1034,1034,1034,1034,1034,1034,1034,1034,0,3592,0,3592,0,3,0,0,0,0,0,0, 1025,1025,1025,1025,1025,1025,1025,1025,0,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025, @@ -8229,15 +7983,11 @@ static kbts_u16 kbts__UnicodeSyllabicInfo_Data[12160] = { 7,7,3592,3592,7,3601,3,3,1025,1025,1025,1025,1025,1040,1040,1040,1040,1040,1040,1040,1040,1040,1040,1040,0,1040,1040,1040,1040,1040,1040,1040, 1040,1040,1040,1040,1040,1040,1040,1040,1040,1040,1040,1040,1040,1040,1040,1040,1040,1040,1040,1040,1040,1040,1040,1040,1040,1040,1040,1040,1040,0,0,0, 0,0,0,0,0,0,3592,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 1025,1025,1025,1025,1039,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1039,1025,1025,1025,1025, 1025,1026,1026,1026,1026,1026,1026,1026,1026,1026,1026,23,23,20,20,21,21,22,3593,20,20,20,3593,3,1034,4,27,33,31,32,30,1025, 1034,1034,1034,1034,1034,1034,1034,1034,1034,1034,1034,1034,0,0,1025,0,1025,1025,1026,1026,1026,1026,23,23,21,21,1039,1025,1025,1025,33,33, 36,1025,23,34,34,1025,1025,23,23,34,34,34,34,34,1025,1025,1025,20,20,20,20,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025, 1025,1025,32,23,22,20,20,1034,1034,1034,1034,1034,1034,1034,1025,1034,1034,1034,1034,1034,1034,1034,1034,1034,1034,1034,1034,1034,1034,20,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 1026,1026,1026,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,7,7,7,7,0,0,0,0,0,0,0,0,0,1025, 1026,1026,1026,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,7,7,7,0,0,0,0,0,0,0,0,0,0,0, 1026,1026,1026,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,7,7,0,0,0,0,0,0,0,0,0,0,0,0, @@ -8248,63 +7998,38 @@ static kbts_u16 kbts__UnicodeSyllabicInfo_Data[12160] = { 1034,1034,1034,1034,1034,1034,1034,1034,1034,1034,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 1025,0,0,0,0,0,0,1025,0,0,1025,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025, - 1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025, 1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,0,0,0,0,0,0,0, 1034,1034,1034,1034,1034,3,3,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,3,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 1034,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,0, 7,7,7,7,7,7,7,7,7,1040,1040,1040,0,0,0,0,1040,1040,3592,1040,1040,1040,1040,1040,1040,1040,3592,3592,0,0,0,0, 0,0,0,0,0,0,1034,1034,1034,1034,1034,1034,1034,1034,1034,1034,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025, 1025,1025,1025,1026,1026,1026,1026,1026,1026,1026,1026,1026,1026,1026,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025, 1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,0,0,0,0,7,7,7,7,7,519,519,519,7,7,519,7,7,7,7,7, 7,1025,1025,1025,1025,1025,1025,1025,3,3,0,0,0,0,0,0,1034,1034,1034,1034,1034,1034,1034,1034,1034,1034,1034,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,7,7,7,7,7,0,0,0,0, - 1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025, 1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1026,1026,1026,1026,1026,1026,1025,1025,1040,1040,1040,1040,1040,1025,1040,1040,1040,1040,0, 4,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,3592,3,3,3,3,3,7,3,3,0,0,3, 1034,1034,1034,1034,1034,1034,1034,1034,1034,1034,0,0,0,0,0,0,1034,1034,1034,1034,1034,1034,1034,1034,1034,1034,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 3592,3592,3592,1040,3592,1026,1026,1026,1026,1026,1026,1026,1026,1026,1026,1026,1026,1026,1026,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025, 1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,3,7,7,7,7,7,7,7,7,7,7,7, 7,7,7,7,4,1025,1025,1025,1025,1025,1025,1025,1025,0,0,0,1034,1034,1034,1034,1034,1034,1034,1034,1034,1034,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 3592,1040,3592,1026,1026,1026,1026,1026,1026,1026,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025, 1025,1040,1040,1040,7,7,7,7,7,7,7,4,1040,1040,1025,1025,1034,1034,1034,1034,1034,1034,1034,1034,1034,1034,3601,1025,1025,1025,1040,1040, - 1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025, 1025,1025,1025,1025,1026,1026,3,7,7,7,7,7,7,7,7,7,1040,1040,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025, 1025,1025,1025,1025,1040,1040,7,7,7,7,7,7,7,1040,1040,1040,1040,1040,1040,1040,3592,3592,0,3,0,0,0,0,0,0,0,0, 1034,1034,1034,1034,1034,1034,1034,1034,1034,1034,0,0,0,1025,1025,1025,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,3593,3593,3593,0,3593,3593,3593,3593,3593,3593,3593,3593,3593,3593,3593,3593, 3593,3593,3593,3593,3593,3593,3593,3593,3593,3601,3601,3601,3601,3593,3601,3601,3601,3601,1025,1025,3593,1025,1025,3593,3593,3593,1034,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,3592,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,5,6,0,0,1034,1034,1034,1034,1034,1034,0,0,0,0,0,0,0,0,0,0, 0,0,1034,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,3592,0,0,0,0,0,0,0,0,0,0,0, 0,0,3592,3592,3592,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,3593,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,1035,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1034,1034,1034,1034,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025, - 1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025, 1025,1025,1025,1025,1025,1025,1025,1025,0,0,0,0,0,0,0,1025,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,4, 1026,1026,7,1026,1026,1026,4,1025,1025,1025,1025,3592,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025, 1025,1025,1025,7,7,7,7,7,0,0,0,0,7,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, @@ -8317,7 +8042,6 @@ static kbts_u16 kbts__UnicodeSyllabicInfo_Data[12160] = { 1034,1034,1034,1034,1034,1034,1034,1034,1034,1034,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025, 1025,1025,1026,1026,1026,1026,1026,1026,1026,1026,1026,3,3,3,0,0,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025, 1025,1025,1025,1025,1025,1025,1025,7,7,7,7,7,7,7,7,1040,1040,1040,1040,7,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 3592,3592,1040,3592,1026,1026,1026,1026,1026,1025,1025,1025,1026,1026,1026,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025, 1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,3,7,7,7,7,7,7,7,7,7,1040,1040,1040, 4,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1034,1034,1034,1034,1034,1034,1034,1034,1034,1034,0,0,0,0,0,0, @@ -8326,54 +8050,26 @@ static kbts_u16 kbts__UnicodeSyllabicInfo_Data[12160] = { 1025,1025,1025,1025,1025,1025,1025,1025,1025,3592,7,7,7,7,7,7,7,7,7,1040,1040,1040,1040,0,0,0,0,0,0,0,0,0, 1040,1040,1040,1040,1040,1040,1040,1040,1040,1040,1040,1040,1040,1040,0,0,1034,1034,1034,1034,1034,1034,1034,1034,1034,1034,0,0,0,0,0,0, 1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,0,1025,1025,1025,1034,1034,1034,0,0,0,1025,34,3,3,1025,1025, - 1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025, 1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,7,1026,7,7,7,519,519,7,7,519,1026,519,519,1026,7,3, 0,3,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 1026,1026,1025,1025,1025,1025,1025,1025,1025,1025,1025,7,7,7,7,7,0,0,0,0,0,3592,4,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1026,1026,1025,1026,1025,1025,1025,1025,1025,1025,1025,1025,1025,1040,1040,1040,1040,1040, 1040,1040,1040,7,7,7,7,7,7,7,7,0,3,7,0,0,1034,1034,1034,1034,1034,1034,1034,1034,1034,1034,0,0,0,0,0,0, 35,35,35,35,35,35,35,35,35,35,35,35,35,35,35,35,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,0,1025,1025,1025,1025, 1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,0,1025,1025,1025,1025,1025,1025,1025,0,1025,1025,0,1025,1025,1025,1025,1025,1025,1025,1025,1025, 1025,1025,0,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,0,1025,1025,1025,1025,1025,1025,1025,0,1025,1025,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 1025,1287,7,7,0,7,1287,0,0,0,0,0,7,3592,3592,3592,1025,1025,1025,1025,0,1025,1025,1025,0,1025,1025,1025,1025,1025,1025,1025, 1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,0,0,3,3,3,0,0,0,0,4, 1034,1034,1034,1034,1034,1034,1034,1034,1034,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 1025,1025,1025,1025,1025,1025,1025,1025,0,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025, 1025,1025,1025,1025,1025,3,3,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025, 1025,1025,1025,1025,3,3,3,3592,0,0,0,0,0,0,0,0,1034,1034,1034,1034,1034,1034,1034,1034,1034,1034,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025, 1025,1025,1025,1025,1025,1025,0,0,0,7,7,7,7,7,0,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025, 1025,1025,1025,1025,1025,1025,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025, 1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,0,7,7,0,0,0,1025,1025,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025, 1025,1025,1025,1025,1025,1025,3,3,3,3,3,3,3,3,3,3,3,1034,1034,1034,1034,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025, 1025,1025,3,3,3,3,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 3592,3592,3592,1042,1042,1026,1026,1026,1026,1026,1026,1026,1026,1026,1026,1026,1026,1026,1026,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025, 1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,7,7,7,7,7,7,7,7, 7,7,7,7,7,7,4,0,0,0,0,0,0,0,0,0,0,0,1034,1034,1034,1034,1034,1034,1034,1034,1034,1034,1034,1034,1034,1034, @@ -8381,7 +8077,6 @@ static kbts_u16 kbts__UnicodeSyllabicInfo_Data[12160] = { 3592,3592,3592,1026,1026,1026,1026,1026,1026,1026,1026,1026,1026,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025, 1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,7,7,7,7,7,7,7,7,7,4,3,0,0,0,0,0, 0,0,7,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 3592,3592,3592,1026,1026,1026,1026,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025, 1025,1025,1025,1025,1025,1025,1025,7,7,7,7,7,7,7,7,7,7,7,7,4,3592,0,1034,1034,1034,1034,1034,1034,1034,1034,1034,1034, 0,0,0,0,1025,7,7,1025,0,0,0,0,0,0,0,0,1026,1026,1026,1026,1026,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025, @@ -8393,7 +8088,6 @@ static kbts_u16 kbts__UnicodeSyllabicInfo_Data[12160] = { 1026,1026,1026,1026,1026,1026,1026,1026,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,0,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025, 1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,7,7,7,7,7,7,7,7,3592,4,3,3592,0,0,0,0,0,0,3593,1025, 1026,7,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 1026,1026,1026,1026,1025,1025,1025,0,1025,0,1025,1025,1025,1025,0,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,0,1025, 1025,1025,1025,1025,1025,1025,1025,1025,1025,0,0,0,0,0,0,0,1026,1026,1026,1026,1026,1026,1026,1026,1026,1026,1025,1025,1025,1025,1025,1025, 1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,3592, @@ -8413,15 +8107,10 @@ static kbts_u16 kbts__UnicodeSyllabicInfo_Data[12160] = { 0,1026,1026,1026,1026,1026,1026,1026,1026,1026,1026,1026,1026,1026,1026,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025, 1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,3592, 3592,3592,4,3,3601,0,0,0,0,0,0,0,0,0,0,0,1034,1034,1034,1034,1034,1034,1034,1034,1034,1034,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 1026,1026,1026,1026,1026,1026,1026,1026,1026,1026,1026,1026,1026,1026,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025, 1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,7,7,7,7,7,7,7,0,0,7,7,7,7,3592,3592,3592,4, 3,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1026,1026,1026,1026,7,7,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 1026,1026,1026,1026,1026,1026,1026,1026,1026,1026,1026,1026,1026,1026,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025, 1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,7,7,7,7,7,7,7,7,7,7,7,7,7,3592,3592,4, 7,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1034,1034,1034,1034,1034,1034,1034,1034,1034,1034,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 1026,1026,1026,1026,1026,1026,1026,1026,1026,1026,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025, 1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,3592,3592,7,7,7,7,7,7,7,7,7,4,3,1025,0,0,0,0,0,0,0, 1034,1034,1034,1034,1034,1034,1034,1034,1034,1034,0,0,0,0,0,0,1034,1034,1034,1034,1034,1034,1034,1034,1034,1034,1034,1034,1034,1034,1034,1034, @@ -8429,119 +8118,59 @@ static kbts_u16 kbts__UnicodeSyllabicInfo_Data[12160] = { 1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,0,0,1040,1040,1040, 7,7,7,7,7,7,7,7,7,7,7,7,0,0,0,0,1034,1034,1034,1034,1034,1034,1034,1034,1034,1034,1034,1034,0,0,0,0, 1025,1025,1025,1025,1025,1025,1025,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 1026,1026,1026,1026,1026,1026,1026,1026,1026,1026,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025, 1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,7,7,7,7,7,7,7,7,7,7,7,3592,3592,4,3,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 1026,1026,1026,1026,1026,1026,1026,0,0,1026,0,0,1025,1025,1025,1025,1025,1025,1025,1025,0,1025,1025,0,1025,1025,1025,1025,1025,1025,1025,1025, 1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,7,7,7,7,7,7,0,7,7,0,0,3592,3592,7,4,0, 1040,14,1040,3,0,0,0,0,0,0,0,0,0,0,0,0,1034,1034,1034,1034,1034,1034,1034,1034,1034,1034,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 1026,1026,1026,1026,1026,1026,1026,1026,0,0,1026,1026,1026,1026,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025, 1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,7,7,7,7,7,7,7,0,0,7,7,7,7,3592,3592, 4,3601,0,0,7,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 1026,7,7,7,7,7,7,7,7,7,7,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025, 1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,3592,7,3592,3592,3592,3592,3592,1042,1040,1040,1040,1040,1034, 0,0,0,0,0,1034,0,4,0,0,0,0,0,0,0,0,1026,7,7,7,7,7,7,7,7,7,7,7,1025,1025,1025,1025, - 1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025, 1025,1025,1025,1025,0,0,0,0,0,0,1040,1040,1040,1040,1040,1040,1040,1040,1040,1040,1040,1040,3592,3592,3592,4,0,0,0,3601,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 1026,1026,1026,1026,1026,1026,1026,1026,1026,0,1026,1026,1026,1026,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025, 1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,7,7,7,7,7,7,7,7,0,7,7,7,7,3592,3592,3592,4, 3601,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1034,1034,1034,1034,1034,1034,1034,1034,1034,1034,1034,1034,1034,1034,1034,1034, 1034,1034,1034,1034,1034,1034,1034,1034,1034,1034,1034,1034,1034,0,0,0,0,0,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025, 1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,0,0,1040,1040,1040,1040,1040,1040,1040,1040,1040,1040,1040,1040,1040,1040, 1040,1040,1040,1040,1040,1040,1040,1040,0,1040,1040,1040,1040,1040,1040,1040,7,7,7,7,7,3592,3592,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 1026,1026,1026,1026,1026,1026,1026,0,1026,1026,0,1026,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025, 1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,7,7,7,7,7,7,0,0,0,7,0,7,7,0,7, 3592,3592,3,7,7,4,14,1040,0,0,0,0,0,0,0,0,1034,1034,1034,1034,1034,1034,1034,1034,1034,1034,0,0,0,0,0,0, 1026,1026,1026,1026,1026,1026,0,1026,1026,0,1026,1026,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025, 1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,7,7,7,7,7,0,7,7,0,7,7,3592,3592,4,0,0,0,0,0,0,0,0, - 1034,1034,1034,1034,1034,1034,1034,1034,1034,1034,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1034,7,7,7,7,0,0,0,0,0,0,0,0,0, 3592,3592,14,3592,1026,1026,1026,1026,1026,1026,1026,1026,1026,1026,1026,1026,1026,0,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025, 1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,7,7,7,7,7,7,7,0,0,0,7,7, 7,7,4,0,0,0,0,0,0,0,0,0,0,0,0,0,1034,1034,1034,1034,1034,1034,1034,1034,1034,1034,3,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 1026,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,7,7, 7,7,7,7,7,7,7,7,7,7,1040,1040,1040,3592,1040,7,1034,1034,1034,1034,1034,1034,1034,1034,1034,1034,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 1034,1034,1034,1034,1034,1034,1034,1034,1034,1034,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025, 1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,3,3,3,3,3,3,3,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 3592,3592,3592,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025, 1025,1025,1025,7,7,7,7,7,7,7,7,7,7,0,0,0,1034,1034,1034,1034,1034,1034,1034,1034,1034,1034,0,0,0,0,0,0, - 1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025, - 1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025, 1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,0,0,0,0,3,0,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7, 7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7, 7,7,7,7,7,7,7,7,0,0,0,0,0,0,0,3,3,3,3,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,1025,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025, - 1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025, - 1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025, - 1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025, - 1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025, - 1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025, - 1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1025, - 1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025, - 1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025, - 1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025, 1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,0,0,0,0,0,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,0,0,0, 1025,1025,1025,1025,1025,1025,1025,1025,1025,0,0,0,0,0,0,0,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,0,0,0,1283,1283,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025, 1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,0,0,0,3,3,3,3,3,3,3,1025,1025,1025,1025,1025,1025,1025,0,0, 1034,1034,1034,1034,1034,1034,1034,1034,1034,1034,0,0,0,0,1025,1025,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025, 1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,3,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025, 1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,3,3,3,3,1034,1034,1034,1034,1034,1034,1034,1034,1034,1034,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025, 1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,7,7,7,7,1034,1034,1034,1034,1034,1034,1034,1034,1034,1034,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025, 1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,7,7,1025,1034,1034,1034,1034,1034,1034,1034,1034,1034,1034,0,0,0,0,0, - 1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025, - 1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025,1025, 1025,1025,1025,1025,3,3,3,3,3,3,3,1025,0,0,0,0,1034,1034,1034,1034,1034,1034,1034,1034,1034,1034,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, }; KBTS_INLINE kbts_u16 kbts__GetUnicodeSyllabicInfo(kbts_u32 Codepoint) { - return (Codepoint < 1114110) ? kbts__UnicodeSyllabicInfo_Data[((kbts_un)kbts__UnicodeSyllabicInfo_PageIndices[Codepoint/128] * 128) | (Codepoint & 127)] : 0; + return (Codepoint < 1114110) ? kbts__UnicodeSyllabicInfo_Data[((Codepoint < 125280) ? ((kbts_un)kbts__UnicodeSyllabicInfo_PageIndices[Codepoint / 32] * 32) : 0) | (Codepoint & 31)] : 0; } -static kbts_u8 kbts__UnicodeFlags_PageIndices[8703] = { +static kbts_u8 kbts__UnicodeFlags_PageIndices[8193] = { 0,1,2,3,2,4,5,6,2,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,33,33,33,39,40,41,42,43,44,45,46,47,48,49,50,51,52,53,54,2,2,2,55, 56,57,58,59,60,61,62,33,63,33,33,33,33,33,64,65,33,33,33,66,67,68,69,70,71,72,73,74,75,76,33,77, @@ -8798,22 +8427,7 @@ static kbts_u8 kbts__UnicodeFlags_PageIndices[8703] = { 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,242, - 83,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, + 83, }; static kbts_u8 kbts__UnicodeFlags_Data[31104] = { @@ -9307,10 +8921,10 @@ static kbts_u8 kbts__UnicodeFlags_Data[31104] = { KBTS_INLINE kbts_u8 kbts__GetUnicodeFlags(kbts_u32 Codepoint) { - return (Codepoint < 1114110) ? kbts__UnicodeFlags_Data[((kbts_un)kbts__UnicodeFlags_PageIndices[Codepoint/128] * 128) | (Codepoint & 127)] : 0; + return (Codepoint < 1114110) ? kbts__UnicodeFlags_Data[((Codepoint < 1048704) ? ((kbts_un)kbts__UnicodeFlags_PageIndices[Codepoint / 128] * 128) : 256) | (Codepoint & 127)] : 0; } -static kbts_u8 kbts__UnicodeBidirectionalClass_PageIndices[8703] = { +static kbts_u8 kbts__UnicodeBidirectionalClass_PageIndices[8193] = { 0,1,2,2,2,3,4,5,2,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,2,2,31,32,33,34,35,2,2,2,2,36,37,38,39,40,41,42,43,44,45,46,47,48,2,49,2,2,50,51, 52,53,54,55,56,57,58,59,57,60,57,57,57,61,57,57,2,2,57,57,57,57,57,57,2,62,63,64,57,57,57,57, @@ -9567,22 +9181,7 @@ static kbts_u8 kbts__UnicodeBidirectionalClass_PageIndices[8703] = { 57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57, 57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57, 57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,231, - 73,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57, - 57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57, - 57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57, - 57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57, - 57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57, - 57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57, - 57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57, - 57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57, - 57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57, - 57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57, - 57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57, - 57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57, - 57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57, - 57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57, - 57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57, - 57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57,57, + 73, }; static kbts_u8 kbts__UnicodeBidirectionalClass_Data[29696] = { @@ -10054,10 +9653,10 @@ static kbts_u8 kbts__UnicodeBidirectionalClass_Data[29696] = { KBTS_INLINE kbts_u8 kbts__GetUnicodeBidirectionalClass(kbts_u32 Codepoint) { - return (Codepoint < 1114110) ? kbts__UnicodeBidirectionalClass_Data[((kbts_un)kbts__UnicodeBidirectionalClass_PageIndices[Codepoint/128] * 128) | (Codepoint & 127)] : 0; + return (Codepoint < 1114110) ? kbts__UnicodeBidirectionalClass_Data[((Codepoint < 1048704) ? ((kbts_un)kbts__UnicodeBidirectionalClass_PageIndices[Codepoint / 128] * 128) : 7296) | (Codepoint & 127)] : 0; } -static kbts_u8 kbts__UnicodeJoiningType_PageIndices[8703] = { +static kbts_u8 kbts__UnicodeJoiningType_PageIndices[7172] = { 0,1,0,0,0,0,2,0,0,3,0,4,5,6,7,8,9,10,11,12,13,14,15,16,17,18,19,20,21,22,23,24, 25,26,0,0,0,0,27,0,0,0,0,0,0,0,28,29,30,31,32,0,33,34,35,36,37,38,0,39,0,0,0,0, 40,41,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,42,43,44,0,0,0,0, @@ -10282,54 +9881,7 @@ static kbts_u8 kbts__UnicodeJoiningType_PageIndices[8703] = { 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 114,0,115,2,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 114,0,115,2, }; static kbts_u8 kbts__UnicodeJoiningType_Data[14848] = { @@ -10569,41 +10121,40 @@ static kbts_u8 kbts__UnicodeJoiningType_Data[14848] = { KBTS_INLINE kbts_u8 kbts__GetUnicodeJoiningType(kbts_u32 Codepoint) { - return (Codepoint < 1114110) ? kbts__UnicodeJoiningType_Data[((kbts_un)kbts__UnicodeJoiningType_PageIndices[Codepoint/128] * 128) | (Codepoint & 127)] : 0; + return (Codepoint < 1114110) ? kbts__UnicodeJoiningType_Data[((Codepoint < 918016) ? ((kbts_un)kbts__UnicodeJoiningType_PageIndices[Codepoint / 128] * 128) : 0) | (Codepoint & 127)] : 0; } -static kbts_u8 kbts__UnicodeCombiningClass_PageIndices[8703] = { - 0,0,0,0,0,0,1,0,0,2,0,3,4,5,6,7,8,9,10,11,12,12,12,13,14,12,15,16,17,18,19,20, - 21,22,0,0,0,0,23,0,0,0,0,0,0,0,24,25,0,26,27,0,28,29,30,31,32,33,0,34,0,0,0,0, - 0,35,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,36,37,38,0,0,0,0, - 39,40,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, +static kbts_u8 kbts__UnicodeCombiningClass_PageIndices[3915] = { + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1,2,3,4,0,0,0,0, + 0,0,0,0,5,0,0,0,0,0,0,0,6,7,8,0,9,0,10,11,0,0,12,13,14,15,16,0,0,0,0,17, + 18,19,20,0,21,0,22,23,0,24,25,0,0,24,26,27,0,24,26,0,0,24,26,0,0,24,26,0,0,0,26,0, + 0,24,28,0,0,24,26,0,0,29,26,0,0,0,30,0,0,31,32,0,0,33,34,0,35,36,0,37,38,0,39,0, + 0,40,0,0,41,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,42,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,43,44,0,0,0,0,45,0, + 0,0,0,0,0,46,0,0,0,47,0,0,0,0,0,0,48,0,0,49,0,50,51,0,0,52,53,54,0,55,0,56, + 0,57,0,0,0,0,58,59,0,0,0,0,0,0,60,61,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,62,63,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,64,0,0,0,65,0,0,0,66,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,67,0,0,68,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,41,42,0,0,43,44,45,46,0,47,0,48,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,49,0,0,0,0,0,50,0,0,0, - 0,0,0,51,0,52,53,0,0,0,0,0,0,0,0,0,0,0,0,0,54,55,0,0,0,0,56,0,0,57,58,59, - 60,61,62,63,64,65,66,67,68,69,0,70,71,72,73,0,61,0,74,75,76,77,0,0,71,0,78,79,0,0,80,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,81,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,82,83,0,0,0,0,0,0,0,0,84, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,85,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,86,87,88,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 89,90,83,0,0,91,0,0,0,92,0,93,0,0,0,0,0,94,95,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, @@ -10615,6 +10166,8 @@ static kbts_u8 kbts__UnicodeCombiningClass_PageIndices[8703] = { 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,69,70,0,0,71,0,0,0,0,0,0,0,0, + 72,73,0,0,0,0,53,74,0,75,76,0,0,77,78,0,0,0,0,0,0,79,80,81,0,0,0,0,0,0,0,26, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, @@ -10634,7 +10187,16 @@ static kbts_u8 kbts__UnicodeCombiningClass_PageIndices[8703] = { 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,82,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,83,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,84,0,0,0,0,0,0,0,85,0,0,0,86,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,87,88,0,0,0,0,0,89,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,90,0,91,0,0,0,0,0,0,0,0,0,92,0,93,0,0,94,0,95,0,0,0, + 0,0,72,96,0,97,0,0,98,99,0,77,0,0,100,0,0,101,0,0,0,0,0,102,0,103,26,104,0,0,105,0, + 0,0,106,0,0,0,107,0,0,0,0,0,0,65,108,0,0,65,0,0,0,109,0,0,0,110,0,0,0,0,0,0, + 0,97,0,0,0,0,0,0,0,111,112,0,0,0,0,78,0,44,113,0,114,0,0,0,0,0,0,0,0,0,0,0, + 0,65,0,0,0,0,0,0,0,0,115,0,116,0,0,0,0,0,0,0,0,0,0,0,0,0,117,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, @@ -10651,7 +10213,10 @@ static kbts_u8 kbts__UnicodeCombiningClass_PageIndices[8703] = { 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,118,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,119,0,120,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,121, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, @@ -10671,434 +10236,257 @@ static kbts_u8 kbts__UnicodeCombiningClass_PageIndices[8703] = { 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,122,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,123,124,125,0,0,0,0,126,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 127,128,0,0,129,0,0,0,0,120,0,0,0,0,0,0,0,0,0,0,0,130,0,131,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,132,0,0,0,0,0,0,0,133,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,134,0,0,0,135, }; -static kbts_u8 kbts__UnicodeCombiningClass_Data[12288] = { - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 230,230,230,230,230,230,230,230,230,230,230,230,230,230,230,230,230,230,230,230,230,232,220,220,220,220,232,216,220,220,220,220,220,202,202,220,220,220,220,202,202,220,220,220,220,220,220,220,220,220,220,220,1,1,1,1,1,220,220,220,220,230,230,230, - 230,230,230,230,230,240,230,220,220,220,230,230,230,220,220,0,230,230,230,220,220,220,220,230,232,220,220,230,233,234,234,233,234,234,233,230,230,230,230,230,230,230,230,230,230,230,230,230,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,230,230,230,230,230,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, +static kbts_u8 kbts__UnicodeCombiningClass_Data[4352] = { + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,230,230,230,230,230,230,230,230,230,230,230,230,230,230,230,230,230,230,230,230,230,232,220,220,220,220,232,216,220,220,220,220, + 220,202,202,220,220,220,220,202,202,220,220,220,220,220,220,220,220,220,220,220,1,1,1,1,1,220,220,220,220,230,230,230,230,230,230,230,230,240,230,220,220,220,230,230,230,220,220,0,230,230,230,220,220,220,220,230,232,220,220,230,233,234,234,233, + 234,234,233,230,230,230,230,230,230,230,230,230,230,230,230,230,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,230,230,230,230,230,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,220,230,230,230,230,220,230,230,230,222,220,230,230,230,230,230,230,220,220,220,220,220,220,230,230,220,230,230,222,228,230,22,15,16,17,23,18,19,20,21,14,14,24,12,25,0,13, - 0,10,11,0,230,220,0,21,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,230,230,230,230,230,230,230,230,31,32,33,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,10,11,0,230,220,0,21,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,230,230,230,230,230,230,230,230,31,32,33,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,28,29,30,31,32,33,27,34,230,230,220,220,230,230,230,230,230,220,230,230,220,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,35,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,230,230,230,230,230,230,230,0,0,230,230,230,230,220,230,0,0,230,230,0,220,230,230,220,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,36,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,230,220,230,230,220,230,230,220,220,220,230,220,220,230,220,230, - 230,230,220,230,220,230,220,230,220,230,230,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,230,230,230,230,230,230,230,220,230,0,0,0,0,0,0,0,0,0,220,0,0, + 230,230,220,230,220,230,220,230,220,230,230,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,230,230,230,230,230,230,230,220,230,0,0,0,0,0,0,0,0,0,220,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,230,230,230,230,0,230,230,230,230,230,230,230,230,230,0,230,230,230,0,230,230,230,230,230,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,220,220,220,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,230,230,220,220,220,230,230,230,230,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,220,220,220,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,230,230,220,220,220,230,230,230,230, 0,0,0,0,0,0,0,0,0,0,230,230,230,230,230,220,220,220,220,220,230,230,230,230,230,230,230,230,230,230,230,230,230,230,0,220,230,230,220,230,230,220,230,230,230,220,220,220,28,29,30,230,230,230,220,230,230,220,220,230,230,230,230,230, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,7,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,9,0,0,0,230,220,230,230,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,7,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,7,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,9,0,0,0,230,220,230,230,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,9,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,230,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,7,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,9,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,9,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,7,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,9,0,0,0,0,0,0,0,4,5,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,9,9,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,9,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,9,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,3,3,9,0,0,0,0,0, - 0,0,0,0,0,0,0,0,107,107,107,107,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,118,118,9,0,0,0,0,0, - 0,0,0,0,0,0,0,0,122,122,122,122,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,220,220,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,220,0,220,0,127,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,129,132,0,131,0,0,0,0,0,132,132,132,132,0,0, - 132,0,230,230,9,0,230,230,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,254,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,7,0,9,9,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,220,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,230,230,230,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,9,9,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,9,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,9,0,0,0,0,0,0,0,0,0,0,230,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,228,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,222,230,220,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,230,220,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,254,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,230,230,230,230,230,230,230,230,0,0,220, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,230,230,230,230,230,220,220,220,220,220,220,230,230,220,0,220, - 220,230,230,220,220,230,230,230,230,230,220,230,230,230,230,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,7,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,9,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,230,220,230,230,230,230,230,230,230,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,9,9,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,7,0,0,0,0,0,0,0,0,0,0,0,9,9,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,7,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,9,0,0,0,0,0,0,0,4,5,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,9,9,0,0,0, + 0,0,0,0,0,0,0,0,0,0,9,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,3,3,9,0,0,0,0,0, + 0,0,0,0,0,0,0,0,107,107,107,107,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,118,118,9,0,0,0,0,0, + 0,0,0,0,0,0,0,0,122,122,122,122,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,220,220,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,220,0,220,0,127,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,129,132,0,131,0,0,0,0,0,132,132,132,132,0,0, + 132,0,230,230,9,0,230,230,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,254,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,7,0,9,9,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,220,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,230,230,230,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,9,9,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,9,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,9,0,0,0,0,0,0,0,0,0,0,230,0,0, + 0,0,0,0,0,0,0,0,0,228,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,222,230,220,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,230,220,0,0,0,0,0,0,0,254,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,230,230,230,230,230,230,230,230,0,0,220, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,230,230,230,230,230,220,220,220,220,220,220,230,230,220,0,220,220,230,230,220,220,230,230,230,230,230,220,230,230,230,230,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,7,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,9,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,230,220,230,230,230,230,230,230,230,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,9,9,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,7,0,0,0,0,0,0,0,0,0,0,0,9,9,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,7,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,230,230,230,0,1,220,220,220,220,220,230,230,220,220,220,220,230,0,1,1,1,1,1,1,1,0,0,0,0,220,0,0,0,0,0,0,230,0,0,0,230,230,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 230,230,220,230,230,230,230,230,230,230,220,230,230,234,214,220,202,230,230,230,230,230,230,230,230,230,230,230,230,230,230,230,230,230,230,230,230,230,230,230,230,230,230,230,230,230,230,230,230,230,230,230,230,230,232,228,228,220,218,230,233,220,230,220, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,230,230,1,1,230,230,230,230,1,1,1,230,230,0,0,0,0,230,0,0,0,1,1,230,220,230,1,1,220,220,220,220,230,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,230,230,230,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,9, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,230,230,230,230,230,230,230,230,230,230,230,230,230,230,230,230,230,230,230,230,230,230,230,230,230,230,230,230,230,230,230,230, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,218,228,232,222,224,224,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,8,8,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,230,0,0,0,0,230,230,230,230,230,230,230,230,230,230,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,230,230,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,230,230,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,230,230,230,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,9, + 230,230,230,230,230,230,230,230,230,230,230,230,230,230,230,230,230,230,230,230,230,230,230,230,230,230,230,230,230,230,230,230,0,0,0,0,0,0,0,0,0,0,218,228,232,222,224,224,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,8,8,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,230,0,0,0,0,230,230,230,230,230,230,230,230,230,230,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,230,230,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,230,230,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,9,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,9,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,9,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,230,230,230,230,230,230,230,230,230,230,230,230,230,230,230,230,230,230,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,220,220,220,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,9,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,7,0,0,0,0,0,0,0,0,0,0,0,0, - 9,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,230,0,230,230,220,0,0,230,230,0,0,0,0,0,230,230, + 230,230,230,230,230,230,230,230,230,230,230,230,230,230,230,230,230,230,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,220,220,220,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,9,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,7,0,0,0,0,0,0,0,0,0,0,0,0, + 9,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,230,0,230,230,220,0,0,230,230,0,0,0,0,0,230,230, 0,230,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,9,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,9,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,26,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,230,230,230,230,230,230,230,220,220,220,220,220,220,220,230,230,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,220,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,220,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,230,230,230,230,230,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,220,0,230,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,230,1,220,0,0,0,0,9, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,230,220,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,230,230,230,230,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,230,230,230,230,230,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,230,230,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,220,220,220, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,220,220,230,230,230,220,230,220,220,220,220,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,230,220,230,220,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,9,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,9,0,0,0,0,0,0,0,0,0,0,0,0,0,0,9, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,9,7,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,26,0,230,230,230,230,230,230,230,220,220,220,220,220,220,220,230,230,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,220,0,0,220,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,230,230,230,230,230,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,220,0,230,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,230,1,220,0,0,0,0,9,0,0,0,0,0,230,220,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,230,230,230,230,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,230,230,230,230,230,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,230,230,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,220,220,220, + 0,0,0,0,0,0,220,220,230,230,230,220,230,220,220,220,220,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,230,220,230,220,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,9,0,0,0,0,0,0,0,0,0,0,0,0,0,0,9,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,9,7,0,0,0,0,0, 230,230,230,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,9,9,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,7,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 9,0,0,0,0,0,0,0,0,0,7,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,9,7,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,7,9,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,7,7,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,9,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,230,230,230,230,230,230,230,0,0,0,230,230,230,230,230,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,9,9,9,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,9,0,0,0,7,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,230,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,9,7,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,9, - 7,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,9, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,9,7,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,9,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,9,9,0, - 0,0,0,7,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,9,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,9,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,9,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,9,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,7,0,9,9,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,9,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,9,9,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,9,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1,1,1,1,1,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,230,230,230,230,230,230,230,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,6,6,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,216,216,1,1,1,0,0,0,226,216,216,216,216,216,0,0,0,0,0,0,0,0,220,220,220,220,220, + 9,0,0,0,0,0,0,0,0,0,7,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,9,7,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,7,9,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,7,7,0,0,0, + 0,0,0,0,0,0,230,230,230,230,230,230,230,0,0,0,230,230,230,230,230,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,9,9,9,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,9,0,0,0,7,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,230,0,0,0,9,7,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 7,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,9,7,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,9,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,9,9,0, + 0,0,0,7,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,9,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,9,0,0,0,0,0,0,0,0,7,0,9,9,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,9,0,0,0,0,0,0,0,0,0,9,9,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,9,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1,1,1,1,1,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,230,230,230,230,230,230,230,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,6,6,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,216,216,1,1,1,0,0,0,226,216,216,216,216,216,0,0,0,0,0,0,0,0,220,220,220,220,220, 220,220,220,0,0,230,230,230,230,230,220,220,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,230,230,230,230,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,230,230,230,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 230,230,230,230,230,230,230,0,230,230,230,230,230,230,230,230,230,230,230,230,230,230,230,230,230,0,0,230,230,230,230,230,230,230,0,230,230,0,230,230,230,230,230,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,230,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,230,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,230,230,230,230,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,232,232,220,230,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,230,220,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,220,220,220,220,220,220,220,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,230,230,230,230,230,230,7,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,230,230,230,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,230,230,230,230,230,230,230,0,230,230,230,230,230,230,230,230,230,230,230,230,230,230,230,230,230,0,0,230,230,230,230,230, + 230,230,0,230,230,0,230,230,230,230,230,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,230,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,230,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,230,230,230,230,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,232,232,220,230,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,230,220,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,220,220,220,220,220,220,220,0,0,0,0,0,0,0,0,0,0,0,0,0,230,230,230,230,230,230,7,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, }; KBTS_INLINE kbts_u8 kbts__GetUnicodeCombiningClass(kbts_u32 Codepoint) { - return (Codepoint < 1114110) ? kbts__UnicodeCombiningClass_Data[((kbts_un)kbts__UnicodeCombiningClass_PageIndices[Codepoint/128] * 128) | (Codepoint & 127)] : 0; + return (Codepoint < 1114110) ? kbts__UnicodeCombiningClass_Data[((Codepoint < 125280) ? ((kbts_un)kbts__UnicodeCombiningClass_PageIndices[Codepoint / 32] * 32) : 0) | (Codepoint & 31)] : 0; } -static kbts_u16 kbts__UnicodeParentInfo_PageIndices[34815] = { - 0,1,2,3,0,4,5,6,7,0,8,9,0,10,0,11,0,12,0,0,13,14,0,0,15,0,0,0,16,17,18,0, - 19,20,21,22,0,0,23,24,0,0,0,0,0,0,25,26,0,27,28,0,0,0,29,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,30,31,0,0,0,32,33,0,34,35,0,0,0,0,0,0,0,36,37,0,0,0,38,0, - 0,0,39,0,0,40,41,0,0,0,38,0,0,0,42,0,0,0,0,0,0,0,0,0,0,0,43,44,45,46,0,0, - 0,47,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,48,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,49,50,51,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,52,53,54,0,55,56,0,57,58,59,60,0,61,62,63, - 64,0,0,0,0,0,0,0,0,0,0,0,65,0,66,0,67,68,69,70,71,72,0,0,0,0,0,0,0,0,0,0, +static kbts_u16 kbts__UnicodeParentInfo_PageIndices[21697] = { + 0,0,0,0,0,0,0,1,2,3,4,5,6,7,8,9,0,0,0,0,0,10,11,0,12,13,14,15,16,17,18,19, + 20,0,21,0,0,0,0,0,0,22,0,23,24,25,0,26,0,0,0,0,27,28,29,0,0,0,0,0,0,30,0,0, + 0,0,0,0,31,32,0,0,0,0,0,0,0,0,0,0,0,0,33,0,0,0,0,34,0,0,0,0,0,0,0,0, + 35,36,37,0,0,0,0,0,0,0,0,0,0,0,0,0,38,39,40,41,42,43,44,45,46,47,48,0,0,0,0,0, + 49,0,50,51,52,53,54,55,56,57,49,0,0,0,58,0,0,0,0,0,0,0,0,0,0,0,0,59,0,59,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,60,61,62,63,64,0, + 0,0,0,0,65,0,0,0,0,66,0,0,0,0,0,0,0,0,0,0,0,0,0,0,67,0,68,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,73,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 74,0,75,76,77,75,76,78,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,79,80,81,0,82,0,0,0,0,0,0,0,0,0,0,0,0,0,0,83,0,0,0,0,84,0,0,0, - 0,85,0,86,0,0,87,88,89,90,0,0,0,0,0,0,0,91,0,92,0,0,0,93,94,0,95,0,96,0,0,0, - 97,0,98,0,0,0,0,0,0,99,0,0,100,0,0,0,0,0,0,0,0,101,0,0,102,0,0,0,0,0,0,103, - 104,105,106,0,107,0,0,108,0,109,0,0,0,0,0,0,110,111,0,0,0,112,0,0,113,114,115,0,0,0,116,0, - 117,0,0,118,0,0,0,0,0,119,120,121,0,0,122,123,0,124,0,0,0,125,126,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,127,0,0,128,0,0,0,129,0,130,0,0,0,131,0,0,0,0,132,0, - 0,0,0,0,0,0,133,134,0,0,135,0,0,0,0,0,136,137,138,0,139,140,141,142,0,0,0,143,144,145,0,0, - 146,147,0,148,149,0,150,151,0,0,152,153,154,155,156,157,158,159,160,161,162,163,164,165,166,167,168,169,0,0,170,171, - 172,173,174,175,176,177,0,178,179,0,180,181,182,183,184,185,186,0,187,188,0,0,0,189,190,0,0,0,191,0,192,193, - 194,195,196,0,0,197,198,199,200,201,202,203,0,0,204,205,206,207,0,208,0,209,0,0,210,211,0,0,212,0,213,214, - 215,216,0,217,218,0,219,0,220,0,221,222,0,223,0,224,0,225,0,226,0,227,228,229,230,231,232,233,234,235,236,237, - 238,0,0,239,240,0,241,242,243,0,244,245,246,247,248,249,250,251,252,0,0,253,254,255,0,256,257,258,259,260,261,262, - 263,264,265,266,267,0,268,0,0,0,269,270,271,0,272,273,274,0,275,276,277,278,279,280,281,282,283,284,285,0,0,286, - 287,0,288,0,289,290,0,0,291,0,292,0,0,293,0,294,295,0,0,0,0,296,297,0,298,299,300,301,302,303,0,0, - 0,0,304,305,306,307,308,309,310,311,312,313,314,0,315,316,317,318,0,319,320,321,322,0,323,324,0,325,0,0,326,327, - 328,329,330,331,332,333,334,0,0,0,335,336,337,0,338,0,339,340,341,342,343,344,345,346,0,347,0,348,349,350,351,0, - 352,353,354,355,356,0,357,0,358,359,360,361,0,0,0,362,363,0,364,365,0,0,366,367,368,0,369,0,370,371,0,0, - 0,0,372,373,374,0,375,376,0,377,378,379,380,381,382,383,384,0,385,0,386,387,388,389,0,390,0,0,0,0,391,0, - 0,392,0,393,394,395,396,397,398,399,400,401,0,402,403,404,405,406,407,0,0,0,0,0,0,408,0,409,410,411,0,412, - 413,0,414,415,416,417,0,0,418,419,0,0,0,0,420,421,422,0,0,423,424,425,0,426,427,428,429,430,0,431,432,433, - 0,434,435,0,0,0,0,436,437,0,0,438,0,0,439,440,441,442,443,444,445,446,0,447,448,449,0,450,451,452,0,453, - 454,0,455,456,0,0,457,458,459,0,460,461,462,0,0,0,0,0,0,0,0,463,464,465,466,467,468,0,469,0,0,0, - 0,0,470,0,0,471,472,0,473,0,0,474,0,475,476,477,0,0,0,0,0,0,478,0,0,479,0,480,481,482,0,0, - 0,483,0,484,485,0,486,487,488,0,0,489,490,491,492,0,0,493,0,494,0,0,495,0,496,0,497,0,0,0,0,498, - 499,0,0,0,0,0,0,0,0,0,0,0,500,501,0,0,0,502,503,504,505,506,507,508,0,509,510,0,0,0,511,512, - 513,514,515,0,0,0,0,516,0,517,0,0,0,518,519,520,0,0,0,521,0,0,0,0,522,0,0,523,0,0,0,0, - 0,0,524,0,0,0,0,525,0,0,0,526,0,527,0,528,529,0,0,530,531,532,533,534,535,536,537,0,538,0,0,0, + 0,0,69,70,71,72,73,0,0,0,0,0,0,0,0,0,0,0,0,0,0,74,0,0,75,0,0,0,0,0,0,0, + 0,0,76,70,0,77,78,79,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,71,0,0,0,80,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,81,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,82,0,0,0,0,0,0,0,0,0,0,0,0,0,0,83,84,78,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,81,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,85,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,86,87,88,73,0,0,89,0,0,0,86,87,88,73,90,0,0,0,0,0,0,0,0,0, + 0,0,0,0,91,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,92,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 93,94,67,0,0,0,0,95,78,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,96,0,0,0,0,30,97,0,0,0,0,0,0,0,98,0,0,99,0,100,0,0,0,0,0,0, + 101,101,102,102,103,103,104,104,102,102,104,105,106,106,107,108,0,0,0,0,0,0,49,109,49,0,0,0,0,0,49,110, + 111,0,112,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,113,0,0,0,0,0,0,0,114,0,0,0,0,0, + 73,115,0,0,116,0,0,117,118,119,0,0,120,0,121,122,121,0,123,0,124,125,126,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,127,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,128,0,0,0,0,0,0,129,130,131,131,132,133,134,135,0,0,0,91,129,130,131,131,132,133,134,135,0,136,137,91, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,138,0,0,0,139,0,0,0,140,0,0,0,0, + 0,0,141,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,142,0,143, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,144,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,145,0,0,0,0,0,0,146,0,0,147,0,0,0,0,0,0,0,0,148,0,0,0,149,0,0,0, + 0,0,0,150,0,0,0,151,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,152,0,0,0,0,0,0,0,153,0,0,0,0,0,0,0,0,0,0,0,0,0,0,154,0,0,0, + 0,155,0,156,0,0,0,0,0,157,0,0,0,0,0,0,0,0,0,158,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,159,0,0,0,0,0,160,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,161,0,0,0,0,0,0,0,0,0,0,0,162,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,163,0,164,0,0,0,0,0,0,0,0, + 0,0,0,165,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,166, + 0,167,0,168,0,0,0,169,170,0,0,0,0,0,0,0,0,0,171,0,0,0,0,0,0,0,0,0,172,0,0,0, + 0,0,0,0,0,173,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 174,0,0,0,175,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,176,0,0,0,0,0,0,0,0,0,0,0, + 177,0,0,0,0,0,178,0,0,0,0,179,0,0,0,0,0,0,0,0,0,0,0,0,0,0,180,181,0,0,0,0, + 0,182,0,0,0,0,0,0,0,0,0,0,0,183,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,184,0,0,0,0,0,185,186,187,0,0,0,0,0,0,0,0,0,0,0,0,188,0,0,0,0,189, + 0,0,0,0,0,0,190,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,191,192,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,193,0,0,0,0,0,0,0,0,0,194,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,195,196,0,0,0,0,0,0,0,197,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,198,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,199,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,200,0,0,0,201,0,202, + 0,0,0,0,0,0,0,0,0,0,203,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,204,0,0,205,0,206,207,208,0,0,0,0,0,0,0,209,0,0,0,0,210,0,0,211,212,0,0,213,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,214,0,0,0,215,216,0,0,0,217,0,218,0,0,0,0,0,0,0,0, + 219,0,0,0,0,220,0,0,0,0,0,0,0,0,0,221,0,0,0,222,0,0,0,0,0,223,0,224,225,0,0,0, + 0,0,0,0,0,0,0,0,226,227,228,0,229,230,231,0,232,233,234,0,235,236,237,0,0,238,0,239,0,0,240,0, + 241,0,242,0,0,243,0,244,245,0,0,0,0,0,246,0,0,247,0,248,249,0,250,0,251,252,253,254,255,0,256,257, + 258,0,259,0,0,0,0,260,0,261,262,263,0,0,264,265,0,0,0,0,0,0,0,0,266,267,0,268,269,270,271,0, + 272,273,0,274,0,0,0,275,276,277,0,0,0,278,0,0,0,0,0,279,280,0,0,281,0,0,0,0,0,0,282,0, + 0,0,283,0,0,0,0,0,0,0,284,0,285,0,0,0,286,0,0,287,0,288,289,0,290,0,0,0,291,0,0,0, + 292,0,0,0,0,0,0,0,0,0,293,0,0,294,295,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,296, + 0,0,297,0,0,0,0,0,0,0,0,0,0,0,0,0,0,298,0,0,0,0,0,0,0,299,0,0,0,0,300,0, + 0,301,0,0,0,0,302,0,303,0,0,304,0,0,0,0,0,0,0,0,0,305,306,0,0,0,0,307,0,308,309,0, + 310,0,0,311,312,0,0,0,313,314,315,0,316,0,317,0,0,0,0,0,0,0,0,0,0,0,0,318,0,319,0,0, + 0,0,0,320,321,0,0,0,0,0,0,0,322,0,0,0,0,0,0,0,0,0,323,0,0,0,0,0,0,0,0,0, + 0,324,0,0,0,325,0,326,0,0,0,0,0,0,0,0,327,0,0,0,0,0,0,0,328,0,0,329,330,331,332,333, + 334,0,0,0,335,0,0,336,0,0,0,0,337,338,0,0,0,339,0,0,0,0,0,0,340,0,0,0,0,0,0,0, + 0,0,0,341,0,0,0,0,342,0,343,0,0,344,0,345,0,0,0,0,0,0,346,347,0,0,0,0,348,0,0,349, + 0,0,0,0,0,350,0,351,0,0,0,0,0,352,353,0,0,0,0,0,354,0,355,0,0,356,357,358,0,359,0,360, + 361,0,0,0,362,0,0,0,0,0,363,0,364,365,0,0,0,366,0,367,0,368,0,0,0,369,370,0,0,0,371,372, + 0,0,373,374,0,0,0,0,0,0,0,0,0,0,375,0,376,0,377,0,0,0,0,0,378,0,0,379,380,0,0,0, + 0,381,0,0,0,0,0,0,382,383,0,0,384,385,0,386,0,387,388,0,389,390,391,0,0,0,0,392,0,0,393,0, + 394,0,395,396,0,397,398,0,0,0,0,399,0,0,0,0,0,0,0,0,0,0,400,0,0,401,402,0,0,0,0,403, + 0,0,0,0,0,0,0,404,0,0,405,0,0,406,0,407,408,0,0,0,409,410,0,0,411,0,0,0,412,0,0,0, + 0,0,0,413,414,0,0,0,0,0,415,0,0,416,417,418,0,0,0,419,0,0,0,0,420,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,421,422,0,0,423,0,424,0,0,0,425,0,0,0,0,426,0,0,0,427,0,0,0, + 0,0,428,0,0,0,0,0,0,429,0,0,0,0,430,0,0,431,432,433,0,0,434,0,435,0,0,0,0,0,436,437, + 437,0,438,439,440,0,0,0,0,441,442,443,0,0,0,444,445,0,446,0,0,0,0,0,0,0,0,0,0,0,447,448, + 0,0,449,450,0,0,0,0,0,0,451,0,0,0,0,0,452,453,0,0,0,454,0,0,0,0,0,0,0,0,0,0, + 0,0,455,0,0,0,0,0,456,0,0,0,0,0,0,0,0,0,0,0,457,0,0,0,0,0,0,0,0,458,0,0, + 459,0,460,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,461,0,0,0,0,462,463,0,0,0,0, + 464,0,0,0,465,0,0,0,0,0,466,0,0,0,467,468,0,0,0,469,0,470,0,471,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,472,0,0,473,0,0,0,0,474,0,0,0,0,0,475,0,476,0,0,477,0,0,0, + 0,0,478,479,0,0,480,481,482,0,0,0,0,483,484,485,486,0,0,0,0,0,0,0,0,487,0,488,0,489,0,490, + 0,0,0,491,0,492,0,0,0,0,0,0,0,493,0,0,0,0,0,494,0,0,0,495,496,497,498,499,0,0,0,0, + 0,500,0,0,501,0,0,0,0,0,0,0,0,502,0,0,0,0,0,0,0,0,0,0,503,0,0,0,0,504,0,505, + 0,0,0,506,0,0,0,507,0,508,0,0,0,0,509,510,0,0,0,511,0,512,0,0,0,513,0,514,0,0,0,0, + 0,0,0,0,0,0,0,0,515,516,0,0,0,517,0,0,0,0,0,518,0,0,0,0,0,519,520,0,0,0,0,0, + 0,0,521,522,0,523,524,0,0,0,525,0,526,0,0,0,527,0,528,0,0,529,0,0,530,0,0,0,0,0,0,531, + 0,0,0,0,0,532,0,0,0,0,0,0,0,0,533,534,535,536,0,0,537,0,538,0,0,0,0,539,0,0,0,0, + 540,541,0,0,542,0,0,0,543,0,0,544,0,545,546,0,547,548,0,549,0,0,0,0,0,550,0,0,0,0,0,0, + 551,0,0,0,552,0,0,553,0,0,0,554,555,0,556,0,0,0,0,0,0,0,0,0,0,0,0,0,557,0,0,0, + 0,0,558,559,0,0,0,0,560,0,0,0,0,561,0,0,0,0,0,0,0,0,0,0,0,562,0,563,564,0,565,0, + 566,0,0,567,0,0,0,0,568,569,0,0,0,0,0,0,0,570,0,0,571,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,572,0,0,0,573,0,0,0,574,575,0,0,0,0,0,0,576,0,0,0,0,0,577, + 0,0,0,0,0,578,0,579,0,580,581,582,583,0,0,584,0,585,0,0,0,586,0,0,0,587,0,0,0,588,0,0, + 0,0,0,589,0,0,0,0,590,591,0,0,0,0,0,0,592,0,0,0,0,0,593,0,0,594,0,0,0,595,0,0, + 0,0,0,0,596,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,597,598,0,0,0,0,0,0, + 0,0,0,0,0,0,0,599,0,0,0,0,600,0,0,601,0,0,602,0,0,0,0,603,0,0,604,0,605,606,0,0, + 607,0,608,0,609,610,0,0,0,0,0,611,612,0,0,0,0,0,0,0,613,0,0,614,615,0,0,0,0,0,616,0, + 617,618,0,0,0,0,619,0,620,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,621,0,0,0,0,0,0,622,623,624,0,625,626,0,0,0,627,0,0,0,0,0,0,0,628, + 629,0,0,0,0,0,0,0,630,0,0,0,631,632,633,634,0,635,0,0,0,636,637,0,0,0,0,0,0,0,0,0, + 638,0,0,0,0,0,0,639,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,640,0,641,0,0, + 642,0,0,643,0,0,0,0,0,0,0,0,0,644,0,645,0,646,647,648,0,0,649,650,0,0,0,0,651,0,0,0, + 0,0,0,652,653,0,654,0,0,0,655,0,656,0,0,0,0,0,0,0,0,657,0,658,0,659,0,660,661,662,663,0, + 0,0,0,0,0,0,0,664,0,665,666,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,667,668,669, + 0,0,670,0,0,0,0,0,0,0,0,0,671,0,0,0,0,0,0,0,0,0,0,0,0,672,0,0,0,0,0,673, + 674,0,675,0,0,676,0,677,0,0,678,679,680,681,0,0,0,682,0,0,0,683,0,0,0,0,0,0,684,0,0,0, + 0,685,0,0,0,686,0,0,0,0,0,0,0,687,0,688,689,0,0,0,0,0,0,690,0,0,0,0,691,0,0,0, + 692,0,0,693,0,0,0,0,0,694,0,0,695,0,0,0,0,0,0,0,0,0,0,0,696,697,698,699,700,0,0,701, + 0,0,702,0,0,0,0,0,703,0,0,0,704,0,0,0,705,706,707,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,708,709,0,710,0,711,712,0,0,713,0,714, + 715,0,0,0,0,0,0,716,0,0,0,717,0,0,0,0,718,719,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,720,721,0,0,0,0,0,0,0,0,0,0,722,0,0,723,724,725,0,0,0,0,0, + 0,726,0,727,0,0,0,0,0,0,0,0,0,0,728,0,0,0,0,0,0,0,0,729,0,730,0,0,0,731,732,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,733,734,0,0,0,0,0, + 0,0,0,0,735,736,0,737,0,0,0,0,738,0,0,0,0,0,0,739,0,0,740,0,0,0,0,0,0,0,0,0, + 0,0,0,0,741,0,0,742,0,0,0,0,0,0,743,744,0,745,746,0,0,0,0,0,0,747,0,748,0,0,749,750, + 0,0,751,752,0,0,0,0,0,0,0,0,0,753,0,0,0,0,0,754,0,0,755,0,0,756,757,0,0,0,0,0, + 0,0,0,0,0,0,758,759,0,0,0,0,0,0,760,761,0,0,0,0,0,0,0,0,0,0,762,763,0,0,0,0, + 764,0,0,0,0,0,0,0,0,765,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,766, + 0,0,767,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,768,0,0,0,769,770,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,771,0,0,0,772,0,0,0,0,773,774,775,0,0,0,776,0,777,778,779,0,0,0,780,0,781,0, + 0,0,0,0,782,0,783,0,0,784,785,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,786,787,0,0,788, + 0,789,0,790,0,791,0,792,0,0,0,793,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,794,795,0,796, + 0,0,0,0,0,797,0,0,0,0,0,0,0,0,0,0,0,0,0,0,798,0,0,0,799,0,0,0,0,0,800,801, + 0,0,0,0,0,0,0,0,0,0,0,0,0,802,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,803,0,0,0,0,0,0,0,0,0,0,804,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,805,0,806,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,807, + 0,0,0,0,0,0,0,0,0,0,0,0,808,0,0,0,0,0,0,0,0,0,809,0,0,0,0,0,0,0,0,810, + 0,0,0,811,0,0,0,0,0,0,0,0,0,0,0,812,0,0,813,814,0,0,0,815,0,816,0,0,0,0,0,817, + 818,819,820,0,0,0,0,821,822,0,0,0,0,0,0,0,0,823,0,824,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, @@ -11110,15 +10498,10 @@ static kbts_u16 kbts__UnicodeParentInfo_PageIndices[34815] = { 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,539,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,540,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,541,542,0,0,0,543,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,33,0,544,0,545,0, - 0,0,0,0,0,546,0,0,0,0,0,0,0,23,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,547,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, @@ -11136,10 +10519,8 @@ static kbts_u16 kbts__UnicodeParentInfo_PageIndices[34815] = { 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,548,549,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,550,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, @@ -11164,7 +10545,6 @@ static kbts_u16 kbts__UnicodeParentInfo_PageIndices[34815] = { 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,551,0,0,552,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, @@ -11176,48 +10556,11 @@ static kbts_u16 kbts__UnicodeParentInfo_PageIndices[34815] = { 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,553,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,554,555,556,0,0,0,0,0,0,557,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 558,0,0,0,0,0,559,0,0,0,0,0,0,0,0,0,0,560,0,0,0,0,0,0,0,0,0,561,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,562,0,0,0,0,0,0,0,0,0,0,0,0,0,563,0,564,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,565,0,0,0,0,0,0,0,0,0,566,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,567,0,0,0,0,0,0,568,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,569,0,0,0,0,0,0,0,0,0,0,0,0,570,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,571,0,0,0,0,0,0,0,0,0, - 0,0,572,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,573,0,0,0,0,0,0,574, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 575,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,576,0,0,0,0,577,0,578,0,579,0, - 0,0,0,580,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,581,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,582,0,0,0,0,0,0,0,0,0,0,0,0,0,0,583,0,0,584,0,0,0,0,0,0,0,0, - 0,0,0,0,0,585,0,0,586,0,0,0,0,0,0,0,0,0,0,0,0,0,587,0,0,0,588,0,589,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,590,0,0,0,591,0,0,0,0,0,592,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,593,0,0,0,0,0,0,0,0,594,0,0,0,0,0,0, - 595,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,596,0,0,597,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,598,0,0, - 0,0,599,0,0,0,0,600,601,602,0,0,0,0,0,0,0,0,603,0,0,0,0,0,0,0,0,0,0,0,0,0, - 604,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,605,0,0,606,0,607,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,608,0,0,0,0,0,0,0,0,0,609,0,0,0,0,0,0,0,610,0,0, - 0,0,0,0,611,0,612,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,613,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,614,0,0,615,616,0,0,0,617,0,0,618,0,0,0,0,0,0, - 0,0,0,0,0,0,619,0,0,620,0,0,0,621,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,622,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,623,0,0,0,0,0,0, - 0,624,0,0,0,0,625,0,0,0,0,626,0,0,0,0,0,0,0,0,0,0,0,0,0,627,0,0,0,628,0,0, - 0,0,0,0,0,0,629,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,630,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,631,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,632,0,0,0,0,0,633,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,634,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,635,0,0,636,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,637,638,0,0,0,0,0,0,0,0,0,639,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,640, - 0,0,0,0,0,0,0,0,0,0,0,641,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,642,0,0,0,643,0,644,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 645,0,0,0,646,0,0,0,0,0,0,0,0,647,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,648,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,649,0,650,0,0,0,0,0,0,0,651,0,0,0,652,0,0,0,0,0,0,0,653,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,654,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, @@ -11235,6 +10578,7 @@ static kbts_u16 kbts__UnicodeParentInfo_PageIndices[34815] = { 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,825,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, @@ -11244,6 +10588,7 @@ static kbts_u16 kbts__UnicodeParentInfo_PageIndices[34815] = { 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,826,124,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, @@ -11254,10 +10599,16 @@ static kbts_u16 kbts__UnicodeParentInfo_PageIndices[34815] = { 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,827,828,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,829,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,75,0,0,0,0,0,0,0,830,831,112,0,0,0,0,0,832,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,833,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,59,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,834,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, @@ -11329,6 +10680,7 @@ static kbts_u16 kbts__UnicodeParentInfo_PageIndices[34815] = { 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,835,836,837,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, @@ -11340,6 +10692,7 @@ static kbts_u16 kbts__UnicodeParentInfo_PageIndices[34815] = { 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,838,67,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, @@ -11439,6 +10792,7 @@ static kbts_u16 kbts__UnicodeParentInfo_PageIndices[34815] = { 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,839,840,0,0,0,0,0,0,0,0,0,0,0,841,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, @@ -11486,11 +10840,17 @@ static kbts_u16 kbts__UnicodeParentInfo_PageIndices[34815] = { 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,842,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,843,844,0,0,0,0,845,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,846,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 847,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,848,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,849,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,850,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, @@ -11499,1468 +10859,539 @@ static kbts_u16 kbts__UnicodeParentInfo_PageIndices[34815] = { 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,851,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,852,0,0,0,0,0,0,0,853,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,854,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,855,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,856,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,857,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,858,0,0,859,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,860,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,861,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,862,863,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,864,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,865,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,866,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,867,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,868,0,0,0,0,0,0,869,870,0,0,0,0,871,0,872,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,873,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,874,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,875,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,876,0,0,0,0,0,0,0,0,0,0,0,0,0,877, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,878,0,0,0,0,0,0,0,0, + 0,0,0,879,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,880,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,881,0,0,0,0,0,882,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,883,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,884,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,885,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,886,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,887,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,888,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,889,0,0,0,0,0,0,0,0,0,0,0,890,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,891,0,0,892,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,893,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,894,0, + 0,0,0,895,0,0,896,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,897,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,898,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,899,0,0,0,0,0,0,0,0,0,0,0,0,0,0,900,0,0,0,0,901,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,902,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,903,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,904,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,905,0,0,0,0,0,0,0,0,0,906,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,907,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,908,0,0,0,0, + 0,0,0,0,0,909,0,0,910,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,911,0,0,0,0, + 0,0,0,0,0,0,0,912,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,913,0,0,0,0, + 0,0,0,0,914,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,915,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,916,0,917,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,918,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,919,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,920,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,921,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,922,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,923,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,924,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,925,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,926,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,927,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,928,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,929,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,930,0,0,0,0,0, + 0,0,0,0,0,931,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,675,0,0,932,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,933,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,934, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,935,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,936,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,937,0,0,0,0, + 0,0,0,938,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,939,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,940,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,941,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,942,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,943,0,0,0,0,0,0, + 944,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,945,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,946,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,947,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 948, }; -static kbts_u32 kbts__UnicodeParentInfo_Data[20960] = { - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66400,66423,66421,66422,0, - 0,1048664,197177,328094,393598,1114167,66418,459018,459046,983176,65810,393556,393562,197183,590017,1048696,131720,0,524539,459032,459067,1245203,131728,393604,131730,590026,393592,0,0,0,0,0, - 66420,1048648,197171,328084,393586,1114150,66417,459011,524515,917655,131704,328109,393538,197180,590008,1048680,131414,0,524499,459025,524523,1245184,131724,459060,131726,655525,393580,0,0,0,0,0, - 0,0,0,0,0,0,0,0,197174,0,0,0,0,0,0,0,0,0,0,0,66419,0,0,66399,0,0,0,0,0,0,0,0, +static kbts_u32 kbts__UnicodeParentInfo_Data[7592] = { + 0,0,0,0,0,0,0,0,0,0,0,66400,66423,66421,66422,0,0,1048664,197177,328094,393598,1114167,66418,459018,459046,983176,65810,393556,393562,197183,590017,1048696, + 131720,0,524539,459032,459067,1245203,131728,393604,131730,590026,393592,0,0,0,0,0,66420,1048648,197171,328084,393586,1114150,66417,459011,524515,917655,131704,328109,393538,197180,590008,1048680, + 131414,0,524499,459025,524523,1245184,131724,459060,131726,655525,393580,0,0,0,0,0,197174,0,0,0,0,0,0,0,0,0,0,0,66419,0,0,66399, 0,0,262650,0,66396,131722,131706,65959,0,0,262654,0,0,0,0,66416,0,0,0,0,262658,197168,66398,0,66397,0,0,0,262626,0,0,0, 0,0,262638,0,66019,66395,131702,66415,0,0,262642,0,0,0,0,65963,0,0,0,0,262646,197165,66186,0,65816,0,0,0,262622,0,0,0, - 0,0,262634,262634,0,0,0,0,0,0,0,0,0,0,0,0,0,0,131716,131716,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,131718,131718,0,0,0,0,0,0,0,0,0,0,0,0,66412,66412,0,0,0,0, - 66183,66183,0,0,0,0,0,0,66413,66413,66413,66413,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66414, - 328104,328104,0,0,0,0,0,0,0,0,0,0,0,0,0,328099,328099,0,0,0,0,0,0,66381,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,65936,65936,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,66367,66367,66405,66405,0,0,0,0,65936,65936,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66366,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66393,0,0,0,0,0,0, - 65973,65973,0,0,0,0,0,0,66384,0,0,0,0,0,0,0,0,0,0,66379,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,66409,65949,0,65829,66406,65873,0,66411,0,66408,66410,65867,459053,0,0,0,262618,0,328089,0,459039,0,0,0,0,0,262610, - 0,66407,0,0,0,393544,0,0,0,393550,0,0,131708,66403,131710,66404,65948,524531,0,0,0,262606,0,393568,0,589999,0,0,0,0,0,262630, - 0,131714,0,0,0,524507,0,0,0,393574,197129,197132,66401,66402,131712,0,0,0,131471,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,65935,0,0,0,0,0,0,0,0,0,131227,0,0,66371,0,197150,131700,65953,262594,0,65945,0,0,0,66394,0, + 0,0,262634,262634,0,0,0,0,0,0,131716,131716,0,0,0,0,0,0,0,0,131718,131718,0,0,0,0,66412,66412,0,0,0,0, + 66183,66183,0,0,0,0,0,0,66413,66413,66413,66413,0,0,0,0,0,0,0,0,0,0,0,66414,328104,328104,0,0,0,0,0,0, + 0,0,0,0,0,0,0,328099,328099,0,0,0,0,0,0,66381,0,0,65936,65936,0,0,0,0,0,0,0,0,0,0,66367,66367, + 66405,66405,0,0,0,0,65936,65936,0,0,66366,0,0,0,0,0,0,66393,0,0,0,0,0,0,65973,65973,0,0,0,0,0,0, + 66384,0,0,0,0,0,0,0,0,0,0,66379,0,0,0,0,0,0,0,0,0,66409,65949,0,65829,66406,65873,0,66411,0,66408,66410, + 65867,459053,0,0,0,262618,0,328089,0,459039,0,0,0,0,0,262610,0,66407,0,0,0,393544,0,0,0,393550,0,0,131708,66403,131710,66404, + 65948,524531,0,0,0,262606,0,393568,0,589999,0,0,0,0,0,262630,0,131714,0,0,0,524507,0,0,0,393574,197129,197132,66401,66402,131712,0, + 0,0,131471,0,0,0,0,0,0,0,0,0,0,0,65935,0,131227,0,0,66371,0,197150,131700,65953,262594,0,65945,0,0,0,66394,0, 0,0,0,262586,0,0,0,65595,0,0,0,65595,0,65630,0,0,131696,0,0,66376,0,197162,131698,65943,262602,0,66377,0,0,0,66392,0, - 0,0,0,262614,0,0,0,65578,0,0,0,65578,0,65614,0,0,0,0,0,0,0,0,65935,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,65936,65936,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,65936,65936,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,65936,65936,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,197216,131836,66134,66134,66134,131828,66134,0,66134,131820,66134,131826,66134,0,66134,0, - 66134,66134,0,66134,131822,0,66134,66134,66134,197204,66134,0,0,0,0,0,0,0,66855,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,197135,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,66370,0,66370,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,65935,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,65935,0,0,65978,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66387,66387,66387,0,0,0,0,66385,0,0,0, - 0,66383,66383,0,0,0,0,0,65935,0,0,66380,0,0,0,66379,0,0,0,65935,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66379,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,131474,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66387,66387,0,0,0,0,66385,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,66380,0,0,0,0,0,0,65935,0,0,0,0,0,65931,0,0,0,0,0,0,0, - 0,66383,66383,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,197147,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,131507,65934,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,65936,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,65935, - 0,0,0,0,0,0,197138,0,0,0,65935,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,197141,0,0,65935,0,0,0, - 66378,0,65935,0,0,0,0,0,0,0,0,0,65935,0,0,0,0,65935,0,0,0,0,65935,0,0,0,0,65935,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,197153,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66378,0,65935,0,0,0,0,0,0,0,0,0,65935,0,0,0, - 0,65935,0,0,0,0,65935,0,0,0,0,65935,0,0,0,0,0,0,66368,66369,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,65935,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,328074,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,65935,0,65935,0,65935,0,65935,0,65935,0,0,0,65935,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,65935,0,65935,0,65936,65936, - 0,0,65935,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,65936,65936,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,65936,65936,0,0,0,0, - 0,0,65755,65755,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 131694,131694,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66087,66087,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,66158,66158,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 262598,262598,65609,65609,65609,65609,65609,65609,262598,262598,65609,65609,65609,65609,65609,65609,131506,131506,0,0,0,0,0,0,131506,131506,0,0,0,0,0,0, - 262590,262590,65985,65985,65985,65985,65985,65985,262590,262590,65985,65985,65985,65985,65985,65985,197042,197042,0,0,0,0,0,0,197042,197042,0,0,0,0,0,0, - 131506,131506,0,0,0,0,0,0,131506,131506,0,0,0,0,0,0,197042,197042,0,0,0,0,0,0,0,197042,0,0,0,0,0,0, - 262578,262578,65973,65973,65973,65973,65973,65973,262578,262578,65973,65973,65973,65973,65973,65973,66386,0,0,0,66388,0,0,0,0,0,0,0,66391,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,65935,0,0,0,0,0,0,0,0,197159, - 0,0,0,0,0,0,65935,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,65935,0,0,0,0,0,0,0,197126,0, - 0,0,65931,65931,0,0,0,0,0,0,0,0,0,0,0,0,65935,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66374,0,66373,0,66375,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,65930,0,65930,0,66372,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,65935,0,0,0,0,65935,0,0,65935,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,65935,0,65935,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,65894,0,0,0, - 0,0,0,65935,0,65936,0,0,65935,0,0,0,0,66376,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,65935,0,0,66158,66158,0,0,0,0,0,0,0,0,0,0,0,0,65936,65936,0,0,65936,65936,0,0,65755,65755,66390,66390,0,0, - 0,0,65936,65936,0,0,65936,65936,0,0,0,0,0,0,0,0,0,66389,66389,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,66374,0,0,0,0,0,65894,65894,0,65934,0,0,0,0,0,0,66382,66382,66382,66382,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,65932,0,0, - 0,0,0,0,0,0,0,0,66365,66365,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,66388,0,0,0,0,65935,0,65935,0,65935,0,65935,0,65935,0,65935,0,65935,0,65935,0,65935,0,65935,0,65935, + 0,0,0,262614,0,0,0,65578,0,0,0,65578,0,65614,0,0,0,0,0,0,65936,65936,0,0,65936,65936,0,0,0,0,0,0, + 197216,131836,66134,66134,66134,131828,66134,0,66134,131820,66134,131826,66134,0,66134,0,66134,66134,0,66134,131822,0,66134,66134,66134,197204,66134,0,0,0,0,0, + 0,0,66855,0,0,0,0,0,0,0,0,0,0,0,0,197135,66370,0,66370,0,0,0,0,0,0,65935,0,0,0,0,0,0, + 0,0,65935,0,0,65978,0,0,0,0,0,0,0,66387,66387,66387,0,0,0,0,66385,0,0,0,0,66383,66383,0,0,0,0,0, + 65935,0,0,66380,0,0,0,66379,0,0,0,65935,0,0,0,0,0,0,0,0,0,0,0,66379,0,0,0,0,0,0,0,131474, + 0,0,0,0,0,0,66387,66387,0,0,0,66380,0,0,0,0,0,0,65935,0,0,0,0,0,65931,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,197147,0,0,0,0,0,0,131507,65934,0,0,0,0,0,0,65936,0,0,0,0,0,0,0,0,65935, + 0,0,0,0,0,0,197138,0,0,197141,0,0,65935,0,0,0,66378,0,65935,0,0,0,0,0,0,0,0,0,65935,0,0,0, + 0,65935,0,0,0,0,65935,0,0,197153,0,0,0,0,0,0,0,0,66368,66369,0,0,0,0,0,0,0,0,0,65935,0,0, + 0,328074,0,0,0,0,0,0,0,0,0,0,0,65935,0,65935,0,65935,0,65935,0,65935,0,0,0,0,65935,0,65935,0,65936,65936, + 0,0,0,0,0,0,65936,65936,0,0,65755,65755,0,0,0,0,131694,131694,0,0,0,0,0,0,66087,66087,0,0,0,0,0,0, + 0,0,0,0,66158,66158,0,0,262598,262598,65609,65609,65609,65609,65609,65609,131506,131506,0,0,0,0,0,0,262590,262590,65985,65985,65985,65985,65985,65985, + 197042,197042,0,0,0,0,0,0,0,197042,0,0,0,0,0,0,262578,262578,65973,65973,65973,65973,65973,65973,66386,0,0,0,66388,0,0,0, + 0,0,0,0,66391,0,0,0,0,0,0,0,0,0,0,197159,0,0,0,0,0,0,197126,0,0,0,65931,65931,0,0,0,0, + 65935,0,0,0,0,0,0,0,66374,0,66373,0,66375,0,0,0,65930,0,65930,0,66372,0,0,0,65935,0,0,65935,0,0,0,0, + 0,0,0,65935,0,65935,0,0,0,0,0,0,65894,0,0,0,0,0,0,65935,0,65936,0,0,65935,0,0,0,0,66376,0,0, + 0,65935,0,0,66158,66158,0,0,0,0,65936,65936,0,0,65936,65936,0,0,65755,65755,66390,66390,0,0,0,66389,66389,0,0,0,0,0, + 0,0,66374,0,0,0,0,0,65894,65894,0,65934,0,0,0,0,0,0,66382,66382,66382,66382,0,0,0,0,0,0,0,65932,0,0, + 66365,66365,0,0,0,0,0,0,0,0,0,0,0,0,66388,0,0,0,0,65935,0,65935,0,65935,0,65935,0,65935,0,65935,0,65935, 0,65935,0,0,65935,0,65935,0,65935,0,0,0,0,0,0,131471,0,0,131471,0,0,131471,0,0,131471,0,0,131471,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,65935,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,65756,65756,65756,65756,0,0,0,0,0,0,0,0,0,0,65935,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,67213,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,67212,0,67214,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,67211, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,67210,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,67209,0,0,0,0,0,0,0,0,0,0,0,0,0,67208,0,0,0, - 0,67207,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,67206,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,67205,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,67204,0,0,0, - 0,0,0,0,0,0,0,67203,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,67202,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,67201,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,67200,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,67199,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,67198,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,67197,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,67196,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,67195,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,67194,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,131892,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,67193,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,67192,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,67191,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,67190,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,67189,0,0,0,0,0,0,0,0,0,0,0,131890,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,67188,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,67187,0,0,0, - 0,0,0,0,0,0,0,0,67186,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66821,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,131888,0,0,0,0,0,0, - 0,0,0,0,0,0,67185,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,67184,0,0,0,0,0,0,0,0,0, - 0,0,0,67183,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,67182,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,67181,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,67180,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 67179,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,67178,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,67177,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,67176,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,67175,0,0,0,67172,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,67174,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,67173,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,67171,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,67170,0,0, - 0,67169,0,0,0,0,0,0,0,0,0,67168,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,67167,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,67166,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,67165,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,67164,0, - 0,0,0,0,0,0,0,67163,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,67162,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,67161,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,67160,0,0,0,0,0,0,0,67159,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,67158,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,67157,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,67156,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,67155,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,67154,0,0,0,0,0,0,0,0,0,0,67153,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,67152,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,66809,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,66810,0,0,0,0,0,0,0,0,0,0,0,66804,0,0,0,0,0,67151,66806,0,0,0,67150,0,0, - 0,67149,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,66801,0,0,0,66807,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,66803,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 66808,0,0,0,0,0,0,0,0,0,0,0,67148,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,66802,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 67147,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 66805,0,0,0,0,0,66798,0,0,0,0,66800,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,131886,0,0,0,0,0,0,0,0,0,0,0,0,67146,0,0,0,66797, - 0,0,67145,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,66799,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,67144,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,67143,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,67140,0,0,0,0,0,0,0,0,0,0,66790,0,0,0,0,0, - 0,0,0,0,0,0,0,131882,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 66789,0,0,0,0,66794,0,0,0,0,0,0,0,131878,0,0,0,0,0,0,67138,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,67136,0,0,67137,66793,66782,0,0,0,66788,0,0,0,0,0,0,0,0,0,67135,0,0,0,0,0,0,0,0, - 66791,0,0,0,0,0,0,0,0,0,0,0,0,67134,0,0,0,0,67142,0,0,67141,0,67133,0,0,0,0,0,0,0,0, - 0,0,0,0,67132,0,0,0,0,0,0,0,67131,0,0,0,0,0,0,0,0,131876,0,66775,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,66779,0,0,66778,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66776,0,66785,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,67130,0,0,0,0,0,0,0,0,0,0, - 0,0,0,67129,0,0,0,131874,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66777,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,66780,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66781,67128,0,0,0,0, - 0,0,0,0,0,0,67127,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,67126,0,0,0,0,67125,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,66774,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66772,0,0,0,0, - 0,0,0,66773,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,67139,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,131872,0,131870,0,0,0,0,0,0,0,0,66769,0,0,0,0,0,0,0,0,0,0,0,66768,0, - 0,0,0,0,131866,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66770,0,0,0,0,131864,0,0,0,0,0, - 0,0,0,0,0,67124,67124,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,131862,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66771, - 0,0,0,0,0,0,0,0,0,67123,0,0,0,0,0,0,0,131860,0,0,0,0,0,0,0,0,67122,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,67121,0,66764,0,0,0,0,0,0,0,66154,0,197225, - 0,0,0,66765,0,0,0,0,0,0,67120,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,67119, - 0,0,0,0,0,66760,0,0,0,0,0,67118,0,0,0,0,0,67117,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,67116,0,0,0,0,0,0,0,0,66766,0,0,0,0,0,0,0,0,0,0,0,0,0,66767,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,67115,0,0,0,0,0,0,0, - 0,0,66763,0,0,0,0,0,67114,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,67113,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,67112,0, - 0,0,67112,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66318,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,67111,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,67110,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,67109,0,66761,0,0,0,0,0,0,0,0,0,0, - 0,0,0,67108,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,131884,0,0,66757,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,131854,0,0,0,131800,0,0, - 0,0,0,0,0,0,0,0,0,0,0,67107,0,0,0,0,0,0,0,67106,0,0,0,0,0,0,0,0,0,0,0,0, - 66759,0,67105,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,66264,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,131852,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,67104,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,66758,0,0,0,0,0,0,0,0,0,0,0,67103,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66754,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,67101,67102,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,67100,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,67099,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,67098,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,67097,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,67096,67095,0,0,0,0,0,0,0,0,0,0,0,0,0, - 66752,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,131798,0,0,0,66747,0, - 0,0,0,0,0,0,0,0,66750,0,0,0,67094,0,0,0,0,0,0,66751,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66746,0,0,0,0,0,0,66745, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,67091,0,0,0,67093,0,0,0,0,67092,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,67090,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,67089,0,0,0,0,0, - 0,0,67088,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,66749,0,0,0,66741,0,0,0,0,0,0,0,0,66740,0,0,66748,0,0,0,0,0,0,0,0,0,0,0, - 0,0,67087,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66743,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,67085,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,67084,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,67083,0,0,0,0, - 0,0,0,0,0,0,0,67082,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,66744,0,0,0,67081,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,67086,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,67080,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,66739,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,131880,0, - 0,0,0,0,0,66738,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,67079,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,67078,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,197222,0,0,0,0,0,0,66314,0,0,0,0,67077,0,0,0,0,0,0,0,0,0,0,0,67076, - 0,0,0,0,0,0,67075,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,67074,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66735, - 67072,0,66733,0,66737,66734,0,0,0,0,0,0,0,0,131850,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,67071,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 67070,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66731,0,0,0,0,0,0, - 0,0,0,67069,0,0,0,0,0,0,0,0,0,0,0,0,66729,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,67068,0,0,67067,0,0,0,0,0,0,0,0,0,0,0,0,0,67066,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,67073,0,0,0,0,0,0,0,66730,0,0,0,0,0, - 0,67065,67065,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,67064,0,0, - 0,0,0,0,0,0,0,0,67063,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,67062,0,0, - 0,0,0,0,0,0,0,0,0,67061,0,0,0,0,0,0,0,0,0,0,66723,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,66726,0,0,0,0,0,0,0,0,0,67060,0,0,67059,0,0,67058,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,66722,131848,0,0,0,0,0,0,0,66728,66725,0,0,0,0,0,66727,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,66720,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,67057,0, - 0,0,0,0,66718,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,131868,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,67054,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,67056,0,0,0,0,0,0,66724,0,67055,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,66719,0,0,0,0,0,0,0,0,0,0,0,0,0,0,67053,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,66715,0,0,0,66721,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,67052,0,0,0,0,0,0,0,0,0,67051,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66714,0,0,0,67050,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66711,0,0,0,0,0,0,0,0,0,66713,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66717,0,0,0,0,0,0,0,0,0,0, - 0,67049,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,131846,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,67048,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66714,0,0,0,0,0,0,0, - 0,66709,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,66712,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,66707,0,0,0,67047,0,0,0,131858,0,131844,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 66710,0,0,0,0,0,0,0,66708,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,67046,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,197219,0,66705,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,67045,0,0,0,0,0,0,0,0,0,0,67044,0,0,67043,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,67042,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,197213,0,0,0,131842,0,0,0,0,0,0,0,0,0, - 66703,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,67041,0,0,0,0,0,0,0,0,0,0,67040,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,66704,0,0,0,0,0,66706,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,67039,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,67038,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,66693,0,0,0,0,0,66696,0,0,0,66701,67037,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,67036,0,66695,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,67035,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,67034,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,67033,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66690,0,0,0,0, - 0,0,0,67032,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 66689,0,0,0,0,0,0,0,0,67031,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,66699,67030,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,67029,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66694,0,0,0, - 0,0,67028,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66692,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,67027,0,0,0,0,0,0,0,0,0,0,0,0,0,67026,0,0,0,0,0,0,67025,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66687,0,0,67024,0,0, - 0,0,0,0,66685,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,131840,0,0,0,0,0,0,66688,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,67023,0,0,0,0,0,0,0,0,0,0,0,66681,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66682,0,0,0,0,0,0, - 0,0,0,0,0,66678,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,66684,67022,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66679,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,67021,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,131790,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,66675,0,0,0,0,0,0,0,0,131834,0,0,0,0,0,0,0,0,0,0,67020,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66677,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,66674,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66670,0,0,0,67019,0,0,0,0,0,0,0, - 67019,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,197210,0,0,0,131832,0,0,0,0, - 0,67018,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,66673,0,0,0,0,67017,0,0,66676,0,0,0,0,0,0,0,67016,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66671,0,0,0,0, - 0,0,0,0,0,67015,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66672,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66668,0,0,0,0,0,0,67014,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66666,0,0,0,0,0,0,0,67013, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,67012,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,66664,0,0,0,131830,0,0,0,0,0,0,0,0,67011,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,66665,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,67010,0,0,0,0,0,0,0,0,0,0,0, - 0,0,67009,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,67008,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,67007,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,197201,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66663,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,67006,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66661,0,0,0,0,0,0,0,67005,0,0,0,0, - 0,0,0,0,66659,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,67004,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,67003,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,67002,0,0,0,0,66658,0,66662,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,67001, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,66657,0,0,0,0,0,0,0,0,0,0,0,197198,66128,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,67000,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,66999,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,66654,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66998, - 0,0,0,0,0,0,0,0,0,0,0,0,66651,66997,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,66655,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66996,0,0,0,0,66647,0,0,66653,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66995,0,0,0,0,0,0,0,0,0,66994,0, - 0,197195,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,66993,66645,0,0,0,0,0,0,0,0,0,0,0,0,131824,66992,0,0,0,0,0,0,0, - 0,0,0,0,0,66991,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,66648,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66643,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,66646,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66990,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66649,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66989,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,66988,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66641,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66642,0,0,0,0,0, + 0,0,0,0,0,0,0,65756,65756,65756,65756,0,0,0,0,0,0,0,0,0,0,0,67213,0,0,67212,0,67214,0,0,0,0, + 0,0,0,0,0,0,0,67211,0,0,0,0,0,67210,0,0,0,0,0,0,0,0,67209,0,0,0,0,0,67208,0,0,0, + 0,67207,0,0,0,0,0,0,0,0,0,0,0,0,0,67206,0,0,67205,0,0,0,0,0,0,0,0,0,67204,0,0,0, + 0,0,0,0,0,0,0,67203,0,0,0,67202,0,0,0,0,0,0,0,0,67201,0,0,0,0,0,67200,0,0,0,0,0, + 0,0,0,0,0,0,67199,0,0,0,0,0,67198,0,0,0,0,0,0,0,67197,0,0,0,67196,0,0,0,0,0,0,0, + 0,67195,0,0,0,0,0,0,0,67194,0,0,0,0,0,0,0,0,0,0,0,131892,0,0,67193,0,0,0,0,0,0,0, + 0,0,0,0,0,0,67192,0,0,0,0,67191,0,0,0,0,0,0,0,0,0,0,67190,0,0,0,0,0,67189,0,0,0, + 131890,0,0,0,0,0,0,0,0,0,0,67188,0,0,0,0,0,0,0,0,67187,0,0,0,67186,0,0,0,0,0,0,0, + 66821,0,0,0,0,0,0,0,0,131888,0,0,0,0,0,0,0,0,0,0,0,0,67185,0,0,0,0,0,0,0,67184,0, + 0,0,0,67183,0,0,0,0,0,0,0,0,0,0,0,67182,0,0,67181,0,0,0,0,0,0,0,0,0,0,0,0,67180, + 67179,0,0,0,0,0,0,0,0,67178,0,0,0,0,0,0,0,0,0,0,67177,0,0,0,0,67176,0,0,0,0,0,0, + 0,0,0,0,0,67175,0,0,0,67172,0,0,0,0,0,0,0,0,0,67174,0,0,0,0,0,0,0,67173,0,0,0,0, + 0,0,0,67171,0,0,0,0,0,0,0,0,0,67170,0,0,0,67169,0,0,0,0,0,0,0,0,0,67168,0,0,0,0, + 0,0,0,0,0,0,0,67167,0,67166,0,0,0,0,0,0,0,0,0,0,0,67165,0,0,0,0,0,0,0,0,67164,0, + 0,0,0,0,0,0,0,67163,0,0,0,0,0,67162,0,0,0,0,0,0,0,0,67161,0,0,0,0,0,0,0,67160,0, + 0,0,0,0,0,0,67159,0,0,0,67158,0,0,0,0,0,0,0,0,67157,0,0,0,0,0,0,0,0,0,0,67156,0, + 0,0,0,0,0,0,67155,0,0,0,0,0,0,67154,0,0,67153,0,0,0,0,0,0,0,0,0,0,0,0,0,67152,0, + 0,0,0,0,0,66809,0,0,0,0,0,0,0,0,66810,0,0,0,66804,0,0,0,0,0,67151,66806,0,0,0,67150,0,0, + 0,67149,0,0,0,0,0,0,0,0,66801,0,0,0,66807,0,0,0,0,0,0,0,66803,0,66808,0,0,0,0,0,0,0, + 0,0,0,0,67148,0,0,0,0,0,0,0,66802,0,0,0,67147,0,0,0,0,0,0,0,66805,0,0,0,0,0,66798,0, + 0,0,0,66800,0,0,0,0,0,0,0,0,0,0,131886,0,0,0,0,67146,0,0,0,66797,0,0,67145,0,0,0,0,0, + 0,0,0,66799,0,0,0,0,0,0,67144,0,0,0,0,0,0,67143,0,0,0,0,0,0,0,0,0,0,0,0,0,67140, + 0,0,66790,0,0,0,0,0,0,0,0,0,0,0,0,131882,66789,0,0,0,0,66794,0,0,0,0,0,0,0,131878,0,0, + 0,0,0,0,67138,0,0,0,0,0,0,0,67136,0,0,67137,66793,66782,0,0,0,66788,0,0,0,0,0,0,0,0,0,67135, + 66791,0,0,0,0,0,0,0,0,0,0,0,0,67134,0,0,0,0,67142,0,0,67141,0,67133,0,0,0,0,67132,0,0,0, + 0,0,0,0,67131,0,0,0,0,0,0,0,0,131876,0,66775,0,66779,0,0,66778,0,0,0,0,0,0,0,66776,0,66785,0, + 0,0,0,0,0,67130,0,0,0,0,0,67129,0,0,0,131874,0,0,0,0,0,0,0,66777,0,66780,0,0,0,0,0,0, + 0,0,66781,67128,0,0,0,0,0,0,0,0,0,0,67127,0,0,0,67126,0,0,0,0,67125,0,66774,0,0,0,0,0,0, + 0,0,0,66772,0,0,0,0,0,0,0,66773,0,0,0,0,0,0,0,67139,0,0,0,0,0,0,0,0,0,0,0,131872, + 0,131870,0,0,0,0,0,0,0,0,66769,0,0,0,0,0,0,0,0,0,0,0,66768,0,0,0,0,0,131866,0,0,0, + 0,0,0,0,0,66770,0,0,0,0,131864,0,0,0,0,0,0,0,0,0,0,67124,67124,0,0,0,0,0,0,0,0,131862, + 0,0,0,0,0,0,0,66771,0,67123,0,0,0,0,0,0,0,131860,0,0,0,0,0,0,0,0,67122,0,0,0,0,0, + 0,0,0,67121,0,66764,0,0,0,0,0,0,0,66154,0,197225,0,0,0,66765,0,0,0,0,0,0,67120,0,0,0,0,0, + 0,0,0,0,0,0,0,67119,0,0,0,0,0,66760,0,0,0,0,0,67118,0,0,0,0,0,67117,0,0,0,0,0,0, + 0,0,0,0,0,0,67116,0,0,0,0,0,0,0,0,66766,0,0,0,0,0,66767,0,0,67115,0,0,0,0,0,0,0, + 0,0,66763,0,0,0,0,0,67114,0,0,0,0,0,0,0,67113,0,0,0,0,0,0,0,0,0,0,0,0,0,67112,0, + 0,0,67112,0,0,0,0,0,0,0,0,0,0,66318,0,0,0,0,0,0,0,0,67111,0,67110,0,0,0,0,0,0,0, + 0,0,0,67109,0,66761,0,0,0,0,0,67108,0,0,0,0,0,0,0,0,131884,0,0,66757,0,131854,0,0,0,131800,0,0, + 0,0,0,67107,0,0,0,0,0,0,0,67106,0,0,0,0,66759,0,67105,0,0,0,0,0,0,0,66264,0,0,0,0,0, + 0,0,0,0,0,0,131852,0,0,67104,0,0,0,0,0,0,66758,0,0,0,0,0,0,0,0,0,0,0,67103,0,0,0, + 0,66754,0,0,0,0,0,0,0,0,0,0,0,0,67101,67102,0,0,0,67100,0,0,0,0,0,0,0,0,0,0,67099,0, + 0,0,0,0,67098,0,0,0,0,0,0,0,0,67097,0,0,0,67096,67095,0,0,0,0,0,66752,0,0,0,0,0,0,0, + 0,0,131798,0,0,0,66747,0,66750,0,0,0,67094,0,0,0,0,0,0,66751,0,0,0,0,66746,0,0,0,0,0,0,66745, + 0,0,0,0,0,0,67091,0,0,0,67093,0,0,0,0,67092,0,0,0,0,0,0,67090,0,0,0,67089,0,0,0,0,0, + 0,0,67088,0,0,0,0,0,0,0,0,0,66749,0,0,0,66741,0,0,0,0,0,0,0,0,66740,0,0,66748,0,0,0, + 0,0,67087,0,0,0,0,0,0,0,0,66743,0,0,0,0,67085,0,0,0,0,0,0,0,0,0,0,0,67084,0,0,0, + 0,0,0,67083,0,0,0,0,0,0,0,0,0,0,0,67082,0,0,66744,0,0,0,67081,0,0,0,0,0,0,67086,0,0, + 67080,0,0,0,0,0,0,0,66739,0,0,0,0,0,0,0,0,0,0,0,0,0,131880,0,0,0,0,0,0,66738,0,0, + 0,0,0,67079,0,0,0,0,67078,0,0,0,0,0,0,0,0,0,0,0,0,0,0,197222,0,0,0,0,0,0,66314,0, + 0,0,0,67077,0,0,0,0,0,0,0,0,0,0,0,67076,0,0,0,0,0,0,67075,0,0,0,67074,0,0,0,0,0, + 0,0,0,0,0,0,0,66735,67072,0,66733,0,66737,66734,0,0,0,0,0,0,0,0,131850,0,0,0,0,0,0,67071,0,0, + 67070,0,0,0,0,0,0,0,0,66731,0,0,0,0,0,0,0,0,0,67069,0,0,0,0,66729,0,0,0,0,0,0,0, + 0,0,0,67068,0,0,67067,0,0,0,0,0,67066,0,0,0,0,0,67073,0,0,0,0,0,0,0,66730,0,0,0,0,0, + 0,67065,67065,0,0,0,0,0,0,0,0,0,0,67064,0,0,67063,0,0,0,0,0,0,0,0,0,0,0,0,67062,0,0, + 0,67061,0,0,0,0,0,0,0,0,0,0,66723,0,0,0,0,0,0,0,0,0,66726,0,67060,0,0,67059,0,0,67058,0, + 0,66722,131848,0,0,0,0,0,0,0,66728,66725,0,0,0,0,0,66727,0,0,0,0,0,0,0,0,0,0,66720,0,0,0, + 0,0,0,0,0,0,67057,0,0,0,0,0,66718,0,0,0,0,0,131868,0,0,0,0,0,0,0,0,67054,0,0,0,0, + 0,0,67056,0,0,0,0,0,0,66724,0,67055,0,0,0,0,0,0,0,66719,0,0,0,0,0,0,67053,0,0,0,0,0, + 0,66715,0,0,0,66721,0,0,0,0,0,0,0,67052,0,0,0,0,0,0,0,0,0,67051,0,0,0,0,0,66714,0,0, + 0,67050,0,0,0,0,0,0,0,0,66711,0,0,0,0,0,0,0,0,0,66713,0,0,0,0,0,0,0,0,66717,0,0, + 0,67049,0,0,0,0,0,0,0,0,0,0,131846,0,0,0,0,0,0,0,0,0,0,67048,66714,0,0,0,0,0,0,0, + 0,66709,0,0,0,0,0,0,66712,0,0,0,0,0,0,0,0,0,0,0,66707,0,0,0,67047,0,0,0,131858,0,131844,0, + 66710,0,0,0,0,0,0,0,66708,0,0,0,0,0,0,0,0,0,67046,0,0,0,0,0,0,0,0,0,0,0,197219,0, + 66705,0,0,0,0,0,0,0,0,0,0,0,67045,0,0,0,0,0,0,0,0,0,0,67044,0,0,67043,0,0,0,0,0, + 0,0,0,0,0,0,67042,0,0,0,197213,0,0,0,131842,0,66703,0,0,0,0,0,0,0,67041,0,0,0,0,0,0,0, + 0,0,0,67040,0,0,0,0,0,0,0,0,0,0,66704,0,0,0,0,0,66706,0,0,0,0,0,0,0,0,67039,0,0, + 0,67038,0,0,0,0,0,0,0,66693,0,0,0,0,0,66696,0,0,0,66701,67037,0,0,0,0,0,0,0,67036,0,66695,0, + 0,0,0,0,0,67035,0,0,67034,0,0,0,0,0,0,0,67033,0,0,0,0,0,0,0,0,0,0,66690,0,0,0,0, + 0,0,0,67032,0,0,0,0,66689,0,0,0,0,0,0,0,0,67031,0,0,0,0,0,0,0,0,0,0,66699,67030,0,0, + 0,0,0,0,67029,0,0,0,0,0,0,0,66694,0,0,0,0,0,67028,0,0,0,0,0,0,0,66692,0,0,0,0,0, + 0,67027,0,0,0,0,0,0,0,0,0,0,0,0,0,67026,0,0,0,0,0,0,67025,0,0,0,66687,0,0,67024,0,0, + 0,0,0,0,66685,0,0,0,0,0,0,0,0,0,0,131840,0,0,0,0,0,0,66688,0,0,0,0,0,67023,0,0,0, + 66681,0,0,0,0,0,0,0,0,66682,0,0,0,0,0,0,0,0,0,0,0,66678,0,0,0,0,66684,67022,0,0,0,0, + 0,0,0,66679,0,0,0,0,0,67021,0,0,0,0,0,0,0,0,0,0,131790,0,0,0,66675,0,0,0,0,0,0,0, + 0,131834,0,0,0,0,0,0,0,0,0,0,67020,0,0,0,0,0,0,0,66677,0,0,0,0,0,0,0,0,0,66674,0, + 0,0,0,0,66670,0,0,0,67019,0,0,0,0,0,0,0,0,0,0,0,0,0,0,197210,0,0,0,131832,0,0,0,0, + 0,67018,0,0,0,0,0,0,0,0,0,0,0,0,66673,0,0,0,0,67017,0,0,66676,0,0,0,0,0,0,0,67016,0, + 0,0,0,66671,0,0,0,0,0,0,0,0,0,67015,0,0,0,0,0,0,0,0,0,66672,0,0,0,66668,0,0,0,0, + 0,0,67014,0,0,0,0,0,0,0,0,0,0,0,0,66666,0,0,0,0,0,0,0,67013,0,0,67012,0,0,0,0,0, + 0,66664,0,0,0,131830,0,0,0,0,0,0,0,0,67011,0,66665,0,0,0,0,0,0,0,0,0,0,0,67010,0,0,0, + 0,0,67009,0,0,0,0,0,0,0,0,67008,0,0,0,0,0,0,67007,0,0,0,0,0,0,0,197201,0,0,0,0,0, + 0,0,0,66663,0,0,0,0,67006,0,0,0,0,0,0,0,0,0,0,66661,0,0,0,0,0,0,0,67005,0,0,0,0, + 0,0,0,0,66659,0,0,0,0,67004,0,0,0,0,0,0,0,0,0,0,67003,0,0,0,0,0,67002,0,0,0,0,66658, + 0,66662,0,0,0,0,0,0,0,0,0,0,0,0,0,67001,0,0,0,0,0,0,66657,0,0,0,197198,66128,0,0,0,0, + 0,0,0,0,0,0,67000,0,0,0,0,0,0,0,0,66999,66654,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66998, + 0,0,0,0,66651,66997,0,0,0,0,0,0,0,66655,0,0,0,0,0,0,0,0,66996,0,0,0,0,66647,0,0,66653,0, + 0,0,0,0,66995,0,0,0,0,0,0,0,0,0,66994,0,0,197195,0,0,0,0,0,0,0,66993,66645,0,0,0,0,0, + 0,0,0,0,0,0,0,131824,66992,0,0,0,0,0,0,0,0,0,0,0,0,66991,0,0,0,0,0,66648,0,0,0,0, + 0,0,66643,0,0,0,0,0,0,0,66646,0,0,0,0,0,0,66990,0,0,0,0,0,0,0,0,66649,0,0,0,0,0, + 0,0,0,0,0,0,0,66989,0,0,0,0,0,0,66988,0,0,0,0,0,66641,0,0,0,0,0,66642,0,0,0,0,0, 0,0,0,0,0,0,0,66987,0,0,0,131818,0,0,0,0,0,66637,0,0,0,0,0,0,0,0,0,66644,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66638,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,131788,66639,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,66986,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,66985,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,66633,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66635,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66984,0,0,131816,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66983,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,66982,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66977,0,0,0,0,0,0,66981,0,0,0,0,0,66980,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66634,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,66979,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,66631,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66629,0,0,0,0,0,0, - 0,0,0,0,0,66978,0,0,0,66630,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,131780,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66976,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,66627,0,66628,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66624,0,0,0,0,0,0,0,0,0,0,66623,0,0,0,0, - 0,0,0,0,0,0,0,0,66975,0,0,66628,0,0,0,0,0,0,0,0,0,131814,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66974,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,66622,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 66973,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66972,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66626,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 66625,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66620,0,0,0, - 0,0,0,0,0,0,0,0,0,0,131774,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66621,0,0,0,0,66971,0,0,0,0,0, - 0,0,0,0,0,0,0,131772,0,0,0,66970,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,66969,0,0,0,0,0,0,0,0,0,0,0,0,66618,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66610,0, - 0,0,0,0,0,0,66616,0,0,66615,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,66619,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,66968,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66967,0,0,0, - 0,0,0,0,0,0,0,0,0,66611,0,0,0,0,0,0,0,131812,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,66966,0,0,0,66609,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66612,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,66965,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,66614,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,66964,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66613,0,0,66963,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66606,0,0,0,0,0,0, - 0,0,0,0,0,66605,0,0,0,0,0,0,0,0,0,0,131810,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,66604,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66962,0,0,0,0,0,0,0,0,0,0,0,0,66608,0,66607, - 0,0,66602,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,66598,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,131766,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66603,0,0,0,0, - 0,0,0,0,0,0,0,66596,0,0,0,0,0,0,0,0,0,0,0,0,131808,0,0,0,0,0,0,0,0,0,0,0, - 0,66597,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66320,131856, - 66601,0,0,0,0,0,0,0,0,0,131806,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,66961,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,66599,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,66960,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,66594,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,131792,0,0,0,0,66595,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,66583,0,131802,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66582,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,66580,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66587,0,66592,0, - 0,0,0,0,0,0,0,0,66591,66590,0,0,0,0,0,0,66589,0,0,0,0,0,131794,0,0,0,0,0,0,66588,66585,0, - 0,0,0,0,0,66584,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66575, - 0,0,0,0,0,0,0,0,0,0,0,0,0,66586,66586,131786,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,66579,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,66576,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,66959,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66574,0,0,0, - 131784,0,0,0,0,0,0,0,0,0,66958,0,0,0,0,66957,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,66577,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66578,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,66573,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,66956,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 66572,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 131760,0,0,0,0,0,66955,0,0,66954,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66570,0, - 66564,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66571,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66569,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66568,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66953,0,0,0,66566,0,0,0,0,0,0,0,0,0, - 0,0,0,66952,0,0,0,66562,66951,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 66950,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66563,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,66560,0,0,0,0,0,0,0,0,0,0,0,0,66559,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66567,0,0,0,0, - 0,0,0,66949,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 66557,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66556,0, - 0,0,0,0,0,0,0,66948,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,197192,0,0,0,0,0,0,0,0,0,0,0, - 0,0,66947,0,0,0,0,0,0,66561,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66554,0,0,0,0,0,0,0,0, - 0,66558,0,0,0,66946,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66555,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66553,0,0,0,0,0,0,66551,66945,0,0,0,0,0, - 0,0,0,0,0,66548,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66944,0,0,0,0,66549,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66550,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66943,0,0,0,0,0, - 0,66546,0,0,0,197189,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,66547,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 66942,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66545,66941,0,0,0,0,0,0,0,0,0,0,0,0,0,66543,0, - 0,0,0,0,0,0,0,0,0,0,0,66544,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,66937,0,0,0,0,66940,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,66939,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66938,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66538,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,66540,0,0,0,0,66542,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,131838,0,0,66936,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66541,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66537,0,0,0,0,0,0,0,0,0,131756,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,66935,0,0,0,0,0,66934,0,0,0,0,0,0,0,0,0,0,0,66933,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66932,0,66931,0,0,0,0,0,0,0,0,0,66930,0,0, - 0,0,0,0,0,131782,66929,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66928,0,0, - 0,0,0,66927,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66536,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66539,66926,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,66925,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,66924,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66923,0,0, - 0,0,0,0,0,0,0,0,0,66534,66922,0,66921,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66920,0,0,0, - 0,0,0,0,0,0,0,66919,0,0,0,0,0,0,0,66535,0,66533,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66531,0,0, - 0,0,0,0,0,0,0,0,0,66532,0,0,0,0,0,0,0,0,0,0,0,0,0,131778,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,66529,0,0,66918,0,66917,0,0,0,0,0,0,0,0,66530,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66916,0,0,0,0,0,0,0,0,0, - 0,0,0,0,66915,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,66525,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66527,0,0,0,0,0, - 0,0,0,0,0,0,66524,0,0,0,0,0,0,0,0,0,0,0,66528,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,66523,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66522, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66914,0,0,0,0,0,0,0,0,0,0,0,131776,0,0,0, - 0,0,0,0,0,0,0,66913,0,66912,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,66911,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,66910,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,66909,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,66908,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,66907,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,66906,0,0,0,0,0,0,0,0,0,0,0,0,0,131770,0,0,0,0,0,0, - 0,0,0,0,0,0,66905,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66518,0,0,0,0,0, - 0,66904,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,66903,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66514, - 0,0,0,0,0,0,0,0,0,0,0,0,66520,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 66902,0,0,66901,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,66638,0,0,131788,66639,0,0,0,0,0,0,0,0,0,0,66986,0,0,0,0,0,0,0,66985,0, + 0,0,0,66633,0,0,0,0,0,0,0,0,0,0,66635,0,0,0,0,66984,0,0,131816,0,0,66983,0,0,0,0,0,0, + 0,0,66982,0,0,0,0,0,66977,0,0,0,0,0,0,66981,0,0,0,0,0,66980,0,0,0,66634,0,0,0,0,0,0, + 0,0,0,0,0,66979,0,0,66631,0,0,0,0,0,0,0,0,66629,0,0,0,0,0,0,0,0,0,0,0,66978,0,0, + 0,66630,0,0,0,0,0,0,0,0,0,0,0,0,131780,0,0,0,0,0,66976,0,0,0,0,0,0,0,0,0,66627,0, + 66628,0,0,0,0,0,0,0,66624,0,0,0,0,0,0,0,0,0,0,66623,0,0,0,0,66975,0,0,66628,0,0,0,0, + 0,0,0,0,0,131814,0,0,66974,0,0,0,0,0,0,0,0,0,66622,0,0,0,0,0,66973,0,0,0,0,0,0,0, + 0,0,0,0,0,66972,0,0,0,0,0,0,0,0,0,66626,66625,0,0,0,0,0,0,0,0,0,0,0,66620,0,0,0, + 0,0,131774,0,0,0,0,0,0,0,0,0,0,66621,0,0,0,0,66971,0,0,0,0,0,0,0,0,0,0,0,0,131772, + 0,0,0,66970,0,0,0,0,0,0,0,0,0,66969,0,0,0,0,66618,0,0,0,0,0,0,0,0,0,0,0,66610,0, + 0,0,0,0,0,0,66616,0,0,66615,0,0,0,0,0,0,0,0,66619,0,0,0,0,0,0,0,0,0,0,0,0,66968, + 0,0,0,0,66967,0,0,0,0,66611,0,0,0,0,0,0,0,131812,0,0,0,0,0,0,0,0,0,0,0,66966,0,0, + 0,66609,0,0,0,0,0,0,66612,0,0,0,0,0,0,0,0,0,66965,0,0,0,0,0,0,0,0,0,0,0,66614,0, + 0,0,0,0,66964,0,0,0,0,0,0,66613,0,0,66963,0,0,66606,0,0,0,0,0,0,0,0,0,0,0,66605,0,0, + 131810,0,0,0,0,0,0,0,0,0,66604,0,0,0,0,0,66962,0,0,0,0,0,0,0,0,0,0,0,0,66608,0,66607, + 0,0,66602,0,0,0,0,0,0,66598,0,0,0,0,0,0,0,0,131766,0,0,0,0,0,0,0,0,66603,0,0,0,0, + 0,0,0,0,0,0,0,66596,0,0,0,0,131808,0,0,0,0,66597,0,0,0,0,0,0,0,0,0,0,0,0,66320,131856, + 66601,0,0,0,0,0,0,0,0,0,131806,0,0,0,0,0,0,0,0,66961,0,0,0,0,0,0,0,0,0,0,0,66599, + 0,0,0,0,0,0,66960,0,0,0,0,66594,0,0,0,0,0,0,0,0,131792,0,0,0,0,66595,0,0,0,0,0,0, + 0,0,66583,0,131802,0,0,0,0,0,0,66582,0,0,0,0,0,0,66580,0,0,0,0,0,0,0,0,0,66587,0,66592,0, + 66591,66590,0,0,0,0,0,0,66589,0,0,0,0,0,131794,0,0,0,0,0,0,66588,66585,0,0,0,0,0,0,66584,0,0, + 0,0,0,0,0,0,0,66575,0,0,0,0,0,66586,66586,131786,0,0,0,0,0,0,66579,0,0,0,66576,0,0,0,0,0, + 0,0,0,66959,0,0,0,0,0,0,0,0,66574,0,0,0,131784,0,0,0,0,0,0,0,0,0,66958,0,0,0,0,66957, + 0,66577,0,0,0,0,0,0,0,66578,0,0,0,0,0,0,0,0,0,66573,0,0,0,0,0,0,0,0,0,0,66956,0, + 66572,0,0,0,0,0,0,0,131760,0,0,0,0,0,66955,0,0,66954,0,0,0,0,0,0,0,0,0,0,0,0,66570,0, + 66564,0,0,0,0,0,0,0,0,0,0,66571,0,0,0,0,0,0,66569,0,0,0,0,0,0,0,0,0,0,0,66568,0, + 0,0,66953,0,0,0,66566,0,0,0,0,66952,0,0,0,66562,66951,0,0,0,0,0,0,0,66950,0,0,0,0,0,0,0, + 66563,0,0,0,0,0,0,0,0,0,66560,0,0,0,0,0,0,0,0,0,0,0,0,66559,0,0,0,66567,0,0,0,0, + 0,0,0,66949,0,0,0,0,66557,0,0,0,0,0,0,0,0,0,0,0,0,0,66556,0,0,0,0,0,0,0,0,66948, + 0,0,0,0,197192,0,0,0,0,0,66947,0,0,0,0,0,0,66561,0,0,0,0,0,0,0,0,0,0,0,0,0,66554, + 0,66558,0,0,0,66946,0,0,0,0,0,0,0,0,66555,0,0,0,66553,0,0,0,0,0,0,66551,66945,0,0,0,0,0, + 0,0,0,0,0,66548,0,0,0,0,0,0,0,66944,0,0,0,0,66549,0,0,0,0,0,0,0,0,0,0,66550,0,0, + 0,0,66943,0,0,0,0,0,0,66546,0,0,0,197189,0,0,0,0,0,0,0,0,66547,0,66942,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,66545,66941,0,0,0,0,0,0,0,0,0,0,0,0,0,66543,0,0,0,0,66544,0,0,0,0, + 0,0,0,0,0,66937,0,0,0,0,66940,0,0,0,0,0,0,0,0,66939,0,0,0,0,0,0,0,0,0,0,66938,0, + 66538,0,0,0,0,0,0,0,66540,0,0,0,0,66542,0,0,0,131838,0,0,66936,0,0,0,66541,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,66537,0,131756,0,0,0,0,0,0,0,0,0,66935,0,0,0,0,0,66934,0,0,0,0,0,0, + 0,0,0,0,0,66933,0,0,0,66932,0,66931,0,0,0,0,0,0,0,0,0,66930,0,0,0,0,0,0,0,131782,66929,0, + 0,0,0,0,0,66928,0,0,0,0,0,66927,0,0,0,0,0,0,0,0,0,0,66536,0,0,0,66539,66926,0,0,0,0, + 0,0,0,66925,0,0,0,0,0,0,0,0,0,66924,0,0,0,0,0,0,0,66923,0,0,0,66534,66922,0,66921,0,0,0, + 0,0,0,0,66920,0,0,0,0,0,0,0,0,0,0,66919,0,0,0,0,0,0,0,66535,0,66533,0,0,0,0,0,0, + 0,0,0,0,0,66531,0,0,0,66532,0,0,0,0,0,0,0,0,0,0,0,0,0,131778,0,0,0,0,0,0,66529,0, + 0,66918,0,66917,0,0,0,0,0,0,0,0,66530,0,0,0,0,0,0,0,0,0,66916,0,0,0,0,0,66915,0,0,0, + 0,0,0,0,0,66525,0,0,0,0,66527,0,0,0,0,0,0,0,0,0,0,0,66524,0,0,0,66528,0,0,0,0,0, + 0,0,0,0,0,66523,0,0,0,0,0,0,0,0,0,66522,66914,0,0,0,0,0,0,0,0,0,0,0,131776,0,0,0, + 0,0,0,0,0,0,0,66913,0,66912,0,0,0,0,0,0,66911,0,0,0,0,0,0,0,0,66910,0,0,0,0,0,0, + 0,0,66909,0,0,0,0,0,0,0,0,0,0,0,66908,0,66907,0,0,0,0,0,0,0,0,0,0,66906,0,0,0,0, + 0,131770,0,0,0,0,0,0,0,0,0,0,0,0,66905,0,0,0,66518,0,0,0,0,0,0,66904,0,0,0,0,0,0, + 0,66903,0,0,0,0,0,0,0,0,0,0,0,0,0,66514,0,0,0,0,66520,0,0,0,66902,0,0,66901,0,0,0,0, 0,0,66512,0,0,0,0,0,0,0,0,0,0,0,0,66515,0,0,0,0,0,0,0,66900,0,0,0,0,0,0,66899,0, - 0,66513,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66510,0,66898,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66517,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,66519,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,66507,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,66516,0,0,0,0,66511,0,0,0,0,0,0,0,0,0,0,131754,0,0,0,0,0,0,0,0,0, - 66897,0,0,0,0,0,0,0,0,0,131746,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66509, - 0,0,0,0,0,0,0,0,0,0,0,66508,0,0,0,0,0,0,66506,0,0,0,66504,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,131768,0,0,0,0,0,0,0,0,0,0,131750,0,0,0,0,0,131744,0, - 0,131752,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,131748,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66505,0,0,0,0,0,0,0, - 66502,0,0,0,0,0,0,0,0,0,131764,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,66500,0,0,0,0,0,0,0,0,0,0,0,0,66896,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,66895,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,66894,66499,0,0,0,0,0,66497,0,0,0,0,0,0,0,0,0,0,66501,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,131742,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66893,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66892,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66891,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,66890,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66496,66889,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,66490,0,0,0,0,0,0,0,0,0,66888,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,66492,0,0,0,66493,0,0,0,0,0,0,0,0,0,0,0,0,0,131762,0,0,66495,0,0,0,0, - 0,0,66489,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66887,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66488,0,0,0,0,0,66494,0,0,0,0,0,0,0,0,0, - 0,0,0,66487,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,131740,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66491,0,0,0,0,0,0,0,0,0,66485,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66481,0,0,0,0,66886,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,66480,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66486,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66885,0,0,0,0,0,0,0,0,0,0,0,66483,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66884,0,0,0,0,0,0,0,0,0,66883,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,66475,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66484,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66477,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,66478,0,0,66475,0,66473,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66474,0,0,0,66882,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66479,0,0,0,0,0,66881,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66880,66880,0,0,0,0,0,0,0, - 0,0,0,0,66470,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,66472,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66879,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66878,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,66877,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,66465,0,0,0,0,0,0,0,0,0,66876,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,66463,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,66457,0,66464,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66454,0,0,66461,0,0,0,66466,0,0,0, - 0,0,0,0,0,0,66460,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,66459,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66462,66456,0,0,0,0,0,0,0, - 0,0,0,66875,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,66458,131738,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66453,66451,0,0,0,0,0,0,0,0, - 0,0,0,66874,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66450,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,66452,0,0,0,0,0,0,0,0,0,0,0,0,0,131736,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66455,0,0,0,0, - 66873,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,131734, - 0,0,0,0,0,0,0,0,0,0,0,197207,0,0,0,0,0,0,0,0,0,0,0,0,66449,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,66872,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,131732,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66448,0, - 0,0,66871,0,0,0,0,0,0,0,0,0,0,0,0,66447,0,0,0,0,0,0,0,0,0,0,0,0,66446,0,0,0, - 0,0,0,0,0,0,0,0,66445,66870,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,66869,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,66868,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66444,0,0,0,0,0,0,0,0,0,0,0,0,66867,0, - 0,0,0,0,0,0,0,0,0,0,66443,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,131758,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66441,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 66866,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66440,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66865,0,0, - 0,0,0,0,0,0,0,66864,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66439,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66435,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66434,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66432, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66433,0,0,0,0,0,0,0,66437, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66863,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,66431,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66862,0,0,0,0,66861,0, - 0,0,0,0,0,66860,0,0,0,0,0,0,0,0,0,66859,0,0,0,0,0,0,66858,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66857,0,0,0,0, - 0,0,0,66438,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,66430,66436,0,0,0,0,0,0,0,0,0,0,0,0,0,197186,0,0,0, - 0,0,0,0,0,0,0,0,0,131692,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,65888,0,0,0,0,0,0,0,66374,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,65935,0,65935,0,0,0,0, - 0,0,0,0,0,65755,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,65930,65930,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,65935,0,65935,0,0,0,0,0,0,65933,0,0,0,0,65935,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,197156,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,197144,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,65933,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,262582,0, - 0,131512,65894,0,0,0,0,0,0,65779,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,65755,0,0,0,65935,0,65935,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,65977,65977,0,0,0,0,0,0,328079, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,65936,65936,131506,131506,0,0,0, - 0,0,66856,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66853,0,0,0, - 0,0,0,0,0,66854,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,66852,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66850,0,0,0,0,0, - 0,0,0,0,66851,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66849,0, - 0,0,0,0,0,0,0,0,0,0,0,0,66848,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,66847,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,66846,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,66845,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,66844,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,66843,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66842,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,66841,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,66840,0,66840,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,66838,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66839, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,131804,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66837,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,66364,0,0,0,0,0,66363,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,66836,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66835,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,66834,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66830,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66833, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66832,0,0,0,0,0,0,0,0,66831,0,0,0, - 0,0,0,66829,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66362,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,66828,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,66827,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,66826,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,66825,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66824,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66823,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66822,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66820,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66819,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,66818,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,66817,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,66361,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,66816,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,66815,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66814,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66813,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66812,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66811,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,66796,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66795,0,0,0,0,0,0,0, - 0,0,0,0,66792,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66787,66786,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66784,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66783,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,66360,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66762,0,0, - 0,0,0,0,0,0,66756,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66755,0,0,0,0,0, - 0,0,0,0,0,66753,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66742,0,0,0, - 0,0,0,0,0,0,0,131796,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,66736,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 66732,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66359,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,66716,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66698,0,0,0,0,0, - 0,0,0,0,0,0,0,0,66702,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,66700,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66697,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66691,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66686,0,0,0,0,0, - 0,0,0,66683,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,66680,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,66669,0,0,0,0,0,0,0,0,0,0,0,0,0,66667,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66660,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66656,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66652,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,66650,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,66640,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66636,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66632,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,66617,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,66600,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,66593,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,66581,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,66565,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66358,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66552,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66528,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,66526,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,66521,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66503,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66498,0,0,0,0,0,0,0,0, - 0,0,0,0,0,66482,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66471, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66476,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,66469,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66468,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66467,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66442,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,66429,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,66428,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,66427,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66426,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66425,0,0,0,0,0,0,0,0,0,0,0,0,0, - 66424,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,66513,0,0,0,0,0,0,66510,0,66898,0,0,0,0,0,66517,0,0,0,0,0,0,0,0,66519,0,0,0,0,0,0, + 0,0,0,0,66507,0,0,0,0,0,0,0,0,0,66516,0,0,0,0,66511,0,0,0,0,0,0,0,0,0,0,131754,0, + 66897,0,0,0,0,0,0,0,0,0,131746,0,0,0,0,0,0,0,0,0,0,0,0,66509,0,0,0,66508,0,0,0,0, + 0,0,66506,0,0,0,66504,0,0,0,0,0,0,131768,0,0,131750,0,0,0,0,0,131744,0,0,131752,0,0,0,0,0,0, + 0,131748,0,0,0,0,0,0,66505,0,0,0,0,0,0,0,66502,0,0,0,0,0,0,0,0,0,131764,0,0,0,0,0, + 66500,0,0,0,0,0,0,0,0,0,0,0,0,66896,0,0,0,0,0,66895,0,0,0,0,0,66894,66499,0,0,0,0,0, + 66497,0,0,0,0,0,0,0,0,0,0,66501,0,0,0,0,131742,0,0,0,0,0,0,0,0,0,0,66893,0,0,0,0, + 0,0,0,0,0,0,0,66892,0,0,0,0,66891,0,0,0,0,0,0,66890,0,0,0,0,0,0,0,0,0,0,0,66496, + 66889,0,0,0,0,0,0,0,0,0,66490,0,0,0,0,0,0,0,0,0,66888,0,0,0,0,0,0,0,0,0,66492,0, + 0,0,66493,0,0,0,0,0,131762,0,0,66495,0,0,0,0,0,0,66489,0,0,0,0,0,0,0,0,0,0,0,66887,0, + 66488,0,0,0,0,0,66494,0,0,0,0,66487,0,0,0,0,131740,0,0,0,0,0,0,0,0,0,66491,0,0,0,0,0, + 0,0,0,0,66485,0,0,0,0,0,0,0,0,0,0,66481,0,0,0,0,66886,0,0,0,0,0,0,0,0,0,66480,0, + 0,0,0,0,0,0,66486,0,0,66885,0,0,0,0,0,0,0,0,0,0,0,66483,0,0,0,66884,0,0,0,0,0,0, + 0,0,0,66883,0,0,0,0,0,0,66475,0,0,0,0,0,0,66484,0,0,0,0,0,0,0,0,0,0,66477,0,0,0, + 0,0,0,0,66478,0,0,66475,0,66473,0,0,0,0,0,0,0,0,0,0,66474,0,0,0,66882,0,0,0,0,0,0,0, + 0,0,0,0,0,0,66479,0,0,0,0,0,66881,0,0,0,0,0,0,0,0,0,0,66880,66880,0,0,0,0,0,0,0, + 0,0,0,0,66470,0,0,0,0,0,66472,0,0,0,0,0,0,66879,0,0,0,0,0,0,0,0,0,0,0,66878,0,0, + 0,0,0,66877,0,0,0,0,0,0,0,0,0,66465,0,0,0,0,0,0,0,0,0,66876,0,0,0,0,0,0,66463,0, + 0,0,0,66457,0,66464,0,0,0,0,0,0,0,66454,0,0,66461,0,0,0,66466,0,0,0,0,0,0,0,0,0,66460,0, + 0,0,0,66459,0,0,0,0,0,0,0,0,0,0,0,66462,66456,0,0,0,0,0,0,0,0,0,0,66875,0,0,0,0, + 0,0,66458,131738,0,0,0,0,0,0,0,0,0,0,66453,66451,0,0,0,66874,0,0,0,0,0,0,66450,0,0,0,0,0, + 66452,0,0,0,0,0,0,0,0,0,0,0,0,0,131736,0,0,0,0,66455,0,0,0,0,66873,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,131734,0,0,0,197207,0,0,0,0,66449,0,0,0,0,0,0,0,0,66872,0,0,0,0,0,0, + 0,0,0,131732,0,0,0,0,0,0,0,0,0,0,66448,0,0,0,66871,0,0,0,0,0,0,0,0,0,0,0,0,66447, + 0,0,0,0,66446,0,0,0,66445,66870,0,0,0,0,0,0,0,0,0,0,0,0,0,66869,0,0,66868,0,0,0,0,0, + 0,66444,0,0,0,0,0,0,0,0,0,0,0,0,66867,0,0,0,66443,0,0,0,0,0,0,0,131758,0,0,0,0,0, + 0,0,0,0,0,0,0,66441,66866,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66440,0,0,0,0,0,66865,0,0, + 0,0,0,0,0,0,0,66864,0,0,0,0,66439,0,0,0,0,0,66435,0,0,0,0,0,0,0,0,0,0,0,66434,0, + 0,0,0,0,0,0,0,66432,0,0,0,0,0,0,0,66433,0,0,0,0,0,0,0,66437,0,0,0,66863,0,0,0,0, + 0,0,0,0,0,0,66431,0,0,66862,0,0,0,0,66861,0,0,0,0,0,0,66860,0,0,0,0,0,0,0,0,0,66859, + 0,0,0,0,0,0,66858,0,0,0,0,66857,0,0,0,0,0,0,0,66438,0,0,0,0,0,0,0,0,0,66430,66436,0, + 0,0,0,0,197186,0,0,0,0,131692,0,0,0,0,0,0,0,0,65888,0,0,0,0,0,0,65935,0,65935,0,0,0,0, + 0,0,0,0,0,65755,0,0,0,65930,65930,0,0,0,0,0,0,0,65935,0,65935,0,0,0,0,0,0,65933,0,0,0,0, + 0,0,197156,0,0,0,0,0,0,197144,0,0,0,0,0,0,0,0,0,0,0,65933,0,0,0,0,0,0,0,0,262582,0, + 0,131512,65894,0,0,0,0,0,0,65779,0,0,0,0,0,0,0,0,0,65755,0,0,0,65935,0,0,0,0,0,0,0,65977, + 65977,0,0,0,0,0,0,328079,0,65936,65936,131506,131506,0,0,0,0,0,66856,0,0,0,0,0,0,0,0,0,66853,0,0,0, + 0,0,0,0,0,66854,0,0,0,0,0,66852,0,0,0,0,0,0,66850,0,0,0,0,0,0,0,0,0,66851,0,0,0, + 0,0,0,0,0,0,66849,0,0,0,0,0,66848,0,0,0,0,0,0,66847,0,0,0,0,0,0,0,0,66846,0,0,0, + 66845,0,0,0,0,0,0,0,0,0,66844,0,0,0,0,0,66843,0,0,0,0,0,0,0,66842,0,0,0,0,0,0,0, + 0,0,0,66841,0,0,0,0,0,0,0,0,66840,0,66840,0,0,0,0,66838,0,0,0,0,0,0,0,0,0,0,0,66839, + 0,131804,0,0,0,0,0,0,0,0,0,0,66837,0,0,0,0,0,0,0,66364,0,0,0,0,0,66363,0,0,0,0,0, + 0,0,0,0,66836,0,0,0,0,66835,0,0,0,0,0,0,0,0,66834,0,0,0,0,0,66830,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,66833,0,0,0,66832,0,0,0,0,0,0,0,0,66831,0,0,0,0,0,0,66829,0,0,0,0, + 0,0,0,0,0,66362,0,0,0,0,0,0,0,66828,0,0,0,0,0,66827,0,0,0,0,0,0,0,0,0,0,0,66826, + 0,0,0,0,0,66825,0,0,0,0,66824,0,0,0,0,0,0,0,0,0,66823,0,0,0,0,0,0,0,0,0,66822,0, + 0,66820,0,0,0,0,0,0,0,0,0,0,0,0,66819,0,0,0,0,0,0,0,66818,0,0,0,0,66817,0,0,0,0, + 0,0,0,0,0,0,66361,0,0,0,0,66816,0,0,0,0,66815,0,0,0,0,0,0,0,0,0,0,0,0,66814,0,0, + 0,0,0,0,66813,0,0,0,0,0,0,0,0,0,66812,0,0,0,66811,0,0,0,0,0,0,66796,0,0,0,0,0,0, + 66795,0,0,0,0,0,0,0,0,0,0,0,66792,0,0,0,0,0,66787,66786,0,0,0,0,0,66784,0,0,0,0,0,0, + 0,0,0,66783,0,0,0,0,0,66360,0,0,0,0,0,0,0,0,0,0,0,66762,0,0,0,0,0,0,0,0,66756,0, + 0,0,66755,0,0,0,0,0,0,0,0,0,0,66753,0,0,0,0,0,0,66742,0,0,0,0,0,0,0,0,0,0,131796, + 0,0,0,66736,0,0,0,0,66732,0,0,0,0,0,0,0,66359,0,0,0,0,0,0,0,0,0,0,0,0,0,66716,0, + 0,0,66698,0,0,0,0,0,66702,0,0,0,0,0,0,0,0,0,0,0,0,0,0,66700,0,66697,0,0,0,0,0,0, + 0,0,0,0,0,0,66691,0,0,0,66686,0,0,0,0,0,0,0,0,66683,0,0,0,0,66680,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,66669,0,0,0,0,0,66667,0,0,0,0,0,0,66660,0,0,0,0,0,0,0,0,0,66656,0, + 0,0,0,0,0,66652,0,0,0,0,0,66650,0,0,0,0,0,0,0,0,66640,0,0,0,0,66636,0,0,0,0,0,0, + 0,0,66632,0,0,0,0,0,0,0,66617,0,0,0,0,0,0,0,0,0,0,0,0,66600,0,0,0,0,0,0,66593,0, + 0,0,0,0,0,0,66581,0,66565,0,0,0,0,0,0,0,0,0,0,66358,0,0,0,0,0,0,0,0,0,0,0,66552, + 0,0,0,0,0,66526,0,0,0,0,0,0,0,0,66521,0,0,0,66503,0,0,0,0,0,0,0,0,0,0,0,0,66498, + 0,0,0,0,0,66482,0,0,0,0,0,0,0,0,0,66471,0,0,66476,0,0,0,0,0,0,0,66469,0,0,0,0,0, + 0,0,0,0,0,0,66468,0,0,0,0,0,0,0,66467,0,66442,0,0,0,0,0,0,0,0,0,0,0,0,0,66429,0, + 0,0,0,0,0,66428,0,0,0,0,0,0,0,0,66427,0,0,66426,0,0,0,0,0,0,0,0,66425,0,0,0,0,0, + 66424,0,0,0,0,0,0,0, }; KBTS_INLINE kbts_u32 kbts__GetUnicodeParentInfo(kbts_u32 Codepoint) { - return (Codepoint < 1114110) ? kbts__UnicodeParentInfo_Data[((kbts_un)kbts__UnicodeParentInfo_PageIndices[Codepoint/32] * 32) | (Codepoint & 31)] : 0; + return (Codepoint < 1114110) ? kbts__UnicodeParentInfo_Data[((Codepoint < 173576) ? ((kbts_un)kbts__UnicodeParentInfo_PageIndices[Codepoint / 8] * 8) : 0) | (Codepoint & 7)] : 0; } -static kbts_u8 kbts__UnicodeUseClass_PageIndices[4351] = { - 0,1,1,1,1,1,2,3,4,5,6,7,8,9,10,11,12,1,1,1,1,1,1,13,14,15,16,17,18,19,1,1, - 20,1,1,1,1,21,1,22,1,1,1,1,1,23,24,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,25,26,27,28,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,29,1,1,1,1,30,31,1,32,33,34,35,36,37,38,39,40,41,42,43,44,45,1,46,47,48,49, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,50,50,50,50,51,50,50,50,50,50,50,50,50,50,50,50, - 50,50,50,52,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,53,1,1,1,1,1,1,1,1,54,55,1,56,1,57,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,58,59,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,60,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,61,62,1,63,64,1,1,1,65,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, +static kbts_u8 kbts__UnicodeUseClass_PageIndices[3915] = { + 0,1,2,2,0,3,4,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,5,6,7,8,8,9,10,11,12,13,8,0,0,14,15, + 16,0,17,18,19,20,21,0,22,23,24,25,26,27,28,29,30,31,32,33,34,35,36,37,38,39,40,41,42,43,44,45, + 46,47,48,49,50,51,52,53,54,55,56,49,57,58,59,60,61,62,63,0,64,65,66,0,67,68,69,70,71,72,73,0, + 8,74,75,76,77,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,78,79,80,81,8,82,83,84, + 85,8,8,86,87,88,0,0,89,90,91,92,8,93,94,0,95,8,96,97,98,0,0,0,99,100,101,0,102,103,8,104, + 8,105,106,0,0,0,107,108,0,0,0,0,0,0,0,109,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 110,111,0,112,113,0,0,114,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,115,116,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,117, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,118,8,119,0,0,0,0,0,120,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 121,122,8,123,124,125,126,127,8,128,129,0,130,131,132,133,8,134,135,136,8,137,138,139,0,0,0,0,0,0,8,140, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,141,142,143,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,144,145,21,0,0,0,69,146,0,0,0,0,147,148,0,0, + 0,0,0,0,0,0,0,0,8,149,14,150,151,0,0,0,0,0,0,0,8,152,153,0,0,118,154,118,155,156,157,0, + 158,159,160,161,162,163,164,0,165,166,167,168,162,169,170,171,172,173,174,0,175,176,177,178,179,180,181,182,183,184,185,186, + 8,187,188,189,61,190,191,0,0,0,0,0,8,192,193,0,8,194,195,0,8,196,197,198,199,200,201,0,0,0,0,0, + 8,202,0,0,0,0,0,0,203,204,205,0,0,206,207,208,209,210,211,8,212,0,0,0,0,0,0,0,0,0,0,0, + 213,214,215,216,217,218,0,0,219,220,221,222,223,84,0,0,0,0,0,0,0,0,0,224,225,226,227,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 228,228,228,228,228,228,228,228,228,228,228,228,228,228,228,228,228,228,228,228,228,228,228,228,228,228,228,228,228,228,228,228, + 228,229,230,228,228,228,228,228,228,228,228,228,228,228,228,228,228,228,228,228,228,228,228,228,228,228,228,228,228,228,228,228, + 228,228,228,228,228,228,228,228,228,228,228,228,228,228,228,228,228,228,228,228,228,228,228,228,228,228,228,228,228,228,228,228, + 228,228,228,228,228,228,228,228,228,228,228,228,228,228,228,228,228,228,228,228,228,228,228,228,228,228,228,228,228,228,228,228, + 228,228,228,228,228,228,228,228,228,228,228,228,228,228,228,228,228,228,228,228,228,228,228,228,228,228,228,228,228,228,228,231, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,232,233,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,84,0,8,234,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,235,236,0,0,0,0,0,0,0,0,0,0,0,0,8,8,237,238,239,0,0,240, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,8,8,8,8,8,8,8,8, + 8,8,8,8,8,8,16,241,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 8,8,8,242,243,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,8,244,245,0,0,0,0,0,0,0,0,0,118,246,8,247,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,118,248,0,0,0,0,0,0,118,249,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,8,8,250, }; -static kbts_u8 kbts__UnicodeUseClass_Data[16896] = { +static kbts_u8 kbts__UnicodeUseClass_Data[8032] = { 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,2,0,0,1,1,1,1,1,1,1,1,1,1,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,19,0,21,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,19,0,21,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,6,6,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,2,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1,0,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,19,0,21,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,6,6,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,2,0,0,0,0,0,0,0,0,1,0,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, 1,1,1,1,1,1,1,1,1,1,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1,1,0,1,1,1,0,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1,1,0,0,0,0,0,0,0,0,0,0,1,1,1,0,0,1, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1,0,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,0,1,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,1,1,0,0,0,0,0,0,0,0,0,0,1,1,1,0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1,0,1,1,1,1,1,1,1,1,1,1,1,1,1,1, + 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, 0,0,0,0,0,0,0,0,0,0,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,29,29,29,29,29,29,29,29,29,0,0,0,0,0,0,1,0,0,29,0,0, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,9,9,9,0,0,0,0,1,0,1,1,1,1,0,1,1,1,1,0,0,0,0,0,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,0,0,1,1,1,1,1,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1,1,1,1,1,1,1,1,1,1,1,1,1,0,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,0,0,0,0,0,0,0,0,0,0,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,9,9,9,0,0,0,0, + 1,0,1,1,1,1,0,1,1,1,1,0,0,0,0,0,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,0,0,1,1,1,1,1,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 1,1,1,1,1,1,1,1,1,1,1,1,1,0,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 29,29,29,31,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,34,36,9,1,36,33, 36,35,35,35,35,34,34,34,34,36,36,36,36,3,33,36,0,29,30,0,0,34,35,35,1,1,1,1,1,1,1,1,1,1,35,35,0,0,1,1,1,1,1,1,1,1,1,1,0,0,1,1,1,1,1,1,1,1,1,1,1,1,1,1, 2,29,31,31,0,1,1,1,1,1,1,1,1,0,0,1,1,0,0,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,0,1,1,1,1,1,1,1,0,1,0,0,0,1,1,1,1,0,0,9,1,36,33, @@ -12978,241 +11409,112 @@ static kbts_u8 kbts__UnicodeUseClass_Data[16896] = { 1,29,31,31,0,1,1,1,1,1,1,1,1,0,1,1,1,0,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,0,1,1,1,1,1,1,1,1,1,1,0,1,1,1,1,1,0,0,9,1,36,34, 34,36,36,36,36,0,34,34,34,0,34,34,34,3,0,0,0,0,0,0,0,36,36,0,0,0,0,0,0,0,1,0,1,1,35,35,0,0,1,1,1,1,1,1,1,1,1,1,0,37,37,31,0,0,0,0,0,0,0,0,0,0,0,0, 29,29,31,31,1,1,1,1,1,1,1,1,1,0,1,1,1,0,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,34,34,1,36,36, - 36,35,35,35,35,0,33,33,33,0,33,33,33,3,38,0,0,0,0,0,0,0,0,36,0,0,0,0,0,0,0,1,1,1,35,35,0,0,1,1,1,1,1,1,1,1,1,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,29,31,31,0,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,0,0,0,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,0,1,1,1,1,1,1,1,1,1,0,1,0,0, - 1,1,1,1,1,1,1,0,0,0,4,0,0,0,0,36,36,36,34,34,35,0,35,0,36,33,33,33,33,33,33,36,0,0,0,0,0,0,1,1,1,1,1,1,1,1,1,1,0,0,36,36,0,0,0,0,0,0,0,0,0,0,0,0, - 0,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,0,1,34,1,1,34,34,34,34,35,35,35,0,0,0,0,0, - 1,1,1,1,1,1,0,34,29,29,29,29,8,29,34,0,1,1,1,1,1,1,1,1,1,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 36,35,35,35,35,0,33,33,33,0,33,33,33,3,38,0,0,0,0,0,0,0,0,36,0,0,0,0,0,0,0,1,0,29,31,31,0,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,0,0,0,1,1,1,1,1,1, + 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,0,1,1,1,1,1,1,1,1,1,0,1,0,0,1,1,1,1,1,1,1,0,0,0,4,0,0,0,0,36,36,36,34,34,35,0,35,0,36,33,33,33,33,33,33,36, + 0,0,0,0,0,0,1,1,1,1,1,1,1,1,1,1,0,0,36,36,0,0,0,0,0,0,0,0,0,0,0,0,0,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, + 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,0,1,34,1,1,34,34,34,34,35,35,35,0,0,0,0,0,1,1,1,1,1,1,0,34,29,29,29,29,8,29,34,0,1,1,1,1,1,1,1,1,1,1,0,0,0,0,0,0, 0,1,1,0,1,0,1,1,1,1,1,0,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,0,1,0,1,1,1,1,1,1,1,1,0,1,34,1,1,34,34,34,34,35,35,35,34,12,1,0,0, - 1,1,1,1,1,0,0,0,29,29,29,29,0,29,6,0,1,1,1,1,1,1,1,1,1,1,0,0,1,1,1,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,0,6,0,6,0,8,0,0,0,0,0,0, - 1,1,1,1,1,1,1,1,0,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,0,0,0,0,9,35,34,34,35,34,34,34,34,35,35,35,35,29,0, - 35,34,29,29,35,0,29,29,1,1,1,1,1,7,7,7,7,7,7,7,7,7,7,7,0,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,0,0,0, - 0,0,0,0,0,0,6,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,36,36,34,34,35,35,33,34,34,34,34,29,30,31,6,34,13,10,12,12,1, - 1,1,1,1,1,1,1,1,1,1,0,0,0,0,0,0,1,1,1,1,1,1,36,36,35,35,1,1,1,1,12,12,12,1,36,31,31,1,1,36,36,31,31,31,31,31,1,1,1,34,34,34,34,1,1,1,1,1,1,1,1,1,1,1, - 1,1,12,36,33,34,34,31,31,31,31,31,31,30,1,31,1,1,1,1,1,1,1,1,1,1,31,31,36,34,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 1,1,1,1,1,0,0,0,29,29,29,29,0,29,6,0,1,1,1,1,1,1,1,1,1,1,0,0,1,1,1,1,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,0,6,0,6,0,8,0,0,0,0,0,0,1,1,1,1,1,1,1,1,0,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, + 1,1,1,1,1,1,1,1,1,1,1,1,1,0,0,0,0,9,35,34,34,35,34,34,34,34,35,35,35,35,29,0,35,34,29,29,35,0,29,29,1,1,1,1,1,7,7,7,7,7,7,7,7,7,7,7,0,7,7,7,7,7,7,7, + 7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,0,0,0,0,0,0,0,0,0,6,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 1,1,1,1,1,1,1,1,1,1,1,36,36,34,34,35,35,33,34,34,34,34,29,30,31,6,34,13,10,12,12,1,1,1,1,1,1,1,1,1,1,1,0,0,0,0,0,0,1,1,1,1,1,1,36,36,35,35,1,1,1,1,12,12, + 12,1,36,31,31,1,1,36,36,31,31,31,31,31,1,1,1,34,34,34,34,1,1,1,1,1,1,1,1,1,1,1,1,1,12,36,33,34,34,31,31,31,31,31,31,30,1,31,1,1,1,1,1,1,1,1,1,1,31,31,36,34,0,0, 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,34,35,35,36,0,0,0,0,0,0,0,0,0,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,34,35,36,0,0,0,0,0,0,0,0,0,0,0, 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,34,35,0,0,0,0,0,0,0,0,0,0,0,0,1,1,1,1,1,1,1,1,1,1,1,1,1,0,1,1,1,0,34,35,0,0,0,0,0,0,0,0,0,0,0,0, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,0,0,36,34,34,34,34,35,35,35,33,33, - 33,33,33,33,33,33,29,31,36,29,29,6,14,8,6,29,6,34,6,6,0,0,0,0,0,0,0,0,1,6,0,0,1,1,1,1,1,1,1,1,1,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,1,0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,0,0,0,0,0,0,0, - 2,2,2,2,2,8,8,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,9,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 2,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,0,34,34,35,36,36,34,34,34,34,7,7,7,0,0,0,0,16,16,30,16,16,16,16,16,16,15,29,6,0,0,0,0, - 0,0,0,0,0,0,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,0,0,1,1,1,1,1,0,0,0,0,0,0,0,0,0,0,0, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,0,0,0,0,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,31,31,0,0,0,0,0,0,1,1,1,1,1,1,1,1,1,1,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,34,34,33,36,34,0,0,0,0,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, + 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,0,0,36,34,34,34,34,35,35,35,33,33,33,33,33,33,33,33,29,31,36,29,29,6,14,8,6,29,6,34,6,6,0,0,0,0,0,0,0,0,1,6,0,0, + 1,1,1,1,1,1,1,1,1,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1,0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,0,0,0,0,0,0,0,2,2,2,2,2,8,8,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, + 1,1,1,1,1,1,1,1,1,9,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,2,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,0, + 34,34,35,36,36,34,34,34,34,7,7,7,0,0,0,0,16,16,30,16,16,16,16,16,16,15,29,6,0,0,0,0,0,0,0,0,0,0,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, + 1,1,1,1,1,1,1,1,1,1,1,1,1,1,0,0,1,1,1,1,1,0,0,0,0,0,0,0,0,0,0,0,1,1,1,1,1,1,1,1,1,1,1,1,0,0,0,0,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, + 1,1,1,1,1,1,1,1,31,31,0,0,0,0,0,0,1,1,1,1,1,1,1,1,1,1,1,0,0,0,0,0,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,34,34,33,36,34,0,0,0,0, 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,10,12,7,14,14,11,7,7,7,7,0,5,36,34,36,36,34,34,34,34,35,35,34,35,36,33,33,33,33,33,34,29,29,29,29,29,29,34,29,29,0,0,30, - 1,1,1,1,1,1,1,1,1,1,0,0,0,0,0,0,1,1,1,1,1,1,1,1,1,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 29,29,29,14,31,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,8,36,34,34,35,35,35,35,34,34,33,33, - 33,33,34,34,3,1,1,1,1,1,1,1,1,0,0,0,1,1,1,1,1,1,1,1,1,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 1,1,1,1,1,1,1,1,1,1,0,0,0,0,0,0,1,1,1,1,1,1,1,1,1,1,0,0,0,0,0,0,29,29,29,14,31,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, + 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,8,36,34,34,35,35,35,35,34,34,33,33,33,33,34,34,3,1,1,1,1,1,1,1,1,0,0,0,1,1,1,1,1,1,1,1,1,1,0,0,0,0,0,0, 29,14,31,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,7,7,7,34,35,33,36,34,34,36,6,7,7,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,8,36,34,34,36,36,36,34,36,34,14,14,17,17,0,0,0,0,0,0,0,0,0,0,0,0, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,7,7,36,33,33,33,36,36,35,14,14,14,14,14,14,14,32,32,0,9,0,0,0,0,0,0,0,0, - 1,1,1,1,1,1,1,1,1,1,0,0,0,1,1,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,29,29,29,0,30,30,30,30,30,30,29,29,30,30,30,30,29,31,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,29,37,37,31,29,29,2,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,6,0,0,0,0, + 1,1,1,1,1,1,8,36,34,34,36,36,36,34,36,34,14,14,17,17,0,0,0,0,0,0,0,0,0,0,0,0,1,1,1,1,7,7,36,33,33,33,36,36,35,14,14,14,14,14,14,14,32,32,0,9,0,0,0,0,0,0,0,0, + 1,1,1,1,1,1,1,1,1,1,0,0,0,1,1,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,29,29,29,0,30,30,30,30,30,30,29,29,30,30,30,30, + 29,31,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,29,37,37,31,29,29,2,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,6,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,39,1,0,0,2,2,2,2,2,2,0,0,0,0,0,0,0,0,0,0,0,0,2,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,6,0,0,0,0,0,0,0,0,0,0,0, - 0,0,6,6,6,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,29,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,2,2,2,2,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,19,21,19,21,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,3, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,19,21,19,21,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 1,1,34,1,1,1,3,1,1,1,1,29,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,36,36,35,34,36,0,0,0,0,35,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,6,0,0,0,0,0,0,0,0,0,0,0,0,0,6,6,6,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,29,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,2,2,2,2,0,0,0,0,0,0,0,19,21,19,21,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,3, + 0,0,19,21,19,21,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1,1,34,1,1,1,3,1,1,1,1,29,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, + 1,1,1,36,36,35,34,36,0,0,0,0,35,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,0,0,0,0,0,0,0,0,0,0,0,0, 31,31,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,13,36,36,36,36,36,36,36,36,36,36,36, 36,36,36,36,3,29,0,0,0,0,0,0,0,0,0,0,1,1,1,1,1,1,1,1,1,1,0,0,0,0,0,0,29,29,29,29,29,29,29,29,29,29,29,29,29,29,29,29,29,29,1,1,0,0,0,0,0,0,0,0,0,0,1,34, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,34,34,34,34,34,30,30,30,0,0,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,35,35,35,34,35,35,35,35,14,14,14,16,36,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 1,1,1,1,1,1,34,34,34,34,34,30,30,30,0,0,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,35,35,35,34,35,35,35,35,14,14,14,16,36,0,0,0,0,0,0,0,0,0,0,0,0, 29,29,14,31,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,8,36,36,34,34,35,35,33,33,34,12,13,12, 3,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1,1,1,1,1,1,1,1,1,1,0,0,0,0,0,0,1,1,1,1,1,34,0,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,0, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,29,34,34,34,35,34,33,33,34,35,13,10,11,12,0,0,0,0,0,0,0,0,0, - 1,1,1,14,1,1,1,1,1,1,1,1,14,16,0,0,1,1,1,1,1,1,1,1,1,1,0,0,0,0,0,0,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,0,1,1,1,2,2,2,0,0,0,1,31,29,31,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,35,1,35,35,34,1,1,35,35,1,1,1,1,1,35,29, + 1,1,1,1,1,1,1,1,1,29,34,34,34,35,34,33,33,34,35,13,10,11,12,0,0,0,0,0,0,0,0,0,1,1,1,14,1,1,1,1,1,1,1,1,14,16,0,0,1,1,1,1,1,1,1,1,1,1,0,0,0,0,0,0, + 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,0,1,1,1,2,2,2,0,0,0,1,31,29,31,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,35,1,35,35,34,1,1,35,35,1,1,1,1,1,35,29, 1,29,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1,1,1,1,1,1,1,1,1,1,1,33,35,34,33,36,0,0,0,0,0,31,6,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,36,36,34,36,36,35,36,36,0,31,35,0,0,1,1,1,1,1,1,1,1,1,1,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1,1,1,1,1,1,1,1,1,1,1,0,1,1,1,1, + 1,1,1,36,36,34,36,36,35,36,36,0,31,35,0,0,1,1,1,1,1,1,1,1,1,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1,1,1,1,1,1,1,1,1,1,1,0,1,1,1,1, 1,1,1,1,1,1,1,1,1,1,1,0,1,1,1,1,1,1,1,0,1,1,0,1,1,1,1,1,1,1,1,1,1,1,0,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,0,1,1,1,1,1,1,1,0,1,1,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 1,35,35,35,0,34,35,0,0,0,0,0,36,30,30,29,1,1,1,1,0,1,1,1,0,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,0,0,9,9,9,0,0,0,0,6, - 1,1,1,1,1,1,1,1,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 1,1,1,1,1,1,1,1,0,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,9,9,0,0,0,0,1,1,1,1,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1,1,1,1,1,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,29,29,29,8,0,0,0,0,0,0,0,0,1,1,1,1,1,1,1,1,1,1,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,0,0,0,34,34,34,34,34,0,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,0,34,34,0,0,0,1,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,1,1,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,30,30,30,30,30,30,30,30,30,30,30,1,1,1,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,9,9,9,9,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1,0,1,1,1,1,1,0,1,1,1,1,1,1,1,1, - 0,1,1,1,1,0,0,0,0,1,1,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 1,1,1,1,1,9,9,0,0,0,0,1,1,1,1,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,1,1,1,1,1,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1,1,1,1,29,29,29,8,0,0,0,0,0,0,0,0,1,1,1,1,1,1,1,1,1,1,0,0,0,0,0,0, + 1,1,1,1,1,1,0,0,0,34,34,34,34,34,0,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 1,1,1,1,1,1,1,1,1,1,0,34,34,0,0,0,1,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1,1,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 1,1,1,1,1,1,30,30,30,30,30,30,30,30,30,30,30,1,1,1,1,0,0,0,0,0,0,0,0,0,0,0,1,1,9,9,9,9,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1,0,1,1,1,1,1,0,1,1,1,1,1,1,1,1,0,1,1,1,1,0,0,0,0,1,1,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 31,29,31,37,37,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,34,34,34,34,35,35,35,35, 35,35,34,34,34,34,3,0,0,0,0,0,0,0,0,0,0,0,41,41,41,41,41,41,41,41,41,41,41,41,41,41,41,41,41,41,41,41,1,1,1,1,1,1,1,1,1,1,34,1,1,34,34,1,0,0,0,0,0,0,0,0,0,40, 29,29,31,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,36,33,36,35,35,34,34,36,36,3,9,0,0,0,0,0, - 0,0,35,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 29,29,29,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,35,35,35,34,34,33,35,34,34,35,34,34,6,8,0,1,1,1,1,1,1,1,1,1,1, - 0,0,0,0,1,36,36,1,0,0,0,0,0,0,0,0,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,9,0,0,0,0,0,0,0,0,0,0,0,0, - 29,29,31,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,36,33,36,35,35,35,35,35,35,34,34,34,34, + 0,0,35,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,29,29,29,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, + 1,1,1,1,1,1,1,35,35,35,34,34,33,35,34,34,35,34,34,6,8,0,1,1,1,1,1,1,1,1,1,1,0,0,0,0,1,36,36,1,0,0,0,0,0,0,0,0,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, + 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,9,0,0,0,0,0,0,0,0,0,0,0,0,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,36,33,36,35,35,35,35,35,35,34,34,34,34, 3,1,38,38,0,0,0,0,0,6,9,34,35,0,33,29,1,1,1,1,1,1,1,1,1,1,1,0,0,0,0,0,0,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,0,0,0,0,0,0,0,0,0,0,0, 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,0,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,36,36,36,35,34,34,34,34,29,3,8,8,0,0,0,0,0,0,29,1, - 1,35,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 1,1,1,1,1,1,1,0,1,0,1,1,1,1,0,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,0,1,1,1,1,1,1,1,1,1,1,0,0,0,0,0,0,0,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,29,36,33,36,35,35,34,34,34,34,9,35,0,0,0,0,0,1,1,1,1,1,1,1,1,1,1,0,0,0,0,0,0, - 29,29,31,31,0,1,1,1,1,1,1,1,1,0,0,1,1,0,0,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,0,1,1,1,1,1,1,1,0,1,1,0,1,1,1,1,1,0,9,9,1,36,36, - 34,36,36,36,36,0,0,33,33,0,0,33,33,3,0,0,0,0,0,0,0,0,0,36,0,0,0,0,0,0,1,1,1,1,36,36,0,0,29,29,29,29,29,29,29,0,0,0,29,29,29,29,29,0,0,0,0,0,0,0,0,0,0,0, - 1,1,1,1,1,1,1,1,1,1,0,1,0,0,1,0,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,0,1,36,34,34,35,35,35,35,35, - 35,0,33,0,0,33,0,33,33,36,31,0,31,31,29,9,6,38,9,0,0,0,0,0,0,0,0,0,0,0,0,0,0,29,30,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,36,33,36,35,35,35,35,35,35,34,34, + 1,35,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1,1,1,1,1,1,1,0,1,0,1,1,1,1,0,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,0,1, + 1,1,1,1,1,1,1,1,1,0,0,0,0,0,0,0,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,29, + 36,33,36,35,35,34,34,34,34,9,35,0,0,0,0,0,1,1,1,1,1,1,1,1,1,1,0,0,0,0,0,0,29,29,31,31,0,1,1,1,1,1,1,1,1,0,0,1,1,0,0,1,1,1,1,1,1,1,1,1,1,1,1,1, + 1,1,1,1,1,1,1,1,1,0,1,1,1,1,1,1,1,0,1,1,0,1,1,1,1,1,0,9,9,1,36,36,34,36,36,36,36,0,0,33,33,0,0,33,33,3,0,0,0,0,0,0,0,0,0,36,0,0,0,0,0,0,1,1, + 1,1,36,36,0,0,29,29,29,29,29,29,29,0,0,0,29,29,29,29,29,0,0,0,0,0,0,0,0,0,0,0,1,1,1,1,1,1,1,1,1,1,0,1,0,0,1,0,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, + 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,0,1,36,34,34,35,35,35,35,35,35,0,33,0,0,33,0,33,33,36,31,0,31,31,29,9,6,38,9,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,29,30,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,36,33,36,35,35,35,35,35,35,34,34, 36,36,3,29,29,31,9,1,0,0,0,0,0,0,0,0,1,1,1,1,1,1,1,1,1,1,0,0,0,0,6,1,37,37,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,36,33,36,35,35,35,35,35,35,33,34,33,33,36,33,29, - 29,31,3,9,1,0,0,0,0,0,0,0,0,0,0,0,1,1,1,1,1,1,1,1,1,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,36,33,36,35,35,35,35,0,0,33,33,33,33,29,29,31,3, - 9,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1,1,1,1,35,35,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,36,36,36,35,35,35,35,35,35,34,34,36,36,29,31,3, - 34,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1,1,1,1,1,1,1,1,1,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,29,31,34,33,36,35,35,34,34,34,34,3,9,1,0,0,0,0,0,0,0, - 1,1,1,1,1,1,1,1,1,1,0,0,0,0,0,0,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,0,0,12,3,11,36,36,34,34,35,35,33,34,35,34,34,34,0,0,0,0,1,1,1,1,1,1,1,1,1,1,1,1,0,0,0,0, - 1,1,1,1,1,1,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,36,33,36,35,35,35,35,34,34,34,34,29,31,3,9,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 1,1,1,1,1,1,1,0,0,1,0,0,1,1,1,1,1,1,1,1,0,1,1,0,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,36,36,36,36,36,33,0,33,33,0,0,29,29,36,6,38, - 13,38,13,9,0,0,0,0,0,0,0,0,0,0,0,0,1,1,1,1,1,1,1,1,1,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1,1,1,1,1,1,1,1,0,0,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,36,33,36,35,35,35,35,0,0,34,34,36,36,31,31,3,1,0,0,33,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 1,34,35,35,34,34,34,34,34,34,35,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,6,35,29,29,29,29,31,37,7,7,7,7,0, - 0,0,0,0,0,0,0,6,0,0,0,0,0,0,0,0,1,34,35,35,34,34,34,36,36,35,35,35,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,38,38,38,38,38,38,15,15,15,15,15,15,15,15,15,15,15,15,29,31,8,6,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 1,1,1,1,1,1,1,1,1,0,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,36,34,34,35,35,35,35,35,0,34,34,34,34,29,29,31,3, - 1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,0,0,0,0,0,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,0,0,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,0,7,7,7,7,7,7,7,35,33,35,34,36,29,29,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 1,1,1,1,1,1,1,0,1,1,0,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,34,34,34,34,34,35,0,0,0,34,0,34,34,0,34, - 29,29,9,34,35,6,38,12,0,0,0,0,0,0,0,0,1,1,1,1,1,1,1,1,1,1,0,0,0,0,0,0,1,1,1,1,1,1,0,1,1,0,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,36,36,36,36,36,0,34,34,0,36,36,29,31,6,0,0,0,0,0,0,0,0,1,1,1,1,1,1,1,1,1,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,2,34,35,33,36,0,0,0,0,0,0,0,0,0, - 29,29,38,31,1,1,1,1,1,1,1,1,1,1,1,1,1,0,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,36,36,34,34,35,35,35,0,0,0,33,33, - 34,36,6,0,0,0,0,0,0,0,0,0,0,0,0,0,1,1,1,1,1,1,1,1,1,1,8,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18, - 18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18, - 18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18, - 18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18, + 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,36,33,36,35,35,35,35,35,35,33,34,33,33,36,33,29,29,31,3,9,1,0,0,0,0,0,0,0,0,0,0,0,1,1,1,1,1,1,1,1,1,1,0,0,0,0,0,0, + 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,36,33,36,35,35,35,35,0,0,33,33,33,33,29,29,31,3,9,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1,1,1,1,35,35,0,0, + 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,36,36,36,35,35,35,35,35,35,34,34,36,36,29,31,3,34,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1,1,1,1,1,1,1,1,1,1,0,0,0,0,0,0, + 1,1,1,1,1,1,1,1,1,1,1,29,31,34,33,36,35,35,34,34,34,34,3,9,1,0,0,0,0,0,0,0,1,1,1,1,1,1,1,1,1,1,0,0,0,0,0,0,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, + 1,1,1,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,0,0,12,3,11, + 36,36,34,34,35,35,33,34,35,34,34,34,0,0,0,0,1,1,1,1,1,1,1,1,1,1,1,1,0,0,0,0,1,1,1,1,1,1,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 1,1,1,1,1,1,1,1,1,1,1,1,36,33,36,35,35,35,35,34,34,34,34,29,31,3,9,0,0,0,0,0,1,1,1,1,1,1,1,0,0,1,0,0,1,1,1,1,1,1,1,1,0,1,1,0,1,1,1,1,1,1,1,1, + 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,36,36,36,36,36,33,0,33,33,0,0,29,29,36,6,38,13,38,13,9,0,0,0,0,0,0,0,0,0,0,0,0,1,1,1,1,1,1,1,1,1,1,0,0,0,0,0,0, + 1,1,1,1,1,1,1,1,0,0,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,36,33,36,35,35,35,35,0,0,34,34,36,36,31,31, + 3,1,0,0,33,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1,34,35,35,34,34,34,34,34,34,35,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, + 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,6,35,29,29,29,29,31,37,7,7,7,7,0,0,0,0,0,0,0,0,6,0,0,0,0,0,0,0,0,1,34,35,35,34,34,34,36,36,35,35,35,1,1,1,1, + 1,1,1,1,38,38,38,38,38,38,15,15,15,15,15,15,15,15,15,15,15,15,29,31,8,6,0,0,0,1,0,0,1,1,1,1,1,1,1,1,1,0,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, + 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,36,34,34,35,35,35,35,35,0,34,34,34,34,29,29,31,3,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, + 1,1,1,1,1,1,1,1,1,1,1,1,1,0,0,0,0,0,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,0,0,7,7,7,7,7,7,7,7,7,7,7,7,7,7, + 7,7,7,7,7,7,7,7,0,7,7,7,7,7,7,7,35,33,35,34,36,29,29,0,0,0,0,0,0,0,0,0,1,1,1,1,1,1,1,0,1,1,0,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, + 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,34,34,34,34,34,35,0,0,0,34,0,34,34,0,34,29,29,9,34,35,6,38,12,0,0,0,0,0,0,0,0,1,1,1,1,1,1,1,1,1,1,0,0,0,0,0,0, + 1,1,1,1,1,1,0,1,1,0,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,36,36,36,36,36,0,34,34,0,36,36,29,31,6,0,0,0,0,0,0,0,0, + 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,2,34,35,33,36,0,0,0,0,0,0,0,0,0,29,29,38,31,1,1,1,1,1,1,1,1,1,1,1,1,1,0,1,1,1,1,1,1,1,1,1,1,1,1,1,1, + 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,36,36,34,34,35,35,35,0,0,0,33,33,34,36,6,0,0,0,0,0,0,0,0,0,0,0,0,0,1,1,1,1,1,1,1,1,1,1,8,0,0,0,0,0, 18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,22,22,22,22,22,22,22,19,21,22,22,22,18,18,18,18, - 23,18,18,18,18,18,18,20,20,20,20,20,20,20,20,20,20,20,20,20,20,20,0,0,0,0,0,0,0,0,0,0,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18, - 18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18, - 18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18, - 18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18, - 18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18, - 18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18, - 18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,0,0,0,0,0, + 23,18,18,18,18,18,18,20,20,20,20,20,20,20,20,20,20,20,20,20,20,20,0,0,0,0,0,0,0,0,0,0,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,18,0,0,0,0,0, 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,34,34,34,34,34,34,34,34,34,34,34,34,10,10,13,29,12,35,1,1,1,1,1,1,1,1,1,1,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 1,1,1,1,1,1,1,1,1,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,29,29,29,29,29,29,29,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 31,31,31,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,36,36,0,0,0,1,1,1,1,1,1,1,1,1,1,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,0,0,0,0,9,0,35,35,35,35,35,35,35,35,35,35,35,35,35,35,35,35,35,35,35,35,35,35,35,35,35,35,35,35,35,35,35,35,35,35,35,35,35,35,35,35,35,35,35,35,35,35,35, - 35,35,35,35,35,35,35,35,0,0,0,0,0,0,0,30,30,30,30,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,0,0,0,0,0,1,1,1,1,1,1,1,1,1,1,1,1,1,0,0,0, - 1,1,1,1,1,1,1,1,1,0,0,0,0,0,0,0,1,1,1,1,1,1,1,1,1,1,0,0,0,4,4,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,0,0,0,29,29,29,29,29,29,29,1,1,1,1,1,1,1,0,0, - 1,1,1,1,1,1,1,1,1,1,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,29,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,29,29,29,29,1,1,1,1,1,1,1,1,1,1,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,34,34,34,34,1,1,1,1,1,1,1,1,1,1,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,35,35,1,1,1,1,1,1,1,1,1,1,1,0,0,0,0,0, - 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, - 1,1,1,1,8,8,8,8,8,8,8,1,0,0,0,0,1,1,1,1,1,1,1,1,1,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,29,29,29,29,29,29,29,0,0,0,0,0,0,0,0,0,31,31,31,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, + 1,1,1,1,1,1,1,1,1,1,1,36,36,0,0,0,1,1,1,1,1,1,1,1,1,1,0,0,0,0,0,0,1,1,1,1,1,1,1,1,1,1,1,0,0,0,0,9,0,35,35,35,35,35,35,35,35,35,35,35,35,35,35,35, + 35,35,35,35,35,35,35,35,35,35,35,35,35,35,35,35,35,35,35,35,35,35,35,35,35,35,35,35,35,35,35,35,35,35,35,35,35,35,35,35,0,0,0,0,0,0,0,30,30,30,30,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1, + 1,1,1,1,1,1,1,1,1,1,1,0,0,0,0,0,1,1,1,1,1,1,1,1,1,1,1,1,1,0,0,0,1,1,1,1,1,1,1,1,1,0,0,0,0,0,0,0,1,1,1,1,1,1,1,1,1,1,0,0,0,4,4,0, + 1,1,1,1,1,1,1,1,1,1,1,1,1,0,0,0,29,29,29,29,29,29,29,1,1,1,1,1,1,1,0,0,1,1,1,1,1,1,1,1,1,1,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 1,1,1,1,1,1,1,1,1,1,1,1,1,1,29,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1,1,1,1,1,1,1,1,1,1,1,1,29,29,29,29,1,1,1,1,1,1,1,1,1,1,0,0,0,0,0,0, + 1,1,1,1,1,1,1,1,1,1,1,1,34,34,34,34,1,1,1,1,1,1,1,1,1,1,0,0,0,0,0,0,1,1,1,1,1,1,1,1,1,1,1,1,1,1,35,35,1,1,1,1,1,1,1,1,1,1,1,0,0,0,0,0, + 1,1,1,1,8,8,8,8,8,8,8,1,0,0,0,0,1,1,1,1,1,1,1,1,1,1,0,0,0,0,0,0, }; KBTS_INLINE kbts_u8 kbts__GetUnicodeUseClass(kbts_u32 Codepoint) { - return (Codepoint < 1114110) ? kbts__UnicodeUseClass_Data[((kbts_un)kbts__UnicodeUseClass_PageIndices[Codepoint/256] * 256) | (Codepoint & 255)] : 0; + return (Codepoint < 1114110) ? kbts__UnicodeUseClass_Data[((Codepoint < 125280) ? ((kbts_un)kbts__UnicodeUseClass_PageIndices[Codepoint / 32] * 32) : 0) | (Codepoint & 31)] : 0; } -static kbts_u8 kbts__UnicodeScriptExtension_PageIndices[8703] = { +static kbts_u8 kbts__UnicodeScriptExtension_PageIndices[7172] = { 0,1,2,2,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,32,33,34,35,36,37,37,37,37,37,38,39,40,41,42,43,44,45,46,47,48,49,50,51,52,2,2,53,54, 55,56,57,58,59,59,59,59,60,59,59,59,59,59,59,59,61,61,59,59,59,59,62,63,64,65,66,67,68,61,61,69, @@ -13437,54 +11739,7 @@ static kbts_u8 kbts__UnicodeScriptExtension_PageIndices[8703] = { 61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61, 61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61, 61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61, - 242,61,243,244,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61, - 61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61, - 61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61, - 61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61, - 61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61, - 61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61, - 61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61, - 61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61, - 61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61, - 61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61, - 61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61, - 61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61, - 61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61, - 61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61, - 61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61, - 61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61, - 61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61, - 61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61, - 61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61, - 61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61, - 61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61, - 61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61, - 61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61, - 61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61, - 61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61, - 61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61, - 61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61, - 61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61, - 61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61, - 61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61, - 61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61, - 61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61, - 61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61, - 61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61, - 61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61, - 61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61, - 61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61, - 61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61, - 61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61, - 61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61, - 61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61, - 61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61, - 61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61, - 61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61, - 61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61, - 61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61, - 61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61, - 61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61,61, + 242,61,243,244, }; static kbts_u16 kbts__UnicodeScriptExtension_Data[31360] = { @@ -14472,313 +12727,88 @@ static kbts_u16 kbts__UnicodeScriptExtension_Data[31360] = { KBTS_INLINE kbts_u16 kbts__GetUnicodeScriptExtension(kbts_u32 Codepoint) { - return (Codepoint < 1114110) ? kbts__UnicodeScriptExtension_Data[((kbts_un)kbts__UnicodeScriptExtension_PageIndices[Codepoint/128] * 128) | (Codepoint & 127)] : 0; + return (Codepoint < 1114110) ? kbts__UnicodeScriptExtension_Data[((Codepoint < 918016) ? ((kbts_un)kbts__UnicodeScriptExtension_PageIndices[Codepoint / 128] * 128) : 7808) | (Codepoint & 127)] : 0; } -static kbts_u8 kbts__UnicodeMirrorCodepoint_PageIndices[8703] = { - 0,1,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,3,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,4,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 5,6,2,2,7,8,9,2,2,2,2,2,2,2,10,11,2,2,2,12,13,14,2,15,2,2,2,2,16,2,2,2, - 17,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,18,2,19,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, - 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, +static kbts_u8 kbts__UnicodeMirrorCodepoint_PageIndices[2044] = { + 0,1,2,3,0,4,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,5,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,6,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,7,8,9,10,0,0,0,0,0,0,0,0,0,0,0,11,12,13,14,15,16,17,18,19,20,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,21,0,0,22,23, + 0,0,0,0,0,0,0,0,0,0,0,0,24,25,26,27,0,28,0,29,30,31,32,33,0,0,0,0,0,0,0,34, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,35,36,37,0,0,0,0,0,0,0,0,0,0,0,0,0, + 38,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,39,40,0,0,0,0,41,42,43,44, }; -static kbts_u32 kbts__UnicodeMirrorCodepoint_Data[2560] = { +static kbts_u32 kbts__UnicodeMirrorCodepoint_Data[1440] = { 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,41,40,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,62,0,60,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,93,0,91,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,125,0,123,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,187,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,171,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,3899,3898,3901,3900,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,5788,5787,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,8250,8249,0,0,0,0,0, 0,0,0,0,0,8262,8261,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,8318,8317,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,8334,8333,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,8715,8716,8717,8712,8713,8714,0,0,0,0,0,0,0,10741,0,0,0,0,0,0,0,0,0,11262, 10659,10651,10656,0,10990,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,8765,8764,0,0, 0,0,0,8909,0,8780,0,0,0,0,0,0,8773,0,0,0,0,0,8787,8786,8789,8788,0,0,0,0,0,0,0,0,0,0, @@ -14789,42 +12819,24 @@ static kbts_u32 kbts__UnicodeMirrorCodepoint_Data[2560] = { 8929,8928,8931,8930,8933,8932,8935,8934,8937,8936,8939,8938,8941,8940,0,0,8945,8944,8954,8955,8956,0,8957,8958,0,0,8946,8947,8948,8950,8951,0, 0,0,0,0,0,0,0,0,8969,8968,8971,8970,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,9002,9001,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,10089,10088,10091,10090,10093,10092,10095,10094,10097,10096,10099,10098,10101,10100,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,10180,10179,10182,10181,0,10185,10184,0,10189,0,10187,0,0,0,0,0,0,0,10198,10197,0,0,0,0,0,8888,10206,10205,0, 0,0,10211,10210,10213,10212,10215,10214,10217,10216,10219,10218,10221,10220,10223,10222,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,10628,10627,10630,10629,10632,10631,10634,10633,10636,10635,10640,10639,10638,10637,10642,10641,10644,10643,10646,10645,10648,10647,0,0,8737,0,0,0,0, 8738,0,0,8736,10661,10660,0,0,10665,10664,10667,10666,10669,10668,10671,10670,0,0,0,0,0,0,0,0,8856,0,0,0,0,0,0,0, 10689,10688,0,0,10693,10692,0,0,0,0,0,0,0,0,0,10704,10703,10706,10705,0,10709,10708,0,0,10713,10712,10715,10714,0,0,0,0, 0,0,0,0,0,0,0,0,10729,10728,0,0,0,0,0,0,0,0,0,0,0,8725,0,0,10745,10744,0,0,10749,10748,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,10796,10795,10798,10797,0,0,0,0,0,10805,10804,0,0,0,0,0,0,10813,10812,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,10853,10852,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,10874,10873,10876,10875,10878,10877,10880, 10879,10882,10881,10884,10883,10886,10885,10888,10887,10890,10889,10892,10891,10894,10893,10896,10895,10898,10897,10900,10899,10902,10901,10904,10903,10906,10905,10908,10907,10910,10909,10912, 10911,10914,10913,0,0,0,10919,10918,10921,10920,10923,10922,10925,10924,0,10928,10927,10930,10929,10932,10931,10934,10933,10936,10935,10938,10937,10940,10939,10942,10941,10944, 10943,10946,10945,10948,10947,10950,10949,10952,10951,10954,10953,10956,10955,10958,10957,10960,10959,10962,10961,10964,10963,10966,10965,0,0,0,0,0,0,0,8870,0, 0,0,0,8873,8872,8875,0,0,0,0,0,0,10989,10988,8740,0,0,0,0,0,0,0,0,11000,10999,11002,11001,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,8735,0, 0,0,11779,11778,11781,11780,0,0,0,11786,11785,0,11789,11788,0,0,0,0,0,0,0,0,0,0,0,0,0,0,11805,11804,0,0, 11809,11808,11811,11810,11813,11812,11815,11814,11817,11816,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,11862,11861,11864,11863,11866,11865,11868,11867,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,12297,12296,12299,12298,12301,12300,12303,12302,12305,12304,0,0,12309,12308,12311,12310,12313,12312,12315,12314,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,65114,65113,65116,65115,65118,65117,0, 0,0,0,0,65125,65124,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,65289,65288,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,65310,0,65308,0, @@ -14835,7 +12847,7 @@ static kbts_u32 kbts__UnicodeMirrorCodepoint_Data[2560] = { KBTS_INLINE kbts_u32 kbts__GetUnicodeMirrorCodepoint(kbts_u32 Codepoint) { - return (Codepoint < 1114110) ? kbts__UnicodeMirrorCodepoint_Data[((kbts_un)kbts__UnicodeMirrorCodepoint_PageIndices[Codepoint/128] * 128) | (Codepoint & 127)] : 0; + return (Codepoint < 1114110) ? kbts__UnicodeMirrorCodepoint_Data[((Codepoint < 65408) ? ((kbts_un)kbts__UnicodeMirrorCodepoint_PageIndices[Codepoint / 32] * 32) : 0) | (Codepoint & 31)] : 0; } KBTS_INLINE kbts_u32 kbts__GetDecompositionSize(kbts_u64 Decomposition) @@ -14852,12 +12864,12 @@ KBTS_INLINE kbts_u32 kbts__GetDecompositionCodepoint(kbts_u64 Decomposition, kbt KBTS_INLINE kbts_u8 kbts__GetSyllabicClass(kbts_u16 SyllabicInfo) { - return SyllabicInfo & 0xFF; + return (kbts_u8)(SyllabicInfo & 0xFF); } KBTS_INLINE kbts_u8 kbts__GetSyllabicPosition(kbts_u16 SyllabicInfo) { - return SyllabicInfo >> 8; + return (kbts_u8)(SyllabicInfo >> 8); } KBTS_INLINE kbts_s32 *kbts__GetParentInfoDeltas(kbts_u32 ParentInfo) @@ -15248,7 +13260,7 @@ KBTS_EXPORT void kbts_FontCoverageTestCodepoint(kbts_font_coverage_test *Test, i { kbts_glyph_parent *Parent = &Test->BaseParents[ParentIndex]; - if(kbts__GetDecompositionCodepoint(Parent->Decomposition, 1) == UCodepoint) + if(Parent->Codepoint1 == UCodepoint) { Test->BaseCodepoint = Parent->Codepoint; RecomposeBase = 1; @@ -15270,7 +13282,7 @@ KBTS_EXPORT void kbts_FontCoverageTestCodepoint(kbts_font_coverage_test *Test, i kbts_u16 BaseId = (kbts_u16)kbts_CodepointToGlyphId(Test->Font, (int)Test->BaseCodepoint); kbts_un BaseParentCount = 0; - int Done = 0; + kbts_b32 Done = 0; while(!Done) { kbts_u32 ParentInfo = kbts__GetUnicodeParentInfo(BaseCodepoint); @@ -15284,12 +13296,15 @@ KBTS_EXPORT void kbts_FontCoverageTestCodepoint(kbts_font_coverage_test *Test, i { kbts_glyph_parent Parent = KBTS__ZERO; Parent.Codepoint = BaseCodepoint + (kbts_u32)ParentDeltas[ParentIndex]; - Parent.Decomposition = kbts__GetUnicodeDecomposition(Parent.Codepoint); + + kbts_u64 Decomposition = kbts__GetUnicodeDecomposition(Parent.Codepoint); + Parent.Codepoint1 = kbts__GetDecompositionCodepoint(Decomposition, 1); + kbts_u16 RecompositionId = (kbts_u16)kbts_CodepointToGlyphId(Test->Font, (int)Parent.Codepoint); if(RecompositionId) { - if(kbts__GetDecompositionSize(Parent.Decomposition) == 1) + if(kbts__GetDecompositionSize(Decomposition) == 1) { BaseCodepoint = Parent.Codepoint; BaseId = RecompositionId; @@ -15324,32 +13339,23 @@ KBTS_EXPORT int kbts_FontCoverageTestEnd(kbts_font_coverage_test *Test) return !Result; } -typedef struct kbts__feature_override_header +typedef struct kbts__enabled_lookup { - struct kbts__feature_override_header *Prev; - struct kbts__feature_override_header *Next; -} kbts__feature_override_header; + kbts_u16 SequentialLookupIndex; + kbts_u16 Value; +} kbts__enabled_lookup; -typedef struct kbts__feature_override +struct kbts_glyph_config { - kbts__feature_override_header Header; - - kbts_u32 Tag; - int Value; -} kbts__feature_override; - -typedef struct kbts_glyph_config -{ - kbts__feature_set EnabledFeatures; - kbts__feature_set DisabledFeatures; - - kbts_u32 ExtraOverrideCount; - kbts_allocator_function *Allocator; void *AllocatorData; - kbts__feature_override_header FeatureOverrideSentinel; -} kbts_glyph_config; + kbts_u32 *EnabledLookupBits; + kbts_u32 *DisabledLookupBits; + + kbts__enabled_lookup *NonBinaryEnabledLookups; + kbts_u32 NonBinaryEnabledLookupCount; +}; typedef struct kbts__arena_block { @@ -15387,26 +13393,53 @@ typedef struct kbts__baked_feature kbts_u32 GlyphFilter; } kbts__baked_feature; -typedef struct kbts__baked_feature_stage +struct kbts__bucketed_glyph { - kbts_u16 FeatureCount; - kbts_u16 FeatureIndexCount; - kbts__baked_feature *Features; // [FeatureCount] - kbts_u16 *FeatureIndices; // [FeatureIndexCount] -} kbts__baked_feature_stage; + kbts_glyph *Glyph; + kbts_u32 SortKey; + kbts_u16 FeatureValue; +}; + +typedef struct kbts__bucketed_glyph_block_header +{ + struct kbts__bucketed_glyph_block_header *Prev; + struct kbts__bucketed_glyph_block_header *Next; +} kbts__bucketed_glyph_block_header; + +typedef struct kbts__bucketed_glyph_block +{ + kbts__bucketed_glyph_block_header Header; + kbts_b32 StartOfAllocation; + kbts_u32 Count; + kbts__bucketed_glyph Glyphs[KBTS__BUCKETED_GLYPHS_PER_BLOCK]; +} kbts__bucketed_glyph_block; #define KBTS_MAX_SIMULTANEOUS_FEATURES 32 -typedef struct kbts__shape_scratchpad +struct kbts_shape_scratchpad { - kbts_arena *Arena; + kbts_allocator_function *Allocator; + void *AllocatorData; + kbts_b32 SelfAllocated; + + void *ScratchMemory; + + kbts_shape_config *Config; + + kbts_u32 *GlyphLookupSubtableMatrix; + kbts_u32 *LookupSubtableIndexOffsets; + kbts_u32 GlyphIdCount; + kbts_u32 LookupSubtableCount; + kbts_u32 GposLookupIndexOffset; + kbts_u32 SequentialLookupCount; + + kbts__bucketed_glyph_block_header *LookupGlyphBuckets; + kbts__bucketed_glyph_block_header FreeBucketedBlockSentinel; kbts__feature_set LookupFeatures; - kbts__feature_set UserFeatures; - kbts__feature_set ScratchFeatures; kbts__glyph_list Cluster; - int RealCluster; - int ClusterAtStartOfWord; + kbts_b32 RealCluster; + kbts_b32 ClusterAtStartOfWord; kbts_glyph *LookupOnePastLastGlyph; kbts_u32 LookupOnePastLastGlyphIndex; @@ -15415,29 +13448,17 @@ typedef struct kbts__shape_scratchpad kbts_u32 Ip; kbts__op_kind OpKind; - kbts__feature_set *OpFeatures; kbts_direction RunDirection; - kbts_u32 FeatureIndexIndex; + kbts_u32 SequentialLookupIndexIndex; kbts_u32 FeatureStagesRead; - kbts_u16 BakedLookupSubtablesRead[KBTS_MAX_SIMULTANEOUS_FEATURES]; - - kbts_u32 UnregisteredFeatureCount; - kbts_feature_tag UnregisteredFeatureTags[KBTS_MAX_SIMULTANEOUS_FEATURES]; kbts_shape_error Error; -} kbts__shape_scratchpad; +}; #define KBTS_CONTEXT_MAX_FONT_COUNT 32 -typedef struct kbts__config_params -{ - kbts_u32 FontIndex; - kbts_script Script; - kbts_language Language; -} kbts__config_params; - #define KBTS__INPUT_CODEPOINT_FIRST_BLOCK_MSB 6 #define KBTS__INPUT_CODEPOINT_ONE_PAST_LAST_BLOCK_MSB 27 @@ -15465,7 +13486,45 @@ typedef struct kbts__context_font kbts__arena_lifetime Lifetime; } kbts__context_font; -typedef struct kbts_shape_context +typedef struct kbts__existing_glyph_config +{ + kbts_shape_config *ShapeConfig; + kbts_feature_override *FeatureOverrides; + int FeatureOverrideCount; + kbts_glyph_config *GlyphConfig; +} kbts__existing_glyph_config; + +typedef struct kbts__existing_shape_config_block_header +{ + struct kbts__existing_shape_config_block_header *Prev; + struct kbts__existing_shape_config_block_header *Next; +} kbts__existing_shape_config_block_header; + +#define KBTS__EXISTING_SHAPE_CONFIGS_PER_BLOCK 32 +typedef struct kbts__existing_shape_config_block +{ + kbts__existing_shape_config_block_header Header; + + kbts_u32 Count; + kbts__existing_shape_config Items[KBTS__EXISTING_SHAPE_CONFIGS_PER_BLOCK]; +} kbts__existing_shape_config_block; + +typedef struct kbts__existing_glyph_config_block_header +{ + struct kbts__existing_glyph_config_block_header *Prev; + struct kbts__existing_glyph_config_block_header *Next; +} kbts__existing_glyph_config_block_header; + +#define KBTS__EXISTING_GLYPH_CONFIGS_PER_BLOCK 32 +typedef struct kbts__existing_glyph_config_block +{ + kbts__existing_glyph_config_block_header Header; + + kbts_u32 Count; + kbts__existing_glyph_config Items[KBTS__EXISTING_GLYPH_CONFIGS_PER_BLOCK]; +} kbts__existing_glyph_config_block; + +struct kbts_shape_context { kbts_arena PermanentArena; kbts_arena FontArena; @@ -15483,32 +13542,19 @@ typedef struct kbts_shape_context kbts_u32 FontCount; kbts__context_font Fonts[KBTS_CONTEXT_MAX_FONT_COUNT]; - kbts__config_params *ConfigTableKeys; - kbts_shape_config **ConfigTable; - - kbts__feature_override_header FeatureOverrideSentinel; - kbts__feature_override_header FreeFeatureOverrideSentinel; - kbts_un InputCodepointCount; int NextUserId; kbts_shape_codepoint *InputBlocks[KBTS__INPUT_CODEPOINT_ONE_PAST_LAST_BLOCK_MSB - KBTS__INPUT_CODEPOINT_FIRST_BLOCK_MSB]; kbts_glyph_storage GlyphStorage; + kbts_shape_context_flags PublicFlags; // Flags that can be set by the user at creation time. kbts__context_flags Flags; kbts_direction ManualRunDirection; kbts_script ManualRunScript; - kbts_u32 BeginClusterIp; - kbts_u32 BeginClusterFeatureStagesRead; - kbts_glyph *TopLevelGlyph; - - kbts__feature_set *OpFeatures; - kbts__feature_set ScratchFeatures; - kbts__feature_set UserFeatures; - kbts__feature_set UserFeaturesEnabled; - - kbts_glyph_config *CurrentGlyphConfig; + kbts_feature_override *CurrentFeatureOverrides; + kbts_u32 CurrentFeatureOverrideCount; int NeedNewGlyphConfig; kbts_direction ParagraphDirection; @@ -15520,17 +13566,29 @@ typedef struct kbts_shape_context kbts_direction RunParagraphDirection; kbts_direction RunDirection; kbts_shape_codepoint_iterator RunCodepointIterator; - int DoneShapingRuns; + kbts_b32 DoneShapingRuns; - kbts_u32 ExistingShapeConfigCount; - kbts__existing_shape_config ExistingShapeConfigs[32]; + kbts__existing_shape_config_block_header ExistingShapeConfigBlockSentinel; + kbts__existing_glyph_config_block_header ExistingGlyphConfigBlockSentinel; + + kbts_u32 ScratchFeatureOverrideCount; + kbts_feature_override ScratchFeatureOverrides[KBTS_MAX_SIMULTANEOUS_FEATURES]; kbts_break_state BreakState; - kbts_u32 FrameCount; - kbts_shape_error Error; -} kbts_shape_context; +}; + +KBTS_INLINE kbts_b32 kbts__ExistingShapeConfigBlockIsValid(kbts_shape_context *Context, kbts__existing_shape_config_block *Block) +{ + kbts_b32 Result = &Block->Header != &Context->ExistingShapeConfigBlockSentinel; + return Result; +} +KBTS_INLINE kbts_b32 kbts__ExistingGlyphConfigBlockIsValid(kbts_shape_context *Context, kbts__existing_glyph_config_block *Block) +{ + kbts_b32 Result = &Block->Header != &Context->ExistingGlyphConfigBlockSentinel; + return Result; +} typedef struct kbts__indic_script_properties { @@ -15543,7 +13601,14 @@ typedef struct kbts__indic_script_properties kbts__syllabic_position BelowBaseMatraPosition; } kbts__indic_script_properties; -typedef struct kbts_shape_config +typedef struct kbts__sequential_lookup +{ + kbts_u32 GlyphFilter; + kbts_u32 SkipFlags; + kbts_u16 LookupIndex; +} kbts__sequential_lookup; + +struct kbts_shape_config { kbts_allocator_function *Allocator; void *AllocatorData; @@ -15568,6 +13633,12 @@ typedef struct kbts_shape_config kbts__feature *Half; kbts__feature *Vatu; + kbts_u32 GlyphCount; + kbts_u32 LookupCount; + kbts_u16 *FeatureStageFirstLookupIndices; // [OpList.FeatureStageCount + 1] + kbts__sequential_lookup *SequentialLookups; // [LookupCount] + kbts_u32 *IdSequentialLookupMatrix; + // Indic kbts_glyph Virama; @@ -15577,9 +13648,7 @@ typedef struct kbts_shape_config // Thai kbts_glyph Nikhahit; kbts_glyph SaraAa; - - kbts__baked_feature_stage *FeatureStages; // [OpList.FeatureStageCount] -} kbts_shape_config; +}; static const kbts_u8 kbts__CmapFormatPrecedence[14] = {1, 0, 1, 0, 2, 0, 1, 0, 3, 0, 3, 0, 4, 5}; @@ -15686,11 +13755,39 @@ static kbts__indic_script_properties kbts__IndicScriptProperties(kbts_u32 Script return Result; } +KBTS_INLINE kbts_u32 kbts__ReadU32Unaligned(kbts_u32 *Data) +{ + kbts_u32 Result; + KBTS_MEMCPY(&Result, (void *)Data, sizeof(kbts_u32)); + return Result; +} +KBTS_INLINE kbts_u16 kbts__ReadU16Unaligned(kbts_u16 *Data) +{ + kbts_u16 Result; + KBTS_MEMCPY(&Result, (void *)Data, sizeof(kbts_u16)); + return Result; +} +KBTS_INLINE kbts_s16 kbts__ReadS16Unaligned(kbts_s16 *Data) +{ + kbts_s16 Result; + KBTS_MEMCPY(&Result, (void *)Data, sizeof(kbts_s16)); + return Result; +} +KBTS_INLINE void kbts__WriteU32Unaligned(kbts_u32 *Dest, kbts_u32 Value) +{ + KBTS_MEMCPY((void *)Dest, &Value, sizeof(kbts_u32)); +} +KBTS_INLINE void kbts__WriteU16Unaligned(kbts_u16 *Dest, kbts_u16 Value) +{ + KBTS_MEMCPY((void *)Dest, &Value, sizeof(kbts_u16)); +} + static void kbts__ByteSwapArray16Unchecked(kbts_u16 *Array, kbts_un Count) { KBTS__FOR(It, 0, Count) { - Array[It] = kbts__ByteSwap16(Array[It]); + kbts_u16 X = kbts__ReadU16Unaligned(&Array[It]); + kbts__WriteU16Unaligned(&Array[It], kbts__ByteSwap16(X)); } } @@ -15708,9 +13805,21 @@ static int kbts__ByteSwapArray16(kbts_u16 *Array, kbts_un Count, char *End) static void kbts__ByteSwapArray32Unchecked(kbts_u32 *Array, kbts_un Count) { - KBTS__FOR(It, 0, Count) + // This is doing byte iteration to work with unaligned arrays. + char *At = (char *)Array; + KBTS__FOR(WordIndex, 0, Count) { - Array[It] = kbts__ByteSwap32(Array[It]); + char Byte0 = At[0]; + char Byte1 = At[1]; + char Byte2 = At[2]; + char Byte3 = At[3]; + + At[0] = Byte3; + At[1] = Byte2; + At[2] = Byte1; + At[3] = Byte0; + + At += sizeof(kbts_u32); } } @@ -15833,13 +13942,13 @@ typedef struct kbts__langsys_record kbts_u16 Offset; } kbts__langsys_record; -typedef struct kbts__langsys +struct kbts__langsys { kbts_u16 LookupOrderOffset; // reserved kbts_u16 RequiredFeatureIndex; kbts_u16 FeatureIndexCount; // kbts_u16 FeatureIndices[FeatureIndexCount]; -} kbts__langsys; +}; typedef struct kbts__feature_list { @@ -15853,12 +13962,12 @@ typedef struct kbts__feature_record kbts_u16 Offset; } kbts__feature_record; -typedef struct kbts__feature +struct kbts__feature { kbts_u16 FeatureParamsOffset; kbts_u16 LookupIndexCount; // kbts_u16 LookupIndices[LookupIndexCount]; -} kbts__feature; +}; typedef struct kbts_lookup_list { @@ -16215,13 +14324,13 @@ typedef struct kbts__sequential_map_group kbts_u32 StartGlyphId; } kbts__sequential_map_group; -typedef struct kbts__cmap_14 +struct kbts__cmap_14 { kbts_u16 Format; kbts_u32 Length; kbts_u32 SelectorCount; // kbts__variation_selector Selectors[SelectorCount]; -} kbts__cmap_14; +}; typedef struct kbts__variation_selector { @@ -16254,7 +14363,7 @@ typedef struct kbts__uvs_mapping kbts_u16 GlyphId; } kbts__uvs_mapping; -typedef struct kbts__gsub_gpos +struct kbts__gsub_gpos { kbts_u16 Major; kbts_u16 Minor; @@ -16262,7 +14371,7 @@ typedef struct kbts__gsub_gpos kbts_u16 FeatureListOffset; kbts_u16 LookupListOffset; kbts_u32 FeatureVariationsOffset; // Only present in v1.1 -} kbts__gsub_gpos; +}; typedef struct kbts__single_substitution { @@ -16352,7 +14461,7 @@ typedef struct kbts__mark_glyph_sets // kbts_u32 CoverageOffsets[MarkGlyphSetCount]; } kbts__mark_glyph_sets; -typedef struct kbts__gdef +struct kbts__gdef { kbts_u16 Major; kbts_u16 Minor; @@ -16362,7 +14471,7 @@ typedef struct kbts__gdef kbts_u16 MarkAttachmentClassDefinitionOffset; // May be 0 kbts_u16 MarkGlyphSetsDefinitionOffset; // v1.2 and up; may be 0 kbts_u32 ItemVariationStoreOffset; // v1.3 and up; may be 0 -} kbts__gdef; +}; typedef struct kbts__anchor { @@ -16522,7 +14631,7 @@ struct kbts__head kbts_s16 GlyphDataFormat; // Only 0 is defined. }; -typedef struct kbts__hea +struct kbts__hea { kbts_u16 Major; kbts_u16 Minor; @@ -16546,7 +14655,7 @@ typedef struct kbts__hea kbts_s16 MetricDataFormat; kbts_u16 MetricCount; -} kbts__hea; +}; typedef struct kbts__os2 { @@ -16566,17 +14675,17 @@ typedef struct kbts__os2 kbts_s16 StrikeoutSize; kbts_s16 StrikeoutPosition; kbts_s16 FamilyClass; - kbts_u8 Panose[10]; - kbts_u32 UnicodeRange[4]; - kbts_u32 VendorId; - kbts_u16 Selection; - kbts_u16 FirstCharacterIndex; - kbts_u16 LastCharacterIndex; + kbts_u8 Panose[10]; // 32 + kbts_u32 UnicodeRange[4]; // 42 + kbts_u32 VendorId; // 58 + kbts_u16 Selection; // 62 + kbts_u16 FirstCharacterIndex; // 64 + kbts_u16 LastCharacterIndex; // 66 // Some version 0 fonts support this, others do not. - kbts_s16 TypoAscender; - kbts_s16 TypoDescender; - kbts_s16 TypoLineGap; + kbts_s16 TypoAscender; // 68 + kbts_s16 TypoDescender; // 70 + kbts_s16 TypoLineGap; // 72 kbts_u16 WinAscent; kbts_u16 WinDescent; @@ -16623,7 +14732,7 @@ typedef struct kbts__name // kbts__lang_tag_record LangTags[LangTagCount]; } kbts__name; -typedef struct kbts__maxp +struct kbts__maxp { kbts_u16 Major; kbts_u16 Minor; @@ -16643,7 +14752,7 @@ typedef struct kbts__maxp kbts_u16 MaximumInstructionSize; kbts_u16 MaximumTopLevelComponentCount; kbts_u16 MaximumComponentDepth; -} kbts__maxp; +}; # pragma pack(pop) @@ -16927,7 +15036,8 @@ static kbts__unpacked_lookup kbts__UnpackLookup(kbts__gdef *Gdef, kbts__lookup * if(MarkGlyphSets->MarkGlyphSetCount > MarkFilteringSetIndex) { kbts_u32 *CoverageOffsets = KBTS__POINTER_AFTER(kbts_u32, MarkGlyphSets); - Result.MarkFilteringSet = KBTS__POINTER_OFFSET(kbts__coverage, MarkGlyphSets, CoverageOffsets[MarkFilteringSetIndex]); + kbts_un CoverageOffset = kbts__ReadU32Unaligned(&CoverageOffsets[MarkFilteringSetIndex]); + Result.MarkFilteringSet = KBTS__POINTER_OFFSET(kbts__coverage, MarkGlyphSets, CoverageOffset); } } } @@ -17131,7 +15241,8 @@ static kbts__ligature_set *kbts__GetLigatureSet(kbts__ligature_substitution *Sub static kbts__ligature *kbts__GetLigature(kbts__ligature_set *Set, kbts_un Index) { kbts_u16 *Offsets = (kbts_u16 *)(Set + 1); - kbts__ligature *Result = KBTS__POINTER_OFFSET(kbts__ligature, Set, Offsets[Index]); + kbts_un Offset = kbts__ReadU16Unaligned(&Offsets[Index]); + kbts__ligature *Result = KBTS__POINTER_OFFSET(kbts__ligature, Set, Offset); return Result; } @@ -17507,6 +15618,11 @@ static int kbts__ByteSwapLookup(kbts__byteswap_context *Context, kbts__lookup *L return Result; } +#define KBTS__DLLIST_REMOVE(Node) do {(Node)->Prev->Next = (Node)->Next; (Node)->Next->Prev = (Node)->Prev;} while(0) +#define KBTS__DLLIST_INSERT_BEFORE(A, B) do{(A)->Next = (B); (A)->Prev = (B)->Prev; (A)->Prev->Next = (A)->Next->Prev = (A);} while(0) +#define KBTS__DLLIST_INSERT_AFTER(A, B) do{(A)->Prev = (B); (A)->Next = (B)->Next; (A)->Prev->Next = (A)->Next->Prev = (A);} while(0) +#define KBTS__DLLIST_SENTINEL_INIT(Sentinel) do{(Sentinel)->Prev = (Sentinel)->Next = (Sentinel);} while(0) + static void kbts__ByteSwapCoverage(kbts__byteswap_context *Context, kbts__coverage *Coverage) { if(!kbts__AlreadyVisited(Context, Coverage)) @@ -17708,9 +15824,9 @@ static kbts__glyph_class_from_table_result kbts__GlyphClassFromTable(kbts_u16 *C // A class definition table (ClassDef) assigns glyphs into classes, beginning with Class 1, then Class 2, and so // on. All glyphs not assigned to a class fall into Class 0. // So, we should be able to pretty reliably just subtract 1 from the class to get. - if(*ClassDefinitionBase == 1) { + KBTS_INSTRUMENT_BLOCK_BEGIN(GlyphClassFromTable1); kbts__class_definition_1 *ClassDef = (kbts__class_definition_1 *)ClassDefinitionBase; kbts_u16 *GlyphClasses = KBTS__POINTER_AFTER(kbts_u16, ClassDef); @@ -17720,6 +15836,7 @@ static kbts__glyph_class_from_table_result kbts__GlyphClassFromTable(kbts_u16 *C Result.Class = GlyphClasses[Offset]; Result.Found = 1; } + KBTS_INSTRUMENT_BLOCK_END(GlyphClassFromTable1); } else if(*ClassDefinitionBase == 2) { @@ -17746,10 +15863,11 @@ static kbts__glyph_class_from_table_result kbts__GlyphClassFromTable(kbts_u16 *C return Result; } -static kbts__cover_glyph_result kbts__CoverGlyph(kbts__coverage *Coverage, kbts_u32 GlyphId) +static kbts__cover_glyph_result kbts__CoverGlyph(kbts__coverage *Coverage, kbts_u16 GlyphId) { KBTS_INSTRUMENT_FUNCTION_BEGIN; kbts__cover_glyph_result Result = KBTS__ZERO; + kbts_un Count = Coverage->Count; if(Count) @@ -17907,6 +16025,7 @@ enum kbts__skip_flags_enum KBTS__SKIP_FLAG_NONE, KBTS__SKIP_FLAG_ZWNJ = (1 << 0), KBTS__SKIP_FLAG_ZWJ = (1 << 1), + KBTS__SKIP_FLAG_DEFAULT_IGNORABLES_NOT_SKIPPED_BY_GSUB = (1 << 2), }; // The Harfbuzz behavior is: // - GPOS lookups always skip ZWNJ. @@ -17915,12 +16034,18 @@ enum kbts__skip_flags_enum // - Regular lookups skip ZWJ when requested. #define KBTS__SKIP_FLAGS_GSUB_REGULAR(RequestedFlags) ((RequestedFlags) & KBTS__SKIP_FLAG_ZWJ) #define KBTS__SKIP_FLAGS_GSUB_SEQUENCE(RequestedFlags) (KBTS__SKIP_FLAG_ZWJ | ((RequestedFlags) & KBTS__SKIP_FLAG_ZWNJ)) -#define KBTS__SKIP_FLAGS_GPOS_REGULAR(RequestedFlags) (((RequestedFlags) & KBTS__SKIP_FLAG_ZWJ) | KBTS__SKIP_FLAG_ZWNJ) -#define KBTS__SKIP_FLAGS_GPOS_SEQUENCE(RequestedFlags) (KBTS__SKIP_FLAG_ZWJ | KBTS__SKIP_FLAG_ZWNJ) +#define KBTS__SKIP_FLAGS_GPOS_REGULAR(RequestedFlags) (((RequestedFlags) & KBTS__SKIP_FLAG_ZWJ) | KBTS__SKIP_FLAG_ZWNJ | KBTS__SKIP_FLAG_DEFAULT_IGNORABLES_NOT_SKIPPED_BY_GSUB) +#define KBTS__SKIP_FLAGS_GPOS_SEQUENCE(RequestedFlags) (KBTS__SKIP_FLAG_ZWJ | KBTS__SKIP_FLAG_ZWNJ | KBTS__SKIP_FLAG_DEFAULT_IGNORABLES_NOT_SKIPPED_BY_GSUB) -static kbts__skip_flags kbts__SkipFlags(kbts__feature_id FeatureId, kbts_shaper Shaper) +static kbts__skip_flags kbts__SkipFlags(kbts_shaping_table ShapingTable, kbts__feature_id FeatureId, kbts_shaper Shaper) { kbts__skip_flags Result = 0; + + if(ShapingTable == KBTS_SHAPING_TABLE_GPOS) + { + Result |= KBTS__SKIP_FLAG_DEFAULT_IGNORABLES_NOT_SKIPPED_BY_GSUB; + } + switch(FeatureId) { case KBTS__FEATURE_ID_nukt: @@ -17989,27 +16114,9 @@ static kbts__skip_flags kbts__SkipFlags(kbts__feature_id FeatureId, kbts_shaper return Result; } -static kbts_u32 kbts__GlyphIncludedInLookup(kbts_font *Font, int Gpos, kbts_un LookupIndex, kbts_u32 Id) +static kbts_b32 kbts__GlyphPassesLookupFilter(kbts_glyph *Glyph, kbts__unpacked_lookup *Lookup) { - kbts_u32 Result = 1; - kbts_u32 GlyphCount = Font->Blob->GlyphCount; - if(Font->Blob->GlyphLookupMatrixOffsetFromStartOfFile && (Id < GlyphCount)) - { - kbts_u32 *GlyphLookupMatrix = KBTS__POINTER_OFFSET(kbts_u32, Font->Blob, Font->Blob->GlyphLookupMatrixOffsetFromStartOfFile); - - kbts_un FlatLookupIndex = (Gpos ? Font->Blob->GposLookupIndexOffset : 0) + LookupIndex; - kbts_un FlatIndex = FlatLookupIndex * Font->Blob->GlyphCount + Id; - kbts_un WordIndex = FlatIndex / 32; - kbts_un BitIndex = FlatIndex % 32; - - Result = GlyphLookupMatrix[WordIndex] & (1 << BitIndex); - } - return Result; -} - -static int kbts__GlyphPassesLookupFilter(kbts_glyph *Glyph, kbts__unpacked_lookup *Lookup) -{ - int Result = 1; + kbts_b32 Result = 1; kbts_u16 Class = Glyph->Classes.Class; if(Class && (Lookup->Flags & (1 << Class))) @@ -18043,18 +16150,50 @@ static int kbts__GlyphPassesLookupFilter(kbts_glyph *Glyph, kbts__unpacked_looku return Result; } -static int kbts__SkipGlyph(kbts_glyph *Glyph, kbts__unpacked_lookup *Lookup, kbts__skip_flags SkipFlags, kbts_u32 SkipUnicodeFlags) +static kbts_b32 kbts__SkipGlyph(kbts_glyph *Glyph, kbts__unpacked_lookup *Lookup, kbts__skip_flags SkipFlags, kbts_u32 SkipUnicodeFlags) { - int Result = (Glyph->UnicodeFlags & SkipUnicodeFlags) || - ((SkipFlags & KBTS__SKIP_FLAG_ZWNJ) && (Glyph->Codepoint == 0x200C)) || - ((SkipFlags & KBTS__SKIP_FLAG_ZWJ) && (Glyph->Codepoint == 0x200D)) || - !kbts__GlyphPassesLookupFilter(Glyph, Lookup); + kbts_b32 Result = 0; + if(!kbts__GlyphPassesLookupFilter(Glyph, Lookup)) + { + Result = 1; + } + else + { + kbts_b32 SkipDefaultIgnorable = !(Glyph->Flags & KBTS_GLYPH_FLAG_GENERATED_BY_GSUB) && (Glyph->UnicodeFlags & SkipUnicodeFlags); + if(SkipDefaultIgnorable) + { + switch(Glyph->Codepoint) + { + // Even though they are default-ignorable codepoints, the following should not be skipped during GSUB: + // Combining grapheme joiner: + case 0x34F: + // Variation selectors: + case 0x180B: case 0x180C: case 0x180D: case 0x180F: // 0x180E is the Mongolian vowel separator; not a variation selector! + // Tags: + case 0xE0020: case 0xE0021: case 0xE0022: case 0xE0023: case 0xE0024: case 0xE0025: case 0xE0026: case 0xE0027: case 0xE0028: case 0xE0029: case 0xE002A: case 0xE002B: case 0xE002C: case 0xE002D: case 0xE002E: case 0xE002F: + case 0xE0030: case 0xE0031: case 0xE0032: case 0xE0033: case 0xE0034: case 0xE0035: case 0xE0036: case 0xE0037: case 0xE0038: case 0xE0039: case 0xE003A: case 0xE003B: case 0xE003C: case 0xE003D: case 0xE003E: case 0xE003F: + case 0xE0040: case 0xE0041: case 0xE0042: case 0xE0043: case 0xE0044: case 0xE0045: case 0xE0046: case 0xE0047: case 0xE0048: case 0xE0049: case 0xE004A: case 0xE004B: case 0xE004C: case 0xE004D: case 0xE004E: case 0xE004F: + case 0xE0050: case 0xE0051: case 0xE0052: case 0xE0053: case 0xE0054: case 0xE0055: case 0xE0056: case 0xE0057: case 0xE0058: case 0xE0059: case 0xE005A: case 0xE005B: case 0xE005C: case 0xE005D: case 0xE005E: case 0xE005F: + case 0xE0060: case 0xE0061: case 0xE0062: case 0xE0063: case 0xE0064: case 0xE0065: case 0xE0066: case 0xE0067: case 0xE0068: case 0xE0069: case 0xE006A: case 0xE006B: case 0xE006C: case 0xE006D: case 0xE006E: case 0xE006F: + case 0xE0070: case 0xE0071: case 0xE0072: case 0xE0073: case 0xE0074: case 0xE0075: case 0xE0076: case 0xE0077: case 0xE0078: case 0xE0079: case 0xE007A: case 0xE007B: case 0xE007C: case 0xE007D: case 0xE007E: case 0xE007F: + { + Result = SkipFlags & KBTS__SKIP_FLAG_DEFAULT_IGNORABLES_NOT_SKIPPED_BY_GSUB; + } break; + + case 0x200C: Result = SkipFlags & KBTS__SKIP_FLAG_ZWNJ; break; + case 0x200D: Result = SkipFlags & KBTS__SKIP_FLAG_ZWJ; break; + + default: Result = 1; break; + } + } + } + return Result; } -static int kbts__GlyphIsValid(kbts_glyph_storage *Storage, kbts_glyph *Glyph) +static kbts_b32 kbts__GlyphIsValid(kbts_glyph_storage *Storage, kbts_glyph *Glyph) { - int Result = Glyph != &Storage->GlyphSentinel; + kbts_b32 Result = Glyph != &Storage->GlyphSentinel; return Result; } @@ -18075,103 +16214,66 @@ static kbts__matrix_index kbts__GlyphLookupMatrixIndex(kbts_un LookupIndex, kbts return Result; } -static kbts__matrix_index kbts__GlyphLookupSubtableMatrixIndex(kbts_un SubtableIndex, kbts_un SubtableCount, kbts_un GlyphIndex) +static kbts__matrix_index kbts__IdSequentialLookupMatrixIndex(kbts_un SequentialLookupIndex, kbts_un GlyphIndex, kbts_un SequentialLookupCount) { - // Since we try many subtables in a row against the same glyphs, the rows are glyph-wise and the columns subtable-wise. - kbts_un FlatIndex = GlyphIndex * SubtableCount + SubtableIndex; + kbts_un BitsPerRow = (SequentialLookupCount + 31) & ~31; + kbts_un FlatIndex = GlyphIndex * BitsPerRow + SequentialLookupIndex; kbts__matrix_index Result = KBTS__ZERO; Result.WordIndex = FlatIndex / 32; Result.BitIndex = FlatIndex % 32; + return Result; } -static int kbts__GlyphsIncludedInLookupSubtable(kbts_glyph_storage *Storage, kbts_font *Font, int Gpos, kbts__unpacked_lookup *Lookup, kbts_un LookupIndex, kbts_un SubtableIndex, kbts_glyph *AtGlyph, kbts__skip_flags SkipFlags, kbts_unicode_flags SkipUnicodeFlags) +static kbts__matrix_index kbts__GlyphLookupSubtableMatrixIndex(kbts_un SubtableIndex, kbts_un SubtableCount, kbts_un GlyphIndex, kbts_un GlyphCount) { - if(Font->Blob->GlyphLookupSubtableMatrixOffsetFromStartOfFile) + kbts_un FlatIndex = SubtableIndex * GlyphCount + GlyphIndex; + + kbts__matrix_index Result = KBTS__ZERO; + Result.WordIndex = FlatIndex / 32; + Result.BitIndex = FlatIndex % 32; + + return Result; +} + +static kbts_un kbts__FlatLookupIndex(kbts_shape_scratchpad *Scratchpad, kbts_shaping_table ShapingTable, kbts_un LookupIndex) +{ + kbts_un Result = (ShapingTable == KBTS_SHAPING_TABLE_GSUB) ? LookupIndex : (LookupIndex + Scratchpad->GposLookupIndexOffset); + return Result; +} + +static kbts_b32 kbts__GlyphIncludedInLookupSubtable(kbts_shape_scratchpad *Scratchpad, + kbts_shaping_table ShapingTable, kbts_un LookupIndex, kbts_un SubtableIndex, + kbts_glyph *AtGlyph) +{ + KBTS_INSTRUMENT_FUNCTION_BEGIN; + kbts_b32 Result = 1; + + if(Scratchpad->GlyphLookupSubtableMatrix) { - kbts_u32 *GlyphLookupSubtableMatrix = KBTS__POINTER_OFFSET(kbts_u32, Font->Blob, Font->Blob->GlyphLookupSubtableMatrixOffsetFromStartOfFile); - kbts_u32 *LookupSubtableIndexOffsets = KBTS__POINTER_OFFSET(kbts_u32, Font->Blob, Font->Blob->LookupSubtableIndexOffsetsOffsetFromStartOfFile); - kbts_lookup_subtable_info *SubtableInfos = KBTS__POINTER_OFFSET(kbts_lookup_subtable_info, Font->Blob, Font->Blob->SubtableInfosOffsetFromStartOfFile); - kbts_un GlyphCount = Font->Blob->GlyphCount; - kbts_un SubtableCount = Font->Blob->LookupSubtableCount; + kbts_u32 *GlyphLookupSubtableMatrix = Scratchpad->GlyphLookupSubtableMatrix; + kbts_u32 *LookupSubtableIndexOffsets = Scratchpad->LookupSubtableIndexOffsets; + kbts_un GlyphCount = Scratchpad->GlyphIdCount; + kbts_un SubtableCount = Scratchpad->LookupSubtableCount; - kbts_un FlatLookupIndex = (Gpos ? Font->Blob->GposLookupIndexOffset : 0) + LookupIndex; + kbts_un FlatLookupIndex = kbts__FlatLookupIndex(Scratchpad, ShapingTable, LookupIndex); kbts_un FlatSubtableIndex = LookupSubtableIndexOffsets[FlatLookupIndex] + SubtableIndex; - kbts_lookup_subtable_info *Info = &SubtableInfos[FlatSubtableIndex]; - kbts_un MinimumBacktrack = (Info->MinimumBacktrackPlusOne) ? Info->MinimumBacktrackPlusOne - 1 : 0; - kbts_un MinimumFollowup = (Info->MinimumFollowupPlusOne) ? Info->MinimumFollowupPlusOne - 1 : 0; - { // Check the current glyph. - kbts_un Id = AtGlyph->Id; - kbts__matrix_index MatrixIndex = kbts__GlyphLookupSubtableMatrixIndex(FlatSubtableIndex, SubtableCount, Id); - if(Id >= GlyphCount) - { - return 1; - } - else if(!(GlyphLookupSubtableMatrix[MatrixIndex.WordIndex] & (1 << MatrixIndex.BitIndex))) - { - return 0; - } - } + kbts_un Id = AtGlyph->Id; + if(Id < GlyphCount) { - kbts_un BacktrackCounter = 0; - kbts_glyph *BacktrackGlyph = AtGlyph->Prev; - while(kbts__GlyphIsValid(Storage, BacktrackGlyph) && - (BacktrackCounter < MinimumBacktrack)) + kbts__matrix_index MatrixIndex = kbts__GlyphLookupSubtableMatrixIndex(FlatSubtableIndex, SubtableCount, Id, GlyphCount); + if(!(GlyphLookupSubtableMatrix[MatrixIndex.WordIndex] & (1u << MatrixIndex.BitIndex))) { - kbts__matrix_index MatrixIndex = kbts__GlyphLookupSubtableMatrixIndex(FlatSubtableIndex, SubtableCount, BacktrackGlyph->Id); - - if((BacktrackGlyph->Id >= GlyphCount) || - kbts__SkipGlyph(BacktrackGlyph, Lookup, SkipFlags, SkipUnicodeFlags) || - GlyphLookupSubtableMatrix[MatrixIndex.WordIndex] & (1 << MatrixIndex.BitIndex)) - { - BacktrackCounter += 1; - } - else - { - return 0; - } - - BacktrackGlyph = BacktrackGlyph->Prev; - } - - if(BacktrackCounter < MinimumBacktrack) - { - return 0; - } - } - - { - kbts_un LookaheadCounter = 0; - kbts_glyph *LookaheadGlyph = AtGlyph->Next; - while(kbts__GlyphIsValid(Storage, LookaheadGlyph) && (LookaheadCounter < MinimumFollowup)) - { - kbts__matrix_index MatrixIndex = kbts__GlyphLookupSubtableMatrixIndex(FlatSubtableIndex, SubtableCount, LookaheadGlyph->Id); - - if((LookaheadGlyph->Id >= GlyphCount) || - kbts__SkipGlyph(LookaheadGlyph, Lookup, SkipFlags, SkipUnicodeFlags) || - GlyphLookupSubtableMatrix[MatrixIndex.WordIndex] & (1 << MatrixIndex.BitIndex)) - { - LookaheadCounter += 1; - } - else - { - return 0; - } - - LookaheadGlyph = LookaheadGlyph->Next; - } - - if(LookaheadCounter < MinimumFollowup) - { - return 0; + Result = 0; } } } - return 1; + KBTS_INSTRUMENT_FUNCTION_END; + return Result; } # ifdef KBTS_DUMP @@ -18611,8 +16713,7 @@ static void kbts__ByteSwapGposLookupSubtable(kbts__byteswap_context *Context, kb At += Unpacked.Size; } } - } - break; + } break; case 2: { @@ -18675,8 +16776,7 @@ static void kbts__ByteSwapGposLookupSubtable(kbts__byteswap_context *Context, kb RecordPair += Size2; } } - } - break; + } break; case 3: { @@ -18699,8 +16799,7 @@ static void kbts__ByteSwapGposLookupSubtable(kbts__byteswap_context *Context, kb kbts__ByteSwapAnchor(Context, KBTS__POINTER_OFFSET(kbts__anchor, Adjust, EntryExit->ExitAnchorOffset)); } } - } - break; + } break; case 4: case 6: @@ -18711,8 +16810,7 @@ static void kbts__ByteSwapGposLookupSubtable(kbts__byteswap_context *Context, kb kbts__ByteSwapCoverage(Context, KBTS__POINTER_OFFSET(kbts__coverage, Adjust, Adjust->BaseCoverageOffset)); kbts__ByteSwapMarkArray(Context, KBTS__POINTER_OFFSET(kbts__mark_array, Adjust, Adjust->MarkArrayOffset)); kbts__ByteSwapBaseArray(Context, Adjust->MarkClassCount, KBTS__POINTER_OFFSET(kbts__base_array, Adjust, Adjust->BaseArrayOffset)); - } - break; + } break; case 5: { @@ -18751,20 +16849,17 @@ static void kbts__ByteSwapGposLookupSubtable(kbts__byteswap_context *Context, kb } } } - } - break; + } break; case 7: { kbts__ByteSwapSequenceContextSubtable(Context, Base); - } - break; + } break; case 8: { kbts__ByteSwapChainedSequenceContextSubtable(Context, Base); - } - break; + } break; case 9: { @@ -18775,8 +16870,7 @@ static void kbts__ByteSwapGposLookupSubtable(kbts__byteswap_context *Context, kb Adjust->Offset = kbts__ByteSwap32(Adjust->Offset); kbts__ByteSwapGposLookupSubtable(Context, LookupList, Adjust->LookupType, KBTS__POINTER_OFFSET(kbts_u16, Adjust, Adjust->Offset)); - } - break; + } break; } } } @@ -18839,7 +16933,8 @@ KBTS_EXPORT int kbts_CodepointToGlyphId(kbts_font *Font, int ICodepoint) kbts_u16 *CmapBase = Font->Cmap; if(CmapBase) { - switch(*CmapBase) + kbts_u16 CmapFormat = kbts__ReadU16Unaligned(CmapBase); + switch(CmapFormat) { case 0: { @@ -18882,13 +16977,14 @@ KBTS_EXPORT int kbts_CodepointToGlyphId(kbts_font *Font, int ICodepoint) if(Offset < SubHeader->EntryCount) { kbts_u16 *GlyphIds = KBTS__POINTER_OFFSET(kbts_u16, &SubHeader->IdRangeOffset, SubHeader->IdRangeOffset); - kbts_u16 GlyphId = GlyphIds[Offset]; + kbts_un GlyphId = GlyphIds[Offset]; + if(GlyphId) { GlyphId += SubHeader->IdDelta; } - Result = GlyphId; + Result = (kbts_u16)GlyphId; } } break; @@ -19057,15 +17153,15 @@ static kbts__iterate_features kbts__IterateFeatures(kbts_shape_config *Config, k return Result; } -static int kbts__IsValidFeatureIteration(kbts__iterate_features *It) +static kbts_b32 kbts__IsValidFeatureIteration(kbts__iterate_features *It) { - int Result = It->Langsys != 0; + kbts_b32 Result = It->Langsys != 0; return Result; } -static int kbts__NextFeature(kbts__iterate_features *It) +static kbts_b32 kbts__NextFeature(kbts__iterate_features *It) { - int Result = 0; + kbts_b32 Result = 0; It->Feature = 0; if(kbts__IsValidFeatureIteration(It)) @@ -19102,7 +17198,7 @@ static int kbts__NextFeature(kbts__iterate_features *It) It->CurrentFeatureTag = Feature.Tag; if(FeatureId && (FeatureId <= 32)) { - It->CurrentFeatureFlag = (1 << (FeatureId - 1)) & KBTS__GLYPH_FEATURE_MASK; + It->CurrentFeatureFlag = (1u << (FeatureId - 1)) & KBTS__GLYPH_FEATURE_MASK; } Result = 1; @@ -19134,9 +17230,9 @@ static kbts__iterate_lookups kbts__IterateLookups(kbts_lookup_list *List, kbts__ return Result; } -static int kbts__NextLookup(kbts__iterate_lookups *It) +static kbts_b32 kbts__NextLookup(kbts__iterate_lookups *It) { - int Result = 0; + kbts_b32 Result = 0; kbts__feature *Feature = It->Feature; if(It->LookupList && Feature && (It->LookupIndexIndex < Feature->LookupIndexCount)) @@ -19153,74 +17249,48 @@ static int kbts__NextLookup(kbts__iterate_lookups *It) return Result; } -KBTS_INLINE void kbts__GsubMutate(kbts_font *Font, kbts_glyph *Glyph, kbts_u16 NewId, kbts_u32 Flags) +static void kbts__UnbucketGlyph(kbts_shape_scratchpad *Scratchpad, kbts_glyph *Glyph) { - Glyph->Id = NewId; - Glyph->Classes = kbts__GlyphClasses(Font, NewId); - Glyph->Flags = (Glyph->Flags & ~KBTS_GLYPH_FLAG_FIRST_IN_MULTIPLE_SUBSTITUTION) | Flags; + if(Glyph->Bucketed) + { + Glyph->Bucketed->SortKey = KBTS__DELETED_SORT_KEY; + + Glyph->Bucketed = 0; + } } -static kbts__op_list *kbts__ShaperOpLists[KBTS_SHAPER_COUNT] = { - /* DEFAULT, */ &kbts__OpList_Default, - /* ARABIC, */ &kbts__OpList_ArabicRclt, - /* HANGUL, */ &kbts__OpList_Hangul, - /* HEBREW, */ &kbts__OpList_Default, - /* INDIC, */ &kbts__OpList_Indic, - /* KHMER, */ &kbts__OpList_Khmer, - /* MYANMAR, */ &kbts__OpList_Myanmar, - /* TIBETAN, */ &kbts__OpList_Tibetan, - /* USE, */ &kbts__OpList_Use, -}; - -typedef struct kbts__gsub_frame +static kbts_un kbts__IdLookupListIndex(kbts_un GlyphId, kbts_un GlyphCount) { - kbts_glyph *InputGlyph; + kbts_un Result = KBTS__MIN(GlyphId, GlyphCount); + return Result; +} - // This isn't really an index into anything, per se. - // It is just an offset that allows reordering for ShapeState->LookupOnePastLastGlyph. - kbts_u32 StartIndex; - - kbts_u16 LookupIndex; - kbts_u16 SubtableIndex; - - // Defined for nested lookups. - kbts__sequence_lookup_record *Records; - kbts_u16 RecordCount; - kbts_u16 RecordIndex; -} kbts__gsub_frame; - -#define KBTS__DLLIST_REMOVE(Node) do {(Node)->Prev->Next = (Node)->Next; (Node)->Next->Prev = (Node)->Prev;} while(0) -#define KBTS__DLLIST_INSERT_BEFORE(A, B) do{(A)->Next = (B); (A)->Prev = (B)->Prev; (A)->Prev->Next = (A)->Next->Prev = (A);} while(0) -#define KBTS__DLLIST_INSERT_AFTER(A, B) do{(A)->Prev = (B); (A)->Next = (B)->Next; (A)->Prev->Next = (A)->Next->Prev = (A);} while(0) -#define KBTS__DLLIST_SENTINEL_INIT(Sentinel) do{(Sentinel)->Prev = (Sentinel)->Next = (Sentinel);} while(0) #define KBTS__DLLIST_SORT(First, OnePastLast, Member) \ do \ { \ - kbts_glyph *DllistSort_OneBeforeFirst = (First)->Prev; \ + kbts_glyph *DllistSort_First = (First); \ kbts_glyph *DllistSort_OnePastLast = (OnePastLast); \ - for(int DllistSort_Swapped = 1; \ - DllistSort_Swapped; \ + kbts_glyph *DllistSort_OneBeforeFirst = DllistSort_First->Prev; \ + for(kbts_glyph *DllistSort_Forward = DllistSort_First; \ + DllistSort_Forward != DllistSort_OnePastLast; \ ) \ { \ - DllistSort_Swapped = 0; \ - kbts_glyph *DllistSort_Glyph0 = DllistSort_OneBeforeFirst->Next; \ - kbts_glyph *DllistSort_Glyph1 = DllistSort_Glyph0->Next; \ - while(DllistSort_Glyph1 != DllistSort_OnePastLast) \ + kbts_glyph *DllistSort_Next = DllistSort_Forward->Next; \ + kbts_un DllistSort_Member = DllistSort_Forward->Member; \ + for(kbts_glyph *DllistSort_Backward = DllistSort_Forward; \ + DllistSort_Backward->Prev != DllistSort_OneBeforeFirst; \ + ) \ { \ - kbts_glyph *DllistSort_Next = DllistSort_Glyph1->Next; \ - \ - if(DllistSort_Glyph0->Member > DllistSort_Glyph1->Member) \ + if(DllistSort_Backward->Prev->Member > DllistSort_Member) \ { \ - KBTS__DLLIST_SWAP(DllistSort_Glyph0, DllistSort_Glyph1); \ - DllistSort_Swapped = 1; \ + KBTS__DLLIST_SWAP(DllistSort_Backward, DllistSort_Backward->Prev); \ } \ else \ { \ - DllistSort_Glyph0 = DllistSort_Glyph1; \ + break; \ } \ - \ - DllistSort_Glyph1 = DllistSort_Next; \ } \ + DllistSort_Forward = DllistSort_Next; \ } \ } while(0) @@ -19245,6 +17315,69 @@ static void KBTS__DLLIST_SWAP(kbts_glyph *A, kbts_glyph *B) B->Prev->Next = B->Next->Prev = B; } +static kbts__op_list *kbts__ShaperOpLists[KBTS_SHAPER_COUNT] = { + /* DEFAULT, */ &kbts__OpList_Default, + /* ARABIC, */ &kbts__OpList_ArabicRclt, + /* HANGUL, */ &kbts__OpList_Hangul, + /* HEBREW, */ &kbts__OpList_Default, + /* INDIC, */ &kbts__OpList_Indic, + /* KHMER, */ &kbts__OpList_Khmer, + /* MYANMAR, */ &kbts__OpList_Myanmar, + /* TIBETAN, */ &kbts__OpList_Tibetan, + /* USE, */ &kbts__OpList_Use, +}; + +typedef struct kbts__gsub_frame +{ + kbts_glyph *InputGlyph; + // This isn't really an index into anything, per se. + // It is just an offset that allows reordering for ShapeState->LookupOnePastLastGlyph. + kbts_u32 StartIndex; + + kbts_u16 LookupIndex; + kbts_u16 SubtableIndex; + + // Defined for nested lookups. + kbts__sequence_lookup_record *Records; + kbts_u16 RecordCount; + kbts_u16 RecordIndex; +} kbts__gsub_frame; + +typedef struct kbts__pointer_bump_allocator +{ + kbts_uptr At; +} kbts__pointer_bump_allocator; + +static kbts__pointer_bump_allocator kbts__PointerBumpAllocator(void *Pointer) +{ + kbts__pointer_bump_allocator Result; + Result.At = (kbts_uptr)Pointer; + return Result; +} + +static void *kbts__PointerPush(kbts__pointer_bump_allocator *Alloc, kbts_un Size, kbts_un Align) +{ + kbts_uptr Aligned = (Alloc->At + (Align - 1)) & ~(Align - 1); + Alloc->At = Aligned + Size; + + void *Result = (void *)Aligned; + return Result; +} +#define kbts__PointerPushType(Pointer, Type) (Type *)kbts__PointerPush((Pointer), sizeof(Type), KBTS_ALIGNOF(Type)) +#define kbts__PointerPushArray(Pointer, Type, Count) (Type *)kbts__PointerPush((Pointer), sizeof(Type) * (Count), KBTS_ALIGNOF(Type)) + +KBTS_INLINE kbts_un kbts__GsubSequentialLookupCount(kbts_shape_config *Config) +{ + kbts_un Result = Config->FeatureStageFirstLookupIndices[Config->OpList.FeatureStageCount - 1]; + return Result; +} + +KBTS_INLINE kbts_un kbts__SequentialLookupCount(kbts_shape_config *Config) +{ + kbts_un Result = Config->FeatureStageFirstLookupIndices[Config->OpList.FeatureStageCount]; + return Result; +} + static void kbts__NullAllocator(void *Data, kbts_allocator_op *Op) { KBTS__UNUSED(Data); @@ -19279,7 +17412,7 @@ static void *kbts__AllocatorAllocate(kbts_allocator_function *Allocator, void *A { Allocator = kbts__DefaultAllocator; } - + Allocator(AllocatorData, &AllocatorOp); void *Result = AllocatorOp.Allocate.Pointer; @@ -19300,7 +17433,7 @@ static void kbts__AllocatorFree(kbts_allocator_function *Allocator, void *Alloca { Allocator = kbts__DefaultAllocator; } - + Allocator(AllocatorData, &AllocatorOp); } } @@ -19573,6 +17706,436 @@ static int kbts__InitializeFixedMemoryArena(kbts_arena *Arena, void *Memory, kbt return Result; } +KBTS_EXPORT kbts_un kbts_SizeOfShapeScratchpad(kbts_shape_config *Config) +{ + kbts_un DecompositionSize = sizeof(kbts_glyph) * KBTS__MAXIMUM_DECOMPOSITION_CODEPOINTS; + kbts_un GsubSize = sizeof(kbts__gsub_frame) * KBTS_LOOKUP_STACK_SIZE; + kbts_un ScratchSize = KBTS__MAX(DecompositionSize, GsubSize); + + kbts_un Result = sizeof(kbts_shape_scratchpad) + ScratchSize; + + if(Config) + { + kbts_un BucketHeadersSize = sizeof(kbts__bucketed_glyph_block_header) * kbts__SequentialLookupCount(Config); + + Result += BucketHeadersSize; + } + + return Result; +} + +KBTS_EXPORT kbts_shape_scratchpad *kbts_PlaceShapeScratchpad(kbts_shape_config *Config, void *Memory, kbts_allocator_function *Allocator, void *AllocatorData) +{ + kbts__pointer_bump_allocator Bump = kbts__PointerBumpAllocator(Memory); + kbts_shape_scratchpad *Result = kbts__PointerPushType(&Bump, kbts_shape_scratchpad); + KBTS_MEMSET(Result, 0, sizeof(*Result)); + + if(!Allocator) + { + Allocator = kbts__DefaultAllocator; + } + + Result->Allocator = Allocator; + Result->AllocatorData = AllocatorData; + Result->Config = Config; + + // @Duplication with SizeOfShapeScratchpad(). + kbts_un DecompositionSize = sizeof(kbts_glyph) * KBTS__MAXIMUM_DECOMPOSITION_CODEPOINTS; + kbts_un GsubSize = sizeof(kbts__gsub_frame) * KBTS_LOOKUP_STACK_SIZE; + kbts_un ScratchSize = KBTS__MAX(DecompositionSize, GsubSize); + + Result->ScratchMemory = kbts__PointerPush(&Bump, ScratchSize, 1); + + if(Config) + { + kbts_un SequentialLookupCount = kbts__SequentialLookupCount(Config); + Result->LookupGlyphBuckets = kbts__PointerPushArray(&Bump, kbts__bucketed_glyph_block_header, SequentialLookupCount); + + { + kbts_blob_header *Blob = Config->Font->Blob; + + Result->GlyphIdCount = Blob->GlyphCount; + Result->LookupSubtableCount = Blob->LookupSubtableCount; + Result->GposLookupIndexOffset = Blob->GposLookupIndexOffset; + + if(Blob->GlyphLookupSubtableMatrixOffsetFromStartOfFile) + { + Result->GlyphLookupSubtableMatrix = KBTS__POINTER_OFFSET(kbts_u32, Blob, Blob->GlyphLookupSubtableMatrixOffsetFromStartOfFile); + } + + if(Blob->LookupSubtableIndexOffsetsOffsetFromStartOfFile) + { + Result->LookupSubtableIndexOffsets = KBTS__POINTER_OFFSET(kbts_u32, Blob, Blob->LookupSubtableIndexOffsetsOffsetFromStartOfFile); + } + } + + KBTS__FOR(LookupIndex, 0, SequentialLookupCount) + { + kbts__bucketed_glyph_block_header *Sentinel = &Result->LookupGlyphBuckets[LookupIndex]; + + KBTS__DLLIST_SENTINEL_INIT(Sentinel); + } + + Result->SequentialLookupCount = (kbts_u32)SequentialLookupCount; + } + + KBTS__DLLIST_SENTINEL_INIT(&Result->FreeBucketedBlockSentinel); + + return Result; +} + +KBTS_EXPORT kbts_shape_scratchpad *kbts_PlaceShapeScratchpadFixedMemory(kbts_shape_config *Config, void *Memory, int Size) +{ + kbts_shape_scratchpad *Result = 0; + kbts_un ScratchpadSize = kbts_SizeOfShapeScratchpad(Config); + kbts_un InitialSize = sizeof(kbts_arena) + ScratchpadSize; + + if((Size >= 0) && + ((kbts_un)Size >= InitialSize)) + { + kbts_arena *Arena = (kbts_arena *)KBTS__POINTER_OFFSET(kbts_arena, Memory, ScratchpadSize); + kbts__InitializeFixedMemoryArena(Arena, KBTS__POINTER_AFTER(void, Arena), (kbts_un)Size - InitialSize); + + Result = kbts_PlaceShapeScratchpad(Config, Memory, kbts__ArenaAllocator, Arena); + } + + return Result; +} + +KBTS_EXPORT kbts_shape_scratchpad *kbts_CreateShapeScratchpad(kbts_shape_config *Config, kbts_allocator_function *Allocator, void *AllocatorData) +{ + kbts_un Size = kbts_SizeOfShapeScratchpad(Config); + kbts_shape_scratchpad *Result = kbts_PlaceShapeScratchpad(Config, kbts__AllocatorAllocate(Allocator, AllocatorData, Size), Allocator, AllocatorData); + Result->SelfAllocated = 1; + return Result; +} + +static kbts__bucketed_glyph_block *kbts__NewBucketedGlyphBlock(kbts_shape_scratchpad *Scratchpad) +{ + kbts__bucketed_glyph_block_header *New = Scratchpad->FreeBucketedBlockSentinel.Prev; + if(New == &Scratchpad->FreeBucketedBlockSentinel) + { + // Allocate new blocks. + kbts_un BlocksToAllocateCount = 4096 / sizeof(kbts__bucketed_glyph_block); + kbts__bucketed_glyph_block *NewBlocks = (kbts__bucketed_glyph_block *)kbts__AllocatorAllocate(Scratchpad->Allocator, Scratchpad->AllocatorData, sizeof(kbts__bucketed_glyph_block) * BlocksToAllocateCount); + + if(NewBlocks) + { + KBTS__FOR(NewBlockIndex, 0, BlocksToAllocateCount) + { + kbts__bucketed_glyph_block *NewBlock = &NewBlocks[NewBlockIndex]; + NewBlock->Count = 0; + + if(NewBlockIndex) + { + KBTS__DLLIST_INSERT_BEFORE(&NewBlock->Header, &Scratchpad->FreeBucketedBlockSentinel); + NewBlock->StartOfAllocation = 0; + } + else + { + NewBlock->StartOfAllocation = 1; + } + } + + kbts__bucketed_glyph_block *NewBlock = &NewBlocks[0]; + New = &NewBlock->Header; + } + else + { + Scratchpad->Error = KBTS_SHAPE_ERROR_OUT_OF_MEMORY; + + return 0; + } + } + else + { + KBTS__DLLIST_REMOVE(New); + } + + kbts__bucketed_glyph_block *Result = (kbts__bucketed_glyph_block *)New; + Result->Count = 0; + + return Result; +} + +static kbts_shape_scratchpad *kbts__CreateShapeScratchpad(kbts_shape_config *Config, kbts_allocator_function *Allocator, void *AllocatorData) +{ + kbts_un ScratchpadSize = kbts_SizeOfShapeScratchpad(Config); + kbts_shape_scratchpad *Result = kbts_PlaceShapeScratchpad(Config, kbts__AllocatorAllocate(Allocator, AllocatorData, ScratchpadSize), Allocator, AllocatorData); + Result->SelfAllocated = 1; + + return Result; +} + +static kbts__bucketed_glyph *kbts__InsertGlyphIntoBucket(kbts_shape_scratchpad *Scratchpad, kbts_un BucketIndex, kbts_glyph *Glyph, kbts_u16 FeatureValue) +{ + kbts__bucketed_glyph_block_header *Sentinel = &Scratchpad->LookupGlyphBuckets[BucketIndex]; + kbts__bucketed_glyph_block_header *Header = Sentinel->Prev; + kbts_b32 NeedNewBlock = (Header == Sentinel); + if(!NeedNewBlock) + { + kbts__bucketed_glyph_block *LastBlock = (kbts__bucketed_glyph_block *)Header; + NeedNewBlock = (LastBlock->Count == KBTS__BUCKETED_GLYPHS_PER_BLOCK); + } + + if(NeedNewBlock) + { + kbts__bucketed_glyph_block *NewBlock = kbts__NewBucketedGlyphBlock(Scratchpad); + if(NewBlock) + { + KBTS__DLLIST_INSERT_BEFORE(&NewBlock->Header, Sentinel); + Header = &NewBlock->Header; + } + } + + kbts__bucketed_glyph_block *Block = (kbts__bucketed_glyph_block *)Header; + kbts__bucketed_glyph *Result = &Block->Glyphs[Block->Count++]; + + Result->Glyph = Glyph; + Result->SortKey = Glyph->SortKey; + Result->FeatureValue = FeatureValue; + + Glyph->Bucketed = Result; + Glyph->BucketedBucketIndex = (kbts_u16)BucketIndex; + + return Result; +} + +static kbts_b32 kbts__BucketGlyph(kbts_shape_scratchpad *Scratchpad, kbts_glyph *Glyph, kbts_un MinimumSequentialLookupIndex, kbts_b32 ScanBackwards) +{ + kbts_b32 Result = 0; + kbts_shape_config *Config = Scratchpad->Config; + kbts_un SequentialLookupCount = kbts__SequentialLookupCount(Config); + + if(MinimumSequentialLookupIndex < SequentialLookupCount) + { + kbts__matrix_index LastIndex = kbts__IdSequentialLookupMatrixIndex(SequentialLookupCount - 1, Glyph->Id, SequentialLookupCount); + kbts_un OnePastLastWordIndex = LastIndex.WordIndex + 1; + + if(Glyph->Id < Config->GlyphCount) + { + kbts_glyph_config *GlyphConfig = Glyph->Config; + + if(!GlyphConfig) + { + kbts_u32 *IdSequentialLookupMatrix = Config->IdSequentialLookupMatrix; + kbts__matrix_index MatrixIndex = kbts__IdSequentialLookupMatrixIndex(MinimumSequentialLookupIndex, Glyph->Id, SequentialLookupCount); + kbts_u32 *At = &IdSequentialLookupMatrix[MatrixIndex.WordIndex]; + // Mask out lookups < MinimumSequentialLookupIndex. + kbts_u32 BeforeFirstMask = ((1u << MatrixIndex.BitIndex) - 1); + kbts_u32 Bits = *At++ & ~BeforeFirstMask; + kbts_un SequentialLookupIndexOffset = 0; + + kbts_un WordIndex = MatrixIndex.WordIndex + 1; + while((WordIndex < OnePastLastWordIndex) && + !Bits) + { + Bits = *At++; + + WordIndex += 1; + SequentialLookupIndexOffset += KBTS__BIT_WIDTH(kbts_u32); + } + + if(Bits) + { + SequentialLookupIndexOffset += (kbts_un)kbts__LsbPositionOrBitWidth32(Bits) - (kbts_un)MatrixIndex.BitIndex; + kbts_un SequentialLookupIndex = MinimumSequentialLookupIndex + SequentialLookupIndexOffset; + kbts__InsertGlyphIntoBucket(Scratchpad, SequentialLookupIndex, Glyph, 1); + Result = 1; + } + } + else + { + kbts_u32 *IdSequentialLookupMatrix = Config->IdSequentialLookupMatrix; + kbts__matrix_index MatrixIndex = kbts__IdSequentialLookupMatrixIndex(MinimumSequentialLookupIndex, Glyph->Id, SequentialLookupCount); + kbts__matrix_index RowIndex = kbts__IdSequentialLookupMatrixIndex(MinimumSequentialLookupIndex, 0, SequentialLookupCount); + + kbts_u32 *At = &IdSequentialLookupMatrix[MatrixIndex.WordIndex]; + kbts_u32 *AtEnabled = &GlyphConfig->EnabledLookupBits[RowIndex.WordIndex]; + kbts_u32 *AtDisabled = &GlyphConfig->DisabledLookupBits[RowIndex.WordIndex]; + + // Mask out lookups < MinimumSequentialLookupIndex. + kbts_u32 BeforeFirstMask = ((1u << MatrixIndex.BitIndex) - 1); + + kbts_u32 Bits = ((*At++ | *AtEnabled++) & ~(*AtDisabled++)) & ~BeforeFirstMask; + kbts_un SequentialLookupIndexOffset = 0; + + kbts_un WordIndex = MatrixIndex.WordIndex + 1; + while((WordIndex < OnePastLastWordIndex) && + !Bits) + { + Bits = ((*At++ | *AtEnabled++) & ~(*AtDisabled++)); + + WordIndex += 1; + SequentialLookupIndexOffset += KBTS__BIT_WIDTH(kbts_u32); + } + + if(Bits) + { + SequentialLookupIndexOffset += (kbts_un)kbts__LsbPositionOrBitWidth32(Bits) - (kbts_un)MatrixIndex.BitIndex; + kbts_un SequentialLookupIndex = MinimumSequentialLookupIndex + SequentialLookupIndexOffset; + kbts_u16 FeatureValue = 1; + + KBTS__FOR(NonBinaryEnabledLookupIndex, 0, GlyphConfig->NonBinaryEnabledLookupCount) + { + kbts__enabled_lookup *Enabled = &GlyphConfig->NonBinaryEnabledLookups[NonBinaryEnabledLookupIndex]; + + if(Enabled->SequentialLookupIndex == SequentialLookupIndex) + { + FeatureValue = (kbts_u16)Enabled->Value; + + break; + } + } + + kbts__InsertGlyphIntoBucket(Scratchpad, SequentialLookupIndex, Glyph, FeatureValue); + Result = 1; + } + } + } + else + { + // Out-of-bounds glyphs traverse every lookup, I guess. + kbts__InsertGlyphIntoBucket(Scratchpad, MinimumSequentialLookupIndex, Glyph, 1); + } + } + + return Result; +} + +KBTS_INLINE void kbts__GsubMutate(kbts_shape_scratchpad *Scratchpad, kbts_font *Font, kbts_glyph *Glyph, kbts_un CurrentSequentialLookupIndex, kbts_u16 NewId, kbts_u32 Flags) +{ + Glyph->Id = NewId; + Glyph->Classes = kbts__GlyphClasses(Font, NewId); + Glyph->Flags = (Glyph->Flags & ~KBTS_GLYPH_FLAG_FIRST_IN_MULTIPLE_SUBSTITUTION) | Flags; + + kbts__UnbucketGlyph(Scratchpad, Glyph); + kbts__BucketGlyph(Scratchpad, Glyph, CurrentSequentialLookupIndex + 1, 0); +} + +static kbts_b32 kbts__BucketedGlyphIsValid(kbts__bucketed_glyph *Bucketed) +{ + kbts_b32 Result = (Bucketed->SortKey != KBTS__DELETED_SORT_KEY); + return Result; +} + +static void kbts__FreeBucketedGlyphBlock(kbts_shape_scratchpad *Scratchpad, kbts__bucketed_glyph_block *Block) +{ + KBTS__DLLIST_REMOVE(&Block->Header); + KBTS__DLLIST_INSERT_BEFORE(&Block->Header, &Scratchpad->FreeBucketedBlockSentinel); +} + +static void kbts__SortGlyphBucket(kbts_shape_scratchpad *Scratchpad, kbts_un SequentialLookupIndex) +{ + kbts__bucketed_glyph_block_header *Sentinel = &Scratchpad->LookupGlyphBuckets[SequentialLookupIndex]; + + // @Speed: Use merge sort? + kbts__bucketed_glyph_block *ForwardBlock = (kbts__bucketed_glyph_block *)Sentinel->Next; + + kbts_un DeletedCount = 0; + if(&ForwardBlock->Header != Sentinel) + { + DeletedCount += (ForwardBlock->Glyphs[0].SortKey == KBTS__DELETED_SORT_KEY); + } + + kbts_un ForwardIndex = 1; + if(ForwardIndex == ForwardBlock->Count) + { + ForwardBlock = (kbts__bucketed_glyph_block *)ForwardBlock->Header.Next; + ForwardIndex = 0; + } + + while(&ForwardBlock->Header != Sentinel) + { + kbts__bucketed_glyph Bucketed = ForwardBlock->Glyphs[ForwardIndex]; + kbts_u32 SortKey = Bucketed.SortKey; + + kbts__bucketed_glyph_block *BackwardBlock = ForwardBlock; + kbts_un BackwardIndex = ForwardIndex; + for(;;) + { + kbts__bucketed_glyph_block *NextBackwardBlock = BackwardBlock; + kbts_un NextBackwardIndex = BackwardIndex; + if(NextBackwardIndex) + { + NextBackwardIndex -= 1; + } + else + { + NextBackwardBlock = (kbts__bucketed_glyph_block *)BackwardBlock->Header.Prev; + + if(&NextBackwardBlock->Header == Sentinel) + { + break; + } + + NextBackwardIndex = NextBackwardBlock->Count - 1; + } + + kbts_u32 ScanSortKey = NextBackwardBlock->Glyphs[NextBackwardIndex].SortKey; + + if(ScanSortKey > SortKey) + { + kbts__bucketed_glyph *From = &NextBackwardBlock->Glyphs[NextBackwardIndex]; + kbts__bucketed_glyph *To = &BackwardBlock->Glyphs[BackwardIndex]; + + BackwardBlock->Glyphs[BackwardIndex] = NextBackwardBlock->Glyphs[NextBackwardIndex]; + + if(From->SortKey != KBTS__DELETED_SORT_KEY) + { + To->Glyph->Bucketed = To; + } + } + else + { + break; + } + + BackwardBlock = NextBackwardBlock; + BackwardIndex = NextBackwardIndex; + } + + kbts__bucketed_glyph *To = &BackwardBlock->Glyphs[BackwardIndex]; + *To = Bucketed; + if(SortKey != KBTS__DELETED_SORT_KEY) + { + To->Glyph->Bucketed = To; + } + + DeletedCount += (SortKey == KBTS__DELETED_SORT_KEY); + + ForwardIndex += 1; + if(ForwardIndex == ForwardBlock->Count) + { + ForwardBlock = (kbts__bucketed_glyph_block *)ForwardBlock->Header.Next; + ForwardIndex = 0; + } + } + + for(kbts__bucketed_glyph_block_header *Header = Sentinel->Prev; + Header != Sentinel; + ) + { + kbts__bucketed_glyph_block_header *Prev = Header->Prev; + kbts__bucketed_glyph_block *Block = (kbts__bucketed_glyph_block *)Header; + + if(DeletedCount >= Block->Count) + { + DeletedCount -= Block->Count; + + kbts__FreeBucketedGlyphBlock(Scratchpad, Block); + } + else + { + Block->Count -= (kbts_u32)DeletedCount; + + break; + } + + Header = Prev; + } +} + static kbts_glyph *kbts__InsertGlyph(kbts_glyph_storage *Storage, kbts_glyph *Anchor, int InsertBeforeAnchor, kbts_glyph *BaseGlyph) { kbts_glyph *Result = Storage->FreeGlyphSentinel.Next; @@ -19591,6 +18154,9 @@ static kbts_glyph *kbts__InsertGlyph(kbts_glyph_storage *Storage, kbts_glyph *An if(BaseGlyph) { *Result = *BaseGlyph; + + // No matter what, clear bucketing information on new glyphs. + Result->Bucketed = 0; } if(InsertBeforeAnchor) @@ -19624,7 +18190,7 @@ static kbts_glyph *kbts__InsertGlyphAfter(kbts_glyph_storage *Storage, kbts_glyp return Result; } -static kbts_glyph *kbts__FreeGlyph(kbts__shape_scratchpad *Scratchpad, kbts_glyph_storage *Storage, kbts_glyph *Glyph) +static kbts_glyph *kbts__FreeGlyph(kbts_shape_scratchpad *Scratchpad, kbts_glyph_storage *Storage, kbts_glyph *Glyph) { if(Glyph == Scratchpad->LookupOnePastLastGlyph) { @@ -19639,6 +18205,8 @@ static kbts_glyph *kbts__FreeGlyph(kbts__shape_scratchpad *Scratchpad, kbts_glyp Scratchpad->Cluster.SentinelNext = Glyph->Next; } + kbts__UnbucketGlyph(Scratchpad, Glyph); + KBTS__DLLIST_REMOVE(Glyph); KBTS__DLLIST_INSERT_BEFORE(Glyph, (kbts_glyph *)&Storage->FreeGlyphSentinel); @@ -19660,44 +18228,9 @@ static kbts_glyph *kbts__SetGlyphPreserveLinksAndUserId(kbts_glyph *Dest, kbts_g return Dest; } -static int kbts__BeginFeatures(kbts__shape_scratchpad *Scratchpad, kbts_shape_config *Config, kbts_shaping_table ShapingTable) -{ - int Result = 0; - kbts_blob_table_id TableId = (ShapingTable == KBTS_SHAPING_TABLE_GSUB) ? KBTS_BLOB_TABLE_ID_GSUB : KBTS_BLOB_TABLE_ID_GPOS; - kbts__gsub_gpos *GsubGpos = kbts__BlobTableDataType(Config->Font->Blob, TableId, kbts__gsub_gpos); - kbts__langsys *Langsys = Config->Langsys[ShapingTable]; - kbts_un FeatureStageIndex = Scratchpad->FeatureStagesRead - 1; - - Scratchpad->FeatureIndexIndex = 0; - - if(GsubGpos && Langsys && (FeatureStageIndex < Config->OpList.FeatureStageCount)) - { - kbts__baked_feature_stage *BakedFeatureStage = &Config->FeatureStages[FeatureStageIndex]; - - // @Incomplete - // if(GsubGpos->Minor == 1) - // { - // kbts__feature_variations *FeatureVariations = KBTS__POINTER_OFFSET(kbts__feature_variations, GsubGpos, GsubGpos->FeatureVariationsOffset); - // } - - - KBTS__FOR(FeatureIndex, 0, BakedFeatureStage->FeatureCount) - { - Scratchpad->BakedLookupSubtablesRead[FeatureIndex] = 0; - } - - Result = 1; - } - - return Result; -} - typedef struct kbts__sequence_lookup_result { kbts__sequence_lookup_record *Records; - kbts_un RecordCount; - kbts_un InputSequenceCountIncludingSkippedGlyphs; - kbts_glyph *OnePastLastGlyphMatched; // This is specified _nowhere_ in the docs, BUT some sequence lookups have 0 records, and exist just to prevent the @@ -19705,19 +18238,23 @@ typedef struct kbts__sequence_lookup_result // So, checking if we got any records is not enough to figure out whether we need to "apply" this lookup. // We need to have an explicit bool as well. // Sigh. - int Matched; + kbts_b32 Matched; + + kbts_u16 RecordCount; + kbts_u16 InputSequenceCountIncludingSkippedGlyphs; } kbts__sequence_lookup_result; typedef struct kbts__sequence_match { - kbts_un MatchCount; - kbts_un MatchOrSkipCount; kbts_glyph *OnePastMatchGlyph; + + kbts_u16 MatchCount; + kbts_u16 MatchOrSkipCount; } kbts__sequence_match; static kbts__sequence_match kbts__MatchCoverageSequence(kbts_glyph_storage *Storage, kbts__unpacked_lookup *Lookup, kbts_u32 SkipFlags, kbts_u32 SkipUnicodeFlags, - void *Base, kbts_u16 *CoverageOffsets, kbts_un CoverageCount, - kbts_glyph *AtGlyph, int Backward) + void *Base, kbts_u16 *CoverageOffsets, kbts_un CoverageCount, + kbts_glyph *AtGlyph, int Backward) { kbts_un CoverageIndex = 0; kbts_un GlyphCounter = 0; @@ -19744,8 +18281,8 @@ static kbts__sequence_match kbts__MatchCoverageSequence(kbts_glyph_storage *Stor } kbts__sequence_match Result = KBTS__ZERO; - Result.MatchCount = CoverageIndex; - Result.MatchOrSkipCount = GlyphCounter; + Result.MatchCount = (kbts_u16)CoverageIndex; + Result.MatchOrSkipCount = (kbts_u16)GlyphCounter; Result.OnePastMatchGlyph = AtGlyph; return Result; } @@ -19761,7 +18298,7 @@ static int kbts__BranchlessCompareArray16(kbts_u16 *A, kbts_u16 *B, kbts_un Coun } static kbts__sequence_lookup_result kbts__DoSequenceLookup(kbts_glyph_storage *Storage, kbts__unpacked_lookup *Lookup, kbts_u16 *Base, kbts__cover_glyph_result Cover, - kbts_glyph *AtGlyph, kbts__skip_flags SkipFlags, kbts_u32 SkipUnicodeFlags) + kbts_glyph *AtGlyph, kbts__skip_flags SkipFlags, kbts_u32 SkipUnicodeFlags) { KBTS_INSTRUMENT_FUNCTION_BEGIN; kbts__sequence_lookup_result Result = KBTS__ZERO; @@ -19984,9 +18521,12 @@ static kbts__sequence_lookup_result kbts__DoSequenceLookup(kbts_glyph_storage *S case 0x80002: { kbts__chained_sequence_context_2 *Subst = (kbts__chained_sequence_context_2 *)Base; - kbts_u16 *BacktrackClassDefinition = KBTS__POINTER_OFFSET(kbts_u16, Subst, Subst->BacktrackClassDefOffset); - kbts_u16 *InputClassDefinition = KBTS__POINTER_OFFSET(kbts_u16, Subst, Subst->InputClassDefOffset); - kbts_u16 *LookaheadClassDefinition = KBTS__POINTER_OFFSET(kbts_u16, Subst, Subst->LookaheadClassDefOffset); + kbts_u16 *BacktrackClassDefinition = 0; + kbts_u16 *InputClassDefinition = 0; + kbts_u16 *LookaheadClassDefinition = 0; + if(Subst->BacktrackClassDefOffset) {BacktrackClassDefinition = KBTS__POINTER_OFFSET(kbts_u16, Subst, Subst->BacktrackClassDefOffset);} + if(Subst->InputClassDefOffset) {InputClassDefinition = KBTS__POINTER_OFFSET(kbts_u16, Subst, Subst->InputClassDefOffset);} + if(Subst->LookaheadClassDefOffset) {LookaheadClassDefinition = KBTS__POINTER_OFFSET(kbts_u16, Subst, Subst->LookaheadClassDefOffset);} // @Incomplete: Do this with all sequence types! @@ -20046,7 +18586,7 @@ static kbts__sequence_lookup_result kbts__DoSequenceLookup(kbts_glyph_storage *S // and it doesn't matter whether those glyphs are in the input sequence or part of the lookahead. // This happens often enough that we care to special-case it. kbts__glyph_class_from_table_result LookaheadClass = InputClass; - if(LookaheadClassDefinition != InputClassDefinition) + if(LookaheadClassDefinition && (LookaheadClassDefinition != InputClassDefinition)) { LookaheadClass = kbts__GlyphClassFromTable(LookaheadClassDefinition, InputGlyph->Id); } @@ -20118,7 +18658,7 @@ static kbts__sequence_lookup_result kbts__DoSequenceLookup(kbts_glyph_storage *S { Result.Records = Unpacked.Records; Result.RecordCount = Unpacked.RecordCount; - Result.InputSequenceCountIncludingSkippedGlyphs = MatchedOrSkippedInputGlyphCount; + Result.InputSequenceCountIncludingSkippedGlyphs = (kbts_u16)MatchedOrSkippedInputGlyphCount; Result.OnePastLastGlyphMatched = InputMatch.OnePastMatchGlyph; Result.Matched = 1; @@ -20142,7 +18682,7 @@ static void kbts__ApplyValueRecord(kbts_glyph *Glyph, kbts__unpacked_value_recor Glyph->AdvanceY += Unpacked->AdvanceY; } -static int kbts__NextGlyph(kbts_glyph_storage *Storage, kbts__unpacked_lookup *Lookup, kbts_glyph *AtGlyph, kbts__skip_flags SkipFlags, kbts_u32 SkipUnicodeFlags, kbts_glyph **Match, int Backward) +static kbts_b32 kbts__NextGlyph(kbts_glyph_storage *Storage, kbts__unpacked_lookup *Lookup, kbts_glyph *AtGlyph, kbts__skip_flags SkipFlags, kbts_u32 SkipUnicodeFlags, kbts_glyph **Match, int Backward) { kbts_glyph *MatchingGlyph = 0; @@ -20170,7 +18710,7 @@ static void kbts__AttachGlyph(kbts_glyph_storage *Storage, kbts_glyph *Parent, k Child->OffsetX = X; Child->OffsetY = Y; - Child->Flags |= KBTS_GLYPH_FLAG_USED_IN_GPOS; + Child->Flags |= KBTS_GLYPH_FLAG_USED_IN_GPOS | KBTS_GLYPH_FLAG_NO_BREAK; Child->AttachGlyph = Parent; Parent->Flags |= KBTS_GLYPH_FLAG_USED_IN_GPOS; @@ -20197,7 +18737,7 @@ static void kbts__AttachGlyph(kbts_glyph_storage *Storage, kbts_glyph *Parent, k } } -static void kbts__SetLookupOnePastLastGlyph(kbts__shape_scratchpad *Scratchpad, kbts_un Index, kbts_glyph *Glyph) +static void kbts__SetLookupOnePastLastGlyph(kbts_shape_scratchpad *Scratchpad, kbts_un Index, kbts_glyph *Glyph) { if(Index > Scratchpad->LookupOnePastLastGlyphIndex) { @@ -20225,21 +18765,21 @@ KBTS_INLINE void kbts__SetCursiveFlags(kbts_glyph *Glyph, kbts__cursive_flags Cu Glyph->MarkOrdering = (kbts_u8)CursiveFlags; } -static int kbts__DoSingleAdjustment(kbts__shape_scratchpad *Scratchpad, kbts_shape_config *Config, kbts_glyph_storage *Storage, - kbts_lookup_list *LookupList, kbts_un LookupIndex, kbts_un SubtableIndex, kbts__unpacked_lookup *Lookup, kbts_u16 *Base, - kbts_glyph *CurrentGlyph, kbts_un StartIndex, kbts__skip_flags RequestedSkipFlags) +static kbts_b32 kbts__DoSingleAdjustment(kbts_shape_scratchpad *Scratchpad, kbts_shape_config *Config, kbts_glyph_storage *Storage, + kbts_lookup_list *LookupList, kbts_un LookupIndex, kbts_un SubtableIndex, kbts__unpacked_lookup *Lookup, kbts_u16 *Base, + kbts_glyph *CurrentGlyph, kbts_un StartIndex, kbts__skip_flags RequestedSkipFlags) { KBTS_INSTRUMENT_FUNCTION_BEGIN; - kbts_unicode_flags SkipUnicodeFlags = KBTS_UNICODE_FLAG_DEFAULT_IGNORABLE; + enum{SkipUnicodeFlags = KBTS_UNICODE_FLAG_DEFAULT_IGNORABLE}; kbts__skip_flags RegularSkipFlags = KBTS__SKIP_FLAGS_GPOS_REGULAR(RequestedSkipFlags); kbts__skip_flags SequenceSkipFlags = KBTS__SKIP_FLAGS_GPOS_SEQUENCE(RequestedSkipFlags); - int Result = 0; + kbts_b32 Result = 0; kbts_un OnePastLastGlyphIndex = StartIndex + 1; kbts_glyph *OnePastLastGlyph = CurrentGlyph->Next; - if(kbts__GlyphsIncludedInLookupSubtable(Storage, Config->Font, 1, Lookup, LookupIndex, SubtableIndex, CurrentGlyph, SequenceSkipFlags, SkipUnicodeFlags)) + if(kbts__GlyphIncludedInLookupSubtable(Scratchpad, KBTS_SHAPING_TABLE_GPOS, LookupIndex, SubtableIndex, CurrentGlyph)) { // CAREFUL: We want kbts__unpacked_lookup to be a useful bag-of-arguments type, but, for extension // lookups, each subtable may specify its own lookup type, so we save it here and restore it at @@ -20267,83 +18807,146 @@ static int kbts__DoSingleAdjustment(kbts__shape_scratchpad *Scratchpad, kbts_sha if(Cover.Valid) { kbts_un OnePastLastGlyphOffset = 0; - + switch(Lookup->Type) { case 1: { + kbts_u16 ValueFormat = Base[2]; + kbts__unpacked_value_record Unpacked = KBTS__ZERO; if(Base[0] == 1) { kbts__single_adjustment_1 *Adjust = (kbts__single_adjustment_1 *)Base; - Unpacked = kbts__UnpackValueRecord(Adjust, Adjust->ValueFormat, KBTS__POINTER_AFTER(kbts_u16, Adjust)); + Unpacked = kbts__UnpackValueRecord(Adjust, ValueFormat, KBTS__POINTER_AFTER(kbts_u16, Adjust)); } else if(Base[0] == 2) { kbts__single_adjustment_2 *Adjust = (kbts__single_adjustment_2 *)Base; - kbts_un RecordSize = kbts__PopCount32(Adjust->ValueFormat); + kbts_un RecordSize = kbts__PopCount32(ValueFormat); kbts_u16 *Records = KBTS__POINTER_AFTER(kbts_u16, Adjust); kbts_u16 *Record = Records + RecordSize * Cover.Index; - Unpacked = kbts__UnpackValueRecord(Adjust, Adjust->ValueFormat, Record); + Unpacked = kbts__UnpackValueRecord(Adjust, ValueFormat, Record); } kbts__ApplyValueRecord(CurrentGlyph, &Unpacked); + CurrentGlyph->Flags |= KBTS_GLYPH_FLAG_USED_IN_GPOS; Result = 1; - } - break; + } break; case 2: { + KBTS_INSTRUMENT_BLOCK_BEGIN(PairPositioning); + kbts_glyph *NextGlyph; if(kbts__NextGlyph(Storage, Lookup, CurrentGlyph->Next, RegularSkipFlags, SkipUnicodeFlags, &NextGlyph, 0)) { - kbts_u32 NextGlyphId = NextGlyph->Id; + kbts_u16 NextGlyphId = NextGlyph->Id; - kbts__unpacked_value_record Unpacked1 = KBTS__ZERO; - kbts__unpacked_value_record Unpacked2 = KBTS__ZERO; - int Valid = 0; + kbts_u16 *Unpacked1Base; + kbts_u16 *Unpacked2Base; + kbts_u16 ValueFormat1; + kbts_u16 ValueFormat2 = 0; + kbts_un Size1; + kbts_un Size2; + + { + kbts__pair_adjustment_1 *Adjust = (kbts__pair_adjustment_1 *)Base; + ValueFormat1 = Adjust->ValueFormat1; + ValueFormat2 = Adjust->ValueFormat2; + + Size1 = kbts__PopCount32(ValueFormat1); + Size2 = kbts__PopCount32(ValueFormat2); + } if(Base[0] == 1) { kbts__pair_adjustment_1 *Adjust = (kbts__pair_adjustment_1 *)Base; - kbts_un Size1 = kbts__PopCount32(Adjust->ValueFormat1); - kbts_un Size2 = kbts__PopCount32(Adjust->ValueFormat2); - kbts_u16 *SetOffsets = KBTS__POINTER_AFTER(kbts_u16, Adjust); kbts__pair_set *Set = KBTS__POINTER_OFFSET(kbts__pair_set, Adjust, SetOffsets[Cover.Index]); kbts_un PairRecordSize = Size1 + Size2 + 1; // + 1 because each pair stores the next glyph ID. kbts__pair_value_record *PairRecords = KBTS__POINTER_AFTER(kbts__pair_value_record, Set); - KBTS__FOR(PairIndex, 0, Set->Count) + + kbts_un PairCount = Set->Count; + + KBTS_INSTRUMENT_BLOCK_BEGIN(PairSearch); + + #define KBTS__PAIR_SEARCH(PairSize) \ + kbts_un PairSizeTimesPairIndex = 0; \ + while(PairCount > 1) \ + { \ + kbts_un HalfCount = PairCount / 2; \ + PairSizeTimesPairIndex = (PairRecords[PairSizeTimesPairIndex + HalfCount*PairSize - PairSize].SecondGlyph < NextGlyphId) ? (PairSizeTimesPairIndex + HalfCount*PairSize) : PairSizeTimesPairIndex; \ + PairCount -= HalfCount; \ + } \ + kbts__pair_value_record *PairRecord = PairRecords + PairSizeTimesPairIndex; \ + if(PairRecord->SecondGlyph == NextGlyphId) \ + { \ + kbts_u16 *Records = KBTS__POINTER_AFTER(kbts_u16, PairRecord); \ + Unpacked1Base = Records; \ + Unpacked2Base = Records + Size1; \ + KBTS_INSTRUMENT_BLOCK_END(PairSearch); \ + goto ApplyPairPositioning; \ + } + + if(PairRecordSize == 2) { - kbts__pair_value_record *PairRecord = PairRecords + PairRecordSize * PairIndex; + KBTS__PAIR_SEARCH(2); + } + else if(PairRecordSize == 3) + { + KBTS__PAIR_SEARCH(3); + } + else + { + kbts__pair_value_record *PairRecord = PairRecords; + + while(PairCount > 1) + { + kbts_un HalfCount = PairCount / 2; + + kbts__pair_value_record *OnePastProbe = PairRecord + HalfCount * PairRecordSize; + kbts__pair_value_record *Probe = OnePastProbe - PairRecordSize; + + if(Probe->SecondGlyph < NextGlyphId) + { + PairRecord = OnePastProbe; + } + + PairCount -= HalfCount; + } if(PairRecord->SecondGlyph == NextGlyphId) { kbts_u16 *Records = KBTS__POINTER_AFTER(kbts_u16, PairRecord); - Unpacked1 = kbts__UnpackValueRecord(Adjust, Adjust->ValueFormat1, Records); - Records += Unpacked1.Size; - Unpacked2 = kbts__UnpackValueRecord(Adjust, Adjust->ValueFormat2, Records); - Valid = 1; + Unpacked1Base = Records; + Unpacked2Base = Records + Size1; + KBTS_INSTRUMENT_BLOCK_END(PairSearch); + + goto ApplyPairPositioning; } } + + #undef KBTS__PAIR_SEARCH + KBTS_INSTRUMENT_BLOCK_END(PairSearch); } else if(Base[0] == 2) { + KBTS_INSTRUMENT_BLOCK_BEGIN(PairClasses); + kbts__pair_adjustment_2 *Adjust = (kbts__pair_adjustment_2 *)Base; kbts_u16 *ClassDef1 = KBTS__POINTER_OFFSET(kbts_u16, Adjust, Adjust->ClassDefinition1Offset); kbts_u16 *ClassDef2 = KBTS__POINTER_OFFSET(kbts_u16, Adjust, Adjust->ClassDefinition2Offset); - kbts_un Size1 = kbts__PopCount32(Adjust->ValueFormat1); - kbts_un Size2 = kbts__PopCount32(Adjust->ValueFormat2); kbts_un PairRecordSize = Size1 + Size2; kbts_u16 *PairRecords = KBTS__POINTER_AFTER(kbts_u16, Adjust); @@ -20354,35 +18957,54 @@ static int kbts__DoSingleAdjustment(kbts__shape_scratchpad *Scratchpad, kbts_sha // class definition table, then we should skip the lookup. // However, this seems wrong in practice. Undefined classes seem to just default to 0, and then // the bounds check takes care of deciding whether the lookup is okay to apply or not. + // @Speed: Interleave these lookups. kbts__glyph_class_from_table_result Class1 = kbts__GlyphClassFromTable(ClassDef1, CurrentGlyph->Id); kbts__glyph_class_from_table_result Class2 = kbts__GlyphClassFromTable(ClassDef2, NextGlyphId); - if((Class1.Class < Adjust->Class1Count) && (Class2.Class < Adjust->Class2Count)) + + if((Class1.Class < Adjust->Class1Count) && + (Class2.Class < Adjust->Class2Count)) { kbts_u16 *PairRecord = PairRecords + Class1.Class * PairRecordSize * Adjust->Class2Count + Class2.Class * PairRecordSize; - Unpacked1 = kbts__UnpackValueRecord(Adjust, Adjust->ValueFormat1, PairRecord); - PairRecord += Size1; + Unpacked1Base = PairRecord; + Unpacked2Base = PairRecord + Size1; - Unpacked2 = kbts__UnpackValueRecord(Adjust, Adjust->ValueFormat2, PairRecord); - PairRecord += Size2; - Valid = 1; + KBTS_INSTRUMENT_BLOCK_END(PairClasses); + goto ApplyPairPositioning; } + + KBTS_INSTRUMENT_BLOCK_END(PairClasses); } - if(Valid) + if(0) { - kbts__ApplyValueRecord(CurrentGlyph, &Unpacked1); - CurrentGlyph->Flags |= KBTS_GLYPH_FLAG_USED_IN_GPOS; + ApplyPairPositioning:; - kbts__ApplyValueRecord(NextGlyph, &Unpacked2); + KBTS_INSTRUMENT_BLOCK_BEGIN(ApplyPairPositioning); + + kbts__unpacked_value_record Unpacked1 = kbts__UnpackValueRecord(Base, ValueFormat1, Unpacked1Base); + kbts__ApplyValueRecord(CurrentGlyph, &Unpacked1); + + CurrentGlyph->Flags |= KBTS_GLYPH_FLAG_USED_IN_GPOS; NextGlyph->Flags |= KBTS_GLYPH_FLAG_USED_IN_GPOS; + OnePastLastGlyph = CurrentGlyph->Next; + + if(ValueFormat2) + { + kbts__unpacked_value_record Unpacked2 = kbts__UnpackValueRecord(Base, ValueFormat2, Unpacked2Base); + kbts__ApplyValueRecord(NextGlyph, &Unpacked2); + OnePastLastGlyph = NextGlyph->Next; + } + + KBTS_INSTRUMENT_BLOCK_END(ApplyPairPositioning); + Result = 1; - OnePastLastGlyph = Unpacked2.Size ? NextGlyph->Next : CurrentGlyph->Next; } } - } - break; + + KBTS_INSTRUMENT_BLOCK_END(PairPositioning); + } break; // All three types of attachment (cursive, mark-to-base, mark-to-ligature) look backward instead of forward. case 3: @@ -20552,8 +19174,7 @@ static int kbts__DoSingleAdjustment(kbts__shape_scratchpad *Scratchpad, kbts_sha } } } - } - break; + } break; case 4: case 6: @@ -20567,12 +19188,14 @@ static int kbts__DoSingleAdjustment(kbts__shape_scratchpad *Scratchpad, kbts_sha // We have to take that into account here, and only accumulate mark advances for shapers that // don't do that zeroing at the end (either because they do it at the beginning of GPOS, or // because they don't do it at all). - int CountMarkAdvances = !kbts__ShaperClearsMarkAdvancesInPostGposFixup(Config->Shaper); + kbts_b32 CountMarkAdvances = !kbts__ShaperClearsMarkAdvancesInPostGposFixup(Config->Shaper); kbts__coverage *BaseCoverage = KBTS__POINTER_OFFSET(kbts__coverage, Adjust, Adjust->BaseCoverageOffset); + kbts_un MarkClassCount = Adjust->MarkClassCount; kbts_s32 AdvanceSinceBaseX = 0; kbts_s32 AdvanceSinceBaseY = 0; kbts_u32 BaseClasses = Lookup->Type == 6 ? (1 << KBTS__GLYPH_CLASS_MARK) : (1 | (1 << KBTS__GLYPH_CLASS_BASE) | (1 << KBTS__GLYPH_CLASS_LIGATURE) | (1 << KBTS__GLYPH_CLASS_COMPONENT)); kbts_glyph *BaseGlyph = 0; + for(kbts_glyph *PrevGlyph = CurrentGlyph->Prev; kbts__GlyphIsValid(Storage, PrevGlyph); PrevGlyph = PrevGlyph->Prev) { if(CountMarkAdvances || (PrevGlyph->Classes.Class != KBTS__GLYPH_CLASS_MARK)) @@ -20600,8 +19223,12 @@ static int kbts__DoSingleAdjustment(kbts__shape_scratchpad *Scratchpad, kbts_sha (PrevGlyph->Prev->Classes.Class != KBTS__GLYPH_CLASS_MARK)) // Stop if we see any mark. { // Otherwise, we allow skipping uncovered glyphs. + kbts_b32 ValidBase = 0; + kbts__cover_glyph_result BaseCover = kbts__CoverGlyph(BaseCoverage, PrevGlyph->Id); - if(BaseCover.Valid) + ValidBase = BaseCover.Valid; + + if(ValidBase) { BaseGlyph = PrevGlyph; break; @@ -20623,18 +19250,20 @@ static int kbts__DoSingleAdjustment(kbts__shape_scratchpad *Scratchpad, kbts_sha if(BaseGlyph) { - int Ok = (Lookup->Type == 4) || // This is a mark-to-base attachment - ((BaseGlyph->LigatureUid == CurrentGlyph->LigatureUid) && (BaseGlyph->LigatureComponentIndexPlusOne == CurrentGlyph->LigatureComponentIndexPlusOne)) || // This is a mark-to-mark attachment, and both marks belong to the same ligature component - ((BaseGlyph->Flags | CurrentGlyph->Flags) & KBTS_GLYPH_FLAG_LIGATURE); // This is a mark-to-mark attachment, and either mark was created by a ligature substitution + kbts_b32 Ok = (Lookup->Type == 4) || // This is a mark-to-base attachment + ((BaseGlyph->LigatureUid == CurrentGlyph->LigatureUid) && (BaseGlyph->LigatureComponentIndexPlusOne == CurrentGlyph->LigatureComponentIndexPlusOne)) || // This is a mark-to-mark attachment, and both marks belong to the same ligature component + ((BaseGlyph->Flags | CurrentGlyph->Flags) & KBTS_GLYPH_FLAG_LIGATURE); // This is a mark-to-mark attachment, and either mark was created by a ligature substitution if(Ok) { // @Speed: This is duplicating work in the :MultipleSubstSadness case. kbts__cover_glyph_result BaseCover = kbts__CoverGlyph(BaseCoverage, BaseGlyph->Id); + kbts_un BaseIndex = BaseCover.Index; + kbts_b32 ValidBase = BaseCover.Valid; - if(BaseCover.Valid) + if(ValidBase) { kbts__base_array *BaseArray = KBTS__POINTER_OFFSET(kbts__base_array, Adjust, Adjust->BaseArrayOffset); - kbts_u16 *BaseAnchorOffsets = KBTS__POINTER_AFTER(kbts_u16, BaseArray) + BaseCover.Index * Adjust->MarkClassCount; + kbts_u16 *BaseAnchorOffsets = KBTS__POINTER_AFTER(kbts_u16, BaseArray) + BaseIndex * MarkClassCount; kbts__mark_info MarkInfo = kbts__GetMarkInfo(Adjust, Adjust->MarkArrayOffset, Cover.Index); kbts_u16 BaseAnchorOffset = BaseAnchorOffsets[MarkInfo.Record->Class]; @@ -20658,13 +19287,11 @@ static int kbts__DoSingleAdjustment(kbts__shape_scratchpad *Scratchpad, kbts_sha } } } - } - break; + } break; case 5: { kbts__mark_to_ligature_attachment *Adjust = (kbts__mark_to_ligature_attachment *)Base; - kbts_s32 AdvanceSinceBaseX = 0; kbts_s32 AdvanceSinceBaseY = 0; kbts_u32 BaseClasses = (1 | (1 << KBTS__GLYPH_CLASS_BASE) | (1 << KBTS__GLYPH_CLASS_LIGATURE) | (1 << KBTS__GLYPH_CLASS_COMPONENT)); @@ -20675,21 +19302,28 @@ static int kbts__DoSingleAdjustment(kbts__shape_scratchpad *Scratchpad, kbts_sha { AdvanceSinceBaseX += PrevGlyph->AdvanceX; AdvanceSinceBaseY += PrevGlyph->AdvanceY; + if(!kbts__SkipGlyph(PrevGlyph, Lookup, RegularSkipFlags, SkipUnicodeFlags) && ((1 << PrevGlyph->Classes.Class) & BaseClasses)) { LigatureGlyph = PrevGlyph; + break; } } if(LigatureGlyph) { + kbts__ligature_array *LigatureArray = KBTS__POINTER_OFFSET(kbts__ligature_array, Adjust, Adjust->LigatureArrayOffset); + kbts__cover_glyph_result LigatureCover = kbts__CoverGlyph(KBTS__POINTER_OFFSET(kbts__coverage, Adjust, Adjust->LigatureCoverageOffset), - LigatureGlyph->Id); - if(LigatureCover.Valid) + LigatureGlyph->Id); + + kbts_un LigatureIndex = LigatureCover.Index; + kbts_b32 ValidLigature = LigatureCover.Valid; + + if(ValidLigature) { - kbts__ligature_array *LigatureArray = KBTS__POINTER_OFFSET(kbts__ligature_array, Adjust, Adjust->LigatureArrayOffset); - kbts__ligature_attach *LigatureAttach = kbts__GetLigatureAttach(LigatureArray, LigatureCover.Index); + kbts__ligature_attach *LigatureAttach = kbts__GetLigatureAttach(LigatureArray, LigatureIndex); kbts__mark_info MarkInfo = kbts__GetMarkInfo(Adjust, Adjust->MarkArrayOffset, Cover.Index); kbts_un LigatureComponentIndexPlusOne = CurrentGlyph->LigatureComponentIndexPlusOne; @@ -20737,8 +19371,7 @@ static int kbts__DoSingleAdjustment(kbts__shape_scratchpad *Scratchpad, kbts_sha } } } - } - break; + } break; case 7: case 8: @@ -20746,11 +19379,13 @@ static int kbts__DoSingleAdjustment(kbts__shape_scratchpad *Scratchpad, kbts_sha kbts__sequence_lookup_result SequenceLookup = kbts__DoSequenceLookup(Storage, Lookup, Base, Cover, CurrentGlyph, SequenceSkipFlags, SkipUnicodeFlags); if(SequenceLookup.RecordCount) { + kbts__gdef *Gdef = kbts__BlobTableDataType(Config->Font->Blob, KBTS_BLOB_TABLE_ID_GDEF, kbts__gdef); + KBTS__FOR(RecordIndex, 0, SequenceLookup.RecordCount) { kbts__sequence_lookup_record *Record = &SequenceLookup.Records[RecordIndex]; kbts__lookup *PackedRecordLookup = kbts__GetLookup(LookupList, Record->LookupListIndex); - kbts__unpacked_lookup RecordLookup = kbts__UnpackLookup(kbts__BlobTableDataType(Config->Font->Blob, KBTS_BLOB_TABLE_ID_GDEF, kbts__gdef), PackedRecordLookup); + kbts__unpacked_lookup RecordLookup = kbts__UnpackLookup(Gdef, PackedRecordLookup); kbts_glyph *NestedCurrentGlyph = CurrentGlyph; kbts_un NestedCurrentGlyphIndex = 0; @@ -20781,16 +19416,15 @@ static int kbts__DoSingleAdjustment(kbts__shape_scratchpad *Scratchpad, kbts_sha kbts_u16 *NestedBase = KBTS__POINTER_OFFSET(kbts_u16, PackedRecordLookup, RecordLookup.SubtableOffsets[NestedSubtableIndex]); kbts__DoSingleAdjustment(Scratchpad, Config, Storage, LookupList, - Record->LookupListIndex, NestedSubtableIndex, &RecordLookup, NestedBase, - NestedCurrentGlyph, StartIndex + NestedCurrentGlyphIndex, RequestedSkipFlags); + Record->LookupListIndex, NestedSubtableIndex, &RecordLookup, NestedBase, + NestedCurrentGlyph, StartIndex + NestedCurrentGlyphIndex, RequestedSkipFlags); } } OnePastLastGlyphIndex = StartIndex + SequenceLookup.InputSequenceCountIncludingSkippedGlyphs; OnePastLastGlyph = SequenceLookup.OnePastLastGlyphMatched; } - } - break; + } break; } kbts__SetLookupOnePastLastGlyph(Scratchpad, OnePastLastGlyphIndex + OnePastLastGlyphOffset, OnePastLastGlyph); @@ -20915,27 +19549,40 @@ enum kbts__substitution_result_flags_enum { KBTS__SUBSTITUTION_RESULT_FLAG_MATCHED_SUBSTITUTION = (1 << 0), KBTS__SUBSTITUTION_RESULT_FLAG_TRIED_TO_INSERT_WHILE_CHECK_ONLY = (1 << 1), + KBTS__SUBSTITUTION_RESULT_FLAG_MATCHED_SEQUENCE = (1 << 2), }; -static void kbts__BeginLookupApplication(kbts__shape_scratchpad *Scratchpad, kbts_glyph *Glyph) +static void kbts__BeginLookupApplication(kbts_shape_scratchpad *Scratchpad, kbts_glyph *Glyph) { Scratchpad->LookupOnePastLastGlyph = Glyph->Next; Scratchpad->LookupOnePastLastGlyphIndex = 0; } - -static kbts_glyph *kbts__EndLookupApplication(kbts__shape_scratchpad *Scratchpad) +static kbts_glyph *kbts__EndLookupApplication(kbts_shape_scratchpad *Scratchpad) { kbts_glyph *Result = Scratchpad->LookupOnePastLastGlyph; return Result; } -static kbts__substitution_result_flags kbts__DoSubstitution(kbts__shape_scratchpad *Scratchpad, kbts_shape_config *Config, kbts_glyph_storage *Storage, - kbts_lookup_list *LookupList, kbts__gsub_frame *Frames, kbts_un *FrameCount_, - int CheckOnly, kbts__skip_flags RequestedSkipFlags, kbts_u32 GeneratedGlyphFlags) +static kbts_un kbts__CurrentBakedFeatureStageIndex(kbts_shape_scratchpad *Scratchpad) +{ + kbts_un Result = Scratchpad->FeatureStagesRead - 1; + return Result; +} + +KBTS_INLINE kbts_u16 kbts__NextGlyphUid(kbts_shape_scratchpad *Scratchpad) +{ + kbts_u16 Result = (kbts_u16)++Scratchpad->NextGlyphUid; + return Result; +} + +static kbts__substitution_result_flags kbts__DoSubstitution(kbts_shape_scratchpad *Scratchpad, kbts_shape_config *Config, kbts_glyph_storage *Storage, + kbts_lookup_list *LookupList, kbts_un SequentialLookupIndex, kbts_u16 FeatureValue, + kbts__gsub_frame *Frames, kbts_un *FrameCount_, + int CheckOnly, kbts__skip_flags RequestedSkipFlags, kbts_u32 GeneratedGlyphFlags) { kbts__substitution_result_flags Result = 0; kbts_font *Font = Config->Font; - kbts_unicode_flags SkipUnicodeFlags = 0; // @Incomplete + enum {SkipUnicodeFlags = KBTS_UNICODE_FLAG_DEFAULT_IGNORABLE}; kbts__skip_flags RegularSkipFlags = KBTS__SKIP_FLAGS_GSUB_REGULAR(RequestedSkipFlags); kbts__skip_flags SequenceSkipFlags = KBTS__SKIP_FLAGS_GSUB_SEQUENCE(RequestedSkipFlags); GeneratedGlyphFlags |= KBTS_GLYPH_FLAG_GENERATED_BY_GSUB; @@ -20947,6 +19594,8 @@ static kbts__substitution_result_flags kbts__DoSubstitution(kbts__shape_scratchp kbts__unpacked_lookup Lookup = kbts__UnpackLookup(kbts__BlobTableDataType(Font->Blob, KBTS_BLOB_TABLE_ID_GDEF, kbts__gdef), PackedLookup); kbts_u16 BaseLookupType = Lookup.Type; + kbts_glyph *CurrentGlyph = Frame->InputGlyph; + while(Frame->SubtableIndex < Lookup.SubtableCount) { kbts_u16 *Subtable = KBTS__POINTER_OFFSET(kbts_u16, PackedLookup, Lookup.SubtableOffsets[Frame->SubtableIndex]); @@ -20962,10 +19611,7 @@ static kbts__substitution_result_flags kbts__DoSubstitution(kbts__shape_scratchp Subtable = KBTS__POINTER_OFFSET(kbts_u16, Extension, Extension->Offset); } - kbts_glyph *CurrentGlyph = Frame->InputGlyph; - - kbts__skip_flags SkipFlags = (Lookup.Type >= 5) ? SequenceSkipFlags : RegularSkipFlags; - if(kbts__GlyphsIncludedInLookupSubtable(Storage, Font, 0, &Lookup, Frame->LookupIndex, Frame->SubtableIndex, CurrentGlyph, SkipFlags, SkipUnicodeFlags)) + if(kbts__GlyphIncludedInLookupSubtable(Scratchpad, KBTS_SHAPING_TABLE_GSUB, Frame->LookupIndex, Frame->SubtableIndex, CurrentGlyph)) { kbts__cover_glyph_result Cover = KBTS__ZERO; Cover.Valid = kbts__GlyphPassesLookupFilter(CurrentGlyph, &Lookup); @@ -21023,7 +19669,7 @@ static kbts__substitution_result_flags kbts__DoSubstitution(kbts__shape_scratchp NewId = SubstituteGlyphIds[Cover.Index]; } - kbts__GsubMutate(Font, CurrentGlyph, NewId, GeneratedGlyphFlags); + kbts__GsubMutate(Scratchpad, Font, CurrentGlyph, SequentialLookupIndex, NewId, GeneratedGlyphFlags); } } break; @@ -21043,9 +19689,13 @@ static kbts__substitution_result_flags kbts__DoSubstitution(kbts__shape_scratchp { kbts_glyph OriginalGlyph = *CurrentGlyph; kbts_glyph *LastInsert = CurrentGlyph; + + kbts_un RunningSortKey = CurrentGlyph->SortKey; + kbts_un SortKeyInterval = CurrentGlyph->SortKeyInterval / Sequence->GlyphCount; KBTS__FOR(SubstGlyphIndex, 0, Sequence->GlyphCount) { kbts_u32 NewGlyphFlags = GeneratedGlyphFlags | KBTS_GLYPH_FLAG_MULTIPLE_SUBSTITUTION; + kbts_u16 NewGlyphId = kbts__ReadU16Unaligned(&SubstGlyphIds[SubstGlyphIndex]); kbts_glyph *NewGlyph = CurrentGlyph; if(SubstGlyphIndex) @@ -21058,15 +19708,20 @@ static kbts__substitution_result_flags kbts__DoSubstitution(kbts__shape_scratchp return Result; } - NewGlyph->Uid = (kbts_u16)++Scratchpad->NextGlyphUid; + NewGlyph->Uid = kbts__NextGlyphUid(Scratchpad); } else { NewGlyphFlags |= KBTS_GLYPH_FLAG_FIRST_IN_MULTIPLE_SUBSTITUTION; } - kbts__GsubMutate(Font, NewGlyph, SubstGlyphIds[SubstGlyphIndex], GeneratedGlyphFlags | NewGlyphFlags); + NewGlyph->SortKey = (kbts_u32)RunningSortKey; + NewGlyph->SortKeyInterval = (kbts_u32)SortKeyInterval; + + kbts__GsubMutate(Scratchpad, Font, NewGlyph, SequentialLookupIndex, NewGlyphId, GeneratedGlyphFlags | NewGlyphFlags); + LastInsert = NewGlyph; + RunningSortKey += SortKeyInterval; } OnePastLastGlyph = LastInsert->Next; @@ -21105,76 +19760,21 @@ static kbts__substitution_result_flags kbts__DoSubstitution(kbts__shape_scratchp kbts__alternate_set *Set = kbts__GetAlternateSet(Subst, Cover.Index); kbts_u16 *AltGlyphIds = KBTS__POINTER_AFTER(kbts_u16, Set); - kbts_un AlternateIndex = 0; - - { - kbts__feature_set *ShaperFeatures = &Config->Features; - kbts_glyph_config *GlyphConfig = CurrentGlyph->Config; - if(GlyphConfig) - { - int HasOverride = 0; - KBTS__FOR(WordIndex, 0, KBTS__ARRAY_LENGTH(GlyphConfig->EnabledFeatures.Flags)) - { - kbts_u64 Flags = GlyphConfig->EnabledFeatures.Flags[WordIndex] & ShaperFeatures->Flags[WordIndex]; - if(Flags) - { - HasOverride = 1; - break; - } - } - - if(HasOverride) - { - for(kbts__feature_override_header *OverrideHeader = GlyphConfig->FeatureOverrideSentinel.Next; - OverrideHeader != &GlyphConfig->FeatureOverrideSentinel; - OverrideHeader = OverrideHeader->Next) - { - kbts__feature_override *Override = (kbts__feature_override *)OverrideHeader; - int Value = Override->Value; - kbts__feature_id Id = kbts__FeatureTagToId(Override->Tag); - - if(Value && - kbts__ContainsFeature(&Scratchpad->LookupFeatures, Id)) - { - int Match = 1; - if(!Id) - { - // Slow path for unregistered features. - Match = 0; - KBTS__FOR(UnregisteredFeatureIndex, 0, Scratchpad->UnregisteredFeatureCount) - { - if(Override->Tag == Scratchpad->UnregisteredFeatureTags[UnregisteredFeatureIndex]) - { - Match = 1; - break; - } - } - } - - if(Match) - { - AlternateIndex = (kbts_un)(Value - 1); - - break; - } - } - } - } - } - } - + kbts_un AlternateIndex = FeatureValue - 1; if(AlternateIndex >= Set->GlyphCount) { AlternateIndex = 0; } kbts_u16 NewId = AltGlyphIds[AlternateIndex]; - kbts__GsubMutate(Font, CurrentGlyph, NewId, GeneratedGlyphFlags); + kbts__GsubMutate(Scratchpad, Font, CurrentGlyph, SequentialLookupIndex, NewId, GeneratedGlyphFlags); } } break; case 4: { + KBTS_INSTRUMENT_BLOCK_BEGIN(GSUB_Ligature); + kbts__ligature_substitution *Subst = (kbts__ligature_substitution *)Subtable; kbts__ligature_set *Set = kbts__GetLigatureSet(Subst, Cover.Index); @@ -21182,17 +19782,19 @@ static kbts__substitution_result_flags kbts__DoSubstitution(kbts__shape_scratchp { kbts__ligature *Ligature = kbts__GetLigature(Set, LigatureIndex); kbts_u16 *ComponentIds = KBTS__POINTER_AFTER(kbts_u16, Ligature); + kbts_un ComponentCount = Ligature->ComponentCount; kbts_un MatchingGlyphCount = 1; { kbts_glyph *LigatureGlyphCursor = CurrentGlyph->Next; - while(kbts__GlyphIsValid(Storage, LigatureGlyphCursor) && (MatchingGlyphCount < Ligature->ComponentCount)) + while(kbts__GlyphIsValid(Storage, LigatureGlyphCursor) && + (MatchingGlyphCount < Ligature->ComponentCount)) { // A ligature may contain an explicit ZWJ, which SkipGlyph() would probably skip. // The expected behavior in that case is to assume the font designer knows what they are doing // and match the ZWJ. - if(LigatureGlyphCursor->Id == ComponentIds[MatchingGlyphCount - 1]) + if(LigatureGlyphCursor->Id == kbts__ReadU16Unaligned(&ComponentIds[MatchingGlyphCount - 1])) { MatchingGlyphCount += 1; } @@ -21205,14 +19807,13 @@ static kbts__substitution_result_flags kbts__DoSubstitution(kbts__shape_scratchp } } - if(MatchingGlyphCount == Ligature->ComponentCount) + if(MatchingGlyphCount == ComponentCount) { Result |= KBTS__SUBSTITUTION_RESULT_FLAG_MATCHED_SUBSTITUTION; if(!CheckOnly) { - kbts_u32 LigatureUid = ++Scratchpad->NextGlyphUid; - kbts_un ComponentCount = Ligature->ComponentCount; + kbts_u32 LigatureUid = kbts__NextGlyphUid(Scratchpad); { // For glyphs that aren't part of the ligature, store which component it is attached to. // For glyphs that _are_, eat them. @@ -21320,7 +19921,7 @@ static kbts__substitution_result_flags kbts__DoSubstitution(kbts__shape_scratchp // Currently, we only take the main glyph's config into account while making the ligature's config. // Maybe we should merge all of the components' configs into one instead? - kbts__GsubMutate(Font, CurrentGlyph, Ligature->Glyph, GeneratedGlyphFlags | KBTS_GLYPH_FLAG_LIGATURE); + kbts__GsubMutate(Scratchpad, Font, CurrentGlyph, SequentialLookupIndex, Ligature->Glyph, GeneratedGlyphFlags | KBTS_GLYPH_FLAG_LIGATURE); CurrentGlyph->Uid = (kbts_u16)LigatureUid; CurrentGlyph->LigatureUid = (kbts_u16)LigatureUid; CurrentGlyph->LigatureComponentCount = (kbts_u16)ComponentCount; @@ -21332,6 +19933,8 @@ static kbts__substitution_result_flags kbts__DoSubstitution(kbts__shape_scratchp break; } } + + KBTS_INSTRUMENT_BLOCK_END(GSUB_Ligature); } break; case 8: @@ -21344,13 +19947,13 @@ static kbts__substitution_result_flags kbts__DoSubstitution(kbts__shape_scratchp { // Should we use regular or sequence skip flags here? kbts__sequence_match BacktrackMatch = kbts__MatchCoverageSequence(Storage, &Lookup, RegularSkipFlags, SkipUnicodeFlags, Subst, Unpacked.BacktrackCoverageOffsets, Unpacked.BacktrackCount, - CurrentGlyph->Prev, 1); + CurrentGlyph->Prev, 1); kbts__sequence_match LookaheadMatch = kbts__MatchCoverageSequence(Storage, &Lookup, RegularSkipFlags, SkipUnicodeFlags, Subst, Unpacked.LookaheadCoverageOffsets, Unpacked.LookaheadCount, - CurrentGlyph->Next, 0); + CurrentGlyph->Next, 0); if((BacktrackMatch.MatchCount == Unpacked.BacktrackCount) && (LookaheadMatch.MatchCount == Unpacked.LookaheadCount)) { Result |= KBTS__SUBSTITUTION_RESULT_FLAG_MATCHED_SUBSTITUTION; - kbts__GsubMutate(Font, CurrentGlyph, Unpacked.SubstituteGlyphIds[Cover.Index], GeneratedGlyphFlags); + kbts__GsubMutate(Scratchpad, Font, CurrentGlyph, SequentialLookupIndex, Unpacked.SubstituteGlyphIds[Cover.Index], GeneratedGlyphFlags); } } } break; @@ -21445,7 +20048,7 @@ static void kbts__PopGlyphList(kbts_glyph_storage *Storage, kbts__glyph_list *Li { List->OneBeforeFirst->Next = Storage->GlyphSentinel.Next; List->OnePastLast->Prev = Storage->GlyphSentinel.Prev; - + // Storage->GlyphSentinel.Prev->Next = Storage->GlyphSentinel.Next->Prev = &Storage->GlyphSentinel; if((kbts_glyph *)&Storage->GlyphSentinel != List->OneBeforeFirst) @@ -21462,7 +20065,7 @@ static void kbts__PopGlyphList(kbts_glyph_storage *Storage, kbts__glyph_list *Li *List = KBTS__ZERO_TYPE(kbts__glyph_list); } -static int kbts__WouldSubstitute(kbts__shape_scratchpad *Scratchpad, kbts_shape_config *Config, kbts_glyph_storage *Storage, +static int kbts__WouldSubstitute(kbts_shape_scratchpad *Scratchpad, kbts_shape_config *Config, kbts_glyph_storage *Storage, kbts_lookup_list *LookupList, kbts__gsub_frame *Frames, kbts__feature *Feature, kbts__skip_flags SkipFlags, kbts_glyph *Glyphs, kbts_un GlyphCount) @@ -21508,7 +20111,7 @@ static int kbts__WouldSubstitute(kbts__shape_scratchpad *Scratchpad, kbts_shape_ while(FrameCount) { - kbts__substitution_result_flags SubstitutionResult = kbts__DoSubstitution(Scratchpad, Config, Storage, LookupList, Frames, &FrameCount, 1, SkipFlags, 0); + kbts__substitution_result_flags SubstitutionResult = kbts__DoSubstitution(Scratchpad, Config, Storage, LookupList, 0, 0, Frames, &FrameCount, 1, SkipFlags, 0); if(SubstitutionResult & KBTS__SUBSTITUTION_RESULT_FLAG_MATCHED_SUBSTITUTION) { @@ -21527,120 +20130,6 @@ Done:; return Result; } -static int kbts__NextLookupIndex(kbts__shape_scratchpad *Scratchpad, kbts_shape_config *Config, kbts_un *LookupIndex_, kbts_u32 *SkipFlags_, kbts_u32 *GlyphFilter_, kbts__feature_set *FeatureSet_) -{ - int Result = 0; - - kbts_un FeatureStageIndex = Scratchpad->FeatureStagesRead - 1; - kbts_u32 SkipFlags = 0; - kbts_u32 GlyphFilter = 0; - Scratchpad->UnregisteredFeatureCount = 0; - kbts_un ResultLookupIndex = 0; - kbts__baked_feature_stage *FeatureStage = &Config->FeatureStages[FeatureStageIndex]; - - *FeatureSet_ = KBTS__ZERO_TYPE(kbts__feature_set); - - while(Scratchpad->FeatureIndexIndex < FeatureStage->FeatureIndexCount) - { - KBTS_ASSERT(Scratchpad->FeatureIndexIndex < FeatureStage->FeatureIndexCount); - - kbts_u16 FeatureIndex = FeatureStage->FeatureIndices[Scratchpad->FeatureIndexIndex]; - KBTS_ASSERT(FeatureIndex < FeatureStage->FeatureIndexCount); - - kbts__baked_feature *Feature = &FeatureStage->Features[FeatureIndex]; - kbts_un Offset = Scratchpad->BakedLookupSubtablesRead[FeatureIndex]; - - KBTS_ASSERT(Offset < Feature->Count); - kbts_un LookupIndex = Feature->Indices[Offset]; - - if(!Result) - { - ResultLookupIndex = LookupIndex; - Result = 1; - } - - if(LookupIndex == ResultLookupIndex) - { - kbts__AddFeature(FeatureSet_, Feature->FeatureId); - SkipFlags |= Feature->SkipFlags; - GlyphFilter |= Feature->GlyphFilter; - - if(!Feature->FeatureId && (Scratchpad->UnregisteredFeatureCount < KBTS_MAX_SIMULTANEOUS_FEATURES)) - { - Scratchpad->UnregisteredFeatureTags[Scratchpad->UnregisteredFeatureCount++] = Feature->FeatureTag; - } - - Scratchpad->FeatureIndexIndex += 1; - Scratchpad->BakedLookupSubtablesRead[FeatureIndex] += 1; - } - else - { - break; - } - } - - *LookupIndex_ = ResultLookupIndex; - *SkipFlags_ = SkipFlags; - *GlyphFilter_ = GlyphFilter; - - return Result; -} - -static int kbts__ConfigAllowsFeatures(kbts__shape_scratchpad *Scratchpad, kbts_shape_config *Config, kbts_glyph_config *GlyphConfig, kbts__feature_set *Features) -{ - kbts_glyph_config DummyGlyphConfig = KBTS__ZERO; - kbts_u64 UserEnabled = 0; // Whether the user enabled _any_ feature corresponding to this lookup. - kbts_u64 UserDisabled = 1; // Whether the user disabled _all_ features corresponding to this lookup. - kbts_u64 DefaultEnabled = 0; // Whether any feature is non-user. - - if(!GlyphConfig) - { - GlyphConfig = &DummyGlyphConfig; - } - - kbts_u64 Mask = 1; // Ignore unregistered features in the broad pass. - KBTS__FOR(WordIndex, 0, KBTS__ARRAY_LENGTH(GlyphConfig->EnabledFeatures.Flags)) - { - kbts_u64 LookupFeatureFlags = Features->Flags[WordIndex] & ~Mask; - Mask = 0; - - UserEnabled |= GlyphConfig->EnabledFeatures.Flags[WordIndex] & LookupFeatureFlags; - UserDisabled &= (GlyphConfig->DisabledFeatures.Flags[WordIndex] & LookupFeatureFlags) == LookupFeatureFlags; - DefaultEnabled |= Features->Flags[WordIndex] & Config->Features.Flags[WordIndex]; - } - - if(Features->Flags[0] & (GlyphConfig->EnabledFeatures.Flags[0] | GlyphConfig->DisabledFeatures.Flags[0]) & KBTS__FEATURE_FLAG0(UNREGISTERED)) - { - // Slow path for unregistered features. - for(kbts__feature_override_header *Header = GlyphConfig->FeatureOverrideSentinel.Next; - Header != &GlyphConfig->FeatureOverrideSentinel; - Header = Header->Next) - { - kbts__feature_override *Override = (kbts__feature_override *)Header; - kbts__feature_id Id = kbts__FeatureTagToId(Override->Tag); - - if(Id == KBTS__FEATURE_ID_UNREGISTERED) - { - kbts_feature_tag OverrideTag = Override->Tag; - - KBTS__FOR(UnregisteredFeatureIndex, 0, Scratchpad->UnregisteredFeatureCount) - { - if(OverrideTag == Scratchpad->UnregisteredFeatureTags[UnregisteredFeatureIndex]) - { - UserEnabled |= (kbts_u32)Override->Value; - UserDisabled &= !Override->Value; - break; - } - } - } - } - } - - int Result = (!UserDisabled && (DefaultEnabled || UserEnabled)); - - return Result; -} - static void kbts__DllistReverseSublist(kbts_glyph *First, kbts_glyph *OnePastLast) { kbts_glyph *OneBeforeFirst = First->Prev; @@ -21672,9 +20161,28 @@ static void kbts__DllistReverseSublist(kbts_glyph *First, kbts_glyph *OnePastLas } } -static void kbts__ExecuteOp(kbts__shape_scratchpad *Scratchpad, kbts_shape_config *Config, kbts_glyph_storage *Storage) +static void kbts__FreeGlyphBucket(kbts_shape_scratchpad *Scratchpad, kbts_un SequentialLookupIndex) +{ + kbts__bucketed_glyph_block_header *Sentinel = &Scratchpad->LookupGlyphBuckets[SequentialLookupIndex]; + + kbts__bucketed_glyph_block_header *First = Sentinel->Next; + kbts__bucketed_glyph_block_header *Last = Sentinel->Prev; + if(First != Sentinel) + { + First->Prev = Scratchpad->FreeBucketedBlockSentinel.Prev; + Last->Next = &Scratchpad->FreeBucketedBlockSentinel; + + First->Prev->Next = First; + Last->Next->Prev = Last; + + KBTS__DLLIST_SENTINEL_INIT(Sentinel); + } +} + +static void kbts__ExecuteOp(kbts_shape_scratchpad *Scratchpad, kbts_glyph_storage *Storage) { KBTS_INSTRUMENT_FUNCTION_BEGIN; + kbts_shape_config *Config = Scratchpad->Config; if(Config) { @@ -21820,25 +20328,10 @@ static void kbts__ExecuteOp(kbts__shape_scratchpad *Scratchpad, kbts_shape_confi case KBTS__OP_KIND_NORMALIZE: { KBTS_INSTRUMENT_BLOCK_BEGIN(NORMALIZE); - // @Incomplete: We need to honor this. - // HB_OT_SHAPE_NORMALIZATION_MODE_NONE, - // HB_OT_SHAPE_NORMALIZATION_MODE_AUTO, - // HB_OT_SHAPE_NORMALIZATION_MODE_COMPOSED_DIACRITICS_NO_SHORT_CIRCUIT, /* Always fully decomposes and then recompose back */ - // - // hangul: HB_OT_SHAPE_NORMALIZATION_MODE_NONE, - // arabic: HB_OT_SHAPE_NORMALIZATION_MODE_AUTO, - // default: HB_OT_SHAPE_NORMALIZATION_MODE_AUTO, - // hebrew: HB_OT_SHAPE_NORMALIZATION_MODE_AUTO, - // thai: HB_OT_SHAPE_NORMALIZATION_MODE_AUTO, - // indic: HB_OT_SHAPE_NORMALIZATION_MODE_COMPOSED_DIACRITICS_NO_SHORT_CIRCUIT, - // khmer: HB_OT_SHAPE_NORMALIZATION_MODE_COMPOSED_DIACRITICS_NO_SHORT_CIRCUIT, - // myanmar: HB_OT_SHAPE_NORMALIZATION_MODE_COMPOSED_DIACRITICS_NO_SHORT_CIRCUIT, - // use: HB_OT_SHAPE_NORMALIZATION_MODE_COMPOSED_DIACRITICS_NO_SHORT_CIRCUIT, KBTS_INSTRUMENT_BLOCK_BEGIN(Decompose); { // Full NFD decomposition - kbts__arena_lifetime Lifetime = kbts__BeginLifetime(Scratchpad->Arena); - kbts_glyph *DecompositionGlyphs = kbts__PushArray(Scratchpad->Arena, kbts_glyph, KBTS__MAXIMUM_DECOMPOSITION_CODEPOINTS); + kbts_glyph *DecompositionGlyphs = (kbts_glyph *)Scratchpad->ScratchMemory; kbts_un CodepointsToDecomposeCount = 0; KBTS__FOR_GLYPH(Storage, Glyph) @@ -21911,8 +20404,6 @@ static void kbts__ExecuteOp(kbts__shape_scratchpad *Scratchpad, kbts_shape_confi Glyph = PrevAnchor; } } - - kbts__EndLifetime(&Lifetime); } KBTS_INSTRUMENT_BLOCK_END(Decompose); @@ -21920,17 +20411,16 @@ static void kbts__ExecuteOp(kbts__shape_scratchpad *Scratchpad, kbts_shape_confi { // Selective recomposition. // The OpenType shaping documents say that Hebrew Alphabetic Presentation Form compositions aren't canonical, // but looking at UnicodeData.txt, it seems like they totally are, so they are handled here. - kbts__arena_lifetime Lifetime = kbts__BeginLifetime(Scratchpad->Arena); kbts_glyph *LastBase = 0; kbts_un LastBaseParentCount = 0; kbts_un PreSlashDecimalDigitCount = 0; kbts_glyph *PreSlashGlyph = 0; kbts_glyph *DigitGlyph = 0; kbts_un DecimalDigitCount = 0; - int InFraction = 0; + kbts_b32 InFraction = 0; kbts_u32 LastBaseParentsLoaded = 0; kbts_glyph_parent LastBaseParents[KBTS_MAXIMUM_RECOMPOSITION_PARENTS]; - kbts_glyph *Parents = kbts__PushArray(Scratchpad->Arena, kbts_glyph, KBTS_MAXIMUM_RECOMPOSITION_PARENTS); + kbts_u16 LastBaseParentIds[KBTS_MAXIMUM_RECOMPOSITION_PARENTS]; kbts_u32 BeforeFractionSlashGlyphFlags = KBTS_GLYPH_FLAG_NUMR | KBTS_GLYPH_FLAG_FRAC; kbts_u32 AfterFractionSlashGlyphFlags = KBTS_GLYPH_FLAG_DNOM | KBTS_GLYPH_FLAG_FRAC; @@ -21942,31 +20432,18 @@ static void kbts__ExecuteOp(kbts__shape_scratchpad *Scratchpad, kbts_shape_confi AfterFractionSlashGlyphFlags = Swap; } - // We also collate user features here. - kbts__feature_set UserFeatures = KBTS__ZERO; - kbts__feature_set *DefaultFeatures = &Config->Features; - - int ShouldFlip = (Scratchpad->RunDirection == KBTS_DIRECTION_RTL); + kbts_b32 ShouldFlip = (Scratchpad->RunDirection == KBTS_DIRECTION_RTL); KBTS__FOR_GLYPH(Storage, Glyph) { - Glyph->Uid = (kbts_u16)++Scratchpad->NextGlyphUid; - - if(Glyph->Config) - { - kbts_glyph_config *GlyphConfig = Glyph->Config; - KBTS__FOR(WordIndex, 0, KBTS__ARRAY_LENGTH(UserFeatures.Flags)) - { - UserFeatures.Flags[WordIndex] |= GlyphConfig->EnabledFeatures.Flags[WordIndex] & ~DefaultFeatures->Flags[WordIndex]; - } - } + Glyph->Uid = kbts__NextGlyphUid(Scratchpad); // In RTL, mirror all glyphs when their mirror is covered. if(ShouldFlip && (Glyph->UnicodeFlags & KBTS_UNICODE_FLAG_MIRRORED)) { kbts_u32 MatchingBracketCodepoint = kbts__GetUnicodeMirrorCodepoint(Glyph->Codepoint); - kbts_glyph MatchingBracket = kbts_CodepointToGlyph(Font, (int)MatchingBracketCodepoint, 0, 0); + kbts_glyph MatchingBracket = kbts_CodepointToGlyph(Font, (int)MatchingBracketCodepoint, Glyph->Config, 0); if(MatchingBracket.Id) { kbts__SetGlyphPreserveLinksAndUserId(Glyph, &MatchingBracket); @@ -21986,7 +20463,8 @@ static void kbts__ExecuteOp(kbts__shape_scratchpad *Scratchpad, kbts_shape_confi // Cluster validation, is done based on the decomposed state of a split vowel. // // (Note: our Matra corresponds to Vowel_Dependent + Pure_Killer.) - if((Config->Shaper != KBTS_SHAPER_USE) || (Glyph->SyllabicClass != KBTS_INDIC_SYLLABIC_CLASS_MATRA)) + if((Config->Shaper != KBTS_SHAPER_USE) || + (Glyph->SyllabicClass != KBTS_INDIC_SYLLABIC_CLASS_MATRA)) { kbts_s32 *LastBaseParentDeltas = kbts__GetParentInfoDeltas(Glyph->ParentInfo); kbts_un ParentCount = kbts__GetParentInfoCount(Glyph->ParentInfo); @@ -21996,9 +20474,11 @@ static void kbts__ExecuteOp(kbts__shape_scratchpad *Scratchpad, kbts_shape_confi { kbts_glyph_parent Parent = KBTS__ZERO; Parent.Codepoint = Glyph->Codepoint + (kbts_u32)LastBaseParentDeltas[ParentIndex]; - Parent.Decomposition = kbts__GetUnicodeDecomposition(Parent.Codepoint); - kbts_un DecompositionSize = kbts__GetDecompositionSize(Parent.Decomposition); + kbts_u64 Decomposition = kbts__GetUnicodeDecomposition(Parent.Codepoint); + Parent.Codepoint1 = kbts__GetDecompositionCodepoint(Decomposition, 1); + + kbts_un DecompositionSize = kbts__GetDecompositionSize(Decomposition); if(DecompositionSize == 1) { SingleRecompositionCodepoints[SingleRecompositionCodepointCount++] = Parent.Codepoint; @@ -22020,7 +20500,7 @@ static void kbts__ExecuteOp(kbts__shape_scratchpad *Scratchpad, kbts_shape_confi DoubleRecompositionCount = 0; } - int Recomposed = 0; + kbts_b32 Recomposed = 0; if(!Recomposed) { @@ -22028,28 +20508,32 @@ static void kbts__ExecuteOp(kbts__shape_scratchpad *Scratchpad, kbts_shape_confi KBTS__FOR(ParentIndex, 0, DoubleRecompositionCount) { kbts_glyph_parent *Parent = &LastBaseParents[ParentIndex]; - kbts_u32 Codepoint1 = kbts__GetDecompositionCodepoint(Parent->Decomposition, 1); + kbts_u32 Codepoint1 = Parent->Codepoint1; if(Glyph->Codepoint == Codepoint1) { - kbts_glyph *ParentGlyph = &Parents[ParentIndex]; - if(!(LastBaseParentsLoaded & (1 << ParentIndex))) + kbts_u16 ParentId = LastBaseParentIds[ParentIndex]; + + if(!(LastBaseParentsLoaded & (1u << ParentIndex))) { - *ParentGlyph = kbts_CodepointToGlyph(Font, (int)Parent->Codepoint, 0, 0); - ParentGlyph->Uid = LastBase->Uid; - ParentGlyph->UserIdOrCodepointIndex = LastBase->UserIdOrCodepointIndex; - ParentGlyph->Config = LastBase->Config; + ParentId = (kbts_u16)kbts_CodepointToGlyphId(Font, (int)Parent->Codepoint); + LastBaseParentIds[ParentIndex] = ParentId; } - if(ParentGlyph->Id) + if(ParentId) { // Both match. Reclaim space. + kbts_glyph ParentGlyph = kbts_CodepointToGlyph(Font, (int)Parent->Codepoint, 0, 0); + ParentGlyph.Uid = LastBase->Uid; + ParentGlyph.UserIdOrCodepointIndex = LastBase->UserIdOrCodepointIndex; + ParentGlyph.Config = LastBase->Config; + kbts_glyph *Next = Glyph->Next; - KBTS__DLLIST_REMOVE(Glyph); + kbts__FreeGlyph(Scratchpad, Storage, Glyph); Glyph = Next; Recomposed = 1; - kbts__SetGlyphPreserveLinksAndUserId(LastBase, ParentGlyph); + kbts__SetGlyphPreserveLinksAndUserId(LastBase, &ParentGlyph); break; } @@ -22057,10 +20541,10 @@ static void kbts__ExecuteOp(kbts__shape_scratchpad *Scratchpad, kbts_shape_confi { // This glyph is never good. Forget it. LastBaseParents[ParentIndex] = LastBaseParents[LastBaseParentCount - 1]; - Parents[ParentIndex] = Parents[LastBaseParentCount - 1]; - LastBaseParentsLoaded &= ~(1 << ParentIndex); - LastBaseParentsLoaded |= (LastBaseParentsLoaded & (1 << (LastBaseParentCount - 1))) >> (LastBaseParentCount - 1 - ParentIndex); - + LastBaseParentIds[ParentIndex] = LastBaseParentIds[LastBaseParentCount - 1]; + LastBaseParentsLoaded &= ~(1u << ParentIndex); + LastBaseParentsLoaded |= (LastBaseParentsLoaded & (1u << (LastBaseParentCount - 1))) >> (LastBaseParentCount - 1 - ParentIndex); + LastBaseParentCount -= 1; DoubleRecompositionCount -= 1; ParentIndex -= 1; @@ -22074,9 +20558,11 @@ static void kbts__ExecuteOp(kbts__shape_scratchpad *Scratchpad, kbts_shape_confi { KBTS__FOR(SingleRecompositionIndex, 0, SingleRecompositionCodepointCount) { - kbts_glyph ParentGlyph = kbts_CodepointToGlyph(Font, (int)SingleRecompositionCodepoints[SingleRecompositionIndex], 0, 0); - if(ParentGlyph.Id) + kbts_u16 ParentGlyphId = (kbts_u16)kbts_CodepointToGlyphId(Font, (int)SingleRecompositionCodepoints[SingleRecompositionIndex]); + + if(ParentGlyphId) { + kbts_glyph ParentGlyph = kbts_CodepointToGlyph(Font, (int)SingleRecompositionCodepoints[SingleRecompositionIndex], 0, 0); ParentGlyph.Config = Glyph->Config; kbts__SetGlyphPreserveLinksAndUserId(Glyph, &ParentGlyph); @@ -22108,7 +20594,8 @@ static void kbts__ExecuteOp(kbts__shape_scratchpad *Scratchpad, kbts_shape_confi } DecimalDigitCount += 1; } - else if((Glyph->Codepoint == 0x2044) && (!InFraction || DecimalDigitCount)) + else if((Glyph->Codepoint == 0x2044) && + (!InFraction || DecimalDigitCount)) { // Fraction slash. Glyph->Flags |= KBTS_GLYPH_FLAG_FRAC; @@ -22128,16 +20615,6 @@ static void kbts__ExecuteOp(kbts__shape_scratchpad *Scratchpad, kbts_shape_confi Glyph = Glyph->Prev; // Handle recursive recomposition. } } - - // Ignore added features that are already part of the shaper. - kbts__feature_set *ShaperFeatures = &Config->Features; - KBTS__FOR(WordIndex, 0, KBTS__ARRAY_LENGTH(UserFeatures.Flags)) - { - UserFeatures.Flags[WordIndex] &= ~ShaperFeatures->Flags[WordIndex]; - } - - Scratchpad->UserFeatures = UserFeatures; - kbts__EndLifetime(&Lifetime); } KBTS_INSTRUMENT_BLOCK_END(Recompose); @@ -22203,6 +20680,8 @@ static void kbts__ExecuteOp(kbts__shape_scratchpad *Scratchpad, kbts_shape_confi if(Config->Script == KBTS_SCRIPT_ARABIC) { + enum {KBTS_REMAPPED_CCC_33 = 27}; + for(kbts_glyph *Glyph = Storage->GlyphSentinel.Next; kbts__GlyphIsValid(Storage, Glyph); ) @@ -22230,8 +20709,6 @@ static void kbts__ExecuteOp(kbts__shape_scratchpad *Scratchpad, kbts_shape_confi kbts__mcm_sequence_state Mcm220SequenceState = 0; kbts__mcm_sequence_state Mcm230SequenceState = 0; - # define KBTS_REMAPPED_CCC_33 27 - kbts_glyph *SequenceGlyph = Glyph; for(; kbts__GlyphIsValid(Storage, SequenceGlyph); @@ -22301,7 +20778,7 @@ static void kbts__ExecuteOp(kbts__shape_scratchpad *Scratchpad, kbts_shape_confi KBTS__DLLIST_SORT(Glyph, SequenceGlyph, MarkOrdering); - #ifdef KBTS_SANITY_CHECK + #ifdef KBTS_SANITY_CHECK { kbts_glyph *Glyph0 = OneBeforeGlyph->Next; for(kbts_glyph *Glyph1 = Glyph0->Next; @@ -22339,13 +20816,22 @@ static void kbts__ExecuteOp(kbts__shape_scratchpad *Scratchpad, kbts_shape_confi // the sara am codepoint. case 0xE33: case 0xEB3: // Sara am { + if(!AboveBaseGlyph) + { + AboveBaseGlyph = Glyph; + } + kbts_glyph *NewGlyph = kbts__InsertGlyphBefore(Storage, AboveBaseGlyph, &Config->Nikhahit); if(!NewGlyph) { goto OutOfMemory; } + kbts_glyph_config *GlyphConfig = Glyph->Config; kbts__SetGlyphPreserveLinksAndUserId(Glyph, &Config->SaraAa); + Glyph->Config = GlyphConfig; + + AboveBaseGlyph = 0; } break; case 0xE31: case 0xE34: case 0xE35: case 0xE36: case 0xE37: case 0xE3B: @@ -22532,6 +21018,7 @@ static void kbts__ExecuteOp(kbts__shape_scratchpad *Scratchpad, kbts_shape_confi { kbts_glyph *LvtGlyph = &LvtGlyphs[LvtGlyphIndex]; kbts_glyph *NewGlyph = kbts__InsertGlyphBefore(Storage, Next, LvtGlyph); + if(!NewGlyph) { goto OutOfMemory; @@ -22541,72 +21028,133 @@ static void kbts__ExecuteOp(kbts__shape_scratchpad *Scratchpad, kbts_shape_confi Glyph = Next; } - // KBTS_INSTRUMENT_BLOCK_END(NORMALIZE_HANGUL); + + KBTS_INSTRUMENT_BLOCK_END(NORMALIZE_HANGUL); } break; + case KBTS__OP_KIND_BEGIN_GSUB: + { + enum{SortKeyInterval = 0x10000}; + kbts_un RunningSortKey = SortKeyInterval; + + KBTS__FOR_GLYPH(Storage, Glyph) + { + Glyph->SortKey = (kbts_u32)RunningSortKey; + Glyph->SortKeyInterval = SortKeyInterval; + + kbts__BucketGlyph(Scratchpad, Glyph, 0, 0); + + RunningSortKey += SortKeyInterval; + } + } break; + case KBTS__OP_KIND_GSUB_FEATURES: { KBTS_INSTRUMENT_BLOCK_BEGIN(GSUB_FEATURES); + // @Duplication kbts__gsub_gpos *FontGsub = kbts__BlobTableDataType(Font->Blob, KBTS_BLOB_TABLE_ID_GSUB, kbts__gsub_gpos); - kbts_lookup_list *LookupList; LookupList = kbts__GetLookupList(FontGsub); - kbts__arena_lifetime Lifetime = kbts__BeginLifetime(Scratchpad->Arena); - kbts__gsub_frame *Frames = kbts__PushArray(Scratchpad->Arena, kbts__gsub_frame, KBTS_LOOKUP_STACK_SIZE); - kbts_u32 GlyphFilter; - kbts__skip_flags SkipFlags; - kbts_un LookupIndex; + kbts_lookup_list *LookupList = kbts__GetLookupList(FontGsub); - if(kbts__BeginFeatures(Scratchpad, Config, KBTS_SHAPING_TABLE_GSUB)) + kbts__gsub_frame *Frames = (kbts__gsub_frame *)Scratchpad->ScratchMemory; + + kbts_u32 FilterMask = Config->Shaper == KBTS_SHAPER_USE ? KBTS__USE_GLYPH_FEATURE_MASK : KBTS__GLYPH_FEATURE_MASK; + + kbts_un FeatureStageIndex = kbts__CurrentBakedFeatureStageIndex(Scratchpad); + kbts_un FirstSequentialLookupIndex = Config->FeatureStageFirstLookupIndices[FeatureStageIndex]; + kbts_un OnePastLastSequentialLookupIndex = Config->FeatureStageFirstLookupIndices[FeatureStageIndex + 1]; + KBTS__FOR(SequentialLookupIndex, FirstSequentialLookupIndex, OnePastLastSequentialLookupIndex) { - while(kbts__NextLookupIndex(Scratchpad, Config, &LookupIndex, &SkipFlags, &GlyphFilter, &Scratchpad->LookupFeatures)) + kbts__sequential_lookup *SequentialLookup = &Config->SequentialLookups[SequentialLookupIndex]; + kbts_un LookupIndex = SequentialLookup->LookupIndex; + kbts_u32 SkipFlags = SequentialLookup->SkipFlags; + kbts_u32 GlyphFilter = SequentialLookup->GlyphFilter; + + kbts__bucketed_glyph_block_header *Sentinel = &Scratchpad->LookupGlyphBuckets[SequentialLookupIndex]; + + if(Sentinel->Next != Sentinel) { - kbts__lookup *Lookup = kbts__GetLookup(LookupList, LookupIndex); + kbts__lookup *PackedLookup = kbts__GetLookup(LookupList, LookupIndex); + kbts_b32 LookupTypeIs8 = (PackedLookup->Type == 8); - // From the Microsoft docs: - // If a Lookup table has multiple subtables, the subtables are processed in order, testing the glyph sequence - // at the current glyph position for a match with the input sequence patterns specified by each subtable in - // turn. - // - // This means the subtable loop is _inside_ of the loop over our glyphs. + KBTS_INSTRUMENT_BLOCK_BEGIN(GsubSortBucket); - // Reverse chaining substitutions are tricky. - // See the comment at :ReverseChaining. - for(kbts_glyph *Glyph = (Lookup->Type == 8) ? Storage->GlyphSentinel.Prev : Storage->GlyphSentinel.Next; - kbts__GlyphIsValid(Storage, Glyph); - ) + if(PackedLookup->Type >= 4) { - kbts__BeginLookupApplication(Scratchpad, Glyph); - kbts_un FrameCount = 0; - kbts_u32 FilterMask = Config->Shaper == KBTS_SHAPER_USE ? KBTS__USE_GLYPH_FEATURE_MASK : KBTS__GLYPH_FEATURE_MASK; - kbts_u32 EffectiveGlyphFilter = GlyphFilter & FilterMask; + kbts__SortGlyphBucket(Scratchpad, SequentialLookupIndex); + } - if(kbts__GlyphIncludedInLookup(Font, 0, LookupIndex, Glyph->Id) && - ((Glyph->Flags & EffectiveGlyphFilter) == EffectiveGlyphFilter) && - kbts__ConfigAllowsFeatures(Scratchpad, Config, Glyph->Config, &Scratchpad->LookupFeatures)) + KBTS_INSTRUMENT_BLOCK_END(GsubSortBucket); + + for(kbts__bucketed_glyph_block_header *BlockHeader = (LookupTypeIs8) ? Sentinel->Prev : Sentinel->Next; + BlockHeader != Sentinel; + BlockHeader = (LookupTypeIs8) ? BlockHeader->Prev : BlockHeader->Next) + { + kbts__bucketed_glyph_block *Block = (kbts__bucketed_glyph_block *)BlockHeader; + kbts_un BlockGlyphCount = Block->Count; + + KBTS__FOR(GlyphIndex_, 0, BlockGlyphCount) { - kbts__gsub_frame FirstFrame = KBTS__ZERO; - FirstFrame.LookupIndex = (kbts_u16)LookupIndex; - FirstFrame.InputGlyph = Glyph; + kbts_un GlyphIndex = (LookupTypeIs8) ? (BlockGlyphCount - 1 - GlyphIndex_) : GlyphIndex_; + kbts__bucketed_glyph *Bucketed = &Block->Glyphs[GlyphIndex]; - Frames[0] = FirstFrame; - FrameCount = 1; - - while(FrameCount) + if(kbts__BucketedGlyphIsValid(Bucketed)) { - // These flags are used by USE. - kbts_u32 GeneratedGlyphFlags = GlyphFilter & (KBTS_GLYPH_FLAG_RPHF | KBTS_GLYPH_FLAG_PREF); - kbts__DoSubstitution(Scratchpad, Config, Storage, LookupList, Frames, &FrameCount, 0, SkipFlags, GeneratedGlyphFlags); + kbts_glyph *Glyph = Bucketed->Glyph; + kbts_u16 FeatureValue = Bucketed->FeatureValue; + kbts_u32 GlyphFlags = Glyph->Flags; + kbts_glyph *Prev = Glyph->Prev; + + kbts__BeginLookupApplication(Scratchpad, Glyph); + kbts_u32 EffectiveGlyphFilter = GlyphFilter & FilterMask; + + if((GlyphFlags & EffectiveGlyphFilter) == EffectiveGlyphFilter) + { + kbts__gsub_frame FirstFrame = KBTS__ZERO; + FirstFrame.LookupIndex = (kbts_u16)LookupIndex; + FirstFrame.InputGlyph = Glyph; + + Frames[0] = FirstFrame; + kbts_un FrameCount = 1; + + while(FrameCount) + { + // These flags are used by USE. + kbts_u32 GeneratedGlyphFlags = GlyphFilter & (KBTS_GLYPH_FLAG_RPHF | KBTS_GLYPH_FLAG_PREF); + kbts__DoSubstitution(Scratchpad, Config, Storage, LookupList, SequentialLookupIndex, FeatureValue, Frames, &FrameCount, 0, SkipFlags, GeneratedGlyphFlags); + } + } + + kbts_glyph *OnePastLast = kbts__EndLookupApplication(Scratchpad); + + KBTS_INSTRUMENT_BLOCK_BEGIN(GsubRebucketTouchedGlyphs); + + for(kbts_glyph *MatchedGlyph = Prev->Next; + MatchedGlyph != OnePastLast; + MatchedGlyph = MatchedGlyph->Next) + { + // Queue for the next bucket. + if(MatchedGlyph->Bucketed && + (MatchedGlyph->BucketedBucketIndex == SequentialLookupIndex)) + { + // We are scheduled for the current bucket. Cancel that, and move on to the next. + kbts__UnbucketGlyph(Scratchpad, MatchedGlyph); + kbts__BucketGlyph(Scratchpad, MatchedGlyph, SequentialLookupIndex + 1, 0); + } + } + + KBTS_INSTRUMENT_BLOCK_END(GsubRebucketTouchedGlyphs); } } - - kbts_glyph *OnePastLast = kbts__EndLookupApplication(Scratchpad); - Glyph = (Lookup->Type == 8) ? Glyph->Prev : OnePastLast; } } + + kbts__FreeGlyphBucket(Scratchpad, SequentialLookupIndex); } - kbts__EndLifetime(&Lifetime); + Scratchpad->SequentialLookupIndexIndex = (kbts_u32)OnePastLastSequentialLookupIndex; + KBTS_INSTRUMENT_BLOCK_END(GSUB_FEATURES); } break; @@ -22661,7 +21209,7 @@ static void kbts__ExecuteOp(kbts__shape_scratchpad *Scratchpad, kbts_shape_confi { KBTS_INSTRUMENT_BLOCK_BEGIN(GPOS_METRICS); // hmtx/vmtx pass. - int ClearMarkAdvances = (Config->Shaper == KBTS_SHAPER_MYANMAR) || (Config->Shaper == KBTS_SHAPER_USE); + kbts_b32 ClearMarkAdvances = (Config->Shaper == KBTS_SHAPER_MYANMAR) || (Config->Shaper == KBTS_SHAPER_USE); kbts_u32 Orientation = KBTS_ORIENTATION_HORIZONTAL; // @Hardcoded kbts_blob_table_id HeaTableId = KBTS_BLOB_TABLE_ID_HHEA; @@ -22752,6 +21300,7 @@ static void kbts__ExecuteOp(kbts__shape_scratchpad *Scratchpad, kbts_shape_confi } } } + KBTS_INSTRUMENT_BLOCK_END(GPOS_METRICS); } break; @@ -22759,44 +21308,96 @@ static void kbts__ExecuteOp(kbts__shape_scratchpad *Scratchpad, kbts_shape_confi case KBTS__OP_KIND_GPOS_FEATURES: { KBTS_INSTRUMENT_BLOCK_BEGIN(GPOS_FEATURES); - kbts__gsub_gpos *Gpos = kbts__BlobTableDataType(Font->Blob, KBTS_BLOB_TABLE_ID_GPOS, kbts__gsub_gpos); - if(kbts__BeginFeatures(Scratchpad, Config, KBTS_SHAPING_TABLE_GPOS)) + kbts__gsub_gpos *Gpos = kbts__BlobTableDataType(Font->Blob, KBTS_BLOB_TABLE_ID_GPOS, kbts__gsub_gpos); + kbts__gdef *Gdef = kbts__BlobTableDataType(Font->Blob, KBTS_BLOB_TABLE_ID_GDEF, kbts__gdef); + + kbts_lookup_list *LookupList = kbts__GetLookupList(Gpos); + kbts_un FeatureStageIndex = kbts__CurrentBakedFeatureStageIndex(Scratchpad); + + kbts_un FirstSequentialLookupIndex = Config->FeatureStageFirstLookupIndices[FeatureStageIndex]; + kbts_un OnePastLastSequentialLookupIndex = Config->FeatureStageFirstLookupIndices[FeatureStageIndex + 1]; + KBTS__FOR(SequentialLookupIndex, FirstSequentialLookupIndex, OnePastLastSequentialLookupIndex) { - kbts_lookup_list *LookupList = kbts__GetLookupList(Gpos); - kbts_un LookupIndex; - kbts__skip_flags SkipFlags; - kbts_u32 GlyphFilter; - kbts__feature_set LookupFeatures; - while(kbts__NextLookupIndex(Scratchpad, Config, &LookupIndex, &SkipFlags, &GlyphFilter, &LookupFeatures)) + kbts__sequential_lookup *SequentialLookup = &Config->SequentialLookups[SequentialLookupIndex]; + kbts_un LookupIndex = SequentialLookup->LookupIndex; + kbts_u32 SkipFlags = SequentialLookup->SkipFlags; + + kbts__bucketed_glyph_block_header *Sentinel = &Scratchpad->LookupGlyphBuckets[SequentialLookupIndex]; + + if(Sentinel->Next != Sentinel) { kbts__lookup *PackedLookup = kbts__GetLookup(LookupList, LookupIndex); - kbts__unpacked_lookup Lookup = kbts__UnpackLookup(kbts__BlobTableDataType(Font->Blob, KBTS_BLOB_TABLE_ID_GDEF, kbts__gdef), PackedLookup); + kbts__unpacked_lookup Lookup = kbts__UnpackLookup(Gdef, PackedLookup); + kbts_un SubtableCount = PackedLookup->SubtableCount; - kbts_glyph *Glyph = Storage->GlyphSentinel.Next; - while(kbts__GlyphIsValid(Storage, Glyph)) + KBTS_INSTRUMENT_BLOCK_BEGIN(GposSortBucket); + + if(PackedLookup->Type >= 2) { - kbts__BeginLookupApplication(Scratchpad, Glyph); + kbts__SortGlyphBucket(Scratchpad, SequentialLookupIndex); + } - if(kbts__GlyphIncludedInLookup(Config->Font, 1, LookupIndex, Glyph->Id) && - kbts__ConfigAllowsFeatures(Scratchpad, Config, Glyph->Config, &LookupFeatures)) + KBTS_INSTRUMENT_BLOCK_END(GposSortBucket); + + for(kbts__bucketed_glyph_block_header *BlockHeader = Sentinel->Next; + BlockHeader != Sentinel; + BlockHeader = BlockHeader->Next) + { + kbts__bucketed_glyph_block *Block = (kbts__bucketed_glyph_block *)BlockHeader; + kbts_un BlockGlyphCount = Block->Count; + + KBTS__FOR(GlyphIndex, 0, BlockGlyphCount) { - KBTS__FOR(SubtableIndex, 0, Lookup.SubtableCount) - { - kbts_u16 *Subtable = KBTS__POINTER_OFFSET(kbts_u16, PackedLookup, Lookup.SubtableOffsets[SubtableIndex]); + kbts__bucketed_glyph *Bucketed = &Block->Glyphs[GlyphIndex]; - if(kbts__DoSingleAdjustment(Scratchpad, Config, Storage, LookupList, - LookupIndex, SubtableIndex, &Lookup, Subtable, - Glyph, 0, SkipFlags)) + if(kbts__BucketedGlyphIsValid(Bucketed)) + { + kbts_glyph *Glyph = Bucketed->Glyph; + kbts__BeginLookupApplication(Scratchpad, Glyph); + + kbts_u16 *SubtableOffsets = KBTS__POINTER_AFTER(kbts_u16, PackedLookup); + + KBTS__FOR(SubtableIndex, 0, SubtableCount) { - break; + kbts_u16 *Subtable = KBTS__POINTER_OFFSET(kbts_u16, PackedLookup, SubtableOffsets[SubtableIndex]); + + if(kbts__DoSingleAdjustment(Scratchpad, Config, Storage, + LookupList, LookupIndex, SubtableIndex, &Lookup, Subtable, + Glyph, 0, SkipFlags)) + { + break; + } } + + kbts_glyph *OnePastLast = kbts__EndLookupApplication(Scratchpad); + + KBTS_INSTRUMENT_BLOCK_BEGIN(GposRebucketTouchedGlyphs); + + // @Speed: We can record just the IDs of the glyphs we traverse here into a flat array + // so that attachment lookups are faster. + for(kbts_glyph *MatchedGlyph = Glyph; + MatchedGlyph != OnePastLast; + MatchedGlyph = MatchedGlyph->Next) + { + // Queue for the next bucket. + + if(MatchedGlyph->Bucketed && + (MatchedGlyph->BucketedBucketIndex == SequentialLookupIndex)) + { + // We are scheduled for the current bucket. Cancel that, and move on to the next. + kbts__UnbucketGlyph(Scratchpad, MatchedGlyph); + kbts__BucketGlyph(Scratchpad, MatchedGlyph, SequentialLookupIndex + 1, 1); + } + } + + KBTS_INSTRUMENT_BLOCK_END(GposRebucketTouchedGlyphs); } } - - Glyph = kbts__EndLookupApplication(Scratchpad); } } + + kbts__FreeGlyphBucket(Scratchpad, SequentialLookupIndex); } KBTS_INSTRUMENT_BLOCK_END(GPOS_FEATURES); @@ -22875,7 +21476,10 @@ static void kbts__ExecuteOp(kbts__shape_scratchpad *Scratchpad, kbts_shape_confi if(WhitespaceGlyph.Id) { Keep = 1; + + kbts_glyph_config *GlyphConfig = Glyph->Config; kbts__SetGlyphPreserveLinksAndUserId(Glyph, &WhitespaceGlyph); + Glyph->Config = GlyphConfig; } } else @@ -22996,7 +21600,7 @@ static void kbts__ExecuteOp(kbts__shape_scratchpad *Scratchpad, kbts_shape_confi KBTS_INSTRUMENT_FUNCTION_END; } -static kbts_glyph kbts__Substitute1(kbts__shape_scratchpad *Scratchpad, kbts_shape_config *Config, kbts_glyph_storage *Storage, +static kbts_glyph kbts__Substitute1(kbts_shape_scratchpad *Scratchpad, kbts_shape_config *Config, kbts_glyph_storage *Storage, kbts_lookup_list *LookupList, kbts__feature *Feature, kbts__skip_flags SkipFlags, kbts_glyph *Glyph) { kbts_glyph Result = *Glyph; @@ -23022,7 +21626,7 @@ static kbts_glyph kbts__Substitute1(kbts__shape_scratchpad *Scratchpad, kbts_sha while(FrameCount) { kbts__substitution_result_flags SubstitutionResult = kbts__DoSubstitution(Scratchpad, Config, Storage, - LookupList, Frames, &FrameCount, 0, SkipFlags, 0); + LookupList, Scratchpad->SequentialLookupIndexIndex, 0, Frames, &FrameCount, 0, SkipFlags, 0); if(SubstitutionResult & KBTS__SUBSTITUTION_RESULT_FLAG_TRIED_TO_INSERT_WHILE_CHECK_ONLY) { goto Done; @@ -23045,12 +21649,12 @@ typedef struct kbts__attach_state kbts_glyph *At; } kbts__attach_state; -static kbts_glyph *kbts__BeginCluster(kbts__shape_scratchpad *Scratchpad, kbts_shape_config *Config, kbts_glyph_storage *Storage, - kbts_glyph *Glyph) +static kbts_glyph *kbts__BeginCluster(kbts_shape_scratchpad *Scratchpad, kbts_glyph_storage *Storage, + kbts_glyph *Glyph) { + kbts_shape_config *Config = Scratchpad->Config; kbts_font *Font = Config->Font; kbts_glyph *Result = Glyph->Next; - kbts__arena_lifetime Lifetime = kbts__BeginLifetime(Scratchpad->Arena); Scratchpad->RealCluster = 0; @@ -23129,7 +21733,7 @@ static kbts_glyph *kbts__BeginCluster(kbts__shape_scratchpad *Scratchpad, kbts_s // _strongly_ recommends that shapers use the font's tables instead, ie. doing tentative lookups // with specific features. - kbts__gsub_frame *Frames = kbts__PushArray(Scratchpad->Arena, kbts__gsub_frame, KBTS_LOOKUP_STACK_SIZE); + kbts__gsub_frame *Frames = (kbts__gsub_frame *)Scratchpad->ScratchMemory; kbts_glyph *OnePastLastSyllableGlyph = Glyph; kbts_glyph *FirstGlyph = FirstGlyphs[0]; kbts_glyph *OneBeforeSyllableGlyph = FirstGlyph->Prev; @@ -23327,6 +21931,7 @@ static kbts_glyph *kbts__BeginCluster(kbts__shape_scratchpad *Scratchpad, kbts_s // Since half forms are always before the base, we can safely stop here. goto DoneScanningForBase; } + KBTS__FALLTHROUGH; case KBTS_INDIC_SYLLABIC_CLASS_NUKTA: case KBTS_INDIC_SYLLABIC_CLASS_ZWJ: case KBTS_INDIC_SYLLABIC_CLASS_ZWNJ: @@ -23415,12 +22020,16 @@ static kbts_glyph *kbts__BeginCluster(kbts__shape_scratchpad *Scratchpad, kbts_s Glyph != OnePastLastSyllableGlyph; Glyph = Glyph->Next) { - Glyph->SyllabicPosition <<= 4; + kbts_un SyllabicPosition = Glyph->SyllabicPosition; - if(Glyph->SyllabicPosition == (KBTS__SYLLABIC_POSITION_PREBASE_MATRA << 4)) + SyllabicPosition <<= 4; + + if(SyllabicPosition == (KBTS__SYLLABIC_POSITION_PREBASE_MATRA << 4)) { - Glyph->SyllabicPosition += (15 - LeftMatraCount++) & 0xF; + SyllabicPosition += (15 - LeftMatraCount++) & 0xF; } + + Glyph->SyllabicPosition = (kbts_u8)SyllabicPosition; } } @@ -23482,6 +22091,7 @@ static kbts_glyph *kbts__BeginCluster(kbts__shape_scratchpad *Scratchpad, kbts_s { Glyph->Prev->SyllabicPosition = Glyph->SyllabicPosition; } + KBTS__FALLTHROUGH; case KBTS_INDIC_SYLLABIC_CLASS_MATRA: Attach->CurrentPosition = Glyph->SyllabicPosition; if((Attach->CurrentPosition >> 4) != KBTS__SYLLABIC_POSITION_PREBASE_MATRA) @@ -23497,6 +22107,7 @@ static kbts_glyph *kbts__BeginCluster(kbts__shape_scratchpad *Scratchpad, kbts_s Glyph->SyllabicPosition = Attach->LastPositionThatWasNotPreBaseMatra; break; } + KBTS__FALLTHROUGH; case KBTS_INDIC_SYLLABIC_CLASS_NUKTA: case KBTS_INDIC_SYLLABIC_CLASS_ZWJ: case KBTS_INDIC_SYLLABIC_CLASS_ZWNJ: @@ -23801,9 +22412,7 @@ static kbts_glyph *kbts__BeginCluster(kbts__shape_scratchpad *Scratchpad, kbts_s { kbts_glyph *PreBaseGlyph = PreBaseGlyphs[--PreBaseGlyphCount]; KBTS__DLLIST_REMOVE(PreBaseGlyph); - PreBaseGlyph->Prev = OneBeforeSyllableGlyph; - PreBaseGlyph->Next = OneBeforeSyllableGlyph->Next; - PreBaseGlyph->Prev->Next = PreBaseGlyph->Next->Prev = PreBaseGlyph; + KBTS__DLLIST_INSERT_AFTER(PreBaseGlyph, OneBeforeSyllableGlyph); } Result = OnePastLastSyllableGlyph; @@ -23858,12 +22467,13 @@ static kbts_glyph *kbts__BeginCluster(kbts__shape_scratchpad *Scratchpad, kbts_s Scratchpad->Error = KBTS_SHAPE_ERROR_OUT_OF_MEMORY; } - kbts__EndLifetime(&Lifetime); return Result; } -static void kbts__EndCluster(kbts__shape_scratchpad *Scratchpad, kbts_shape_config *Config, kbts_glyph_storage *Storage) +static void kbts__EndCluster(kbts_shape_scratchpad *Scratchpad, kbts_glyph_storage *Storage) { + kbts_shape_config *Config = Scratchpad->Config; + switch(Config->Shaper) { case KBTS_SHAPER_INDIC: @@ -24189,7 +22799,8 @@ static void kbts__EndCluster(kbts__shape_scratchpad *Scratchpad, kbts_shape_conf } } - if(To) + if(To && + (First != To)) { // Do the reorder! KBTS__DLLIST_REMOVE(First); @@ -24437,11 +23048,11 @@ static void kbts__EndCluster(kbts__shape_scratchpad *Scratchpad, kbts_shape_conf } kbts_u64 BaseSet = KBTS__SET64((KBTS_INDIC_SYLLABIC_CLASS_CONSONANT) - (KBTS_INDIC_SYLLABIC_CLASS_CONSONANT_WITH_STACKER) - (KBTS_INDIC_SYLLABIC_CLASS_RA) - (KBTS_INDIC_SYLLABIC_CLASS_VOWEL) - (KBTS_INDIC_SYLLABIC_CLASS_PLACEHOLDER) - (KBTS_INDIC_SYLLABIC_CLASS_DOTTED_CIRCLE)); + (KBTS_INDIC_SYLLABIC_CLASS_CONSONANT_WITH_STACKER) + (KBTS_INDIC_SYLLABIC_CLASS_RA) + (KBTS_INDIC_SYLLABIC_CLASS_VOWEL) + (KBTS_INDIC_SYLLABIC_CLASS_PLACEHOLDER) + (KBTS_INDIC_SYLLABIC_CLASS_DOTTED_CIRCLE)); // Scan for a non-reph base glyph. for(kbts_glyph *Glyph = OnePastReph; @@ -24569,37 +23180,56 @@ static void kbts__EndCluster(kbts__shape_scratchpad *Scratchpad, kbts_shape_conf } } -static void *kbts__PointerPush(char **Pointer, kbts_un Size, kbts_un Align) +static kbts_b32 kbts__ReadOp(kbts_shape_scratchpad *Scratchpad, kbts__op_kind End) { - char *At = *Pointer; - char *Aligned = KBTS__ALIGN_POINTER(char, At, Align); - *Pointer = Aligned + Size; + KBTS_INSTRUMENT_FUNCTION_BEGIN; + kbts_b32 Result = 0; + kbts_shape_config *Config = Scratchpad->Config; - void *Result = Aligned; + if(Config) + { + kbts__op_list *OpList = &Config->OpList; + + if(Scratchpad->Ip < OpList->OpCount) + { + kbts__op_kind Kind = OpList->Ops[Scratchpad->Ip++]; + + // @Cleanup + if((Kind == KBTS__OP_KIND_GSUB_FEATURES_WITH_USER) || + (Kind == KBTS__OP_KIND_GSUB_FEATURES) || + (Kind == KBTS__OP_KIND_GPOS_FEATURES)) + { + Scratchpad->FeatureStagesRead += 1; + + if(Kind == KBTS__OP_KIND_GSUB_FEATURES_WITH_USER) + { + Kind = KBTS__OP_KIND_GSUB_FEATURES; + } + } + + Scratchpad->OpKind = Kind; + Result = (Kind != End); + } + } + + KBTS_INSTRUMENT_FUNCTION_END; return Result; } -#define kbts__PointerPushType(Pointer, Type) (Type *)kbts__PointerPush((Pointer), sizeof(Type), KBTS_ALIGNOF(Type)) -#define kbts__PointerPushArray(Pointer, Type, Count) (Type *)kbts__PointerPush((Pointer), sizeof(Type) * (Count), KBTS_ALIGNOF(Type)) static kbts_shape_config *kbts__PlaceShapeConfig(kbts_font *Font, kbts_script Script, kbts_language Language, void *Memory, kbts_un *Size) { kbts_shape_config *Result = 0; - kbts_shape_config DummyConfig; - char *MemoryAt = (char *)Memory; + kbts__pointer_bump_allocator Bump = kbts__PointerBumpAllocator(Memory); if(Font) { - Result = kbts__PointerPushType(&MemoryAt, kbts_shape_config); - if(!Memory) - { - Result = &DummyConfig; - } + kbts_shape_config Config = KBTS__ZERO; - KBTS_MEMSET(Result, 0, sizeof(*Result)); + Result = kbts__PointerPushType(&Bump, kbts_shape_config); - Result->Font = Font; - Result->Script = Script; - Result->Language = Language; + Config.Font = Font; + Config.Script = Script; + Config.Language = Language; kbts__gsub_gpos *ShapingTables[2] = { kbts__BlobTableDataType(Font->Blob, KBTS_BLOB_TABLE_ID_GSUB, kbts__gsub_gpos), @@ -24649,7 +23279,6 @@ static kbts_shape_config *kbts__PlaceShapeConfig(kbts_font *Font, kbts_script Sc // It is tempting to try to look for another script if the one we want has no langsys. // However, it is possible for a script to purposefully have no langsys at all. In that case, // the shaper should not apply any GSUB features. - // In that case, the shaper should not apply any GSUB features. // So, store the result _regardless_ of whether Langsys is null or not. ChosenLangsys = Langsys; if(ShapingTableIndex == KBTS_SHAPING_TABLE_GSUB) @@ -24665,206 +23294,218 @@ static kbts_shape_config *kbts__PlaceShapeConfig(kbts_font *Font, kbts_script Sc } } - Result->Langsys[ShapingTableIndex] = ChosenLangsys; + Config.Langsys[ShapingTableIndex] = ChosenLangsys; } } + + Config.IndicScriptProperties = kbts__IndicScriptProperties(Script); + Config.Shaper = FoundScriptIsIndic3 ? KBTS_SHAPER_USE : ScriptProperties->Shaper; + Config.OpList = *kbts__ShaperOpLists[Config.Shaper]; - Result->IndicScriptProperties = kbts__IndicScriptProperties(Script); - Result->Shaper = FoundScriptIsIndic3 ? KBTS_SHAPER_USE : ScriptProperties->Shaper; - Result->OpList = *kbts__ShaperOpLists[Result->Shaper]; - - Result->Features = KBTS__ZERO_TYPE(kbts__feature_set); - KBTS__FOR(StageIndex, 0, Result->OpList.FeatureStageCount) + Config.Features = KBTS__ZERO_TYPE(kbts__feature_set); + KBTS__FOR(StageIndex, 0, Config.OpList.FeatureStageCount) { - kbts__feature_stage *Stage = &Result->OpList.FeatureStages[StageIndex]; + kbts__feature_stage *Stage = &Config.OpList.FeatureStages[StageIndex]; - KBTS__FOR(WordIndex, 0, KBTS__ARRAY_LENGTH(Result->Features.Flags)) + KBTS__FOR(WordIndex, 0, KBTS__ARRAY_LENGTH(Config.Features.Flags)) { - Result->Features.Flags[WordIndex] |= Stage->Features.Flags[WordIndex]; + Config.Features.Flags[WordIndex] |= Stage->Features.Flags[WordIndex]; } } kbts__feature *Rclt = 0; kbts__feature_set SyllableFeatureSet = {{KBTS__FEATURE_FLAG0(rphf) | KBTS__FEATURE_FLAG0(blwf) | KBTS__FEATURE_FLAG0(half) | KBTS__FEATURE_FLAG0(pstf) | KBTS__FEATURE_FLAG0(pref), 0, KBTS__FEATURE_FLAG2(rclt) | KBTS__FEATURE_FLAG2(locl), KBTS__FEATURE_FLAG3(vatu)}}; - kbts__iterate_features IterateFeatures = kbts__IterateFeatures(Result, KBTS_SHAPING_TABLE_GSUB, SyllableFeatureSet); + kbts__iterate_features IterateFeatures = kbts__IterateFeatures(&Config, KBTS_SHAPING_TABLE_GSUB, SyllableFeatureSet); while(kbts__NextFeature(&IterateFeatures)) { switch(IterateFeatures.CurrentFeatureTag) { - case KBTS_FEATURE_TAG_blwf: Result->Blwf = IterateFeatures.Feature; break; - case KBTS_FEATURE_TAG_pref: Result->Pref = IterateFeatures.Feature; break; - case KBTS_FEATURE_TAG_pstf: Result->Pstf = IterateFeatures.Feature; break; - case KBTS_FEATURE_TAG_locl: Result->Locl = IterateFeatures.Feature; break; - case KBTS_FEATURE_TAG_rphf: Result->Rphf = IterateFeatures.Feature; break; - case KBTS_FEATURE_TAG_half: Result->Half = IterateFeatures.Feature; break; - case KBTS_FEATURE_TAG_vatu: Result->Vatu = IterateFeatures.Feature; break; + case KBTS_FEATURE_TAG_blwf: Config.Blwf = IterateFeatures.Feature; break; + case KBTS_FEATURE_TAG_pref: Config.Pref = IterateFeatures.Feature; break; + case KBTS_FEATURE_TAG_pstf: Config.Pstf = IterateFeatures.Feature; break; + case KBTS_FEATURE_TAG_locl: Config.Locl = IterateFeatures.Feature; break; + case KBTS_FEATURE_TAG_rphf: Config.Rphf = IterateFeatures.Feature; break; + case KBTS_FEATURE_TAG_half: Config.Half = IterateFeatures.Feature; break; + case KBTS_FEATURE_TAG_vatu: Config.Vatu = IterateFeatures.Feature; break; case KBTS_FEATURE_TAG_rclt: Rclt = IterateFeatures.Feature; break; } } - if((Result->Shaper == KBTS_SHAPER_ARABIC) && !Rclt) + if((Config.Shaper == KBTS_SHAPER_ARABIC) && !Rclt) { - Result->OpList = kbts__OpList_ArabicNoRclt; + Config.OpList = kbts__OpList_ArabicNoRclt; } - if(Result->IndicScriptProperties.ViramaCodepoint) + if(Config.IndicScriptProperties.ViramaCodepoint) { - kbts__shape_scratchpad DummyScratchpad = KBTS__ZERO; + kbts_shape_scratchpad DummyScratchpad = KBTS__ZERO; kbts_glyph_storage DummyStorage = KBTS__ZERO; KBTS__DLLIST_SENTINEL_INIT(&DummyStorage.GlyphSentinel); KBTS__DLLIST_SENTINEL_INIT(&DummyStorage.FreeGlyphSentinel); // Bake the locl-ized virama. - kbts_glyph Virama = kbts_CodepointToGlyph(Font, (int)Result->IndicScriptProperties.ViramaCodepoint, 0, 0); - Result->Virama = kbts__Substitute1(&DummyScratchpad, Result, &DummyStorage, kbts__GetLookupList(Gsub), Result->Locl, KBTS__SKIP_FLAG_ZWNJ | KBTS__SKIP_FLAG_ZWJ, &Virama); + kbts_glyph Virama = kbts_CodepointToGlyph(Font, (int)Config.IndicScriptProperties.ViramaCodepoint, 0, 0); + Config.Virama = kbts__Substitute1(&DummyScratchpad, &Config, &DummyStorage, kbts__GetLookupList(Gsub), Config.Locl, KBTS__SKIP_FLAG_ZWNJ | KBTS__SKIP_FLAG_ZWJ, &Virama); } - if((Result->Script == KBTS_SCRIPT_THAI) || (Result->Script == KBTS_SCRIPT_LAO)) + if((Config.Script == KBTS_SCRIPT_THAI) || (Config.Script == KBTS_SCRIPT_LAO)) { - kbts_u32 NikhahitCodepoint = (Result->Script == KBTS_SCRIPT_THAI) ? 0xE4D : 0xECD; - kbts_u32 SaraAaCodepoint = (Result->Script == KBTS_SCRIPT_THAI) ? 0xE32 : 0xEB2; - Result->Nikhahit = kbts_CodepointToGlyph(Font, (int)NikhahitCodepoint, 0, 0); - Result->SaraAa = kbts_CodepointToGlyph(Font, (int)SaraAaCodepoint, 0, 0); + kbts_u32 NikhahitCodepoint = (Config.Script == KBTS_SCRIPT_THAI) ? 0xE4D : 0xECD; + kbts_u32 SaraAaCodepoint = (Config.Script == KBTS_SCRIPT_THAI) ? 0xE32 : 0xEB2; + Config.Nikhahit = kbts_CodepointToGlyph(Font, (int)NikhahitCodepoint, 0, 0); + Config.SaraAa = kbts_CodepointToGlyph(Font, (int)SaraAaCodepoint, 0, 0); } - Result->DottedCircle = kbts_CodepointToGlyph(Font, 0x25CC, 0, 0); - Result->Whitespace = kbts_CodepointToGlyph(Font, ' ', 0, 0); + Config.DottedCircle = kbts_CodepointToGlyph(Font, 0x25CC, 0, 0); + Config.Whitespace = kbts_CodepointToGlyph(Font, ' ', 0, 0); + + kbts_u16 *FeatureStageFirstLookupIndices = kbts__PointerPushArray(&Bump, kbts_u16, Config.OpList.FeatureStageCount + 1); + if(Memory) + { + KBTS__FOR(FeatureStageIndex, 0, Config.OpList.FeatureStageCount + 1) + { + FeatureStageFirstLookupIndices[FeatureStageIndex] = 0; + } + } if(ShapingTables[KBTS_SHAPING_TABLE_GSUB] || ShapingTables[KBTS_SHAPING_TABLE_GPOS]) - { // Initialize the per-feature lookup lists. - kbts__baked_feature_stage *BakedStages = kbts__PointerPushArray(&MemoryAt, kbts__baked_feature_stage, Result->OpList.FeatureStageCount); + { // Initialize sequential lookups. + kbts_un GlyphCount = Font->Blob->GlyphCount; - kbts_un FeatureStageIndex = 0; - KBTS__FOR(OpIndex, 0, Result->OpList.OpCount) + kbts_u32 *GlyphLookupMatrix = KBTS__POINTER_OFFSET(kbts_u32, Font->Blob, Font->Blob->GlyphLookupMatrixOffsetFromStartOfFile); + kbts_un GposLookupIndexOffset = Font->Blob->GposLookupIndexOffset; + + kbts__sequential_lookup *SequentialLookups = 0; + kbts_u32 *IdSequentialLookupMatrix = 0; + kbts_un SequentialLookupCount = 0; + + KBTS__FOR(Iter, 0, 2) { - kbts__op_kind Op = Result->OpList.Ops[OpIndex]; - int UserFeaturesAllowed = KBTS__IN_SET(Op, KBTS__SET32((KBTS__OP_KIND_GSUB_FEATURES_WITH_USER) - (KBTS__OP_KIND_GPOS_FEATURES))); + kbts_un ThisSequentialLookupCount = 0; + kbts_un FeatureStageIndex = 0; - if(KBTS__IN_SET(Op, KBTS__SET32((KBTS__OP_KIND_GSUB_FEATURES) - (KBTS__OP_KIND_GSUB_FEATURES_WITH_USER) - (KBTS__OP_KIND_GPOS_FEATURES)))) + KBTS__FOR(OpIndex, 0, Config.OpList.OpCount) { - kbts__feature_stage *FeatureStage = &Result->OpList.FeatureStages[FeatureStageIndex]; - kbts_shaping_table ShapingTable = (kbts_shaping_table)((Op == KBTS__OP_KIND_GPOS_FEATURES) ? KBTS_SHAPING_TABLE_GPOS : KBTS_SHAPING_TABLE_GSUB); - kbts__gsub_gpos *GsubGpos = ShapingTables[ShapingTable]; - kbts__langsys *Langsys = Result->Langsys[ShapingTable]; - kbts_un BakedFeatureCount = 0; - kbts_un BakedFeatureLookupIndexCount = 0; + kbts__op_kind Op = Config.OpList.Ops[OpIndex]; + kbts_b32 UserFeaturesAllowed = KBTS__IN_SET(Op, KBTS__SET32((KBTS__OP_KIND_GSUB_FEATURES_WITH_USER) + (KBTS__OP_KIND_GPOS_FEATURES))); - kbts__baked_feature_stage *BakedStage = &BakedStages[FeatureStageIndex]; - - if(GsubGpos && Langsys) + if(KBTS__IN_SET(Op, KBTS__SET32((KBTS__OP_KIND_GSUB_FEATURES) + (KBTS__OP_KIND_GSUB_FEATURES_WITH_USER) + (KBTS__OP_KIND_GPOS_FEATURES)))) { - kbts__feature_list *FeatureList = KBTS__POINTER_OFFSET(kbts__feature_list, GsubGpos, GsubGpos->FeatureListOffset); - kbts_u16 *FeatureIndices = KBTS__POINTER_AFTER(kbts_u16, Langsys); - kbts__baked_feature *BakedFeatures; + kbts__feature_stage *FeatureStage = &Config.OpList.FeatureStages[FeatureStageIndex]; + kbts_shaping_table ShapingTable = (kbts_shaping_table)((Op == KBTS__OP_KIND_GPOS_FEATURES) ? KBTS_SHAPING_TABLE_GPOS : KBTS_SHAPING_TABLE_GSUB); + kbts__gsub_gpos *GsubGpos = ShapingTables[ShapingTable]; + kbts__langsys *Langsys = Config.Langsys[ShapingTable]; + kbts_un BakedFeatureLookupIndexCount = 0; - // @Speed: Maybe we don't care about fragmentation and we just allocate Langsys->FeatureIndexCount in advance? - KBTS__FOR(FeatureIndexIndex, 0, Langsys->FeatureIndexCount) + kbts__baked_feature BakedFeatures[KBTS_MAX_SIMULTANEOUS_FEATURES]; + kbts_u16 BakedFeatureLookupIndicesRead[KBTS_MAX_SIMULTANEOUS_FEATURES]; + kbts_un BakedFeatureCount = 0; + + if(GsubGpos && Langsys) { - kbts_un FeatureIndex = FeatureIndices[FeatureIndexIndex]; - kbts__feature_pointer Feature = kbts__GetFeature(FeatureList, FeatureIndex); + kbts__feature_list *FeatureList = KBTS__POINTER_OFFSET(kbts__feature_list, GsubGpos, GsubGpos->FeatureListOffset); + kbts_u16 *FeatureIndices = KBTS__POINTER_AFTER(kbts_u16, Langsys); - kbts_u32 FeatureId = kbts__FeatureTagToId(Feature.Tag); - - if(Feature.Feature->LookupIndexCount && - ((UserFeaturesAllowed && - !kbts__ContainsFeature(&Result->Features, FeatureId)) || - kbts__ContainsFeature(&FeatureStage->Features, FeatureId))) + // @Speed: Maybe we don't care about fragmentation and we just allocate Langsys->FeatureIndexCount in advance? + KBTS__FOR(FeatureIndexIndex, 0, Langsys->FeatureIndexCount) { - BakedFeatureCount += 1; - } - } + kbts_un FeatureIndex = FeatureIndices[FeatureIndexIndex]; + kbts__feature_pointer Feature = kbts__GetFeature(FeatureList, FeatureIndex); - kbts_un BakedFeatureCapacity = KBTS__MIN(BakedFeatureCount, KBTS_MAX_SIMULTANEOUS_FEATURES); + kbts_u32 FeatureId = kbts__FeatureTagToId(Feature.Tag); - BakedFeatures = kbts__PointerPushArray(&MemoryAt, kbts__baked_feature, BakedFeatureCapacity); - - BakedFeatureCount = 0; - - KBTS__FOR(FeatureIndexIndex, 0, Langsys->FeatureIndexCount) - { - kbts_un FeatureIndex = FeatureIndices[FeatureIndexIndex]; - kbts__feature_pointer Feature = kbts__GetFeature(FeatureList, FeatureIndex); - - kbts_u32 FeatureId = kbts__FeatureTagToId(Feature.Tag); - // We add all features indiscriminately for ops that might incorporate user features. - if(Feature.Feature->LookupIndexCount && - // We add "all" features to user feature stages, except for features that are a default part of the shaper. - // If a user asks for a default feature explicitly, it is unclear whether it should be applied now or in its - // default stage. - // Leaving the feature in its default stage seems like the better thing to do, because, supposedly, these features - // will have been designed to apply at a specific point in the pipeline, and moving them makes little sense. - ((UserFeaturesAllowed && - !kbts__ContainsFeature(&Result->Features, FeatureId)) || - kbts__ContainsFeature(&FeatureStage->Features, FeatureId))) - { - kbts__baked_feature BakedFeature = KBTS__ZERO; - BakedFeature.FeatureTag = Feature.Tag; - BakedFeature.FeatureId = FeatureId; - // CAREFUL: We use SkipFlags as a temporary index until the end of the stage. - BakedFeature.SkipFlags = kbts__SkipFlags(BakedFeature.FeatureId, Result->Shaper); - BakedFeature.Count = Feature.Feature->LookupIndexCount; - // These point directly into the file. - BakedFeature.Indices = KBTS__POINTER_AFTER(kbts_u16, Feature.Feature); - - // @Incomplete - //if(FeatureVariations) - //{ - // KBTS__FOR(VariationIndex, 0, FeatureVariations->RecordCount) - // { - // kbts__feature_variation_pointer Variation = kbts__GetFeatureVariation(FeatureVariations, VariationIndex); - // KBTS__FOR(ConditionIndex, 0, Variation.ConditionSet->Count) - // { - // kbts__condition_1 *Condition = kbts__GetCondition(Variation.ConditionSet, ConditionIndex); - // KBTS_ASSERT(0); - // } - // } - //} - - // For Myanmar, we could try and tag glyphs depending on their Indic properties in BeginCluster, just like we do for - // Indic scripts. - // However, Harfbuzz does _not_ do this, so it seems like a bunch of work that would, at best, make us diverge from - // Harfbuzz more often. - if((Result->Shaper != KBTS_SHAPER_MYANMAR) && (FeatureId >= 1) && (FeatureId <= 32)) + if(Feature.Feature->LookupIndexCount && + ((UserFeaturesAllowed && + !kbts__ContainsFeature(&Config.Features, FeatureId)) || + kbts__ContainsFeature(&FeatureStage->Features, FeatureId))) { - // These must properly map KBTS__FEATURE_ID to kbts_glyph_flags! - BakedFeature.GlyphFilter = (1 << (FeatureId - 1)) & KBTS__GLYPH_FEATURE_MASK; + BakedFeatureCount += 1; + + if(BakedFeatureCount == KBTS_MAX_SIMULTANEOUS_FEATURES) + { + break; + } } + } - if(Memory) + BakedFeatureCount = 0; + + KBTS__FOR(FeatureIndexIndex, 0, Langsys->FeatureIndexCount) + { + kbts_un FeatureIndex = FeatureIndices[FeatureIndexIndex]; + kbts__feature_pointer Feature = kbts__GetFeature(FeatureList, FeatureIndex); + + kbts_u32 FeatureId = kbts__FeatureTagToId(Feature.Tag); + // We add all features indiscriminately for ops that might incorporate user features. + if(Feature.Feature->LookupIndexCount && + // We add "all" features to user feature stages, except for features that are a default part of the shaper. + // If a user asks for a default feature explicitly, it is unclear whether it should be applied now or in its + // default stage. + // Leaving the feature in its default stage seems like the better thing to do, because, supposedly, these features + // will have been designed to apply at a specific point in the pipeline, and moving them makes little sense. + ((UserFeaturesAllowed && + !kbts__ContainsFeature(&Config.Features, FeatureId)) || + kbts__ContainsFeature(&FeatureStage->Features, FeatureId))) { + kbts__baked_feature BakedFeature = KBTS__ZERO; + BakedFeature.FeatureTag = Feature.Tag; + BakedFeature.FeatureId = FeatureId; + // CAREFUL: We use SkipFlags as a temporary index until the end of the stage. + BakedFeature.SkipFlags = kbts__SkipFlags(ShapingTable, BakedFeature.FeatureId, Config.Shaper); + BakedFeature.Count = Feature.Feature->LookupIndexCount; + // These point directly into the file. + BakedFeature.Indices = KBTS__POINTER_AFTER(kbts_u16, Feature.Feature); + + // @Incomplete + //if(FeatureVariations) + //{ + // KBTS__FOR(VariationIndex, 0, FeatureVariations->RecordCount) + // { + // kbts__feature_variation_pointer Variation = kbts__GetFeatureVariation(FeatureVariations, VariationIndex); + // KBTS__FOR(ConditionIndex, 0, Variation.ConditionSet->Count) + // { + // kbts__condition_1 *Condition = kbts__GetCondition(Variation.ConditionSet, ConditionIndex); + // KBTS_ASSERT(0); + // } + // } + //} + + // For Myanmar, we could try and tag glyphs depending on their Indic properties in BeginCluster, just like we do for + // Indic scripts. + // However, Harfbuzz does _not_ do this, so it seems like a bunch of work that would, at best, make us diverge from + // Harfbuzz more often. + if((Config.Shaper != KBTS_SHAPER_MYANMAR) && (FeatureId >= 1) && (FeatureId <= 32)) + { + // These must properly map KBTS__FEATURE_ID to kbts_glyph_flags! + BakedFeature.GlyphFilter = (1u << (FeatureId - 1)) & KBTS__GLYPH_FEATURE_MASK; + } + BakedFeatures[BakedFeatureCount] = BakedFeature; - } - BakedFeatureCount += 1; - BakedFeatureLookupIndexCount += BakedFeature.Count; + BakedFeatureCount += 1; + BakedFeatureLookupIndexCount += BakedFeature.Count; - if(BakedFeatureCount >= BakedFeatureCapacity) - { - break; + if(BakedFeatureCount >= KBTS_MAX_SIMULTANEOUS_FEATURES) + { + break; + } } } - } - - kbts_u16 *OrderedFeatureIndices = kbts__PointerPushArray(&MemoryAt, kbts_u16, BakedFeatureLookupIndexCount); - kbts_u16 *BakedFeatureLookupIndicesRead = kbts__PointerPushArray(&MemoryAt, kbts_u16, BakedFeatureCount); - - if(Memory) - { - kbts_un OrderedFeatureIndicesWritten = 0; KBTS__FOR(BakedFeatureIndex, 0, BakedFeatureCount) { BakedFeatureLookupIndicesRead[BakedFeatureIndex] = 0; } - KBTS__FOR(OrderedLookupIndex, 0, BakedFeatureLookupIndexCount) + kbts_u16 LastLowestLookupIndex = 0xFFFF; + kbts_un BakedFeatureLookupIndicesLeft = BakedFeatureLookupIndexCount; + while(BakedFeatureLookupIndicesLeft) { kbts_u16 LowestLookupIndex = 0xFFFF; - kbts_un BestBakedFeatureIndex = 0; KBTS__FOR(BakedFeatureIndex, 0, BakedFeatureCount) { @@ -24878,36 +23519,131 @@ static kbts_shape_config *kbts__PlaceShapeConfig(kbts_font *Font, kbts_script Sc if(NextIndex < LowestLookupIndex) { LowestLookupIndex = NextIndex; - BestBakedFeatureIndex = BakedFeatureIndex; } } } - BakedFeatureLookupIndicesRead[BestBakedFeatureIndex] += 1; - OrderedFeatureIndices[OrderedFeatureIndicesWritten++] = (kbts_u16)BestBakedFeatureIndex; + // Handle the case where a single lookup is part of multiple features. + kbts_u32 GlyphFilter = 0; + kbts_u32 SkipFlags = 0; + kbts_b32 DefaultEnabled = 0; + KBTS__FOR(BakedFeatureIndex, 0, BakedFeatureCount) + { + kbts__baked_feature *BakedFeature = &BakedFeatures[BakedFeatureIndex]; + kbts_un LookupIndicesRead = BakedFeatureLookupIndicesRead[BakedFeatureIndex]; + + if(LookupIndicesRead < BakedFeature->Count) + { + kbts_u16 NextIndex = BakedFeature->Indices[LookupIndicesRead]; + + if(NextIndex == LowestLookupIndex) + { + GlyphFilter |= BakedFeature->GlyphFilter; + SkipFlags |= BakedFeature->SkipFlags; + + BakedFeatureLookupIndicesRead[BakedFeatureIndex] += 1; + BakedFeatureLookupIndicesLeft -= 1; + + if(kbts__ContainsFeature(&FeatureStage->Features, BakedFeature->FeatureId)) + { + DefaultEnabled = 1; + } + } + } + } + + if(LowestLookupIndex != LastLowestLookupIndex) + { + if(Iter && Memory && DefaultEnabled) + { + kbts_un FlatLookupIndex = (ShapingTable == KBTS_SHAPING_TABLE_GSUB) ? LowestLookupIndex : (LowestLookupIndex + GposLookupIndexOffset); + KBTS__FOR(GlyphIndex, 0, GlyphCount) + { + kbts__matrix_index MatrixIndex = kbts__GlyphLookupMatrixIndex(FlatLookupIndex, GlyphIndex, GlyphCount); + if(GlyphLookupMatrix[MatrixIndex.WordIndex] & (1u << MatrixIndex.BitIndex)) + { + kbts__matrix_index SequentialMatrixIndex = kbts__IdSequentialLookupMatrixIndex(ThisSequentialLookupCount, GlyphIndex, SequentialLookupCount); + IdSequentialLookupMatrix[SequentialMatrixIndex.WordIndex] |= 1u << SequentialMatrixIndex.BitIndex; + } + } + } + + if(!Iter) + { + ThisSequentialLookupCount += 1; + } + else + { + kbts__sequential_lookup *SequentialLookup = &SequentialLookups[ThisSequentialLookupCount++]; + if(Memory) + { + SequentialLookup->GlyphFilter = GlyphFilter; + SequentialLookup->SkipFlags = SkipFlags; + SequentialLookup->LookupIndex = LowestLookupIndex; + } + } + } + + LastLowestLookupIndex = LowestLookupIndex; } - - KBTS_ASSERT(OrderedFeatureIndicesWritten == BakedFeatureLookupIndexCount); - - BakedStage->FeatureCount = (kbts_u16)BakedFeatureCount; - BakedStage->FeatureIndexCount = (kbts_u16)BakedFeatureLookupIndexCount; - BakedStage->Features = BakedFeatures; - BakedStage->FeatureIndices = OrderedFeatureIndices; } + + if(Iter && Memory) + { + FeatureStageFirstLookupIndices[FeatureStageIndex + 1] = (kbts_u16)ThisSequentialLookupCount; + } + + KBTS_ASSERT(FeatureStageIndex < Config.OpList.FeatureStageCount); + FeatureStageIndex += 1; + } + } + + if(!Iter) + { + // We have the sequential lookup count. We can allocate our stuff. + SequentialLookupCount = ThisSequentialLookupCount; + SequentialLookups = kbts__PointerPushArray(&Bump, kbts__sequential_lookup, SequentialLookupCount); + + kbts_un LastSequentialLookupIndex = 0; + if(SequentialLookupCount) + { + LastSequentialLookupIndex = SequentialLookupCount - 1; + } + kbts_un LastGlyphIndex = 0; + if(GlyphCount) + { + LastGlyphIndex = GlyphCount - 1; } - KBTS_ASSERT(FeatureStageIndex < Result->OpList.FeatureStageCount); - FeatureStageIndex += 1; + kbts__matrix_index LastMatrixIndex = kbts__IdSequentialLookupMatrixIndex(LastSequentialLookupIndex, LastGlyphIndex, SequentialLookupCount); + kbts_un IdSequentialLookupMatrixSizeInWords = LastMatrixIndex.WordIndex + 1; + kbts_un IdSequentialLookupMatrixSizeInBytes = sizeof(kbts_u32) * IdSequentialLookupMatrixSizeInWords; + IdSequentialLookupMatrix = (kbts_u32 *)kbts__PointerPush(&Bump, IdSequentialLookupMatrixSizeInBytes, KBTS_ALIGNOF(kbts_u32)); + if(Memory) + { + KBTS_MEMSET(IdSequentialLookupMatrix, 0, IdSequentialLookupMatrixSizeInBytes); + } } } - Result->FeatureStages = BakedStages; + Config.SequentialLookups = SequentialLookups; + Config.IdSequentialLookupMatrix = IdSequentialLookupMatrix; + Config.GlyphCount = Font->Blob->GlyphCount; + Config.LookupCount = Font->Blob->LookupCount; + } + + Config.FeatureStageFirstLookupIndices = FeatureStageFirstLookupIndices; + + if(Memory) + { + *Result = Config; } } if(!Memory) { - *Size = (kbts_un)(MemoryAt - (char *)Memory) + KBTS_ALIGNOF(kbts_shape_config); + // We add the align, just to make sure we can accept any incoming pointer. + *Size = (Bump.At - (kbts_uptr)(void *)0) + KBTS_ALIGNOF(kbts_shape_config); Result = 0; } @@ -24956,60 +23692,13 @@ KBTS_EXPORT void kbts_DestroyShapeConfig(kbts_shape_config *Config) } } -static int kbts__ReadOp(kbts__shape_scratchpad *Scratchpad, kbts_shape_config *Config, kbts__op_kind End) -{ - KBTS_INSTRUMENT_FUNCTION_BEGIN; - int Result = 0; - - if(Config) - { - kbts__op_list *OpList = &Config->OpList; - - if(Scratchpad->Ip < OpList->OpCount) - { - kbts__op_kind Kind = OpList->Ops[Scratchpad->Ip++]; - - if((Kind == KBTS__OP_KIND_GSUB_FEATURES_WITH_USER) || - (Kind == KBTS__OP_KIND_GSUB_FEATURES) || - (Kind == KBTS__OP_KIND_GPOS_FEATURES)) - { - kbts__feature_stage *FeatureStage = &OpList->FeatureStages[Scratchpad->FeatureStagesRead++]; - Scratchpad->OpFeatures = &FeatureStage->Features; - - // We only have one GPOS_FEATURES op per op list, so it's fine to add user features like this. - if((Kind == KBTS__OP_KIND_GSUB_FEATURES_WITH_USER) || - (Kind == KBTS__OP_KIND_GPOS_FEATURES)) - { - if(Kind == KBTS__OP_KIND_GSUB_FEATURES_WITH_USER) - { - Kind = KBTS__OP_KIND_GSUB_FEATURES; - } - - KBTS__FOR(WordIndex, 0, KBTS__ARRAY_LENGTH(Scratchpad->ScratchFeatures.Flags)) - { - Scratchpad->ScratchFeatures.Flags[WordIndex] = Scratchpad->UserFeatures.Flags[WordIndex] | FeatureStage->Features.Flags[WordIndex]; - } - - Scratchpad->OpFeatures = &Scratchpad->ScratchFeatures; - } - } - - Scratchpad->OpKind = Kind; - Result = (Kind != End); - } - } - - KBTS_INSTRUMENT_FUNCTION_END; - return Result; -} - KBTS_EXPORT int kbts_SizeOfShapeContext(void) { int Result = sizeof(kbts_shape_context); return Result; } -KBTS_EXPORT kbts_shape_context *kbts_PlaceShapeContext(kbts_allocator_function *Allocator, void *AllocatorData, void *Memory) +KBTS_EXPORT kbts_shape_context *kbts_PlaceShapeContext2(kbts_allocator_function *Allocator, void *AllocatorData, void *Memory, kbts_shape_context_flags Flags) { kbts_shape_context *Result = (kbts_shape_context *)Memory; @@ -25037,14 +23726,22 @@ KBTS_EXPORT kbts_shape_context *kbts_PlaceShapeContext(kbts_allocator_function * Result->GlyphStorage.Arena.Allocator = Allocator; Result->GlyphStorage.Arena.AllocatorData = AllocatorData; - KBTS__DLLIST_SENTINEL_INIT(&Result->FeatureOverrideSentinel); - KBTS__DLLIST_SENTINEL_INIT(&Result->FreeFeatureOverrideSentinel); + Result->PublicFlags = Flags; + + KBTS__DLLIST_SENTINEL_INIT(&Result->ExistingShapeConfigBlockSentinel); + KBTS__DLLIST_SENTINEL_INIT(&Result->ExistingGlyphConfigBlockSentinel); } return Result; } -KBTS_EXPORT kbts_shape_context *kbts_PlaceShapeContextFixedMemory(void *Memory, int Size) +KBTS_EXPORT kbts_shape_context *kbts_PlaceShapeContext(kbts_allocator_function *Allocator, void *AllocatorData, void *Memory) +{ + kbts_shape_context *Result = kbts_PlaceShapeContext2(Allocator, AllocatorData, Memory, KBTS_SHAPE_CONTEXT_FLAG_NONE); + return Result; +} + +KBTS_EXPORT kbts_shape_context *kbts_PlaceShapeContextFixedMemory2(void *Memory, int Size, kbts_shape_context_flags Flags) { kbts_shape_context *Result = 0; kbts_un SizeOfShapeContext = kbts_SizeOfShapeContext(); @@ -25055,25 +23752,37 @@ KBTS_EXPORT kbts_shape_context *kbts_PlaceShapeContextFixedMemory(void *Memory, kbts_arena *Arena = (kbts_arena *)KBTS__POINTER_OFFSET(kbts_arena, Memory, SizeOfShapeContext); kbts__InitializeFixedMemoryArena(Arena, KBTS__POINTER_AFTER(void, Arena), (kbts_un)Size - ContextAndArenaSize); - Result = kbts_PlaceShapeContext(kbts__ArenaAllocator, Arena, Memory); + Result = kbts_PlaceShapeContext2(kbts__ArenaAllocator, Arena, Memory, Flags); } return Result; } -KBTS_EXPORT kbts_shape_context *kbts_CreateShapeContext(kbts_allocator_function *Allocator, void *AllocatorData) +KBTS_EXPORT kbts_shape_context *kbts_PlaceShapeContextFixedMemory(void *Memory, int Size) +{ + kbts_shape_context *Result = kbts_PlaceShapeContextFixedMemory2(Memory, Size, KBTS_SHAPE_CONTEXT_FLAG_NONE); + return Result; +} + +KBTS_EXPORT kbts_shape_context *kbts_CreateShapeContext2(kbts_allocator_function *Allocator, void *AllocatorData, kbts_shape_context_flags Flags) { if(!Allocator) { Allocator = kbts__DefaultAllocator; } - kbts_shape_context *Result = kbts_PlaceShapeContext(Allocator, AllocatorData, kbts__AllocatorAllocate(Allocator, AllocatorData, (kbts_un)kbts_SizeOfShapeContext())); + kbts_shape_context *Result = kbts_PlaceShapeContext2(Allocator, AllocatorData, kbts__AllocatorAllocate(Allocator, AllocatorData, (kbts_un)kbts_SizeOfShapeContext()), Flags); Result->SelfAllocator = Allocator; Result->SelfAllocatorData = AllocatorData; return Result; } +KBTS_EXPORT kbts_shape_context *kbts_CreateShapeContext(kbts_allocator_function *Allocator, void *AllocatorData) +{ + kbts_shape_context *Result = kbts_CreateShapeContext2(Allocator, AllocatorData, KBTS_SHAPE_CONTEXT_FLAG_NONE); + return Result; +} + KBTS_EXPORT void kbts_DestroyShapeContext(kbts_shape_context *Context) { if(Context) @@ -25326,95 +24035,176 @@ KBTS_EXPORT kbts_glyph *kbts_PushGlyph(kbts_glyph_storage *Storage, kbts_font *F return Result; } -static int kbts__FeatureOverrideIsExtra(kbts_u32 Tag, int Value) -{ - int Result = 0; - kbts__feature_id Id = kbts__FeatureTagToId(Tag); - if(!Id || (kbts_u32)(Value > 1)) +KBTS_EXPORT int kbts_SizeOfGlyphConfig(kbts_shape_config *ShapeConfig, kbts_feature_override *Overrides, int OverrideCount) +{ + kbts_un NonBinaryOverrideCount = 0; + KBTS__FOR(OverrideIndex, 0, (kbts_un)OverrideCount) { - Result = 1; - } + kbts_feature_override *Override = &Overrides[OverrideIndex]; - return Result; -} - -static void kbts__AddExtraFeatureOverride(kbts_glyph_config *Config, kbts__feature_override *Extra, kbts_u32 Tag, int Value) -{ - Extra->Tag = Tag; - Extra->Value = Value; - - KBTS__DLLIST_INSERT_BEFORE(&Extra->Header, &Config->FeatureOverrideSentinel); -} - -KBTS_EXPORT int kbts_SizeOfGlyphConfig(kbts_feature_override *Overrides, int OverrideCount) -{ - kbts_un StoredOverrideCount = 0; - - if(OverrideCount > 0) - { - KBTS__FOR(OverrideIndex, 0, (kbts_un)OverrideCount) + if(Override->Value > 1) { - kbts_feature_override *Override = &Overrides[OverrideIndex]; - - if(kbts__FeatureOverrideIsExtra(Override->Tag, Override->Value)) - { - StoredOverrideCount += 1; - } + NonBinaryOverrideCount += 1; } } - kbts_un Result = sizeof(kbts_glyph_config) + StoredOverrideCount * sizeof(kbts__feature_override); + kbts_un SequentialLookupCount = kbts__SequentialLookupCount(ShapeConfig); + kbts_un LastSequentialLookupIndex = (SequentialLookupCount) ? (SequentialLookupCount - 1) : 0; + kbts__matrix_index LastRowEntryMatrixIndex = kbts__IdSequentialLookupMatrixIndex(LastSequentialLookupIndex, 0, SequentialLookupCount); + kbts_un MatrixRowSizeInBytes = sizeof(kbts_u32) * (LastRowEntryMatrixIndex.WordIndex + 1); + + kbts_un Result = sizeof(kbts_glyph_config) + + NonBinaryOverrideCount * sizeof(kbts__enabled_lookup) + + MatrixRowSizeInBytes * 2; return (int)Result; } -KBTS_EXPORT kbts_glyph_config *kbts_PlaceGlyphConfig(kbts_feature_override *Overrides, int OverrideCount, void *Memory) +KBTS_EXPORT kbts_glyph_config *kbts_PlaceGlyphConfig(kbts_shape_config *ShapeConfig, kbts_feature_override *Overrides, int OverrideCount, void *Memory) { - kbts_glyph_config *Result = (kbts_glyph_config *)Memory; - if(Memory) + kbts__pointer_bump_allocator Bump = kbts__PointerBumpAllocator(Memory); + kbts_glyph_config *Result = kbts__PointerPushType(&Bump, kbts_glyph_config); + + if(ShapeConfig && Memory) { KBTS_MEMSET(Result, 0, sizeof(*Result)); - KBTS__DLLIST_SENTINEL_INIT(&Result->FeatureOverrideSentinel); - kbts__feature_override *ExtraOverrides = KBTS__POINTER_AFTER(kbts__feature_override, Result); + kbts_un SequentialLookupCount = kbts__SequentialLookupCount(ShapeConfig); + kbts_un LastSequentialLookupIndex = (SequentialLookupCount) ? (SequentialLookupCount - 1) : 0; + kbts__matrix_index LastRowEntryMatrixIndex = kbts__IdSequentialLookupMatrixIndex(LastSequentialLookupIndex, 0, SequentialLookupCount); + kbts_un MatrixRowSizeInBytes = sizeof(kbts_u32) * (LastRowEntryMatrixIndex.WordIndex + 1); + kbts_u32 *EnabledLookupBits = (kbts_u32 *)kbts__PointerPush(&Bump, MatrixRowSizeInBytes, KBTS_ALIGNOF(kbts_u32)); + KBTS_MEMSET(EnabledLookupBits, 0, MatrixRowSizeInBytes); + kbts_u32 *DisabledLookupBits = (kbts_u32 *)kbts__PointerPush(&Bump, MatrixRowSizeInBytes, KBTS_ALIGNOF(kbts_u32)); + KBTS_MEMSET(DisabledLookupBits, 0, MatrixRowSizeInBytes); - if(OverrideCount > 0) + kbts__enabled_lookup NonBinaryEnabledLookups[KBTS_MAX_SIMULTANEOUS_FEATURES]; + kbts_un NonBinaryEnabledLookupCount = 0; + + kbts__feature_set OverriddenFeatures = KBTS__ZERO; + KBTS__FOR(OverrideIndex, 0, (kbts_un)OverrideCount) { - KBTS__FOR(OverrideIndex, 0, (kbts_un)OverrideCount) + kbts_feature_override *Override = &Overrides[OverrideIndex]; + kbts__AddFeature(&OverriddenFeatures, kbts__FeatureTagToId(Override->Tag)); + } + + KBTS__FOR(ShapingTable, 0, KBTS_SHAPING_TABLE_COUNT) + { + // @Duplication + kbts__gsub_gpos *GsubGpos = kbts__BlobTableDataType(ShapeConfig->Font->Blob, (ShapingTable == KBTS_SHAPING_TABLE_GSUB) ? KBTS_BLOB_TABLE_ID_GSUB : KBTS_BLOB_TABLE_ID_GPOS, kbts__gsub_gpos); + + kbts_un FirstSequentialLookupIndex = 0; + kbts_un OnePastLastSequentialLookupIndex = kbts__GsubSequentialLookupCount(ShapeConfig); + if(ShapingTable == KBTS_SHAPING_TABLE_GPOS) { - kbts_feature_override *Override = &Overrides[OverrideIndex]; - kbts__feature_id Id = kbts__FeatureTagToId(Override->Tag); + FirstSequentialLookupIndex = OnePastLastSequentialLookupIndex; + OnePastLastSequentialLookupIndex = kbts__SequentialLookupCount(ShapeConfig); + } - if(kbts__FeatureOverrideIsExtra(Override->Tag, Override->Value)) + kbts_lookup_list *LookupList = kbts__GetLookupList(GsubGpos); + + kbts__iterate_features IterateFeatures = kbts__IterateFeatures(ShapeConfig, (kbts_shaping_table)ShapingTable, OverriddenFeatures); + + while(kbts__NextFeature(&IterateFeatures)) + { + kbts_feature_override *FoundOverride = 0; + + KBTS__FOR(OverrideIndex, 0, (kbts_un)OverrideCount) { - kbts__feature_override *ExtraOverride = &ExtraOverrides[Result->ExtraOverrideCount++]; + kbts_feature_override *Override = &Overrides[OverrideIndex]; - kbts__AddExtraFeatureOverride(Result, ExtraOverride, Override->Tag, Override->Value); + if(Override->Tag == IterateFeatures.CurrentFeatureTag) + { + FoundOverride = Override; + + break; + } } - kbts__AddFeature((Override->Value) ? &Result->EnabledFeatures : &Result->DisabledFeatures, Id); - } + // @Robustness: Why did we check for unregistered features here? + // if(!FoundOverride && + // (kbts__FeatureTagToId(IterateFeatures.CurrentFeatureTag) == KBTS__FEATURE_ID_UNREGISTERED)) + // { + // continue; + // } + if(FoundOverride) + { + kbts__iterate_lookups IterateLookups = kbts__IterateLookups(LookupList, IterateFeatures.Feature); - if(Result->ExtraOverrideCount) - { - kbts__feature_override *Last = &ExtraOverrides[Result->ExtraOverrideCount - 1]; - Last->Header.Next = &Result->FeatureOverrideSentinel; - Result->FeatureOverrideSentinel.Prev = &Last->Header; + while(kbts__NextLookup(&IterateLookups)) + { + kbts_u16 LookupIndex = IterateLookups.LookupIndex; + kbts_un FoundSequentialLookupIndex = 0; + kbts_b32 Found = 0; + KBTS__FOR(SequentialLookupIndex, FirstSequentialLookupIndex, OnePastLastSequentialLookupIndex) + { + kbts__sequential_lookup *SequentialLookup = &ShapeConfig->SequentialLookups[SequentialLookupIndex]; + + if(SequentialLookup->LookupIndex == LookupIndex) + { + FoundSequentialLookupIndex = SequentialLookupIndex; + Found = 1; + + break; + } + } + + if(Found) + { + kbts__matrix_index SequentialMatrixIndex = kbts__IdSequentialLookupMatrixIndex(FoundSequentialLookupIndex, 0, SequentialLookupCount); + if(FoundOverride->Value) + { + EnabledLookupBits[SequentialMatrixIndex.WordIndex] |= 1u << SequentialMatrixIndex.BitIndex; + + if((FoundOverride->Value > 1) && + (NonBinaryEnabledLookupCount < KBTS_MAX_SIMULTANEOUS_FEATURES)) + { + kbts__enabled_lookup *NewEnabled = &NonBinaryEnabledLookups[NonBinaryEnabledLookupCount++]; + NewEnabled->SequentialLookupIndex = (kbts_u16)FoundSequentialLookupIndex; + NewEnabled->Value = (kbts_u16)FoundOverride->Value; + } + } + else + { + DisabledLookupBits[SequentialMatrixIndex.WordIndex] |= 1u << SequentialMatrixIndex.BitIndex; + } + } + } + } } } + + kbts__enabled_lookup *OutNonBinaryEnabledLookups = 0; + if(NonBinaryEnabledLookupCount) + { + OutNonBinaryEnabledLookups = kbts__PointerPushArray(&Bump, kbts__enabled_lookup, NonBinaryEnabledLookupCount); + KBTS_MEMCPY(OutNonBinaryEnabledLookups, NonBinaryEnabledLookups, sizeof(*NonBinaryEnabledLookups) * NonBinaryEnabledLookupCount); + } + + Result->NonBinaryEnabledLookups = OutNonBinaryEnabledLookups; + Result->NonBinaryEnabledLookupCount = (kbts_u32)NonBinaryEnabledLookupCount; + Result->EnabledLookupBits = EnabledLookupBits; + Result->DisabledLookupBits = DisabledLookupBits; + } + + kbts__feature_set OverriddenFeatures = KBTS__ZERO; + KBTS__FOR(OverrideIndex, 0, (kbts_un)OverrideCount) + { + kbts_feature_override *Override = &Overrides[OverrideIndex]; + kbts__AddFeature(&OverriddenFeatures, kbts__FeatureTagToId(Override->Tag)); } return Result; } -KBTS_EXPORT kbts_glyph_config *kbts_CreateGlyphConfig(kbts_feature_override *Overrides, int OverrideCount, kbts_allocator_function *Allocator, void *AllocatorData) +KBTS_EXPORT kbts_glyph_config *kbts_CreateGlyphConfig(kbts_shape_config *ShapeConfig, kbts_feature_override *Overrides, int OverrideCount, kbts_allocator_function *Allocator, void *AllocatorData) { if(!Allocator) { Allocator = kbts__DefaultAllocator; } - kbts_glyph_config *Result = kbts_PlaceGlyphConfig(Overrides, OverrideCount, kbts__AllocatorAllocate(Allocator, AllocatorData, (kbts_un)kbts_SizeOfGlyphConfig(Overrides, OverrideCount))); + kbts_glyph_config *Result = kbts_PlaceGlyphConfig(ShapeConfig, Overrides, OverrideCount, kbts__AllocatorAllocate(Allocator, AllocatorData, (kbts_un)kbts_SizeOfGlyphConfig(ShapeConfig, Overrides, OverrideCount))); if(Result) { @@ -25447,12 +24237,14 @@ KBTS_EXPORT void kbts_ShapeBegin(kbts_shape_context *Context, kbts_direction Par kbts_ClearActiveGlyphs(&Context->GlyphStorage); Context->BreakStartIndex = 0; - Context->CurrentGlyphConfig = 0; + + // Scratch features persist across frames. Don't reset ScratchFeatureOverrideCount. + Context->CurrentFeatureOverrides = 0; + Context->CurrentFeatureOverrideCount = 0; Context->NeedNewGlyphConfig = 1; Context->ParagraphDirection = ParagraphDirection; Context->Language = Language; - Context->UserFeatures = KBTS__ZERO_TYPE(kbts__feature_set); Context->InputCodepointCount = 0; Context->NextUserId = 0; @@ -25461,7 +24253,6 @@ KBTS_EXPORT void kbts_ShapeBegin(kbts_shape_context *Context, kbts_direction Par Context->RunFont = 0; Context->RunScript = 0; Context->RunDirection = 0; - Context->ExistingShapeConfigCount = 0; kbts_BreakBegin(&Context->BreakState, ParagraphDirection, KBTS_JAPANESE_LINE_BREAK_STYLE_NORMAL, 0); } @@ -25483,19 +24274,19 @@ static kbts__input_codepoint_index kbts__InputCodepointIndex(kbts_un FlatCodepoi { Result.BlockIndex = 0; Result.CodepointIndex = (kbts_u32)FlatCodepointIndex; - Result.BlockCodepointCount = 1 << (KBTS__INPUT_CODEPOINT_FIRST_BLOCK_MSB + 1); + Result.BlockCodepointCount = 1u << (KBTS__INPUT_CODEPOINT_FIRST_BLOCK_MSB + 1); } else { Result.BlockIndex = (kbts_u32)(MsbPosition - KBTS__INPUT_CODEPOINT_FIRST_BLOCK_MSB); - Result.CodepointIndex = (kbts_u32)(FlatCodepointIndex & ~(1 << MsbPosition)); - Result.BlockCodepointCount = (kbts_u32)(1 << MsbPosition); + Result.CodepointIndex = (kbts_u32)(FlatCodepointIndex & ~(1ull << MsbPosition)); + Result.BlockCodepointCount = (kbts_u32)(1u << MsbPosition); } return Result; } -static kbts_shape_codepoint *kbts__InputCodepoint(kbts_shape_context *Context, kbts_un Index) +static kbts_shape_codepoint *kbts__InputCodepoint(kbts_shape_context *Context, kbts_un Index, kbts_b32 AllocateIfNeeded) { kbts_shape_codepoint *Result = 0; @@ -25504,7 +24295,7 @@ static kbts_shape_codepoint *kbts__InputCodepoint(kbts_shape_context *Context, k kbts__input_codepoint_index InputIndex = kbts__InputCodepointIndex(Index); kbts_shape_codepoint *Block = Context->InputBlocks[InputIndex.BlockIndex]; - if(!Block) + if(!Block && AllocateIfNeeded) { Block = kbts__PushArray(&Context->PermanentArena, kbts_shape_codepoint, InputIndex.BlockCodepointCount); if(!Block) @@ -25560,10 +24351,11 @@ static int kbts__NextInputCodepoint(kbts_shape_codepoint_iterator *It, int *Code { It->BlockIndex += 1; It->CodepointIndex = 0; - It->CurrentBlockCodepointCount = (It->BlockIndex == It->EndBlockIndex) ? It->OnePastLastCodepointIndex : (1 << (It->BlockIndex + KBTS__INPUT_CODEPOINT_FIRST_BLOCK_MSB)); + It->CurrentBlockCodepointCount = (It->BlockIndex == It->EndBlockIndex) ? It->OnePastLastCodepointIndex : (1u << (It->BlockIndex + KBTS__INPUT_CODEPOINT_FIRST_BLOCK_MSB)); } - if(It->BlockIndex <= It->EndBlockIndex) + if((It->BlockIndex <= It->EndBlockIndex) && + (It->CodepointIndex < It->CurrentBlockCodepointCount)) { if(CodepointIndex) { @@ -25604,8 +24396,15 @@ KBTS_EXPORT int kbts_ShapeGetShapeCodepoint(kbts_shape_context *Context, int Cod if((CodepointIndex >= 0) && ((kbts_un)CodepointIndex < Context->InputCodepointCount)) { - kbts_shape_codepoint *Source = kbts__InputCodepoint(Context, (kbts_un)CodepointIndex); - *Codepoint = *Source; + kbts_shape_codepoint *Source = kbts__InputCodepoint(Context, (kbts_un)CodepointIndex, 0); + if(Source) + { + *Codepoint = *Source; + } + else + { + KBTS_MEMSET(Codepoint, 0, sizeof(*Codepoint)); + } Result = 1; } @@ -25622,7 +24421,7 @@ static void kbts__UpdateBreaks(kbts_shape_context *Context) { // Strictly speaking, we do not need all of the flags, but we record them all anyway so we can expose them to the user. kbts_un BreakPosition = (kbts_u32)Break.Position + Context->BreakStartIndex; - kbts_shape_codepoint *InputCodepoint = kbts__InputCodepoint(Context, BreakPosition); + kbts_shape_codepoint *InputCodepoint = kbts__InputCodepoint(Context, BreakPosition, 1); if(Break.Flags & KBTS_BREAK_FLAG_LINE_HARD) { @@ -25633,12 +24432,12 @@ static void kbts__UpdateBreaks(kbts_shape_context *Context) { // Try fonts, potentially break run. kbts_font *MatchFont = 0; + kbts_shape_context_flags ShapeContextFlags = Context->PublicFlags; - for(kbts_un FontIndex = Context->FontCount; - FontIndex; - --FontIndex) + KBTS__FOR(FontIndex_, 0, Context->FontCount) { - kbts_font *Font = Context->Fonts[FontIndex - 1].Font; + kbts_un FontIndex = (ShapeContextFlags & KBTS_SHAPE_CONTEXT_FLAG_FONT_PRIORITY_BOTTOM_TO_TOP) ? FontIndex_ : (Context->FontCount - 1 - FontIndex_); + kbts_font *Font = Context->Fonts[FontIndex].Font; kbts_font_coverage_test CoverageTest; kbts_FontCoverageTestBegin(&CoverageTest, Font); @@ -25666,18 +24465,21 @@ static void kbts__UpdateBreaks(kbts_shape_context *Context) Context->LastGraphemeBreakIndex = (kbts_u32)BreakPosition; } - InputCodepoint->BreakFlags |= Break.Flags; - if(Break.Flags & KBTS_BREAK_FLAG_SCRIPT) + if(InputCodepoint) { - InputCodepoint->Script = Break.Script; - } - if(Break.Flags & KBTS_BREAK_FLAG_DIRECTION) - { - InputCodepoint->Direction = Break.Direction; - } - if(Break.Flags & KBTS_BREAK_FLAG_PARAGRAPH_DIRECTION) - { - InputCodepoint->ParagraphDirection = Break.ParagraphDirection; + InputCodepoint->BreakFlags |= Break.Flags; + if(Break.Flags & KBTS_BREAK_FLAG_SCRIPT) + { + InputCodepoint->Script = Break.Script; + } + if(Break.Flags & KBTS_BREAK_FLAG_DIRECTION) + { + InputCodepoint->Direction = Break.Direction; + } + if(Break.Flags & KBTS_BREAK_FLAG_PARAGRAPH_DIRECTION) + { + InputCodepoint->ParagraphDirection = Break.ParagraphDirection; + } } } } @@ -25737,44 +24539,51 @@ KBTS_EXPORT void kbts_ShapeCodepointWithUserId(kbts_shape_context *Context, int { if(Context->NeedNewGlyphConfig) { - kbts_glyph_config *Config = kbts__PushType(&Context->ScratchArena, kbts_glyph_config); - if(!Config) + kbts_un NewFeatureOverrideCount = Context->ScratchFeatureOverrideCount; + + if(Context->ScratchFeatureOverrideCount) { - Context->Error = KBTS_SHAPE_ERROR_OUT_OF_MEMORY; - return; - } + kbts_feature_override UniqueFeatureOverrides[KBTS_MAX_SIMULTANEOUS_FEATURES]; + kbts_un UniqueFeatureOverrideCount = 0; - *Config = KBTS__ZERO_TYPE(kbts_glyph_config); - KBTS__DLLIST_SENTINEL_INIT(&Config->FeatureOverrideSentinel); - - kbts__feature_set Features = KBTS__ZERO; - for(kbts__feature_override_header *OverrideHeader = Context->FeatureOverrideSentinel.Prev; - OverrideHeader != &Context->FeatureOverrideSentinel; - OverrideHeader = OverrideHeader->Prev) - { - kbts__feature_override *Override = (kbts__feature_override *)OverrideHeader; - kbts__feature_id Id = kbts__FeatureTagToId(Override->Tag); - - if(!Id || !kbts__ContainsFeature(&Features, Id)) + for(kbts_un ScratchFeatureOverrideIndex = Context->ScratchFeatureOverrideCount; + ScratchFeatureOverrideIndex; + --ScratchFeatureOverrideIndex) { - if(kbts__FeatureOverrideIsExtra(Override->Tag, Override->Value)) - { - kbts__feature_override *Insert = kbts__PushType(&Context->ScratchArena, kbts__feature_override); - if(!Insert) - { - Context->Error = KBTS_SHAPE_ERROR_OUT_OF_MEMORY; - return; - } + kbts_feature_override *ScratchOverride = &Context->ScratchFeatureOverrides[ScratchFeatureOverrideIndex - 1]; + kbts_u32 ScratchTag = ScratchOverride->Tag; - kbts__AddExtraFeatureOverride(Config, Insert, Override->Tag, Override->Value); + kbts_b32 Dupe = 0; + KBTS__FOR(UniqueFeatureOverrideIndex, 0, UniqueFeatureOverrideCount) + { + kbts_feature_override *UniqueOverride = &UniqueFeatureOverrides[UniqueFeatureOverrideIndex]; + + if(UniqueOverride->Tag == ScratchTag) + { + Dupe = 1; + break; + } } - kbts__AddFeature(&Features, Id); - kbts__AddFeature((Override->Value) ? &Config->EnabledFeatures : &Config->DisabledFeatures, Id); + if(!Dupe) + { + UniqueFeatureOverrides[UniqueFeatureOverrideCount++] = *ScratchOverride; + } } + + kbts_feature_override *Hoisted = kbts__PushArray(&Context->ScratchArena, kbts_feature_override, UniqueFeatureOverrideCount); + if(!Hoisted) + { + Context->Error = KBTS_SHAPE_ERROR_OUT_OF_MEMORY; + return; + } + KBTS_MEMCPY(Hoisted, Context->ScratchFeatureOverrides, sizeof(*Hoisted) * UniqueFeatureOverrideCount); + + Context->CurrentFeatureOverrides = Hoisted; + NewFeatureOverrideCount = UniqueFeatureOverrideCount; } - Context->CurrentGlyphConfig = Config; + Context->CurrentFeatureOverrideCount = (kbts_u32)NewFeatureOverrideCount; Context->NeedNewGlyphConfig = 0; } @@ -25783,7 +24592,8 @@ KBTS_EXPORT void kbts_ShapeCodepointWithUserId(kbts_shape_context *Context, int kbts_shape_codepoint InputCodepoint = KBTS__ZERO; InputCodepoint.Codepoint = Codepoint; InputCodepoint.UserId = UserId; - InputCodepoint.Config = Context->CurrentGlyphConfig; + InputCodepoint.FeatureOverrides = Context->CurrentFeatureOverrides; + InputCodepoint.FeatureOverrideCount = Context->CurrentFeatureOverrideCount; // @Robustness: There is probably a saner way of doing this. // When we do a manual break, we may have line breaks go out-of-bounds, and we @@ -25807,7 +24617,7 @@ KBTS_EXPORT void kbts_ShapeCodepointWithUserId(kbts_shape_context *Context, int Context->Flags &= ~KBTS__CONTEXT_FLAG_START_OF_MANUAL_RUN; } - kbts_shape_codepoint *To = kbts__InputCodepoint(Context, FlatCodepointIndex); + kbts_shape_codepoint *To = kbts__InputCodepoint(Context, FlatCodepointIndex, 1); if(To) { *To = InputCodepoint; @@ -25887,7 +24697,7 @@ KBTS_EXPORT void kbts_ShapeUtf8WithUserId(kbts_shape_context *Context, const cha kbts_decode Decode = kbts_DecodeUtf8(At, (kbts_un)(End - At)); if(Decode.Valid) - { + { kbts_ShapeCodepointWithUserId(Context, Decode.Codepoint, UserId); UserId += CodepointIncrement; @@ -25932,23 +24742,12 @@ KBTS_EXPORT void kbts_ShapePushFeature(kbts_shape_context *Context, kbts_u32 Tag { if(!Context->Error) { - kbts__feature_override *Override; - kbts__feature_override_header *Header = Context->FreeFeatureOverrideSentinel.Prev; - if(Header != &Context->FreeFeatureOverrideSentinel) + if(Context->ScratchFeatureOverrideCount < KBTS__ARRAY_LENGTH(Context->ScratchFeatureOverrides)) { - Override = (kbts__feature_override *)Header; + kbts_feature_override *Override = &Context->ScratchFeatureOverrides[Context->ScratchFeatureOverrideCount++]; + Override->Tag = Tag; + Override->Value = Value; } - else - { - Override = kbts__PushType(&Context->PermanentArena, kbts__feature_override); - } - - KBTS__DLLIST_INSERT_BEFORE(&Override->Header, &Context->FeatureOverrideSentinel); - Override->Tag = Tag; - Override->Value = Value; - - kbts__feature_id Id = kbts__FeatureTagToId(Tag); - kbts__AddFeature(&Context->UserFeaturesEnabled, Id); Context->NeedNewGlyphConfig = 1; } @@ -25960,19 +24759,20 @@ KBTS_EXPORT int kbts_ShapePopFeature(kbts_shape_context *Context, kbts_u32 Tag) if(!Context->Error) { - for(kbts__feature_override_header *Header = Context->FeatureOverrideSentinel.Prev; - Header != &Context->FeatureOverrideSentinel; - Header = Header->Prev) + for(kbts_un ScratchFeatureOverrideIndex = Context->ScratchFeatureOverrideCount; + ScratchFeatureOverrideIndex; + --ScratchFeatureOverrideIndex) { - kbts__feature_override *Override = (kbts__feature_override *)Header; + kbts_feature_override *Override = &Context->ScratchFeatureOverrides[ScratchFeatureOverrideIndex - 1]; if(Override->Tag == Tag) { - Result = 1; - - KBTS__DLLIST_REMOVE(Header); - KBTS__DLLIST_INSERT_BEFORE(Header, &Context->FreeFeatureOverrideSentinel); + KBTS__FOR(MoveIndex, ScratchFeatureOverrideIndex, Context->ScratchFeatureOverrideCount) + { + Context->ScratchFeatureOverrides[ScratchFeatureOverrideIndex - 1] = Context->ScratchFeatureOverrides[ScratchFeatureOverrideIndex]; + } + Context->ScratchFeatureOverrideCount -= 1; break; } } @@ -25982,122 +24782,217 @@ KBTS_EXPORT int kbts_ShapePopFeature(kbts_shape_context *Context, kbts_u32 Tag) Context->NeedNewGlyphConfig = 1; } } - + return Result; } -static void kbts__ShapeDirect(kbts__shape_scratchpad *Scratchpad, kbts_shape_config *Config, kbts_glyph_storage *Storage) +static void kbts__ShapeDirect(kbts_shape_scratchpad *Scratchpad, kbts_glyph_storage *Storage, kbts_direction RunDirection) { - KBTS_INSTRUMENT_BLOCK_BEGIN(ReadOpLoop0); + KBTS_INSTRUMENT_FUNCTION_BEGIN; - // For simple shapers, all of the shaping happens in this single loop. - // For complex shapers, this loop is preparing the text for clustering logic, which happens below. - while(kbts__ReadOp(Scratchpad, Config, KBTS__OP_KIND_BEGIN_CLUSTER)) + if(kbts__GlyphIsValid(Storage, Storage->GlyphSentinel.Next)) { - kbts__ExecuteOp(Scratchpad, Config, Storage); - - if(Scratchpad->Error) - { - return; - } - } - - KBTS_INSTRUMENT_BLOCK_END(ReadOpLoop0); - - if(Scratchpad->OpKind == KBTS__OP_KIND_BEGIN_CLUSTER) - { - kbts_u32 BeginClusterIp = Scratchpad->Ip; - kbts_u32 BeginClusterFeatureStagesRead = Scratchpad->FeatureStagesRead; - kbts_glyph *TopLevelGlyph = Storage->GlyphSentinel.Next; - - Scratchpad->ClusterAtStartOfWord = 1; - - while(kbts__GlyphIsValid(Storage, TopLevelGlyph)) - { - int WordBreak = 0; - - Scratchpad->Ip = BeginClusterIp; - Scratchpad->FeatureStagesRead = BeginClusterFeatureStagesRead; + Scratchpad->Ip = 0; + Scratchpad->FeatureStagesRead = 0; + Scratchpad->OpKind = 0; + Scratchpad->RunDirection = RunDirection; + Scratchpad->NextGlyphUid = 0; + Scratchpad->SequentialLookupIndexIndex = 0; + { // @Cleanup @Speed + kbts_un SequentialLookupCount = kbts__SequentialLookupCount(Scratchpad->Config); + KBTS__FOR(LookupIndex, 0, SequentialLookupCount) { - // We need to store this in case TopLevelGlyph is reordered. - kbts_glyph *OneBeforeFirstClusterGlyph = TopLevelGlyph->Prev; - kbts_glyph *OnePastLastClusterGlyph = kbts__BeginCluster(Scratchpad, Config, Storage, TopLevelGlyph); - - if(Scratchpad->Error) - { - return; - } - - WordBreak = !(OnePastLastClusterGlyph->Prev->UnicodeFlags & KBTS_UNICODE_FLAG_PART_OF_WORD); - Scratchpad->Cluster = kbts__PushGlyphList(Storage, OneBeforeFirstClusterGlyph->Next, OnePastLastClusterGlyph->Prev); + kbts__FreeGlyphBucket(Scratchpad, LookupIndex); } - - while(kbts__ReadOp(Scratchpad, Config, KBTS__OP_KIND_END_CLUSTER)) - { - kbts__ExecuteOp(Scratchpad, Config, Storage); - - if(Scratchpad->Error) - { - return; - } - } - - kbts__EndCluster(Scratchpad, Config, Storage); - - while(kbts__ReadOp(Scratchpad, Config, KBTS__OP_KIND_END_SYLLABLE)) - { - kbts__ExecuteOp(Scratchpad, Config, Storage); - - if(Scratchpad->Error) - { - return; - } - } - - // Reattach the cluster to the main list. - Storage->GlyphSentinel.Next->Prev = Scratchpad->Cluster.OneBeforeFirst; - Storage->GlyphSentinel.Prev->Next = Scratchpad->Cluster.OnePastLast; - TopLevelGlyph = Scratchpad->Cluster.OnePastLast; - - kbts__PopGlyphList(Storage, &Scratchpad->Cluster); - - Scratchpad->ClusterAtStartOfWord = WordBreak; } - // Post-clustering ops work across clusters. - // This is where Indic GPOS + post-passes happen. - while(kbts__ReadOp(Scratchpad, Config, 0)) + KBTS_INSTRUMENT_BLOCK_BEGIN(ReadOpLoop0); + + // For simple shapers, all of the shaping happens in this single loop. + // For complex shapers, this loop is preparing the text for clustering logic, which happens below. + while(kbts__ReadOp(Scratchpad, KBTS__OP_KIND_BEGIN_CLUSTER)) { - kbts__ExecuteOp(Scratchpad, Config, Storage); + kbts__ExecuteOp(Scratchpad, Storage); if(Scratchpad->Error) { return; } } + + KBTS_INSTRUMENT_BLOCK_END(ReadOpLoop0); + + if(Scratchpad->OpKind == KBTS__OP_KIND_BEGIN_CLUSTER) + { + kbts_u32 BeginClusterSequentialLookupIndexIndex = Scratchpad->SequentialLookupIndexIndex; + kbts_u32 BeginClusterIp = Scratchpad->Ip; + kbts_u32 BeginClusterFeatureStagesRead = Scratchpad->FeatureStagesRead; + kbts_glyph *TopLevelGlyph = Storage->GlyphSentinel.Next; + + { // @Cleanup @Speed + // We have been bucketing all glyphs into cluster lookups. That's no good! + // For now, we zero all the buckets here and bucket the glyphs again, one cluster at a time. + kbts_un SequentialLookupCount = kbts__SequentialLookupCount(Scratchpad->Config); + KBTS__FOR(LookupIndex, BeginClusterSequentialLookupIndexIndex, SequentialLookupCount) + { + kbts__FreeGlyphBucket(Scratchpad, LookupIndex); + } + } + + Scratchpad->ClusterAtStartOfWord = 1; + + while(kbts__GlyphIsValid(Storage, TopLevelGlyph)) + { + kbts_b32 WordBreak = 0; + + Scratchpad->Ip = BeginClusterIp; + Scratchpad->FeatureStagesRead = BeginClusterFeatureStagesRead; + Scratchpad->SequentialLookupIndexIndex = BeginClusterSequentialLookupIndexIndex; + + { + // We need to store this in case TopLevelGlyph is reordered. + kbts_glyph *OneBeforeFirstClusterGlyph = TopLevelGlyph->Prev; + kbts_glyph *OnePastLastClusterGlyph = kbts__BeginCluster(Scratchpad, Storage, TopLevelGlyph); + + if(Scratchpad->Error) + { + return; + } + + WordBreak = !(OnePastLastClusterGlyph->Prev->UnicodeFlags & KBTS_UNICODE_FLAG_PART_OF_WORD); + Scratchpad->Cluster = kbts__PushGlyphList(Storage, OneBeforeFirstClusterGlyph->Next, OnePastLastClusterGlyph->Prev); + } + + // Bucket cluster glyphs for the first cluster op (or later). + KBTS__FOR_GLYPH(Storage, Glyph) + { + if(!kbts__BucketGlyph(Scratchpad, Glyph, BeginClusterSequentialLookupIndexIndex, 0)) + { + kbts__UnbucketGlyph(Scratchpad, Glyph); + } + } + + while(kbts__ReadOp(Scratchpad, KBTS__OP_KIND_END_CLUSTER)) + { + kbts__ExecuteOp(Scratchpad, Storage); + + if(Scratchpad->Error) + { + return; + } + } + + kbts__EndCluster(Scratchpad, Storage); + + while(kbts__ReadOp(Scratchpad, KBTS__OP_KIND_END_SYLLABLE)) + { + kbts__ExecuteOp(Scratchpad, Storage); + + if(Scratchpad->Error) + { + return; + } + } + + // Reattach the cluster to the main list. + Storage->GlyphSentinel.Next->Prev = Scratchpad->Cluster.OneBeforeFirst; + Storage->GlyphSentinel.Prev->Next = Scratchpad->Cluster.OnePastLast; + TopLevelGlyph = Scratchpad->Cluster.OnePastLast; + + kbts__PopGlyphList(Storage, &Scratchpad->Cluster); + + Scratchpad->ClusterAtStartOfWord = WordBreak; + } + + // Post-clustering ops work across clusters. + // This is where Indic GPOS + post-passes happen. + while(kbts__ReadOp(Scratchpad, 0)) + { + kbts__ExecuteOp(Scratchpad, Storage); + + if(Scratchpad->Error) + { + return; + } + } + } } KBTS_INSTRUMENT_FUNCTION_END; } -KBTS_EXPORT kbts_shape_error kbts_ShapeDirect(kbts_shape_config *Config, kbts_glyph_storage *Storage, kbts_direction RunDirection, kbts_allocator_function *Allocator, void *AllocatorData, kbts_glyph_iterator *Output) +KBTS_EXPORT void kbts_DestroyShapeScratchpad(kbts_shape_scratchpad *Scratchpad) { - if(!Allocator) + if(Scratchpad) { - Allocator = kbts__DefaultAllocator; + kbts_allocator_function *Allocator = Scratchpad->Allocator; + void *AllocatorData = Scratchpad->AllocatorData; + + // We cannot just free the blocks inline, because freeing a start-of-allocation block will + // invalidate a bunch of other, unrelated blocks. + // We first store all of the start-of-allocation blocks in this list, and we then free them all at the end. + kbts__bucketed_glyph_block_header StartOfAllocationSentinel; + KBTS__DLLIST_SENTINEL_INIT(&StartOfAllocationSentinel); + + kbts_un SequentialLookupCount = Scratchpad->SequentialLookupCount; + KBTS__FOR(LookupIndex, 0, SequentialLookupCount) + { + kbts__bucketed_glyph_block_header *Sentinel = &Scratchpad->LookupGlyphBuckets[LookupIndex]; + for(kbts__bucketed_glyph_block_header *Header = Sentinel->Next; + Header != Sentinel; + ) + { + kbts__bucketed_glyph_block_header *Next = Header->Next; + + kbts__bucketed_glyph_block *Block = (kbts__bucketed_glyph_block *)Header; + if(Block->StartOfAllocation) + { + KBTS__DLLIST_REMOVE(&Block->Header); + KBTS__DLLIST_INSERT_BEFORE(&Block->Header, &StartOfAllocationSentinel); + } + + Header = Next; + } + } + + for(kbts__bucketed_glyph_block_header *Header = Scratchpad->FreeBucketedBlockSentinel.Next; + Header != &Scratchpad->FreeBucketedBlockSentinel; + ) + { + kbts__bucketed_glyph_block_header *Next = Header->Next; + + kbts__bucketed_glyph_block *Block = (kbts__bucketed_glyph_block *)Header; + if(Block->StartOfAllocation) + { + KBTS__DLLIST_REMOVE(&Block->Header); + KBTS__DLLIST_INSERT_BEFORE(&Block->Header, &StartOfAllocationSentinel); + } + + Header = Next; + } + + for(kbts__bucketed_glyph_block_header *Header = StartOfAllocationSentinel.Next; + Header != &StartOfAllocationSentinel; + ) + { + kbts__bucketed_glyph_block_header *Next = Header->Next; + + kbts__AllocatorFree(Allocator, AllocatorData, Header); + + Header = Next; + } + + if(Scratchpad->SelfAllocated) + { + kbts__AllocatorFree(Allocator, AllocatorData, Scratchpad); + } } +} - kbts_arena Arena = KBTS__ZERO; - Arena.Allocator = Allocator; - Arena.AllocatorData = AllocatorData; - - kbts__shape_scratchpad *Scratchpad = kbts__PushType(&Arena, kbts__shape_scratchpad); - KBTS_MEMSET(Scratchpad, 0, sizeof(*Scratchpad)); - Scratchpad->Arena = &Arena; - Scratchpad->RunDirection = RunDirection; - - kbts__ShapeDirect(Scratchpad, Config, Storage); +KBTS_EXPORT kbts_shape_error kbts_ShapeDirect(kbts_shape_scratchpad *Scratchpad, kbts_glyph_storage *Storage, kbts_direction RunDirection, kbts_glyph_iterator *Output) +{ + kbts__ShapeDirect(Scratchpad, Storage, RunDirection); kbts_shape_error Result = Scratchpad->Error; if(!Result) @@ -26109,25 +25004,6 @@ KBTS_EXPORT kbts_shape_error kbts_ShapeDirect(kbts_shape_config *Config, kbts_gl *Output = KBTS__ZERO_TYPE(kbts_glyph_iterator); } - kbts__FreeArena(&Arena); - - return Result; -} - -KBTS_EXPORT kbts_shape_error kbts_ShapeDirectFixedMemory(kbts_shape_config *Config, kbts_glyph_storage *Storage, kbts_direction RunDirection, void *Memory, int MemorySize, kbts_glyph_iterator *Output) -{ - kbts_shape_error Result = 0; - - if(Config && Storage && !Storage->Error && Memory && (MemorySize > 0)) - { - kbts_arena Arena; - - if(kbts__InitializeFixedMemoryArena(&Arena, Memory, (kbts_un)MemorySize)) - { - Result = kbts_ShapeDirect(Config, Storage, RunDirection, kbts__ArenaAllocator, &Arena, Output); - } - } - return Result; } @@ -26149,8 +25025,11 @@ KBTS_EXPORT void kbts_ShapeEnd(kbts_shape_context *Context) if(!Context->Error) { // We check the break flags of the one-past-last codepoint, so reset it here. - kbts_shape_codepoint *OnePastLastCodepoint = kbts__InputCodepoint(Context, Context->InputCodepointCount); - *OnePastLastCodepoint = KBTS__ZERO_TYPE(kbts_shape_codepoint); + kbts_shape_codepoint *OnePastLastCodepoint = kbts__InputCodepoint(Context, Context->InputCodepointCount, 1); + if(OnePastLastCodepoint) + { + KBTS_MEMSET(OnePastLastCodepoint, 0, sizeof(*OnePastLastCodepoint)); + } kbts_BreakEnd(&Context->BreakState); kbts__UpdateBreaks(Context); @@ -26162,6 +25041,119 @@ KBTS_EXPORT void kbts_ShapeEnd(kbts_shape_context *Context) } } +static kbts_shape_config *kbts__FindOrCreateShapeConfig(kbts_shape_context *Context, kbts_font *Font, kbts_script Script, kbts_language Language) +{ + kbts_shape_config *Result = 0; + + for(kbts__existing_shape_config_block *ExistingBlock = (kbts__existing_shape_config_block *)Context->ExistingShapeConfigBlockSentinel.Next; + kbts__ExistingShapeConfigBlockIsValid(Context, ExistingBlock); + ExistingBlock = (kbts__existing_shape_config_block *)ExistingBlock->Header.Next) + { + KBTS__FOR(ExistingIndex, 0, ExistingBlock->Count) + { + kbts__existing_shape_config *Existing = &ExistingBlock->Items[ExistingIndex]; + + if((Existing->Font == Font) && + (Existing->Script == Script)) + { + Result = Existing->Config; + + break; + } + } + } + + if(!Result) + { + kbts__existing_shape_config_block *Last = (kbts__existing_shape_config_block *)Context->ExistingShapeConfigBlockSentinel.Prev; + if(!kbts__ExistingShapeConfigBlockIsValid(Context, Last) || + (Last->Count == KBTS__EXISTING_SHAPE_CONFIGS_PER_BLOCK)) + { + kbts__existing_shape_config_block *NewBlock = kbts__PushType(&Context->ConfigArena, kbts__existing_shape_config_block); + if(!NewBlock) + { + Context->Error = KBTS_SHAPE_ERROR_OUT_OF_MEMORY; + + return 0; + } + + KBTS__DLLIST_INSERT_BEFORE(&NewBlock->Header, &Context->ExistingShapeConfigBlockSentinel); + NewBlock->Count = 0; + + Last = NewBlock; + } + + Result = kbts_CreateShapeConfig(Font, Script, Language, kbts__ArenaAllocator, &Context->ConfigArena); + + KBTS_ASSERT(Last->Count < KBTS__EXISTING_SHAPE_CONFIGS_PER_BLOCK); + kbts__existing_shape_config *NewExisting = &Last->Items[Last->Count++]; + NewExisting->Config = Result; + NewExisting->Font = Font; + NewExisting->Script = Script; + } + + return Result; +} + +static kbts_glyph_config *kbts__FindOrCreateGlyphConfig(kbts_shape_context *Context, kbts_shape_config *ShapeConfig, kbts_feature_override *FeatureOverrides, int FeatureOverrideCount) +{ + kbts_glyph_config *Result = 0; + + if(FeatureOverrideCount) + { + for(kbts__existing_glyph_config_block *ExistingBlock = (kbts__existing_glyph_config_block *)Context->ExistingGlyphConfigBlockSentinel.Next; + kbts__ExistingGlyphConfigBlockIsValid(Context, ExistingBlock); + ExistingBlock = (kbts__existing_glyph_config_block *)ExistingBlock->Header.Next) + { + KBTS__FOR(ExistingIndex, 0, ExistingBlock->Count) + { + kbts__existing_glyph_config *Existing = &ExistingBlock->Items[ExistingIndex]; + + if((Existing->ShapeConfig == ShapeConfig) && + (Existing->FeatureOverrides == FeatureOverrides) && + (Existing->FeatureOverrideCount == FeatureOverrideCount)) + { + Result = Existing->GlyphConfig; + + break; + } + } + } + + if(!Result) + { + kbts__existing_glyph_config_block *Last = (kbts__existing_glyph_config_block *)Context->ExistingGlyphConfigBlockSentinel.Prev; + if(!kbts__ExistingGlyphConfigBlockIsValid(Context, Last) || + (Last->Count == KBTS__EXISTING_GLYPH_CONFIGS_PER_BLOCK)) + { + kbts__existing_glyph_config_block *NewBlock = kbts__PushType(&Context->ConfigArena, kbts__existing_glyph_config_block); + if(!NewBlock) + { + Context->Error = KBTS_SHAPE_ERROR_OUT_OF_MEMORY; + + return 0; + } + + KBTS__DLLIST_INSERT_BEFORE(&NewBlock->Header, &Context->ExistingGlyphConfigBlockSentinel); + NewBlock->Count = 0; + + Last = NewBlock; + } + + Result = kbts_CreateGlyphConfig(ShapeConfig, FeatureOverrides, FeatureOverrideCount, kbts__ArenaAllocator, &Context->ConfigArena); + + KBTS_ASSERT(Last->Count < KBTS__EXISTING_GLYPH_CONFIGS_PER_BLOCK); + kbts__existing_glyph_config *Existing = &Last->Items[Last->Count++]; + Existing->ShapeConfig = ShapeConfig; + Existing->FeatureOverrides = FeatureOverrides; + Existing->FeatureOverrideCount = FeatureOverrideCount; + Existing->GlyphConfig = Result; + } + } + + return Result; +} + KBTS_EXPORT int kbts_ShapeRun(kbts_shape_context *Context, kbts_run *Run) { int Result = 0; @@ -26174,6 +25166,7 @@ KBTS_EXPORT int kbts_ShapeRun(kbts_shape_context *Context, kbts_run *Run) kbts_direction RunParagraphDirection = Context->RunParagraphDirection; kbts_direction RunDirection = Context->RunDirection; kbts_language Language = Context->Language; + kbts_shape_config *ShapeConfig = 0; Run->Flags = 0; @@ -26261,13 +25254,13 @@ KBTS_EXPORT int kbts_ShapeRun(kbts_shape_context *Context, kbts_run *Run) if(!It->CodepointIndex) { It->BlockIndex -= 1; - It->CodepointIndex = (1 << (It->BlockIndex + KBTS__INPUT_CODEPOINT_FIRST_BLOCK_MSB)) - 1; + It->CodepointIndex = (1u << (It->BlockIndex + KBTS__INPUT_CODEPOINT_FIRST_BLOCK_MSB)) - 1; } else { It->CodepointIndex -= 1; } - + It->FlatCodepointIndex -= 1; goto FoundBreak; @@ -26280,14 +25273,25 @@ KBTS_EXPORT int kbts_ShapeRun(kbts_shape_context *Context, kbts_run *Run) RunDirection = Context->RunDirection; RunParagraphDirection = Context->RunParagraphDirection; - kbts_PushGlyph(&Context->GlyphStorage, RunFont, InputCodepoint->Codepoint, InputCodepoint->Config, InputCodepointIndex); + // Initialize the shape_config now, before pushing glyphs. + ShapeConfig = kbts__FindOrCreateShapeConfig(Context, RunFont, RunScript, Language); + + kbts_glyph_config *GlyphConfig = kbts__FindOrCreateGlyphConfig(Context, ShapeConfig, InputCodepoint->FeatureOverrides, InputCodepoint->FeatureOverrideCount); + kbts_PushGlyph(&Context->GlyphStorage, RunFont, InputCodepoint->Codepoint, GlyphConfig, InputCodepointIndex); Initialized = 1; } } else { - kbts_PushGlyph(&Context->GlyphStorage, RunFont, InputCodepoint->Codepoint, InputCodepoint->Config, InputCodepointIndex); + // This is an exceptional case, but it can happen when we detect no script. + if(!ShapeConfig) + { + ShapeConfig = kbts__FindOrCreateShapeConfig(Context, RunFont, RunScript, Language); + } + + kbts_glyph_config *GlyphConfig = kbts__FindOrCreateGlyphConfig(Context, ShapeConfig, InputCodepoint->FeatureOverrides, InputCodepoint->FeatureOverrideCount); + kbts_PushGlyph(&Context->GlyphStorage, RunFont, InputCodepoint->Codepoint, GlyphConfig, InputCodepointIndex); } } @@ -26295,51 +25299,22 @@ KBTS_EXPORT int kbts_ShapeRun(kbts_shape_context *Context, kbts_run *Run) if(Result) { - kbts_shape_config *Config = 0; - KBTS__FOR(ExistingConfigIndex, 0, Context->ExistingShapeConfigCount) - { - kbts__existing_shape_config *Existing = &Context->ExistingShapeConfigs[ExistingConfigIndex]; - - if((Existing->Font == RunFont) && - (Existing->Script == RunScript)) - { - Config = Existing->Config; - - break; - } - } - - if(!Config) - { - Config = kbts_CreateShapeConfig(RunFont, RunScript, Language, kbts__ArenaAllocator, &Context->ScratchArena); - - if(Context->ExistingShapeConfigCount < KBTS__ARRAY_LENGTH(Context->ExistingShapeConfigs)) - { - kbts__existing_shape_config *NewExisting = &Context->ExistingShapeConfigs[Context->ExistingShapeConfigCount++]; - - NewExisting->Config = Config; - NewExisting->Font = RunFont; - NewExisting->Script = RunScript; - } - } - if(!RunDirection) { RunDirection = KBTS_DIRECTION_LTR; - if((Config->Shaper == KBTS_SHAPER_ARABIC) || - (Config->Shaper == KBTS_SHAPER_HEBREW)) + if((ShapeConfig->Shaper == KBTS_SHAPER_ARABIC) || + (ShapeConfig->Shaper == KBTS_SHAPER_HEBREW)) { RunDirection = KBTS_DIRECTION_RTL; } } - kbts__shape_scratchpad *Scratchpad = kbts__PushType(&Context->ScratchArena, kbts__shape_scratchpad); - KBTS_MEMSET(Scratchpad, 0, sizeof(*Scratchpad)); - Scratchpad->Arena = &Context->ScratchArena; - Scratchpad->RunDirection = RunDirection; + // @Memory: Store this alongside the shape_config! + kbts_un ScratchpadSize = kbts_SizeOfShapeScratchpad(ShapeConfig); + kbts_shape_scratchpad *Scratchpad = kbts_PlaceShapeScratchpad(ShapeConfig, kbts__PushSize(&Context->ScratchArena, ScratchpadSize, 8), kbts__ArenaAllocator, &Context->ScratchArena); - kbts__ShapeDirect(Scratchpad, Config, &Context->GlyphStorage); + kbts__ShapeDirect(Scratchpad, &Context->GlyphStorage, RunDirection); if(Scratchpad->Error) { @@ -26349,14 +25324,16 @@ KBTS_EXPORT int kbts_ShapeRun(kbts_shape_context *Context, kbts_run *Run) } else { - kbts_shape_codepoint *OnePastLast = kbts__InputCodepoint(Context, Context->InputCodepointCount); - - if(OnePastLast->BreakFlags & KBTS_BREAK_FLAG_LINE_HARD) + kbts_shape_codepoint *OnePastLast = kbts__InputCodepoint(Context, Context->InputCodepointCount, 1); + if(OnePastLast) { - // Signal the terminating line break with a 0-sized run. - Run->Flags = OnePastLast->BreakFlags; + if(OnePastLast->BreakFlags & KBTS_BREAK_FLAG_LINE_HARD) + { + // Signal the terminating line break with a 0-sized run. + Run->Flags = OnePastLast->BreakFlags; - Result = 1; + Result = 1; + } } Context->DoneShapingRuns = 1; @@ -26440,16 +25417,16 @@ static kbts__cmap_subtable_pointer kbts__SelectCmapSubtable(kbts_blob_header *He kbts__cmap_subtable_pointer Subtable = kbts__GetCmapSubtable(Cmap, It); if((char *)(Subtable.Subtable + 1) <= TableEnd) { - kbts_u16 Format = *Subtable.Subtable; + kbts_u16 Format = kbts__ReadU16Unaligned(Subtable.Subtable); // This is kind of iffy, but the statelessness is useful for selecting // the cmap from an already-prepared blob without having to deal with // the byteswap context. - if((Format > 0xFF) && + if((Format > 0xFF) && ((Format >> 8) <= 14)) { Format = kbts__ByteSwap16(Format); - *Subtable.Subtable = Format; + kbts__WriteU16Unaligned(Subtable.Subtable, Format); } if(Format == 14) @@ -26490,8 +25467,6 @@ static kbts__cmap_subtable_pointer kbts__SelectCmapSubtable(kbts_blob_header *He KBTS_EXPORT kbts_load_font_error kbts_LoadFont(kbts_font *Font, kbts_load_font_state *State, void *FontData, int FontDataSize, int FontIndex, int *ScratchSize_, int *OutputSize_) { kbts_load_font_error Result = 0; - kbts_un ScratchSize = 0; - kbts_un OutputSize = 0; if(FontDataSize >= 4) { @@ -26618,30 +25593,48 @@ KBTS_EXPORT kbts_load_font_error kbts_LoadFont(kbts_font *Font, kbts_load_font_s State->GlyphCount = (kbts_u32)GlyphCount; } - ScratchSize = (State->Tables[KBTS_BLOB_TABLE_ID_GSUB].Length + - State->Tables[KBTS_BLOB_TABLE_ID_GPOS].Length + - State->Tables[KBTS_BLOB_TABLE_ID_GDEF].Length) * sizeof(kbts_u32) / 2; + kbts_un ScratchSize = (State->Tables[KBTS_BLOB_TABLE_ID_GSUB].Length + + State->Tables[KBTS_BLOB_TABLE_ID_GPOS].Length + + State->Tables[KBTS_BLOB_TABLE_ID_GDEF].Length) * sizeof(kbts_u32) / 2; - kbts_un GlyphLookupMatrixSizeInBytes = ((((State->LookupCount * State->GlyphCount) + 7) / 8) + 3) & ~3u; - kbts_un GlyphLookupSubtableMatrixSizeInBytes = ((((State->LookupSubtableCount * State->GlyphCount) + 7) / 8) + 3) & ~3u; - OutputSize = sizeof(kbts_blob_header) + - sizeof(kbts_blob_table) * KBTS_BLOB_TABLE_ID_COUNT + - GlyphLookupMatrixSizeInBytes + - GlyphLookupSubtableMatrixSizeInBytes + - sizeof(kbts_u32) * State->LookupCount + - sizeof(kbts_lookup_subtable_info) * State->LookupSubtableCount; + kbts_un GlyphLookupMatrixSizeInWords = 0; + if(State->LookupCount && + State->GlyphCount) + { + kbts__matrix_index LastIndex = kbts__GlyphLookupMatrixIndex(State->LookupCount - 1, State->GlyphCount - 1, State->GlyphCount); + GlyphLookupMatrixSizeInWords = LastIndex.WordIndex + 1; + } + + kbts_un GlyphLookupSubtableMatrixSizeInWords = 0; + if(State->LookupSubtableCount && + State->GlyphCount) + { + kbts__matrix_index LastIndex = kbts__GlyphLookupSubtableMatrixIndex(State->LookupSubtableCount - 1, State->LookupSubtableCount, State->GlyphCount - 1, State->GlyphCount); + GlyphLookupSubtableMatrixSizeInWords = LastIndex.WordIndex + 1; + } + + kbts__pointer_bump_allocator Bump = kbts__PointerBumpAllocator(0); + + kbts__PointerPushType(&Bump, kbts_blob_header); KBTS__FOR(TableId, 0, KBTS_BLOB_TABLE_ID_COUNT) { - OutputSize += State->Tables[TableId].Length; + kbts__PointerPush(&Bump, State->Tables[TableId].Length, 4); } + kbts__PointerPushArray(&Bump, kbts_u32, GlyphLookupMatrixSizeInWords); + kbts__PointerPushArray(&Bump, kbts_u32, GlyphLookupSubtableMatrixSizeInWords); + kbts__PointerPushArray(&Bump, kbts_u32, State->LookupCount); + + // Add the align just to make sure we can accept any pointer. + kbts_un OutputSize = Bump.At + KBTS_ALIGNOF(kbts_blob_header); + *ScratchSize_ = (int)ScratchSize; *OutputSize_ = (int)OutputSize; State->ScratchSize = (kbts_u32)ScratchSize; - State->GlyphLookupMatrixSizeInBytes = (kbts_u32)GlyphLookupMatrixSizeInBytes; - State->GlyphLookupSubtableMatrixSizeInBytes = (kbts_u32)GlyphLookupSubtableMatrixSizeInBytes; + State->GlyphLookupMatrixSizeInBytes = (kbts_u32)(GlyphLookupMatrixSizeInWords * sizeof(kbts_u32)); + State->GlyphLookupSubtableMatrixSizeInBytes = (kbts_u32)(GlyphLookupSubtableMatrixSizeInWords * sizeof(kbts_u32)); State->TotalSize = (kbts_u32)OutputSize; } else if(Magic == KBTS_FOURCC('k', 'b', 't', 's')) @@ -26681,13 +25674,13 @@ static void kbts__MarkMatrixCoverage(kbts_u32 *Matrix, kbts_un TableIndex, kbts_ KBTS__FOR(GlyphIndex, 0, Coverage->Count) { kbts_un GlyphId = GlyphIds[GlyphIndex]; - kbts__matrix_index MatrixIndex = SubtableMatrix ? - kbts__GlyphLookupSubtableMatrixIndex(TableIndex, TableCount, GlyphId) : + kbts__matrix_index MatrixIndex = SubtableMatrix ? + kbts__GlyphLookupSubtableMatrixIndex(TableIndex, TableCount, GlyphId, GlyphCount) : kbts__GlyphLookupMatrixIndex(TableIndex, GlyphId, GlyphCount); if(GlyphId < GlyphCount) { - Matrix[MatrixIndex.WordIndex] |= 1 << MatrixIndex.BitIndex; + Matrix[MatrixIndex.WordIndex] |= 1u << MatrixIndex.BitIndex; } } } @@ -26700,13 +25693,13 @@ static void kbts__MarkMatrixCoverage(kbts_u32 *Matrix, kbts_un TableIndex, kbts_ kbts__range_record *Range = &Ranges[RangeIndex]; KBTS__FOR(GlyphId, Range->StartGlyphId, (kbts_un)Range->EndGlyphId + 1) { - kbts__matrix_index MatrixIndex = SubtableMatrix ? - kbts__GlyphLookupSubtableMatrixIndex(TableIndex, TableCount, GlyphId) : + kbts__matrix_index MatrixIndex = SubtableMatrix ? + kbts__GlyphLookupSubtableMatrixIndex(TableIndex, TableCount, GlyphId, GlyphCount) : kbts__GlyphLookupMatrixIndex(TableIndex, GlyphId, GlyphCount); if(GlyphId < GlyphCount) { - Matrix[MatrixIndex.WordIndex] |= 1 << MatrixIndex.BitIndex; + Matrix[MatrixIndex.WordIndex] |= 1u << MatrixIndex.BitIndex; } } } @@ -26731,8 +25724,8 @@ static void kbts__MarkMatrixClassDef(kbts_u32 *Matrix, kbts_un SubtableIndex, kb if((GlyphId >= (ClassesIncludedLength * 64)) || (ClassesIncluded[GlyphClass / 64] & (1ull << (GlyphClass % 64)))) { - kbts__matrix_index SubtableMatrixIndex = kbts__GlyphLookupSubtableMatrixIndex(SubtableIndex, SubtableCount, GlyphId); - Matrix[SubtableMatrixIndex.WordIndex] |= 1 << SubtableMatrixIndex.BitIndex; + kbts__matrix_index SubtableMatrixIndex = kbts__GlyphLookupSubtableMatrixIndex(SubtableIndex, SubtableCount, GlyphId, GlyphCount); + Matrix[SubtableMatrixIndex.WordIndex] |= 1u << SubtableMatrixIndex.BitIndex; } } } @@ -26757,8 +25750,8 @@ static void kbts__MarkMatrixClassDef(kbts_u32 *Matrix, kbts_un SubtableIndex, kb KBTS__FOR(GlyphId, Range->StartGlyphId, OnePastLastGlyphId) { - kbts__matrix_index SubtableMatrixIndex = kbts__GlyphLookupSubtableMatrixIndex(SubtableIndex, SubtableCount, GlyphId); - Matrix[SubtableMatrixIndex.WordIndex] |= 1 << SubtableMatrixIndex.BitIndex; + kbts__matrix_index SubtableMatrixIndex = kbts__GlyphLookupSubtableMatrixIndex(SubtableIndex, SubtableCount, GlyphId, GlyphCount); + Matrix[SubtableMatrixIndex.WordIndex] |= 1u << SubtableMatrixIndex.BitIndex; } } } @@ -26782,27 +25775,27 @@ KBTS_EXPORT kbts_load_font_error kbts_PlaceBlob(kbts_font *Font, kbts_load_font_ if(Result == KBTS_LOAD_FONT_ERROR_NONE) { - kbts_blob_header *Header = (kbts_blob_header *)OutputMemory; + kbts__pointer_bump_allocator Bump = kbts__PointerBumpAllocator(OutputMemory); + + kbts_blob_header *Header = kbts__PointerPushType(&Bump, kbts_blob_header); *Header = KBTS__ZERO_TYPE(kbts_blob_header); Header->Magic = KBTS_FOURCC('k', 'b', 't', 's'); - Header->Version = 1; + Header->Version = KBTS_BLOB_VERSION_CURRENT; Header->LookupCount = State->LookupCount; Header->LookupSubtableCount = State->LookupSubtableCount; // Stamp packed font data. - char *OutData = KBTS__POINTER_AFTER(char, Header); KBTS__FOR(TableId, 0, KBTS_BLOB_TABLE_ID_COUNT) { kbts_blob_table InTable = State->Tables[TableId]; kbts_blob_table *OutTable = &Header->Tables[TableId]; - OutTable->OffsetFromStartOfFile = KBTS__POINTER_DIFF32(OutData, Header); + char *TableBase = (char *)kbts__PointerPush(&Bump, InTable.Length, 4); + OutTable->OffsetFromStartOfFile = KBTS__POINTER_DIFF32(TableBase, Header); OutTable->Length = InTable.Length; void *InData = KBTS__POINTER_OFFSET(void, State->FontData, InTable.OffsetFromStartOfFile); - KBTS_MEMCPY(OutData, InData, InTable.Length); - - OutData += InTable.Length; + KBTS_MEMCPY(TableBase, InData, InTable.Length); } // Byteswap it. @@ -26862,7 +25855,8 @@ KBTS_EXPORT kbts_load_font_error kbts_PlaceBlob(kbts_font *Font, kbts_load_font_ kbts__cmap_subtable_pointer PreferredSubtable = kbts__SelectCmapSubtable(Header, CmapTable, &Font->Cmap14, &PreferredFormat); if(PreferredSubtable.Subtable) { - switch(*PreferredSubtable.Subtable) + kbts_u16 SubtableFormat = kbts__ReadU16Unaligned(PreferredSubtable.Subtable); + switch(SubtableFormat) { case 0: { @@ -27222,7 +26216,8 @@ KBTS_EXPORT kbts_load_font_error kbts_PlaceBlob(kbts_font *Font, kbts_load_font_ KBTS__FOR(MarkGlyphSetIndex, 0, MarkGlyphSets->MarkGlyphSetCount) { - kbts__coverage *Coverage = KBTS__POINTER_OFFSET(kbts__coverage, MarkGlyphSets, CoverageOffsets[MarkGlyphSetIndex]); + kbts_un CoverageOffset = kbts__ReadU32Unaligned(&CoverageOffsets[MarkGlyphSetIndex]); + kbts__coverage *Coverage = KBTS__POINTER_OFFSET(kbts__coverage, MarkGlyphSets, CoverageOffset); kbts__ByteSwapCoverage(&ByteSwapContext, Coverage); } } @@ -27287,7 +26282,7 @@ KBTS_EXPORT kbts_load_font_error kbts_PlaceBlob(kbts_font *Font, kbts_load_font_ KBTS__FOR(SubstitutionIndex, 0, Lookup.SubtableCount) { kbts_u16 *Base = KBTS__POINTER_OFFSET(kbts_u16, PackedLookup, Lookup.SubtableOffsets[SubstitutionIndex]); - + KBTS_DUMPF(" Subtable %llu:\n", (kbts_un)SubstitutionIndex); kbts__ByteSwapGposLookupSubtable(&ByteSwapContext, LookupList, Lookup.Type, Base); @@ -27306,15 +26301,13 @@ KBTS_EXPORT kbts_load_font_error kbts_PlaceBlob(kbts_font *Font, kbts_load_font_ kbts_un LookupCount = Header->LookupCount; kbts_un SubtableCount = Header->LookupSubtableCount; - kbts_u32 *GlyphLookupMatrix = (kbts_u32 *)OutData; - kbts_u32 *GlyphLookupSubtableMatrix = KBTS__POINTER_OFFSET(kbts_u32, GlyphLookupMatrix, State->GlyphLookupMatrixSizeInBytes); - kbts_u32 *LookupSubtableIndexOffsets = KBTS__POINTER_OFFSET(kbts_u32, GlyphLookupSubtableMatrix, State->GlyphLookupSubtableMatrixSizeInBytes); - kbts_lookup_subtable_info *SubtableInfos = KBTS__POINTER_OFFSET(kbts_lookup_subtable_info, LookupSubtableIndexOffsets, sizeof(kbts_u32) * Header->LookupCount); + kbts_u32 *GlyphLookupMatrix = kbts__PointerPushArray(&Bump, kbts_u32, State->GlyphLookupMatrixSizeInBytes / sizeof(kbts_u32)); + kbts_u32 *GlyphLookupSubtableMatrix = kbts__PointerPushArray(&Bump, kbts_u32, State->GlyphLookupSubtableMatrixSizeInBytes / sizeof(kbts_u32)); + kbts_u32 *LookupSubtableIndexOffsets = kbts__PointerPushArray(&Bump, kbts_u32, State->LookupCount); - KBTS_MEMSET(GlyphLookupMatrix, 0, State->GlyphLookupMatrixSizeInBytes + - State->GlyphLookupSubtableMatrixSizeInBytes + - sizeof(kbts_u32) * Header->LookupCount + - sizeof(kbts_lookup_subtable_info) * Header->LookupSubtableCount); + KBTS_MEMSET(GlyphLookupMatrix, 0, State->GlyphLookupMatrixSizeInBytes); + KBTS_MEMSET(GlyphLookupSubtableMatrix, 0, State->GlyphLookupSubtableMatrixSizeInBytes); + KBTS_MEMSET(LookupSubtableIndexOffsets, 0, sizeof(kbts_u32) * State->LookupCount); kbts_un GposLookupIndexOffset = 0; kbts_un RunningLookupIndex = 0; @@ -27332,11 +26325,6 @@ KBTS_EXPORT kbts_load_font_error kbts_PlaceBlob(kbts_font *Font, kbts_load_font_ int InGpos = (TableId == KBTS_BLOB_TABLE_ID_GPOS); kbts_shaping_table ShapingTableId = (kbts_shaping_table)(InGpos ? KBTS_SHAPING_TABLE_GPOS : KBTS_SHAPING_TABLE_GSUB); - if(InGpos) - { - GposLookupIndexOffset = RunningLookupIndex; - } - if(ShapingTable) { kbts_lookup_list *LookupList = kbts__GetLookupList(ShapingTable); @@ -27349,7 +26337,6 @@ KBTS_EXPORT kbts_load_font_error kbts_PlaceBlob(kbts_font *Font, kbts_load_font_ KBTS__FOR(SubtableIndex, 0, Lookup.SubtableCount) { - kbts_lookup_subtable_info SubtableInfo = KBTS__ZERO; kbts_u16 LookupType = Lookup.Type; kbts_u16 *Base = KBTS__POINTER_OFFSET(kbts_u16, PackedLookup, Lookup.SubtableOffsets[SubtableIndex]); @@ -27386,11 +26373,9 @@ KBTS_EXPORT kbts_load_font_error kbts_PlaceBlob(kbts_font *Font, kbts_load_font_ { kbts_un GlyphId = Ids[IdIndex - 1]; - kbts__matrix_index SubtableMatrixIndex = kbts__GlyphLookupSubtableMatrixIndex(RunningSubtableIndex, SubtableCount, GlyphId); - GlyphLookupSubtableMatrix[SubtableMatrixIndex.WordIndex] |= 1 << SubtableMatrixIndex.BitIndex; + kbts__matrix_index SubtableMatrixIndex = kbts__GlyphLookupSubtableMatrixIndex(RunningSubtableIndex, SubtableCount, GlyphId, GlyphCount); + GlyphLookupSubtableMatrix[SubtableMatrixIndex.WordIndex] |= 1u << SubtableMatrixIndex.BitIndex; } - - SubtableInfo.MinimumFollowupPlusOne = KBTS__MIN(SubtableInfo.MinimumFollowupPlusOne - 1, Ligature->ComponentCount - 1) + 1; } } } @@ -27411,15 +26396,14 @@ KBTS_EXPORT kbts_load_font_error kbts_PlaceBlob(kbts_font *Font, kbts_load_font_ KBTS__FOR(RuleIndex, 0, Set->Count) { kbts__sequence_rule *Rule = kbts__GetSequenceRule(Set, RuleIndex); - SubtableInfo.MinimumFollowupPlusOne = KBTS__MIN(SubtableInfo.MinimumFollowupPlusOne - 1, Rule->GlyphCount - 1) + 1; kbts_u16 *SequenceGlyphIds = KBTS__POINTER_AFTER(kbts_u16, Rule); KBTS__FOR(InputIndex, 1, Rule->GlyphCount) { kbts_un GlyphId = SequenceGlyphIds[InputIndex - 1]; - kbts__matrix_index SubtableMatrixIndex = kbts__GlyphLookupSubtableMatrixIndex(RunningSubtableIndex, SubtableCount, GlyphId); - GlyphLookupSubtableMatrix[SubtableMatrixIndex.WordIndex] |= 1 << SubtableMatrixIndex.BitIndex; + kbts__matrix_index SubtableMatrixIndex = kbts__GlyphLookupSubtableMatrixIndex(RunningSubtableIndex, SubtableCount, GlyphId, GlyphCount); + GlyphLookupSubtableMatrix[SubtableMatrixIndex.WordIndex] |= 1u << SubtableMatrixIndex.BitIndex; } } } @@ -27443,8 +26427,6 @@ KBTS_EXPORT kbts_load_font_error kbts_PlaceBlob(kbts_font *Font, kbts_load_font_ kbts__class_sequence_rule *Rule = kbts__GetClassSequenceRule(Set, RuleIndex); kbts_u16 *SequenceClasses = KBTS__POINTER_AFTER(kbts_u16, Rule); - SubtableInfo.MinimumFollowupPlusOne = KBTS__MIN(SubtableInfo.MinimumFollowupPlusOne - 1, Rule->GlyphCount - 1) + 1; - KBTS__FOR(SequenceIndex, 1, Rule->GlyphCount) { kbts_un Class = SequenceClasses[SequenceIndex - 1]; @@ -27466,8 +26448,6 @@ KBTS_EXPORT kbts_load_font_error kbts_PlaceBlob(kbts_font *Font, kbts_load_font_ kbts__sequence_context_3 *Subst = (kbts__sequence_context_3 *)Base; kbts_u16 *CoverageOffsets = KBTS__POINTER_AFTER(kbts_u16, Subst); - SubtableInfo.MinimumFollowupPlusOne = KBTS__MIN(SubtableInfo.MinimumFollowupPlusOne - 1, Subst->GlyphCount - 1) + 1; - KBTS__FOR(CoverageIndex, 1, Subst->GlyphCount) { kbts__coverage *SubstCoverage = KBTS__POINTER_OFFSET(kbts__coverage, Subst, CoverageOffsets[CoverageIndex]); @@ -27497,28 +26477,25 @@ KBTS_EXPORT kbts_load_font_error kbts_PlaceBlob(kbts_font *Font, kbts_load_font_ kbts__chained_sequence_rule *Rule = kbts__GetChainedClassSequenceRule(Set, RuleIndex); kbts__unpacked_chained_sequence_rule Unpacked = kbts__UnpackChainedSequenceRule(Rule, 0); - SubtableInfo.MinimumBacktrackPlusOne = KBTS__MIN(SubtableInfo.MinimumBacktrackPlusOne - 1, Unpacked.BacktrackCount) + 1; - SubtableInfo.MinimumFollowupPlusOne = KBTS__MIN(SubtableInfo.MinimumFollowupPlusOne - 1, Unpacked.InputCount - 1 + Unpacked.LookaheadCount) + 1; - KBTS__FOR(BacktrackIndex, 0, Unpacked.BacktrackCount) { kbts_un GlyphId = Unpacked.Backtrack[BacktrackIndex]; - kbts__matrix_index SubtableMatrixIndex = kbts__GlyphLookupSubtableMatrixIndex(RunningSubtableIndex, SubtableCount, GlyphId); - GlyphLookupSubtableMatrix[SubtableMatrixIndex.WordIndex] |= 1 << SubtableMatrixIndex.BitIndex; + kbts__matrix_index SubtableMatrixIndex = kbts__GlyphLookupSubtableMatrixIndex(RunningSubtableIndex, SubtableCount, GlyphId, GlyphCount); + GlyphLookupSubtableMatrix[SubtableMatrixIndex.WordIndex] |= 1u << SubtableMatrixIndex.BitIndex; } KBTS__FOR(InputIndex, 1, Unpacked.InputCount) { kbts_un GlyphId = Unpacked.Input[InputIndex - 1]; - kbts__matrix_index SubtableMatrixIndex = kbts__GlyphLookupSubtableMatrixIndex(RunningSubtableIndex, SubtableCount, GlyphId); - GlyphLookupSubtableMatrix[SubtableMatrixIndex.WordIndex] |= 1 << SubtableMatrixIndex.BitIndex; + kbts__matrix_index SubtableMatrixIndex = kbts__GlyphLookupSubtableMatrixIndex(RunningSubtableIndex, SubtableCount, GlyphId, GlyphCount); + GlyphLookupSubtableMatrix[SubtableMatrixIndex.WordIndex] |= 1u << SubtableMatrixIndex.BitIndex; } KBTS__FOR(LookaheadIndex, 0, Unpacked.LookaheadCount) { kbts_un GlyphId = Unpacked.Lookahead[LookaheadIndex]; - kbts__matrix_index SubtableMatrixIndex = kbts__GlyphLookupSubtableMatrixIndex(RunningSubtableIndex, SubtableCount, GlyphId); - GlyphLookupSubtableMatrix[SubtableMatrixIndex.WordIndex] |= 1 << SubtableMatrixIndex.BitIndex; + kbts__matrix_index SubtableMatrixIndex = kbts__GlyphLookupSubtableMatrixIndex(RunningSubtableIndex, SubtableCount, GlyphId, GlyphCount); + GlyphLookupSubtableMatrix[SubtableMatrixIndex.WordIndex] |= 1u << SubtableMatrixIndex.BitIndex; } } } @@ -27560,9 +26537,6 @@ KBTS_EXPORT kbts_load_font_error kbts_PlaceBlob(kbts_font *Font, kbts_load_font_ kbts__chained_sequence_rule *Rule = kbts__GetChainedSequenceRule(Set, RuleIndex); kbts__unpacked_chained_sequence_rule Unpacked = kbts__UnpackChainedSequenceRule(Rule, 0); - SubtableInfo.MinimumBacktrackPlusOne = KBTS__MIN(SubtableInfo.MinimumBacktrackPlusOne - 1, Unpacked.BacktrackCount) + 1; - SubtableInfo.MinimumFollowupPlusOne = KBTS__MIN(SubtableInfo.MinimumFollowupPlusOne - 1, Unpacked.InputCount - 1 + Unpacked.LookaheadCount) + 1; - KBTS__FOR(BacktrackIndex, 0, Unpacked.BacktrackCount) { kbts_un Class = Unpacked.Backtrack[BacktrackIndex]; @@ -27605,9 +26579,6 @@ KBTS_EXPORT kbts_load_font_error kbts_PlaceBlob(kbts_font *Font, kbts_load_font_ Coverage = KBTS__POINTER_OFFSET(kbts__coverage, Subst, Unpacked.InputCoverageOffsets[0]); - SubtableInfo.MinimumBacktrackPlusOne = KBTS__MIN(SubtableInfo.MinimumBacktrackPlusOne - 1, Unpacked.BacktrackCount) + 1; - SubtableInfo.MinimumFollowupPlusOne = KBTS__MIN(SubtableInfo.MinimumFollowupPlusOne - 1, Unpacked.InputCount - 1 + Unpacked.LookaheadCount) + 1; - KBTS__FOR(BacktrackCoverageIndex, 0, Unpacked.BacktrackCount) { kbts__coverage *SubCoverage = KBTS__POINTER_OFFSET(kbts__coverage, Subst, Unpacked.BacktrackCoverageOffsets[BacktrackCoverageIndex]); @@ -27633,8 +26604,6 @@ KBTS_EXPORT kbts_load_font_error kbts_PlaceBlob(kbts_font *Font, kbts_load_font_ { kbts__reverse_chain_substitution *Subst = (kbts__reverse_chain_substitution *)Base; kbts__unpacked_reverse_chain_substitution Unpacked = kbts__UnpackReverseChainSubstitution(Subst, 0); - SubtableInfo.MinimumBacktrackPlusOne = KBTS__MIN(SubtableInfo.MinimumBacktrackPlusOne - 1, Unpacked.BacktrackCount) + 1; - SubtableInfo.MinimumFollowupPlusOne = KBTS__MIN(SubtableInfo.MinimumFollowupPlusOne - 1, Unpacked.LookaheadCount) + 1; KBTS__FOR(BacktrackIndex, 0, Unpacked.BacktrackCount) { @@ -27652,13 +26621,17 @@ KBTS_EXPORT kbts_load_font_error kbts_PlaceBlob(kbts_font *Font, kbts_load_font_ kbts__MarkMatrixCoverage(GlyphLookupMatrix, RunningLookupIndex, LookupCount, GlyphCount, Coverage, 0); kbts__MarkMatrixCoverage(GlyphLookupSubtableMatrix, RunningSubtableIndex, SubtableCount, GlyphCount, Coverage, 1); - SubtableInfos[RunningSubtableIndex] = SubtableInfo; RunningSubtableIndex += 1; } RunningLookupIndex += 1; } } + + if(!InGpos) + { + GposLookupIndexOffset = RunningLookupIndex; + } } } @@ -27666,7 +26639,6 @@ KBTS_EXPORT kbts_load_font_error kbts_PlaceBlob(kbts_font *Font, kbts_load_font_ Header->GlyphLookupMatrixOffsetFromStartOfFile = KBTS__POINTER_DIFF32(GlyphLookupMatrix, Header); Header->GlyphLookupSubtableMatrixOffsetFromStartOfFile = KBTS__POINTER_DIFF32(GlyphLookupSubtableMatrix, Header); Header->LookupSubtableIndexOffsetsOffsetFromStartOfFile = KBTS__POINTER_DIFF32(LookupSubtableIndexOffsets, Header); - Header->SubtableInfosOffsetFromStartOfFile = KBTS__POINTER_DIFF32(SubtableInfos, Header); } } @@ -27679,121 +26651,183 @@ KBTS_EXPORT kbts_load_font_error kbts_PlaceBlob(kbts_font *Font, kbts_load_font_ return Result; } -KBTS_EXPORT void kbts_GetFontInfo(kbts_font *Font, kbts_font_info *Info) +KBTS_EXPORT void kbts_GetFontInfo2(kbts_font *Font, kbts_font_info2 *Info) { - KBTS_MEMSET(Info, 0, sizeof(*Info)); - kbts_blob_header *Blob = Font->Blob; - - if(kbts_FontIsValid(Font) && Blob) + if(Info && Info->Size) { - kbts_blob_table *NameTable = &Blob->Tables[KBTS_BLOB_TABLE_ID_NAME]; + kbts_un InfoSize = Info->Size; + KBTS_MEMSET(Info, 0, InfoSize); + Info->Size = (kbts_u32)InfoSize; - if(NameTable->Length) + kbts_blob_header *Blob = Font->Blob; + + if(Font && kbts_FontIsValid(Font) && Blob) { - kbts__name *Name = KBTS__POINTER_OFFSET(kbts__name, Blob, NameTable->OffsetFromStartOfFile); - kbts__name_record *Records = KBTS__POINTER_AFTER(kbts__name_record, Name); - char *StringBase = KBTS__POINTER_OFFSET(char, Name, Name->StringStorageOffset); + kbts__name *Name = kbts__BlobTableDataType(Blob, KBTS_BLOB_TABLE_ID_NAME, kbts__name); + kbts__os2 *Os2 = kbts__BlobTableDataType(Blob, KBTS_BLOB_TABLE_ID_OS2, kbts__os2); + // @Incomplete: Support vhea, too. + kbts__hea *Hhea = kbts__BlobTableDataType(Blob, KBTS_BLOB_TABLE_ID_HHEA, kbts__hea); + kbts__head *Head = kbts__BlobTableDataType(Blob, KBTS_BLOB_TABLE_ID_HEAD, kbts__head); - KBTS__FOR(RecordIndex, 0, Name->Count) + switch(InfoSize) { - kbts__name_record *Record = &Records[RecordIndex]; + case sizeof(kbts_font_info2_2): + { + kbts_font_info2_2 *Info2_2 = (kbts_font_info2_2 *)Info; - if(!Record->LanguageId) + if(Os2) { - kbts_font_info_string_id Id = KBTS_FONT_INFO_STRING_ID_NONE; - - switch(Record->NameId) - { - case 0: Id = KBTS_FONT_INFO_STRING_ID_COPYRIGHT; break; - case 1: Id = KBTS_FONT_INFO_STRING_ID_FAMILY; break; - case 2: Id = KBTS_FONT_INFO_STRING_ID_SUBFAMILY; break; - case 3: Id = KBTS_FONT_INFO_STRING_ID_UID; break; - case 4: Id = KBTS_FONT_INFO_STRING_ID_FULL_NAME; break; - case 5: Id = KBTS_FONT_INFO_STRING_ID_VERSION; break; - case 6: Id = KBTS_FONT_INFO_STRING_ID_POSTSCRIPT_NAME; break; - case 7: Id = KBTS_FONT_INFO_STRING_ID_TRADEMARK; break; - case 8: Id = KBTS_FONT_INFO_STRING_ID_MANUFACTURER; break; - case 9: Id = KBTS_FONT_INFO_STRING_ID_DESIGNER; break; - case 10: Id = KBTS_FONT_INFO_STRING_ID_TYPOGRAPHIC_FAMILY; break; - case 11: Id = KBTS_FONT_INFO_STRING_ID_TYPOGRAPHIC_SUBFAMILY; break; - } - - if(Id) - { - Info->Strings[Id] = KBTS__POINTER_OFFSET(char, StringBase, Record->StringOffset); - Info->StringLengths[Id] = Record->Length; - } + Info2_2->CapitalHeight = Os2->CapHeight; } } + KBTS__FALLTHROUGH; - if(!Info->Strings[KBTS_FONT_INFO_STRING_ID_TYPOGRAPHIC_FAMILY]) + case sizeof(kbts_font_info2_1): { - Info->Strings[KBTS_FONT_INFO_STRING_ID_TYPOGRAPHIC_FAMILY] = Info->Strings[KBTS_FONT_INFO_STRING_ID_FAMILY]; - Info->StringLengths[KBTS_FONT_INFO_STRING_ID_TYPOGRAPHIC_FAMILY] = Info->StringLengths[KBTS_FONT_INFO_STRING_ID_FAMILY]; - } + kbts_font_info2_1 *Info2_1 = (kbts_font_info2_1 *)Info; - if(!Info->Strings[KBTS_FONT_INFO_STRING_ID_TYPOGRAPHIC_SUBFAMILY]) + if(Os2) + { + Info2_1->Ascent = Os2->TypoAscender; + Info2_1->Descent = Os2->TypoDescender; + Info2_1->LineGap = Os2->TypoLineGap; + } + else if(Hhea) + { + Info2_1->Ascent = Hhea->Ascent; + Info2_1->Descent = Hhea->Descent; + Info2_1->LineGap = Hhea->LineGap; + } + + if(Head) + { + Info2_1->UnitsPerEm = Head->UnitsPerEm; + + Info2_1->XMin = Head->XMin; + Info2_1->YMin = Head->YMin; + Info2_1->XMax = Head->XMax; + Info2_1->YMax = Head->YMax; + } + } + KBTS__FALLTHROUGH; + + case sizeof(kbts_font_info2): { - Info->Strings[KBTS_FONT_INFO_STRING_ID_TYPOGRAPHIC_SUBFAMILY] = Info->Strings[KBTS_FONT_INFO_STRING_ID_SUBFAMILY]; - Info->StringLengths[KBTS_FONT_INFO_STRING_ID_TYPOGRAPHIC_SUBFAMILY] = Info->StringLengths[KBTS_FONT_INFO_STRING_ID_SUBFAMILY]; - } - } + if(Name) + { + kbts__name_record *Records = KBTS__POINTER_AFTER(kbts__name_record, Name); + char *StringBase = KBTS__POINTER_OFFSET(char, Name, Name->StringStorageOffset); - kbts_blob_table *Os2Table = &Blob->Tables[KBTS_BLOB_TABLE_ID_OS2]; + KBTS__FOR(RecordIndex, 0, Name->Count) + { + kbts__name_record *Record = &Records[RecordIndex]; - if(Os2Table->Length) - { - kbts__os2 *Os2 = KBTS__POINTER_OFFSET(kbts__os2, Blob, Os2Table->OffsetFromStartOfFile); - kbts_font_weight Weight = KBTS_FONT_WEIGHT_UNKNOWN; - kbts_font_width Width = KBTS_FONT_WIDTH_UNKNOWN; - kbts_font_style_flags StyleFlags = KBTS_FONT_STYLE_FLAG_NONE; + if(!Record->LanguageId) + { + kbts_font_info_string_id Id = KBTS_FONT_INFO_STRING_ID_NONE; - switch(Os2->WeightClass) - { - case 100: Weight = KBTS_FONT_WEIGHT_THIN; break; - case 200: Weight = KBTS_FONT_WEIGHT_EXTRA_LIGHT; break; - case 300: Weight = KBTS_FONT_WEIGHT_LIGHT; break; - case 400: Weight = KBTS_FONT_WEIGHT_NORMAL; break; - case 500: Weight = KBTS_FONT_WEIGHT_MEDIUM; break; - case 600: Weight = KBTS_FONT_WEIGHT_SEMI_BOLD; break; - case 700: Weight = KBTS_FONT_WEIGHT_BOLD; break; - case 800: Weight = KBTS_FONT_WEIGHT_EXTRA_BOLD; break; - case 900: Weight = KBTS_FONT_WEIGHT_BLACK; break; - } + switch(Record->NameId) + { + case 0: Id = KBTS_FONT_INFO_STRING_ID_COPYRIGHT; break; + case 1: Id = KBTS_FONT_INFO_STRING_ID_FAMILY; break; + case 2: Id = KBTS_FONT_INFO_STRING_ID_SUBFAMILY; break; + case 3: Id = KBTS_FONT_INFO_STRING_ID_UID; break; + case 4: Id = KBTS_FONT_INFO_STRING_ID_FULL_NAME; break; + case 5: Id = KBTS_FONT_INFO_STRING_ID_VERSION; break; + case 6: Id = KBTS_FONT_INFO_STRING_ID_POSTSCRIPT_NAME; break; + case 7: Id = KBTS_FONT_INFO_STRING_ID_TRADEMARK; break; + case 8: Id = KBTS_FONT_INFO_STRING_ID_MANUFACTURER; break; + case 9: Id = KBTS_FONT_INFO_STRING_ID_DESIGNER; break; + case 10: Id = KBTS_FONT_INFO_STRING_ID_TYPOGRAPHIC_FAMILY; break; + case 11: Id = KBTS_FONT_INFO_STRING_ID_TYPOGRAPHIC_SUBFAMILY; break; + } - switch(Os2->WidthClass) - { - case 1: Width = KBTS_FONT_WIDTH_ULTRA_CONDENSED; break; - case 2: Width = KBTS_FONT_WIDTH_EXTRA_CONDENSED; break; - case 3: Width = KBTS_FONT_WIDTH_CONDENSED; break; - case 4: Width = KBTS_FONT_WIDTH_SEMI_CONDENSED; break; - case 5: Width = KBTS_FONT_WIDTH_NORMAL; break; - case 6: Width = KBTS_FONT_WIDTH_SEMI_EXPANDED; break; - case 7: Width = KBTS_FONT_WIDTH_EXPANDED; break; - case 8: Width = KBTS_FONT_WIDTH_EXTRA_EXPANDED; break; - case 9: Width = KBTS_FONT_WIDTH_ULTRA_EXPANDED; break; - } + if(Id) + { + Info->Strings[Id] = KBTS__POINTER_OFFSET(char, StringBase, Record->StringOffset); + Info->StringLengths[Id] = Record->Length; + } + } + } - if(Os2->Selection & (KBTS__OS2_SELECTION_FLAG_ITALIC | KBTS__OS2_SELECTION_FLAG_OBLIQUE)) - { - StyleFlags |= KBTS_FONT_STYLE_FLAG_ITALIC; - } - if(Os2->Selection & KBTS__OS2_SELECTION_FLAG_BOLD) - { - StyleFlags |= KBTS_FONT_STYLE_FLAG_BOLD; - } - if(Os2->Selection & KBTS__OS2_SELECTION_FLAG_REGULAR) - { - StyleFlags |= KBTS_FONT_STYLE_FLAG_REGULAR; - } + if(!Info->Strings[KBTS_FONT_INFO_STRING_ID_TYPOGRAPHIC_FAMILY]) + { + Info->Strings[KBTS_FONT_INFO_STRING_ID_TYPOGRAPHIC_FAMILY] = Info->Strings[KBTS_FONT_INFO_STRING_ID_FAMILY]; + Info->StringLengths[KBTS_FONT_INFO_STRING_ID_TYPOGRAPHIC_FAMILY] = Info->StringLengths[KBTS_FONT_INFO_STRING_ID_FAMILY]; + } - Info->Weight = Weight; - Info->Width = Width; - Info->StyleFlags = StyleFlags; + if(!Info->Strings[KBTS_FONT_INFO_STRING_ID_TYPOGRAPHIC_SUBFAMILY]) + { + Info->Strings[KBTS_FONT_INFO_STRING_ID_TYPOGRAPHIC_SUBFAMILY] = Info->Strings[KBTS_FONT_INFO_STRING_ID_SUBFAMILY]; + Info->StringLengths[KBTS_FONT_INFO_STRING_ID_TYPOGRAPHIC_SUBFAMILY] = Info->StringLengths[KBTS_FONT_INFO_STRING_ID_SUBFAMILY]; + } + } + + if(Os2) + { + kbts_font_weight Weight = KBTS_FONT_WEIGHT_UNKNOWN; + kbts_font_width Width = KBTS_FONT_WIDTH_UNKNOWN; + kbts_font_style_flags StyleFlags = KBTS_FONT_STYLE_FLAG_NONE; + + switch(Os2->WeightClass) + { + case 100: Weight = KBTS_FONT_WEIGHT_THIN; break; + case 200: Weight = KBTS_FONT_WEIGHT_EXTRA_LIGHT; break; + case 300: Weight = KBTS_FONT_WEIGHT_LIGHT; break; + case 400: Weight = KBTS_FONT_WEIGHT_NORMAL; break; + case 500: Weight = KBTS_FONT_WEIGHT_MEDIUM; break; + case 600: Weight = KBTS_FONT_WEIGHT_SEMI_BOLD; break; + case 700: Weight = KBTS_FONT_WEIGHT_BOLD; break; + case 800: Weight = KBTS_FONT_WEIGHT_EXTRA_BOLD; break; + case 900: Weight = KBTS_FONT_WEIGHT_BLACK; break; + } + + switch(Os2->WidthClass) + { + case 1: Width = KBTS_FONT_WIDTH_ULTRA_CONDENSED; break; + case 2: Width = KBTS_FONT_WIDTH_EXTRA_CONDENSED; break; + case 3: Width = KBTS_FONT_WIDTH_CONDENSED; break; + case 4: Width = KBTS_FONT_WIDTH_SEMI_CONDENSED; break; + case 5: Width = KBTS_FONT_WIDTH_NORMAL; break; + case 6: Width = KBTS_FONT_WIDTH_SEMI_EXPANDED; break; + case 7: Width = KBTS_FONT_WIDTH_EXPANDED; break; + case 8: Width = KBTS_FONT_WIDTH_EXTRA_EXPANDED; break; + case 9: Width = KBTS_FONT_WIDTH_ULTRA_EXPANDED; break; + } + + if(Os2->Selection & (KBTS__OS2_SELECTION_FLAG_ITALIC | KBTS__OS2_SELECTION_FLAG_OBLIQUE)) + { + StyleFlags |= KBTS_FONT_STYLE_FLAG_ITALIC; + } + if(Os2->Selection & KBTS__OS2_SELECTION_FLAG_BOLD) + { + StyleFlags |= KBTS_FONT_STYLE_FLAG_BOLD; + } + if(Os2->Selection & KBTS__OS2_SELECTION_FLAG_REGULAR) + { + StyleFlags |= KBTS_FONT_STYLE_FLAG_REGULAR; + } + + Info->Weight = Weight; + Info->Width = Width; + Info->StyleFlags = StyleFlags; + } + } break; + } } } } +KBTS_EXPORT void kbts_GetFontInfo(kbts_font *Font, kbts_font_info *Info) +{ + kbts_font_info2 Info2; + Info2.Size = sizeof(Info2); + + kbts_GetFontInfo2(Font, &Info2); + + KBTS_MEMCPY(Info, Info2.Strings, sizeof(*Info)); +} + KBTS_EXPORT kbts_font kbts_FontFromMemory(void *FileData, int FileSize, int FontIndex, kbts_allocator_function *Allocator, void *AllocatorData) { kbts_font Result = KBTS__ZERO; @@ -27808,7 +26842,7 @@ KBTS_EXPORT kbts_font kbts_FontFromMemory(void *FileData, int FileSize, int Font kbts_load_font_state LoadFontState = KBTS__ZERO; int ScratchSize, OutputSize; kbts_load_font_error Error = kbts_LoadFont(&Result, &LoadFontState, FileData, (int)FileSize, FontIndex, &ScratchSize, &OutputSize); - + if(Error == KBTS_LOAD_FONT_ERROR_NEED_TO_CREATE_BLOB) { void *ScratchMemory = kbts__AllocatorAllocate(Allocator, AllocatorData, (kbts_un)ScratchSize); @@ -28151,7 +27185,7 @@ static void kbts__FlushDirection(kbts_break_state *State, kbts_direction *LastDi *LastDirection = Direction; kbts__DoBreak(State, PositionOffset, KBTS_BREAK_FLAG_DIRECTION, Direction, 0, 0); } - + if((BreakFlags & KBTS_BREAK_FLAG_PARAGRAPH_DIRECTION) && !State->ParagraphDirection) { @@ -28476,19 +27510,16 @@ static void kbts__BreakAddCodepoint(kbts_break_state *State, kbts_u32 Codepoint, (KBTS_UNICODE_BIDIRECTIONAL_CLASS_AN) (KBTS_UNICODE_BIDIRECTIONAL_CLASS_EN)))) { + // From the Unicode Bidirectional Algorithm: + // European and Arabic numbers act as if they were R in terms of their influence on NIs. + // // Note that the way we resolve digits is different from the way the Unicode standard specifies it. // This is because the standard assumes the paragraph direction is always known, whereas in our case it isn't. // We want neutral surrounded by uncoerced digits to resolve to the paragraph direction, which may be DONT_KNOW. Bidirectional1 = KBTS_UNICODE_BIDIRECTIONAL_CLASS_R; } - else if(((Bidirectional2 == KBTS_UNICODE_BIDIRECTIONAL_CLASS_L) || - (BidirectionalClass == KBTS_UNICODE_BIDIRECTIONAL_CLASS_L)) && - KBTS__IN_SET(Bidirectional2, KBTS__SET32((KBTS_UNICODE_BIDIRECTIONAL_CLASS_L) - (KBTS_UNICODE_BIDIRECTIONAL_CLASS_AN) - (KBTS_UNICODE_BIDIRECTIONAL_CLASS_EN))) && - KBTS__IN_SET(BidirectionalClass, KBTS__SET32((KBTS_UNICODE_BIDIRECTIONAL_CLASS_L) - (KBTS_UNICODE_BIDIRECTIONAL_CLASS_AN) - (KBTS_UNICODE_BIDIRECTIONAL_CLASS_EN)))) + else if((Bidirectional2 == KBTS_UNICODE_BIDIRECTIONAL_CLASS_L) && + (BidirectionalClass == KBTS_UNICODE_BIDIRECTIONAL_CLASS_L)) { Bidirectional1 = KBTS_UNICODE_BIDIRECTIONAL_CLASS_L; } @@ -28496,6 +27527,7 @@ static void kbts__BreakAddCodepoint(kbts_break_state *State, kbts_u32 Codepoint, { if (State->ParagraphDirection == KBTS_DIRECTION_LTR) Bidirectional1 = KBTS_UNICODE_BIDIRECTIONAL_CLASS_L; else if(State->ParagraphDirection == KBTS_DIRECTION_RTL) Bidirectional1 = KBTS_UNICODE_BIDIRECTIONAL_CLASS_R; + // Otherwise, don't coerce to anything. } } @@ -28550,7 +27582,7 @@ static void kbts__BreakAddCodepoint(kbts_break_state *State, kbts_u32 Codepoint, // Word breaks. // We buffer 3 characters for word breaks. // Each character gets 3 bits (padded to 4) representing 3 levels of priority. - #define KBTS_WORD_BREAK_BITS(Priority, Position) (((1 << ((Priority) + 1)) - 1) << ((Position) * 4)) + #define KBTS_WORD_BREAK_BITS(Priority, Position) (((1u << ((Priority) + 1)) - 1) << ((Position) * 4)) #define KBTS_C2(A, B) case (KBTS_WORD_BREAK_CLASS_##A << 8) | (KBTS_WORD_BREAK_CLASS_##B) #define KBTS_C3(A, B, C) case (KBTS_WORD_BREAK_CLASS_##A << 16) | (KBTS_WORD_BREAK_CLASS_##B << 8) | (KBTS_WORD_BREAK_CLASS_##C) @@ -28599,7 +27631,7 @@ static void kbts__BreakAddCodepoint(kbts_break_state *State, kbts_u32 Codepoint, // (RI RI)* RI x RI KBTS_C2(RI, RI): WordBreakHistory = 0; - // fallthrough + KBTS__FALLTHROUGH; KBTS_C2(HL, SQ): KBTS_C2(ALnep, ALnep): KBTS_C2(ALnep, ALep): KBTS_C2(ALnep, HL): KBTS_C2(ALnep, NM): KBTS_C2(ALnep, ENL): KBTS_C2(ALep, ALnep): KBTS_C2(ALep, ALep): KBTS_C2(ALep, HL): KBTS_C2(ALep, NM): KBTS_C2(ALep, ENL): @@ -28847,7 +27879,7 @@ static void kbts__BreakAddCodepoint(kbts_break_state *State, kbts_u32 Codepoint, KBTS_C2(QUPf, BK): KBTS_C2(QUPf, CR): KBTS_C2(QUPf, LF): - KBTS_C2(QUPf, NL): + KBTS_C2(QUPf, NL): KBTS_C2(QUPf, ZW): KBTS_C2(QUPf, WJ): KBTS_C2(QUPf, CLnea): @@ -28860,7 +27892,7 @@ static void kbts__BreakAddCodepoint(kbts_break_state *State, kbts_u32 Codepoint, KBTS_LINE_UNBREAK(1, 1); break; - KBTS_C2(QUPf, QUPf): + KBTS_C2(QUPf, QUPf): KBTS_LINE_UNBREAK(3, 2); KBTS_LINE_UNBREAK(1, 1); KBTS_LINE_UNBREAK(1, 0); @@ -29080,7 +28112,7 @@ static void kbts__BreakAddCodepoint(kbts_break_state *State, kbts_u32 Codepoint, KBTS_C3(NU, CPnea, POea): KBTS_C3(NU, CPnea, POnea): KBTS_C3(NU, CPnea, PRea): KBTS_C3(NU, CPnea, PRnea): KBTS_C3(AK, VI, AK): KBTS_C3(AK, VI, DOTTED_CIRCLE): KBTS_C3(DOTTED_CIRCLE, VI, AK): KBTS_C3(DOTTED_CIRCLE, VI, DOTTED_CIRCLE): KBTS_C3(AS, VI, AK): KBTS_C3(AS, VI, DOTTED_CIRCLE): KBTS_LINE_UNBREAK(0, 1); break; - + KBTS_C3(POea, OPea, NU): KBTS_C3(POea, OPnea, NU): KBTS_C3(POnea, OPea, NU): KBTS_C3(POnea, OPnea, NU): KBTS_C3(PRea, OPea, NU): KBTS_C3(PRea, OPnea, NU): KBTS_C3(PRnea, OPea, NU): KBTS_C3(PRnea, OPnea, NU): KBTS_LINE_UNBREAK(0, 2); break; @@ -29527,11 +28559,11 @@ KBTS_EXPORT void kbts_BreakEntireString(kbts_direction Direction, kbts_japanese_ } } - if(!Breaks && BreakCount) + if(BreakCount) { *BreakCount = (int)BreaksWritten; } - if(!BreakFlags && BreakFlagCount) + if(BreakFlagCount) { *BreakFlagCount = (int)(MaxBreakPosition + 1); } @@ -29644,23 +28676,23 @@ KBTS_EXPORT kbts_encode_utf8 kbts_EncodeUtf8(int Codepoint) } else if(Codepoint <= 0x7FF) { - Result.Encoded[1] = (Codepoint & 0x3F) | 0x80; - Result.Encoded[0] = ((Codepoint >> 6) & 0x1F) | 0xC0; + Result.Encoded[1] = (char)((Codepoint & 0x3F) | 0x80); + Result.Encoded[0] = (char)(((Codepoint >> 6) & 0x1F) | 0xC0); Result.EncodedLength = 2; } else if(Codepoint <= 0xFFFF) { - Result.Encoded[2] = (Codepoint & 0x3F) | 0x80; - Result.Encoded[1] = ((Codepoint >> 6) & 0x3F) | 0x80; - Result.Encoded[0] = ((Codepoint >> 12) & 0xF) | 0xE0; + Result.Encoded[2] = (char)((Codepoint & 0x3F) | 0x80); + Result.Encoded[1] = (char)(((Codepoint >> 6) & 0x3F) | 0x80); + Result.Encoded[0] = (char)(((Codepoint >> 12) & 0xF) | 0xE0); Result.EncodedLength = 3; } else if(Codepoint <= 0x10FFFF) { - Result.Encoded[3] = (Codepoint & 0x3F) | 0x80; - Result.Encoded[2] = ((Codepoint >> 6) & 0x3F) | 0x80; - Result.Encoded[1] = ((Codepoint >> 12) & 0x3F) | 0x80; - Result.Encoded[0] = ((Codepoint >> 18) & 0x7) | 0xF0; + Result.Encoded[3] = (char)((Codepoint & 0x3F) | 0x80); + Result.Encoded[2] = (char)(((Codepoint >> 6) & 0x3F) | 0x80); + Result.Encoded[1] = (char)(((Codepoint >> 12) & 0x3F) | 0x80); + Result.Encoded[0] = (char)(((Codepoint >> 18) & 0x7) | 0xF0); Result.EncodedLength = 4; } @@ -29688,4 +28720,4 @@ KBTS_EXPORT int kbts_ScriptIsComplex(kbts_script Script) } #endif -#undef KBTS_X_FEATURES \ No newline at end of file +#undef KBTS_X_FEATURES diff --git a/vendor/raylib/LICENSE b/vendor/raylib/LICENSE index 91da62ed9..bc6f4b851 100644 --- a/vendor/raylib/LICENSE +++ b/vendor/raylib/LICENSE @@ -1,4 +1,4 @@ -Copyright (c) 2013-2023 Ramon Santamaria (@raysan5) +Copyright (c) 2013-2026 Ramon Santamaria (@raysan5) This software is provided "as-is", without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software. diff --git a/vendor/raylib/README.md b/vendor/raylib/README.md index 666c41643..1f9740f6d 100644 --- a/vendor/raylib/README.md +++ b/vendor/raylib/README.md @@ -1,8 +1,8 @@ - + **raylib is a simple and easy-to-use library to enjoy videogames programming.** -raylib is highly inspired by Borland BGI graphics lib and by XNA framework and it's specially well suited for prototyping, tooling, graphical applications, embedded systems and education. +raylib is highly inspired by Borland BGI graphics lib and by XNA framework and it's especially well suited for prototyping, tooling, graphical applications, embedded systems and education. *NOTE for ADVENTURERS: raylib is a programming library to enjoy videogames programming; no fancy interface, no visual helpers, no debug button... just coding in the most pure spartan-programmers way.* @@ -14,45 +14,45 @@ Ready to learn? Jump to [code examples!](https://www.raylib.com/examples.html) [![GitHub Releases Downloads](https://img.shields.io/github/downloads/raysan5/raylib/total)](https://github.com/raysan5/raylib/releases) [![GitHub Stars](https://img.shields.io/github/stars/raysan5/raylib?style=flat&label=stars)](https://github.com/raysan5/raylib/stargazers) -[![GitHub commits since tagged version](https://img.shields.io/github/commits-since/raysan5/raylib/4.2.0)](https://github.com/raysan5/raylib/commits/master) +[![GitHub commits since tagged version](https://img.shields.io/github/commits-since/raysan5/raylib/5.5)](https://github.com/raysan5/raylib/commits/master) [![GitHub Sponsors](https://img.shields.io/github/sponsors/raysan5?label=sponsors)](https://github.com/sponsors/raysan5) [![Packaging Status](https://repology.org/badge/tiny-repos/raylib.svg)](https://repology.org/project/raylib/versions) [![License](https://img.shields.io/badge/license-zlib%2Flibpng-blue.svg)](LICENSE) [![Discord Members](https://img.shields.io/discord/426912293134270465.svg?label=Discord&logo=discord)](https://discord.gg/raylib) -[![Subreddit Subscribers](https://img.shields.io/reddit/subreddit-subscribers/raylib?label=reddit%20r%2Fraylib&logo=reddit)](https://www.reddit.com/r/raylib/) +[![Reddit Static Badge](https://img.shields.io/badge/-r%2Fraylib-red?style=flat&logo=reddit&label=reddit)](https://www.reddit.com/r/raylib/) [![Youtube Subscribers](https://img.shields.io/youtube/channel/subscribers/UC8WIBkhYb5sBNqXO1mZ7WSQ?style=flat&label=Youtube&logo=youtube)](https://www.youtube.com/c/raylib) [![Twitch Status](https://img.shields.io/twitch/status/raysan5?style=flat&label=Twitch&logo=twitch)](https://www.twitch.tv/raysan5) -[![Windows](https://github.com/raysan5/raylib/workflows/Windows/badge.svg)](https://github.com/raysan5/raylib/actions?query=workflow%3AWindows) -[![Linux](https://github.com/raysan5/raylib/workflows/Linux/badge.svg)](https://github.com/raysan5/raylib/actions?query=workflow%3ALinux) -[![macOS](https://github.com/raysan5/raylib/workflows/macOS/badge.svg)](https://github.com/raysan5/raylib/actions?query=workflow%3AmacOS) -[![Android](https://github.com/raysan5/raylib/workflows/Android/badge.svg)](https://github.com/raysan5/raylib/actions?query=workflow%3AAndroid) -[![WebAssembly](https://github.com/raysan5/raylib/workflows/WebAssembly/badge.svg)](https://github.com/raysan5/raylib/actions?query=workflow%3AWebAssembly) +[![Build Windows](https://github.com/raysan5/raylib/actions/workflows/build_windows.yml/badge.svg)](https://github.com/raysan5/raylib/actions/workflows/build_windows.yml) +[![Build Linux](https://github.com/raysan5/raylib/actions/workflows/build_linux.yml/badge.svg)](https://github.com/raysan5/raylib/actions/workflows/build_linux.yml) +[![Build macOS](https://github.com/raysan5/raylib/actions/workflows/build_macos.yml/badge.svg)](https://github.com/raysan5/raylib/actions/workflows/build_macos.yml) +[![Build WebAssembly](https://github.com/raysan5/raylib/actions/workflows/build_webassembly.yml/badge.svg)](https://github.com/raysan5/raylib/actions/workflows/build_webassembly.yml) -[![CMakeBuilds](https://github.com/raysan5/raylib/workflows/CMakeBuilds/badge.svg)](https://github.com/raysan5/raylib/actions?query=workflow%3ACMakeBuilds) -[![Windows Examples](https://github.com/raysan5/raylib/actions/workflows/windows_examples.yml/badge.svg)](https://github.com/raysan5/raylib/actions/workflows/windows_examples.yml) -[![Linux Examples](https://github.com/raysan5/raylib/actions/workflows/linux_examples.yml/badge.svg)](https://github.com/raysan5/raylib/actions/workflows/linux_examples.yml) +[![Build CMake](https://github.com/raysan5/raylib/actions/workflows/build_cmake.yml/badge.svg)](https://github.com/raysan5/raylib/actions/workflows/build_cmake.yml) +[![Build examples Windows](https://github.com/raysan5/raylib/actions/workflows/build_examples_windows.yml/badge.svg)](https://github.com/raysan5/raylib/actions/workflows/build_examples_windows.yml) +[![Build examples Linux](https://github.com/raysan5/raylib/actions/workflows/build_examples_linux.yml/badge.svg)](https://github.com/raysan5/raylib/actions/workflows/build_examples_linux.yml) features -------- - - **NO external dependencies**, all required libraries are [bundled into raylib](https://github.com/raysan5/raylib/tree/master/src/external) + - **NO external dependencies**, all required libraries are [included into raylib](https://github.com/raysan5/raylib/tree/master/src/external) - Multiple platforms supported: **Windows, Linux, MacOS, RPI, Android, HTML5... and more!** - Written in plain C code (C99) using PascalCase/camelCase notation - - Hardware accelerated with OpenGL (**1.1, 2.1, 3.3, 4.3, ES 2.0 or ES 3.0**) + - Hardware accelerated with OpenGL: **1.1, 2.1, 3.3, 4.3, ES 2.0, ES 3.0** - **Unique OpenGL abstraction layer** (usable as standalone module): [rlgl](https://github.com/raysan5/raylib/blob/master/src/rlgl.h) - - Multiple **Fonts** formats supported (TTF, Image fonts, AngelCode fonts) + - **Software Renderer** backend (no OpenGL required!): [rlsw](https://github.com/raysan5/raylib/blob/master/src/external/rlsw.h) + - Multiple **Fonts** formats supported (TTF, OTF, FNT, BDF, sprite fonts) - Multiple texture formats supported, including **compressed formats** (DXT, ETC, ASTC) - - **Full 3D support**, including 3D Shapes, Models, Billboards, Heightmaps and more! + - **Full 3D support**, including 3D Shapes, Models, Billboards, Heightmaps and more! - Flexible Materials system, supporting classic maps and **PBR maps** - - **Animated 3D models** supported (skeletal bones animation) (IQM) - - Shaders support, including model and **postprocessing** shaders. + - **Animated 3D models** supported (skeletal bones animation) (IQM, M3D, glTF) + - Shaders support, including model shaders and **postprocessing** shaders - **Powerful math module** for Vector, Matrix and Quaternion operations: [raymath](https://github.com/raysan5/raylib/blob/master/src/raymath.h) - - Audio loading and playing with streaming support (WAV, OGG, MP3, FLAC, XM, MOD) + - Audio loading and playing with streaming support (WAV, QOA, OGG, MP3, FLAC, XM, MOD) - **VR stereo rendering** support with configurable HMD device parameters - - Huge examples collection with [+120 code examples](https://github.com/raysan5/raylib/tree/master/examples)! - - Bindings to [+60 programming languages](https://github.com/raysan5/raylib/blob/master/BINDINGS.md)! - - **Free and open source**. + - Huge examples collection with [+140 code examples](https://github.com/raysan5/raylib/tree/master/examples)! + - Bindings to [+70 programming languages](https://github.com/raysan5/raylib/blob/master/BINDINGS.md)! + - **Free and open source** basic example -------------- @@ -70,19 +70,17 @@ main :: proc() { rl.ClearBackground(rl.RAYWHITE) rl.DrawText("Congrats! You created your first window!", 190, 200, 20, rl.LIGHTGRAY) rl.EndDrawing() - } - + } rl.CloseWindow() } ``` - build and installation ---------------------- raylib binary releases for Windows, Linux, macOS, Android and HTML5 are available at the [Github Releases page](https://github.com/raysan5/raylib/releases). -raylib is also available via multiple [package managers](https://github.com/raysan5/raylib/issues/613) on multiple OS distributions. +raylib is also available via multiple package managers on multiple OS distributions. #### Installing and building raylib on multiple platforms @@ -113,7 +111,7 @@ learning and docs raylib is designed to be learned using [the examples](https://github.com/raysan5/raylib/tree/master/examples) as the main reference. There is no standard API documentation but there is a [**cheatsheet**](https://www.raylib.com/cheatsheet/cheatsheet.html) containing all the functions available on the library a short description of each one of them, input parameters and result value names should be intuitive enough to understand how each function works. -Some additional documentation about raylib design can be found in raylib GitHub Wiki. Here are the relevant links: +Some additional documentation about raylib design can be found in [raylib GitHub Wiki](https://github.com/raysan5/raylib/wiki). Here are the relevant links: - [raylib cheatsheet](https://www.raylib.com/cheatsheet/cheatsheet.html) - [raylib architecture](https://github.com/raysan5/raylib/wiki/raylib-architecture) @@ -129,15 +127,23 @@ raylib is present in several networks and raylib community is growing everyday. - Webpage: [https://www.raylib.com](https://www.raylib.com) - Discord: [https://discord.gg/raylib](https://discord.gg/raylib) - - Twitter: [https://www.twitter.com/raysan5](https://www.twitter.com/raysan5) + - X: [https://x.com/raysan5](https://x.com/raysan5) + - BlueSky: [https://bsky.app/profile/raysan5](https://bsky.app/profile/raysan5.bsky.social) - Twitch: [https://www.twitch.tv/raysan5](https://www.twitch.tv/raysan5) - Reddit: [https://www.reddit.com/r/raylib](https://www.reddit.com/r/raylib) - Patreon: [https://www.patreon.com/raylib](https://www.patreon.com/raylib) - YouTube: [https://www.youtube.com/channel/raylib](https://www.youtube.com/c/raylib) +contributors +------------ + + + + + license ------- raylib is licensed under an unmodified zlib/libpng license, which is an OSI-certified, BSD-like license that allows static linking with closed source software. Check [LICENSE](LICENSE) for further details. -raylib uses internally some libraries for window/graphics/inputs management and also to support different file formats loading, all those libraries are embedded with and are available in [src/external](https://github.com/raysan5/raylib/tree/master/src/external) directory. Check [raylib dependencies LICENSES](https://github.com/raysan5/raylib/wiki/raylib-dependencies) on raylib Wiki for details. +raylib uses internally some libraries for window/graphics/inputs management and also to support different file formats loading, all those libraries are embedded with and are available in [src/external](https://github.com/raysan5/raylib/tree/master/src/external) directory. Check [raylib dependencies LICENSES](https://github.com/raysan5/raylib/wiki/raylib-dependencies) on [raylib Wiki](https://github.com/raysan5/raylib/wiki) for details. diff --git a/vendor/raylib/linux-arm64/libraylib.a b/vendor/raylib/linux-arm64/libraylib.a new file mode 100644 index 000000000..0a8f83813 --- /dev/null +++ b/vendor/raylib/linux-arm64/libraylib.a @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:f35da654ad1f0223e833a76110d627659ab316a816dc770157c78bf18aeed65b +size 2930116 diff --git a/vendor/raylib/linux-arm64/libraylib.so.600 b/vendor/raylib/linux-arm64/libraylib.so.600 new file mode 100644 index 000000000..457d5b749 --- /dev/null +++ b/vendor/raylib/linux-arm64/libraylib.so.600 @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:af10d9d92cb18ae0770b0069a5d60d7bfd1da8f8d40bbf4ee8fed1d330028659 +size 2107056 diff --git a/vendor/raylib/linux/libraygui.a b/vendor/raylib/linux/libraygui.a index 46f13cd03..3b35e9d99 100644 Binary files a/vendor/raylib/linux/libraygui.a and b/vendor/raylib/linux/libraygui.a differ diff --git a/vendor/raylib/linux/libraygui.so b/vendor/raylib/linux/libraygui.so index bf589e047..750ec9e63 100644 Binary files a/vendor/raylib/linux/libraygui.so and b/vendor/raylib/linux/libraygui.so differ diff --git a/vendor/raylib/linux/libraylib.a b/vendor/raylib/linux/libraylib.a index 6e2d85c36..34c680e01 100644 Binary files a/vendor/raylib/linux/libraylib.a and b/vendor/raylib/linux/libraylib.a differ diff --git a/vendor/raylib/linux/libraylib.so b/vendor/raylib/linux/libraylib.so deleted file mode 100755 index 077150288..000000000 Binary files a/vendor/raylib/linux/libraylib.so and /dev/null differ diff --git a/vendor/raylib/linux/libraylib.so.5.5.0 b/vendor/raylib/linux/libraylib.so.5.5.0 deleted file mode 100755 index 077150288..000000000 Binary files a/vendor/raylib/linux/libraylib.so.5.5.0 and /dev/null differ diff --git a/vendor/raylib/linux/libraylib.so.550 b/vendor/raylib/linux/libraylib.so.550 deleted file mode 100755 index 077150288..000000000 Binary files a/vendor/raylib/linux/libraylib.so.550 and /dev/null differ diff --git a/vendor/raylib/linux/libraylib.so.600 b/vendor/raylib/linux/libraylib.so.600 new file mode 100644 index 000000000..f8240d6f8 --- /dev/null +++ b/vendor/raylib/linux/libraylib.so.600 @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:1041653dd5c1cb8c67494fa398a296520d3ffec20cf1bf71b1eaa4b96ac61aee +size 2100056 diff --git a/vendor/raylib/macos-arm64/libraygui.dylib b/vendor/raylib/macos-arm64/libraygui.dylib deleted file mode 100644 index 8a2eb1897..000000000 Binary files a/vendor/raylib/macos-arm64/libraygui.dylib and /dev/null differ diff --git a/vendor/raylib/macos-arm64/libraygui.a b/vendor/raylib/macos/libraygui-arm64.a similarity index 100% rename from vendor/raylib/macos-arm64/libraygui.a rename to vendor/raylib/macos/libraygui-arm64.a diff --git a/vendor/raylib/macos/libraygui-arm64.dylib b/vendor/raylib/macos/libraygui-arm64.dylib new file mode 100644 index 000000000..c876cf54c --- /dev/null +++ b/vendor/raylib/macos/libraygui-arm64.dylib @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:43fda6ec19b95c3d19306989c4eb2e168ac5603fd828ca774f673646376bcf23 +size 122992 diff --git a/vendor/raylib/macos/libraygui.dylib b/vendor/raylib/macos/libraygui.dylib index 9872f7af8..bd9984b6d 100644 Binary files a/vendor/raylib/macos/libraygui.dylib and b/vendor/raylib/macos/libraygui.dylib differ diff --git a/vendor/raylib/macos/libraylib.5.5.0.dylib b/vendor/raylib/macos/libraylib.5.5.0.dylib deleted file mode 100755 index 5019b64fc..000000000 Binary files a/vendor/raylib/macos/libraylib.5.5.0.dylib and /dev/null differ diff --git a/vendor/raylib/macos/libraylib.550.dylib b/vendor/raylib/macos/libraylib.550.dylib deleted file mode 100755 index 5019b64fc..000000000 Binary files a/vendor/raylib/macos/libraylib.550.dylib and /dev/null differ diff --git a/vendor/raylib/macos/libraylib.600.dylib b/vendor/raylib/macos/libraylib.600.dylib new file mode 100644 index 000000000..2f82abdde --- /dev/null +++ b/vendor/raylib/macos/libraylib.600.dylib @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:7e08b3372babd415f36f23a6852cb5bfce8b491c4f6c18756718142f274f5c66 +size 3606792 diff --git a/vendor/raylib/macos/libraylib.a b/vendor/raylib/macos/libraylib.a index c75576d0e..1e37a15c1 100644 Binary files a/vendor/raylib/macos/libraylib.a and b/vendor/raylib/macos/libraylib.a differ diff --git a/vendor/raylib/macos/libraylib.dylib b/vendor/raylib/macos/libraylib.dylib deleted file mode 100755 index 5019b64fc..000000000 Binary files a/vendor/raylib/macos/libraylib.dylib and /dev/null differ diff --git a/vendor/raylib/raygui.odin b/vendor/raylib/raygui.odin index 559437a60..b02fa4438 100644 --- a/vendor/raylib/raygui.odin +++ b/vendor/raylib/raygui.odin @@ -16,7 +16,7 @@ when ODIN_OS == .Windows { } else when ODIN_OS == .Darwin { when ODIN_ARCH == .arm64 { foreign import lib { - "macos-arm64/libraygui.dylib" when RAYGUI_SHARED else "macos-arm64/libraygui.a", + "macos/libraygui-arm64.dylib" when RAYGUI_SHARED else "macos/libraygui-arm64.a", } } else { foreign import lib { diff --git a/vendor/raylib/raylib.odin b/vendor/raylib/raylib.odin index b051f1885..01f4199ff 100644 --- a/vendor/raylib/raylib.odin +++ b/vendor/raylib/raylib.odin @@ -1,19 +1,19 @@ /* -Bindings for [[ raylib v5.5 ; https://www.raylib.com ]]. +Bindings for [[ raylib v6.0 ; https://www.raylib.com ]]. - ********************************************************************************************* + ********************************************************************************************** * - * raylib v5.5 - A simple and easy-to-use library to enjoy videogames programming (www.raylib.com) + * raylib v6.0 - A simple and easy-to-use library to enjoy videogames programming (www.raylib.com) * * FEATURES: * - NO external dependencies, all required libraries included with raylib - * - Multiplatform: Windows, Linux, FreeBSD, OpenBSD, NetBSD, DragonFly, - * MacOS, Haiku, Android, Raspberry Pi, DRM native, HTML5. + * - Multiplatform: Windows, Linux, macOS, FreeBSD, Web, Android, Raspberry Pi, DRM native... * - Written in plain C code (C99) in PascalCase/camelCase notation * - Hardware accelerated with OpenGL (1.1, 2.1, 3.3, 4.3, ES2, ES3 - choose at compile) - * - Unique OpenGL abstraction layer (usable as standalone module): [rlgl] + * - Software renderer optional, for systems with no GPU: [rlsw] + * - Custom OpenGL abstraction layer (usable as standalone module): [rlgl] * - Multiple Fonts formats supported (TTF, OTF, FNT, BDF, Sprite fonts) - * - Outstanding texture formats support, including compressed formats (DXT, ETC, ASTC) + * - Many texture formats supported, including compressed formats (DXT, ETC, ASTC) * - Full 3d support for 3d Shapes, Models, Billboards, Heightmaps and more! * - Flexible Materials system, supporting classic maps and PBR maps * - Animated 3D models supported (skeletal bones animation) (IQM, M3D, GLTF) @@ -29,24 +29,23 @@ Bindings for [[ raylib v5.5 ; https://www.raylib.com ]]. * - One default Shader is loaded on rlglInit()->rlLoadShaderDefault() [rlgl] (OpenGL 3.3 or ES2) * - One default RenderBatch is loaded on rlglInit()->rlLoadRenderBatch() [rlgl] (OpenGL 3.3 or ES2) * - * DEPENDENCIES (included): - * [rcore][GLFW] rglfw (Camilla Löwy - github.com/glfw/glfw) for window/context management and input - * [rcore][RGFW] rgfw (ColleagueRiley - github.com/ColleagueRiley/RGFW) for window/context management and input - * [rlgl] glad/glad_gles2 (David Herberth - github.com/Dav1dde/glad) for OpenGL 3.3 extensions loading + * DEPENDENCIES: + * [rcore] Depends on the selected platform backend, check rcore.c header for details + * [rlgl] glad/glad_gles2 (David Herberth - github.com/Dav1dde/glad) for OpenGL extensions loading * [raudio] miniaudio (David Reid - github.com/mackron/miniaudio) for audio device/context management * * OPTIONAL DEPENDENCIES (included): - * [rcore] msf_gif (Miles Fogle) for GIF recording * [rcore] sinfl (Micha Mettke) for DEFLATE decompression algorithm * [rcore] sdefl (Micha Mettke) for DEFLATE compression algorithm - * [rcore] rprand (Ramon Snatamaria) for pseudo-random numbers generation - * [rtextures] qoi (Dominic Szablewski - https://phoboslab.org) for QOI image manage - * [rtextures] stb_image (Sean Barret) for images loading (BMP, TGA, PNG, JPEG, HDR...) - * [rtextures] stb_image_write (Sean Barret) for image writing (BMP, TGA, PNG, JPG) - * [rtextures] stb_image_resize2 (Sean Barret) for image resizing algorithms - * [rtextures] stb_perlin (Sean Barret) for Perlin Noise image generation - * [rtext] stb_truetype (Sean Barret) for ttf fonts loading - * [rtext] stb_rect_pack (Sean Barret) for rectangles packing + * [rcore] rprand (Ramon Santamaria) for pseudo-random numbers generation + * [rtextures] qoi (Dominic Szablewski - https://phoboslab.org) for QOI image management + * [rtextures] stb_image (Sean Barrett) for images loading (BMP, TGA, PNG, JPEG, HDR...) + * [rtextures] stb_image_write (Sean Barrett) for image writing (BMP, TGA, PNG, JPG) + * [rtextures] stb_image_resize2 (Sean Barrett) for image resizing algorithms + * [rtextures] stb_perlin (Sean Barrett) for Perlin Noise image generation + * [rtextures] rltexgpu (Ramon Santamaria) for GPU-compressed texture formats + * [rtext] stb_truetype (Sean Barrett) for ttf fonts loading + * [rtext] stb_rect_pack (Sean Barrett) for rectangles packing * [rmodels] par_shapes (Philip Rideout) for parametric 3d shapes generation * [rmodels] tinyobj_loader_c (Syoyo Fujita) for models loading (OBJ, MTL) * [rmodels] cgltf (Johannes Kuhlmann) for models loading (glTF) @@ -55,10 +54,10 @@ Bindings for [[ raylib v5.5 ; https://www.raylib.com ]]. * [raudio] dr_wav (David Reid) for WAV audio file loading * [raudio] dr_flac (David Reid) for FLAC audio file loading * [raudio] dr_mp3 (David Reid) for MP3 audio file loading - * [raudio] stb_vorbis (Sean Barret) for OGG audio loading + * [raudio] stb_vorbis (Sean Barrett) for OGG audio loading * [raudio] jar_xm (Joshua Reisenauer) for XM audio module loading * [raudio] jar_mod (Joshua Reisenauer) for MOD audio module loading - * [raudio] qoa (Dominic Szablewski - https://phoboslab.org) for QOA audio manage + * [raudio] qoa (Dominic Szablewski - https://phoboslab.org) for QOA audio management * * * LICENSE: zlib/libpng @@ -66,7 +65,7 @@ Bindings for [[ raylib v5.5 ; https://www.raylib.com ]]. * raylib is licensed under an unmodified zlib/libpng license, which is an OSI-certified, * BSD-like license that allows static linking with closed source software: * - * Copyright (c) 2013-2024 Ramon Santamaria (@raysan5) + * Copyright (c) 2013-2026 Ramon Santamaria (@raysan5) * * This software is provided "as-is", without any express or implied warranty. In no event * will the authors be held liable for any damages arising from the use of this software. @@ -83,7 +82,7 @@ Bindings for [[ raylib v5.5 ; https://www.raylib.com ]]. * * 3. This notice may not be removed or altered from any source distribution. * - ********************************************************************************************* + ********************************************************************************************** */ package raylib @@ -98,7 +97,7 @@ MAX_MATERIAL_MAPS :: #config(RAYLIB_MAX_MATERIAL_MAPS, 12) #assert(size_of(rune) == size_of(c.int)) RAYLIB_SHARED :: #config(RAYLIB_SHARED, false) -RAYLIB_WASM_LIB :: #config(RAYLIB_WASM_LIB, "wasm/libraylib.a") +RAYLIB_WASM_LIB :: #config(RAYLIB_WASM_LIB, "wasm/libraylib.web.a") when ODIN_OS == .Windows { @(extra_linker_flags="/NODEFAULTLIB:" + ("msvcrt" when RAYLIB_SHARED else "libcmt")) @@ -110,18 +109,32 @@ when ODIN_OS == .Windows { "system:Shell32.lib", } } else when ODIN_OS == .Linux { - foreign import lib { - // Note(bumbread): I'm not sure why in `linux/` folder there are - // multiple copies of raylib.so, but since these bindings are for - // particular version of the library, I better specify it. Ideally, - // though, it's best specified in terms of major (.so.4) - "linux/libraylib.so.550" when RAYLIB_SHARED else "linux/libraylib.a", - "system:dl", - "system:pthread", + when ODIN_ARCH == .arm64 { + foreign import lib { + // Note(bumbread): I'm not sure why in `linux/` folder there are + // multiple copies of raylib.so, but since these bindings are for + // particular version of the library, I better specify it. Ideally, + // though, it's best specified in terms of major (.so.4) + "linux-arm64/libraylib.so.600" when RAYLIB_SHARED else "linux-arm/libraylib.a", + "system:dl", + "system:pthread", + "system:X11", + } + } else { + foreign import lib { + // Note(bumbread): I'm not sure why in `linux/` folder there are + // multiple copies of raylib.so, but since these bindings are for + // particular version of the library, I better specify it. Ideally, + // though, it's best specified in terms of major (.so.4) + "linux/libraylib.so.600" when RAYLIB_SHARED else "linux/libraylib.a", + "system:dl", + "system:pthread", + "system:X11", + } } } else when ODIN_OS == .Darwin { foreign import lib { - "macos/libraylib.550.dylib" when RAYLIB_SHARED else "macos/libraylib.a", + "macos/libraylib.600.dylib" when RAYLIB_SHARED else "macos/libraylib.a", "system:Cocoa.framework", "system:OpenGL.framework", "system:IOKit.framework", @@ -134,10 +147,10 @@ when ODIN_OS == .Windows { foreign import lib "system:raylib" } -VERSION_MAJOR :: 5 -VERSION_MINOR :: 5 +VERSION_MAJOR :: 6 +VERSION_MINOR :: 0 VERSION_PATCH :: 0 -VERSION :: "5.5" +VERSION :: "6.0" PI :: 3.14159265358979323846 DEG2RAD :: PI/180.0 @@ -193,7 +206,7 @@ Matrix :: #row_major matrix[4, 4]f32 // Note: In Raylib this is a struct. But here we use a fixed array, so that .rgba swizzling etc work. Color :: distinct [4]u8 -// Rectangle type +// Rectangle, 4 components Rectangle :: struct { x: f32, // Rectangle top-left corner position x y: f32, // Rectangle top-left corner position y @@ -201,8 +214,7 @@ Rectangle :: struct { height: f32, // Rectangle height } -// Image type, bpp always RGBA (32bit) -// NOTE: Data stored in CPU memory (RAM) +// Image, pixel data stored in CPU memory (RAM) Image :: struct { data: rawptr, // Image raw data width: c.int, // Image base width @@ -211,8 +223,7 @@ Image :: struct { format: PixelFormat, // Data format (PixelFormat type) } -// Texture type -// NOTE: Data stored in GPU memory +// Texture, tex data stored in GPU memory (VRAM) Texture :: struct { id: c.uint, // OpenGL texture id width: c.int, // Texture base width @@ -221,13 +232,13 @@ Texture :: struct { format: PixelFormat, // Data format (PixelFormat type) } -// Texture2D type, same as Texture +// Texture2D, same as Texture Texture2D :: Texture -// TextureCubemap type, actually, same as Texture +// TextureCubemap, same as Texture TextureCubemap :: Texture -// RenderTexture type, for texture rendering +// RenderTexture, fbo for texture rendering RenderTexture :: struct { id: c.uint, // OpenGL framebuffer object id texture: Texture, // Color buffer attachment texture @@ -237,7 +248,7 @@ RenderTexture :: struct { // RenderTexture2D type, same as RenderTexture RenderTexture2D :: RenderTexture -// N-Patch layout info +// NPatchInfo, n-patch layout info NPatchInfo :: struct { source: Rectangle, // Texture source rectangle left: c.int, // Left border offset @@ -247,7 +258,7 @@ NPatchInfo :: struct { layout: NPatchLayout, // Layout of the n-patch: 3x3, 1x3 or 3x1 } -// Font character info +// GlyphInfo, font characters glyphs info GlyphInfo :: struct { value: rune, // Character value (Unicode) offsetX: c.int, // Character offset X when drawing @@ -256,28 +267,28 @@ GlyphInfo :: struct { image: Image, // Character image data } -// Font type, includes texture and charSet array data +// Font, font texture and GlyphInfo array data Font :: struct { baseSize: c.int, // Base size (default chars height) glyphCount: c.int, // Number of characters glyphPadding: c.int, // Padding around the chars texture: Texture2D, // Characters texture atlas recs: [^]Rectangle, // Characters rectangles in texture - glyphs: [^]GlyphInfo, // Characters info data + glyphs: [^]GlyphInfo, // Characters info data } -// Camera type, defines a camera position/orientation in 3d space +// Camera, defines position/orientation in 3d space Camera3D :: struct { - position: Vector3, // Camera position - target: Vector3, // Camera target it looks-at - up: Vector3, // Camera up vector (rotation over its axis) - fovy: f32, // Camera field-of-view apperture in Y (degrees) in perspective, used as near plane width in orthographic + position: Vector3, // Camera position + target: Vector3, // Camera target it looks-at + up: Vector3, // Camera up vector (rotation over its axis) + fovy: f32, // Camera field-of-view aperture in Y (degrees) in perspective, used as near plane height in world units in orthographic projection: CameraProjection, // Camera projection: `.PERSPECTIVE` or `.ORTHOGRAPHIC` } Camera :: Camera3D // Camera type fallback, defaults to Camera3D -// Camera2D type, defines a 2d camera +// Camera2D, defines position/orientation in 2d space Camera2D :: struct { offset: Vector2, // Camera offset (displacement from target) target: Vector2, // Camera target (rotation and zoom origin) @@ -285,93 +296,107 @@ Camera2D :: struct { zoom: f32, // Camera zoom (scaling), should be 1.0f by default } -// Vertex data defining a mesh +// Mesh, vertex data and vao/vbo // NOTE: Data stored in CPU memory (and GPU) Mesh :: struct { vertexCount: c.int, // Number of vertices stored in arrays triangleCount: c.int, // Number of triangles stored (indexed or not) // Default vertex data - vertices: [^]f32, // Vertex position (XYZ - 3 components per vertex) (shader-location = 0) - texcoords: [^]f32, // Vertex texture coordinates (UV - 2 components per vertex) (shader-location = 1) - texcoords2: [^]f32, // Vertex second texture coordinates (useful for lightmaps) (shader-location = 5) - normals: [^]f32, // Vertex normals (XYZ - 3 components per vertex) (shader-location = 2) - tangents: [^]f32, // Vertex tangents (XYZW - 4 components per vertex) (shader-location = 4) - colors: [^]u8, // Vertex colors (RGBA - 4 components per vertex) (shader-location = 3) - indices: [^]u16, // Vertex indices (in case vertex data comes indexed) + vertices: [^]f32, // Vertex position (XYZ - 3 components per vertex) (shader-location = 0) + texcoords: [^]f32, // Vertex texture coordinates (UV - 2 components per vertex) (shader-location = 1) + texcoords2: [^]f32, // Vertex second texture coordinates (useful for lightmaps) (shader-location = 5) + normals: [^]f32, // Vertex normals (XYZ - 3 components per vertex) (shader-location = 2) + tangents: [^]f32, // Vertex tangents (XYZW - 4 components per vertex) (shader-location = 4) + colors: [^]u8, // Vertex colors (RGBA - 4 components per vertex) (shader-location = 3) + indices: [^]u16, // Vertex indices (in case vertex data comes indexed) - // Animation vertex data + // Skin data for animation + boneCount: c.int, // Number of bones (MAX: 256 bones) + boneIndices: [^]u8, // Vertex bone indices, up to 4 bones influence by vertex (skinning) (shader-location = 6) + boneWeights: [^]f32, // Vertex bone weight, up to 4 bones influence by vertex (skinning) (shader-location = 7) + + // Runtime animation vertex data (CPU skinning) + // NOTE: In case of GPU skinning, not used, pointers are NULL animVertices: [^]f32, // Animated vertex positions (after bones transformations) animNormals: [^]f32, // Animated normals (after bones transformations) - boneIds: [^]u8, // Vertex bone ids, up to 4 bones influence by vertex (skinning) - boneWeights: [^]f32, // Vertex bone weight, up to 4 bones influence by vertex (skinning) - boneMatrices: [^]Matrix, // Bones animated transformation matrices - boneCount: c.int, // Number of bones // OpenGL identifiers - vaoId: u32, // OpenGL Vertex Array Object id - vboId: [^]u32, // OpenGL Vertex Buffer Objects id (default vertex data) + vaoId: u32, // OpenGL Vertex Array Object id + vboId: [^]u32, // OpenGL Vertex Buffer Objects id (default vertex data) } -// Shader type (generic) +// Shader Shader :: struct { id: c.uint, // Shader program id locs: [^]c.int, // Shader locations array (MAX_SHADER_LOCATIONS) } -// Material texture map +// MaterialMap MaterialMap :: struct { texture: Texture2D, // Material map texture color: Color, // Material map color value: f32, // Material map value } -// Material type (generic) +// Material, includes shader and maps Material :: struct { shader: Shader, // Material shader maps: [^]MaterialMap, // Material maps array (MAX_MATERIAL_MAPS) params: [4]f32, // Material generic parameters (if required) } -// Transformation properties +// Transform, vertex transformation data Transform :: struct { translation: Vector3, // Translation rotation: Quaternion, // Rotation scale: Vector3, // Scale } -// Bone information +// Anim pose, an array of Transform[] +ModelAnimPose :: [^]Transform + +// Bone, skeletal animation bone BoneInfo :: struct { name: [32]byte `fmt:"s,0"`, // Bone name parent: c.int, // Bone parent } -// Model type +// Skeleton, animation bones hierarchy +ModelSkeleton :: struct { + boneCount: c.int, // Number of bones + bones: [^]BoneInfo, // Bones information (skeleton) + bindPose: ModelAnimPose, // Bones base transformation (Transform[]) +} + +// Model, meshes, materials and animation data Model :: struct #align(align_of(uintptr)) { transform: Matrix, // Local transform matrix - meshCount: c.int, // Number of meshes + meshCount: c.int, // Number of meshes materialCount: c.int, // Number of materials - meshes: [^]Mesh, // Meshes array - materials: [^]Material, // Materials array - meshMaterial: [^]c.int, // Mesh material number + meshes: [^]Mesh, // Meshes array + materials: [^]Material, // Materials array + meshMaterial: [^]c.int, // Mesh material number // Animation data - boneCount: c.int, // Number of bones - bones: [^]BoneInfo, // Bones information (skeleton) - bindPose: [^]Transform, // Bones base transformation (pose) + skeleton: ModelSkeleton, // Skeleton for animation + + // Runtime animation data (CPU/GPU skinning) + currentPose: ModelAnimPose, // Current animation pose (Transform[]) + boneMatrices: [^]Matrix, // Bones animated transformation matrices } -// Model animation +// ModelAnimation, contains a full animation sequence ModelAnimation :: struct { - boneCount: c.int, // Number of bones - frameCount: c.int, // Number of animation frames - bones: [^]BoneInfo, // Bones information (skeleton) - framePoses: [^][^]Transform, // Poses array by frame - name: [32]byte `fmt:"s,0"`, // Animation name + name: [32]byte `fmt:"s,0"`, // Animation name + + boneCount: c.int, // Number of bones (per pose) + keyframeCount: c.int, // Number of animation key frames + keyframePoses: [^][^]Transform, // Animation sequence keyframe poses [keyframe][pose] } -// Ray type (useful for raycast) +// Ray, ray for raycasting Ray :: struct { position: Vector3, // Ray position (origin) direction: Vector3, // Ray direction (normalized) @@ -381,7 +406,7 @@ Ray :: struct { RayCollision :: struct { hit: bool, // Did the ray hit something? distance: f32, // Distance to nearest hit - point: Vector3, // Point of nearest hit + point: Vector3, // Point of the nearest hit normal: Vector3, // Surface normal of hit } @@ -391,27 +416,27 @@ BoundingBox :: struct { max: Vector3, // Maximum vertex box-corner } -// Wave type, defines audio wave data +// Wave, audio wave data Wave :: struct { frameCount: c.uint, // Total number of frames (considering channels) sampleRate: c.uint, // Frequency (samples per second) sampleSize: c.uint, // Bit depth (bits per sample): 8, 16, 32 (24 not supported) - channels: c.uint, // Number of channels (1-mono, 2-stereo) + channels: c.uint, // Number of channels (1-mono, 2-stereo, ...) data: rawptr, // Buffer data pointer } // Audio stream type // NOTE: Actual structs are defined internally in raudio module AudioStream :: struct { - buffer: rawptr, // Pointer to internal data used by the audio system - processor: rawptr, // Pointer to internal data processor, useful for audio effects + buffer: rawptr, // Pointer to internal data used by the audio system + processor: rawptr, // Pointer to internal data processor, useful for audio effects sampleRate: c.uint, // Frequency (samples per second) sampleSize: c.uint, // Bit depth (bits per sample): 8, 16, 32 (24 not supported) - channels: c.uint, // Number of channels (1-mono, 2-stereo) + channels: c.uint, // Number of channels (1-mono, 2-stereo, ...) } -// Sound source type +// Sound Sound :: struct { using stream: AudioStream, // Audio stream frameCount: c.uint, // Total number of frames (considering channels) @@ -428,7 +453,7 @@ Music :: struct { ctxData: rawptr, // Audio context data, depends on type } -// Head-Mounted-Display device parameters +// VrDeviceInfo, Head-Mounted-Display device parameters VrDeviceInfo :: struct { hResolution: c.int, // Horizontal resolution in pixels vResolution: c.int, // Vertical resolution in pixels @@ -441,7 +466,7 @@ VrDeviceInfo :: struct { chromaAbCorrection: [4]f32, // Chromatic aberration correction parameters } -// VR Stereo rendering configuration for simulator +// VrStereoConfig, VR stereo rendering configuration for simulator VrStereoConfig :: struct #align(4) { projection: [2]Matrix, // VR projection matrices (per eye) viewOffset: [2]Matrix, // VR view offset matrices (per eye) @@ -455,16 +480,15 @@ VrStereoConfig :: struct #align(4) { // File path list FilePathList :: struct { - capacity: c.uint, // Filepaths max entries - count: c.uint, // Filepaths entries count - paths: [^]cstring, // Filepaths entries + count: c.uint, // Filepaths entries count + paths: [^]cstring, // Filepaths entries } // Automation event AutomationEvent :: struct { frame: c.uint, // Event frame type: c.uint, // Event type (AutomationEventType) - params: [4]c.int, // Event parameters (if required) --- + params: [4]c.int, // Event parameters (if required) } // Automation event list @@ -500,8 +524,8 @@ ConfigFlag :: enum c.int { } ConfigFlags :: distinct bit_set[ConfigFlag; c.int] - // Trace log level +// NOTE: Organized by priority level TraceLogLevel :: enum c.int { ALL = 0, // Display all logs TRACE, // Trace logging, intended for internal use only @@ -514,8 +538,7 @@ TraceLogLevel :: enum c.int { } // Keyboard keys (US keyboard layout) -// NOTE: Use GetKeyPressed() to allow redefining -// required keys for alternative layouts +// NOTE: Use GetKeyPressed() to allow redefining required keys for alternative layouts KeyboardKey :: enum c.int { KEY_NULL = 0, // Key: NULL, used for no key pressed // Alphanumeric keys @@ -681,7 +704,7 @@ GamepadButton :: enum c.int { RIGHT_THUMB, // Gamepad joystick pressed button right } -// Gamepad axis +// Gamepad axes GamepadAxis :: enum c.int { LEFT_X = 0, // Gamepad left stick X axis LEFT_Y = 1, // Gamepad left stick Y axis @@ -706,8 +729,9 @@ MaterialMapIndex :: enum c.int { BRDF, // Brdf material } - // Shader location index +// NOTE: Some locations are tried to be set automatically on shader loading, +// but only if default attributes/uniforms names are found, check config.h for names ShaderLocationIndex :: enum c.int { VERTEX_POSITION = 0, // Shader location: vertex attribute: position VERTEX_TEXCOORD01, // Shader location: vertex attribute: texcoord01 @@ -730,17 +754,17 @@ ShaderLocationIndex :: enum c.int { MAP_ROUGHNESS, // Shader location: sampler2d texture: roughness MAP_OCCLUSION, // Shader location: sampler2d texture: occlusion MAP_EMISSION, // Shader location: sampler2d texture: emission - MAP_HEIGHT, // Shader location: sampler2d texture: height + MAP_HEIGHT, // Shader location: sampler2d texture: heightmap MAP_CUBEMAP, // Shader location: samplerCube texture: cubemap MAP_IRRADIANCE, // Shader location: samplerCube texture: irradiance MAP_PREFILTER, // Shader location: samplerCube texture: prefilter MAP_BRDF, // Shader location: sampler2d texture: brdf - VERTEX_BONEIDS, // Shader location: vertex attribute: boneIds - VERTEX_BONEWEIGHTS, // Shader location: vertex attribute: boneWeights - BONE_MATRICES, // Shader location: array of matrices uniform: boneMatrices + VERTEX_BONEIDS, // Shader location: vertex attribute: bone indices + VERTEX_BONEWEIGHTS, // Shader location: vertex attribute: bone weights + MATRIX_BONETRANSFORMS, // Shader location: matrix attribute: bone transforms (animation) + VERTEX_INSTANCETRANSFORM, // Shader location: vertex attribute: instance transforms } - // Shader uniform data type ShaderUniformDataType :: enum c.int { FLOAT = 0, // Shader uniform type: float @@ -751,9 +775,21 @@ ShaderUniformDataType :: enum c.int { IVEC2, // Shader uniform type: ivec2 (2 int) IVEC3, // Shader uniform type: ivec3 (3 int) IVEC4, // Shader uniform type: ivec4 (4 int) + UINT, // Shader uniform type: unsigned int + UIVEC2, // Shader uniform type: uivec2 (2 unsigned int) + UIVEC3, // Shader uniform type: uivec3 (3 unsigned int) + UIVEC4, // Shader uniform type: uivec4 (4 unsigned int) SAMPLER2D, // Shader uniform type: sampler2d } +// Shader attribute data types +ShaderAttributeDataType :: enum c.int { + FLOAT = 0, // Shader attribute type: float + VEC2, // Shader attribute type: vec2 (2 float) + VEC3, // Shader attribute type: vec3 (3 float) + VEC4, // Shader attribute type: vec4 (4 float) +} + // Pixel formats // NOTE: Support depends on OpenGL version and platform PixelFormat :: enum c.int { @@ -883,6 +919,7 @@ NPatchLayout :: enum c.int { // Callbacks to hook some internal functions // WARNING: This callbacks are intended for advanced users + TraceLogCallback :: #type proc "c" (logLevel: TraceLogLevel, text: cstring, args: ^c.va_list) // Logging: Redirect trace log messages LoadFileDataCallback :: #type proc "c"(fileName: cstring, dataSize: ^c.int) -> [^]u8 // FileIO: Load binary data SaveFileDataCallback :: #type proc "c" (fileName: cstring, data: rawptr, dataSize: c.int) -> bool // FileIO: Save binary data @@ -906,8 +943,8 @@ foreign lib { // Window-related functions InitWindow :: proc(width, height: c.int, title: cstring) --- // Initialize window and OpenGL context - WindowShouldClose :: proc() -> bool --- // Check if application should close (KEY_ESCAPE pressed or windows close icon clicked) CloseWindow :: proc() --- // Close window and unload OpenGL context + WindowShouldClose :: proc() -> bool --- // Check if application should close (KEY_ESCAPE pressed or windows close icon clicked) IsWindowReady :: proc() -> bool --- // Check if window has been initialized successfully IsWindowFullscreen :: proc() -> bool --- // Check if window is currently fullscreen IsWindowHidden :: proc() -> bool --- // Check if window is currently hidden @@ -955,17 +992,6 @@ foreign lib { EnableEventWaiting :: proc() --- // Enable waiting for events on EndDrawing(), no automatic event polling DisableEventWaiting :: proc() --- // Disable waiting for events on EndDrawing(), automatic events polling - - // Custom frame control functions - // NOTE: Those functions are intended for advance users that want full control over the frame processing - // By default EndDrawing() does this job: draws everything + SwapScreenBuffer() + manage frame timing + PollInputEvents() - // To avoid that behaviour and control frame processes manually, enable in config.h: SUPPORT_CUSTOM_FRAME_CONTROL - - SwapScreenBuffer :: proc() --- // Swap back buffer with front buffer (screen drawing) - PollInputEvents :: proc() --- // Register all input events - WaitTime :: proc(seconds: f64) --- // Wait for some time (halt program execution) - - // Cursor-related functions ShowCursor :: proc() --- // Shows cursor @@ -1010,6 +1036,7 @@ foreign lib { GetShaderLocationAttrib :: proc(shader: Shader, attribName: cstring) -> c.int --- // Get shader attribute location // We use #any_int here so we can pass ShaderLocationIndex + SetShaderValue :: proc(shader: Shader, #any_int locIndex: c.int, value: rawptr, uniformType: ShaderUniformDataType) --- // Set shader uniform value SetShaderValueV :: proc(shader: Shader, #any_int locIndex: c.int, value: rawptr, uniformType: ShaderUniformDataType, count: c.int) --- // Set shader uniform value vector SetShaderValueMatrix :: proc(shader: Shader, #any_int locIndex: c.int, mat: Matrix) --- // Set shader uniform value (matrix 4x4) @@ -1019,7 +1046,7 @@ foreign lib { // Screen-space-related functions GetScreenToWorldRay :: proc(position: Vector2, camera: Camera) -> Ray --- // Get a ray trace from screen position (i.e mouse) - GetScreenToWorldRayEx :: proc(position: Vector2, camera: Camera, width: c.int, height: c.int) ->Ray --- // Get a ray trace from screen position (i.e mouse) in a viewport + GetScreenToWorldRayEx :: proc(position: Vector2, camera: Camera, width: c.int, height: c.int) -> Ray --- // Get a ray trace from screen position (i.e mouse) in a viewport GetWorldToScreen :: proc(position: Vector3, camera: Camera) -> Vector2 --- // Get the screen space position for a 3d world space position GetWorldToScreenEx :: proc(position: Vector3, camera: Camera, width: c.int, height: c.int) -> Vector2 --- // Get size position for a 3d world space position GetWorldToScreen2D :: proc(position: Vector2, camera: Camera2D) -> Vector2 --- // Get the screen space position for a 2d camera world space position @@ -1030,9 +1057,18 @@ foreign lib { // Timing-related functions SetTargetFPS :: proc(fps: c.int) --- // Set target FPS (maximum) - GetFPS :: proc() -> c.int --- // Returns current FPS GetFrameTime :: proc() -> f32 --- // Returns time in seconds for last frame drawn (delta time) GetTime :: proc() -> f64 --- // Returns elapsed time in seconds since InitWindow() + GetFPS :: proc() -> c.int --- // Returns current FPS + + // Custom frame control functions + // NOTE: Those functions are intended for advance users that want full control over the frame processing + // By default EndDrawing() does this job: draws everything + SwapScreenBuffer() + manage frame timing + PollInputEvents() + // To avoid that behaviour and control frame processes manually, enable in config.h: SUPPORT_CUSTOM_FRAME_CONTROL + + SwapScreenBuffer :: proc() --- // Swap back buffer with front buffer (screen drawing) + PollInputEvents :: proc() --- // Register all input events + WaitTime :: proc(seconds: f64) --- // Wait for some time (halt program execution) // Random value generation functions @@ -1042,26 +1078,23 @@ foreign lib { UnloadRandomSequence :: proc(sequence: [^]c.int) --- // Unload random values sequence // Misc. functions + TakeScreenshot :: proc(fileName: cstring) --- // Takes a screenshot of current screen (filename extension defines format) SetConfigFlags :: proc(flags: ConfigFlags) --- // Setup init configuration flags (view FLAGS). NOTE: This function is expected to be called before window creation OpenURL :: proc(url: cstring) --- // Open URL with default system browser (if available) // NOTE: Following functions implemented in module [utils] //------------------------------------------------------------------ + TraceLog :: proc(logLevel: TraceLogLevel, text: cstring, #c_vararg args: ..any) --- // Show trace log messages (LOG_DEBUG, LOG_INFO, LOG_WARNING, LOG_ERROR) SetTraceLogLevel :: proc(logLevel: TraceLogLevel) --- // Set the current threshold (minimum) log level + SetTraceLogCallback :: proc(callback: TraceLogCallback) --- // Set custom trace log + + // Memory management, using internal allocators + MemAlloc :: proc(size: c.uint) -> rawptr --- // Internal memory allocator MemRealloc :: proc(ptr: rawptr, size: c.uint) -> rawptr --- // Internal memory reallocator - // Set custom callbacks - // WARNING: Callbacks setup is intended for advanced users - - SetTraceLogCallback :: proc(callback: TraceLogCallback) --- // Set custom trace log - SetLoadFileDataCallback :: proc(callback: LoadFileDataCallback) --- // Set custom file binary data loader - SetSaveFileDataCallback :: proc(callback: SaveFileDataCallback) --- // Set custom file binary data saver - SetLoadFileTextCallback :: proc(callback: LoadFileTextCallback) --- // Set custom file text data loader - SetSaveFileTextCallback :: proc(callback: SaveFileTextCallback) --- // Set custom file text data saver - // Files management functions LoadFileData :: proc(fileName: cstring, dataSize: ^c.int) -> [^]byte --- // Load file data as byte array (read) @@ -1072,12 +1105,27 @@ foreign lib { UnloadFileText :: proc(text: [^]byte) --- // Unload file text data allocated by LoadFileText() SaveFileText :: proc(fileName: cstring, text: [^]byte) -> bool --- // Save text data to file (write), string must be '\0' terminated, returns true on success + // File access custom callbacks + // WARNING: Callbacks setup is intended for advanced users + + SetLoadFileDataCallback :: proc(callback: LoadFileDataCallback) --- // Set custom file binary data loader + SetSaveFileDataCallback :: proc(callback: SaveFileDataCallback) --- // Set custom file binary data saver + SetLoadFileTextCallback :: proc(callback: LoadFileTextCallback) --- // Set custom file text data loader + SetSaveFileTextCallback :: proc(callback: SaveFileTextCallback) --- // Set custom file text data saver + // File system functions + FileRename :: proc(fileName: cstring, fileRename: cstring) -> c.int --- // Rename file (if exists) + FileRemove :: proc(fileName: cstring) -> c.int --- // Remove file (if exists) + FileCopy :: proc(srcPath: cstring, dstPath: cstring) -> c.int --- // Copy file from one path to another, dstPath created if it doesn't exist + FileMove :: proc(srcPath: cstring, dstPath: cstring) -> c.int --- // Move file from one directory to another, dstPath created if it doesn't exist + FileTextReplace :: proc(fileName: cstring, search: cstring, replacement: cstring) -> c.int --- // Replace text in an existing file + FileTextFindIndex :: proc(fileName: cstring, search: cstring) -> c.int --- // Find text in existing file FileExists :: proc(fileName: cstring) -> bool --- // Check if file exists DirectoryExists :: proc(dirPath: cstring) -> bool --- // Check if a directory path exists IsFileExtension :: proc(fileName, ext: cstring) -> bool --- // Check file extension (including point: .png, .wav) GetFileLength :: proc(fileName: cstring) -> c.int --- // Get file length in bytes (NOTE: GetFileSize() conflicts with windows.h) + GetFileModTime :: proc(fileName: cstring) -> c.long --- // Get file modification time (last write time) GetFileExtension :: proc(fileName: cstring) -> cstring --- // Get pointer to extension for a filename string (includes dot: '.png') GetFileName :: proc(filePath: cstring) -> cstring --- // Get pointer to filename for a path string GetFileNameWithoutExt :: proc(filePath: cstring) -> cstring --- // Get filename string without extension (uses static string) @@ -1088,25 +1136,26 @@ foreign lib { MakeDirectory :: proc(dirPath: cstring) -> c.int --- // Create directories (including full path requested), returns 0 on success ChangeDirectory :: proc(dir: cstring) -> bool --- // Change working directory, return true on success IsPathFile :: proc(path: cstring) -> bool --- // Check if a given path is a file or a directory - IsFileNameValid :: proc (fileName: cstring) -> bool --- // Check if fileName is valid for the platform/OS + IsFileNameValid :: proc(fileName: cstring) -> bool --- // Check if fileName is valid for the platform/OS LoadDirectoryFiles :: proc(dirPath: cstring) -> FilePathList --- // Load directory filepaths LoadDirectoryFilesEx :: proc(basePath: cstring, filter: cstring, scanSubdirs: bool) -> FilePathList --- // Load directory filepaths with extension filtering and recursive directory scan. Use 'DIR' in the filter string to include directories in the result UnloadDirectoryFiles :: proc(files: FilePathList) --- // Unload filepaths IsFileDropped :: proc() -> bool --- // Check if a file has been dropped into window LoadDroppedFiles :: proc() -> FilePathList --- // Load dropped filepaths UnloadDroppedFiles :: proc(files: FilePathList) --- // Unload dropped filepaths - GetFileModTime :: proc(fileName: cstring) -> c.long --- // Get file modification time (last write time) + GetDirectoryFileCount :: proc(dirPath: cstring) -> c.uint --- // Get the file count in a directory + GetDirectoryFileCountEx :: proc(basePath: cstring, filter: cstring, scanSubdirs: bool) -> c.uint --- // Get the file count in a directory with extension filtering and recursive directory scan. Use 'DIR' in the filter string to include directories in the result // Compression/Encoding functionality CompressData :: proc(data: rawptr, dataSize: c.int, compDataSize: ^c.int) -> [^]byte --- // Compress data (DEFLATE algorithm), memory must be MemFree() DecompressData :: proc(compData: rawptr, compDataSize: c.int, dataSize: ^c.int) -> [^]byte --- // Decompress data (DEFLATE algorithm), memory must be MemFree() - EncodeDataBase64 :: proc(data: rawptr, dataSize: c.int, outputSize: ^c.int) -> [^]byte --- // Encode data to Base64 string, memory must be MemFree() - DecodeDataBase64 :: proc(data: rawptr, outputSize: ^c.int) -> [^]byte --- // Decode Base64 string data, memory must be MemFree() + EncodeDataBase64 :: proc(data: rawptr, dataSize: c.int, outputSize: ^c.int) -> cstring --- // Encode data to Base64 string (includes NULL terminator), memory must be MemFree() + DecodeDataBase64 :: proc(data: rawptr, outputSize: ^c.int) -> cstring --- // Decode Base64 string (expected NULL terminated), memory must be MemFree() ComputeCRC32 :: proc(data: rawptr, dataSize: c.int) -> c.uint --- // Compute CRC32 hash code - ComputeMD5 :: proc (data: rawptr, dataSize: c.int) -> [^]c.uint --- // Compute MD5 hash code, returns static int[4] (16 bytes) + ComputeMD5 :: proc(data: rawptr, dataSize: c.int) -> [^]c.uint --- // Compute MD5 hash code, returns static int[4] (16 bytes) ComputeSHA1 :: proc(data: rawptr, dataSize: c.int) -> [^]c.uint --- // Compute SHA1 hash code, returns static int[5] (20 bytes) - + ComputeSHA256 :: proc(data: rawptr, dataSize: c.int) -> [^]c.uint --- // Compute SHA256 hash code, returns static int[8] (32 bytes) // Automation events functionality @@ -1132,6 +1181,7 @@ foreign lib { IsKeyUp :: proc(key: KeyboardKey) -> bool --- // Detect if a key is NOT being pressed GetKeyPressed :: proc() -> KeyboardKey --- // Get key pressed (keycode), call it multiple times for keys queued GetCharPressed :: proc() -> rune --- // Get char pressed (unicode), call it multiple times for chars queued + GetKeyName :: proc(key: KeyboardKey) -> cstring --- // Get name of a QWERTY key on the current keyboard layout (eg returns string 'q' for KEY_A on an AZERTY keyboard) SetExitKey :: proc(key: KeyboardKey) --- // Set a custom key to exit program (default is ESC) // Input-related functions: gamepads @@ -1148,45 +1198,43 @@ foreign lib { SetGamepadMappings :: proc(mappings: cstring) -> c.int --- // Set internal gamepad mappings (SDL_GameControllerDB) SetGamepadVibration :: proc(gamepad: c.int, leftMotor: f32, rightMotor: f32, duration: f32) --- // Set gamepad vibration for both motors (duration in seconds) - // Input-related functions: mouse - IsMouseButtonPressed :: proc(button: MouseButton) -> bool --- // Detect if a mouse button has been pressed once - IsMouseButtonDown :: proc(button: MouseButton) -> bool --- // Detect if a mouse button is being pressed - IsMouseButtonReleased :: proc(button: MouseButton) -> bool --- // Detect if a mouse button has been released once - IsMouseButtonUp :: proc(button: MouseButton) -> bool --- + IsMouseButtonPressed :: proc(button: MouseButton) -> bool --- // Check if a mouse button has been pressed once + IsMouseButtonDown :: proc(button: MouseButton) -> bool --- // Check if a mouse button is being pressed + IsMouseButtonReleased :: proc(button: MouseButton) -> bool --- // Check if a mouse button has been released once + IsMouseButtonUp :: proc(button: MouseButton) -> bool --- // Check if a mouse button is NOT being pressed - GetMouseX :: proc() -> c.int --- // Returns mouse position X - GetMouseY :: proc() -> c.int --- // Returns mouse position Y - GetMousePosition :: proc() -> Vector2 --- // Returns mouse position XY - GetMouseDelta :: proc() -> Vector2 --- // Returns mouse delta XY + GetMouseX :: proc() -> c.int --- // Get mouse position X + GetMouseY :: proc() -> c.int --- // Get mouse position Y + GetMousePosition :: proc() -> Vector2 --- // Get mouse position XY + GetMouseDelta :: proc() -> Vector2 --- // Get mouse delta between frames SetMousePosition :: proc(x, y: c.int) --- // Set mouse position XY SetMouseOffset :: proc(offsetX, offsetY: c.int) --- // Set mouse offset SetMouseScale :: proc(scaleX, scaleY: f32) --- // Set mouse scaling - GetMouseWheelMove :: proc() -> f32 --- // Returns mouse wheel movement Y + GetMouseWheelMove :: proc() -> f32 --- // Get mouse wheel movement for X or Y, whichever is larger GetMouseWheelMoveV :: proc() -> Vector2 --- // Get mouse wheel movement for both X and Y SetMouseCursor :: proc(cursor: MouseCursor) --- // Set mouse cursor // Input-related functions: touch - GetTouchX :: proc() -> c.int --- // Returns touch position X for touch point 0 (relative to screen size) - GetTouchY :: proc() -> c.int --- // Returns touch position Y for touch point 0 (relative to screen size) - GetTouchPosition :: proc(index: c.int) -> Vector2 --- // Returns touch position XY for a touch point index (relative to screen size) - GetTouchPointId :: proc(index: c.int) -> c.int --- // Get touch point identifier for given index - GetTouchPointCount :: proc() -> c.int --- // Get number of touch points + GetTouchX :: proc() -> c.int --- // Get touch position X for touch point 0 (relative to screen size) + GetTouchY :: proc() -> c.int --- // Get touch position Y for touch point 0 (relative to screen size) + GetTouchPosition :: proc(index: c.int) -> Vector2 --- // Get touch position XY for a touch point index (relative to screen size) + GetTouchPointId :: proc(index: c.int) -> c.int --- // Get touch point identifier for given index + GetTouchPointCount :: proc() -> c.int --- // Get number of touch points //------------------------------------------------------------------------------------ // Gestures and Touch Handling Functions (Module: rgestures) //------------------------------------------------------------------------------------ - SetGesturesEnabled :: proc(flags: Gestures) --- // Enable a set of gestures using flags - - GetGestureDetected :: proc() -> Gestures --- // Get latest detected gesture - GetGestureHoldDuration :: proc() -> f32 --- // Get gesture hold time in seconds - GetGestureDragVector :: proc() -> Vector2 --- // Get gesture drag vector - GetGestureDragAngle :: proc() -> f32 --- // Get gesture drag angle - GetGesturePinchVector :: proc() -> Vector2 --- // Get gesture pinch delta - GetGesturePinchAngle :: proc() -> f32 --- // Get gesture pinch angle + SetGesturesEnabled :: proc(flags: Gestures) --- // Enable a set of gestures using flags + // IsGestureDetected is declared near the bottom + GetGestureHoldDuration :: proc() -> f32 --- // Get gesture hold time in seconds + GetGestureDragVector :: proc() -> Vector2 --- // Get gesture drag vector + GetGestureDragAngle :: proc() -> f32 --- // Get gesture drag angle + GetGesturePinchVector :: proc() -> Vector2 --- // Get gesture pinch delta + GetGesturePinchAngle :: proc() -> f32 --- // Get gesture pinch angle //------------------------------------------------------------------------------------ // Camera System Functions (Module: camera) @@ -1223,7 +1271,6 @@ foreign lib { GetShapesTexture :: proc() -> Texture2D --- // Get texture that is used for shapes drawing GetShapesTextureRectangle :: proc() -> Rectangle --- // Get texture source rectangle that is used for shapes drawing - // Basic shapes drawing functions DrawPixel :: proc(posX, posY: c.int, color: Color) --- // Draw a pixel using geometry [Can be slow, use with care] @@ -1233,15 +1280,18 @@ foreign lib { DrawLineEx :: proc(startPos, endPos: Vector2, thick: f32, color: Color) --- // Draw a line (using triangles/quads) DrawLineStrip :: proc(points: [^]Vector2, pointCount: c.int, color: Color) --- // Draw lines sequence (using gl lines) DrawLineBezier :: proc(startPos, endPos: Vector2, thick: f32, color: Color) --- // Draw line segment cubic-bezier in-out interpolation + DrawLineDashed :: proc(startPos, endPos: Vector2, dashSize: c.int, spaceSize: c.int, color: Color) --- // Draw a dashed line DrawCircle :: proc(centerX, centerY: c.int, radius: f32, color: Color) --- // Draw a color-filled circle + DrawCircleV :: proc(center: Vector2, radius: f32, color: Color) --- // Draw a color-filled circle (Vector version) + DrawCircleGradient :: proc(center: Vector2, radius: f32, inner, outer: Color) --- // Draw a gradient-filled circle DrawCircleSector :: proc(center: Vector2, radius: f32, startAngle, endAngle: f32, segments: c.int, color: Color) --- // Draw a piece of a circle DrawCircleSectorLines :: proc(center: Vector2, radius: f32, startAngle, endAngle: f32, segments: c.int, color: Color) --- // Draw circle sector outline - DrawCircleGradient :: proc(centerX, centerY: c.int, radius: f32, inner, outer: Color) --- // Draw a gradient-filled circle - DrawCircleV :: proc(center: Vector2, radius: f32, color: Color) --- // Draw a color-filled circle (Vector version) DrawCircleLines :: proc(centerX, centerY: c.int, radius: f32, color: Color) --- // Draw circle outline DrawCircleLinesV :: proc(center: Vector2, radius: f32, color: Color) --- // Draw circle outline (Vector version) DrawEllipse :: proc(centerX, centerY: c.int, radiusH, radiusV: f32, color: Color) --- // Draw ellipse + DrawEllipseV :: proc(center: Vector2, radiusH, radiusV: f32, color: Color) --- // Draw ellipse (Vector version) DrawEllipseLines :: proc(centerX, centerY: c.int, radiusH, radiusV: f32, color: Color) --- // Draw ellipse outline + DrawEllipseLinesV :: proc(center: Vector2, radiusH, radiusV: f32, color: Color) --- // Draw ellipse outline (Vector version) DrawRing :: proc(center: Vector2, innerRadius, outerRadius: f32, startAngle, endAngle: f32, segments: c.int, color: Color) --- // Draw ring DrawRingLines :: proc(center: Vector2, innerRadius, outerRadius: f32, startAngle, endAngle: f32, segments: c.int, color: Color) --- // Draw ring outline DrawRectangle :: proc(posX, posY: c.int, width, height: c.int, color: Color) --- // Draw a color-filled rectangle @@ -1250,7 +1300,7 @@ foreign lib { DrawRectanglePro :: proc(rec: Rectangle, origin: Vector2, rotation: f32, color: Color) --- // Draw a color-filled rectangle with pro parameters DrawRectangleGradientV :: proc(posX, posY: c.int, width, height: c.int, top, bottom: Color) --- // Draw a vertical-gradient-filled rectangle DrawRectangleGradientH :: proc(posX, posY: c.int, width, height: c.int, left, right: Color) --- // Draw a horizontal-gradient-filled rectangle - DrawRectangleGradientEx :: proc(rec: Rectangle, topLeft, bottomLeft, topRight, bottomRight: Color) --- // Draw a gradient-filled rectangle with custom vertex colors + DrawRectangleGradientEx :: proc(rec: Rectangle, topLeft, bottomLeft, bottomRight, topRight: Color) --- // Draw a gradient-filled rectangle with custom vertex colors DrawRectangleLines :: proc(posX, posY: c.int, width, height: c.int, color: Color) --- // Draw rectangle outline DrawRectangleLinesEx :: proc(rec: Rectangle, lineThick: f32, color: Color) --- // Draw rectangle outline with extended parameters DrawRectangleRounded :: proc(rec: Rectangle, roundness: f32, segments: c.int, color: Color) --- // Draw rectangle with rounded edges @@ -1265,6 +1315,7 @@ foreign lib { DrawPolyLinesEx :: proc(center: Vector2, sides: c.int, radius: f32, rotation: f32, lineThick: f32, color: Color) --- // Draw a polygon outline of n sides with extended parameters // Splines drawing functions + DrawSplineLinear :: proc(points: [^]Vector2, pointCount: c.int, thick: f32, color: Color) --- // Draw spline: Linear, minimum 2 points DrawSplineBasis :: proc(points: [^]Vector2, pointCount: c.int, thick: f32, color: Color) --- // Draw spline: B-Spline, minimum 4 points DrawSplineCatmullRom :: proc(points: [^]Vector2, pointCount: c.int, thick: f32, color: Color) --- // Draw spline: Catmull-Rom, minimum 4 points @@ -1277,12 +1328,15 @@ foreign lib { DrawSplineSegmentBezierCubic :: proc(p1, c2, c3, p4: Vector2, thick: f32, color: Color) --- // Draw spline segment: Cubic Bezier, 2 points, 2 control points // Spline segment point evaluation functions, for a given t [0.0f .. 1.0f] + GetSplinePointLinear :: proc(startPos, endPos: Vector2, t: f32) -> Vector2 --- // Get (evaluate) spline point: Linear GetSplinePointBasis :: proc(p1, p2, p3, p4: Vector2, t: f32) -> Vector2 --- // Get (evaluate) spline point: B-Spline GetSplinePointCatmullRom :: proc(p1, p2, p3, p4: Vector2, t: f32) -> Vector2 --- // Get (evaluate) spline point: Catmull-Rom GetSplinePointBezierQuad :: proc(p1, c2, p3: Vector2, t: f32) -> Vector2 --- // Get (evaluate) spline point: Quadratic Bezier GetSplinePointBezierCubic :: proc(p1, c2, c3, p4: Vector2, t: f32) -> Vector2 --- // Get (evaluate) spline point: Cubic Bezier - // Basic shapes collision detection functions + + // Basic shapes collision detection functions + CheckCollisionRecs :: proc(rec1, rec2: Rectangle) -> bool --- // Check collision between two rectangles CheckCollisionCircles :: proc(center1: Vector2, radius1: f32, center2: Vector2, radius2: f32) -> bool --- // Check collision between two circles CheckCollisionCircleRec :: proc(center: Vector2, radius: f32, rec: Rectangle) -> bool --- // Check collision between circle and rectangle @@ -1295,7 +1349,9 @@ foreign lib { CheckCollisionLines :: proc(startPos1, endPos1, startPos2, endPos2: Vector2, collisionPoint: [^]Vector2) -> bool --- // Check the collision between two lines defined by two points each, returns collision point by reference GetCollisionRec :: proc(rec1, rec2: Rectangle) -> Rectangle --- // Get collision rectangle for two rectangles collision - + //------------------------------------------------------------------------------------ + // Texture Loading and Drawing Functions (Module: textures) + //------------------------------------------------------------------------------------ // Image loading functions // NOTE: These functions do not require GPU access @@ -1410,7 +1466,8 @@ foreign lib { SetTextureFilter :: proc(texture: Texture2D, filter: TextureFilter) --- // Set texture scaling filter mode SetTextureWrap :: proc(texture: Texture2D, wrap: TextureWrap) --- // Set texture wrapping mode - // Texture drawing functions + // Texture drawing functions + DrawTexture :: proc(texture: Texture2D, posX, posY: c.int, tint: Color) --- // Draw a Texture2D DrawTextureV :: proc(texture: Texture2D, position: Vector2, tint: Color) --- // Draw a Texture2D with position defined as Vector2 DrawTextureEx :: proc(texture: Texture2D, position: Vector2, rotation: f32, scale: f32, tint: Color) --- // Draw a Texture2D with extended parameters @@ -1439,26 +1496,23 @@ foreign lib { SetPixelColor :: proc(dstPtr: rawptr, color: Color, format: PixelFormat) --- // Set color formatted into destination pixel pointer GetPixelDataSize :: proc(width, height: c.int, format: PixelFormat) -> c.int --- // Get pixel data size in bytes for certain format - - - //------------------------------------------------------------------------------------ // Font Loading and Text Drawing Functions (Module: text) //------------------------------------------------------------------------------------ // Font loading/unloading functions - GetFontDefault :: proc() -> Font --- // Get the default Font - LoadFont :: proc(fileName: cstring) -> Font --- // Load font from file into GPU memory (VRAM) - LoadFontEx :: proc(fileName: cstring, fontSize: c.int, codepoints: [^]rune, codepointCount: c.int) -> Font --- // Load font from file with extended parameters, use NULL for codepoints and 0 for codepointCount to load the default character set, font size is provided in pixels height - LoadFontFromImage :: proc(image: Image, key: Color, firstChar: rune) -> Font --- // Load font from Image (XNA style) - LoadFontFromMemory :: proc(fileType: cstring, fileData: rawptr, dataSize: c.int, fontSize: c.int, codepoints: [^]rune, codepointCount: c.int) -> Font --- // Load font from memory buffer, fileType refers to extension: i.e. '.ttf' - IsFontValid :: proc(font: Font) -> bool --- // Check if a font is valid (font data loaded, WARNING: GPU texture not checked) - LoadFontData :: proc(fileData: rawptr, dataSize: c.int, fontSize: c.int, codepoints: [^]rune, codepointCount: c.int, type: FontType) -> [^]GlyphInfo --- // Load font data for further use - GenImageFontAtlas :: proc(glyphs: [^]GlyphInfo, glyphRecs: ^[^]Rectangle, codepointCount: c.int, fontSize: c.int, padding: c.int, packMethod: c.int) -> Image --- // Generate image font atlas using chars info - UnloadFontData :: proc(glyphs: [^]GlyphInfo, glyphCount: c.int) --- // Unload font chars info data (RAM) - UnloadFont :: proc(font: Font) --- // Unload font from GPU memory (VRAM) - ExportFontAsCode :: proc(font: Font, fileName: cstring) -> bool --- // Export font as code file, returns true on success + GetFontDefault :: proc() -> Font --- // Get the default Font + LoadFont :: proc(fileName: cstring) -> Font --- // Load font from file into GPU memory (VRAM) + LoadFontEx :: proc(fileName: cstring, fontSize: c.int, codepoints: [^]rune, codepointCount: c.int) -> Font --- // Load font from file with extended parameters, use NULL for codepoints and 0 for codepointCount to load the default character set, font size is provided in pixels height + LoadFontFromImage :: proc(image: Image, key: Color, firstChar: rune) -> Font --- // Load font from Image (XNA style) + LoadFontFromMemory :: proc(fileType: cstring, fileData: rawptr, dataSize: c.int, fontSize: c.int, codepoints: [^]rune, codepointCount: c.int) -> Font --- // Load font from memory buffer, fileType refers to extension: i.e. '.ttf' + IsFontValid :: proc(font: Font) -> bool --- // Check if a font is valid (font data loaded, WARNING: GPU texture not checked) + LoadFontData :: proc(fileData: rawptr, dataSize: c.int, fontSize: c.int, codepoints: [^]rune, codepointCount: c.int, type: FontType, glyphCount: ^c.int) -> [^]GlyphInfo --- // Load font data for further use + GenImageFontAtlas :: proc(glyphs: [^]GlyphInfo, glyphRecs: ^[^]Rectangle, codepointCount: c.int, fontSize: c.int, padding: c.int, packMethod: c.int) -> Image --- // Generate image font atlas using chars info + UnloadFontData :: proc(glyphs: [^]GlyphInfo, glyphCount: c.int) --- // Unload font chars info data (RAM) + UnloadFont :: proc(font: Font) --- // Unload font from GPU memory (VRAM) + ExportFontAsCode :: proc(font: Font, fileName: cstring) -> bool --- // Export font as code file, returns true on success // Text drawing functions @@ -1467,16 +1521,17 @@ foreign lib { DrawTextEx :: proc(font: Font, text: cstring, position: Vector2, fontSize: f32, spacing: f32, tint: Color) --- // Draw text using font and additional parameters DrawTextPro :: proc(font: Font, text: cstring, position, origin: Vector2, rotation: f32, fontSize: f32, spacing: f32, tint: Color) --- // Draw text using Font and pro parameters (rotation) DrawTextCodepoint :: proc(font: Font, codepoint: rune, position: Vector2, fontSize: f32, tint: Color) --- // Draw one character (codepoint) - DrawTextCodepoints :: proc(font: Font, codepoints: [^]rune, codepointCount: c.int, position: Vector2, fontSize: f32, spacing: f32, tint: Color) --- // Draw multiple character (codepoint) + DrawTextCodepoints :: proc(font: Font, codepoints: [^]rune, codepointCount: c.int, position: Vector2, fontSize: f32, spacing: f32, tint: Color) --- // Draw multiple character (codepoint) // Text font info functions - SetTextLineSpacing :: proc(spacing: c.int) --- // Set vertical line spacing when drawing with line-breaks - MeasureText :: proc(text: cstring, fontSize: c.int) -> c.int --- // Measure string width for default font - MeasureTextEx :: proc(font: Font, text: cstring, fontSize: f32, spacing: f32) -> Vector2 --- // Measure string size for Font - GetGlyphIndex :: proc(font: Font, codepoint: rune) -> c.int --- // Get glyph index position in font for a codepoint (unicode character), fallback to '?' if not found - GetGlyphInfo :: proc(font: Font, codepoint: rune) -> GlyphInfo --- // Get glyph font info data for a codepoint (unicode character), fallback to '?' if not found - GetGlyphAtlasRec :: proc(font: Font, codepoint: rune) -> Rectangle --- // Get glyph rectangle in font atlas for a codepoint (unicode character), fallback to '?' if not found + SetTextLineSpacing :: proc(spacing: c.int) --- // Set vertical line spacing when drawing with line-breaks + MeasureText :: proc(text: cstring, fontSize: c.int) -> c.int --- // Measure string width for default font + MeasureTextEx :: proc(font: Font, text: cstring, fontSize: f32, spacing: f32) -> Vector2 --- // Measure string size for Font + MeasureTextCodepoints :: proc(font: Font, codepoints: [^]c.int, length: c.int, fontSize, spacing: f32) -> Vector2 --- // Measure string size for an existing array of codepoints for Font + GetGlyphIndex :: proc(font: Font, codepoint: rune) -> c.int --- // Get glyph index position in font for a codepoint (unicode character), fallback to '?' if not found + GetGlyphInfo :: proc(font: Font, codepoint: rune) -> GlyphInfo --- // Get glyph font info data for a codepoint (unicode character), fallback to '?' if not found + GetGlyphAtlasRec :: proc(font: Font, codepoint: rune) -> Rectangle --- // Get glyph rectangle in font atlas for a codepoint (unicode character), fallback to '?' if not found // Text codepoints management functions (unicode characters) @@ -1491,29 +1546,37 @@ foreign lib { CodepointToUTF8 :: proc(codepoint: rune, utf8Size: ^c.int) -> cstring --- // Encode one codepoint into UTF-8 byte array (array length returned as parameter) // Text strings management functions (no UTF-8 strings, only byte chars) - // NOTE: Some strings allocate memory internally for returned strings, just be careful! + // WARNING 1: Most of these functions use internal static buffers[], it's recommended to store returned data on user-side for re-use + // WARNING 2: Some functions allocate memory internally for the returned strings, those strings must be freed by user using MemFree() - TextCopy :: proc(dst: [^]byte, src: cstring) -> c.int --- // Copy one string to another, returns bytes copied - TextIsEqual :: proc(text1, text2: cstring) -> bool --- // Check if two text string are equal - TextLength :: proc(text: cstring) -> c.uint --- // Get text length, checks for '\0' ending + LoadTextLines :: proc(text: cstring, count: ^c.int) -> [^]cstring --- // Load text as separate lines ('\n') + UnloadTextLines :: proc(text: [^]cstring, lineCount: c.int) --- // Unload text lines + TextCopy :: proc(dst: [^]byte, src: cstring) -> c.int --- // Copy one string to another, returns bytes copied + TextIsEqual :: proc(text1, text2: cstring) -> bool --- // Check if two text string are equal + TextLength :: proc(text: cstring) -> c.uint --- // Get text length, checks for '\0' ending // TextFormat is defined at the bottom of this file - TextSubtext :: proc(text: cstring, position: c.int, length: c.int) -> cstring --- // Get a piece of a text string - TextReplace :: proc(text: [^]byte, replace, by: cstring) -> [^]byte --- // Replace text string (WARNING: memory must be freed!) - TextInsert :: proc(text, insert: cstring, position: c.int) -> [^]byte --- // Insert text in a position (WARNING: memory must be freed!) - TextJoin :: proc(textList: [^]cstring, count: c.int, delimiter: cstring) -> cstring --- // Join text strings with delimiter - TextSplit :: proc(text: cstring, delimiter: byte, count: ^c.int) -> [^]cstring --- // Split text into multiple strings - TextAppend :: proc(text: [^]byte, append: cstring, position: ^c.int) --- // Append text at specific position and move cursor! - TextFindIndex :: proc(text, find: cstring) -> c.int --- // Find first text occurrence within a string - TextToUpper :: proc(text: cstring) -> cstring --- // Get upper case version of provided string - TextToLower :: proc(text: cstring) -> cstring --- // Get lower case version of provided string - TextToPascal :: proc(text: cstring) -> cstring --- // Get Pascal case notation version of provided string - TextToSnake :: proc(text: cstring) -> cstring --- // Get Snake case notation version of provided string - TextToCamel :: proc(text: cstring) -> cstring --- // Get Camel case notation version of provided string - - TextToInteger :: proc(text: cstring) -> c.int --- // Get integer value from text (negative values not supported) - TextToFloat :: proc(text: cstring) -> f32 --- // Get float value from text (negative values not supported) + TextSubtext :: proc(text: cstring, position: c.int, length: c.int) -> cstring --- // Get a piece of a text string + TextRemoveSpaces :: proc(text: cstring) -> cstring --- // Remove text spaces, concat words + GetTextBetween :: proc(text, begin, end: cstring) -> [^]byte --- // Get text between two strings + TextReplace :: proc(text, search, replacement: cstring) -> [^]byte --- // Replace text string with new string + TextReplaceAlloc :: proc(text, search, replacement: cstring) -> [^]byte --- // Replace text string with new string, memory must be MemFree() + TextReplaceBetween :: proc(text, begin, end, replacement: cstring) -> [^]byte --- // Replace text between two specific strings + TextReplaceBetweenAlloc :: proc(text, begin, end, replacement: cstring) -> [^]byte --- // Replace text between two specific strings, memory must be MemFree() + TextInsert :: proc(text, insert: cstring, position: c.int) -> [^]byte --- // Insert text in a position (WARNING: memory must be freed!) + TextInsertAlloc :: proc(text: cstring, insert: cstring, position: c.int) -> [^]byte --- // Insert text in a defined byte position, memory must be MemFree() + TextJoin :: proc(textList: [^]cstring, count: c.int, delimiter: cstring) -> cstring --- // Join text strings with delimiter + TextSplit :: proc(text: cstring, delimiter: byte, count: ^c.int) -> [^]cstring --- // Split text into multiple strings + TextAppend :: proc(text: [^]byte, append: cstring, position: ^c.int) --- // Append text at specific position and move cursor! + TextFindIndex :: proc(text, find: cstring) -> c.int --- // Find first text occurrence within a string + TextToUpper :: proc(text: cstring) -> cstring --- // Get upper case version of provided string + TextToLower :: proc(text: cstring) -> cstring --- // Get lower case version of provided string + TextToPascal :: proc(text: cstring) -> cstring --- // Get Pascal case notation version of provided string + TextToSnake :: proc(text: cstring) -> cstring --- // Get Snake case notation version of provided string + TextToCamel :: proc(text: cstring) -> cstring --- // Get Camel case notation version of provided string + TextToInteger :: proc(text: cstring) -> c.int --- // Get integer value from text (negative values not supported) + TextToFloat :: proc(text: cstring) -> f32 --- // Get float value from text (negative values not supported) //------------------------------------------------------------------------------------ @@ -1562,8 +1625,6 @@ foreign lib { DrawModelEx :: proc(model: Model, position: Vector3, rotationAxis: Vector3, rotationAngle: f32, scale: Vector3, tint: Color) --- // Draw a model with extended parameters DrawModelWires :: proc(model: Model, position: Vector3, scale: f32, tint: Color) --- // Draw a model wires (with texture if set) DrawModelWiresEx :: proc(model: Model, position: Vector3, rotationAxis: Vector3, rotationAngle: f32, scale: Vector3, tint: Color) --- // Draw a model wires (with texture if set) with extended parameters - DrawModelPoints :: proc(model: Model, position: Vector3, scale: f32, tint: Color) --- // Draw a model as points - DrawModelPointsEx :: proc(model: Model, position: Vector3, rotationAxis: Vector3, rotationAngle: f32, scale: Vector3, tint: Color) --- // Draw a model as points with extended parameters DrawBoundingBox :: proc(box: BoundingBox, color: Color) --- // Draw bounding box (wires) DrawBillboard :: proc(camera: Camera, texture: Texture2D, position: Vector3, scale: f32, tint: Color) --- // Draw a billboard texture DrawBillboardRec :: proc(camera: Camera, texture: Texture2D, source: Rectangle, position: Vector3, size: Vector2, tint: Color) --- // Draw a billboard texture defined by source @@ -1606,12 +1667,11 @@ foreign lib { // Model animations loading/unloading functions - LoadModelAnimations :: proc(fileName: cstring, animCount: ^c.int) -> [^]ModelAnimation --- // Load model animations from file - UpdateModelAnimation :: proc(model: Model, anim: ModelAnimation, frame: c.int) --- // Update model animation pose (CPU) - UpdateModelAnimationBones :: proc(model: Model, anim: ModelAnimation, frame: c.int) --- // Update model animation mesh bone matrices (GPU skinning) - UnloadModelAnimation :: proc(anim: ModelAnimation) --- // Unload animation data - UnloadModelAnimations :: proc(animations: [^]ModelAnimation, animCount: c.int) --- // Unload animation array data - IsModelAnimationValid :: proc(model: Model, anim: ModelAnimation) -> bool --- // Check model animation skeleton match + LoadModelAnimations :: proc(fileName: cstring, animCount: ^c.int) -> [^]ModelAnimation --- // Load model animations from file + UpdateModelAnimation :: proc(model: Model, anim: ModelAnimation, frame: f32) --- // Update model animation pose (vertex buffers and bone matrices) + UpdateModelAnimationEx :: proc(model: Model, animA: ModelAnimation, frameA: f32, animB: ModelAnimation, frameB: f32, blend: f32) --- // Update model animation pose, blending two animations + UnloadModelAnimations :: proc(animations: [^]ModelAnimation, animCount: c.int) --- // Unload animation array data + IsModelAnimationValid :: proc(model: Model, anim: ModelAnimation) -> bool --- // Check model animation skeleton match // Collision detection functions @@ -1640,7 +1700,7 @@ foreign lib { LoadWave :: proc(fileName: cstring) -> Wave --- // Load wave data from file LoadWaveFromMemory :: proc(fileType: cstring, fileData: rawptr, dataSize: c.int) -> Wave --- // Load wave from memory buffer, fileType refers to extension: i.e. '.wav' - IsWaveValid :: proc(wave: Wave) -> bool --- // Checks if wave data is // Checks if wave data is valid (data loaded and parameters) + IsWaveValid :: proc(wave: Wave) -> bool --- // Checks if wave data is valid (data loaded and parameters) LoadSound :: proc(fileName: cstring) -> Sound --- // Load sound from file LoadSoundFromWave :: proc(wave: Wave) -> Sound --- // Load sound from wave data LoadSoundAlias :: proc(source: Sound) -> Sound --- // Create a new sound that shares the same sample data as the source sound, does not own the sound data @@ -1661,7 +1721,7 @@ foreign lib { IsSoundPlaying :: proc(sound: Sound) -> bool --- // Check if a sound is currently playing SetSoundVolume :: proc(sound: Sound, volume: f32) --- // Set volume for a sound (1.0 is max level) SetSoundPitch :: proc(sound: Sound, pitch: f32) --- // Set pitch for a sound (1.0 is base level) - SetSoundPan :: proc(sound: Sound, pan: f32) --- // Set pan for a sound (0.5 is center) + SetSoundPan :: proc(sound: Sound, pan: f32) --- // Set pan for a sound (-1.0 left, 0.0 center, 1.0 right) WaveCopy :: proc(wave: Wave) -> Wave --- // Copy a wave to a new wave WaveCrop :: proc(wave: ^Wave, initFrame, finalFrame: c.int) --- // Crop a wave to defined samples range WaveFormat :: proc(wave: ^Wave, sampleRate, sampleSize: c.int, channels: c.int) --- // Convert wave data to desired format @@ -1684,7 +1744,7 @@ foreign lib { SeekMusicStream :: proc(music: Music, position: f32) --- // Seek music to a position (in seconds) SetMusicVolume :: proc(music: Music, volume: f32) --- // Set volume for music (1.0 is max level) SetMusicPitch :: proc(music: Music, pitch: f32) --- // Set pitch for a music (1.0 is base level) - SetMusicPan :: proc(music: Music, pan: f32) --- // Set pan for a music (0.5 is center) + SetMusicPan :: proc(music: Music, pan: f32) --- // Set pan for a music (-1.0 left, 0.0 center, 1.0 right) GetMusicTimeLength :: proc(music: Music) -> f32 --- // Get music time length (in seconds) GetMusicTimePlayed :: proc(music: Music) -> f32 --- // Get current music time played (in seconds) @@ -1722,7 +1782,6 @@ IsGestureDetected :: proc "c" (gesture: Gesture) -> bool { return IsGestureDetected({gesture}) } - // Text formatting with variables (sprintf style) TextFormat :: proc(text: cstring, args: ..any) -> cstring { @static buffers: [MAX_TEXTFORMAT_BUFFERS][MAX_TEXT_BUFFER_LENGTH]byte diff --git a/vendor/raylib/raymath.odin b/vendor/raylib/raymath.odin index 9874d5086..98eb5c03e 100644 --- a/vendor/raylib/raymath.odin +++ b/vendor/raylib/raymath.odin @@ -99,6 +99,11 @@ Vector2LengthSqr :: proc "c" (v: Vector2) -> f32 { Vector2DotProduct :: proc "c" (v1, v2: Vector2) -> f32 { return linalg.dot(v1, v2) } +// Calculate two vectors dot product +@(require_results) +Vector2CrossProduct :: proc "c" (v1, v2: Vector2) -> f32 { + return linalg.cross(v1, v2) +} // Calculate distance between two vectors @(require_results) Vector2Distance :: proc "c" (v1, v2: Vector2) -> f32 { @@ -106,7 +111,7 @@ Vector2Distance :: proc "c" (v1, v2: Vector2) -> f32 { } // Calculate square distance between two vectors @(require_results) -Vector2DistanceSqrt :: proc "c" (v1, v2: Vector2) -> f32 { +Vector2DistanceSqr :: proc "c" (v1, v2: Vector2) -> f32 { return linalg.length2(v2-v1) } // Calculate angle between two vectors @@ -296,7 +301,7 @@ Vector3Distance :: proc "c" (v1, v2: Vector3) -> f32 { } // Calculate square distance between two vectors @(require_results) -Vector3DistanceSqrt :: proc "c" (v1, v2: Vector3) -> f32 { +Vector3DistanceSqr :: proc "c" (v1, v2: Vector3) -> f32 { return linalg.length2(v2-v1) } // Calculate angle between two vectors @@ -587,6 +592,12 @@ MatrixMultiply :: proc "c" (left, right: Matrix) -> Matrix { return left * right } +// Multiply matrix components by value +@(require_results, deprecated="Prefer left * value") +MatrixMultiplyValue :: proc "c" (left: Matrix, value: f32) -> Matrix { + return left * Matrix(value) +} + // Get translation matrix @(require_results) MatrixTranslate :: proc "c" (x, y, z: f32) -> Matrix { @@ -821,6 +832,29 @@ QuaternionEquals :: proc "c" (p, q: Quaternion) -> bool { FloatEquals(p.w, q.w) } +@(require_results) +MatrixCompose :: proc "c" (translation: Vector3, rotation: Quaternion, scale: Vector3) -> Matrix { + // Initialize and scale vectors + right := Vector3 {1, 0, 0} * scale.x + up := Vector3 {0, 1, 0} * scale.y + forward := Vector3 {0, 0, 1} * scale.z + + // Rotate vectors + right = Vector3RotateByQuaternion(right, rotation) + up = Vector3RotateByQuaternion(up, rotation) + forward = Vector3RotateByQuaternion(forward, rotation) + + // Set result matrix output + result := Matrix { + right.x, up.x, forward.x, translation.x, + right.y, up.y, forward.y, translation.y, + right.z, up.z, forward.z, translation.z, + 0, 0, 0, 1, + } + + return result +} + @(private, require_results) fmaxf :: proc "contextless" (x, y: f32) -> f32 { if math.is_nan(x) { diff --git a/vendor/raylib/rlgl/rlgl.odin b/vendor/raylib/rlgl/rlgl.odin index a08479680..d1655d363 100644 --- a/vendor/raylib/rlgl/rlgl.odin +++ b/vendor/raylib/rlgl/rlgl.odin @@ -1,26 +1,27 @@ /********************************************************************************************** * -* rlgl v5.0 - A multi-OpenGL abstraction layer with an immediate-mode style API +* rlgl v6.0 - A multi-OpenGL abstraction layer with an immediate-mode style API * * DESCRIPTION: -* An abstraction layer for multiple OpenGL versions (1.1, 2.1, 3.3 Core, 4.3 Core, ES 2.0) +* An abstraction layer for multiple OpenGL versions (1.1, 2.1, 3.3 Core, 4.3 Core, ES 2.0, ES 3.0) * that provides a pseudo-OpenGL 1.1 immediate-mode style API (rlVertex, rlTranslate, rlRotate...) * * ADDITIONAL NOTES: -* When choosing an OpenGL backend different than OpenGL 1.1, some internal buffer are -* initialized on rlglInit() to accumulate vertex data. +* When choosing an OpenGL backend different than OpenGL 1.1, some internal buffers are +* initialized on rlglInit() to accumulate vertex data * -* When an internal state change is required all the stored vertex data is renderer in batch, -* additionally, rlDrawRenderBatchActive() could be called to force flushing of the batch. +* When an internal state change is required all the stored vertex data is rendered in a batch, +* additionally, rlDrawRenderBatchActive() could be called to force flushing of the batch * * Some resources are also loaded for convenience, here the complete list: * - Default batch (RLGL.defaultBatch): RenderBatch system to accumulate vertex data * - Default texture (RLGL.defaultTextureId): 1x1 white pixel R8G8B8A8 * - Default shader (RLGL.State.defaultShaderId, RLGL.State.defaultShaderLocs) * -* Internal buffer (and resources) must be manually unloaded calling rlglClose(). +* Internal buffer (and resources) must be manually unloaded calling rlglClose() * * CONFIGURATION: +* #define GRAPHICS_API_OPENGL_SOFTWARE * #define GRAPHICS_API_OPENGL_11 * #define GRAPHICS_API_OPENGL_21 * #define GRAPHICS_API_OPENGL_33 @@ -28,52 +29,51 @@ * #define GRAPHICS_API_OPENGL_ES2 * #define GRAPHICS_API_OPENGL_ES3 * Use selected OpenGL graphics backend, should be supported by platform -* Those preprocessor defines are only used on rlgl module, if OpenGL version is +* Those preprocessor defines are only used on the rlgl module, if OpenGL version is * required by any other module, use rlGetVersion() to check it * * #define RLGL_IMPLEMENTATION -* Generates the implementation of the library into the included file. +* Generates the implementation of the library into the included file * If not defined, the library is in header only mode and can be included in other headers -* or source files without problems. But only ONE file should hold the implementation. +* or source files without problems. But only ONE file should hold the implementation * -* #define RLGL_RENDER_TEXTURES_HINT -* Enable framebuffer objects (fbo) support (enabled by default) -* Some GPUs could not support them despite the OpenGL version -* -* #define RLGL_SHOW_GL_DETAILS_INFO +* #if RLGL_SHOW_GL_DETAILS_INFO * Show OpenGL extensions and capabilities detailed logs on init * -* #define RLGL_ENABLE_OPENGL_DEBUG_CONTEXT +* #if RLGL_ENABLE_OPENGL_DEBUG_CONTEXT * Enable debug context (only available on OpenGL 4.3) * -* rlgl capabilities could be customized just defining some internal +* rlgl capabilities could be customized defining some internal * values before library inclusion (default values listed): * * #define RL_DEFAULT_BATCH_BUFFER_ELEMENTS 8192 // Default internal render batch elements limits * #define RL_DEFAULT_BATCH_BUFFERS 1 // Default number of batch buffers (multi-buffering) * #define RL_DEFAULT_BATCH_DRAWCALLS 256 // Default number of batch draw calls (by state changes: mode, texture) -* #define RL_DEFAULT_BATCH_MAX_TEXTURE_UNITS 4 // Maximum number of textures units that can be activated on batch drawing (SetShaderValueTexture()) +* #define RL_DEFAULT_BATCH_MAX_TEXTURE_UNITS 4 // Maximum number of texture units that can be activated on batch drawing (SetShaderValueTexture()) * * #define RL_MAX_MATRIX_STACK_SIZE 32 // Maximum size of internal Matrix stack * #define RL_MAX_SHADER_LOCATIONS 32 // Maximum number of shader locations supported -* #define RL_CULL_DISTANCE_NEAR 0.01 // Default projection matrix near cull distance -* #define RL_CULL_DISTANCE_FAR 1000.0 // Default projection matrix far cull distance +* #define RL_CULL_DISTANCE_NEAR 0.05 // Default projection matrix near cull distance +* #define RL_CULL_DISTANCE_FAR 4000.0 // Default projection matrix far cull distance * * When loading a shader, the following vertex attributes and uniform * location names are tried to be set automatically: * -* #define RL_DEFAULT_SHADER_ATTRIB_NAME_POSITION "vertexPosition" // Bound by default to shader location: 0 -* #define RL_DEFAULT_SHADER_ATTRIB_NAME_TEXCOORD "vertexTexCoord" // Bound by default to shader location: 1 -* #define RL_DEFAULT_SHADER_ATTRIB_NAME_NORMAL "vertexNormal" // Bound by default to shader location: 2 -* #define RL_DEFAULT_SHADER_ATTRIB_NAME_COLOR "vertexColor" // Bound by default to shader location: 3 -* #define RL_DEFAULT_SHADER_ATTRIB_NAME_TANGENT "vertexTangent" // Bound by default to shader location: 4 -* #define RL_DEFAULT_SHADER_ATTRIB_NAME_TEXCOORD2 "vertexTexCoord2" // Bound by default to shader location: 5 +* #define RL_DEFAULT_SHADER_ATTRIB_NAME_POSITION "vertexPosition" // Bound by default to shader location: RL_DEFAULT_SHADER_ATTRIB_LOCATION_POSITION +* #define RL_DEFAULT_SHADER_ATTRIB_NAME_TEXCOORD "vertexTexCoord" // Bound by default to shader location: RL_DEFAULT_SHADER_ATTRIB_LOCATION_TEXCOORD +* #define RL_DEFAULT_SHADER_ATTRIB_NAME_NORMAL "vertexNormal" // Bound by default to shader location: RL_DEFAULT_SHADER_ATTRIB_LOCATION_NORMAL +* #define RL_DEFAULT_SHADER_ATTRIB_NAME_COLOR "vertexColor" // Bound by default to shader location: RL_DEFAULT_SHADER_ATTRIB_LOCATION_COLOR +* #define RL_DEFAULT_SHADER_ATTRIB_NAME_TANGENT "vertexTangent" // Bound by default to shader location: RL_DEFAULT_SHADER_ATTRIB_LOCATION_TANGENT +* #define RL_DEFAULT_SHADER_ATTRIB_NAME_TEXCOORD2 "vertexTexCoord2" // Bound by default to shader location: RL_DEFAULT_SHADER_ATTRIB_LOCATION_TEXCOORD2 +* #define RL_DEFAULT_SHADER_ATTRIB_NAME_BONEINDICES "vertexBoneIndices" // Bound by default to shader location: RL_DEFAULT_SHADER_ATTRIB_LOCATION_BONEINDICES +* #define RL_DEFAULT_SHADER_ATTRIB_NAME_BONEWEIGHTS "vertexBoneWeights" // Bound by default to shader location: RL_DEFAULT_SHADER_ATTRIB_LOCATION_BONEWEIGHTS * #define RL_DEFAULT_SHADER_UNIFORM_NAME_MVP "mvp" // model-view-projection matrix * #define RL_DEFAULT_SHADER_UNIFORM_NAME_VIEW "matView" // view matrix * #define RL_DEFAULT_SHADER_UNIFORM_NAME_PROJECTION "matProjection" // projection matrix * #define RL_DEFAULT_SHADER_UNIFORM_NAME_MODEL "matModel" // model matrix -* #define RL_DEFAULT_SHADER_UNIFORM_NAME_NORMAL "matNormal" // normal matrix (transpose(inverse(matModelView)) +* #define RL_DEFAULT_SHADER_UNIFORM_NAME_NORMAL "matNormal" // normal matrix (transpose(inverse(matModelView))) * #define RL_DEFAULT_SHADER_UNIFORM_NAME_COLOR "colDiffuse" // color diffuse (base tint color, multiplied by texture color) +* #define RL_DEFAULT_SHADER_UNIFORM_NAME_BONEMATRICES "boneMatrices" // bone matrices * #define RL_DEFAULT_SHADER_SAMPLER2D_NAME_TEXTURE0 "texture0" // texture0 (texture slot active 0) * #define RL_DEFAULT_SHADER_SAMPLER2D_NAME_TEXTURE1 "texture1" // texture1 (texture slot active 1) * #define RL_DEFAULT_SHADER_SAMPLER2D_NAME_TEXTURE2 "texture2" // texture2 (texture slot active 2) @@ -85,7 +85,7 @@ * * LICENSE: zlib/libpng * -* Copyright (c) 2014-2023 Ramon Santamaria (@raysan5) +* Copyright (c) 2014-2026 Ramon Santamaria (@raysan5) * * This software is provided "as-is", without any express or implied warranty. In no event * will the authors be held liable for any damages arising from the use of this software. @@ -110,7 +110,7 @@ package rlgl import "core:c" import rl "../." -VERSION :: "5.0" +VERSION :: "6.0" RAYLIB_SHARED :: #config(RAYLIB_SHARED, false) RAYLIB_WASM_LIB :: #config(RAYLIB_WASM_LIB, "../wasm/libraylib.a") @@ -431,19 +431,17 @@ foreign lib { EnableTextureCubemap :: proc(id: c.uint) --- // Enable texture cubemap DisableTextureCubemap :: proc() --- // Disable texture cubemap TextureParameters :: proc(id: c.uint, param: c.int, value: c.int) --- // Set texture parameters (filter, wrap) - CubemapParameters :: proc(id: c.uint, param: c.int, value: c.int) --- // Set cubemap parameters (filter, wrap) + CubemapParameters :: proc(id: i32, param: c.int, value: c.int) --- // Set cubemap parameters (filter, wrap) // Shader state EnableShader :: proc(id: c.uint) --- // Enable shader program DisableShader :: proc() --- // Disable shader program // Framebuffer state - EnableFramebuffer :: proc(id: c.uint) --- // Enable render texture (fbo) - DisableFramebuffer :: proc() --- // Disable render texture (fbo), return to default framebuffer - GetActiveFramebuffer :: proc() -> c.uint --- // Get the currently active render texture (fbo), 0 for default framebuffer - ActiveDrawBuffers :: proc(count: c.int) --- // Activate multiple draw color buffers - BlitFramebuffer :: proc(srcX, srcY, srcWidth, srcHeight: c.int, dstX, dstY, dstWidth, dstHeight: c.int, bufferMask: c.int) --- // Blit active framebuffer to main framebuffer - BindFramebuffer :: proc(target, framebuffer: c.uint) --- // Bind framebuffer (FBO) + EnableFramebuffer :: proc(id: c.uint) --- // Enable render texture (fbo) + DisableFramebuffer :: proc() --- // Disable render texture (fbo), return to default framebuffer + ActiveDrawBuffers :: proc(count: c.int) --- // Activate multiple draw color buffers + BlitFramebuffer :: proc(srcX, srcY, srcWidth, srcHeight, dstX, dstY, dstWidth, dstHeight, bufferMask: c.int) --- // Blit active framebuffer to main framebuffer // General render state EnableColorBlend :: proc() --- // Enable color blending @@ -454,13 +452,12 @@ foreign lib { DisableDepthMask :: proc() --- // Disable depth write EnableBackfaceCulling :: proc() --- // Enable backface culling DisableBackfaceCulling :: proc() --- // Disable backface culling - ColorMask :: proc(r, g, b, a: bool) --- // Color mask control SetCullFace :: proc(mode: CullMode) --- // Set face culling mode EnableScissorTest :: proc() --- // Enable scissor test DisableScissorTest :: proc() --- // Disable scissor test Scissor :: proc(x, y, width, height: c.int) --- // Scissor test EnableWireMode :: proc() --- // Enable wire mode - EnablePointMode :: proc() --- // Enable point mode + EnablePointMode :: proc() --- // Enable point mode DisableWireMode :: proc() --- // Disable wire and point modes SetLineWidth :: proc(width: f32) --- // Set the line drawing width GetLineWidth :: proc() -> f32 --- // Get the line drawing width @@ -487,6 +484,7 @@ foreign lib { @(link_prefix="rlgl") Close :: proc() --- // De-initialize rlgl (buffers, shaders, textures) LoadExtensions :: proc(loader: rawptr) --- // Load OpenGL extensions (loader function required) + GetProcAddress :: proc(procName: cstring) -> rawptr --- // Get OpenGL procedure address GetVersion :: proc() -> GlVersion --- // Get current OpenGL version SetFramebufferWidth :: proc(width: c.int) --- // Set current framebuffer width GetFramebufferWidth :: proc() -> c.int --- // Get default framebuffer width @@ -506,7 +504,7 @@ foreign lib { DrawRenderBatch :: proc(batch: ^RenderBatch) --- // Draw render batch data (Update->Draw->Reset) SetRenderBatchActive :: proc(batch: ^RenderBatch) --- // Set the active render batch for rlgl (NULL for default internal) DrawRenderBatchActive :: proc() --- // Update and draw internal render batch - CheckRenderBatchLimit :: proc(vCount: c.int) -> bool --- // Check internal buffer overflow for a given number of vertex + CheckRenderBatchLimit :: proc(vCount: c.int) -> c.int --- // Check internal buffer overflow for a given number of vertex SetTexture :: proc(id: c.uint) --- // Set current texture for render batch and check buffers limits @@ -531,7 +529,7 @@ foreign lib { // Textures management LoadTexture :: proc(data: rawptr, width, height: c.int, format: c.int, mipmapCount: c.int) -> c.uint --- // Load texture in GPU LoadTextureDepth :: proc(width, height: c.int, useRenderBuffer: bool) -> c.uint --- // Load depth texture/renderbuffer (to be attached to fbo) - LoadTextureCubemap :: proc(data: rawptr, size: c.int, format: c.int, mipmapCount: c.int) -> c.uint --- // Load texture cubemap + LoadTextureCubemap :: proc(data: rawptr, size: c.int, format: c.int) -> c.uint --- // Load texture cubemap UpdateTexture :: proc(id: c.uint, offsetX, offsetY: c.int, width, height: c.int, format: c.int, data: rawptr) --- // Update GPU texture with new data GetGlTextureFormats :: proc(format: c.int, glInternalFormat, glFormat, glType: ^c.uint) --- // Get OpenGL internal formats GetPixelFormatName :: proc(format: c.uint) -> cstring --- // Get name string for pixel format @@ -550,12 +548,12 @@ foreign lib { LoadShaderCode :: proc(vsCode, fsCode: cstring) -> c.uint --- // Load shader from code strings CompileShader :: proc(shaderCode: cstring, type: c.int) -> c.uint --- // Compile custom shader and return shader id (type: VERTEX_SHADER, FRAGMENT_SHADER, COMPUTE_SHADER) LoadShaderProgram :: proc(vShaderId, fShaderId: c.uint) -> c.uint --- // Load custom shader program + UnloadShader :: proc(id: c.uint) --- // Unload shader, loaded with rlLoadShader() UnloadShaderProgram :: proc(id: c.uint) --- // Unload shader program GetLocationUniform :: proc(shaderId: c.uint, uniformName: cstring) -> c.int --- // Get shader location uniform GetLocationAttrib :: proc(shaderId: c.uint, attribName: cstring) -> c.int --- // Get shader location attribute SetUniform :: proc(locIndex: c.int, value: rawptr, uniformType: c.int, count: c.int) --- // Set shader value uniform SetUniformMatrix :: proc(locIndex: c.int, mat: Matrix) --- // Set shader value matrix - SetUniformMatrices :: proc(locIndex: c.int, matrices: [^]Matrix, count: c.int) --- // Set shader value matrices SetUniformSampler :: proc(locIndex: c.int, textureId: c.uint) --- // Set shader value sampler SetShader :: proc(id: c.uint, locs: [^]c.int) --- // Set shader currently active (id and locations) diff --git a/vendor/raylib/wasm/libraylib.a b/vendor/raylib/wasm/libraylib.a deleted file mode 100644 index 4cdbfa694..000000000 Binary files a/vendor/raylib/wasm/libraylib.a and /dev/null differ diff --git a/vendor/raylib/wasm/libraylib.web.a b/vendor/raylib/wasm/libraylib.web.a new file mode 100644 index 000000000..d490826aa --- /dev/null +++ b/vendor/raylib/wasm/libraylib.web.a @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:e1fc3ce2afd4bf1ac08420dd9350749512d9b6e34bc05a78c9ea9d17b5f440b3 +size 1360242 diff --git a/vendor/raylib/windows/raygui.dll b/vendor/raylib/windows/raygui.dll index ca99ee7e4..93d746039 100644 Binary files a/vendor/raylib/windows/raygui.dll and b/vendor/raylib/windows/raygui.dll differ diff --git a/vendor/raylib/windows/raygui.lib b/vendor/raylib/windows/raygui.lib index ea10fad55..292436e3f 100644 Binary files a/vendor/raylib/windows/raygui.lib and b/vendor/raylib/windows/raygui.lib differ diff --git a/vendor/raylib/windows/rayguidll.lib b/vendor/raylib/windows/rayguidll.lib index d846635a1..ba39835c2 100644 Binary files a/vendor/raylib/windows/rayguidll.lib and b/vendor/raylib/windows/rayguidll.lib differ diff --git a/vendor/raylib/windows/raylib.dll b/vendor/raylib/windows/raylib.dll index 12ea7c85e..a5dc0d3cc 100644 Binary files a/vendor/raylib/windows/raylib.dll and b/vendor/raylib/windows/raylib.dll differ diff --git a/vendor/raylib/windows/raylib.lib b/vendor/raylib/windows/raylib.lib index 736bdec69..47282542e 100644 Binary files a/vendor/raylib/windows/raylib.lib and b/vendor/raylib/windows/raylib.lib differ diff --git a/vendor/raylib/windows/raylibdll.lib b/vendor/raylib/windows/raylibdll.lib index c21e0447b..da40d85e6 100644 Binary files a/vendor/raylib/windows/raylibdll.lib and b/vendor/raylib/windows/raylibdll.lib differ diff --git a/vendor/wgpu/doc.odin b/vendor/wgpu/doc.odin index 5f72b9147..ce7aaa253 100644 --- a/vendor/wgpu/doc.odin +++ b/vendor/wgpu/doc.odin @@ -12,8 +12,8 @@ You can find a number of examples on [[Odin's official examples repository; http **Getting the wgpu-native libraries** For native support (not the browser), some libraries are required. Fortunately this is -extremely easy, just download them from the [[releases on GitHub; https://github.com/gfx-rs/wgpu-native/releases/tag/v29.0.0.0]]. -the bindings are for v29.0.0.0 at the moment. +extremely easy, just download them from the [[releases on GitHub; https://github.com/gfx-rs/wgpu-native/releases/tag/v29.0.1.1]]. +the bindings are for v29.0.1.1 at the moment. These are expected in the `lib` folder under the same name as they are released (just unzipped). By default it will look for a static release version (`wgpu-OS-ARCH-release.a|lib`), diff --git a/vendor/wgpu/lib/wgpu-windows-x86_64-msvc-release/lib/wgpu_native.dll b/vendor/wgpu/lib/wgpu-windows-x86_64-msvc-release/lib/wgpu_native.dll index 574cdda82..99abac1a9 100644 --- a/vendor/wgpu/lib/wgpu-windows-x86_64-msvc-release/lib/wgpu_native.dll +++ b/vendor/wgpu/lib/wgpu-windows-x86_64-msvc-release/lib/wgpu_native.dll @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:681c1f28050eebe67e31fc40be1361abfebf4429c181c150f0f4a5044c144fc9 -size 9099264 +oid sha256:5e26249c40dcdac43f5adca3195fcf40e4ec6c67472b38510165f411fcc198e7 +size 9117696 diff --git a/vendor/wgpu/lib/wgpu-windows-x86_64-msvc-release/lib/wgpu_native.dll.lib b/vendor/wgpu/lib/wgpu-windows-x86_64-msvc-release/lib/wgpu_native.dll.lib index 33443a527..36e47bb7e 100644 --- a/vendor/wgpu/lib/wgpu-windows-x86_64-msvc-release/lib/wgpu_native.dll.lib +++ b/vendor/wgpu/lib/wgpu-windows-x86_64-msvc-release/lib/wgpu_native.dll.lib @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:9f3818cf86365e01b203d7ddf57d134061687ded55ac7d38a117b170f78c1dea -size 61760 +oid sha256:261adb64aef86a3ebaf7f6f370d14bed083f93e25e2eec70a7c54d33954e4572 +size 62344 diff --git a/vendor/wgpu/lib/wgpu-windows-x86_64-msvc-release/lib/wgpu_native.lib b/vendor/wgpu/lib/wgpu-windows-x86_64-msvc-release/lib/wgpu_native.lib index 7c089da19..6d0f341bb 100644 --- a/vendor/wgpu/lib/wgpu-windows-x86_64-msvc-release/lib/wgpu_native.lib +++ b/vendor/wgpu/lib/wgpu-windows-x86_64-msvc-release/lib/wgpu_native.lib @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:021c502f17235ceed0d0dde197a9c10548224bd34b27a56a81aa938d8b4102a4 -size 54820956 +oid sha256:c47ed51546823c02c6d33e24a58e62e0535442ca1897a17d2305b2e55df35729 +size 54811560 diff --git a/vendor/wgpu/wgpu.js b/vendor/wgpu/wgpu.js index 671b4a258..7bdf3aa5e 100644 --- a/vendor/wgpu/wgpu.js +++ b/vendor/wgpu/wgpu.js @@ -26,7 +26,7 @@ const ENUMS = { CompareFunction: [undefined, "never", "less", "equal", "less-equal", "greater", "not-equal", "greater-equal", "always", ], TextureDimension: [undefined, "1d", "2d", "3d", ], ErrorType: [undefined, "no-error", "validation", "out-of-memory", "internal", "unknown", ], - WGSLLanguageFeatureName: [undefined, "readonly_and_readwrite_storage_textures", "packed_4x8_integer_dot_product", "unrestricted_pointer_parameters", "pointer_composite_access", "uniform_buffer_standard_layout", "subgroup_id", "texture_and_sampler_let", "subgroup_uniformity", "texture_formats_tier1" ], + WGSLLanguageFeatureName: [undefined, "readonly_and_readwrite_storage_textures", "packed_4x8_integer_dot_product", "unrestricted_pointer_parameters", "pointer_composite_access", "uniform_buffer_standard_layout", "subgroup_id", "texture_and_sampler_let", "subgroup_uniformity", "texture_formats_tier1", "linear_indexing" ], PowerPreference: [undefined, "low-power", "high-performance", ], CompositeAlphaMode: ["auto", "opaque", "premultiplied", "unpremultiplied", "inherit", ], StencilOperation: [undefined, "keep", "zero", "replace", "invert", "increment-clamp", "decrement-clamp", "increment-wrap", "decrement-wrap", ], @@ -1806,6 +1806,16 @@ class WebGPUInterface { computePassEncoder.setBindGroup(groupIndex, bindGroup, dynamicOffsets); }, + /** + * @param {number} computePassEncoderIdx + * @param {number} offset + * @param {number} data + * @param {number|BigInt} size + */ + wgpuComputePassEncoderSetImmediates: (computePassEncoderIdx, offset, data, size) => { + console.warn("wgpuComputePassEncoderSetImmediates: unimplemented in browsers at the time of writing"); + }, + /** * @param {number} computePassEncoderIdx * @param {number} pipelineIdx @@ -2696,6 +2706,16 @@ class WebGPUInterface { renderBundleEncoder.setBindGroup(groupIndex, group, dynamicOffsets); }, + /** + * @param {number} renderBundleEncoderIdx + * @param {number} offset + * @param {number} data + * @param {number|BigInt} size + */ + wgpuRenderBundleEncoderSetImmediates: (renderBundleEncoderIdx, offset, data, size) => { + console.warn("wgpuRenderBundleEncoderSetImmediates: unimplemented in browsers at the time of writing"); + }, + /** * @param {number} renderBundleEncoderIdx * @param {number} bufferIdx @@ -2893,6 +2913,16 @@ class WebGPUInterface { renderPassEncoder.setBindGroup(groupIndex, group, dynamicOffsets); }, + /** + * @param {number} renderPassEncoderIdx + * @param {number} offset + * @param {number} data + * @param {number|BigInt} size + */ + wgpuRenderPassEncoderSetImmediates: (renderPassEncoderIdx, offset, data, size) => { + console.warn("wgpuRenderPassEncoderSetImmediates: unimplemented in browsers at the time of writing"); + }, + /** * @param {number} renderPassEncoderIdx * @param {number} colorPtr diff --git a/vendor/wgpu/wgpu.odin b/vendor/wgpu/wgpu.odin index 796cebec7..1a7930c10 100644 --- a/vendor/wgpu/wgpu.odin +++ b/vendor/wgpu/wgpu.odin @@ -13,7 +13,7 @@ when ODIN_OS == .Windows { @(private) LIB :: "lib/wgpu-windows-" + ARCH + "-msvc-" + TYPE + "/lib/wgpu_native" + EXT when !#exists(LIB) { - #panic("Could not find the compiled WGPU Native library at '" + #directory + LIB + "', these can be downloaded from https://github.com/gfx-rs/wgpu-native/releases/tag/v29.0.0.0, make sure to read the docs at '" + #directory + "doc.odin'") + #panic("Could not find the compiled WGPU Native library at '" + #directory + LIB + "', these can be downloaded from https://github.com/gfx-rs/wgpu-native/releases/tag/v29.0.1.1, make sure to read the docs at '" + #directory + "doc.odin'") } @(export) @@ -39,7 +39,7 @@ when ODIN_OS == .Windows { @(private) LIB :: "lib/wgpu-macos-" + ARCH + "-" + TYPE + "/lib/libwgpu_native" + EXT when !#exists(LIB) { - #panic("Could not find the compiled WGPU Native library at '" + #directory + LIB + "', these can be downloaded from https://github.com/gfx-rs/wgpu-native/releases/tag/v29.0.0.0, make sure to read the docs at '" + #directory + "doc.odin'") + #panic("Could not find the compiled WGPU Native library at '" + #directory + LIB + "', these can be downloaded from https://github.com/gfx-rs/wgpu-native/releases/tag/v29.0.1.1, make sure to read the docs at '" + #directory + "doc.odin'") } @(export) @@ -56,7 +56,7 @@ when ODIN_OS == .Windows { @(private) LIB :: "lib/wgpu-linux-" + ARCH + "-" + TYPE + "/lib/libwgpu_native" + EXT when !#exists(LIB) { - #panic("Could not find the compiled WGPU Native library at '" + #directory + LIB + "', these can be downloaded from https://github.com/gfx-rs/wgpu-native/releases/tag/v29.0.0.0, make sure to read the docs at '" + #directory + "doc.odin'") + #panic("Could not find the compiled WGPU Native library at '" + #directory + LIB + "', these can be downloaded from https://github.com/gfx-rs/wgpu-native/releases/tag/v29.0.1.1, make sure to read the docs at '" + #directory + "doc.odin'") } @(export) @@ -123,6 +123,10 @@ AddressMode :: enum i32 { ClampToEdge = 0x00000001, Repeat = 0x00000002, MirrorRepeat = 0x00000003, + + // Native. + + ClampToBorder = 0x00000004, } BackendType :: enum i32 { @@ -309,16 +313,14 @@ FeatureName :: enum i32 { TextureCompressionAstcHdr, MappablePrimaryBuffers = 0x0003000E, BufferBindingArray, - UniformBufferAndStorageTextureArrayNonUniformIndexing, - // TODO: requires wgpu.h api change - // AddressModeClampToZero, - // AddressModeClampToBorder, + StorageTextureArrayNonUniformIndexing = 0x00030010, + AdressModeClampToZero = 0x00030011, + AdressModeClampToBorder = 0x00030012, PolygonModeLine = 0x00030013, PolygonModePoint, ConservativeRasterization, - // ClearTexture, - SpirvShaderPassthrough = 0x00030017, - // MultiView, + ClearTexture, + MultiView = 0x00030018, VertexAttribute64bit = 0x00030019, TextureFormatNv12, RayQuery = 0x0003001C, @@ -331,6 +333,37 @@ FeatureName :: enum i32 { TimestampQueryInsideEncoders, TimestampQueryInsidePasses, ShaderInt64, + ShaderFloat32Atomic, + TextureAtomic, + TextureFormatP010, + // TODO: requires wgpu.h api change + // ExternalTexture, + PipelineCache = 0x0003002B, + ShaderInt64AtomicMinMax, + ShaderInt64AtomicAllOps, + // TODO: requires wgpu.h api change + // VulkanGoogleDisplayTiming, + // VulkanExternalMemoryWin32, + TextureInt64Atomic = 0x00030030, + // TODO: not implemented yet, see https://github.com/gfx-rs/wgpu/issues/7149 + UniformBufferBindingArrays, + // TODO: requires wgpu.h api change + // MeshShader, + // RayHitVertexReturn, + // MeshShaderMultiview, + // ExtendedAccelerationStructureVertexFormat, + // PassThroughShaders, + ShaderBarycentrics = 0x00030037, + SelectiveMultiview, + // TODO: requires wgpu.h api change + // MeshShaderPoints, + MultisampleArray = 0x0003003A, + CooperativeMatrix, + ShaderPerVertex, + ShaderDrawIndex, + AccelerationStructureBindingArray, + MemoryDecorationCoherent, + MemoryDecorationVolative, } FilterMode :: enum i32 { @@ -505,9 +538,7 @@ SType :: enum i32 { // Native. DeviceExtras = 0x00030001, NativeLimits, - PipelineLayoutExtras, ShaderSourceGLSL, - SupportedLimitsExtras, InstanceExtras, BindGroupEntryExtras, BindGroupLayoutEntryExtras, @@ -515,6 +546,7 @@ SType :: enum i32 { SurfaceConfigurationExtras, SurfaceSourceSwapChainPanel, PrimitiveStateExtras, + SamplerDescriptorExtras, // Odin. SurfaceSourceCanvasHTMLSelector = 0x00040001, @@ -652,13 +684,6 @@ TextureFormat :: enum i32 { // Native. - // From FeatureName.TextureFormat16bitNorm - NativeR16Unorm = 0x00030001, - NativeR16Snorm, - Rg16Unorm, - Rg16Snorm, - Rgba16Unorm, - Rgba16Snorm, NV12, P010, } @@ -754,6 +779,7 @@ WGSLLanguageFeatureName :: enum i32 { TextureAndSamplerLet = 0x00000007, SubgroupUniformity = 0x00000008, TextureFormatsTier1 = 0x00000009, + LinearIndexing = 0x0000000A, } BufferUsage :: enum i32 { @@ -1635,6 +1661,7 @@ foreign libwgpu { ComputePassEncoderPushDebugGroup :: proc(computePassEncoder: ComputePassEncoder, groupLabel: StringView) --- @(link_name="wgpuComputePassEncoderSetBindGroup") RawComputePassEncoderSetBindGroup :: proc(computePassEncoder: ComputePassEncoder, groupIndex: u32, /* NULLABLE */ group: BindGroup, dynamicOffsetCount: uint, dynamicOffsets: /* const */ [^]u32) --- + ComputePassEncoderSetImmediates :: proc(computePassEncoder: ComputePassEncoder, offset: u32, data: rawptr, size: uint) --- ComputePassEncoderSetLabel :: proc(computePassEncoder: ComputePassEncoder, label: StringView) --- ComputePassEncoderSetPipeline :: proc(computePassEncoder: ComputePassEncoder, pipeline: ComputePipeline) --- ComputePassEncoderAddRef :: proc(computePassEncoder: ComputePassEncoder) --- @@ -1731,6 +1758,7 @@ foreign libwgpu { RenderBundleEncoderPushDebugGroup :: proc(renderBundleEncoder: RenderBundleEncoder, groupLabel: StringView) --- @(link_name="wgpuRenderBundleEncoderSetBindGroup") RawRenderBundleEncoderSetBindGroup :: proc(renderBundleEncoder: RenderBundleEncoder, groupIndex: u32, /* NULLABLE */ group: BindGroup, dynamicOffsetCount: uint, dynamicOffsets: /* const */ [^]u32) --- + RenderBundleEncoderSetImmediates :: proc(renderBundleEncoder: RenderBundleEncoder, offset: u32, data: rawptr, size: uint) --- RenderBundleEncoderSetIndexBuffer :: proc(renderBundleEncoder: RenderBundleEncoder, buffer: Buffer, format: IndexFormat, offset: u64, size: u64) --- RenderBundleEncoderSetLabel :: proc(renderBundleEncoder: RenderBundleEncoder, label: StringView) --- RenderBundleEncoderSetPipeline :: proc(renderBundleEncoder: RenderBundleEncoder, pipeline: RenderPipeline) --- @@ -1753,6 +1781,7 @@ foreign libwgpu { RenderPassEncoderPushDebugGroup :: proc(renderPassEncoder: RenderPassEncoder, groupLabel: StringView) --- @(link_name="wgpuRenderPassEncoderSetBindGroup") RawRenderPassEncoderSetBindGroup :: proc(renderPassEncoder: RenderPassEncoder, groupIndex: u32, /* NULLABLE */ group: BindGroup, dynamicOffsetCount: uint, dynamicOffsets: /* const */ [^]u32) --- + RenderPassEncoderSetImmediates :: proc(renderPassEncoder: RenderPassEncoder, offset: u32, data: rawptr, size: uint) --- RenderPassEncoderSetBlendConstant :: proc(renderPassEncoder: RenderPassEncoder, color: /* const */ ^Color) --- RenderPassEncoderSetIndexBuffer :: proc(renderPassEncoder: RenderPassEncoder, buffer: Buffer, format: IndexFormat, offset: u64, size: u64) --- RenderPassEncoderSetLabel :: proc(renderPassEncoder: RenderPassEncoder, label: StringView) --- diff --git a/vendor/wgpu/wgpu_native.odin b/vendor/wgpu/wgpu_native.odin index 0fbd6338c..0cebdf70a 100644 --- a/vendor/wgpu/wgpu_native.odin +++ b/vendor/wgpu/wgpu_native.odin @@ -27,10 +27,6 @@ foreign libwgpu { DeviceGetNativeMetalCommandQueue :: proc(device: Device) -> rawptr --- DeviceGetNativeMetalTexture :: proc(device: Device) -> rawptr --- - RenderPassEncoderSetImmediates :: proc(encoder: RenderPassEncoder, offset: u32, sizeBytes: u32, data: rawptr) --- - ComputePassEncoderSetImmediates :: proc(encoder: ComputePassEncoder, offset: u32, sizeBytes: u32, data: rawptr) --- - RenderBundleEncoderSetImmediates :: proc(encoder: RenderBundleEncoder, offset: u32, sizeBytes: u32, data: rawptr) --- - RenderPassEncoderMultiDrawIndirect :: proc(encoder: RenderPassEncoder, buffer: Buffer, offset: u64, count: u32) --- RenderPassEncoderMultiDrawIndexedIndirect :: proc(encoder: RenderPassEncoder, buffer: Buffer, offset: u64, count: u32) --- @@ -47,6 +43,10 @@ foreign libwgpu { DeviceStartGraphicsDebuggerCapture :: proc(device: Device) -> b32 --- DeviceStopGraphicsDebuggerCapture :: proc(device: Device) --- + + CommandEncoderClearTexture :: proc(commandEncoder: CommandEncoder, texture: Texture, range: /* const */ ^ImageSubresourceRange = nil) --- + + DeviceCreateShaderModuleTrusted :: proc(device: Device, descriptor: /* const */ ^ShaderModuleDescriptor, runtimeChecks: ShaderRuntimeChecks) -> ShaderModule --- } GenerateReport :: proc "c" (instance: Instance) -> (report: GlobalReport) { diff --git a/vendor/wgpu/wgpu_native_types.odin b/vendor/wgpu/wgpu_native_types.odin index 931aa96db..c0cbbf2a5 100644 --- a/vendor/wgpu/wgpu_native_types.odin +++ b/vendor/wgpu/wgpu_native_types.odin @@ -2,8 +2,8 @@ package wgpu import "base:runtime" -BINDINGS_VERSION :: [4]u8{29, 0, 0, 0} -BINDINGS_VERSION_STRING :: "29.0.0.0" +BINDINGS_VERSION :: [4]u8{29, 0, 1, 1} +BINDINGS_VERSION_STRING :: "29.0.1.1" LogLevel :: enum i32 { Off, @@ -138,14 +138,10 @@ DeviceExtras :: struct { NativeLimits :: struct { using chain: ChainedStruct, - maxImmediateSize: u32, maxNonSamplerBindings: u32, maxBindingArrayElementsPerShaderStage: u32, -} - -PipelineLayoutExtras :: struct { - using chain: ChainedStruct, - immediateDataSize: u32, + maxBindingArraySamplerElementsPerShaderStage: u32, + maxMultiviewCount: u32, } SubmissionIndex :: distinct u64 @@ -258,6 +254,36 @@ PrimitiveStateExtras :: struct { LogCallback :: #type proc "c" (level: LogLevel, message: StringView, userdata: rawptr) +ImageSubresourceRange :: struct { + aspect: TextureAspect, + baseMipLevel: u32, + mipLevelCount: u32, + baseArrayLayer: u32, + arrayLayerCount: u32, +} + +ShaderRuntimeCheck :: enum i32 { + BoundsChecks, + ForceLoopBounding, + RayQueryInitializationTracking, + TaskShaderDispatchTracking, + MeshShaderPrimitiveIndicesClamp, +} +ShaderRuntimeChecks :: bit_set[ShaderRuntimeCheck; Flags] + +SamplerBorderColor :: enum i32 { + Undefined, + TransparentBlack, + OpaqueBlack, + OpaqueWhite, + Zero, +} + +SamplerDescriptorExtras :: struct { + using chain: ChainedStruct, + samplerBoderColor: SamplerBorderColor, +} + // Wrappers ConvertOdinToWGPULogLevel :: proc(level: runtime.Logger_Level) -> LogLevel {