bgc-c/basic-geometry/tangent.h

#ifndef _GEOMETRY_TANGENT_H_
#define _GEOMETRY_TANGENT_H_

#include <math.h>

#include "basis.h"
#include "angle.h"
#include "vector2.h"
#include "matrix2x2.h"

// =================== Types ==================== //

typedef struct
{
    const float cos, sin;
} BgFP32Tangent;

typedef struct
{
    const double cos, sin;
} BgFP64Tangent;

// ================= Dark Twins ================= //

typedef struct {
    float cos, sin;
} __BgFP32DarkTwinTangent;

typedef struct {
    double cos, sin;
} __BgFP64DarkTwinTangent;

// ================= Constants ================== //

extern const BgFP32Tangent BG_FP32_IDLE_TANGENT;
extern const BgFP64Tangent BG_FP64_IDLE_TANGENT;

// =================== Reset ==================== //

static inline void bg_fp32_tangent_reset(BgFP32Tangent* tangent)
{
    __BgFP32DarkTwinTangent* twin = (__BgFP32DarkTwinTangent*)tangent;

    twin->cos = 1.0f;
    twin->sin = 0.0f;
}

static inline void bg_fp64_tangent_reset(BgFP64Tangent* tangent)
{
    __BgFP64DarkTwinTangent* twin = (__BgFP64DarkTwinTangent*)tangent;

    twin->cos = 1.0;
    twin->sin = 0.0;
}

// ==================== Set ===================== //

static inline void bg_fp32_tangent_set_values(const float x1, const float x2, BgFP32Tangent* tangent)
{
    const float square_module = x1 * x1 + x2 * x2;

    __BgFP32DarkTwinTangent* twin = (__BgFP32DarkTwinTangent*)tangent;

    twin->cos = x1;
    twin->sin = x2;

    if (1.0f - BG_FP32_TWO_EPSYLON <= square_module && square_module <= 1.0f + BG_FP32_TWO_EPSYLON) {
        return;
    }

    if (square_module <= BG_FP32_SQUARE_EPSYLON) {
        twin->cos = 1.0f;
        twin->sin = 0.0f;
        return;
    }

    const float multiplier = sqrtf(1.0f / square_module);

    twin->cos = x1 * multiplier;
    twin->sin = x2 * multiplier;
}

static inline void bg_fp64_tangent_set_values(const double x1, const double x2, BgFP64Tangent* tangent)
{
    const double square_module = x1 * x1 + x2 * x2;

    __BgFP64DarkTwinTangent* twin = (__BgFP64DarkTwinTangent*)tangent;

    twin->cos = x1;
    twin->sin = x2;

    if (1.0 - BG_FP64_TWO_EPSYLON <= square_module && square_module <= 1.0 + BG_FP64_TWO_EPSYLON) {
        return;
    }

    if (square_module <= BG_FP64_SQUARE_EPSYLON) {
        twin->cos = 1.0;
        twin->sin = 0.0;
        return;
    }

    const double multiplier = sqrt(1.0 / square_module);

    twin->cos = x1 * multiplier;
    twin->sin = x2 * multiplier;
}

// ==================== Copy ==================== //

static inline void bg_fp32_tangent_copy(const BgFP32Tangent* from, BgFP32Tangent* to)
{
    __BgFP32DarkTwinTangent* twin = (__BgFP32DarkTwinTangent*)to;

    twin->cos = from->cos;
    twin->sin = from->sin;
}

static inline void bg_fp64_tangent_copy(const BgFP64Tangent* from, BgFP64Tangent* to)
{
    __BgFP64DarkTwinTangent* twin = (__BgFP64DarkTwinTangent*)to;

    twin->cos = from->cos;
    twin->sin = from->sin;
}

// ==================== Swap ==================== //

static inline void bg_fp32_tangent_swap(BgFP32Tangent* tangent1, BgFP32Tangent* tangent2)
{
    const float cos = tangent1->cos;
    const float sin = tangent1->sin;

    __BgFP32DarkTwinTangent* twin1 = (__BgFP32DarkTwinTangent*)tangent1;

    twin1->cos = tangent2->cos;
    twin1->sin = tangent2->sin;

    __BgFP32DarkTwinTangent* twin2 = (__BgFP32DarkTwinTangent*)tangent2;

    twin2->cos = cos;
    twin2->sin = sin;
}

static inline void bg_fp64_tangent_swap(BgFP64Tangent* tangent1, BgFP64Tangent* tangent2)
{
    const double cos = tangent1->cos;
    const double sin = tangent1->sin;

    __BgFP64DarkTwinTangent* twin1 = (__BgFP64DarkTwinTangent*)tangent1;

    twin1->cos = tangent2->cos;
    twin1->sin = tangent2->sin;

    __BgFP64DarkTwinTangent* twin2 = (__BgFP64DarkTwinTangent*)tangent2;

    twin2->cos = cos;
    twin2->sin = sin;
}

// ================== Set Turn ================== //

static inline void bg_fp32_tangent_set_turn(const float angle, const angle_unit_t unit, BgFP32Tangent* tangent)
{
    const float radians = bg_fp32_angle_to_radians(angle, unit);

    __BgFP32DarkTwinTangent* twin = (__BgFP32DarkTwinTangent*)tangent;

    twin->cos = cosf(radians);
    twin->sin = sinf(radians);
}

static inline void bg_fp64_tangent_set_turn(const double angle, const angle_unit_t unit, BgFP64Tangent* tangent)
{
    const double radians = bg_fp64_angle_to_radians(angle, unit);

    __BgFP64DarkTwinTangent* twin = (__BgFP64DarkTwinTangent*)tangent;

    twin->cos = cos(radians);
    twin->sin = sin(radians);
}

// ============= Copy to twin type ============== //

static inline void bg_fp32_tangent_set_from_fp64(const BgFP64Tangent* from, BgFP32Tangent* to)
{
    bg_fp32_tangent_set_values((float)from->cos, (float)from->sin, to);
}

static inline void bg_fp64_tangent_set_from_fp32(const BgFP32Tangent* from, BgFP64Tangent* to)
{
    bg_fp64_tangent_set_values((double)from->cos, (double)from->sin, to);
}

// ================= Inversion ================== //

static inline void bg_fp32_tangent_invert(BgFP32Tangent* tangent)
{
    ((__BgFP32DarkTwinTangent*)tangent)->sin = -tangent->sin;
}

static inline void bg_fp64_tangent_invert(BgFP64Tangent* tangent)
{
    ((__BgFP64DarkTwinTangent*)tangent)->sin = -tangent->sin;
}

// ================ Set Inverted ================ //

static inline void bg_fp32_tangent_set_inverted(const BgFP32Tangent* tangent, BgFP32Tangent* result)
{
    __BgFP32DarkTwinTangent* twin = (__BgFP32DarkTwinTangent*)result;

    twin->cos = tangent->cos;
    twin->sin = -tangent->sin;
}

static inline void bg_fp64_tangent_set_inverted(const BgFP64Tangent* tangent, BgFP64Tangent* result)
{
    __BgFP64DarkTwinTangent* twin = (__BgFP64DarkTwinTangent*)result;

    twin->cos = tangent->cos;
    twin->sin = -tangent->sin;
}

// ============== Rotation Matrix =============== //

static inline void bg_fp32_tangent_make_rotation_matrix(const BgFP32Tangent* tangent, BgFP32Matrix2x2* matrix)
{
    matrix->r1c1 = tangent->cos;
    matrix->r1c2 = -tangent->sin;
    matrix->r2c1 = tangent->sin;
    matrix->r2c2 = tangent->cos;
}

static inline void bg_fp64_tangent_make_rotation_matrix(const BgFP64Tangent* tangent, BgFP64Matrix2x2* matrix)
{
    matrix->r1c1 = tangent->cos;
    matrix->r1c2 = -tangent->sin;
    matrix->r2c1 = tangent->sin;
    matrix->r2c2 = tangent->cos;
}

// ============== Reverse Matrix ================ //

static inline void bg_fp32_tangent_make_reverse_matrix(const BgFP32Tangent* tangent, BgFP32Matrix2x2* matrix)
{
    matrix->r1c1 = tangent->cos;
    matrix->r1c2 = tangent->sin;
    matrix->r2c1 = -tangent->sin;
    matrix->r2c2 = tangent->cos;
}

static inline void bg_fp64_tangent_make_reverse_matrix(const BgFP64Tangent* tangent, BgFP64Matrix2x2* matrix)
{
    matrix->r1c1 = tangent->cos;
    matrix->r1c2 = tangent->sin;
    matrix->r2c1 = -tangent->sin;
    matrix->r2c2 = tangent->cos;
}

// =================== Angle =================== //

static inline float bg_fp32_tangent_get_angle(const BgFP32Tangent* tangent, const angle_unit_t unit)
{
    if (tangent->cos >= 1.0f - BG_FP32_TWO_EPSYLON) {
        return 0.0f;
    }

    if (tangent->cos <= -1.0f + BG_FP32_TWO_EPSYLON) {
        return bg_fp32_angle_get_half_circle(unit);
    }

    if (tangent->sin >= 1.0f - BG_FP32_TWO_EPSYLON) {
        return bg_fp32_angle_get_quater_circle(unit);
    }

    if (tangent->sin <= -1.0f + BG_FP32_TWO_EPSYLON) {
        return 0.75f * bg_fp32_angle_get_full_circle(unit);
    }

    return bg_fp32_radians_to_units(atan2f(tangent->cos, tangent->sin), unit);
}

static inline double bg_fp64_tangent_get_angle(const BgFP64Tangent* tangent, const angle_unit_t unit)
{
    if (tangent->cos >= 1.0 - BG_FP64_TWO_EPSYLON) {
        return 0.0;
    }

    if (tangent->cos <= -1.0 + BG_FP64_TWO_EPSYLON) {
        return bg_fp64_angle_get_half_circle(unit);
    }

    if (tangent->sin >= 1.0 - BG_FP64_TWO_EPSYLON) {
        return bg_fp64_angle_get_quater_circle(unit);
    }

    if (tangent->sin <= -1.0 + BG_FP64_TWO_EPSYLON) {
        return 0.75 * bg_fp64_angle_get_full_circle(unit);
    }

    return bg_fp64_radians_to_units(atan2(tangent->cos, tangent->sin), unit);
}

// ================ Combination ================= //

static inline void bg_fp32_tangent_combine(const BgFP32Tangent* tangent1, const BgFP32Tangent* tangent2, BgFP32Tangent* result)
{
    bg_fp32_tangent_set_values(
        tangent1->cos * tangent2->cos - tangent1->sin * tangent2->sin,
        tangent1->cos * tangent2->sin + tangent1->sin * tangent2->cos,
        result
    );
}

static inline void bg_fp64_tangent_combine(const BgFP64Tangent* tangent1, const BgFP64Tangent* tangent2, BgFP64Tangent* result)
{
    bg_fp64_tangent_set_values(
        tangent1->cos * tangent2->cos - tangent1->sin * tangent2->sin,
        tangent1->cos * tangent2->sin + tangent1->sin * tangent2->cos,
        result
    );
}

// ================ Turn Vector ================= //

static inline void bg_fp32_tangent_turn(const BgFP32Tangent* tangent, const BgFP32Vector2* vector, BgFP32Vector2* result)
{
    const float x1 = tangent->cos * vector->x1 - tangent->sin * vector->x2;
    const float x2 = tangent->sin * vector->x1 + tangent->cos * vector->x2;

    result->x1 = x1;
    result->x2 = x2;
}

static inline void bg_fp64_tangent_turn(const BgFP64Tangent* tangent, const BgFP64Vector2* vector, BgFP64Vector2* result)
{
    const double x1 = tangent->cos * vector->x1 - tangent->sin * vector->x2;
    const double x2 = tangent->sin * vector->x1 + tangent->cos * vector->x2;

    result->x1 = x1;
    result->x2 = x2;
}

// ============ Turn Vector Backward ============ //

static inline void bg_fp32_tangent_turn_back(const BgFP32Tangent* tangent, const BgFP32Vector2* vector, BgFP32Vector2* result)
{
    const float x1 = tangent->sin * vector->x2 + tangent->cos * vector->x1;
    const float x2 = tangent->cos * vector->x2 - tangent->sin * vector->x1;

    result->x1 = x1;
    result->x2 = x2;
}

static inline void bg_fp64_tangent_turn_back(const BgFP64Tangent* tangent, const BgFP64Vector2* vector, BgFP64Vector2* result)
{
    const double x1 = tangent->sin * vector->x2 + tangent->cos * vector->x1;
    const double x2 = tangent->cos * vector->x2 - tangent->sin * vector->x1;

    result->x1 = x1;
    result->x2 = x2;
}

#endif