Merge pull request #26 from florent-lamiraux/master

Check that quaternion is normalized when converting to rotation matrix

src/math/transform.cpp

@@ -85,6 +85,10 @@ void Quaternion3f::fromRotation(const Matrix3f& R)
 
 void Quaternion3f::toRotation(Matrix3f& R) const
 {
+  assert (.99 < data [0]*data [0] + data [1]*data [1] +
+	  data [2]*data [2] + data [3]*data [3]);
+  assert (data [0]*data [0] + data [1]*data [1] +
+	  data [2]*data [2] + data [3]*data [3] < 1.01);
   FCL_REAL twoX  = 2.0*data[1];
   FCL_REAL twoY  = 2.0*data[2];
   FCL_REAL twoZ  = 2.0*data[3];