From 2f8d7092bc2c9609fa98d6888106b96f38b22828 Mon Sep 17 00:00:00 2001 From: dan miller Date: Sun, 21 Oct 2007 08:36:32 +0000 Subject: libraries moved to opensim-libs, a new repository --- libraries/ode-0.9/ode/src/mass.cpp | 529 ------------------------------------- 1 file changed, 529 deletions(-) delete mode 100644 libraries/ode-0.9/ode/src/mass.cpp (limited to 'libraries/ode-0.9/ode/src/mass.cpp') diff --git a/libraries/ode-0.9/ode/src/mass.cpp b/libraries/ode-0.9/ode/src/mass.cpp deleted file mode 100644 index 7d0b1c2..0000000 --- a/libraries/ode-0.9/ode/src/mass.cpp +++ /dev/null @@ -1,529 +0,0 @@ -/************************************************************************* - * * - * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * - * All rights reserved. Email: russ@q12.org Web: www.q12.org * - * * - * This library is free software; you can redistribute it and/or * - * modify it under the terms of EITHER: * - * (1) The GNU Lesser General Public License as published by the Free * - * Software Foundation; either version 2.1 of the License, or (at * - * your option) any later version. The text of the GNU Lesser * - * General Public License is included with this library in the * - * file LICENSE.TXT. * - * (2) The BSD-style license that is included with this library in * - * the file LICENSE-BSD.TXT. * - * * - * This library is distributed in the hope that it will be useful, * - * but WITHOUT ANY WARRANTY; without even the implied warranty of * - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * - * LICENSE.TXT and LICENSE-BSD.TXT for more details. * - * * - *************************************************************************/ - -#include -#include -#include -#include - -// Local dependencies -#include "collision_kernel.h" - -#define SQR(x) ((x)*(x)) //!< Returns x square -#define CUBE(x) ((x)*(x)*(x)) //!< Returns x cube - -#define _I(i,j) I[(i)*4+(j)] - - -// return 1 if ok, 0 if bad - -int dMassCheck (const dMass *m) -{ - int i; - - if (m->mass <= 0) { - dDEBUGMSG ("mass must be > 0"); - return 0; - } - if (!dIsPositiveDefinite (m->I,3)) { - dDEBUGMSG ("inertia must be positive definite"); - return 0; - } - - // verify that the center of mass position is consistent with the mass - // and inertia matrix. this is done by checking that the inertia around - // the center of mass is also positive definite. from the comment in - // dMassTranslate(), if the body is translated so that its center of mass - // is at the point of reference, then the new inertia is: - // I + mass*crossmat(c)^2 - // note that requiring this to be positive definite is exactly equivalent - // to requiring that the spatial inertia matrix - // [ mass*eye(3,3) M*crossmat(c)^T ] - // [ M*crossmat(c) I ] - // is positive definite, given that I is PD and mass>0. see the theorem - // about partitioned PD matrices for proof. - - dMatrix3 I2,chat; - dSetZero (chat,12); - dCROSSMAT (chat,m->c,4,+,-); - dMULTIPLY0_333 (I2,chat,chat); - for (i=0; i<3; i++) I2[i] = m->I[i] + m->mass*I2[i]; - for (i=4; i<7; i++) I2[i] = m->I[i] + m->mass*I2[i]; - for (i=8; i<11; i++) I2[i] = m->I[i] + m->mass*I2[i]; - if (!dIsPositiveDefinite (I2,3)) { - dDEBUGMSG ("center of mass inconsistent with mass parameters"); - return 0; - } - return 1; -} - - -void dMassSetZero (dMass *m) -{ - dAASSERT (m); - m->mass = REAL(0.0); - dSetZero (m->c,sizeof(m->c) / sizeof(dReal)); - dSetZero (m->I,sizeof(m->I) / sizeof(dReal)); -} - - -void dMassSetParameters (dMass *m, dReal themass, - dReal cgx, dReal cgy, dReal cgz, - dReal I11, dReal I22, dReal I33, - dReal I12, dReal I13, dReal I23) -{ - dAASSERT (m); - dMassSetZero (m); - m->mass = themass; - m->c[0] = cgx; - m->c[1] = cgy; - m->c[2] = cgz; - m->_I(0,0) = I11; - m->_I(1,1) = I22; - m->_I(2,2) = I33; - m->_I(0,1) = I12; - m->_I(0,2) = I13; - m->_I(1,2) = I23; - m->_I(1,0) = I12; - m->_I(2,0) = I13; - m->_I(2,1) = I23; - dMassCheck (m); -} - - -void dMassSetSphere (dMass *m, dReal density, dReal radius) -{ - dMassSetSphereTotal (m, (REAL(4.0)/REAL(3.0)) * M_PI * - radius*radius*radius * density, radius); -} - - -void dMassSetSphereTotal (dMass *m, dReal total_mass, dReal radius) -{ - dAASSERT (m); - dMassSetZero (m); - m->mass = total_mass; - dReal II = REAL(0.4) * total_mass * radius*radius; - m->_I(0,0) = II; - m->_I(1,1) = II; - m->_I(2,2) = II; - -# ifndef dNODEBUG - dMassCheck (m); -# endif -} - - -void dMassSetCapsule (dMass *m, dReal density, int direction, - dReal radius, dReal length) -{ - dReal M1,M2,Ia,Ib; - dAASSERT (m); - dUASSERT (direction >= 1 && direction <= 3,"bad direction number"); - dMassSetZero (m); - M1 = M_PI*radius*radius*length*density; // cylinder mass - M2 = (REAL(4.0)/REAL(3.0))*M_PI*radius*radius*radius*density; // total cap mass - m->mass = M1+M2; - Ia = M1*(REAL(0.25)*radius*radius + (REAL(1.0)/REAL(12.0))*length*length) + - M2*(REAL(0.4)*radius*radius + REAL(0.375)*radius*length + REAL(0.25)*length*length); - Ib = (M1*REAL(0.5) + M2*REAL(0.4))*radius*radius; - m->_I(0,0) = Ia; - m->_I(1,1) = Ia; - m->_I(2,2) = Ia; - m->_I(direction-1,direction-1) = Ib; - -# ifndef dNODEBUG - dMassCheck (m); -# endif -} - - -void dMassSetCapsuleTotal (dMass *m, dReal total_mass, int direction, - dReal a, dReal b) -{ - dMassSetCapsule (m, 1.0, direction, a, b); - dMassAdjust (m, total_mass); -} - - -void dMassSetCylinder (dMass *m, dReal density, int direction, - dReal radius, dReal length) -{ - dMassSetCylinderTotal (m, M_PI*radius*radius*length*density, - direction, radius, length); -} - -void dMassSetCylinderTotal (dMass *m, dReal total_mass, int direction, - dReal radius, dReal length) -{ - dReal r2,I; - dAASSERT (m); - dUASSERT (direction >= 1 && direction <= 3,"bad direction number"); - dMassSetZero (m); - r2 = radius*radius; - m->mass = total_mass; - I = total_mass*(REAL(0.25)*r2 + (REAL(1.0)/REAL(12.0))*length*length); - m->_I(0,0) = I; - m->_I(1,1) = I; - m->_I(2,2) = I; - m->_I(direction-1,direction-1) = total_mass*REAL(0.5)*r2; - -# ifndef dNODEBUG - dMassCheck (m); -# endif -} - - -void dMassSetBox (dMass *m, dReal density, - dReal lx, dReal ly, dReal lz) -{ - dMassSetBoxTotal (m, lx*ly*lz*density, lx, ly, lz); -} - - -void dMassSetBoxTotal (dMass *m, dReal total_mass, - dReal lx, dReal ly, dReal lz) -{ - dAASSERT (m); - dMassSetZero (m); - m->mass = total_mass; - m->_I(0,0) = total_mass/REAL(12.0) * (ly*ly + lz*lz); - m->_I(1,1) = total_mass/REAL(12.0) * (lx*lx + lz*lz); - m->_I(2,2) = total_mass/REAL(12.0) * (lx*lx + ly*ly); - -# ifndef dNODEBUG - dMassCheck (m); -# endif -} - - - - - - -#if dTRIMESH_ENABLED - -/* - * dMassSetTrimesh, implementation by Gero Mueller. - * Based on Brian Mirtich, "Fast and Accurate Computation of - * Polyhedral Mass Properties," journal of graphics tools, volume 1, - * number 2, 1996. -*/ -void dMassSetTrimesh( dMass *m, dReal density, dGeomID g ) -{ - dAASSERT (m); - dUASSERT(g && g->type == dTriMeshClass, "argument not a trimesh"); - - dMassSetZero (m); - - unsigned int triangles = dGeomTriMeshGetTriangleCount( g ); - - dReal nx, ny, nz; - unsigned int i, A, B, C; - // face integrals - dReal Fa, Fb, Fc, Faa, Fbb, Fcc, Faaa, Fbbb, Fccc, Faab, Fbbc, Fcca; - - // projection integrals - dReal P1, Pa, Pb, Paa, Pab, Pbb, Paaa, Paab, Pabb, Pbbb; - - dReal T0 = 0; - dReal T1[3] = {0., 0., 0.}; - dReal T2[3] = {0., 0., 0.}; - dReal TP[3] = {0., 0., 0.}; - - for( i = 0; i < triangles; i++ ) - { - dVector3 v0, v1, v2; - dGeomTriMeshGetTriangle( g, i, &v0, &v1, &v2); - - dVector3 n, a, b; - dOP( a, -, v1, v0 ); - dOP( b, -, v2, v0 ); - dCROSS( n, =, b, a ); - nx = fabs(n[0]); - ny = fabs(n[1]); - nz = fabs(n[2]); - - if( nx > ny && nx > nz ) - C = 0; - else - C = (ny > nz) ? 1 : 2; - - A = (C + 1) % 3; - B = (A + 1) % 3; - - // calculate face integrals - { - dReal w; - dReal k1, k2, k3, k4; - - //compProjectionIntegrals(f); - { - dReal a0, a1, da; - dReal b0, b1, db; - dReal a0_2, a0_3, a0_4, b0_2, b0_3, b0_4; - dReal a1_2, a1_3, b1_2, b1_3; - dReal C1, Ca, Caa, Caaa, Cb, Cbb, Cbbb; - dReal Cab, Kab, Caab, Kaab, Cabb, Kabb; - - P1 = Pa = Pb = Paa = Pab = Pbb = Paaa = Paab = Pabb = Pbbb = 0.0; - - for( int j = 0; j < 3; j++) - { - switch(j) - { - case 0: - a0 = v0[A]; - b0 = v0[B]; - a1 = v1[A]; - b1 = v1[B]; - break; - case 1: - a0 = v1[A]; - b0 = v1[B]; - a1 = v2[A]; - b1 = v2[B]; - break; - case 2: - a0 = v2[A]; - b0 = v2[B]; - a1 = v0[A]; - b1 = v0[B]; - break; - } - da = a1 - a0; - db = b1 - b0; - a0_2 = a0 * a0; a0_3 = a0_2 * a0; a0_4 = a0_3 * a0; - b0_2 = b0 * b0; b0_3 = b0_2 * b0; b0_4 = b0_3 * b0; - a1_2 = a1 * a1; a1_3 = a1_2 * a1; - b1_2 = b1 * b1; b1_3 = b1_2 * b1; - - C1 = a1 + a0; - Ca = a1*C1 + a0_2; Caa = a1*Ca + a0_3; Caaa = a1*Caa + a0_4; - Cb = b1*(b1 + b0) + b0_2; Cbb = b1*Cb + b0_3; Cbbb = b1*Cbb + b0_4; - Cab = 3*a1_2 + 2*a1*a0 + a0_2; Kab = a1_2 + 2*a1*a0 + 3*a0_2; - Caab = a0*Cab + 4*a1_3; Kaab = a1*Kab + 4*a0_3; - Cabb = 4*b1_3 + 3*b1_2*b0 + 2*b1*b0_2 + b0_3; - Kabb = b1_3 + 2*b1_2*b0 + 3*b1*b0_2 + 4*b0_3; - - P1 += db*C1; - Pa += db*Ca; - Paa += db*Caa; - Paaa += db*Caaa; - Pb += da*Cb; - Pbb += da*Cbb; - Pbbb += da*Cbbb; - Pab += db*(b1*Cab + b0*Kab); - Paab += db*(b1*Caab + b0*Kaab); - Pabb += da*(a1*Cabb + a0*Kabb); - } - - P1 /= 2.0; - Pa /= 6.0; - Paa /= 12.0; - Paaa /= 20.0; - Pb /= -6.0; - Pbb /= -12.0; - Pbbb /= -20.0; - Pab /= 24.0; - Paab /= 60.0; - Pabb /= -60.0; - } - - w = - dDOT(n, v0); - - k1 = 1 / n[C]; k2 = k1 * k1; k3 = k2 * k1; k4 = k3 * k1; - - Fa = k1 * Pa; - Fb = k1 * Pb; - Fc = -k2 * (n[A]*Pa + n[B]*Pb + w*P1); - - Faa = k1 * Paa; - Fbb = k1 * Pbb; - Fcc = k3 * (SQR(n[A])*Paa + 2*n[A]*n[B]*Pab + SQR(n[B])*Pbb + - w*(2*(n[A]*Pa + n[B]*Pb) + w*P1)); - - Faaa = k1 * Paaa; - Fbbb = k1 * Pbbb; - Fccc = -k4 * (CUBE(n[A])*Paaa + 3*SQR(n[A])*n[B]*Paab - + 3*n[A]*SQR(n[B])*Pabb + CUBE(n[B])*Pbbb - + 3*w*(SQR(n[A])*Paa + 2*n[A]*n[B]*Pab + SQR(n[B])*Pbb) - + w*w*(3*(n[A]*Pa + n[B]*Pb) + w*P1)); - - Faab = k1 * Paab; - Fbbc = -k2 * (n[A]*Pabb + n[B]*Pbbb + w*Pbb); - Fcca = k3 * (SQR(n[A])*Paaa + 2*n[A]*n[B]*Paab + SQR(n[B])*Pabb - + w*(2*(n[A]*Paa + n[B]*Pab) + w*Pa)); - } - - - T0 += n[0] * ((A == 0) ? Fa : ((B == 0) ? Fb : Fc)); - - T1[A] += n[A] * Faa; - T1[B] += n[B] * Fbb; - T1[C] += n[C] * Fcc; - T2[A] += n[A] * Faaa; - T2[B] += n[B] * Fbbb; - T2[C] += n[C] * Fccc; - TP[A] += n[A] * Faab; - TP[B] += n[B] * Fbbc; - TP[C] += n[C] * Fcca; - } - - T1[0] /= 2; T1[1] /= 2; T1[2] /= 2; - T2[0] /= 3; T2[1] /= 3; T2[2] /= 3; - TP[0] /= 2; TP[1] /= 2; TP[2] /= 2; - - m->mass = density * T0; - m->_I(0,0) = density * (T2[1] + T2[2]); - m->_I(1,1) = density * (T2[2] + T2[0]); - m->_I(2,2) = density * (T2[0] + T2[1]); - m->_I(0,1) = - density * TP[0]; - m->_I(1,0) = - density * TP[0]; - m->_I(2,1) = - density * TP[1]; - m->_I(1,2) = - density * TP[1]; - m->_I(2,0) = - density * TP[2]; - m->_I(0,2) = - density * TP[2]; - - // Added to address SF bug 1729095 - dMassTranslate( m, T1[0] / T0, T1[1] / T0, T1[2] / T0 ); - -# ifndef dNODEBUG - dMassCheck (m); -# endif -} - - -void dMassSetTrimeshTotal( dMass *m, dReal total_mass, dGeomID g) -{ - dAASSERT( m ); - dUASSERT( g && g->type == dTriMeshClass, "argument not a trimesh" ); - dMassSetTrimesh( m, 1.0, g ); - dMassAdjust( m, total_mass ); -} - -#endif // dTRIMESH_ENABLED - - - - -void dMassAdjust (dMass *m, dReal newmass) -{ - dAASSERT (m); - dReal scale = newmass / m->mass; - m->mass = newmass; - for (int i=0; i<3; i++) for (int j=0; j<3; j++) m->_I(i,j) *= scale; - -# ifndef dNODEBUG - dMassCheck (m); -# endif -} - - -void dMassTranslate (dMass *m, dReal x, dReal y, dReal z) -{ - // if the body is translated by `a' relative to its point of reference, - // the new inertia about the point of reference is: - // - // I + mass*(crossmat(c)^2 - crossmat(c+a)^2) - // - // where c is the existing center of mass and I is the old inertia. - - int i,j; - dMatrix3 ahat,chat,t1,t2; - dReal a[3]; - - dAASSERT (m); - - // adjust inertia matrix - dSetZero (chat,12); - dCROSSMAT (chat,m->c,4,+,-); - a[0] = x + m->c[0]; - a[1] = y + m->c[1]; - a[2] = z + m->c[2]; - dSetZero (ahat,12); - dCROSSMAT (ahat,a,4,+,-); - dMULTIPLY0_333 (t1,ahat,ahat); - dMULTIPLY0_333 (t2,chat,chat); - for (i=0; i<3; i++) for (j=0; j<3; j++) - m->_I(i,j) += m->mass * (t2[i*4+j]-t1[i*4+j]); - - // ensure perfect symmetry - m->_I(1,0) = m->_I(0,1); - m->_I(2,0) = m->_I(0,2); - m->_I(2,1) = m->_I(1,2); - - // adjust center of mass - m->c[0] += x; - m->c[1] += y; - m->c[2] += z; - -# ifndef dNODEBUG - dMassCheck (m); -# endif -} - - -void dMassRotate (dMass *m, const dMatrix3 R) -{ - // if the body is rotated by `R' relative to its point of reference, - // the new inertia about the point of reference is: - // - // R * I * R' - // - // where I is the old inertia. - - dMatrix3 t1; - dReal t2[3]; - - dAASSERT (m); - - // rotate inertia matrix - dMULTIPLY2_333 (t1,m->I,R); - dMULTIPLY0_333 (m->I,R,t1); - - // ensure perfect symmetry - m->_I(1,0) = m->_I(0,1); - m->_I(2,0) = m->_I(0,2); - m->_I(2,1) = m->_I(1,2); - - // rotate center of mass - dMULTIPLY0_331 (t2,R,m->c); - m->c[0] = t2[0]; - m->c[1] = t2[1]; - m->c[2] = t2[2]; - -# ifndef dNODEBUG - dMassCheck (m); -# endif -} - - -void dMassAdd (dMass *a, const dMass *b) -{ - int i; - dAASSERT (a && b); - dReal denom = dRecip (a->mass + b->mass); - for (i=0; i<3; i++) a->c[i] = (a->c[i]*a->mass + b->c[i]*b->mass)*denom; - a->mass += b->mass; - for (i=0; i<12; i++) a->I[i] += b->I[i]; -} -- cgit v1.1