From 90fff0caff869c18efa232585775dae410f545ad Mon Sep 17 00:00:00 2001
From: Edouard DUPIN <yui.heero@gmail.com>
Date: Fri, 16 Jun 2017 20:43:14 +0200
Subject: [PATCH] [DEV] update some functions

---
 etk/math/Quaternion.hpp | 29 +++++++++++++++++++++++++++++
 etk/math/Vector3D.hpp   | 26 ++++++++++++++++++++++++++
 2 files changed, 55 insertions(+)

diff --git a/etk/math/Quaternion.hpp b/etk/math/Quaternion.hpp
index 87007ff..c3ece39 100644
--- a/etk/math/Quaternion.hpp
+++ b/etk/math/Quaternion.hpp
@@ -234,6 +234,35 @@ namespace etk {
 				tmp.normalize();
 				return tmp;
 			}
+			/**
+			 * @brief Normalize this quaternion x^2 + y^2 + z^2 + w^2 = 1
+			 * @return Local reference of the quaternion normalized
+			 */
+			Quaternion& safeNormalize() {
+				float lengthTmp = length();
+				if (lengthTmp == 0.0f) {
+					m_floats[0] = 0.0f;
+					m_floats[1] = 0.0f;
+					m_floats[2] = 0.0f;
+					m_floats[3] = 1.0f;
+					return *this;
+				}
+				float invLength = 1.0f / lengthTmp;
+				m_floats[0] *= invLength;
+				m_floats[1] *= invLength;
+				m_floats[2] *= invLength;
+				m_floats[3] *= invLength;
+				return *this;
+			}
+			/**
+			 * @brief Return a normalized version of this quaternion
+			 * @return New quaternion containing the value
+			 */
+			Quaternion safeNormalized() const {
+				etk::Quaternion tmp = *this;
+				tmp.normalized();
+				return tmp;
+			}
 			/**
 			 * @brief Return a quaternion will the absolute values of each element
 			 * @return New quaternion with the absolute value
diff --git a/etk/math/Vector3D.hpp b/etk/math/Vector3D.hpp
index 1517961..d48a901 100644
--- a/etk/math/Vector3D.hpp
+++ b/etk/math/Vector3D.hpp
@@ -36,6 +36,7 @@ namespace etk {
 			 * @brief No initialization constructor (faster ...)
 			 */
 			Vector3D() {
+				#if 0
 				#ifdef DEBUG
 					// in debug mode we set supid value to prevent forget of the inits ...
 					m_floats[0] = (T)34673363;
@@ -49,6 +50,13 @@ namespace etk {
 					// hide a bullet warning
 					(void)btInfinityMask;
 				#endif
+				#else
+				m_floats[0] = 0;
+				m_floats[1] = 0;
+				m_floats[2] = 0;
+				m_floats[3] = 0;
+				#endif
+				
 			}
 			/**
 			 * @brief Constructor from scalars 
@@ -551,6 +559,24 @@ namespace etk {
 			int32_t getMaxAxis() const {
 				return (m_floats[0] < m_floats[1] ? (m_floats[1] < m_floats[2] ? 2 : 1) : (m_floats[0] < m_floats[2] ? 2 : 0));
 			}
+			/**
+			 * @breif Get the orthogonalm vector of the current vector
+			 * @return The ortho vector
+			 */
+			Vector3D<T> getOrthoVector() const {
+				Vector3D<T> vectorAbs(std::abs(m_floats[0]), std::abs(m_floats[1]), std::abs(m_floats[2]));
+				int32_t minElement = vectorAbs.getMinAxis();
+				if (minElement == 0) {
+					float devider = 1.0f / std::sqrt(m_floats[1]*m_floats[1] + m_floats[2]*m_floats[2]);
+					return Vector3D<T>(0.0f, -m_floats[2]*devider, m_floats[1]*devider);
+				} else if (minElement == 1) {
+					float devider = 1.0f / std::sqrt(m_floats[0]*m_floats[0] + m_floats[2]*m_floats[2]);
+					return Vector3D<T>(-m_floats[2]*devider, 0.0f, m_floats[0]*devider);
+				}
+					float devider = 1.0f / std::sqrt(m_floats[0]*m_floats[0] + m_floats[1]*m_floats[1]);
+				return Vector3D<T>(-m_floats[1]*devider, m_floats[0]*devider, 0.0f);
+			}
+
 	};
 	//! @not_in_doc
 	std::ostream& operator <<(std::ostream& _os, const etk::Vector3D<float>& _obj);