path: root/thirdparty/openxr/src/common/xr_linear.h

// Copyright (c) 2017 The Khronos Group Inc.
// Copyright (c) 2016 Oculus VR, LLC.
//
// SPDX-License-Identifier: Apache-2.0
//
// 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.
//
// Author: J.M.P. van Waveren
//

#ifndef XR_LINEAR_H_
#define XR_LINEAR_H_

#if defined(OS_LINUX_XCB) || defined(OS_LINUX_XCB_GLX) || defined(OS_LINUX_WAYLAND)
#pragma GCC diagnostic ignored "-Wunused-function"
#pragma clang diagnostic ignored "-Wunused-function"
#endif

#include <openxr/openxr.h>

/*
================================================================================================

Description     :       Vector, matrix and quaternion math.
Author          :       J.M.P. van Waveren
Date            :       12/10/2016
Language        :       C99
Format          :       Indent 4 spaces - no tabs.
Copyright       :       Copyright (c) 2016 Oculus VR, LLC. All Rights reserved.


DESCRIPTION
===========

All matrices are column-major.

INTERFACE
=========

XrVector2f
XrVector3f
XrVector4f
XrQuaternionf
XrMatrix4x4f

inline static void XrVector3f_Set(XrVector3f* v, const float value);
inline static void XrVector3f_Add(XrVector3f* result, const XrVector3f* a, const XrVector3f* b);
inline static void XrVector3f_Sub(XrVector3f* result, const XrVector3f* a, const XrVector3f* b);
inline static void XrVector3f_Min(XrVector3f* result, const XrVector3f* a, const XrVector3f* b);
inline static void XrVector3f_Max(XrVector3f* result, const XrVector3f* a, const XrVector3f* b);
inline static void XrVector3f_Decay(XrVector3f* result, const XrVector3f* a, const float value);
inline static void XrVector3f_Lerp(XrVector3f* result, const XrVector3f* a, const XrVector3f* b, const float fraction);
inline static void XrVector3f_Scale(XrVector3f* result, const XrVector3f* a, const float scaleFactor);
inline static void XrVector3f_Normalize(XrVector3f* v);
inline static float XrVector3f_Length(const XrVector3f* v);

inline static void XrQuaternionf_Lerp(XrQuaternionf* result, const XrQuaternionf* a, const XrQuaternionf* b, const float fraction);
inline static void XrQuaternionf_Multiply(XrQuaternionf* result, const XrQuaternionf* a, const XrQuaternionf* b;

inline static void XrMatrix4x4f_CreateIdentity(XrMatrix4x4f* result);
inline static void XrMatrix4x4f_CreateTranslation(XrMatrix4x4f* result, const float x, const float y, const float z);
inline static void XrMatrix4x4f_CreateRotation(XrMatrix4x4f* result, const float degreesX, const float degreesY,
                                               const float degreesZ);
inline static void XrMatrix4x4f_CreateScale(XrMatrix4x4f* result, const float x, const float y, const float z);
inline static void XrMatrix4x4f_CreateTranslationRotationScale(XrMatrix4x4f* result, const XrVector3f* translation,
                                                               const XrQuaternionf* rotation, const XrVector3f* scale);
inline static void XrMatrix4x4f_CreateProjection(XrMatrix4x4f* result, const float tanAngleLeft, const float tanAngleRight,
                                                 const float tanAngleUp, float const tanAngleDown, const float nearZ,
                                                 const float farZ);
inline static void XrMatrix4x4f_CreateProjectionFov(XrMatrix4x4f* result, const float fovDegreesLeft, const float fovDegreesRight,
                                                    const float fovDegreeUp, const float fovDegreesDown, const float nearZ,
                                                    const float farZ);
inline static void XrMatrix4x4f_CreateFromQuaternion(XrMatrix4x4f* result, const XrQuaternionf* src);
inline static void XrMatrix4x4f_CreateOffsetScaleForBounds(XrMatrix4x4f* result, const XrMatrix4x4f* matrix, const XrVector3f* mins,
                                                           const XrVector3f* maxs);

inline static bool XrMatrix4x4f_IsAffine(const XrMatrix4x4f* matrix, const float epsilon);
inline static bool XrMatrix4x4f_IsOrthogonal(const XrMatrix4x4f* matrix, const float epsilon);
inline static bool XrMatrix4x4f_IsOrthonormal(const XrMatrix4x4f* matrix, const float epsilon);
inline static bool XrMatrix4x4f_IsRigidBody(const XrMatrix4x4f* matrix, const float epsilon);

inline static void XrMatrix4x4f_GetTranslation(XrVector3f* result, const XrMatrix4x4f* src);
inline static void XrMatrix4x4f_GetRotation(XrQuaternionf* result, const XrMatrix4x4f* src);
inline static void XrMatrix4x4f_GetScale(XrVector3f* result, const XrMatrix4x4f* src);

inline static void XrMatrix4x4f_Multiply(XrMatrix4x4f* result, const XrMatrix4x4f* a, const XrMatrix4x4f* b);
inline static void XrMatrix4x4f_Transpose(XrMatrix4x4f* result, const XrMatrix4x4f* src);
inline static void XrMatrix4x4f_Invert(XrMatrix4x4f* result, const XrMatrix4x4f* src);
inline static void XrMatrix4x4f_InvertRigidBody(XrMatrix4x4f* result, const XrMatrix4x4f* src);

inline static void XrMatrix4x4f_TransformVector3f(XrVector3f* result, const XrMatrix4x4f* m, const XrVector3f* v);
inline static void XrMatrix4x4f_TransformVector4f(XrVector4f* result, const XrMatrix4x4f* m, const XrVector4f* v);

inline static void XrMatrix4x4f_TransformBounds(XrVector3f* resultMins, XrVector3f* resultMaxs, const XrMatrix4x4f* matrix,
                                                const XrVector3f* mins, const XrVector3f* maxs);
inline static bool XrMatrix4x4f_CullBounds(const XrMatrix4x4f* mvp, const XrVector3f* mins, const XrVector3f* maxs);

================================================================================================
*/

#include <assert.h>
#include <math.h>
#include <stdbool.h>

#define MATH_PI 3.14159265358979323846f

#define DEFAULT_NEAR_Z 0.015625f  // exact floating point representation
#define INFINITE_FAR_Z 0.0f

static const XrColor4f XrColorRed = {1.0f, 0.0f, 0.0f, 1.0f};
static const XrColor4f XrColorGreen = {0.0f, 1.0f, 0.0f, 1.0f};
static const XrColor4f XrColorBlue = {0.0f, 0.0f, 1.0f, 1.0f};
static const XrColor4f XrColorYellow = {1.0f, 1.0f, 0.0f, 1.0f};
static const XrColor4f XrColorPurple = {1.0f, 0.0f, 1.0f, 1.0f};
static const XrColor4f XrColorCyan = {0.0f, 1.0f, 1.0f, 1.0f};
static const XrColor4f XrColorLightGrey = {0.7f, 0.7f, 0.7f, 1.0f};
static const XrColor4f XrColorDarkGrey = {0.3f, 0.3f, 0.3f, 1.0f};

typedef enum GraphicsAPI { GRAPHICS_VULKAN, GRAPHICS_OPENGL, GRAPHICS_OPENGL_ES, GRAPHICS_D3D } GraphicsAPI;

// Column-major, pre-multiplied. This type does not exist in the OpenXR API and is provided for convenience.
typedef struct XrMatrix4x4f {
    float m[16];
} XrMatrix4x4f;

inline static float XrRcpSqrt(const float x) {
    const float SMALLEST_NON_DENORMAL = 1.1754943508222875e-038f;  // ( 1U << 23 )
    const float rcp = (x >= SMALLEST_NON_DENORMAL) ? 1.0f / sqrtf(x) : 1.0f;
    return rcp;
}

inline static void XrVector3f_Set(XrVector3f* v, const float value) {
    v->x = value;
    v->y = value;
    v->z = value;
}

inline static void XrVector3f_Add(XrVector3f* result, const XrVector3f* a, const XrVector3f* b) {
    result->x = a->x + b->x;
    result->y = a->y + b->y;
    result->z = a->z + b->z;
}

inline static void XrVector3f_Sub(XrVector3f* result, const XrVector3f* a, const XrVector3f* b) {
    result->x = a->x - b->x;
    result->y = a->y - b->y;
    result->z = a->z - b->z;
}

inline static void XrVector3f_Min(XrVector3f* result, const XrVector3f* a, const XrVector3f* b) {
    result->x = (a->x < b->x) ? a->x : b->x;
    result->y = (a->y < b->y) ? a->y : b->y;
    result->z = (a->z < b->z) ? a->z : b->z;
}

inline static void XrVector3f_Max(XrVector3f* result, const XrVector3f* a, const XrVector3f* b) {
    result->x = (a->x > b->x) ? a->x : b->x;
    result->y = (a->y > b->y) ? a->y : b->y;
    result->z = (a->z > b->z) ? a->z : b->z;
}

inline static void XrVector3f_Decay(XrVector3f* result, const XrVector3f* a, const float value) {
    result->x = (fabsf(a->x) > value) ? ((a->x > 0.0f) ? (a->x - value) : (a->x + value)) : 0.0f;
    result->y = (fabsf(a->y) > value) ? ((a->y > 0.0f) ? (a->y - value) : (a->y + value)) : 0.0f;
    result->z = (fabsf(a->z) > value) ? ((a->z > 0.0f) ? (a->z - value) : (a->z + value)) : 0.0f;
}

inline static void XrVector3f_Lerp(XrVector3f* result, const XrVector3f* a, const XrVector3f* b, const float fraction) {
    result->x = a->x + fraction * (b->x - a->x);
    result->y = a->y + fraction * (b->y - a->y);
    result->z = a->z + fraction * (b->z - a->z);
}

inline static void XrVector3f_Scale(XrVector3f* result, const XrVector3f* a, const float scaleFactor) {
    result->x = a->x * scaleFactor;
    result->y = a->y * scaleFactor;
    result->z = a->z * scaleFactor;
}

inline static float XrVector3f_Dot(const XrVector3f* a, const XrVector3f* b) { return a->x * b->x + a->y * b->y + a->z * b->z; }

// Compute cross product, which generates a normal vector.
// Direction vector can be determined by right-hand rule: Pointing index finder in
// direction a and middle finger in direction b, thumb will point in Cross(a, b).
inline static void XrVector3f_Cross(XrVector3f* result, const XrVector3f* a, const XrVector3f* b) {
    result->x = a->y * b->z - a->z * b->y;
    result->y = a->z * b->x - a->x * b->z;
    result->z = a->x * b->y - a->y * b->x;
}

inline static void XrVector3f_Normalize(XrVector3f* v) {
    const float lengthRcp = XrRcpSqrt(v->x * v->x + v->y * v->y + v->z * v->z);
    v->x *= lengthRcp;
    v->y *= lengthRcp;
    v->z *= lengthRcp;
}

inline static float XrVector3f_Length(const XrVector3f* v) { return sqrtf(v->x * v->x + v->y * v->y + v->z * v->z); }

inline static void XrQuaternionf_CreateFromAxisAngle(XrQuaternionf* result, const XrVector3f* axis, const float angleInRadians) {
    float s = sinf(angleInRadians / 2.0f);
    float lengthRcp = XrRcpSqrt(axis->x * axis->x + axis->y * axis->y + axis->z * axis->z);
    result->x = s * axis->x * lengthRcp;
    result->y = s * axis->y * lengthRcp;
    result->z = s * axis->z * lengthRcp;
    result->w = cosf(angleInRadians / 2.0f);
}

inline static void XrQuaternionf_Lerp(XrQuaternionf* result, const XrQuaternionf* a, const XrQuaternionf* b, const float fraction) {
    const float s = a->x * b->x + a->y * b->y + a->z * b->z + a->w * b->w;
    const float fa = 1.0f - fraction;
    const float fb = (s < 0.0f) ? -fraction : fraction;
    const float x = a->x * fa + b->x * fb;
    const float y = a->y * fa + b->y * fb;
    const float z = a->z * fa + b->z * fb;
    const float w = a->w * fa + b->w * fb;
    const float lengthRcp = XrRcpSqrt(x * x + y * y + z * z + w * w);
    result->x = x * lengthRcp;
    result->y = y * lengthRcp;
    result->z = z * lengthRcp;
    result->w = w * lengthRcp;
}

inline static void XrQuaternionf_Multiply(XrQuaternionf* result, const XrQuaternionf* a, const XrQuaternionf* b) {
    result->x = (b->w * a->x) + (b->x * a->w) + (b->y * a->z) - (b->z * a->y);
    result->y = (b->w * a->y) - (b->x * a->z) + (b->y * a->w) + (b->z * a->x);
    result->z = (b->w * a->z) + (b->x * a->y) - (b->y * a->x) + (b->z * a->w);
    result->w = (b->w * a->w) - (b->x * a->x) - (b->y * a->y) - (b->z * a->z);
}

// Use left-multiplication to accumulate transformations.
inline static void XrMatrix4x4f_Multiply(XrMatrix4x4f* result, const XrMatrix4x4f* a, const XrMatrix4x4f* b) {
    result->m[0] = a->m[0] * b->m[0] + a->m[4] * b->m[1] + a->m[8] * b->m[2] + a->m[12] * b->m[3];
    result->m[1] = a->m[1] * b->m[0] + a->m[5] * b->m[1] + a->m[9] * b->m[2] + a->m[13] * b->m[3];
    result->m[2] = a->m[2] * b->m[0] + a->m[6] * b->m[1] + a->m[10] * b->m[2] + a->m[14] * b->m[3];
    result->m[3] = a->m[3] * b->m[0] + a->m[7] * b->m[1] + a->m[11] * b->m[2] + a->m[15] * b->m[3];

    result->m[4] = a->m[0] * b->m[4] + a->m[4] * b->m[5] + a->m[8] * b->m[6] + a->m[12] * b->m[7];
    result->m[5] = a->m[1] * b->m[4] + a->m[5] * b->m[5] + a->m[9] * b->m[6] + a->m[13] * b->m[7];
    result->m[6] = a->m[2] * b->m[4] + a->m[6] * b->m[5] + a->m[10] * b->m[6] + a->m[14] * b->m[7];
    result->m[7] = a->m[3] * b->m[4] + a->m[7] * b->m[5] + a->m[11] * b->m[6] + a->m[15] * b->m[7];

    result->m[8] = a->m[0] * b->m[8] + a->m[4] * b->m[9] + a->m[8] * b->m[10] + a->m[12] * b->m[11];
    result->m[9] = a->m[1] * b->m[8] + a->m[5] * b->m[9] + a->m[9] * b->m[10] + a->m[13] * b->m[11];
    result->m[10] = a->m[2] * b->m[8] + a->m[6] * b->m[9] + a->m[10] * b->m[10] + a->m[14] * b->m[11];
    result->m[11] = a->m[3] * b->m[8] + a->m[7] * b->m[9] + a->m[11] * b->m[10] + a->m[15] * b->m[11];

    result->m[12] = a->m[0] * b->m[12] + a->m[4] * b->m[13] + a->m[8] * b->m[14] + a->m[12] * b->m[15];
    result->m[13] = a->m[1] * b->m[12] + a->m[5] * b->m[13] + a->m[9] * b->m[14] + a->m[13] * b->m[15];
    result->m[14] = a->m[2] * b->m[12] + a->m[6] * b->m[13] + a->m[10] * b->m[14] + a->m[14] * b->m[15];
    result->m[15] = a->m[3] * b->m[12] + a->m[7] * b->m[13] + a->m[11] * b->m[14] + a->m[15] * b->m[15];
}

// Creates the transpose of the given matrix.
inline static void XrMatrix4x4f_Transpose(XrMatrix4x4f* result, const XrMatrix4x4f* src) {
    result->m[0] = src->m[0];
    result->m[1] = src->m[4];
    result->m[2] = src->m[8];
    result->m[3] = src->m[12];

    result->m[4] = src->m[1];
    result->m[5] = src->m[5];
    result->m[6] = src->m[9];
    result->m[7] = src->m[13];

    result->m[8] = src->m[2];
    result->m[9] = src->m[6];
    result->m[10] = src->m[10];
    result->m[11] = src->m[14];

    result->m[12] = src->m[3];
    result->m[13] = src->m[7];
    result->m[14] = src->m[11];
    result->m[15] = src->m[15];
}

// Returns a 3x3 minor of a 4x4 matrix.
inline static float XrMatrix4x4f_Minor(const XrMatrix4x4f* matrix, int r0, int r1, int r2, int c0, int c1, int c2) {
    return matrix->m[4 * r0 + c0] *
               (matrix->m[4 * r1 + c1] * matrix->m[4 * r2 + c2] - matrix->m[4 * r2 + c1] * matrix->m[4 * r1 + c2]) -
           matrix->m[4 * r0 + c1] *
               (matrix->m[4 * r1 + c0] * matrix->m[4 * r2 + c2] - matrix->m[4 * r2 + c0] * matrix->m[4 * r1 + c2]) +
           matrix->m[4 * r0 + c2] *
               (matrix->m[4 * r1 + c0] * matrix->m[4 * r2 + c1] - matrix->m[4 * r2 + c0] * matrix->m[4 * r1 + c1]);
}

// Calculates the inverse of a 4x4 matrix.
inline static void XrMatrix4x4f_Invert(XrMatrix4x4f* result, const XrMatrix4x4f* src) {
    const float rcpDet =
        1.0f / (src->m[0] * XrMatrix4x4f_Minor(src, 1, 2, 3, 1, 2, 3) - src->m[1] * XrMatrix4x4f_Minor(src, 1, 2, 3, 0, 2, 3) +
                src->m[2] * XrMatrix4x4f_Minor(src, 1, 2, 3, 0, 1, 3) - src->m[3] * XrMatrix4x4f_Minor(src, 1, 2, 3, 0, 1, 2));

    result->m[0] = XrMatrix4x4f_Minor(src, 1, 2, 3, 1, 2, 3) * rcpDet;
    result->m[1] = -XrMatrix4x4f_Minor(src, 0, 2, 3, 1, 2, 3) * rcpDet;
    result->m[2] = XrMatrix4x4f_Minor(src, 0, 1, 3, 1, 2, 3) * rcpDet;
    result->m[3] = -XrMatrix4x4f_Minor(src, 0, 1, 2, 1, 2, 3) * rcpDet;
    result->m[4] = -XrMatrix4x4f_Minor(src, 1, 2, 3, 0, 2, 3) * rcpDet;
    result->m[5] = XrMatrix4x4f_Minor(src, 0, 2, 3, 0, 2, 3) * rcpDet;
    result->m[6] = -XrMatrix4x4f_Minor(src, 0, 1, 3, 0, 2, 3) * rcpDet;
    result->m[7] = XrMatrix4x4f_Minor(src, 0, 1, 2, 0, 2, 3) * rcpDet;
    result->m[8] = XrMatrix4x4f_Minor(src, 1, 2, 3, 0, 1, 3) * rcpDet;
    result->m[9] = -XrMatrix4x4f_Minor(src, 0, 2, 3, 0, 1, 3) * rcpDet;
    result->m[10] = XrMatrix4x4f_Minor(src, 0, 1, 3, 0, 1, 3) * rcpDet;
    result->m[11] = -XrMatrix4x4f_Minor(src, 0, 1, 2, 0, 1, 3) * rcpDet;
    result->m[12] = -XrMatrix4x4f_Minor(src, 1, 2, 3, 0, 1, 2) * rcpDet;
    result->m[13] = XrMatrix4x4f_Minor(src, 0, 2, 3, 0, 1, 2) * rcpDet;
    result->m[14] = -XrMatrix4x4f_Minor(src, 0, 1, 3, 0, 1, 2) * rcpDet;
    result->m[15] = XrMatrix4x4f_Minor(src, 0, 1, 2, 0, 1, 2) * rcpDet;
}

// Calculates the inverse of a rigid body transform.
inline static void XrMatrix4x4f_InvertRigidBody(XrMatrix4x4f* result, const XrMatrix4x4f* src) {
    result->m[0] = src->m[0];
    result->m[1] = src->m[4];
    result->m[2] = src->m[8];
    result->m[3] = 0.0f;
    result->m[4] = src->m[1];
    result->m[5] = src->m[5];
    result->m[6] = src->m[9];
    result->m[7] = 0.0f;
    result->m[8] = src->m[2];
    result->m[9] = src->m[6];
    result->m[10] = src->m[10];
    result->m[11] = 0.0f;
    result->m[12] = -(src->m[0] * src->m[12] + src->m[1] * src->m[13] + src->m[2] * src->m[14]);
    result->m[13] = -(src->m[4] * src->m[12] + src->m[5] * src->m[13] + src->m[6] * src->m[14]);
    result->m[14] = -(src->m[8] * src->m[12] + src->m[9] * src->m[13] + src->m[10] * src->m[14]);
    result->m[15] = 1.0f;
}

// Creates an identity matrix.
inline static void XrMatrix4x4f_CreateIdentity(XrMatrix4x4f* result) {
    result->m[0] = 1.0f;
    result->m[1] = 0.0f;
    result->m[2] = 0.0f;
    result->m[3] = 0.0f;
    result->m[4] = 0.0f;
    result->m[5] = 1.0f;
    result->m[6] = 0.0f;
    result->m[7] = 0.0f;
    result->m[8] = 0.0f;
    result->m[9] = 0.0f;
    result->m[10] = 1.0f;
    result->m[11] = 0.0f;
    result->m[12] = 0.0f;
    result->m[13] = 0.0f;
    result->m[14] = 0.0f;
    result->m[15] = 1.0f;
}

// Creates a translation matrix.
inline static void XrMatrix4x4f_CreateTranslation(XrMatrix4x4f* result, const float x, const float y, const float z) {
    result->m[0] = 1.0f;
    result->m[1] = 0.0f;
    result->m[2] = 0.0f;
    result->m[3] = 0.0f;
    result->m[4] = 0.0f;
    result->m[5] = 1.0f;
    result->m[6] = 0.0f;
    result->m[7] = 0.0f;
    result->m[8] = 0.0f;
    result->m[9] = 0.0f;
    result->m[10] = 1.0f;
    result->m[11] = 0.0f;
    result->m[12] = x;
    result->m[13] = y;
    result->m[14] = z;
    result->m[15] = 1.0f;
}

// Creates a rotation matrix.
// If -Z=forward, +Y=up, +X=right, then degreesX=pitch, degreesY=yaw, degreesZ=roll.
inline static void XrMatrix4x4f_CreateRotation(XrMatrix4x4f* result, const float degreesX, const float degreesY,
                                               const float degreesZ) {
    const float sinX = sinf(degreesX * (MATH_PI / 180.0f));
    const float cosX = cosf(degreesX * (MATH_PI / 180.0f));
    const XrMatrix4x4f rotationX = {{1, 0, 0, 0, 0, cosX, sinX, 0, 0, -sinX, cosX, 0, 0, 0, 0, 1}};
    const float sinY = sinf(degreesY * (MATH_PI / 180.0f));
    const float cosY = cosf(degreesY * (MATH_PI / 180.0f));
    const XrMatrix4x4f rotationY = {{cosY, 0, -sinY, 0, 0, 1, 0, 0, sinY, 0, cosY, 0, 0, 0, 0, 1}};
    const float sinZ = sinf(degreesZ * (MATH_PI / 180.0f));
    const float cosZ = cosf(degreesZ * (MATH_PI / 180.0f));
    const XrMatrix4x4f rotationZ = {{cosZ, sinZ, 0, 0, -sinZ, cosZ, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1}};
    XrMatrix4x4f rotationXY;
    XrMatrix4x4f_Multiply(&rotationXY, &rotationY, &rotationX);
    XrMatrix4x4f_Multiply(result, &rotationZ, &rotationXY);
}

// Creates a scale matrix.
inline static void XrMatrix4x4f_CreateScale(XrMatrix4x4f* result, const float x, const float y, const float z) {
    result->m[0] = x;
    result->m[1] = 0.0f;
    result->m[2] = 0.0f;
    result->m[3] = 0.0f;
    result->m[4] = 0.0f;
    result->m[5] = y;
    result->m[6] = 0.0f;
    result->m[7] = 0.0f;
    result->m[8] = 0.0f;
    result->m[9] = 0.0f;
    result->m[10] = z;
    result->m[11] = 0.0f;
    result->m[12] = 0.0f;
    result->m[13] = 0.0f;
    result->m[14] = 0.0f;
    result->m[15] = 1.0f;
}

// Creates a matrix from a quaternion.
inline static void XrMatrix4x4f_CreateFromQuaternion(XrMatrix4x4f* result, const XrQuaternionf* quat) {
    const float x2 = quat->x + quat->x;
    const float y2 = quat->y + quat->y;
    const float z2 = quat->z + quat->z;

    const float xx2 = quat->x * x2;
    const float yy2 = quat->y * y2;
    const float zz2 = quat->z * z2;

    const float yz2 = quat->y * z2;
    const float wx2 = quat->w * x2;
    const float xy2 = quat->x * y2;
    const float wz2 = quat->w * z2;
    const float xz2 = quat->x * z2;
    const float wy2 = quat->w * y2;

    result->m[0] = 1.0f - yy2 - zz2;
    result->m[1] = xy2 + wz2;
    result->m[2] = xz2 - wy2;
    result->m[3] = 0.0f;

    result->m[4] = xy2 - wz2;
    result->m[5] = 1.0f - xx2 - zz2;
    result->m[6] = yz2 + wx2;
    result->m[7] = 0.0f;

    result->m[8] = xz2 + wy2;
    result->m[9] = yz2 - wx2;
    result->m[10] = 1.0f - xx2 - yy2;
    result->m[11] = 0.0f;

    result->m[12] = 0.0f;
    result->m[13] = 0.0f;
    result->m[14] = 0.0f;
    result->m[15] = 1.0f;
}

// Creates a combined translation(rotation(scale(object))) matrix.
inline static void XrMatrix4x4f_CreateTranslationRotationScale(XrMatrix4x4f* result, const XrVector3f* translation,
                                                               const XrQuaternionf* rotation, const XrVector3f* scale) {
    XrMatrix4x4f scaleMatrix;
    XrMatrix4x4f_CreateScale(&scaleMatrix, scale->x, scale->y, scale->z);

    XrMatrix4x4f rotationMatrix;
    XrMatrix4x4f_CreateFromQuaternion(&rotationMatrix, rotation);

    XrMatrix4x4f translationMatrix;
    XrMatrix4x4f_CreateTranslation(&translationMatrix, translation->x, translation->y, translation->z);

    XrMatrix4x4f combinedMatrix;
    XrMatrix4x4f_Multiply(&combinedMatrix, &rotationMatrix, &scaleMatrix);
    XrMatrix4x4f_Multiply(result, &translationMatrix, &combinedMatrix);
}

// Creates a projection matrix based on the specified dimensions.
// The projection matrix transforms -Z=forward, +Y=up, +X=right to the appropriate clip space for the graphics API.
// The far plane is placed at infinity if farZ <= nearZ.
// An infinite projection matrix is preferred for rasterization because, except for
// things *right* up against the near plane, it always provides better precision:
//              "Tightening the Precision of Perspective Rendering"
//              Paul Upchurch, Mathieu Desbrun
//              Journal of Graphics Tools, Volume 16, Issue 1, 2012
inline static void XrMatrix4x4f_CreateProjection(XrMatrix4x4f* result, GraphicsAPI graphicsApi, const float tanAngleLeft,
                                                 const float tanAngleRight, const float tanAngleUp, float const tanAngleDown,
                                                 const float nearZ, const float farZ) {
    const float tanAngleWidth = tanAngleRight - tanAngleLeft;

    // Set to tanAngleDown - tanAngleUp for a clip space with positive Y down (Vulkan).
    // Set to tanAngleUp - tanAngleDown for a clip space with positive Y up (OpenGL / D3D / Metal).
    const float tanAngleHeight = graphicsApi == GRAPHICS_VULKAN ? (tanAngleDown - tanAngleUp) : (tanAngleUp - tanAngleDown);

    // Set to nearZ for a [-1,1] Z clip space (OpenGL / OpenGL ES).
    // Set to zero for a [0,1] Z clip space (Vulkan / D3D / Metal).
    const float offsetZ = (graphicsApi == GRAPHICS_OPENGL || graphicsApi == GRAPHICS_OPENGL_ES) ? nearZ : 0;

    if (farZ <= nearZ) {
        // place the far plane at infinity
        result->m[0] = 2.0f / tanAngleWidth;
        result->m[4] = 0.0f;
        result->m[8] = (tanAngleRight + tanAngleLeft) / tanAngleWidth;
        result->m[12] = 0.0f;

        result->m[1] = 0.0f;
        result->m[5] = 2.0f / tanAngleHeight;
        result->m[9] = (tanAngleUp + tanAngleDown) / tanAngleHeight;
        result->m[13] = 0.0f;

        result->m[2] = 0.0f;
        result->m[6] = 0.0f;
        result->m[10] = -1.0f;
        result->m[14] = -(nearZ + offsetZ);

        result->m[3] = 0.0f;
        result->m[7] = 0.0f;
        result->m[11] = -1.0f;
        result->m[15] = 0.0f;
    } else {
        // normal projection
        result->m[0] = 2.0f / tanAngleWidth;
        result->m[4] = 0.0f;
        result->m[8] = (tanAngleRight + tanAngleLeft) / tanAngleWidth;
        result->m[12] = 0.0f;

        result->m[1] = 0.0f;
        result->m[5] = 2.0f / tanAngleHeight;
        result->m[9] = (tanAngleUp + tanAngleDown) / tanAngleHeight;
        result->m[13] = 0.0f;

        result->m[2] = 0.0f;
        result->m[6] = 0.0f;
        result->m[10] = -(farZ + offsetZ) / (farZ - nearZ);
        result->m[14] = -(farZ * (nearZ + offsetZ)) / (farZ - nearZ);

        result->m[3] = 0.0f;
        result->m[7] = 0.0f;
        result->m[11] = -1.0f;
        result->m[15] = 0.0f;
    }
}

// Creates a projection matrix based on the specified FOV.
inline static void XrMatrix4x4f_CreateProjectionFov(XrMatrix4x4f* result, GraphicsAPI graphicsApi, const XrFovf fov,
                                                    const float nearZ, const float farZ) {
    const float tanLeft = tanf(fov.angleLeft);
    const float tanRight = tanf(fov.angleRight);

    const float tanDown = tanf(fov.angleDown);
    const float tanUp = tanf(fov.angleUp);

    XrMatrix4x4f_CreateProjection(result, graphicsApi, tanLeft, tanRight, tanUp, tanDown, nearZ, farZ);
}

// Creates a matrix that transforms the -1 to 1 cube to cover the given 'mins' and 'maxs' transformed with the given 'matrix'.
inline static void XrMatrix4x4f_CreateOffsetScaleForBounds(XrMatrix4x4f* result, const XrMatrix4x4f* matrix, const XrVector3f* mins,
                                                           const XrVector3f* maxs) {
    const XrVector3f offset = {(maxs->x + mins->x) * 0.5f, (maxs->y + mins->y) * 0.5f, (maxs->z + mins->z) * 0.5f};
    const XrVector3f scale = {(maxs->x - mins->x) * 0.5f, (maxs->y - mins->y) * 0.5f, (maxs->z - mins->z) * 0.5f};

    result->m[0] = matrix->m[0] * scale.x;
    result->m[1] = matrix->m[1] * scale.x;
    result->m[2] = matrix->m[2] * scale.x;
    result->m[3] = matrix->m[3] * scale.x;

    result->m[4] = matrix->m[4] * scale.y;
    result->m[5] = matrix->m[5] * scale.y;
    result->m[6] = matrix->m[6] * scale.y;
    result->m[7] = matrix->m[7] * scale.y;

    result->m[8] = matrix->m[8] * scale.z;
    result->m[9] = matrix->m[9] * scale.z;
    result->m[10] = matrix->m[10] * scale.z;
    result->m[11] = matrix->m[11] * scale.z;

    result->m[12] = matrix->m[12] + matrix->m[0] * offset.x + matrix->m[4] * offset.y + matrix->m[8] * offset.z;
    result->m[13] = matrix->m[13] + matrix->m[1] * offset.x + matrix->m[5] * offset.y + matrix->m[9] * offset.z;
    result->m[14] = matrix->m[14] + matrix->m[2] * offset.x + matrix->m[6] * offset.y + matrix->m[10] * offset.z;
    result->m[15] = matrix->m[15] + matrix->m[3] * offset.x + matrix->m[7] * offset.y + matrix->m[11] * offset.z;
}

// Returns true if the given matrix is affine.
inline static bool XrMatrix4x4f_IsAffine(const XrMatrix4x4f* matrix, const float epsilon) {
    return fabsf(matrix->m[3]) <= epsilon && fabsf(matrix->m[7]) <= epsilon && fabsf(matrix->m[11]) <= epsilon &&
           fabsf(matrix->m[15] - 1.0f) <= epsilon;
}

// Returns true if the given matrix is orthogonal.
inline static bool XrMatrix4x4f_IsOrthogonal(const XrMatrix4x4f* matrix, const float epsilon) {
    for (int i = 0; i < 3; i++) {
        for (int j = 0; j < 3; j++) {
            if (i != j) {
                if (fabsf(matrix->m[4 * i + 0] * matrix->m[4 * j + 0] + matrix->m[4 * i + 1] * matrix->m[4 * j + 1] +
                          matrix->m[4 * i + 2] * matrix->m[4 * j + 2]) > epsilon) {
                    return false;
                }
                if (fabsf(matrix->m[4 * 0 + i] * matrix->m[4 * 0 + j] + matrix->m[4 * 1 + i] * matrix->m[4 * 1 + j] +
                          matrix->m[4 * 2 + i] * matrix->m[4 * 2 + j]) > epsilon) {
                    return false;
                }
            }
        }
    }
    return true;
}

// Returns true if the given matrix is orthonormal.
inline static bool XrMatrix4x4f_IsOrthonormal(const XrMatrix4x4f* matrix, const float epsilon) {
    for (int i = 0; i < 3; i++) {
        for (int j = 0; j < 3; j++) {
            const float kd = (i == j) ? 1.0f : 0.0f;  // Kronecker delta
            if (fabsf(kd - (matrix->m[4 * i + 0] * matrix->m[4 * j + 0] + matrix->m[4 * i + 1] * matrix->m[4 * j + 1] +
                            matrix->m[4 * i + 2] * matrix->m[4 * j + 2])) > epsilon) {
                return false;
            }
            if (fabsf(kd - (matrix->m[4 * 0 + i] * matrix->m[4 * 0 + j] + matrix->m[4 * 1 + i] * matrix->m[4 * 1 + j] +
                            matrix->m[4 * 2 + i] * matrix->m[4 * 2 + j])) > epsilon) {
                return false;
            }
        }
    }
    return true;
}

// Returns true if the given matrix is a rigid body transform.
inline static bool XrMatrix4x4f_IsRigidBody(const XrMatrix4x4f* matrix, const float epsilon) {
    return XrMatrix4x4f_IsAffine(matrix, epsilon) && XrMatrix4x4f_IsOrthonormal(matrix, epsilon);
}

// Get the translation from a combined translation(rotation(scale(object))) matrix.
inline static void XrMatrix4x4f_GetTranslation(XrVector3f* result, const XrMatrix4x4f* src) {
    assert(XrMatrix4x4f_IsAffine(src, 1e-4f));
    assert(XrMatrix4x4f_IsOrthogonal(src, 1e-4f));

    result->x = src->m[12];
    result->y = src->m[13];
    result->z = src->m[14];
}

// Get the rotation from a combined translation(rotation(scale(object))) matrix.
inline static void XrMatrix4x4f_GetRotation(XrQuaternionf* result, const XrMatrix4x4f* src) {
    assert(XrMatrix4x4f_IsAffine(src, 1e-4f));
    assert(XrMatrix4x4f_IsOrthogonal(src, 1e-4f));

    const float rcpScaleX = XrRcpSqrt(src->m[0] * src->m[0] + src->m[1] * src->m[1] + src->m[2] * src->m[2]);
    const float rcpScaleY = XrRcpSqrt(src->m[4] * src->m[4] + src->m[5] * src->m[5] + src->m[6] * src->m[6]);
    const float rcpScaleZ = XrRcpSqrt(src->m[8] * src->m[8] + src->m[9] * src->m[9] + src->m[10] * src->m[10]);
    const float m[9] = {src->m[0] * rcpScaleX, src->m[1] * rcpScaleX, src->m[2] * rcpScaleX,
                        src->m[4] * rcpScaleY, src->m[5] * rcpScaleY, src->m[6] * rcpScaleY,
                        src->m[8] * rcpScaleZ, src->m[9] * rcpScaleZ, src->m[10] * rcpScaleZ};
    if (m[0 * 3 + 0] + m[1 * 3 + 1] + m[2 * 3 + 2] > 0.0f) {
        float t = +m[0 * 3 + 0] + m[1 * 3 + 1] + m[2 * 3 + 2] + 1.0f;
        float s = XrRcpSqrt(t) * 0.5f;
        result->w = s * t;
        result->z = (m[0 * 3 + 1] - m[1 * 3 + 0]) * s;
        result->y = (m[2 * 3 + 0] - m[0 * 3 + 2]) * s;
        result->x = (m[1 * 3 + 2] - m[2 * 3 + 1]) * s;
    } else if (m[0 * 3 + 0] > m[1 * 3 + 1] && m[0 * 3 + 0] > m[2 * 3 + 2]) {
        float t = +m[0 * 3 + 0] - m[1 * 3 + 1] - m[2 * 3 + 2] + 1.0f;
        float s = XrRcpSqrt(t) * 0.5f;
        result->x = s * t;
        result->y = (m[0 * 3 + 1] + m[1 * 3 + 0]) * s;
        result->z = (m[2 * 3 + 0] + m[0 * 3 + 2]) * s;
        result->w = (m[1 * 3 + 2] - m[2 * 3 + 1]) * s;
    } else if (m[1 * 3 + 1] > m[2 * 3 + 2]) {
        float t = -m[0 * 3 + 0] + m[1 * 3 + 1] - m[2 * 3 + 2] + 1.0f;
        float s = XrRcpSqrt(t) * 0.5f;
        result->y = s * t;
        result->x = (m[0 * 3 + 1] + m[1 * 3 + 0]) * s;
        result->w = (m[2 * 3 + 0] - m[0 * 3 + 2]) * s;
        result->z = (m[1 * 3 + 2] + m[2 * 3 + 1]) * s;
    } else {
        float t = -m[0 * 3 + 0] - m[1 * 3 + 1] + m[2 * 3 + 2] + 1.0f;
        float s = XrRcpSqrt(t) * 0.5f;
        result->z = s * t;
        result->w = (m[0 * 3 + 1] - m[1 * 3 + 0]) * s;
        result->x = (m[2 * 3 + 0] + m[0 * 3 + 2]) * s;
        result->y = (m[1 * 3 + 2] + m[2 * 3 + 1]) * s;
    }
}

// Get the scale from a combined translation(rotation(scale(object))) matrix.
inline static void XrMatrix4x4f_GetScale(XrVector3f* result, const XrMatrix4x4f* src) {
    assert(XrMatrix4x4f_IsAffine(src, 1e-4f));
    assert(XrMatrix4x4f_IsOrthogonal(src, 1e-4f));

    result->x = sqrtf(src->m[0] * src->m[0] + src->m[1] * src->m[1] + src->m[2] * src->m[2]);
    result->y = sqrtf(src->m[4] * src->m[4] + src->m[5] * src->m[5] + src->m[6] * src->m[6]);
    result->z = sqrtf(src->m[8] * src->m[8] + src->m[9] * src->m[9] + src->m[10] * src->m[10]);
}

// Transforms a 3D vector.
inline static void XrMatrix4x4f_TransformVector3f(XrVector3f* result, const XrMatrix4x4f* m, const XrVector3f* v) {
    const float w = m->m[3] * v->x + m->m[7] * v->y + m->m[11] * v->z + m->m[15];
    const float rcpW = 1.0f / w;
    result->x = (m->m[0] * v->x + m->m[4] * v->y + m->m[8] * v->z + m->m[12]) * rcpW;
    result->y = (m->m[1] * v->x + m->m[5] * v->y + m->m[9] * v->z + m->m[13]) * rcpW;
    result->z = (m->m[2] * v->x + m->m[6] * v->y + m->m[10] * v->z + m->m[14]) * rcpW;
}

// Transforms a 4D vector.
inline static void XrMatrix4x4f_TransformVector4f(XrVector4f* result, const XrMatrix4x4f* m, const XrVector4f* v) {
    result->x = m->m[0] * v->x + m->m[4] * v->y + m->m[8] * v->z + m->m[12] * v->w;
    result->y = m->m[1] * v->x + m->m[5] * v->y + m->m[9] * v->z + m->m[13] * v->w;
    result->z = m->m[2] * v->x + m->m[6] * v->y + m->m[10] * v->z + m->m[14] * v->w;
    result->w = m->m[3] * v->x + m->m[7] * v->y + m->m[11] * v->z + m->m[15] * v->w;
}

// Transforms the 'mins' and 'maxs' bounds with the given 'matrix'.
inline static void XrMatrix4x4f_TransformBounds(XrVector3f* resultMins, XrVector3f* resultMaxs, const XrMatrix4x4f* matrix,
                                                const XrVector3f* mins, const XrVector3f* maxs) {
    assert(XrMatrix4x4f_IsAffine(matrix, 1e-4f));

    const XrVector3f center = {(mins->x + maxs->x) * 0.5f, (mins->y + maxs->y) * 0.5f, (mins->z + maxs->z) * 0.5f};
    const XrVector3f extents = {maxs->x - center.x, maxs->y - center.y, maxs->z - center.z};
    const XrVector3f newCenter = {matrix->m[0] * center.x + matrix->m[4] * center.y + matrix->m[8] * center.z + matrix->m[12],
                                  matrix->m[1] * center.x + matrix->m[5] * center.y + matrix->m[9] * center.z + matrix->m[13],
                                  matrix->m[2] * center.x + matrix->m[6] * center.y + matrix->m[10] * center.z + matrix->m[14]};
    const XrVector3f newExtents = {
        fabsf(extents.x * matrix->m[0]) + fabsf(extents.y * matrix->m[4]) + fabsf(extents.z * matrix->m[8]),
        fabsf(extents.x * matrix->m[1]) + fabsf(extents.y * matrix->m[5]) + fabsf(extents.z * matrix->m[9]),
        fabsf(extents.x * matrix->m[2]) + fabsf(extents.y * matrix->m[6]) + fabsf(extents.z * matrix->m[10])};
    XrVector3f_Sub(resultMins, &newCenter, &newExtents);
    XrVector3f_Add(resultMaxs, &newCenter, &newExtents);
}

// Returns true if the 'mins' and 'maxs' bounds is completely off to one side of the projection matrix.
inline static bool XrMatrix4x4f_CullBounds(const XrMatrix4x4f* mvp, const XrVector3f* mins, const XrVector3f* maxs) {
    if (maxs->x <= mins->x && maxs->y <= mins->y && maxs->z <= mins->z) {
        return false;
    }

    XrVector4f c[8];
    for (int i = 0; i < 8; i++) {
        const XrVector4f corner = {(i & 1) != 0 ? maxs->x : mins->x, (i & 2) != 0 ? maxs->y : mins->y,
                                   (i & 4) != 0 ? maxs->z : mins->z, 1.0f};
        XrMatrix4x4f_TransformVector4f(&c[i], mvp, &corner);
    }

    int i;
    for (i = 0; i < 8; i++) {
        if (c[i].x > -c[i].w) {
            break;
        }
    }
    if (i == 8) {
        return true;
    }
    for (i = 0; i < 8; i++) {
        if (c[i].x < c[i].w) {
            break;
        }
    }
    if (i == 8) {
        return true;
    }

    for (i = 0; i < 8; i++) {
        if (c[i].y > -c[i].w) {
            break;
        }
    }
    if (i == 8) {
        return true;
    }
    for (i = 0; i < 8; i++) {
        if (c[i].y < c[i].w) {
            break;
        }
    }
    if (i == 8) {
        return true;
    }
    for (i = 0; i < 8; i++) {
        if (c[i].z > -c[i].w) {
            break;
        }
    }
    if (i == 8) {
        return true;
    }
    for (i = 0; i < 8; i++) {
        if (c[i].z < c[i].w) {
            break;
        }
    }
    return i == 8;
}

#endif  // XR_LINEAR_H_