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