diff --git a/sources/reactphysics3d/body/OBB.cpp b/sources/reactphysics3d/body/OBB.cpp index fabfa134..b96c368c 100644 --- a/sources/reactphysics3d/body/OBB.cpp +++ b/sources/reactphysics3d/body/OBB.cpp @@ -120,6 +120,18 @@ std::vector OBB::getExtremeVertices(const Vector3D& directionAxis) con std::vector extremeVertices; + // TODO : Delete this + std::cout << "------------------------------------" << std::endl; + for (int i=0; i<8; i++) + { + Vector3D vertex = getVertex(i); + + // Compute the projection length of the current vertex onto the projection axis + double projectionLength = directionAxis.scalarProduct(vertex-center) / directionAxis.length(); + std::cout << "Point : x=" << vertex.getX() << " y=" << vertex.getY() << "z=" << vertex.getZ() << std::endl; + std::cout << "projection length = " << projectionLength << std::endl; + } + // Check if the given axis is parallel to an axis on the OBB if (axis[0].isParallelWith(directionAxis)) { if (axis[0].scalarProduct(directionAxis) >= 0) { // If both axis are in the same direction @@ -226,3 +238,30 @@ std::vector OBB::getFace(unsigned int index) const throw(std::invalid_ throw std::invalid_argument("Exception: The argument must be between 0 and 5"); } } + +// Return the axis that correspond the better to the vector +Vector3D OBB::getBestAxis(const Vector3D& vector) const { + double vectorLength = vector.length(); + double minDifference = DBL_MAX; + int bestAxis = -1; + bool opposite = false; + + for (int i=0; i<3; i++) { + double scalarProd = axis[i].scalarProduct(vector); + double lengthValue = axis[i].length() * vectorLength; + + if (std::abs(std::abs(scalarProd) - lengthValue) < minDifference) { + bestAxis = i; + minDifference = std::abs(std::abs(scalarProd) - lengthValue); + + if (scalarProd >= 0) { + opposite = false; + } + else { + opposite = true; + } + } + } + + return opposite ? axis[bestAxis].getOpposite() : axis[bestAxis]; +} diff --git a/sources/reactphysics3d/body/OBB.h b/sources/reactphysics3d/body/OBB.h index 675f1cd1..f4695fef 100644 --- a/sources/reactphysics3d/body/OBB.h +++ b/sources/reactphysics3d/body/OBB.h @@ -21,6 +21,7 @@ #define OBB_H // Libraries +#include #include "BoundingVolume.h" #include "../mathematics/mathematics.h" @@ -60,6 +61,7 @@ class OBB : public BoundingVolume { virtual std::vector getExtremeVertices(const Vector3D& axis) const; // Return all the vertices that are projected at the extreme of the projection of the bouding volume on the axis virtual void updateOrientation(const Vector3D& newCenter, const Quaternion& rotationQuaternion); // Update the oriented bounding box orientation according to a new orientation of the rigid body virtual void draw() const; // Draw the OBB (only for testing purpose) + Vector3D getBestAxis(const Vector3D& vector) const; // Return the axis that correspond the better to the vector }; // Return the center point of the OBB @@ -167,7 +169,6 @@ inline void OBB::updateOrientation(const Vector3D& newCenter, const Quaternion& axis[2] = rotateVectorWithQuaternion(oldAxis[2], rotationQuaternion).getUnit(); } - }; // End of the ReactPhysics3D namespace #endif