/* Copyright (c) <2003-2011> <Julio Jerez, Newton Game Dynamics>
 * 
 * This software is provided 'as-is', without any express or implied
 * warranty. In no event will the authors be held liable for any damages
 * arising from the use of this software.
 * 
 * Permission is granted to anyone to use this software for any purpose,
 * including commercial applications, and to alter it and redistribute it
 * freely, subject to the following restrictions:
 * 
 * 1. The origin of this software must not be misrepresented; you must not
 * claim that you wrote the original software. If you use this software
 * in a product, an acknowledgment in the product documentation would be
 * appreciated but is not required.
 * 
 * 2. Altered source versions must be plainly marked as such, and must not be
 * misrepresented as being the original software.
 * 
 * 3. This notice may not be removed or altered from any source distribution.
 */

#include "dgStdafx.h"
#include "dgVector.h"
#include "dgPolyhedraMassProperties.h"

#if 0
class dgPolyhedraMassProperties
{
#define X 0
#define Y 1
#define Z 2
#define SQR(x) ((x)*(x))
#define CUBE(x) ((x)*(x)*(x))

public:
  dgPolyhedraMassProperties()
  {
    memset (this, 0, sizeof (dgPolyhedraMassProperties));
  }

  void ProjectionIntegrals(
      dgInt32 indexCount,
      const dgVector* faceVertex)
  {
    dgInt32 i0;
    dgInt32 i1;
    dgFloat32 a0, a1, da;
    dgFloat32 b0, b1, db;

    dgFloat32 C1;
    dgFloat32 Ca;
    dgFloat32 Cb;
    dgFloat32 Caa;
    dgFloat32 Cbb;
    dgFloat32 Cab;
    dgFloat32 Kab;
    dgFloat32 a0_2;
    dgFloat32 a0_3;
    dgFloat32 a1_2;
    dgFloat32 b0_2;
    dgFloat32 b0_3;

    m_P1 = dgFloat32 (0.0f);
    m_Pa = dgFloat32 (0.0f);
    m_Pb = dgFloat32 (0.0f);
    m_Paa = dgFloat32 (0.0f);
    m_Pbb = dgFloat32 (0.0f);
    m_Pab = dgFloat32 (0.0f);

    i0 = indexCount - 1;
    for (i1 = 0; i1 < indexCount; i1 ++)
    {
      a0 = faceVertex[i0][m_A];
      b0 = faceVertex[i0][m_B];

      a1 = faceVertex[i1][m_A];
      b1 = faceVertex[i1][m_B];
      i0 = i1;

      da = a1 - a0;
      db = b1 - b0;

      a0_2 = a0 * a0;
      a0_3 = a0_2 * a0;

      b0_2 = b0 * b0;
      b0_3 = b0_2 * b0;

      a1_2 = a1 * a1;

      C1 = a1 + a0;
      Ca = a1 * C1 + a0_2;
      Caa = a1*Ca + a0_3;

      Cb = b1 * (b1 + b0) + b0_2;
      Cbb = b1*Cb + b0_3;

      Cab = dgFloat32 (3.0f) * a1_2 + dgFloat32 (2.0f) * a1 * a0 + a0_2;
      Kab = a1_2 + dgFloat32 (2.0f) * a1 * a0 + dgFloat32 (3.0f) * a0_2;

      m_P1 += db * C1;
      m_Pa += db * Ca;
      m_Paa += db * Caa;

      m_Pb += da * Cb;
      m_Pbb += da * Cbb;
      m_Pab += db * (b1 * Cab + b0 * Kab);
    }

    m_P1 *= dgFloat32 (0.5f);
    m_Pa *= dgFloat32 ( 1.0f / 6.0f);
    m_Pb *= dgFloat32 (-1.0f / 6.0f);
    m_Paa *= dgFloat32 (1.0f / 12.0f);
    m_Pbb *= dgFloat32 (-1.0f / 12.0f);
    m_Pab *= dgFloat32 ( 1.0f / 24.0f);
  }

  void FaceIntegrals (
      dgInt32 count,
      const dgPlane& plane,
      const dgVector* faceVertex)
  {

    dgFloat32 k1, k2, k3, k4;

    ProjectionIntegrals (count, faceVertex);

    k1 = dgFloat32 (1.0f) / plane[m_C];
    k2 = k1 * k1;
    k3 = k2 * k1;
    k4 = k3 * k1;

    m_Fa = k1 * m_Pa;
    m_Fb = k1 * m_Pb;
    m_Fc = -k2 * (plane[m_A] * m_Pa + plane[m_B] * m_Pb + plane[3] * m_P1);

    m_Faa = k1 * m_Paa;
    m_Fbb = k1 * m_Pbb;
    m_Fcc = k3 * (SQR(plane[m_A]) * m_Paa + dgFloat32 (2.0f) * plane[m_A] * plane[m_B] * m_Pab +
        SQR(plane[m_B]) * m_Pbb + plane[3] * (dgFloat32 (2.0f) *(plane[m_A] * m_Pa + plane[m_B] * m_Pb) + plane[3] * m_P1));
  }

  void VolumeIntegrals(
      dgInt32 indexCount,
      const dgPlane& plane,
      const dgVector* faceVertex)
  {
    dgFloat32 mag2;

    mag2 = plane % plane;
    if (mag2 > dgFloat32 (1.0e-8f))
    {
      if ((dgAbsf (plane.m_x) > dgAbsf (plane.m_y)) && (dgAbsf (plane.m_x) > dgAbsf (plane.m_z)))
      {
        m_C = X;
      }
      else
      {
        m_C = (dgAbsf (plane.m_y) > dgAbsf (plane.m_z)) ? Y : Z;
      }
      m_A = (m_C + 1) % 3;
      m_B = (m_A + 1) % 3;
      FaceIntegrals (indexCount, plane, faceVertex);

      m_T0 += plane[X] * ((m_A == X) ? m_Fa : ((m_B == X) ? m_Fb : m_Fc));

      m_T1[m_A] += plane[m_A] * m_Faa;
      m_T1[m_B] += plane[m_B] * m_Fbb;
      m_T1[m_C] += plane[m_C] * m_Fcc;
      //dgTrace (("(%f %f %f) (%f %f %f) (%f %f %f)\n", m_T1[m_A], m_T1[m_B], m_T1[m_C], plane[m_A], plane[m_B], plane[m_C], m_Faa, m_Fbb, m_Fcc))
    }
  }

  void AddInertia (int indexCount, const dgFloat32* faceVertex)
  {
    dgInt32 i0;
    dgInt32 i1;
    dgFloat32 a0, a1, da;
    dgFloat32 b0, b1, db;
    dgFloat32 C1;
    dgFloat32 Ca;
    dgFloat32 Cb;
    dgFloat32 Caa;
    dgFloat32 Cbb;
    dgFloat32 Cab;
    dgFloat32 Kab;
    dgFloat32 Caaa;
    dgFloat32 Cbbb;
    dgFloat32 Cabb;
    dgFloat32 Caab;
    dgFloat32 Kabb;
    dgFloat32 Kaab;
    dgFloat32 a0_2;
    dgFloat32 a0_3;
    dgFloat32 a0_4;
    dgFloat32 a1_2;
    dgFloat32 a1_3;
    dgFloat32 b0_2;
    dgFloat32 b0_3;
    dgFloat32 b0_4;
    dgFloat32 b1_2;
    dgFloat32 b1_3;
    dgFloat32 mag2;

    dgVector p0 (&faceVertex[0]);
    dgVector p1 (&faceVertex[3]);
    dgVector p2 (&faceVertex[6]);
    dgPlane plane (p0, p1, p2);

    mag2 = plane % plane;
    if (mag2 > dgFloat32 (1.0e-8f))
    {
      plane = plane.Scale (dgRsqrt ((plane % plane)));
      if ((dgAbsf (plane.m_x) > dgAbsf (plane.m_y)) && (dgAbsf (plane.m_x) > dgAbsf (plane.m_z)))
      {
        m_C = X;
      }
      else
      {
        m_C = (dgAbsf (plane.m_y) > dgAbsf (plane.m_z)) ? Y : Z;
      }
      m_A = (m_C + 1) % 3;
      m_B = (m_A + 1) % 3;

      //	FaceIntegrals (indexCount, plane, faceVertex);
      dgFloat32 k1, k2, k3, k4;
      {
        {
          //ProjectionIntegrals (count, faceVertex);
          m_P1 = dgFloat32 (0.0f);
          m_Pa = dgFloat32 (0.0f);
          m_Pb = dgFloat32 (0.0f);
          m_Paa = dgFloat32 (0.0f);
          m_Pbb = dgFloat32 (0.0f);
          m_Pab = dgFloat32 (0.0f);

          m_Paaa = dgFloat32 (0.0f);
          m_Pbbb = dgFloat32 (0.0f);
          m_Paab = dgFloat32 (0.0f);
          m_Pabb = dgFloat32 (0.0f);

          i0 = indexCount - 1;
          for (i1 = 0; i1 < indexCount; i1 ++)
          {
            a0 = faceVertex[i0 * 3 + m_A];
            b0 = faceVertex[i0 * 3 + m_B];

            a1 = faceVertex[i1 * 3 + m_A];
            b1 = faceVertex[i1 * 3 + m_B];

            i0 = i1;

            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 = dgFloat32 (3.0f) * a1_2 + dgFloat32 (2.0f) * a1 * a0 + a0_2;
            Kab = a1_2 + dgFloat32 (2.0f) * a1 * a0 + dgFloat32 (3.0f) * a0_2;

            Caab = a0 * Cab + dgFloat32 (4.0f) * a1_3;
            Kaab = a1 * Kab + dgFloat32 (4.0f) * a0_3;
            Cabb = dgFloat32 (4.0f) * b1_3 + dgFloat32 (3.0f) * b1_2 * b0 + dgFloat32 (2.0f) * b1 * b0_2 + b0_3;
            Kabb = b1_3 + dgFloat32 (2.0f) * b1_2 * b0 + dgFloat32 (3.0f) * b1 * b0_2 + dgFloat32 (4.0f) * b0_3;

            m_P1 += (db * C1);
            m_Pa += (db * Ca);
            m_Paa += (db * Caa);

            m_Pb += (da * Cb);
            m_Pbb += (da * Cbb);
            m_Pab += (db * (b1 * Cab + b0 * Kab));

            m_Paaa += (db * Caaa);
            m_Pbbb += (da * Cbbb);
            m_Paab += (db * (b1 * Caab + b0 * Kaab));
            m_Pabb += (da * (a1 * Cabb + a0 * Kabb));
          }

          m_P1 *= dgFloat32 (0.5f);
          m_Pa *= dgFloat32 ( 1.0f / 6.0f);
          m_Pb *= dgFloat32 (-1.0f / 6.0f);
          m_Paa *= dgFloat32 (1.0f / 12.0f);
          m_Pbb *= dgFloat32 (-1.0f / 12.0f);
          m_Pab *= dgFloat32 ( 1.0f / 24.0f);

          m_Paaa *= dgFloat32 (1.0f / 20.0);
          m_Pbbb *= dgFloat32 (-1.0f / 20.0);
          m_Paab *= dgFloat32 (1.0f / 60.0);
          m_Pabb *= dgFloat32 (-1.0f / 60.0);
        }

        k1 = dgFloat32 (1.0f) / plane[m_C];
        k2 = k1 * k1;
        k3 = k2 * k1;
        k4 = k3 * k1;

        m_Fa = k1 * m_Pa;
        m_Fb = k1 * m_Pb;
        m_Fc = -k2 * (plane[m_A] * m_Pa + plane[m_B] * m_Pb + plane[3] * m_P1);

        m_Faa = k1 * m_Paa;
        m_Fbb = k1 * m_Pbb;
        m_Fcc = k3 * (SQR(plane[m_A]) * m_Paa + dgFloat32 (2.0f) * plane[m_A] * plane[m_B] * m_Pab +
            SQR(plane[m_B]) * m_Pbb + plane[3] * (dgFloat32 (2.0f) *(plane[m_A] * m_Pa + plane[m_B] * m_Pb) + plane[3] * m_P1));

        m_Faaa = k1 * m_Paaa;
        m_Fbbb = k1 * m_Pbbb;
        m_Fccc = -k4 * (CUBE(plane[m_A]) * m_Paaa +
            dgFloat32(3.0f) * SQR(plane[m_A]) * plane[m_B] * m_Paab +
            dgFloat32(3.0f) * plane[m_A] * SQR(plane[m_B]) * m_Pabb + CUBE(plane[m_B]) * m_Pbbb +
            dgFloat32(3.0f) * plane[3] * (SQR(plane[m_A]) * m_Paa + dgFloat32 (2.0f) * plane[m_A] * plane[m_B] * m_Pab + SQR(plane[m_B]) * m_Pbb) +
            SQR(plane[3]) * (dgFloat32(3.0f) * (plane[m_A] * m_Pa + plane[m_B] * m_Pb) + plane[3] * m_P1));

        m_Faab = k1 * m_Paab;
        m_Fbbc = -k2 * (plane[m_A] * m_Pabb + plane[m_B] * m_Pbbb + plane[3] * m_Pbb);
        m_Fcca = k3 * (SQR(plane[m_A]) * m_Paaa + dgFloat32 (2.0f) * plane[m_A] * plane[m_B] * m_Paab + SQR(plane[m_B]) * m_Pabb +
            plane[3] * (dgFloat32 (2.0f) * (plane[m_A] * m_Paa + plane[m_B] * m_Pab) + plane[3] * m_Pa));
      }

      m_T0 += (plane[X] * ((m_A == X) ? m_Fa : ((m_B == X) ? m_Fb : m_Fc)));

      m_T1[m_A] += (plane[m_A] * m_Faa);
      m_T1[m_B] += (plane[m_B] * m_Fbb);
      m_T1[m_C] += (plane[m_C] * m_Fcc);

      m_T2[m_A] += (plane[m_A] * m_Faaa);
      m_T2[m_B] += (plane[m_B] * m_Fbbb);
      m_T2[m_C] += (plane[m_C] * m_Fccc);

      m_TP[m_A] += (plane[m_A] * m_Faab);
      m_TP[m_B] += (plane[m_B] * m_Fbbc);
      m_TP[m_C] += (plane[m_C] * m_Fcca);
    }
  }

  dgInt32 m_A; // alpha 
  dgInt32 m_B;// beta 
  dgInt32 m_C;// gamma 

  dgFloat32 m_T0;
  dgFloat32 m_T1[3];
  dgFloat32 m_T2[3];
  dgFloat32 m_TP[3];

  dgFloat32 m_P1;
  dgFloat32 m_Pa;
  dgFloat32 m_Pb;

  dgFloat32 m_Paa;
  dgFloat32 m_Pbb;
  dgFloat32 m_Pab;

  dgFloat32 m_Paaa;
  dgFloat32 m_Pbbb;
  dgFloat32 m_Paab;
  dgFloat32 m_Pabb;

  dgFloat32 m_Fa;
  dgFloat32 m_Fb;
  dgFloat32 m_Fc;

  dgFloat32 m_Faa;
  dgFloat32 m_Fbb;
  dgFloat32 m_Fcc;

  dgFloat32 m_Faaa;
  dgFloat32 m_Fbbb;
  dgFloat32 m_Fccc;

  dgFloat32 m_Faab;
  dgFloat32 m_Fbbc;
  dgFloat32 m_Fcca;
};
#endif

dgPolyhedraMassProperties::dgPolyhedraMassProperties()
{
  memset(this, 0, sizeof(dgPolyhedraMassProperties));
}

void dgPolyhedraMassProperties::AddCGFace(dgInt32 indexCount,
    const dgVector* faceVertex)
{
  dgVector f1;
  dgVector f2;
  dgFloat32 temp0;

#define CDSubexpressions(w0,w1,w2,f1,f2) \
	{					\
		temp0 = w0 + w1; \
		f1 = temp0 + w2; \
		f2 = w0 * w0 + w1 * temp0 + w2 * f1; \
	}					

  const dgVector& p0 = faceVertex[0];
  dgVector p1(faceVertex[1]);

  for (dgInt32 i = 2; i < indexCount; i++)
  {
    const dgVector& p2 = faceVertex[i];

    dgVector e01(p1 - p0);
    dgVector e02(p2 - p0);
    dgVector d(e01 * e02);

    CDSubexpressions(p0.m_x, p1.m_x, p2.m_x, f1.m_x, f2.m_x);
    CDSubexpressions(p0.m_y, p1.m_y, p2.m_y, f1.m_y, f2.m_y);
    CDSubexpressions(p0.m_z, p1.m_z, p2.m_z, f1.m_z, f2.m_z);

    // update integrals
    intg[0] += d[0] * f1.m_x;

    intg[1] += d[0] * f2.m_x;
    intg[2] += d[1] * f2.m_y;
    intg[3] += d[2] * f2.m_z;

    p1 = p2;
  }
}

void dgPolyhedraMassProperties::AddInertiaFace(dgInt32 indexCount,
    const dgFloat32* faceVertex)
{
  dgInt32 i;
  dgFloat32 temp0;
  dgFloat32 temp1;
  dgFloat32 temp2;
  dgVector f1;
  dgVector f2;
  dgVector f3;

#define InertiaSubexpression(w0,w1,w2,f1,f2,f3) \
	{					 \
		temp0 = w0 + w1; \
		temp1 = w0 * w0; \
		temp2 = temp1 + w1 * temp0; \
		f1 = temp0 + w2; \
		f2 = temp2 + w2 * f1;  \
		f3 = w0 * temp1 + w1 * temp2 + w2 * f2; \
	}

  dgVector p0(faceVertex[0], faceVertex[1], faceVertex[2], 0.0f);
  dgVector p1(faceVertex[3], faceVertex[4], faceVertex[5], 0.0f);

  for (i = 2; i < indexCount; i++)
  {
    dgVector p2(faceVertex[i * 3], faceVertex[i * 3 + 1], faceVertex[i * 3 + 2],
        0.0f);

    dgVector e01(p1 - p0);
    dgVector e02(p2 - p0);
    dgVector d(e01 * e02);

    InertiaSubexpression(p0.m_x, p1.m_x, p2.m_x, f1.m_x, f2.m_x, f3.m_x);
    InertiaSubexpression(p0.m_y, p1.m_y, p2.m_y, f1.m_y, f2.m_y, f3.m_y);
    InertiaSubexpression(p0.m_z, p1.m_z, p2.m_z, f1.m_z, f2.m_z, f3.m_z);

    // update integrals
    intg[0] += d[0] * f1.m_x;

    intg[1] += d[0] * f2.m_x;
    intg[2] += d[1] * f2.m_y;
    intg[3] += d[2] * f2.m_z;

    intg[4] += d[0] * f3.m_x;
    intg[5] += d[1] * f3.m_y;
    intg[6] += d[2] * f3.m_z;

    p1 = p2;
  }
}

void dgPolyhedraMassProperties::AddInertiaAndCrossFace(dgInt32 indexCount,
    const dgFloat32* faceVertex)
{
  dgInt32 i;
  dgFloat32 temp0;
  dgFloat32 temp1;
  dgFloat32 temp2;
  dgVector f1;
  dgVector f2;
  dgVector f3;
  dgVector g0;
  dgVector g1;
  dgVector g2;

#define Subexpressions(w0,w1,w2,f1,f2,f3,g0,g1,g2) \
	{												   \
		temp0 = w0 + w1; \
		temp1 = w0 * w0; \
		temp2 = temp1 + w1 * temp0; \
		f1 = temp0 + w2; \
		f2 = temp2 + w2 * f1;  \
		f3 = w0 * temp1 + w1 * temp2 + w2 * f2; \
		g0 = f2 + w0 * (f1 + w0); \
		g1 = f2 + w1 * (f1 + w1); \
		g2 = f2 + w2 * (f1 + w2); \
	}

  dgVector p0(&faceVertex[0]);
  dgVector p1(&faceVertex[3]);

  for (i = 2; i < indexCount; i++)
  {
    dgVector p2(&faceVertex[i * 3]);

    dgVector e01(p1 - p0);
    dgVector e02(p2 - p0);
    dgVector d(e01 * e02);

    Subexpressions(p0.m_x, p1.m_x, p2.m_x, f1.m_x, f2.m_x, f3.m_x, g0.m_x,
        g1.m_x, g2.m_x);
    Subexpressions(p0.m_y, p1.m_y, p2.m_y, f1.m_y, f2.m_y, f3.m_y, g0.m_y,
        g1.m_y, g2.m_y);
    Subexpressions(p0.m_z, p1.m_z, p2.m_z, f1.m_z, f2.m_z, f3.m_z, g0.m_z,
        g1.m_z, g2.m_z);

    // update integrals
    intg[0] += d[0] * f1.m_x;

    intg[1] += d[0] * f2.m_x;
    intg[2] += d[1] * f2.m_y;
    intg[3] += d[2] * f2.m_z;

    intg[4] += d[0] * f3.m_x;
    intg[5] += d[1] * f3.m_y;
    intg[6] += d[2] * f3.m_z;

    intg[7] += d[0] * (p0.m_y * g0.m_x + p1.m_y * g1.m_x + p2.m_y * g2.m_x);
    intg[8] += d[1] * (p0.m_z * g0.m_y + p1.m_z * g1.m_y + p2.m_z * g2.m_y);
    intg[9] += d[2] * (p0.m_x * g0.m_z + p1.m_x * g1.m_z + p2.m_x * g2.m_z);

    p1 = p2;
  }
}

dgFloat32 dgPolyhedraMassProperties::MassProperties(dgVector& cg,
    dgVector& inertia, dgVector& crossInertia)
{
  dgFloat32 volume;

  for (dgInt32 i = 0; i < 10; i++)
  {
    intg[i] *= mult[i];
  }

  volume = intg[0];

  cg.m_x = intg[1];
  cg.m_y = intg[2];
  cg.m_z = intg[3];
  inertia.m_x = intg[5] + intg[6];
  inertia.m_y = intg[4] + intg[6];
  inertia.m_z = intg[4] + intg[5];
  inertia.m_w = dgFloat32(0.0f);
  crossInertia.m_x = -intg[8];
  crossInertia.m_y = -intg[9];
  crossInertia.m_z = -intg[7];
  crossInertia.m_w = dgFloat32(0.0f);
  return volume;
}

dgFloat32 dgPolyhedraMassProperties::mult[10] =
{ dgFloat32(1.0f / 6.0f), dgFloat32(1.0f / 24.0f), dgFloat32(1.0f / 24.0f),
    dgFloat32(1.0f / 24.0f), dgFloat32(1.0f / 60.0f), dgFloat32(1.0f / 60.0f),
    dgFloat32(1.0f / 60.0f), dgFloat32(1.0f / 120.0f), dgFloat32(1.0f / 120.0f),
    dgFloat32(1.0f / 120.0f) };

