Deploy Kimodo ZeroGPU motion API

.gitattributes

@@ -33,3 +33,71 @@ saved_model/**/* filter=lfs diff=lfs merge=lfs -text
 *.zip filter=lfs diff=lfs merge=lfs -text
 *.zst filter=lfs diff=lfs merge=lfs -text
 *tfevents* filter=lfs diff=lfs merge=lfs -text
+assets/demo_screenshot.png filter=lfs diff=lfs merge=lfs -text
+assets/gmr_results.gif filter=lfs diff=lfs merge=lfs -text
+assets/mujoco_result.gif filter=lfs diff=lfs merge=lfs -text
+assets/protomotions_results.gif filter=lfs diff=lfs merge=lfs -text
+assets/sonic_kimodo_demo.gif filter=lfs diff=lfs merge=lfs -text
+assets/teaser.gif filter=lfs diff=lfs merge=lfs -text
+docs/source/_static/arch.png filter=lfs diff=lfs merge=lfs -text
+docs/source/_static/constraints.png filter=lfs diff=lfs merge=lfs -text
+docs/source/_static/demo/editing_mode.png filter=lfs diff=lfs merge=lfs -text
+docs/source/_static/demo/viewer.png filter=lfs diff=lfs merge=lfs -text
+docs/source/_static/overview.png filter=lfs diff=lfs merge=lfs -text
+docs/source/_static/skeletons/g1.png filter=lfs diff=lfs merge=lfs -text
+docs/source/_static/skeletons/skeletons.png filter=lfs diff=lfs merge=lfs -text
+docs/source/_static/skeletons/smplx.png filter=lfs diff=lfs merge=lfs -text
+docs/source/_static/skeletons/soma.png filter=lfs diff=lfs merge=lfs -text
+docs/source/_static/skeletons/soma_skels.png filter=lfs diff=lfs merge=lfs -text
+docs/source/_static/smoothed_root.png filter=lfs diff=lfs merge=lfs -text
+kimodo/assets/skeletons/g1skel34/meshes/g1/head_link.STL filter=lfs diff=lfs merge=lfs -text
+kimodo/assets/skeletons/g1skel34/meshes/g1/left_ankle_roll_link.STL filter=lfs diff=lfs merge=lfs -text
+kimodo/assets/skeletons/g1skel34/meshes/g1/left_hand_index_0_link.STL filter=lfs diff=lfs merge=lfs -text
+kimodo/assets/skeletons/g1skel34/meshes/g1/left_hand_index_1_link.STL filter=lfs diff=lfs merge=lfs -text
+kimodo/assets/skeletons/g1skel34/meshes/g1/left_hand_middle_0_link.STL filter=lfs diff=lfs merge=lfs -text
+kimodo/assets/skeletons/g1skel34/meshes/g1/left_hand_middle_1_link.STL filter=lfs diff=lfs merge=lfs -text
+kimodo/assets/skeletons/g1skel34/meshes/g1/left_hand_palm_link.STL filter=lfs diff=lfs merge=lfs -text
+kimodo/assets/skeletons/g1skel34/meshes/g1/left_hand_thumb_1_link.STL filter=lfs diff=lfs merge=lfs -text
+kimodo/assets/skeletons/g1skel34/meshes/g1/left_hand_thumb_2_link.STL filter=lfs diff=lfs merge=lfs -text
+kimodo/assets/skeletons/g1skel34/meshes/g1/left_hip_pitch_link.STL filter=lfs diff=lfs merge=lfs -text
+kimodo/assets/skeletons/g1skel34/meshes/g1/left_hip_roll_link.STL filter=lfs diff=lfs merge=lfs -text
+kimodo/assets/skeletons/g1skel34/meshes/g1/left_hip_yaw_link.STL filter=lfs diff=lfs merge=lfs -text
+kimodo/assets/skeletons/g1skel34/meshes/g1/left_knee_link.STL filter=lfs diff=lfs merge=lfs -text
+kimodo/assets/skeletons/g1skel34/meshes/g1/left_rubber_hand.STL filter=lfs diff=lfs merge=lfs -text
+kimodo/assets/skeletons/g1skel34/meshes/g1/left_shoulder_pitch_link.STL filter=lfs diff=lfs merge=lfs -text
+kimodo/assets/skeletons/g1skel34/meshes/g1/left_shoulder_roll_link.STL filter=lfs diff=lfs merge=lfs -text
+kimodo/assets/skeletons/g1skel34/meshes/g1/left_shoulder_yaw_link.STL filter=lfs diff=lfs merge=lfs -text
+kimodo/assets/skeletons/g1skel34/meshes/g1/left_wrist_roll_link.STL filter=lfs diff=lfs merge=lfs -text
+kimodo/assets/skeletons/g1skel34/meshes/g1/left_wrist_roll_rubber_hand.STL filter=lfs diff=lfs merge=lfs -text
+kimodo/assets/skeletons/g1skel34/meshes/g1/left_wrist_yaw_link.STL filter=lfs diff=lfs merge=lfs -text
+kimodo/assets/skeletons/g1skel34/meshes/g1/logo_link.STL filter=lfs diff=lfs merge=lfs -text
+kimodo/assets/skeletons/g1skel34/meshes/g1/pelvis.STL filter=lfs diff=lfs merge=lfs -text
+kimodo/assets/skeletons/g1skel34/meshes/g1/pelvis_contour_link.STL filter=lfs diff=lfs merge=lfs -text
+kimodo/assets/skeletons/g1skel34/meshes/g1/right_ankle_roll_link.STL filter=lfs diff=lfs merge=lfs -text
+kimodo/assets/skeletons/g1skel34/meshes/g1/right_hand_index_0_link.STL filter=lfs diff=lfs merge=lfs -text
+kimodo/assets/skeletons/g1skel34/meshes/g1/right_hand_index_1_link.STL filter=lfs diff=lfs merge=lfs -text
+kimodo/assets/skeletons/g1skel34/meshes/g1/right_hand_middle_0_link.STL filter=lfs diff=lfs merge=lfs -text
+kimodo/assets/skeletons/g1skel34/meshes/g1/right_hand_middle_1_link.STL filter=lfs diff=lfs merge=lfs -text
+kimodo/assets/skeletons/g1skel34/meshes/g1/right_hand_palm_link.STL filter=lfs diff=lfs merge=lfs -text
+kimodo/assets/skeletons/g1skel34/meshes/g1/right_hand_thumb_1_link.STL filter=lfs diff=lfs merge=lfs -text
+kimodo/assets/skeletons/g1skel34/meshes/g1/right_hand_thumb_2_link.STL filter=lfs diff=lfs merge=lfs -text
+kimodo/assets/skeletons/g1skel34/meshes/g1/right_hip_pitch_link.STL filter=lfs diff=lfs merge=lfs -text
+kimodo/assets/skeletons/g1skel34/meshes/g1/right_hip_roll_link.STL filter=lfs diff=lfs merge=lfs -text
+kimodo/assets/skeletons/g1skel34/meshes/g1/right_hip_yaw_link.STL filter=lfs diff=lfs merge=lfs -text
+kimodo/assets/skeletons/g1skel34/meshes/g1/right_knee_link.STL filter=lfs diff=lfs merge=lfs -text
+kimodo/assets/skeletons/g1skel34/meshes/g1/right_rubber_hand.STL filter=lfs diff=lfs merge=lfs -text
+kimodo/assets/skeletons/g1skel34/meshes/g1/right_shoulder_pitch_link.STL filter=lfs diff=lfs merge=lfs -text
+kimodo/assets/skeletons/g1skel34/meshes/g1/right_shoulder_roll_link.STL filter=lfs diff=lfs merge=lfs -text
+kimodo/assets/skeletons/g1skel34/meshes/g1/right_shoulder_yaw_link.STL filter=lfs diff=lfs merge=lfs -text
+kimodo/assets/skeletons/g1skel34/meshes/g1/right_wrist_roll_link.STL filter=lfs diff=lfs merge=lfs -text
+kimodo/assets/skeletons/g1skel34/meshes/g1/right_wrist_roll_rubber_hand.STL filter=lfs diff=lfs merge=lfs -text
+kimodo/assets/skeletons/g1skel34/meshes/g1/right_wrist_yaw_link.STL filter=lfs diff=lfs merge=lfs -text
+kimodo/assets/skeletons/g1skel34/meshes/g1/torso_constraint_L_link.STL filter=lfs diff=lfs merge=lfs -text
+kimodo/assets/skeletons/g1skel34/meshes/g1/torso_constraint_R_link.STL filter=lfs diff=lfs merge=lfs -text
+kimodo/assets/skeletons/g1skel34/meshes/g1/torso_link.STL filter=lfs diff=lfs merge=lfs -text
+kimodo/assets/skeletons/g1skel34/meshes/g1/torso_link_23dof_rev_1_0.STL filter=lfs diff=lfs merge=lfs -text
+kimodo/assets/skeletons/g1skel34/meshes/g1/torso_link_rev_1_0.STL filter=lfs diff=lfs merge=lfs -text
+kimodo/assets/skeletons/g1skel34/meshes/g1/waist_constraint_L.STL filter=lfs diff=lfs merge=lfs -text
+kimodo/assets/skeletons/g1skel34/meshes/g1/waist_constraint_R.STL filter=lfs diff=lfs merge=lfs -text
+kimodo/assets/skeletons/g1skel34/meshes/g1/waist_support_link.STL filter=lfs diff=lfs merge=lfs -text
+kimodo/assets/skeletons/g1skel34/meshes/g1/waist_yaw_link.STL filter=lfs diff=lfs merge=lfs -text

.gitignore

@@ -0,0 +1,160 @@
+# debugging files
+debug/
+SMPLX_NEUTRAL.npz
+
+# Byte-compiled / optimized / DLL files
+__pycache__/
+*.py[cod]
+*$py.class
+
+# C extensions
+*.so
+
+datasets
+
+# Distribution / packaging
+.Python
+build/
+develop-eggs/
+dist/
+downloads/
+eggs/
+.eggs/
+parts/
+sdist/
+var/
+wheels/
+pip-wheel-metadata/
+share/python-wheels/
+*.egg-info/
+.installed.cfg
+*.egg
+MANIFEST
+
+# PyInstaller
+#  Usually these files are written by a python script from a template
+#  before PyInstaller builds the exe, so as to inject date/other infos into it.
+*.manifest
+*.spec
+
+# Installer logs
+pip-log.txt
+pip-delete-this-directory.txt
+
+# Unit test / coverage reports
+htmlcov/
+.tox/
+.nox/
+.coverage
+.coverage.*
+.cache
+nosetests.xml
+coverage.xml
+*.cover
+*.py,cover
+.hypothesis/
+.pytest_cache/
+
+# Translations
+*.mo
+*.pot
+
+# Django stuff:
+*.log
+local_settings.py
+db.sqlite3
+db.sqlite3-journal
+
+# Flask stuff:
+instance/
+.webassets-cache
+
+# Scrapy stuff:
+.scrapy
+
+# Sphinx documentation
+docs/_build/
+
+# PyBuilder
+target/
+
+# Jupyter Notebook
+.ipynb_checkpoints
+
+# IPython
+profile_default/
+ipython_config.py
+
+# pyenv
+.python-version
+
+# pipenv
+#   According to pypa/pipenv#598, it is recommended to include Pipfile.lock in version control.
+#   However, in case of collaboration, if having platform-specific dependencies or dependencies
+#   having no cross-platform support, pipenv may install dependencies that don't work, or not
+#   install all needed dependencies.
+#Pipfile.lock
+
+# PEP 582; used by e.g. github.com/David-OConnor/pyflow
+__pypackages__/
+
+# Celery stuff
+celerybeat-schedule
+celerybeat.pid
+
+# SageMath parsed files
+*.sage.py
+
+# Environments
+.env
+.venv
+venv/
+ENV/
+env.bak/
+venv.bak/
+
+# Spyder project settings
+.spyderproject
+.spyproject
+
+# Rope project settings
+.ropeproject
+
+# mkdocs documentation
+/site
+
+# mypy
+.mypy_cache/
+.dmypy.json
+dmypy.json
+
+# Pyre type checker
+.pyre/
+
+# vscode
+.vscode
+*.code-workspace
+/pyrightconfig.json
+wandb/
+
+# others
+out
+tmr_out
+.ruff_cache
+outputs
+/debug
+/batch*.sh
+checkpoints/**/test/*
+nohup.out
+
+*.swp
+*.swo
+*.txt~*
+*.un~
+*~
+train_done
+.aider*
+onelogger.err
+
+# deploy files
+/helm-library

.pre-commit-config.yaml

@@ -0,0 +1,50 @@
+repos:
+  # code formatting
+  - repo: https://github.com/astral-sh/ruff-pre-commit
+    rev: v0.6.4
+    hooks:
+      - id: ruff
+        name: sort imports with ruff
+        args: [--select, I, --fix]
+      - id: ruff-format
+        name: format with ruff
+
+  # docstring formatting
+  - repo: https://github.com/PyCQA/docformatter
+    rev: v1.7.7
+    hooks:
+      - id: docformatter
+        args:
+          [
+            --in-place,
+            --wrap-summaries=100,
+            --wrap-descriptions=100,
+            --style=sphinx,
+          ]
+
+  # yaml formatting
+  - repo: https://github.com/pre-commit/mirrors-prettier
+    rev: v3.0.0-alpha.6
+    hooks:
+      - id: prettier
+        types: [yaml]
+        exclude: |
+          (?x)^(
+            environment\.yaml$ |
+            \.gitlab-ci\.yml$ |
+            \.k8s/.*\.(ya?ml)$
+          )
+
+  - repo: https://github.com/pre-commit/pre-commit-hooks
+    rev: v4.0.1
+    hooks:
+      - id: trailing-whitespace # Trims trailing whitespace.
+      - id: end-of-file-fixer # Makes sure files end in a newline and only a newline.
+      - id: check-yaml # Attempts to load all yaml files to verify syntax.
+        exclude: |
+          (?x)^(
+            \.gitlab-ci\.yml$ |
+            \.k8s/.*\.(ya?ml)$
+          )
+
+exclude: "checkpoints/.*"

ATTRIBUTIONS.MD

@@ -0,0 +1,48 @@
+LLM2Vec MIT License https://github.com/McGill-NLP/llm2vec Copyright (c) 2024 McGill NLP
+
+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.
+
+Unitree mujoco BSD 3-Clause License https://github.com/unitreerobotics/unitree_mujoco/blob/main/LICENSE
+Copyright (c) 2016-2024 HangZhou YuShu TECHNOLOGY CO.,LTD. ("Unitree Robotics")
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+* Redistributions of source code must retain the above copyright notice, this
+  list of conditions and the following disclaimer.
+
+* Redistributions in binary form must reproduce the above copyright notice,
+  this list of conditions and the following disclaimer in the documentation
+  and/or other materials provided with the distribution.
+
+* Neither the name of the copyright holder nor the names of its
+  contributors may be used to endorse or promote products derived from
+  this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "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 COPYRIGHT HOLDER OR CONTRIBUTORS 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.

CHANGELOG.md

@@ -0,0 +1,84 @@
+# Changelog
+
+All notable changes to this project will be documented in this file.
+
+The format is based on [Keep a Changelog](https://keepachangelog.com/en/1.1.0/).
+
+## [2026-05-03]
+
+### Fixed
+- `benchmark/parse_folder.py` now averages each metric only over the testcases that actually report it. Previously, sparse constraint metrics (`constraint_root2d_acc`, `constraint_root2d_err`, `constraint_root2d_err_p95`, `constraint_fullbody_keyframe`, `constraint_end_effector`) were divided by the total motion count of the (split, category), including testcases of other constraint kinds that did not report the metric. This silently scaled values by `motions_with_metric / total_motions` (e.g. `constraint_root2d_acc` displayed as ~0.57 when the true value was ~0.93). Both the printed table and `summary_rows.json` are affected, including the combined constraints row that merges `constraints_withtext` and `constraints_notext`. Text-following metrics, foot-skate/contact metrics, and TMR metrics are unchanged.
+- Updated Kimodo benchmark results in the documentation with this fix applied.
+
+## [2026-04-24]
+
+### Added
+- Support for `TEXT_ENCODER_DEVICE` environment variable to force LLM2Vec to use the CPU instead of GPU. Setting `TEXT_ENCODER_DEVICE=cpu` reduces VRAM usage to <3 GB with a fairly small speed hit.
+- `--save_example_dir` argument to `kimodo_gen` to save outputs to an example directory that can be directly loaded into `kimodo_demo`
+
+### Fixed
+- Bug in post-processing that was incorrectly making the smoothed root the target for the root in full-body constraints
+- Modified how transitions are handled in multi-prompt generation to improve smoothness
+
+### Removed
+- `share_transition` and `percentage_transition_override` options from python API for multi-prompt generation
+
+## [2026-04-13]
+
+### Added
+- Option `--bvh_standard_tpose` to use standard T-pose for BVH file saved from `generate.py`
+- Option to use standard T-pose for BVH file saved or downloaded from demo
+- Option to input/output BVH files that use standard T-pose with `motion_convert.py`
+- Added BVH file containing the standard Kimodo T-pose to `kimodo/assets/skeletons/somaskel77/somaskel77_standard_tpose.bvh`
+- Updated documentation with these new options
+
+## [2026-04-10]
+
+### Added
+- [Kimodo-SOMA-RP-v1.1](https://huggingface.co/nvidia/Kimodo-SOMA-RP-v1.1) and [Kimodo-SOMA-SEED-v1.1](https://huggingface.co/nvidia/Kimodo-SOMA-SEED-v1.1) models and added support in the codebase. If not specified, the latest version of the models will be used automatically with the demo and CLI.
+- [Kimodo Motion Generation Benchmark](https://huggingface.co/datasets/nvidia/Kimodo-Motion-Gen-Benchmark) for standardized evaluation of motion generation models training on the BONES-SEED dataset.
+- Scripts to construct the full benchmark, generate motions for test cases, and compute evaluation metrics. 
+- Documentation explaining the benchmark and how to use the evaluation pipeline.
+- [TMR-SOMA-RP-v1](https://huggingface.co/nvidia/TMR-SOMA-RP-v1) motion-text embedding model to be used for evaluation metrics.
+- Added option to load LLM2Vec text encoder in fp32 precision.
+
+### Fixed
+- Always use batch size 1 with LLM2Vec to avoid unexpected behavior of different embeddings based on batch size.
+- Load LLM2Vec directly onto the GPU, if available.
+- Updated documentation on constraints with more details.
+
+## [2026-04-01]
+
+### Fixed
+- Fix unnecessary text encoder reload when switching between models in the interactive demo (if not using the text encoder server API).
+
+## [2026-03-31]
+
+### Added
+- New `kimodo_convert` CLI tool for converting generated motions between formats (NPZ, BVH, MuJoCo CSV, AMASS NPZ).
+- Support for loading and saving BVH, CSV, and NPZ motion files in the interactive demo.
+
+## [2026-03-27]
+
+### Fixed
+- Bug fix for foot contact visualization in the interactive demo.
+- Patch bug with BVH export for SOMA models.
+
+## [2026-03-19]
+
+### Changed
+- **Breaking:** Model inputs/outputs now use the SOMA 77-joint skeleton (`somaskel77`). This affects saved motion formats and constraint files from previous versions.
+
+### Added
+- Released timeline annotations for the BONES-SEED dataset on HuggingFace.
+
+## [2026-03-16] - Initial Release
+
+### Added
+- Open-source release of Kimodo codebase under Apache-2.0 license.
+- Five model variants: Kimodo-SOMA-RP-v1, Kimodo-G1-RP-v1, Kimodo-SOMA-SEED-v1, Kimodo-G1-SEED-v1, Kimodo-SMPLX-RP-v1.
+- Command-line interface (`kimodo_gen`) for motion generation with text prompts and kinematic constraints.
+- Interactive web-based motion authoring demo (`kimodo_demo`) with timeline editor, constraint tracks, and 3D visualization.
+- Support for multiple output formats: default NPZ, MuJoCo qpos CSV (G1), AMASS NPZ (SMPL-X).
+- Documentation site with quick start guide, installation instructions, CLI reference, and API docs.
+- Compatibility with downstream tools: ProtoMotions (physics-based policy training) and GMR (motion retargeting).

CONTRIBUTING.MD

@@ -0,0 +1,49 @@
+# How to Contribute
+
+## Code Reviews
+
+All submissions require review. We use GitHub pull requests for this purpose. Consult
+[GitHub Help](https://help.github.com/articles/about-pull-requests/) for more information on using pull requests.
+
+## Signing Your Work
+
+* We require that all contributors "sign-off" on their commits. This certifies that the contribution is your original work, or you have rights to submit it under the same license, or a compatible license.
+
+  * Any contribution which contains commits that are not Signed-Off will not be accepted.
+
+* To sign off on a commit you simply use the `--signoff` (or `-s`) option when committing your changes:
+  ```bash
+  $ git commit -s -m "Add cool feature."
+  ```
+  This will append the following to your commit message:
+  ```
+  Signed-off-by: Your Name <your@email.com>
+  ```
+
+* Full text of the DCO:
+
+  ```
+    Developer Certificate of Origin
+    Version 1.1
+
+    Copyright (C) 2004, 2006 The Linux Foundation and its contributors.
+    1 Letterman Drive
+    Suite D4700
+    San Francisco, CA, 94129
+
+    Everyone is permitted to copy and distribute verbatim copies of this license document, but changing it is not allowed.
+  ```
+
+  ```
+    Developer's Certificate of Origin 1.1
+
+    By making a contribution to this project, I certify that:
+
+    (a) The contribution was created in whole or in part by me and I have the right to submit it under the open source license indicated in the file; or
+
+    (b) The contribution is based upon previous work that, to the best of my knowledge, is covered under an appropriate open source license and I have the right under that license to submit that work with modifications, whether created in whole or in part by me, under the same open source license (unless I am permitted to submit under a different license), as indicated in the file; or
+
+    (c) The contribution was provided directly to me by some other person who certified (a), (b) or (c) and I have not modified it.
+
+    (d) I understand and agree that this project and the contribution are public and that a record of the contribution (including all personal information I submit with it, including my sign-off) is maintained indefinitely and may be redistributed consistent with this project or the open source license(s) involved.
+  ```

Dockerfile

@@ -0,0 +1,46 @@
+FROM nvcr.io/nvidia/pytorch:24.10-py3
+
+# Avoid some interactive prompts + make pip quieter/reproducible-ish
+ENV DEBIAN_FRONTEND=noninteractive \
+    PIP_DISABLE_PIP_VERSION_CHECK=1 \
+    PYTHONDONTWRITEBYTECODE=1 \
+    PYTHONUNBUFFERED=1
+
+# Where your code will live inside the container
+WORKDIR /workspace
+
+# System deps
+RUN apt-get update && apt-get install -y --no-install-recommends \
+      git curl ca-certificates \
+      cmake build-essential \
+      gosu \
+    && rm -rf /var/lib/apt/lists/*
+
+# Some base images ship a broken `/usr/local/bin/cmake` shim (from a partial pip install),
+# which shadows `/usr/bin/cmake` and breaks builds that invoke `cmake` (e.g. MotionCorrection).
+# Prefer the system cmake.
+RUN rm -f /usr/local/bin/cmake || true
+
+# Install from docker_requirements.txt: kimodo editable (-e .),
+# but MotionCorrection non-editable (./MotionCorrection). The -e . line ensures [project.scripts]
+# from pyproject.toml are installed (kimodo_gen, kimodo_demo, kimodo_textencoder).
+# SKIP_MOTION_CORRECTION_IN_SETUP=1 so setup.py does not bundle motion_correction; it is
+# installed separately from ./MotionCorrection in the requirements file (non-editable).
+COPY docker_requirements.txt /workspace/docker_requirements.txt
+COPY setup.py /workspace/setup.py
+COPY pyproject.toml /workspace/pyproject.toml
+COPY kimodo /workspace/kimodo
+COPY kimodo-viser /workspace/kimodo-viser
+COPY MotionCorrection /workspace/MotionCorrection
+
+RUN --mount=type=cache,target=/root/.cache/pip \
+    python -m pip install --upgrade pip \
+ && SKIP_MOTION_CORRECTION_IN_SETUP=1 python -m pip install -r docker_requirements.txt
+
+# Use the docker-entrypoint script, to allow the docker to run as the actual user instead of root
+COPY kimodo/scripts/docker-entrypoint.sh /usr/local/bin/docker-entrypoint
+RUN chmod +x /usr/local/bin/docker-entrypoint
+
+# Default command (change to your entrypoint if you have one)
+ENTRYPOINT ["docker-entrypoint"]
+CMD ["bash"]

LICENSE

@@ -0,0 +1,201 @@
+                                 Apache License
+                           Version 2.0, January 2004
+                        http://www.apache.org/licenses/
+
+   TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION
+
+   1. Definitions.
+
+      "License" shall mean the terms and conditions for use, reproduction,
+      and distribution as defined by Sections 1 through 9 of this document.
+
+      "Licensor" shall mean the copyright owner or entity authorized by
+      the copyright owner that is granting the License.
+
+      "Legal Entity" shall mean the union of the acting entity and all
+      other entities that control, are controlled by, or are under common
+      control with that entity. For the purposes of this definition,
+      "control" means (i) the power, direct or indirect, to cause the
+      direction or management of such entity, whether by contract or
+      otherwise, or (ii) ownership of fifty percent (50%) or more of the
+      outstanding shares, or (iii) beneficial ownership of such entity.
+
+      "You" (or "Your") shall mean an individual or Legal Entity
+      exercising permissions granted by this License.
+
+      "Source" form shall mean the preferred form for making modifications,
+      including but not limited to software source code, documentation
+      source, and configuration files.
+
+      "Object" form shall mean any form resulting from mechanical
+      transformation or translation of a Source form, including but
+      not limited to compiled object code, generated documentation,
+      and conversions to other media types.
+
+      "Work" shall mean the work of authorship, whether in Source or
+      Object form, made available under the License, as indicated by a
+      copyright notice that is included in or attached to the work
+      (an example is provided in the Appendix below).
+
+      "Derivative Works" shall mean any work, whether in Source or Object
+      form, that is based on (or derived from) the Work and for which the
+      editorial revisions, annotations, elaborations, or other modifications
+      represent, as a whole, an original work of authorship. For the purposes
+      of this License, Derivative Works shall not include works that remain
+      separable from, or merely link (or bind by name) to the interfaces of,
+      the Work and Derivative Works thereof.
+
+      "Contribution" shall mean any work of authorship, including
+      the original version of the Work and any modifications or additions
+      to that Work or Derivative Works thereof, that is intentionally
+      submitted to Licensor for inclusion in the Work by the copyright owner
+      or by an individual or Legal Entity authorized to submit on behalf of
+      the copyright owner. For the purposes of this definition, "submitted"
+      means any form of electronic, verbal, or written communication sent
+      to the Licensor or its representatives, including but not limited to
+      communication on electronic mailing lists, source code control systems,
+      and issue tracking systems that are managed by, or on behalf of, the
+      Licensor for the purpose of discussing and improving the Work, but
+      excluding communication that is conspicuously marked or otherwise
+      designated in writing by the copyright owner as "Not a Contribution."
+
+      "Contributor" shall mean Licensor and any individual or Legal Entity
+      on behalf of whom a Contribution has been received by Licensor and
+      subsequently incorporated within the Work.
+
+   2. Grant of Copyright License. Subject to the terms and conditions of
+      this License, each Contributor hereby grants to You a perpetual,
+      worldwide, non-exclusive, no-charge, royalty-free, irrevocable
+      copyright license to reproduce, prepare Derivative Works of,
+      publicly display, publicly perform, sublicense, and distribute the
+      Work and such Derivative Works in Source or Object form.
+
+   3. Grant of Patent License. Subject to the terms and conditions of
+      this License, each Contributor hereby grants to You a perpetual,
+      worldwide, non-exclusive, no-charge, royalty-free, irrevocable
+      (except as stated in this section) patent license to make, have made,
+      use, offer to sell, sell, import, and otherwise transfer the Work,
+      where such license applies only to those patent claims licensable
+      by such Contributor that are necessarily infringed by their
+      Contribution(s) alone or by combination of their Contribution(s)
+      with the Work to which such Contribution(s) was submitted. If You
+      institute patent litigation against any entity (including a
+      cross-claim or counterclaim in a lawsuit) alleging that the Work
+      or a Contribution incorporated within the Work constitutes direct
+      or contributory patent infringement, then any patent licenses
+      granted to You under this License for that Work shall terminate
+      as of the date such litigation is filed.
+
+   4. Redistribution. You may reproduce and distribute copies of the
+      Work or Derivative Works thereof in any medium, with or without
+      modifications, and in Source or Object form, provided that You
+      meet the following conditions:
+
+      (a) You must give any other recipients of the Work or
+          Derivative Works a copy of this License; and
+
+      (b) You must cause any modified files to carry prominent notices
+          stating that You changed the files; and
+
+      (c) You must retain, in the Source form of any Derivative Works
+          that You distribute, all copyright, patent, trademark, and
+          attribution notices from the Source form of the Work,
+          excluding those notices that do not pertain to any part of
+          the Derivative Works; and
+
+      (d) If the Work includes a "NOTICE" text file as part of its
+          distribution, then any Derivative Works that You distribute must
+          include a readable copy of the attribution notices contained
+          within such NOTICE file, excluding those notices that do not
+          pertain to any part of the Derivative Works, in at least one
+          of the following places: within a NOTICE text file distributed
+          as part of the Derivative Works; within the Source form or
+          documentation, if provided along with the Derivative Works; or,
+          within a display generated by the Derivative Works, if and
+          wherever such third-party notices normally appear. The contents
+          of the NOTICE file are for informational purposes only and
+          do not modify the License. You may add Your own attribution
+          notices within Derivative Works that You distribute, alongside
+          or as an addendum to the NOTICE text from the Work, provided
+          that such additional attribution notices cannot be construed
+          as modifying the License.
+
+      You may add Your own copyright statement to Your modifications and
+      may provide additional or different license terms and conditions
+      for use, reproduction, or distribution of Your modifications, or
+      for any such Derivative Works as a whole, provided Your use,
+      reproduction, and distribution of the Work otherwise complies with
+      the conditions stated in this License.
+
+   5. Submission of Contributions. Unless You explicitly state otherwise,
+      any Contribution intentionally submitted for inclusion in the Work
+      by You to the Licensor shall be under the terms and conditions of
+      this License, without any additional terms or conditions.
+      Notwithstanding the above, nothing herein shall supersede or modify
+      the terms of any separate license agreement you may have executed
+      with Licensor regarding such Contributions.
+
+   6. Trademarks. This License does not grant permission to use the trade
+      names, trademarks, service marks, or product names of the Licensor,
+      except as required for reasonable and customary use in describing the
+      origin of the Work and reproducing the content of the NOTICE file.
+
+   7. Disclaimer of Warranty. Unless required by applicable law or
+      agreed to in writing, Licensor provides the Work (and each
+      Contributor provides its Contributions) on an "AS IS" BASIS,
+      WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or
+      implied, including, without limitation, any warranties or conditions
+      of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A
+      PARTICULAR PURPOSE. You are solely responsible for determining the
+      appropriateness of using or redistributing the Work and assume any
+      risks associated with Your exercise of permissions under this License.
+
+   8. Limitation of Liability. In no event and under no legal theory,
+      whether in tort (including negligence), contract, or otherwise,
+      unless required by applicable law (such as deliberate and grossly
+      negligent acts) or agreed to in writing, shall any Contributor be
+      liable to You for damages, including any direct, indirect, special,
+      incidental, or consequential damages of any character arising as a
+      result of this License or out of the use or inability to use the
+      Work (including but not limited to damages for loss of goodwill,
+      work stoppage, computer failure or malfunction, or any and all
+      other commercial damages or losses), even if such Contributor
+      has been advised of the possibility of such damages.
+
+   9. Accepting Warranty or Additional Liability. While redistributing
+      the Work or Derivative Works thereof, You may choose to offer,
+      and charge a fee for, acceptance of support, warranty, indemnity,
+      or other liability obligations and/or rights consistent with this
+      License. However, in accepting such obligations, You may act only
+      on Your own behalf and on Your sole responsibility, not on behalf
+      of any other Contributor, and only if You agree to indemnify,
+      defend, and hold each Contributor harmless for any liability
+      incurred by, or claims asserted against, such Contributor by reason
+      of your accepting any such warranty or additional liability.
+
+   END OF TERMS AND CONDITIONS
+
+   APPENDIX: How to apply the Apache License to your work.
+
+      To apply the Apache License to your work, attach the following
+      boilerplate notice, with the fields enclosed by brackets "[]"
+      replaced with your own identifying information. (Don't include
+      the brackets!)  The text should be enclosed in the appropriate
+      comment syntax for the file format. We also recommend that a
+      file or class name and description of purpose be included on the
+      same "printed page" as the copyright notice for easier
+      identification within third-party archives.
+
+   Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
+
+   Licensed under the Apache License, Version 2.0 (the "License");
+   you may not use this file except in compliance with the License.
+   You may obtain a copy of the License at
+
+       http://www.apache.org/licenses/LICENSE-2.0
+
+   Unless required by applicable law or agreed to in writing, software
+   distributed under the License is distributed on an "AS IS" BASIS,
+   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+   See the License for the specific language governing permissions and
+   limitations under the License.

MANIFEST.in

@@ -0,0 +1,6 @@
+include setup.py
+recursive-include kimodo/assets *
+recursive-include MotionCorrection/src *.cpp *.h *.inl
+recursive-include MotionCorrection/python *.py *.dll
+include MotionCorrection/CMakeLists.txt
+include MotionCorrection/test_example.py

MotionCorrection/.gitignore

@@ -0,0 +1,91 @@
+# Python
+__pycache__/
+*.py[cod]
+*$py.class
+*.so
+*.egg
+*.egg-info/
+dist/
+build/
+*.whl
+.Python
+develop-eggs/
+.installed.cfg
+pip-log.txt
+pip-delete-this-directory.txt
+.pytest_cache/
+.coverage
+htmlcov/
+.tox/
+.venv
+venv/
+ENV/
+env/
+
+# C/C++
+*.o
+*.obj
+*.exe
+*.out
+*.app
+*.dll
+*.dylib
+*.lib
+*.a
+*.la
+*.lo
+*.slo
+*.ko
+*.elf
+*.ilk
+*.map
+*.exp
+*.gch
+*.pch
+*.idb
+*.pdb
+*.mod
+*.smod
+*.lai
+
+# CMake
+CMakeCache.txt
+CMakeFiles/
+CMakeScripts/
+cmake_install.cmake
+install_manifest.txt
+CTestTestfile.cmake
+_deps/
+cmake-build-*/
+CMakeUserPresets.json
+
+# IDE
+.vscode/
+.idea/
+*.swp
+*.swo
+*~
+.DS_Store
+*.iml
+.project
+.cproject
+.settings/
+
+# Visual Studio
+.vs/
+*.user
+*.suo
+*.userosscache
+*.sln.docstates
+*.VC.db
+*.VC.opendb
+
+# Build directories
+build/
+Build/
+out/
+dist/
+temp/
+
+# Logs
+*.log

MotionCorrection/CMakeLists.txt

@@ -0,0 +1,95 @@
+cmake_minimum_required(VERSION 3.15)
+project(motion_correction)
+
+set(CMAKE_CXX_STANDARD 17)
+set(CMAKE_CXX_STANDARD_REQUIRED ON)
+
+# Find Python
+find_package(Python3 COMPONENTS Interpreter Development REQUIRED)
+
+# Find or fetch pybind11
+find_package(pybind11 CONFIG QUIET)
+if(NOT pybind11_FOUND)
+    message(STATUS "pybind11 not found, fetching from GitHub...")
+    include(FetchContent)
+    FetchContent_Declare(
+        pybind11
+        GIT_REPOSITORY https://github.com/pybind/pybind11.git
+        GIT_TAG v2.11.1
+    )
+    FetchContent_MakeAvailable(pybind11)
+endif()
+
+# Find or fetch Eigen
+find_package(Eigen3 3.3 CONFIG QUIET)
+if(NOT Eigen3_FOUND)
+    message(STATUS "Eigen3 not found, fetching from GitLab...")
+    include(FetchContent)
+    FetchContent_Declare(
+        Eigen
+        GIT_REPOSITORY https://gitlab.com/libeigen/eigen.git
+        GIT_TAG 3.4.0
+    )
+    set(EIGEN_BUILD_DOC OFF CACHE BOOL "" FORCE)
+    set(BUILD_TESTING OFF CACHE BOOL "" FORCE)
+    set(EIGEN_BUILD_PKGCONFIG OFF CACHE BOOL "" FORCE)
+    FetchContent_MakeAvailable(Eigen)
+endif()
+
+# Source files
+set(MATH_SOURCES
+    src/cpp/Math/Matrix.cpp
+    src/cpp/Math/Quaternion.cpp
+    src/cpp/Math/Transform.cpp
+    src/cpp/Math/Types.cpp
+    src/cpp/Math/Vector.cpp
+)
+
+set(ANIM_SOURCES
+    src/cpp/AnimProcessing/InverseKinematics.cpp
+    src/cpp/AnimProcessing/TrajectoryCorrector.cpp
+    src/cpp/AnimProcessing/Utility.cpp
+)
+
+# Create static library for the core functionality
+add_library(motion_correction_cpp_base STATIC ${MATH_SOURCES} ${ANIM_SOURCES})
+
+# Enable Position Independent Code (required for linking into shared library)
+set_target_properties(motion_correction_cpp_base PROPERTIES POSITION_INDEPENDENT_CODE ON)
+
+target_include_directories(motion_correction_cpp_base PUBLIC
+    ${CMAKE_CURRENT_SOURCE_DIR}/src/cpp
+)
+
+if(TARGET Eigen3::Eigen)
+    target_link_libraries(motion_correction_cpp_base PUBLIC Eigen3::Eigen)
+else()
+    target_link_libraries(motion_correction_cpp_base PUBLIC eigen)
+endif()
+
+target_compile_definitions(motion_correction_cpp_base PUBLIC EIGEN_MPL2_ONLY)
+
+# Compiler-specific settings
+if(MSVC)
+    # MSVC-specific flags
+    target_compile_options(motion_correction_cpp_base PRIVATE /W4 /arch:AVX)
+else()
+    # GCC/Clang flags (also applies to MinGW on Windows)
+    # Enable SSE4.1 and AVX instructions for SIMD operations
+    target_compile_options(motion_correction_cpp_base PRIVATE -Wall -Wextra -msse4.1 -mavx)
+endif()
+
+# Python bindings
+pybind11_add_module(_motion_correction src/cpp/BindingsPython.cpp)
+
+target_link_libraries(_motion_correction PRIVATE motion_correction_cpp_base)
+
+target_include_directories(_motion_correction PRIVATE
+    ${CMAKE_CURRENT_SOURCE_DIR}/src/cpp
+)
+
+
+# Install the Python module
+install(TARGETS _motion_correction LIBRARY DESTINATION python/motion_correction)
+install(FILES python/motion_correction/__init__.py DESTINATION python/motion_correction)
+install(FILES python/motion_correction/motion_postprocess.py DESTINATION python/motion_correction)

MotionCorrection/MANIFEST.in

@@ -0,0 +1,4 @@
+include CMakeLists.txt
+include test_example.py
+recursive-include src *.cpp *.h *.inl
+recursive-include python *.py *.dll

MotionCorrection/README.md

@@ -0,0 +1,31 @@
+# motion_correction
+
+Standalone `correct_motion` implementation packaged as a small C++ motion trajectory correction library with Python bindings.
+
+## Installation Guide
+
+### Prerequisites
+
+Ensure you have a C++17 compatible compiler (GCC 7.0+, Clang 5.0+, or MSVC 2017+) and CMake 3.15+. On Windows, install MinGW-w64 or Visual Studio with C++ tools. On Linux, install `build-essential` and `cmake`.
+
+This project will download and install additional third-party open source software projects. Review the license terms of these open source projects before use.
+
+### Build & Install
+
+#### Standard Installation
+```bash
+pip install .
+```
+
+#### Development Installation
+```bash
+pip install -e .
+```
+
+### Verify Installation
+
+```python
+import motion_correction
+print("Installation successful!")
+```
+You can also run `python run_test.py` for a simple test.

MotionCorrection/python/motion_correction/__init__.py

@@ -0,0 +1 @@
+from ._motion_correction import *

MotionCorrection/python/motion_correction/motion_postprocess.py

@@ -0,0 +1,102 @@
+# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
+# SPDX-License-Identifier: Apache-2.0
+
+import os
+import pickle
+
+import numpy as np
+import torch
+
+import motion_correction
+
+
+def correct_motion(
+    hipTranslations,
+    jointRotations,
+    contacts,
+    hipTranslationsInput,
+    rotationsInput,
+    constraint_masks,
+    contact_threshold,
+    root_margin,
+    working_rig,
+    has_double_ankle_joints=False,
+):
+    joint_names = [x.name for x in working_rig]
+    joint_parents = [
+        joint_names.index(working_rig[i].parent) if working_rig[i].parent in joint_names else -1
+        for i in range(len(working_rig))
+    ]
+    joint_ref_translations = [list(x.t_pose_translation) for x in working_rig]
+    joint_ref_rotations = [list(x.t_pose_rotation) for x in working_rig]
+
+    left_hand_idx = [i for i in range(len(joint_names)) if working_rig[i].retarget_tag == "LeftHand"]
+    if len(left_hand_idx) != 1:
+        raise RuntimeError(f"correct_motion: Expected exactly one joint with LeftHand tag")
+    left_hand_idx = left_hand_idx[0]
+
+    right_hand_idx = [i for i in range(len(joint_names)) if working_rig[i].retarget_tag == "RightHand"]
+    if len(right_hand_idx) != 1:
+        raise RuntimeError(f"correct_motion: Expected exactly one joint with RightHand tag")
+    right_hand_idx = right_hand_idx[0]
+
+    left_foot_idx = [i for i in range(len(joint_names)) if working_rig[i].retarget_tag == "LeftFoot"]
+    if len(left_foot_idx) != 1:
+        raise RuntimeError(f"correct_motion: Expected exactly one joint with LeftFoot tag")
+    left_foot_idx = left_foot_idx[0]
+
+    right_foot_idx = [i for i in range(len(joint_names)) if working_rig[i].retarget_tag == "RightFoot"]
+    if len(right_foot_idx) != 1:
+        raise RuntimeError(f"correct_motion: Expected exactly one joint with RightFoot tag")
+    right_foot_idx = right_foot_idx[0]
+
+    end_frame = hipTranslations.shape[1]
+
+    default_mask = torch.zeros(hipTranslations.shape[1], dtype=torch.float32)
+    root_mask = constraint_masks.get("Root", default_mask)
+    full_body_mask = constraint_masks.get("FullBody", default_mask)
+    left_hand_mask = constraint_masks.get("LeftHand", default_mask)
+    right_hand_mask = constraint_masks.get("RightHand", default_mask)
+    left_foot_mask = constraint_masks.get("LeftFoot", default_mask)
+    right_foot_mask = constraint_masks.get("RightFoot", default_mask)
+
+    batch_size = hipTranslations.shape[0]
+
+    for b in range(batch_size):
+        hipTranslationsCorrected = hipTranslations[b, :end_frame].detach().cpu().flatten().numpy().astype(np.float32)
+        rotationsCorrected = jointRotations[b, :end_frame].detach().cpu().flatten().numpy().astype(np.float32)
+
+        hipTranslationsInput_flat = hipTranslationsInput.detach().cpu().flatten().numpy().astype(np.float32)
+        rotationsInput_flat = rotationsInput.detach().cpu().flatten().numpy().astype(np.float32)
+        ctcs = contacts[b].detach().cpu().flatten().numpy().astype(np.float32)
+
+        motion_correction.correct_motion(
+            hipTranslationsCorrected,
+            rotationsCorrected,
+            hipTranslationsInput_flat,
+            rotationsInput_flat,
+            full_body_mask,
+            left_hand_mask,
+            right_hand_mask,
+            left_foot_mask,
+            right_foot_mask,
+            root_mask,
+            np.array(ctcs, dtype=np.float32),
+            joint_parents,
+            joint_ref_translations,
+            joint_ref_rotations,
+            left_hand_idx,
+            right_hand_idx,
+            left_foot_idx,
+            right_foot_idx,
+            contact_threshold,
+            root_margin,
+            has_double_ankle_joints,
+        )
+
+        hipTranslations[b, :end_frame] = torch.from_numpy(
+            hipTranslationsCorrected.reshape(*hipTranslations[b, :end_frame].shape)
+        )
+        jointRotations[b, :end_frame] = torch.from_numpy(
+            rotationsCorrected.reshape(*jointRotations[b, :end_frame].shape)
+        )

MotionCorrection/run_test.py

@@ -0,0 +1,72 @@
+#!/usr/bin/env python3
+
+# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
+# SPDX-License-Identifier: Apache-2.0
+
+import torch
+from motion_correction.motion_postprocess import correct_motion
+
+
+class Joint:
+    def __init__(self, name, parent, t_pose_translation, t_pose_rotation, retarget_tag=""):
+        self.name = name
+        self.parent = parent
+        self.t_pose_translation = t_pose_translation
+        self.t_pose_rotation = t_pose_rotation
+        self.retarget_tag = retarget_tag
+
+
+def create_test_rig():
+    return [
+        Joint("Hips", None, [0.0, 1.0, 0.0], [0.0, 0.0, 0.0, 1.0], "Root"),
+        Joint("Spine", "Hips", [0.0, 0.1, 0.0], [0.0, 0.0, 0.0, 1.0]),
+        Joint("LeftUpLeg", "Hips", [-0.1, -0.05, 0.0], [0.0, 0.0, 0.0, 1.0]),
+        Joint("LeftLeg", "LeftUpLeg", [0.0, -0.4, 0.0], [0.0, 0.0, 0.0, 1.0]),
+        Joint("LeftFoot", "LeftLeg", [0.0, -0.4, 0.0], [0.0, 0.0, 0.0, 1.0], "LeftFoot"),
+        Joint("RightUpLeg", "Hips", [0.1, -0.05, 0.0], [0.0, 0.0, 0.0, 1.0]),
+        Joint("RightLeg", "RightUpLeg", [0.0, -0.4, 0.0], [0.0, 0.0, 0.0, 1.0]),
+        Joint("RightFoot", "RightLeg", [0.0, -0.4, 0.0], [0.0, 0.0, 0.0, 1.0], "RightFoot"),
+        Joint("LeftArm", "Spine", [-0.3, 0.3, 0.0], [0.0, 0.0, 0.0, 1.0]),
+        Joint("LeftHand", "LeftArm", [-0.3, 0.0, 0.0], [0.0, 0.0, 0.0, 1.0], "LeftHand"),
+        Joint("RightArm", "Spine", [0.3, 0.3, 0.0], [0.0, 0.0, 0.0, 1.0]),
+        Joint("RightHand", "RightArm", [0.3, 0.0, 0.0], [0.0, 0.0, 0.0, 1.0], "RightHand"),
+    ]
+
+
+if __name__ == "__main__":
+    # Test data
+    batch_size, num_frames, num_joints = 1, 60, 12
+
+    hipTranslations = torch.randn(batch_size, num_frames, 3)
+    jointRotations = torch.randn(batch_size, num_frames, num_joints, 4)
+    jointRotations = jointRotations / jointRotations.norm(dim=-1, keepdim=True)
+
+    contacts = torch.rand(batch_size, num_frames, 4)
+    hipTranslationsInput = hipTranslations.clone()
+    rotationsInput = jointRotations.clone()
+
+    constraint_masks = {
+        "Root": torch.zeros(num_frames),
+        "FullBody": torch.zeros(num_frames),
+        "LeftHand": torch.zeros(num_frames),
+        "RightHand": torch.zeros(num_frames),
+        "LeftFoot": torch.zeros(num_frames),
+        "RightFoot": torch.zeros(num_frames),
+    }
+
+    working_rig = create_test_rig()
+
+    # Run correction
+    correct_motion(
+        hipTranslations=hipTranslations,
+        jointRotations=jointRotations,
+        contacts=contacts,
+        hipTranslationsInput=hipTranslationsInput,
+        rotationsInput=rotationsInput,
+        constraint_masks=constraint_masks,
+        contact_threshold=0.5,
+        root_margin=0.01,
+        working_rig=working_rig,
+    )
+
+    print("Test completed successfully")

MotionCorrection/setup.py

@@ -0,0 +1,122 @@
+#!/usr/bin/env python3
+
+# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
+# SPDX-License-Identifier: Apache-2.0
+"""Setup script for correct_motion standalone package."""
+
+import os
+import shutil
+import subprocess
+import sys
+from pathlib import Path
+
+from setuptools import Extension, setup
+from setuptools.command.build_ext import build_ext
+
+
+class CMakeExtension(Extension):
+    def __init__(self, name, sourcedir=""):
+        Extension.__init__(self, name, sources=[])
+        self.sourcedir = os.path.abspath(sourcedir)
+
+
+class CMakeBuild(build_ext):
+    def run(self):
+        try:
+            subprocess.check_output(["cmake", "--version"])
+        except OSError:
+            raise RuntimeError("CMake must be installed to build this package")
+
+        for ext in self.extensions:
+            self.build_extension(ext)
+
+    def build_extension(self, ext):
+        # import pdb; pdb.set_trace()  # Debug build process
+
+        extdir = os.path.abspath(os.path.dirname(self.get_ext_fullpath(ext.name)))
+        cmake_args = [
+            f"-DCMAKE_LIBRARY_OUTPUT_DIRECTORY={extdir}",
+            f"-DPYTHON_EXECUTABLE={sys.executable}",
+        ]
+
+        cfg = "Debug" if self.debug else "Release"
+        build_args = ["--config", cfg]
+
+        cmake_args += [f"-DCMAKE_BUILD_TYPE={cfg}"]
+
+        use_mingw = False
+        mingw_bin = None
+
+        if sys.platform == "win32":
+            generator = os.environ.get("CMAKE_GENERATOR", "")
+            if generator:
+                cmake_args = ["-G", generator] + cmake_args
+                if "mingw" in generator.lower():
+                    use_mingw = True
+                else:
+                    cmake_args += [f"-DCMAKE_LIBRARY_OUTPUT_DIRECTORY_{cfg.upper()}={extdir}"]
+            else:
+                # Try MinGW Makefiles as default on Windows
+                try:
+                    subprocess.check_output(["g++", "--version"], stderr=subprocess.STDOUT)
+                    use_mingw = True
+                    cmake_args = ["-G", "MinGW Makefiles"] + cmake_args
+                    build_args = []  # MinGW Makefiles do not accept --config
+                except (OSError, subprocess.CalledProcessError):
+                    # If g++ is not found, let CMake use its default (Visual Studio)
+                    cmake_args += [f"-DCMAKE_LIBRARY_OUTPUT_DIRECTORY_{cfg.upper()}={extdir}"]
+
+            if use_mingw:
+                gxx_path = shutil.which("g++")
+                if gxx_path:
+                    mingw_bin = Path(gxx_path).parent
+        else:
+            build_args += ["--", "-j4"]
+
+        env = os.environ.copy()
+        env["CXXFLAGS"] = f'{env.get("CXXFLAGS", "")} -DVERSION_INFO=\\"{self.distribution.get_version()}\\"'
+
+        if not os.path.exists(self.build_temp):
+            os.makedirs(self.build_temp)
+
+        subprocess.check_call(["cmake", ext.sourcedir] + cmake_args, cwd=self.build_temp, env=env)
+        subprocess.check_call(["cmake", "--build", "."] + build_args, cwd=self.build_temp)
+
+        if use_mingw and mingw_bin is not None:
+            runtime_libs = [
+                "libstdc++-6.dll",
+                "libgcc_s_seh-1.dll",
+                "libwinpthread-1.dll",
+            ]
+            extdir_path = Path(extdir)
+            extdir_path.mkdir(parents=True, exist_ok=True)
+            for lib_name in runtime_libs:
+                src_path = mingw_bin / lib_name
+                if src_path.exists():
+                    shutil.copy2(src_path, extdir_path / lib_name)
+                else:
+                    self.announce(
+                        f"Warning: Expected MinGW runtime DLL '{lib_name}' not found next to g++ (looked in {mingw_bin}). "
+                        "The built extension may fail to import if the DLL is not on PATH.",
+                        level=3,
+                    )
+
+
+setup(
+    name="motion_correction",
+    version="1.0.0",
+    author="NVIDIA",
+    description="Standalone correct_motion function",
+    long_description="",
+    packages=["motion_correction"],
+    package_dir={"": "python"},
+    ext_modules=[CMakeExtension("motion_correction._motion_correction")],
+    cmdclass={"build_ext": CMakeBuild},
+    zip_safe=False,
+    python_requires=">=3.8",
+    install_requires=[
+        "torch>=1.10.0",
+        "numpy>=1.19.0",
+        # 'cmake' # can install this via pip if the windows system does not have it. But need to run this by yourself before build, not in here.
+    ],
+)

MotionCorrection/src/cpp/AnimProcessing/InverseKinematics.cpp

@@ -0,0 +1,200 @@
+/*
+ * SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
+ * SPDX-License-Identifier: Apache-2.0
+ */
+
+#include "InverseKinematics.h"
+#include "Math/Scalar.h"
+#include <iostream>
+
+
+using namespace IK;
+
+namespace
+{
+
+float getAngleWithTwoSideVectors(const Math::Vector& vecLeft, const Math::Vector& vecRight)
+{
+    auto lNorm = vecLeft.GetNormalized3();
+    auto rNorm = vecRight.GetNormalized3();
+
+    float cosine = lNorm.GetDot3(rNorm);
+    float sine = lNorm.Cross3(rNorm).GetLength3();
+
+    return atan2f(sine, cosine);  // in radian
+}
+
+float getAngleWithCosineRule (const float lSideLeft, const float lSideRight, const float lSideAcross)
+{
+    float val =
+        (lSideRight * lSideRight + lSideLeft * lSideLeft - lSideAcross * lSideAcross) /
+            (2.0f * lSideLeft * lSideRight);
+    val = Math::Clamp(val, -1.0f, 1.0f);  // numerical stability. also avoid impossible trangulars
+    return acosf(val);  // in radian
+}
+
+}
+
+
+void IK::TwoBoneIk(
+    Pose& pose,
+    const Math::Transform& rootTransform,
+    uint32_t cIdx,
+    float weight,
+    const Math::Vector& target,
+    const std::vector<int>& joint_parents_vec,
+    const Math::Vector& hintOffset
+)
+{
+    weight = Math::Clamp(weight, 0.0f, 1.0f);
+    if (!(weight > 0.0f))
+        return;
+
+    // Two bone IK: joints are represented as "a", "b", "c" in the below comments:
+    //  1. stage 1, bend joint a and joint b, so that |ac| = |at|, while vec_ac maintain the same direction
+    //  2. stage 2, rotate start joint a so that c and t are in the same place
+
+    //  a                   a                   a             |
+    //  |\                  |\                  |\            |
+    //  | \                 |  \                | \           |
+    //  |  \  (stage 1 ->)  |   \  (stage 2 ->) |  \          |
+    //  |   b               |    b              |   b         |
+    //  |    \              |    |              |  /          |
+    //  |     \             |     |             | /           |
+    //  t      c            t      c            t(c)          |
+    //  (a is the root joint, b is the middle joint and c is the end joint)
+    //
+
+    int32_t bIdx = joint_parents_vec[cIdx];
+    if (bIdx < 0)
+    {
+        return;
+    }
+    int32_t aIdx = joint_parents_vec[bIdx];
+    if (aIdx < 0)
+    {
+        return;
+    }
+
+    // Find the parent world transform of joint a:
+    Math::Transform aParentWorldTransform = Math::Transform::Identity;
+    int32_t idx = joint_parents_vec[aIdx];
+    while (idx >= 0)
+    {
+        aParentWorldTransform = aParentWorldTransform * pose[idx];
+        idx = joint_parents_vec[idx];
+    }
+    aParentWorldTransform = aParentWorldTransform * rootTransform;
+
+    // Compute world space transforms of a, b and c:
+    Math::Transform aWorld = pose[aIdx] * aParentWorldTransform;
+    Math::Transform bWorld = pose[bIdx] * aWorld;
+    Math::Transform cWorld = pose[cIdx] * bWorld;
+
+    auto a = aWorld.GetTranslation();
+    auto b = bWorld.GetTranslation();
+    auto c = cWorld.GetTranslation();
+    auto t = Math::Vector::Lerp(c, target, weight);
+
+    // step 1 (stage 1): extend / contract the joint chain to match the distance
+    float eps = 0.0001f;  // numerical stability
+    float l_ab = (b - a).Length3().GetX();
+    float l_bc = (c - b).Length3().GetX();
+    float l_at = (a - t).Length3().GetX();
+    l_at = Math::Clamp(l_at, eps, (l_ab + l_bc) * 0.999f); // when not reachable, replace with maximum reachable length
+
+    // get the current angles
+    float theta_bac_current = getAngleWithTwoSideVectors(a - b, a - c);
+    float theta_abc_current = getAngleWithTwoSideVectors(b - a, b - c);
+    // get the desired angles
+    if (l_ab < eps || l_bc < eps || l_at < eps)
+    {
+        return;  // the length is too small. rejecting potentially numerically unstable requests.
+    }
+    float theta_bac_desired = getAngleWithCosineRule(l_ab, l_at, l_bc);
+    float theta_abc_desired = getAngleWithCosineRule(l_ab, l_bc, l_at);
+
+    // in joint[0]'s parent's space
+    Math::Vector rotationAxis = Math::Vector::Cross3(c - a, bWorld.TransformPoint(hintOffset) - a);
+    float l = rotationAxis.GetLength3();
+    if (l == 0)
+    {
+        rotationAxis = Math::Vector(0,0,1);
+    }
+    else
+    {
+        rotationAxis /= l;
+    }
+
+    // get the rotation with axis in the local space of joint a and joint b
+    Math::Vector rotationAxisLocalInBSpace = bWorld.GetRotation().RotateVectorInverse(rotationAxis);
+    Math::Transform rotateInB(
+        Math::Quaternion(rotationAxisLocalInBSpace,
+            (theta_abc_desired - theta_abc_current)), Math::Vector::Zero);
+
+    pose[bIdx] = rotateInB * pose[bIdx];
+
+    Math::Vector rotationAxisLocalInASpace = aWorld.GetRotation().RotateVectorInverse(rotationAxis);
+    Math::Transform rotateInA(
+        Math::Quaternion(rotationAxisLocalInASpace,
+            (theta_bac_desired - theta_bac_current)), Math::Vector::Zero);
+
+    pose[aIdx] = rotateInA * pose[aIdx];
+
+    // recompute a's world space transform as we're going to need it:
+    aWorld = pose[aIdx] * aParentWorldTransform;
+
+    // step 2 (stage 2): rotate joint a so that the target and the end joint c matches
+    auto acLocal = aWorld.GetRotation().RotateVectorInverse(
+        c - a);
+    auto atLocal = aWorld.GetRotation().RotateVectorInverse(
+        target - a);
+    Math::Transform rotateStageTwo(
+        Math::Quaternion::FromRotationBetweenVectors(acLocal, atLocal), Math::Vector::Zero
+    );
+
+    pose[aIdx] = rotateStageTwo * pose[aIdx];
+
+}
+
+void IK::OneBoneIk(
+    Pose& pose,
+    const Math::Transform& rootTransform,
+    uint32_t bIdx,
+    float weight,
+    const Math::Vector& target,
+    const std::vector<int>& joint_parents_vec
+)
+{
+    weight = Math::Clamp(weight, 0.0f, 1.0f);
+    if (!(weight > 0.0f))
+        return;
+
+    int32_t aIdx = joint_parents_vec[bIdx];
+    if (aIdx < 0)
+    {
+        return;
+    }
+
+    // Find the parent world transform of joint a:
+    Math::Transform aParentWorldTransform = Math::Transform::Identity;
+    int32_t idx = joint_parents_vec[aIdx];
+    while (idx >= 0)
+    {
+        aParentWorldTransform = aParentWorldTransform * pose[idx];
+        idx = joint_parents_vec[idx];
+    }
+    aParentWorldTransform = aParentWorldTransform * rootTransform;
+
+    // Compute world space transforms of a, b and c:
+    Math::Transform aWorld = pose[aIdx] * aParentWorldTransform;
+    Math::Transform bWorld = pose[bIdx] * aWorld;
+
+    auto abLocal = aWorld.GetRotation().RotateVectorInverse(
+        bWorld.GetTranslation() - aWorld.GetTranslation());
+    auto atLocal = aWorld.GetRotation().RotateVectorInverse(
+        target - aWorld.GetTranslation());
+
+    auto deltaRLocal = Math::Quaternion::NLerp(Math::Quaternion::Identity, Math::Quaternion::FromRotationBetweenVectors(abLocal, atLocal), weight);
+    pose[aIdx].SetRotation(deltaRLocal * pose[aIdx].GetRotation());
+}

MotionCorrection/src/cpp/AnimProcessing/InverseKinematics.h

@@ -0,0 +1,35 @@
+/*
+ * SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
+ * SPDX-License-Identifier: Apache-2.0
+ */
+
+#pragma once
+
+#include "Math/Transform.h"
+
+#include <vector>
+
+using Pose = std::vector<Math::Transform>;
+
+namespace IK {
+
+    void TwoBoneIk(
+        Pose& pose,
+        const Math::Transform& rootTransform,
+        uint32_t jointIdx,
+        float weight,
+        const Math::Vector& target,
+        const std::vector<int>& joint_parents_vec,
+        const Math::Vector& hintOffset = Math::Vector::Zero
+    );
+
+    void OneBoneIk(
+        Pose& pose,
+        const Math::Transform& rootTransform,
+        uint32_t jointIdx,
+        float weight,
+        const Math::Vector& target,
+        const std::vector<int>& joint_parents_vec
+    );
+
+}

MotionCorrection/src/cpp/AnimProcessing/TrajectoryCorrector.cpp

@@ -0,0 +1,524 @@
+/*
+ * SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
+ * SPDX-License-Identifier: Apache-2.0
+ */
+
+#include "TrajectoryCorrector.h"
+#include <iostream>
+
+static void removeRows(
+    Eigen::SparseMatrix<double>& M,
+    Eigen::MatrixXd *v,
+    int minCoeffs)
+{
+    Eigen::SparseMatrix<double, Eigen::RowMajor> rowMajorMat = M;
+    rowMajorMat.makeCompressed(); // Ensure compressed format
+
+    std::vector<Eigen::Triplet<double>> triplets;
+    triplets.reserve(rowMajorMat.nonZeros());
+
+    int newRow = 0;
+    for (int i = 0; i < rowMajorMat.outerSize(); ++i) {
+        // Get nonzero count via outerIndexPtr (compressed format only)
+        int nnz = rowMajorMat.outerIndexPtr()[i + 1] - rowMajorMat.outerIndexPtr()[i];
+
+        if (nnz >= minCoeffs) {
+            // Iterate through nonzeros in this row
+            for (Eigen::SparseMatrix<double, Eigen::RowMajor>::InnerIterator it(rowMajorMat, i); it; ++it) {
+                triplets.emplace_back(newRow, it.col(), it.value());
+            }
+            if (v)
+            {
+                v->row(newRow) = v->row(i);
+            }
+            newRow++;
+        }
+    }
+
+    M = Eigen::SparseMatrix<double>(newRow, M.cols());
+    M.setFromTriplets(triplets.begin(), triplets.end());
+    if (v)
+    {
+        v->conservativeResize(newRow, v->cols());
+    }
+}
+
+static void multVelWeights(
+    Eigen::SparseMatrix<double>& V,
+    Eigen::MatrixXd* v_rhs,
+    const Eigen::VectorXd& velocityWeights
+)
+{
+    Eigen::SparseMatrix<double, Eigen::RowMajor> rowMajorMat = V;
+    rowMajorMat.makeCompressed(); // Ensure compressed format
+
+    std::vector<Eigen::Triplet<double>> triplets;
+    triplets.reserve(rowMajorMat.nonZeros());
+
+    for (int i = 0; i < rowMajorMat.outerSize(); ++i) {
+        // Iterate through nonzeros in this row
+        Eigen::SparseMatrix<double, Eigen::RowMajor>::InnerIterator it(rowMajorMat, i);
+        double vel_weight = velocityWeights[it.col()];
+        for(; it; ++it)
+        {
+            triplets.emplace_back(i, it.col(), it.value() * vel_weight);
+        }
+        if (v_rhs)
+        {
+            (*v_rhs).row(i) = (*v_rhs).row(i) * vel_weight;
+        }
+    }
+}
+
+
+void TrajectoryCorrector::computeDiffMats(
+    Eigen::SparseMatrix<double>& V,
+    Eigen::SparseMatrix<double>& A,
+    uint32_t N,
+    const Eigen::VectorXd& velocityWeights,
+    Eigen::MatrixXd* v_rhs,
+    Eigen::MatrixXd* a_rhs)
+{
+    std::vector<Eigen::Triplet<double>> tripletList;
+
+    // Identity matrix"
+    tripletList.clear();
+    Eigen::SparseMatrix<double> I(N, N);
+    for (uint32_t i = 0; i < N; ++i)
+    {
+        tripletList.emplace_back(i, i, 1);
+    }
+    I.setFromTriplets(tripletList.begin(), tripletList.end());
+
+    // urr, a time translation operator? Gives you the value on the next frame.
+    // Leave the last row blank because that's the end of the timeline.
+    tripletList.clear();
+    Eigen::SparseMatrix<double> T(N, N);
+    Eigen::MatrixXd t_rhs;
+    for(uint32_t i = 0; i < N-1; ++i)
+    {
+        // next frame is
+        tripletList.emplace_back(i, i+1, 1.0);
+    }
+    T.setFromTriplets(tripletList.begin(), tripletList.end());
+
+    // v = Tx + t_rhs - x;
+    // v = (T - I)x + t_rhs;
+    V = T - I;
+    if (v_rhs)
+    {
+        *v_rhs = t_rhs;
+    }
+    removeRows(V, v_rhs, 2);
+
+    // a = -x + 2 (T x + t_rhs) - (T (T x + t_rhs) + t_rhs)
+    // a = (-I + 2 T - T^2) x + t_rhs - T t_rhs
+    A = 2 * T - I - T * T;
+    if (a_rhs)
+    {
+        *a_rhs = t_rhs - T * t_rhs;
+    }
+    removeRows(A, a_rhs, 3);
+
+    if (velocityWeights.size() > 0)
+    {
+        multVelWeights(V, v_rhs, velocityWeights);
+    }
+}
+
+TrajectoryCorrector::TrajectoryCorrector(
+    const Eigen::VectorXd& margins,
+    float pos_weight,
+    float vel_weight,
+    float acc_weight,
+    const Eigen::VectorXd& velocityWeights,
+    uint32_t admm_iters ) :
+    m_admm_iters(admm_iters)
+{
+
+    // This class is used to modify a trajectory to hit specific values at
+    // specific frames, while respecting the following soft constraints:
+
+    // * Preserve the original positions
+    // * Preserve the original velocities
+    // * Preserve the original accelerations
+
+    // The weights of these soft constraints are specified in "pos_weight" etc.
+
+    // This is posed as a minimization problem:
+
+    // E(x) = pos_weight * |x - x_orig|^2 + vel_weight * |V x - V x_orig| + acc_weight * |A x - A x_orig|
+
+    // where you minimize E(x) subject to specified values at indices where "mask"
+    // is equal to 1. V is a matrix that computes the N-1 velocities between frame n-1 and frame n,
+    // and A computes the N-2 accelerations associated with frames n-1, n and n+1.
+
+    // In addition to this, there are constraints where the trajectory is allowed to
+    // deviate from the target points by a maximum margin. The "margins" input to this
+    // constructor specifies what type of constraint is active on a particular frame:
+
+    // margins[0] < 0   ==> unconstrained
+    // margins[i] == 0  ==> pinned on this frame
+    // margins[i] > 0   ==> can deviate within the margin
+
+    // I'm solving the optimization problem using ADMM, ie following equations
+    // 8,9,10 on this paper:
+
+    // https://mattoverby.net/files/admm-pd-overby17.pdf
+
+    uint32_t N = uint32_t(margins.rows());
+    for(uint32_t i = 0; i < N; ++i)
+    {
+        if( margins[i] > 0 )
+        {
+            m_margin_locs.push_back(i);
+            m_margin_vals.push_back(margins[i]);
+        }
+
+        if(margins[i] == 0)
+        {
+            m_constrained_locs.push_back(i);
+        }
+        else
+        {
+            m_unconstrained_locs.push_back(i);
+        }
+    }
+
+    Eigen::SparseMatrix<double> V, A;
+    computeDiffMats(
+        V,  A,
+        N, velocityWeights
+    );
+
+    // build an identity matrix:
+    std::vector<Eigen::Triplet<double>> tripletList;
+    Eigen::SparseMatrix<double> I(N, N);
+    for (uint32_t i = 0; i < N; ++i)
+    {
+        tripletList.emplace_back(i, i, 1.0f);
+    }
+    I.setFromTriplets(tripletList.begin(), tripletList.end());
+
+    /*
+    self.N = (
+            self.pos_weight * torch.diag_embed(torch.full_like(interp_mask, 1)) +
+            self.vel_weight * torch.matmul(self.V.T, self.V) +
+            self.acc_weight * torch.matmul(self.A.T, self.A)
+        )
+    */
+
+    m_N = pos_weight * I + vel_weight * (V.transpose() * V) + acc_weight * (A.transpose() * A);
+
+    double diagMax = 0;
+    for (uint32_t i = 0; i < N; ++i)
+    {
+        diagMax = std::max(m_N.coeff(i,i), diagMax);
+    }
+    m_admm_stepsize = 0.5f * sqrtf(float(diagMax));
+
+    /*
+    M = (
+        self.N +
+        self.admm_stepsize * torch.matmul(self.S.T, self.S)
+    )
+    */
+    tripletList.clear();
+    Eigen::SparseMatrix<double> M(N, N);
+    for( auto i : m_margin_locs)
+    {
+        tripletList.emplace_back(i, i, m_admm_stepsize);
+    }
+    M.setFromTriplets(tripletList.begin(), tripletList.end());
+    M += m_N;
+
+    /*
+    self.lhsmat = torch.matmul(self.U.T, torch.matmul(self.M, self.U))
+    self.lhsmat_inv = torch.inverse(self.lhsmat)
+    */
+    tripletList.clear();
+    Eigen::SparseMatrix<double> S(m_unconstrained_locs.size(), N);
+    for (uint32_t i = 0; i < m_unconstrained_locs.size(); ++i)
+    {
+        uint32_t ifull = m_unconstrained_locs[i];
+        tripletList.emplace_back(i, ifull, 1.0f);
+    }
+    S.setFromTriplets(tripletList.begin(), tripletList.end());
+    M = S * M * S.transpose();
+
+    if(m_unconstrained_locs.size())
+    {
+        m_system_lu.compute(M);
+    }
+}
+
+
+void TrajectoryCorrector::Interpolate(
+    Eigen::MatrixXd& x,
+    const Eigen::MatrixXd& observations,
+    const Eigen::MatrixXd& ref_positions
+) const
+{
+    if(
+        m_constrained_locs.empty() &&
+        m_margin_locs.empty()
+    )
+    {
+        x = ref_positions;
+        return;
+    }
+
+    uint32_t numCols = uint32_t(x.cols());
+    if(m_margin_locs.empty())
+    {
+        x_update(
+            x,
+            Eigen::MatrixXd(0, numCols),
+            Eigen::MatrixXd(0, numCols),
+            ref_positions,
+            observations
+        );
+    }
+    else
+    {
+        x = ref_positions;
+        Eigen::MatrixXd z(m_margin_locs.size(), numCols);
+        Eigen::MatrixXd z_t(m_margin_locs.size(), numCols);
+        Eigen::MatrixXd u(m_margin_locs.size(), numCols);
+        for( uint32_t i = 0; i < m_margin_locs.size(); ++i)
+        {
+            for(uint32_t j = 0; j < numCols; ++j)
+            {
+                z_t(i, j) = observations(m_margin_locs[i], j);
+                z(i, j) = ref_positions(m_margin_locs[i], j);
+                u(i, j) =0;
+            }
+        }
+
+        for(uint32_t i = 0; i < m_admm_iters; ++i)
+        {
+            x_update(
+                x,
+                z,
+                u,
+                ref_positions,
+                observations
+            );
+            z_update(z, x, z_t, u);
+            u_update(u, x, z);
+        }
+    }
+
+}
+
+void TrajectoryCorrector::x_update(
+    Eigen::MatrixXd &x,
+    const Eigen::MatrixXd &z,
+    const Eigen::MatrixXd &u,
+    const Eigen::MatrixXd &x_t, // reference positions - defines the original shape of the curve that we want to preserve
+    const Eigen::MatrixXd &x_o  // target positions for constraints
+) const
+{
+
+    uint32_t numRows = uint32_t(x.rows());
+    uint32_t numCols = uint32_t(x.cols());
+
+    // Here's what we're minimizing with ADMM:
+    // min f(x) + g(z)
+    // s.t A x + B z = c
+
+    // Make these choices so that z = S x:
+    // A = S, B = -I, c = 0
+    //
+    // g(z) = infinity when it's too far away from z_target, zero otherwise
+    //
+    // f(x) penalizes deviations in position, velocity and acceleration
+    // from a reference trajectory:
+    //
+    // f(x) = 1/2(
+    //    kx |I x - x_t|^2 +
+    //    kv |V x - v_t|^2 +
+    //    kx |A x - a_t|^2
+    // )
+    //
+    // It's also infinite when components of x devaiate from their target
+    // values where they're pinned...
+
+    // Substituting the matrices into the standard admm update rules gives us this:
+    // x{n+1} = argmin(f(x) + ρ/2 |S x - z{n} + u{n}|^2)
+    // z{n+1} = argmin(g(z) + ρ/2 |S x{n+1} - z + u{n}|^2)
+    // u{n+1} = u{n} + (S x{n+1} - z{n+1})
+    //
+
+    // x update:
+    //
+    // x{n+1} = argmin  1/2 (
+    //     kx |I x - x_t|^2 +
+    //     kv |V x - v_t|^2 +
+    //     ka |A x - a_t|^2 +
+    //     ρ  |S x - d|^2
+    // )
+    // d = (z{n} - u{n})
+
+    // Rewrite in a friendlier way:
+    // |A x - b|^2 = x^T A^T A x - 2 x^T A^T b + C
+    // 1/2 (
+    //     kx (x^T x - 2 x^T x_t) +
+    //     kv (x^T V^T V x - 2 x^T V^T v_t) +
+    //     ka (x^T A^T A x - 2 x^T A^T a_t) +
+    //     ρ  (x^T S^T S x - 2 x^T S^T d)
+    // ) + C
+    //
+    // 1/2 x^T (kx I + kv V^T V + ka A^T A + ρ S^T S) x
+    //   - x^T (kx x_t + kv V^T v_t + ka A^T a_t + ρ S^T d)
+    // + C
+    //
+    // voila:
+    // M = kx I + kv V^T V + ka A^T A + ρ S^T S
+    // r = kx x_t + kv V^T v_t + ka A^T a_t + ρ S^T d
+    // E = 1/2 x^T M x - x^T r + C
+
+    /*
+    r = (
+        torch.matmul(self.N, x_t - x_o_filtered) +
+        self.admm_stepsize * torch.matmul(self.S.T, - u + z)
+    )
+    */
+    Eigen::MatrixXd x_diffs(x_t);
+    for(auto i : m_constrained_locs)
+    {
+        for(uint32_t j = 0; j < numCols; ++j)
+        {
+            x_diffs(i, j) = x_diffs(i,j) - x_o(i,j);
+        }
+    }
+
+    Eigen::MatrixXd r = m_N * x_diffs;
+
+    for(uint32_t i = 0; i < m_margin_locs.size(); ++i)
+    {
+        uint32_t ifull = m_margin_locs[i];
+        for(uint32_t j = 0; j < numCols; ++j)
+        {
+            r(ifull, j) = r(ifull, j) + m_admm_stepsize * (z(i,j) - u(i,j));
+        }
+    }
+
+    // Solve with respect to pin constraints:
+    // x = U x_r + x_o
+    // E = 1/2 (U x_r + x_o)^T M (U x_r + x_o) - (U x_r + x_o)^T r + C
+    // E = 1/2 (x_r^T U^T + x_o^T) M (U x_r + x_o) - (x_r^T U^T + x_o^T) r + C
+    // E = 1/2 (x_r^T U^T M (U x_r + x_o) + x_o^T M (U x_r + x_o)) - x_r^T U^T r - x_o^T r + C
+    // E = 1/2 (x_r^T U^T M U x_r) + x_r^T U^T (M x_o - r) + C
+
+    // minimized when x_r solves this equation:
+    // U^T M U x_r + U^T (M x_o - r) = 0
+    // x_r = (U^T M U)^-1 U^T (r - M x_o)
+
+    // collapse r down to unconstrained variable set:
+    // rhs = torch.matmul(self.U.T, r)
+
+    uint32_t numRows_reduced = m_unconstrained_locs.size();
+    Eigen::MatrixXd r_reduced(numRows_reduced, numCols);
+    for(uint32_t i = 0; i < numRows_reduced; ++i)
+    {
+        uint32_t ifull = m_unconstrained_locs[i];
+        for(uint32_t j = 0; j < numCols; ++j)
+        {
+            r_reduced(i,j) = r(ifull, j);
+        }
+    }
+
+    // solve system:
+    // x_r = torch.matmul(self.lhsmat_inv, rhs)
+    r_reduced.conservativeResize(r_reduced.rows(), r_reduced.cols());
+
+    Eigen::MatrixXd result;
+    if(m_unconstrained_locs.size())
+    {
+        result = m_system_lu.solve(r_reduced);
+    }
+
+    // map back to full variable set:
+    // return torch.matmul(self.U, x_r) + x_o_filtered
+    for(uint32_t i = 0; i < numRows_reduced; ++i)
+    {
+        uint32_t ifull = m_unconstrained_locs[i];
+        for(uint32_t j = 0; j < numCols; ++j)
+        {
+            x(ifull, j) = result(i, j);
+        }
+    }
+    for(auto i : m_constrained_locs)
+    {
+        for(uint32_t j = 0; j < numCols; ++j)
+        {
+            x(i, j) = x_o(i, j);
+        }
+    }
+}
+
+void TrajectoryCorrector::z_update(
+    Eigen::MatrixXd &z,
+    const Eigen::MatrixXd &x,
+    const Eigen::MatrixXd &z_t,
+    const Eigen::MatrixXd &u
+) const
+{
+    uint32_t numCols = uint32_t(z.cols());
+
+    for(uint32_t i = 0; i < m_margin_locs.size(); ++i)
+    {
+
+        // z_diffs = S x + u - z_t
+        uint32_t ifull = m_margin_locs[i];
+        for(uint32_t j = 0; j < numCols; ++j)
+        {
+            z(i, j) = x(ifull, j) + u(i, j) - z_t(i, j);
+        }
+
+        // find the norm of the current z diff vector:
+        double z_diff_norm = 0.0;
+        for(uint32_t j = 0; j < numCols; ++j)
+        {
+            double z_diff = z(i, j);
+            z_diff_norm += z_diff * z_diff;
+        }
+        z_diff_norm = sqrt(z_diff_norm);
+
+        // if the norm is greater than the margin size, we need to rescale
+        // the diff:
+        if( z_diff_norm > m_margin_vals[i] )
+        {
+            for(uint32_t j = 0; j < numCols; ++j)
+            {
+                z(i, j) = z(i, j) * m_margin_vals[i] / z_diff_norm;
+            }
+        }
+
+        // add the diff back on to the target:
+        for(uint32_t j = 0; j < numCols; ++j)
+        {
+            z(i, j) = z_t(i, j) + z(i, j);
+        }
+    }
+}
+
+void TrajectoryCorrector::u_update(
+    Eigen::MatrixXd &u,
+    const Eigen::MatrixXd &x,
+    const Eigen::MatrixXd &z
+) const
+{
+    uint32_t numCols = uint32_t(z.cols());
+
+    // u += S x - z
+    for(uint32_t i = 0; i < m_margin_locs.size(); ++i)
+    {
+        uint32_t ifull = m_margin_locs[i];
+        for(uint32_t j = 0; j < numCols; ++j)
+        {
+            u(i,j) += x(ifull, j) - z(i,j);
+        }
+    }
+}

MotionCorrection/src/cpp/AnimProcessing/TrajectoryCorrector.h

@@ -0,0 +1,77 @@
+/*
+ * SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
+ * SPDX-License-Identifier: Apache-2.0
+ */
+
+#pragma once
+
+#include <Eigen/Sparse>
+
+class TrajectoryCorrector
+{
+public:
+
+	static void computeDiffMats(
+		Eigen::SparseMatrix<double>& V,
+		Eigen::SparseMatrix<double>& A,
+		uint32_t N,
+		const Eigen::VectorXd& velocityWeights = Eigen::VectorXd(),
+		Eigen::MatrixXd* v_rhs = nullptr,
+		Eigen::MatrixXd* a_rhs = nullptr
+	);
+
+	TrajectoryCorrector(
+		const Eigen::VectorXd& margins,
+        float pos_weight,
+        float vel_weight,
+        float acc_weight,
+		const Eigen::VectorXd& velocityWeights = Eigen::VectorXd(),
+		uint32_t admm_iters=100 );
+
+	void Interpolate(
+		Eigen::MatrixXd& ret,
+		const Eigen::MatrixXd& observations,
+		const Eigen::MatrixXd& ref_positions
+	) const;
+
+	void x_update(
+		Eigen::MatrixXd& x,
+		const Eigen::MatrixXd& z,
+		const Eigen::MatrixXd& u,
+		const Eigen::MatrixXd& x_t,
+		const Eigen::MatrixXd& x_o
+	) const;
+
+	void z_update(
+		Eigen::MatrixXd& z,
+		const Eigen::MatrixXd& x,
+		const Eigen::MatrixXd& z_t,
+		const Eigen::MatrixXd& u
+	) const;
+
+	void u_update(
+		Eigen::MatrixXd& u,
+		const Eigen::MatrixXd& x,
+		const Eigen::MatrixXd& z
+	) const;
+
+	float admm_stepsize() const { return m_admm_stepsize; }
+
+	const std::vector<uint32_t>& margin_locs() { return m_margin_locs; }
+
+private:
+
+	Eigen::SparseMatrix<double> m_N;
+	Eigen::SparseLU<Eigen::SparseMatrix<double>> m_system_lu;
+
+	uint32_t m_admm_iters;
+
+	std::vector<uint32_t> m_margin_locs;
+	std::vector<double> m_margin_vals;
+
+	std::vector<uint32_t> m_unconstrained_locs;
+	std::vector<uint32_t> m_constrained_locs;
+
+	float m_admm_stepsize;
+
+};

MotionCorrection/src/cpp/AnimProcessing/Utility.cpp

@@ -0,0 +1,1197 @@
+/*
+ * SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
+ * SPDX-License-Identifier: Apache-2.0
+ */
+
+#include "TrajectoryCorrector.h"
+#include "InverseKinematics.h"
+
+#include "Utility.h"
+
+#include <map>
+#include <array>
+#include <cmath>
+#include <cstdlib>
+#include <iostream>
+using Pose = std::vector<Math::Transform>;
+
+static const float pos_weight = 0.001f;
+static const float vel_weight = 1.0f;
+static const float acc_weight = 10.0f;
+
+
+namespace {
+
+    // Enable with: MOTIONCORRECTION_DEBUG_INTERVALS=1
+    // Default: off (no Interval printing).
+    bool DebugPrintIntervalsEnabled()
+    {
+        const char* v = std::getenv("MOTIONCORRECTION_DEBUG_INTERVALS");
+        if (v == nullptr || v[0] == '\0')
+        {
+            return false;
+        }
+        // Treat "0" as false; any other non-empty value enables.
+        return v[0] != '0';
+    }
+
+
+    void FilterContactIntervals(
+        std::vector<std::pair<int, int>>& contactIntervals,
+        const std::vector<float>& mask,
+        bool one_bone_contact = false)
+    {
+        std::vector<uint32_t> keepIntervals;
+        for (size_t i = 0; i < contactIntervals.size(); ++i)
+        {
+            const auto& interval = contactIntervals[i];
+
+            bool startConstrained = (interval.first != 0 && mask[interval.first - 1]);
+            bool endConstrained;
+
+            endConstrained = (interval.second != mask.size() && mask[interval.second]);
+
+            if (one_bone_contact)
+            {
+                if (startConstrained || endConstrained)
+                {
+                    continue;
+                }
+            }
+            else
+            {
+                // If both the start and end of the contact interval are masked,
+                // there's no way we can correct the contact without popping, so
+                // let's filter these out:
+                if (startConstrained && endConstrained)
+                {
+                    continue;
+                }
+            }
+
+            keepIntervals.push_back(i);
+        }
+
+        for (size_t i = 0; i < keepIntervals.size(); ++i)
+        {
+            contactIntervals[i] = contactIntervals[keepIntervals[i]];
+        }
+        contactIntervals.resize(keepIntervals.size());
+    }
+
+    std::vector<std::pair<int, int>> ComputeContactIntervals(
+        const std::vector<float>& contacts,
+        const std::vector<float>& mask,
+        float contactThreshold)
+    {
+        // turn off the contacts for all frames that are constrained/masked:
+        std::vector<float> contactsNoMask = contacts;
+        for (size_t i = 0; i < mask.size(); ++i)
+        {
+            if (mask[i])
+            {
+                contactsNoMask[i] = 0;
+            }
+        }
+
+        // Find intervals that are in contact:
+        std::vector<std::pair<int, int>> contactIntervals;
+        int start = -1;
+        for (int frame = 0; frame < mask.size(); ++frame)
+        {
+            bool isContact = contactsNoMask[frame] > contactThreshold;
+            if (isContact && start == -1)
+            {
+                start = frame;
+            }
+            else if (!isContact && start != -1)
+            {
+                contactIntervals.emplace_back(start, frame);
+                start = -1;
+            }
+        }
+
+        // Close the final interval if needed:
+        if (start != -1)
+        {
+            contactIntervals.emplace_back(start, mask.size());
+        }
+        return contactIntervals;
+    }
+
+    void FindContactPoints(
+        std::vector<Math::Vector> &points,
+        std::vector<int> &inContact,
+        const std::vector<int>& joint_parents_vec,
+        int32_t jointIndex,
+        const std::vector<Pose> &poses,
+        const std::vector<std::pair<int, int>>& contactIntervals,
+        const std::vector<float>& mask,
+        size_t frameCount,
+        float minHeight)
+    {
+        // Find a representative frame for each interval.
+        // If the interval starts after a masked frame, use the start
+        // of the interval, if it ends before a mask use the end,
+        // otherwise use the middle frame.
+        inContact.clear();
+        inContact.resize(frameCount, 0);
+        points.clear();
+        points.resize(frameCount);
+        for (size_t i = 0; i < contactIntervals.size(); ++i)
+        {
+            const auto& interval = contactIntervals[i];
+            int frame = -1;
+            bool startConstrained = (interval.first != 0 && mask[interval.first - 1]);
+            bool endConstrained;
+
+            endConstrained = (interval.second != mask.size() && mask[interval.second]);
+
+            // Debug output (opt-in via env var)
+            if (DebugPrintIntervalsEnabled())
+            {
+                std::cout << "Interval " << i << ": start=" << interval.first
+                          << ", end=" << interval.second
+                          << ", startConstrained=" << startConstrained
+                          << ", endConstrained=" << endConstrained << std::endl;
+            }
+
+            if(startConstrained)
+            {
+                // If the interval starts on a constraint, use the constrained frame
+                // as a target (doing this modulo mask.size() in case we're looping)
+                frame = interval.first - 1;
+            }
+            else if (endConstrained)
+            {
+                // If the interval ends on a constraint, use the constrained frame
+                // as a target:
+                frame = interval.second;
+            }
+            else
+            {
+                // Otherwise use the midpoint of the interval:
+                frame = (interval.first + interval.second) / 2;
+            }
+
+            // get the target point:
+            Math::Vector target = Animation::JointLocalToGlobal(joint_parents_vec, jointIndex, poses[frame]).GetTranslation();
+            for(int i = interval.first; i < interval.second; ++i)
+            {
+                Math::Vector framePt = Animation::JointLocalToGlobal(joint_parents_vec, jointIndex, poses[i]).GetTranslation();
+                inContact[i] = 1;
+                points[i] = target;
+                if (!startConstrained && !endConstrained)
+                {
+                    points[i].SetY(std::max(framePt.GetY(), minHeight));
+                    // std::cout << "  Frame " << i << ": SetY with framePt.GetY()=" << framePt.GetY()
+                    //           << ", minHeight=" << minHeight << std::endl;
+                }
+            }
+        }
+    }
+
+    float TargetReachFalloff(
+        const std::vector<int>& joint_parents_vec,
+        const Pose& defaultPose,
+        int32_t jointIndex,
+        Animation::IKType ikType,
+        const Math::Vector& target,
+        const Pose& pose,
+        const Math::Transform& rootTx = Math::Transform::Identity)
+    {
+        float maxReach = defaultPose[jointIndex].GetTranslation().GetLength3();
+        if (ikType == Animation::IKType::kTwoBone)
+        {
+            jointIndex = joint_parents_vec[jointIndex];
+            ASSERT(jointIndex > -1);
+            maxReach += defaultPose[jointIndex].GetTranslation().GetLength3();
+        }
+        // Get base joint world Tx
+        jointIndex = joint_parents_vec[jointIndex];
+        ASSERT(jointIndex > -1);
+        const auto worldTx = Animation::JointLocalToGlobal(joint_parents_vec, jointIndex, pose, rootTx);
+
+        // Gaussian falloff
+        float targetDist = target.GetDistance3(worldTx.GetTranslation());
+        float tmp = Math::Max(targetDist / maxReach - 0.99f, 0.f) / 0.01f;
+        tmp = tmp * tmp;
+        return std::exp(-2.f * tmp * tmp);
+    }
+
+    void CorrectHipsY(
+        std::vector<Pose>& poses,
+        const std::vector<Pose>& targetPoses,
+        const std::vector<float>& fullBodyMask,
+        const std::vector<Animation::ContactInfo>& contacts,
+        float contactThreshold
+    )
+    {
+        // Correct the y coordinates of the root.
+        auto N = poses.size();
+        Eigen::MatrixXd x(N, 1);
+        Eigen::MatrixXd observations(N, 1);
+        Eigen::MatrixXd xfixed(N, 1);
+
+        // Fill in the initial trajectory (x) and the values we want to hit when we
+        // warp it (observations):
+        Eigen::VectorXd yCorrectMargins(N);
+        for(size_t frame = 0; frame < N; ++frame)
+        {
+            yCorrectMargins[frame] = fullBodyMask[frame] ? 0.0f : -1.0f;
+            x(frame, 0) = ((float*)&poses[frame][0].GetTranslation())[1];
+            observations(frame, 0) = ((float*)&targetPoses[frame][0].GetTranslation())[1];
+        }
+
+        TrajectoryCorrector ycorrector(
+            yCorrectMargins,
+            pos_weight * 10,
+            vel_weight,
+            acc_weight * 0.1f
+        );
+        ycorrector.Interpolate(
+            xfixed,
+            observations,
+            x
+        );
+
+        // fill channel again:
+        for (uint32_t frame = 0; frame < N; ++frame)
+        {
+            ((float*)&poses[frame][0].GetTranslation())[1] = float(xfixed(frame, 0));
+        }
+    }
+
+    void SmoothChannels(
+        Eigen::MatrixXd &x,
+        const std::vector<float>& mask
+    )
+    {
+        for( uint32_t i=0; i < mask.size(); ++i)
+        {
+            uint32_t i_prev = i == 0 ? 0 : i-1;
+            uint32_t i_next = std::min(uint32_t(i+1), uint32_t(mask.size()-1));
+            if(i > 0 && mask[i] > 0 && mask[i_prev] == 0)
+            {
+                // if the previous frame is unconstrained and the current frame is constrained,
+                // replace the current frame with the average of its neighbors:
+                for(long j=0; j < x.cols(); ++j)
+                {
+                    x(i, j) = 0.5f * (x(i_prev, j) + x(i_next, j));
+                }
+            }
+            if(mask[i] > 0 && mask[i_next] == 0)
+            {
+                // if the next frame is unconstrained and the current frame is constrained,
+                // replace the current frame with the average of its neighbors:
+                for(long j=0; j < x.cols(); ++j)
+                {
+                    x(i, j) = 0.5f * (x(i_prev, j) + x(i_next, j));
+                }
+            }
+        }
+    }
+
+
+    void CorrectHipsXZ(
+        std::vector<Pose>& poses,
+        const std::vector<Pose>& targetPoses,
+        const std::vector<float>& fullBodyMask,
+        const std::vector<float>& rootMask,
+        const std::vector<Animation::ContactInfo>& endEffectorPins,
+        const Eigen::VectorXd& velocity_weights,
+        float root_margin
+    )
+    {
+        auto N = poses.size();
+        Eigen::VectorXd margins(N);
+        for( size_t i = 0; i < N; ++i )
+        {
+            margins[i] = fullBodyMask[i] ? 0.0f : -1.0f;
+        }
+
+        std::vector<float> rootCombinedMask(N, 0.0f);
+        for(size_t i = 0; i < N; ++i)
+        {
+            rootCombinedMask[i] = (fullBodyMask[i] > 0) || (rootMask[i] > 0);
+            if(rootMask[i] > 0 && margins[i] != 0)
+            {
+                margins[i] = root_margin;
+            }
+            for (auto& c : endEffectorPins)
+            {
+                if (c.contactMask[i] && margins[i] != 0)
+                {
+                    margins[i] = root_margin;
+                }
+            }
+        }
+        TrajectoryCorrector xzcorrector(
+            margins,
+            pos_weight,
+            vel_weight,
+            acc_weight,
+            velocity_weights
+        );
+
+        // Enforce pose constraints on root xz trajectory:
+        Eigen::MatrixXd x(N, 2);
+        Eigen::MatrixXd observations(N, 2);
+        Eigen::MatrixXd x_fixed(N, 2);
+
+        observations.setZero();
+        for (uint32_t frame = 0; frame < N; ++frame)
+        {
+            x(frame, 0) = ((float*)&poses[frame][0].GetTranslation())[0];
+            x(frame, 1) = ((float*)&poses[frame][0].GetTranslation())[2];
+
+            observations(frame, 0) = ((float*)&targetPoses[frame][0].GetTranslation())[0];
+            observations(frame, 1) = ((float*)&targetPoses[frame][0].GetTranslation())[2];
+        }
+
+        SmoothChannels(x, rootCombinedMask);
+
+        xzcorrector.Interpolate(
+            x_fixed,
+            observations,
+            x
+        );
+
+        // fill channels again:
+        for (uint32_t frame = 0; frame < N; ++frame)
+        {
+            ((float*)&poses[frame][0].GetTranslation())[0] = float(x_fixed(frame, 0));
+            ((float*)&poses[frame][0].GetTranslation())[2] = float(x_fixed(frame, 1));
+        }
+    }
+
+    void CorrectRotationsForBone(
+        std::vector<Pose>& poses,
+        const std::vector<Pose>& targetPoses,
+        const std::vector<float>& mask,
+        const TrajectoryCorrector& corrector,
+        int boneIdx,
+        bool performChannelSmoothing)
+    {
+        auto N = poses.size();
+        Eigen::MatrixXd x(N, 1);
+        Eigen::MatrixXd observations(N, 1);
+        observations.setZero();
+        Eigen::MatrixXd x_fixed(N, 1);
+
+        // Quaternion components can flip when they pass through 180 degree
+        // rotations, so let's convert all the quaternions in this channel to
+        // the forward/up vector representation, modify them, then convert back
+        // to quaternions:
+
+        // convert time series to 6d forward/up:
+        std::vector<float> forwardUp(6 * N);
+        std::vector<float> targetForwardUp(6 * N);
+        for (uint32_t frame = 0; frame < N; ++frame)
+        {
+            auto q = poses[frame][boneIdx].GetRotation();
+            auto forward = q.ZAxis();
+            auto up = q.YAxis();
+            forwardUp[N * 0 + frame] = forward.GetX();
+            forwardUp[N * 1 + frame] = forward.GetY();
+            forwardUp[N * 2 + frame] = forward.GetZ();
+            forwardUp[N * 3 + frame] = up.GetX();
+            forwardUp[N * 4 + frame] = up.GetY();
+            forwardUp[N * 5 + frame] = up.GetZ();
+
+            q = targetPoses[frame][boneIdx].GetRotation();
+            forward = q.ZAxis();
+            up = q.YAxis();
+            targetForwardUp[N * 0 + frame] = forward.GetX();
+            targetForwardUp[N * 1 + frame] = forward.GetY();
+            targetForwardUp[N * 2 + frame] = forward.GetZ();
+            targetForwardUp[N * 3 + frame] = up.GetX();
+            targetForwardUp[N * 4 + frame] = up.GetY();
+            targetForwardUp[N * 5 + frame] = up.GetZ();
+        }
+
+        // correct trajectories:
+        for (uint32_t dim = 0; dim < 6; ++dim)
+        {
+            for (uint32_t frame = 0; frame < N; ++frame)
+            {
+                x(frame, 0) = forwardUp[N * dim + frame];
+                observations(frame, 0) = mask[frame] * targetForwardUp[N * dim + frame];
+            }
+
+            if (performChannelSmoothing)
+            {
+                SmoothChannels(x, mask);
+            }
+
+            corrector.Interpolate(
+                x_fixed,
+                observations,
+                x
+            );
+
+            // fill channel again:
+            for (uint32_t frame = 0; frame < N; ++frame)
+            {
+                forwardUp[N * dim + frame] = float(x_fixed(frame, 0));
+            }
+        }
+
+        for (uint32_t frame = 0; frame < N; ++frame)
+        {
+            Math::Vector forward = { forwardUp[N * 0 + frame] ,forwardUp[N * 1 + frame] ,forwardUp[N * 2 + frame] };
+            Math::Vector up = { forwardUp[N * 3 + frame] ,forwardUp[N * 4 + frame] ,forwardUp[N * 5 + frame] };
+
+            forward.Normalize3();
+            up.Normalize3();
+
+            poses[frame][boneIdx].SetRotation(Math::Quaternion::LookRotation(forward, up));
+        }
+    }
+
+    void CorrectJointRotations(
+        std::vector<Pose>& poses,
+        const std::vector<Pose>& targetPoses,
+        const std::vector<float>& fullBodyMask,
+        const Eigen::VectorXd& velocity_weights
+    )
+    {
+        auto N = poses.size();
+
+        // Create a trajectory corrector for fixing the full body fullBodyMask positions:
+        Eigen::VectorXd margins(N);
+        for( size_t i = 0; i < N; ++i )
+        {
+            margins[i] = fullBodyMask[i] ? 0.0f : -1.0f;
+        }
+        TrajectoryCorrector corrector(
+            margins,
+            pos_weight * 10,
+            vel_weight,
+            acc_weight,
+            velocity_weights
+        );
+
+        for (uint32_t boneIdx = 0; boneIdx < poses[0].size(); ++boneIdx)
+        {
+            CorrectRotationsForBone(
+                poses,
+                targetPoses,
+                fullBodyMask,
+                corrector,
+                boneIdx,
+                true
+            );
+        }
+    }
+
+    void DoEffectorIK(
+        std::vector<Pose>& poses,
+        const std::vector<Pose>& targetPoses,
+        const std::vector<float>& fullBodyMask,
+        const std::vector<Animation::ContactInfo>& endEffectorPins,
+        const std::vector<int>& joint_parents_vec,
+        const std::vector<Math::Transform>& defaultPose
+    )
+    {
+        // Apply IK for effector pins
+        auto N = poses.size();
+        std::map<uint32_t, std::vector<float>> jointCorrectionMasks;
+        std::vector<Pose> ikFixedPoses = poses;
+        for (auto& c : endEffectorPins)
+        {
+            auto jointIdx = c.jointIndex;
+
+            if(jointCorrectionMasks[jointIdx].empty())
+            {
+                // initialize to the full body constraint mask because we
+                // want to constrain that anyway:
+                jointCorrectionMasks[jointIdx] = fullBodyMask;
+            }
+
+            // Add a trajectory correction mask for the parent joint:
+            auto parentIdx = joint_parents_vec[jointIdx];
+            if(jointCorrectionMasks[parentIdx].empty())
+            {
+                // initialize to the full body constraint mask because we
+                // want to constrain that anyway:
+                jointCorrectionMasks[parentIdx] = fullBodyMask;
+            }
+
+            // Add a trajectory correction mask for its parent if this is
+            // 2 bone IK:
+            auto parentParentIdx = joint_parents_vec[parentIdx];
+            if(c.contactType == Animation::kTwoBone)
+            {
+                if(jointCorrectionMasks[parentParentIdx].empty())
+                {
+                    // initialize to the full body constraint mask because we
+                    // want to constrain that anyway:
+                    jointCorrectionMasks[parentParentIdx] = fullBodyMask;
+                }
+            }
+
+            for (uint32_t fixFrame = 0; fixFrame < fullBodyMask.size(); ++fixFrame)
+            {
+                if (c.contactMask[fixFrame])
+                {
+                    const auto targetGlobalTransform = Animation::JointLocalToGlobal(joint_parents_vec, jointIdx, targetPoses[fixFrame]);
+
+                    // flag the parent joint as fixed in its correction mask:
+                    jointCorrectionMasks[parentIdx][fixFrame] = 1;
+                    switch(c.contactType)
+                    {
+                        case Animation::kOneBone:
+                        {
+                            IK::OneBoneIk(
+                                ikFixedPoses[fixFrame],
+                                Math::Transform::Identity,
+                                jointIdx,
+                                1.0,
+                                targetGlobalTransform.GetTranslation(),
+                                joint_parents_vec
+                            );
+                            break;
+                        }
+                        case Animation::kTwoBone:
+                        {
+                            // flag the parent parent joint as fixed in its correction mask:
+                            jointCorrectionMasks[parentParentIdx][fixFrame] = 1;
+                            IK::TwoBoneIk(
+                                ikFixedPoses[fixFrame],
+                                Math::Transform::Identity,
+                                jointIdx,
+                                1.0,
+                                targetGlobalTransform.GetTranslation(),
+                                joint_parents_vec,
+                                c.hintOffset
+                            );
+                            break;
+                        }
+                    }
+
+                    // now we need to fix things so the global rotation of the joint
+                    // matches the input:
+                    jointCorrectionMasks[jointIdx][fixFrame] = 1;
+                    auto parentGlobalTransform = Animation::JointLocalToGlobal(joint_parents_vec, parentIdx, ikFixedPoses[fixFrame]);
+                    ikFixedPoses[fixFrame][jointIdx].SetRotation(
+                        targetGlobalTransform.GetRotation() * parentGlobalTransform.GetRotation().GetConjugate()
+                    );
+
+                }
+            }
+        }
+
+        // Applying the effector pin IK introduces popping into the animation,
+        // so let's apply the interpolator to all the joints we modified so as to
+        // line the trajectory up properly again:
+        Eigen::VectorXd margins(N);
+        for( auto &kv : jointCorrectionMasks)
+        {
+            for( size_t i = 0; i < N; ++i )
+            {
+                margins[i] = kv.second[i] ? 0.0f : -1.0f;
+            }
+            TrajectoryCorrector corrector(margins, pos_weight * 10, vel_weight, acc_weight);
+
+            CorrectRotationsForBone(
+                poses,
+                ikFixedPoses,
+                kv.second,
+                corrector,
+                kv.first,
+                false
+            );
+        }
+    }
+
+    void DoContactIK(
+        std::vector<Pose>& poses,
+        const std::vector<float>& fullBodyMask,
+        const std::vector<Animation::ContactInfo>& contacts,
+        const std::vector<Animation::ContactInfo>& endEffectorPins,
+        const std::vector<int>& joint_parents_vec,
+        const std::vector<Math::Transform>& defaultPose,
+        float contactThreshold,
+        bool has_double_ankle_joints
+    )
+    {
+        auto N = poses.size();
+        Eigen::VectorXd margins = Eigen::VectorXd::Zero(N);
+
+        // Apply IK to stabilize limbs on contacts
+        std::map<uint32_t, std::vector<float>> jointCorrectionMasks;
+        std::vector<Pose> ikFixedPoses = poses;
+
+        // Save original poses before any modifications (for double ankle correction later)
+        const std::vector<Pose> originalPoses = poses;
+
+        // Track which frames were corrected for each 2-bone contact (for double ankle correction later)
+        std::map<uint32_t, std::vector<bool>> twoBoneContactFrames;
+
+        auto addEndEffectorMask = [&](uint32_t jointIdx, uint32_t parentIdx, std::vector<float>& jointMask)
+        {
+            auto it = std::find_if(
+                endEffectorPins.begin(), endEffectorPins.end(),
+                [&](const auto &c)
+                {
+                    if(jointIdx == c.jointIndex)
+                    {
+                        return true;
+                    }
+                    return false;
+                }
+            );
+            if(it == endEffectorPins.end())
+            {
+                // We could be correcting the toe joint, in which case we need to use
+                // the parent joint instead:
+                it = std::find_if(
+                    endEffectorPins.begin(), endEffectorPins.end(),
+                    [&](const auto &c)
+                    {
+                        if(parentIdx == c.jointIndex)
+                        {
+                            return true;
+                        }
+                        return false;
+                    }
+                );
+            }
+            if(it != endEffectorPins.end())
+            {
+                const auto &msk = it->contactMask;
+                for(size_t i=0; i < msk.size(); ++i)
+                {
+                    if(msk[i])
+                    {
+                        jointMask[i] = 1.0f;
+                    }
+                }
+            }
+        };
+
+        // Process two bone contacts first:
+        for (auto& c : contacts)
+        {
+            if(c.contactType != Animation::kTwoBone)
+            {
+                continue;
+            }
+            const auto jointIdx = c.jointIndex;
+            auto parentIdx = joint_parents_vec[jointIdx];
+            auto parentParentIdx = joint_parents_vec[parentIdx];
+
+            auto jointMask = fullBodyMask;
+            addEndEffectorMask(jointIdx, parentIdx, jointMask);
+
+            // We'll actually be modifying 3 joints here:
+            // * The two joints immediately up in the hierarchy because of the 2 bone IK
+            // * The joint itself because we restore its original global rotation
+            if(jointCorrectionMasks[parentIdx].empty())
+            {
+                jointCorrectionMasks[parentIdx] = jointMask;
+            }
+            if(jointCorrectionMasks[parentParentIdx].empty())
+            {
+                jointCorrectionMasks[parentParentIdx] = jointMask;
+            }
+            if(jointCorrectionMasks[jointIdx].empty())
+            {
+                jointCorrectionMasks[jointIdx] = jointMask;
+            }
+
+            // Compute the intervals in which the joint is in contact with the floor:
+            auto contactIntervals = ComputeContactIntervals(c.contactMask, jointMask, contactThreshold);
+            FilterContactIntervals(contactIntervals, jointMask);
+
+            std::vector<Math::Vector> contactPoints;
+            std::vector<int> inContact;
+            FindContactPoints(
+                contactPoints,
+                inContact,
+                joint_parents_vec,
+                jointIdx,
+                poses,
+                contactIntervals,
+                jointMask,
+                c.contactMask.size(),
+                c.minHeight
+            );
+
+            for (uint32_t fixFrame = 0; fixFrame < fullBodyMask.size(); ++fixFrame)
+            {
+                if (inContact[fixFrame])
+                {
+                    auto target = contactPoints[fixFrame];
+                    jointCorrectionMasks[parentIdx][fixFrame] = 1.0f;
+                    jointCorrectionMasks[parentParentIdx][fixFrame] = 1.0f;
+                    jointCorrectionMasks[jointIdx][fixFrame] = 1.0f;
+
+                    // Track this frame for double ankle correction later
+                    if (has_double_ankle_joints)
+                    {
+                        if (twoBoneContactFrames[jointIdx].empty())
+                            twoBoneContactFrames[jointIdx].resize(fullBodyMask.size(), false);
+                        twoBoneContactFrames[jointIdx][fixFrame] = true;
+                    }
+
+                    // save the original global rotation of the joint:
+                    auto jointGlobalRotation = Animation::JointLocalToGlobal(
+                        joint_parents_vec,
+                        jointIdx,
+                        ikFixedPoses[fixFrame]
+                    ).GetRotation();
+
+                    const float w = TargetReachFalloff(
+                        joint_parents_vec,
+                        defaultPose,
+                        jointIdx,
+                        c.contactType,
+                        target,
+                        ikFixedPoses[fixFrame]
+                    );
+                    // std::cout << "Frame " << fixFrame << ": w=" << w << std::endl;
+
+                    // apply the 2 bone IK:
+                    auto origParentRotation = ikFixedPoses[fixFrame][parentIdx].GetRotation();
+                    auto origParentParentRotation = ikFixedPoses[fixFrame][parentParentIdx].GetRotation();
+                    IK::TwoBoneIk(
+                        ikFixedPoses[fixFrame],
+                        Math::Transform::Identity,
+                        jointIdx,
+                        1.0f,
+                        target,
+                        joint_parents_vec,
+                        c.hintOffset
+                    );
+                    ikFixedPoses[fixFrame][parentIdx].SetRotation(Math::Quaternion::SLerp(origParentRotation, ikFixedPoses[fixFrame][parentIdx].GetRotation(), w));
+                    ikFixedPoses[fixFrame][parentParentIdx].SetRotation(Math::Quaternion::SLerp(origParentParentRotation, ikFixedPoses[fixFrame][parentParentIdx].GetRotation(), w));
+
+                    // restore previous global rotation of this joint:
+                    auto parentGloblalRotation = Animation::JointLocalToGlobal(
+                        joint_parents_vec,
+                        parentIdx,
+                        ikFixedPoses[fixFrame]
+                    ).GetRotation();
+
+                    jointCorrectionMasks[jointIdx][fixFrame] = 1.0f;
+                    ikFixedPoses[fixFrame][jointIdx].SetRotation(
+                        jointGlobalRotation * parentGloblalRotation.GetConjugate()
+                    );
+
+                    auto result = Animation::JointLocalToGlobal(
+                        joint_parents_vec,
+                        jointIdx,
+                        ikFixedPoses[fixFrame]
+                    ).GetTranslation();
+                }
+            }
+
+        }
+
+        for( auto &kv : jointCorrectionMasks)
+        {
+            for( size_t i = 0; i < N; ++i )
+            {
+                margins[i] = kv.second[i] ? 0.0f : -1.0f;
+            }
+            TrajectoryCorrector corrector(margins, pos_weight * 10, vel_weight, acc_weight);
+            CorrectRotationsForBone(
+                poses,
+                ikFixedPoses,
+                kv.second,
+                corrector,
+                kv.first,
+                false
+            );
+        }
+        jointCorrectionMasks.clear();
+
+        // Then process one bone contacts:
+        for(auto &c : contacts)
+        {
+            if(c.contactType != Animation::kOneBone)
+            {
+                continue;
+            }
+            const auto jointIdx = c.jointIndex;
+            auto parentIdx = joint_parents_vec[jointIdx];
+
+            // We can't touch frames that have been constrained with full body constraints
+            // or the end effector constraints for this joint, so let's combine fullBodyMask
+            // with the end effector mask for this joint if it exists so we can use that
+            // information later:
+            auto jointMask = fullBodyMask;
+            addEndEffectorMask(jointIdx, parentIdx, jointMask);
+
+            // Add a trajectory correction mask for the parent joint:
+            if(jointCorrectionMasks[parentIdx].empty())
+            {
+                jointCorrectionMasks[parentIdx] = jointMask;
+            }
+
+            // Compute the intervals in which the joint is in contact with the floor:
+            auto contactIntervals = ComputeContactIntervals(c.contactMask, jointMask, contactThreshold);
+            FilterContactIntervals(contactIntervals, jointMask, true);
+            for(const auto &interval : contactIntervals)
+            {
+                for (int fixFrame = interval.first; fixFrame < interval.second; ++fixFrame)
+                {
+                    // All we're going to do here is stick the joint to the floor -
+                    // we're going to allow it to slide from side to side.
+
+                    // Find a target position that lies on the floor by iteratively
+                    // projecting the joint to the floor (pure laziness really, this could
+                    // be done analytically):
+                    Math::Vector parentPos = Animation::JointLocalToGlobal(joint_parents_vec, parentIdx, ikFixedPoses[fixFrame]).GetTranslation();
+                    Math::Vector target = Animation::JointLocalToGlobal(joint_parents_vec, jointIdx, ikFixedPoses[fixFrame]).GetTranslation();
+                    float jointLength = (target - parentPos).GetLength3();
+                    for(int32_t i = 0; i < 10; ++i)
+                    {
+                        target.SetY(c.minHeight);
+                        auto dir = (target - parentPos).GetNormalized3();
+                        target = parentPos + dir * jointLength;
+                    }
+
+                    IK::OneBoneIk(
+                        ikFixedPoses[fixFrame],
+                        Math::Transform::Identity,
+                        jointIdx,
+                        1.0f,
+                        target,
+                        joint_parents_vec
+                    );
+                    jointCorrectionMasks[parentIdx][fixFrame] = 1.0f;
+                }
+            }
+
+        }
+
+        // Fixing the contacts with IK will introduce popping into the animation,
+        // so let's apply the interpolator to all the joints we modified so as to
+        // line the trajectory up properly again:
+        for( auto &kv : jointCorrectionMasks)
+        {
+            for( size_t i = 0; i < N; ++i )
+            {
+                margins[i] = kv.second[i] ? 0.0f : -1.0f;
+            }
+            TrajectoryCorrector corrector(margins, pos_weight * 10, vel_weight, acc_weight);
+            CorrectRotationsForBone(
+                poses,
+                ikFixedPoses,
+                kv.second,
+                corrector,
+                kv.first,
+                false
+            );
+        }
+
+        if (has_double_ankle_joints)
+        {
+            // Maps to save target positions BEFORE 2-bone IK modifies them
+            std::map<uint32_t, std::map<uint32_t, Math::Vector>> savedFirstAnkleTargets;  // [firstAnkleIdx][frame] -> position
+            std::map<uint32_t, std::map<uint32_t, Math::Vector>> savedToeTargets;         // [firstAnkleIdx][frame] -> position
+            std::map<uint32_t, uint32_t> contactToToeIdx;  // firstAnkleIdx -> toeIdx
+
+            // Find toe joints for each leg
+            for (const auto& tc : contacts)
+            {
+                if (tc.contactType == Animation::kOneBone)
+                {
+                    // The parent of the toe is the 1st ankle
+                    int parentIdx = joint_parents_vec[tc.jointIndex];
+                    if (parentIdx >= 0)
+                    {
+                        contactToToeIdx[parentIdx] = tc.jointIndex;
+                    }
+                }
+            }
+
+            // For each 2-bone contact, correct the parent (2nd ankle) joint
+            for (auto& c : contacts)
+            {
+                if (c.contactType != Animation::kTwoBone)
+                    continue;
+
+                const auto firstAnkleIdx = c.jointIndex;
+                const auto secondAnkleIdx = joint_parents_vec[firstAnkleIdx];
+                const auto kneeIdx = joint_parents_vec[secondAnkleIdx];
+                const auto hipIdx = joint_parents_vec[kneeIdx];
+
+                if (hipIdx < 0) continue;  // safety check
+
+                // Get saved contact frames for this ankle
+                auto it = twoBoneContactFrames.find(firstAnkleIdx);
+                if (it == twoBoneContactFrames.end())
+                    continue;
+                const auto& contactFrames = it->second;
+
+                // Add correction mask for knee and hip
+                auto jointMask = fullBodyMask;
+                addEndEffectorMask(firstAnkleIdx, secondAnkleIdx, jointMask);
+
+                if (jointCorrectionMasks[kneeIdx].empty())
+                    jointCorrectionMasks[kneeIdx] = jointMask;
+                if (jointCorrectionMasks[hipIdx].empty())
+                    jointCorrectionMasks[hipIdx] = jointMask;
+
+                for (uint32_t fixFrame = 0; fixFrame < fullBodyMask.size(); ++fixFrame)
+                {
+                    // Only correct frames where the 1st ankle was corrected
+                    if (!contactFrames[fixFrame])
+                        continue;
+
+                    // *** SAVE TARGET POSITIONS BEFORE 2-BONE IK ***
+                    savedFirstAnkleTargets[firstAnkleIdx][fixFrame] = Animation::JointLocalToGlobal(
+                        joint_parents_vec, firstAnkleIdx, ikFixedPoses[fixFrame]).GetTranslation();
+
+                    if (contactToToeIdx.count(firstAnkleIdx))
+                    {
+                        savedToeTargets[firstAnkleIdx][fixFrame] = Animation::JointLocalToGlobal(
+                            joint_parents_vec, contactToToeIdx[firstAnkleIdx], ikFixedPoses[fixFrame]).GetTranslation();
+                    }
+
+                    // Get original global transforms (before any IK corrections)
+                    auto originalFirstAnkleGlobal = Animation::JointLocalToGlobal(
+                        joint_parents_vec, firstAnkleIdx, originalPoses[fixFrame]);
+                    auto originalSecondAnkleGlobal = Animation::JointLocalToGlobal(
+                        joint_parents_vec, secondAnkleIdx, originalPoses[fixFrame]);
+
+                    // Compute delta from 1st ankle to 2nd ankle in original animation
+                    auto deltaFirstToSecond = originalFirstAnkleGlobal.GetDeltaToOther(originalSecondAnkleGlobal);
+
+                    // Get corrected 1st ankle global transform
+                    auto correctedFirstAnkleGlobal = Animation::JointLocalToGlobal(
+                        joint_parents_vec, firstAnkleIdx, ikFixedPoses[fixFrame]);
+
+                    // Apply the original delta to the corrected 1st ankle to get target for 2nd ankle
+                    auto target = (deltaFirstToSecond * correctedFirstAnkleGlobal).GetTranslation();
+
+                    // print current and target second ankle positions
+                    auto currPos = Animation::JointLocalToGlobal(
+                        joint_parents_vec, secondAnkleIdx, ikFixedPoses[fixFrame]).GetTranslation();
+
+                    // Apply 2-bone IK: Hip -> Knee -> 2nd Ankle
+                    IK::TwoBoneIk(
+                        ikFixedPoses[fixFrame],
+                        Math::Transform::Identity,
+                        secondAnkleIdx,
+                        1.0f,
+                        target,
+                        joint_parents_vec,
+                        c.hintOffset
+                    );
+
+                    // auto correctedPos = Animation::JointLocalToGlobal(
+                    //     joint_parents_vec, secondAnkleIdx, ikFixedPoses[fixFrame]).GetTranslation();
+                    // std::cout << "Frame " << fixFrame << ": target second ankle=(" << target.GetX() << ", " << target.GetY() << ", " << target.GetZ() << "), corrected second ankle position=(" << correctedPos.GetX() << ", " << correctedPos.GetY() << ", " << correctedPos.GetZ() << ")" << std::endl;
+
+                    jointCorrectionMasks[kneeIdx][fixFrame] = 1.0f;
+                    jointCorrectionMasks[hipIdx][fixFrame] = 1.0f;
+                }
+            }
+
+            // Smooth the corrected joints
+            for (auto& kv : jointCorrectionMasks)
+            {
+                for (size_t i = 0; i < N; ++i)
+                    margins[i] = kv.second[i] ? 0.0f : -1.0f;
+
+                TrajectoryCorrector corrector(margins, pos_weight * 10, vel_weight, acc_weight);
+                CorrectRotationsForBone(poses, ikFixedPoses, kv.second, corrector, kv.first, false);
+            }
+
+            // *** PHASE 2: 1-bone IKs to restore 1st ankle and toe ***
+            jointCorrectionMasks.clear();
+
+            for (auto& c : contacts)
+            {
+                if (c.contactType != Animation::kTwoBone)
+                    continue;
+
+                const auto firstAnkleIdx = c.jointIndex;
+                const auto secondAnkleIdx = joint_parents_vec[firstAnkleIdx];
+
+                auto it = twoBoneContactFrames.find(firstAnkleIdx);
+                if (it == twoBoneContactFrames.end())
+                    continue;
+
+                // Setup correction masks
+                auto jointMask = fullBodyMask;
+                addEndEffectorMask(firstAnkleIdx, secondAnkleIdx, jointMask);
+
+                if (jointCorrectionMasks[secondAnkleIdx].empty())
+                    jointCorrectionMasks[secondAnkleIdx] = jointMask;
+                if (jointCorrectionMasks[firstAnkleIdx].empty())
+                    jointCorrectionMasks[firstAnkleIdx] = jointMask;
+
+                for (uint32_t fixFrame = 0; fixFrame < fullBodyMask.size(); ++fixFrame)
+                {
+                    if (!it->second[fixFrame])
+                        continue;
+
+                    // 1-bone IK: Rotate 2nd ankle so 1st ankle reaches saved target
+                    IK::OneBoneIk(
+                        ikFixedPoses[fixFrame],
+                        Math::Transform::Identity,
+                        firstAnkleIdx,
+                        1.0f,
+                        savedFirstAnkleTargets[firstAnkleIdx][fixFrame],
+                        joint_parents_vec
+                    );
+                    jointCorrectionMasks[secondAnkleIdx][fixFrame] = 1.0f;
+
+                    // auto target = savedFirstAnkleTargets[firstAnkleIdx][fixFrame];
+                    // auto corrected = Animation::JointLocalToGlobal(
+                    //     joint_parents_vec, firstAnkleIdx, ikFixedPoses[fixFrame]).GetTranslation();
+                    // std::cout << "Frame " << fixFrame << ": target first ankle=(" << target.GetX() << ", " << target.GetY() << ", " << target.GetZ() << "), corrected first ankle=(" << corrected.GetX() << ", " << corrected.GetY() << ", " << corrected.GetZ() << ")" << std::endl;
+
+                    // 1-bone IK: Rotate 1st ankle so toe reaches saved target
+                    if (contactToToeIdx.count(firstAnkleIdx) && savedToeTargets[firstAnkleIdx].count(fixFrame))
+                    {
+                        IK::OneBoneIk(
+                            ikFixedPoses[fixFrame],
+                            Math::Transform::Identity,
+                            contactToToeIdx[firstAnkleIdx],
+                            1.0f,
+                            savedToeTargets[firstAnkleIdx][fixFrame],
+                            joint_parents_vec
+                        );
+                        jointCorrectionMasks[firstAnkleIdx][fixFrame] = 1.0f;
+                    }
+
+                    // target = savedToeTargets[firstAnkleIdx][fixFrame];
+                    // corrected = Animation::JointLocalToGlobal(
+                    //     joint_parents_vec, contactToToeIdx[firstAnkleIdx], ikFixedPoses[fixFrame]).GetTranslation();
+                    // std::cout << "Frame " << fixFrame << ": target toe=(" << target.GetX() << ", " << target.GetY() << ", " << target.GetZ() << "), corrected toe=(" << corrected.GetX() << ", " << corrected.GetY() << ", " << corrected.GetZ() << ")" << std::endl;
+                }
+            }
+
+            // Smooth 2nd ankle and 1st ankle
+            for (auto& kv : jointCorrectionMasks)
+            {
+                for (size_t i = 0; i < N; ++i)
+                    margins[i] = kv.second[i] ? 0.0f : -1.0f;
+
+                TrajectoryCorrector corrector(margins, pos_weight * 10, vel_weight, acc_weight);
+                CorrectRotationsForBone(poses, ikFixedPoses, kv.second, corrector, kv.first, false);
+            }
+        }
+    }
+
+}
+
+
+Math::Transform Animation::JointLocalToGlobal(
+    const std::vector<int>& joint_parents_vec,
+    int32_t index,
+    const Pose& localPose,
+    const Math::Transform& rootTx)
+{
+    Math::Transform worldTx = Math::Transform::Identity;
+    while (index > -1)
+    {
+        worldTx = worldTx * localPose[index];
+        index = joint_parents_vec[index];
+    }
+
+    return worldTx * rootTx;
+}
+
+void Animation::CorrectMotion(
+    std::vector<Pose>& poses,
+    const std::vector<Pose>& targetPoses,
+    const std::vector<float>& fullBodyMask,
+    const std::vector<float>& rootMask,
+    const std::vector<ContactInfo>& contacts,
+    const std::vector<ContactInfo>& endEffectorPins,
+    const std::vector<int>& joint_parents_vec,
+    const std::vector<Math::Transform>& defaultPose,
+    float contactThreshold,
+    float root_margin,
+    bool has_double_ankle_joints
+)
+{
+
+    // Calculate some weights so we can preserve velocities more strongly on frames where
+    // the root velocity is low
+    const uint32_t N = poses.size();
+    Eigen::VectorXd velocity_weights(N);
+    for (uint32_t frame = 1; frame < N; ++frame)
+    {
+        // work out xz velocity for this frame:
+        float xdiff = poses[frame][0].GetTranslation()[0] - poses[frame - 1][0].GetTranslation()[0];
+        float zdiff = poses[frame][0].GetTranslation()[2] - poses[frame - 1][0].GetTranslation()[2];
+
+        // find velocity magnitude, divided by a typical walking speed:
+        float v_mag = sqrtf(xdiff*xdiff + zdiff*zdiff) / 0.05f;
+
+        // weight lower velocities higher so that the corrector doesn't make the character drift around
+        // when it's supposed to stand still:
+        v_mag = std::max(v_mag, 1.0f/1000.0f);
+        velocity_weights(frame) = 1.0f / v_mag;
+    }
+    velocity_weights[0] = velocity_weights[1];
+
+    // Correct root y coordinates.
+    // This will warp the root y coordinates in "poses" so they match the root y coordinates
+    // in "targetPoses", on frames where the root y coordinates are constrained, ie the frames
+    // where fullBodyMask = 1.
+    // In addition to this, it preserves the root y coordinates in "pose" on frames where foot
+    // contacts are active, to avoid mushiness when characters are jumping.
+    CorrectHipsY(
+        poses,
+        targetPoses,
+        fullBodyMask,
+        contacts,
+        contactThreshold
+    );
+
+    // Correct root xz coordinates:
+    // This will warp the root xz coordinates in "poses" so they match the xz coordinates
+    // in "targetPoses" on frames where fullBodyMask = 1, and warp them so they're within
+    // "root_margin" units of targetPoses on frames where rootMask = 1.
+    CorrectHipsXZ(
+        poses,
+        targetPoses,
+        fullBodyMask,
+        rootMask,
+        endEffectorPins,
+        velocity_weights,
+        root_margin
+    );
+
+    // Correct joint rotations by warping the rotations so they match targetPoses on frames
+    // where fullBodyMask = 1:
+    CorrectJointRotations(
+        poses,
+        targetPoses,
+        fullBodyMask,
+        velocity_weights
+    );
+
+    // Apply IK for end effector pins
+    DoEffectorIK(
+        poses,
+        targetPoses,
+        fullBodyMask,
+        endEffectorPins,
+        joint_parents_vec,
+        defaultPose
+    );
+
+    // Apply IK to stabilize limbs on contacts
+    DoContactIK(
+        poses,
+        fullBodyMask,
+        contacts,
+        endEffectorPins,
+        joint_parents_vec,
+        defaultPose,
+        contactThreshold,
+        has_double_ankle_joints
+    );
+    // std::cout << "Running post processing." << std::endl;
+}

MotionCorrection/src/cpp/AnimProcessing/Utility.h

@@ -0,0 +1,54 @@
+/*
+ * SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
+ * SPDX-License-Identifier: Apache-2.0
+ */
+
+#pragma once
+
+#include "Math/Transform.h"
+
+#include <string>
+#include <vector>
+
+namespace Animation
+{
+    enum IKType {
+        kOneBone,
+        kTwoBone
+    };
+
+    Math::Transform JointLocalToGlobal(
+        const std::vector<int>& joint_parents_vec,
+        int32_t index,
+        const std::vector<Math::Transform>& localPose,
+        const Math::Transform& rootTx = Math::Transform::Identity
+    );
+
+    struct ContactInfo {
+        // index IK contact joint:
+        int jointIndex;
+        // mask indicating which frames are in contact:
+        std::vector<float> contactMask;
+        // contact type:
+        IKType contactType = kTwoBone;
+
+        // Extra info for TwoBoneIK
+        Math::Vector hintOffset = Math::Vector::Zero;
+
+        float minHeight = 0.0f;
+    };
+
+    void CorrectMotion(
+        std::vector< std::vector<Math::Transform> >& poses,
+        const std::vector< std::vector<Math::Transform> >& targetPoses,
+        const std::vector<float>& mask,
+        const std::vector<float>& rootMask,
+        const std::vector<ContactInfo>& contacts,
+        const std::vector<ContactInfo>& endEffectorPins,
+        const std::vector<int>& joint_parents_vec,
+        const std::vector<Math::Transform>& defaultPose,
+        float contactThreshold,
+        float root_margin,
+        bool has_double_ankle_joints
+    );
+}

MotionCorrection/src/cpp/BindingsPython.cpp

@@ -0,0 +1,364 @@
+/*
+ * SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
+ * SPDX-License-Identifier: Apache-2.0
+ */
+
+#include "AnimProcessing/Utility.h"
+
+#ifdef _WIN32
+#pragma warning(push)
+#pragma warning(disable : 4623 4191 4686 4868 5219 4191 4355)
+#endif
+#include <pybind11/pybind11.h>
+#include <pybind11/numpy.h>
+#include <pybind11/stl.h>
+#ifdef _WIN32
+#pragma warning(pop)
+#endif
+
+namespace py = pybind11;
+
+float strip_nan_inf(float x) noexcept
+{
+    if (std::isnan(x)) return 0;
+    if (std::isinf(x)) return 0;
+    return x;
+}
+
+void correct_motion(
+    py::array_t<float> &rootTranslations,
+    py::array_t<float> &jointRotations,
+    const py::array_t<float>& rootTranslationsTarget,
+    const py::array_t<float>& jointRotationsTarget,
+    const py::array_t<float>& fullPoseMask,
+    const py::array_t<float>& leftHandMask,
+    const py::array_t<float>& rightHandMask,
+    const py::array_t<float>& leftFootMask,
+    const py::array_t<float>& rightFootMask,
+    const py::array_t<float>& rootMask,
+    const py::array_t<float>& contacts,
+    const py::list& joint_parents,
+    const py::list& joint_ref_translations,
+    const py::list& joint_ref_rotations,
+    int left_hand_idx,
+    int right_hand_idx,
+    int left_foot_idx,
+    int right_foot_idx,
+    float contact_threshold,
+    float root_margin,
+    bool has_double_ankle_joints
+)
+{
+    if(joint_parents.size() != joint_ref_translations.size())
+    {
+        throw std::runtime_error("correct_motion python bindings: joint_parents and joint_ref_translations must have the same size");
+    }
+    if(joint_parents.size() != joint_ref_rotations.size())
+    {
+        throw std::runtime_error("correct_motion python bindings: joint_parents and joint_ref_rotations must have the same size");
+    }
+    if(left_hand_idx < 0 || right_hand_idx < 0 || left_foot_idx < 0 || right_foot_idx < 0)
+    {
+        throw std::runtime_error("correct_motion python bindings: left_hand_idx, right_hand_idx, left_foot_idx, and right_foot_idx must be non-negative");
+    }
+    if(left_hand_idx >= joint_parents.size() || right_hand_idx >= joint_parents.size() || left_foot_idx >= joint_parents.size() || right_foot_idx >= joint_parents.size())
+    {
+        throw std::runtime_error("correct_motion python bindings: left_hand_idx, right_hand_idx, left_foot_idx, and right_foot_idx must be less than the number of joints");
+    }
+
+    std::vector<Math::Transform> defaultPose(joint_parents.size());
+    for (size_t i = 0; i < joint_ref_translations.size(); ++i)
+    {
+        if (!py::isinstance<py::list>(joint_ref_translations[i]))
+        {
+            throw std::runtime_error("correct_motion python bindings: Expected joint_ref_translations to be a list of lists");
+        }
+        py::list inner_list = joint_ref_translations[i].cast<py::list>();
+        if (inner_list.size() != 3) {
+            throw std::runtime_error("correct_motion python bindings: Expected joint_ref_translations to be a list of lists, length 3");
+        }
+
+        if (
+            !py::isinstance<py::float_>(inner_list[0]) ||
+            !py::isinstance<py::float_>(inner_list[1]) ||
+            !py::isinstance<py::float_>(inner_list[2])
+        )
+        {
+            throw std::runtime_error("correct_motion python bindings: Expected joint_ref_translations to be a list of lists, length 3, float values");
+        }
+
+
+        if (!py::isinstance<py::list>(joint_ref_rotations[i]))
+        {
+            throw std::runtime_error("correct_motion python bindings: Expected joint_ref_rotations to be a list of lists");
+        }
+        py::list inner_list_rot = joint_ref_rotations[i].cast<py::list>();
+        if (inner_list_rot.size() != 4) {
+            throw std::runtime_error("correct_motion python bindings: Expected joint_ref_rotations to be a list of lists, length 4");
+        }
+
+        if (
+            !py::isinstance<py::float_>(inner_list_rot[0]) ||
+            !py::isinstance<py::float_>(inner_list_rot[1]) ||
+            !py::isinstance<py::float_>(inner_list_rot[2]) ||
+            !py::isinstance<py::float_>(inner_list_rot[3])
+        )
+        {
+            throw std::runtime_error("correct_motion python bindings: Expected joint_ref_rotations to be a list of lists, length 4, float values");
+        }
+
+        defaultPose[i].SetTranslation(Math::Vector(
+            inner_list[0].cast<float>(),
+            inner_list[1].cast<float>(),
+            inner_list[2].cast<float>()));
+        defaultPose[i].SetRotation(Math::Quaternion(
+            inner_list_rot[0].cast<float>(),
+            inner_list_rot[1].cast<float>(),
+            inner_list_rot[2].cast<float>(),
+            inner_list_rot[3].cast<float>()
+        ));
+    }
+
+    std::vector<int> joint_parents_vec(joint_parents.size());
+    for (size_t i = 0; i < joint_parents.size(); ++i)
+    {
+        if (!py::isinstance<py::int_>(joint_parents[i]))
+        {
+            throw std::runtime_error("correct_motion python bindings: Expected joint_parents to be a list of ints");
+        }
+        joint_parents_vec[i] = joint_parents[i].cast<int>();
+        if (joint_parents_vec[i] >= (int)joint_parents.size())
+        {
+            throw std::runtime_error("correct_motion python bindings: joint_parents must be a list of ints, and all values must be less than the number of joints");
+        }
+    }
+
+    size_t num_joints = defaultPose.size();
+    size_t gen_length = fullPoseMask.size();
+
+    if(
+        leftHandMask.size() != (int)gen_length ||
+        rightHandMask.size() != (int)gen_length ||
+        leftFootMask.size() != (int)gen_length ||
+        rightFootMask.size() != (int)gen_length ||
+        rootMask.size() != (int)gen_length
+    )
+    {
+        throw std::runtime_error("correct_motion python bindings: all masks must have the same size");
+    }
+
+    if(rootTranslations.size() != 3 * (int)gen_length)
+    {
+        throw std::runtime_error("correct_motion python bindings: rootTranslations has the wrong size");
+    }
+    if(jointRotations.size() != 4 * (int)num_joints * (int)gen_length)
+    {
+        throw std::runtime_error("correct_motion python bindings: jointRotations has the wrong size");
+    }
+
+    if(rootTranslationsTarget.size() != 3 * (int)gen_length)
+    {
+        throw std::runtime_error("correct_motion python bindings: rootTranslationsTarget has the wrong size");
+    }
+    if(jointRotationsTarget.size() != 4 * (int)num_joints * (int)gen_length)
+    {
+        throw std::runtime_error("correct_motion python bindings: jointRotationsTarget has the wrong size");
+    }
+
+    std::vector<Animation::ContactInfo> endEffectorPins(4);
+    endEffectorPins[0].jointIndex = left_hand_idx;
+    endEffectorPins[0].hintOffset = Math::Vector(0.0f, 0.0f, -0.1f);
+
+    endEffectorPins[1].jointIndex = right_hand_idx;
+    endEffectorPins[1].hintOffset = Math::Vector(0.0f, 0.0f, -0.1f);
+
+    endEffectorPins[2].jointIndex = left_foot_idx;
+    endEffectorPins[2].hintOffset = Math::Vector(0.0f, 0.0f, 0.1f);
+
+    endEffectorPins[3].jointIndex = right_foot_idx;
+    endEffectorPins[3].hintOffset = Math::Vector(0.0f, 0.0f, 0.1f);
+
+    endEffectorPins[0].contactMask.reserve(gen_length);
+    endEffectorPins[1].contactMask.reserve(gen_length);
+    endEffectorPins[2].contactMask.reserve(gen_length);
+    endEffectorPins[3].contactMask.reserve(gen_length);
+    for(size_t i = 0; i < gen_length; ++i)
+    {
+        endEffectorPins[0].contactMask.push_back((1.0f - fullPoseMask.at(i)) * leftHandMask.at(i));
+        endEffectorPins[1].contactMask.push_back((1.0f - fullPoseMask.at(i)) * rightHandMask.at(i));
+        endEffectorPins[2].contactMask.push_back((1.0f - fullPoseMask.at(i)) * leftFootMask.at(i));
+        endEffectorPins[3].contactMask.push_back((1.0f - fullPoseMask.at(i)) * rightFootMask.at(i));
+    }
+
+    std::vector<Animation::ContactInfo> contactInfo(2);
+
+    auto footTranslation = Animation::JointLocalToGlobal(
+        joint_parents_vec,
+        right_foot_idx,
+        defaultPose
+    ).GetTranslation();
+
+    contactInfo[0].jointIndex = right_foot_idx;
+    contactInfo[0].hintOffset = Math::Vector(0.0f, 0.0f, 0.1f);
+    contactInfo[0].minHeight = footTranslation.GetY();
+
+    footTranslation = Animation::JointLocalToGlobal(
+        joint_parents_vec,
+        left_foot_idx,
+        defaultPose
+    ).GetTranslation();
+
+    contactInfo[1].jointIndex = left_foot_idx;
+    contactInfo[1].hintOffset = Math::Vector(0.0f, 0.0f, 0.1f);
+    contactInfo[1].minHeight = footTranslation.GetY();
+
+    auto& rContacts = contactInfo[0].contactMask;
+    auto& lContacts = contactInfo[1].contactMask;
+
+    rContacts.resize(fullPoseMask.size());
+    lContacts.resize(fullPoseMask.size());
+    for (int i = 0; i < fullPoseMask.size(); ++i)
+    {
+        // don't flag it as a contact if it's been masked:
+        rContacts[i] = rightFootMask.at(i) ? 0 : contacts.at(4 * i + 2);
+        lContacts[i] = leftFootMask.at(i) ? 0 : contacts.at(4 * i + 0);
+
+        // Flag the heel as a contact if the toe is a contact:
+        rContacts[i] = std::min((rightFootMask.at(i) ? 0 : contacts.at(4 * i + 3)) + rContacts[i], 1.0f);
+        lContacts[i] = std::min((leftFootMask.at(i) ? 0 : contacts.at(4 * i + 1)) + lContacts[i], 1.0f);
+    }
+
+    int left_toe_idx = -1;
+    int right_toe_idx = -1;
+    for(int i = 0; i < num_joints; ++i)
+    {
+        if(joint_parents_vec[i] == left_foot_idx)
+        {
+            left_toe_idx = i;
+        }
+        if(joint_parents_vec[i] == right_foot_idx)
+        {
+            right_toe_idx = i;
+        }
+    }
+
+    if(left_toe_idx != -1 && right_toe_idx != -1)
+    {
+        auto toeTranslation = Animation::JointLocalToGlobal(
+            joint_parents_vec,
+            right_toe_idx,
+            defaultPose
+        ).GetTranslation();
+
+        contactInfo.resize(4);
+        contactInfo[2].jointIndex = right_toe_idx;
+        contactInfo[2].contactType = Animation::kOneBone;
+        contactInfo[2].minHeight = toeTranslation.GetY();
+
+        contactInfo[3].jointIndex = left_toe_idx;
+        contactInfo[3].contactType = Animation::kOneBone;
+        contactInfo[3].minHeight = toeTranslation.GetY();
+
+        auto& rToeContacts = contactInfo[2].contactMask;
+        auto& lToeContacts = contactInfo[3].contactMask;
+
+        // fill up the ankle contacts:
+        rToeContacts.resize(fullPoseMask.size());
+        lToeContacts.resize(fullPoseMask.size());
+
+        for (int i = 0; i < fullPoseMask.size(); ++i)
+        {
+            // don't flag it as a contact if it's been masked:
+            rToeContacts[i] = rightFootMask.at(i) ? 0 : contacts.at(4 * i + 3);
+            lToeContacts[i] = leftFootMask.at(i) ? 0 : contacts.at(4 * i + 1);
+        }
+    }
+
+
+    auto setTransforms = [gen_length, num_joints](
+        std::vector< std::vector<Math::Transform> > &poses,
+        const py::array_t<float> &rootTranslations,
+        const py::array_t<float> &jointRotations
+    )
+    {
+        for (size_t f = 0; f < gen_length; ++f)
+        {
+            poses[f][0].SetTranslation({
+                strip_nan_inf(rootTranslations.at(3*f+0)),
+                strip_nan_inf(rootTranslations.at(3*f+1)),
+                strip_nan_inf(rootTranslations.at(3*f+2))
+            });
+        }
+
+        for (size_t f = 0; f < gen_length; ++f)
+        {
+            for (size_t j = 0; j < num_joints; ++j)
+            {
+                // x y z w order:
+                Math::Quaternion q(
+                    strip_nan_inf(jointRotations.at(4 * (num_joints * f + j) + 1)),
+                    strip_nan_inf(jointRotations.at(4 * (num_joints * f + j) + 2)),
+                    strip_nan_inf(jointRotations.at(4 * (num_joints * f + j) + 3)),
+                    strip_nan_inf(jointRotations.at(4 * (num_joints * f + j) + 0))
+                );
+                q.Normalize();
+                poses[f][j].SetRotation(q);
+            }
+        }
+    };
+
+    std::vector< std::vector<Math::Transform> > posesFixed(gen_length, defaultPose);
+    setTransforms(posesFixed, rootTranslations, jointRotations);
+
+    std::vector< std::vector<Math::Transform> > posesTarget(gen_length, defaultPose);
+    setTransforms(posesTarget, rootTranslationsTarget, jointRotationsTarget);
+
+    std::vector<float> fullPoseMask_vec;
+    std::vector<float> rootMask_vec;
+    for (size_t f = 0; f < gen_length; ++f)
+    {
+        fullPoseMask_vec.push_back(fullPoseMask.at(f));
+        rootMask_vec.push_back(rootMask.at(f));
+    }
+
+    Animation::CorrectMotion(
+        posesFixed,
+        posesTarget,
+        fullPoseMask_vec,
+        rootMask_vec,
+        contactInfo,
+        endEffectorPins,
+        joint_parents_vec,
+        defaultPose,
+        contact_threshold,
+        root_margin,
+        has_double_ankle_joints
+    );
+
+    for (size_t f = 0; f < gen_length; ++f)
+    {
+        auto t = posesFixed[f][0].GetTranslation();
+        rootTranslations.mutable_at(3*f+0) = t.GetX();
+        rootTranslations.mutable_at(3*f+1) = t.GetY();
+        rootTranslations.mutable_at(3*f+2) = t.GetZ();
+    }
+
+    for (size_t f = 0; f < gen_length; ++f)
+    {
+        for (size_t j = 0; j < num_joints; ++j)
+        {
+            auto q = posesFixed[f][j].GetRotation();
+            // w x y z order
+            jointRotations.mutable_at(4 * (num_joints * f + j) + 0) = ((float*)&q)[3];
+            jointRotations.mutable_at(4 * (num_joints * f + j) + 1) = ((float*)&q)[0];
+            jointRotations.mutable_at(4 * (num_joints * f + j) + 2) = ((float*)&q)[1];
+            jointRotations.mutable_at(4 * (num_joints * f + j) + 3) = ((float*)&q)[2];
+        }
+    }
+
+}
+
+PYBIND11_MODULE(_motion_correction, m) {
+    m.doc() = "Motion Correction Python bindings";
+    m.def("correct_motion", &correct_motion);
+}

MotionCorrection/src/cpp/Compiler.h

@@ -0,0 +1,25 @@
+/*
+ * SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
+ * SPDX-License-Identifier: Apache-2.0
+ */
+
+#pragma once
+
+// Compiler specific defines
+
+// Finds the compiler type and version.
+#if defined(__clang__)
+#    define COMPILER_CLANG
+#elif defined(__GNUC__) // Check after Clang, as Clang defines this too
+#    define COMPILER_GNUC
+#elif defined(_MSC_VER) // Check after Clang, since we could be building with either within VS
+#    define COMPILER_MSVC
+#else
+#    pragma error "Unknown compiler. "
+#endif
+
+#if defined(COMPILER_MSVC)
+	#define FORCE_INLINE __forceinline
+#elif defined(COMPILER_GNUC)
+	#define FORCE_INLINE inline __attribute__((always_inline))
+#endif

MotionCorrection/src/cpp/Debug.h

@@ -0,0 +1,13 @@
+/*
+ * SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
+ * SPDX-License-Identifier: Apache-2.0
+ */
+
+#pragma once
+
+#include "Platform.h"
+
+#define ASSERT( cond ) do { if( !(cond) ) { DEBUG_BREAK(); } } while( 0 )
+#define HALT() { DEBUG_BREAK(); }
+#define UNIMPLEMENTED_FUNCTION() { DEBUG_BREAK(); }
+#define UNREACHABLE_CODE() { DEBUG_BREAK(); }

MotionCorrection/src/cpp/Math/Constants.h

@@ -0,0 +1,32 @@
+/*
+ * SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
+ * SPDX-License-Identifier: Apache-2.0
+ */
+
+#pragma once
+
+#include <limits>
+
+// Mathematical constants
+
+namespace Math
+{
+    static constexpr float const Epsilon = 1.0e-06f;
+    static constexpr float const LargeEpsilon = 1.0e-04f;
+    static constexpr float const HugeEpsilon = 1.0e-02f;
+    static constexpr float const Pi = 3.141592654f;
+    static constexpr float const TwoPi = 6.283185307f;
+    static constexpr float const OneDivPi = 0.318309886f;
+    static constexpr float const OneDivTwoPi = 0.159154943f;
+    static constexpr float const PiDivTwo = 1.570796327f;
+    static constexpr float const PiDivFour = 0.785398163f;
+
+    static constexpr float const SqrtTwo = 1.4142135623730950488016887242097f;
+    static constexpr float const OneDivSqrtTwo = 1.0f / SqrtTwo;
+
+    static constexpr float const DegreesToRadians = 0.0174532925f;
+    static constexpr float const RadiansToDegrees = 57.2957795f;
+
+    static constexpr float const Infinity = std::numeric_limits<float>::infinity();
+    static constexpr float const QNaN = std::numeric_limits<float>::quiet_NaN();
+}

MotionCorrection/src/cpp/Math/Matrix.cpp

@@ -0,0 +1,305 @@
+/*
+ * SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
+ * SPDX-License-Identifier: Apache-2.0
+ */
+
+#include "Matrix.h"
+
+#include <cfloat>
+
+using namespace Math;
+
+namespace
+{
+    static bool CheckForZeroScaleInRow(float scale, const Vector& row)
+    {
+        float const absScale = Math::Abs(scale);
+
+        for (int i = 0; i < 3; i++)
+        {
+            if (absScale < 1 && Math::Abs(row[i]) >= FLT_MAX * absScale)
+            {
+                return false;
+            }
+        }
+
+        return true;
+    }
+
+    static bool ExtractAndRemoveScalingAndShear(Matrix& matrix, Vector& scale, Vector& shear)
+    {
+        scale = Vector::Zero;
+        shear = Vector::Zero;
+
+        Float3 scaleValues = Float3::Zero;
+        Float3 shearValues = Float3::Zero;
+
+        // This implementation follows the technique described in the paper by
+        // Spencer W. Thomas in the Graphics Gems II article: "Decomposing a
+        // Matrix into Simple Transformations", p. 320.
+
+        Vector row[3];
+        row[0] = Vector(matrix[0][0], matrix[0][1], matrix[0][2]);
+        row[1] = Vector(matrix[1][0], matrix[1][1], matrix[1][2]);
+        row[2] = Vector(matrix[2][0], matrix[2][1], matrix[2][2]);
+
+        float maxVal = 0;
+        for (int i = 0; i < 3; i++)
+        {
+            for (int j = 0; j < 3; j++)
+            {
+                if (Math::Abs(row[i][j]) > maxVal)
+                {
+                    maxVal = Math::Abs(row[i][j]);
+                }
+            }
+        }
+
+        // We normalize the 3x3 matrix here.
+        // It was noticed that this can improve numerical stability significantly,
+        // especially when many of the upper 3x3 matrix's coefficients are very
+        // close to zero; we correct for this step at the end by multiplying the
+        // scaling factors by maxVal at the end (shear and rotation are not
+        // affected by the normalization).
+
+        if (maxVal != 0)
+        {
+            for (int i = 0; i < 3; i++)
+            {
+                if (!CheckForZeroScaleInRow(maxVal, row[i]))
+                {
+                    return false;
+                }
+                else
+                {
+                    row[i] /= maxVal;
+                }
+            }
+        }
+
+        // Compute X scale factor.
+        scaleValues.m_x = row[0].Length3().ToFloat();
+        if (!CheckForZeroScaleInRow(scaleValues.m_x, row[0]))
+        {
+            return false;
+        }
+
+        // Normalize first row.
+        row[0] /= scaleValues.m_x;
+
+        // An XY shear factor will shear the X coord. as the Y coord. changes.
+        // There are 6 combinations (XY, XZ, YZ, YX, ZX, ZY), although we only
+        // extract the first 3 because we can effect the last 3 by shearing in
+        // XY, XZ, YZ combined rotations and scales.
+        //
+        // shear matrix <   1,  YX,  ZX,  0,
+        //                 XY,   1,  ZY,  0,
+        //                 XZ,  YZ,   1,  0,
+        //                  0,   0,   0,  1 >
+
+        // Compute XY shear factor and make 2nd row orthogonal to 1st.
+        shearValues[0] = Vector::Dot3(row[0], row[1]).ToFloat();
+        row[1] -= row[0] * shearValues[0];
+
+        // Now, compute Y scale.
+        scaleValues.m_y = row[1].Length3().ToFloat();
+        if (!CheckForZeroScaleInRow(scaleValues.m_y, row[1]))
+        {
+            return false;
+        }
+
+        // Normalize 2nd row and correct the XY shear factor for Y scaling.
+        row[1] /= scaleValues.m_y;
+        shearValues[0] /= scaleValues.m_y;
+
+        // Compute XZ and YZ shears, orthogonalize 3rd row.
+        shearValues[1] = Vector::Dot3(row[0], row[2]).ToFloat();
+        row[2] -= row[0] * shearValues[1];
+        shearValues[2] = Vector::Dot3(row[1], row[2]).ToFloat();
+        row[2] -= row[1] * shearValues[2];
+
+        // Next, get Z scale.
+        scaleValues.m_z = row[2].Length3().ToFloat();
+        if (!CheckForZeroScaleInRow(scaleValues.m_z, row[2]))
+        {
+            return false;
+        }
+
+        // Normalize 3rd row and correct the XZ and YZ shear factors for Z scaling.
+        row[2] /= scaleValues.m_z;
+        shearValues[1] /= scaleValues.m_z;
+        shearValues[2] /= scaleValues.m_z;
+
+        // At this point, the upper 3x3 matrix in mat is orthonormal.
+        // Check for a coordinate system flip. If the determinant
+        // is less than zero, then negate the matrix and the scaling factors.
+        if (Vector::Dot3(row[0], Vector::Cross3(row[1], row[2])).ToFloat() < 0)
+        {
+            for (int i = 0; i < 3; i++)
+            {
+                scaleValues[i] *= -1;
+                row[i] *= -1;
+            }
+        }
+
+        // Copy over the orthonormal rows into the returned matrix.
+        // The upper 3x3 matrix in mat is now a rotation matrix.
+        for (int i = 0; i < 3; i++)
+        {
+            matrix[i].SetX(row[i][0]);
+            matrix[i].SetY(row[i][1]);
+            matrix[i].SetZ(row[i][2]);
+        }
+
+        // Correct the scaling factors for the normalization step that we
+        // performed above; shear and rotation are not affected by the
+        // normalization.
+        scaleValues *= maxVal;
+
+        scale = Vector(scaleValues);
+        shear = Vector(shearValues);
+
+        return true;
+    }
+}
+
+namespace Math
+{
+    Matrix const Matrix::Identity(1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1);
+
+    Matrix::Matrix(float v00, float v01, float v02, float v03, float v10, float v11, float v12, float v13, float v20, float v21, float v22, float v23, float v30, float v31, float v32, float v33)
+    {
+        m_rows[0] = Vector(v00, v01, v02, v03);
+        m_rows[1] = Vector(v10, v11, v12, v13);
+        m_rows[2] = Vector(v20, v21, v22, v23);
+        m_rows[3] = Vector(v30, v31, v32, v33);
+    }
+
+    Matrix::Matrix(float values[16])
+    {
+        m_rows[0] = Vector(values[0], values[1], values[2], values[3]);
+        m_rows[1] = Vector(values[4], values[5], values[6], values[7]);
+        m_rows[2] = Vector(values[8], values[9], values[10], values[11]);
+        m_rows[3] = Vector(values[12], values[13], values[14], values[15]);
+    }
+
+    Matrix::Matrix(const Vector& xAxis, const Vector& yAxis, const Vector& zAxis)
+    {
+        m_rows[0] = xAxis;
+        m_rows[1] = yAxis;
+        m_rows[2] = zAxis;
+        m_rows[3] = Vector::UnitW;
+    }
+
+    Matrix::Matrix(const Vector& xAxis, const Vector& yAxis, const Vector& zAxis, const Vector& translation)
+    {
+        m_rows[0] = xAxis;
+        m_rows[1] = yAxis;
+        m_rows[2] = zAxis;
+        m_rows[3] = translation.GetWithW1();
+    }
+
+    Matrix::Matrix(const EulerAngles& eulerAngles, const Vector translation)
+    {
+        float cx, cy, cz, sx, sy, sz, czsx, cxcz, sysz;
+
+        sx = sinf((float)eulerAngles.m_x); cx = cosf((float)eulerAngles.m_x);
+        sy = sinf((float)eulerAngles.m_y); cy = cosf((float)eulerAngles.m_y);
+        sz = sinf((float)eulerAngles.m_z); cz = cosf((float)eulerAngles.m_z);
+
+        czsx = cz * sx;
+        cxcz = cx * cz;
+        sysz = sy * sz;
+
+        // Order is XYZ
+        m_values[0][0] = cy * cz;
+        m_values[0][1] = cy * sz;
+        m_values[0][2] = -sy;
+        m_values[1][0] = czsx * sy - cx * sz;
+        m_values[1][1] = cxcz + sx * sysz;
+        m_values[1][2] = cy * sx;
+        m_values[2][0] = cxcz * sy + sx * sz;
+        m_values[2][1] = -czsx + cx * sysz;
+        m_values[2][2] = cx * cy;
+        m_values[0][3] = 0.0f;
+        m_values[1][3] = 0.0f;
+        m_values[2][3] = 0.0f;
+
+        // Translation
+        m_rows[3] = translation.GetWithW1();
+    }
+
+    EulerAngles Matrix::ToEulerAngles() const
+    {
+        EulerAngles result;
+
+        result.m_x = Radians(Math::ATan2(m_values[1][2], m_values[2][2]));
+
+        float const c2 = Math::Sqrt((m_values[0][0] * m_values[0][0]) + (m_values[0][1] * m_values[0][1]));
+        result.m_y = Radians(Math::ATan2(-m_values[0][2], c2));
+
+        float const s1 = Math::Sin((float)result.m_x);
+        float const c1 = Math::Cos((float)result.m_x);
+        result.m_z = Radians(Math::ATan2((s1 * m_values[2][0]) - (c1 * m_values[1][0]), (c1 * m_values[1][1]) - (s1 * m_values[2][1])));
+
+        return result;
+    }
+
+    bool Matrix::Decompose(Quaternion& outRotation, Vector& outTranslation, Vector& outScale) const
+    {
+        Matrix copy = *this;
+        Vector shr = Vector::Zero;
+        outScale = Vector::Zero;
+
+        // Extract and remove scale and shear from matrix
+        if (ExtractAndRemoveScalingAndShear(copy, outScale, shr))
+        {
+            // Extract rotation and translation from unscaled matrix
+            outRotation = copy.GetRotation();
+            outTranslation = copy.GetTranslation().GetWithW0();
+            return true;
+        }
+
+        return false;
+    }
+
+    Vector Matrix::GetScale() const
+    {
+        Matrix copy = *this;
+        Vector scale = Vector::Zero, shear;
+        if (!ExtractAndRemoveScalingAndShear(copy, scale, shear))
+        {
+            float const lengthX = m_rows[0].Length3().ToFloat();
+            float const lengthY = m_rows[1].Length3().ToFloat();
+            float const lengthZ = m_rows[2].Length3().ToFloat();
+            scale = Vector(lengthX, lengthY, lengthZ, 0.0f);
+        }
+
+        return scale;
+    }
+
+    Matrix& Matrix::SetScale(const Vector& newScale)
+    {
+        Vector scale, shear;
+        bool result = ExtractAndRemoveScalingAndShear(*this, scale, shear);
+
+        // Cannot set scale on matrix that contains zero-scale
+        ASSERT(result);
+
+        m_rows[0] = m_rows[0] * newScale.GetSplatX();
+        m_rows[1] = m_rows[1] * newScale.GetSplatY();
+        m_rows[2] = m_rows[2] * newScale.GetSplatZ();
+        return *this;
+    }
+
+    Matrix& Matrix::RemoveScale()
+    {
+        Vector scale, shear;
+        bool result = ExtractAndRemoveScalingAndShear(*this, scale, shear);
+
+        // Cannot remove zero scale from matrix
+        ASSERT(result);
+
+        return *this;
+    }
+}

MotionCorrection/src/cpp/Math/Matrix.h

@@ -0,0 +1,163 @@
+/*
+ * SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
+ * SPDX-License-Identifier: Apache-2.0
+ */
+
+#pragma once
+
+#include "Vector.h"
+#include "Quaternion.h"
+
+enum class CoordinateSpace : uint8_t
+{
+    World,
+    Local,
+};
+
+//
+// Matrices are Row-Major
+// Multiplication order is right to left
+// ObjectWorldTransform = LocalObjectTransform * WorldTransform
+//
+
+namespace Math
+{
+    class alignas(16) Matrix
+    {
+    public:
+
+        static Matrix const Identity;
+
+    public:
+
+        static Matrix FromRotation(const Quaternion& rotation);
+        static Matrix FromTranslation(const Vector& translation);
+        static Matrix FromScale(const Vector& scale);
+        static Matrix FromUniformScale(float uniformScale);
+        static Matrix FromTranslationAndScale(const Vector& translation, const Vector& scale);
+        static Matrix FromRotationBetweenVectors(const Vector sourceVector, const Vector targetVector);
+
+    public:
+
+        explicit Matrix();
+        explicit Matrix(NoInit_t);
+        explicit Matrix(ZeroInit_t);
+        explicit Matrix(float v00, float v01, float v02, float v03,
+                        float v10, float v11, float v12, float v13,
+                        float v20, float v21, float v22, float v23,
+                        float v30, float v31, float v32, float v33);
+        explicit Matrix(float values[16]);
+        explicit Matrix(Vector const& xAxis, Vector const& yAxis, Vector const& zAxis);
+        explicit Matrix(Vector const& xAxis, Vector const& yAxis, Vector const& zAxis, Vector const& translation);
+
+        Matrix(const Vector axis, Radians angleRadians);
+        Matrix(const AxisAngle axisAngle);
+
+        explicit Matrix(const Quaternion& rotation);
+        explicit Matrix(const Quaternion& rotation, const Vector& translation, const Vector& scale = Vector::One);
+        explicit Matrix(const Quaternion& rotation, const Vector& translation, float scale = 1.0f);
+        explicit Matrix(const EulerAngles& eulerAngles, const Vector translation = Vector::UnitW);
+
+        EulerAngles ToEulerAngles() const;
+
+        float* AsFloatArray();
+        const float* AsFloatArray() const;
+        const Vector& GetRow(uint32_t row) const;
+
+        const Vector& GetAxisX() const;
+        const Vector& GetAxisY() const;
+        const Vector& GetAxisZ() const;
+
+        void SetAxisX(const Vector& xAxis);
+        void SetAxisY(const Vector& yAxis);
+        void SetAxisZ(const Vector& zAxis);
+
+        Float3 GetForwardVector() const;
+        Float3 GetRightVector() const;
+        Float3 GetUpVector() const;
+
+        Vector GetUnitAxisX() const;
+        Vector GetUnitAxisY() const;
+        Vector GetUnitAxisZ() const;
+
+        bool IsIdentity() const;
+        bool IsOrthogonal() const;
+        bool IsOrthonormal() const;
+
+        bool Decompose(Quaternion& outRotation, Vector& outTranslation, Vector& outScale) const;
+
+        Matrix& Transpose();
+        Matrix GetTransposed() const;
+
+        Matrix& Invert();
+        Matrix GetInverse() const;
+
+        Vector GetDeterminant() const;
+        float GetDeterminantAsFloat() const;
+
+        Vector GetTranslation() const;
+        const Vector& GetTranslationWithW() const;
+        Matrix& SetTranslation(Vector const& v);
+        Matrix& SetTranslation(Float3 const& v);
+        Matrix& SetTranslation(Float4 const& v);
+
+        Quaternion GetRotation() const;
+
+        Matrix& SetRotation(const Matrix& rotation);
+        Matrix& SetRotation(const Quaternion& rotation);
+
+        Matrix& SetRotationMaintainingScale(const Matrix& rotation);
+        Matrix& SetRotationMaintainingScale(const Quaternion& rotation);
+
+        Vector GetScale() const;
+
+        Matrix& RemoveScale();
+        Matrix& SetScale(const Vector& scale);
+        Matrix& SetScale(float uniformScale);
+
+        Matrix& RemoveScaleFast();
+        Matrix& SetScaleFast(const Vector& scale);
+        Matrix& SetScaleFast(float uniformScale);
+
+        //
+        // Operators
+        //
+
+        // Applies rotation and scale to a vector and returns a result with the W = 0
+        Vector RotateVector(const Vector& vector) const;
+
+        // Applies rotation and scale to a vector and returns a result with the W = 0
+        Vector TransformNormal(const Vector& vector) const;
+
+        // Applies the transformation to a given point and ensures the resulting W = 1
+        Vector TransformPoint(const Vector& point) const;
+
+        // Applies the transformation to a vector ignoring the W value.
+        // Same as TransformPoint with the result W left unchanged
+        Vector TransformVector3(const Vector& vector) const;
+
+        // Applies the transformation to a given vector with the result W left unchanged
+        Vector TransformVector4(const Vector& vector) const;
+
+        Vector& operator[](uint32_t i);
+        const Vector operator[](uint32_t i) const;
+
+        Matrix operator*(const Matrix& rhs) const;
+        Matrix& operator*=(const Matrix& rhs);
+
+        Matrix operator*(const Quaternion& rhs) const;
+        Matrix operator*=(const Quaternion& rhs);
+
+        bool operator==(const Matrix& rhs) const;
+
+    public:
+
+        union
+        {
+            Vector      m_rows[4];
+            float       m_values[4][4];
+        };
+    };
+}
+
+#include "Matrix.inl"

MotionCorrection/src/cpp/Math/Matrix.inl

@@ -0,0 +1,802 @@
+/*
+ * SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
+ * SPDX-License-Identifier: Apache-2.0
+ */
+
+#pragma once
+
+#include <cstring>
+
+#include "Matrix.h"
+
+namespace Math
+{
+    inline Matrix Matrix::FromRotation(const Quaternion& rotation)
+    {
+        return Matrix(rotation);
+    }
+
+    inline Matrix Matrix::FromTranslation(const Vector& translation)
+    {
+        Matrix M;
+        M.m_rows[0] = Vector::UnitX;
+        M.m_rows[1] = Vector::UnitY;
+        M.m_rows[2] = Vector::UnitZ;
+        M.m_rows[3] = translation.GetWithW1();
+        return M;
+    }
+
+    inline Matrix Matrix::FromScale(const Vector& scale)
+    {
+        Matrix M;
+        M.m_rows[0] = _mm_and_ps(scale, SIMD::g_maskX000);
+        M.m_rows[1] = _mm_and_ps(scale, SIMD::g_mask0Y00);
+        M.m_rows[2] = _mm_and_ps(scale, SIMD::g_mask00Z0);
+        M.m_rows[3] = Vector::UnitW;
+        return M;
+    }
+
+    inline Matrix Matrix::FromUniformScale(float uniformScale)
+    {
+        Matrix M;
+        M.m_rows[0] = _mm_set_ps(0, 0, 0, uniformScale);
+        M.m_rows[1] = _mm_set_ps(0, 0, uniformScale, 0);
+        M.m_rows[2] = _mm_set_ps(0, uniformScale, 0, 0);
+        M.m_rows[3] = Vector::UnitW;
+        return M;
+    }
+
+    inline Matrix Matrix::FromTranslationAndScale(const Vector& translation, const Vector& scale)
+    {
+        Matrix M;
+        M.m_rows[0] = _mm_and_ps(scale, SIMD::g_maskX000);
+        M.m_rows[1] = _mm_and_ps(scale, SIMD::g_mask0Y00);
+        M.m_rows[2] = _mm_and_ps(scale, SIMD::g_mask00Z0);
+        M.m_rows[3] = translation.GetWithW1();
+        return M;
+    }
+
+    inline Matrix Matrix::FromRotationBetweenVectors(Vector const sourceVector, Vector const targetVector)
+    {
+        return Matrix(Quaternion::FromRotationBetweenNormalizedVectors(sourceVector, targetVector));
+    }
+
+    inline Matrix::Matrix()
+    {
+        memcpy(this, &Matrix::Identity, sizeof(Matrix));
+    }
+
+    inline Matrix::Matrix(NoInit_t)
+    {
+    }
+
+    inline Matrix::Matrix(ZeroInit_t)
+    {
+        memset(this, 0, sizeof(Matrix));
+    }
+
+    inline Matrix::Matrix(const Vector axis, Radians angleRadians)
+    {
+        Vector normal = axis.GetNormalized3();
+
+        Vector C0, C1;
+        Vector::SinCos(C0, C1, Vector((float)angleRadians));
+        Vector C2 = Vector::One - C1;
+
+        __m128 N0 = _mm_shuffle_ps(normal, normal, _MM_SHUFFLE(3, 0, 2, 1));
+        __m128 N1 = _mm_shuffle_ps(normal, normal, _MM_SHUFFLE(3, 1, 0, 2));
+
+        __m128 V0 = _mm_mul_ps(C2, N0);
+        V0 = _mm_mul_ps(V0, N1);
+
+        __m128 R0 = _mm_mul_ps(C2, normal);
+        R0 = _mm_mul_ps(R0, normal);
+        R0 = _mm_add_ps(R0, C1);
+
+        __m128 R1 = _mm_mul_ps(C0, normal);
+        R1 = _mm_add_ps(R1, V0);
+        __m128 R2 = _mm_mul_ps(C0, normal);
+        R2 = _mm_sub_ps(V0, R2);
+
+        V0 = _mm_and_ps(R0, SIMD::g_maskXYZ0);
+        __m128 V1 = _mm_shuffle_ps(R1, R2, _MM_SHUFFLE(2, 1, 2, 0));
+        V1 = _mm_shuffle_ps(V1, V1, _MM_SHUFFLE(0, 3, 2, 1));
+        __m128 V2 = _mm_shuffle_ps(R1, R2, _MM_SHUFFLE(0, 0, 1, 1));
+        V2 = _mm_shuffle_ps(V2, V2, _MM_SHUFFLE(2, 0, 2, 0));
+
+        R2 = _mm_shuffle_ps(V0, V1, _MM_SHUFFLE(1, 0, 3, 0));
+        R2 = _mm_shuffle_ps(R2, R2, _MM_SHUFFLE(1, 3, 2, 0));
+
+        m_rows[0] = R2;
+
+        R2 = _mm_shuffle_ps(V0, V1, _MM_SHUFFLE(3, 2, 3, 1));
+        R2 = _mm_shuffle_ps(R2, R2, _MM_SHUFFLE(1, 3, 0, 2));
+        m_rows[1] = R2;
+
+        V2 = _mm_shuffle_ps(V2, V0, _MM_SHUFFLE(3, 2, 1, 0));
+        m_rows[2] = V2;
+        m_rows[3] = Vector::UnitW;
+    }
+
+    inline Matrix::Matrix(const AxisAngle axisAngle)
+        : Matrix(Vector(axisAngle.m_axis), axisAngle.m_angle)
+    {
+    }
+
+    inline Matrix::Matrix(const Quaternion& rotation)
+    {
+        SetRotation(rotation);
+        m_rows[3] = Vector::UnitW;
+    }
+
+    inline Matrix::Matrix(const Quaternion& rotation, const Vector& translation, const Vector& scale)
+    {
+        SetRotation(rotation);
+        m_rows[0] = m_rows[0] * scale.GetSplatX();
+        m_rows[1] = m_rows[1] * scale.GetSplatY();
+        m_rows[2] = m_rows[2] * scale.GetSplatZ();
+        m_rows[3] = translation.GetWithW1();
+    }
+
+    inline Matrix::Matrix(const Quaternion& rotation, const Vector& translation, float scale)
+        : Matrix(rotation, translation, Vector(scale))
+    {
+    }
+
+    inline float* Matrix::AsFloatArray()
+    {
+        return &m_values[0][0];
+    }
+
+    inline const float* Matrix::AsFloatArray() const
+    {
+        return &m_values[0][0];
+    }
+
+    inline const Vector& Matrix::GetRow(uint32_t row) const
+    {
+        return m_rows[row];
+    }
+
+    inline const Vector& Matrix::GetAxisX() const
+    {
+        return m_rows[0];
+    }
+
+    inline const Vector& Matrix::GetAxisY() const
+    {
+        return m_rows[1];
+    }
+
+    inline const Vector& Matrix::GetAxisZ() const
+    {
+        return m_rows[2];
+    }
+
+    inline void Matrix::SetAxisX(const Vector& xAxis)
+    {
+        m_rows[0] = xAxis;
+    }
+
+    inline void Matrix::SetAxisY(const Vector& yAxis)
+    {
+        m_rows[1] = yAxis;
+    }
+
+    inline void Matrix::SetAxisZ(const Vector& zAxis)
+    {
+        m_rows[2] = zAxis;
+    }
+
+
+    inline Float3 Matrix::GetForwardVector() const
+    {
+        return GetAxisZ();
+    }
+
+    inline Float3 Matrix::GetRightVector() const
+    {
+        return GetAxisX();
+    }
+
+    inline Float3 Matrix::GetUpVector() const
+    {
+        return GetAxisY();
+    }
+
+    inline Vector Matrix::GetUnitAxisX() const
+    {
+        return m_rows[0].GetNormalized3();
+    }
+
+    inline Vector Matrix::GetUnitAxisY() const
+    {
+        return m_rows[1].GetNormalized3();
+    }
+
+    inline Vector Matrix::GetUnitAxisZ() const
+    {
+        return m_rows[2].GetNormalized3();
+    }
+
+    inline bool Matrix::IsIdentity() const
+    {
+        __m128 vTemp1 = _mm_cmpeq_ps(m_rows[0], Vector::UnitX);
+        __m128 vTemp2 = _mm_cmpeq_ps(m_rows[1], Vector::UnitY);
+        __m128 vTemp3 = _mm_cmpeq_ps(m_rows[2], Vector::UnitZ);
+        __m128 vTemp4 = _mm_cmpeq_ps(m_rows[3], Vector::UnitW);
+        vTemp1 = _mm_and_ps(vTemp1, vTemp2);
+        vTemp3 = _mm_and_ps(vTemp3, vTemp4);
+        vTemp1 = _mm_and_ps(vTemp1, vTemp3);
+        return (_mm_movemask_ps(vTemp1) == 0x0f);
+    }
+
+    inline bool Matrix::IsOrthogonal() const
+    {
+        Matrix const transpose = GetTransposed();
+        Matrix result = *this * transpose;
+        return result.IsIdentity();
+    }
+
+    inline bool Matrix::IsOrthonormal() const
+    {
+        static const Vector three(3);
+        auto dotCheck = Vector::Dot3(m_rows[0], m_rows[1]) + Vector::Dot3(m_rows[0], m_rows[2]) + Vector::Dot3(m_rows[1], m_rows[2]);
+        auto magnitudeCheck = m_rows[0].LengthSquared3() + m_rows[1].LengthSquared3() + m_rows[2].LengthSquared3();
+        auto result = dotCheck + magnitudeCheck;
+        return result.IsNearEqual3(three);
+    }
+
+    inline Matrix& Matrix::Transpose()
+    {
+        __m128 vTemp1 = _mm_shuffle_ps(m_rows[0], m_rows[1], _MM_SHUFFLE(1, 0, 1, 0));
+        __m128 vTemp3 = _mm_shuffle_ps(m_rows[0], m_rows[1], _MM_SHUFFLE(3, 2, 3, 2));
+        __m128 vTemp2 = _mm_shuffle_ps(m_rows[2], m_rows[3], _MM_SHUFFLE(1, 0, 1, 0));
+        __m128 vTemp4 = _mm_shuffle_ps(m_rows[2], m_rows[3], _MM_SHUFFLE(3, 2, 3, 2));
+        m_rows[0] = _mm_shuffle_ps(vTemp1, vTemp2, _MM_SHUFFLE(2, 0, 2, 0));
+        m_rows[1] = _mm_shuffle_ps(vTemp1, vTemp2, _MM_SHUFFLE(3, 1, 3, 1));
+        m_rows[2] = _mm_shuffle_ps(vTemp3, vTemp4, _MM_SHUFFLE(2, 0, 2, 0));
+        m_rows[3] = _mm_shuffle_ps(vTemp3, vTemp4, _MM_SHUFFLE(3, 1, 3, 1));
+        return *this;
+    }
+
+    inline Matrix Matrix::GetTransposed() const
+    {
+        Matrix m = *this;
+        m.Transpose();
+        return m;
+    }
+
+    inline Matrix& Matrix::Invert()
+    {
+        Matrix MT = GetTransposed();
+        __m128 V00 = _mm_shuffle_ps(MT.m_rows[2], MT.m_rows[2], _MM_SHUFFLE(1, 1, 0, 0));
+        __m128 V10 = _mm_shuffle_ps(MT.m_rows[3], MT.m_rows[3], _MM_SHUFFLE(3, 2, 3, 2));
+        __m128 V01 = _mm_shuffle_ps(MT.m_rows[0], MT.m_rows[0], _MM_SHUFFLE(1, 1, 0, 0));
+        __m128 V11 = _mm_shuffle_ps(MT.m_rows[1], MT.m_rows[1], _MM_SHUFFLE(3, 2, 3, 2));
+        __m128 V02 = _mm_shuffle_ps(MT.m_rows[2], MT.m_rows[0], _MM_SHUFFLE(2, 0, 2, 0));
+        __m128 V12 = _mm_shuffle_ps(MT.m_rows[3], MT.m_rows[1], _MM_SHUFFLE(3, 1, 3, 1));
+
+        __m128 D0 = _mm_mul_ps(V00, V10);
+        __m128 D1 = _mm_mul_ps(V01, V11);
+        __m128 D2 = _mm_mul_ps(V02, V12);
+
+        V00 = _mm_shuffle_ps(MT.m_rows[2], MT.m_rows[2], _MM_SHUFFLE(3, 2, 3, 2));
+        V10 = _mm_shuffle_ps(MT.m_rows[3], MT.m_rows[3], _MM_SHUFFLE(1, 1, 0, 0));
+        V01 = _mm_shuffle_ps(MT.m_rows[0], MT.m_rows[0], _MM_SHUFFLE(3, 2, 3, 2));
+        V11 = _mm_shuffle_ps(MT.m_rows[1], MT.m_rows[1], _MM_SHUFFLE(1, 1, 0, 0));
+        V02 = _mm_shuffle_ps(MT.m_rows[2], MT.m_rows[0], _MM_SHUFFLE(3, 1, 3, 1));
+        V12 = _mm_shuffle_ps(MT.m_rows[3], MT.m_rows[1], _MM_SHUFFLE(2, 0, 2, 0));
+
+        V00 = _mm_mul_ps(V00, V10);
+        V01 = _mm_mul_ps(V01, V11);
+        V02 = _mm_mul_ps(V02, V12);
+        D0 = _mm_sub_ps(D0, V00);
+        D1 = _mm_sub_ps(D1, V01);
+        D2 = _mm_sub_ps(D2, V02);
+        // V11 = D0Y,D0W,D2Y,D2Y
+        V11 = _mm_shuffle_ps(D0, D2, _MM_SHUFFLE(1, 1, 3, 1));
+        V00 = _mm_shuffle_ps(MT.m_rows[1], MT.m_rows[1], _MM_SHUFFLE(1, 0, 2, 1));
+        V10 = _mm_shuffle_ps(V11, D0, _MM_SHUFFLE(0, 3, 0, 2));
+        V01 = _mm_shuffle_ps(MT.m_rows[0], MT.m_rows[0], _MM_SHUFFLE(0, 1, 0, 2));
+        V11 = _mm_shuffle_ps(V11, D0, _MM_SHUFFLE(2, 1, 2, 1));
+        // V13 = D1Y,D1W,D2W,D2W
+        __m128 V13 = _mm_shuffle_ps(D1, D2, _MM_SHUFFLE(3, 3, 3, 1));
+        V02 = _mm_shuffle_ps(MT.m_rows[3], MT.m_rows[3], _MM_SHUFFLE(1, 0, 2, 1));
+        V12 = _mm_shuffle_ps(V13, D1, _MM_SHUFFLE(0, 3, 0, 2));
+        __m128 V03 = _mm_shuffle_ps(MT.m_rows[2], MT.m_rows[2], _MM_SHUFFLE(0, 1, 0, 2));
+        V13 = _mm_shuffle_ps(V13, D1, _MM_SHUFFLE(2, 1, 2, 1));
+
+        __m128 C0 = _mm_mul_ps(V00, V10);
+        __m128 C2 = _mm_mul_ps(V01, V11);
+        __m128 C4 = _mm_mul_ps(V02, V12);
+        __m128 C6 = _mm_mul_ps(V03, V13);
+
+        // V11 = D0X,D0Y,D2X,D2X
+        V11 = _mm_shuffle_ps(D0, D2, _MM_SHUFFLE(0, 0, 1, 0));
+        V00 = _mm_shuffle_ps(MT.m_rows[1], MT.m_rows[1], _MM_SHUFFLE(2, 1, 3, 2));
+        V10 = _mm_shuffle_ps(D0, V11, _MM_SHUFFLE(2, 1, 0, 3));
+        V01 = _mm_shuffle_ps(MT.m_rows[0], MT.m_rows[0], _MM_SHUFFLE(1, 3, 2, 3));
+        V11 = _mm_shuffle_ps(D0, V11, _MM_SHUFFLE(0, 2, 1, 2));
+        // V13 = D1X,D1Y,D2Z,D2Z
+        V13 = _mm_shuffle_ps(D1, D2, _MM_SHUFFLE(2, 2, 1, 0));
+        V02 = _mm_shuffle_ps(MT.m_rows[3], MT.m_rows[3], _MM_SHUFFLE(2, 1, 3, 2));
+        V12 = _mm_shuffle_ps(D1, V13, _MM_SHUFFLE(2, 1, 0, 3));
+        V03 = _mm_shuffle_ps(MT.m_rows[2], MT.m_rows[2], _MM_SHUFFLE(1, 3, 2, 3));
+        V13 = _mm_shuffle_ps(D1, V13, _MM_SHUFFLE(0, 2, 1, 2));
+
+        V00 = _mm_mul_ps(V00, V10);
+        V01 = _mm_mul_ps(V01, V11);
+        V02 = _mm_mul_ps(V02, V12);
+        V03 = _mm_mul_ps(V03, V13);
+        C0 = _mm_sub_ps(C0, V00);
+        C2 = _mm_sub_ps(C2, V01);
+        C4 = _mm_sub_ps(C4, V02);
+        C6 = _mm_sub_ps(C6, V03);
+
+        V00 = _mm_shuffle_ps(MT.m_rows[1], MT.m_rows[1], _MM_SHUFFLE(0, 3, 0, 3));
+        // V10 = D0Z,D0Z,D2X,D2Y
+        V10 = _mm_shuffle_ps(D0, D2, _MM_SHUFFLE(1, 0, 2, 2));
+        V10 = _mm_shuffle_ps(V10, V10, _MM_SHUFFLE(0, 2, 3, 0));
+        V01 = _mm_shuffle_ps(MT.m_rows[0], MT.m_rows[0], _MM_SHUFFLE(2, 0, 3, 1));
+        // V11 = D0X,D0W,D2X,D2Y
+        V11 = _mm_shuffle_ps(D0, D2, _MM_SHUFFLE(1, 0, 3, 0));
+        V11 = _mm_shuffle_ps(V11, V11, _MM_SHUFFLE(2, 1, 0, 3));
+        V02 = _mm_shuffle_ps(MT.m_rows[3], MT.m_rows[3], _MM_SHUFFLE(0, 3, 0, 3));
+        // V12 = D1Z,D1Z,D2Z,D2W
+        V12 = _mm_shuffle_ps(D1, D2, _MM_SHUFFLE(3, 2, 2, 2));
+        V12 = _mm_shuffle_ps(V12, V12, _MM_SHUFFLE(0, 2, 3, 0));
+        V03 = _mm_shuffle_ps(MT.m_rows[2], MT.m_rows[2], _MM_SHUFFLE(2, 0, 3, 1));
+        // V13 = D1X,D1W,D2Z,D2W
+        V13 = _mm_shuffle_ps(D1, D2, _MM_SHUFFLE(3, 2, 3, 0));
+        V13 = _mm_shuffle_ps(V13, V13, _MM_SHUFFLE(2, 1, 0, 3));
+
+        V00 = _mm_mul_ps(V00, V10);
+        V01 = _mm_mul_ps(V01, V11);
+        V02 = _mm_mul_ps(V02, V12);
+        V03 = _mm_mul_ps(V03, V13);
+        __m128 C1 = _mm_sub_ps(C0, V00);
+        C0 = _mm_add_ps(C0, V00);
+        __m128 C3 = _mm_add_ps(C2, V01);
+        C2 = _mm_sub_ps(C2, V01);
+        __m128 C5 = _mm_sub_ps(C4, V02);
+        C4 = _mm_add_ps(C4, V02);
+        __m128 C7 = _mm_add_ps(C6, V03);
+        C6 = _mm_sub_ps(C6, V03);
+
+        C0 = _mm_shuffle_ps(C0, C1, _MM_SHUFFLE(3, 1, 2, 0));
+        C2 = _mm_shuffle_ps(C2, C3, _MM_SHUFFLE(3, 1, 2, 0));
+        C4 = _mm_shuffle_ps(C4, C5, _MM_SHUFFLE(3, 1, 2, 0));
+        C6 = _mm_shuffle_ps(C6, C7, _MM_SHUFFLE(3, 1, 2, 0));
+        C0 = _mm_shuffle_ps(C0, C0, _MM_SHUFFLE(3, 1, 2, 0));
+        C2 = _mm_shuffle_ps(C2, C2, _MM_SHUFFLE(3, 1, 2, 0));
+        C4 = _mm_shuffle_ps(C4, C4, _MM_SHUFFLE(3, 1, 2, 0));
+        C6 = _mm_shuffle_ps(C6, C6, _MM_SHUFFLE(3, 1, 2, 0));
+
+        __m128 vTemp = Vector::Dot4(C0, MT.m_rows[0]);
+        vTemp = _mm_div_ps(Vector::One, vTemp);
+        m_rows[0] = _mm_mul_ps(C0, vTemp);
+        m_rows[1] = _mm_mul_ps(C2, vTemp);
+        m_rows[2] = _mm_mul_ps(C4, vTemp);
+        m_rows[3] = _mm_mul_ps(C6, vTemp);
+        return *this;
+    }
+
+    inline Matrix Matrix::GetInverse() const
+    {
+        Matrix m = *this;
+        m.Invert();
+        return m;
+    }
+
+    inline Vector Matrix::GetDeterminant() const
+    {
+        Vector V0 = m_rows[2].Shuffle(1, 0, 0, 0);
+        Vector V1 = m_rows[3].Shuffle(2, 2, 1, 1);
+        Vector V2 = m_rows[2].Shuffle(1, 0, 0, 0);
+        Vector V3 = m_rows[3].Shuffle(3, 3, 3, 2);
+        Vector V4 = m_rows[2].Shuffle(2, 2, 1, 1);
+        Vector V5 = m_rows[3].Shuffle(3, 3, 3, 2);
+
+        Vector P0 = V0 * V1;
+        Vector P1 = V2 * V3;
+        Vector P2 = V4 * V5;
+
+        V0 = m_rows[2].Shuffle(2, 2, 1, 1);
+        V1 = m_rows[3].Shuffle(1, 0, 0, 0);
+        V2 = m_rows[2].Shuffle(3, 3, 3, 2);
+        V3 = m_rows[3].Shuffle(1, 0, 0, 0);
+        V4 = m_rows[2].Shuffle(3, 3, 3, 2);
+        V5 = m_rows[3].Shuffle(2, 2, 1, 1);
+
+        P0 = Vector::NegativeMultiplySubtract(V0, V1, P0);
+        P1 = Vector::NegativeMultiplySubtract(V2, V3, P1);
+        P2 = Vector::NegativeMultiplySubtract(V4, V5, P2);
+
+        V0 = m_rows[1].Shuffle(3, 3, 3, 2);
+        V1 = m_rows[1].Shuffle(2, 2, 1, 1);
+        V2 = m_rows[1].Shuffle(1, 0, 0, 0);
+
+        static Vector const Sign(1.0f, -1.0f, 1.0f, -1.0f);
+        Vector S = m_rows[0] * Sign;
+        Vector R = V0 * P0;
+        R = Vector::NegativeMultiplySubtract(V1, P1, R);
+        R = Vector::MultiplyAdd(V2, P2, R);
+
+        return Vector::Dot4(S, R);
+    }
+
+    inline float Matrix::GetDeterminantAsFloat() const
+    {
+        return GetDeterminant().GetX();
+    }
+
+    inline Vector Matrix::GetTranslation() const
+    {
+        return m_rows[3].GetWithW0();
+    }
+
+    inline const Vector& Matrix::GetTranslationWithW() const
+    {
+        return m_rows[3];
+    }
+
+    inline Matrix& Matrix::SetTranslation(const Vector& v)
+    {
+        m_rows[3] = v.GetWithW1();
+        return *this;
+    }
+
+    inline Matrix& Matrix::SetTranslation(const Float3& v)
+    {
+        m_rows[3] = Vector(v, 1.0f);
+        return *this;
+    }
+
+    inline Matrix& Matrix::SetTranslation(const Float4& v)
+    {
+        m_rows[3] = Vector(v.m_x, v.m_y, v.m_z, 1.0f);
+        return *this;
+    }
+
+    inline Quaternion Matrix::GetRotation() const
+    {
+        // based on RTM: https://github.com/nfrechette/rtm
+
+        const Vector& axisX = m_rows[0];
+        const Vector& axisY = m_rows[1];
+        const Vector& axisZ = m_rows[2];
+
+        // Zero scale is not supported
+        if (axisX.IsNearZero4() || axisY.IsNearZero4() || axisZ.IsNearZero4())
+        {
+            HALT();
+        }
+
+        float const axisX_X = axisX.GetX();
+        float const axisY_Y = axisY.GetY();
+        float const axisZ_Z = axisZ.GetZ();
+
+        float const mtx_trace = axisX_X + axisY_Y + axisZ_Z;
+        if (mtx_trace > 0.0)
+        {
+            float const axisX_y = axisX.GetY();
+            float const axisX_z = axisX.GetZ();
+
+            float const axisY_x = axisY.GetX();
+            float const axisY_z = axisY.GetZ();
+
+            float const axisZ_x = axisZ.GetX();
+            float const axisZ_y = axisZ.GetY();
+
+            float const inv_trace = Math::Reciprocal(Math::Sqrt(mtx_trace + 1.0f));
+            float const half_inv_trace = inv_trace * 0.5f;
+
+            float const m_x = (axisY_z - axisZ_y) * half_inv_trace;
+            float const m_y = (axisZ_x - axisX_z) * half_inv_trace;
+            float const m_z = (axisX_y - axisY_x) * half_inv_trace;
+            float const m_w = Math::Reciprocal(inv_trace) * 0.5f;
+
+            return Quaternion(m_x, m_y, m_z, m_w).GetNormalized();
+        }
+        else
+        {
+            // Find the axis with the highest diagonal value
+            int32_t axisIdx0 = 0;
+            if (axisY_Y > axisX_X)
+            {
+                axisIdx0 = 1;
+            }
+
+            if (axisZ_Z > m_rows[axisIdx0][axisIdx0])
+            {
+                axisIdx0 = 2;
+            }
+
+            int32_t const axisIdx1 = (axisIdx0 + 1) % 3;
+            int32_t const axisIdx2 = (axisIdx1 + 1) % 3;
+
+            float const pseudoTrace = 1.0f + m_rows[axisIdx0][axisIdx0] - m_rows[axisIdx1][axisIdx1] - m_rows[axisIdx2][axisIdx2];
+            float const inversePseudoTrace = Math::Reciprocal(Math::Sqrt(pseudoTrace));
+            float const halfInversePseudoTrace = inversePseudoTrace * 0.5f;
+
+            Float4 rawQuatValues;
+            rawQuatValues[axisIdx0] = Math::Reciprocal(inversePseudoTrace) * 0.5f;
+            rawQuatValues[axisIdx1] = halfInversePseudoTrace * (m_rows[axisIdx0][axisIdx1] + m_rows[axisIdx1][axisIdx0]);
+            rawQuatValues[axisIdx2] = halfInversePseudoTrace * (m_rows[axisIdx0][axisIdx2] + m_rows[axisIdx2][axisIdx0]);
+            rawQuatValues[3] = halfInversePseudoTrace * (m_rows[axisIdx1][axisIdx2] - m_rows[axisIdx2][axisIdx1]);
+            return Quaternion(rawQuatValues).GetNormalized();
+        }
+    }
+
+    inline Matrix& Matrix::SetRotation(const Matrix& rotation)
+    {
+        ASSERT(Math::Abs(rotation.GetDeterminant().GetX()) == 1.0f);
+        m_rows[0] = rotation.m_rows[0];
+        m_rows[1] = rotation.m_rows[1];
+        m_rows[2] = rotation.m_rows[2];
+        return *this;
+    }
+
+    inline Matrix& Matrix::SetRotation(const Quaternion& rotation)
+    {
+        static __m128 const constant1110 = { 1.0f, 1.0f, 1.0f, 0.0f };
+
+        __m128 Q0 = _mm_add_ps(rotation, rotation);
+        __m128 Q1 = _mm_mul_ps(rotation, Q0);
+
+        __m128 V0 = _mm_shuffle_ps(Q1, Q1, _MM_SHUFFLE(3, 0, 0, 1));
+        V0 = _mm_and_ps(V0, SIMD::g_maskXYZ0);
+        __m128 V1 = _mm_shuffle_ps(Q1, Q1, _MM_SHUFFLE(3, 1, 2, 2));
+        V1 = _mm_and_ps(V1, SIMD::g_maskXYZ0);
+        __m128 R0 = _mm_sub_ps(constant1110, V0);
+        R0 = _mm_sub_ps(R0, V1);
+
+        V0 = _mm_shuffle_ps(rotation, rotation, _MM_SHUFFLE(3, 1, 0, 0));
+        V1 = _mm_shuffle_ps(Q0, Q0, _MM_SHUFFLE(3, 2, 1, 2));
+        V0 = _mm_mul_ps(V0, V1);
+
+        V1 = _mm_shuffle_ps(rotation, rotation, _MM_SHUFFLE(3, 3, 3, 3));
+        __m128 V2 = _mm_shuffle_ps(Q0, Q0, _MM_SHUFFLE(3, 0, 2, 1));
+        V1 = _mm_mul_ps(V1, V2);
+
+        __m128 R1 = _mm_add_ps(V0, V1);
+        __m128 R2 = _mm_sub_ps(V0, V1);
+
+        V0 = _mm_shuffle_ps(R1, R2, _MM_SHUFFLE(1, 0, 2, 1));
+        V0 = _mm_shuffle_ps(V0, V0, _MM_SHUFFLE(1, 3, 2, 0));
+        V1 = _mm_shuffle_ps(R1, R2, _MM_SHUFFLE(2, 2, 0, 0));
+        V1 = _mm_shuffle_ps(V1, V1, _MM_SHUFFLE(2, 0, 2, 0));
+
+        Q1 = _mm_shuffle_ps(R0, V0, _MM_SHUFFLE(1, 0, 3, 0));
+        Q1 = _mm_shuffle_ps(Q1, Q1, _MM_SHUFFLE(1, 3, 2, 0));
+
+        m_rows[0] = Q1;
+
+        Q1 = _mm_shuffle_ps(R0, V0, _MM_SHUFFLE(3, 2, 3, 1));
+        Q1 = _mm_shuffle_ps(Q1, Q1, _MM_SHUFFLE(1, 3, 0, 2));
+        m_rows[1] = Q1;
+
+        Q1 = _mm_shuffle_ps(V1, R0, _MM_SHUFFLE(3, 2, 1, 0));
+        m_rows[2] = Q1;
+        return *this;
+    }
+
+    inline Matrix& Matrix::SetRotationMaintainingScale(const Matrix& rotation)
+    {
+        Vector const scale = GetScale();
+        SetRotation(rotation);
+        return SetScale(scale);
+    }
+
+    inline Matrix& Matrix::SetRotationMaintainingScale(const Quaternion& rotation)
+    {
+        Vector const scale = GetScale();
+        SetRotation(rotation);
+        return SetScale(scale);
+    }
+
+    inline Matrix& Matrix::SetScale(float uniformScale)
+    {
+        SetScale(Vector(uniformScale));
+        return *this;
+    }
+
+    inline Matrix& Matrix::RemoveScaleFast()
+    {
+        m_rows[0] = m_rows[0].GetNormalized4();
+        m_rows[1] = m_rows[1].GetNormalized4();
+        m_rows[2] = m_rows[2].GetNormalized4();
+        return *this;
+    }
+
+    inline Matrix& Matrix::SetScaleFast(const Vector& scale)
+    {
+        m_rows[0] = m_rows[0].GetNormalized3() * scale.GetSplatX();
+        m_rows[1] = m_rows[1].GetNormalized3() * scale.GetSplatY();
+        m_rows[2] = m_rows[2].GetNormalized3() * scale.GetSplatZ();
+        return *this;
+    }
+
+    inline Matrix& Matrix::SetScaleFast(float uniformScale)
+    {
+        SetScaleFast(Vector(uniformScale));
+        return *this;
+    }
+
+    inline Vector Matrix::RotateVector(const Vector& vector) const
+    {
+        Vector const X = vector.GetSplatX();
+        Vector const Y = vector.GetSplatY();
+        Vector const Z = vector.GetSplatZ();
+
+        Vector Result = Z * m_rows[2];
+        Result = Vector::MultiplyAdd(Y, m_rows[1], Result);
+        Result = Vector::MultiplyAdd(X, m_rows[0], Result);
+
+        return Result;
+    }
+
+    inline Vector Matrix::TransformNormal(const Vector& vector) const
+    {
+        return RotateVector(vector);
+    }
+
+    inline Vector Matrix::TransformPoint(const Vector& point) const
+    {
+        Vector const X = point.GetSplatX();
+        Vector const Y = point.GetSplatY();
+        Vector const Z = point.GetSplatZ();
+
+        Vector result = Vector::MultiplyAdd(Z, m_rows[2], m_rows[3]);
+        result = Vector::MultiplyAdd(Y, m_rows[1], result);
+        result = Vector::MultiplyAdd(X, m_rows[0], result);
+
+        Vector const W = result.GetSplatW();
+        return result / W;
+    }
+
+    inline Vector Matrix::TransformVector3(const Vector& V) const
+    {
+        Vector const X = V.GetSplatX();
+        Vector const Y = V.GetSplatY();
+        Vector const Z = V.GetSplatZ();
+
+        Vector result = Vector::MultiplyAdd(Z, m_rows[2], m_rows[3]);
+        result = Vector::MultiplyAdd(Y, m_rows[1], result);
+        result = Vector::MultiplyAdd(X, m_rows[0], result);
+
+        return result;
+    }
+
+    inline Vector Matrix::TransformVector4(const Vector& V) const
+    {
+        // Splat m_x,m_y,m_z and m_w
+        Vector vTempX = V.GetSplatX();
+        Vector vTempY = V.GetSplatY();
+        Vector vTempZ = V.GetSplatZ();
+        Vector vTempW = V.GetSplatW();
+
+        // Mul by the matrix
+        vTempX = _mm_mul_ps(vTempX, m_rows[0]);
+        vTempY = _mm_mul_ps(vTempY, m_rows[1]);
+        vTempZ = _mm_mul_ps(vTempZ, m_rows[2]);
+        vTempW = _mm_mul_ps(vTempW, m_rows[3]);
+
+        // Add them all together
+        vTempX = _mm_add_ps(vTempX, vTempY);
+        vTempZ = _mm_add_ps(vTempZ, vTempW);
+        vTempX = _mm_add_ps(vTempX, vTempZ);
+
+        return vTempX;
+    }
+
+    inline Vector& Matrix::operator[](uint32_t i)
+    {
+        ASSERT(i < 4);
+        return m_rows[i];
+    }
+
+    inline const Vector Matrix::operator[](uint32_t i) const
+    {
+        ASSERT(i < 4);
+        return m_rows[i];
+    }
+
+    inline Matrix Matrix::operator*(const Matrix& rhs) const
+    {
+        Matrix result = *this;
+        result *= rhs;
+        return result;
+    }
+
+    inline Matrix& Matrix::operator*= (const Matrix& rhs)
+    {
+        Vector vX, vY, vZ, vW;
+
+        // Use vW to hold the original row
+        vW = m_rows[0];
+        vX = _mm_shuffle_ps(vW, vW, _MM_SHUFFLE(0, 0, 0, 0));
+        vY = _mm_shuffle_ps(vW, vW, _MM_SHUFFLE(1, 1, 1, 1));
+        vZ = _mm_shuffle_ps(vW, vW, _MM_SHUFFLE(2, 2, 2, 2));
+        vW = _mm_shuffle_ps(vW, vW, _MM_SHUFFLE(3, 3, 3, 3));
+        vX = _mm_mul_ps(vX, rhs.m_rows[0]);
+        vY = _mm_mul_ps(vY, rhs.m_rows[1]);
+        vZ = _mm_mul_ps(vZ, rhs.m_rows[2]);
+        vW = _mm_mul_ps(vW, rhs.m_rows[3]);
+        vX = _mm_add_ps(vX, vZ);
+        vY = _mm_add_ps(vY, vW);
+        vX = _mm_add_ps(vX, vY);
+        m_rows[0] = vX;
+
+        // Repeat for the other 3 rows
+        vW = m_rows[1];
+        vX = _mm_shuffle_ps(vW, vW, _MM_SHUFFLE(0, 0, 0, 0));
+        vY = _mm_shuffle_ps(vW, vW, _MM_SHUFFLE(1, 1, 1, 1));
+        vZ = _mm_shuffle_ps(vW, vW, _MM_SHUFFLE(2, 2, 2, 2));
+        vW = _mm_shuffle_ps(vW, vW, _MM_SHUFFLE(3, 3, 3, 3));
+        vX = _mm_mul_ps(vX, rhs.m_rows[0]);
+        vY = _mm_mul_ps(vY, rhs.m_rows[1]);
+        vZ = _mm_mul_ps(vZ, rhs.m_rows[2]);
+        vW = _mm_mul_ps(vW, rhs.m_rows[3]);
+        vX = _mm_add_ps(vX, vZ);
+        vY = _mm_add_ps(vY, vW);
+        vX = _mm_add_ps(vX, vY);
+        m_rows[1] = vX;
+
+        vW = m_rows[2];
+        vX = _mm_shuffle_ps(vW, vW, _MM_SHUFFLE(0, 0, 0, 0));
+        vY = _mm_shuffle_ps(vW, vW, _MM_SHUFFLE(1, 1, 1, 1));
+        vZ = _mm_shuffle_ps(vW, vW, _MM_SHUFFLE(2, 2, 2, 2));
+        vW = _mm_shuffle_ps(vW, vW, _MM_SHUFFLE(3, 3, 3, 3));
+        vX = _mm_mul_ps(vX, rhs.m_rows[0]);
+        vY = _mm_mul_ps(vY, rhs.m_rows[1]);
+        vZ = _mm_mul_ps(vZ, rhs.m_rows[2]);
+        vW = _mm_mul_ps(vW, rhs.m_rows[3]);
+        vX = _mm_add_ps(vX, vZ);
+        vY = _mm_add_ps(vY, vW);
+        vX = _mm_add_ps(vX, vY);
+        m_rows[2] = vX;
+
+        vW = m_rows[3];
+        vX = _mm_shuffle_ps(vW, vW, _MM_SHUFFLE(0, 0, 0, 0));
+        vY = _mm_shuffle_ps(vW, vW, _MM_SHUFFLE(1, 1, 1, 1));
+        vZ = _mm_shuffle_ps(vW, vW, _MM_SHUFFLE(2, 2, 2, 2));
+        vW = _mm_shuffle_ps(vW, vW, _MM_SHUFFLE(3, 3, 3, 3));
+        vX = _mm_mul_ps(vX, rhs.m_rows[0]);
+        vY = _mm_mul_ps(vY, rhs.m_rows[1]);
+        vZ = _mm_mul_ps(vZ, rhs.m_rows[2]);
+        vW = _mm_mul_ps(vW, rhs.m_rows[3]);
+        vX = _mm_add_ps(vX, vZ);
+        vY = _mm_add_ps(vY, vW);
+        vX = _mm_add_ps(vX, vY);
+        m_rows[3] = vX;
+        return *this;
+    }
+
+    inline Matrix Matrix::operator*(const Quaternion& rhs) const
+    {
+        return operator*(Matrix(rhs));
+    }
+
+    inline Matrix Matrix::operator*=(const Quaternion& rhs)
+    {
+        return operator*=(Matrix(rhs));
+    }
+
+    inline bool Matrix::operator==(const Matrix& rhs) const
+    {
+        for (auto i = 0; i < 4; i++)
+        {
+            for (auto j = 0; j < 4; j++)
+            {
+                if (m_values[i][j] != rhs.m_values[i][j])
+                {
+                    return false;
+                }
+            }
+        }
+
+        return true;
+    }
+}

MotionCorrection/src/cpp/Math/Quaternion.cpp

@@ -0,0 +1,24 @@
+/*
+ * SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
+ * SPDX-License-Identifier: Apache-2.0
+ */
+
+#include "Quaternion.h"
+#include "Matrix.h"
+
+namespace Math
+{
+    Quaternion const Quaternion::Identity(0, 0, 0, 1);
+
+    // Rotation order is XYZ
+    EulerAngles Quaternion::ToEulerAngles() const
+    {
+        return Matrix(*this).ToEulerAngles();
+    }
+
+    Quaternion Quaternion::LookRotation(const Vector& forward, const Vector& up)
+    {
+        const Vector t = Vector::Cross3(up, forward).Normalize3();
+        return Matrix(t, Vector::Cross3(forward, t), forward).GetRotation();
+    }
+}

MotionCorrection/src/cpp/Math/Quaternion.h

@@ -0,0 +1,144 @@
+/*
+ * SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
+ * SPDX-License-Identifier: Apache-2.0
+ */
+
+#pragma once
+
+#include "Vector.h"
+
+namespace Math
+{
+    class alignas(16) Quaternion
+    {
+    public:
+
+        static Quaternion const Identity;
+
+        // Calculate the rotation required to align the source vector to the target vector (shortest path)
+        static Quaternion FromRotationBetweenNormalizedVectors(const Vector& sourceVector, const Vector& targetVector);
+
+        // Calculate the rotation required to align one vector onto another but also taking account a fallback rotation axis for opposite parallel vectors
+        static Quaternion FromRotationBetweenNormalizedVectors(const Vector& sourceVector, const Vector& targetVector, const Vector& fallbackRotationAxis);
+
+        // Calculate the rotation required to align the source vector to the target vector (shortest path)
+        static Quaternion FromRotationBetweenVectors(const Vector& sourceVector, const Vector& targetVector);
+
+        // Normalized LERP - not accurate - only use for really short distances
+        static Quaternion NLerp(const Quaternion& from, const Quaternion& to, float t);
+
+        // Standard and accurate Spherical LERP - based on DirectX Math
+        static Quaternion SLerp(const Quaternion& from, const Quaternion& to, float t);
+
+        // Fast approximation of a Spherical LERP - based on "A fast and accurate estimate for SLERP" by David Eberly
+        static Quaternion FastSLerp(const Quaternion& from, const Quaternion& to, float t);
+
+        // Spherical quadrangle/cubic interpolation for quaternions
+        static Quaternion SQuad(const Quaternion& q0, const Quaternion& q1, const Quaternion& q2, const Quaternion& q3, float t);
+
+        // Calculate the shortest delta quaternion needed to rotate 'from' onto 'to'
+        static Quaternion Delta(const Quaternion& from, const Quaternion& to);
+
+        // Simple vector dot product between two quaternions
+        static Vector Dot(const Quaternion& q0, const Quaternion& q1);
+
+        // Calculate the angular distance between two quaternions
+        static Radians Distance(const Quaternion& q0, const Quaternion& q1);
+
+        // Calculate look rotation given forward and up vectors
+        static Quaternion LookRotation(const Vector& forward, const Vector& up);
+
+    public:
+
+        Quaternion() = default;
+        explicit Quaternion(NoInit_t);
+        explicit Quaternion(IdentityInit_t);
+        explicit Quaternion(const Vector& v);
+        explicit Quaternion(float ix, float iy, float iz, float iw);
+        explicit Quaternion(const Float4& v);
+
+        explicit Quaternion(const Vector& axis, Radians angle);
+        explicit Quaternion(AxisAngle axisAngle);
+
+        explicit Quaternion(const EulerAngles& eulerAngles);
+        explicit Quaternion(Radians rotX, Radians rotY, Radians rotZ);
+
+        operator __m128& ();
+        operator const __m128& () const;
+
+        Float4 ToFloat4() const;
+        Vector ToVector() const;
+
+        Vector Length();
+        float GetLength() const;
+
+        // Get the angle this rotation represents around the specified axis
+        Radians GetAngle() const;
+
+        AxisAngle ToAxisAngle() const;
+        EulerAngles ToEulerAngles() const;
+
+        Vector RotateVector(const Vector& vector) const;
+        Vector RotateVectorInverse(const Vector& vector) const;
+
+        Quaternion& Conjugate();
+        Quaternion GetConjugate() const;
+
+        Quaternion& Negate();
+        Quaternion GetNegated() const;
+
+        Quaternion& Invert();
+        Quaternion GetInverse() const;
+
+        Quaternion& Normalize();
+        Quaternion GetNormalized() const;
+
+        Vector XAxis() const noexcept;
+        Vector YAxis() const noexcept;
+        Vector ZAxis() const noexcept;
+
+        // Ensure that this rotation is the shortest in terms of the angle (i.e. -5 instead of 355)
+        Quaternion& MakeShortestPath();
+
+        // Ensure that this rotation is the shortest in terms of the angle (i.e. -5 instead of 355)
+        Quaternion GetShortestPath() const;
+
+        // This function will return the estimated normalized quaternion, this is not super accurate but a lot faster (use with care)
+        Quaternion& NormalizeInaccurate();
+
+        // This function will return the estimated normalized quaternion, this is not super accurate but a lot faster (use with care)
+        Quaternion GetNormalizedInaccurate() const;
+
+        bool IsNormalized() const;
+        bool IsIdentity() const;
+
+        // Concatenate the rotation of this onto rhs and return the result i.e. first rotate by rhs then by this
+        // This means order of rotation is right-to-left: child-rotation * parent-rotation
+        Quaternion operator*(const Quaternion& rhs) const;
+        Quaternion& operator*=(const Quaternion& rhs);
+
+        // Is the distance between this quaternion and another one under the threshold?
+        bool IsNearEqual(const Quaternion& rhs, Radians const threshold = Math::DegreesToRadians) const;
+
+        // Exact equality
+        bool operator==(const Quaternion& rhs) const;
+
+        // Exact equality
+        bool operator!=(const Quaternion& rhs) const;
+
+    private:
+
+        Vector GetSplatW() const;
+        float GetW() const;
+
+        Quaternion& operator=(const Vector& v) = delete;
+
+    public:
+
+        __m128 m_data;
+    };
+
+    static_assert(sizeof(Vector) == 16, "Quaternion size must be 16 bytes!");
+}
+
+#include "Quaternion.inl"

MotionCorrection/src/cpp/Math/Quaternion.inl

@@ -0,0 +1,606 @@
+/*
+ * SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
+ * SPDX-License-Identifier: Apache-2.0
+ */
+
+#pragma once
+
+#include "Quaternion.h"
+
+namespace Math
+{
+    inline Quaternion Quaternion::FromRotationBetweenNormalizedVectors(const Vector& from, const Vector& to)
+    {
+        ASSERT(from.IsNormalized3() && to.IsNormalized3());
+
+        Quaternion result;
+
+        // Parallel vectors - return zero rotation
+        Vector const dot = Vector::Dot3(from, to);
+        if (dot.IsGreaterThanEqual4(Vector::OneMinusEpsilon))
+        {
+            result = Quaternion::Identity;
+        }
+        // Opposite vectors - return 180 rotation around any orthogonal axis
+        else if (dot.IsLessThanEqual4(Vector::EpsilonMinusOne))
+        {
+            Float4 const fromValues = from.ToFloat4();
+            result = Quaternion(-fromValues.m_z, fromValues.m_y, fromValues.m_x, 0);
+            result.Normalize();
+        }
+        else // Calculate quaternion rotation
+        {
+            Vector const cross = Vector::Cross3(from, to);
+            Vector Q = Vector::Select(cross, dot, Vector::Select0001);
+            Q += Vector::Select(Vector::Zero, Q.Length4(), Vector::Select0001);
+            result = Quaternion(Q);
+            result.Normalize();
+        }
+
+        return result;
+    }
+
+    inline Quaternion Quaternion::FromRotationBetweenNormalizedVectors(const Vector& from, const Vector& to, const Vector& fallbackRotationAxis)
+    {
+        ASSERT(from.IsNormalized3() && to.IsNormalized3());
+
+        Quaternion Q(NoInit);
+
+        Vector rotationAxis = from.Cross3(to).GetNormalized3();
+        if (rotationAxis.GetLengthSquared3() == 0)
+        {
+            rotationAxis = fallbackRotationAxis;
+        }
+
+        float const dot = from.GetDot3(to);
+        if (dot >= (1.0f - Math::Epsilon))
+        {
+            Q = Quaternion::Identity;
+        }
+        else
+        {
+            float const angle = Math::ACos(dot);
+            Q = Quaternion(rotationAxis, angle);
+        }
+
+        return Q;
+    }
+
+    inline Quaternion Quaternion::FromRotationBetweenVectors(const Vector& sourceVector, const Vector& targetVector)
+    {
+        return FromRotationBetweenNormalizedVectors(
+            sourceVector.GetNormalized3(),
+                targetVector.GetNormalized3());
+    }
+
+    inline Quaternion Quaternion::NLerp(const Quaternion& from, const Quaternion& to, float T)
+    {
+        ASSERT(T >= 0.0f && T <= 1.0f);
+
+        Quaternion adjustedFrom(from);
+
+        // Ensure that the rotations are in the same direction
+        if (Quaternion::Dot(from, to).IsLessThan4(Vector::Zero))
+        {
+            adjustedFrom.Negate();
+        }
+
+        Quaternion result(Vector::Lerp(adjustedFrom.ToVector(), to.ToVector(), T));
+        result.Normalize();
+        return result;
+    }
+
+    inline Quaternion Quaternion::SLerp(const Quaternion& from, const Quaternion& to, float T)
+    {
+        ASSERT(T >= 0.0f && T <= 1.0f);
+
+        static SIMD::UIntMask const maskSign = { 0x80000000,0x00000000,0x00000000,0x00000000 };
+        static __m128 const oneMinusEpsilon = { 1.0f - 0.00001f, 1.0f - 0.00001f, 1.0f - 0.00001f, 1.0f - 0.00001f };
+
+        Vector const VecT(T);
+
+        Vector cosOmega = Quaternion::Dot(from, to);
+
+        Vector control = cosOmega.LessThan(Vector::Zero);
+        Vector sign = Vector::Select(Vector::One, Vector::NegativeOne, control);
+
+        cosOmega = _mm_mul_ps(cosOmega, sign);
+        control = cosOmega.LessThan(oneMinusEpsilon);
+
+        Vector sinOmega = _mm_mul_ps(cosOmega, cosOmega);
+        sinOmega = _mm_sub_ps(Vector::One, sinOmega);
+        sinOmega = _mm_sqrt_ps(sinOmega);
+
+        Vector omega = Vector::ATan2(sinOmega, cosOmega);
+
+        Vector V01 = _mm_shuffle_ps(VecT, VecT, _MM_SHUFFLE(2, 3, 0, 1));
+        V01 = _mm_and_ps(V01, SIMD::g_maskXY00);
+        V01 = _mm_xor_ps(V01, maskSign);
+        V01 = _mm_add_ps(Vector::UnitX, V01);
+
+        Vector S0 = _mm_mul_ps(V01, omega);
+        S0 = Vector::Sin(S0);
+        S0 = _mm_div_ps(S0, sinOmega);
+        S0 = Vector::Select(V01, S0, control);
+
+        Vector S1 = S0.GetSplatY();
+        S0 = S0.GetSplatX();
+
+        S1 = _mm_mul_ps(S1, sign);
+        Vector result = _mm_mul_ps(from, S0);
+        S1 = _mm_mul_ps(S1, to);
+        result = _mm_add_ps(result, S1);
+
+        return Quaternion(result);
+    }
+
+    inline Quaternion Quaternion::FastSLerp(const Quaternion& q0, const Quaternion& q1, float t)
+    {
+        // Precomputed constants
+        constexpr float const mu = 1.85298109240830f;
+        static Vector const u0123 = _mm_setr_ps(1.f / (1 * 3), 1.f / (2 * 5), 1.f / (3 * 7), 1.f / (4 * 9));
+        static Vector const u4567 = _mm_setr_ps(1.f / (5 * 11), 1.f / (6 * 13), 1.f / (7 * 15), mu / (8 * 17));
+        static Vector const v0123 = _mm_setr_ps(1.f / 3, 2.f / 5, 3.f / 7, 4.f / 9);
+        static Vector const v4567 = _mm_setr_ps(5.f / 11, 6.f / 13, 7.f / 15, mu * 8 / 17);
+        static Vector const vSignMask = _mm_set1_ps(-0.f);
+
+        // Common code for computing the scalar coefficients of SLERP
+        auto CalculateCoefficient = [](Vector vT, Vector xm1)
+        {
+            Vector const vTSquared = vT * vT;
+
+            // ( b4, b5, b6, b7 ) = ( x-1 ) * ( u4 * t^2 - v4, u5 * t^2 - v5, u6 * t^2 - v6, u7 * t^2 - v7 )
+            Vector b4567 = Vector::MultiplySubtract(u4567, vTSquared, v4567);
+            b4567 *= xm1;
+
+            // ( b7, b7, b7, b7 )
+            Vector b = b4567.GetSplatW();
+            Vector c = b + Vector::One;
+
+            // ( b6, b6, b6, b6 )
+            b = b4567.GetSplatZ();
+            c = Vector::MultiplyAdd(b, c, Vector::One);
+
+            // ( b5, b5, b5, b5 )
+            b = b4567.GetSplatY();
+            c = Vector::MultiplyAdd(b, c, Vector::One);
+
+            // ( b4, b4, b4, b4 )
+            b = b4567.GetSplatX();
+            c = Vector::MultiplyAdd(b, c, Vector::One);
+
+            // ( b0, b1, b2, b3 ) =
+            // ( x-1)*(u0* t^2-v0, u1 * t^2 -v1, u2* t^2-v2, u3* t^2-v3 )
+            Vector b0123 = Vector::MultiplySubtract(u0123, vTSquared, v0123);
+            b0123 *= xm1;
+
+            // ( b3, b3, b3, b3 )
+            b = b0123.GetSplatW();
+            c = Vector::MultiplyAdd(b, c, Vector::One);
+
+            // ( b2, b2, b2, b2 )
+            b = b0123.GetSplatZ();
+            c = Vector::MultiplyAdd(b, c, Vector::One);
+
+            // ( b1, b1, b1, b1 )
+            b = b0123.GetSplatY();
+            c = Vector::MultiplyAdd(b, c, Vector::One);
+
+            // ( b0, b0, b0, b0 )
+            b = b0123.GetSplatX();
+            c = Vector::MultiplyAdd(b, c, Vector::One);
+            c *= vT;
+
+            return c;
+        };
+
+        Vector x = Vector::Dot4(q0.m_data, q1.m_data); // cos ( theta ) in all components
+
+        Vector sign = _mm_and_ps(vSignMask, x);
+        x = _mm_xor_ps(sign, x);
+        Vector localQ1 = _mm_xor_ps(sign, q1);
+
+        Vector xm1 = x - Vector::One;
+
+        Vector cT = CalculateCoefficient(Vector(t), xm1);
+        Vector cD = CalculateCoefficient(Vector(1.0f - t), xm1);
+        cT = cT * localQ1;
+
+        Quaternion result(Vector::MultiplyAdd(cD, q0.m_data, cT));
+        return result;
+    }
+
+    inline Quaternion Quaternion::SQuad(const Quaternion& q0, const Quaternion& q1, const Quaternion& q2, const Quaternion& q3, float t)
+    {
+        ASSERT(t >= 0.0f && t <= 1.0f);
+
+        Quaternion const q03 = Quaternion::SLerp(q0, q3, t);
+        Quaternion const q12 = Quaternion::SLerp(q1, q2, t);
+        t = (t - (t * t)) * 2;
+        Quaternion const result = Quaternion::SLerp(q03, q12, t);
+        return result;
+    }
+
+    inline Quaternion Quaternion::Delta(const Quaternion& from, const Quaternion& to)
+    {
+        return to * from.GetInverse();
+    }
+
+    inline Vector Quaternion::Dot(const Quaternion& q0, const Quaternion& q1)
+    {
+        return Vector::Dot4(q0.m_data, q1.m_data);
+    }
+
+    inline Radians Quaternion::Distance(const Quaternion& q0, const Quaternion& q1)
+    {
+        float const dot = Math::Clamp(Dot(q0, q1).ToFloat(), -1.0f, 1.0f);
+        return Radians(2 * Math::ACos(Math::Abs(dot)));
+    }
+
+    inline Quaternion::Quaternion(NoInit_t)
+    {
+    }
+
+    inline Quaternion::Quaternion(IdentityInit_t)
+        : m_data(Vector::UnitW.m_data)
+    {
+    }
+
+    inline Quaternion::Quaternion(const Vector& v)
+        : m_data(v.m_data)
+    {
+    }
+
+    inline Quaternion::Quaternion(float ix, float iy, float iz, float iw)
+    {
+        m_data = _mm_set_ps(iw, iz, iy, ix);
+    }
+
+    inline Quaternion::Quaternion(const Float4& v)
+        : Quaternion(v.m_x, v.m_y, v.m_z, v.m_w)
+    {
+    }
+
+    inline Quaternion::Quaternion(const Vector& axis, Radians angle)
+    {
+        ASSERT(axis.IsNormalized3());
+
+        auto N = _mm_and_ps(axis, SIMD::g_maskXYZ0);
+        N = _mm_or_ps(N, Vector::UnitW);
+        auto scale = _mm_set_ps1(0.5f * (float)angle);
+
+        Vector sine, cosine;
+        Vector::SinCos(sine, cosine, scale);
+
+        scale = _mm_and_ps(sine, SIMD::g_maskXYZ0);
+        cosine = _mm_and_ps(cosine, SIMD::g_mask000W);
+        scale = _mm_or_ps(scale, cosine);
+
+        N = _mm_mul_ps(N, scale);
+        m_data = N;
+    }
+
+    inline Quaternion::Quaternion(AxisAngle axisAngle)
+        : Quaternion(Vector(axisAngle.m_axis), axisAngle.m_angle)
+    {
+    }
+
+    inline Quaternion::Quaternion(const EulerAngles& eulerAngles)
+    {
+        auto const rotationX = Quaternion(Vector::UnitX, eulerAngles.m_x);
+        auto const rotationY = Quaternion(Vector::UnitY, eulerAngles.m_y);
+        auto const rotationZ = Quaternion(Vector::UnitZ, eulerAngles.m_z);
+
+        // Rotation order is XYZ - all in global space, hence the order is reversed
+        m_data = (rotationX * rotationY * rotationZ).GetNormalized().m_data;
+    }
+
+    inline Quaternion::Quaternion(Radians rotX, Radians rotY, Radians rotZ)
+        : Quaternion(EulerAngles(rotX, rotY, rotZ))
+    {
+    }
+
+    inline Quaternion::operator __m128& ()
+    {
+        return m_data;
+    }
+
+    inline Quaternion::operator const __m128& () const
+    {
+        return m_data;
+    }
+
+    inline Float4 Quaternion::ToFloat4() const
+    {
+        Float4 v;
+        _mm_storeu_ps(&v.m_x, m_data);
+        return v;
+    }
+
+    inline Vector Quaternion::ToVector() const
+    {
+        return Vector(m_data);
+    }
+
+    inline Vector Quaternion::Length()
+    {
+        return ToVector().Length4();
+    }
+
+    inline float Quaternion::GetLength() const
+    {
+        return ToVector().GetLength4();
+    }
+
+    inline Radians Quaternion::GetAngle() const
+    {
+        return Radians(2.0f * Math::ACos(GetW()));
+    }
+
+    inline AxisAngle Quaternion::ToAxisAngle() const
+    {
+        return AxisAngle(ToVector(), Radians(2.0f * Math::ACos(GetW())));
+    }
+
+    inline Vector Quaternion::RotateVector(const Vector& vector) const
+    {
+        Quaternion const A(Vector::Select(Vector::Select1110, vector, Vector::Select1110));
+        Quaternion const result = GetConjugate() * A;
+        return (result * *this).ToVector();
+    }
+
+    inline Vector Quaternion::RotateVectorInverse(const Vector& vector) const
+    {
+        Quaternion const A(Vector::Select(Vector::Select1110, vector, Vector::Select1110));
+        Quaternion const result = *this * A;
+        return (result * GetConjugate()).ToVector();
+    }
+
+    inline Quaternion& Quaternion::Conjugate()
+    {
+        static __m128 const conj = { -1.0f, -1.0f, -1.0f, 1.0f };
+        m_data = _mm_mul_ps(*this, conj);
+        return *this;
+    }
+
+    inline Quaternion Quaternion::GetConjugate() const
+    {
+        Quaternion q = *this;
+        q.Conjugate();
+        return q;
+    }
+    inline Quaternion& Quaternion::Negate()
+    {
+        m_data = _mm_mul_ps(*this, Vector::NegativeOne);
+        return *this;
+    }
+
+    inline Quaternion Quaternion::GetNegated() const
+    {
+        Quaternion q = *this;
+        q.Negate();
+        return q;
+    }
+
+    inline Quaternion& Quaternion::Invert()
+    {
+        Vector const conjugate(GetConjugate().m_data);
+        Vector const length = ToVector().Length4();
+        Vector const mask = length.LessThanEqual(Vector::Epsilon);
+        Vector const result = conjugate / length;
+        m_data = result.Select(result, Vector::Zero, mask);
+        return *this;
+    }
+
+    inline Quaternion Quaternion::GetInverse() const
+    {
+        Quaternion q = *this;
+        q.Invert();
+        return q;
+    }
+
+    inline Quaternion& Quaternion::Normalize()
+    {
+        m_data = ToVector().GetNormalized4().m_data;
+        return *this;
+    }
+
+    inline Quaternion Quaternion::GetNormalized() const
+    {
+        Quaternion q = *this;
+        q.Normalize();
+        return q;
+    }
+
+    inline Vector Quaternion::XAxis() const noexcept
+    {
+        const float x = _mm_cvtss_f32(m_data);
+        const float y = _mm_cvtss_f32(
+            _mm_shuffle_ps(m_data, m_data,
+                _MM_SHUFFLE(1, 1, 1, 1)));
+        const float z = _mm_cvtss_f32(
+            _mm_shuffle_ps(m_data, m_data,
+                _MM_SHUFFLE(2, 2, 2, 2)));
+        const float w = _mm_cvtss_f32(
+            _mm_shuffle_ps(m_data, m_data,
+                _MM_SHUFFLE(3, 3, 3, 3)));
+
+        const float s = 2.0f * w;
+        const float x2 = 2.0f * x;
+
+        return Vector(
+            x2 * x + s * w - 1.0f,
+                x2 * y + s * z,
+                    x2 * z + s * -y);
+    }
+
+    inline Vector Quaternion::YAxis() const noexcept
+    {
+        const float x = _mm_cvtss_f32(m_data);
+        const float y = _mm_cvtss_f32(
+            _mm_shuffle_ps(m_data, m_data,
+                _MM_SHUFFLE(1, 1, 1, 1)));
+        const float z = _mm_cvtss_f32(
+            _mm_shuffle_ps(m_data, m_data,
+                _MM_SHUFFLE(2, 2, 2, 2)));
+        const float w = _mm_cvtss_f32(
+            _mm_shuffle_ps(m_data, m_data,
+                _MM_SHUFFLE(3, 3, 3, 3)));
+
+        const float s = 2.0f * w;
+        const float y2 = 2.0f * y;
+
+        return Vector(
+            y2 * x + s * -z,
+                y2 * y + s * w - 1.0f,
+                    y2 * z + s * x);
+    }
+
+    inline Vector Quaternion::ZAxis() const noexcept
+    {
+        const float x = _mm_cvtss_f32(m_data);
+        const float y = _mm_cvtss_f32(
+            _mm_shuffle_ps(m_data, m_data,
+                _MM_SHUFFLE(1, 1, 1, 1)));
+        const float z = _mm_cvtss_f32(
+            _mm_shuffle_ps(m_data, m_data,
+                _MM_SHUFFLE(2, 2, 2, 2)));
+        const float w = _mm_cvtss_f32(
+            _mm_shuffle_ps(m_data, m_data,
+                _MM_SHUFFLE(3, 3, 3, 3)));
+
+        const float s = 2.0f * w;
+        const float z2 = 2.0f * z;
+
+        return Vector(
+            x * z2 + s * y,
+                y * z2 + s * -x,
+                    z * z2 + s * w - 1.0f);
+    }
+
+    inline Quaternion& Quaternion::MakeShortestPath()
+    {
+        // If we have a > 180 angle, negate
+        // w < 0.0f is the same as dot( identity, q ) < 0
+        if (GetW() < 0.0f)
+        {
+            Negate();
+        }
+
+        return *this;
+    }
+
+    inline Quaternion Quaternion::GetShortestPath() const
+    {
+        Quaternion sp = *this;
+        sp.MakeShortestPath();
+        return sp;
+    }
+
+    inline Quaternion& Quaternion::NormalizeInaccurate()
+    {
+        *this = GetNormalizedInaccurate();
+        return *this;
+    }
+
+    inline Quaternion Quaternion::GetNormalizedInaccurate() const
+    {
+        __m128 vLengthSq = _mm_mul_ps(m_data, m_data);
+        __m128 vTemp = _mm_shuffle_ps(vLengthSq, vLengthSq, _MM_SHUFFLE(3, 2, 3, 2));
+        vLengthSq = _mm_add_ps(vLengthSq, vTemp);
+        vLengthSq = _mm_shuffle_ps(vLengthSq, vLengthSq, _MM_SHUFFLE(1, 0, 0, 0));
+        vTemp = _mm_shuffle_ps(vTemp, vLengthSq, _MM_SHUFFLE(3, 3, 0, 0));
+        vLengthSq = _mm_add_ps(vLengthSq, vTemp);
+        vLengthSq = _mm_shuffle_ps(vLengthSq, vLengthSq, _MM_SHUFFLE(2, 2, 2, 2));
+
+        // Get the reciprocal and mul to perform the normalization
+        Quaternion result;
+        result.m_data = _mm_rsqrt_ps(vLengthSq);
+        result.m_data = _mm_mul_ps(result.m_data, m_data);
+        return result;
+    }
+
+    inline bool Quaternion::IsNormalized() const
+    {
+        return ToVector().IsNormalized4();
+    }
+
+    inline bool Quaternion::IsIdentity() const
+    {
+        return ToVector().IsEqual3(Vector::UnitW);
+    }
+
+    inline Quaternion Quaternion::operator*(const Quaternion& rhs) const
+    {
+        static const __m128 controlWZYX = { 1.0f,-1.0f, 1.0f,-1.0f };
+        static const __m128 controlZWXY = { 1.0f, 1.0f,-1.0f,-1.0f };
+        static const __m128 controlYXWZ = { -1.0f, 1.0f, 1.0f,-1.0f };
+
+        // Copy to SSE registers and use as few as possible for x86
+        __m128 Q2X = rhs;
+        __m128 Q2Y = rhs;
+        __m128 Q2Z = rhs;
+        __m128 vResult = rhs;
+        // Splat with one instruction
+        vResult = _mm_shuffle_ps(vResult, vResult, _MM_SHUFFLE(3, 3, 3, 3));
+        Q2X = _mm_shuffle_ps(Q2X, Q2X, _MM_SHUFFLE(0, 0, 0, 0));
+        Q2Y = _mm_shuffle_ps(Q2Y, Q2Y, _MM_SHUFFLE(1, 1, 1, 1));
+        Q2Z = _mm_shuffle_ps(Q2Z, Q2Z, _MM_SHUFFLE(2, 2, 2, 2));
+        // Retire Q1 and perform Q1*Q2W
+        vResult = _mm_mul_ps(vResult, *this);
+        __m128 Q1Shuffle = *this;
+        // Shuffle the copies of Q1
+        Q1Shuffle = _mm_shuffle_ps(Q1Shuffle, Q1Shuffle, _MM_SHUFFLE(0, 1, 2, 3));
+        // Mul by Q1WZYX
+        Q2X = _mm_mul_ps(Q2X, Q1Shuffle);
+        Q1Shuffle = _mm_shuffle_ps(Q1Shuffle, Q1Shuffle, _MM_SHUFFLE(2, 3, 0, 1));
+        // Flip the signs on m_y and m_z
+        Q2X = _mm_mul_ps(Q2X, controlWZYX);
+        // Mul by Q1ZWXY
+        Q2Y = _mm_mul_ps(Q2Y, Q1Shuffle);
+        Q1Shuffle = _mm_shuffle_ps(Q1Shuffle, Q1Shuffle, _MM_SHUFFLE(0, 1, 2, 3));
+        // Flip the signs on m_z and m_w
+        Q2Y = _mm_mul_ps(Q2Y, controlZWXY);
+        // Mul by Q1YXWZ
+        Q2Z = _mm_mul_ps(Q2Z, Q1Shuffle);
+        vResult = _mm_add_ps(vResult, Q2X);
+        // Flip the signs on m_x and m_w
+        Q2Z = _mm_mul_ps(Q2Z, controlYXWZ);
+        Q2Y = _mm_add_ps(Q2Y, Q2Z);
+        vResult = _mm_add_ps(vResult, Q2Y);
+
+        return Quaternion(vResult);
+    }
+
+    inline Quaternion& Quaternion::operator*=(const Quaternion& rhs)
+    {
+        *this = *this * rhs;
+        return *this;
+    }
+
+    inline bool Quaternion::IsNearEqual(const Quaternion& rhs, Radians const threshold) const
+    {
+        return Quaternion::Distance(*this, rhs) <= threshold;
+    }
+
+    inline bool Quaternion::operator==(const Quaternion& rhs) const
+    {
+        return ToVector() == rhs.ToVector();
+    }
+
+    inline bool Quaternion::operator!=(const Quaternion& rhs) const
+    {
+        return !operator==(rhs);
+    }
+
+    inline Vector Quaternion::GetSplatW() const
+    {
+        return _mm_shuffle_ps(m_data, m_data, _MM_SHUFFLE(3, 3, 3, 3));
+    }
+
+    inline float Quaternion::GetW() const
+    {
+        auto vTemp = GetSplatW();
+        return _mm_cvtss_f32(vTemp);
+    }
+}

MotionCorrection/src/cpp/Math/SIMD.h

@@ -0,0 +1,100 @@
+/*
+ * SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
+ * SPDX-License-Identifier: Apache-2.0
+ */
+
+#pragma once
+
+#include <stdint.h>
+#include <immintrin.h>
+
+namespace SIMD
+{
+    struct alignas( 16 ) IntMask
+    {
+        inline operator __m128( ) const { return reinterpret_cast<__m128 const&>( *this ); }
+        inline operator __m128i( ) const { return _mm_castps_si128( *this ); }
+        inline operator __m128d( ) const { return _mm_castps_pd( *this ); }
+
+        int32_t i[4];
+    };
+
+    struct alignas( 16 ) UIntMask
+    {
+        inline operator __m128( ) const { return reinterpret_cast<__m128 const&>( *this ); }
+        inline operator __m128i( ) const { return _mm_castps_si128( *this ); }
+        inline operator __m128d( ) const { return _mm_castps_pd( *this ); }
+
+        uint32_t v[4];
+    };
+
+    struct alignas( 16 ) FloatMask
+    {
+        inline operator __m128() const { return reinterpret_cast<__m128 const&>( *this ); }
+        inline operator __m128i() const { return _mm_castps_si128( *this ); }
+        inline operator __m128d() const { return _mm_castps_pd( *this ); }
+
+        float v[4];
+    };
+
+    // Int Operations
+    //-------------------------------------------------------------------------
+
+    namespace Int
+    {
+        FORCE_INLINE bool Equal( __m128 V1, __m128 V2 )
+        {
+            __m128i vTemp = _mm_cmpeq_epi32( _mm_castps_si128( V1 ), _mm_castps_si128( V2 ) );
+            return ( ( ( _mm_movemask_ps( _mm_castsi128_ps( vTemp ) ) & 7 ) == 7 ) != 0 );
+        }
+
+        FORCE_INLINE bool NotEqual( __m128 V1, __m128 V2 )
+        {
+            __m128i vTemp = _mm_cmpeq_epi32( _mm_castps_si128( V1 ), _mm_castps_si128( V2 ) );
+            return ( ( _mm_movemask_ps( _mm_castsi128_ps( vTemp ) ) != 0xF ) != 0 );
+        }
+
+        FORCE_INLINE __m128 And( __m128 V1, __m128 V2 )
+        {
+            return _mm_and_ps( V1, V2 );
+        }
+
+        FORCE_INLINE __m128 Or( __m128 V1, __m128 V2 )
+        {
+            __m128i V = _mm_or_si128( _mm_castps_si128( V1 ), _mm_castps_si128( V2 ) );
+            return _mm_castsi128_ps( V );
+        }
+    }
+
+    //-------------------------------------------------------------------------
+
+    static __m128 const g_sinCoefficients0 = { -0.16666667f, +0.0083333310f, -0.00019840874f, +2.7525562e-06f };
+    static __m128 const g_sinCoefficients1 = { -2.3889859e-08f, -0.16665852f, +0.0083139502f, -0.00018524670f };
+    static __m128 const g_cosCoefficients0 = { -0.5f, +0.041666638f, -0.0013888378f, +2.4760495e-05f };
+    static __m128 const g_cosCoefficients1 = { -2.6051615e-07f, -0.49992746f, +0.041493919f, -0.0012712436f };
+    static __m128 const g_tanCoefficients0 = { 1.0f, 0.333333333f, 0.133333333f, 5.396825397e-2f };
+    static __m128 const g_tanCoefficients1 = { 2.186948854e-2f, 8.863235530e-3f, 3.592128167e-3f, 1.455834485e-3f };
+    static __m128 const g_tanCoefficients2 = { 5.900274264e-4f, 2.391290764e-4f, 9.691537707e-5f, 3.927832950e-5f };
+    static __m128 const g_arcCoefficients0 = { +1.5707963050f, -0.2145988016f, +0.0889789874f, -0.0501743046f };
+    static __m128 const g_arcCoefficients1 = { +0.0308918810f, -0.0170881256f, +0.0066700901f, -0.0012624911f };
+    static __m128 const g_aTanCoefficients0 = { -0.3333314528f, +0.1999355085f, -0.1420889944f, +0.1065626393f };
+    static __m128 const g_aTanCoefficients1 = { -0.0752896400f, +0.0429096138f, -0.0161657367f, +0.0028662257f };
+    static __m128 const g_aTanEstCoefficients0 = { +0.999866f, +0.999866f, +0.999866f, +0.999866f };
+    static __m128 const g_aTanEstCoefficients1 = { -0.3302995f, +0.180141f, -0.085133f, +0.0208351f };
+    static __m128 const g_tanEstCoefficients = { 2.484f, -1.954923183e-1f, 2.467401101f, Math::OneDivPi };
+    static __m128 const g_arcEstCoefficients = { +1.5707288f,-0.2121144f,+0.0742610f,-0.0187293f };
+    static __m128 const g_aTan2Constants = { Math::Pi, Math::PiDivTwo, Math::PiDivFour, 2.3561944905f /* 3/4 Pi */ };
+
+    //-------------------------------------------------------------------------
+
+    static FloatMask const g_noFraction = { 8388608.0f,8388608.0f,8388608.0f,8388608.0f };
+    static IntMask const g_absMask = { 0x7FFFFFFF, 0x7FFFFFFF, 0x7FFFFFFF, 0x7FFFFFFF };
+    static UIntMask const g_trueMask = { 0xFFFFFFFF, 0xFFFFFFFF, 0xFFFFFFFF, 0xFFFFFFFF };
+    static UIntMask const g_signMask = { 0x80000000, 0x80000000, 0x80000000, 0x80000000 };
+    static UIntMask const g_maskX000 = { 0xFFFFFFFF, 0x00000000, 0x00000000, 0x00000000 };
+    static UIntMask const g_mask0Y00 = { 0x00000000, 0xFFFFFFFF, 0x00000000, 0x00000000 };
+    static UIntMask const g_mask00Z0 = { 0x00000000, 0x00000000, 0xFFFFFFFF, 0x00000000 };
+    static UIntMask const g_mask000W = { 0x00000000, 0x00000000, 0x00000000, 0xFFFFFFFF };
+    static UIntMask const g_maskXY00 = { 0xFFFFFFFF, 0xFFFFFFFF, 0x00000000, 0x00000000 };
+    static UIntMask const g_maskXYZ0 = { 0xFFFFFFFF, 0xFFFFFFFF, 0xFFFFFFFF, 0x00000000 };
+}

MotionCorrection/src/cpp/Math/Scalar.h

@@ -0,0 +1,187 @@
+/*
+ * SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
+ * SPDX-License-Identifier: Apache-2.0
+ */
+
+#pragma once
+
+#include "Compiler.h"
+#include "Debug.h"
+
+#include "Constants.h"
+
+#include <math.h>
+#include <stdint.h>
+
+//
+// Scalar related methods
+//
+
+namespace Math
+{
+    FORCE_INLINE float Sin( float value ) { return sinf( value ); }
+    FORCE_INLINE float Cos( float value ) { return cosf( value ); }
+    FORCE_INLINE float Tan( float value ) { return tanf( value ); }
+
+    FORCE_INLINE float ASin( float value ) { return asinf( value ); }
+    FORCE_INLINE float ACos( float value ) { return acosf( value ); }
+    FORCE_INLINE float ATan( float value ) { return atanf( value ); }
+    FORCE_INLINE float ATan2( float y, float x ) { return atan2f( y, x ); }
+
+    FORCE_INLINE float Cosec( float value ) { return 1.0f / sinf( value ); }
+    FORCE_INLINE float Sec( float value ) { return 1.0f / cosf( value ); }
+    FORCE_INLINE float Cot( float value ) { return 1.0f / tanf( PiDivTwo - value ); }
+
+    FORCE_INLINE float Pow( float x, float y ) { return powf( x, y ); }
+    FORCE_INLINE float Sqr( float value ) { return value * value; }
+    FORCE_INLINE float Sqrt( float value ) { return sqrtf( value ); }
+
+    FORCE_INLINE float Log( float value ) { return logf( value ); }
+    FORCE_INLINE float Log2f( float value ) { return log2f( value ); }
+
+    FORCE_INLINE float AddToMovingAverage( float currentAverage, uint64_t numCurrentSamples, float newValue )
+    {
+        return currentAverage + ( ( newValue - currentAverage ) / float( numCurrentSamples + 1 ) );
+    }
+
+    FORCE_INLINE float Abs( float a ) { return fabsf( a ); }
+    FORCE_INLINE double Abs( double a ) { return fabs( a ); }
+    FORCE_INLINE int8_t Abs( int8_t a ) { return (int8_t) abs( a ); }
+    FORCE_INLINE int16_t Abs( int16_t a ) { return (int16_t) abs( a ); }
+    FORCE_INLINE int32_t Abs( int32_t a ) { return labs( a ); }
+    FORCE_INLINE int64_t Abs( int64_t a ) { return llabs( a ); }
+
+    FORCE_INLINE float Reciprocal( float r ) { return 1.0f / r; }
+    FORCE_INLINE double Reciprocal( double r ) { return 1.0 / r; }
+
+    template<typename T>
+    FORCE_INLINE T Min( T a, T b ) { return a <= b ? a : b; }
+
+    template<typename T>
+    FORCE_INLINE T Max( T a, T b ) { return a >= b ? a : b; }
+
+    template<typename T>
+    FORCE_INLINE T AbsMin( T a, T b ) { return Abs( a ) <= Abs( b ) ? a : b; }
+
+    template<typename T>
+    FORCE_INLINE T AbsMax( T a, T b ) { return Abs( a ) >= Abs( b ) ? a : b; }
+
+    template<typename T>
+    FORCE_INLINE T Sqrt( T a ) { return sqrt( a ); }
+
+    template<typename T>
+    FORCE_INLINE T Clamp( T value, T lowerBound, T upperBound )
+    {
+        ASSERT( lowerBound <= upperBound );
+        return Min( Max( value, lowerBound ), upperBound );
+    }
+
+    template<typename T>
+    FORCE_INLINE bool IsInRangeInclusive( T value, T lowerBound, T upperBound )
+    {
+        ASSERT( lowerBound < upperBound );
+        return value >= lowerBound && value <= upperBound;
+    }
+
+    template<typename T>
+    FORCE_INLINE bool IsInRangeExclusive( T value, T lowerBound, T upperBound )
+    {
+        ASSERT( lowerBound < upperBound );
+        return value > lowerBound && value < upperBound;
+    }
+
+    // Decomposes a float into integer and remainder portions, remainder is return and the integer result is stored in the integer portion
+    FORCE_INLINE float ModF( float value, float& integerPortion )
+    {
+        return modff( value, &integerPortion );
+    }
+
+    // Returns the floating point remainder of x/y
+    FORCE_INLINE float FModF( float x, float y )
+    {
+        return fmodf( x, y );
+    }
+
+    template<typename T>
+    FORCE_INLINE T Lerp( T A, T B, float t )
+    {
+        return A + ( B - A ) * t;
+    }
+
+    FORCE_INLINE float PercentageThroughRange( float value, float lowerBound, float upperBound )
+    {
+        ASSERT( lowerBound < upperBound );
+        return Clamp( value, lowerBound, upperBound ) / ( upperBound - lowerBound );
+    }
+
+    FORCE_INLINE bool IsNearEqual( float value, float comparand, float epsilon = Epsilon )
+    {
+        return fabsf( value - comparand ) <= epsilon;
+    }
+
+    FORCE_INLINE bool IsNearZero( float value, float epsilon = Epsilon )
+    {
+        return fabsf( value ) <= epsilon;
+    }
+
+    FORCE_INLINE bool IsNearEqual( double value, double comparand, double epsilon = Epsilon )
+    {
+        return fabs( value - comparand ) <= epsilon;
+    }
+
+    FORCE_INLINE bool IsNearZero( double value, double epsilon = Epsilon )
+    {
+        return fabs( value ) <= epsilon;
+    }
+
+    FORCE_INLINE float Ceiling( float value )
+    {
+        return ceilf( value );
+    }
+
+    FORCE_INLINE int32_t CeilingToInt( float value )
+    {
+        return (int32_t) ceilf( value );
+    }
+
+    FORCE_INLINE float Floor( float value )
+    {
+        return floorf( value );
+    }
+
+    FORCE_INLINE int32_t FloorToInt( float value )
+    {
+        return (int32_t) floorf( value );
+    }
+
+    FORCE_INLINE float Round( float value )
+    {
+        return roundf( value );
+    }
+
+    FORCE_INLINE int32_t RoundToInt( float value )
+    {
+        return (int32_t) roundf( value );
+    }
+
+    inline float RemapRange( float value, float fromRangeBegin, float fromRangeEnd, float toRangeBegin, float toRangeEnd )
+    {
+        float const fromRangeLength = fromRangeEnd - fromRangeBegin;
+        float const percentageThroughFromRange = Clamp( ( value - fromRangeBegin ) / fromRangeLength, 0.0f, 1.0f );
+        float const toRangeLength = toRangeEnd - toRangeBegin;
+        float const result = toRangeBegin + ( percentageThroughFromRange * toRangeLength );
+
+        return result;
+    }
+
+    FORCE_INLINE float Square( float value )
+    {
+        return value * value;
+    }
+
+    FORCE_INLINE float SmoothStep01( float value )
+    {
+        value = Clamp( value, 0.0f, 1.0f );
+        return value * value * ( 3.0f - 2.0f * value );
+    }
+}

MotionCorrection/src/cpp/Math/Transform.cpp

@@ -0,0 +1,19 @@
+/*
+ * SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
+ * SPDX-License-Identifier: Apache-2.0
+ */
+
+#include "Transform.h"
+
+namespace Math
+{
+    Transform const Transform::Identity = Transform(Quaternion(0, 0, 0, 1), Vector(0, 0, 0, 1), 1.0f);
+
+    void Transform::SanitizeScaleValue()
+    {
+        if (Math::IsNearEqual(GetScale(), 1.0f, Math::LargeEpsilon))
+        {
+            SetScale(1.0f);
+        }
+    }
+}

MotionCorrection/src/cpp/Math/Transform.h

@@ -0,0 +1,213 @@
+/*
+ * SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
+ * SPDX-License-Identifier: Apache-2.0
+ */
+
+#pragma once
+
+#include "Matrix.h"
+
+namespace Math
+{
+    //
+    // VQS Transform
+    //
+
+    class Transform
+    {
+    public:
+
+        static Transform const Identity;
+
+        static Transform FromRotation(const Quaternion& rotation);
+        static Transform FromTranslation(const Vector& translation);
+        static Transform FromScale(float uniformScale);
+        static Transform FromTranslationAndScale(const Vector& translation, float uniformScale);
+        static Transform FromRotationBetweenVectors(const Vector sourceVector, const Vector targetVector);
+
+        // Linearly interpolate between two transforms - uses NLerp for rotations
+        static Transform Lerp(const Transform& from, const Transform& to, float t);
+
+        // Spherically interpolate between two transforms - uses SLerp for rotations
+        static Transform Slerp(const Transform& from, const Transform& to, float t);
+
+        // Spherically interpolate between two transforms - uses FastSLerp (SLerp approximation) for rotations
+        static Transform FastSlerp(const Transform& from, const Transform& to, float t);
+
+        // Calculate a delta transform that you can concatenate to the
+        // 'from' transform to get the 'to' transform. Properly handles the non-uniform scaling case.
+        static Transform Delta(const Transform& from, const Transform& to);
+
+        // Calculates a delta transform that you can concatenate to the
+        // 'from' transform to get the 'to' transform (ignoring scale)
+        static Transform DeltaNoScale(const Transform& from, const Transform& to);
+
+        static void DirectlySetRotation(Transform& transform, Quaternion&& rotation);
+        static void DirectlySetRotation(Transform& transform, const Quaternion& rotation);
+        static void DirectlySetTranslationScale(Transform& transform, Vector&& translationScale);
+        static void DirectlySetTranslationScale(Transform& transform, const Vector& translationScale);
+
+    public:
+
+        Transform() = default;
+
+        explicit Transform(NoInit_t);
+        explicit Transform(const Matrix& m);
+        explicit Transform(
+            const Quaternion& rotation,
+                const Vector& translation = Vector(0, 0, 0, 0),
+                    float scale = 1.0f);
+        explicit Transform(const AxisAngle& rotation);
+
+        Matrix ToMatrix() const;
+        Matrix ToMatrixNoScale() const;
+        EulerAngles ToEulerAngles() const;
+
+        Vector GetAxisX() const;
+        Vector GetAxisY() const;
+        Vector GetAxisZ() const;
+
+        Vector GetRightVector() const;
+        Vector GetForwardVector() const;
+        Vector GetUpVector() const;
+
+        bool IsIdentity() const;
+        bool IsRigidTransform() const;
+        void MakeRigidTransform();
+
+        //
+        // Inverse and Deltas
+        //
+
+        // Invert this transform.
+        // If you want a delta transform that you can
+        // concatenate, then you should use the 'Delta' functions
+        Transform& Inverse();
+
+        // Get the inverse of this transform.
+        // If you want a delta transform that you can
+        // concatenate, then you should use the 'Delta' functions
+        Transform GetInverse() const;
+
+        // Return the delta required to a given target
+        // transform (i.e., what do we need to add to reach that transform)
+        Transform GetDeltaToOther(const Transform& targetTransform) const;
+
+        // Return the delta relative from a given a start
+        // transform (i.e., how much do we differ from it)
+        Transform GetDeltaFromOther(const Transform& startTransform) const;
+
+        //
+        // Rotation
+
+        const Quaternion& GetRotation() const;
+        void SetRotation(const Quaternion& rotation);
+        void AddRotation(const Quaternion& delta);
+
+        //
+        // Translation
+        //
+
+        // Get the translation for this transform
+        // NOTE: you cannot rely on the W value as that will be the scale
+        const Vector& GetTranslation() const;
+
+        // Get the translation and scale for this transform
+        const Vector& GetTranslationAndScale() const;
+
+        // Set the translation
+        void SetTranslation(const Vector& newTranslation);
+
+        // Set the translation and scale simultaneously
+        void SetTranslationAndScale(const Vector& newTranslationScale);
+
+        // Add an offset to the current translation
+        void AddTranslation(const Vector& translationDelta);
+
+        // Get the translation as a homogeneous coordinates' vector (W=0)
+        Vector GetTranslationAsVector() const;
+
+        // Get the translation as a homogeneous coordinates' point (W=1)
+        Vector GetTranslationAsPoint() const;
+
+        //
+        // Scale
+        //
+
+        float GetScale() const;
+        Vector GetScaleVector() const;
+        Vector GetInverseScaleVector() const;
+        void SetScale(float uniformScale);
+        bool HasScale() const;
+        bool HasNegativeScale() const;
+
+        // This function will sanitize the scale values to remove any
+        // trailing values from scale factors i.e. 1.000000012 will be converted to 1
+        // This is primarily needed in import steps where scale values
+        // might be sampled from curves or have multiple conversions applied resulting in variance.
+        void SanitizeScaleValue();
+
+        //
+        // Transformations
+        //
+
+        Vector TranslateVector(const Vector& vector) const;
+        Vector ScaleVector(const Vector& vector) const;
+        Vector TransformPoint(const Vector& vector) const;
+        Vector TransformPointNoScale(const Vector& vector) const;
+
+        // Rotate a vector (same as TransformVectorNoScale)
+        Vector RotateVector(const Vector& vector) const;
+
+        // Rotate a vector (same as TransformVectorNoScale)
+        Vector TransformNormal(const Vector& vector) const;
+
+        // Unrotate a vector (same as InverseTransformVectorNoScale)
+        Vector InverseRotateVector(const Vector& vector) const;
+
+        // Invert the operation order when doing inverse transformation: first translation then rotation then scale
+        Vector InverseTransformPoint(const Vector& point) const;
+
+        // Invert the operation order when doing inverse transformation: first translation then rotation
+        Vector InverseTransformPointNoScale(const Vector& point) const;
+
+        // Applies scale and rotation to a vector (no translation)
+        Vector TransformVector(const Vector& vector) const;
+
+        // Rotate a vector
+        Vector TransformVectorNoScale(const Vector& vector) const;
+
+        // Invert the operation order when performing inverse transformation: first rotation then scale
+        Vector InverseTransformVector(const Vector& vector) const;
+
+        // Unrotate a vector
+        Vector InverseTransformVectorNoScale(const Vector& vector) const;
+
+        // WARNING: The results from multiplying transforms with shear or skew is ill-defined
+        Transform operator*(const Transform& rhs) const;
+
+        // WARNING: The results from multiplying transforms with shear or skew is ill-defined
+        Transform& operator*=(const Transform& rhs);
+
+        //
+        // Operators
+        //
+
+        bool IsNearEqual(
+            const Transform& rhs,
+                const Radians angleThreshold = Math::DegreesToRadians,
+                    float translationScaleThreshold = Math::Epsilon) const;
+
+        // Exact equality
+        bool operator==(const Transform& rhs) const;
+
+        bool operator!=(const Transform& rhs) const;
+
+    private:
+
+        Quaternion m_rotation = Quaternion(0, 0, 0, 1);
+        Vector m_translationScale = Vector(0, 0, 0, 1);
+    };
+}
+
+#include "Transform.inl"

MotionCorrection/src/cpp/Math/Transform.inl

@@ -0,0 +1,508 @@
+/*
+ * SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
+ * SPDX-License-Identifier: Apache-2.0
+ */
+
+#pragma once
+
+#include "Transform.h"
+
+namespace Math
+{
+    inline Transform Transform::FromRotation(const Quaternion& rotation)
+    {
+        return Transform(rotation);
+    }
+
+    inline Transform Transform::FromTranslation(const Vector& translation)
+    {
+        return Transform(Quaternion::Identity, translation);
+    }
+
+    inline Transform Transform::FromScale(float uniformScale)
+    {
+        return Transform(Quaternion::Identity, Vector::Zero, uniformScale);
+    }
+
+    inline Transform Transform::FromTranslationAndScale(const Vector& translation, float uniformScale)
+    {
+        return Transform(Quaternion::Identity, translation, uniformScale);
+    }
+
+    inline Transform Transform::FromRotationBetweenVectors(const Vector sourceVector, const Vector targetVector)
+    {
+        return Transform(Quaternion::FromRotationBetweenNormalizedVectors(sourceVector, targetVector));
+    }
+
+    inline Transform Transform::Lerp(const Transform& from, const Transform& to, float t)
+    {
+        Quaternion const rotation = Quaternion::NLerp(Quaternion(from.m_rotation), Quaternion(to.m_rotation), t);
+        Vector const translationAndScale = Vector::Lerp(from.m_translationScale, to.m_translationScale, t);
+
+        Transform lerped(NoInit);
+        lerped.m_rotation = rotation;
+        lerped.m_translationScale = translationAndScale;
+        return lerped;
+    }
+
+    inline Transform Transform::Slerp(const Transform& from, const Transform& to, float t)
+    {
+        Quaternion const rotation = Quaternion::SLerp(Quaternion(from.m_rotation), Quaternion(to.m_rotation), t);
+        Vector const translationAndScale = Vector::Lerp(Vector(from.m_translationScale), Vector(to.m_translationScale), t);
+
+        Transform lerped(NoInit);
+        lerped.m_rotation = rotation;
+        lerped.m_translationScale = translationAndScale;
+        return lerped;
+    }
+
+    inline Transform Transform::FastSlerp(const Transform& from, const Transform& to, float t)
+    {
+        Quaternion const rotation = Quaternion::FastSLerp(Quaternion(from.m_rotation), Quaternion(to.m_rotation), t);
+        Vector const translationAndScale = Vector::Lerp(Vector(from.m_translationScale), Vector(to.m_translationScale), t);
+
+        Transform lerped(NoInit);
+        lerped.m_rotation = rotation;
+        lerped.m_translationScale = translationAndScale;
+        return lerped;
+    }
+
+    inline Transform Transform::Delta(const Transform& from, const Transform& to)
+    {
+        ASSERT(from.m_rotation.IsNormalized() && to.m_rotation.IsNormalized());
+        ASSERT(!from.m_translationScale.IsW0() && !to.m_translationScale.IsW0());
+
+        Transform result;
+
+        Vector const inverseScale = from.GetInverseScaleVector();
+        Vector const deltaScale = to.GetScaleVector() * inverseScale;
+
+        // If we have negative scaling, we need to use matrices to calculate the deltas
+        Vector const minScale = Vector::Min(from.m_translationScale.GetSplatW(), to.m_translationScale.GetSplatW());
+        if (minScale.IsAnyLessThan(Vector::Zero))
+        {
+            // Multiply the transforms using matrices to get the correct rotation and then remove the scale;
+            Matrix const toMtx = to.ToMatrix();
+            Matrix const fromMtx = from.ToMatrix();
+            Matrix resultMtx = toMtx * fromMtx.GetInverse();
+            resultMtx.RemoveScaleFast();
+
+            // Apply back the signs from the final scale
+            Vector const sign = deltaScale.GetSign();
+            resultMtx[0] *= sign.GetSplatX();
+            resultMtx[1] *= sign.GetSplatY();
+            resultMtx[2] *= sign.GetSplatZ();
+
+            result.m_rotation = resultMtx.GetRotation();
+            ASSERT(result.m_rotation.IsNormalized());
+            result.m_translationScale = Vector::Select(resultMtx.GetTranslation(), deltaScale, Vector::Select0001);
+        }
+        else
+        {
+            Quaternion const fromInverseRotation = from.m_rotation.GetInverse();
+            result.m_rotation = to.m_rotation * fromInverseRotation;
+
+            Vector const deltaTranslation = to.m_translationScale - from.m_translationScale;
+            Vector const translation = fromInverseRotation.RotateVector(deltaTranslation) * inverseScale;
+            result.m_translationScale = Vector::Select(translation, deltaScale, Vector::Select0001);
+        }
+
+        return result;
+    }
+
+    inline Transform Transform::DeltaNoScale(const Transform& from, const Transform& to)
+    {
+        Quaternion const inverseFromRotation = from.m_rotation.GetInverse();
+        Vector const deltaTranslation = to.GetTranslation() - from.GetTranslation();
+
+        Transform delta;
+        delta.m_rotation = to.m_rotation * inverseFromRotation;
+        delta.m_translationScale = inverseFromRotation.RotateVector(deltaTranslation).GetWithW1();
+        return delta;
+    }
+
+    inline void Transform::DirectlySetRotation(Transform& transform, Quaternion&& rotation)
+    {
+        transform.m_rotation = rotation;
+    }
+
+    inline void Transform::DirectlySetRotation(Transform& transform, const Quaternion& rotation)
+    {
+        transform.m_rotation = rotation;
+    }
+
+    inline void Transform::DirectlySetTranslationScale(Transform& transform, Vector&& translationScale)
+    {
+        transform.m_translationScale = translationScale;
+    }
+
+    inline void Transform::DirectlySetTranslationScale(Transform& transform, const Vector& translationScale)
+    {
+        transform.m_translationScale = translationScale;
+    }
+
+    inline Transform::Transform(NoInit_t)
+        : m_rotation(NoInit)
+        , m_translationScale(NoInit)
+    {
+    }
+
+    inline Transform::Transform(const Matrix& m)
+    {
+        Vector mTranslation, mScale;
+        m.Decompose(m_rotation, mTranslation, mScale);
+        ASSERT(Math::IsNearEqual(mScale.GetX(), mScale.GetY()) && Math::IsNearEqual(mScale.GetY(),mScale.GetZ()));
+        m_translationScale = Vector::Select(mTranslation, mScale, Vector::Select0001);
+    }
+
+    inline Transform::Transform(const Quaternion& rotation, const Vector& translation, float scale)
+        : m_rotation(rotation)
+        , m_translationScale(Vector::Select(translation, Vector(scale), Vector::Select0001))
+    {
+    }
+
+    inline Transform::Transform(const AxisAngle& rotation)
+        : m_rotation(rotation)
+        , m_translationScale(Vector::UnitW)
+    {
+    }
+
+    inline Matrix Transform::ToMatrix() const
+    {
+        return Matrix(m_rotation, m_translationScale.GetWithW1(), m_translationScale.GetSplatW());
+    }
+
+    inline Matrix Transform::ToMatrixNoScale() const
+    {
+        return Matrix(m_rotation, m_translationScale.GetWithW1(), Vector::One);
+    }
+
+    inline EulerAngles Transform::ToEulerAngles() const
+    {
+        return m_rotation.ToEulerAngles();
+    }
+
+    inline Vector Transform::GetAxisX() const
+    {
+        return m_rotation.RotateVector(Vector::UnitX);
+    }
+
+    inline Vector Transform::GetAxisY() const
+    {
+        return m_rotation.RotateVector(Vector::UnitY);
+    }
+
+    inline Vector Transform::GetAxisZ() const
+    {
+        return m_rotation.RotateVector(Vector::UnitZ);
+    }
+
+    inline Vector Transform::GetRightVector() const
+    {
+        return m_rotation.RotateVector(Vector::WorldRight);
+    }
+
+    inline Vector Transform::GetForwardVector() const
+    {
+        return m_rotation.RotateVector(Vector::WorldForward);
+    }
+
+    inline Vector Transform::GetUpVector() const
+    {
+        return m_rotation.RotateVector(Vector::WorldUp);
+    }
+
+    inline bool Transform::IsIdentity() const
+    {
+        return m_rotation.IsIdentity() && m_translationScale.IsEqual4(Vector::UnitW);
+    }
+
+    inline bool Transform::IsRigidTransform() const
+    {
+        return GetScale() == 1.0f;
+    }
+
+    inline void Transform::MakeRigidTransform()
+    {
+        SetScale(1.0f);
+    }
+
+    inline Transform& Transform::Inverse()
+    {
+        ASSERT(!m_translationScale.IsW0());
+
+        Quaternion const inverseRotation = m_rotation.GetInverse();
+        m_rotation = inverseRotation;
+
+        Vector const inverseScale = GetInverseScaleVector();
+        Vector const inverselyScaledTranslation = inverseScale * m_translationScale.GetWithW0();
+        Vector const inverselyRotatedTranslation = inverseRotation.RotateVector(inverselyScaledTranslation);
+        Vector const inverseTranslation = inverselyRotatedTranslation.GetNegated().SetW0();
+
+        m_translationScale = Vector::Select(inverseTranslation, inverseScale, Vector::Select0001);
+
+        return *this;
+    }
+
+    inline Transform Transform::GetInverse() const
+    {
+        Transform inverse = *this;
+        return inverse.Inverse();
+    }
+
+    inline Transform Transform::GetDeltaToOther(const Transform& targetTransform) const
+    {
+        return Transform::Delta(*this, targetTransform);
+    }
+
+    inline Transform Transform::GetDeltaFromOther(const Transform& startTransform) const
+    {
+        return Transform::Delta(startTransform, *this);
+    }
+
+    inline const Quaternion& Transform::GetRotation() const
+    {
+        return m_rotation;
+    }
+
+    inline void Transform::SetRotation(const Quaternion& rotation)
+    {
+        ASSERT(rotation.IsNormalized());
+        m_rotation = rotation;
+    }
+
+    inline void Transform::AddRotation(const Quaternion& delta)
+    {
+        ASSERT(delta.IsNormalized());
+        m_rotation = delta * m_rotation;
+    }
+
+    inline const Vector& Transform::GetTranslation() const
+    {
+        return m_translationScale;
+    }
+
+    inline const Vector& Transform::GetTranslationAndScale() const
+    {
+        return m_translationScale;
+    }
+
+    inline void Transform::SetTranslation(const Vector& newTranslation)
+    {
+        m_translationScale = Vector::Select(newTranslation, m_translationScale, Vector::Select0001);
+    }
+
+    inline void Transform::SetTranslationAndScale(const Vector& newTranslationScale)
+    {
+        ASSERT(newTranslationScale.GetW() != 0.0f);
+        m_translationScale = newTranslationScale;
+    }
+
+    inline void Transform::AddTranslation(const Vector& translationDelta)
+    {
+        m_translationScale += translationDelta.GetWithW0();
+    }
+
+    inline Vector Transform::GetTranslationAsVector() const
+    {
+        return m_translationScale.GetWithW0();
+    }
+
+    inline Vector Transform::GetTranslationAsPoint() const
+    {
+        return m_translationScale.GetWithW1();
+    }
+
+    inline float Transform::GetScale() const
+    {
+        return m_translationScale.GetW();
+    }
+
+    inline Vector Transform::GetScaleVector() const
+    {
+        return m_translationScale.GetSplatW();
+    }
+
+    inline Vector Transform::GetInverseScaleVector() const
+    {
+        return m_translationScale.GetSplatW().GetInverse();
+    }
+
+    inline void Transform::SetScale(float uniformScale)
+    {
+        m_translationScale.SetW(uniformScale);
+    }
+
+    inline bool Transform::HasScale() const
+    {
+        return m_translationScale.GetW() != 1.0f;
+    }
+
+    inline bool Transform::HasNegativeScale() const
+    {
+        return m_translationScale.GetW() < 0.0f;
+    }
+
+    inline Vector Transform::TranslateVector(const Vector& vector) const
+    {
+        return vector + m_translationScale.GetWithW0();
+    }
+
+    inline Vector Transform::ScaleVector(const Vector& vector) const
+    {
+        return vector * GetScaleVector();
+    }
+
+    inline Vector Transform::TransformPoint(const Vector& point) const
+    {
+        ASSERT(!m_translationScale.IsW0());
+        Vector transformedPoint = point * m_translationScale.GetSplatW();
+        transformedPoint = (m_translationScale + m_rotation.RotateVector(transformedPoint)).GetWithW0();
+        return transformedPoint;
+    }
+
+    inline Vector Transform::TransformPointNoScale(const Vector& point) const
+    {
+        Vector transformedPoint = (m_translationScale + m_rotation.RotateVector(point)).GetWithW0();;
+        return transformedPoint;
+    }
+
+    inline Vector Transform::RotateVector(const Vector& vector) const
+    {
+        return m_rotation.RotateVector(vector);
+    }
+
+    inline Vector Transform::TransformNormal(const Vector& vector) const
+    {
+        return RotateVector(vector);
+    }
+
+    inline Vector Transform::InverseRotateVector(const Vector& vector) const
+    {
+        return m_rotation.RotateVectorInverse(vector);
+    }
+
+    inline Vector Transform::InverseTransformPoint(const Vector& point) const
+    {
+        ASSERT(!m_translationScale.IsW0());
+        Vector const shiftedPoint = point - m_translationScale;
+        Vector const unrotatedShiftedPoint = m_rotation.RotateVectorInverse(shiftedPoint);
+        Vector const inverseScale = GetInverseScaleVector();
+        Vector const result = unrotatedShiftedPoint * inverseScale;
+        return result;
+    }
+
+    inline Vector Transform::InverseTransformPointNoScale(const Vector& point) const
+    {
+        Vector const shiftedPoint = point - m_translationScale;
+        Vector const unrotatedShiftedPoint = m_rotation.RotateVectorInverse(shiftedPoint);
+        return unrotatedShiftedPoint;
+    }
+
+    inline Vector Transform::TransformVector(const Vector& vector) const
+    {
+        ASSERT(!m_translationScale.IsW0());
+        Vector transformedVector = vector * GetScaleVector();
+        transformedVector = m_rotation.RotateVector(transformedVector);
+        return transformedVector;
+    }
+
+    inline Vector Transform::TransformVectorNoScale(const Vector& vector) const
+    {
+        return RotateVector(vector);
+    }
+
+    inline Vector Transform::InverseTransformVector(const Vector& vector) const
+    {
+        ASSERT(!m_translationScale.IsW0());
+        Vector const unrotatedVector = m_rotation.RotateVectorInverse(vector);
+        Vector const inverseScale = GetInverseScaleVector();
+        Vector const result = unrotatedVector * inverseScale;
+        return result;
+    }
+
+    inline Vector Transform::InverseTransformVectorNoScale(const Vector& vector) const
+    {
+        return m_rotation.RotateVectorInverse(vector);
+    }
+
+    inline Transform Transform::operator*(const Transform& rhs) const
+    {
+        Transform transform = *this;
+        transform *= rhs;
+        return transform;
+    }
+
+    inline Transform& Transform::operator*=(const Transform& rhs)
+    {
+        Vector const scale = GetScaleVector();
+        Vector const rhsScale = rhs.GetScaleVector();
+        Vector const minScale = Vector::Min(scale, rhsScale);
+        Vector const finalScale = scale * rhsScale;
+
+        if (minScale.IsAnyLessThan(Vector::Zero))
+        {
+            // Multiply the transforms using matrices to
+            // get the correct rotation and then remove the scale;
+            Matrix const lhsMtx = ToMatrix();
+            Matrix const rhsMtx = rhs.ToMatrix();
+            Matrix resultMtx = lhsMtx * rhsMtx;
+            resultMtx.RemoveScaleFast();
+
+            // Apply back the signs from the final scale
+            Vector const sign = finalScale.GetSign();
+            resultMtx[0] *= sign.GetSplatX();
+            resultMtx[1] *= sign.GetSplatY();
+            resultMtx[2] *= sign.GetSplatZ();
+
+            m_rotation = resultMtx.GetRotation();
+            ASSERT(m_rotation.IsNormalized());
+            m_translationScale = Vector::Select(resultMtx.GetTranslation(), finalScale, Vector::Select0001);
+        }
+        else
+        {
+            // Normal case
+            m_rotation = m_rotation * rhs.m_rotation;
+            m_rotation.Normalize();
+            Vector const translation = rhs.m_rotation.RotateVector(m_translationScale * rhsScale) + rhs.m_translationScale;
+            m_translationScale = Vector::Select(translation, finalScale, Vector::Select0001);
+        }
+
+        return *this;
+    }
+
+    inline bool Transform::IsNearEqual(const Transform& rhs, const Radians angleThreshold, float translationScaleThreshold) const
+    {
+        if (!m_rotation.IsNearEqual(rhs.m_rotation, angleThreshold))
+        {
+            return false;
+        }
+
+        if (!m_translationScale.IsNearEqual4(rhs.m_translationScale, translationScaleThreshold))
+        {
+            return false;
+        }
+
+        return true;
+    }
+
+    inline bool Transform::operator==(const Transform& rhs) const
+    {
+        if (m_translationScale != rhs.m_translationScale)
+        {
+            return false;
+        }
+
+        if (m_rotation != rhs.m_rotation)
+        {
+            return false;
+        }
+
+        return true;
+    }
+
+    inline bool Transform::operator!=(const Transform& rhs) const
+    {
+        return !operator==(rhs);
+    }
+}

MotionCorrection/src/cpp/Math/Types.cpp

@@ -0,0 +1,44 @@
+/*
+ * SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
+ * SPDX-License-Identifier: Apache-2.0
+ */
+
+#include "Types.h"
+
+Int2 const Int2::Zero = Int2( 0, 0 );
+
+Int4 const Int4::Zero = Int4( 0, 0, 0, 0 );
+Int4 const Int4::MinusOne = Int4( -1, -1, -1, -1 );
+
+Float2 const Float2::Zero = Float2( 0, 0 );
+Float2 const Float2::One = Float2( 1, 1 );
+Float2 const Float2::UnitX = Float2( 1, 0 );
+Float2 const Float2::UnitY = Float2( 0, 1 );
+
+Float3 const Float3::Zero = Float3( 0, 0, 0 );
+Float3 const Float3::One = Float3( 1, 1, 1 );
+Float3 const Float3::UnitX = Float3( 1, 0, 0 );
+Float3 const Float3::UnitY = Float3( 0, 1, 0 );
+Float3 const Float3::UnitZ = Float3( 0, 0, 1 );
+
+Float3 const Float3::WorldForward = Float3( 0, -1, 0 );
+Float3 const Float3::WorldUp = Float3( 0, 0, 1 );
+Float3 const Float3::WorldRight = Float3( -1, 0, 0 );
+
+Float4 const Float4::Zero = Float4( 0, 0, 0, 0 );
+Float4 const Float4::One = Float4( 1, 1, 1, 1 );
+Float4 const Float4::UnitX = Float4( 1, 0, 0, 0 );
+Float4 const Float4::UnitY = Float4( 0, 1, 0, 0 );
+Float4 const Float4::UnitZ = Float4( 0, 0, 1, 0 );
+Float4 const Float4::UnitW = Float4( 0, 0, 0, 1 );
+
+Float4 const Float4::WorldForward = Float4( 0, -1, 0, 0 );
+Float4 const Float4::WorldUp = Float4( 0, 0, 1, 0 );
+Float4 const Float4::WorldRight = Float4( -1, 0, 0, 0 );
+
+Radians const Radians::Pi = Radians( Math::Pi );
+Radians const Radians::TwoPi = Radians( Math::TwoPi );
+Radians const Radians::OneDivPi = Radians( Math::OneDivPi );
+Radians const Radians::OneDivTwoPi = Radians( Math::OneDivTwoPi );
+Radians const Radians::PiDivTwo = Radians( Math::PiDivTwo );
+Radians const Radians::PiDivFour = Radians( Math::PiDivFour );

MotionCorrection/src/cpp/Math/Types.h

@@ -0,0 +1,808 @@
+/*
+ * SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
+ * SPDX-License-Identifier: Apache-2.0
+ */
+
+#pragma once
+
+#include "Scalar.h"
+
+enum NoInit_t { NoInit };
+enum ZeroInit_t { ZeroInit };
+enum IdentityInit_t { IdentityInit };
+
+enum class Axis : uint8_t
+{
+    X = 0,
+    Y,
+    Z,
+    NegX,
+    NegY,
+    NegZ
+};
+
+struct Float2;
+struct Float3;
+struct Float4;
+
+struct Int2
+{
+    static Int2 const Zero;
+
+public:
+
+    inline Int2() {}
+    inline Int2( ZeroInit_t ) : m_x( 0 ), m_y( 0 ) {}
+    inline Int2( Float2 const& v );
+    inline explicit Int2( int32_t v ) : m_x( v ), m_y( v ) {}
+    inline explicit Int2( int32_t ix, int32_t iy ) : m_x( ix ), m_y( iy ) {}
+
+    inline bool IsZero() const { return *this == Zero; }
+
+    inline int32_t& operator[]( uint32_t i ) { return ( (int32_t*) this )[i]; }
+    inline int32_t const& operator[]( uint32_t i ) const { return ( (int32_t*) this )[i]; }
+
+    inline bool operator==( Int2 const rhs ) const { return m_x == rhs.m_x && m_y == rhs.m_y; }
+    inline bool operator!=( Int2 const rhs ) const { return m_x != rhs.m_x || m_y != rhs.m_y; }
+
+    inline Int2 operator+( Int2 const& rhs ) const { return Int2( m_x + rhs.m_x, m_y + rhs.m_y ); }
+    inline Int2 operator-( Int2 const& rhs ) const { return Int2( m_x - rhs.m_x, m_y - rhs.m_y ); }
+    inline Int2 operator*( Int2 const& rhs ) const { return Int2( m_x * rhs.m_x, m_y * rhs.m_y ); }
+    inline Int2 operator/( Int2 const& rhs ) const { return Int2( m_x / rhs.m_x, m_y / rhs.m_y ); }
+
+    inline Int2& operator+=( int32_t const& rhs ) { m_x += rhs; m_y += rhs; return *this; }
+    inline Int2& operator-=( int32_t const& rhs ) { m_x -= rhs; m_y -= rhs; return *this; }
+    inline Int2& operator*=( int32_t const& rhs ) { m_x *= rhs; m_y *= rhs; return *this; }
+    inline Int2& operator/=( int32_t const& rhs ) { m_x /= rhs; m_y /= rhs; return *this; }
+
+    // Component wise operation
+    inline Int2 operator+( int32_t const& rhs ) const { return Int2( m_x + rhs, m_y + rhs ); }
+    inline Int2 operator-( int32_t const& rhs ) const { return Int2( m_x - rhs, m_y - rhs ); }
+    inline Int2 operator*( int32_t const& rhs ) const { return Int2( m_x * rhs, m_y * rhs ); }
+    inline Int2 operator/( int32_t const& rhs ) const { return Int2( m_x / rhs, m_y / rhs ); }
+
+    inline Int2& operator+=( Int2 const& rhs ) { m_x += rhs.m_x; m_y += rhs.m_y; return *this; }
+    inline Int2& operator-=( Int2 const& rhs ) { m_x -= rhs.m_x; m_y -= rhs.m_y; return *this; }
+    inline Int2& operator*=( Int2 const& rhs ) { m_x *= rhs.m_x; m_y *= rhs.m_y; return *this; }
+    inline Int2& operator/=( Int2 const& rhs ) { m_x /= rhs.m_x; m_y /= rhs.m_y; return *this; }
+
+public:
+
+    int32_t m_x, m_y;
+};
+
+struct Int3
+{
+    static Int3 const Zero;
+
+public:
+
+    inline Int3() {}
+    inline Int3( ZeroInit_t ) : m_x( 0 ), m_y( 0 ), m_z( 0 ) {}
+    inline Int3( Float3 const& v );
+    inline explicit Int3( int32_t v ) : m_x( v ), m_y( v ), m_z( v ) {}
+    inline explicit Int3( int32_t ix, int32_t iy, int32_t iz ) : m_x( ix ), m_y( iy ), m_z( iz ) {}
+
+    inline bool IsZero() const { return *this == Zero; }
+
+    inline int32_t& operator[]( uint32_t i ) { return ( (int32_t*) this )[i]; }
+    inline int32_t const& operator[]( uint32_t i ) const { return ( (int32_t*) this )[i]; }
+
+    inline bool operator==( Int3 const rhs ) const { return m_x == rhs.m_x && m_y == rhs.m_y && m_z == rhs.m_z; }
+    inline bool operator!=( Int3 const rhs ) const { return m_x != rhs.m_x || m_y != rhs.m_y || m_z != rhs.m_z; }
+
+    inline Int3 operator+( Int3 const& rhs ) const { return Int3( m_x + rhs.m_x, m_y + rhs.m_y, m_z + rhs.m_z ); }
+    inline Int3 operator-( Int3 const& rhs ) const { return Int3( m_x - rhs.m_x, m_y - rhs.m_y, m_z - rhs.m_z ); }
+    inline Int3 operator*( Int3 const& rhs ) const { return Int3( m_x * rhs.m_x, m_y * rhs.m_y, m_z * rhs.m_z ); }
+    inline Int3 operator/( Int3 const& rhs ) const { return Int3( m_x / rhs.m_x, m_y / rhs.m_y, m_z / rhs.m_z ); }
+
+    inline Int3& operator+=( int32_t const& rhs ) { m_x += rhs; m_y += rhs; m_z += rhs; return *this; }
+    inline Int3& operator-=( int32_t const& rhs ) { m_x -= rhs; m_y -= rhs; m_z -= rhs; return *this; }
+    inline Int3& operator*=( int32_t const& rhs ) { m_x *= rhs; m_y *= rhs; m_z *= rhs; return *this; }
+    inline Int3& operator/=( int32_t const& rhs ) { m_x /= rhs; m_y /= rhs; m_z /= rhs; return *this; }
+
+    // Component wise operation
+    inline Int3 operator+( int32_t const& rhs ) const { return Int3( m_x + rhs, m_y + rhs, m_z + rhs ); }
+    inline Int3 operator-( int32_t const& rhs ) const { return Int3( m_x - rhs, m_y - rhs, m_z - rhs ); }
+    inline Int3 operator*( int32_t const& rhs ) const { return Int3( m_x * rhs, m_y * rhs, m_z * rhs ); }
+    inline Int3 operator/( int32_t const& rhs ) const { return Int3( m_x / rhs, m_y / rhs, m_z / rhs ); }
+
+    inline Int3& operator+=( Int3 const& rhs ) { m_x += rhs.m_x; m_y += rhs.m_y; m_z += rhs.m_z; return *this; }
+    inline Int3& operator-=( Int3 const& rhs ) { m_x -= rhs.m_x; m_y -= rhs.m_y; m_z -= rhs.m_z; return *this; }
+    inline Int3& operator*=( Int3 const& rhs ) { m_x *= rhs.m_x; m_y *= rhs.m_y; m_z *= rhs.m_z; return *this; }
+    inline Int3& operator/=( Int3 const& rhs ) { m_x /= rhs.m_x; m_y /= rhs.m_y; m_z /= rhs.m_z; return *this; }
+
+public:
+
+    int32_t m_x, m_y, m_z;
+};
+
+struct Int4
+{
+    static Int4 const Zero;
+    static Int4 const MinusOne;
+
+public:
+
+    inline Int4() {}
+    inline Int4( ZeroInit_t ) : m_x( 0 ), m_y( 0 ), m_z( 0 ), m_w( 0 ) {}
+    inline explicit Int4( int32_t v ) : m_x( v ), m_y( v ), m_z( v ), m_w( v ) {}
+    inline explicit Int4( int32_t ix, int32_t iy, int32_t iz, int32_t iw ) : m_x( ix ), m_y( iy ), m_z( iz ), m_w( iw ) {}
+
+    inline bool IsZero() const { return *this == Zero; }
+
+    inline int32_t& operator[]( uint32_t i ) { return ( (int32_t*) this )[i]; }
+    inline int32_t const& operator[]( uint32_t i ) const { return ( (int32_t*) this )[i]; }
+
+    inline bool operator==( Int4 const rhs ) const { return m_x == rhs.m_x && m_y == rhs.m_y && m_z == rhs.m_z && m_w == rhs.m_w; }
+    inline bool operator!=( Int4 const rhs ) const { return m_x != rhs.m_x || m_y != rhs.m_y || m_z != rhs.m_z || m_w != rhs.m_w; }
+
+    inline Int4 operator+( int32_t const& rhs ) const { return Int4( m_x + rhs, m_y + rhs, m_z + rhs, m_w + rhs ); }
+    inline Int4 operator-( int32_t const& rhs ) const { return Int4( m_x - rhs, m_y - rhs, m_z - rhs, m_w - rhs ); }
+    inline Int4 operator*( int32_t const& rhs ) const { return Int4( m_x * rhs, m_y * rhs, m_z * rhs, m_w * rhs ); }
+    inline Int4 operator/( int32_t const& rhs ) const { return Int4( m_x / rhs, m_y / rhs, m_z / rhs, m_w / rhs ); }
+
+    inline Int4& operator+=( int32_t const& rhs ) { m_x += rhs; m_y += rhs; m_z += rhs; m_w += rhs; return *this; }
+    inline Int4& operator-=( int32_t const& rhs ) { m_x -= rhs; m_y -= rhs; m_z -= rhs; m_w -= rhs; return *this; }
+    inline Int4& operator*=( int32_t const& rhs ) { m_x *= rhs; m_y *= rhs; m_z *= rhs; m_w *= rhs; return *this; }
+    inline Int4& operator/=( int32_t const& rhs ) { m_x /= rhs; m_y /= rhs; m_z /= rhs; m_w /= rhs; return *this; }
+
+    // Component wise operation
+    inline Int4 operator+( Int4 const& rhs ) const { return Int4( m_x + rhs.m_x, m_y + rhs.m_y, m_z + rhs.m_z, m_w + rhs.m_w ); }
+    inline Int4 operator-( Int4 const& rhs ) const { return Int4( m_x - rhs.m_x, m_y - rhs.m_y, m_z - rhs.m_z, m_w - rhs.m_w ); }
+    inline Int4 operator*( Int4 const& rhs ) const { return Int4( m_x * rhs.m_x, m_y * rhs.m_y, m_z * rhs.m_z, m_w * rhs.m_w ); }
+    inline Int4 operator/( Int4 const& rhs ) const { return Int4( m_x / rhs.m_x, m_y / rhs.m_y, m_z / rhs.m_z, m_w / rhs.m_w ); }
+
+    inline Int4& operator+=( Int4 const& rhs ) { m_x += rhs.m_x; m_y += rhs.m_y; m_z += rhs.m_z; m_w += rhs.m_w; return *this; }
+    inline Int4& operator-=( Int4 const& rhs ) { m_x -= rhs.m_x; m_y -= rhs.m_y; m_z -= rhs.m_z; m_w -= rhs.m_w; return *this; }
+    inline Int4& operator*=( Int4 const& rhs ) { m_x *= rhs.m_x; m_y *= rhs.m_y; m_z *= rhs.m_z; m_w *= rhs.m_w; return *this; }
+    inline Int4& operator/=( Int4 const& rhs ) { m_x /= rhs.m_x; m_y /= rhs.m_y; m_z /= rhs.m_z; m_w /= rhs.m_w; return *this; }
+
+public:
+
+    int32_t m_x, m_y, m_z, m_w;
+};
+
+struct Float2
+{
+    static Float2 const Zero;
+    static Float2 const One;
+    static Float2 const UnitX;
+    static Float2 const UnitY;
+
+public:
+
+    inline Float2() {}
+    FORCE_INLINE Float2( ZeroInit_t ) : m_x( 0 ), m_y( 0 ) {}
+    FORCE_INLINE explicit Float2( float v ) : m_x( v ), m_y( v ) {}
+    FORCE_INLINE explicit Float2( float ix, float iy ) : m_x( ix ), m_y( iy ) {}
+    FORCE_INLINE explicit Float2( int32_t ix, int32_t iy ) : m_x( (float) ix ), m_y( (float) iy ) {}
+    inline explicit Float2( Int2 const& v ) : m_x( (float) v.m_x ), m_y( (float) v.m_y ) {}
+    inline explicit Float2( Float3 const& v );
+    inline explicit Float2( Float4 const& v );
+
+    inline bool IsZero() const { return *this == Zero; }
+
+    inline float& operator[]( uint32_t i ) { return ( (float*) this )[i]; }
+    inline float const& operator[]( uint32_t i ) const { return ( (float*) this )[i]; }
+
+    FORCE_INLINE Float2 operator-() const { return Float2( -m_x, -m_y ); }
+
+    inline bool operator==( Float2 const rhs ) const { return m_x == rhs.m_x && m_y == rhs.m_y; }
+    inline bool operator!=( Float2 const rhs ) const { return m_x != rhs.m_x || m_y != rhs.m_y; }
+
+    inline Float2 operator+( Float2 const& rhs ) const { return Float2( m_x + rhs.m_x, m_y + rhs.m_y ); }
+    inline Float2 operator-( Float2 const& rhs ) const { return Float2( m_x - rhs.m_x, m_y - rhs.m_y ); }
+    inline Float2 operator*( Float2 const& rhs ) const { return Float2( m_x * rhs.m_x, m_y * rhs.m_y ); }
+    inline Float2 operator/( Float2 const& rhs ) const { return Float2( m_x / rhs.m_x, m_y / rhs.m_y ); }
+
+    inline Float2 operator+( float const& rhs ) const { return Float2( m_x + rhs, m_y + rhs ); }
+    inline Float2 operator-( float const& rhs ) const { return Float2( m_x - rhs, m_y - rhs ); }
+    inline Float2 operator*( float const& rhs ) const { return Float2( m_x * rhs, m_y * rhs ); }
+    inline Float2 operator/( float const& rhs ) const { return Float2( m_x / rhs, m_y / rhs ); }
+
+    inline Float2& operator+=( Float2 const& rhs ) { m_x += rhs.m_x; m_y += rhs.m_y; return *this; }
+    inline Float2& operator-=( Float2 const& rhs ) { m_x -= rhs.m_x; m_y -= rhs.m_y; return *this; }
+    inline Float2& operator*=( Float2 const& rhs ) { m_x *= rhs.m_x; m_y *= rhs.m_y; return *this; }
+    inline Float2& operator/=( Float2 const& rhs ) { m_x /= rhs.m_x; m_y /= rhs.m_y; return *this; }
+
+    inline Float2& operator+=( float const& rhs ) { m_x += rhs; m_y += rhs; return *this; }
+    inline Float2& operator-=( float const& rhs ) { m_x -= rhs; m_y -= rhs; return *this; }
+    inline Float2& operator*=( float const& rhs ) { m_x *= rhs; m_y *= rhs; return *this; }
+    inline Float2& operator/=( float const& rhs ) { m_x /= rhs; m_y /= rhs; return *this; }
+
+    float m_x, m_y;
+};
+
+struct Float3
+{
+    static Float3 const Zero;
+    static Float3 const One;
+    static Float3 const UnitX;
+    static Float3 const UnitY;
+    static Float3 const UnitZ;
+
+    static Float3 const WorldForward;
+    static Float3 const WorldUp;
+    static Float3 const WorldRight;
+
+public:
+
+    inline Float3() {}
+    FORCE_INLINE Float3( ZeroInit_t ) : m_x( 0 ), m_y( 0 ), m_z( 0 ) {}
+    FORCE_INLINE explicit Float3( float v ) : m_x( v ), m_y( v ), m_z( v ) {}
+    FORCE_INLINE explicit Float3( float ix, float iy, float iz ) : m_x( ix ), m_y( iy ), m_z( iz ) {}
+    inline explicit Float3( Float2 const& v, float iz = 0.0f ) : m_x( v.m_x ), m_y( v.m_y ), m_z( iz ) {}
+    inline explicit Float3( Float4 const& v );
+
+    inline bool IsZero() const { return *this == Zero; }
+
+    inline float& operator[]( uint32_t i ) { return ( (float*) this )[i]; }
+    inline float const& operator[]( uint32_t i ) const { return ( (float*) this )[i]; }
+
+    FORCE_INLINE Float3 operator-() const { return Float3( -m_x, -m_y, -m_z ); }
+
+    inline bool operator==( Float3 const rhs ) const { return m_x == rhs.m_x && m_y == rhs.m_y && m_z == rhs.m_z; }
+    inline bool operator!=( Float3 const rhs ) const { return m_x != rhs.m_x || m_y != rhs.m_y || m_z != rhs.m_z; }
+
+    inline operator Float2() const { return Float2( m_x, m_y ); }
+
+    inline Float3 operator+( Float3 const& rhs ) const { return Float3( m_x + rhs.m_x, m_y + rhs.m_y, m_z + rhs.m_z ); }
+    inline Float3 operator-( Float3 const& rhs ) const { return Float3( m_x - rhs.m_x, m_y - rhs.m_y, m_z - rhs.m_z ); }
+    inline Float3 operator*( Float3 const& rhs ) const { return Float3( m_x * rhs.m_x, m_y * rhs.m_y, m_z * rhs.m_z ); }
+    inline Float3 operator/( Float3 const& rhs ) const { return Float3( m_x / rhs.m_x, m_y / rhs.m_y, m_z / rhs.m_z ); }
+
+    inline Float3 operator+( float const& rhs ) const { return Float3( m_x + rhs, m_y + rhs, m_z + rhs ); }
+    inline Float3 operator-( float const& rhs ) const { return Float3( m_x - rhs, m_y - rhs, m_z - rhs ); }
+    inline Float3 operator*( float const& rhs ) const { return Float3( m_x * rhs, m_y * rhs, m_z * rhs ); }
+    inline Float3 operator/( float const& rhs ) const { return Float3( m_x / rhs, m_y / rhs, m_z / rhs ); }
+
+    inline Float3& operator+=( Float3 const& rhs ) { m_x += rhs.m_x; m_y += rhs.m_y; m_z += rhs.m_z; return *this; }
+    inline Float3& operator-=( Float3 const& rhs ) { m_x -= rhs.m_x; m_y -= rhs.m_y; m_z -= rhs.m_z; return *this; }
+    inline Float3& operator*=( Float3 const& rhs ) { m_x *= rhs.m_x; m_y *= rhs.m_y; m_z *= rhs.m_z; return *this; }
+    inline Float3& operator/=( Float3 const& rhs ) { m_x /= rhs.m_x; m_y /= rhs.m_y; m_z /= rhs.m_z; return *this; }
+
+    inline Float3& operator+=( float const& rhs ) { m_x += rhs; m_y += rhs; m_z += rhs; return *this; }
+    inline Float3& operator-=( float const& rhs ) { m_x -= rhs; m_y -= rhs; m_z -= rhs; return *this; }
+    inline Float3& operator*=( float const& rhs ) { m_x *= rhs; m_y *= rhs; m_z *= rhs; return *this; }
+    inline Float3& operator/=( float const& rhs ) { m_x /= rhs; m_y /= rhs; m_z /= rhs; return *this; }
+
+    float m_x, m_y, m_z;
+};
+
+struct Float4
+{
+    static Float4 const Zero;
+    static Float4 const One;
+    static Float4 const UnitX;
+    static Float4 const UnitY;
+    static Float4 const UnitZ;
+    static Float4 const UnitW;
+
+    static Float4 const WorldForward;
+    static Float4 const WorldUp;
+    static Float4 const WorldRight;
+
+public:
+
+    Float4() {}
+    FORCE_INLINE Float4( ZeroInit_t ) : m_x( 0 ), m_y( 0 ), m_z( 0 ), m_w( 0 ) {}
+    FORCE_INLINE explicit Float4( float v ) : m_x( v ), m_y( v ), m_z( v ), m_w( v ) {}
+    FORCE_INLINE explicit Float4( float ix, float iy, float iz, float iw ) : m_x( ix ), m_y( iy ), m_z( iz ), m_w( iw ) {}
+    explicit Float4( Float2 const& v, float iz = 0.0f, float iw = 0.0f ) : m_x( v.m_x ), m_y( v.m_y ), m_z( iz ), m_w( iw ) {}
+    explicit Float4( Float3 const& v, float iw = 0.0f ) : m_x( v.m_x ), m_y( v.m_y ), m_z( v.m_z ), m_w( iw ) {}
+
+    inline bool IsZero() const { return *this == Zero; }
+
+    float& operator[]( uint32_t i ) { return ( (float*) this )[i]; }
+    float const& operator[]( uint32_t i ) const { return ( (float*) this )[i]; }
+
+    FORCE_INLINE Float4 operator-() const { return Float4( -m_x, -m_y, -m_z, -m_w ); }
+
+    bool operator==( Float4 const rhs ) const { return m_x == rhs.m_x && m_y == rhs.m_y && m_z == rhs.m_z && m_w == rhs.m_w; }
+    bool operator!=( Float4 const rhs ) const { return m_x != rhs.m_x || m_y != rhs.m_y || m_z != rhs.m_z || m_w != rhs.m_w; }
+
+    inline operator Float2() const { return Float2( m_x, m_y ); }
+    inline operator Float3() const { return Float3( m_x, m_y, m_z ); }
+
+    inline Float4 operator+( Float4 const& rhs ) const { return Float4( m_x + rhs.m_x, m_y + rhs.m_y, m_z + rhs.m_z, m_w + rhs.m_w ); }
+    inline Float4 operator-( Float4 const& rhs ) const { return Float4( m_x - rhs.m_x, m_y - rhs.m_y, m_z - rhs.m_z, m_w - rhs.m_w ); }
+    inline Float4 operator*( Float4 const& rhs ) const { return Float4( m_x * rhs.m_x, m_y * rhs.m_y, m_z * rhs.m_z, m_w * rhs.m_w ); }
+    inline Float4 operator/( Float4 const& rhs ) const { return Float4( m_x / rhs.m_x, m_y / rhs.m_y, m_z / rhs.m_z, m_w / rhs.m_w ); }
+
+    inline Float4 operator+( float const& rhs ) const { return Float4( m_x + rhs, m_y + rhs, m_z + rhs, m_w + rhs ); }
+    inline Float4 operator-( float const& rhs ) const { return Float4( m_x - rhs, m_y - rhs, m_z - rhs, m_w - rhs ); }
+    inline Float4 operator*( float const& rhs ) const { return Float4( m_x * rhs, m_y * rhs, m_z * rhs, m_w * rhs ); }
+    inline Float4 operator/( float const& rhs ) const { return Float4( m_x / rhs, m_y / rhs, m_z / rhs, m_w / rhs ); }
+
+    inline Float4& operator+=( Float4 const& rhs ) { m_x += rhs.m_x; m_y += rhs.m_y; m_z += rhs.m_z; m_w += rhs.m_w; return *this; }
+    inline Float4& operator-=( Float4 const& rhs ) { m_x -= rhs.m_x; m_y -= rhs.m_y; m_z -= rhs.m_z; m_w -= rhs.m_w; return *this; }
+    inline Float4& operator*=( Float4 const& rhs ) { m_x *= rhs.m_x; m_y *= rhs.m_y; m_z *= rhs.m_z; m_w *= rhs.m_w; return *this; }
+    inline Float4& operator/=( Float4 const& rhs ) { m_x /= rhs.m_x; m_y /= rhs.m_y; m_z /= rhs.m_z; m_w /= rhs.m_w; return *this; }
+
+    inline Float4& operator+=( float const& rhs ) { m_x += rhs; m_y += rhs; m_z += rhs; m_w += rhs; return *this; }
+    inline Float4& operator-=( float const& rhs ) { m_x -= rhs; m_y -= rhs; m_z -= rhs; m_w -= rhs; return *this; }
+    inline Float4& operator*=( float const& rhs ) { m_x *= rhs; m_y *= rhs; m_z *= rhs; m_w *= rhs; return *this; }
+    inline Float4& operator/=( float const& rhs ) { m_x /= rhs; m_y /= rhs; m_z /= rhs; m_w /= rhs; return *this; }
+
+    float m_x, m_y, m_z, m_w;
+};
+
+inline Int2::Int2( Float2 const& v )
+    : m_x( (int32_t) v.m_x )
+    , m_y( (int32_t) v.m_y )
+{
+}
+
+inline Int3::Int3( Float3 const& v )
+    : m_x( (int32_t) v.m_x )
+    , m_y( (int32_t) v.m_y )
+    , m_z( (int32_t) v.m_z )
+{
+}
+
+inline Float2::Float2( Float3 const& v )
+    : m_x( v.m_x )
+    , m_y( v.m_y )
+{
+}
+
+inline Float2::Float2( Float4 const& v )
+    : m_x( v.m_x )
+    , m_y( v.m_y )
+{
+}
+
+inline Float3::Float3( Float4 const& v )
+    : m_x( v.m_x )
+    , m_y( v.m_y )
+    , m_z( v.m_z )
+{
+}
+
+struct Radians;
+struct Degrees;
+
+struct Degrees
+{
+public:
+
+    inline Degrees() = default;
+    inline Degrees( float degrees ) : m_value( degrees ) {}
+    inline explicit Degrees( Radians const& radians );
+
+    FORCE_INLINE explicit operator float() const { return m_value; }
+    FORCE_INLINE operator Radians() const;
+    FORCE_INLINE float ToFloat() const { return m_value; }
+    FORCE_INLINE Radians ToRadians() const;
+
+    inline Degrees operator-() const { return Degrees( -m_value ); }
+
+    inline Degrees operator+( Degrees const& rhs ) const { return Degrees( m_value + rhs.m_value ); }
+    inline Degrees operator-( Degrees const& rhs ) const { return Degrees( m_value - rhs.m_value ); }
+    inline Degrees operator*( Degrees const& rhs ) const { return Degrees( m_value * rhs.m_value ); }
+    inline Degrees operator/( Degrees const& rhs ) const { return Degrees( m_value / rhs.m_value ); }
+
+    inline Degrees& operator+=( Degrees const& rhs ) { m_value += rhs.m_value; return *this; }
+    inline Degrees& operator-=( Degrees const& rhs ) { m_value -= rhs.m_value; return *this; }
+    inline Degrees& operator*=( Degrees const& rhs ) { m_value *= rhs.m_value; return *this; }
+    inline Degrees& operator/=( Degrees const& rhs ) { m_value /= rhs.m_value; return *this; }
+
+    inline Degrees operator+( float const& rhs ) const { return Degrees( m_value + rhs ); }
+    inline Degrees operator-( float const& rhs ) const { return Degrees( m_value - rhs ); }
+    inline Degrees operator*( float const& rhs ) const { return Degrees( m_value * rhs ); }
+    inline Degrees operator/( float const& rhs ) const { return Degrees( m_value / rhs ); }
+
+    inline Degrees& operator+=( float const& rhs ) { m_value += rhs; return *this; }
+    inline Degrees& operator-=( float const& rhs ) { m_value -= rhs; return *this; }
+    inline Degrees& operator*=( float const& rhs ) { m_value *= rhs; return *this; }
+    inline Degrees& operator/=( float const& rhs ) { m_value /= rhs; return *this; }
+
+    inline Degrees operator+( int32_t const& rhs ) const { return Degrees( m_value + rhs ); }
+    inline Degrees operator-( int32_t const& rhs ) const { return Degrees( m_value - rhs ); }
+    inline Degrees operator*( int32_t const& rhs ) const { return Degrees( m_value * rhs ); }
+    inline Degrees operator/( int32_t const& rhs ) const { return Degrees( m_value / rhs ); }
+
+    inline Degrees& operator+=( int32_t const& rhs ) { m_value += rhs; return *this; }
+    inline Degrees& operator-=( int32_t const& rhs ) { m_value -= rhs; return *this; }
+    inline Degrees& operator*=( int32_t const& rhs ) { m_value *= rhs; return *this; }
+    inline Degrees& operator/=( int32_t const& rhs ) { m_value /= rhs; return *this; }
+
+    inline Degrees operator+( uint32_t const& rhs ) const { return Degrees( m_value + rhs ); }
+    inline Degrees operator-( uint32_t const& rhs ) const { return Degrees( m_value - rhs ); }
+    inline Degrees operator*( uint32_t const& rhs ) const { return Degrees( m_value * rhs ); }
+    inline Degrees operator/( uint32_t const& rhs ) const { return Degrees( m_value / rhs ); }
+
+    inline Degrees& operator+=( uint32_t const& rhs ) { m_value += rhs; return *this; }
+    inline Degrees& operator-=( uint32_t const& rhs ) { m_value -= rhs; return *this; }
+    inline Degrees& operator*=( uint32_t const& rhs ) { m_value *= rhs; return *this; }
+    inline Degrees& operator/=( uint32_t const& rhs ) { m_value /= rhs; return *this; }
+
+    inline bool operator>( float const& rhs ) const { return m_value > rhs; };
+    inline bool operator<( float const& rhs ) const { return m_value < rhs; }
+    inline bool operator>=( float const& rhs ) const { return m_value >= rhs; }
+    inline bool operator<=( float const& rhs ) const { return m_value <= rhs; }
+
+    inline bool operator>( Degrees const& rhs ) const { return m_value > rhs.m_value; }
+    inline bool operator<( Degrees const& rhs ) const { return m_value < rhs.m_value; }
+    inline bool operator>=( Degrees const& rhs ) const { return m_value >= rhs.m_value; }
+    inline bool operator<=( Degrees const& rhs ) const { return m_value <= rhs.m_value; }
+
+    inline bool operator>( Radians const& rhs ) const;
+    inline bool operator<( Radians const& rhs ) const;
+    inline bool operator>=( Radians const& rhs ) const;
+    inline bool operator<=( Radians const& rhs ) const;
+
+    inline bool operator==( float const& v ) const { return Math::IsNearEqual( m_value, v ); }
+    inline bool operator!=( float const& v ) const { return !Math::IsNearEqual( m_value, v ); }
+
+    inline bool operator==( Degrees const& rhs ) const  { return m_value == rhs.m_value; }
+    inline bool operator!=( Degrees const& rhs ) const  { return m_value != rhs.m_value; }
+
+    inline bool operator==( Radians const& rhs ) const;
+    inline bool operator!=( Radians const& rhs ) const;
+
+    inline void Clamp( Degrees min, Degrees max )
+    {
+        m_value = Math::Clamp( m_value, min.m_value, max.m_value );
+    }
+
+    // Clamps between -360 and 360
+    inline void Clamp360()
+    {
+        m_value -= ( int32_t( m_value / 360.0f ) * 360.0f );
+    }
+
+    // Clamps between -360 and 360
+    inline Degrees GetClamped360() const
+    {
+        Degrees d( m_value );
+        d.Clamp360();
+        return d;
+    }
+
+    // Clamps to -180 to 180
+    inline void Clamp180()
+    {
+        Clamp360();
+
+        float delta = 180 - Math::Abs( m_value );
+        if ( delta < 0 )
+        {
+            delta += 180;
+            m_value = ( m_value < 0 ) ? delta : -delta;
+        }
+    }
+
+    // Clamps to -180 to 180
+    inline Degrees GetClamped180() const
+    {
+        Degrees r( m_value );
+        r.Clamp180();
+        return r;
+    }
+
+    // Clamps between 0 to 360
+    inline Degrees& ClampPositive360()
+    {
+        Clamp360();
+        if ( m_value < 0 )
+        {
+            m_value += 360;
+        }
+        return *this;
+    }
+
+    // Clamps between 0 to 360
+    inline Degrees GetClampedPositive360() const
+    {
+        Degrees d( m_value );
+        d.ClampPositive360();
+        return d;
+    }
+
+private:
+
+    float m_value = 0;
+};
+
+struct Radians
+{
+    static Radians const Pi;
+    static Radians const TwoPi;
+    static Radians const OneDivPi;
+    static Radians const OneDivTwoPi;
+    static Radians const PiDivTwo;
+    static Radians const PiDivFour;
+
+public:
+
+    inline Radians() = default;
+    inline Radians( float radians ) : m_value( radians ) {}
+    inline explicit Radians( Degrees const& degrees );
+
+    FORCE_INLINE explicit operator float() const { return m_value; }
+    FORCE_INLINE operator Degrees() const { return ToDegrees(); }
+    FORCE_INLINE float ToFloat() const { return m_value; }
+    FORCE_INLINE Degrees ToDegrees() const { return Degrees( m_value * Math::RadiansToDegrees ); }
+
+    inline Radians operator-() const { return Radians( -m_value ); }
+
+    inline Radians operator+( Radians const& rhs ) const { return Radians( m_value + rhs.m_value ); }
+    inline Radians operator-( Radians const& rhs ) const { return Radians( m_value - rhs.m_value ); }
+    inline Radians operator*( Radians const& rhs ) const { return Radians( m_value * rhs.m_value ); }
+    inline Radians operator/( Radians const& rhs ) const { return Radians( m_value / rhs.m_value ); }
+
+    inline Radians& operator+=( Radians const& rhs ) { m_value += rhs.m_value; return *this; }
+    inline Radians& operator-=( Radians const& rhs ) { m_value -= rhs.m_value; return *this; }
+    inline Radians& operator*=( Radians const& rhs ) { m_value *= rhs.m_value; return *this; }
+    inline Radians& operator/=( Radians const& rhs ) { m_value /= rhs.m_value; return *this; }
+
+    inline Radians operator+( float const& rhs ) const { return Radians( m_value + rhs ); }
+    inline Radians operator-( float const& rhs ) const { return Radians( m_value - rhs ); }
+    inline Radians operator*( float const& rhs ) const { return Radians( m_value * rhs ); }
+    inline Radians operator/( float const& rhs ) const { return Radians( m_value / rhs ); }
+
+    inline Radians& operator+=( float const& rhs ) { m_value += rhs; return *this; }
+    inline Radians& operator-=( float const& rhs ) { m_value -= rhs; return *this; }
+    inline Radians& operator*=( float const& rhs ) { m_value *= rhs; return *this; }
+    inline Radians& operator/=( float const& rhs ) { m_value /= rhs; return *this; }
+
+    inline Radians operator+( int32_t const& rhs ) const { return Radians( m_value + rhs ); }
+    inline Radians operator-( int32_t const& rhs ) const { return Radians( m_value - rhs ); }
+    inline Radians operator*( int32_t const& rhs ) const { return Radians( m_value * rhs ); }
+    inline Radians operator/( int32_t const& rhs ) const { return Radians( m_value / rhs ); }
+
+    inline Radians& operator+=( int32_t const& rhs ) { m_value += rhs; return *this; }
+    inline Radians& operator-=( int32_t const& rhs ) { m_value -= rhs; return *this; }
+    inline Radians& operator*=( int32_t const& rhs ) { m_value *= rhs; return *this; }
+    inline Radians& operator/=( int32_t const& rhs ) { m_value /= rhs; return *this; }
+
+    inline Radians operator+( uint32_t const& rhs ) const { return Radians( m_value + rhs ); }
+    inline Radians operator-( uint32_t const& rhs ) const { return Radians( m_value - rhs ); }
+    inline Radians operator*( uint32_t const& rhs ) const { return Radians( m_value * rhs ); }
+    inline Radians operator/( uint32_t const& rhs ) const { return Radians( m_value / rhs ); }
+
+    inline Radians& operator+=( uint32_t const& rhs ) { m_value += rhs; return *this; }
+    inline Radians& operator-=( uint32_t const& rhs ) { m_value -= rhs; return *this; }
+    inline Radians& operator*=( uint32_t const& rhs ) { m_value *= rhs; return *this; }
+    inline Radians& operator/=( uint32_t const& rhs ) { m_value /= rhs; return *this; }
+
+    inline bool operator>( float const& rhs ) const { return m_value > rhs; };
+    inline bool operator<( float const& rhs ) const { return m_value < rhs; }
+    inline bool operator>=( float const& rhs ) const { return m_value >= rhs; }
+    inline bool operator<=( float const& rhs ) const { return m_value <= rhs; }
+
+    inline bool operator>( Radians const& rhs ) const { return m_value > rhs.m_value; }
+    inline bool operator<( Radians const& rhs ) const { return m_value < rhs.m_value; }
+    inline bool operator>=( Radians const& rhs ) const { return m_value >= rhs.m_value; }
+    inline bool operator<=( Radians const& rhs ) const { return m_value <= rhs.m_value; }
+
+    inline bool operator>( Degrees const& rhs ) const;
+    inline bool operator<( Degrees const& rhs ) const;
+    inline bool operator>=( Degrees const& rhs ) const;
+    inline bool operator<=( Degrees const& rhs ) const;
+
+    inline bool operator==( float const& v ) const { return Math::IsNearEqual( m_value, v ); }
+    inline bool operator!=( float const& v ) const { return !Math::IsNearEqual( m_value, v ); }
+
+    inline bool operator==( Radians const& rhs ) const { return m_value == rhs.m_value; }
+    inline bool operator!=( Radians const& rhs ) const { return m_value != rhs.m_value; }
+
+    inline bool operator==( Degrees const& rhs ) const;
+    inline bool operator!=( Degrees const& rhs ) const;
+
+    inline void Clamp( Radians min, Radians max )
+    {
+        m_value = Math::Clamp( m_value, min.m_value, max.m_value );
+    }
+
+    // Clamps between -2Pi to 2Pi
+    inline void Clamp360()
+    {
+        m_value -= int32_t( m_value / Math::TwoPi ) * Math::TwoPi;
+    }
+
+    // Clamps between -2Pi to 2Pi
+    inline Radians GetClamped360() const
+    {
+        Radians r( m_value );
+        r.Clamp360();
+        return r;
+    }
+
+    // Clamps between 0 to 2Pi
+    inline void ClampPositive360()
+    {
+        Clamp360();
+        if( m_value < 0 )
+        {
+            m_value += Math::TwoPi;
+        }
+    }
+
+    // Clamps between 0 to 2Pi
+    inline Radians GetClampedToPositive360() const
+    {
+        Radians r( m_value );
+        r.ClampPositive360();
+        return r;
+    }
+
+    // Clamps to -Pi to Pi
+    inline void Clamp180()
+    {
+        Clamp360();
+
+        float delta = Math::Pi - Math::Abs( m_value );
+        if ( delta < 0 )
+        {
+            delta += Math::Pi;
+            m_value = ( m_value < 0 ) ? delta : -delta;
+        }
+    }
+
+    // Clamps to -Pi to Pi
+    inline Radians GetClamped180() const
+    {
+        Radians r( m_value );
+        r.Clamp180();
+        return r;
+    }
+
+    // Inverts angle between [0;2Pi] and [-2Pi;0]
+    inline void Invert()
+    {
+        Clamp360();
+        float const delta = Math::TwoPi - Math::Abs( m_value );
+        m_value = ( m_value < 0 ) ? delta : -delta;
+    }
+
+    // Inverts angle between [0;2Pi] and [-2Pi;0]
+    inline Radians GetInverse() const
+    {
+        Radians r( m_value );
+        r.Invert();
+        return r;
+    }
+
+    // Flips the front and rear 180 degree arc i.e. 135 becomes -45, -90 becomes 90, etc.
+    inline void Flip()
+    {
+        Clamp180();
+        float const delta = Math::Pi - Math::Abs( m_value );
+        m_value = ( m_value < 0 ) ? delta : -delta;
+    }
+
+    // Flips the front and rear 180 degree arc i.e. 135 becomes -45, -90 becomes 90, etc.
+    inline Radians GetFlipped() const
+    {
+        Radians r( m_value );
+        r.Flip();
+        return r;
+    }
+
+private:
+
+    float m_value = 0;
+};
+
+inline Degrees::Degrees( Radians const& radians )
+    : m_value( radians.ToDegrees() )
+{}
+
+inline Radians Degrees::ToRadians() const
+{
+    return Radians( m_value * Math::DegreesToRadians );
+}
+
+inline Degrees::operator Radians() const
+{
+    return ToRadians();
+}
+
+inline bool Degrees::operator>( Radians const& rhs ) const { return m_value > rhs.ToDegrees().m_value; }
+inline bool Degrees::operator<( Radians const& rhs ) const { return m_value < rhs.ToDegrees().m_value; }
+inline bool Degrees::operator>=( Radians const& rhs ) const { return m_value >= rhs.ToDegrees().m_value; }
+inline bool Degrees::operator<=( Radians const& rhs ) const { return m_value <= rhs.ToDegrees().m_value; }
+
+inline bool Degrees::operator==( Radians const& rhs ) const { return Math::IsNearEqual( m_value, rhs.ToDegrees().m_value ); }
+inline bool Degrees::operator!=( Radians const& rhs ) const { return !Math::IsNearEqual( m_value, rhs.ToDegrees().m_value ); }
+
+inline Radians::Radians( Degrees const& degrees )
+    : m_value( degrees.ToRadians() )
+{}
+
+inline bool Radians::operator>( Degrees const& rhs ) const { return m_value > rhs.ToRadians().m_value; }
+inline bool Radians::operator<( Degrees const& rhs ) const { return m_value < rhs.ToRadians().m_value; }
+inline bool Radians::operator>=( Degrees const& rhs ) const { return m_value >= rhs.ToRadians().m_value; }
+inline bool Radians::operator<=( Degrees const& rhs ) const { return m_value <= rhs.ToRadians().m_value; }
+
+inline bool Radians::operator==( Degrees const& rhs ) const { return Math::IsNearEqual( m_value, rhs.ToRadians().m_value ); }
+inline bool Radians::operator!=( Degrees const& rhs ) const { return !Math::IsNearEqual( m_value, rhs.ToRadians().m_value ); }
+
+struct EulerAngles
+{
+public:
+
+    EulerAngles() = default;
+
+    inline explicit EulerAngles( Degrees inX, Degrees inY, Degrees inZ )
+        : m_x( inX )
+        , m_y( inY )
+        , m_z( inZ )
+    {}
+
+    inline explicit EulerAngles( Radians inX, Radians inY, Radians inZ )
+        : m_x( inX )
+        , m_y( inY )
+        , m_z( inZ )
+    {}
+
+    inline explicit EulerAngles( float inDegreesX, float inDegreesY, float inDegreesZ )
+        : m_x( Math::DegreesToRadians * inDegreesX )
+        , m_y( Math::DegreesToRadians * inDegreesY )
+        , m_z( Math::DegreesToRadians * inDegreesZ )
+    {}
+
+    inline EulerAngles( Float3 const& anglesInDegrees )
+        : m_x( Math::DegreesToRadians * anglesInDegrees.m_x )
+        , m_y( Math::DegreesToRadians * anglesInDegrees.m_y )
+        , m_z( Math::DegreesToRadians * anglesInDegrees.m_z )
+    {}
+
+    inline void Clamp()
+    {
+        m_x.Clamp360();
+        m_y.Clamp360();
+        m_z.Clamp360();
+    }
+
+    inline EulerAngles GetClamped() const
+    {
+        EulerAngles clamped = *this;
+        clamped.Clamp();
+        return clamped;
+    }
+
+    inline Radians GetYaw() const { return m_z; }
+    inline Radians GetPitch() const { return m_x; }
+    inline Radians GetRoll() const { return m_y; }
+
+    inline Float3 GetAsRadians() const { return Float3( m_x.ToFloat(), m_y.ToFloat(), m_z.ToFloat() ); }
+    inline Float3 GetAsDegrees() const { return Float3( m_x.ToDegrees().ToFloat(), m_y.ToDegrees().ToFloat(), m_z.ToDegrees().ToFloat() ); }
+
+    inline bool operator==( EulerAngles const& other ) const { return m_x == other.m_x && m_y == other.m_y && m_z == other.m_z; }
+    inline bool operator!=( EulerAngles const& other ) const { return m_x != other.m_x || m_y != other.m_y || m_z != other.m_z; }
+
+    inline Radians& operator[]( uint32_t i ) { return ( (Radians*) this )[i]; }
+    inline Radians const& operator[]( uint32_t i ) const { return ( (Radians*) this )[i]; }
+    // in degrees
+    inline Float3 ToFloat3() const { return Float3( Math::RadiansToDegrees * m_x.ToFloat(), Math::RadiansToDegrees * m_y.ToFloat(), Math::RadiansToDegrees * m_z.ToFloat() ); }
+
+public:
+
+    Radians m_x = 0.0f;
+    Radians m_y = 0.0f;
+    Radians m_z = 0.0f;
+};
+
+struct AxisAngle
+{
+public:
+
+    inline AxisAngle() = default;
+    inline explicit AxisAngle( Float3 axis, Radians angle ) : m_axis( axis ), m_angle( angle ) {}
+    inline explicit AxisAngle( Float3 axis, Degrees angle ) : m_axis( axis ), m_angle( angle.ToRadians() ) {}
+
+    inline bool IsValid() const
+    {
+        float const lengthSq = m_axis.m_x * m_axis.m_x + m_axis.m_y * m_axis.m_y + m_axis.m_z * m_axis.m_z;
+        return Math::Abs( lengthSq - 1.0f ) < Math::Epsilon;
+    }
+
+public:
+
+    Float3      m_axis = Float3::Zero;
+    Radians     m_angle = Radians( 0.0f );
+};

MotionCorrection/src/cpp/Math/Vector.cpp

@@ -0,0 +1,110 @@
+/*
+ * SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
+ * SPDX-License-Identifier: Apache-2.0
+ */
+
+#include "Vector.h"
+#include "Quaternion.h"
+
+namespace Math
+{
+    Vector const Vector::UnitX = { 1, 0, 0, 0 };
+    Vector const Vector::UnitY = { 0, 1, 0, 0 };
+    Vector const Vector::UnitZ = { 0, 0, 1, 0 };
+    Vector const Vector::UnitW = { 0, 0, 0, 1 };
+
+    Vector const Vector::Origin = { 0, 0, 0, 1 };
+    Vector const Vector::WorldForward = { 0, -1, 0, 0 };
+    Vector const Vector::WorldBackward = { 0, 1, 0, 0 };
+    Vector const Vector::WorldUp = { 0, 0, 1, 0 };
+    Vector const Vector::WorldDown = { 0, 0, -1, 0 };
+    Vector const Vector::WorldLeft = { 1, 0, 0, 0 };
+    Vector const Vector::WorldRight = { -1, 0, 0, 0 };
+
+    Vector const Vector::Infinity = { 0x7F800000, 0x7F800000, 0x7F800000, 0x7F800000 };
+    Vector const Vector::QNaN = { 0x7FC00000, 0x7FC00000, 0x7FC00000, 0x7FC00000 };
+
+    Vector const Vector::NegativeOne(-1.0f);
+    Vector const Vector::Zero(0.0f);
+    Vector const Vector::Half(0.5f);
+    Vector const Vector::One(1.0f);
+
+    Vector const Vector::Epsilon(Math::Epsilon);
+    Vector const Vector::LargeEpsilon(Math::LargeEpsilon);
+    Vector const Vector::OneMinusEpsilon(1.0f - Math::Epsilon);
+    Vector const Vector::EpsilonMinusOne(Math::Epsilon - 1.0f);
+    Vector const Vector::NormalizeCheckThreshold(0.01f); // Squared Error
+
+    Vector const Vector::Pi(Math::Pi);
+    Vector const Vector::PiDivTwo(Math::PiDivTwo);
+    Vector const Vector::TwoPi(Math::TwoPi);
+    Vector const Vector::OneDivTwoPi(Math::OneDivTwoPi);
+
+    Vector const Vector::Select0000(0, 0, 0, 0);
+    Vector const Vector::Select0001(0, 0, 0, 1);
+    Vector const Vector::Select0010(0, 0, 1, 0);
+    Vector const Vector::Select0011(0, 0, 1, 1);
+    Vector const Vector::Select0100(0, 1, 0, 0);
+    Vector const Vector::Select0101(0, 1, 0, 1);
+    Vector const Vector::Select0110(0, 1, 1, 0);
+    Vector const Vector::Select0111(0, 1, 1, 1);
+    Vector const Vector::Select1000(1, 0, 0, 0);
+    Vector const Vector::Select1001(1, 0, 0, 1);
+    Vector const Vector::Select1010(1, 0, 1, 0);
+    Vector const Vector::Select1011(1, 0, 1, 1);
+    Vector const Vector::Select1100(1, 1, 0, 0);
+    Vector const Vector::Select1101(1, 1, 0, 1);
+    Vector const Vector::Select1110(1, 1, 1, 0);
+    Vector const Vector::Select1111(1, 1, 1, 1);
+
+    Vector const Vector::BoxCorners[8] =
+    {
+        { -1.0f, -1.0f,  1.0f, 0.0f },
+        {  1.0f, -1.0f,  1.0f, 0.0f },
+        {  1.0f,  1.0f,  1.0f, 0.0f },
+        { -1.0f,  1.0f,  1.0f, 0.0f },
+        { -1.0f, -1.0f, -1.0f, 0.0f },
+        {  1.0f, -1.0f, -1.0f, 0.0f },
+        {  1.0f,  1.0f, -1.0f, 0.0f },
+        { -1.0f,  1.0f, -1.0f, 0.0f },
+    };
+
+    Vector Vector::SLerp(const Vector& from, const Vector& to, float t)
+    {
+        ASSERT(t >= 0.0f && t <= 1.0f);
+        if (from.LengthSquared3().IsLessThan4(Epsilon) || to.LengthSquared3().IsLessThan4(Epsilon))
+        {
+            return Lerp(from, to, t);
+        }
+
+        // Calculate the final length
+        const Vector fromLength = from.Length3();
+        const Vector toLength = to.Length3();
+        const Vector finalLength = Lerp(fromLength, toLength, t);
+
+        // Normalize vectors
+        const Vector normalizedFrom = from / fromLength;
+        const Vector normalizedTo = to / toLength;
+
+        // Handle parallel vector
+        Vector result;
+        if (normalizedFrom.IsParallelTo(normalizedTo))
+        {
+            result = normalizedFrom;
+        }
+        else
+        {
+            // Interpolate the rotation between the vectors
+            const Vector dot = Dot3(normalizedFrom, normalizedTo);
+            const Vector angle = ACos(dot);
+            const Vector axis = Cross3(normalizedFrom, normalizedTo).Normalize3();
+            const Vector interpolatedAngle = Lerp(Zero, angle, t);
+
+            const Quaternion rotation(axis, Radians(interpolatedAngle.ToFloat()));
+            const Vector finalDirection = rotation.RotateVector(normalizedFrom);
+            result = finalDirection.GetNormalized3() * finalLength;
+        }
+
+        return result;
+    }
+}

MotionCorrection/src/cpp/Math/Vector.h

@@ -0,0 +1,454 @@
+/*
+ * SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
+ * SPDX-License-Identifier: Apache-2.0
+ */
+
+#pragma once
+
+#include "Compiler.h"
+
+#include "Types.h"
+#include "Constants.h"
+#include "SIMD.h"
+
+namespace Math
+{
+    class alignas(16) Vector
+    {
+    public:
+
+        static Vector const UnitX;
+        static Vector const UnitY;
+        static Vector const UnitZ;
+        static Vector const UnitW;
+
+        static Vector const Origin;
+        static Vector const WorldForward;
+        static Vector const WorldBackward;
+        static Vector const WorldUp;
+        static Vector const WorldDown;
+        static Vector const WorldLeft;
+        static Vector const WorldRight;
+
+        static Vector const NegativeOne;
+        static Vector const Zero;
+        static Vector const Half;
+        static Vector const One;
+        static Vector const Epsilon;
+        static Vector const LargeEpsilon;
+        static Vector const OneMinusEpsilon;
+        static Vector const EpsilonMinusOne;
+        static Vector const NormalizeCheckThreshold;
+        static Vector const Pi;
+        static Vector const PiDivTwo;
+        static Vector const TwoPi;
+        static Vector const OneDivTwoPi;
+
+        static Vector const Select0000;
+        static Vector const Select0001;
+        static Vector const Select0010;
+        static Vector const Select0011;
+        static Vector const Select0100;
+        static Vector const Select0101;
+        static Vector const Select0110;
+        static Vector const Select0111;
+        static Vector const Select1000;
+        static Vector const Select1001;
+        static Vector const Select1010;
+        static Vector const Select1011;
+        static Vector const Select1100;
+        static Vector const Select1101;
+        static Vector const Select1110;
+        static Vector const Select1111;
+
+        static Vector const Infinity;
+        static Vector const QNaN;
+
+        static Vector const BoxCorners[8];
+
+        //
+        // Utils
+        //
+
+        static Vector Cross2(const Vector& v0, const Vector& v1);
+        static Vector Cross3(const Vector& v0, const Vector& v1);
+        static Vector Dot2(const Vector& v0, const Vector& v1);
+        static Vector Dot3(const Vector& v0, const Vector& v1);
+        static Vector Dot4(const Vector& v0, const Vector& v1);
+        static Vector Average2(const Vector& v0, const Vector& v1);
+        static Vector Average3(const Vector& v0, const Vector& v1);
+        static Vector Average4(const Vector& v0, const Vector& v1);
+        static Vector Min(const Vector& v0, const Vector& v1);
+        static Vector Max(const Vector& v0, const Vector& v1);
+        static float Min(const Vector& v);
+        static float Max(const Vector& v);
+        static Vector Clamp(const Vector& v, const Vector& min, const Vector& max);
+        static Vector Xor(const Vector& vec0, const Vector& vec1);
+
+        // Add the multiplied results to a vector: ( vec * mul ) + addend
+        static Vector MultiplyAdd(const Vector& vec, const Vector& multiplier, const Vector& addend);
+
+        // Subtract a vector from the multiplied result: (vec * mul ) - subtrahend
+        static Vector MultiplySubtract(const Vector& vec, const Vector& multiplier, const Vector& subtrahend);
+
+        // Subtract the multiplied result from a vector: minuend - (vec * mul )
+        static Vector NegativeMultiplySubtract(const Vector& vec, const Vector& multiplier, const Vector& minuend);
+
+        // Sum up scaled versions of two vectors
+        static Vector LinearCombination(const Vector& v0, const Vector& v1, float scale0, float scale1);
+
+        // Linear interpolation of one vector to another
+        static Vector Lerp(const Vector& from, const Vector& to, float t);
+
+        // Normalized linear interpolation of one vector to another
+        static Vector NLerp(const Vector& from, const Vector& to, float t);
+
+        // Spherical interpolation of one vector to another
+        static Vector SLerp(const Vector& from, const Vector& to, float t);
+
+        // Combine the two vectors based on the control: 0 means select from v0, 1 means select from v1. E.G. To select XY from v0 and ZW from v1, control = Vector( 0, 0, 1, 1 )
+        static Vector Select(const Vector& v0, const Vector& v1, const Vector& control);
+
+        // Get a permutation of two vectors, each template argument represents the element index to select ( v0: 0-3, v1: 4-7 );
+        template<uint32_t PermuteX, uint32_t PermuteY, uint32_t PermuteZ, uint32_t PermuteW>
+        static Vector Permute(const Vector& v0, const Vector& v1);
+
+        //
+        // Trigonometry
+        //
+
+        static Vector Sin(const Vector& vec);
+        static Vector Cos(const Vector& vec);
+        static Vector Tan(const Vector& vec);
+        static Vector ASin(const Vector& vec);
+        static Vector ACos(const Vector& vec);
+        static Vector ATan(const Vector& vec);
+        static Vector ATan2(const Vector& vec0, const Vector& vec1);
+
+        static Vector SinEst(const Vector& vec);
+        static Vector CosEst(const Vector& vec);
+        static Vector TanEst(const Vector& vec);
+        static Vector ASinEst(const Vector& vec);
+        static Vector ACosEst(const Vector& vec);
+        static Vector ATanEst(const Vector& vec);
+        static Vector ATan2Est(const Vector& vec0, const Vector& vec1);
+
+        static void SinCos(Vector& sin, Vector& cos, float angle);
+        static void SinCos(Vector& sin, Vector& cos, const Vector& angle);
+
+        static Vector AngleMod2Pi(const Vector& angles);
+
+    public:
+
+        operator __m128& ();
+        operator const __m128& () const;
+
+        Vector();
+        explicit Vector(Axis axis);
+        explicit Vector(ZeroInit_t);
+        explicit Vector(float v);
+        Vector(__m128 v);
+        Vector(float ix, float iy, float iz, float iw = 1.0f);
+
+        Vector(const Float2& v, float iz = 0.0f, float iw = 0.0f);
+        Vector(const Float3& v, float iw = 1.0f);
+        Vector(const Float4& v);
+        Vector(const float* pValues);
+
+        bool IsValid() const;
+
+        void Store(float* pValues) const;
+        void StoreFloat(float& value) const;
+        void StoreFloat2(Float2& value) const;
+        void StoreFloat3(Float3& value) const;
+        void StoreFloat4(Float4& value) const;
+
+        float ToFloat() const;
+        Float2 ToFloat2() const;
+        Float3 ToFloat3() const;
+        Float4 ToFloat4() const;
+
+        operator Float2() const;
+        operator Float3() const;
+        operator Float4() const;
+
+        //
+        // Element accessors
+        //
+
+        float GetX() const;
+        float GetY() const;
+        float GetZ() const;
+        float GetW() const;
+
+        void SetX(float x);
+        void SetY(float y);
+        void SetZ(float z);
+        void SetW(float w);
+
+        float operator[](uint32_t i) const;
+
+        //
+        // W component operations
+        //
+
+        bool IsW1() const;
+        bool IsW0() const;
+        Vector& SetW0();
+        Vector& SetW1();
+        Vector GetWithW0() const;
+        Vector GetWithW1() const;
+
+        //
+        // Dimensional Getters
+        //
+
+        // Returns only the first two components, z=w=0
+        Vector Get2D() const;
+
+        // Returns only the first three components, w = 0
+        Vector Get3D() const;
+
+        //
+        // Algebraic operators
+        //
+
+        Vector operator+(const Vector& v) const;
+        Vector& operator+=(const Vector& v);
+        Vector operator-(const Vector& v) const;
+        Vector& operator-=(const Vector& v);
+        Vector operator*(const Vector& v) const;
+        Vector& operator*=(const Vector& v);
+        Vector operator/(const Vector& v) const;
+        Vector& operator/=(const Vector& v);
+
+        Vector operator*(float const f) const;
+        Vector& operator*=(float const f);
+        Vector operator/(float const f) const;
+        Vector& operator/=(float const f);
+
+        Vector operator-() const;
+
+        Vector Orthogonal2D() const;
+        Vector Cross2(const Vector& other) const;
+        Vector Cross3(const Vector& other) const;
+        Vector Dot2(const Vector& other) const;
+        Vector Dot3(const Vector& other) const;
+        Vector Dot4(const Vector& other) const;
+        float GetDot2(const Vector& other) const;
+        float GetDot3(const Vector& other) const;
+        float GetDot4(const Vector& other) const;
+
+        Vector ScalarProjection(const Vector& other) const;
+        float GetScalarProjection(const Vector& other) const;
+        Vector VectorProjection(const Vector& other) const;
+
+        //
+        // Transformations
+        //
+
+        Vector& Invert();
+        Vector GetInverse() const;
+        Vector GetReciprocal() const;
+
+        Vector& InvertEst();
+        Vector GetInverseEst() const;
+
+        Vector& Negate();
+        Vector GetNegated() const;
+
+        Vector& Abs();
+        Vector GetAbs() const;
+
+        Vector& Sqrt();
+        Vector GetSqrt();
+
+        Vector& ReciprocalSqrt();
+        Vector GetReciprocalSqrt();
+
+        Vector& EstimatedReciprocalSqrt();
+        Vector GetEstimatedReciprocalSqrt();
+
+        Vector& Normalize2();
+        Vector& Normalize3();
+        Vector& Normalize4();
+
+        Vector GetNormalized2() const;
+        Vector GetNormalized3() const;
+        Vector GetNormalized4() const;
+
+        Vector& Floor();
+        Vector GetFloor() const;
+        Vector& Ceil();
+        Vector GetCeil() const;
+        Vector& Round();
+        Vector GetRound() const;
+
+        Vector GetSign() const;
+
+        //
+        // Permutations
+        //
+
+        Vector GetSplatX() const;
+        Vector GetSplatY() const;
+        Vector GetSplatZ() const;
+        Vector GetSplatW() const;
+
+        // Get a shuffled version of this vector, each argument represents the element index in the original vector
+        template<uint32_t xIdx, uint32_t yIdx, uint32_t zIdx, uint32_t wIdx>
+        Vector Swizzle() const;
+
+        // Get a shuffled version of this vector, each argument represents the element index in the original vector
+        Vector Swizzle(uint32_t xIdx, uint32_t yIdx, uint32_t zIdx, uint32_t wIdx) const;
+
+        // Get a shuffled version of this vector, each argument represents the element index in the original vector
+        Vector Shuffle(uint32_t xIdx, uint32_t yIdx, uint32_t zIdx, uint32_t wIdx) const;
+
+        // Get a shuffled version of this vector, each argument represents the element index in the original vector
+        template<uint32_t xIdx, uint32_t yIdx, uint32_t zIdx, uint32_t wIdx>
+        Vector Shuffle() const;
+
+        //
+        // Queries
+        //
+
+        Vector Length2() const;
+        Vector Length3() const;
+        Vector Length4() const;
+
+        float GetLength2() const;
+        float GetLength3() const;
+        float GetLength4() const;
+
+        Vector InverseLength2() const;
+        Vector InverseLength3() const;
+        Vector InverseLength4() const;
+
+        float GetInverseLength2() const;
+        float GetInverseLength3() const;
+        float GetInverseLength4() const;
+
+        Vector LengthSquared2() const;
+        Vector LengthSquared3() const;
+        Vector LengthSquared4() const;
+
+        float GetLengthSquared2() const;
+        float GetLengthSquared3() const;
+        float GetLengthSquared4() const;
+
+        Vector Distance2(const Vector& to) const;
+        Vector Distance3(const Vector& to) const;
+        Vector Distance4(const Vector& to) const;
+
+        float GetDistance2(const Vector& to) const;
+        float GetDistance3(const Vector& to) const;
+        float GetDistance4(const Vector& to) const;
+
+        Vector DistanceSquared2(const Vector& to) const;
+        Vector DistanceSquared3(const Vector& to) const;
+        Vector DistanceSquared4(const Vector& to) const;
+
+        float GetDistanceSquared2(const Vector& to) const;
+        float GetDistanceSquared3(const Vector& to) const;
+        float GetDistanceSquared4(const Vector& to) const;
+
+        bool IsNormalized2() const;
+        bool IsNormalized3() const;
+        bool IsNormalized4() const;
+
+        // Is this vector within the range [-bounds, bounds]
+        Vector InBounds(const Vector& bounds) const;
+
+        bool IsInBounds2(const Vector& bounds) const;
+        bool IsInBounds3(const Vector& bounds) const;
+        bool IsInBounds4(const Vector& bounds) const;
+
+        Vector Equal(const Vector& v) const;
+
+        bool IsEqual2(const Vector& v) const;
+        bool IsEqual3(const Vector& v) const;
+        bool IsEqual4(const Vector& v) const;
+
+        Vector NearEqual(const Vector& v, const Vector& epsilon) const;
+
+        bool IsNearEqual2(const Vector& v, float epsilon) const;
+        bool IsNearEqual3(const Vector& v, float epsilon) const;
+        bool IsNearEqual4(const Vector& v, float epsilon) const;
+
+        bool IsNearEqual2(const Vector& v, const Vector& epsilon = Vector::Epsilon) const;
+        bool IsNearEqual3(const Vector& v, const Vector& epsilon = Vector::Epsilon) const;
+        bool IsNearEqual4(const Vector& v, const Vector& epsilon = Vector::Epsilon) const;
+
+        Vector GreaterThan(const Vector& v) const;
+        bool IsAnyGreaterThan(const Vector& v) const;
+
+        bool IsGreaterThan2(const Vector& v) const;
+        bool IsGreaterThan3(const Vector& v) const;
+        bool IsGreaterThan4(const Vector& v) const;
+
+        Vector GreaterThanEqual(const Vector& v) const;
+        bool IsAnyGreaterThanEqual(const Vector& v) const;
+
+        bool IsGreaterThanEqual2(const Vector& v) const;
+        bool IsGreaterThanEqual3(const Vector& v) const;
+        bool IsGreaterThanEqual4(const Vector& v) const;
+
+        Vector LessThan(const Vector& v) const;
+        bool IsAnyLessThan(const Vector& v) const;
+
+        bool IsLessThan2(const Vector& v) const;
+        bool IsLessThan3(const Vector& v) const;
+        bool IsLessThan4(const Vector& v) const;
+
+        Vector LessThanEqual(const Vector& v) const;
+        bool IsAnyLessThanEqual(const Vector& v) const;
+
+        bool IsLessThanEqual2(const Vector& v) const;
+        bool IsLessThanEqual3(const Vector& v) const;
+        bool IsLessThanEqual4(const Vector& v) const;
+
+        Vector EqualsZero() const;
+        bool IsAnyEqualToZero2() const;
+        bool IsAnyEqualToZero3() const;
+        bool IsAnyEqualToZero4() const;
+
+        bool IsZero2() const;
+        bool IsZero3() const;
+        bool IsZero4() const;
+
+        Vector NearEqualsZero(float epsilon = Math::Epsilon) const;
+
+        bool IsNearZero2(float epsilon = Math::Epsilon) const;
+        bool IsNearZero3(float epsilon = Math::Epsilon) const;
+        bool IsNearZero4(float epsilon = Math::Epsilon) const;
+
+        Vector EqualsInfinity() const;
+
+        bool IsInfinite2() const;
+        bool IsInfinite3() const;
+        bool IsInfinite4() const;
+
+        Vector EqualsNaN() const;
+
+        bool IsNaN2() const;
+        bool IsNaN3() const;
+        bool IsNaN4() const;
+
+        bool IsParallelTo(const Vector& v) const;
+
+        void ToDirectionAndLength2(Vector& direction, float& length) const;
+        void ToDirectionAndLength3(Vector& direction, float& length) const;
+
+        bool operator==(const Vector& rhs) const;
+        bool operator!=(const Vector& rhs) const;
+
+    public:
+
+        __m128 m_data;
+    };
+
+    static_assert(sizeof(Vector) == 16, "Vector size must be 16 bytes!");
+}
+
+#include "Vector.inl"

MotionCorrection/src/cpp/Math/Vector.inl

@@ -0,0 +1,2224 @@
+/*
+ * SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
+ * SPDX-License-Identifier: Apache-2.0
+ */
+
+#pragma once
+
+#include <cstring>
+
+#include "Vector.h"
+
+namespace Math
+{
+    FORCE_INLINE Vector Vector::Cross2(const Vector& v0, const Vector& v1)
+    {
+        return v0.Cross2(v1);
+    }
+
+    FORCE_INLINE Vector Vector::Cross3(const Vector& v0, const Vector& v1)
+    {
+        return v0.Cross3(v1);
+    }
+
+    FORCE_INLINE Vector Vector::Dot2(const Vector& v0, const Vector& v1)
+    {
+        return v0.Dot2(v1);
+    }
+
+    FORCE_INLINE Vector Vector::Dot3(const Vector& v0, const Vector& v1)
+    {
+        return v0.Dot3(v1);
+    }
+
+    FORCE_INLINE Vector Vector::Dot4(const Vector& v0, const Vector& v1)
+    {
+        return v0.Dot4(v1);
+    }
+
+    FORCE_INLINE Vector Vector::Average2(const Vector& v0, const Vector& v1)
+    {
+        auto avg4 = Average4(v0, v1);
+        return Vector::Select(avg4, Vector::Zero, Vector(0, 0, 1, 1));
+    }
+
+    FORCE_INLINE Vector Vector::Average3(const Vector& v0, const Vector& v1)
+    {
+        auto avg4 = Average4(v0, v1);
+        return Vector::Select(avg4, Vector::Zero, Vector(0, 0, 0, 1));
+    }
+
+    FORCE_INLINE Vector Vector::Average4(const Vector& v0, const Vector& v1)
+    {
+        return (v0 + v1) * Vector::Half;
+    }
+
+    FORCE_INLINE Vector Vector::Min(const Vector& v0, const Vector& v1)
+    {
+        Vector result;
+        result = _mm_min_ps(v0, v1);
+        return result;
+    }
+
+    FORCE_INLINE Vector Vector::Max(const Vector& v0, const Vector& v1)
+    {
+        Vector result;
+        result = _mm_max_ps(v0, v1);
+        return result;
+    }
+
+    FORCE_INLINE float Vector::Min(const Vector& v)
+    {
+        __m128 shufReg, sumsReg;
+        shufReg = _mm_movehdup_ps(v);
+        sumsReg = _mm_min_ps(v, shufReg);
+        shufReg = _mm_movehl_ps(shufReg, sumsReg);
+        sumsReg = _mm_min_ss(sumsReg, shufReg);
+        return _mm_cvtss_f32(sumsReg);
+    }
+
+    FORCE_INLINE float Vector::Max(const Vector& v)
+    {
+        __m128 shufReg, sumsReg;
+        shufReg = _mm_movehdup_ps(v);
+        sumsReg = _mm_max_ps(v, shufReg);
+        shufReg = _mm_movehl_ps(shufReg, sumsReg);
+        sumsReg = _mm_max_ss(sumsReg, shufReg);
+        return _mm_cvtss_f32(sumsReg);
+    }
+
+    FORCE_INLINE Vector Vector::Clamp(const Vector& v, const Vector& min, const Vector& max)
+    {
+        Vector result;
+        result = _mm_max_ps(min, v);
+        result = _mm_min_ps(result, max);
+        return result;
+    }
+
+    FORCE_INLINE Vector Vector::Xor(const Vector& v0, const Vector& v1)
+    {
+        __m128i V = _mm_xor_si128(_mm_castps_si128(v0), _mm_castps_si128(v1));
+
+        Vector result;
+        result = _mm_castsi128_ps(V);
+        return result;
+    }
+
+    FORCE_INLINE Vector Vector::MultiplyAdd(const Vector& v, const Vector& multiplier, const Vector& addend)
+    {
+        // result = addend + ( vec * multiplier )
+        Vector result;
+        result = _mm_mul_ps(v, multiplier);
+        result = _mm_add_ps(result, addend);
+        return result;
+    }
+
+    FORCE_INLINE Vector Vector::MultiplySubtract(const Vector& vec, const Vector& multiplier, const Vector& subtrahend)
+    {
+        // result = ( vec * multiplier ) - subtrahend
+        auto r = _mm_mul_ps(vec, multiplier);
+        return _mm_sub_ps(r, subtrahend);
+    }
+
+    FORCE_INLINE Vector Vector::NegativeMultiplySubtract(const Vector& vec, const Vector& multiplier, const Vector& minuend)
+    {
+        // result = minuend - ( vec * multiplier )
+        auto r = _mm_mul_ps(vec, multiplier);
+        return _mm_sub_ps(minuend, r);
+    }
+
+    FORCE_INLINE Vector Vector::LinearCombination(const Vector& v0, const Vector& v1, float scale0, float scale1)
+    {
+        return (v0 * scale0) + (v1 * scale1);
+    }
+
+    FORCE_INLINE Vector Vector::Lerp(const Vector& from, const Vector& to, float t)
+    {
+        ASSERT(t >= 0.0f && t <= 1.0f);
+
+        Vector L = _mm_sub_ps(to, from);
+        Vector S = _mm_set_ps1(t);
+
+        Vector result;
+        result = _mm_mul_ps(L, S);
+        result = _mm_add_ps(result, from);
+        return result;
+    }
+
+    FORCE_INLINE Vector Vector::NLerp(const Vector& from, const Vector& to, float t)
+    {
+        ASSERT(t >= 0.0f && t <= 1.0f);
+
+        // Calculate the final length
+        auto const fromLength = from.Length3();
+        auto const toLength = to.Length3();
+        auto const finalLength = Vector::Lerp(fromLength, toLength, t);
+
+        // Normalize vectors
+        Vector const normalizedFrom = from / fromLength;
+        Vector const normalizedTo = to / toLength;
+
+        // LERP
+        auto const finalDirection = Lerp(normalizedFrom, normalizedTo, t);
+        auto result = finalDirection.GetNormalized3() * finalLength;
+        return result;
+    }
+
+    FORCE_INLINE Vector Vector::Select(const Vector& v0, const Vector& v1, const Vector& control)
+    {
+        auto const ctrl = _mm_cmpneq_ps(control, Vector::Zero);
+
+        Vector result;
+        auto vTemp1 = _mm_andnot_ps(ctrl, v0);
+        auto vTemp2 = _mm_and_ps(v1, ctrl);
+        result = _mm_or_ps(vTemp1, vTemp2);
+        return result;
+    }
+
+    template<uint32_t PermuteX, uint32_t PermuteY, uint32_t PermuteZ, uint32_t PermuteW>
+    FORCE_INLINE Vector Vector::Permute(const Vector& v0, const Vector& v1)
+    {
+        static_assert(PermuteX <= 7, "Element index parameter out of range");
+        static_assert(PermuteY <= 7, "Element index parameter out of range");
+        static_assert(PermuteZ <= 7, "Element index parameter out of range");
+        static_assert(PermuteW <= 7, "Element index parameter out of range");
+
+        uint32_t const shuffle = _MM_SHUFFLE(PermuteW & 3, PermuteZ & 3, PermuteY & 3, PermuteX & 3);
+        bool const whichX = PermuteX > 3;
+        bool const whichY = PermuteY > 3;
+        bool const whichZ = PermuteZ > 3;
+        bool const whichW = PermuteW > 3;
+
+        static SIMD::UIntMask const selectMask = { whichX ? 0xFFFFFFFF : 0, whichY ? 0xFFFFFFFF : 0, whichZ ? 0xFFFFFFFF : 0, whichW ? 0xFFFFFFFF : 0 };
+        __m128 shuffled1 = _mm_shuffle_ps(v0, v0, shuffle);
+        __m128 shuffled2 = _mm_shuffle_ps(v1, v1, shuffle);
+        __m128 masked1 = _mm_andnot_ps(selectMask, shuffled1);
+        __m128 masked2 = _mm_and_ps(selectMask, shuffled2);
+        return _mm_or_ps(masked1, masked2);
+    }
+
+    FORCE_INLINE Vector Vector::Sin(const Vector& vec)
+    {
+        // Force the value within the bounds of pi
+        auto m_x = Vector::AngleMod2Pi(vec);
+
+        // Map in [-pi/2,pi/2] with sin(m_y) = sin(m_x).
+        __m128 sign = _mm_and_ps(m_x, SIMD::g_signMask);
+        __m128 c = _mm_or_ps(Vector::Pi, sign);  // pi when m_x >= 0, -pi when m_x < 0
+        __m128 absx = _mm_andnot_ps(sign, m_x);  // |m_x|
+        __m128 rflx = _mm_sub_ps(c, m_x);
+        __m128 comp = _mm_cmple_ps(absx, Vector::PiDivTwo);
+        __m128 select0 = _mm_and_ps(comp, m_x);
+        __m128 select1 = _mm_andnot_ps(comp, rflx);
+        m_x = _mm_or_ps(select0, select1);
+
+        __m128 x2 = _mm_mul_ps(m_x, m_x);
+
+        // Compute polynomial approximation
+        const auto SC1 = SIMD::g_sinCoefficients1;
+        auto vConstants = _mm_shuffle_ps(SC1, SC1, _MM_SHUFFLE(0, 0, 0, 0));
+        __m128 Result = _mm_mul_ps(vConstants, x2);
+
+        const auto SC0 = SIMD::g_sinCoefficients0;
+        vConstants = _mm_shuffle_ps(SC0, SC0, _MM_SHUFFLE(3, 3, 3, 3));
+        Result = _mm_add_ps(Result, vConstants);
+        Result = _mm_mul_ps(Result, x2);
+
+        vConstants = _mm_shuffle_ps(SC0, SC0, _MM_SHUFFLE(2, 2, 2, 2));
+        Result = _mm_add_ps(Result, vConstants);
+        Result = _mm_mul_ps(Result, x2);
+
+        vConstants = _mm_shuffle_ps(SC0, SC0, _MM_SHUFFLE(1, 1, 1, 1));
+        Result = _mm_add_ps(Result, vConstants);
+        Result = _mm_mul_ps(Result, x2);
+
+        vConstants = _mm_shuffle_ps(SC0, SC0, _MM_SHUFFLE(0, 0, 0, 0));
+        Result = _mm_add_ps(Result, vConstants);
+        Result = _mm_mul_ps(Result, x2);
+        Result = _mm_add_ps(Result, Vector::One);
+        Result = _mm_mul_ps(Result, m_x);
+        return Result;
+    }
+
+    FORCE_INLINE Vector Vector::Cos(const Vector& vec)
+    {
+        // Map V to m_x in [-pi,pi].
+        auto m_x = Vector::AngleMod2Pi(vec);
+
+        // Map in [-pi/2,pi/2] with cos(m_y) = sign*cos(m_x).
+        auto sign = _mm_and_ps(m_x, SIMD::g_signMask);
+        __m128 c = _mm_or_ps(Vector::Pi, sign);  // pi when m_x >= 0, -pi when m_x < 0
+        __m128 absx = _mm_andnot_ps(sign, m_x);  // |m_x|
+        __m128 rflx = _mm_sub_ps(c, m_x);
+        __m128 comp = _mm_cmple_ps(absx, Vector::PiDivTwo);
+        __m128 select0 = _mm_and_ps(comp, m_x);
+        __m128 select1 = _mm_andnot_ps(comp, rflx);
+        m_x = _mm_or_ps(select0, select1);
+        select0 = _mm_and_ps(comp, Vector::One);
+        select1 = _mm_andnot_ps(comp, Vector::NegativeOne);
+        sign = _mm_or_ps(select0, select1);
+
+        __m128 x2 = _mm_mul_ps(m_x, m_x);
+
+        // Compute polynomial approximation
+        const auto CC1 = SIMD::g_cosCoefficients1;
+        auto vConstants = _mm_shuffle_ps(CC1, CC1, _MM_SHUFFLE(0, 0, 0, 0));
+        __m128 Result = _mm_mul_ps(vConstants, x2);
+
+        const auto CC0 = SIMD::g_cosCoefficients0;
+        vConstants = _mm_shuffle_ps(CC0, CC0, _MM_SHUFFLE(3, 3, 3, 3));
+        Result = _mm_add_ps(Result, vConstants);
+        Result = _mm_mul_ps(Result, x2);
+
+        vConstants = _mm_shuffle_ps(CC0, CC0, _MM_SHUFFLE(2, 2, 2, 2));
+        Result = _mm_add_ps(Result, vConstants);
+        Result = _mm_mul_ps(Result, x2);
+
+        vConstants = _mm_shuffle_ps(CC0, CC0, _MM_SHUFFLE(1, 1, 1, 1));
+        Result = _mm_add_ps(Result, vConstants);
+        Result = _mm_mul_ps(Result, x2);
+
+        vConstants = _mm_shuffle_ps(CC0, CC0, _MM_SHUFFLE(0, 0, 0, 0));
+        Result = _mm_add_ps(Result, vConstants);
+        Result = _mm_mul_ps(Result, x2);
+        Result = _mm_add_ps(Result, Vector::One);
+        Result = _mm_mul_ps(Result, sign);
+        return Result;
+    }
+
+    FORCE_INLINE Vector Vector::Tan(const Vector& vec)
+    {
+        static const Vector tanCoefficients0 = { 1.0f, -4.667168334e-1f, 2.566383229e-2f, -3.118153191e-4f };
+        static const Vector tanCoefficients1 = { 4.981943399e-7f, -1.333835001e-1f, 3.424887824e-3f, -1.786170734e-5f };
+        static const Vector tanConstants = { 1.570796371f, 6.077100628e-11f, 0.000244140625f, 0.63661977228f /*2 / Pi*/ };
+        static const SIMD::UIntMask mask = { 0x1, 0x1, 0x1, 0x1 };
+
+        Vector TwoDivPi = tanConstants.GetSplatW();
+        Vector C0 = tanConstants.GetSplatX();
+        Vector C1 = tanConstants.GetSplatY();
+        Vector vEpsilon = tanConstants.GetSplatZ();
+
+        Vector VA = (vec * TwoDivPi).Round();
+        Vector VC = Vector::NegativeMultiplySubtract(VA, C0, vec);
+        Vector VB = VA.GetAbs();
+        VC = Vector::NegativeMultiplySubtract(VA, C1, VC);
+        reinterpret_cast<__m128i*>(&VB)[0] = _mm_cvttps_epi32(VB);
+
+        Vector VC2 = VC * VC;
+        Vector T7 = tanCoefficients1.GetSplatW();
+        Vector T6 = tanCoefficients1.GetSplatZ();
+        Vector T4 = tanCoefficients1.GetSplatX();
+        Vector T3 = tanCoefficients0.GetSplatW();
+        Vector T5 = tanCoefficients1.GetSplatY();
+        Vector T2 = tanCoefficients0.GetSplatZ();
+        Vector T1 = tanCoefficients0.GetSplatY();
+        Vector T0 = tanCoefficients0.GetSplatX();
+
+        Vector VBIsEven = _mm_and_ps(VB, mask);
+        VBIsEven = _mm_castsi128_ps(_mm_cmpeq_epi32(_mm_castps_si128(VBIsEven), _mm_castps_si128(Vector::Zero)));
+
+        Vector N = Vector::MultiplyAdd(VC2, T7, T6);
+        Vector D = Vector::MultiplyAdd(VC2, T4, T3);
+        N = Vector::MultiplyAdd(VC2, N, T5);
+        D = Vector::MultiplyAdd(VC2, D, T2);
+        N = VC2 * N;
+        D = Vector::MultiplyAdd(VC2, D, T1);
+        N = Vector::MultiplyAdd(VC, N, VC);
+        Vector VCNearZero = VC.InBounds(vEpsilon);
+        D = Vector::MultiplyAdd(VC2, D, T0);
+
+        N = Vector::Select(N, VC, VCNearZero);
+        D = Vector::Select(D, Vector::One, VCNearZero);
+
+        Vector R0 = N.GetNegated();
+        Vector R1 = N / D;
+        R0 = D / R0;
+
+        Vector VIsZero = vec.EqualsZero();
+        Vector Result = Vector::Select(R0, R1, VBIsEven);
+        Result = Vector::Select(Result, Zero, VIsZero);
+
+        return Result;
+    }
+
+    FORCE_INLINE Vector Vector::ASin(const Vector& vec)
+    {
+        __m128 nonnegative = _mm_cmpge_ps(vec, Vector::Zero);
+        __m128 mvalue = _mm_sub_ps(Vector::Zero, vec);
+        __m128 m_x = _mm_max_ps(vec, mvalue);  // |vec|
+
+        // Compute (1-|vec|), clamp to zero to avoid sqrt of negative number.
+        __m128 oneMValue = _mm_sub_ps(Vector::One, m_x);
+        __m128 clampOneMValue = _mm_max_ps(Vector::Zero, oneMValue);
+        __m128 root = _mm_sqrt_ps(clampOneMValue);  // sqrt(1-|vec|)
+
+        // Compute polynomial approximation
+        const auto AC1 = SIMD::g_arcCoefficients1;
+        auto vConstants = _mm_shuffle_ps(AC1, AC1, _MM_SHUFFLE(3, 3, 3, 3));
+        __m128 t0 = _mm_mul_ps(vConstants, m_x);
+
+        vConstants = _mm_shuffle_ps(AC1, AC1, _MM_SHUFFLE(2, 2, 2, 2));
+        t0 = _mm_add_ps(t0, vConstants);
+        t0 = _mm_mul_ps(t0, m_x);
+
+        vConstants = _mm_shuffle_ps(AC1, AC1, _MM_SHUFFLE(1, 1, 1, 1));
+        t0 = _mm_add_ps(t0, vConstants);
+        t0 = _mm_mul_ps(t0, m_x);
+
+        vConstants = _mm_shuffle_ps(AC1, AC1, _MM_SHUFFLE(0, 0, 0, 0));
+        t0 = _mm_add_ps(t0, vConstants);
+        t0 = _mm_mul_ps(t0, m_x);
+
+        const auto AC0 = SIMD::g_arcCoefficients0;
+        vConstants = _mm_shuffle_ps(AC0, AC0, _MM_SHUFFLE(3, 3, 3, 3));
+        t0 = _mm_add_ps(t0, vConstants);
+        t0 = _mm_mul_ps(t0, m_x);
+
+        vConstants = _mm_shuffle_ps(AC0, AC0, _MM_SHUFFLE(2, 2, 2, 2));
+        t0 = _mm_add_ps(t0, vConstants);
+        t0 = _mm_mul_ps(t0, m_x);
+
+        vConstants = _mm_shuffle_ps(AC0, AC0, _MM_SHUFFLE(1, 1, 1, 1));
+        t0 = _mm_add_ps(t0, vConstants);
+        t0 = _mm_mul_ps(t0, m_x);
+
+        vConstants = _mm_shuffle_ps(AC0, AC0, _MM_SHUFFLE(0, 0, 0, 0));
+        t0 = _mm_add_ps(t0, vConstants);
+        t0 = _mm_mul_ps(t0, root);
+
+        __m128 t1 = _mm_sub_ps(Vector::Pi, t0);
+        t0 = _mm_and_ps(nonnegative, t0);
+        t1 = _mm_andnot_ps(nonnegative, t1);
+        t0 = _mm_or_ps(t0, t1);
+        t0 = _mm_sub_ps(Vector::PiDivTwo, t0);
+        return t0;
+    }
+
+    FORCE_INLINE Vector Vector::ACos(const Vector& vec)
+    {
+        __m128 nonnegative = _mm_cmpge_ps(vec, Vector::Zero);
+        __m128 mvalue = _mm_sub_ps(Vector::Zero, vec);
+        __m128 m_x = _mm_max_ps(vec, mvalue);  // |vec|
+
+        // Compute (1-|vec|), clamp to zero to avoid sqrt of negative number.
+        __m128 oneMValue = _mm_sub_ps(Vector::One, m_x);
+        __m128 clampOneMValue = _mm_max_ps(Vector::Zero, oneMValue);
+        __m128 root = _mm_sqrt_ps(clampOneMValue);  // sqrt(1-|vec|)
+
+        // Compute polynomial approximation
+        const auto AC1 = SIMD::g_arcCoefficients1;
+        auto vConstants = _mm_shuffle_ps(AC1, AC1, _MM_SHUFFLE(3, 3, 3, 3));
+        __m128 t0 = _mm_mul_ps(vConstants, m_x);
+
+        vConstants = _mm_shuffle_ps(AC1, AC1, _MM_SHUFFLE(2, 2, 2, 2));
+        t0 = _mm_add_ps(t0, vConstants);
+        t0 = _mm_mul_ps(t0, m_x);
+
+        vConstants = _mm_shuffle_ps(AC1, AC1, _MM_SHUFFLE(1, 1, 1, 1));
+        t0 = _mm_add_ps(t0, vConstants);
+        t0 = _mm_mul_ps(t0, m_x);
+
+        vConstants = _mm_shuffle_ps(AC1, AC1, _MM_SHUFFLE(0, 0, 0, 0));
+        t0 = _mm_add_ps(t0, vConstants);
+        t0 = _mm_mul_ps(t0, m_x);
+
+        const auto AC0 = SIMD::g_arcCoefficients0;
+        vConstants = _mm_shuffle_ps(AC0, AC0, _MM_SHUFFLE(3, 3, 3, 3));
+        t0 = _mm_add_ps(t0, vConstants);
+        t0 = _mm_mul_ps(t0, m_x);
+
+        vConstants = _mm_shuffle_ps(AC0, AC0, _MM_SHUFFLE(2, 2, 2, 2));
+        t0 = _mm_add_ps(t0, vConstants);
+        t0 = _mm_mul_ps(t0, m_x);
+
+        vConstants = _mm_shuffle_ps(AC0, AC0, _MM_SHUFFLE(1, 1, 1, 1));
+        t0 = _mm_add_ps(t0, vConstants);
+        t0 = _mm_mul_ps(t0, m_x);
+
+        vConstants = _mm_shuffle_ps(AC0, AC0, _MM_SHUFFLE(0, 0, 0, 0));
+        t0 = _mm_add_ps(t0, vConstants);
+        t0 = _mm_mul_ps(t0, root);
+
+        __m128 t1 = _mm_sub_ps(Vector::Pi, t0);
+        t0 = _mm_and_ps(nonnegative, t0);
+        t1 = _mm_andnot_ps(nonnegative, t1);
+        t0 = _mm_or_ps(t0, t1);
+        return t0;
+    }
+
+    FORCE_INLINE Vector Vector::ATan(const Vector& vec)
+    {
+        __m128 absV = vec.GetAbs();
+        __m128 invV = _mm_div_ps(Vector::One, vec);
+        __m128 comp = _mm_cmpgt_ps(vec, Vector::One);
+        __m128 select0 = _mm_and_ps(comp, Vector::One);
+        __m128 select1 = _mm_andnot_ps(comp, Vector::NegativeOne);
+        __m128 sign = _mm_or_ps(select0, select1);
+        comp = _mm_cmple_ps(absV, Vector::One);
+        select0 = _mm_and_ps(comp, Vector::Zero);
+        select1 = _mm_andnot_ps(comp, sign);
+        sign = _mm_or_ps(select0, select1);
+        select0 = _mm_and_ps(comp, vec);
+        select1 = _mm_andnot_ps(comp, invV);
+        __m128 m_x = _mm_or_ps(select0, select1);
+
+        __m128 x2 = _mm_mul_ps(m_x, m_x);
+
+        // Compute polynomial approximation
+        Vector const TC1 = SIMD::g_aTanCoefficients1;
+        Vector vConstants = _mm_shuffle_ps(TC1, TC1, _MM_SHUFFLE(3, 3, 3, 3));
+        __m128 Result = _mm_mul_ps(vConstants, x2);
+
+        vConstants = _mm_shuffle_ps(TC1, TC1, _MM_SHUFFLE(2, 2, 2, 2));
+        Result = _mm_add_ps(Result, vConstants);
+        Result = _mm_mul_ps(Result, x2);
+
+        vConstants = _mm_shuffle_ps(TC1, TC1, _MM_SHUFFLE(1, 1, 1, 1));
+        Result = _mm_add_ps(Result, vConstants);
+        Result = _mm_mul_ps(Result, x2);
+
+        vConstants = _mm_shuffle_ps(TC1, TC1, _MM_SHUFFLE(0, 0, 0, 0));
+        Result = _mm_add_ps(Result, vConstants);
+        Result = _mm_mul_ps(Result, x2);
+
+        Vector const TC0 = SIMD::g_aTanCoefficients0;
+        vConstants = _mm_shuffle_ps(TC0, TC0, _MM_SHUFFLE(3, 3, 3, 3));
+        Result = _mm_add_ps(Result, vConstants);
+        Result = _mm_mul_ps(Result, x2);
+
+        vConstants = _mm_shuffle_ps(TC0, TC0, _MM_SHUFFLE(2, 2, 2, 2));
+        Result = _mm_add_ps(Result, vConstants);
+        Result = _mm_mul_ps(Result, x2);
+
+        vConstants = _mm_shuffle_ps(TC0, TC0, _MM_SHUFFLE(1, 1, 1, 1));
+        Result = _mm_add_ps(Result, vConstants);
+        Result = _mm_mul_ps(Result, x2);
+
+        vConstants = _mm_shuffle_ps(TC0, TC0, _MM_SHUFFLE(0, 0, 0, 0));
+        Result = _mm_add_ps(Result, vConstants);
+        Result = _mm_mul_ps(Result, x2);
+        Result = _mm_add_ps(Result, Vector::One);
+        Result = _mm_mul_ps(Result, m_x);
+        __m128 result1 = _mm_mul_ps(sign, Vector::PiDivTwo);
+        result1 = _mm_sub_ps(result1, Result);
+
+        comp = _mm_cmpeq_ps(sign, Vector::Zero);
+        select0 = _mm_and_ps(comp, Result);
+        select1 = _mm_andnot_ps(comp, result1);
+        Result = _mm_or_ps(select0, select1);
+        return Result;
+    }
+
+    FORCE_INLINE Vector Vector::ATan2(const Vector& Y, const Vector& X)
+    {
+        Vector ATanResultValid = Vector(SIMD::g_trueMask);
+
+        Vector vPi = Vector(SIMD::g_aTan2Constants).GetSplatX();
+        Vector vPiOverTwo = Vector(SIMD::g_aTan2Constants).GetSplatY();
+        Vector vPiOverFour = Vector(SIMD::g_aTan2Constants).GetSplatZ();
+        Vector vThreePiOverFour = Vector(SIMD::g_aTan2Constants).GetSplatW();
+
+        Vector YEqualsZero = Y.EqualsZero();
+        Vector XEqualsZero = X.EqualsZero();
+        Vector XIsPositive = _mm_and_ps(X, SIMD::g_signMask);
+        XIsPositive = _mm_castsi128_ps(_mm_cmpeq_epi32(_mm_castps_si128(XIsPositive), _mm_castps_si128(Vector::Zero)));
+        Vector YEqualsInfinity = Y.EqualsInfinity();
+        Vector XEqualsInfinity = X.EqualsInfinity();
+
+        Vector YSign = _mm_and_ps(Y, SIMD::g_signMask);
+        vPi = _mm_castsi128_ps(_mm_or_si128(_mm_castps_si128(vPi), _mm_castps_si128(YSign)));
+        vPiOverTwo = _mm_castsi128_ps(_mm_or_si128(_mm_castps_si128(vPiOverTwo), _mm_castps_si128(YSign)));
+        vPiOverFour = _mm_castsi128_ps(_mm_or_si128(_mm_castps_si128(vPiOverFour), _mm_castps_si128(YSign)));
+        vThreePiOverFour = _mm_castsi128_ps(_mm_or_si128(_mm_castps_si128(vThreePiOverFour), _mm_castps_si128(YSign)));
+
+        Vector R1 = Vector::Select(vPi, YSign, XIsPositive);
+        Vector R2 = Vector::Select(ATanResultValid, vPiOverTwo, XEqualsZero);
+        Vector R3 = Vector::Select(R2, R1, YEqualsZero);
+        Vector R4 = Vector::Select(vThreePiOverFour, vPiOverFour, XIsPositive);
+        Vector R5 = Vector::Select(vPiOverTwo, R4, XEqualsInfinity);
+        Vector Result = Vector::Select(R3, R5, YEqualsInfinity);
+        ATanResultValid = _mm_castsi128_ps(_mm_cmpeq_epi32(_mm_castps_si128(Result), _mm_castps_si128(ATanResultValid)));
+
+        Vector V = Y / X;
+        Vector R0 = Vector::ATan(V);
+        R1 = Vector::Select(vPi, Vector(SIMD::g_signMask), XIsPositive);
+        R2 = R0 + R1;
+
+        return Vector::Select(Result, R2, ATanResultValid);
+    }
+
+    FORCE_INLINE Vector Vector::SinEst(const Vector& vec)
+    {
+        // Force the value within the bounds of pi
+        auto m_x = Vector::AngleMod2Pi(vec);
+
+        // Map in [-pi/2,pi/2] with sin(m_y) = sin(m_x).
+        __m128 sign = _mm_and_ps(m_x, SIMD::g_signMask);
+        __m128 c = _mm_or_ps(Vector::Pi, sign);  // pi when m_x >= 0, -pi when m_x < 0
+        __m128 absx = _mm_andnot_ps(sign, m_x);  // |m_x|
+        __m128 rflx = _mm_sub_ps(c, m_x);
+        __m128 comp = _mm_cmple_ps(absx, Vector::PiDivTwo);
+        __m128 select0 = _mm_and_ps(comp, m_x);
+        __m128 select1 = _mm_andnot_ps(comp, rflx);
+        m_x = _mm_or_ps(select0, select1);
+
+        __m128 x2 = _mm_mul_ps(m_x, m_x);
+
+        // Compute polynomial approximation
+        const auto SEC = SIMD::g_sinCoefficients1;
+        auto vConstants = _mm_shuffle_ps(SEC, SEC, _MM_SHUFFLE(3, 3, 3, 3));
+        __m128 Result = _mm_mul_ps(vConstants, x2);
+
+        vConstants = _mm_shuffle_ps(SEC, SEC, _MM_SHUFFLE(2, 2, 2, 2));
+        Result = _mm_add_ps(Result, vConstants);
+        Result = _mm_mul_ps(Result, x2);
+
+        vConstants = _mm_shuffle_ps(SEC, SEC, _MM_SHUFFLE(1, 1, 1, 1));
+        Result = _mm_add_ps(Result, vConstants);
+        Result = _mm_mul_ps(Result, x2);
+
+        Result = _mm_add_ps(Result, Vector::One);
+        Result = _mm_mul_ps(Result, m_x);
+        return Result;
+    }
+
+    FORCE_INLINE Vector Vector::CosEst(const Vector& vec)
+    {
+        // Map V to m_x in [-pi,pi].
+        auto m_x = Vector::AngleMod2Pi(vec);
+
+        // Map in [-pi/2,pi/2] with cos(m_y) = sign*cos(m_x).
+        auto sign = _mm_and_ps(m_x, SIMD::g_signMask);
+        __m128 c = _mm_or_ps(Vector::Pi, sign);  // pi when m_x >= 0, -pi when m_x < 0
+        __m128 absx = _mm_andnot_ps(sign, m_x);  // |m_x|
+        __m128 rflx = _mm_sub_ps(c, m_x);
+        __m128 comp = _mm_cmple_ps(absx, Vector::PiDivTwo);
+        __m128 select0 = _mm_and_ps(comp, m_x);
+        __m128 select1 = _mm_andnot_ps(comp, rflx);
+        m_x = _mm_or_ps(select0, select1);
+        select0 = _mm_and_ps(comp, Vector::One);
+        select1 = _mm_andnot_ps(comp, Vector::NegativeOne);
+        sign = _mm_or_ps(select0, select1);
+
+        __m128 x2 = _mm_mul_ps(m_x, m_x);
+
+        // Compute polynomial approximation
+        const auto CEC = SIMD::g_cosCoefficients1;
+        auto vConstants = _mm_shuffle_ps(CEC, CEC, _MM_SHUFFLE(3, 3, 3, 3));
+        __m128 Result = _mm_mul_ps(vConstants, x2);
+
+        vConstants = _mm_shuffle_ps(CEC, CEC, _MM_SHUFFLE(2, 2, 2, 2));
+        Result = _mm_add_ps(Result, vConstants);
+        Result = _mm_mul_ps(Result, x2);
+
+        vConstants = _mm_shuffle_ps(CEC, CEC, _MM_SHUFFLE(1, 1, 1, 1));
+        Result = _mm_add_ps(Result, vConstants);
+        Result = _mm_mul_ps(Result, x2);
+
+        Result = _mm_add_ps(Result, Vector::One);
+        Result = _mm_mul_ps(Result, sign);
+        return Result;
+    }
+
+    FORCE_INLINE Vector Vector::TanEst(const Vector& vec)
+    {
+        Vector W = Vector(SIMD::g_tanEstCoefficients).GetSplatW();
+        Vector V1 = (vec * W).Round();
+        V1 = Vector::NegativeMultiplySubtract(Vector::Pi, V1, vec);
+
+        Vector const T0 = Vector(SIMD::g_tanEstCoefficients).GetSplatX();
+        Vector const T1 = Vector(SIMD::g_tanEstCoefficients).GetSplatY();
+        Vector const T2 = Vector(SIMD::g_tanEstCoefficients).GetSplatZ();
+
+        auto V2T2 = Vector::NegativeMultiplySubtract(V1, V1, T2);
+        auto V2 = V1 * V1;
+        auto V1T0 = V1 * T0;
+        auto V1T1 = V1 * T1;
+
+        auto N = Vector::MultiplyAdd(V2, V1T1, V1T0);
+        auto D = V2T2.GetInverseEst();
+        return N * D;
+    }
+
+    FORCE_INLINE Vector Vector::ASinEst(const Vector& vec)
+    {
+        __m128 nonnegative = _mm_cmpge_ps(vec, Vector::Zero);
+        __m128 mvalue = _mm_sub_ps(Vector::Zero, vec);
+        __m128 m_x = _mm_max_ps(vec, mvalue);  // |vec|
+
+        // Compute (1-|vec|), clamp to zero to avoid sqrt of negative number.
+        __m128 oneMValue = _mm_sub_ps(Vector::One, m_x);
+        __m128 clampOneMValue = _mm_max_ps(Vector::Zero, oneMValue);
+        __m128 root = _mm_sqrt_ps(clampOneMValue);  // sqrt(1-|vec|)
+
+        // Compute polynomial approximation
+        const auto AEC = SIMD::g_arcEstCoefficients;
+        auto vConstants = _mm_shuffle_ps(AEC, AEC, _MM_SHUFFLE(3, 3, 3, 3));
+        __m128 t0 = _mm_mul_ps(vConstants, m_x);
+
+        vConstants = _mm_shuffle_ps(AEC, AEC, _MM_SHUFFLE(2, 2, 2, 2));
+        t0 = _mm_add_ps(t0, vConstants);
+        t0 = _mm_mul_ps(t0, m_x);
+
+        vConstants = _mm_shuffle_ps(AEC, AEC, _MM_SHUFFLE(1, 1, 1, 1));
+        t0 = _mm_add_ps(t0, vConstants);
+        t0 = _mm_mul_ps(t0, m_x);
+
+        vConstants = _mm_shuffle_ps(AEC, AEC, _MM_SHUFFLE(0, 0, 0, 0));
+        t0 = _mm_add_ps(t0, vConstants);
+        t0 = _mm_mul_ps(t0, root);
+
+        __m128 t1 = _mm_sub_ps(Vector::Pi, t0);
+        t0 = _mm_and_ps(nonnegative, t0);
+        t1 = _mm_andnot_ps(nonnegative, t1);
+        t0 = _mm_or_ps(t0, t1);
+        t0 = _mm_sub_ps(Vector::PiDivTwo, t0);
+        return t0;
+    }
+
+    FORCE_INLINE Vector Vector::ACosEst(const Vector& vec)
+    {
+        __m128 nonnegative = _mm_cmpge_ps(vec, Vector::Zero);
+        __m128 mvalue = _mm_sub_ps(Vector::Zero, vec);
+        __m128 m_x = _mm_max_ps(vec, mvalue);  // |vec|
+
+        // Compute (1-|vec|), clamp to zero to avoid sqrt of negative number.
+        __m128 oneMValue = _mm_sub_ps(Vector::One, m_x);
+        __m128 clampOneMValue = _mm_max_ps(Vector::Zero, oneMValue);
+        __m128 root = _mm_sqrt_ps(clampOneMValue);  // sqrt(1-|vec|)
+
+        // Compute polynomial approximation
+        auto vConstants = _mm_shuffle_ps(SIMD::g_arcEstCoefficients, SIMD::g_arcEstCoefficients, _MM_SHUFFLE(3, 3, 3, 3));
+        __m128 t0 = _mm_mul_ps(vConstants, m_x);
+
+        vConstants = _mm_shuffle_ps(SIMD::g_arcEstCoefficients, SIMD::g_arcEstCoefficients, _MM_SHUFFLE(2, 2, 2, 2));
+        t0 = _mm_add_ps(t0, vConstants);
+        t0 = _mm_mul_ps(t0, m_x);
+
+        vConstants = _mm_shuffle_ps(SIMD::g_arcEstCoefficients, SIMD::g_arcEstCoefficients, _MM_SHUFFLE(1, 1, 1, 1));
+        t0 = _mm_add_ps(t0, vConstants);
+        t0 = _mm_mul_ps(t0, m_x);
+
+        vConstants = _mm_shuffle_ps(SIMD::g_arcEstCoefficients, SIMD::g_arcEstCoefficients, _MM_SHUFFLE(0, 0, 0, 0));
+        t0 = _mm_add_ps(t0, vConstants);
+        t0 = _mm_mul_ps(t0, root);
+
+        __m128 t1 = _mm_sub_ps(Vector::Pi, t0);
+        t0 = _mm_and_ps(nonnegative, t0);
+        t1 = _mm_andnot_ps(nonnegative, t1);
+        t0 = _mm_or_ps(t0, t1);
+        return t0;
+    }
+
+    FORCE_INLINE Vector Vector::ATanEst(const Vector& vec)
+    {
+        __m128 absV = vec.GetAbs();
+        __m128 invV = _mm_div_ps(Vector::One, vec);
+        __m128 comp = _mm_cmpgt_ps(vec, Vector::One);
+        __m128 select0 = _mm_and_ps(comp, Vector::One);
+        __m128 select1 = _mm_andnot_ps(comp, Vector::NegativeOne);
+        __m128 sign = _mm_or_ps(select0, select1);
+        comp = _mm_cmple_ps(absV, Vector::One);
+        select0 = _mm_and_ps(comp, Vector::Zero);
+        select1 = _mm_andnot_ps(comp, sign);
+        sign = _mm_or_ps(select0, select1);
+        select0 = _mm_and_ps(comp, vec);
+        select1 = _mm_andnot_ps(comp, invV);
+        __m128 m_x = _mm_or_ps(select0, select1);
+
+        __m128 x2 = _mm_mul_ps(m_x, m_x);
+
+        // Compute polynomial approximation
+        Vector const AEC = SIMD::g_aTanEstCoefficients1;
+        Vector vConstants = _mm_shuffle_ps(AEC, AEC, _MM_SHUFFLE(3, 3, 3, 3));
+        __m128 Result = _mm_mul_ps(vConstants, x2);
+
+        vConstants = _mm_shuffle_ps(AEC, AEC, _MM_SHUFFLE(2, 2, 2, 2));
+        Result = _mm_add_ps(Result, vConstants);
+        Result = _mm_mul_ps(Result, x2);
+
+        vConstants = _mm_shuffle_ps(AEC, AEC, _MM_SHUFFLE(1, 1, 1, 1));
+        Result = _mm_add_ps(Result, vConstants);
+        Result = _mm_mul_ps(Result, x2);
+
+        vConstants = _mm_shuffle_ps(AEC, AEC, _MM_SHUFFLE(0, 0, 0, 0));
+        Result = _mm_add_ps(Result, vConstants);
+        Result = _mm_mul_ps(Result, x2);
+
+        // ATanEstCoefficients0 is already splatted
+        Result = _mm_add_ps(Result, SIMD::g_aTanEstCoefficients0);
+        Result = _mm_mul_ps(Result, m_x);
+        __m128 result1 = _mm_mul_ps(sign, Vector::PiDivTwo);
+        result1 = _mm_sub_ps(result1, Result);
+
+        comp = _mm_cmpeq_ps(sign, Vector::Zero);
+        select0 = _mm_and_ps(comp, Result);
+        select1 = _mm_andnot_ps(comp, result1);
+        Result = _mm_or_ps(select0, select1);
+        return Result;
+    }
+
+    FORCE_INLINE Vector Vector::ATan2Est(const Vector& X, const Vector& Y)
+    {
+        Vector ATanResultValid = Vector(SIMD::g_trueMask);
+
+        Vector vPi = Vector(SIMD::g_aTan2Constants).GetSplatX();
+        Vector vPiOverTwo = Vector(SIMD::g_aTan2Constants).GetSplatY();
+        Vector vPiOverFour = Vector(SIMD::g_aTan2Constants).GetSplatZ();
+        Vector vThreePiOverFour = Vector(SIMD::g_aTan2Constants).GetSplatW();
+
+        Vector YEqualsZero = Y.EqualsZero();
+        Vector XEqualsZero = X.EqualsZero();
+        Vector XIsPositive = _mm_and_ps(X, SIMD::g_signMask);
+        XIsPositive = _mm_castsi128_ps(_mm_cmpeq_epi32(_mm_castps_si128(XIsPositive), _mm_castps_si128(Vector::Zero)));
+        Vector YEqualsInfinity = Y.EqualsInfinity();
+        Vector XEqualsInfinity = X.EqualsInfinity();
+
+        Vector YSign = _mm_and_ps(Y, SIMD::g_signMask);
+        vPi = _mm_castsi128_ps(_mm_or_si128(_mm_castps_si128(vPi), _mm_castps_si128(YSign)));
+        vPiOverTwo = _mm_castsi128_ps(_mm_or_si128(_mm_castps_si128(vPiOverTwo), _mm_castps_si128(YSign)));
+        vPiOverFour = _mm_castsi128_ps(_mm_or_si128(_mm_castps_si128(vPiOverFour), _mm_castps_si128(YSign)));
+        vThreePiOverFour = _mm_castsi128_ps(_mm_or_si128(_mm_castps_si128(vThreePiOverFour), _mm_castps_si128(YSign)));
+
+        Vector R1 = Vector::Select(vPi, YSign, XIsPositive);
+        Vector R2 = Vector::Select(ATanResultValid, vPiOverTwo, XEqualsZero);
+        Vector R3 = Vector::Select(R2, R1, YEqualsZero);
+        Vector R4 = Vector::Select(vThreePiOverFour, vPiOverFour, XIsPositive);
+        Vector R5 = Vector::Select(vPiOverTwo, R4, XEqualsInfinity);
+        Vector Result = Vector::Select(R3, R5, YEqualsInfinity);
+        ATanResultValid = _mm_castsi128_ps(_mm_cmpeq_epi32(_mm_castps_si128(Result), _mm_castps_si128(ATanResultValid)));
+
+        Vector Reciprocal = X.GetInverseEst();
+        Vector V = Y * Reciprocal;
+        Vector R0 = Vector::ATanEst(V);
+
+        R1 = Vector::Select(vPi, Vector(SIMD::g_signMask), XIsPositive);
+        R2 = R0 + R1;
+        Result = Vector::Select(Result, R2, ATanResultValid);
+
+        return Result;
+    }
+
+    FORCE_INLINE void Vector::SinCos(Vector& sin, Vector& cos, float angle)
+    {
+        return SinCos(sin, cos, Vector(angle));
+    }
+
+    FORCE_INLINE void Vector::SinCos(Vector& sin, Vector& cos, const Vector& angle)
+    {
+        // Force the value within the bounds of pi
+        auto m_x = Vector::AngleMod2Pi(angle);
+
+        // Map in [-pi/2,pi/2] with sin(m_y) = sin(m_x), cos(m_y) = sign*cos(m_x).
+        auto sign = _mm_and_ps(m_x, SIMD::g_signMask);
+        __m128 c = _mm_or_ps(Vector::Pi, sign);  // pi when m_x >= 0, -pi when m_x < 0
+        __m128 absx = _mm_andnot_ps(sign, m_x);  // |m_x|
+        __m128 rflx = _mm_sub_ps(c, m_x);
+        __m128 comp = _mm_cmple_ps(absx, Vector::PiDivTwo);
+        __m128 select0 = _mm_and_ps(comp, m_x);
+        __m128 select1 = _mm_andnot_ps(comp, rflx);
+        m_x = _mm_or_ps(select0, select1);
+        select0 = _mm_and_ps(comp, Vector::One);
+        select1 = _mm_andnot_ps(comp, Vector::NegativeOne);
+        sign = _mm_or_ps(select0, select1);
+
+        __m128 x2 = _mm_mul_ps(m_x, m_x);
+
+        // Compute polynomial approximation of sine
+        const auto SC1 = SIMD::g_sinCoefficients1;
+        auto vConstants = _mm_shuffle_ps(SC1, SC1, _MM_SHUFFLE(0, 0, 0, 0));
+        __m128 Result = _mm_mul_ps(vConstants, x2);
+
+        const auto SC0 = SIMD::g_sinCoefficients0;
+        vConstants = _mm_shuffle_ps(SC0, SC0, _MM_SHUFFLE(3, 3, 3, 3));
+        Result = _mm_add_ps(Result, vConstants);
+        Result = _mm_mul_ps(Result, x2);
+
+        vConstants = _mm_shuffle_ps(SC0, SC0, _MM_SHUFFLE(2, 2, 2, 2));
+        Result = _mm_add_ps(Result, vConstants);
+        Result = _mm_mul_ps(Result, x2);
+
+        vConstants = _mm_shuffle_ps(SC0, SC0, _MM_SHUFFLE(1, 1, 1, 1));
+        Result = _mm_add_ps(Result, vConstants);
+        Result = _mm_mul_ps(Result, x2);
+
+        vConstants = _mm_shuffle_ps(SC0, SC0, _MM_SHUFFLE(0, 0, 0, 0));
+        Result = _mm_add_ps(Result, vConstants);
+        Result = _mm_mul_ps(Result, x2);
+        Result = _mm_add_ps(Result, Vector::One);
+        Result = _mm_mul_ps(Result, m_x);
+        sin = Result;
+
+        // Compute polynomial approximation of cosine
+        const auto CC1 = SIMD::g_cosCoefficients1;
+        vConstants = _mm_shuffle_ps(CC1, CC1, _MM_SHUFFLE(0, 0, 0, 0));
+        Result = _mm_mul_ps(vConstants, x2);
+
+        const auto CC0 = SIMD::g_cosCoefficients0;
+        vConstants = _mm_shuffle_ps(CC0, CC0, _MM_SHUFFLE(3, 3, 3, 3));
+        Result = _mm_add_ps(Result, vConstants);
+        Result = _mm_mul_ps(Result, x2);
+
+        vConstants = _mm_shuffle_ps(CC0, CC0, _MM_SHUFFLE(2, 2, 2, 2));
+        Result = _mm_add_ps(Result, vConstants);
+        Result = _mm_mul_ps(Result, x2);
+
+        vConstants = _mm_shuffle_ps(CC0, CC0, _MM_SHUFFLE(1, 1, 1, 1));
+        Result = _mm_add_ps(Result, vConstants);
+        Result = _mm_mul_ps(Result, x2);
+
+        vConstants = _mm_shuffle_ps(CC0, CC0, _MM_SHUFFLE(0, 0, 0, 0));
+        Result = _mm_add_ps(Result, vConstants);
+        Result = _mm_mul_ps(Result, x2);
+        Result = _mm_add_ps(Result, Vector::One);
+        Result = _mm_mul_ps(Result, sign);
+        cos = Result;
+    }
+
+    FORCE_INLINE Vector Vector::AngleMod2Pi(const Vector& angles)
+    {
+        // Modulo the range of the given angles such that -Pi <= Angles < Pi
+        Vector result = _mm_mul_ps(angles, Vector::OneDivTwoPi);
+        result.Round();
+        result = _mm_mul_ps(result, Vector::TwoPi);
+        result = _mm_sub_ps(angles, result);
+        return result;
+    }
+
+    FORCE_INLINE Vector::operator __m128& ()
+    {
+        return m_data;
+    }
+
+    FORCE_INLINE Vector::operator const __m128& () const
+    {
+        return m_data;
+    }
+
+    FORCE_INLINE Vector::Vector()
+    {
+    }
+
+    FORCE_INLINE Vector::Vector(Axis axis)
+    {
+        switch (axis)
+        {
+        case Axis::X: *this = Vector::UnitX; break;
+        case Axis::Y: *this = Vector::UnitY; break;
+        case Axis::Z: *this = Vector::UnitZ; break;
+        default: HALT(); break;
+        }
+    }
+
+    FORCE_INLINE Vector::Vector(ZeroInit_t)
+    {
+        memset(this, 0, sizeof(Vector));
+    }
+
+    FORCE_INLINE Vector::Vector(float v)
+    {
+        m_data = _mm_set1_ps(v);
+    }
+
+    FORCE_INLINE Vector::Vector(__m128 v)
+        : m_data(v)
+    {
+    }
+
+    FORCE_INLINE Vector::Vector(float ix, float iy, float iz, float iw)
+    {
+        m_data = _mm_set_ps(iw, iz, iy, ix);
+    }
+
+    FORCE_INLINE Vector::Vector(const Float2& v, float iz, float iw)
+    {
+        m_data = _mm_set_ps(iw, iz, v.m_y, v.m_x);
+    }
+
+    FORCE_INLINE Vector::Vector(const Float3& v, float iw)
+    {
+        m_data = _mm_set_ps(iw, v.m_z, v.m_y, v.m_x);
+    }
+
+    FORCE_INLINE Vector::Vector(const Float4& v)
+    {
+        m_data = _mm_loadu_ps(&v.m_x);
+    }
+
+    FORCE_INLINE Vector::Vector(const float* pValues)
+    {
+        m_data = _mm_loadu_ps(pValues);
+    }
+
+    FORCE_INLINE bool Vector::IsValid() const
+    {
+        return !IsNaN4() && !IsInfinite4();
+    }
+
+    FORCE_INLINE void Vector::Store(float* pValues) const
+    {
+        _mm_storeu_ps(pValues, m_data);
+    }
+
+    FORCE_INLINE void Vector::StoreFloat(float& value) const
+    {
+        _mm_store_ss(&value, m_data);
+    }
+
+    FORCE_INLINE void Vector::StoreFloat2(Float2& value) const
+    {
+        auto yVec = _mm_shuffle_ps(m_data, m_data, _MM_SHUFFLE(1, 1, 1, 1));
+        _mm_store_ss(&value.m_x, m_data);
+        _mm_store_ss(&value.m_y, yVec);
+    }
+
+    FORCE_INLINE void Vector::StoreFloat3(Float3& value) const
+    {
+        auto yVec = _mm_shuffle_ps(m_data, m_data, _MM_SHUFFLE(1, 1, 1, 1));
+        auto zVec = _mm_shuffle_ps(m_data, m_data, _MM_SHUFFLE(2, 2, 2, 2));
+        _mm_store_ss(&value.m_x, m_data);
+        _mm_store_ss(&value.m_y, yVec);
+        _mm_store_ss(&value.m_z, zVec);
+    }
+
+    FORCE_INLINE void Vector::StoreFloat4(Float4& value) const
+    {
+        _mm_storeu_ps(&value.m_x, m_data);
+    }
+
+    FORCE_INLINE float Vector::ToFloat() const
+    {
+        float v;
+        StoreFloat(v);
+        return v;
+    }
+
+    FORCE_INLINE Float2 Vector::ToFloat2() const
+    {
+        Float2 v;
+        StoreFloat2(v);
+        return v;
+    }
+
+    FORCE_INLINE Float3 Vector::ToFloat3() const
+    {
+        Float3 v;
+        StoreFloat3(v);
+        return v;
+    }
+
+    FORCE_INLINE Float4 Vector::ToFloat4() const
+    {
+        Float4 v;
+        StoreFloat4(v);
+        return v;
+    }
+
+    FORCE_INLINE Vector::operator Float2() const
+    {
+        return ToFloat2();
+    }
+
+    FORCE_INLINE Vector::operator Float3() const
+    {
+        return ToFloat3();
+    }
+
+    FORCE_INLINE Vector::operator Float4() const
+    {
+        return ToFloat4();
+    }
+
+    FORCE_INLINE float Vector::GetX() const
+    {
+        return _mm_cvtss_f32(m_data);
+    }
+
+    FORCE_INLINE float Vector::GetY() const
+    {
+        auto vTemp = GetSplatY();
+        return _mm_cvtss_f32(vTemp);
+    }
+
+    FORCE_INLINE float Vector::GetZ() const
+    {
+        auto vTemp = GetSplatZ();
+        return _mm_cvtss_f32(vTemp);
+    }
+
+    FORCE_INLINE float Vector::GetW() const
+    {
+        auto vTemp = GetSplatW();
+        return _mm_cvtss_f32(vTemp);
+    }
+
+    FORCE_INLINE void Vector::SetX(float x)
+    {
+        m_data = _mm_move_ss(m_data, _mm_set_ss(x));
+    }
+
+    FORCE_INLINE void Vector::SetY(float y)
+    {
+        m_data = _mm_insert_ps(m_data, _mm_set_ss(y), 0x10);
+    }
+
+    FORCE_INLINE void Vector::SetZ(float z)
+    {
+        m_data = _mm_insert_ps(m_data, _mm_set_ss(z), 0x20);
+    }
+
+    FORCE_INLINE void Vector::SetW(float w)
+    {
+        m_data = _mm_insert_ps(m_data, _mm_set_ss(w), 0x30);
+    }
+
+    FORCE_INLINE float Vector::operator[](uint32_t i) const
+    {
+        ASSERT(i < 4);
+
+        switch (i)
+        {
+        case 0: return GetX(); break;
+        case 1: return GetY(); break;
+        case 2: return GetZ(); break;
+        case 3: return GetW(); break;
+        }
+
+        UNREACHABLE_CODE();
+        return 0.0f;
+    }
+
+    FORCE_INLINE bool Vector::IsW1() const
+    {
+        return GetSplatW().IsEqual4(Vector::One);
+    }
+
+    FORCE_INLINE bool Vector::IsW0() const
+    {
+        return GetSplatW().IsZero4();
+    }
+
+    FORCE_INLINE Vector& Vector::SetW0()
+    {
+        SetW(0.0f);
+        return *this;
+    }
+
+    FORCE_INLINE Vector& Vector::SetW1()
+    {
+        SetW(1.0f);
+        return *this;
+    }
+
+    FORCE_INLINE Vector Vector::GetWithW0() const
+    {
+        Vector v = *this;
+        v.SetW0();
+        return v;
+    }
+
+    FORCE_INLINE Vector Vector::GetWithW1() const
+    {
+        Vector v = *this;
+        v.SetW1();
+        return v;
+    }
+
+    FORCE_INLINE Vector Vector::Get2D() const
+    {
+        return Vector::Select(*this, Vector::Zero, Vector::Select0011);
+    }
+
+    FORCE_INLINE Vector Vector::Get3D() const
+    {
+        return Vector::Select(*this, Vector::Zero, Vector::Select0001);
+    }
+
+    FORCE_INLINE Vector Vector::operator+(const Vector& v) const
+    {
+        return _mm_add_ps(m_data, v);
+    }
+
+    FORCE_INLINE Vector& Vector::operator+=(const Vector& v)
+    {
+        m_data = _mm_add_ps(m_data, v);
+        return *this;
+    }
+
+    FORCE_INLINE Vector Vector::operator-(const Vector& v) const
+    {
+        return _mm_sub_ps(m_data, v);
+    }
+
+    FORCE_INLINE Vector& Vector::operator-=(const Vector& v)
+    {
+        m_data = _mm_sub_ps(m_data, v);
+        return *this;
+    }
+
+    FORCE_INLINE Vector Vector::operator*(const Vector& v) const
+    {
+        return _mm_mul_ps(m_data, v);
+    }
+
+    FORCE_INLINE Vector& Vector::operator*=(const Vector& v)
+    {
+        m_data = _mm_mul_ps(m_data, v);
+        return *this;
+    }
+
+    FORCE_INLINE Vector Vector::operator/(const Vector& v) const
+    {
+        return _mm_div_ps(m_data, v);
+    }
+
+    FORCE_INLINE Vector& Vector::operator/=(const Vector& v)
+    {
+        m_data = _mm_div_ps(m_data, v);
+        return *this;
+    }
+
+    FORCE_INLINE Vector Vector::operator*(float const f) const
+    {
+        return operator*(Vector(f));
+    }
+
+    FORCE_INLINE Vector& Vector::operator*=(float const f)
+    {
+        return operator*=(Vector(f));
+    }
+
+    FORCE_INLINE Vector Vector::operator/(float const f) const
+    {
+        return operator/(Vector(f));
+    }
+
+    FORCE_INLINE Vector& Vector::operator/=(float const f)
+    {
+        return operator/=(Vector(f));
+    }
+
+    FORCE_INLINE Vector Vector::operator-() const
+    {
+        return GetNegated();
+    }
+
+    FORCE_INLINE Vector Vector::Orthogonal2D() const
+    {
+        static Vector const negX(-1.0f, 1.0f, 1.0f, 1.0f);
+
+        Vector result;
+        result = _mm_shuffle_ps(*this, *this, _MM_SHUFFLE(3, 2, 0, 1));
+        result = _mm_mul_ps(result, negX);
+        return result;
+    }
+
+    FORCE_INLINE Vector Vector::Cross2(const Vector& other) const
+    {
+        Vector vResult = _mm_shuffle_ps(other.m_data, other.m_data, _MM_SHUFFLE(0, 1, 0, 1));
+        vResult = _mm_mul_ps(vResult, m_data);
+        Vector vTemp = vResult.GetSplatY();
+        vResult = _mm_sub_ss(vResult, vTemp);
+        vResult = vResult.GetSplatX();
+        return vResult;
+    }
+
+    FORCE_INLINE Vector Vector::Cross3(const Vector& other) const
+    {
+        auto vTemp1 = _mm_shuffle_ps(m_data, m_data, _MM_SHUFFLE(3, 0, 2, 1));
+        auto vTemp2 = _mm_shuffle_ps(other, other, _MM_SHUFFLE(3, 1, 0, 2));
+        Vector result = _mm_mul_ps(vTemp1, vTemp2);
+        vTemp1 = _mm_shuffle_ps(vTemp1, vTemp1, _MM_SHUFFLE(3, 0, 2, 1));
+        vTemp2 = _mm_shuffle_ps(vTemp2, vTemp2, _MM_SHUFFLE(3, 1, 0, 2));
+        vTemp1 = _mm_mul_ps(vTemp1, vTemp2);
+        result = _mm_sub_ps(result, vTemp1);
+        result = _mm_and_ps(result, SIMD::g_maskXYZ0);
+        return result;
+    }
+
+    FORCE_INLINE Vector Vector::Dot2(const Vector& other) const
+    {
+        // Perform the dot product on m_x and m_y
+        Vector result = _mm_mul_ps(m_data, other);
+        // vTemp has m_y splatted
+        auto vTemp = _mm_shuffle_ps(result, result, _MM_SHUFFLE(1, 1, 1, 1));
+        // m_x+m_y
+        result = _mm_add_ss(result, vTemp);
+        result = _mm_shuffle_ps(result, result, _MM_SHUFFLE(0, 0, 0, 0));
+        return result;
+    }
+
+    FORCE_INLINE Vector Vector::Dot3(const Vector& vOther) const
+    {
+        // Perform the dot product
+        auto vDot = _mm_mul_ps(m_data, vOther);
+        // m_x=Dot.vector4_f32[1], m_y=Dot.vector4_f32[2]
+        auto vTemp = _mm_shuffle_ps(vDot, vDot, _MM_SHUFFLE(2, 1, 2, 1));
+        // Result.vector4_f32[0] = m_x+m_y
+        vDot = _mm_add_ss(vDot, vTemp);
+        // m_x=Dot.vector4_f32[2]
+        vTemp = _mm_shuffle_ps(vTemp, vTemp, _MM_SHUFFLE(1, 1, 1, 1));
+        // Result.vector4_f32[0] = (m_x+m_y)+m_z
+        vDot = _mm_add_ss(vDot, vTemp);
+        // Splat m_x
+        Vector result = _mm_shuffle_ps(vDot, vDot, _MM_SHUFFLE(0, 0, 0, 0));
+        return result;
+    }
+
+    FORCE_INLINE Vector Vector::Dot4(const Vector& other) const
+    {
+        auto vTemp2 = other;
+        auto vTemp = _mm_mul_ps(m_data, vTemp2);
+        vTemp2 = _mm_shuffle_ps(vTemp2, vTemp, _MM_SHUFFLE(1, 0, 0, 0)); // Copy X to the Z position and Y to the W position
+        vTemp2 = _mm_add_ps(vTemp2, vTemp); // Add Z = X+Z; W = Y+W;
+        vTemp = _mm_shuffle_ps(vTemp, vTemp2, _MM_SHUFFLE(0, 3, 0, 0));  // Copy W to the Z position
+        vTemp = _mm_add_ps(vTemp, vTemp2); // Add Z and W together
+        return _mm_shuffle_ps(vTemp, vTemp, _MM_SHUFFLE(2, 2, 2, 2)); // Splat Z and return
+    }
+
+    FORCE_INLINE float Vector::GetDot2(const Vector& other) const
+    {
+        return Dot2(other).ToFloat();
+    }
+
+    FORCE_INLINE float Vector::GetDot3(const Vector& other) const
+    {
+        return Dot3(other).ToFloat();
+    }
+
+    FORCE_INLINE float Vector::GetDot4(const Vector& other) const
+    {
+        return Dot4(other).ToFloat();
+    }
+
+    FORCE_INLINE Vector Vector::ScalarProjection(const Vector& other) const
+    {
+        Vector const normalizedThis = GetNormalized3();
+        Vector const projection = other.Dot3(normalizedThis);
+        return projection;
+    }
+
+    FORCE_INLINE float Vector::GetScalarProjection(const Vector& other) const
+    {
+        return ScalarProjection(other).ToFloat();
+    }
+
+    FORCE_INLINE Vector Vector::VectorProjection(const Vector& other) const
+    {
+        Vector const normalizedThis = GetNormalized3();
+        Vector const dotOther = other.Dot3(normalizedThis);
+        Vector const projection = normalizedThis * dotOther;
+        return projection;
+    }
+
+    FORCE_INLINE Vector& Vector::Invert()
+    {
+        m_data = _mm_div_ps(Vector::One, m_data);
+        return *this;
+    }
+
+    FORCE_INLINE Vector Vector::GetInverse() const
+    {
+        return _mm_div_ps(Vector::One, m_data);
+    }
+
+    FORCE_INLINE Vector Vector::GetReciprocal() const
+    {
+        return GetInverse();
+    }
+
+    FORCE_INLINE Vector& Vector::InvertEst()
+    {
+        m_data = _mm_rcp_ps(m_data);
+        return *this;
+    }
+
+    FORCE_INLINE Vector Vector::GetInverseEst() const
+    {
+        return _mm_rcp_ps(m_data);
+    }
+
+    FORCE_INLINE Vector& Vector::Negate()
+    {
+        m_data = _mm_sub_ps(Vector::Zero, m_data);
+        return *this;
+    }
+
+    FORCE_INLINE Vector Vector::GetNegated() const
+    {
+        return _mm_sub_ps(Vector::Zero, m_data);
+    }
+
+    FORCE_INLINE Vector& Vector::Abs()
+    {
+        m_data = _mm_max_ps(_mm_sub_ps(Vector::Zero, m_data), m_data);
+        return *this;
+    }
+
+    FORCE_INLINE Vector Vector::GetAbs() const
+    {
+        return _mm_max_ps(_mm_sub_ps(Vector::Zero, m_data), m_data);
+    }
+
+    FORCE_INLINE Vector& Vector::Sqrt()
+    {
+        m_data = _mm_sqrt_ps(m_data);
+        return *this;
+    }
+
+    FORCE_INLINE Vector Vector::GetSqrt()
+    {
+        return _mm_sqrt_ps(m_data);
+    }
+
+    FORCE_INLINE Vector& Vector::ReciprocalSqrt()
+    {
+        m_data = _mm_div_ps(Vector::One, _mm_sqrt_ps(m_data));
+        return *this;
+    }
+
+    FORCE_INLINE Vector Vector::GetReciprocalSqrt()
+    {
+        return _mm_div_ps(Vector::One, _mm_sqrt_ps(m_data));
+    }
+
+    FORCE_INLINE Vector& Vector::EstimatedReciprocalSqrt()
+    {
+        m_data = _mm_rsqrt_ps(m_data);
+        return *this;
+    }
+
+    FORCE_INLINE Vector Vector::GetEstimatedReciprocalSqrt()
+    {
+        return _mm_rsqrt_ps(m_data);
+    }
+
+    FORCE_INLINE Vector& Vector::Normalize2()
+    {
+        // Perform the dot product on m_x and m_y only
+        auto vLengthSq = _mm_mul_ps(m_data, m_data);
+        auto vTemp = _mm_shuffle_ps(vLengthSq, vLengthSq, _MM_SHUFFLE(1, 1, 1, 1));
+        vLengthSq = _mm_add_ss(vLengthSq, vTemp);
+        vLengthSq = _mm_shuffle_ps(vLengthSq, vLengthSq, _MM_SHUFFLE(0, 0, 0, 0));
+        // Prepare for the division
+        auto vResult = _mm_sqrt_ps(vLengthSq);
+        // Create zero with a single instruction
+        auto vZeroMask = _mm_setzero_ps();
+        // Test for a divide by zero (Must be FP to detect -0.0)
+        vZeroMask = _mm_cmpneq_ps(vZeroMask, vResult);
+        // Failsafe on zero (Or epsilon) length planes
+        // If the length is infinity, set the elements to zero
+        vLengthSq = _mm_cmpneq_ps(vLengthSq, Vector::Infinity);
+        // Divide to perform the normalization
+        vResult = _mm_div_ps(m_data, vResult);
+        // Any that are infinity, set to zero
+        vResult = _mm_and_ps(vResult, vZeroMask);
+        // Select qnan or result based on infinite length
+        auto vTemp1 = _mm_andnot_ps(vLengthSq, Vector::QNaN);
+        auto vTemp2 = _mm_and_ps(vResult, vLengthSq);
+        m_data = _mm_or_ps(vTemp1, vTemp2);
+
+        *this = Select(*this, Vector::Zero, Select0011);
+
+        return *this;
+    }
+
+    FORCE_INLINE Vector& Vector::Normalize3()
+    {
+        // Perform the dot product on m_x,m_y and m_z only
+        auto vLengthSq = _mm_mul_ps(m_data, m_data);
+        auto vTemp = _mm_shuffle_ps(vLengthSq, vLengthSq, _MM_SHUFFLE(2, 1, 2, 1));
+        vLengthSq = _mm_add_ss(vLengthSq, vTemp);
+        vTemp = _mm_shuffle_ps(vTemp, vTemp, _MM_SHUFFLE(1, 1, 1, 1));
+        vLengthSq = _mm_add_ss(vLengthSq, vTemp);
+        vLengthSq = _mm_shuffle_ps(vLengthSq, vLengthSq, _MM_SHUFFLE(0, 0, 0, 0));
+        // Prepare for the division
+        auto vResult = _mm_sqrt_ps(vLengthSq);
+        // Create zero with a single instruction
+        auto vZeroMask = _mm_setzero_ps();
+        // Test for a divide by zero (Must be FP to detect -0.0)
+        vZeroMask = _mm_cmpneq_ps(vZeroMask, vResult);
+        // Failsafe on zero (Or epsilon) length planes
+        // If the length is infinity, set the elements to zero
+        vLengthSq = _mm_cmpneq_ps(vLengthSq, Vector::Infinity);
+        // Divide to perform the normalization
+        vResult = _mm_div_ps(m_data, vResult);
+        // Any that are infinity, set to zero
+        vResult = _mm_and_ps(vResult, vZeroMask);
+        // Select qnan or result based on infinite length
+        auto vTemp1 = _mm_andnot_ps(vLengthSq, Vector::QNaN);
+        auto vTemp2 = _mm_and_ps(vResult, vLengthSq);
+        m_data = _mm_or_ps(vTemp1, vTemp2);
+
+        *this = Select(*this, Vector::Zero, Select0001);
+
+        return *this;
+    }
+
+    FORCE_INLINE Vector& Vector::Normalize4()
+    {
+        // Perform the dot product on m_x,m_y,m_z and m_w
+        auto vLengthSq = _mm_mul_ps(m_data, m_data);
+        // vTemp has m_z and m_w
+        auto vTemp = _mm_shuffle_ps(vLengthSq, vLengthSq, _MM_SHUFFLE(3, 2, 3, 2));
+        // m_x+m_z, m_y+m_w
+        vLengthSq = _mm_add_ps(vLengthSq, vTemp);
+        // m_x+m_z,m_x+m_z,m_x+m_z,m_y+m_w
+        vLengthSq = _mm_shuffle_ps(vLengthSq, vLengthSq, _MM_SHUFFLE(1, 0, 0, 0));
+        // ??,??,m_y+m_w,m_y+m_w
+        vTemp = _mm_shuffle_ps(vTemp, vLengthSq, _MM_SHUFFLE(3, 3, 0, 0));
+        // ??,??,m_x+m_z+m_y+m_w,??
+        vLengthSq = _mm_add_ps(vLengthSq, vTemp);
+        // Splat the length
+        vLengthSq = _mm_shuffle_ps(vLengthSq, vLengthSq, _MM_SHUFFLE(2, 2, 2, 2));
+        // Prepare for the division
+        auto vResult = _mm_sqrt_ps(vLengthSq);
+        // Create zero with a single instruction
+        auto vZeroMask = _mm_setzero_ps();
+        // Test for a divide by zero (Must be FP to detect -0.0)
+        vZeroMask = _mm_cmpneq_ps(vZeroMask, vResult);
+        // Failsafe on zero (Or epsilon) length planes
+        // If the length is infinity, set the elements to zero
+        vLengthSq = _mm_cmpneq_ps(vLengthSq, Vector::Infinity);
+        // Divide to perform the normalization
+        vResult = _mm_div_ps(m_data, vResult);
+        // Any that are infinity, set to zero
+        vResult = _mm_and_ps(vResult, vZeroMask);
+        // Select qnan or result based on infinite length
+        auto vTemp1 = _mm_andnot_ps(vLengthSq, Vector::QNaN);
+        auto vTemp2 = _mm_and_ps(vResult, vLengthSq);
+        m_data = _mm_or_ps(vTemp1, vTemp2);
+
+        return *this;
+    }
+
+    FORCE_INLINE Vector Vector::GetNormalized2() const
+    {
+        Vector v = *this;
+        v.Normalize2();
+        return v;
+    }
+
+    FORCE_INLINE Vector Vector::GetNormalized3() const
+    {
+        Vector v = *this;
+        v.Normalize3();
+        return v;
+    }
+
+    FORCE_INLINE Vector Vector::GetNormalized4() const
+    {
+        Vector v = *this;
+        v.Normalize4();
+        return v;
+    }
+
+    FORCE_INLINE Vector& Vector::Floor()
+    {
+        Vector result;
+
+        // To handle NAN, INF and numbers greater than 8388608, use masking
+        __m128i vTest = _mm_and_si128(_mm_castps_si128(m_data), SIMD::g_absMask);
+        vTest = _mm_cmplt_epi32(vTest, SIMD::g_noFraction);
+        // Truncate
+        __m128i vInt = _mm_cvttps_epi32(m_data);
+        result = _mm_cvtepi32_ps(vInt);
+        __m128 vLarger = _mm_cmpgt_ps(result, m_data);
+        // 0 -> 0, 0xffffffff -> -1.0f
+        vLarger = _mm_cvtepi32_ps(_mm_castps_si128(vLarger));
+        result = _mm_add_ps(result, vLarger);
+        // All numbers less than 8388608 will use the round to int
+        result = _mm_and_ps(result, _mm_castsi128_ps(vTest));
+        // All others, use the ORIGINAL value
+        vTest = _mm_andnot_si128(vTest, _mm_castps_si128(m_data));
+        result = _mm_or_ps(result, _mm_castsi128_ps(vTest));
+
+        m_data = result;
+        return *this;
+    }
+
+    FORCE_INLINE Vector Vector::GetFloor() const
+    {
+        Vector v = *this;
+        v.Floor();
+        return v;
+    }
+
+    FORCE_INLINE Vector& Vector::Ceil()
+    {
+        Vector result;
+
+        // To handle NAN, INF and numbers greater than 8388608, use masking
+        __m128i vTest = _mm_and_si128(_mm_castps_si128(m_data), SIMD::g_absMask);
+        vTest = _mm_cmplt_epi32(vTest, SIMD::g_noFraction);
+        // Truncate
+        __m128i vInt = _mm_cvttps_epi32(m_data);
+        result = _mm_cvtepi32_ps(vInt);
+        __m128 vSmaller = _mm_cmplt_ps(result, m_data);
+        // 0 -> 0, 0xffffffff -> -1.0f
+        vSmaller = _mm_cvtepi32_ps(_mm_castps_si128(vSmaller));
+        result = _mm_sub_ps(result, vSmaller);
+        // All numbers less than 8388608 will use the round to int
+        result = _mm_and_ps(result, _mm_castsi128_ps(vTest));
+        // All others, use the ORIGINAL value
+        vTest = _mm_andnot_si128(vTest, _mm_castps_si128(m_data));
+        result = _mm_or_ps(result, _mm_castsi128_ps(vTest));
+
+        m_data = result;
+        return *this;
+    }
+
+    FORCE_INLINE Vector Vector::GetCeil() const
+    {
+        Vector v = *this;
+        v.Ceil();
+        return v;
+    }
+
+    FORCE_INLINE Vector& Vector::Round()
+    {
+        __m128 sign = _mm_and_ps(m_data, SIMD::g_signMask);
+        __m128 sMagic = _mm_or_ps(SIMD::g_noFraction, sign);
+        __m128 R1 = _mm_add_ps(m_data, sMagic);
+        R1 = _mm_sub_ps(R1, sMagic);
+        __m128 R2 = _mm_and_ps(m_data, SIMD::g_absMask);
+        __m128 mask = _mm_cmple_ps(R2, SIMD::g_noFraction);
+        R2 = _mm_andnot_ps(mask, m_data);
+        R1 = _mm_and_ps(R1, mask);
+        m_data = _mm_xor_ps(R1, R2);
+        return  *this;
+    }
+
+    FORCE_INLINE Vector Vector::GetRound() const
+    {
+        Vector v = *this;
+        v.Round();
+        return v;
+    }
+
+    FORCE_INLINE Vector Vector::GetSign() const
+    {
+        Vector const selectMask = GreaterThanEqual(Vector::Zero);
+        return Vector::Select(Vector::NegativeOne, Vector::One, selectMask);
+    }
+
+    FORCE_INLINE Vector Vector::GetSplatX() const
+    {
+        return _mm_shuffle_ps(m_data, m_data, _MM_SHUFFLE(0, 0, 0, 0));
+    }
+
+    FORCE_INLINE Vector Vector::GetSplatY() const
+    {
+        return _mm_shuffle_ps(m_data, m_data, _MM_SHUFFLE(1, 1, 1, 1));
+    }
+
+    FORCE_INLINE Vector Vector::GetSplatZ() const
+    {
+        return _mm_shuffle_ps(m_data, m_data, _MM_SHUFFLE(2, 2, 2, 2));
+    }
+
+    FORCE_INLINE Vector Vector::GetSplatW() const
+    {
+        return _mm_shuffle_ps(m_data, m_data, _MM_SHUFFLE(3, 3, 3, 3));
+    }
+
+    template<uint32_t xIdx, uint32_t yIdx, uint32_t zIdx, uint32_t wIdx>
+    FORCE_INLINE Vector Vector::Swizzle() const
+    {
+        static_assert(xIdx < 4, "Element index parameter out of range");
+        static_assert(yIdx < 4, "Element index parameter out of range");
+        static_assert(zIdx < 4, "Element index parameter out of range");
+        static_assert(wIdx < 4, "Element index parameter out of range");
+        return _mm_shuffle_ps(m_data, m_data, _MM_SHUFFLE(wIdx, zIdx, yIdx, xIdx));
+    }
+
+    FORCE_INLINE Vector Vector::Swizzle(uint32_t xIdx, uint32_t yIdx, uint32_t zIdx, uint32_t wIdx) const
+    {
+        ASSERT(xIdx < 4 && yIdx < 4 && zIdx < 4 && wIdx < 4);
+        uint32_t const elem[4] = { xIdx, yIdx, zIdx, wIdx };
+        __m128i vControl = _mm_loadu_si128(reinterpret_cast<const __m128i*>(&elem[0]));
+        return _mm_permutevar_ps(m_data, vControl);
+    }
+
+    FORCE_INLINE Vector Vector::Shuffle(uint32_t xIdx, uint32_t yIdx, uint32_t zIdx, uint32_t wIdx) const
+    {
+        return Swizzle(xIdx, yIdx, zIdx, wIdx);
+    }
+
+    template<uint32_t xIdx, uint32_t yIdx, uint32_t zIdx, uint32_t wIdx>
+    FORCE_INLINE Vector Vector::Shuffle() const
+    {
+        return Swizzle<xIdx, yIdx, zIdx, wIdx>();
+    }
+
+    FORCE_INLINE Vector Vector::Length2() const
+    {
+        Vector result;
+
+        result = _mm_mul_ps(m_data, m_data);
+        auto vTemp = _mm_shuffle_ps(result, result, _MM_SHUFFLE(1, 1, 1, 1));
+        // m_x+m_y
+        result = _mm_add_ss(result, vTemp);
+        result = _mm_shuffle_ps(result, result, _MM_SHUFFLE(0, 0, 0, 0));
+        result = _mm_sqrt_ps(result);
+        return result;
+    }
+
+    FORCE_INLINE Vector Vector::Length3() const
+    {
+        Vector result;
+
+        // Perform the dot product on m_x,m_y and m_z
+        result = _mm_mul_ps(m_data, m_data);
+        // vTemp has m_z and m_y
+        auto vTemp = _mm_shuffle_ps(result, result, _MM_SHUFFLE(1, 2, 1, 2));
+        // m_x+m_z, m_y
+        result = _mm_add_ss(result, vTemp);
+        // m_y,m_y,m_y,m_y
+        vTemp = _mm_shuffle_ps(vTemp, vTemp, _MM_SHUFFLE(1, 1, 1, 1));
+        // m_x+m_z+m_y,??,??,??
+        result = _mm_add_ss(result, vTemp);
+        // Splat the length squared
+        result = _mm_shuffle_ps(result, result, _MM_SHUFFLE(0, 0, 0, 0));
+        // Get the length
+        result = _mm_sqrt_ps(result);
+
+        return result;
+    }
+
+    FORCE_INLINE Vector Vector::Length4() const
+    {
+        Vector result;
+
+        // Perform the dot product on m_x,m_y,m_z and m_w
+        result = _mm_mul_ps(m_data, m_data);
+        // vTemp has m_z and m_w
+        auto vTemp = _mm_shuffle_ps(result, result, _MM_SHUFFLE(3, 2, 3, 2));
+        // m_x+m_z, m_y+m_w
+        result = _mm_add_ps(result, vTemp);
+        // m_x+m_z,m_x+m_z,m_x+m_z,m_y+m_w
+        result = _mm_shuffle_ps(result, result, _MM_SHUFFLE(1, 0, 0, 0));
+        // ??,??,m_y+m_w,m_y+m_w
+        vTemp = _mm_shuffle_ps(vTemp, result, _MM_SHUFFLE(3, 3, 0, 0));
+        // ??,??,m_x+m_z+m_y+m_w,??
+        result = _mm_add_ps(result, vTemp);
+        // Splat the length
+        result = _mm_shuffle_ps(result, result, _MM_SHUFFLE(2, 2, 2, 2));
+        // Get the length
+        result = _mm_sqrt_ps(result);
+
+        return result;
+    }
+
+    FORCE_INLINE float Vector::GetLength2() const
+    {
+        return Length2().GetX();
+    }
+
+    FORCE_INLINE float Vector::GetLength3() const
+    {
+        return Length3().GetX();
+    }
+
+    FORCE_INLINE float Vector::GetLength4() const
+    {
+        return Length4().GetX();
+    }
+
+    FORCE_INLINE Vector Vector::InverseLength2() const
+    {
+        // Perform the dot product on m_x and m_y
+        auto vLengthSq = _mm_mul_ps(m_data, m_data);
+        // vTemp has m_y splatted
+        auto vTemp = _mm_shuffle_ps(vLengthSq, vLengthSq, _MM_SHUFFLE(1, 1, 1, 1));
+        // m_x+m_y
+        vLengthSq = _mm_add_ss(vLengthSq, vTemp);
+        vLengthSq = _mm_sqrt_ss(vLengthSq);
+        vLengthSq = _mm_div_ss(Vector::One, vLengthSq);
+        vLengthSq = _mm_shuffle_ps(vLengthSq, vLengthSq, _MM_SHUFFLE(0, 0, 0, 0));
+        return vLengthSq;
+    }
+
+    FORCE_INLINE Vector Vector::InverseLength3() const
+    {
+        // Perform the dot product
+        auto vDot = _mm_mul_ps(m_data, m_data);
+        // m_x=Dot.m_y, m_y=Dot.m_z
+        auto vTemp = _mm_shuffle_ps(vDot, vDot, _MM_SHUFFLE(2, 1, 2, 1));
+        // Result.m_x = m_x+m_y
+        vDot = _mm_add_ss(vDot, vTemp);
+        // m_x=Dot.m_z
+        vTemp = _mm_shuffle_ps(vTemp, vTemp, _MM_SHUFFLE(1, 1, 1, 1));
+        // Result.m_x = (m_x+m_y)+m_z
+        vDot = _mm_add_ss(vDot, vTemp);
+        // Splat m_x
+        vDot = _mm_shuffle_ps(vDot, vDot, _MM_SHUFFLE(0, 0, 0, 0));
+        // Get the reciprocal
+        vDot = _mm_sqrt_ps(vDot);
+        // Get the reciprocal
+        vDot = _mm_div_ps(Vector::One, vDot);
+        return vDot;
+    }
+
+    FORCE_INLINE Vector Vector::InverseLength4() const
+    {
+        // Perform the dot product on m_x,m_y,m_z and m_w
+        auto vLengthSq = _mm_mul_ps(m_data, m_data);
+        // vTemp has m_z and m_w
+        auto vTemp = _mm_shuffle_ps(vLengthSq, vLengthSq, _MM_SHUFFLE(3, 2, 3, 2));
+        // m_x+m_z, m_y+m_w
+        vLengthSq = _mm_add_ps(vLengthSq, vTemp);
+        // m_x+m_z,m_x+m_z,m_x+m_z,m_y+m_w
+        vLengthSq = _mm_shuffle_ps(vLengthSq, vLengthSq, _MM_SHUFFLE(1, 0, 0, 0));
+        // ??,??,m_y+m_w,m_y+m_w
+        vTemp = _mm_shuffle_ps(vTemp, vLengthSq, _MM_SHUFFLE(3, 3, 0, 0));
+        // ??,??,m_x+m_z+m_y+m_w,??
+        vLengthSq = _mm_add_ps(vLengthSq, vTemp);
+        // Splat the length
+        vLengthSq = _mm_shuffle_ps(vLengthSq, vLengthSq, _MM_SHUFFLE(2, 2, 2, 2));
+        // Get the reciprocal
+        vLengthSq = _mm_sqrt_ps(vLengthSq);
+        // Accurate!
+        vLengthSq = _mm_div_ps(Vector::One, vLengthSq);
+        return vLengthSq;
+    }
+
+    FORCE_INLINE float Vector::GetInverseLength2() const
+    {
+        return InverseLength2().GetX();
+    }
+
+    FORCE_INLINE float Vector::GetInverseLength3() const
+    {
+        return InverseLength3().GetX();
+    }
+
+    FORCE_INLINE float Vector::GetInverseLength4() const
+    {
+        return InverseLength4().GetX();
+    }
+
+    FORCE_INLINE Vector Vector::LengthSquared2() const
+    {
+        return Vector::Dot2(m_data, m_data);
+    }
+
+    FORCE_INLINE Vector Vector::LengthSquared3() const
+    {
+        return Vector::Dot3(m_data, m_data);
+    }
+
+    FORCE_INLINE Vector Vector::LengthSquared4() const
+    {
+        return Vector::Dot4(m_data, m_data);
+    }
+
+    FORCE_INLINE float Vector::GetLengthSquared2() const
+    {
+        return LengthSquared2().GetX();
+    }
+
+    FORCE_INLINE float Vector::GetLengthSquared3() const
+    {
+        return LengthSquared3().GetX();
+    }
+
+    FORCE_INLINE float Vector::GetLengthSquared4() const
+    {
+        return LengthSquared4().GetX();
+    }
+
+    FORCE_INLINE Vector Vector::Distance2(const Vector& to) const
+    {
+        return (to - *this).Length2();
+    }
+
+    FORCE_INLINE Vector Vector::Distance3(const Vector& to) const
+    {
+        return (to - *this).Length3();
+    }
+
+    FORCE_INLINE Vector Vector::Distance4(const Vector& to) const
+    {
+        return (to - *this).Length4();
+    }
+
+    FORCE_INLINE float Vector::GetDistance2(const Vector& to) const
+    {
+        return (to - *this).Length2().GetX();
+    }
+
+    FORCE_INLINE float Vector::GetDistance3(const Vector& to) const
+    {
+        return (to - *this).Length3().GetX();
+    }
+
+    FORCE_INLINE float Vector::GetDistance4(const Vector& to) const
+    {
+        return (to - *this).Length4().GetX();
+    }
+
+    FORCE_INLINE Vector Vector::DistanceSquared2(const Vector& to) const
+    {
+        return (to - *this).LengthSquared2();
+    }
+
+    FORCE_INLINE Vector Vector::DistanceSquared3(const Vector& to) const
+    {
+        return (to - *this).LengthSquared3();
+    }
+
+    FORCE_INLINE Vector Vector::DistanceSquared4(const Vector& to) const
+    {
+        return (to - *this).LengthSquared4();
+    }
+
+    FORCE_INLINE float Vector::GetDistanceSquared2(const Vector& to) const
+    {
+        return (to - *this).GetLengthSquared2();
+    }
+
+    FORCE_INLINE float Vector::GetDistanceSquared3(const Vector& to) const
+    {
+        return (to - *this).GetLengthSquared3();
+    }
+
+    FORCE_INLINE float Vector::GetDistanceSquared4(const Vector& to) const
+    {
+        return (to - *this).GetLengthSquared4();
+    }
+
+    FORCE_INLINE bool Vector::IsNormalized2() const
+    {
+        return (LengthSquared2() - Vector::One).Abs().IsLessThanEqual4(Vector::NormalizeCheckThreshold);
+    }
+
+    FORCE_INLINE bool Vector::IsNormalized3() const
+    {
+        return (LengthSquared3() - Vector::One).Abs().IsLessThanEqual4(Vector::NormalizeCheckThreshold);
+    }
+
+    FORCE_INLINE bool Vector::IsNormalized4() const
+    {
+        return (LengthSquared4() - Vector::One).Abs().IsLessThanEqual4(Vector::NormalizeCheckThreshold);
+    }
+
+    FORCE_INLINE Vector Vector::InBounds(const Vector& bounds) const
+    {
+        // Test if less than or equal
+        auto vTemp1 = _mm_cmple_ps(m_data, bounds);
+        // Negate the bounds
+        auto vTemp2 = _mm_mul_ps(bounds, Vector::NegativeOne);
+        // Test if greater or equal (Reversed)
+        vTemp2 = _mm_cmple_ps(vTemp2, m_data);
+        // Blend answers
+        vTemp1 = _mm_and_ps(vTemp1, vTemp2);
+        return vTemp1;
+    }
+
+    FORCE_INLINE bool Vector::IsInBounds2(const Vector& bounds) const
+    {
+        return ((_mm_movemask_ps(InBounds(bounds)) & 0x3) == 0x3) != 0;
+    }
+
+    FORCE_INLINE bool Vector::IsInBounds3(const Vector& bounds) const
+    {
+        return ((_mm_movemask_ps(InBounds(bounds)) & 0x7) == 0x7) != 0;
+    }
+
+    FORCE_INLINE bool Vector::IsInBounds4(const Vector& bounds) const
+    {
+        return (_mm_movemask_ps(InBounds(bounds)) == 0x0f) != 0;
+    }
+
+    FORCE_INLINE Vector Vector::Equal(const Vector& v) const
+    {
+        return _mm_cmpeq_ps(*this, v);
+    }
+
+    FORCE_INLINE bool Vector::IsEqual2(const Vector& v) const
+    {
+        return (((_mm_movemask_ps(Equal(v)) & 3) == 3) != 0);
+    }
+
+    FORCE_INLINE bool Vector::IsEqual3(const Vector& v) const
+    {
+        return (((_mm_movemask_ps(Equal(v)) & 7) == 7) != 0);
+    }
+
+    FORCE_INLINE bool Vector::IsEqual4(const Vector& v) const
+    {
+        return ((_mm_movemask_ps(Equal(v)) == 0x0f) != 0);
+    }
+
+    FORCE_INLINE Vector Vector::NearEqual(const Vector& v, const Vector& epsilon) const
+    {
+        // Get the difference
+        auto vDelta = _mm_sub_ps(m_data, v);
+        // Get the absolute value of the difference
+        auto vTemp = _mm_setzero_ps();
+        vTemp = _mm_sub_ps(vTemp, vDelta);
+        vTemp = _mm_max_ps(vTemp, vDelta);
+        vTemp = _mm_cmple_ps(vTemp, epsilon);
+        return vTemp;
+    }
+
+    FORCE_INLINE bool Vector::IsNearEqual2(const Vector& v, float epsilon) const
+    {
+        return IsNearEqual2(v, Vector(epsilon));
+    }
+
+    FORCE_INLINE bool Vector::IsNearEqual3(const Vector& v, float epsilon) const
+    {
+        return IsNearEqual3(v, Vector(epsilon));
+    }
+
+    FORCE_INLINE bool Vector::IsNearEqual4(const Vector& v, float epsilon) const
+    {
+        return IsNearEqual4(v, Vector(epsilon));
+    }
+
+    FORCE_INLINE bool Vector::IsNearEqual2(const Vector& v, const Vector& epsilon) const
+    {
+        return (((_mm_movemask_ps(NearEqual(v, epsilon)) & 3) == 0x3) != 0);
+    }
+
+    FORCE_INLINE bool Vector::IsNearEqual3(const Vector& v, const Vector& epsilon) const
+    {
+        return (((_mm_movemask_ps(NearEqual(v, epsilon)) & 7) == 0x7) != 0);
+    }
+
+    FORCE_INLINE bool Vector::IsNearEqual4(const Vector& v, const Vector& epsilon) const
+    {
+        return ((_mm_movemask_ps(NearEqual(v, epsilon)) == 0xf) != 0);
+    }
+
+    FORCE_INLINE Vector Vector::GreaterThan(const Vector& v) const
+    {
+        return _mm_cmpgt_ps(m_data, v);
+    }
+
+    FORCE_INLINE bool Vector::IsAnyGreaterThan(const Vector& v) const
+    {
+        return !GreaterThan(v).IsZero4();
+    }
+
+    FORCE_INLINE bool Vector::IsGreaterThan2(const Vector& v) const
+    {
+        return (((_mm_movemask_ps(GreaterThan(v)) & 3) == 3) != 0);
+    }
+
+    FORCE_INLINE bool Vector::IsGreaterThan3(const Vector& v) const
+    {
+        return (((_mm_movemask_ps(GreaterThan(v)) & 7) == 7) != 0);
+    }
+
+    FORCE_INLINE bool Vector::IsGreaterThan4(const Vector& v) const
+    {
+        return ((_mm_movemask_ps(GreaterThan(v)) == 0x0f) != 0);
+    }
+
+    FORCE_INLINE Vector Vector::GreaterThanEqual(const Vector& v) const
+    {
+        return _mm_cmpge_ps(m_data, v);
+    }
+
+    FORCE_INLINE bool Vector::IsAnyGreaterThanEqual(const Vector& v) const
+    {
+        return !GreaterThanEqual(v).IsZero4();
+    }
+
+    FORCE_INLINE bool Vector::IsGreaterThanEqual2(const Vector& v) const
+    {
+        return ((_mm_movemask_ps(GreaterThanEqual(v)) & 3) == 3) != 0;
+    }
+
+    FORCE_INLINE bool Vector::IsGreaterThanEqual3(const Vector& v) const
+    {
+        return ((_mm_movemask_ps(GreaterThanEqual(v)) & 7) == 7) != 0;
+    }
+
+    FORCE_INLINE bool Vector::IsGreaterThanEqual4(const Vector& v) const
+    {
+        return (_mm_movemask_ps(GreaterThanEqual(v)) == 0x0f) != 0;
+    }
+
+    FORCE_INLINE Vector Vector::LessThan(const Vector& v) const
+    {
+        return _mm_cmplt_ps(m_data, v);
+    }
+
+    FORCE_INLINE bool Vector::IsAnyLessThan(const Vector& v) const
+    {
+        return !LessThan(v).IsZero4();
+    }
+
+    FORCE_INLINE bool Vector::IsLessThan2(const Vector& v) const
+    {
+        return (((_mm_movemask_ps(LessThan(v)) & 3) == 3) != 0);
+    }
+
+    FORCE_INLINE bool Vector::IsLessThan3(const Vector& v) const
+    {
+        return (((_mm_movemask_ps(LessThan(v)) & 7) == 7) != 0);
+    }
+
+    FORCE_INLINE bool Vector::IsLessThan4(const Vector& v) const
+    {
+        return ((_mm_movemask_ps(LessThan(v)) == 0x0f) != 0);
+    }
+
+    FORCE_INLINE Vector Vector::LessThanEqual(const Vector& v) const
+    {
+        return _mm_cmple_ps(m_data, v);
+    }
+
+    FORCE_INLINE bool Vector::IsAnyLessThanEqual(const Vector& v) const
+    {
+        return !LessThanEqual(v).IsZero4();
+    }
+
+    FORCE_INLINE bool Vector::IsLessThanEqual2(const Vector& v) const
+    {
+        return (((_mm_movemask_ps(LessThanEqual(v)) & 3) == 3) != 0);
+    }
+
+    FORCE_INLINE bool Vector::IsLessThanEqual3(const Vector& v) const
+    {
+        return (((_mm_movemask_ps(LessThanEqual(v)) & 7) == 7) != 0);
+    }
+
+    FORCE_INLINE bool Vector::IsLessThanEqual4(const Vector& v) const
+    {
+        return ((_mm_movemask_ps(LessThanEqual(v)) == 0x0f) != 0);
+    }
+
+    FORCE_INLINE Vector Vector::EqualsZero() const
+    {
+        return Equal(Vector::Zero);
+    }
+
+    FORCE_INLINE bool Vector::IsAnyEqualToZero2() const
+    {
+        return !EqualsZero().IsZero2();
+    }
+
+    FORCE_INLINE bool Vector::IsAnyEqualToZero3() const
+    {
+        return !EqualsZero().IsZero3();
+    }
+
+    FORCE_INLINE bool Vector::IsAnyEqualToZero4() const
+    {
+        return !EqualsZero().IsZero4();
+    }
+
+    FORCE_INLINE bool Vector::IsZero2() const
+    {
+        return IsEqual2(Vector::Zero);
+    }
+
+    FORCE_INLINE bool Vector::IsZero3() const
+    {
+        return IsEqual3(Vector::Zero);
+    }
+
+    FORCE_INLINE bool Vector::IsZero4() const
+    {
+        return IsEqual4(Vector::Zero);
+    }
+
+    FORCE_INLINE Vector Vector::NearEqualsZero(float epsilon) const
+    {
+        return NearEqual(Vector::Zero, Vector(epsilon));
+    }
+
+    FORCE_INLINE bool Vector::IsNearZero2(float epsilon) const
+    {
+        return IsNearEqual2(Vector::Zero, Vector(epsilon));
+    }
+
+    FORCE_INLINE bool Vector::IsNearZero3(float epsilon) const
+    {
+        return IsNearEqual3(Vector::Zero, Vector(epsilon));
+    }
+
+    FORCE_INLINE bool Vector::IsNearZero4(float epsilon) const
+    {
+        return IsNearEqual4(Vector::Zero, Vector(epsilon));
+    }
+
+    FORCE_INLINE Vector Vector::EqualsInfinity() const
+    {
+        __m128 vTemp = _mm_and_ps(m_data, SIMD::g_absMask);
+        return _mm_cmpeq_ps(vTemp, Vector::Infinity);
+    }
+
+    FORCE_INLINE bool Vector::IsInfinite2() const
+    {
+        return (_mm_movemask_ps(EqualsInfinity()) & 3) != 0;
+    }
+
+    FORCE_INLINE bool Vector::IsInfinite3() const
+    {
+        return (_mm_movemask_ps(EqualsInfinity()) & 7) != 0;
+    }
+
+    FORCE_INLINE bool Vector::IsInfinite4() const
+    {
+        return (_mm_movemask_ps(EqualsInfinity()) != 0);
+    }
+
+    FORCE_INLINE Vector Vector::EqualsNaN() const
+    {
+        return _mm_cmpneq_ps(m_data, m_data);
+    }
+
+    FORCE_INLINE bool Vector::IsNaN2() const
+    {
+        return (_mm_movemask_ps(EqualsNaN()) & 3) != 0;
+    }
+
+    FORCE_INLINE bool Vector::IsNaN3() const
+    {
+        return (_mm_movemask_ps(EqualsNaN()) & 7) != 0;
+    }
+
+    FORCE_INLINE bool Vector::IsNaN4() const
+    {
+        return (_mm_movemask_ps(EqualsNaN()) != 0);
+    }
+
+    FORCE_INLINE bool Vector::IsParallelTo(const Vector& v) const
+    {
+        Vector const vAbsDot = Vector::Dot3(*this, v).GetAbs();
+        Vector const vAbsDelta = Vector::One - vAbsDot;
+        return vAbsDelta.IsLessThanEqual4(Vector::Epsilon);
+    }
+
+    FORCE_INLINE void Vector::ToDirectionAndLength2(Vector& direction, float& length) const
+    {
+        Vector const vLength = Length2();
+        direction = Vector::Select(*this, Vector::Zero, Select0011);
+        direction /= vLength;
+        length = vLength.ToFloat();
+    }
+
+    FORCE_INLINE void Vector::ToDirectionAndLength3(Vector& direction, float& length) const
+    {
+        Vector const vLength = Length3();
+        direction = Vector::Select(*this, Vector::Zero, Select0001);
+        direction /= vLength;
+        length = vLength.ToFloat();
+    }
+
+    FORCE_INLINE bool Vector::operator==(const Vector& rhs) const
+    {
+        return IsEqual4(rhs);
+    }
+
+    FORCE_INLINE bool Vector::operator!=(const Vector& rhs) const
+    {
+        return !IsEqual4(rhs);
+    }
+}

MotionCorrection/src/cpp/Platform.h

@@ -0,0 +1,51 @@
+/*
+ * SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
+ * SPDX-License-Identifier: Apache-2.0
+ */
+
+#pragma once
+
+// Finds the current platform
+#if defined( __WIN32__ ) || defined( _WIN32 )
+#    define PLATFORM_WIN32
+#else
+#    define PLATFORM_LINUX
+#endif
+
+//
+// Platform Specific Helpers/Functions
+//
+
+// DLL export
+#if defined(PLATFORM_WIN32) // Windows
+#    if defined(COMPILER_MSVC)
+#        if defined(STATIC_LIB)
+#            define API
+#        else
+#            if defined(API)
+#                define API __declspec(dllexport)
+#            else
+#                define API __declspec(dllimport)
+#            endif
+#        endif
+#    else
+#        if defined(STATIC_LIB)
+#            define API
+#        else
+#            if defined(API)
+#                define API __attribute__ ((dllexport))
+#            else
+#                define API __attribute__ ((dllimport))
+#            endif
+#        endif
+#    endif
+#    define DISABLE_OPTIMIZATION __pragma( optimize( "", off ) )
+#    define ENABLE_OPTIMIZATION __pragma( optimize( "", on ) )
+#    define DEBUG_BREAK() // __debugbreak()
+#else // Linux settings
+#    include <signal.h>
+#    define API __attribute__ ((visibility ("default")))
+#    define DISABLE_OPTIMIZATION
+#    define ENABLE_OPTIMIZATION
+#    define DEBUG_BREAK() // raise(SIGTRAP)
+#endif

README.md

@@ -1,13 +1,278 @@
 ---
-title: Kimodo Motion Api
-emoji: 🏃
-colorFrom: yellow
+title: Kimodo Motion API
+emoji: 💃
+colorFrom: green
 colorTo: blue
 sdk: gradio
 sdk_version: 6.20.0
-python_version: '3.13'
 app_file: app.py
-pinned: false
+python_version: "3.12"
+startup_duration_timeout: 1h
+short_description: Text-to-motion BVH generation for Blender
 ---
 
-Check out the configuration reference at https://huggingface.co/docs/hub/spaces-config-reference
+<p align="center">
+  <img src="./assets/banner.png" alt="Banner" width="100%">
+  <a href="LICENSE"><img src="https://img.shields.io/badge/License-Apache%202.0-76B900.svg" alt="License"></a>
+  <a href="https://research.nvidia.com/labs/sil/projects/kimodo/"><img src="https://img.shields.io/badge/Project-Page-blue" alt="Project Page"></a>
+  <a href="https://research.nvidia.com/labs/sil/projects/kimodo/docs/index.html"><img src="https://img.shields.io/badge/docs-online-green.svg" alt="Documentation"></a>
+</p>
+
+## Overview
+
+Kimodo is a **ki**nematic **mo**tion **d**iffusi**o**n model trained on a large-scale (700 hours) commercially-friendly optical motion capture dataset. The model generates high-quality 3D human and robot motions, and is controlled through text prompts and an extensive set of constraints such as full-body pose keyframes, end-effector positions/rotations, 2D paths, and 2D waypoints. Full details of the model architecture and training are available in the [technical report](https://research.nvidia.com/labs/sil/projects/kimodo/assets/kimodo_tech_report.pdf).
+
+This repository provides:
+- **Inference**: code and CLI to generate motions on both human and robot skeletons
+- **Interactive Demo**: easily author motions with a timeline interface of text prompts and kinematic controls
+- **Benchmark**: [test cases](https://huggingface.co/datasets/nvidia/Kimodo-Motion-Gen-Benchmark) and evaluation code built on the [BONES-SEED](https://huggingface.co/datasets/bones-studio/seed) dataset to evaluate motion generation models based on text and constraint-following abilities
+- **Annotations**: fine-grained temporal text descriptions created for the Kimodo project are included in the [BONES-SEED](https://huggingface.co/datasets/bones-studio/seed) dataset. For more information on these labels, see our separate [Hugging Face repo](https://huggingface.co/datasets/nvidia/SEED-Timeline-Annotations).
+
+### ZeroGPU motion API
+
+This fork includes a lightweight Hugging Face Gradio app in `app.py`. It exposes a
+`/generate_motion` endpoint that generates one SOMA motion and returns Blender-compatible
+BVH, Kimodo NPZ, and JSON metadata. The endpoint is designed for ZeroGPU and deliberately
+disables the optional native motion-correction pass so the Space does not need to compile
+the `MotionCorrection` extension.
+
+Set the Space hardware to **ZeroGPU** and add an `HF_TOKEN` secret with access to the gated
+Llama 3 repositories used by Kimodo's LLM2Vec text encoder. The default model is
+`Kimodo-SOMA-RP-v1.1`; override it with the `KIMODO_MODEL` Space variable if needed.
+
+<div align="center">
+  <img src="assets/teaser.gif" width="1280">
+</div>
+
+## News
+
+See the [full changelog](CHANGELOG.md) for a detailed list of all changes.
+
+- **[2026-07-10]** Released the [ARDY project](https://research.nvidia.com/labs/sil/projects/ardy/) -- a _real-time_ motion generation model with all the controllability of Kimodo!
+- **[2026-05-03]** _FIX_: fixed a bug causing incorrect calculation of averaged metrics for constraint test cases in the benchmark
+- **[2026-04-24]** _NEW_: improved multi-prompt generation and better support for small VRAM GPUs via `TEXT_ENCODER_DEVICE=cpu` env var
+- **[2026-04-10]** Released the [Kimodo Motion Generation Benchmark](#kimodo-motion-generation-benchmark) alongside new v1.1 Kimodo-SOMA models
+- **[2026-03-19]** **Breaking:** Model inputs/outputs now use the SOMA 77-joint skeleton (`somaskel77`).
+- **[2026-03-16]** Initial open-source release of Kimodo with five model variants (SOMA, G1, SMPL-X), CLI, interactive demo, and timeline annotations for BONES-SEED.
+
+
+## Kimodo Models
+
+Several variations of Kimodo are available trained on various skeletons and datasets. All models support text-to-motion and kinematic controls.
+
+> Note: models will be downloaded automatically when attempting to generate from the CLI or Interactive Demo, so there is no need to download them manually
+
+| Model | Skeleton | Training Data | Release Date | Hugging Face | License |
+|:-------|:-------------|:------:|:------:|:-------------:|:-------------:|
+| **Kimodo-SOMA-RP-v1.1** | [SOMA](https://github.com/NVlabs/SOMA-X) | [Bones Rigplay 1](https://bones.studio/datasets#rp01) | April 10, 2026 | [Link](https://huggingface.co/nvidia/Kimodo-SOMA-RP-v1.1) | [NVIDIA Open Model](https://www.nvidia.com/en-us/agreements/enterprise-software/nvidia-open-model-license/) |
+| **Kimodo-SOMA-SEED-v1.1** | [SOMA](https://github.com/NVlabs/SOMA-X) | [BONES-SEED](https://huggingface.co/datasets/bones-studio/seed) | April 10, 2026  | [Link](https://huggingface.co/nvidia/Kimodo-SOMA-SEED-v1.1) | [NVIDIA Open Model](https://www.nvidia.com/en-us/agreements/enterprise-software/nvidia-open-model-license/) |
+| **Kimodo-SOMA-RP-v1** | [SOMA](https://github.com/NVlabs/SOMA-X) | [Bones Rigplay 1](https://bones.studio/datasets#rp01) | March 16, 2026 | [Link](https://huggingface.co/nvidia/Kimodo-SOMA-RP-v1) | [NVIDIA Open Model](https://www.nvidia.com/en-us/agreements/enterprise-software/nvidia-open-model-license/) |
+| **Kimodo-G1-RP-v1** | [Unitree G1](https://github.com/unitreerobotics/unitree_mujoco/tree/main/unitree_robots/g1) | [Bones Rigplay 1](https://bones.studio/datasets#rp01) | March 16, 2026  | [Link](https://huggingface.co/nvidia/Kimodo-G1-RP-v1) | [NVIDIA Open Model](https://www.nvidia.com/en-us/agreements/enterprise-software/nvidia-open-model-license/) |
+| **Kimodo-SOMA-SEED-v1** | [SOMA](https://github.com/NVlabs/SOMA-X) | [BONES-SEED](https://huggingface.co/datasets/bones-studio/seed) | March 16, 2026  | [Link](https://huggingface.co/nvidia/Kimodo-SOMA-SEED-v1) | [NVIDIA Open Model](https://www.nvidia.com/en-us/agreements/enterprise-software/nvidia-open-model-license/) |
+| **Kimodo-G1-SEED-v1** | [Unitree G1](https://github.com/unitreerobotics/unitree_mujoco/tree/main/unitree_robots/g1) | [BONES-SEED](https://huggingface.co/datasets/bones-studio/seed) | March 16, 2026  | [Link](https://huggingface.co/nvidia/Kimodo-G1-SEED-v1) | [NVIDIA Open Model](https://www.nvidia.com/en-us/agreements/enterprise-software/nvidia-open-model-license/) |
+| **Kimodo-SMPLX-RP-v1** | [SMPL-X](https://github.com/vchoutas/smplx) | [Bones Rigplay 1](https://bones.studio/datasets#rp01) | March 16, 2026  | [Link](https://huggingface.co/nvidia/Kimodo-SMPLX-RP-v1) | [NVIDIA R&D Model](https://www.nvidia.com/en-us/agreements/enterprise-software/nvidia-internal-scientific-research-and-development-model-license/) |
+
+By default, we recommend using the models trained on the full Bones Rigplay 1 dataset (700 hours of mocap) for your motion generation needs.
+The models trained on BONES-SEED use 288 hours of [publicly available mocap data](https://huggingface.co/datasets/bones-studio/seed) so are less capable, but are useful for comparing to other models trained on BONES-SEED. To easily compare motion generation models to Kimodo, check out our [Motion Generation Benchmark](#kimodo-motion-generation-benchmark).
+
+### Changes in v1.1
+The latest v1.1 Kimodo-SOMA models were released primarily for compatibility with our new [Motion Generation Benchmark](#kimodo-motion-generation-benchmark), but also contain minor quality improvements over v1. For details on these improvements, please see the Hugging Face pages for [Kimodo-SOMA-RP-v1.1](https://huggingface.co/nvidia/Kimodo-SOMA-RP-v1.1#changes-in-v11) and [Kimodo-SOMA-SEED-v1.1](https://huggingface.co/nvidia/Kimodo-SOMA-SEED-v1.1#changes-in-v11).
+
+## Getting Started
+
+Please see the full documentation for detailed installation instructions, how to use the CLI and Interactive Demo, and other practical tips for generating motions with Kimodo:
+
+**[Full Documentation](https://research.nvidia.com/labs/sil/projects/kimodo/docs)**
+- [Quick Start Guide](https://research.nvidia.com/labs/sil/projects/kimodo/docs/getting_started/quick_start.html)
+- [Installation Instructions](https://research.nvidia.com/labs/sil/projects/kimodo/docs/getting_started/installation.html)
+- [Interactive Motion Authoring Demo](https://research.nvidia.com/labs/sil/projects/kimodo/docs/interactive_demo/index.html)
+- [Command-Line Interface](https://research.nvidia.com/labs/sil/projects/kimodo/docs/user_guide/cli.html)
+- [Benchmark Instructions](https://research.nvidia.com/labs/sil/projects/kimodo/docs/benchmark/introduction.html)
+- [API Reference](https://research.nvidia.com/labs/sil/projects/kimodo/docs/api_reference/index.html)
+
+**Before getting started** with motion generation, please review the [best practices](https://research.nvidia.com/labs/sil/projects/kimodo/docs/key_concepts/limitations.html) and be aware of [model limitations](https://research.nvidia.com/labs/sil/projects/kimodo/docs/key_concepts/limitations.html#limitations).
+
+
+Some notes on installation environment:
+- Kimodo requires ~17GB of VRAM to generate locally entirely on GPU, primarily due to the text embedding model. If you have a smaller card, set `TEXT_ENCODER_DEVICE=cpu` when running Kimodo commands to force text encoding to the CPU. This is slightly slower but reduces VRAM usage to <3 GB.
+- The model has been most extensively tested on GeForce RTX 3090, GeForce RTX 4090, and NVIDIA A100 GPUs, but should work on other recent cards with sufficient VRAM
+- This repo was developed on Linux, though Windows should work especially if using Docker
+
+## Interactive Motion Authoring Demo
+
+<div align="center">
+  <img src="assets/demo_screenshot.png" width="1000">
+</div>
+
+</br>
+
+**[Demo Documentation and Tutorial](https://research.nvidia.com/labs/sil/projects/kimodo/docs/interactive_demo/index.html)**
+
+The web-based interactive demo provides an intuitive interface for generating motions with any of the Kimodo model variations. After installation, the demo can be launched with the `kimodo_demo` command. It runs locally on http://127.0.0.1:7860. Open this URL in your browser to access the interface (or use port forwarding if set up on a server).
+
+### Demo Features
+- **Multiple Characters**: Supports generating with the SOMA, G1, and SMPL-X versions of Kimodo
+- **Text Prompts**: Enter one or more natural language descriptions of desired motions on the timeline
+- **Timeline Editor**: Add and edit keyframes and constrained intervals on multiple constraint tracks
+- **Constraint Types**:
+  - Full-Body: Complete joint position constraints at specific frames
+  - 2D Root: Define waypoints or full paths to follow on the ground plane
+  - End-Effectors: Control hands and feet positions/rotations
+- **Constraint Editing**: Editing mode allows for re-posing of constraints or adjusting waypoints
+- **3D Visualization**: Real-time rendering of generated motions with skeleton and skinned mesh options
+- **Playback Controls**: Preview generated motions with adjustable playback speed
+- **Multiple Samples**: Generate and compare multiple motion variations
+- **Examples**: Load pre-existing examples to better understand Kimodo's capabilities
+- **Export**: Save constraints and generated motions for later use
+
+## Command-Line Interface
+
+**[CLI Documentation and Examples](https://research.nvidia.com/labs/sil/projects/kimodo/docs/user_guide/cli.html)**
+
+Motions can also be generated directly from the command line with the `kimodo_gen` command or by running `python -m kimodo.scripts.generate` directly.
+
+**Key Arguments:**
+- `prompt`: A single text description or sequence of texts for the desired motion (required)
+- `--model`: Which Kimodo model to use for generation
+- `--duration`: Motion duration in seconds
+- `--num_samples`: Number of motion variations to generate
+- `--constraints`: Constraint file to control the generated motion (e.g., saved from the web demo)
+- `--diffusion_steps`: Number of denoising steps
+- `--cfg_type` / `--cfg_weight`: Classifier-free guidance (`nocfg`, `regular` with one weight, or `separated` with two weights for text vs. constraints); see the [CLI docs](https://research.nvidia.com/labs/sil/projects/kimodo/docs/user_guide/cli.html#classifier-free-guidance-cfg)
+- `--no-postprocess`: Flag to disable foot skate and constraint cleanup post-processing
+- `--seed`: Random seed for reproducible results
+
+The script supports different output formats depending on which skeleton is used. By default, a custom NPZ format is saved that is compatible with the web demo.
+For Kimodo-G1 models, the motion can be saved in the standard MuJoCo qpos CSV format.
+For Kimodo-SMPLX, motion can be saved in the standard AMASS npz format for compability with existing pipelines.
+
+### Default NPZ Output Format
+Generated motions are saved as NPZ files containing:
+- `posed_joints`: Global joint positions `[T, J, 3]`
+- `global_rot_mats`: Global joint rotation matrices `[T, J, 3, 3]`
+- `local_rot_mats`: Local (parent-relative) joint rotation matrices `[T, J, 3, 3]`
+- `foot_contacts`: Foot contact labels [left heel, left toe, right heel, right toes] `[T, 4]`
+- `smooth_root_pos`: Smoothed root representations outputted from the model `[T, 3]`
+- `root_positions`: The (non-smoothed) trajectory of the actual root joint (e.g., pelvis) `[T, 3]`
+- `global_root_heading`: The heading direction output from the model `[T, 2]`
+
+`T` the number of frames and `J` the number of joints.
+
+## Low-Level Python API
+
+**[Model API Documentation](https://research.nvidia.com/labs/sil/projects/kimodo/docs/api_reference/model.html#kimodo.model.kimodo_model.Kimodo.__call__)**
+
+For maximum flexibility, the low-level model inference API can be called directly, rather than going through our high-level CLI.
+This allows for advanced model configuration including classifier-free guidance weights and parameters related to transitions in multi-prompt sequences.
+
+## Downstream Robotics Applications of Kimodo
+
+### Visualizing G1 Motions with MuJoCo
+
+<div align="center">
+  <img src="assets/mujoco_result.gif" width="800">
+</div>
+
+After generating motions on the G1 robot skeleton and saving to the MuJoCo qpos CSV file format, they can be easily used and visualized within MuJoCo.
+A minimal visualization script is available with:
+```
+python -m kimodo.scripts.mujoco_load
+```
+Make sure to edit the script to correctly point to your CSV file and install Mujoco before running this.
+
+### Tracking Generated Motions with ProtoMotions
+
+<div align="center">
+  <img src="assets/protomotions_results.gif" width="1280">
+</div>
+
+[ProtoMotions](https://github.com/NVlabs/ProtoMotions) is a GPU-accelerated simulation and learning framework for training physically simulated digital humans and humanoid robots. The Kimodo NPZ and CSV output formats are both compatible with ProtoMotions making it easy to train physics-based policies with generated motions from Kimodo. ProtoMotions supports outputs on both the SOMA skeleton and Unitree G1
+
+After generating motions with Kimodo, head over to the [ProtoMotions docs](https://github.com/NVlabs/ProtoMotions?tab=readme-ov-file#-motion-authoring-with-kimodo) to see how to import them.
+
+### Retargeting Motions to Other Robots with GMR
+
+<div align="center">
+  <img src="assets/gmr_results.gif" width="1280">
+</div>
+
+Motions generated by Kimodo-SMPLX can be retargeted to other robots using [General Motion Retargeting (GMR)](https://github.com/YanjieZe/GMR).
+GMR supports the AMASS NPZ format out of the box, so simply generate motions with Kimodo and use `--output` to save; the AMASS NPZ is written to `stem_amass.npz` (single sample) or in the output folder (multiple samples). Then, use the [SMPL-X to Robot script](https://github.com/YanjieZe/GMR?tab=readme-ov-file#retargeting-from-smpl-x-amass-omomo-to-robot) in GMR to retarget to any supported robot. For example:
+```
+# run within GMR codebase
+python scripts/smplx_to_robot.py --smplx_file /path/to/saved/amass_format.npz --robot booster_t1
+```
+
+### Combining Kimodo with GEAR-SONIC
+
+<div align="center">
+  <img src="assets/sonic_kimodo_demo.gif" width="800">
+</div>
+
+As a proof of concept, we have also incorporated Kimodo into the [interactive GEAR-SONIC demo](https://nvlabs.github.io/GEAR-SONIC/demo.html). In the demo, Kimodo can be used to generate a kinematic motion on the G1 robot skeleton, then GEAR-SONIC tracks the motion in simulation.
+
+## Kimodo Motion Generation Benchmark
+
+[**[Benchmark Documentation](https://research.nvidia.com/labs/sil/projects/kimodo/docs/benchmark/introduction.html)**]
+[**[Test Suite on Hugging Face](https://huggingface.co/datasets/nvidia/Kimodo-Motion-Gen-Benchmark)**]
+
+Alongside the Kimodo models, we provide a benchmark designed to standardize evaluation for motion generation models with a comprehensive set of test cases. This includes:
+
+* **Evaluation Data**: A suite of test cases [available on Hugging Face](https://huggingface.co/datasets/nvidia/Kimodo-Motion-Gen-Benchmark) is used in concert with the [BONES-SEED](https://huggingface.co/datasets/bones-studio/seed) dataset to construct the full benchmark. 
+* **Diverse Test Cases**: Test cases cover a wide range of text-conditioned and constraint-conditioned motion generation.
+* **Evaluation Pipeline**: Code for the full evaluation pipeline including benchmark construction, motion generation, and evaluation.
+* **Metrics**: Several metrics to evaluate generated motions that cover motion quality, constraint following, and text alignment. Our [TMR-SOMA-RP-v1](https://huggingface.co/nvidia/TMR-SOMA-RP-v1) model trained on all 700 hours of the Bones Rigplay dataset is a powerful embedding model to compute common metrics like R-precision and FID.
+
+To facilitate future research, we [report benchmark results](https://research.nvidia.com/labs/sil/projects/kimodo/docs/benchmark/results.html) for Kimodo-SOMA-v1.1 models, which are reproducible and easily comparable to other methods trained on the BONES-SEED data. 
+
+## Timeline Annotations for BONES-SEED
+
+As detailed in the [tech report](https://research.nvidia.com/labs/sil/projects/kimodo/assets/kimodo_tech_report.pdf), Kimodo is trained using fine-grained temporal text annotations of mocap clips.
+While the full [Rigplay 1](https://bones.studio/datasets#rp01) dataset is proprietary, we have released the temporal segmentations for the public [BONES-SEED](https://huggingface.co/datasets/bones-studio/seed) subset.
+These annotations are already included in the BONES-SEED dataset, but the standalone labels and additional information about them is [available on HuggingFace](https://huggingface.co/datasets/nvidia/SEED-Timeline-Annotations).
+
+
+## Related Humanoid Work at NVIDIA
+Kimodo is part of a larger effort to enable humanoid motion data for robotics, physical AI, and other applications.
+
+Check out these related works:
+* [ARDY](https://github.com/nv-tlabs/ardy) - builds on top of Kimodo to enable real-time controllable motion generation for interactive applications
+* [MotionBricks](https://nvlabs.github.io/motionbricks/) - real-time motion generation framework that specializes in fast and robust motion in-betweening
+* [SOMA Body Model](https://github.com/NVlabs/SOMA-X) - a unified parameteric human body model
+* [BONES-SEED Dataset](https://huggingface.co/datasets/bones-studio/seed) - a large scale human(oid) motion capture dataset in SOMA and G1 format
+* [ProtoMotions](https://github.com/NVlabs/ProtoMotions) - simulation and learning framework for training physically simulated human(oid)s
+* [SOMA Retargeter](https://github.com/NVIDIA/soma-retargeter) - SOMA to G1 retargeting tool
+* [GEM](https://github.com/NVlabs/GEM-X) - human motion reconstruction from video
+* [GEAR SONIC](https://github.com/NVlabs/GR00T-WholeBodyControl) - humanoid behavior foundation model for physical robots
+
+## Citation
+
+If you use this code in your research, please cite:
+
+```bibtex
+@article{Kimodo2026,
+  title={Kimodo: Scaling Controllable Human Motion Generation},
+  author={Rempe, Davis and Petrovich, Mathis and Yuan, Ye and Zhang, Haotian and Peng, Xue Bin and Jiang, Yifeng and Wang, Tingwu and Iqbal, Umar and Minor, David and de Ruyter, Michael and Li, Jiefeng and Tessler, Chen and Lim, Edy and Jeong, Eugene and Wu, Sam and Hassani, Ehsan and Huang, Michael and Yu, Jin-Bey and Chung, Chaeyeon and Song, Lina and Dionne, Olivier and Kautz, Jan and Yuen, Simon and Fidler, Sanja},
+  journal={arXiv:2603.15546},
+  year={2026}
+}
+```
+
+## License
+
+This codebase is licensed under [Apache-2.0](LICENSE). Note that model checkpoints and data are licensed separately as indicated on the HuggingFace download pages.
+
+This project will download and install additional third-party open source software projects. Review the license terms of these open source projects before use.
+
+## Acknowledgments
+
+This project builds upon excellent open-source projects:
+- [Viser](https://github.com/nerfstudio-project/viser) for 3D motion authoring demo
+- [LLM2Vec](https://github.com/McGill-NLP/llm2vec) for text encoding
+
+## Contact
+
+For questions or issues, please open an issue on this repository or reach out directly to the authors.
+
+---
+ 

app.py

@@ -0,0 +1,172 @@
+"""Minimal Kimodo text-to-motion API for Hugging Face ZeroGPU."""
+
+from __future__ import annotations
+
+import os
+import tempfile
+from pathlib import Path
+
+os.environ.setdefault("PYTORCH_CUDA_ALLOC_CONF", "expandable_segments:True")
+os.environ.setdefault("TEXT_ENCODER_MODE", "local")
+os.environ.setdefault("TEXT_ENCODER_DEVICE", "cuda")
+
+# ZeroGPU must patch CUDA before torch or any Kimodo module imports it.
+import spaces
+import gradio as gr
+import torch
+
+from kimodo import load_model
+from kimodo.exports.bvh import save_motion_bvh
+from kimodo.exports.motion_io import save_kimodo_npz
+from kimodo.tools import seed_everything
+from space_utils import estimate_zero_gpu_duration, validate_motion_request
+
+
+MODEL_NAME = os.environ.get("KIMODO_MODEL", "Kimodo-SOMA-RP-v1.1")
+
+# Eager module-scope placement is required by ZeroGPU's weight packing mechanism.
+MODEL, RESOLVED_MODEL_NAME = load_model(
+    MODEL_NAME,
+    device="cuda",
+    default_family="Kimodo",
+    return_resolved_name=True,
+)
+
+
+def _without_progress(iterable):
+    return iterable
+
+
+def _single_sample(output: dict) -> dict:
+    """Remove the leading batch dimension from a one-sample model result."""
+
+    sample = {}
+    for key, value in output.items():
+        if hasattr(value, "shape") and len(value.shape) > 0 and int(value.shape[0]) == 1:
+            sample[key] = value[0]
+        else:
+            sample[key] = value
+    return sample
+
+
+@spaces.GPU(duration=estimate_zero_gpu_duration, size="large")
+def generate_motion(
+    prompt: str,
+    duration_seconds: float,
+    seed: int,
+    diffusion_steps: int,
+    standard_tpose: bool,
+) -> tuple[str, str, dict]:
+    """Generate one text-conditioned human motion and return BVH, NPZ, and metadata."""
+
+    request = validate_motion_request(
+        prompt,
+        duration_seconds,
+        seed,
+        diffusion_steps,
+        standard_tpose,
+    )
+    seed_everything(request.seed)
+
+    num_frames = max(1, int(round(request.duration_seconds * float(MODEL.fps))))
+    output = MODEL(
+        request.prompt,
+        num_frames,
+        num_denoising_steps=request.diffusion_steps,
+        multi_prompt=False,
+        constraint_lst=[],
+        cfg_weight=[2.0, 2.0],
+        num_samples=1,
+        return_numpy=True,
+        post_processing=False,
+        progress_bar=_without_progress,
+    )
+    sample = _single_sample(output)
+
+    output_dir = Path(tempfile.mkdtemp(prefix="kimodo-motion-"))
+    bvh_path = output_dir / "motion.bvh"
+    npz_path = output_dir / "motion.npz"
+    save_kimodo_npz(str(npz_path), sample)
+
+    skeleton = MODEL.output_skeleton
+    local_rot_mats = torch.as_tensor(sample["local_rot_mats"], device="cuda")
+    posed_joints = torch.as_tensor(sample["posed_joints"], device="cuda")
+    root_positions = posed_joints[:, int(skeleton.root_idx), :]
+    save_motion_bvh(
+        bvh_path,
+        local_rot_mats,
+        root_positions,
+        skeleton=skeleton,
+        fps=float(MODEL.fps),
+        standard_tpose=request.standard_tpose,
+    )
+
+    metadata = {
+        "prompt": request.prompt,
+        "duration_seconds": request.duration_seconds,
+        "seed": request.seed,
+        "diffusion_steps": request.diffusion_steps,
+        "model": MODEL_NAME,
+        "resolved_model": RESOLVED_MODEL_NAME,
+        "fps": float(MODEL.fps),
+        "frames": num_frames,
+        "skeleton": str(skeleton.name),
+        "standard_tpose": request.standard_tpose,
+        "post_processing": False,
+    }
+    return str(bvh_path), str(npz_path), metadata
+
+
+with gr.Blocks(title="Kimodo Motion API") as demo:
+    gr.Markdown(
+        """
+        # Kimodo Motion API
+
+        Generate a text-conditioned SOMA motion on ZeroGPU. Download the BVH for Blender
+        or keep the NPZ for a later Kimodo workflow. The first call after an idle period may
+        take longer while ZeroGPU restores model weights.
+        """
+    )
+    prompt_input = gr.Textbox(
+        label="Motion prompt",
+        lines=3,
+        max_length=1_000,
+        value="A person walks forward cautiously, looks over the left shoulder, then stops.",
+    )
+    with gr.Row():
+        duration_input = gr.Slider(1.0, 10.0, value=5.0, step=0.5, label="Duration (seconds)")
+        seed_input = gr.Number(value=42, precision=0, minimum=0, maximum=2**31 - 1, label="Seed")
+        steps_input = gr.Slider(10, 100, value=50, step=5, label="Diffusion steps")
+    standard_tpose_input = gr.Checkbox(
+        value=True,
+        label="Export a standard T-pose rest skeleton",
+        info="Recommended for Blender retargeting.",
+    )
+    generate_button = gr.Button("Generate motion", variant="primary")
+    with gr.Row():
+        bvh_output = gr.File(label="Blender BVH")
+        npz_output = gr.File(label="Kimodo NPZ")
+    metadata_output = gr.JSON(label="Generation metadata")
+
+    generate_button.click(
+        fn=generate_motion,
+        inputs=[prompt_input, duration_input, seed_input, steps_input, standard_tpose_input],
+        outputs=[bvh_output, npz_output, metadata_output],
+        api_name="generate_motion",
+        api_description="Generate a Kimodo motion as BVH and NPZ files.",
+        concurrency_limit=1,
+    )
+    gr.Examples(
+        examples=[
+            ["A person takes three slow steps forward and waves with the right hand."],
+            ["A person crouches, jumps upward, lands, and regains balance."],
+            ["A person performs a short defensive boxing combination."],
+        ],
+        inputs=[prompt_input],
+        cache_examples=False,
+    )
+
+demo.queue(default_concurrency_limit=1)
+
+if __name__ == "__main__":
+    demo.launch(mcp_server=True)