diff -r 000000000000 -r 2f259fa3e83a ode/src/mass.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/ode/src/mass.cpp Tue Feb 02 01:00:49 2010 +0200 @@ -0,0 +1,300 @@ +/************************************************************************* + * * + * 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) (dMUL((x),(x))) //!< Returns x square +#define CUBE(x) (dMUL((x),dMUL((x),(x)))) //!< Returns x cube + +#define _I(i,j) I[(i)*4+(j)] + + +// return 1 if ok, 0 if bad + +EXPORT_C int dMassCheck (const dMass *m) +{ + int i; + + if (m->mass <= 0) { + return 0; + } + if (!dIsPositiveDefinite (m->I,3)) { + 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] + dMUL(m->mass,I2[i]); + for (i=4; i<7; i++) I2[i] = m->I[i] + dMUL(m->mass,I2[i]); + for (i=8; i<11; i++) I2[i] = m->I[i] + dMUL(m->mass,I2[i]); + if (!dIsPositiveDefinite (I2,3)) { + return 0; + } + return 1; +} + + +EXPORT_C void dMassSetZero (dMass *m) +{ + m->mass = REAL(0.0); + dSetZero (m->c,sizeof(m->c) / sizeof(dReal)); + dSetZero (m->I,sizeof(m->I) / sizeof(dReal)); +} + + +EXPORT_C void dMassSetParameters (dMass *m, dReal themass, + dReal cgx, dReal cgy, dReal cgz, + dReal I11, dReal I22, dReal I33, + dReal I12, dReal I13, dReal I23) +{ + 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); +} + + +EXPORT_C void dMassSetSphere (dMass *m, dReal density, dReal radius) +{ + dMassSetSphereTotal (m, dMUL(dDIV(REAL(4.0),REAL(3.0)), dMUL(dPI,dMUL(CUBE(radius),density))), radius); +} + + +EXPORT_C void dMassSetSphereTotal (dMass *m, dReal total_mass, dReal radius) +{ + dMassSetZero (m); + m->mass = total_mass; + dReal II = dMUL(REAL(0.4),dMUL(total_mass,SQR(radius))); + m->_I(0,0) = II; + m->_I(1,1) = II; + m->_I(2,2) = II; + +# ifndef dNODEBUG + dMassCheck (m); +# endif +} + + +EXPORT_C void dMassSetCapsule (dMass *m, dReal density, int direction, + dReal radius, dReal length) +{ + dReal M1,M2,Ia,Ib; + dMassSetZero (m); + M1 = dMUL(dPI,dMUL(SQR(radius),dMUL(length,density))); // cylinder mass + M2 = dMUL(dDIV(REAL(4.0),REAL(3.0)),dMUL(dPI,dMUL(CUBE(radius),density))); // total cap mass + m->mass = M1+M2; + Ia = dMUL(M1,(dMUL(REAL(0.25),SQR(radius)) + dMUL(dDIV(REAL(1.0),REAL(12.0)),SQR(length)))) + + dMUL(M2,(dMUL(REAL(0.4),SQR(radius)) + dMUL(REAL(0.375),dMUL(radius,length)) + dMUL(REAL(0.25),SQR(length)))); + Ib = dMUL((dMUL(M1,REAL(0.5)) + dMUL(M2,REAL(0.4))),SQR(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 +} + + +EXPORT_C void dMassSetCapsuleTotal (dMass *m, dReal total_mass, int direction, + dReal a, dReal b) +{ + dMassSetCapsule (m, REAL(1.0), direction, a, b); + dMassAdjust (m, total_mass); +} + + +EXPORT_C void dMassSetCylinder (dMass *m, dReal density, int direction, + dReal radius, dReal length) +{ + dMassSetCylinderTotal (m, dMUL(dPI,dMUL(SQR(radius),dMUL(length,density))), + direction, radius, length); +} + +EXPORT_C void dMassSetCylinderTotal (dMass *m, dReal total_mass, int direction, + dReal radius, dReal length) +{ + dReal r2,I; + dMassSetZero (m); + r2 = SQR(radius); + m->mass = total_mass; + I = dMUL(total_mass,(dMUL(REAL(0.25),r2) + dMUL(dDIV(REAL(1.0),REAL(12.0)),SQR(length)))); + m->_I(0,0) = I; + m->_I(1,1) = I; + m->_I(2,2) = I; + m->_I(direction-1,direction-1) = dMUL(total_mass,dMUL(REAL(0.5),r2)); + +# ifndef dNODEBUG + dMassCheck (m); +# endif +} + + +EXPORT_C void dMassSetBox (dMass *m, dReal density, + dReal lx, dReal ly, dReal lz) +{ + dMassSetBoxTotal (m, dMUL(lx,dMUL(ly,dMUL(lz,density))), lx, ly, lz); +} + + +EXPORT_C void dMassSetBoxTotal (dMass *m, dReal total_mass, + dReal lx, dReal ly, dReal lz) +{ + dMassSetZero (m); + m->mass = total_mass; + m->_I(0,0) = dMUL(dDIV(total_mass,REAL(12.0)),(SQR(ly) + SQR(lz))); + m->_I(1,1) = dMUL(dDIV(total_mass,REAL(12.0)),(SQR(lx) + SQR(lz))); + m->_I(2,2) = dMUL(dDIV(total_mass,REAL(12.0)),(SQR(lx) + SQR(ly))); + +# ifndef dNODEBUG + dMassCheck (m); +# endif +} + +EXPORT_C void dMassAdjust (dMass *m, dReal newmass) +{ + dReal scale = dDIV(newmass,m->mass); + m->mass = newmass; + for (int i=0; i<3; i++) for (int j=0; j<3; j++) m->_I(i,j) = dMUL(m->_I(i,j),scale); + +# ifndef dNODEBUG + dMassCheck (m); +# endif +} + + +EXPORT_C 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]; + + // 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) += dMUL(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 +} + + +EXPORT_C 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]; + + // 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 +} + + +EXPORT_C void dMassAdd (dMass *a, const dMass *b) +{ + int i; + dReal denom = dRecip (a->mass + b->mass); + for (i=0; i<3; i++) a->c[i] = dMUL((dMUL(a->c[i],a->mass) + dMUL(b->c[i],b->mass)),denom); + a->mass += b->mass; + for (i=0; i<12; i++) a->I[i] += b->I[i]; +}