AAPLMathUtilities.m

/*
 See LICENSE folder for this sample’s licensing information.
 
 Abstract:
 Implementation of vector, matrix, and quaternion math utility functions useful for 3D graphics
 rendering with Metal
 */

#import "AAPLMathUtilities.h"
#include <assert.h>
#include <stdlib.h>

uint32_t seed_lo, seed_hi;

static float inline F16ToF32(const __fp16 *address) {
    return *address;
}

float AAPL_SIMD_OVERLOAD float32_from_float16(uint16_t i) {
    return F16ToF32((__fp16 *)&i);
}

static inline void F32ToF16(float F32, __fp16 *F16Ptr) {
    *F16Ptr = F32;
}

uint16_t AAPL_SIMD_OVERLOAD float16_from_float32(float f) {
    uint16_t f16;
    F32ToF16(f, (__fp16 *)&f16);
    return f16;
}

float AAPL_SIMD_OVERLOAD  random_float(float min, float max) {
    return (((double)rand()/RAND_MAX) * (max-min)) + min;
}

float AAPL_SIMD_OVERLOAD degrees_from_radians(float radians) {
    return (radians / M_PI) * 180;
}

float AAPL_SIMD_OVERLOAD radians_from_degrees(float degrees) {
    return (degrees / 180) * M_PI;
}

static vector_float3 AAPL_SIMD_OVERLOAD vector_make(float x, float y, float z) {
    return (vector_float3){ x, y, z };
}

vector_float3 AAPL_SIMD_OVERLOAD vector_lerp(vector_float3 v0, vector_float3 v1, float t) {
    return ((1 - t) * v0) + (t * v1);
}

vector_float4 AAPL_SIMD_OVERLOAD vector_lerp(vector_float4 v0, vector_float4 v1, float t) {
    return ((1 - t) * v0) + (t * v1);
}

matrix_float3x3 AAPL_SIMD_OVERLOAD matrix_make(float m00, float m10, float m20,
                                               float m01, float m11, float m21,
                                               float m02, float m12, float m22) {
    return (matrix_float3x3){ {
        { m00, m10, m20 },
        { m01, m11, m21 },
        { m02, m12, m22 } } };
}

matrix_float4x4 AAPL_SIMD_OVERLOAD matrix_make(float m00, float m10, float m20, float m30,
                                               float m01, float m11, float m21, float m31,
                                               float m02, float m12, float m22, float m32,
                                               float m03, float m13, float m23, float m33) {
    return (matrix_float4x4){ {
        { m00, m10, m20, m30 },
        { m01, m11, m21, m31 },
        { m02, m12, m22, m32 },
        { m03, m13, m23, m33 } } };
}

matrix_float3x3 AAPL_SIMD_OVERLOAD matrix_make(vector_float3 col0,
                                               vector_float3 col1,
                                               vector_float3 col2) {
    return (matrix_float3x3){ col0, col1, col2 };
}

matrix_float4x4 AAPL_SIMD_OVERLOAD matrix_make(vector_float4 col0,
                                               vector_float4 col1,
                                               vector_float4 col2,
                                               vector_float4 col3) {
    return (matrix_float4x4){ col0, col1, col2, col3};
}

matrix_float3x3 AAPL_SIMD_OVERLOAD matrix3x3_from_quaternion(quaternion_float q) {
    float xx = q.x * q.x;
    float xy = q.x * q.y;
    float xz = q.x * q.z;
    float xw = q.x * q.w;
    float yy = q.y * q.y;
    float yz = q.y * q.z;
    float yw = q.y * q.w;
    float zz = q.z * q.z;
    float zw = q.z * q.w;
    
    float m00 = 1 - 2 * (yy + zz);
    float m01 = 2 * (xy - zw);
    float m02 = 2 * (xz + yw);
    
    float m10 = 2 * (xy + zw);
    float m11 = 1 - 2 * (xx + zz);
    float m12 = 2 * (yz - xw);
    
    float m20 = 2 * (xz - yw);
    float m21 = 2 * (yz + xw);
    float m22 = 1 - 2 * (xx + yy);
    
    return matrix_make(m00, m10, m20,
                       m01, m11, m21,
                       m02, m12, m22);
}

matrix_float3x3 AAPL_SIMD_OVERLOAD matrix3x3_rotation(float radians, vector_float3 axis) {
    axis = vector_normalize(axis);
    float ct = cosf(radians);
    float st = sinf(radians);
    float ci = 1 - ct;
    float x = axis.x, y = axis.y, z = axis.z;
    return matrix_make(ct + x * x * ci, y * x * ci + z * st, z * x * ci - y * st,
                       x * y * ci - z * st, ct + y * y * ci, z * y * ci + x * st,
                       x * z * ci + y * st, y * z * ci - x * st, ct + z * z * ci);
}

matrix_float3x3 AAPL_SIMD_OVERLOAD matrix3x3_rotation(float radians, float x, float y, float z) {
    return matrix3x3_rotation(radians, vector_make(x, y, z));
}

matrix_float3x3 AAPL_SIMD_OVERLOAD matrix3x3_scale(float sx, float sy, float sz) {
    return matrix_make(sx, 0, 0, 0, sy, 0, 0, 0, sz);
}

matrix_float3x3 AAPL_SIMD_OVERLOAD matrix3x3_scale(vector_float3 s) {
    return matrix_make(s.x, 0, 0, 0, s.y, 0, 0, 0, s.z);
}

matrix_float3x3 AAPL_SIMD_OVERLOAD matrix3x3_upper_left(matrix_float4x4 m) {
    vector_float3 x = m.columns[0].xyz;
    vector_float3 y = m.columns[1].xyz;
    vector_float3 z = m.columns[2].xyz;
    return matrix_make(x, y, z);
}

matrix_float3x3 AAPL_SIMD_OVERLOAD matrix_inverse_transpose(matrix_float3x3 m) {
    return matrix_invert(matrix_transpose(m));
}

matrix_float4x4 AAPL_SIMD_OVERLOAD matrix4x4_from_quaternion(quaternion_float q) {
    
    float xx = q.x * q.x;
    float xy = q.x * q.y;
    float xz = q.x * q.z;
    float xw = q.x * q.w;
    float yy = q.y * q.y;
    float yz = q.y * q.z;
    float yw = q.y * q.w;
    float zz = q.z * q.z;
    float zw = q.z * q.w;
    
    float m00 = 1 - 2 * (yy + zz);
    float m01 = 2 * (xy - zw);
    float m02 = 2 * (xz + yw);
    
    float m10 = 2 * (xy + zw);
    float m11 = 1 - 2 * (xx + zz);
    float m12 = 2 * (yz - xw);
    
    float m20 = 2 * (xz - yw);
    float m21 = 2 * (yz + xw);
    float m22 = 1 - 2 * (xx + yy);
    
    matrix_float4x4 matrix = matrix_make(m00, m10, m20, 0,
                                         m01, m11, m21, 0,
                                         m02, m12, m22, 0,
                                         0, 0, 0, 1);
    return matrix;
}

matrix_float4x4 AAPL_SIMD_OVERLOAD matrix4x4_rotation(float radians, vector_float3 axis) {
    axis = vector_normalize(axis);
    float ct = cosf(radians);
    float st = sinf(radians);
    float ci = 1 - ct;
    float x = axis.x, y = axis.y, z = axis.z;
    return matrix_make(ct + x * x * ci, y * x * ci + z * st, z * x * ci - y * st, 0,
                       x * y * ci - z * st, ct + y * y * ci, z * y * ci + x * st, 0,
                       x * z * ci + y * st, y * z * ci - x * st, ct + z * z * ci, 0,
                       0, 0, 0, 1);
}

matrix_float4x4 AAPL_SIMD_OVERLOAD matrix4x4_rotation(float radians, float x, float y, float z) {
    return matrix4x4_rotation(radians, vector_make(x, y, z));
}

matrix_float4x4 AAPL_SIMD_OVERLOAD matrix4x4_scale(float sx, float sy, float sz) {
    return matrix_make(sx, 0, 0, 0,
                       0, sy, 0, 0,
                       0, 0, sz, 0,
                       0, 0, 0, 1);
}

matrix_float4x4 AAPL_SIMD_OVERLOAD matrix4x4_scale(vector_float3 s) {
    return matrix_make(s.x, 0, 0, 0,
                       0, s.y, 0, 0,
                       0, 0, s.z, 0,
                       0, 0, 0, 1);
}

matrix_float4x4 AAPL_SIMD_OVERLOAD matrix4x4_translation(float tx, float ty, float tz) {
    return matrix_make(1, 0, 0, 0,
                       0, 1, 0, 0,
                       0, 0, 1, 0,
                       tx, ty, tz, 1);
}

matrix_float4x4 AAPL_SIMD_OVERLOAD matrix4x4_translation(vector_float3 t) {
    return matrix_make(1, 0, 0, 0,
                       0, 1, 0, 0,
                       0, 0, 1, 0,
                       t.x, t.y, t.z, 1);
}

matrix_float4x4 AAPL_SIMD_OVERLOAD matrix_look_at_left_hand(vector_float3 eye,
                                                            vector_float3 target,
                                                            vector_float3 up) {
    vector_float3 z = vector_normalize(target - eye);
    vector_float3 x = vector_normalize(vector_cross(up, z));
    vector_float3 y = vector_cross(z, x);
    vector_float3 t = vector_make(-vector_dot(x, eye), -vector_dot(y, eye), -vector_dot(z, eye));
    
    return matrix_make(x.x, y.x, z.x, 0,
                       x.y, y.y, z.y, 0,
                       x.z, y.z, z.z, 0,
                       t.x, t.y, t.z, 1);
}

matrix_float4x4 AAPL_SIMD_OVERLOAD matrix_look_at_left_hand(float eyeX, float eyeY, float eyeZ,
                                                            float centerX, float centerY, float centerZ,
                                                            float upX, float upY, float upZ) {
    vector_float3 eye = vector_make(eyeX, eyeY, eyeZ);
    vector_float3 center = vector_make(centerX, centerY, centerZ);
    vector_float3 up = vector_make(upX, upY, upZ);
    
    return matrix_look_at_left_hand(eye, center, up);
}

matrix_float4x4 AAPL_SIMD_OVERLOAD matrix_look_at_right_hand(vector_float3 eye,
                                                             vector_float3 target,
                                                             vector_float3 up) {
    vector_float3 z = vector_normalize(eye - target);
    vector_float3 x = vector_normalize(vector_cross(up, z));
    vector_float3 y = vector_cross(z, x);
    vector_float3 t = vector_make(-vector_dot(x, eye), -vector_dot(y, eye), -vector_dot(z, eye));
    
    return matrix_make(x.x, y.x, z.x, 0,
                       x.y, y.y, z.y, 0,
                       x.z, y.z, z.z, 0,
                       t.x, t.y, t.z, 1);
}

matrix_float4x4 AAPL_SIMD_OVERLOAD matrix_look_at_right_hand(float eyeX, float eyeY, float eyeZ,
                                                             float centerX, float centerY, float centerZ,
                                                             float upX, float upY, float upZ) {
    vector_float3 eye = vector_make(eyeX, eyeY, eyeZ);
    vector_float3 center = vector_make(centerX, centerY, centerZ);
    vector_float3 up = vector_make(upX, upY, upZ);
    
    return matrix_look_at_right_hand(eye, center, up);
}

matrix_float4x4 AAPL_SIMD_OVERLOAD matrix_ortho_right_hand(float left, float right, float bottom, float top, float nearZ, float farZ) {
    return matrix_make(2 / (right - left), 0, 0, 0,
                       0, 2 / (top - bottom), 0, 0,
                       0, 0, -1 / (farZ - nearZ), 0,
                       (left + right) / (left - right), (top + bottom) / (bottom - top), nearZ / (nearZ - farZ), 1);
}

matrix_float4x4 AAPL_SIMD_OVERLOAD matrix_ortho_left_hand(float left, float right, float bottom, float top, float nearZ, float farZ) {
    return matrix_make(2 / (right - left), 0, 0, 0,
                       0, 2 / (top - bottom), 0, 0,
                       0, 0, 1 / (farZ - nearZ), 0,
                       (left + right) / (left - right), (top + bottom) / (bottom - top), nearZ / (nearZ - farZ), 1);
}

matrix_float4x4 AAPL_SIMD_OVERLOAD matrix_perspective_right_hand(float fovyRadians, float aspect, float nearZ, float farZ) {
    float ys = 1 / tanf(fovyRadians * 0.5);
    float xs = ys / aspect;
    float zs = farZ / (nearZ - farZ);
    return matrix_make(xs, 0, 0,  0,
                       0, ys, 0,  0,
                       0, 0, zs, -1,
                       0, 0, nearZ * zs, 0);
}

matrix_float4x4 AAPL_SIMD_OVERLOAD matrix_perspective_left_hand(float fovyRadians, float aspect, float nearZ, float farZ) {
    float ys = 1 / tanf(fovyRadians * 0.5);
    float xs = ys / aspect;
    float zs = farZ / (farZ - nearZ);
    return matrix_make(xs, 0, 0, 0,
                       0, ys, 0, 0,
                       0, 0, zs, 1,
                       0, 0, -nearZ * zs, 0);
}

matrix_float4x4 AAPL_SIMD_OVERLOAD matrix_inverse_transpose(matrix_float4x4 m) {
    return matrix_invert(matrix_transpose(m));
}

quaternion_float AAPL_SIMD_OVERLOAD quaternion(float x, float y, float z, float w) {
    return (quaternion_float){ x, y, z, w };
}

quaternion_float AAPL_SIMD_OVERLOAD quaternion(vector_float3 v, float w) {
    return (quaternion_float){ v.x, v.y, v.z, w };
}

quaternion_float AAPL_SIMD_OVERLOAD quaternion_identity() {
    return quaternion(0, 0, 0, 1);
}

quaternion_float AAPL_SIMD_OVERLOAD quaternion_from_axis_angle(vector_float3 axis, float radians) {
    float t = radians * 0.5;
    return quaternion(axis.x * sinf(t), axis.y * sinf(t), axis.z * sinf(t), cosf(t));
}

quaternion_float AAPL_SIMD_OVERLOAD quaternion_from_euler(vector_float3 euler) {
    quaternion_float q;
    
    float cx = cosf(euler.x / 2.f);
    float cy = cosf(euler.y / 2.f);
    float cz = cosf(euler.z / 2.f);
    float sx = sinf(euler.x / 2.f);
    float sy = sinf(euler.y / 2.f);
    float sz = sinf(euler.z / 2.f);
    
    q.w = cx * cy * cz + sx * sy * sz;
    q.x = sx * cy * cz - cx * sy * sz;
    q.y = cx * sy * cz + sx * cy * sz;
    q.z = cx * cy * sz - sx * sy * cz;
    
    return q;
}

quaternion_float AAPL_SIMD_OVERLOAD quaternion(matrix_float3x3 m) {
    float m00 = m.columns[0].x;
    float m11 = m.columns[1].y;
    float m22 = m.columns[2].z;
    float x = sqrtf(1 + m00 - m11 - m22) * 0.5;
    float y = sqrtf(1 - m00 + m11 - m22) * 0.5;
    float z = sqrtf(1 - m00 - m11 + m22) * 0.5;
    float w = sqrtf(1 + m00 + m11 + m22) * 0.5;
    return quaternion(x, y, z, w);
}

quaternion_float AAPL_SIMD_OVERLOAD quaternion(matrix_float4x4 m) {
    return quaternion(matrix3x3_upper_left(m));
}

float AAPL_SIMD_OVERLOAD quaternion_length(quaternion_float q) {
    return sqrt(q.x*q.x + q.y*q.y + q.z*q.z + q.w*q.w);
}

float AAPL_SIMD_OVERLOAD quaternion_length_squared(quaternion_float q) {
    return q.x*q.x + q.y*q.y + q.z*q.z + q.w*q.w;
}

vector_float3 AAPL_SIMD_OVERLOAD quaternion_axis(quaternion_float q) {
    // This query doesn't make sense if w > 1, but we do our best by
    // forcing q to be a unit quaternion if it obviously isn't
    if (q.w > 1.0)
    {
        q = quaternion_normalize(q);
    }
    
    float axisLen = sqrtf(1 - q.w * q.w);
    
    if (axisLen < 1e-5)
    {
        // At lengths this small, direction is arbitrary
        return vector_make(1, 0, 0);
    }
    else
    {
        return vector_make(q.x / axisLen, q.y / axisLen, q.z / axisLen);
    }
}

float AAPL_SIMD_OVERLOAD quaternion_angle(quaternion_float q) {
    return 2 * acosf(q.w);
}

quaternion_float AAPL_SIMD_OVERLOAD quaternion_normalize(quaternion_float q) {
    return q / quaternion_length(q);
}

quaternion_float AAPL_SIMD_OVERLOAD quaternion_inverse(quaternion_float q) {
    return quaternion_conjugate(q) / quaternion_length_squared(q);
}

quaternion_float AAPL_SIMD_OVERLOAD quaternion_conjugate(quaternion_float q) {
    return quaternion(-q.x, -q.y, -q.z, q.w);
}

quaternion_float AAPL_SIMD_OVERLOAD quaternion_multiply(quaternion_float q0, quaternion_float q1) {
    quaternion_float q;
    
    q.x = q0.w*q1.x + q0.x*q1.w + q0.y*q1.z - q0.z*q1.y;
    q.y = q0.w*q1.y - q0.x*q1.z + q0.y*q1.w + q0.z*q1.x;
    q.z = q0.w*q1.z + q0.x*q1.y - q0.y*q1.x + q0.z*q1.w;
    q.w = q0.w*q1.w - q0.x*q1.x - q0.y*q1.y - q0.z*q1.z;
    return q;
}

quaternion_float AAPL_SIMD_OVERLOAD quaternion_slerp(quaternion_float q0, quaternion_float q1, float t) {
    
    quaternion_float q;
    
    float cosHalfTheta = vector_dot(q0, q1);
    if (fabs(cosHalfTheta) >= 1.f) ///q0=q1 or q0=q1
    {
        return q0;
    }
    
    float halfTheta = acosf(cosHalfTheta);
    float sinHalfTheta = sqrtf(1.f - cosHalfTheta * cosHalfTheta);
    if (fabs(sinHalfTheta) < 0.001f)
    {    // q0 & q1 180 degrees not defined
        return q0*0.5f + q1*0.5f;
    }
    float srcWeight = sin((1 - t) * halfTheta) / sinHalfTheta;
    float dstWeight = sin(t * halfTheta) / sinHalfTheta;
    
    q = srcWeight*q0 + dstWeight*q1;
    
    return q;
}

vector_float3 AAPL_SIMD_OVERLOAD quaternion_rotate_vector(quaternion_float q, vector_float3 v) {
    vector_float3 qp = vector_make(q.x, q.y, q.z);
    float w = q.w;
    return 2 * vector_dot(qp, v) * qp +
    ((w * w) - vector_dot(qp, qp)) * v +
    2 * w * vector_cross(qp, v);
}

quaternion_float AAPL_SIMD_OVERLOAD quaternion_from_matrix3x3(matrix_float3x3 m)
{
    quaternion_float q;
    
    float trace = 1 + m.columns[0][0] + m.columns[1][1] + m.columns[2][2];
    
    if(trace > 0)
    {
        float diagonal = sqrt(trace) * 2.0;
        
        q.x = (m.columns[2][1] - m.columns[1][2]) / diagonal;
        q.y = (m.columns[0][2] - m.columns[2][0]) / diagonal;
        q.z = (m.columns[1][0] - m.columns[0][1]) / diagonal;
        q.w = diagonal / 4.0;
        
    }  else if ((m.columns[0][0] > m.columns[1][1] ) &&
                (m.columns[0][0] > m.columns[2][2])) {
        
        float diagonal = sqrt(1.0 + m.columns[0][0] - m.columns[1][1] -  m.columns[2][2]) * 2.0;
        
        q.x = diagonal / 4.0;
        q.y = (m.columns[0][1] + m.columns[1][0]) / diagonal;
        q.z = (m.columns[0][2] + m.columns[2][0]) / diagonal;
        q.w = (m.columns[2][1] - m.columns[1][2]) / diagonal;
        
    } else if ( m.columns[1][1] >  m.columns[2][2]) {
        
        float diagonal = sqrt(1.0 + m.columns[1][1] - m.columns[0][0] - m.columns[2][2]) * 2.0;
        
        q.x = (m.columns[0][1] + m.columns[1][0]) / diagonal;
        q.y = diagonal / 4.0;
        q.z = (m.columns[1][2] + m.columns[2][1]) / diagonal;
        q.w = (m.columns[0][2] - m.columns[2][0]) / diagonal;
        
    } else {
        
        float diagonal = sqrt(1.0 + m.columns[2][2] - m.columns[0][0] - m.columns[1][1]) * 2.0;
        
        q.x = (m.columns[0][2] + m.columns[2][0]) / diagonal;
        q.y = (m.columns[1][2] + m.columns[2][1]) / diagonal;
        q.z = diagonal / 4.0;
        q.w = (m.columns[1][0] - m.columns[0][1]) / diagonal;
    }
    
    q = quaternion_normalize(q);
    return q;
}

static inline quaternion_float AAPL_SIMD_OVERLOAD quaternion_from_direction_vectors(vector_float3 forward, vector_float3 up, int right_handed) {
    
    forward = vector_normalize(forward);
    up = vector_normalize(up);
    
    vector_float3 side = vector_normalize(vector_cross(up, forward));
    
    matrix_float3x3 m = { side, up, forward };
    
    quaternion_float q = quaternion_from_matrix3x3(m);
    
    if(right_handed) {
        q = q.yxwz;
        q.xw = -q.xw;
    }
    
    q = vector_normalize(q);
    
    return q;
}

quaternion_float AAPL_SIMD_OVERLOAD quaternion_from_direction_vectors_right_hand(vector_float3 forward, vector_float3 up) {
    
    return quaternion_from_direction_vectors(forward, up, 1);
}

quaternion_float AAPL_SIMD_OVERLOAD quaternion_from_direction_vectors_left_hand(vector_float3 forward, vector_float3 up) {
    
    return quaternion_from_direction_vectors(forward, up, 0);
}

vector_float3 AAPL_SIMD_OVERLOAD forward_direction_vector_from_quaternion(quaternion_float q) {
    vector_float3 direction;
    direction.x = 2.0 * (q.x*q.z - q.w*q.y);
    direction.y = 2.0 * (q.y*q.z + q.w*q.x);
    direction.z = 1.0 - 2.0 * ((q.x * q.x) + (q.y * q.y));
    
    direction = vector_normalize(direction);
    return direction;
}

vector_float3 AAPL_SIMD_OVERLOAD up_direction_vector_from_quaternion(quaternion_float q) {
    vector_float3 direction;
    direction.x = 2.0 * (q.x*q.y + q.w*q.z);
    direction.y = 1.0 - 2.0 * (q.x*q.x + q.z*q.z);
    direction.z = 2.0 * (q.y*q.z - q.w*q.x);
    
    direction = vector_normalize(direction);
    // Negate for a right-handed coordinate system
    return direction;
}

vector_float3 AAPL_SIMD_OVERLOAD right_direction_vector_from_quaternion(quaternion_float q) {
    vector_float3 direction;
    direction.x = 1.0 - 2.0 * (q.y * q.y + q.z * q.z);
    direction.y = 2.0 * (q.x * q.y - q.w * q.z);
    direction.z = 2.0 * (q.x * q.z + q.w * q.y);
    
    direction = vector_normalize(direction);
    // Negate for a right-handed coordinate system
    return direction;
}