/*------------------------------------\
|  Liberally adpated From:
|  volInt.c
|
|  This code computes volume integrals needed for
|  determining mass properties of polyhedral bodies.
|
|  Brian Mirtich, "Fast and Accurate Computation of
|  Polyhedral Mass Properties," journal of graphics
|  tools, volume 1, number 2, 1996.
|
|  This source code is public domain, and may be used
|  in any way, shape or form, free of charge.
|
|  Copyright 1995 by Brian Mirtich
|
|  mirtich@cs.berkeley.edu
|  http://www.cs.berkeley.edu/~mirtich
|
+--------------------------------------
| Rewritten by Adam Freidin to take advantage of his
| vector library, and optimized a bit more
|     Jan 2002
|
| Redesigned to be usefull
|    -Adam Freidin, Jan 2002.
\--------------------------------------*/

/*--------------------------------------\
| Disclaimer:
|   This code is certified by me to work, and work well,
|   Since I have no credentials as yet,
|    (perhaps other than this code here)
|   my certification means nothing whatsoever.
|
| Simply stated: USE AT YOUR OWN RISK
|
| You have permision to use this library free
|  free programs free of charge. 
| However for anything commercial, I get 
|  a copy of the product.
\---------------------------------------*/

#include<string.h>
#include<stdlib.h>
#include<math.h>

#include"vector.h"
#include"geom.h"

typedef struct volumeDataD_tag {
  double 
    P1, 
    Pa, Pb, 
    Paa, Pab, Pbb, 
    Paaa, Paab, Pabb, Pbbb;
  double F1[3];
  double F2[3];
  double F3[3];
  double F3p[3];
  double p[4];
  double density, volume;
  double cm[3];
  double mi[3];
  double ip[3];
  int A, B, C;
} volumeDataD_t, *volumeDataD_ptr;

typedef struct volumeDataF_tag {
  float 
    P1, 
    Pa, Pb, 
    Paa, Pab, Pbb, 
    Paaa, Paab, Pabb, Pbbb;
  float F1[3];
  float F2[3];
  float F3[3];
  float F3p[3];
  float p[4];
  float density, volume;
  float cm[3];
  float mi[3];
  float ip[3];
  int A, B, C;
} volumeDataF_t, *volumeDataF_ptr;

#define VOLUME_TYPES_DEFINED
#include"volume.h"

void volumeEdged(volumeDataD_ptr vd, const double a[3], const double b[3])
{ 
  double 
    a0, a1, da,
    b0, b1, db,
    a0_2, a0_3, a0_4, b0_2, b0_3, b0_4,
    a1_2, a1_3, b1_2, b1_3,
    C1, Ca, Caa, Caaa, 
        Cb, Cbb, Cbbb,
    Cab, Caab, Cabb,
    Kab, Kaab, Kabb;

  a0 = a[vd->A];
  b0 = a[vd->B];
  a1 = b[vd->A];
  b1 = b[vd->B];
  da = a1 - a0;
  db = b1 - b0;
  a0_2 = a0 * a0; a0_3 = a0_2 * a0; a0_4 = a0_3 * a0;
  b0_2 = b0 * b0; b0_3 = b0_2 * b0; b0_4 = b0_3 * b0;
  a1_2 = a1 * a1; a1_3 = a1_2 * a1; 
  b1_2 = b1 * b1; b1_3 = b1_2 * b1;

  C1   = a1 + a0;
  Ca   = a1*C1  + a0_2;
  Caa  = a1*Ca  + a0_3;
  Caaa = a1*Caa + a0_4;
  Cb   = b1*(b1 + b0) + b0_2;
  Cbb  = b1*Cb  + b0_3; 
  Cbbb = b1*Cbb + b0_4;
  Cab  = 3.0*a1_2 + 2.0*a1*a0 +     a0_2;
  Kab  =     a1_2 + 2.0*a1*a0 + 3.0*a0_2;
  Caab = a0*Cab + 4.0*a1_3;
  Kaab = a1*Kab + 4.0*a0_3;
  Cabb = 4.0*b1_3 + 3.0*b1_2*b0 + 2.0*b1*b0_2 +     b0_3;
  Kabb =     b1_3 + 2.0*b1_2*b0 + 3.0*b1*b0_2 + 4.0*b0_3;

  vd->P1   += db*C1;
  vd->Pa   += db*Ca;
  vd->Paa  += db*Caa;
  vd->Paaa += db*Caaa;
  vd->Pb   += da*Cb;
  vd->Pbb  += da*Cbb;
  vd->Pbbb += da*Cbbb;
  vd->Pab  += db*(b1*Cab  + b0*Kab);
  vd->Paab += db*(b1*Caab + b0*Kaab);
  vd->Pabb += da*(a1*Cabb + a0*Kabb);
}

void volumeEdgef(volumeDataF_ptr vd, const float a[3], const float b[3])
{ 
  float 
    a0, a1, da,
    b0, b1, db,
    a0_2, a0_3, a0_4, b0_2, b0_3, b0_4,
    a1_2, a1_3, b1_2, b1_3,
    C1, Ca, Caa, Caaa, 
        Cb, Cbb, Cbbb,
    Cab, Caab, Cabb,
    Kab, Kaab, Kabb;

  a0 = a[vd->A];
  b0 = a[vd->B];
  a1 = b[vd->A];
  b1 = b[vd->B];
  da = a1 - a0;
  db = b1 - b0;
  a0_2 = a0 * a0; a0_3 = a0_2 * a0; a0_4 = a0_3 * a0;
  b0_2 = b0 * b0; b0_3 = b0_2 * b0; b0_4 = b0_3 * b0;
  a1_2 = a1 * a1; a1_3 = a1_2 * a1; 
  b1_2 = b1 * b1; b1_3 = b1_2 * b1;

  C1   = a1 + a0;
  Ca   = a1*C1  + a0_2;
  Caa  = a1*Ca  + a0_3;
  Caaa = a1*Caa + a0_4;
  Cb   = b1*(b1 + b0) + b0_2;
  Cbb  = b1*Cb  + b0_3; 
  Cbbb = b1*Cbb + b0_4;
  Cab  = 3.0f*a1_2 + 2.0f*a1*a0 +      a0_2;
  Kab  =      a1_2 + 2.0f*a1*a0 + 3.0f*a0_2;
  Caab = a0*Cab + 4.0f*a1_3;
  Kaab = a1*Kab + 4.0f*a0_3;
  Cabb = 4.0f*b1_3 + 3.0f*b1_2*b0 + 2.0f*b1*b0_2 +      b0_3;
  Kabb =      b1_3 + 2.0f*b1_2*b0 + 3.0f*b1*b0_2 + 4.0f*b0_3;

  vd->P1   += db*C1;
  vd->Pa   += db*Ca;
  vd->Paa  += db*Caa;
  vd->Paaa += db*Caaa;
  vd->Pb   += da*Cb;
  vd->Pbb  += da*Cbb;
  vd->Pbbb += da*Cbbb;
  vd->Pab  += db*(b1*Cab  + b0*Kab);
  vd->Paab += db*(b1*Caab + b0*Kaab);
  vd->Pabb += da*(a1*Cabb + a0*Kabb);
}

void volumeEndFaced(volumeDataD_ptr vd)
{ 
  int A, B, C;
  int _idx[3] = {0, 2, 1};
  double k1, k2, k3, k4;
  double Saabb, Sp3p1, Sa2b;
  double *p = vd->p;

  A = vd->A;
  B = vd->B;
  C = vd->C;

  vd->P1   /=   2.0;
  vd->Pa   /=   6.0;
  vd->Paa  /=  12.0;
  vd->Paaa /=  20.0;
  vd->Pb   /=  -6.0;
  vd->Pbb  /= -12.0;
  vd->Pbbb /= -20.0;
  vd->Pab  /=  24.0;
  vd->Paab /=  60.0;
  vd->Pabb /= -60.0;

  k1 = 1.0 / p[C];
  k2 = k1 * k1;
  k3 = k2 * k1;
  k4 = k3 * k1;

  Saabb = p[A]*vd->Pa + p[B]*vd->Pb;
  Sp3p1 = p[3]*vd->P1;
  Sa2b =  p[A]*(p[A]*vd->Paa + p[B]*vd->Pab) + 
          p[B]*(p[B]*vd->Pbb + p[A]*vd->Pab);

  vd->F1[0] =  k1 * vd->Pa;
  vd->F1[1] =  k1 * vd->Pb;
  vd->F1[2] = -k2 * (Saabb + Sp3p1);

  vd->F2[0] = k1 * vd->Paa;
  vd->F2[1] = k1 * vd->Pbb;
  vd->F2[2] = k3 * (Sa2b + p[3]*(2.0*Saabb + Sp3p1));

  vd->F3[0] =  k1 * vd->Paaa;
  vd->F3[1] =  k1 * vd->Pbbb;
  vd->F3[2] = -k4 * (
       p[A]*p[A]*(p[A]*vd->Paaa + 3.0*p[B]*vd->Paab) 
           + p[B]*p[B]*(p[B]*vd->Pbbb + 3.0*p[A]*vd->Pabb) 
           + 3.0*vd->p[3]*(
         Sa2b
     ) + p[3]*p[3]*(3.0*Saabb + Sp3p1)
  );

  vd->F3p[0] =  k1 * vd->Paab;
  vd->F3p[1] = -k2 * (p[A]*vd->Pabb + p[B]*vd->Pbbb + p[3]*vd->Pbb);
  vd->F3p[2] =  k3 * (
    p[A]*(p[A]*vd->Paaa + p[B]*vd->Paab) + 
    p[B]*(p[B]*vd->Pabb + p[A]*vd->Paab)
       + p[3]*(2.0*(p[A]*vd->Paa + p[B]*vd->Pab) + p[3]*vd->Pa)
  );

  vd->volume += p[0] * vd->F1[_idx[A]];

  vd->cm[A] += p[A] * vd->F2[0];
  vd->cm[B] += p[B] * vd->F2[1];
  vd->cm[C] += p[C] * vd->F2[2];
  vd->mi[A] += p[A] * vd->F3[0];
  vd->mi[B] += p[B] * vd->F3[1];
  vd->mi[C] += p[C] * vd->F3[2];
  vd->ip[A] += p[A] * vd->F3p[0];
  vd->ip[B] += p[B] * vd->F3p[1];
  vd->ip[C] += p[C] * vd->F3p[2];
}

void volumeEndFacef(volumeDataF_ptr vd)
{ 
  int A, B, C;
  int _idx[3] = {0, 2, 1};
  float k1, k2, k3, k4;
  float Saabb, Sp3p1, Sa2b;
  float *p = vd->p;

  A = vd->A;
  B = vd->B;
  C = vd->C;

  vd->P1   /=   2.0f;
  vd->Pa   /=   6.0f;
  vd->Paa  /=  12.0f;
  vd->Paaa /=  20.0f;
  vd->Pb   /=  -6.0f;
  vd->Pbb  /= -12.0f;
  vd->Pbbb /= -20.0f;
  vd->Pab  /=  24.0f;
  vd->Paab /=  60.0f;
  vd->Pabb /= -60.0f;

  k1 = 1.0f / p[C];
  k2 = k1 * k1;
  k3 = k2 * k1;
  k4 = k3 * k1;

  Saabb = p[A]*vd->Pa + p[B]*vd->Pb;
  Sp3p1 = p[3]*vd->P1;
  Sa2b =  p[A]*(p[A]*vd->Paa + p[B]*vd->Pab) + 
          p[B]*(p[B]*vd->Pbb + p[A]*vd->Pab);

  vd->F1[0] =  k1 * vd->Pa;
  vd->F1[1] =  k1 * vd->Pb;
  vd->F1[2] = -k2 * (Saabb + Sp3p1);

  vd->F2[0] = k1 * vd->Paa;
  vd->F2[1] = k1 * vd->Pbb;
  vd->F2[2] = k3 * (Sa2b + p[3]*(2.0f*Saabb + Sp3p1));

  vd->F3[0] =  k1 * vd->Paaa;
  vd->F3[1] =  k1 * vd->Pbbb;
  vd->F3[2] = -k4 * (
       p[A]*p[A]*(p[A]*vd->Paaa + 3.0f*p[B]*vd->Paab) 
     + p[B]*p[B]*(p[B]*vd->Pbbb + 3.0f*p[A]*vd->Pabb) 
     + 3.0f*vd->p[3]*(
       Sa2b
     ) + p[3]*p[3]*(3.0f*Saabb + Sp3p1)
  );

  vd->F3p[0] =  k1 * vd->Paab;
  vd->F3p[1] = -k2 * (p[A]*vd->Pabb + p[B]*vd->Pbbb + p[3]*vd->Pbb);
  vd->F3p[2] =  k3 * (
    p[A]*(p[A]*vd->Paaa + p[B]*vd->Paab) + 
    p[B]*(p[B]*vd->Pabb + p[A]*vd->Paab)
      + p[3]*(2.0f*(p[A]*vd->Paa + p[B]*vd->Pab) + p[3]*vd->Pa)
  );

  vd->volume += p[0] * vd->F1[_idx[A]];

  vd->cm[A] += p[A] * vd->F2[0];
  vd->cm[B] += p[B] * vd->F2[1];
  vd->cm[C] += p[C] * vd->F2[2];
  vd->mi[A] += p[A] * vd->F3[0];
  vd->mi[B] += p[B] * vd->F3[1];
  vd->mi[C] += p[C] * vd->F3[2];
  vd->ip[A] += p[A] * vd->F3p[0];
  vd->ip[B] += p[B] * vd->F3p[1];
  vd->ip[C] += p[C] * vd->F3p[2];
}

void volumeStartFaced(volumeDataD_ptr vd, double p[4])
{
  vd->C = absmaxindex3d(p);
  vd->A = (vd->C + 1) % 3;
  vd->B = (vd->A + 1) % 3;

  vd->P1 = 
  vd->Pa = vd->Pb = 
  vd->Paa = vd->Pab = vd->Pbb =
  vd->Paaa = vd->Paab = vd->Pabb = vd->Pbbb =
    0.0;
  cpy4d(vd->p, p);
}

void volumeStartFacef(volumeDataF_ptr vd, float p[4])
{
  vd->C = absmaxindex3f(p);
  vd->A = (vd->C + 1) % 3;
  vd->B = (vd->A + 1) % 3;

  vd->P1 = 
  vd->Pa = vd->Pb = 
  vd->Paa = vd->Pab = vd->Pbb =
  vd->Paaa = vd->Paab = vd->Pabb = vd->Pbbb =
    0.0f;
  cpy4f(vd->p, p);
}

volumeDataF_ptr volumeStartf(float density)
{
  volumeDataF_ptr vd;

  vd = malloc(sizeof(volumeDataF_t));

  vd->density = density;
  vd->volume = 0.0f;
  set3f(vd->cm, 0.0f, 0.0f, 0.0f);
  set3f(vd->mi, 0.0f, 0.0f, 0.0f);
  set3f(vd->ip, 0.0f, 0.0f, 0.0f);

  return vd;
}

volumeDataD_ptr volumeStartd(double density)
{
  volumeDataD_ptr vd;

  vd = malloc(sizeof(volumeDataD_t));
  
  vd->density = density;
  vd->volume = 0.0;
  set3d(vd->cm, 0.0, 0.0, 0.0);
  set3d(vd->mi, 0.0, 0.0, 0.0);
  set3d(vd->ip, 0.0, 0.0, 0.0);

  return vd;
}

void volumeEndd(volumeDataD_ptr vd, double *mass, double cm[3], double J[3])
{
  mul3d(vd->cm, vd->cm, 0.5);
  mul3d(vd->mi, vd->mi, 1.0/3.0);
  mul3d(vd->ip, vd->ip, 0.5);

  *mass = vd->volume*vd->density;
  mul3d(cm, vd->cm, 1.0/vd->volume);

  J[0+0*3] = vd->density * (vd->mi[1] + vd->mi[2]);
  J[1+1*3] = vd->density * (vd->mi[2] + vd->mi[0]);
  J[2+2*3] = vd->density * (vd->mi[0] + vd->mi[1]);

  J[0+1*3] = J[1+0*3] = -vd->density*vd->ip[0];
  J[1+2*3] = J[2+1*3] = -vd->density*vd->ip[1];
  J[2+0*3] = J[0+2*3] = -vd->density*vd->ip[2];

  J[0+0*3] -= (*mass)*(cm[1]*cm[1] + cm[2]*cm[2]);
  J[1+1*3] -= (*mass)*(cm[2]*cm[2] + cm[0]*cm[0]);
  J[2+2*3] -= (*mass)*(cm[0]*cm[0] + cm[1]*cm[1]);
  
  J[0+1*3] = J[1+0*3] += (*mass)*cm[0]*cm[1];
  J[1+2*3] = J[2+1*3] += (*mass)*cm[1]*cm[2];
  J[2+0*3] = J[0+2*3] += (*mass)*cm[2]*cm[0];

  free(vd);
}

void volumeEndf(volumeDataF_ptr vd, float *mass, float cm[3], float J[9])
{
  mul3f(vd->cm, vd->cm, 0.5f);
  mul3f(vd->mi, vd->mi, 1.0f/3.0f);
  mul3f(vd->ip, vd->ip, 0.5f);

  *mass = vd->volume*vd->density;
  mul3f(cm, vd->cm, 1.0f/vd->volume);

  J[0+0*3] = vd->density * (vd->mi[1] + vd->mi[2]);
  J[1+1*3] = vd->density * (vd->mi[2] + vd->mi[0]);
  J[2+2*3] = vd->density * (vd->mi[0] + vd->mi[1]);

  J[0+1*3] = J[1+0*3] = -vd->density*vd->ip[0];
  J[1+2*3] = J[2+1*3] = -vd->density*vd->ip[1];
  J[2+0*3] = J[0+2*3] = -vd->density*vd->ip[2];

  J[0+0*3] -= (*mass)*(cm[1]*cm[1] + cm[2]*cm[2]);
  J[1+1*3] -= (*mass)*(cm[2]*cm[2] + cm[0]*cm[0]);
  J[2+2*3] -= (*mass)*(cm[0]*cm[0] + cm[1]*cm[1]);
  
  J[0+1*3] = J[1+0*3] += (*mass)*cm[0]*cm[1];
  J[1+2*3] = J[2+1*3] += (*mass)*cm[1]*cm[2];
  J[2+0*3] = J[0+2*3] += (*mass)*cm[2]*cm[0];

  free(vd);
}