Logo Search packages:      
Sourcecode: yade version File versions  Download package

Tetra.cpp

// © 2007 Václav Šmilauer <eudoxos@arcig.cz>

#include"Tetra.hpp"

YADE_PLUGIN(/* self-contained in hpp: */ (Tetra) (TTetraGeom) (Bo1_Tetra_Aabb) 
      /* some code in cpp (this file): */ (TetraVolumetricLaw) (Ig2_Tetra_Tetra_TTetraGeom)
      #ifdef YADE_OPENGL
            (Gl1_Tetra)
      #endif      
      );

Tetra::~Tetra(){}
TTetraGeom::~TTetraGeom(){}

#include<yade/core/Interaction.hpp>
#include<yade/core/Omega.hpp>
#include<yade/core/Scene.hpp>
#include<yade/core/State.hpp>
#include<yade/pkg-common/ElastMat.hpp>

#include<yade/pkg-common/Aabb.hpp>

void Bo1_Tetra_Aabb::go(const shared_ptr<Shape>& ig, shared_ptr<Bound>& bv, const Se3r& se3, const Body*){
      Tetra* t=static_cast<Tetra*>(ig.get());
      if(!bv){ bv=shared_ptr<Bound>(new Aabb); }
      Aabb* aabb=static_cast<Aabb*>(bv.get());
      Quaternionr invRot=se3.orientation.conjugate();
      Vector3r v_g[4]; for(int i=0; i<4; i++) v_g[i]=se3.orientation*t->v[i]; // vertices in global coordinates
      #define __VOP(op,ix) op(v_g[0][ix],op(v_g[1][ix],op(v_g[2][ix],v_g[3][ix])))
            aabb->min=se3.position+Vector3r(__VOP(std::min,0),__VOP(std::min,1),__VOP(std::min,2));
            aabb->max=se3.position+Vector3r(__VOP(std::max,0),__VOP(std::max,1),__VOP(std::max,2));
      #undef __VOP
}

CREATE_LOGGER(Ig2_Tetra_Tetra_TTetraGeom);

/*! Calculate configuration of Tetra - Tetra intersection.
 *
 * Wildmagick's functions are used here: intersection is returned as a set of tetrahedra (may be empty, inwhich case there is no real intersection).
 * Then we calcualte volumetric proeprties of this intersection volume: inertia, centroid, volume.
 *
 * Contact normal (the direction in which repulsive force will act) coincides with the direction of least inertia,
 * since that is the gradient that maximizes the drop of elastic deformation energy and will reach minimum fastest.
 *
 * Equivalent cross section of the penetrating volume (as if it were a cuboid with the same inertia) and equivalent penetration depth are calculated;
 * Equivalent solid size in the dimension of normal serves as reference for strain calculation and is different for solids A and B.
 *
 * Strain will be then approximated by equivalentPenetrationDepth/.5*(maxPenetrationDepthA+maxPenetrationDepthB) (the average of A and B)
 *
 * All the relevant results are fed into TTetraGeom which is passed to TetraVolumetricLaw later that makes actual use of all this.
 *
 * @todo thoroughly test this for numerical correctness.
 *
 */
00055 bool Ig2_Tetra_Tetra_TTetraGeom::go(const shared_ptr<Shape>& cm1,const shared_ptr<Shape>& cm2,const State& state1,const State& state2, const Vector3r& shift2, const bool& force, const shared_ptr<Interaction>& interaction){
      const Se3r& se31=state1.se3; const Se3r& se32=state2.se3;
      Tetra* A = static_cast<Tetra*>(cm1.get());
      Tetra* B = static_cast<Tetra*>(cm2.get());
      //return false;
      
      shared_ptr<TTetraGeom> bang;
      // depending whether it's a new interaction: create new one, or use the existing one.
      if (!interaction->geom) bang=shared_ptr<TTetraGeom>(new TTetraGeom());
      else bang=YADE_PTR_CAST<TTetraGeom>(interaction->geom);     
      interaction->geom=bang;
      
      // use wildmagick's intersection routine?
      #if 0
            // transform to global coordinates, build Tetrahedron3r objects to make wm3 happy
            Tetrahedron3r tA(se31.orientation*A->v[0]+se31.position,se31.orientation*A->v[1]+se31.position,se31.orientation*A->v[2]+se31.position,se31.orientation*A->v[3]+se31.position);
            Tetrahedron3r tB(se32.orientation*B->v[0]+se32.position,se32.orientation*B->v[1]+se32.position,se32.orientation*B->v[2]+se32.position,se32.orientation*B->v[3]+se32.position);

            IntrTetrahedron3Tetrahedron3r iAB(tA,tB);
            bool found=iAB.Find();  //calculates the intersection volume as a composition of 0 or more tetrahedra

            if(!found) return false; // no intersecting volume

            Real V(0); // volume of intersection (cummulative)
            Vector3r Sg(0,0,0); // static moment of intersection
            vector<vector<Vector3r> > tAB;

            Wm3::TArray<Wm3::Tetrahedron3d> iABinfo(iAB.GetIntersection()); // retrieve the array of 4hedra
            for(int i=0; i<iABinfo.GetQuantity(); i++){
                  iABinfo[i];  // has i-th tehtrahedron as Tetrahedron3r&
                  #define v0 iABinfo[i].V[0]
                  #define v1 iABinfo[i].V[1]
                  #define v2 iABinfo[i].V[2]
                  #define v3 iABinfo[i].V[3]
                  Real dV=fabs(Vector3r(v1-v0).Dot((v2-v0).Cross(v3-v0)))/6.;
                  V+=dV;
                  Sg+=dV*(v0+v1+v2+v3)*.25;
                  vector<Vector3r> t; t.push_back(v0); t.push_back(v1); t.push_back(v2); t.push_back(v3);
                  tAB.push_back(t);
                  #undef v0
                  #undef v1
                  #undef v2
                  #undef v3
            }
      #endif

      // transform to global coordinates, build Tetra objects
      Tetra tA(se31.orientation*A->v[0]+se31.position,se31.orientation*A->v[1]+se31.position,se31.orientation*A->v[2]+se31.position,se31.orientation*A->v[3]+se31.position);
      Tetra tB(se32.orientation*B->v[0]+se32.position+shift2,se32.orientation*B->v[1]+se32.position+shift2,se32.orientation*B->v[2]+se32.position+shift2,se32.orientation*B->v[3]+se32.position+shift2);
      // calculate intersection
      #if 0
            tB=Tetra(Vector3r(0,0,0),Vector3r(1.5,1,1),Vector3r(0.5,1,1),Vector3r(1,1,.5));
            tA=Tetra(Vector3r(0,0,0),Vector3r(1,0,0),Vector3r(0,1,0),Vector3r(0,0,1));
      #endif
      list<Tetra> tAB=Tetra2TetraIntersection(tA,tB);
      if(tAB.size()==0) { /* LOG_DEBUG("No intersection."); */ return false;} //no intersecting volume

      Real V(0); // volume of intersection (cummulative)
      Vector3r Sg(0,0,0); // static moment of intersection

      Vector3r tt[4]; for(int i=0; i<4; i++) tt[i]=tA.v[i];
      //DEBUG TRWM3VEC(tt[0]); TRWM3VEC(tt[1]); TRWM3VEC(tt[2]); TRWM3VEC(tt[3]); TRVAR1(TetrahedronVolume(tA.v)); TRVAR1(TetrahedronVolume(tt)); TRWM3MAT(TetrahedronInertiaTensor(tA.v));

      for(list<Tetra>::iterator II=tAB.begin(); II!=tAB.end(); II++){
            Real dV=TetrahedronVolume(II->v);
            V+=dV;
            //DEBUG TRVAR1(dV); TRWM3VEC(II->v[0]); TRWM3VEC(II->v[1]); TRWM3VEC(II->v[2]); TRWM3VEC(II->v[3]); LOG_TRACE("====")
            Sg+=dV*(II->v[0]+II->v[1]+II->v[2]+II->v[3])*.25;
      }
      Vector3r centroid=Sg/V;
      Matrix3r I(Matrix3r::Zero()); // inertia tensor for the composition; zero matrix initially
            // I is purely geometrical (as if with unit density)
      
      // get total 
      Vector3r dist;    for(list<Tetra>::iterator II=tAB.begin(); II!=tAB.end(); II++){
            II->v[0]-=centroid; II->v[1]-=centroid; II->v[2]-=centroid; II->v[3]-=centroid;
            dist=(II->v[0]+II->v[1]+II->v[2]+II->v[3])*.25-centroid;
            /* use parallel axis theorem */ 
            Matrix3r distSq(Matrix3r::Zero()); distSq(0,0)=dist[0]*dist[0]; distSq(1,1)=dist[1]*dist[1]; distSq(2,2)=dist[2]*dist[2]; // could be done more intelligently with eigen
            I+=TetrahedronInertiaTensor(II->v)+TetrahedronVolume(II->v)*distSq;
      }
      
      /* Now, we have the collision volumetrically described by intersection volume (V), its inertia tensor (I) and centroid (centroid; contact point).
       * The inertia tensor is in global coordinates; by eigendecomposition, we find principal axes, which will give us
       *  1. normal, the direction of the lest inertia; this is the gradient of penetration energy
       *    it may have either direction mathematically, but since 4hedra are convex, 
       *    normal will be always the direction pointing more towards the centroid of the other 4hedron
       *  2. tangent?! hopefully not needed at all. */

      Matrix3r Ip, R; // principal moments of inertia, rotation matrix
      /* should check convergence*/ matrixEigenDecomposition(I,R,Ip);
      // according to the documentation in Wm3 header, diagonal entries are in ascending order: d0<=d1<=d2;
      // but keep it algorithmic for now and just assert that.
      int ix=(Ip(0,0)<Ip(1,1) && Ip(0,0)<Ip(2,2))?0:( (Ip(1,1)<Ip(0,0) && Ip(1,1)<Ip(2,2))?1:2); // index of the minimum moment of inertia
      // the other two indices, modulated by 3, since they are ∈ {0,1,2}
      int ixx=(ix+1)%3, ixxx=(ix+2)%3;
      // assert what the documentation says (d0<=d1<=d2)
      assert(ix==0);
      Vector3r minAxis(0,0,0); minAxis[ix]=1; // the axis of minimum inertia
      Vector3r normal=R*minAxis; normal.normalize(); // normal is minAxis in global coordinates (normalization shouldn't be needed since R is rotation matrix, but to make sure...)

      // centroid of B
      Vector3r Bcent=se31.orientation*((B->v[0]+B->v[1]+B->v[2]+B->v[3])*.25)+se31.position;
      // reverse direction if projection of the (contact_point-centroid_of_B) vector onto the normal is negative (i.e. the normal points more towards A)
      if((Bcent-centroid).dot(normal)<0) normal*=-1;

      /* now estimate the area of the solid that is perpendicular to the normal. This will be needed to estimate elastic force based on Young's modulus.
       * Suppose we have cuboid, with edges of lengths x,y,z in the direction of respective axes.
       * It's inertia are Ix=(V/12)*(y^2+z^2), Iy=(V/12)*(x^2+z^2), Iz=(V/12)*(x^2+y^2) and suppose Iz is maximal; Ix, Iy and Iz are known (from decomposition above).
       * Then the area perpendicular to z (normal direction) is given by x*y=V/z, where V is known.
       * Ix+Iy-Iz=(V/12)*(y^2+z^2+x^2+z^2-x^2-y^2)=(V*z^2)/6, z=√(6*(Ix+Iy-Iz)/V)
       * Az=V/z=√(V^3/(6*(Ix+Iy-Iz))).
       *
       * In our case, the greatest inertia is along ixxx, the other coordinates are ixx and ix. equivalentPenetrationDepth means what was z.
       */
      //DEBUG
      TRWM3MAT(Ip); TRWM3MAT(I);
      Real equivalentPenetrationDepth=sqrt(6.*(Ip(ix,ix)+Ip(ixx,ixx)-Ip(ixxx,ixxx))/V);
      Real equivalentCrossSection=V/equivalentPenetrationDepth;
      TRVAR3(V,equivalentPenetrationDepth,equivalentCrossSection);

      /* Now rotate the whole inertia tensors of A and B and estimate maxPenetrationDepth -- the length of the body in the direction of the contact normal.
       * This will be used to calculate relative deformation, which is needed for elastic response. */
      const State* physA=Body::byId(interaction->getId1())->state.get(); const State* physB=Body::byId(interaction->getId2())->state.get();
      // WARNING: Matrix3r(Vector3r(...)) is compiled, but gives zero matrix??!! Use explicitly constructor from diagonal entries
      //Matrix3r IA(physA->inertia[0],physA->inertia[1],physA->inertia[2]); Matrix3r IB(physB->inertia[0],physB->inertia[1],physB->inertia[2]);
      Matrix3r IA=Matrix3r::Zero(), IB=Matrix3r::Zero(); for(int i=0; i<3; i++){ IA(i,i)=physA->inertia[i]; IB(i,i)=physB->inertia[i]; }
      // see Clump::inertiaTensorRotate for references
      IA=R.transpose()*IA*R; IB=R.transpose()*IB*R;

      Real maxPenetrationDepthA=sqrt(6*(IA(ix,ix)+IA(ixx,ixx)-IA(ixxx,ixxx))/V);
      Real maxPenetrationDepthB=sqrt(6*(IB(ix,ix)+IB(ixx,ixx)-IB(ixxx,ixxx))/V);
      TRVAR2(maxPenetrationDepthA,maxPenetrationDepthB);

      /* store calculated stuff in bang; some is redundant */
      bang->normal=normal;
      bang->equivalentCrossSection=equivalentCrossSection;
      bang->contactPoint=centroid;
      bang->penetrationVolume=V;

      bang->equivalentPenetrationDepth=equivalentPenetrationDepth;
      bang->maxPenetrationDepthA=maxPenetrationDepthA;
      bang->maxPenetrationDepthB=maxPenetrationDepthB;

      return true;
}

/*! Calculate intersection o Tetrahedron A and B as union of set (std::list) of 4hedra.
 *
 * intersecting tetrahedra A and B
 * S=intersection set (4hedra)
 * S={A}
 * for face in B_faces:
 *          for t in S:  [ S is mutable, but if list, iterators remain valid? ]
 *          tmp = clip t by face // may return multiple 4hedra or none
 *          replace t by tmp (possibly none) in S
 * return S
 *
 */
00214 list<Tetra> Ig2_Tetra_Tetra_TTetraGeom::Tetra2TetraIntersection(const Tetra& A, const Tetra& B){
      // list of 4hedra to split; initially A
      list<Tetra> ret; ret.push_back(A);
      /* I is vertex index at B;
       * clipping face is [i i1 i2], normal points away from i3 */
      int i,i1,i2,i3;
      Vector3r normal;
      /* LOG_TRACE("===========================================================================================")
      LOG_TRACE("===========================================================================================")
      LOG_TRACE(ret.size());
      LOG_TRACE("DUMP A and B:"); A.dump(); B.dump(); */
      for(i=0; i<4; i++){
            i1=(i+1)%4; i2=(i+2)%4; i3=(i+3)%4;
            const Vector3r& P(B.v[i]); // reference point on the plane
            normal=(B.v[i1]-P).cross(B.v[i2]-P); normal.normalize(); // normal
            if((B.v[i3]-P).dot(normal)>0) normal*=-1; // outer normal
            /* TRWM3VEC(P); TRWM3VEC(normal); LOG_TRACE("DUMP initial tetrahedron list:"); for(list<Tetra>::iterator I=ret.begin(); I!=ret.end(); I++) (*I).dump(); */
            for(list<Tetra>::iterator I=ret.begin(); I!=ret.end(); /* I++ */ ){
                  list<Tetra> splitDecomposition=TetraClipByPlane(*I,P,normal);
                  // replace current list element by the result of decomposition;
                  // I points after the erased one, so decomposed 4hedra will not be touched in this iteration, just as we want.
                  // Since it will be incremented by I++ at the end of the cycle, compensate for that by I--;
                  I=ret.erase(I); ret.insert(I,splitDecomposition.begin(),splitDecomposition.end()); /* I--; */
                  /* LOG_TRACE("DUMP current tetrahedron list:"); for(list<Tetra>::iterator I=ret.begin(); I!=ret.end(); I++) (*I).dump();*/ 
            }
      }
      //exit(0);
      return ret;
}

/*! Clip Tetra T by plane give by point P and outer normal n.
 *
 * Algorithm: 
 *
 * clip t by face
 *    sort points of t into positive, negative, zero (face normal n points outside)
 *          -: inside; +: outside; 0: on face
 *          homogeneous cases (no split):
 *                ++++, +++0, ++00, +000 :
 *                      0Δ full clip (everything outside), nothing left; return ∅
 *                ----, ---0, --00, -000 :
 *                      1Δ all inside, return identity
 *                split (at least one - and one +)
 *                      -+++
 *                      1Δ [A AB AC AD]
 *                      -++0
 *                      1Δ [A AB AC D]
 *                      -+00:
 *                      1Δ [A AB C D]
 *                --++:
 *                      3Δ [A AC AD B BC BD] ⇒ (e.g.) [A AC AD B] [B BC BD AD] [B AD AC BC]
 *                --+0:
 *                      2Δ [A B AC BC D] ⇒ (e.g.) [A AC BC D] [B BC A D] 
 *                ---+:
 *                      3Δ tetrahedrize [A B C AD BD CD]
 *
 * http://members.tripod.com/~Paul_Kirby/vector/Vplanelineint.html
 */
00272 list<Tetra> Ig2_Tetra_Tetra_TTetraGeom::TetraClipByPlane(const Tetra& T, const Vector3r& P, const Vector3r& normal){
      
      list<Tetra> ret;
      // scaling factor for Mathr::EPSILON: average edge length
      Real scaledEPSILON=Mathr::EPSILON*(1/6.)*((T.v[1]-T.v[0])+(T.v[2]-T.v[0])+(T.v[3]-T.v[0])+(T.v[2]-T.v[1])+(T.v[3]-T.v[1])+(T.v[3]-T.v[2])).norm();

      /* TRWM3VEC(P); TRWM3VEC(normal); T.dump(); */

      vector<size_t> pos, neg, zer; Real dist[4];
      for(size_t i=0; i<4; i++){
            dist[i]=(T.v[i]-P).dot(normal);
            if(dist[i]>scaledEPSILON) pos.push_back(i);
            else if(dist[i]<-scaledEPSILON) neg.push_back(i);
            else zer.push_back(i);
      }
      /* LOG_TRACE("dist[i]=["<<dist[0]<<","<<dist[1]<<","<<dist[2]<<","<<dist[3]<<"]"); */
      #define NEG neg.size()
      #define POS pos.size()
      #define ZER zer.size()
      #define PTPT(i,j) PtPtPlaneIntr(v[i],v[j],P,normal)
      assert(NEG+POS+ZER==4);

      // HOMOGENEOUS CASES
            // ++++, +++0, ++00, +000, 0000 (degenerate (planar) tetrahedron)
            if(POS==4 || (POS==3 && ZER==1) || (POS==2 && ZER==2) || (POS==1 && ZER==3) || ZER==4) return ret; // ∅
            // ----, ---0, --00, -000 :
            if(NEG==4 || (NEG==3 && ZER==1) || (NEG==2 && ZER==2) || (NEG==1 && ZER==3)) {ret.push_back(T); return ret;}
      // HETEROGENEOUS CASES
            // points are ordered -+0
            Vector3r v[4];
            for(size_t i=0; i<NEG; i++) v[i+  0+  0]=T.v[neg[i]];
            for(size_t i=0; i<POS; i++) v[i+  0+NEG]=T.v[pos[i]];
            for(size_t i=0; i<ZER; i++) v[i+POS+NEG]=T.v[zer[i]];
            /* LOG_TRACE("NEG(in)="<<NEG<<", POS(out)="<<POS<<", ZER(boundary)="<<ZER); TRWM3VEC(v[0]); TRWM3VEC(v[1]); TRWM3VEC(v[2]); TRWM3VEC(v[3]); */
            #define _A v[0]
            #define _B v[1]
            #define _C v[2]
            #define _D v[3]
            #define _AB PTPT(0,1)
            #define _AC PTPT(0,2)
            #define _AD PTPT(0,3)
            #define _BC PTPT(1,2)
            #define _BD PTPT(1,3)
            #define _CD PTPT(2,3)
            // -+++ → 1Δ [A AB AC AD]
            if(NEG==1 && POS==3){ret.push_back(Tetra(_A,_AB,_AC,_AD)); return ret;}
            // -++0 → 1Δ [A AB AC D]
            if(NEG==1 && POS==2 && ZER==1){ret.push_back(Tetra(_A,_AB,_AC,_D)); return ret;}
            //    -+00 → 1Δ [A AB C D]
            if(NEG==1 && POS==1 && ZER==2){ret.push_back(Tetra(_A,_AB,_C,_D)); return ret;}
            // --++ → 3Δ [A AC AD B BC BD] ⇒ (e.g.) [A AC AD B] [B BC BD AD] [B AD AC BC]
            if(NEG==2 && POS ==2){
                  // [A AC AD B]
                  ret.push_back(Tetra(_A,_AC,_AD,_B));
                  // [B BC BD AD]
                  ret.push_back(Tetra(_B,_BC,_BD,_AD));
                  // [B AD AC BC]
                  ret.push_back(Tetra(_B,_AD,_AC,_BC));
                  return ret;
            }
            // --+0 → 2Δ [A B AC BC D] ⇒ (e.g.) [A AC BC D] [B BC A D] 
            if(NEG==2 && POS==1 && ZER==1){
                  // [A AC BC D]
                  ret.push_back(Tetra(_A,_AC,_BC,_D));
                  // [B BC A D]
                  ret.push_back(Tetra(_B,_BC,_A,_D));
                  return ret;
            }
            // ---+ → 3Δ [A B C AD BD CD] ⇒ (e.g.) [A B C AD] [AD BD CD B] [AD C B BD]
            if(NEG==3 && POS==1){
                  //[A B C AD]
                  ret.push_back(Tetra(_A,_B,_C,_AD));
                  //[AD BD CD B]
                  ret.push_back(Tetra(_AD,_BD,_CD,_B));
                  //[AD C B BD]
                  ret.push_back(Tetra(_AD,_C,_B,_BD));
                  return ret;
            }
            #undef _A
            #undef _B
            #undef _C
            #undef _D
            #undef _AB
            #undef _AC
            #undef _AD
            #undef _BC
            #undef _BD
            #undef _CD

      #undef PTPT
      #undef NEG
      #undef POS
      #undef ZER
      // unreachable
      assert(false);
      return(ret); // prevent warning
}

CREATE_LOGGER(TetraVolumetricLaw);

/*! Apply forces on tetrahedra in collision based on geometric configuration provided by Ig2_Tetra_Tetra_TTetraGeom.
 *
 * DO NOT USE, probably doesn't work.
 * Comments on functionality limitations are in the code. It has not been tested at all!!! */
00376 void TetraVolumetricLaw::action()
{
      FOREACH(const shared_ptr<Interaction>& I, *scene->interactions){
            // normally, we would test isReal(), but TetraVolumetricLaw doesn't use phys at all
            if (!I->geom) continue; // Ig2_Tetra_Tetra_TTetraGeom::go returned false for this interaction, skip it
            const shared_ptr<TTetraGeom>& contactGeom(dynamic_pointer_cast<TTetraGeom>(I->geom));
            if(!contactGeom) continue;

            const Body::id_t idA=I->getId1(), idB=I->getId2();
            const shared_ptr<Body>& A=Body::byId(idA), B=Body::byId(idB);
                  
            const shared_ptr<ElastMat>& physA(dynamic_pointer_cast<ElastMat>(A->material));
            const shared_ptr<ElastMat>& physB(dynamic_pointer_cast<ElastMat>(B->material));
            

            /* Cross-section is volumetrically equivalent to the penetration configuration */
            Real averageStrain=contactGeom->equivalentPenetrationDepth/(.5*(contactGeom->maxPenetrationDepthA+contactGeom->maxPenetrationDepthB));

            /* Do not use NormPhys::kn (as calculated by ElasticBodySimpleRelationship).
             * NormPhys::kn is not Young's modulus, it is calculated by MacroMicroElasticRelationships. So perhaps
             * a new IPhysFunctor will be needed that will just pass the average Young's modulus here?
             * For now, just go back to Young's moduli directly here. */
            Real young=.5*(physA->young+physB->young);
            TRVAR3(young,averageStrain,contactGeom->equivalentCrossSection);
            // F=σA=εEA
            // this is unused; should it?: contactPhys->kn
            Vector3r F=contactGeom->normal*averageStrain*young*contactGeom->equivalentCrossSection;
            TRWM3VEC(contactGeom->normal);
            TRWM3VEC(F);
            TRWM3VEC((A->state->pos-contactGeom->contactPoint).cross(F));

            scene->forces.addForce (idA,-F);
            scene->forces.addForce (idB, F);
            scene->forces.addTorque(idA,-(A->state->pos-contactGeom->contactPoint).cross(F));
            scene->forces.addTorque(idB, (B->state->pos-contactGeom->contactPoint).cross(F));
      }
}

#ifdef YADE_OPENGL
      #include<yade/lib-opengl/OpenGLWrapper.hpp>
      void Gl1_Tetra::go(const shared_ptr<Shape>& cm, const shared_ptr<State>&,bool,const GLViewInfo&)
      {
            glMaterialv(GL_FRONT,GL_AMBIENT_AND_DIFFUSE,Vector3r(cm->color[0],cm->color[1],cm->color[2]));
            glColor3v(cm->color);
            Tetra* t=static_cast<Tetra*>(cm.get());
            if (0) { // wireframe, as for Tetrahedron
                  glDisable(GL_LIGHTING);
                  glBegin(GL_LINES);
                        #define __ONEWIRE(a,b) glVertex3v(t->v[a]);glVertex3v(t->v[b])
                              __ONEWIRE(0,1);__ONEWIRE(0,2);__ONEWIRE(0,3);__ONEWIRE(1,2);__ONEWIRE(1,3);__ONEWIRE(2,3);
                        #undef __ONEWIRE
                  glEnd();
            }
            else
            {
                  Vector3r center = (t->v[0]+t->v[1]+t->v[2]+t->v[3])*.25, faceCenter, n;
                  glDisable(GL_CULL_FACE); glEnable(GL_LIGHTING);
                  glBegin(GL_TRIANGLES);
                        #define __ONEFACE(a,b,c) n=(t->v[b]-t->v[a]).cross(t->v[c]-t->v[a]); n.normalize(); faceCenter=(t->v[a]+t->v[b]+t->v[c])/3.; if((faceCenter-center).dot(n)<0)n=-n; glNormal3v(n); glVertex3v(t->v[a]); glVertex3v(t->v[b]); glVertex3v(t->v[c]);
                              __ONEFACE(3,0,1);
                              __ONEFACE(0,1,2);
                              __ONEFACE(1,2,3);
                              __ONEFACE(2,3,0);
                        #undef __ONEFACE
                  glEnd();
            }
      }
#endif

/*! Calculates tetrahedron inertia relative to the origin (0,0,0), with unit density (scales linearly).

See article F. Tonon, "Explicit Exact Formulas for the 3-D Tetrahedron Inertia Tensor in Terms of its Vertex Coordinates", http://www.scipub.org/fulltext/jms2/jms2118-11.pdf

Numerical example to check:

vertices:
      (8.33220, 11.86875, 0.93355)
      (0.75523 ,5.00000, 16.37072)
      (52.61236, 5.00000, 5.38580)
      (2.00000, 5.00000, 3.00000)
centroid:
      (15.92492, 0.78281, 3.72962)
intertia/density WRT centroid:
      a/μ = 43520.33257 m⁵
      b/μ = 194711.28938 m⁵
      c/μ = 191168.76173 m⁵
      a’/μ= 4417.66150 m⁵
      b’/μ=-46343.16662 m⁵
      c’/μ= 11996.20119 m⁵

The numerical testcase (in TetraTestGen::generate) is exact as in the article for inertia (as well as centroid):

43520.3
194711
191169
4417.66
-46343.2
11996.2

*/
//Matrix3r TetrahedronInertiaTensor(const Vector3r v[4]){
Matrix3r TetrahedronInertiaTensor(const vector<Vector3r>& v){
      #define x1 v[0][0]
      #define y1 v[0][1]
      #define z1 v[0][2]
      #define x2 v[1][0]
      #define y2 v[1][1]
      #define z2 v[1][2]
      #define x3 v[2][0]
      #define y3 v[2][1]
      #define z3 v[2][2]
      #define x4 v[3][0]
      #define y4 v[3][1]
      #define z4 v[3][2]

// FIXME - C array
      assert(v.size()==4);

      // Jacobian of transformation to the reference 4hedron
      double detJ=(x2-x1)*(y3-y1)*(z4-z1)+(x3-x1)*(y4-y1)*(z2-z1)+(x4-x1)*(y2-y1)*(z3-z1)
            -(x2-x1)*(y4-y1)*(z3-z1)-(x3-x1)*(y2-y1)*(z4-z1)-(x4-x1)*(y3-y1)*(z2-z1);
      detJ=fabs(detJ);
      double a=detJ*(y1*y1+y1*y2+y2*y2+y1*y3+y2*y3+
            y3*y3+y1*y4+y2*y4+y3*y4+y4*y4+z1*z1+z1*z2+
            z2*z2+z1*z3+z2*z3+z3*z3+z1*z4+z2*z4+z3*z4+z4*z4)/60.;
      double b=detJ*(x1*x1+x1*x2+x2*x2+x1*x3+x2*x3+x3*x3+
            x1*x4+x2*x4+x3*x4+x4*x4+z1*z1+z1*z2+z2*z2+z1*z3+
            z2*z3+z3*z3+z1*z4+z2*z4+z3*z4+z4*z4)/60.;
      double c=detJ*(x1*x1+x1*x2+x2*x2+x1*x3+x2*x3+x3*x3+x1*x4+
            x2*x4+x3*x4+x4*x4+y1*y1+y1*y2+y2*y2+y1*y3+
            y2*y3+y3*y3+y1*y4+y2*y4+y3*y4+y4*y4)/60.;
      // a' in the article etc.
      double a__=detJ*(2*y1*z1+y2*z1+y3*z1+y4*z1+y1*z2+
            2*y2*z2+y3*z2+y4*z2+y1*z3+y2*z3+2*y3*z3+
            y4*z3+y1*z4+y2*z4+y3*z4+2*y4*z4)/120.;
      double b__=detJ*(2*x1*z1+x2*z1+x3*z1+x4*z1+x1*z2+
            2*x2*z2+x3*z2+x4*z2+x1*z3+x2*z3+2*x3*z3+
            x4*z3+x1*z4+x2*z4+x3*z4+2*x4*z4)/120.;
      double c__=detJ*(2*x1*y1+x2*y1+x3*y1+x4*y1+x1*y2+
            2*x2*y2+x3*y2+x4*y2+x1*y3+x2*y3+2*x3*y3+
            x4*y3+x1*y4+x2*y4+x3*y4+2*x4*y4)/120.;

      Matrix3r ret; ret<<
            a   , -b__, -c__,
            -b__, b   , -a__,
            -c__, -a__, c    ;
      return ret;

      #undef x1
      #undef y1
      #undef z1
      #undef x2
      #undef y2
      #undef z2
      #undef x3
      #undef y3
      #undef z3
      #undef x4
      #undef y4
      #undef z4
}

/*! Caluclate tetrahedron's central inertia tensor */
//Matrix3r TetrahedronCentralInertiaTensor(const Vector3r v[4]){
Matrix3r TetrahedronCentralInertiaTensor(const vector<Vector3r>& v){
      assert(v.size()==4);
      vector<Vector3r> vv;

//    Vector3r vv[4];
      Vector3r cg=(v[0]+v[1]+v[2]+v[3])*.25;
//    vv[0]=v[0]-cg;
//    vv[1]=v[1]-cg;
//    vv[2]=v[2]-cg;
//    vv[3]=v[3]-cg;
      vv.push_back(v[0]-cg);
      vv.push_back(v[1]-cg);
      vv.push_back(v[2]-cg);
      vv.push_back(v[3]-cg);

      return TetrahedronInertiaTensor(vv);
}

/*! Rotate and translate terahedron body so that its local axes are principal, keeping global position by updating vertex positions as well.
 * Updates all body parameters as need.
 *
 * @returns rotation that was done as Wm3::Quaternionr.
 * @todo check for geometrical correctness...
 * */
Quaternionr TetrahedronWithLocalAxesPrincipal(shared_ptr<Body>& tetraBody){
      //const shared_ptr<RigidBodyParameters>& rbp(YADE_PTR_CAST<RigidBodyParameters>(tetraBody->physicalParameters));
      State* rbp=tetraBody->state.get();
      const shared_ptr<Tetra>& tMold(dynamic_pointer_cast<Tetra>(tetraBody->shape));

      #define v0 tMold->v[0]
      #define v1 tMold->v[1]
      #define v2 tMold->v[2]
      #define v3 tMold->v[3]

      // adjust position (origin to centroid)
      Vector3r cg=(v0+v1+v2+v3)*.25;
      v0-=cg; v1-=cg; v2-=cg; v3-=cg;
      //tMold->v[0]=v0; tMold->v[1]=v1; tMold->v[2]=v2; tMold->v[3]=v3;
      rbp->se3.position+=cg;

      // adjust orientation (local axes to principal axes)
      Matrix3r I_old=TetrahedronInertiaTensor(tMold->v); //≡TetrahedronCentralInertiaTensor
      Matrix3r I_rot(Matrix3r::Zero()), I_new(Matrix3r::Zero()); 
      matrixEigenDecomposition(I_old,I_rot,I_new);
      Quaternionr I_Qrot(I_rot);
      //! @fixme from right to left: rotate by I_rot, then add original rotation (?!!)
      rbp->se3.orientation=rbp->se3.orientation*I_Qrot;
      for(size_t i=0; i<4; i++){
            tMold->v[i]=I_Qrot.conjugate()*tMold->v[i];
      }

      // set inertia
      rbp->inertia=Vector3r(I_new(0,0),I_new(1,1),I_new(2,2));

      return I_Qrot;
      #undef v0
      #undef v1
      #undef v2
      #undef v3
}


Real TetrahedronVolume(const Vector3r v[4]) { return fabs((Vector3r(v[3])-Vector3r(v[0])).dot((Vector3r(v[3])-Vector3r(v[1])).cross(Vector3r(v[3])-Vector3r(v[2]))))/6.; }
Real TetrahedronVolume(const vector<Vector3r>& v) { return fabs(Vector3r(v[1]-v[0]).dot(Vector3r(v[2]-v[0]).cross(v[3]-v[0])))/6.; }


Generated by  Doxygen 1.6.0   Back to index