Main Page   Namespace List   Class Hierarchy   Compound List   File List   Namespace Members   Compound Members  

SIG_Mirtich.cpp

00001 // may be ignored so far
00002 #include "SIGEL_Robot/SIG_Mirtich.h"
00003 #include <newmatap.h>
00004 
00005 #define X 0
00006 #define Y 1
00007 #define Z 2
00008 
00009 #define SQR(x) ((x)*(x))
00010 #define CUBE(x) ((x)*(x)*(x))
00011 
00012 namespace SIGEL_Robot {
00013 
00014         void SIG_Mirtich::compProjectionIntegrals(FACE *f)
00015         {
00016                 double a0, a1, da;
00017                 double b0, b1, db;
00018                 double a0_2, a0_3, a0_4, b0_2, b0_3, b0_4;
00019                 double a1_2, a1_3, b1_2, b1_3;
00020                 double C1, Ca, Caa, Caaa, Cb, Cbb, Cbbb;
00021                 double Cab, Kab, Caab, Kaab, Cabb, Kabb;
00022                 int i;
00023                 
00024                 P1 = Pa = Pb = Paa = Pab = Pbb = Paaa = Paab = Pabb = Pbbb = 0.0;
00025 
00026                 for (i = 0; i < f->numVerts; i++) {
00027                         a0 = f->poly->verts[f->verts[i]][A];
00028                         b0 = f->poly->verts[f->verts[i]][B];
00029                         a1 = f->poly->verts[f->verts[(i+1) % f->numVerts]][A];
00030                         b1 = f->poly->verts[f->verts[(i+1) % f->numVerts]][B];
00031                         da = a1 - a0;
00032                         db = b1 - b0;
00033                         a0_2 = a0 * a0; a0_3 = a0_2 * a0; a0_4 = a0_3 * a0;
00034                         b0_2 = b0 * b0; b0_3 = b0_2 * b0; b0_4 = b0_3 * b0;
00035                         a1_2 = a1 * a1; a1_3 = a1_2 * a1; 
00036                         b1_2 = b1 * b1; b1_3 = b1_2 * b1;
00037                         
00038                         C1 = a1 + a0;
00039                         Ca = a1*C1 + a0_2; Caa = a1*Ca + a0_3; Caaa = a1*Caa + a0_4;
00040                         Cb = b1*(b1 + b0) + b0_2; Cbb = b1*Cb + b0_3; Cbbb = b1*Cbb + b0_4;
00041                         Cab = 3*a1_2 + 2*a1*a0 + a0_2; Kab = a1_2 + 2*a1*a0 + 3*a0_2;
00042                         Caab = a0*Cab + 4*a1_3; Kaab = a1*Kab + 4*a0_3;
00043                         Cabb = 4*b1_3 + 3*b1_2*b0 + 2*b1*b0_2 + b0_3;
00044                         Kabb = b1_3 + 2*b1_2*b0 + 3*b1*b0_2 + 4*b0_3;
00045                         
00046                         P1 += db*C1;
00047                         Pa += db*Ca;
00048                         Paa += db*Caa;
00049                         Paaa += db*Caaa;
00050                         Pb += da*Cb;
00051                         Pbb += da*Cbb;
00052                         Pbbb += da*Cbbb;
00053                         Pab += db*(b1*Cab + b0*Kab);
00054                         Paab += db*(b1*Caab + b0*Kaab);
00055                         Pabb += da*(a1*Cabb + a0*Kabb);
00056                 }
00057                 
00058                 P1 /= 2.0;
00059                 Pa /= 6.0;
00060                 Paa /= 12.0;
00061                 Paaa /= 20.0;
00062                 Pb /= -6.0;
00063                 Pbb /= -12.0;
00064                 Pbbb /= -20.0;
00065                 Pab /= 24.0;
00066                 Paab /= 60.0;
00067                 Pabb /= -60.0;
00068         }
00069 
00070         void SIG_Mirtich::compFaceIntegrals(FACE *f)
00071         { 
00072                 double *n, w;
00073                 double k1, k2, k3, k4;
00074                 
00075                 compProjectionIntegrals(f);
00076                 
00077                 w = f->w;
00078                 n = f->norm;
00079                 k1 = 1 / n[C]; k2 = k1 * k1; k3 = k2 * k1; k4 = k3 * k1;
00080                 
00081                 Fa = k1 * Pa;
00082                 Fb = k1 * Pb;
00083                 Fc = -k2 * (n[A]*Pa + n[B]*Pb + w*P1);
00084                 
00085                 Faa = k1 * Paa;
00086                 Fbb = k1 * Pbb;
00087                 Fcc = k3 * (SQR(n[A])*Paa + 2*n[A]*n[B]*Pab + SQR(n[B])*Pbb
00088                             + w*(2*(n[A]*Pa + n[B]*Pb) + w*P1));
00089                 
00090                 Faaa = k1 * Paaa;
00091                 Fbbb = k1 * Pbbb;
00092                 Fccc = -k4 * (CUBE(n[A])*Paaa + 3*SQR(n[A])*n[B]*Paab 
00093                               + 3*n[A]*SQR(n[B])*Pabb + CUBE(n[B])*Pbbb
00094                               + 3*w*(SQR(n[A])*Paa + 2*n[A]*n[B]*Pab + SQR(n[B])*Pbb)
00095                               + w*w*(3*(n[A]*Pa + n[B]*Pb) + w*P1));
00096                 
00097                 Faab = k1 * Paab;
00098                 Fbbc = -k2 * (n[A]*Pabb + n[B]*Pbbb + w*Pbb);
00099                 Fcca = k3 * (SQR(n[A])*Paaa + 2*n[A]*n[B]*Paab + SQR(n[B])*Pabb
00100                              + w*(2*(n[A]*Paa + n[B]*Pab) + w*Pa));
00101         }
00102 
00103         void SIG_Mirtich::compVolumeIntegrals(POLYHEDRON *p)
00104         {
00105                 FACE *f;
00106                 double nx, ny, nz;
00107                 int i;
00108 
00109                 T0 = T1[X] = T1[Y] = T1[Z] 
00110                         = T2[X] = T2[Y] = T2[Z] 
00111                         = TP[X] = TP[Y] = TP[Z] = 0;
00112                 
00113                 for (i = 0; i < p->numFaces; i++) {
00114 
00115                         f = &p->faces[i];
00116 
00117                         nx = fabs(f->norm[X]);
00118                         ny = fabs(f->norm[Y]);
00119                         nz = fabs(f->norm[Z]);
00120                         if (nx > ny && nx > nz) C = X;
00121                         else C = (ny > nz) ? Y : Z;
00122                         A = (C + 1) % 3;
00123                         B = (A + 1) % 3;
00124 
00125                         compFaceIntegrals(f);
00126 
00127                         T0 += f->norm[X] * ((A == X) ? Fa : ((B == X) ? Fb : Fc));
00128 
00129                         T1[A] += f->norm[A] * Faa;
00130                         T1[B] += f->norm[B] * Fbb;
00131                         T1[C] += f->norm[C] * Fcc;
00132                         T2[A] += f->norm[A] * Faaa;
00133                         T2[B] += f->norm[B] * Fbbb;
00134                         T2[C] += f->norm[C] * Fccc;
00135                         TP[A] += f->norm[A] * Faab;
00136                         TP[B] += f->norm[B] * Fbbc;
00137                         TP[C] += f->norm[C] * Fcca;
00138                 }
00139 
00140                 T1[X] /= 2; T1[Y] /= 2; T1[Z] /= 2;
00141                 T2[X] /= 3; T2[Y] /= 3; T2[Z] /= 3;
00142                 TP[X] /= 2; TP[Y] /= 2; TP[Z] /= 2;
00143         }
00144 
00145         void SIG_Mirtich::compFaceNormal (FACE *f)
00146         {
00147                 POLYHEDRON *p = f->poly;
00148                 double dx1, dy1, dz1, dx2, dy2, dz2, nx, ny, nz, len;
00149 
00150                 dx1 = p->verts[f->verts[1]][X] - p->verts[f->verts[0]][X];
00151                 dy1 = p->verts[f->verts[1]][Y] - p->verts[f->verts[0]][Y];
00152                 dz1 = p->verts[f->verts[1]][Z] - p->verts[f->verts[0]][Z];
00153                 dx2 = p->verts[f->verts[2]][X] - p->verts[f->verts[1]][X];
00154                 dy2 = p->verts[f->verts[2]][Y] - p->verts[f->verts[1]][Y];
00155                 dz2 = p->verts[f->verts[2]][Z] - p->verts[f->verts[1]][Z];
00156                 nx = dy1 * dz2 - dy2 * dz1;
00157                 ny = dz1 * dx2 - dz2 * dx1;
00158                 nz = dx1 * dy2 - dx2 * dy1;
00159                 len = sqrt(nx * nx + ny * ny + nz * nz);
00160                 f->norm[X] = nx / len;
00161                 f->norm[Y] = ny / len;
00162                 f->norm[Z] = nz / len;
00163                 f->w = - f->norm[X] * p->verts[f->verts[0]][X]
00164                         - f->norm[Y] * p->verts[f->verts[0]][Y]
00165                         - f->norm[Z] * p->verts[f->verts[0]][Z];
00166         }
00167 
00168         void SIG_Mirtich::compute (void)
00169         {
00170                 if (computed)
00171                         return;
00172                 computed = true;
00173                 
00174                 POLYHEDRON p;
00175                 p.verts = new tridouble [geom->getNumVertices ()];
00176                 p.faces = new FACE [geom->getNumPolygons ()];
00177 
00178                 p.numVerts = geom->getNumVertices ();
00179                 p.numFaces = geom->getNumPolygons ();
00180 
00181                 for (int i = 0; i < p.numVerts; i++) {
00182                         DL_vector v = geom->getVertex (i);
00183                         p.verts [i][X] = v.get (X);
00184                         p.verts [i][Y] = v.get (Y);
00185                         p.verts [i][Z] = v.get (Z);
00186                 }
00187 
00188                 SIG_GeometryIterator it (geom);
00189                 for (int i = 0; (i < p.numFaces) && it; i++) {
00190                         p.faces [i].poly = &p;
00191                         
00192                         SIG_Polygon const & pl = it.iterate ();
00193                         int numv = pl.getNumVertices ();
00194                         p.faces [i].numVerts = numv;
00195                         p.faces [i].verts = new int [numv];
00196 
00197                         for (int j = 0; j < numv; j++)
00198                                 p.faces [i].verts [j] = pl.getVertexIndex (j);
00199 
00200                         compFaceNormal (&(p.faces [i]));
00201                 }
00202 
00203                 compVolumeIntegrals (&p);
00204 
00205                 // na toll, muehsam aufgebaut, ein Aufruf, und jetzt
00206                 // kann ich den ganzen Scheiss wieder loeschen.
00207 
00208                 for (int i = 0; i < p.numFaces; i++)
00209                         delete [] p.faces [i].verts;
00210                 delete [] p.faces;
00211                 delete [] p.verts;
00212         }
00213 
00214         SIG_Mirtich::SIG_Mirtich (SIG_Geometry *geometr, QString nameOfGeom)
00215                 : computed (false),
00216                   myExcName (nameOfGeom),
00217                   geom (geometr)
00218         { }
00219 
00220         SIG_Mirtich::~SIG_Mirtich ()
00221         { }
00222 
00223         void SIG_Mirtich::computePhysics (double density,
00224                                           DL_Scalar & masse,
00225                                           DL_vector & centreOfMass,
00226                                           DL_matrix & inertiaTensor)
00227         {
00228                 double mass;
00229                 double r[3];            /* center of mass */
00230                 double J[3][3];         /* inertia tensor */
00231 
00232                 compute ();
00233 
00234                 mass = density * T0;
00235 
00236                 r[X] = T1[X] / T0;
00237                 r[Y] = T1[Y] / T0;
00238                 r[Z] = T1[Z] / T0;
00239 
00240                 /* compute inertia tensor */
00241                 J[X][X] = density * (T2[Y] + T2[Z]);
00242                 J[Y][Y] = density * (T2[Z] + T2[X]);
00243                 J[Z][Z] = density * (T2[X] + T2[Y]);
00244                 J[X][Y] = J[Y][X] = - density * TP[X];
00245                 J[Y][Z] = J[Z][Y] = - density * TP[Y];
00246                 J[Z][X] = J[X][Z] = - density * TP[Z];
00247 
00248                 /* translate inertia tensor to center of mass */
00249                 /*
00250                   J[X][X] -= mass * (r[Y]*r[Y] + r[Z]*r[Z]);
00251                   J[Y][Y] -= mass * (r[Z]*r[Z] + r[X]*r[X]);
00252                   J[Z][Z] -= mass * (r[X]*r[X] + r[Y]*r[Y]);
00253                   J[X][Y] = J[Y][X] += mass * r[X] * r[Y]; 
00254                   J[Y][Z] = J[Z][Y] += mass * r[Y] * r[Z]; 
00255                   J[Z][X] = J[X][Z] += mass * r[Z] * r[X];
00256                 */
00257 
00258                 for (int i = 0; i < 3; i++) {
00259                         if (isnan (r [i]))
00260                                 throw SIG_CannotMirtich (__FILE__, __LINE__,
00261                                                          myExcName);
00262                         centreOfMass.set (i, r[i]);
00263                 }
00264 
00265                 for (int i = 0; i < 3; i++)
00266                         for (int j = 0; j < 3; j++) {
00267                                 if (isnan (J[i][j]))
00268                                         throw SIG_CannotMirtich (__FILE__,
00269                                                                  __LINE__,
00270                                                                  myExcName);
00271                                 inertiaTensor.set (i, j, J[i][j]);
00272                         }
00273 
00274                 if (isnan (mass))
00275                         throw SIG_CannotMirtich (__FILE__, __LINE__,
00276                                                  myExcName);
00277                 masse = mass;
00278         }
00279 
00280         void SIG_Mirtich::computeMajorAxes (DL_vector &centreOfMass,
00281                                             DL_vector &v1,
00282                                             DL_vector &v2,
00283                                             DL_vector &v3)
00284         {
00285                 double density = 1.0;
00286                 
00287                 double mass;
00288                 double r[3];            /* center of mass */
00289                 double J[3][3];         /* inertia tensor */
00290 
00291                 NEWMAT::SymmetricMatrix S (3);
00292                 NEWMAT::DiagonalMatrix D (3);
00293                 NEWMAT::Matrix V (3, 3);
00294                 NEWMAT::ColumnVector a1, a2, a3;
00295 
00296                 compute ();
00297 
00298                 mass = density * T0;
00299 
00300                 r[X] = T1[X] / T0;
00301                 r[Y] = T1[Y] / T0;
00302                 r[Z] = T1[Z] / T0;
00303 
00304                 /* compute inertia tensor */
00305                 J[X][X] = density * (T2[Y] + T2[Z]);
00306                 J[Y][Y] = density * (T2[Z] + T2[X]);
00307                 J[Z][Z] = density * (T2[X] + T2[Y]);
00308                 J[X][Y] = J[Y][X] = - density * TP[X];
00309                 J[Y][Z] = J[Z][Y] = - density * TP[Y];
00310                 J[Z][X] = J[X][Z] = - density * TP[Z];
00311 
00312                 /* translate inertia tensor to center of mass */
00313                 J[X][X] -= mass * (r[Y]*r[Y] + r[Z]*r[Z]);
00314                 J[Y][Y] -= mass * (r[Z]*r[Z] + r[X]*r[X]);
00315                 J[Z][Z] -= mass * (r[X]*r[X] + r[Y]*r[Y]);
00316                 J[X][Y] = J[Y][X] += mass * r[X] * r[Y]; 
00317                 J[Y][Z] = J[Z][Y] += mass * r[Y] * r[Z]; 
00318                 J[Z][X] = J[X][Z] += mass * r[Z] * r[X];
00319 
00320                 for (int i = 0; i < 3; i++)
00321                         centreOfMass.set (i, r[i]);
00322 
00323                 for (int i = 0; i < 3; i++)
00324                         for (int j = 0; j <= i; j++)
00325                                 S (i + 1, j + 1) = J [i] [j];
00326 
00327                 NEWMAT::Jacobi (S, D, V);
00328 
00329                 a1 = V.Column (1);
00330                 a2 = V.Column (2);
00331                 a3 = V.Column (3);
00332 
00333                 for (int i = 0; i < 3; i++) {
00334                         v1.set (i, a1 (i+1));
00335                         v2.set (i, a2 (i+1));
00336                         v3.set (i, a3 (i+1));
00337                 }
00338                 
00339                 v1.normalize ();
00340                 v2.normalize ();
00341                 v3.normalize ();
00342         }
00343 
00344         void SIG_Mirtich::moveToOriginAndMajorAxes (DL_vector & translation,
00345                                                     DL_matrix & rotation)
00346         {
00347                 DL_matrix axes;
00348 
00349                 computeMajorAxes (translation, axes.c0, axes.c1, axes.c2);
00350 
00351                 axes.invert (&rotation);
00352                 translation.timesis (-1.0);
00353 
00354                 geom->translate (translation);
00355                 geom->rotate (rotation);
00356 
00357                 computed = false;
00358         }
00359         
00360 } // namespace

Generated at Mon Sep 3 01:32:28 2001 for PG 368 - SIGEL by doxygen1.2.3 written by Dimitri van Heesch, © 1997-2000