From beba85290c9378b6dda7d80d5d0051c54c3f17c0 Mon Sep 17 00:00:00 2001
From: Edouard DUPIN <yui.heero@gmail.com>
Date: Mon, 1 Apr 2019 22:07:43 +0200
Subject: [PATCH] [DEBUG] correct segmentation fault

---
 ephysics/body/CollisionBody.cpp | 18 ++++++++++++++++++
 ephysics/body/CollisionBody.hpp |  5 +----
 ephysics/body/RigidBody.cpp     | 18 ++++++++++++++++++
 ephysics/body/RigidBody.hpp     |  7 ++-----
 4 files changed, 39 insertions(+), 9 deletions(-)

diff --git a/ephysics/body/CollisionBody.cpp b/ephysics/body/CollisionBody.cpp
index b45f2a4..b1362a7 100644
--- a/ephysics/body/CollisionBody.cpp
+++ b/ephysics/body/CollisionBody.cpp
@@ -23,6 +23,13 @@ CollisionBody::CollisionBody(const etk::Transform3D& _transform, CollisionWorld&
   m_contactManifoldsList(null),
   m_world(_world) {
 	
+	EPHY_DEBUG("         set transform: " << _transform);
+	if (isnan(_transform.getPosition().x()) == true) { // check NAN
+		EPHY_CRITICAL("         set transform: " << _transform);
+	}
+	if (isinf(_transform.getOrientation().z()) == true) {
+		EPHY_CRITICAL("         set transform: " << _transform);
+	}
 }
 
 CollisionBody::~CollisionBody() {
@@ -40,6 +47,17 @@ inline void CollisionBody::setType(BodyType _type) {
 	}
 }
 
+void CollisionBody::setTransform(const etk::Transform3D& _transform) {
+	EPHY_DEBUG("         set transform: " << m_transform << " ==> " << _transform);
+	if (isnan(_transform.getPosition().x()) == true) { // check NAN
+		EPHY_CRITICAL("         set transform: " << m_transform << " ==> " << _transform);
+	}
+	if (isinf(_transform.getOrientation().z()) == true) {
+		EPHY_CRITICAL("         set transform: " << m_transform << " ==> " << _transform);
+	}
+	m_transform = _transform;
+	updateBroadPhaseState();
+}
 
 ProxyShape* CollisionBody::addCollisionShape(CollisionShape* _collisionShape,
                                              const etk::Transform3D& _transform) {
diff --git a/ephysics/body/CollisionBody.hpp b/ephysics/body/CollisionBody.hpp
index 27576d6..e1430b2 100644
--- a/ephysics/body/CollisionBody.hpp
+++ b/ephysics/body/CollisionBody.hpp
@@ -106,10 +106,7 @@ namespace ephysics {
 			 * @brief Set the current position and orientation
 			 * @param transform The transformation of the body that transforms the local-space of the body int32_to world-space
 			 */
-			void setTransform(const etk::Transform3D& _transform) {
-				m_transform = _transform;
-				updateBroadPhaseState();
-			}
+			virtual void setTransform(const etk::Transform3D& _transform);
 			/**
 			 * @brief Add a collision shape to the body. Note that you can share a collision shape between several bodies using the same collision shape instance to
 			 * when you add the shape to the different bodies. Do not forget to delete the collision shape you have created at the end of your program.
diff --git a/ephysics/body/RigidBody.cpp b/ephysics/body/RigidBody.cpp
index a0f6582..2ae9832 100644
--- a/ephysics/body/RigidBody.cpp
+++ b/ephysics/body/RigidBody.cpp
@@ -182,8 +182,25 @@ void RigidBody::setIsSleeping(bool _isSleeping) {
 	Body::setIsSleeping(_isSleeping);
 }
 
+void RigidBody::updateTransformWithCenterOfMass() {
+	// Translate the body according to the translation of the center of mass position
+	m_transform.setPosition(m_centerOfMassWorld - m_transform.getOrientation() * m_centerOfMassLocal);
+	if (isnan(m_transform.getPosition().x()) == true) {
+		EPHY_CRITICAL("updateTransformWithCenterOfMass: " << m_transform);
+	}
+	if (isinf(m_transform.getOrientation().z()) == true) {
+		EPHY_CRITICAL("         set transform: " << m_transform);
+	}
+}
 
 void RigidBody::setTransform(const etk::Transform3D& _transform) {
+	EPHY_DEBUG("         set transform: " << m_transform << " ==> " << _transform);
+	if (isnan(_transform.getPosition().x()) == true) {
+		EPHY_CRITICAL("         set transform: " << m_transform << " ==> " << _transform);
+	}
+	if (isinf(_transform.getOrientation().z()) == true) {
+		EPHY_CRITICAL("         set transform: " << m_transform << " ==> " << _transform);
+	}
 	m_transform = _transform;
 	const vec3 oldCenterOfMass = m_centerOfMassWorld;
 	// Compute the new center of mass in world-space coordinates
@@ -258,6 +275,7 @@ void RigidBody::updateBroadPhaseState() const {
 		// Recompute the world-space AABB of the collision shape
 		AABB aabb;
 		EPHY_VERBOSE("         : " << aabb.getMin() << " " << aabb.getMax());
+		EPHY_VERBOSE("         m_transform: " << m_transform);
 		shape->getCollisionShape()->computeAABB(aabb, m_transform *shape->getLocalToBodyTransform());
 		EPHY_VERBOSE("         : " << aabb.getMin() << " " << aabb.getMax());
 		// Update the broad-phase state for the proxy collision shape
diff --git a/ephysics/body/RigidBody.hpp b/ephysics/body/RigidBody.hpp
index f4f27fd..201b9f3 100644
--- a/ephysics/body/RigidBody.hpp
+++ b/ephysics/body/RigidBody.hpp
@@ -53,10 +53,7 @@ namespace ephysics {
 			/**
 			 * @brief Update the transform of the body after a change of the center of mass
 			 */
-			void updateTransformWithCenterOfMass() {
-				// Translate the body according to the translation of the center of mass position
-				m_transform.setPosition(m_centerOfMassWorld - m_transform.getOrientation() * m_centerOfMassLocal);
-			}
+			void updateTransformWithCenterOfMass();
 			void updateBroadPhaseState() const override;
 		public :
 			/**
@@ -75,7 +72,7 @@ namespace ephysics {
 			 * @brief Set the current position and orientation
 			 * @param[in] _transform The transformation of the body that transforms the local-space of the body int32_to world-space
 			 */
-			virtual void setTransform(const etk::Transform3D& _transform);
+			void setTransform(const etk::Transform3D& _transform) override;
 			/**
 			 * @brief Get the mass of the body
 			 * @return The mass (in kilograms) of the body