R3D - Rotate Bone Toward

r3d_anim.c

#include <r3d/r3d.h>
#include <raymath.h>

#ifndef RESOURCES_PATH
#	define RESOURCES_PATH "./"
#endif

typedef struct {
    Vector3 forwardAxis;
    Vector3 upAxis;
    float weight;
    float maxAngle;
} LookAtConstraint;

void RotateBoneToward(R3D_AnimationPlayer *player, int boneIndex, Matrix matModel, Vector3 target, LookAtConstraint constraint)
{
    Matrix* boneLocal = &player->localPose[boneIndex];

    int parentIndex = player->skeleton.bones[boneIndex].parent;
    Matrix parentGlobal = (parentIndex >= 0) ? player->modelPose[parentIndex] : matModel;

    Vector3 bonePos = {boneLocal->m12, boneLocal->m13, boneLocal->m14};
    bonePos = Vector3Transform(bonePos, parentGlobal);

    Vector3 currentDir = Vector3Transform(constraint.forwardAxis, player->modelPose[boneIndex]);
    currentDir = Vector3Normalize(Vector3Subtract(currentDir, bonePos));
    Vector3 targetDir = Vector3Normalize(Vector3Subtract(target, bonePos));

    float dot = Vector3DotProduct(currentDir, targetDir);
    Quaternion delta;
    
    if (dot > 0.999f) {
        delta = QuaternionIdentity();
    }
    else if (dot < -0.999f) {
        delta = QuaternionFromAxisAngle(constraint.upAxis, PI);
    }
    else {
        Vector3 axis = Vector3Normalize(Vector3CrossProduct(currentDir, targetDir));
        delta = QuaternionFromAxisAngle(axis, acosf(Clamp(dot, -1.0f, 1.0f)));
    }

    Matrix rot = player->modelPose[boneIndex];
    rot.m12 = rot.m13 = rot.m14 = 0;
    Quaternion q = QuaternionFromMatrix(rot);
    q = QuaternionNormalize(QuaternionMultiply(delta, q));

    if (constraint.weight < 1.0f) {
        q = QuaternionSlerp(QuaternionFromMatrix(rot), q, constraint.weight);
    }

    Matrix result = QuaternionToMatrix(q);
    result.m12 = bonePos.x;
    result.m13 = bonePos.y;
    result.m14 = bonePos.z;

    *boneLocal = MatrixMultiply(result, MatrixInvert(parentGlobal));
}

int main(void)
{
    // Initialize window
    InitWindow(800, 450, "[r3d] - Animation example");
    SetTargetFPS(60);

    // Initialize R3D with FXAA
    R3D_Init(GetScreenWidth(), GetScreenHeight());
    R3D_SetAntiAliasing(R3D_ANTI_ALIASING_FXAA);

    // Setup environment sky
    R3D_Cubemap cubemap = R3D_LoadCubemap(RESOURCES_PATH "panorama/indoor.hdr", R3D_CUBEMAP_LAYOUT_AUTO_DETECT);
    R3D_ENVIRONMENT_SET(background.skyBlur, 0.3f);
    R3D_ENVIRONMENT_SET(background.energy, 0.6f);
    R3D_ENVIRONMENT_SET(background.sky, cubemap);

    // Setup environment ambient
    R3D_AmbientMap ambientMap = R3D_GenAmbientMap(cubemap, R3D_AMBIENT_ILLUMINATION);
    R3D_ENVIRONMENT_SET(ambient.map, ambientMap);
    R3D_ENVIRONMENT_SET(ambient.energy, 0.25f);

    // Setup tonemapping
    R3D_ENVIRONMENT_SET(tonemap.mode, R3D_TONEMAP_FILMIC);
    R3D_ENVIRONMENT_SET(tonemap.exposure, 0.75f);

    // Generate a ground plane and load the animated model
    R3D_Mesh plane = R3D_GenMeshPlane(10, 10, 1, 1);
    R3D_Model model = R3D_LoadModel(RESOURCES_PATH "models/CesiumMan.glb");
    int neckIndex = R3D_GetSkeletonBoneIndex(model.skeleton, "Skeleton_neck_joint_1");

    // Load animations
    R3D_AnimationLib modelAnims = R3D_LoadAnimationLib(RESOURCES_PATH "models/CesiumMan.glb");
    R3D_AnimationPlayer modelPlayer = R3D_LoadAnimationPlayer(model.skeleton, modelAnims);

    // Setup animation playing
    R3D_SetAnimationWeight(&modelPlayer, 0, 1.0f);
    R3D_SetAnimationLoop(&modelPlayer, 0, true);
    R3D_PlayAnimation(&modelPlayer, 0);

    // Setup lights with shadows
    R3D_Light light = R3D_CreateLight(R3D_LIGHT_DIR);
    R3D_SetLightDirection(light, (Vector3){-1.0f, -1.0f, -1.0f});
    R3D_SetLightActive(light, true);
    R3D_SetLightRange(light, 10.0f);
    R3D_EnableShadow(light);

    // Setup camera
    Camera3D camera = {
        .position = {0, 1.5f, 3.0f},
        .target = {0, 0.75f, 0.0f},
        .up = {0, 1, 0},
        .fovy = 60
    };

    // Main loop
    while (!WindowShouldClose())
    {
        float delta = GetFrameTime();

        UpdateCamera(&camera, CAMERA_ORBITAL);

        R3D_CalculateAnimationPlayerLocalPose(&modelPlayer);
        R3D_CalculateAnimationPlayerModelPose(&modelPlayer);

        LookAtConstraint config = {.forwardAxis = {0, 0, 1}, .upAxis = {0, 1, 0}, .weight = 1.0f, .maxAngle = 80.0f};
        RotateBoneToward(&modelPlayer, neckIndex, MatrixIdentity(), camera.position, config);

        R3D_CalculateAnimationPlayerModelPose(&modelPlayer);
        R3D_UploadAnimationPlayerPose(&modelPlayer);

        R3D_AdvanceAnimationPlayerTime(&modelPlayer, GetFrameTime());

        BeginDrawing();
            ClearBackground(RAYWHITE);
            R3D_Begin(camera);
                R3D_DrawMesh(plane, R3D_MATERIAL_BASE, Vector3Zero(), 1.0f);
                R3D_DrawAnimatedModel(model, modelPlayer, Vector3Zero(), 1.25f);
            R3D_End();
        EndDrawing();
    }

    // Cleanup
    R3D_UnloadAnimationPlayer(modelPlayer);
    R3D_UnloadAnimationLib(modelAnims);
    R3D_UnloadModel(model, true);
    R3D_UnloadMesh(plane);
    R3D_Close();

    CloseWindow();

    return 0;
}