/**
Compute the normal for a polygon.
Basically, the method simply takes the first three vertices A, B, C and
computes the normal of that triangle.
However, it is checked that A!=B and B!=C, so it is possible that more than
three vertices are looked up.
\param poly Polygon index
\param[out] N Receives the resulting normal
*/
void PolyhedronGeom::computeNormal(int poly, vec3d& N)
{
VertexLoop& loop = *(*polys[poly])[0];
int size = loop.size();
int i = 2;
if (size<3)
return;
const vec3d* a = &(verts.getValue(loop[0]));
const vec3d* b = &(verts.getValue(loop[1]));
while(a==b)
{
if (i>=size)
return;
b = &(verts.getValue(loop[i]));
i++;
}
const vec3d* c = &(verts.getValue(loop[i]));
while(b==c)
{
if (i>=size)
return;
c = &(verts.getValue(loop[i]));
i++;
}
N.cross((*b)-(*a), (*c)-(*a));
try
{
N.normalize(N);
}
catch(...)
{
N.set(0,0,0);
}
}
void System::solveForceFreeRigidMotion(vec3d &Utf,
vec3d &Otf){
smallmatrix Mag_uf; Mag_uf.allocate_memory(3,3);
smallmatrix Mag_ut; Mag_ut.allocate_memory(3,3);
smallmatrix Mag_us; Mag_us.allocate_memory(3,5);
//////////////////////////
smallmatrix Mag_of; Mag_of.allocate_memory(3,3);
smallmatrix Mag_ot; Mag_ot.allocate_memory(3,3);
smallmatrix Mag_os; Mag_os.allocate_memory(3,5);
//////////////////////////
smallmatrix Mag_ef; Mag_ef.allocate_memory(5,3);
smallmatrix Mag_et; Mag_et.allocate_memory(5,3);
smallmatrix Mag_es; Mag_es.allocate_memory(5,5);
for (int i=0; i < 121; i++){
Mag[i] = Rag[i];
}
lapack_inv_ (11, Mag);
for (int l=0; l< 3; l++){
for (int k=0; k < 3 ; k++){
Mag_uf.element[l][k] = Mag[k + 11*l];
Mag_ut.element[l][k] = Mag[3+k + 11*l];
Mag_of.element[l][k] = Mag[k + 11*(3+l)];
Mag_ot.element[l][k] = Mag[3+k + 11*(3+l)];
}
for (int k=0; k < 5 ; k++){
Mag_us.element[l][k] = Mag[6+k + 11*l];
Mag_os.element[l][k] = Mag[6+k + 11*(3+l)];
}
}
for (int l=0; l< 5; l++){
for (int k=0; k < 3 ; k++){
Mag_ef.element[l][k] = Mag[k + 11*(6+l)];
Mag_et.element[l][k] = Mag[3+k + 11*(6+l)];
}
for (int k=0; k < 5 ; k++){
Mag_es.element[l][k] = Mag[6+k + 11*(6+l)];
}
}
double *array_inv_Mag_es;
array_inv_Mag_es = new double [25];
for (int l=0; l< 5; l++){
for (int k=0; k < 5 ; k++){
array_inv_Mag_es[k + l*5] = Mag_es.element[l][k];
}
}
lapack_inv_ (5, array_inv_Mag_es);
smallmatrix inv_Mag_es;
inv_Mag_es.allocate_memory(5,5);
for (int l=0; l< 5; l++){
for (int k=0; k < 5 ; k++){
inv_Mag_es.element[l][k] = array_inv_Mag_es[k + l*5];
}
}
double *Sag;
Sag = new double [5];
for (int l=0; l < 5; l++) {Sag[l] = 0;}
inv_Mag_es.multiplyVector( sd->Ei, Sag );
double *Uag, *Oag;
Uag = new double[3];
Oag = new double[3];
Mag_us.multiplyVector( Sag, Uag );
for (int l=0; l < 3 ; l++) Uag[l] = - Uag[l];
Mag_os.multiplyVector( Sag, Oag );
for (int l=0; l < 3 ; l++) Oag[l] = - Oag[l];
Oag[0] += sd->Oi[0];
Oag[1] += sd->Oi[1];
Oag[2] += sd->Oi[2];
Utf.set(Uag[0], Uag[1], Uag[2]);
Otf.set(Oag[0], Oag[1], Oag[2]);
DELETE(Sag);
DELETE(Uag);
DELETE(Oag);
DELETE(array_inv_Mag_es);
return;
}