Revision: 8852
http://playerstage.svn.sourceforge.net/playerstage/?rev=8852&view=rev
Author: hsujohnhsu
Date: 2010-08-12 18:29:55 +0000 (Thu, 12 Aug 2010)
Log Message:
-----------
fix a possible divide by zero error and possible asin(>1) error in Quatern
Modified Paths:
--------------
code/gazebo/trunk/server/Quatern.cc
Modified: code/gazebo/trunk/server/Quatern.cc
===================================================================
--- code/gazebo/trunk/server/Quatern.cc 2010-08-12 18:29:10 UTC (rev 8851)
+++ code/gazebo/trunk/server/Quatern.cc 2010-08-12 18:29:55 UTC (rev 8852)
@@ -126,10 +126,20 @@
s = sqrt(this->u * this->u + this->x * this->x + this->y * this->y + this->z
* this->z);
- this->u /= s;
- this->x /= s;
- this->y /= s;
- this->z /= s;
+ if (s == 0)
+ {
+ this->u = 0.0;
+ this->x = 0.0;
+ this->y = 0.0;
+ this->z = 1.0;
+ }
+ else
+ {
+ this->u /= s;
+ this->x /= s;
+ this->y /= s;
+ this->z /= s;
+ }
}
////////////////////////////////////////////////////////////////////////////////
@@ -212,7 +222,8 @@
vec.x = atan2(2 * (this->y*this->z + this->u*this->x), squ - sqx - sqy +
sqz);
// Pitch
- vec.y = asin(-2 * (this->x*this->z - this->u * this->y));
+ double sarg = -2 * (this->x*this->z - this->u * this->y);
+ vec.y = sarg <= -1.0 ? -0.5*M_PI : (sarg >= 1.0 ? 0.5*M_PI : asin(sarg));
// Yaw
vec.z = atan2(2 * (this->x*this->y + this->u*this->z), squ + sqx - sqy -
sqz);
This was sent by the SourceForge.net collaborative development platform, the
world's largest Open Source development site.
------------------------------------------------------------------------------
This SF.net email is sponsored by
Make an app they can't live without
Enter the BlackBerry Developer Challenge
http://p.sf.net/sfu/RIM-dev2dev
_______________________________________________
Playerstage-commit mailing list
[email protected]
https://lists.sourceforge.net/lists/listinfo/playerstage-commit
