From 875064c8514fd65f64c68f6d3092f4e0654ffeaf Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Wed, 27 May 2020 23:09:23 +0200 Subject: [PATCH] Check that the initial transforms when creating a body or a collider are valid --- .../reactphysics3d/mathematics/Quaternion.h | 28 +++++++++++++++++-- .../reactphysics3d/mathematics/Transform.h | 9 ++++++ include/reactphysics3d/mathematics/Vector2.h | 8 ++++++ include/reactphysics3d/mathematics/Vector3.h | 8 ++++++ src/body/CollisionBody.cpp | 7 +++++ src/body/RigidBody.cpp | 7 +++++ src/engine/PhysicsWorld.cpp | 14 ++++++++++ 7 files changed, 79 insertions(+), 2 deletions(-) diff --git a/include/reactphysics3d/mathematics/Quaternion.h b/include/reactphysics3d/mathematics/Quaternion.h index a6708b6d..fcb5baee 100644 --- a/include/reactphysics3d/mathematics/Quaternion.h +++ b/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 { diff --git a/include/reactphysics3d/mathematics/Transform.h b/include/reactphysics3d/mathematics/Transform.h index 97dd48c9..aca59848 100644 --- a/include/reactphysics3d/mathematics/Transform.h +++ b/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; diff --git a/include/reactphysics3d/mathematics/Vector2.h b/include/reactphysics3d/mathematics/Vector2.h index 13bba0ed..7a088398 100644 --- a/include/reactphysics3d/mathematics/Vector2.h +++ b/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); diff --git a/include/reactphysics3d/mathematics/Vector3.h b/include/reactphysics3d/mathematics/Vector3.h index 3dc481b5..ffec1b56 100644 --- a/include/reactphysics3d/mathematics/Vector3.h +++ b/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); diff --git a/src/body/CollisionBody.cpp b/src/body/CollisionBody.cpp index 11ad72b0..16279cf7 100644 --- a/src/body/CollisionBody.cpp +++ b/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); diff --git a/src/body/RigidBody.cpp b/src/body/RigidBody.cpp index de03b503..3ee7fca2 100644 --- a/src/body/RigidBody.cpp +++ b/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); diff --git a/src/engine/PhysicsWorld.cpp b/src/engine/PhysicsWorld.cpp index f1d3d816..b68528e4 100644 --- a/src/engine/PhysicsWorld.cpp +++ b/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