364 lines
9.9 KiB
C
364 lines
9.9 KiB
C
#ifndef _BASIC_GEOMETRY_TANGENT_H_
|
|
#define _BASIC_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;
|
|
} fp32_tangent_t;
|
|
|
|
typedef struct
|
|
{
|
|
const double cos, sin;
|
|
} fp64_tangent_t;
|
|
|
|
// ================= Dark Twins ================= //
|
|
|
|
typedef struct {
|
|
float cos, sin;
|
|
} __BgFP32DarkTwinTangent;
|
|
|
|
typedef struct {
|
|
double cos, sin;
|
|
} __BgFP64DarkTwinTangent;
|
|
|
|
// ================= Constants ================== //
|
|
|
|
extern const fp32_tangent_t FP32_IDLE_TANGENT;
|
|
extern const fp64_tangent_t FP64_IDLE_TANGENT;
|
|
|
|
// =================== Reset ==================== //
|
|
|
|
static inline void fp32_tangent_reset(fp32_tangent_t* tangent)
|
|
{
|
|
__BgFP32DarkTwinTangent* twin = (__BgFP32DarkTwinTangent*)tangent;
|
|
|
|
twin->cos = 1.0f;
|
|
twin->sin = 0.0f;
|
|
}
|
|
|
|
static inline void fp64_tangent_reset(fp64_tangent_t* tangent)
|
|
{
|
|
__BgFP64DarkTwinTangent* twin = (__BgFP64DarkTwinTangent*)tangent;
|
|
|
|
twin->cos = 1.0;
|
|
twin->sin = 0.0;
|
|
}
|
|
|
|
// ==================== Set ===================== //
|
|
|
|
static inline void fp32_tangent_set_values(const float x1, const float x2, fp32_tangent_t* tangent)
|
|
{
|
|
const float square_module = x1 * x1 + x2 * x2;
|
|
|
|
__BgFP32DarkTwinTangent* twin = (__BgFP32DarkTwinTangent*)tangent;
|
|
|
|
twin->cos = x1;
|
|
twin->sin = x2;
|
|
|
|
if (1.0f - FP32_TWO_EPSYLON <= square_module && square_module <= 1.0f + FP32_TWO_EPSYLON) {
|
|
return;
|
|
}
|
|
|
|
if (square_module <= 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 fp64_tangent_set_values(const double x1, const double x2, fp64_tangent_t* tangent)
|
|
{
|
|
const double square_module = x1 * x1 + x2 * x2;
|
|
|
|
__BgFP64DarkTwinTangent* twin = (__BgFP64DarkTwinTangent*)tangent;
|
|
|
|
twin->cos = x1;
|
|
twin->sin = x2;
|
|
|
|
if (1.0 - FP64_TWO_EPSYLON <= square_module && square_module <= 1.0 + FP64_TWO_EPSYLON) {
|
|
return;
|
|
}
|
|
|
|
if (square_module <= 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 fp32_tangent_copy(const fp32_tangent_t* from, fp32_tangent_t* to)
|
|
{
|
|
__BgFP32DarkTwinTangent* twin = (__BgFP32DarkTwinTangent*)to;
|
|
|
|
twin->cos = from->cos;
|
|
twin->sin = from->sin;
|
|
}
|
|
|
|
static inline void fp64_tangent_copy(const fp64_tangent_t* from, fp64_tangent_t* to)
|
|
{
|
|
__BgFP64DarkTwinTangent* twin = (__BgFP64DarkTwinTangent*)to;
|
|
|
|
twin->cos = from->cos;
|
|
twin->sin = from->sin;
|
|
}
|
|
|
|
// ==================== Swap ==================== //
|
|
|
|
static inline void fp32_tangent_swap(fp32_tangent_t* tangent1, fp32_tangent_t* 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 fp64_tangent_swap(fp64_tangent_t* tangent1, fp64_tangent_t* 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 fp32_tangent_set_turn(const float angle, const angle_unit_t unit, fp32_tangent_t* tangent)
|
|
{
|
|
const float radians = fp32_angle_to_radians(angle, unit);
|
|
|
|
__BgFP32DarkTwinTangent* twin = (__BgFP32DarkTwinTangent*)tangent;
|
|
|
|
twin->cos = cosf(radians);
|
|
twin->sin = sinf(radians);
|
|
}
|
|
|
|
static inline void fp64_tangent_set_turn(const double angle, const angle_unit_t unit, fp64_tangent_t* tangent)
|
|
{
|
|
const double radians = 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 fp32_tangent_set_from_fp64(const fp64_tangent_t* from, fp32_tangent_t* to)
|
|
{
|
|
fp32_tangent_set_values((float)from->cos, (float)from->sin, to);
|
|
}
|
|
|
|
static inline void fp64_tangent_set_from_fp32(const fp32_tangent_t* from, fp64_tangent_t* to)
|
|
{
|
|
fp64_tangent_set_values((double)from->cos, (double)from->sin, to);
|
|
}
|
|
|
|
// ================= Inversion ================== //
|
|
|
|
static inline void fp32_tangent_invert(fp32_tangent_t* tangent)
|
|
{
|
|
((__BgFP32DarkTwinTangent*)tangent)->sin = -tangent->sin;
|
|
}
|
|
|
|
static inline void fp64_tangent_invert(fp64_tangent_t* tangent)
|
|
{
|
|
((__BgFP64DarkTwinTangent*)tangent)->sin = -tangent->sin;
|
|
}
|
|
|
|
// ================ Set Inverted ================ //
|
|
|
|
static inline void fp32_tangent_set_inverted(const fp32_tangent_t* tangent, fp32_tangent_t* result)
|
|
{
|
|
__BgFP32DarkTwinTangent* twin = (__BgFP32DarkTwinTangent*)result;
|
|
|
|
twin->cos = tangent->cos;
|
|
twin->sin = -tangent->sin;
|
|
}
|
|
|
|
static inline void fp64_tangent_set_inverted(const fp64_tangent_t* tangent, fp64_tangent_t* result)
|
|
{
|
|
__BgFP64DarkTwinTangent* twin = (__BgFP64DarkTwinTangent*)result;
|
|
|
|
twin->cos = tangent->cos;
|
|
twin->sin = -tangent->sin;
|
|
}
|
|
|
|
// ============== Rotation Matrix =============== //
|
|
|
|
static inline void fp32_tangent_make_rotation_matrix(const fp32_tangent_t* tangent, fp32_matrix2x2_t* matrix)
|
|
{
|
|
matrix->r1c1 = tangent->cos;
|
|
matrix->r1c2 = -tangent->sin;
|
|
matrix->r2c1 = tangent->sin;
|
|
matrix->r2c2 = tangent->cos;
|
|
}
|
|
|
|
static inline void fp64_tangent_make_rotation_matrix(const fp64_tangent_t* tangent, fp64_matrix2x2_t* matrix)
|
|
{
|
|
matrix->r1c1 = tangent->cos;
|
|
matrix->r1c2 = -tangent->sin;
|
|
matrix->r2c1 = tangent->sin;
|
|
matrix->r2c2 = tangent->cos;
|
|
}
|
|
|
|
// ============== Reverse Matrix ================ //
|
|
|
|
static inline void fp32_tangent_make_reverse_matrix(const fp32_tangent_t* tangent, fp32_matrix2x2_t* matrix)
|
|
{
|
|
matrix->r1c1 = tangent->cos;
|
|
matrix->r1c2 = tangent->sin;
|
|
matrix->r2c1 = -tangent->sin;
|
|
matrix->r2c2 = tangent->cos;
|
|
}
|
|
|
|
static inline void fp64_tangent_make_reverse_matrix(const fp64_tangent_t* tangent, fp64_matrix2x2_t* matrix)
|
|
{
|
|
matrix->r1c1 = tangent->cos;
|
|
matrix->r1c2 = tangent->sin;
|
|
matrix->r2c1 = -tangent->sin;
|
|
matrix->r2c2 = tangent->cos;
|
|
}
|
|
|
|
// =================== Angle =================== //
|
|
|
|
static inline float fp32_tangent_get_angle(const fp32_tangent_t* tangent, const angle_unit_t unit)
|
|
{
|
|
if (tangent->cos >= 1.0f - FP32_TWO_EPSYLON) {
|
|
return 0.0f;
|
|
}
|
|
|
|
if (tangent->cos <= -1.0f + FP32_TWO_EPSYLON) {
|
|
return fp32_angle_get_half_circle(unit);
|
|
}
|
|
|
|
if (tangent->sin >= 1.0f - FP32_TWO_EPSYLON) {
|
|
return fp32_angle_get_quater_circle(unit);
|
|
}
|
|
|
|
if (tangent->sin <= -1.0f + FP32_TWO_EPSYLON) {
|
|
return 0.75f * fp32_angle_get_full_circle(unit);
|
|
}
|
|
|
|
return fp32_radians_to_units(atan2f(tangent->cos, tangent->sin), unit);
|
|
}
|
|
|
|
static inline double fp64_tangent_get_angle(const fp64_tangent_t* tangent, const angle_unit_t unit)
|
|
{
|
|
if (tangent->cos >= 1.0 - FP64_TWO_EPSYLON) {
|
|
return 0.0;
|
|
}
|
|
|
|
if (tangent->cos <= -1.0 + FP64_TWO_EPSYLON) {
|
|
return fp64_angle_get_half_circle(unit);
|
|
}
|
|
|
|
if (tangent->sin >= 1.0 - FP64_TWO_EPSYLON) {
|
|
return fp64_angle_get_quater_circle(unit);
|
|
}
|
|
|
|
if (tangent->sin <= -1.0 + FP64_TWO_EPSYLON) {
|
|
return 0.75 * fp64_angle_get_full_circle(unit);
|
|
}
|
|
|
|
return fp64_radians_to_units(atan2(tangent->cos, tangent->sin), unit);
|
|
}
|
|
|
|
// ================ Combination ================= //
|
|
|
|
static inline void fp32_tangent_combine(const fp32_tangent_t* tangent1, const fp32_tangent_t* tangent2, fp32_tangent_t* result)
|
|
{
|
|
fp32_tangent_set_values(
|
|
tangent1->cos * tangent2->cos - tangent1->sin * tangent2->sin,
|
|
tangent1->cos * tangent2->sin + tangent1->sin * tangent2->cos,
|
|
result
|
|
);
|
|
}
|
|
|
|
static inline void fp64_tangent_combine(const fp64_tangent_t* tangent1, const fp64_tangent_t* tangent2, fp64_tangent_t* result)
|
|
{
|
|
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 fp32_tangent_turn(const fp32_tangent_t* tangent, const fp32_vector2_t* vector, fp32_vector2_t* 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 fp64_tangent_turn(const fp64_tangent_t* tangent, const fp64_vector2_t* vector, fp64_vector2_t* 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 fp32_tangent_turn_back(const fp32_tangent_t* tangent, const fp32_vector2_t* vector, fp32_vector2_t* 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 fp64_tangent_turn_back(const fp64_tangent_t* tangent, const fp64_vector2_t* vector, fp64_vector2_t* 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
|