From 89be14e58b8925d19affbe534317ea7c508e5ccb Mon Sep 17 00:00:00 2001
From: hsu <devnull@localhost>
Date: Mon, 12 Jul 2010 22:04:37 +0000
Subject: [PATCH] fix NaN on normalize for ticket #4247

---
 urdf/include/urdf/pose.h | 18 ++++++++++++++----
 1 file changed, 14 insertions(+), 4 deletions(-)

diff --git a/urdf/include/urdf/pose.h b/urdf/include/urdf/pose.h
index 48e57ab..89e0ec9 100644
--- a/urdf/include/urdf/pose.h
+++ b/urdf/include/urdf/pose.h
@@ -168,10 +168,20 @@ public:
                     this->y * this->y +
                     this->z * this->z +
                     this->w * this->w);
-    this->x /= s;
-    this->y /= s;
-    this->z /= s;
-    this->w /= s;
+    if (s == 0.0)
+    {
+      this->x = 0.0;
+      this->y = 0.0;
+      this->z = 0.0;
+      this->w = 1.0;
+    }
+    else
+    {
+      this->x /= s;
+      this->y /= s;
+      this->z /= s;
+      this->w /= s;
+    }
   };
 };