Menu
▾
▴
[Opende-svn] SF.net SVN: opende: [1106] trunk
|
From: <hid...@us...> - 2006-10-30 11:11:36
|
Revision: 1106
http://svn.sourceforge.net/opende/?rev=1106&view=rev
Author: hidden_asbestos
Date: 2006-10-30 03:11:27 -0800 (Mon, 30 Oct 2006)
Log Message:
-----------
Added dMassSetTrimesh implemented by Gero Mueller, based on a paper by Brian Mirtich. Used in test_moving_trimesh for reference.
Modified Paths:
--------------
trunk/include/ode/mass.h
trunk/ode/src/mass.cpp
trunk/ode/test/test_moving_trimesh.cpp
Modified: trunk/include/ode/mass.h
===================================================================
--- trunk/include/ode/mass.h 2006-10-30 07:06:53 UTC (rev 1105)
+++ trunk/include/ode/mass.h 2006-10-30 11:11:27 UTC (rev 1106)
@@ -67,6 +67,10 @@
ODE_API void dMassSetBoxTotal (dMass *, dReal total_mass,
dReal lx, dReal ly, dReal lz);
+#if dTRIMESH_ENABLED
+ODE_API void dMassSetTrimesh (dMass *, dReal density, dGeomID g);
+#endif // dTRIMESH_ENABLED
+
ODE_API void dMassAdjust (dMass *, dReal newmass);
ODE_API void dMassTranslate (dMass *, dReal x, dReal y, dReal z);
Modified: trunk/ode/src/mass.cpp
===================================================================
--- trunk/ode/src/mass.cpp 2006-10-30 07:06:53 UTC (rev 1105)
+++ trunk/ode/src/mass.cpp 2006-10-30 11:11:27 UTC (rev 1106)
@@ -25,7 +25,12 @@
#include <ode/odemath.h>
#include <ode/matrix.h>
+// 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)]
@@ -211,6 +216,204 @@
}
+
+
+
+
+#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];
+
+# ifndef dNODEBUG
+ dMassCheck (m);
+# endif
+}
+
+#endif // dTRIMESH_ENABLED
+
+
+
+
void dMassAdjust (dMass *m, dReal newmass)
{
dAASSERT (m);
Modified: trunk/ode/test/test_moving_trimesh.cpp
===================================================================
--- trunk/ode/test/test_moving_trimesh.cpp 2006-10-30 07:06:53 UTC (rev 1105)
+++ trunk/ode/test/test_moving_trimesh.cpp 2006-10-30 11:11:27 UTC (rev 1106)
@@ -1585,9 +1585,10 @@
obj[i].geom[0] = dCreateTriMesh(space, new_tmdata, 0, 0, 0);
// remember the mesh's dTriMeshDataID on its userdata for convenience.
- dGeomSetData(obj[i].geom[0], new_tmdata);
+ dGeomSetData(obj[i].geom[0], new_tmdata);
- dMassSetBox (&m,DENSITY,sides[0],sides[1],sides[2]);
+ dMassSetTrimesh( &m, DENSITY, obj[i].geom[0] );
+// dMassSetBox (&m,DENSITY,sides[0],sides[1],sides[2]);
}
else if (cmd == 'x') {
dGeomID g2[GPB]; // encapsulated geometries
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|