Fix normalization error

Refer to resibots/robdyn#3
bitsandscraps · May 20, 2019 · a19a25e · a19a25e
1 parent 54b44c8
commit a19a25e
Showing 1 changed file with 4 additions and 0 deletions.
diff --git a/src/ode/ode/src/odemath.h b/src/ode/ode/src/odemath.h
@@ -32,12 +32,16 @@ int  _dSafeNormalize4 (dVector4 a);
 ODE_PURE_INLINE void _dNormalize3(dVector3 a)
 {
     int bNormalizationResult = _dSafeNormalize3(a);
+    if(bNormalizationResult==false)
+        throw(3);
     dIVERIFY(bNormalizationResult);
 }
 
 ODE_PURE_INLINE void _dNormalize4(dVector4 a)
 {
     int bNormalizationResult = _dSafeNormalize4(a);
+    if(bNormalizationResult==false)
+        throw(4);
     dIVERIFY(bNormalizationResult);
 }