Revision: 7721
http://playerstage.svn.sourceforge.net/playerstage/?rev=7721&view=rev
Author: natepak
Date: 2009-05-26 16:22:43 +0000 (Tue, 26 May 2009)
Log Message:
-----------
More bullet updates
Modified Paths:
--------------
code/gazebo/branches/bullet/server/physics/Body.cc
code/gazebo/branches/bullet/server/physics/Body.hh
code/gazebo/branches/bullet/server/physics/bullet/BulletBody.cc
code/gazebo/branches/bullet/server/physics/ode/ODEBody.cc
code/gazebo/branches/bullet/worlds/bullet.world
code/gazebo/branches/bullet/worlds/test.world
Modified: code/gazebo/branches/bullet/server/physics/Body.cc
===================================================================
--- code/gazebo/branches/bullet/server/physics/Body.cc 2009-05-26 14:06:50 UTC
(rev 7720)
+++ code/gazebo/branches/bullet/server/physics/Body.cc 2009-05-26 16:22:43 UTC
(rev 7721)
@@ -66,8 +66,10 @@
this->rpyP = new ParamT<Quatern>("rpy", Quatern(), 0);
this->rpyP->Callback( &Body::SetRotation, this );
- this->dampingFactorP = new ParamT<double>("dampingFactor", 0.03, 0);
+ this->linearDampingFactorP = new ParamT<double>("linearDamping", 0.03, 0);
+ this->angularDampingFactorP = new ParamT<double>("angularDamping", 0.03, 0);
+
// option to turn gravity off for individual body
this->turnGravityOffP = new ParamT<bool>("turnGravityOff", false, 0);
@@ -116,7 +118,8 @@
delete this->xyzP;
delete this->rpyP;
- delete this->dampingFactorP;
+ delete this->linearDampingFactorP;
+ delete this->angularDampingFactorP;
delete this->turnGravityOffP;
delete this->selfCollideP;
@@ -168,7 +171,8 @@
this->nameP->Load(node);
this->xyzP->Load(node);
this->rpyP->Load(node);
- this->dampingFactorP->Load(node);
+ this->linearDampingFactorP->Load(node);
+ this->angularDampingFactorP->Load(node);
this->turnGravityOffP->Load(node);
Pose3d initPose;
@@ -359,7 +363,7 @@
// Set the pose of the scene node
this->visualNode->SetPose(this->pose);
- if (this->mass.mass > 0.0)
+ if (this->mass->GetMass() > 0.0)
{
// Visualization of Body CG
if (this->cgVisual)
@@ -368,7 +372,9 @@
// set visual pose of CG (CoM)
// comPose is only good for initial pose, to represent dynamically
changing CoM, rotate by pose.rot
- this->cgVisual->SetPose(this->pose +
this->comPose.RotatePositionAboutOrigin(this->pose.rot.GetInverse()));
+ this->cgVisual->SetPose(this->pose +
+ this->comPose.RotatePositionAboutOrigin(
+ this->pose.rot.GetInverse()));
}
}
}*/
Modified: code/gazebo/branches/bullet/server/physics/Body.hh
===================================================================
--- code/gazebo/branches/bullet/server/physics/Body.hh 2009-05-26 14:06:50 UTC
(rev 7720)
+++ code/gazebo/branches/bullet/server/physics/Body.hh 2009-05-26 16:22:43 UTC
(rev 7721)
@@ -217,7 +217,8 @@
protected: ParamT<Vector3> *xyzP;
protected: ParamT<Quatern> *rpyP;
- protected: ParamT<double> *dampingFactorP;
+ protected: ParamT<double> *linearDampingFactorP;
+ protected: ParamT<double> *angularDampingFactorP;
protected: PhysicsEngine *physicsEngine;
Modified: code/gazebo/branches/bullet/server/physics/bullet/BulletBody.cc
===================================================================
--- code/gazebo/branches/bullet/server/physics/bullet/BulletBody.cc
2009-05-26 14:06:50 UTC (rev 7720)
+++ code/gazebo/branches/bullet/server/physics/bullet/BulletBody.cc
2009-05-26 16:22:43 UTC (rev 7721)
@@ -73,15 +73,19 @@
void BulletBody::Load(XMLConfigNode *node)
{
Body::Load(node);
+ btScalar btMass = 0.0;
+ btVector3 fallInertia(0,0,0);
// Set the initial pose of the body
this->motionState->SetVisual( this->visualNode );
this->motionState->SetPosition(**this->xyzP);
this->motionState->SetRotation(**this->rpyP);
- btScalar btMass = this->mass->GetMass();
- btVector3 fallInertia(0,0,0);
- this->compoundShape->calculateLocalInertia(btMass,fallInertia);
+ if (!this->IsStatic())
+ {
+ btMass = this->mass->GetMass();
+ this->compoundShape->calculateLocalInertia(btMass,fallInertia);
+ }
// Create a construction info object
btRigidBody::btRigidBodyConstructionInfo
@@ -90,6 +94,9 @@
// Create the new rigid body
this->rigidBody = new btRigidBody( rigidBodyCI );
+ this->rigidBody->setDamping(**this->linearDampingFactorP,
+ **this->angularDampingFactorP);
+
// Add the entity to the world
this->bulletPhysics->AddEntity( this );
@@ -97,6 +104,7 @@
// and modify parent class Entity so this body has its own spaceId
if (**this->selfCollideP)
{
+ // TODO: Implement this if required for bullet
}
}
@@ -118,10 +126,6 @@
// Update the body
void BulletBody::Update()
{
- double force;
- Vector3 vel;
- Vector3 avel;
-
Body::Update();
}
@@ -136,6 +140,11 @@
this->rigidBody->setMassProps(0, btVector3(0,0,0));
else
{
+ btScalar btMass = this->mass->GetMass();
+ btVector3 fallInertia(0,0,0);
+
+ this->compoundShape->calculateLocalInertia(btMass,fallInertia);
+ this->rigidBody->setMassProps(btMass, fallInertia);
}
}
@@ -198,9 +207,11 @@
// Return the position of the body. in global CS
Vector3 BulletBody::GetPositionRate() const
{
- Vector3 vel;
+ btVector3 btVec;
- return vel;
+ btVec = this->rigidBody->getLinearVelocity();
+
+ return Vector3(btVec.x(), btVec.y(), btVec.z());
}
@@ -209,7 +220,12 @@
Quatern BulletBody::GetRotationRate() const
{
Quatern velQ;
+ btVector3 btVec;
+ btVec = this->rigidBody->getAngularVelocity();
+
+ velQ.SetFromEuler(Vector3(btVec.x(), btVec.y(), btVec.z()));
+
return velQ;
}
@@ -217,20 +233,27 @@
// Return the rotation
Vector3 BulletBody::GetEulerRate() const
{
- Vector3 vel;
- return vel;
+ btVector3 btVec;
+
+ btVec = this->rigidBody->getAngularVelocity();
+
+ return Vector3(btVec.x(), btVec.y(), btVec.z());
}
////////////////////////////////////////////////////////////////////////////////
// Set whether this body is enabled
void BulletBody::SetEnabled(bool enable) const
{
+ if (!this->rigidBody)
+ return;
+
+ this->rigidBody->setActivationState(WANTS_DEACTIVATION);
}
/////////////////////////////////////////////////////////////////////
// Update the CoM and mass matrix
/*
- What's going on here? In Bullet the CoM of a body corresponds to the
+ What's going on here? In ODE the CoM of a body corresponds to the
origin of the body-fixed coordinate system. In Gazebo, however, we
want to have arbitrary body coordinate systems (i.e., CoM may be
displaced from the body-fixed cs). To get around this limitation in
@@ -247,64 +270,100 @@
*/
void BulletBody::UpdateCoM()
{
+ printf("UpdateCom\n");
}
////////////////////////////////////////////////////////////////////////////////
/// Set the velocity of the body
void BulletBody::SetLinearVel(const Vector3 &vel)
{
+ if (!this->rigidBody)
+ return;
+
+ this->rigidBody->setLinearVelocity( btVector3(vel.x, vel.y, vel.z) );
}
////////////////////////////////////////////////////////////////////////////////
/// Get the velocity of the body
Vector3 BulletBody::GetLinearVel() const
{
- Vector3 vel;
+ if (!this->rigidBody)
+ return Vector3(0,0,0);
- return vel;
+ btVector3 btVec = this->rigidBody->getLinearVelocity();
+
+ return Vector3(btVec.x(), btVec.y(), btVec.z());
}
////////////////////////////////////////////////////////////////////////////////
/// Set the velocity of the body
void BulletBody::SetAngularVel(const Vector3 &vel)
{
+ if (!this->rigidBody)
+ return;
+
+ this->rigidBody->setAngularVelocity( btVector3(vel.x, vel.y, vel.z) );
}
////////////////////////////////////////////////////////////////////////////////
/// Get the velocity of the body
Vector3 BulletBody::GetAngularVel() const
{
- Vector3 vel;
- return vel;
+ if (!this->rigidBody)
+ return Vector3(0,0,0);
+
+ btVector3 btVec = this->rigidBody->getAngularVelocity();
+
+ return Vector3(btVec.x(), btVec.y(), btVec.z());
}
////////////////////////////////////////////////////////////////////////////////
/// \brief Set the force applied to the body
void BulletBody::SetForce(const Vector3 &force)
{
+ if (!this->rigidBody)
+ return;
+
+ this->rigidBody->applyCentralForce(btVector3(force.x, force.y, force.z) );
}
////////////////////////////////////////////////////////////////////////////////
/// \brief Get the force applied to the body
Vector3 BulletBody::GetForce() const
{
- Vector3 force;
+ if (!this->rigidBody)
+ return Vector3(0,0,0);
- return force;
+ btVector3 btVec;
+
+ btVec = this->rigidBody->getTotalForce();
+
+ return Vector3(btVec.x(), btVec.y(), btVec.z());
}
////////////////////////////////////////////////////////////////////////////////
/// \brief Set the torque applied to the body
void BulletBody::SetTorque(const Vector3 &torque)
{
+ if (!this->rigidBody)
+ return;
+
+ this->rigidBody->applyTorque(btVector3(torque.x, torque.y, torque.z));
+
}
////////////////////////////////////////////////////////////////////////////////
/// \brief Get the torque applied to the body
Vector3 BulletBody::GetTorque() const
{
- Vector3 torque;
- return torque;
+ if (!this->rigidBody)
+ return Vector3(0,0,0);
+
+ btVector3 btVec;
+
+ btVec = this->rigidBody->getTotalTorque();
+
+ return Vector3(btVec.x(), btVec.y(), btVec.z());
}
////////////////////////////////////////////////////////////////////////////////
Modified: code/gazebo/branches/bullet/server/physics/ode/ODEBody.cc
===================================================================
--- code/gazebo/branches/bullet/server/physics/ode/ODEBody.cc 2009-05-26
14:06:50 UTC (rev 7720)
+++ code/gazebo/branches/bullet/server/physics/ode/ODEBody.cc 2009-05-26
16:22:43 UTC (rev 7721)
@@ -104,29 +104,30 @@
// Update the body
void ODEBody::Update()
{
- double force;
+ double lForce, aForce;
Vector3 vel;
Vector3 avel;
Body::Update();
- /*if (this->GetName() == "sphere1_body")
- std::cout << "Body[" << this->GetName() << "] Position[" <<
this->GetPose().pos << "]\n";
- */
-
if(this->GetId())
{
this->physicsEngine->LockMutex();
- force = this->dampingFactorP->GetValue() * this->mass->GetMass();
+ lForce = (**this->linearDampingFactorP) * this->mass->GetMass();
+ aForce = (**this->angularDampingFactorP) * this->mass->GetMass();
+
vel = this->GetLinearVel();
- dBodyAddForce(this->GetId(), -((vel.x * fabs(vel.x)) * force),
- -((vel.y * fabs(vel.y)) * force),
- -((vel.z * fabs(vel.z)) * force));
+ dBodyAddForce(this->GetId(),
+ -((vel.x * fabs(vel.x)) * lForce),
+ -((vel.y * fabs(vel.y)) * lForce),
+ -((vel.z * fabs(vel.z)) * lForce));
avel = this->GetAngularVel();
- dBodyAddTorque(this->GetId(), -avel.x * force, -avel.y * force,
- -avel.z * force);
+ dBodyAddTorque(this->GetId(),
+ -avel.x * aForce,
+ -avel.y * aForce,
+ -avel.z * aForce);
this->physicsEngine->UnlockMutex();
}
Modified: code/gazebo/branches/bullet/worlds/bullet.world
===================================================================
--- code/gazebo/branches/bullet/worlds/bullet.world 2009-05-26 14:06:50 UTC
(rev 7720)
+++ code/gazebo/branches/bullet/worlds/bullet.world 2009-05-26 16:22:43 UTC
(rev 7721)
@@ -35,14 +35,15 @@
</rendering:ogre>
<model:physical name="sphere1_model">
- <xyz>1 0 1.5</xyz>
+ <xyz>1 0 2.5</xyz>
<rpy>0.0 0.0 0.0</rpy>
<static>false</static>
<body:sphere name="sphere1_body">
<geom:sphere name="sphere1_geom">
+ <xyz>0 0 0</xyz>
<size>0.2</size>
- <mass>.1</mass>
+ <mass>1</mass>
<mu1>109999.0</mu1>
@@ -54,9 +55,9 @@
</geom:sphere>
<geom:sphere name="sphere2_geom">
- <xyz>0.5 0 0.5</xyz>
+ <xyz>-0.5 0 -0.8</xyz>
<size>0.1</size>
- <mass>1</mass>
+ <mass>.1</mass>
<mu1>109999.0</mu1>
<visual>
<size>0.2 0.2 0.2</size>
Modified: code/gazebo/branches/bullet/worlds/test.world
===================================================================
--- code/gazebo/branches/bullet/worlds/test.world 2009-05-26 14:06:50 UTC
(rev 7720)
+++ code/gazebo/branches/bullet/worlds/test.world 2009-05-26 16:22:43 UTC
(rev 7721)
@@ -56,7 +56,7 @@
<geom:sphere name="sphere2_geom">
<xyz>0.5 0 0.5</xyz>
<size>0.1</size>
- <mass>1</mass>
+ <mass>10</mass>
<mu1>109999.0</mu1>
<visual>
<size>0.2 0.2 0.2</size>
This was sent by the SourceForge.net collaborative development platform, the
world's largest Open Source development site.
------------------------------------------------------------------------------
Register Now for Creativity and Technology (CaT), June 3rd, NYC. CaT
is a gathering of tech-side developers & brand creativity professionals. Meet
the minds behind Google Creative Lab, Visual Complexity, Processing, &
iPhoneDevCamp as they present alongside digital heavyweights like Barbarian
Group, R/GA, & Big Spaceship. http://p.sf.net/sfu/creativitycat-com
_______________________________________________
Playerstage-commit mailing list
[email protected]
https://lists.sourceforge.net/lists/listinfo/playerstage-commit
