void inertia_triangle(double *v0, double *v1, double *v2,
double mass, double *inertia)
{
double s[3][3] = {{2.0, 1.0, 1.0}, {1.0, 2.0, 1.0}, {1.0, 1.0, 2.0}};
double v[3][3],sv[3][3],vtsv[3][3];
double vvv[3],v1mv0[3],v2mv0[3],normal[3];
v[0][0] = v0[0]; v[0][1] = v0[1]; v[0][2] = v0[2];
v[1][0] = v1[0]; v[1][1] = v1[1]; v[1][2] = v1[2];
v[2][0] = v2[0]; v[2][1] = v2[1]; v[2][2] = v2[2];
times3(s,v,sv);
transpose_times3(v,sv,vtsv);
double sum = lensq3(v0) + lensq3(v1) + lensq3(v2);
vvv[0] = v0[0] + v1[0] + v2[0];
vvv[1] = v0[1] + v1[1] + v2[1];
vvv[2] = v0[2] + v1[2] + v2[2];
sum += lensq3(vvv);
sub3(v1,v0,v1mv0);
sub3(v2,v0,v2mv0);
cross3(v1mv0,v2mv0,normal);
double a = len3(normal);
double inv24 = mass/24.0;
inertia[0] = inv24*a*(sum-vtsv[0][0]);
inertia[1] = inv24*a*(sum-vtsv[1][1]);
inertia[2] = inv24*a*(sum-vtsv[2][2]);
inertia[3] = -inv24*a*vtsv[1][2];
inertia[4] = -inv24*a*vtsv[0][2];
inertia[5] = -inv24*a*vtsv[0][1];
}
int main()
{
int a = 2; int b=0;
times3(a, b);
msg(3, a, b);
times5(a, b);
msg(5, a, b);
times9(a, b);
msg(9, a, b);
return 0;
}
void inertia_triangle(double *idiag, double *quat, double mass,
double *inertia)
{
double p[3][3],ptrans[3][3],itemp[3][3],tensor[3][3];
quat_to_mat(quat,p);
quat_to_mat_trans(quat,ptrans);
diag_times3(idiag,ptrans,itemp);
times3(p,itemp,tensor);
inertia[0] = tensor[0][0];
inertia[1] = tensor[1][1];
inertia[2] = tensor[2][2];
inertia[3] = tensor[1][2];
inertia[4] = tensor[0][2];
inertia[5] = tensor[0][1];
}
void inertia_ellipsoid(double *radii, double *quat, double mass,
double *inertia)
{
double p[3][3],ptrans[3][3],itemp[3][3],tensor[3][3];
double idiag[3];
quat_to_mat(quat,p);
quat_to_mat_trans(quat,ptrans);
idiag[0] = 0.2*mass * (radii[1]*radii[1] + radii[2]*radii[2]);
idiag[1] = 0.2*mass * (radii[0]*radii[0] + radii[2]*radii[2]);
idiag[2] = 0.2*mass * (radii[0]*radii[0] + radii[1]*radii[1]);
diag_times3(idiag,ptrans,itemp);
times3(p,itemp,tensor);
inertia[0] = tensor[0][0];
inertia[1] = tensor[1][1];
inertia[2] = tensor[2][2];
inertia[3] = tensor[1][2];
inertia[4] = tensor[0][2];
inertia[5] = tensor[0][1];
}
