From 5d845e8f0f5ff8288ce6edca85527a3305e2b459 Mon Sep 17 00:00:00 2001 From: Paolo Cignoni cignoni Date: Mon, 1 Feb 2010 23:08:18 +0000 Subject: [PATCH] Center of gravity issues solved --- .../filter_physics/DynamicMeshSubFilter.cpp | 3 +- src/fgt/filter_physics/MeshSubFilter.cpp | 2 +- src/fgt/filter_physics/ODEFacade.cpp | 107 +++++++++--------- src/fgt/filter_physics/ODEFacade.h | 2 +- src/fgt/filter_physics/filter_physics.h | 2 +- 5 files changed, 59 insertions(+), 57 deletions(-) diff --git a/src/fgt/filter_physics/DynamicMeshSubFilter.cpp b/src/fgt/filter_physics/DynamicMeshSubFilter.cpp index 42106776a..9ddaab1d6 100644 --- a/src/fgt/filter_physics/DynamicMeshSubFilter.cpp +++ b/src/fgt/filter_physics/DynamicMeshSubFilter.cpp @@ -12,7 +12,7 @@ void DynamicMeshSubFilter::initParameterSet(QAction* action,MeshDocument& md, Ri } bool DynamicMeshSubFilter::applyFilter(QAction* filter, MeshDocument &md, RichParameterSet& par, vcg::CallBackPos* cb){ - if(par.getInt("seconds") < 0 || par.getInt("fps") <= 0 || par.getInt("iterations") <= 0 || par.getInt("contacts") <= 0 || par.getFloat("bounciness") < 0.f || par.getFloat("bounciness") > 1.f) + if(par.getInt("seconds") < 0 || par.getInt("fps") <= 0 || par.getInt("iterations") <= 0 || par.getInt("contacts") <= 0 || par.getFloat("bounciness") < 0 || par.getFloat("bounciness") > 1) return false; if(configurationHasChanged(md, par)) @@ -45,7 +45,6 @@ bool DynamicMeshSubFilter::configurationHasChanged(MeshDocument& md, RichParamet saveMeshState(md);*/ - // Old correctness testing if(md.size() == m_files.size()) for(int i = 0; i < m_files.size(); i++) changed |= m_files.at(i) != md.getMesh(i)->fileName; diff --git a/src/fgt/filter_physics/MeshSubFilter.cpp b/src/fgt/filter_physics/MeshSubFilter.cpp index f1ff97412..60cee61a9 100644 --- a/src/fgt/filter_physics/MeshSubFilter.cpp +++ b/src/fgt/filter_physics/MeshSubFilter.cpp @@ -6,7 +6,7 @@ MeshSubFilter::MeshSubFilter(){ } void MeshSubFilter::initParameterSet(QAction* action,MeshDocument& md, RichParameterSet & par){ - par.addParam(new RichInt("fps", 50, "Frames per second", "The number of times per second the physics simulation is updated")); + par.addParam(new RichInt("fps", 100, "Frames per second", "The number of times per second the physics simulation is updated")); par.addParam(new RichInt("iterations", 10, "Physics method iterations", "Number of iterations of the physics iterative method for equation solving")); par.addParam(new RichInt("contacts", 20, "Max contacts", "Maximum number of contact points to generate per object pair")); par.addParam(new RichFloat("bounciness", 0.1f, "Bounciness", "The amount of bounciness of a collision: 0 means the surfaces are not bouncy at all, 1 is the maximum bounciness")); diff --git a/src/fgt/filter_physics/ODEFacade.cpp b/src/fgt/filter_physics/ODEFacade.cpp index cc68b2dde..96d9ef73a 100644 --- a/src/fgt/filter_physics/ODEFacade.cpp +++ b/src/fgt/filter_physics/ODEFacade.cpp @@ -29,9 +29,6 @@ void ODEFacade::initialize(){ m_world = dWorldCreate(); m_space = dSimpleSpaceCreate(0); m_contactGroup = dJointGroupCreate(0); - - /*dWorldSetCFM(m_world, 0.001f); - dWorldSetERP(m_world, 0.9f);*/ } void ODEFacade::clear(MeshDocument& md){ @@ -56,15 +53,27 @@ void ODEFacade::registerTriMesh(MeshModel& mesh, bool scenery){ if(tri::HasPerMeshAttribute(mesh.cm, "physicsID")) return; + if(mesh.cm.Tr != vcg::Matrix44f::Identity()){ + vcg::tri::UpdatePosition::Matrix(mesh.cm, mesh.cm.Tr); + mesh.cm.Tr.SetIdentity(); + } + + tri::Inertia inertia; + inertia.Compute(mesh.cm); + Point3f center = mesh.cm.bbox.Center(); + ODEMesh* odeMesh = new ODEMesh(); odeMesh->vertices = new dReal[mesh.cm.vert.size()][3]; odeMesh->normals = new dReal[mesh.cm.face.size()][3]; odeMesh->indices = new dTriIndex[mesh.cm.face.size()][3]; + odeMesh->centerOfMass[0] = scenery ? 0.f : inertia.CenterOfMass()[0]; + odeMesh->centerOfMass[1] = scenery ? 0.f : inertia.CenterOfMass()[1]; + odeMesh->centerOfMass[2] = scenery ? 0.f : inertia.CenterOfMass()[2]; int i = 0; for(CMeshO::ConstVertexIterator vi = mesh.cm.vert.begin(); vi != mesh.cm.vert.end(); vi++, i++){ for(int j = 0; j < 3; j++){ - odeMesh->vertices[i][j] = vi->P()[j]; + odeMesh->vertices[i][j] = vi->P()[j] - odeMesh->centerOfMass[j] ; } } @@ -104,61 +113,50 @@ void ODEFacade::setAsRigidBody(MeshModel& mesh, bool isRigidBody){ tri::Inertia inertia; inertia.Compute(m_registeredMeshes[index()].first->cm); - Matrix33f IT; - inertia.InertiaTensor(IT); - dMassSetParameters(&m_registeredMeshes[index()].second->mass, abs(inertia.Mass()), - inertia.CenterOfMass()[0], inertia.CenterOfMass()[1], inertia.CenterOfMass()[2], - IT[0][0], IT[1][1], IT[2][2], IT[0][1], IT[0][2], IT[1][2]); - //For now it's best to let ODE do the job automatically - //dBodySetMass(m_registeredMeshes[index()].second->body, &m_registeredMeshes[index()].second->mass); + dMass* mass = &m_registeredMeshes[index()].second->mass; + dMassSetTrimesh(mass, 1.0f, m_registeredMeshes[index()].second->geom); + mass->mass = 10; + dReal centerOfMass[3] = { mass->c[0], mass->c[1], mass->c[2] }; + dMassTranslate(&m_registeredMeshes[index()].second->mass, -centerOfMass[0], -centerOfMass[1], -centerOfMass[2]); + dBodySetMass(m_registeredMeshes[index()].second->body, &m_registeredMeshes[index()].second->mass); + dBodySetPosition(m_registeredMeshes[index()].second->body, inertia.CenterOfMass()[0], inertia.CenterOfMass()[1], inertia.CenterOfMass()[2]); } - - updateEngineTransform(mesh); -} - -void ODEFacade::updateEngineTransform(MeshModel& mesh){ - if(!tri::HasPerMeshAttribute(mesh.cm, "physicsID")) - return; - - MeshIndex index = tri::Allocator::GetPerMeshAttribute(mesh.cm, "physicsID"); - - Point3 trans = mesh.cm.Tr.GetColumn3(3); - dGeomSetPosition(m_registeredMeshes[index()].second->geom, trans.X(), trans.Y(), trans.Z()); - - dMatrix3 rotationMatrix; - - rotationMatrix[0] = mesh.cm.Tr[0][0]; - rotationMatrix[1] = mesh.cm.Tr[0][1]; - rotationMatrix[2] = mesh.cm.Tr[0][2]; - rotationMatrix[3] = 0.f; - rotationMatrix[4] = mesh.cm.Tr[1][0]; - rotationMatrix[5] = mesh.cm.Tr[1][1]; - rotationMatrix[6] = mesh.cm.Tr[1][2]; - rotationMatrix[7] = 0.f; - rotationMatrix[8] = mesh.cm.Tr[2][0]; - rotationMatrix[9] = mesh.cm.Tr[2][1]; - rotationMatrix[10] = mesh.cm.Tr[2][2]; - rotationMatrix[11] = 0.f; - - dGeomSetRotation(m_registeredMeshes[index()].second->geom, rotationMatrix); } void ODEFacade::updateTransform(){ for(MeshContainer::iterator i = m_registeredMeshes.begin(); i != m_registeredMeshes.end(); i++){ - const dReal* position = dGeomGetPosition(i->second->geom); - i->first->cm.Tr.SetTranslate(position[0], position[1], position[2]); + const dReal* position = 0; + const dReal* rotation = 0; + + if(i->second->body != 0){ + position = dBodyGetPosition(i->second->body); + rotation = dBodyGetRotation(i->second->body); + }else{ + position = dGeomGetPosition(i->second->geom); + rotation = dGeomGetRotation(i->second->geom); + } - const dReal* rotation = dGeomGetRotation(i->second->geom); i->first->cm.Tr[0][0] = rotation[0]; i->first->cm.Tr[0][1] = rotation[1]; i->first->cm.Tr[0][2] = rotation[2]; + i->first->cm.Tr[0][3] = position[0]; i->first->cm.Tr[1][0] = rotation[4]; i->first->cm.Tr[1][1] = rotation[5]; i->first->cm.Tr[1][2] = rotation[6]; + i->first->cm.Tr[1][3] = position[1]; i->first->cm.Tr[2][0] = rotation[8]; i->first->cm.Tr[2][1] = rotation[9]; i->first->cm.Tr[2][2] = rotation[10]; + i->first->cm.Tr[2][3] = position[2]; + i->first->cm.Tr[3][0] = 0.f; + i->first->cm.Tr[3][1] = 0.f; + i->first->cm.Tr[3][2] = 0.f; + i->first->cm.Tr[3][3] = 1.f; + + vcg::Matrix44f m; + m.SetTranslate(-i->second->centerOfMass[0], -i->second->centerOfMass[1], -i->second->centerOfMass[2]); + i->first->cm.Tr *= m; } } @@ -183,12 +181,6 @@ void ODEFacade::collisionCallback(dGeomID o1, dGeomID o2){ m_contacts[i].surface.bounce = m_bounciness; m_contacts[i].surface.bounce_vel = 0.1; m_contacts[i].surface.soft_cfm = 0.01; - /*m_contacts[i].surface.mode = dContactBounce | dContactSoftCFM; - m_contacts[i].surface.mu = dInfinity; - m_contacts[i].surface.mu2 = 0; - m_contacts[i].surface.bounce = 0.1; - m_contacts[i].surface.bounce_vel = 0.1; - m_contacts[i].surface.soft_cfm = 0.01;*/ } int collisions = dCollide(o1, o2, m_contacts.size(), &m_contacts[0].geom, sizeof(dContact)); @@ -205,8 +197,16 @@ vcg::Matrix44f ODEFacade::getTransformationMatrix(MeshModel& mesh){ MeshIndex index = tri::Allocator::GetPerMeshAttribute(mesh.cm, "physicsID"); vcg::Matrix44f matrix; - const dReal* position = dGeomGetPosition(m_registeredMeshes[index()].second->geom); - const dReal* rotation = dGeomGetRotation(m_registeredMeshes[index()].second->geom); + const dReal* position = 0; + const dReal* rotation = 0; + + if(m_registeredMeshes[index()].second->body != 0){ + position = dBodyGetPosition(m_registeredMeshes[index()].second->body); + rotation = dBodyGetRotation(m_registeredMeshes[index()].second->body); + }else{ + position = dGeomGetPosition(m_registeredMeshes[index()].second->geom); + rotation = dGeomGetRotation(m_registeredMeshes[index()].second->geom); + } matrix[0][0] = rotation[0]; matrix[0][1] = rotation[1]; @@ -225,7 +225,10 @@ vcg::Matrix44f ODEFacade::getTransformationMatrix(MeshModel& mesh){ matrix[3][2] = 0.f; matrix[3][3] = 1.f; - return matrix; + vcg::Matrix44f m; + m.SetTranslate(-m_registeredMeshes[index()].second->centerOfMass[0], -m_registeredMeshes[index()].second->centerOfMass[1], -m_registeredMeshes[index()].second->centerOfMass[2]); + + return matrix * m; } void ODEFacade::setIterations(int iterations){ diff --git a/src/fgt/filter_physics/ODEFacade.h b/src/fgt/filter_physics/ODEFacade.h index dee658d89..5a9ab1a6e 100644 --- a/src/fgt/filter_physics/ODEFacade.h +++ b/src/fgt/filter_physics/ODEFacade.h @@ -52,6 +52,7 @@ protected: dTriMeshDataID data; dReal (*vertices)[3]; dReal (*normals)[3]; + dReal centerOfMass[3]; dTriIndex (*indices)[3]; }; @@ -60,7 +61,6 @@ protected: static void collisionCallback(void* data, dGeomID o1, dGeomID o2); void collisionCallback(dGeomID o1, dGeomID o2); - void updateEngineTransform(MeshModel& mesh); //This class is a monostate static bool m_initialized; diff --git a/src/fgt/filter_physics/filter_physics.h b/src/fgt/filter_physics/filter_physics.h index 3ead6c0bf..a94454cdf 100644 --- a/src/fgt/filter_physics/filter_physics.h +++ b/src/fgt/filter_physics/filter_physics.h @@ -32,7 +32,7 @@ class FilterPhysics : public QObject, public MeshFilterInterface virtual QString filterInfo(FilterIDType filter) const; virtual int getRequirements(QAction*){return MeshModel::MM_FACEVERT | MeshModel::MM_FACENORMAL;} - virtual int postCondition( QAction* ) const{return MeshModel::MM_TRANSFMATRIX;} + virtual int postCondition( QAction* ) const{return MeshModel::MM_FACENORMAL; /*MeshModel::MM_TRANSFMATRIX; */} virtual bool autoDialog(QAction*) {return true;} virtual void initParameterSet(QAction*, MeshDocument&, RichParameterSet&);