/* 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)
};