FcLibs/FcMath/src/FcMath_Quaternion.h - master - vTech/FcApps

FcLibs/FcMath/src/FcMath_Quaternion.h
92 строки4 KB
vTech
alpha_00
03 дек 2025, 17:45
03 дек 2025, 17:45ab8382d
#ifndef __FCMATH_QUATERNION_H__
#define __FCMATH_QUATERNION_H__
//https://paroj.github.io/gltut/Positioning/Tut08%20Quaternions.html

typedef struct FcMath_Quaternion_t{
    FcVec4f_t mV;
}FcMath_Quaternion_t;

FCDEF FcMatrix4x4f_t FcMath_Quaternion_Mq(FcMath_Quaternion_t q);
FCDEF FcMatrix4x4f_t FcMath_Quaternion_M(float x, float y, float z, float angle);
FCDEF FcMath_Quaternion_t FcMath_Quaternion_Q(float x, float y, float z, float angle);
FCDEF FcMath_Quaternion_t FcMath_Quaternion_Mul_Qqq(FcMath_Quaternion_t* q0, FcMath_Quaternion_t* q1);
FCDEF float FcMath_Quaternion_length(FcMath_Quaternion_t* q0);


FCDEF FcMath_Quaternion_t FcMath_QuaternionQ(float x, float y, float z, float angle){
    float L = sin( angle / 2 );
    FcMath_Quaternion_t mQ;
    mQ.mV.m[0] = x * L;
    mQ.mV.m[1] = y * L;
    mQ.mV.m[2] = z * L;
    mQ.mV.m[3] = cos(angle / 2);
    return mQ;
}

FCDEF FcMatrix4x4f_t FcMath_QuaternionMq(FcMath_Quaternion_t* q){
    float mX = q->mV.m[0];
    float mY = q->mV.m[1];
    float mZ = q->mV.m[2];
    float mW = q->mV.m[3];
    return FcMatrix4x4f(1 - 2 * (mY * mY + mZ * mZ),
                       2 * (mX * mY + mZ * mW),
                       2 * (mX * mZ - mY * mW),
                       0,
                       2 * (mX * mY - mZ * mW ),
                       1 - 2 * (mX * mX + mZ * mZ),
                       2 * (mZ * mY + mX * mW ),
                       0,
                       2 * (mX * mZ + mY * mW),
                       2 * (mY * mZ - mX * mW),
                       1 - 2 * (mX * mX + mY * mY),
                       0,
                       0, 0, 0, 1);
}

FCDEF FcMatrix4x4f_t FcMath_QuaternionM(float x, float y, float z, float angle){
    float result = sin( angle / 2 );
    float mW = cos( angle / 2 );
    float mX = x * result;
    float mY = y * result;
    float mZ = z * result;

    return FcMatrix4x4f(1 - 2 * (mY * mY + mZ * mZ),
                       2 * (mX * mY + mZ * mW),
                       2 * (mX * mZ - mY * mW),
                       0,
                       2 * (mX * mY - mZ * mW ),
                       1 - 2 * (mX * mX + mZ * mZ),
                       2 * (mZ * mY + mX * mW ),
                       0,
                       2 * (mX * mZ + mY * mW),
                       2 * (mY * mZ - mX * mW),
                       1 - 2 * (mX * mX + mY * mY),
                       0,
                       0, 0, 0, 1);
}

FCDEF float FcMath_Quaternion_length(FcMath_Quaternion_t* q0){
    return FcVec4f_length(q0->mV);
}

FCDEF FcMath_Quaternion_t FcMath_Quaternion_Mul_Qqq(FcMath_Quaternion_t* q0, FcMath_Quaternion_t* q1){
    FcVec4f_t t0 = FcVec4f(q0->mV.m[3], q0->mV.m[3], q0->mV.m[3], q0->mV.m[3]);
    FcVec4f_t t1 = FcVec4f(q1->mV.m[3], q1->mV.m[0], q1->mV.m[1], q1->mV.m[2]);
    FcVec4f_t t2 = FcVec4f(-q0->mV.m[0], q0->mV.m[0], q0->mV.m[1], q0->mV.m[2]);
    FcVec4f_t t3 = FcVec4f(q1->mV.m[0], q1->mV.m[3], q1->mV.m[3], q1->mV.m[3]);
    FcVec4f_t t4 = FcVec4f(-q0->mV.m[1], q0->mV.m[1], q0->mV.m[2], q0->mV.m[0]);
    FcVec4f_t t5 = FcVec4f(q1->mV.m[1], q1->mV.m[2], q1->mV.m[0], q1->mV.m[1]);
    FcVec4f_t t6 = FcVec4f(-q0->mV.m[2], -q0->mV.m[2], -q0->mV.m[0], -q0->mV.m[1]);
    FcVec4f_t t7 = FcVec4f(q1->mV.m[2], q1->mV.m[1], q1->mV.m[2], q1->mV.m[0]);
    FcMath_Quaternion_t mQ = {
            FcVec4f_add(
            FcVec4f_add(
            FcVec4f_add(FcVec4f_mul(t0, t1),
                        FcVec4f_mul(t2, t3)),
                        FcVec4f_mul(t4, t5)),
                        FcVec4f_mul(t6, t7))
    };
    return mQ;
}

#endif //__FCMATH_QUATERNION_H__