Revision: 8322
http://playerstage.svn.sourceforge.net/playerstage/?rev=8322&view=rev
Author: natepak
Date: 2009-10-21 16:04:08 +0000 (Wed, 21 Oct 2009)
Log Message:
-----------
Move all entity pose storage in the Entityclass.
Modified Paths:
--------------
code/gazebo/branches/bullet2/server/Entity.cc
code/gazebo/branches/bullet2/server/Pose3d.cc
code/gazebo/branches/bullet2/server/Vector3.cc
code/gazebo/branches/bullet2/server/sensors/ray/RaySensor.cc
Modified: code/gazebo/branches/bullet2/server/Entity.cc
===================================================================
--- code/gazebo/branches/bullet2/server/Entity.cc 2009-10-21 16:03:26 UTC
(rev 8321)
+++ code/gazebo/branches/bullet2/server/Entity.cc 2009-10-21 16:04:08 UTC
(rev 8322)
@@ -266,9 +266,16 @@
{
if (this->parent)
{
- Pose3d p = this->parent->GetAbsPose();
+ Pose3d p = this->relativePose + this->parent->GetAbsPose();
+
+ /*if (this->GetName() == "cylinder1_body")
+ printf("Get ABS: Rel[%4.2f %4.2f %4.2f] Parent[%4.2f %4.2f %4.2f]
Result[%4.2f %4.2f %4.2f]\n", this->relativePose.pos.x,
this->relativePose.pos.y, this->relativePose.pos.z,
this->parent->GetAbsPose().pos.x,this->parent->GetAbsPose().pos.y,this->parent->GetAbsPose().pos.z,
p.pos.x, p.pos.y, p.pos.z);
+ */
+
+ /*Pose3d p = this->parent->GetAbsPose();
p.pos += this->relativePose.pos;
p.rot *= this->relativePose.rot;
+ */
return p;
}
else
@@ -297,8 +304,20 @@
Pose3d p = pose;
if (this->parent)
{
- p.pos -= this->parent->GetAbsPose().pos;
+ p -= this->parent->GetAbsPose();
+ /*p.pos = p.pos - this->parent->GetAbsPose().pos;
p.rot = this->parent->GetAbsPose().rot.GetInverse() * p.rot;
+ */
+
+ /*if (this->GetName() == "cylinder1_body")
+ {
+ printf("SetABS: Cylinder Body Old[%4.2f %4.2f %4.2f] Set[%4.2f %4.2f
%4.2f] Parent[%4.2f %4.2f %4.2f] New[%4.2f %4.2f %4.2f]\n" ,
+ this->relativePose.pos.x,this->relativePose.pos.y,
this->relativePose.pos.z,
+ pose.pos.x, pose.pos.y,pose.pos.z,
+ this->parent->GetAbsPose().pos.x,this->parent->GetAbsPose().pos.y,
+ this->parent->GetAbsPose().pos.z,
+ p.pos.x, p.pos.y, p.pos.z);
+ }*/
}
this->SetRelativePose(p, notify);
}
Modified: code/gazebo/branches/bullet2/server/Pose3d.cc
===================================================================
--- code/gazebo/branches/bullet2/server/Pose3d.cc 2009-10-21 16:03:26 UTC
(rev 8321)
+++ code/gazebo/branches/bullet2/server/Pose3d.cc 2009-10-21 16:04:08 UTC
(rev 8322)
@@ -179,7 +179,7 @@
tmp.z = (this->pos - pose.pos).z;
tmp.u = 0.0;
- tmp = (this->rot.GetInverse() * tmp) * this->rot;
+ tmp = pose.rot.GetInverse() * (tmp * pose.rot);
result.x = tmp.x;
result.y = tmp.y;
Modified: code/gazebo/branches/bullet2/server/Vector3.cc
===================================================================
--- code/gazebo/branches/bullet2/server/Vector3.cc 2009-10-21 16:03:26 UTC
(rev 8321)
+++ code/gazebo/branches/bullet2/server/Vector3.cc 2009-10-21 16:04:08 UTC
(rev 8322)
@@ -84,9 +84,12 @@
{
double d = sqrt(this->x * this->x + this->y * this->y + this->z * this->z);
- this->x /= d;
- this->y /= d;
- this->z /= d;
+ if (d != 0.0)
+ {
+ this->x /= d;
+ this->y /= d;
+ this->z /= d;
+ }
}
////////////////////////////////////////////////////////////////////////////////
Modified: code/gazebo/branches/bullet2/server/sensors/ray/RaySensor.cc
===================================================================
--- code/gazebo/branches/bullet2/server/sensors/ray/RaySensor.cc
2009-10-21 16:03:26 UTC (rev 8321)
+++ code/gazebo/branches/bullet2/server/sensors/ray/RaySensor.cc
2009-10-21 16:04:08 UTC (rev 8322)
@@ -265,6 +265,6 @@
poseDelta = this->body->GetAbsPose() - this->prevPose;
this->prevPose = this->body->GetAbsPose();
- //this->laserShape->Update();
+ this->laserShape->Update();
}
}
This was sent by the SourceForge.net collaborative development platform, the
world's largest Open Source development site.
------------------------------------------------------------------------------
Come build with us! The BlackBerry(R) Developer Conference in SF, CA
is the only developer event you need to attend this year. Jumpstart your
developing skills, take BlackBerry mobile applications to market and stay
ahead of the curve. Join us from November 9 - 12, 2009. Register now!
http://p.sf.net/sfu/devconference
_______________________________________________
Playerstage-commit mailing list
Playerstage-commit@lists.sourceforge.net
https://lists.sourceforge.net/lists/listinfo/playerstage-commit
