toys/sww/source/util.c

#include "sww/util.h"

#include <stddef.h>
#include <stdio.h>  /**< FILE */
#include <stdlib.h> /**< atoi atof */
#include <string.h> /**< strtok_r */

#include "dynamic_buffer.h"
#include "file_helper.h"
#include "image.h"
#include "sww/cstl_vector.h"
#include "sww/vmath.h"

#if defined(OS_WINDOWS) || defined(_WIN32) || defined(_WIN64)
#include <windows.h>
#define strtok_r strtok_s
#endif

uint32_t swwUtil_PngBufferSize(uint32_t w, uint32_t h, int32_t alpha) {
  return 63 + (h * (6 + w * (alpha ? 4 : 3)));
}

int swwUtil_SaveBufferToPngBuffer(uint8_t *output, uint32_t output_size,
                                  const uint8_t *img, uint32_t w, uint32_t h,
                                  int32_t alpha) {
  static const uint32_t kCrc32Table[] = {0,
                                         0x1db71064,
                                         0x3b6e20c8,
                                         0x26d930ac,
                                         0x76dc4190,
                                         0x6b6b51f4,
                                         0x4db26158,
                                         0x5005713c,
                                         /* CRC32 Table */ 0xedb88320,
                                         0xf00f9344,
                                         0xd6d6a3e8,
                                         0xcb61b38c,
                                         0x9b64c2b0,
                                         0x86d3d2d4,
                                         0xa00ae278,
                                         0xbdbdf21c};

  /* ADLER-a, ADLER-b, CRC, pitch */
  uint32_t a = 1;
  uint32_t b = 0;
  uint32_t c = 0;
  uint32_t pixel_bytes_num = alpha ? 4 : 3;
  uint32_t p = w * (pixel_bytes_num ? 4 : 3) + 1;
  uint32_t x = 0;
  uint32_t y = 0;
  uint32_t i = 0;
  const uint8_t *rp = NULL;
  const uint8_t *gp = NULL;
  const uint8_t *bp = NULL;

  uint8_t *outp = output;

#define SVPNG_U8A(ua, l)                                                       \
  for (i = 0; i < l; i++) {                                                    \
    *outp++ = (ua)[i];                                                         \
  }
#define SVPNG_U32(u)                                                           \
  do {                                                                         \
    *outp++ = ((u) >> 24);                                                     \
    *outp++ = (((u) >> 16) & 255);                                             \
    *outp++ = (((u) >> 8) & 255);                                              \
    *outp++ = ((u)&255);                                                       \
  } while (0)
#define SVPNG_U8C(u)                                                           \
  do {                                                                         \
    *outp++ = (u);                                                             \
    c ^= (u);                                                                  \
    c = (c >> 4) ^ kCrc32Table[c & 15];                                        \
    c = (c >> 4) ^ kCrc32Table[c & 15];                                        \
  } while (0)
#define SVPNG_U8AC(ua, l)                                                      \
  for (i = 0; i < l; i++)                                                      \
  SVPNG_U8C((ua)[i])
#define SVPNG_U16LC(u)                                                         \
  do {                                                                         \
    SVPNG_U8C((u)&255);                                                        \
    SVPNG_U8C(((u) >> 8) & 255);                                               \
  } while (0)
#define SVPNG_U32C(u)                                                          \
  do {                                                                         \
    SVPNG_U8C((u) >> 24);                                                      \
    SVPNG_U8C(((u) >> 16) & 255);                                              \
    SVPNG_U8C(((u) >> 8) & 255);                                               \
    SVPNG_U8C((u)&255);                                                        \
  } while (0)
#define SVPNG_U8ADLER(u)                                                       \
  do {                                                                         \
    SVPNG_U8C(u);                                                              \
    a = (a + (u)) % 65521;                                                     \
    b = (b + a) % 65521;                                                       \
  } while (0)
#define SVPNG_BEGIN(s, l)                                                      \
  do {                                                                         \
    SVPNG_U32(l);                                                              \
    c = ~0U;                                                                   \
    SVPNG_U8AC(s, 4);                                                          \
  } while (0)
#define SVPNG_END() SVPNG_U32(~c)

  SVPNG_U8A("\x89PNG\r\n\32\n", 8); /* Magic */
  SVPNG_BEGIN("IHDR", 13);          /* IHDR chunk { */
  SVPNG_U32C(w);
  SVPNG_U32C(h); /*   Width & Height (8 bytes) */
  SVPNG_U8C(8);
  SVPNG_U8C(
      alpha ? 6
            : 2); /*   Depth=8, Color=True color with/without alpha (2 bytes) */
  SVPNG_U8AC("\0\0\0",
             3); /*   Compression=Deflate, Filter=No, Interlace=No (3 bytes) */
  SVPNG_END();   /* } */
  SVPNG_BEGIN("IDAT", 2 + h * (5 + p) + 4); /* IDAT chunk { */
  SVPNG_U8AC("\x78\1", 2); /*   Deflate block begin (2 bytes) */
  for (y = 0; y < h;
       y++) { /*   Each horizontal line makes a block for simplicity */
    SVPNG_U8C(y == h - 1); /*   1 for the last block, 0 for others (1 byte) */
    SVPNG_U16LC(p);
    SVPNG_U16LC(~p);  /*   Size of block in little endian and its 1's complement
                         (4 bytes) */
    SVPNG_U8ADLER(0); /*   No filter prefix (1 byte) */
    for (x = 0; x < p - 1; x += pixel_bytes_num, img += pixel_bytes_num) {
      // BGRA -> RGBA or BGR -> RGB
      // 0xAARRGGBB -> 0xAABBGGRR or 0xRRGGBB -> 0xBBGGRR
      rp = img + 2;
      gp = img + 1;
      bp = img;
      /* Image pixel data */
      SVPNG_U8ADLER(*rp);
      SVPNG_U8ADLER(*gp);
      SVPNG_U8ADLER(*bp);
      if (pixel_bytes_num == 4) {
        rp = img + 3;
        SVPNG_U8ADLER(*rp);
      }
    }
  }
  SVPNG_U32C((b << 16) | a); /*   Deflate block end with adler (4 bytes) */
  SVPNG_END();               /* } */
  SVPNG_BEGIN("IEND", 0);
  SVPNG_END(); /* IEND chunk {} */

  return 1;
}

#pragma pack(push, 1)
typedef struct {
  uint16_t type;
  uint32_t size;
  uint16_t reserved1;
  uint16_t reserved2;
  uint32_t offset;
} BmpFileHeader;

typedef struct {
  uint32_t size;
  int32_t width;
  int32_t height;
  uint16_t planes;
  uint16_t bit_count;
  uint32_t compress;
  uint32_t size_image;
  int32_t x_pels_per_meter;
  int32_t y_pels_per_meter;
  uint32_t clr_used;
  uint32_t clr_important;
} BmpInfoHeader;
#pragma pack(pop)

uint32_t swwUtil_BmpBufferSize(uint32_t w, uint32_t h, int32_t alpha) {
  return sizeof(BmpFileHeader) + sizeof(BmpInfoHeader) + w * h * 4;
}

int swwUtil_SaveBufferToBmpBuffer(uint8_t *output, uint32_t output_size,
                                  const uint8_t *img, uint32_t w, uint32_t h,
                                  int32_t alpha) {
  uint8_t *p = output;
  int32_t i = 0;

  BmpFileHeader bmp_head;
  BmpInfoHeader bmp_info;
  uint32_t size = w * h * 4;

  // initialize bmp head.
  bmp_head.type = 0x4d42; // BM
  bmp_head.size = size + sizeof(BmpFileHeader) + sizeof(BmpInfoHeader);
  bmp_head.reserved1 = bmp_head.reserved2 = 0;
  bmp_head.offset = bmp_head.size - size;

  // initialize bmp info.
  bmp_info.size = 40;
  bmp_info.width = w;
  bmp_info.height = h;
  bmp_info.planes = 1;
  bmp_info.bit_count = 32;
  bmp_info.compress = 0;
  bmp_info.size_image = size;
  bmp_info.x_pels_per_meter = 0;
  bmp_info.y_pels_per_meter = 0;
  bmp_info.clr_used = 0;
  bmp_info.clr_important = 0;

  // copy the data
  memcpy(p, &bmp_head, sizeof(BmpFileHeader));
  p += sizeof(BmpFileHeader);
  memcpy(p, &bmp_info, sizeof(BmpInfoHeader));
  p += sizeof(BmpInfoHeader);
  for (i = (int32_t)(h - 1); i >= 0; --i) {
    memcpy(p, &img[i * w * 4], w * 4);
    p += w * 4;
  }

  return 1;
}

CSTLVector(VertexVector, Vector3f);
typedef VertexVector NormVector;
CSTLVector(TexCoordVector, Vector2f);
CSTLVector(IntVector, int32_t);

struct swwModel {
  VertexVector verts;       //< array of vertices
  TexCoordVector tex_coord; //< per-vertex array of tex coords
  NormVector norms;         //< per-vertex array of normal vectors
  IntVector face_vrt;
  IntVector face_tex; //< per-triangle indices in the above arrays
  IntVector face_nrm;
  swwImage *diffuse_map;
};

static void swwModel_Construct(swwModel *o) {
  if (o != NULL) {
    VertexVector_Construct(&o->verts, 16);
    TexCoordVector_Construct(&o->tex_coord, 16);
    VertexVector_Construct(&o->norms, 16);

    IntVector_Construct(&o->face_vrt, 16);
    IntVector_Construct(&o->face_tex, 16);
    IntVector_Construct(&o->face_nrm, 16);
    o->diffuse_map = NULL;
  }
}

static void swwModel_Destruct(swwModel *o) {
  if (o != NULL) {
    VertexVector_Destruct(&o->verts);
    TexCoordVector_Destruct(&o->tex_coord);
    VertexVector_Destruct(&o->norms);

    IntVector_Destruct(&o->face_vrt);
    IntVector_Destruct(&o->face_tex);
    IntVector_Destruct(&o->face_nrm);
    swwImage_Destroy(o->diffuse_map);
  }
}

// .obj model interfaces
swwModel *swwModel_CreateFromFile(const char *filepath) {
  DynamicBuffer db;
  swwModel *o = NULL;
  swwFileLineReader *line_reader = NULL;
  char *line = NULL;
  size_t len = 0;
  const char delimiters[] = " \t";
  int i = 1;
  char *token = NULL;
  char *last = NULL;

  FILE *fp = fopen(filepath, "r");

  if (fp == NULL) {
    return o;
  }

  o = (swwModel *)malloc(sizeof(swwModel));
  if (o == NULL) {
    fclose(fp);
    return o;
  }
  swwModel_Construct(o);

  // Read the obj file line by line
  line_reader = swwFileLineReader_Create(fp);

  line = swwFileLineReader_NextLineWithLength(line_reader, &len);
  while (line != NULL) {
    if (len >= 2 && line[0] == 'v' && line[1] == ' ') {
      // v -1 -1 -1
      static const int kVertexCount = 3;
      Vector3f v;
      i = 0;

      token = strtok_r(line + 2, delimiters, &last);

      while (token != NULL) {
        v.d[i] = (float)atof(token);
        token = strtok_r(NULL, delimiters, &last);
        i++;
        if (i >= kVertexCount) {
          // TODO(donkey): Output warning information
          break;
        }
      }
      VertexVector_PushBack(&o->verts, v);
    } else if (len >= 3 && line[0] == 'v' && line[1] == 'n' && line[2] == ' ') {
      // vn 0 1 0
      static const int kVertexNormalCount = 3;
      Vector3f v;
      i = 0;

      token = strtok_r(line + 3, delimiters, &last);

      while (token != NULL) {
        v.d[i] = (float)atof(token);
        token = strtok_r(NULL, delimiters, &last);
        i++;
        if (i >= kVertexNormalCount) {
          // TODO(donkey): Output warning information
          break;
        }
      }
      VertexVector_PushBack(&o->norms, Vector3f_Normalize(v));
    } else if (len >= 3 && line[0] == 'v' && line[1] == 't' && line[2] == ' ') {
      // vt 0 1
      static const int kVertexTextureCount = 2;
      Vector2f uv;
      i = 0;

      token = strtok_r(line + 3, delimiters, &last);

      while (token != NULL) {
        uv.d[i] = (float)atof(token);
        token = strtok_r(NULL, delimiters, &last);
        i++;
        if (i >= kVertexTextureCount) {
          // TODO(donkey): Output warning information
          break;
        }
      }

      TexCoordVector_PushBack(&o->tex_coord, (Vector2f){uv.x, 1 - uv.y});
    } else if (len >= 2 && line[0] == 'f' && line[1] == ' ') {
      // 顶点索引/纹理坐标索引/法线向量索引
      // f 3/3/1 2/2/1 1/1/1
      static const int kFaceCount = 3;
      const char ds[] = "/";
      char *sublast = NULL;
      int ftn[3];
      int cnt = 0;

      i = 0;

      token = strtok_r(line + 2, delimiters, &last);

      while (token != NULL) {
        if (strchr(token, ds[0]) != NULL) { // contains '/'
          char *t2 = strtok_r(token, ds, &sublast);
          while (t2 != NULL) {
            if (i < kFaceCount) {
              ftn[i] = atoi(t2) - 1;
              t2 = strtok_r(NULL, ds, &sublast);
            }
            i++;
          }

          IntVector_PushBack(&o->face_vrt, ftn[0]);
          IntVector_PushBack(&o->face_tex, ftn[1]);
          IntVector_PushBack(&o->face_nrm, ftn[2]);
        } else {
          ftn[0] = atoi(token) - 1;
          IntVector_PushBack(&o->face_vrt, ftn[0]);
        }

        token = strtok_r(NULL, delimiters, &last);

        cnt++;
        i = 0;
      }

      if (3 != cnt) {
        break;
      }
    }

    // Read next line
    line = swwFileLineReader_NextLineWithLength(line_reader, &len);

    /*
    std::cerr << "# v# " << nverts() << " f# " << nfaces() << " vt# "
              << tex_coord.size() << " vn# " << norms.size() << std::endl;
    load_texture(filename, "_diffuse.tga", diffusemap);
    load_texture(filename, "_nm_tangent.tga", normalmap);
    load_texture(filename, "_spec.tga", specularmap);
    */
  }

  swwFileLineReader_Destroy(line_reader);

  fclose(fp);
  return o;
}

int swwModel_LoadTexture(swwModel *o, const char *filepath) {
  swwImage *tex = swwImage_Load(filepath);

  if (tex == NULL) {
    return 0;
  }

  o->diffuse_map = tex;
  swwImage_FlipV(o->diffuse_map);

  return 1;
}

void swwModel_Destroy(swwModel *o) {
  swwModel_Destruct(o);
  free(o);
}

uint32_t swwModel_DiffuseUv(swwModel *o, uint32_t u, uint32_t v) {
  return swwImage_GetPixel(o->diffuse_map, u, v);
}

Vector2i swwModel_Uv(swwModel *o, int iface, int nvert) {
  int idx = Vector_At(&o->face_tex, nvert);

  float x = Vector_GetElement(&o->tex_coord, idx, x);
  float y = Vector_GetElement(&o->tex_coord, idx, y);

  return (Vector2i){x * o->diffuse_map->width, y * o->diffuse_map->height};
}

void swwModel_Save(swwModel *o, const char *filepath) {
  FILE *fp = fopen(filepath, "w+");
  size_t i = 0;
  size_t sz = 0;

  if (fp == NULL) {
    return;
  }

  // v
  sz = Vector_Size(&o->verts);
  for (i = 0; i < sz; i++) {
    fprintf(fp, "v %f %f %f\n", Vector_GetElement(&o->verts, i, x),
            Vector_GetElement(&o->verts, i, y),
            Vector_GetElement(&o->verts, i, z));
  }

  fprintf(fp, "\n");

  // vt
  sz = Vector_Size(&o->tex_coord);
  for (i = 0; i < sz; i++) {
    fprintf(fp, "vt %f %f\n", Vector_GetElement(&o->tex_coord, i, x),
            Vector_GetElement(&o->tex_coord, i, y));
  }

  fprintf(fp, "\n");

  // vn
  sz = Vector_Size(&o->norms);
  for (i = 0; i < sz; i++) {
    fprintf(fp, "vn %f %f %f\n", Vector_GetElement(&o->norms, i, x),
            Vector_GetElement(&o->norms, i, y),
            Vector_GetElement(&o->norms, i, z));
  }

  fprintf(fp, "\n");

  // f
  sz = Vector_Size(&o->face_vrt);
  if (Vector_Size(&o->face_tex) > 0 && Vector_Size(&o->face_nrm) > 0) {
    for (i = 0; i < sz; i += 3) {
      fprintf(fp, "f %d/%d/%d %d/%d/%d %d/%d/%d\n",
              Vector_At(&o->face_vrt, i) + 1, Vector_At(&o->face_tex, i) + 1,
              Vector_At(&o->face_nrm, i) + 1,
              Vector_At(&o->face_vrt, i + 1) + 1,
              Vector_At(&o->face_tex, i + 1) + 1,
              Vector_At(&o->face_nrm, i + 1) + 1,
              Vector_At(&o->face_vrt, i + 2) + 1,
              Vector_At(&o->face_tex, i + 2) + 1,
              Vector_At(&o->face_nrm, i + 2) + 1);
    }
  } else {
    for (i = 0; i < sz; i += 3) {
      fprintf(fp, "f %d %d %d\n", Vector_At(&o->face_vrt, i) + 1,
              Vector_At(&o->face_vrt, i + 1) + 1,
              Vector_At(&o->face_vrt, i + 2) + 1);
    }
  }

  fclose(fp);
}

int swwModel_VertexSize(swwModel *o) { return (int)Vector_Size(&o->verts); }

int swwModel_FaceSize(swwModel *o) {
  return (int)(Vector_Size(&o->face_vrt) / 3);
}

static Vector3f _swwModel_Vertex(swwModel *o, int i) {
  return Vector_At(&o->verts, i);
}

static Vector3f _swwModel_FaceVertex(swwModel *o, int iface,
                                     const int nthvert) {
  return Vector_At(&o->verts, Vector_At(&o->face_vrt, iface * 3 + nthvert));
}

void swwModel_Vertex(swwModel *o, int idx, float v[3]) {
  v[0] = Vector_GetElement(&o->verts, idx, x);
  v[1] = Vector_GetElement(&o->verts, idx, y);
  v[2] = Vector_GetElement(&o->verts, idx, z);
}

void swwModel_FaceVertex(swwModel *o, int iface, int nth, float v[3]) {
  int ifi = iface * 3 + nth;

  v[0] = Vector_GetElement(&o->verts, Vector_At(&o->face_vrt, ifi), x);
  v[1] = Vector_GetElement(&o->verts, Vector_At(&o->face_vrt, ifi), y);
  v[2] = Vector_GetElement(&o->verts, Vector_At(&o->face_vrt, ifi), z);
}

/*
void Model_LoadTexture(swwModel *o, const char *filename, const char *suffix)
{
}

Vector3f swwModel_normal(const vec2 &uvf) const {
    TGAColor c = normalmap.get(uvf[0]*normalmap.width(),
    uvf[1]*normalmap.height()); return
    vec3{(double)c[2],(double)c[1],(double)c[0]}*2./255. - vec3{1,1,1};
}

Vector2i swwModel_uv(swwModel *o, const int iface, const int nthvert) const {
    return tex_coord[facet_tex[iface*3+nthvert]];
}

Vecto3f swwModel_normal(swwModel *o, const int iface, const int nthvert) const {
    return norms[facet_nrm[iface*3+nthvert]];
}
*/

// normalized linear intep
// Vec3d swwUtil_Vector3Nlerp(const Vec3d& a, const Vec3d& b, double t)
// {
//     double am = a.length(), bm = b.length();
//     osg::Vec3d c = a*(1.0-t) + b*t;
//     c.normalize();
//     c *= (1.0-t)*am + t*bm;
//     return c;
// }

// linear interp
// Vec3d swwUtil_Vector3Lerp(const Vec3d& a, const Vec3d& b, double t)
// {
//     return a*(1.0-t) + b*t;
// }

// double swwUtil_NormalizeAzimRad(double input)
// {
//     if (fabs(input) > 2*PI)
//     {
//         input = fmod(input,2*osg::PI);
//     }
//     if (input < -PI) input += PI*2.0;
//     if (input > PI) input -= PI*2.0;
//     return input;
// }

/// FFT functions
#define q 3        /* for 2^3 points */
#define N (1 << q) /* N-point FFT, iFFT */

typedef float real;
typedef struct {
  real Re;
  real Im;
} complex;

#ifndef PI
#define PI 3.14159265358979323846264338327950288
#endif

/* Print a vector of complexes as ordered pairs. */
static void print_vector(const char *title, complex *x, int n) {
  int i;
  printf("%s (dim=%d):", title, n);
  for (i = 0; i < n; i++)
    printf(" %5.2f,%5.2f ", x[i].Re, x[i].Im);
  putchar('\n');
  return;
}

/*
   swwUtil:Fft(v,N):
   [0] If N==1 then return.
   [1] For k = 0 to N/2-1, let ve[k] = v[2*k]
   [2] Compute fft(ve, N/2);
   [3] For k = 0 to N/2-1, let vo[k] = v[2*k+1]
   [4] Compute fft(vo, N/2);
   [5] For m = 0 to N/2-1, do [6] through [9]
   [6]   Let w.re = cos(2*PI*m/N)
   [7]   Let w.im = -sin(2*PI*m/N)
   [8]   Let v[m] = ve[m] + w*vo[m]
   [9]   Let v[m+N/2] = ve[m] - w*vo[m]
 */
void swwUtil_Fft(complex *v, int n, complex *tmp) {
  if (n > 1) { /* otherwise, do nothing and return */
    int k, m;
    complex z, w, *vo, *ve;
    ve = tmp;
    vo = tmp + n / 2;
    for (k = 0; k < n / 2; k++) {
      ve[k] = v[2 * k];
      vo[k] = v[2 * k + 1];
    }
    swwUtil_Fft(ve, n / 2, v); /* FFT on even-indexed elements of v[] */
    swwUtil_Fft(vo, n / 2, v); /* FFT on odd-indexed elements of v[] */
    for (m = 0; m < n / 2; m++) {
      w.Re = cos(2 * PI * m / (double)n);
      w.Im = -sin(2 * PI * m / (double)n);
      z.Re = w.Re * vo[m].Re - w.Im * vo[m].Im; /* Re(w*vo[m]) */
      z.Im = w.Re * vo[m].Im + w.Im * vo[m].Re; /* Im(w*vo[m]) */
      v[m].Re = ve[m].Re + z.Re;
      v[m].Im = ve[m].Im + z.Im;
      v[m + n / 2].Re = ve[m].Re - z.Re;
      v[m + n / 2].Im = ve[m].Im - z.Im;
    }
  }
  return;
}

/*
   swwUtil_InverseFft(v,N):
   [0] If N==1 then return.
   [1] For k = 0 to N/2-1, let ve[k] = v[2*k]
   [2] Compute ifft(ve, N/2);
   [3] For k = 0 to N/2-1, let vo[k] = v[2*k+1]
   [4] Compute ifft(vo, N/2);
   [5] For m = 0 to N/2-1, do [6] through [9]
   [6]   Let w.re = cos(2*PI*m/N)
   [7]   Let w.im = sin(2*PI*m/N)
   [8]   Let v[m] = ve[m] + w*vo[m]
   [9]   Let v[m+N/2] = ve[m] - w*vo[m]
 */
void swwUtil_InverseFft(complex *v, int n, complex *tmp) {
  if (n > 1) { /* otherwise, do nothing and return */
    int k, m;
    complex z, w, *vo, *ve;
    ve = tmp;
    vo = tmp + n / 2;
    for (k = 0; k < n / 2; k++) {
      ve[k] = v[2 * k];
      vo[k] = v[2 * k + 1];
    }
    swwUtil_InverseFft(ve, n / 2, v); /* FFT on even-indexed elements of v[] */
    swwUtil_InverseFft(vo, n / 2, v); /* FFT on odd-indexed elements of v[] */
    for (m = 0; m < n / 2; m++) {
      w.Re = cos(2 * PI * m / (double)n);
      w.Im = sin(2 * PI * m / (double)n);
      z.Re = w.Re * vo[m].Re - w.Im * vo[m].Im; /* Re(w*vo[m]) */
      z.Im = w.Re * vo[m].Im + w.Im * vo[m].Re; /* Im(w*vo[m]) */
      v[m].Re = ve[m].Re + z.Re;
      v[m].Im = ve[m].Im + z.Im;
      v[m + n / 2].Re = ve[m].Re - z.Re;
      v[m + n / 2].Im = ve[m].Im - z.Im;
    }
  }
  return;
}