Check that the initial transforms when creating a body or a collider are valid

include/reactphysics3d/mathematics/Quaternion.h

@@ -133,9 +133,17 @@ struct Quaternion {
         /// Compute the rotation angle (in radians) and the rotation axis
         void getRotationAngleAxis(decimal& angle, Vector3& axis) const;
 
+        /// Return true if the values are not NAN OR INF
+        bool isFinite() const;
+
+        /// Return true if it is a unit quaternion
+        bool isUnit() const;
+
+        /// Return true if it is a valid quaternion
+        bool isValid() const;
+
         /// Compute the spherical linear interpolation between two quaternions
-        static Quaternion slerp(const Quaternion& quaternion1, const Quaternion& quaternion2,
-                                decimal t);
+        static Quaternion slerp(const Quaternion& quaternion1, const Quaternion& quaternion2, decimal t);
 
         /// Overloaded operator for the addition
         Quaternion operator+(const Quaternion& quaternion) const;
@@ -292,6 +300,22 @@ inline decimal Quaternion::dot(const Quaternion& quaternion) const {
     return (x*quaternion.x + y*quaternion.y + z*quaternion.z + w*quaternion.w);
 }
 
+// Return true if the values are not NAN OR INF
+inline bool Quaternion::isFinite() const {
+    return std::isfinite(x) && std::isfinite(y) && std::isfinite(z) && std::isfinite(w);
+}
+
+// Return true if it is a unit quaternion
+inline bool Quaternion::isUnit() const {
+    decimal length = std::sqrt(x*x + y*y + z*z + w*w);
+    return std::abs(length - decimal(1.0)) < MACHINE_EPSILON;
+}
+
+// Return true if it is a valid quaternion
+inline bool Quaternion::isValid() const {
+   return isFinite() && isUnit();
+}
+
 // Overloaded operator for the addition of two quaternions
 inline Quaternion Quaternion::operator+(const Quaternion& quaternion) const {
 

include/reactphysics3d/mathematics/Transform.h

@@ -101,6 +101,9 @@ class Transform {
         /// Return the identity transform
         static Transform identity();
 
+        /// Return true if it is a valid transform
+        bool isValid() const;
+
         /// Return the transformed vector
         Vector3 operator*(const Vector3& vector) const;
 
@@ -118,6 +121,7 @@ class Transform {
 
         /// Return the string representation
         std::string to_string() const;
+
 };
 
 // Constructor
@@ -195,6 +199,11 @@ inline Transform Transform::identity() {
     return Transform(Vector3(0, 0, 0), Quaternion::identity());
 }
 
+// Return true if it is a valid transform
+inline bool Transform::isValid() const {
+    return mPosition.isFinite() && mOrientation.isValid();
+}
+
 // Return the transformed vector
 inline Vector3 Transform::operator*(const Vector3& vector) const {
     return (mOrientation * vector) + mPosition;

include/reactphysics3d/mathematics/Vector2.h

@@ -85,6 +85,9 @@ struct Vector2 {
         /// Return true if the vector is unit and false otherwise
         bool isUnit() const;
 
+        /// Return true if the values are not NAN OR INF
+        bool isFinite() const;
+
         /// Return true if the current vector is the zero vector
         bool isZero() const;
 
@@ -230,6 +233,11 @@ inline bool Vector2::isUnit() const {
     return approxEqual(lengthSquare(), 1.0);
 }
 
+// Return true if the values are not NAN OR INF
+inline bool Vector2::isFinite() const {
+    return std::isfinite(x) && std::isfinite(y);
+}
+
 // Return true if the vector is the zero vector
 inline bool Vector2::isZero() const {
     return approxEqual(lengthSquare(), 0.0);

include/reactphysics3d/mathematics/Vector3.h

@@ -88,6 +88,9 @@ struct Vector3 {
         /// Return true if the vector is unit and false otherwise
         bool isUnit() const;
 
+        /// Return true if the values are not NAN OR INF
+        bool isFinite() const;
+
         /// Return true if the current vector is the zero vector
         bool isZero() const;
 
@@ -251,6 +254,11 @@ inline bool Vector3::isUnit() const {
     return approxEqual(lengthSquare(), 1.0);
 }
 
+// Return true if the values are not NAN OR INF
+inline bool Vector3::isFinite() const {
+    return std::isfinite(x) && std::isfinite(y) && std::isfinite(z);
+}
+
 // Return true if the vector is the zero vector
 inline bool Vector3::isZero() const {
     return approxEqual(lengthSquare(), 0.0);

src/body/CollisionBody.cpp

@@ -72,6 +72,13 @@ Collider* CollisionBody::addCollider(CollisionShape* collisionShape, const Trans
     // Create a new entity for the collider
     Entity colliderEntity = mWorld.mEntityManager.createEntity();
 
+    // Check that the transform is valid
+    if (!transform.isValid()) {
+        RP3D_LOG(mWorld.mConfig.worldName, Logger::Level::Error, Logger::Category::Collider,
+                 "Error when adding a collider: the init transform is not valid",  __FILE__, __LINE__);
+    }
+    assert(transform.isValid());
+
     // Create a new collider to attach the collision shape to the body
     Collider* collider = new (mWorld.mMemoryManager.allocate(MemoryManager::AllocationType::Pool,
                                       sizeof(Collider))) Collider(colliderEntity, this, mWorld.mMemoryManager);

src/body/RigidBody.cpp

@@ -572,6 +572,13 @@ Collider* RigidBody::addCollider(CollisionShape* collisionShape, const Transform
     // Create a new entity for the collider
     Entity colliderEntity = mWorld.mEntityManager.createEntity();
 
+    // Check that the transform is valid
+    if (!transform.isValid()) {
+        RP3D_LOG(mWorld.mConfig.worldName, Logger::Level::Error, Logger::Category::Collider,
+                 "Error when adding a collider: the init transform is not valid",  __FILE__, __LINE__);
+    }
+    assert(transform.isValid());
+
     // Create a new collider for the body
     Collider* collider = new (mWorld.mMemoryManager.allocate(MemoryManager::AllocationType::Pool,
                                       sizeof(Collider))) Collider(colliderEntity, this, mWorld.mMemoryManager);

src/engine/PhysicsWorld.cpp

@@ -161,6 +161,13 @@ CollisionBody* PhysicsWorld::createCollisionBody(const Transform& transform) {
     // Create a new entity for the body
     Entity entity = mEntityManager.createEntity();
 
+    // Check that the transform is valid
+    if (!transform.isValid()) {
+        RP3D_LOG(mConfig.worldName, Logger::Level::Error, Logger::Category::Body,
+                 "Error when creating a collision body: the init transform is not valid",  __FILE__, __LINE__);
+    }
+    assert(transform.isValid());
+
     mTransformComponents.addComponent(entity, false, TransformComponents::TransformComponent(transform));
 
     // Create the collision body
@@ -447,6 +454,13 @@ RigidBody* PhysicsWorld::createRigidBody(const Transform& transform) {
     // Create a new entity for the body
     Entity entity = mEntityManager.createEntity();
 
+    // Check that the transform is valid
+    if (!transform.isValid()) {
+        RP3D_LOG(mConfig.worldName, Logger::Level::Error, Logger::Category::Body,
+                 "Error when creating a rigid body: the init transform is not valid",  __FILE__, __LINE__);
+    }
+    assert(transform.isValid());
+
     mTransformComponents.addComponent(entity, false, TransformComponents::TransformComponent(transform));
 
     // Create the rigid body