/**
  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);
  } 
}
示例#2
0
文件: system.cpp 项目: ryseto/stodyn
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;
}