Revision: 7691
http://playerstage.svn.sourceforge.net/playerstage/?rev=7691&view=rev
Author: hsujohnhsu
Date: 2009-05-19 03:46:26 +0000 (Tue, 19 May 2009)
Log Message:
-----------
fix body mass assignment when using custom mass matrix.
Modified Paths:
--------------
code/gazebo/trunk/server/physics/Body.cc
Modified: code/gazebo/trunk/server/physics/Body.cc
===================================================================
--- code/gazebo/trunk/server/physics/Body.cc 2009-05-19 03:42:06 UTC (rev
7690)
+++ code/gazebo/trunk/server/physics/Body.cc 2009-05-19 03:46:26 UTC (rev
7691)
@@ -218,6 +218,13 @@
childNode = childNode->GetNextByNSPrefix("geom");
}
+ // update mass CoM if using customMassMatrix
+ if (this->customMassMatrix)
+ {
+ this->UpdateCoM();
+ this->UpdatePose();
+ }
+
/// Attach mesh for CG visualization
/// Add a renderable visual for CG, make visible in Update()
if (this->mass.mass > 0.0)
@@ -878,9 +885,6 @@
// newPose is mass CoM
oldPose = this->comPose;
- //std::cout << " in UpdateCoM, name: " << this->GetName() << std::endl;
- //std::cout << " in UpdateCoM, comPose or oldPose: " << this->comPose <<
std::endl;
-
// New pose for the CoM
newPose.pos.x = this->cx;
newPose.pos.y = this->cy;
@@ -899,7 +903,7 @@
// get pose with comPose set to newPose
this->comPose = newPose;
- giter->second->SetPose(tmpPose, true);
+ giter->second->SetPose(tmpPose, false);
}
}
@@ -911,20 +915,15 @@
this->comPose = newPose;
this->SetPose(tmpPose);
+
// Settle on the new CoM pose
this->comPose = newPose;
+ // My Cheap Hack, to put the center of mass at the origin
+ this->cx = this->cy = this->cz = 0;
-
- // comPose is zero in this case, we'll keep cx, cy, cz
- this->comPose.Reset();
-
- this->comPose.pos.x = this->cx;
- this->comPose.pos.y = this->cy;
- this->comPose.pos.z = this->cz;
-
this->physicsEngine->LockMutex();
- // setup this->mass as well
+
dMassSetParameters(&this->mass, this->bodyMass,
this->cx, this->cy, this->cz,
//0,0,0,
@@ -932,10 +931,8 @@
this->ixy,this->ixz,this->iyz);
//dMassTranslate( &this->mass, -this->cx, -this->cy, -this->cz);
+ //dMassTranslate( &this->mass, this->cx, this->cy, this->cz);
- // dMatrix3 rot;
- // dMassRotate(&this->mass, rot);
-
// Set the mass matrix
if (this->mass.mass > 0)
dBodySetMass( this->bodyId, &this->mass );
@@ -954,6 +951,7 @@
// std::cout << " I[8] " << this->mass.I[8] << std::endl;
this->physicsEngine->UnlockMutex();
+
}
else
{
This was sent by the SourceForge.net collaborative development platform, the
world's largest Open Source development site.
------------------------------------------------------------------------------
Crystal Reports - New Free Runtime and 30 Day Trial
Check out the new simplified licensing option that enables
unlimited royalty-free distribution of the report engine
for externally facing server and web deployment.
http://p.sf.net/sfu/businessobjects
_______________________________________________
Playerstage-commit mailing list
[email protected]
https://lists.sourceforge.net/lists/listinfo/playerstage-commit
