traj.h

/*
 * Yifei Chen 2021/12/21
 * Fourier feature phase-based animation controller
 * 
 */

#ifndef TRAJ_H
#define TRAJ_H


#include <vector>
#include <set>
#define GL_SILENCE_DEPRECATION
#include <GLFW/glfw3.h>
#include "delfem2/rig_geo3.h"
#include "delfem2/opengl/old/funcs.h"


class CParticle {
public:
    float pos[2] = { 0.f, 0.f };
    float velo[2] = { 0.f, 0.f };
    float color[3] = { 0.f, 0.f, 0.f };
    float mass = 1.f;
};


void draw_traj_line(std::vector<CParticle>& traj_points, std::vector<CParticle>& traj_points_sample)
{
    ::glLineWidth(2);
    ::glColor3d(0, 0, 0);
    ::glBegin(GL_LINE_STRIP);
    int jframe_total = traj_points.size();
    for (unsigned int jframe = 0; jframe < jframe_total - 1; ++jframe) {
        const double x0 = traj_points[jframe].pos[0];
        const double z0 = traj_points[jframe].pos[1];
        ::glVertex3d(x0, 0, z0);
    }
    ::glEnd();

    if (jframe_total > 1)
    {
        ::glColor3d(1, 0, 0);
        ::glBegin(GL_LINE_STRIP);
        const double x0 = traj_points[jframe_total - 2].pos[0];
        const double z0 = traj_points[jframe_total - 2].pos[1];
        const double dx1 = traj_points[jframe_total - 1].pos[0] - x0;
        const double dz1 = traj_points[jframe_total - 1].pos[1] - z0;
        ::glVertex3d(x0, 0, z0);
        ::glVertex3d(x0 + 10 * dx1, 0, z0 + 10 * dz1);
        ::glEnd();
    }

    for (auto& p : traj_points_sample)
    {
        ::glColor3d(1, 1, 1);
        ::delfem2::opengl::DrawSphereAt(32, 32, 0.5, p.pos[0], 0, p.pos[1]);
    }


}


void draw(
    CParticle& ip)
{

    ::glColor3fv(ip.color);
    ::delfem2::opengl::DrawSphereAt(32, 32, 0.5, ip.pos[0], 0, ip.pos[1]);
}


// -------------------------------------------


class damper_p {
public:
    float pos[2] = { 0.f, 0.f };
};


void draw_red_sphere(
    std::vector<float>& ip)
{
    // draw points
    float color[3] = { 0.9f,0.1f,0.1f };
    ::glColor3fv(color);
    ::delfem2::opengl::DrawSphereAt(32, 32, 0.5, ip[0], 0, ip[1]);
}

void draw_blue_sphere(
    std::vector<float>& ip)
{
    // draw points
    float color[3] = { 0.1f,0.1f,0.9f };
    ::glColor3fv(color);
    ::delfem2::opengl::DrawSphereAt(32, 32, 0.5, ip[0], 0, ip[1]);
}

void draw_black_sphere(
    std::vector<float>& ip)
{
    // draw points
    float color[3] = { 0.1f,0.1f,0.1f };
    ::glColor3fv(color);
    ::delfem2::opengl::DrawSphereAt(32, 32, 0.5, ip[0], 0, ip[1]);
}


// some utility function learned from Holden's Website Guide
float lerp(float x, float y, float a)
{
    return (1.0f - a) * x + a * y;
}


float damper(float x, float g, float factor)
{
    return lerp(x, g, factor);
}

float z_target = 5.0f;

float halflife_to_damping(float halflife, float eps = 1e-5f)
{
    return (4.0f * 0.69314718056f) / (halflife + eps);
}

float fast_negexp(float x)
{
    return 1.0f / (1.0f + x + 0.48f * x * x + 0.235f * x * x * x);
}

void spring_character_update_dt(
    float& x,
    float& v,
    float& a,
    float v_goal,
    float halflife,
    float dt)
{
    float y = halflife_to_damping(halflife) / 2.0f;
    float j0 = v - v_goal;
    float j1 = a + j0 * y;
    float exp_result = fast_negexp(y * dt);

    x = exp_result * (((-j1) / (y * y)) + ((-j0 - j1 * dt) / y)) +
        (j1 / (y * y)) + j0 / y + v_goal * dt + x;

    v = exp_result * (j0 + j1 * dt) + v_goal;

    a = exp_result * (a - j1 * y * dt);
}

void spring_character_predict(
    float px[],
    float pv[],
    float pa[],
    int count,
    float x,
    float v,
    float a,
    float v_goal,
    float halflife,
    float dt)
{
    for (int i = 0; i < count; i++)
    {
        // num of samples
        px[i] = x;
        pv[i] = v;
        pa[i] = a;
    }

    for (int i = 0; i < count; i++)
    {
        // update pos
        spring_character_update_dt(px[i], pv[i], pa[i], v_goal, halflife, i * dt);
    }
}


#endif