00001
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
00206
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];
00230 double J[3][3];
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
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
00249
00250
00251
00252
00253
00254
00255
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 ¢reOfMass,
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];
00289 double J[3][3];
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
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
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 }