diff --git a/CHANGELOG.md b/CHANGELOG.md index 6c0b4267..a7a36e30 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -7,19 +7,31 @@ do not hesitate to take a look at the user manual. ### Added - - Method RigidBody::resetForce() to reset the accumulated external force on a rigid body has beend added - - Method RigidBody::resetTorque() to reset the accumulated external torque on a rigid body has beend added + - Method RigidBody::resetForce() to reset the accumulated external force on a rigid body has been added + - Method RigidBody::resetTorque() to reset the accumulated external torque on a rigid body has been added - Constructors with local-space anchor/axis have been added to BallAndSocketJointInfo, HingeJointInfo, FixedJointInfo and SliderJointInfo classes - Robustness of polyhedron vs polyhedron collision detection has been improved in SAT algorithm (face contacts are favored over edge-edge contacts for better stability) + - Method HingeJoint::getAngle() to get the current angle of the hinge joint has been added ### Changed - The PhysicsWorld::setGravity() method now takes a const parameter + - Rolling resistance constraint is not solved anymore in the solver. Angular damping needs to be used instead to simulate this + - The List class has been renamed to Array + - The default number of iterations for the velocity solver is now 6 instead of 10 + - The default number of iterations for the position solver is now 3 instead of 5 + - The raycasting broad-phase performance has been improved + - The raycasting performance against HeighFieldShape has been improved (better middle-phase algorithm) ### Removed + - Method Material::getRollingResistance() has been removed (angular damping has to be used instead of rolling resistance) + - Method Material::setRollingResistance() has been removed (angular damping has to be used instead of rolling resistance) + ### Fixed +- Issue [#165](https://github.com/DanielChappuis/reactphysics3d/issues/165) with order of contact manifolds in islands creation has been fixed +- Issue [#179](https://github.com/DanielChappuis/reactphysics3d/issues/179) with FixedJoint constraint - Issue with concave vs convex shape collision detection has been fixed - Issue with edge vs edge collision has been fixed in SAT algorithm (wrong contact normal was computed) - Issue with sphere radius in DebugRenderer diff --git a/CMakeLists.txt b/CMakeLists.txt index 11cbd844..6688ba44 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -63,9 +63,6 @@ set (REACTPHYSICS3D_HEADERS "include/reactphysics3d/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.h" "include/reactphysics3d/collision/narrowphase/NarrowPhaseInput.h" "include/reactphysics3d/collision/narrowphase/NarrowPhaseInfoBatch.h" - "include/reactphysics3d/collision/narrowphase/SphereVsSphereNarrowPhaseInfoBatch.h" - "include/reactphysics3d/collision/narrowphase/CapsuleVsCapsuleNarrowPhaseInfoBatch.h" - "include/reactphysics3d/collision/narrowphase/SphereVsCapsuleNarrowPhaseInfoBatch.h" "include/reactphysics3d/collision/shapes/AABB.h" "include/reactphysics3d/collision/shapes/ConvexShape.h" "include/reactphysics3d/collision/shapes/ConvexPolyhedronShape.h" @@ -124,6 +121,7 @@ set (REACTPHYSICS3D_HEADERS "include/reactphysics3d/collision/CollisionCallback.h" "include/reactphysics3d/collision/OverlapCallback.h" "include/reactphysics3d/mathematics/mathematics.h" + "include/reactphysics3d/mathematics/mathematics_common.h" "include/reactphysics3d/mathematics/mathematics_functions.h" "include/reactphysics3d/mathematics/Matrix2x2.h" "include/reactphysics3d/mathematics/Matrix3x3.h" @@ -140,7 +138,7 @@ set (REACTPHYSICS3D_HEADERS "include/reactphysics3d/memory/MemoryManager.h" "include/reactphysics3d/containers/Stack.h" "include/reactphysics3d/containers/LinkedList.h" - "include/reactphysics3d/containers/List.h" + "include/reactphysics3d/containers/Array.h" "include/reactphysics3d/containers/Map.h" "include/reactphysics3d/containers/Set.h" "include/reactphysics3d/containers/Pair.h" @@ -168,9 +166,6 @@ set (REACTPHYSICS3D_SOURCES "src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.cpp" "src/collision/narrowphase/NarrowPhaseInput.cpp" "src/collision/narrowphase/NarrowPhaseInfoBatch.cpp" - "src/collision/narrowphase/SphereVsSphereNarrowPhaseInfoBatch.cpp" - "src/collision/narrowphase/CapsuleVsCapsuleNarrowPhaseInfoBatch.cpp" - "src/collision/narrowphase/SphereVsCapsuleNarrowPhaseInfoBatch.cpp" "src/collision/shapes/AABB.cpp" "src/collision/shapes/ConvexShape.cpp" "src/collision/shapes/ConvexPolyhedronShape.cpp" @@ -226,7 +221,6 @@ set (REACTPHYSICS3D_SOURCES "src/components/SliderJointComponents.cpp" "src/collision/CollisionCallback.cpp" "src/collision/OverlapCallback.cpp" - "src/mathematics/mathematics_functions.cpp" "src/mathematics/Matrix2x2.cpp" "src/mathematics/Matrix3x3.cpp" "src/mathematics/Quaternion.cpp" diff --git a/documentation/UserManual/ReactPhysics3D-UserManual.pdf b/documentation/UserManual/ReactPhysics3D-UserManual.pdf index f98fe369..fdff5ec6 100644 Binary files a/documentation/UserManual/ReactPhysics3D-UserManual.pdf and b/documentation/UserManual/ReactPhysics3D-UserManual.pdf differ diff --git a/documentation/UserManual/ReactPhysics3D-UserManual.tex b/documentation/UserManual/ReactPhysics3D-UserManual.tex index 43f83c90..f18e9ee9 100644 --- a/documentation/UserManual/ReactPhysics3D-UserManual.tex +++ b/documentation/UserManual/ReactPhysics3D-UserManual.tex @@ -1201,7 +1201,7 @@ indices[16]=2; indices[17]=3; indices[18]=7; indices[19]=6; indices[20]=0; indices[21]=4; indices[22]=7; indices[23]=3; // Description of the six faces of the convex mesh -polygonFaces = new PolygonVertexArray::PolygonFace[6]; +PolygonVertexArray::PolygonFace* polygonFaces = new PolygonVertexArray::PolygonFace[6]; PolygonVertexArray::PolygonFace* face = polygonFaces; for (int f = 0; f < 6; f++) { @@ -1236,6 +1236,9 @@ ConvexMeshShape* convexMeshShape = physicsCommon.createConvexMeshShape(polyhedro that you need to avoid coplanar faces in your convex mesh shape. Coplanar faces have to be merged together. Remember that convex meshes are not limited to triangular faces, you can create faces with more than three vertices. \\ + Also note that meshes with duplicated vertices are not supported. The number of vertices you pass to create the PolygonVertexArray must be exactly + the number of vertices in your convex mesh. \\ + When you specify the vertices for each face of your convex mesh, be careful with their order. The vertices of a face must be specified in counter clockwise order as seen from the outside of your convex mesh. \\ @@ -1316,8 +1319,11 @@ ConcaveMeshShape* concaveMesh = physicsCommon.createConcaveMeshShape(triangleMes \texttt{TriangleMesh} for multiple \texttt{ConcaveMeshShape} with a different scaling factor each time. \\ \end{sloppypar} - In the previous example, the vertex normals that are needed for collision detection are automatically computed. However, if you want to specify your own - vertex normals, you can do it by using another constructor for the \texttt{TriangleVertexArray}. \\ + In the previous example, the vertices normals that are needed for collision detection are automatically computed. However, you can specify your own + vertices normals by using another constructor for the \texttt{TriangleVertexArray}. Note that each vertex normal is computed as weighted average + of the face normals of all the neighboring triangle faces. Therefore, if you specify your mesh with duplicated vertices when you create the + \emph{TriangleVertexArray}, the automatic vertices normals computation will not give correct normals because each vertex of the mesh will only be + part of a single triangle face. In this case, you should provide your own vertices normals when you create the \emph{TriangleVertexArray}. \\ \subsubsection{Heightfield Shape} @@ -1440,14 +1446,13 @@ colliderBody4->setCollideWithMaskBits(CATEGORY2); \subsection{Material} \label{sec:material} - The material of a rigid body is used to describe the physical properties it is made of. This is represented by the \texttt{Material} class. Each body that - you create will have a default material. You can get the material of the rigid body using the \texttt{RigidBody::\allowbreak getMaterial()} method. \\ + The material of a collider is used to describe the physical properties it is made of. This is represented by the \texttt{Material} class. Each collider that you create will have a default material. You can get the material of the collider using the \texttt{Collider::\allowbreak getMaterial()} method. \\ You can use the material to set those physical properties. \\ - For instance, you can change the bounciness of the rigid body. The bounciness is a value between 0 and 1. The value 1 is used for a very bouncy - object and the value 0 means that the body will not be bouncy at all. To change the bounciness of the material, you can use the - \texttt{Material::\allowbreak setBounciness()} method. \\ + For instance, you can change the bounciness of the collider. The bounciness is a value between 0 and 1. The value 1 is used for + a very bouncy object and the value 0 means that the collider will not be bouncy at all. To change the bounciness of the material, + you can use the \texttt{Material::\allowbreak setBounciness()} method. \\ \begin{sloppypar} It is also possible to set the mass density of the collider which has a default value of 1. As described in section \ref{sec:rigidbodymass}, the @@ -1455,25 +1460,20 @@ colliderBody4->setCollideWithMaskBits(CATEGORY2); mass density of a collider, you need to use the \texttt{Material::setMassDensity()} method. \\ \end{sloppypar} - You are also able to change the friction coefficient of the body. This value needs to be between 0 and 1. If the value is 0, no friction will be - applied when the body is in contact with another body. However, if the value is 1, the friction force will be high. You can change the - friction coefficient of the material with the \texttt{Material::\allowbreak setFrictionCoefficient()} method. \\ + You are also able to change the friction coefficient of the collider. This value needs to be between 0 and 1. If the value is 0, + no friction will be applied when the collider is in contact with another collider. However, if the value is 1, the friction force will be high. You can + change the friction coefficient of the material with the \texttt{Material::\allowbreak setFrictionCoefficient()} method. \\ - You can use the material to add rolling resistance to a rigid body. Rolling resistance can be used to stop - a rolling object on a flat surface for instance. You should use this only with SphereShape or - CapsuleShape collision shapes. By default, rolling resistance is zero but you can - set a positive value using the \texttt{Material::\allowbreak setRollingResistance()} method to increase resistance. \\ - - Here is how to get the material of a rigid body and how to modify some of its properties: \\ + Here is how to get the material of a collider and how to modify some of its properties: \\ \begin{lstlisting} -// Get the current material of the body -Material& material = rigidBody->getMaterial(); +// Get the current material of the collider +Material& material = collider->getMaterial(); -// Change the bounciness of the body +// Change the bounciness of the collider material.setBounciness(0.4); -// Change the friction coefficient of the body +// Change the friction coefficient of the collider material.setFrictionCoefficient(0.2); \end{lstlisting} diff --git a/include/reactphysics3d/body/CollisionBody.h b/include/reactphysics3d/body/CollisionBody.h index 9b77bec6..41d7923c 100644 --- a/include/reactphysics3d/body/CollisionBody.h +++ b/include/reactphysics3d/body/CollisionBody.h @@ -143,7 +143,7 @@ class CollisionBody { Collider* getCollider(uint colliderIndex); /// Return the number of colliders associated with this body - uint getNbColliders() const; + uint32 getNbColliders() const; /// Return the world-space coordinates of a point given the local-space coordinates of the body Vector3 getWorldPoint(const Vector3& localPoint) const; @@ -178,7 +178,7 @@ class CollisionBody { * @param worldAABB The AABB (in world-space coordinates) that will be used to test overlap * @return True if the given AABB overlaps with the AABB of the collision body */ -inline bool CollisionBody::testAABBOverlap(const AABB& worldAABB) const { +RP3D_FORCE_INLINE bool CollisionBody::testAABBOverlap(const AABB& worldAABB) const { return worldAABB.testCollision(getAABB()); } @@ -186,14 +186,14 @@ inline bool CollisionBody::testAABBOverlap(const AABB& worldAABB) const { /** * @return The entity of the body */ -inline Entity CollisionBody::getEntity() const { +RP3D_FORCE_INLINE Entity CollisionBody::getEntity() const { return mEntity; } #ifdef IS_RP3D_PROFILING_ENABLED // Set the profiler -inline void CollisionBody::setProfiler(Profiler* profiler) { +RP3D_FORCE_INLINE void CollisionBody::setProfiler(Profiler* profiler) { mProfiler = profiler; } diff --git a/include/reactphysics3d/body/RigidBody.h b/include/reactphysics3d/body/RigidBody.h index bc2353e0..3e262dcb 100644 --- a/include/reactphysics3d/body/RigidBody.h +++ b/include/reactphysics3d/body/RigidBody.h @@ -57,7 +57,7 @@ class RigidBody : public CollisionBody { void setIsSleeping(bool isSleeping); /// Update whether the current overlapping pairs where this body is involed are active or not - void updateOverlappingPairs(); + void resetOverlappingPairs(); /// Compute and return the local-space center of mass of the body using its colliders Vector3 computeCenterOfMass() const; @@ -65,8 +65,8 @@ class RigidBody : public CollisionBody { /// Compute the local-space inertia tensor and total mass of the body using its colliders void computeMassAndInertiaTensorLocal(Vector3& inertiaTensorLocal, decimal& totalMass) const; - /// Return the inverse of the inertia tensor in world coordinates. - static const Matrix3x3 getWorldInertiaTensorInverse(PhysicsWorld& world, Entity bodyEntity); + /// Compute the inverse of the inertia tensor in world coordinates. + static void computeWorldInertiaTensorInverse(const Matrix3x3& orientation, const Vector3& inverseInertiaTensorLocal, Matrix3x3& outInverseInertiaTensorWorld); public : @@ -212,6 +212,24 @@ class RigidBody : public CollisionBody { friend class Joint; }; +/// Compute the inverse of the inertia tensor in world coordinates. +RP3D_FORCE_INLINE void RigidBody::computeWorldInertiaTensorInverse(const Matrix3x3& orientation, const Vector3& inverseInertiaTensorLocal, Matrix3x3& outInverseInertiaTensorWorld) { + + outInverseInertiaTensorWorld[0][0] = orientation[0][0] * inverseInertiaTensorLocal.x; + outInverseInertiaTensorWorld[0][1] = orientation[1][0] * inverseInertiaTensorLocal.x; + outInverseInertiaTensorWorld[0][2] = orientation[2][0] * inverseInertiaTensorLocal.x; + + outInverseInertiaTensorWorld[1][0] = orientation[0][1] * inverseInertiaTensorLocal.y; + outInverseInertiaTensorWorld[1][1] = orientation[1][1] * inverseInertiaTensorLocal.y; + outInverseInertiaTensorWorld[1][2] = orientation[2][1] * inverseInertiaTensorLocal.y; + + outInverseInertiaTensorWorld[2][0] = orientation[0][2] * inverseInertiaTensorLocal.z; + outInverseInertiaTensorWorld[2][1] = orientation[1][2] * inverseInertiaTensorLocal.z; + outInverseInertiaTensorWorld[2][2] = orientation[2][2] * inverseInertiaTensorLocal.z; + + outInverseInertiaTensorWorld = orientation * outInverseInertiaTensorWorld; +} + } #endif diff --git a/include/reactphysics3d/collision/Collider.h b/include/reactphysics3d/collision/Collider.h index 9fee33da..20b83405 100644 --- a/include/reactphysics3d/collision/Collider.h +++ b/include/reactphysics3d/collision/Collider.h @@ -59,9 +59,6 @@ class Collider { /// Pointer to the parent body CollisionBody* mBody; - /// Material properties of the rigid body - Material mMaterial; - /// Pointer to user data void* mUserData; @@ -188,7 +185,7 @@ class Collider { /** * @return The entity of the collider */ -inline Entity Collider::getEntity() const { +RP3D_FORCE_INLINE Entity Collider::getEntity() const { return mEntity; } @@ -196,7 +193,7 @@ inline Entity Collider::getEntity() const { /** * @return Pointer to the parent body */ -inline CollisionBody* Collider::getBody() const { +RP3D_FORCE_INLINE CollisionBody* Collider::getBody() const { return mBody; } @@ -204,7 +201,7 @@ inline CollisionBody* Collider::getBody() const { /** * @return A pointer to the user data stored into the collider */ -inline void* Collider::getUserData() const { +RP3D_FORCE_INLINE void* Collider::getUserData() const { return mUserData; } @@ -212,7 +209,7 @@ inline void* Collider::getUserData() const { /** * @param userData Pointer to the user data you want to store within the collider */ -inline void Collider::setUserData(void* userData) { +RP3D_FORCE_INLINE void Collider::setUserData(void* userData) { mUserData = userData; } @@ -221,18 +218,10 @@ inline void Collider::setUserData(void* userData) { * @param worldAABB The AABB (in world-space coordinates) that will be used to test overlap * @return True if the given AABB overlaps with the AABB of the collision body */ -inline bool Collider::testAABBOverlap(const AABB& worldAABB) const { +RP3D_FORCE_INLINE bool Collider::testAABBOverlap(const AABB& worldAABB) const { return worldAABB.testCollision(getWorldAABB()); } -// Return a reference to the material properties of the collider -/** - * @return A reference to the material of the body - */ -inline Material& Collider::getMaterial() { - return mMaterial; -} - } #endif diff --git a/include/reactphysics3d/collision/CollisionCallback.h b/include/reactphysics3d/collision/CollisionCallback.h index f3f2b41d..a398a3c9 100644 --- a/include/reactphysics3d/collision/CollisionCallback.h +++ b/include/reactphysics3d/collision/CollisionCallback.h @@ -27,7 +27,7 @@ #define REACTPHYSICS3D_COLLISION_CALLBACK_H // Libraries -#include +#include #include #include @@ -114,7 +114,7 @@ class CollisionCallback { // Class ContactPair /** * This class represents the contact between two colliders of the physics world. - * A contact pair contains a list of contact points. + * A contact pair contains an array of contact points. */ class ContactPair { @@ -141,7 +141,7 @@ class CollisionCallback { const reactphysics3d::ContactPair& mContactPair; /// Pointer to the contact points - List* mContactPoints; + Array* mContactPoints; /// Reference to the physics world PhysicsWorld& mWorld; @@ -152,7 +152,7 @@ class CollisionCallback { // -------------------- Methods -------------------- // /// Constructor - ContactPair(const reactphysics3d::ContactPair& contactPair, List* contactPoints, + ContactPair(const reactphysics3d::ContactPair& contactPair, Array* contactPoints, PhysicsWorld& world, bool mIsLostContactPair); public: @@ -226,23 +226,23 @@ class CollisionCallback { // -------------------- Attributes -------------------- // - /// Pointer to the list of contact pairs (contains contacts and triggers events) - List* mContactPairs; + /// Pointer to the array of contact pairs (contains contacts and triggers events) + Array* mContactPairs; - /// Pointer to the list of contact manifolds - List* mContactManifolds; + /// Pointer to the array of contact manifolds + Array* mContactManifolds; /// Pointer to the contact points - List* mContactPoints; + Array* mContactPoints; - /// Pointer to the list of lost contact pairs (contains contacts and triggers events) - List& mLostContactPairs; + /// Pointer to the array of lost contact pairs (contains contacts and triggers events) + Array& mLostContactPairs; - /// List of indices of the mContactPairs list that are contact events (not overlap/triggers) - List mContactPairsIndices; + /// Array of indices in the mContactPairs array that are contact events (not overlap/triggers) + Array mContactPairsIndices; - /// List of indices of the mLostContactPairs list that are contact events (not overlap/triggers) - List mLostContactPairsIndices; + /// Array of indices in the mLostContactPairs array that are contact events (not overlap/triggers) + Array mLostContactPairsIndices; /// Reference to the physics world PhysicsWorld& mWorld; @@ -250,8 +250,8 @@ class CollisionCallback { // -------------------- Methods -------------------- // /// Constructor - CallbackData(List* contactPairs, List* manifolds, - List* contactPoints, List& lostContactPairs, + CallbackData(Array* contactPairs, Array* manifolds, + Array* contactPoints, Array& lostContactPairs, PhysicsWorld& world); /// Deleted copy constructor @@ -278,7 +278,7 @@ class CollisionCallback { * @param index Index of the contact pair to retrieve * @return A contact pair object */ - ContactPair getContactPair(uint index) const; + ContactPair getContactPair(uint32 index) const; // -------------------- Friendship -------------------- // @@ -296,7 +296,7 @@ class CollisionCallback { /** * @return The number of contact pairs */ -inline uint CollisionCallback::CallbackData::getNbContactPairs() const { +RP3D_FORCE_INLINE uint32 CollisionCallback::CallbackData::getNbContactPairs() const { return mContactPairsIndices.size() + mLostContactPairsIndices.size(); } @@ -304,7 +304,7 @@ inline uint CollisionCallback::CallbackData::getNbContactPairs() const { /** * @return The number of contact points */ -inline uint CollisionCallback::ContactPair::getNbContactPoints() const { +RP3D_FORCE_INLINE uint CollisionCallback::ContactPair::getNbContactPoints() const { return mContactPair.nbToTalContactPoints; } @@ -312,7 +312,7 @@ inline uint CollisionCallback::ContactPair::getNbContactPoints() const { /** * @return The penetration depth (larger than zero) */ -inline decimal CollisionCallback::ContactPoint::getPenetrationDepth() const { +RP3D_FORCE_INLINE decimal CollisionCallback::ContactPoint::getPenetrationDepth() const { return mContactPoint.getPenetrationDepth(); } @@ -320,7 +320,7 @@ inline decimal CollisionCallback::ContactPoint::getPenetrationDepth() const { /** * @return The contact normal direction at the contact point (in world-space) */ -inline const Vector3& CollisionCallback::ContactPoint::getWorldNormal() const { +RP3D_FORCE_INLINE const Vector3& CollisionCallback::ContactPoint::getWorldNormal() const { return mContactPoint.getNormal(); } @@ -328,7 +328,7 @@ inline const Vector3& CollisionCallback::ContactPoint::getWorldNormal() const { /** * @return The contact point in the local-space of the first collider (from body1) in contact */ -inline const Vector3& CollisionCallback::ContactPoint::getLocalPointOnCollider1() const { +RP3D_FORCE_INLINE const Vector3& CollisionCallback::ContactPoint::getLocalPointOnCollider1() const { return mContactPoint.getLocalPointOnShape1(); } @@ -336,7 +336,7 @@ inline const Vector3& CollisionCallback::ContactPoint::getLocalPointOnCollider1( /** * @return The contact point in the local-space of the second collider (from body2) in contact */ -inline const Vector3& CollisionCallback::ContactPoint::getLocalPointOnCollider2() const { +RP3D_FORCE_INLINE const Vector3& CollisionCallback::ContactPoint::getLocalPointOnCollider2() const { return mContactPoint.getLocalPointOnShape2(); } diff --git a/include/reactphysics3d/collision/ContactManifold.h b/include/reactphysics3d/collision/ContactManifold.h index 3e4c546a..738e1565 100644 --- a/include/reactphysics3d/collision/ContactManifold.h +++ b/include/reactphysics3d/collision/ContactManifold.h @@ -58,11 +58,11 @@ class ContactManifold { // -------------------- Constants -------------------- // /// Maximum number of contact points in a reduced contact manifold - const int MAX_CONTACT_POINTS_IN_MANIFOLD = 4; + static constexpr int MAX_CONTACT_POINTS_IN_MANIFOLD = 4; // -------------------- Attributes -------------------- // - /// Index of the first contact point of the manifold in the list of contact points + /// Index of the first contact point of the manifold in the array of contact points uint contactPointsIndex; /// Entity of the first body in contact @@ -78,7 +78,7 @@ class ContactManifold { Entity colliderEntity2; /// Number of contacts in the cache - int8 nbContactPoints; + uint8 nbContactPoints; /// First friction vector of the contact manifold Vector3 frictionVector1; @@ -95,9 +95,6 @@ class ContactManifold { /// Twist friction constraint accumulated impulse decimal frictionTwistImpulse; - /// Accumulated rolling resistance impulse - Vector3 rollingResistanceImpulse; - /// True if the contact manifold has already been added into an island bool isAlreadyInIsland; @@ -107,16 +104,7 @@ class ContactManifold { /// Constructor ContactManifold(Entity bodyEntity1, Entity bodyEntity2, Entity colliderEntity1, Entity colliderEntity2, - uint contactPointsIndex, int8 nbContactPoints); - - /// Destructor - ~ContactManifold(); - - /// Copy-constructor - ContactManifold(const ContactManifold& contactManifold) = default; - - /// Assignment operator - ContactManifold& operator=(const ContactManifold& contactManifold) = default; + uint contactPointsIndex, uint8 nbContactPoints); // -------------------- Friendship -------------------- // diff --git a/include/reactphysics3d/collision/ContactManifoldInfo.h b/include/reactphysics3d/collision/ContactManifoldInfo.h index 728ee1ed..b6dd9219 100644 --- a/include/reactphysics3d/collision/ContactManifoldInfo.h +++ b/include/reactphysics3d/collision/ContactManifoldInfo.h @@ -45,8 +45,12 @@ struct ContactManifoldInfo { // -------------------- Attributes -------------------- // + /// Number of potential contact points + uint8 nbPotentialContactPoints; + /// Indices of the contact points in the mPotentialContactPoints array - List potentialContactPointsIndices; + uint potentialContactPointsIndices[NB_MAX_CONTACT_POINTS_IN_POTENTIAL_MANIFOLD]; + /// Overlapping pair id uint64 pairId; @@ -54,8 +58,7 @@ struct ContactManifoldInfo { // -------------------- Methods -------------------- // /// Constructor - ContactManifoldInfo(uint64 pairId, MemoryAllocator& allocator) - : potentialContactPointsIndices(allocator), pairId(pairId) { + ContactManifoldInfo(uint64 pairId) : nbPotentialContactPoints(0), pairId(pairId) { } diff --git a/include/reactphysics3d/collision/ContactPair.h b/include/reactphysics3d/collision/ContactPair.h index 82bf9d20..53e4cbfb 100644 --- a/include/reactphysics3d/collision/ContactPair.h +++ b/include/reactphysics3d/collision/ContactPair.h @@ -47,8 +47,11 @@ struct ContactPair { /// Overlapping pair Id uint64 pairId; + /// Number of potential contact manifolds + uint8 nbPotentialContactManifolds; + /// Indices of the potential contact manifolds - List potentialContactManifoldsIndices; + uint potentialContactManifoldsIndices[NB_MAX_POTENTIAL_CONTACT_MANIFOLDS]; /// Entity of the first body of the contact Entity body1Entity; @@ -66,19 +69,19 @@ struct ContactPair { bool isAlreadyInIsland; /// Index of the contact pair in the array of pairs - uint contactPairIndex; + uint32 contactPairIndex; /// Index of the first contact manifold in the array - uint contactManifoldsIndex; + uint32 contactManifoldsIndex; /// Number of contact manifolds int8 nbContactManifolds; /// Index of the first contact point in the array of contact points - uint contactPointsIndex; + uint32 contactPointsIndex; /// Total number of contact points in all the manifolds of the contact pair - uint nbToTalContactPoints; + uint32 nbToTalContactPoints; /// True if the colliders of the pair were already colliding in the previous frame bool collidingInPreviousFrame; @@ -90,13 +93,21 @@ struct ContactPair { /// Constructor ContactPair(uint64 pairId, Entity body1Entity, Entity body2Entity, Entity collider1Entity, - Entity collider2Entity, uint contactPairIndex, bool collidingInPreviousFrame, bool isTrigger, MemoryAllocator& allocator) - : pairId(pairId), potentialContactManifoldsIndices(allocator), body1Entity(body1Entity), body2Entity(body2Entity), + Entity collider2Entity, uint contactPairIndex, bool collidingInPreviousFrame, bool isTrigger) + : pairId(pairId), nbPotentialContactManifolds(0), body1Entity(body1Entity), body2Entity(body2Entity), collider1Entity(collider1Entity), collider2Entity(collider2Entity), isAlreadyInIsland(false), contactPairIndex(contactPairIndex), contactManifoldsIndex(0), nbContactManifolds(0), contactPointsIndex(0), nbToTalContactPoints(0), collidingInPreviousFrame(collidingInPreviousFrame), isTrigger(isTrigger) { } + + // Remove a potential manifold at a given index in the array + void removePotentialManifoldAtIndex(uint index) { + assert(index < nbPotentialContactManifolds); + + potentialContactManifoldsIndices[index] = potentialContactManifoldsIndices[nbPotentialContactManifolds - 1]; + nbPotentialContactManifolds--; + } }; } diff --git a/include/reactphysics3d/collision/ContactPointInfo.h b/include/reactphysics3d/collision/ContactPointInfo.h index cb44673d..0b296b5b 100644 --- a/include/reactphysics3d/collision/ContactPointInfo.h +++ b/include/reactphysics3d/collision/ContactPointInfo.h @@ -45,10 +45,6 @@ class CollisionBody; */ struct ContactPointInfo { - private: - - // -------------------- Methods -------------------- // - public: // -------------------- Attributes -------------------- // @@ -56,29 +52,15 @@ struct ContactPointInfo { /// Normalized normal vector of the collision contact in world space Vector3 normal; - /// Penetration depth of the contact - decimal penetrationDepth; - /// Contact point of body 1 in local space of body 1 Vector3 localPoint1; /// Contact point of body 2 in local space of body 2 Vector3 localPoint2; - // -------------------- Methods -------------------- // + /// Penetration depth of the contact + decimal penetrationDepth; - /// Constructor - ContactPointInfo(const Vector3& contactNormal, decimal penDepth, - const Vector3& localPt1, const Vector3& localPt2) - : normal(contactNormal), penetrationDepth(penDepth), - localPoint1(localPt1), localPoint2(localPt2) { - - assert(contactNormal.lengthSquare() > decimal(0.8)); - assert(penDepth > decimal(0.0)); - } - - /// Destructor - ~ContactPointInfo() = default; }; } diff --git a/include/reactphysics3d/collision/HalfEdgeStructure.h b/include/reactphysics3d/collision/HalfEdgeStructure.h index d0d915df..8b9f8e4a 100644 --- a/include/reactphysics3d/collision/HalfEdgeStructure.h +++ b/include/reactphysics3d/collision/HalfEdgeStructure.h @@ -54,22 +54,22 @@ class HalfEdgeStructure { /// Face struct Face { uint edgeIndex; // Index of an half-edge of the face - List faceVertices; // Index of the vertices of the face + Array faceVertices; // Index of the vertices of the face /// Constructor Face(MemoryAllocator& allocator) : faceVertices(allocator) {} /// Constructor - Face(List vertices) : faceVertices(vertices) {} + Face(Array vertices) : faceVertices(vertices) {} }; /// Vertex struct Vertex { - uint vertexPointIndex; // Index of the vertex point in the origin vertex array - uint edgeIndex; // Index of one edge emanting from this vertex + uint32 vertexPointIndex; // Index of the vertex point in the origin vertex array + uint32 edgeIndex; // Index of one edge emanting from this vertex /// Constructor - Vertex(uint vertexCoordsIndex) : vertexPointIndex(vertexCoordsIndex) { } + Vertex(uint32 vertexCoordsIndex) : vertexPointIndex(vertexCoordsIndex) { } }; private: @@ -78,13 +78,13 @@ class HalfEdgeStructure { MemoryAllocator& mAllocator; /// All the faces - List mFaces; + Array mFaces; /// All the vertices - List mVertices; + Array mVertices; /// All the half-edges - List mEdges; + Array mEdges; public: @@ -103,7 +103,7 @@ class HalfEdgeStructure { uint addVertex(uint vertexPointIndex); /// Add a face - void addFace(List faceVertices); + void addFace(Array faceVertices); /// Return the number of faces uint getNbFaces() const; @@ -129,7 +129,7 @@ class HalfEdgeStructure { /** * @param vertexPointIndex Index of the vertex in the vertex data array */ -inline uint HalfEdgeStructure::addVertex(uint vertexPointIndex) { +RP3D_FORCE_INLINE uint32 HalfEdgeStructure::addVertex(uint32 vertexPointIndex) { Vertex vertex(vertexPointIndex); mVertices.add(vertex); return mVertices.size() - 1; @@ -137,10 +137,10 @@ inline uint HalfEdgeStructure::addVertex(uint vertexPointIndex) { // Add a face /** - * @param faceVertices List of the vertices in a face (ordered in CCW order as seen from outside + * @param faceVertices Array of the vertices in a face (ordered in CCW order as seen from outside * the polyhedron */ -inline void HalfEdgeStructure::addFace(List faceVertices) { +RP3D_FORCE_INLINE void HalfEdgeStructure::addFace(Array faceVertices) { // Create a new face Face face(faceVertices); @@ -151,31 +151,31 @@ inline void HalfEdgeStructure::addFace(List faceVertices) { /** * @return The number of faces in the polyhedron */ -inline uint HalfEdgeStructure::getNbFaces() const { - return static_cast(mFaces.size()); +RP3D_FORCE_INLINE uint32 HalfEdgeStructure::getNbFaces() const { + return mFaces.size(); } // Return the number of edges /** * @return The number of edges in the polyhedron */ -inline uint HalfEdgeStructure::getNbHalfEdges() const { - return static_cast(mEdges.size()); +RP3D_FORCE_INLINE uint32 HalfEdgeStructure::getNbHalfEdges() const { + return mEdges.size(); } // Return the number of vertices /** * @return The number of vertices in the polyhedron */ -inline uint HalfEdgeStructure::getNbVertices() const { - return static_cast(mVertices.size()); +RP3D_FORCE_INLINE uint32 HalfEdgeStructure::getNbVertices() const { + return mVertices.size(); } // Return a given face /** * @return A given face of the polyhedron */ -inline const HalfEdgeStructure::Face& HalfEdgeStructure::getFace(uint index) const { +RP3D_FORCE_INLINE const HalfEdgeStructure::Face& HalfEdgeStructure::getFace(uint32 index) const { assert(index < mFaces.size()); return mFaces[index]; } @@ -184,7 +184,7 @@ inline const HalfEdgeStructure::Face& HalfEdgeStructure::getFace(uint index) con /** * @return A given edge of the polyhedron */ -inline const HalfEdgeStructure::Edge& HalfEdgeStructure::getHalfEdge(uint index) const { +RP3D_FORCE_INLINE const HalfEdgeStructure::Edge& HalfEdgeStructure::getHalfEdge(uint32 index) const { assert(index < mEdges.size()); return mEdges[index]; } @@ -193,7 +193,7 @@ inline const HalfEdgeStructure::Edge& HalfEdgeStructure::getHalfEdge(uint index) /** * @return A given vertex of the polyhedron */ -inline const HalfEdgeStructure::Vertex& HalfEdgeStructure::getVertex(uint index) const { +RP3D_FORCE_INLINE const HalfEdgeStructure::Vertex& HalfEdgeStructure::getVertex(uint32 index) const { assert(index < mVertices.size()); return mVertices[index]; } diff --git a/include/reactphysics3d/collision/OverlapCallback.h b/include/reactphysics3d/collision/OverlapCallback.h index 1fb99660..aa0b55de 100644 --- a/include/reactphysics3d/collision/OverlapCallback.h +++ b/include/reactphysics3d/collision/OverlapCallback.h @@ -27,7 +27,7 @@ #define REACTPHYSICS3D_OVERLAP_CALLBACK_H // Libraries -#include +#include #include /// ReactPhysics3D namespace @@ -131,17 +131,17 @@ class OverlapCallback { // -------------------- Attributes -------------------- // - /// Reference to the list of contact pairs (contains contacts and triggers events) - List& mContactPairs; + /// Reference to the array of contact pairs (contains contacts and triggers events) + Array& mContactPairs; - /// Reference to the list of lost contact pairs (contains contacts and triggers events) - List& mLostContactPairs; + /// Reference to the array of lost contact pairs (contains contacts and triggers events) + Array& mLostContactPairs; - /// List of indices of the mContactPairs list that are overlap/triggers events (not contact events) - List mContactPairsIndices; + /// Array of indices of the mContactPairs array that are overlap/triggers events (not contact events) + Array mContactPairsIndices; - /// List of indices of the mLostContactPairs list that are overlap/triggers events (not contact events) - List mLostContactPairsIndices; + /// Array of indices of the mLostContactPairs array that are overlap/triggers events (not contact events) + Array mLostContactPairsIndices; /// Reference to the physics world PhysicsWorld& mWorld; @@ -149,7 +149,7 @@ class OverlapCallback { // -------------------- Methods -------------------- // /// Constructor - CallbackData(List& contactPairs, List& lostContactPairs, bool onlyReportTriggers, PhysicsWorld& world); + CallbackData(Array& contactPairs, Array& lostContactPairs, bool onlyReportTriggers, PhysicsWorld& world); /// Deleted copy constructor CallbackData(const CallbackData& callbackData) = delete; @@ -165,10 +165,10 @@ class OverlapCallback { // -------------------- Methods -------------------- // /// Return the number of overlapping pairs of bodies - uint getNbOverlappingPairs() const; + uint32 getNbOverlappingPairs() const; /// Return a given overlapping pair of bodies - OverlapPair getOverlappingPair(uint index) const; + OverlapPair getOverlappingPair(uint32 index) const; // -------------------- Friendship -------------------- // @@ -185,7 +185,7 @@ class OverlapCallback { }; // Return the number of overlapping pairs of bodies -inline uint OverlapCallback::CallbackData::getNbOverlappingPairs() const { +RP3D_FORCE_INLINE uint32 OverlapCallback::CallbackData::getNbOverlappingPairs() const { return mContactPairsIndices.size() + mLostContactPairsIndices.size(); } @@ -193,7 +193,7 @@ inline uint OverlapCallback::CallbackData::getNbOverlappingPairs() const { /// Note that the returned OverlapPair object is only valid during the call of the CollisionCallback::onOverlap() /// method. Therefore, you need to get contact data from it and make a copy. Do not make a copy of the OverlapPair /// object itself because it won't be valid after the CollisionCallback::onOverlap() call. -inline OverlapCallback::OverlapPair OverlapCallback::CallbackData::getOverlappingPair(uint index) const { +RP3D_FORCE_INLINE OverlapCallback::OverlapPair OverlapCallback::CallbackData::getOverlappingPair(uint32 index) const { assert(index < getNbOverlappingPairs()); diff --git a/include/reactphysics3d/collision/PolygonVertexArray.h b/include/reactphysics3d/collision/PolygonVertexArray.h index fe97d146..4512bc15 100644 --- a/include/reactphysics3d/collision/PolygonVertexArray.h +++ b/include/reactphysics3d/collision/PolygonVertexArray.h @@ -140,7 +140,7 @@ class PolygonVertexArray { /** * @return The data type of the vertices in the array */ -inline PolygonVertexArray::VertexDataType PolygonVertexArray::getVertexDataType() const { +RP3D_FORCE_INLINE PolygonVertexArray::VertexDataType PolygonVertexArray::getVertexDataType() const { return mVertexDataType; } @@ -148,7 +148,7 @@ inline PolygonVertexArray::VertexDataType PolygonVertexArray::getVertexDataType( /** * @return The data type of the indices in the array */ -inline PolygonVertexArray::IndexDataType PolygonVertexArray::getIndexDataType() const { +RP3D_FORCE_INLINE PolygonVertexArray::IndexDataType PolygonVertexArray::getIndexDataType() const { return mIndexDataType; } @@ -156,7 +156,7 @@ inline PolygonVertexArray::IndexDataType PolygonVertexArray::getIndexDataType() /** * @return The number of vertices in the array */ -inline uint PolygonVertexArray::getNbVertices() const { +RP3D_FORCE_INLINE uint PolygonVertexArray::getNbVertices() const { return mNbVertices; } @@ -164,7 +164,7 @@ inline uint PolygonVertexArray::getNbVertices() const { /** * @return The number of faces in the array */ -inline uint PolygonVertexArray::getNbFaces() const { +RP3D_FORCE_INLINE uint PolygonVertexArray::getNbFaces() const { return mNbFaces; } @@ -172,7 +172,7 @@ inline uint PolygonVertexArray::getNbFaces() const { /** * @return The number of bytes between two vertices */ -inline int PolygonVertexArray::getVerticesStride() const { +RP3D_FORCE_INLINE int PolygonVertexArray::getVerticesStride() const { return mVerticesStride; } @@ -180,7 +180,7 @@ inline int PolygonVertexArray::getVerticesStride() const { /** * @return The number of bytes between two consecutive face indices */ -inline int PolygonVertexArray::getIndicesStride() const { +RP3D_FORCE_INLINE int PolygonVertexArray::getIndicesStride() const { return mIndicesStride; } @@ -189,7 +189,7 @@ inline int PolygonVertexArray::getIndicesStride() const { * @param faceIndex Index of a given face * @return A polygon face */ -inline PolygonVertexArray::PolygonFace* PolygonVertexArray::getPolygonFace(uint faceIndex) const { +RP3D_FORCE_INLINE PolygonVertexArray::PolygonFace* PolygonVertexArray::getPolygonFace(uint faceIndex) const { assert(faceIndex < mNbFaces); return &mPolygonFacesStart[faceIndex]; } @@ -198,7 +198,7 @@ inline PolygonVertexArray::PolygonFace* PolygonVertexArray::getPolygonFace(uint /** * @return A pointer to the start of the vertex array of the polyhedron */ -inline const unsigned char* PolygonVertexArray::getVerticesStart() const { +RP3D_FORCE_INLINE const unsigned char* PolygonVertexArray::getVerticesStart() const { return mVerticesStart; } @@ -206,7 +206,7 @@ inline const unsigned char* PolygonVertexArray::getVerticesStart() const { /** * @return A pointer to the start of the face indices array of the polyhedron */ -inline const unsigned char* PolygonVertexArray::getIndicesStart() const { +RP3D_FORCE_INLINE const unsigned char* PolygonVertexArray::getIndicesStart() const { return mIndicesStart; } diff --git a/include/reactphysics3d/collision/PolyhedronMesh.h b/include/reactphysics3d/collision/PolyhedronMesh.h index e7fd932f..210173d7 100644 --- a/include/reactphysics3d/collision/PolyhedronMesh.h +++ b/include/reactphysics3d/collision/PolyhedronMesh.h @@ -28,6 +28,7 @@ // Libraries #include +#include #include "HalfEdgeStructure.h" namespace reactphysics3d { @@ -69,7 +70,7 @@ class PolyhedronMesh { PolyhedronMesh(PolygonVertexArray* polygonVertexArray, MemoryAllocator& allocator); /// Create the half-edge structure of the mesh - void createHalfEdgeStructure(); + bool createHalfEdgeStructure(); /// Compute the faces normals void computeFacesNormals(); @@ -80,6 +81,9 @@ class PolyhedronMesh { /// Compute and return the area of a face decimal getFaceArea(uint faceIndex) const; + /// Static factory method to create a polyhedron mesh + static PolyhedronMesh* create(PolygonVertexArray* polygonVertexArray, MemoryAllocator& polyhedronMeshAllocator, MemoryAllocator& dataAllocator); + public: // -------------------- Methods -------------------- // @@ -117,7 +121,7 @@ class PolyhedronMesh { /** * @return The number of vertices in the mesh */ -inline uint PolyhedronMesh::getNbVertices() const { +RP3D_FORCE_INLINE uint PolyhedronMesh::getNbVertices() const { return mHalfEdgeStructure.getNbVertices(); } @@ -125,7 +129,7 @@ inline uint PolyhedronMesh::getNbVertices() const { /** * @return The number of faces in the mesh */ -inline uint PolyhedronMesh::getNbFaces() const { +RP3D_FORCE_INLINE uint PolyhedronMesh::getNbFaces() const { return mHalfEdgeStructure.getNbFaces(); } @@ -134,7 +138,7 @@ inline uint PolyhedronMesh::getNbFaces() const { * @param faceIndex The index of a given face of the mesh * @return The normal vector of a given face of the mesh */ -inline Vector3 PolyhedronMesh::getFaceNormal(uint faceIndex) const { +RP3D_FORCE_INLINE Vector3 PolyhedronMesh::getFaceNormal(uint faceIndex) const { assert(faceIndex < mHalfEdgeStructure.getNbFaces()); return mFacesNormals[faceIndex]; } @@ -143,7 +147,7 @@ inline Vector3 PolyhedronMesh::getFaceNormal(uint faceIndex) const { /** * @return The Half-Edge structure of the mesh */ -inline const HalfEdgeStructure& PolyhedronMesh::getHalfEdgeStructure() const { +RP3D_FORCE_INLINE const HalfEdgeStructure& PolyhedronMesh::getHalfEdgeStructure() const { return mHalfEdgeStructure; } @@ -151,7 +155,7 @@ inline const HalfEdgeStructure& PolyhedronMesh::getHalfEdgeStructure() const { /** * @return The centroid of the mesh */ -inline Vector3 PolyhedronMesh::getCentroid() const { +RP3D_FORCE_INLINE Vector3 PolyhedronMesh::getCentroid() const { return mCentroid; } diff --git a/include/reactphysics3d/collision/RaycastInfo.h b/include/reactphysics3d/collision/RaycastInfo.h index 62b9aa93..21283abe 100644 --- a/include/reactphysics3d/collision/RaycastInfo.h +++ b/include/reactphysics3d/collision/RaycastInfo.h @@ -137,6 +137,7 @@ struct RaycastTest { /// Constructor RaycastTest(RaycastCallback* callback) { userCallback = callback; + } /// Ray cast test against a collider diff --git a/include/reactphysics3d/collision/TriangleMesh.h b/include/reactphysics3d/collision/TriangleMesh.h index 155f7a44..2088a312 100644 --- a/include/reactphysics3d/collision/TriangleMesh.h +++ b/include/reactphysics3d/collision/TriangleMesh.h @@ -28,7 +28,7 @@ // Libraries #include -#include +#include #include namespace reactphysics3d { @@ -49,7 +49,7 @@ class TriangleMesh { protected: /// All the triangle arrays of the mesh (one triangle array per part) - List mTriangleArrays; + Array mTriangleArrays; /// Constructor TriangleMesh(reactphysics3d::MemoryAllocator& allocator); @@ -78,7 +78,7 @@ class TriangleMesh { /** * @param triangleVertexArray Pointer to the TriangleVertexArray to add into the mesh */ -inline void TriangleMesh::addSubpart(TriangleVertexArray* triangleVertexArray) { +RP3D_FORCE_INLINE void TriangleMesh::addSubpart(TriangleVertexArray* triangleVertexArray) { mTriangleArrays.add(triangleVertexArray ); } @@ -87,7 +87,7 @@ inline void TriangleMesh::addSubpart(TriangleVertexArray* triangleVertexArray) { * @param indexSubpart The index of the sub-part of the mesh * @return A pointer to the triangle vertex array of a given sub-part of the mesh */ -inline TriangleVertexArray* TriangleMesh::getSubpart(uint indexSubpart) const { +RP3D_FORCE_INLINE TriangleVertexArray* TriangleMesh::getSubpart(uint32 indexSubpart) const { assert(indexSubpart < mTriangleArrays.size()); return mTriangleArrays[indexSubpart]; } @@ -96,7 +96,7 @@ inline TriangleVertexArray* TriangleMesh::getSubpart(uint indexSubpart) const { /** * @return The number of sub-parts of the mesh */ -inline uint TriangleMesh::getNbSubparts() const { +RP3D_FORCE_INLINE uint32 TriangleMesh::getNbSubparts() const { return mTriangleArrays.size(); } diff --git a/include/reactphysics3d/collision/TriangleVertexArray.h b/include/reactphysics3d/collision/TriangleVertexArray.h index b5002be5..f68c8e22 100644 --- a/include/reactphysics3d/collision/TriangleVertexArray.h +++ b/include/reactphysics3d/collision/TriangleVertexArray.h @@ -182,7 +182,7 @@ class TriangleVertexArray { /** * @return The data type of the vertices in the array */ -inline TriangleVertexArray::VertexDataType TriangleVertexArray::getVertexDataType() const { +RP3D_FORCE_INLINE TriangleVertexArray::VertexDataType TriangleVertexArray::getVertexDataType() const { return mVertexDataType; } @@ -190,7 +190,7 @@ inline TriangleVertexArray::VertexDataType TriangleVertexArray::getVertexDataTyp /** * @return The data type of the normals in the array */ -inline TriangleVertexArray::NormalDataType TriangleVertexArray::getVertexNormalDataType() const { +RP3D_FORCE_INLINE TriangleVertexArray::NormalDataType TriangleVertexArray::getVertexNormalDataType() const { return mVertexNormaldDataType; } @@ -198,7 +198,7 @@ inline TriangleVertexArray::NormalDataType TriangleVertexArray::getVertexNormalD /** * @return The data type of the face indices in the array */ -inline TriangleVertexArray::IndexDataType TriangleVertexArray::getIndexDataType() const { +RP3D_FORCE_INLINE TriangleVertexArray::IndexDataType TriangleVertexArray::getIndexDataType() const { return mIndexDataType; } @@ -206,7 +206,7 @@ inline TriangleVertexArray::IndexDataType TriangleVertexArray::getIndexDataType( /** * @return The number of vertices in the array */ -inline uint TriangleVertexArray::getNbVertices() const { +RP3D_FORCE_INLINE uint TriangleVertexArray::getNbVertices() const { return mNbVertices; } @@ -214,7 +214,7 @@ inline uint TriangleVertexArray::getNbVertices() const { /** * @return The number of triangles in the array */ -inline uint TriangleVertexArray::getNbTriangles() const { +RP3D_FORCE_INLINE uint TriangleVertexArray::getNbTriangles() const { return mNbTriangles; } @@ -222,7 +222,7 @@ inline uint TriangleVertexArray::getNbTriangles() const { /** * @return The number of bytes separating two consecutive vertices in the array */ -inline uint TriangleVertexArray::getVerticesStride() const { +RP3D_FORCE_INLINE uint TriangleVertexArray::getVerticesStride() const { return mVerticesStride; } @@ -230,7 +230,7 @@ inline uint TriangleVertexArray::getVerticesStride() const { /** * @return The number of bytes separating two consecutive normals in the array */ -inline uint TriangleVertexArray::getVerticesNormalsStride() const { +RP3D_FORCE_INLINE uint TriangleVertexArray::getVerticesNormalsStride() const { return mVerticesNormalsStride; } @@ -238,7 +238,7 @@ inline uint TriangleVertexArray::getVerticesNormalsStride() const { /** * @return The number of bytes separating two consecutive face indices in the array */ -inline uint TriangleVertexArray::getIndicesStride() const { +RP3D_FORCE_INLINE uint TriangleVertexArray::getIndicesStride() const { return mIndicesStride; } @@ -246,7 +246,7 @@ inline uint TriangleVertexArray::getIndicesStride() const { /** * @return A pointer to the start of the vertices data in the array */ -inline const void* TriangleVertexArray::getVerticesStart() const { +RP3D_FORCE_INLINE const void* TriangleVertexArray::getVerticesStart() const { return mVerticesStart; } @@ -254,7 +254,7 @@ inline const void* TriangleVertexArray::getVerticesStart() const { /** * @return A pointer to the start of the normals data in the array */ -inline const void* TriangleVertexArray::getVerticesNormalsStart() const { +RP3D_FORCE_INLINE const void* TriangleVertexArray::getVerticesNormalsStart() const { return mVerticesNormalsStart; } @@ -262,7 +262,7 @@ inline const void* TriangleVertexArray::getVerticesNormalsStart() const { /** * @return A pointer to the start of the face indices data in the array */ -inline const void* TriangleVertexArray::getIndicesStart() const { +RP3D_FORCE_INLINE const void* TriangleVertexArray::getIndicesStart() const { return mIndicesStart; } diff --git a/include/reactphysics3d/collision/broadphase/DynamicAABBTree.h b/include/reactphysics3d/collision/broadphase/DynamicAABBTree.h index 315d54d1..a354de7c 100644 --- a/include/reactphysics3d/collision/broadphase/DynamicAABBTree.h +++ b/include/reactphysics3d/collision/broadphase/DynamicAABBTree.h @@ -58,7 +58,7 @@ struct TreeNode { // -------------------- Attributes -------------------- // - // A node is either in the tree (has a parent) or in the free nodes list + // A node is either in the tree (has a parent) or in the free nodes array // (has a next node) union { @@ -149,7 +149,7 @@ class DynamicAABBTree { /// ID of the root node of the tree int32 mRootNodeID; - /// ID of the first node of the list of free (allocated) nodes in the tree that we can use + /// ID of the first node of the array of free (allocated) nodes in the tree that we can use int32 mFreeNodeID; /// Number of allocated nodes in the tree @@ -236,11 +236,11 @@ class DynamicAABBTree { void* getNodeDataPointer(int32 nodeID) const; /// Report all shapes overlapping with all the shapes in the map in parameter - void reportAllShapesOverlappingWithShapes(const List& nodesToTest, size_t startIndex, - size_t endIndex, List>& outOverlappingNodes) const; + void reportAllShapesOverlappingWithShapes(const Array& nodesToTest, size_t startIndex, + size_t endIndex, Array>& outOverlappingNodes) const; /// Report all shapes overlapping with the AABB given in parameter. - void reportAllShapesOverlappingWithAABB(const AABB& aabb, List& overlappingNodes) const; + void reportAllShapesOverlappingWithAABB(const AABB& aabb, Array& overlappingNodes) const; /// Ray casting method void raycast(const Ray& ray, DynamicAABBTreeRaycastCallback& callback) const; @@ -264,38 +264,38 @@ class DynamicAABBTree { }; // Return true if the node is a leaf of the tree -inline bool TreeNode::isLeaf() const { +RP3D_FORCE_INLINE bool TreeNode::isLeaf() const { return (height == 0); } // Return the fat AABB corresponding to a given node ID -inline const AABB& DynamicAABBTree::getFatAABB(int32 nodeID) const { +RP3D_FORCE_INLINE const AABB& DynamicAABBTree::getFatAABB(int32 nodeID) const { assert(nodeID >= 0 && nodeID < mNbAllocatedNodes); return mNodes[nodeID].aabb; } // Return the pointer to the data array of a given leaf node of the tree -inline int32* DynamicAABBTree::getNodeDataInt(int32 nodeID) const { +RP3D_FORCE_INLINE int32* DynamicAABBTree::getNodeDataInt(int32 nodeID) const { assert(nodeID >= 0 && nodeID < mNbAllocatedNodes); assert(mNodes[nodeID].isLeaf()); return mNodes[nodeID].dataInt; } // Return the pointer to the data pointer of a given leaf node of the tree -inline void* DynamicAABBTree::getNodeDataPointer(int32 nodeID) const { +RP3D_FORCE_INLINE void* DynamicAABBTree::getNodeDataPointer(int32 nodeID) const { assert(nodeID >= 0 && nodeID < mNbAllocatedNodes); assert(mNodes[nodeID].isLeaf()); return mNodes[nodeID].dataPointer; } // Return the root AABB of the tree -inline AABB DynamicAABBTree::getRootAABB() const { +RP3D_FORCE_INLINE AABB DynamicAABBTree::getRootAABB() const { return getFatAABB(mRootNodeID); } // Add an object into the tree. This method creates a new leaf node in the tree and // returns the ID of the corresponding node. -inline int32 DynamicAABBTree::addObject(const AABB& aabb, int32 data1, int32 data2) { +RP3D_FORCE_INLINE int32 DynamicAABBTree::addObject(const AABB& aabb, int32 data1, int32 data2) { int32 nodeId = addObjectInternal(aabb); @@ -307,7 +307,7 @@ inline int32 DynamicAABBTree::addObject(const AABB& aabb, int32 data1, int32 dat // Add an object into the tree. This method creates a new leaf node in the tree and // returns the ID of the corresponding node. -inline int32 DynamicAABBTree::addObject(const AABB& aabb, void* data) { +RP3D_FORCE_INLINE int32 DynamicAABBTree::addObject(const AABB& aabb, void* data) { int32 nodeId = addObjectInternal(aabb); @@ -319,7 +319,7 @@ inline int32 DynamicAABBTree::addObject(const AABB& aabb, void* data) { #ifdef IS_RP3D_PROFILING_ENABLED // Set the profiler -inline void DynamicAABBTree::setProfiler(Profiler* profiler) { +RP3D_FORCE_INLINE void DynamicAABBTree::setProfiler(Profiler* profiler) { mProfiler = profiler; } diff --git a/include/reactphysics3d/collision/narrowphase/CapsuleVsCapsuleAlgorithm.h b/include/reactphysics3d/collision/narrowphase/CapsuleVsCapsuleAlgorithm.h index a34d58bf..f065235c 100644 --- a/include/reactphysics3d/collision/narrowphase/CapsuleVsCapsuleAlgorithm.h +++ b/include/reactphysics3d/collision/narrowphase/CapsuleVsCapsuleAlgorithm.h @@ -34,7 +34,7 @@ namespace reactphysics3d { // Declarations -struct CapsuleVsCapsuleNarrowPhaseInfoBatch; +struct NarrowPhaseInfoBatch; class ContactPoint; // Class CapsuleVsCapsuleAlgorithm @@ -66,7 +66,7 @@ class CapsuleVsCapsuleAlgorithm : public NarrowPhaseAlgorithm { CapsuleVsCapsuleAlgorithm& operator=(const CapsuleVsCapsuleAlgorithm& algorithm) = delete; /// Compute the narrow-phase collision detection between two capsules - bool testCollision(CapsuleVsCapsuleNarrowPhaseInfoBatch& narrowPhaseInfoBatch, uint batchStartIndex, + bool testCollision(NarrowPhaseInfoBatch& narrowPhaseInfoBatch, uint batchStartIndex, uint batchNbItems, MemoryAllocator& memoryAllocator); }; diff --git a/include/reactphysics3d/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.h b/include/reactphysics3d/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.h index 80987bf3..f72e7db2 100644 --- a/include/reactphysics3d/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.h +++ b/include/reactphysics3d/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.h @@ -34,6 +34,7 @@ namespace reactphysics3d { // Declarations class ContactPoint; +struct NarrowPhaseInfoBatch; // Class CapsuleVsConvexPolyhedronAlgorithm /** diff --git a/include/reactphysics3d/collision/narrowphase/CollisionDispatch.h b/include/reactphysics3d/collision/narrowphase/CollisionDispatch.h index 8b77f8d9..95bfb990 100644 --- a/include/reactphysics3d/collision/narrowphase/CollisionDispatch.h +++ b/include/reactphysics3d/collision/narrowphase/CollisionDispatch.h @@ -173,39 +173,39 @@ class CollisionDispatch { }; // Get the Sphere vs Sphere narrow-phase collision detection algorithm -inline SphereVsSphereAlgorithm* CollisionDispatch::getSphereVsSphereAlgorithm() { +RP3D_FORCE_INLINE SphereVsSphereAlgorithm* CollisionDispatch::getSphereVsSphereAlgorithm() { return mSphereVsSphereAlgorithm; } // Get the Sphere vs Capsule narrow-phase collision detection algorithm -inline SphereVsCapsuleAlgorithm* CollisionDispatch::getSphereVsCapsuleAlgorithm() { +RP3D_FORCE_INLINE SphereVsCapsuleAlgorithm* CollisionDispatch::getSphereVsCapsuleAlgorithm() { return mSphereVsCapsuleAlgorithm; } // Get the Capsule vs Capsule narrow-phase collision detection algorithm -inline CapsuleVsCapsuleAlgorithm* CollisionDispatch::getCapsuleVsCapsuleAlgorithm() { +RP3D_FORCE_INLINE CapsuleVsCapsuleAlgorithm* CollisionDispatch::getCapsuleVsCapsuleAlgorithm() { return mCapsuleVsCapsuleAlgorithm; } // Get the Sphere vs Convex Polyhedron narrow-phase collision detection algorithm -inline SphereVsConvexPolyhedronAlgorithm* CollisionDispatch::getSphereVsConvexPolyhedronAlgorithm() { +RP3D_FORCE_INLINE SphereVsConvexPolyhedronAlgorithm* CollisionDispatch::getSphereVsConvexPolyhedronAlgorithm() { return mSphereVsConvexPolyhedronAlgorithm; } // Get the Capsule vs Convex Polyhedron narrow-phase collision detection algorithm -inline CapsuleVsConvexPolyhedronAlgorithm* CollisionDispatch::getCapsuleVsConvexPolyhedronAlgorithm() { +RP3D_FORCE_INLINE CapsuleVsConvexPolyhedronAlgorithm* CollisionDispatch::getCapsuleVsConvexPolyhedronAlgorithm() { return mCapsuleVsConvexPolyhedronAlgorithm; } // Get the Convex Polyhedron vs Convex Polyhedron narrow-phase collision detection algorithm -inline ConvexPolyhedronVsConvexPolyhedronAlgorithm* CollisionDispatch::getConvexPolyhedronVsConvexPolyhedronAlgorithm() { +RP3D_FORCE_INLINE ConvexPolyhedronVsConvexPolyhedronAlgorithm* CollisionDispatch::getConvexPolyhedronVsConvexPolyhedronAlgorithm() { return mConvexPolyhedronVsConvexPolyhedronAlgorithm; } #ifdef IS_RP3D_PROFILING_ENABLED // Set the profiler -inline void CollisionDispatch::setProfiler(Profiler* profiler) { +RP3D_FORCE_INLINE void CollisionDispatch::setProfiler(Profiler* profiler) { mProfiler = profiler; mSphereVsSphereAlgorithm->setProfiler(profiler); diff --git a/include/reactphysics3d/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.h b/include/reactphysics3d/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.h index c3cb6e0d..2b6525e7 100644 --- a/include/reactphysics3d/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.h +++ b/include/reactphysics3d/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.h @@ -34,6 +34,7 @@ namespace reactphysics3d { // Declarations class ContactPoint; +struct NarrowPhaseInfoBatch; // Class ConvexPolyhedronVsConvexPolyhedronAlgorithm /** diff --git a/include/reactphysics3d/collision/narrowphase/GJK/GJKAlgorithm.h b/include/reactphysics3d/collision/narrowphase/GJK/GJKAlgorithm.h index 7b727920..2383be1f 100644 --- a/include/reactphysics3d/collision/narrowphase/GJK/GJKAlgorithm.h +++ b/include/reactphysics3d/collision/narrowphase/GJK/GJKAlgorithm.h @@ -39,7 +39,7 @@ struct NarrowPhaseInfoBatch; class ConvexShape; class Profiler; class VoronoiSimplex; -template class List; +template class Array; // Constants constexpr decimal REL_ERROR = decimal(1.0e-3); @@ -98,7 +98,7 @@ class GJKAlgorithm { /// Compute a contact info if the two bounding volumes collide. void testCollision(NarrowPhaseInfoBatch& narrowPhaseInfoBatch, uint batchStartIndex, - uint batchNbItems, List& gjkResults); + uint batchNbItems, Array& gjkResults); #ifdef IS_RP3D_PROFILING_ENABLED @@ -112,7 +112,7 @@ class GJKAlgorithm { #ifdef IS_RP3D_PROFILING_ENABLED // Set the profiler -inline void GJKAlgorithm::setProfiler(Profiler* profiler) { +RP3D_FORCE_INLINE void GJKAlgorithm::setProfiler(Profiler* profiler) { mProfiler = profiler; } diff --git a/include/reactphysics3d/collision/narrowphase/GJK/VoronoiSimplex.h b/include/reactphysics3d/collision/narrowphase/GJK/VoronoiSimplex.h index 4d721b0d..785d4edf 100644 --- a/include/reactphysics3d/collision/narrowphase/GJK/VoronoiSimplex.h +++ b/include/reactphysics3d/collision/narrowphase/GJK/VoronoiSimplex.h @@ -28,6 +28,7 @@ // Libraries #include +#include /// ReactPhysics3D namespace namespace reactphysics3d { @@ -171,17 +172,17 @@ class VoronoiSimplex { }; // Return true if the simplex contains 4 points -inline bool VoronoiSimplex::isFull() const { +RP3D_FORCE_INLINE bool VoronoiSimplex::isFull() const { return mNbPoints == 4; } // Return true if the simple is empty -inline bool VoronoiSimplex::isEmpty() const { +RP3D_FORCE_INLINE bool VoronoiSimplex::isEmpty() const { return mNbPoints == 0; } // Set the barycentric coordinates of the closest point -inline void VoronoiSimplex::setBarycentricCoords(decimal a, decimal b, decimal c, decimal d) { +RP3D_FORCE_INLINE void VoronoiSimplex::setBarycentricCoords(decimal a, decimal b, decimal c, decimal d) { mBarycentricCoords[0] = a; mBarycentricCoords[1] = b; mBarycentricCoords[2] = c; @@ -189,7 +190,7 @@ inline void VoronoiSimplex::setBarycentricCoords(decimal a, decimal b, decimal c } // Compute the closest point "v" to the origin of the current simplex. -inline bool VoronoiSimplex::computeClosestPoint(Vector3& v) { +RP3D_FORCE_INLINE bool VoronoiSimplex::computeClosestPoint(Vector3& v) { bool isValid = recomputeClosestPoint(); v = mClosestPoint; @@ -197,7 +198,7 @@ inline bool VoronoiSimplex::computeClosestPoint(Vector3& v) { } // Return true if the -inline bool VoronoiSimplex::checkClosestPointValid() const { +RP3D_FORCE_INLINE bool VoronoiSimplex::checkClosestPointValid() const { return mBarycentricCoords[0] >= decimal(0.0) && mBarycentricCoords[1] >= decimal(0.0) && mBarycentricCoords[2] >= decimal(0.0) && mBarycentricCoords[3] >= decimal(0.0); } diff --git a/include/reactphysics3d/collision/narrowphase/NarrowPhaseAlgorithm.h b/include/reactphysics3d/collision/narrowphase/NarrowPhaseAlgorithm.h index c360b2bc..444b1744 100644 --- a/include/reactphysics3d/collision/narrowphase/NarrowPhaseAlgorithm.h +++ b/include/reactphysics3d/collision/narrowphase/NarrowPhaseAlgorithm.h @@ -106,7 +106,7 @@ class NarrowPhaseAlgorithm { #ifdef IS_RP3D_PROFILING_ENABLED // Set the profiler -inline void NarrowPhaseAlgorithm::setProfiler(Profiler* profiler) { +RP3D_FORCE_INLINE void NarrowPhaseAlgorithm::setProfiler(Profiler* profiler) { mProfiler = profiler; } diff --git a/include/reactphysics3d/collision/narrowphase/NarrowPhaseInfoBatch.h b/include/reactphysics3d/collision/narrowphase/NarrowPhaseInfoBatch.h index 73ce4e33..fd296536 100644 --- a/include/reactphysics3d/collision/narrowphase/NarrowPhaseInfoBatch.h +++ b/include/reactphysics3d/collision/narrowphase/NarrowPhaseInfoBatch.h @@ -28,6 +28,8 @@ // Libraries #include +#include +#include /// Namespace ReactPhysics3D namespace reactphysics3d { @@ -40,93 +42,143 @@ struct ContactPointInfo; // Struct NarrowPhaseInfoBatch /** - * This abstract structure collects all the potential collisions from the middle-phase algorithm - * that have to be tested during narrow-phase collision detection. There is an implementation of - * this class for each kind of collision detection test. For instance, one for sphere vs sphere, - * one for sphere vs capsule, ... + * This structure collects all the potential collisions from the middle-phase algorithm + * that have to be tested during narrow-phase collision detection. */ struct NarrowPhaseInfoBatch { + struct NarrowPhaseInfo { + + /// Broadphase overlapping pairs ids + uint64 overlappingPairId; + + /// Entity of the first collider to test collision with + Entity colliderEntity1; + + /// Entity of the second collider to test collision with + Entity colliderEntity2; + + /// Collision info of the previous frame + LastFrameCollisionInfo* lastFrameCollisionInfo; + + /// Memory allocator for the collision shape (Used to release TriangleShape memory in destructor) + MemoryAllocator* collisionShapeAllocator; + + /// Shape local to world transform of sphere 1 + Transform shape1ToWorldTransform; + + /// Shape local to world transform of sphere 2 + Transform shape2ToWorldTransform; + + /// Pointer to the first collision shapes to test collision with + CollisionShape* collisionShape1; + + /// Pointer to the second collision shapes to test collision with + CollisionShape* collisionShape2; + + /// True if we need to report contacts (false for triggers for instance) + bool reportContacts; + + /// Result of the narrow-phase collision detection test + bool isColliding; + + /// Number of contact points + uint8 nbContactPoints; + + /// Array of contact points created during the narrow-phase + ContactPointInfo contactPoints[NB_MAX_CONTACT_POINTS_IN_NARROWPHASE_INFO]; + + /// Constructor + NarrowPhaseInfo(uint64 pairId, Entity collider1, Entity collider2, LastFrameCollisionInfo* lastFrameInfo, MemoryAllocator& shapeAllocator, + const Transform& shape1ToWorldTransform, const Transform& shape2ToWorldTransform, CollisionShape* shape1, + CollisionShape* shape2, bool needToReportContacts) + : overlappingPairId(pairId), colliderEntity1(collider1), colliderEntity2(collider2), lastFrameCollisionInfo(lastFrameInfo), + collisionShapeAllocator(&shapeAllocator), shape1ToWorldTransform(shape1ToWorldTransform), + shape2ToWorldTransform(shape2ToWorldTransform), collisionShape1(shape1), + collisionShape2(shape2), reportContacts(needToReportContacts), isColliding(false), nbContactPoints(0) { + + } + }; + protected: - /// Memory allocator + /// Reference to the memory allocator MemoryAllocator& mMemoryAllocator; /// Reference to all the broad-phase overlapping pairs OverlappingPairs& mOverlappingPairs; /// Cached capacity - uint mCachedCapacity = 0; + uint32 mCachedCapacity = 0; public: - /// List of Broadphase overlapping pairs ids - List overlappingPairIds; - - /// List of pointers to the first colliders to test collision with - List colliderEntities1; - - /// List of pointers to the second colliders to test collision with - List colliderEntities2; - - /// List of pointers to the first collision shapes to test collision with - List collisionShapes1; - - /// List of pointers to the second collision shapes to test collision with - List collisionShapes2; - - /// List of transforms that maps from collision shape 1 local-space to world-space - List shape1ToWorldTransforms; - - /// List of transforms that maps from collision shape 2 local-space to world-space - List shape2ToWorldTransforms; - - /// True for each pair of objects that we need to report contacts (false for triggers for instance) - List reportContacts; - - /// Result of the narrow-phase collision detection test - List isColliding; - - /// List of contact points created during the narrow-phase - List> contactPoints; - - /// Memory allocators for the collision shape (Used to release TriangleShape memory in destructor) - List collisionShapeAllocators; - - /// Collision infos of the previous frame - List lastFrameCollisionInfos; + /// For each collision test, we keep some meta data + Array narrowPhaseInfos; /// Constructor - NarrowPhaseInfoBatch(MemoryAllocator& allocator, OverlappingPairs& overlappingPairs); + NarrowPhaseInfoBatch(OverlappingPairs& overlappingPairs, MemoryAllocator& allocator); /// Destructor - virtual ~NarrowPhaseInfoBatch(); - - /// Return the number of objects in the batch - uint getNbObjects() const; + ~NarrowPhaseInfoBatch(); /// Add shapes to be tested during narrow-phase collision detection into the batch - virtual void addNarrowPhaseInfo(uint64 pairId, uint64 pairIndex, Entity collider1, Entity collider2, CollisionShape* shape1, - CollisionShape* shape2, const Transform& shape1Transform, - const Transform& shape2Transform, bool needToReportContacts, MemoryAllocator& shapeAllocator); + void addNarrowPhaseInfo(uint64 pairId, Entity collider1, Entity collider2, CollisionShape* shape1, + CollisionShape* shape2, const Transform& shape1Transform, const Transform& shape2Transform, + bool needToReportContacts, LastFrameCollisionInfo* lastFrameInfo, MemoryAllocator& shapeAllocator); + + /// Return the number of objects in the batch + uint32 getNbObjects() const; /// Add a new contact point - virtual void addContactPoint(uint index, const Vector3& contactNormal, decimal penDepth, + void addContactPoint(uint32 index, const Vector3& contactNormal, decimal penDepth, const Vector3& localPt1, const Vector3& localPt2); /// Reset the remaining contact points void resetContactPoints(uint index); // Initialize the containers using cached capacity - virtual void reserveMemory(); + void reserveMemory(); /// Clear all the objects in the batch - virtual void clear(); + void clear(); }; /// Return the number of objects in the batch -inline uint NarrowPhaseInfoBatch::getNbObjects() const { - return overlappingPairIds.size(); +RP3D_FORCE_INLINE uint32 NarrowPhaseInfoBatch::getNbObjects() const { + return narrowPhaseInfos.size(); +} + +// Add shapes to be tested during narrow-phase collision detection into the batch +RP3D_FORCE_INLINE void NarrowPhaseInfoBatch::addNarrowPhaseInfo(uint64 pairId, Entity collider1, Entity collider2, CollisionShape* shape1, + CollisionShape* shape2, const Transform& shape1Transform, const Transform& shape2Transform, + bool needToReportContacts, LastFrameCollisionInfo* lastFrameInfo, MemoryAllocator& shapeAllocator) { + + // Create a meta data object + narrowPhaseInfos.emplace(pairId, collider1, collider2, lastFrameInfo, shapeAllocator, shape1Transform, shape2Transform, shape1, shape2, needToReportContacts); +} + +// Add a new contact point +RP3D_FORCE_INLINE void NarrowPhaseInfoBatch::addContactPoint(uint index, const Vector3& contactNormal, decimal penDepth, const Vector3& localPt1, const Vector3& localPt2) { + + assert(penDepth > decimal(0.0)); + + if (narrowPhaseInfos[index].nbContactPoints < NB_MAX_CONTACT_POINTS_IN_NARROWPHASE_INFO) { + + assert(contactNormal.length() > 0.8f); + + // Add it into the array of contact points + narrowPhaseInfos[index].contactPoints[narrowPhaseInfos[index].nbContactPoints].normal = contactNormal; + narrowPhaseInfos[index].contactPoints[narrowPhaseInfos[index].nbContactPoints].penetrationDepth = penDepth; + narrowPhaseInfos[index].contactPoints[narrowPhaseInfos[index].nbContactPoints].localPoint1 = localPt1; + narrowPhaseInfos[index].contactPoints[narrowPhaseInfos[index].nbContactPoints].localPoint2 = localPt2; + narrowPhaseInfos[index].nbContactPoints++; + } +} + +// Reset the remaining contact points +RP3D_FORCE_INLINE void NarrowPhaseInfoBatch::resetContactPoints(uint index) { + narrowPhaseInfos[index].nbContactPoints = 0; } } diff --git a/include/reactphysics3d/collision/narrowphase/NarrowPhaseInput.h b/include/reactphysics3d/collision/narrowphase/NarrowPhaseInput.h index 5e8b4d9b..b3770176 100644 --- a/include/reactphysics3d/collision/narrowphase/NarrowPhaseInput.h +++ b/include/reactphysics3d/collision/narrowphase/NarrowPhaseInput.h @@ -27,11 +27,9 @@ #define REACTPHYSICS3D_NARROW_PHASE_INPUT_H // Libraries -#include +#include #include -#include -#include -#include +#include /// Namespace ReactPhysics3D namespace reactphysics3d { @@ -55,9 +53,9 @@ class NarrowPhaseInput { private: - SphereVsSphereNarrowPhaseInfoBatch mSphereVsSphereBatch; - SphereVsCapsuleNarrowPhaseInfoBatch mSphereVsCapsuleBatch; - CapsuleVsCapsuleNarrowPhaseInfoBatch mCapsuleVsCapsuleBatch; + NarrowPhaseInfoBatch mSphereVsSphereBatch; + NarrowPhaseInfoBatch mSphereVsCapsuleBatch; + NarrowPhaseInfoBatch mCapsuleVsCapsuleBatch; NarrowPhaseInfoBatch mSphereVsConvexPolyhedronBatch; NarrowPhaseInfoBatch mCapsuleVsConvexPolyhedronBatch; NarrowPhaseInfoBatch mConvexPolyhedronVsConvexPolyhedronBatch; @@ -68,19 +66,19 @@ class NarrowPhaseInput { NarrowPhaseInput(MemoryAllocator& allocator, OverlappingPairs& overlappingPairs); /// Add shapes to be tested during narrow-phase collision detection into the batch - void addNarrowPhaseTest(uint64 pairId, uint64 pairIndex, Entity collider1, Entity collider2, CollisionShape* shape1, + void addNarrowPhaseTest(uint64 pairId, Entity collider1, Entity collider2, CollisionShape* shape1, CollisionShape* shape2, const Transform& shape1Transform, const Transform& shape2Transform, NarrowPhaseAlgorithmType narrowPhaseAlgorithmType, bool reportContacts, - MemoryAllocator& shapeAllocator); + LastFrameCollisionInfo* lastFrameInfo, MemoryAllocator& shapeAllocator); /// Get a reference to the sphere vs sphere batch - SphereVsSphereNarrowPhaseInfoBatch& getSphereVsSphereBatch(); + NarrowPhaseInfoBatch& getSphereVsSphereBatch(); /// Get a reference to the sphere vs capsule batch - SphereVsCapsuleNarrowPhaseInfoBatch& getSphereVsCapsuleBatch(); + NarrowPhaseInfoBatch& getSphereVsCapsuleBatch(); /// Get a reference to the capsule vs capsule batch - CapsuleVsCapsuleNarrowPhaseInfoBatch& getCapsuleVsCapsuleBatch(); + NarrowPhaseInfoBatch& getCapsuleVsCapsuleBatch(); /// Get a reference to the sphere vs convex polyhedron batch NarrowPhaseInfoBatch& getSphereVsConvexPolyhedronBatch(); @@ -100,34 +98,65 @@ class NarrowPhaseInput { // Get a reference to the sphere vs sphere batch contacts -inline SphereVsSphereNarrowPhaseInfoBatch& NarrowPhaseInput::getSphereVsSphereBatch() { +RP3D_FORCE_INLINE NarrowPhaseInfoBatch& NarrowPhaseInput::getSphereVsSphereBatch() { return mSphereVsSphereBatch; } // Get a reference to the sphere vs capsule batch contacts -inline SphereVsCapsuleNarrowPhaseInfoBatch& NarrowPhaseInput::getSphereVsCapsuleBatch() { +RP3D_FORCE_INLINE NarrowPhaseInfoBatch& NarrowPhaseInput::getSphereVsCapsuleBatch() { return mSphereVsCapsuleBatch; } // Get a reference to the capsule vs capsule batch contacts -inline CapsuleVsCapsuleNarrowPhaseInfoBatch& NarrowPhaseInput::getCapsuleVsCapsuleBatch() { +RP3D_FORCE_INLINE NarrowPhaseInfoBatch& NarrowPhaseInput::getCapsuleVsCapsuleBatch() { return mCapsuleVsCapsuleBatch; } // Get a reference to the sphere vs convex polyhedron batch contacts -inline NarrowPhaseInfoBatch& NarrowPhaseInput::getSphereVsConvexPolyhedronBatch() { +RP3D_FORCE_INLINE NarrowPhaseInfoBatch& NarrowPhaseInput::getSphereVsConvexPolyhedronBatch() { return mSphereVsConvexPolyhedronBatch; } // Get a reference to the capsule vs convex polyhedron batch contacts -inline NarrowPhaseInfoBatch& NarrowPhaseInput::getCapsuleVsConvexPolyhedronBatch() { +RP3D_FORCE_INLINE NarrowPhaseInfoBatch& NarrowPhaseInput::getCapsuleVsConvexPolyhedronBatch() { return mCapsuleVsConvexPolyhedronBatch; } // Get a reference to the convex polyhedron vs convex polyhedron batch contacts -inline NarrowPhaseInfoBatch& NarrowPhaseInput::getConvexPolyhedronVsConvexPolyhedronBatch() { +RP3D_FORCE_INLINE NarrowPhaseInfoBatch &NarrowPhaseInput::getConvexPolyhedronVsConvexPolyhedronBatch() { return mConvexPolyhedronVsConvexPolyhedronBatch; } +// Add shapes to be tested during narrow-phase collision detection into the batch +RP3D_FORCE_INLINE void NarrowPhaseInput::addNarrowPhaseTest(uint64 pairId, Entity collider1, Entity collider2, CollisionShape* shape1, CollisionShape* shape2, + const Transform& shape1Transform, const Transform& shape2Transform, + NarrowPhaseAlgorithmType narrowPhaseAlgorithmType, bool reportContacts, LastFrameCollisionInfo* lastFrameInfo, + MemoryAllocator& shapeAllocator) { + + switch (narrowPhaseAlgorithmType) { + case NarrowPhaseAlgorithmType::SphereVsSphere: + mSphereVsSphereBatch.addNarrowPhaseInfo(pairId, collider1, collider2, shape1, shape2, shape1Transform, shape2Transform, reportContacts, lastFrameInfo, shapeAllocator); + break; + case NarrowPhaseAlgorithmType::SphereVsCapsule: + mSphereVsCapsuleBatch.addNarrowPhaseInfo(pairId, collider1, collider2, shape1, shape2, shape1Transform, shape2Transform, reportContacts, lastFrameInfo, shapeAllocator); + break; + case NarrowPhaseAlgorithmType::CapsuleVsCapsule: + mCapsuleVsCapsuleBatch.addNarrowPhaseInfo(pairId, collider1, collider2, shape1, shape2, shape1Transform, shape2Transform, reportContacts, lastFrameInfo, shapeAllocator); + break; + case NarrowPhaseAlgorithmType::SphereVsConvexPolyhedron: + mSphereVsConvexPolyhedronBatch.addNarrowPhaseInfo(pairId, collider1, collider2, shape1, shape2, shape1Transform, shape2Transform, reportContacts, lastFrameInfo, shapeAllocator); + break; + case NarrowPhaseAlgorithmType::CapsuleVsConvexPolyhedron: + mCapsuleVsConvexPolyhedronBatch.addNarrowPhaseInfo(pairId, collider1, collider2, shape1, shape2, shape1Transform, shape2Transform, reportContacts, lastFrameInfo, shapeAllocator); + break; + case NarrowPhaseAlgorithmType::ConvexPolyhedronVsConvexPolyhedron: + mConvexPolyhedronVsConvexPolyhedronBatch.addNarrowPhaseInfo(pairId, collider1, collider2, shape1, shape2, shape1Transform, shape2Transform, reportContacts, lastFrameInfo, shapeAllocator); + break; + case NarrowPhaseAlgorithmType::None: + // Must never happen + assert(false); + break; + } +} } #endif diff --git a/include/reactphysics3d/collision/narrowphase/SAT/SATAlgorithm.h b/include/reactphysics3d/collision/narrowphase/SAT/SATAlgorithm.h index 9f8a4bef..9f486013 100644 --- a/include/reactphysics3d/collision/narrowphase/SAT/SATAlgorithm.h +++ b/include/reactphysics3d/collision/narrowphase/SAT/SATAlgorithm.h @@ -185,7 +185,7 @@ class SATAlgorithm { #ifdef IS_RP3D_PROFILING_ENABLED // Set the profiler -inline void SATAlgorithm::setProfiler(Profiler* profiler) { +RP3D_FORCE_INLINE void SATAlgorithm::setProfiler(Profiler* profiler) { mProfiler = profiler; } diff --git a/include/reactphysics3d/collision/narrowphase/SphereVsCapsuleAlgorithm.h b/include/reactphysics3d/collision/narrowphase/SphereVsCapsuleAlgorithm.h index 21c85918..49d27a4f 100644 --- a/include/reactphysics3d/collision/narrowphase/SphereVsCapsuleAlgorithm.h +++ b/include/reactphysics3d/collision/narrowphase/SphereVsCapsuleAlgorithm.h @@ -34,7 +34,7 @@ namespace reactphysics3d { // Declarations class ContactPoint; -struct SphereVsCapsuleNarrowPhaseInfoBatch; +struct NarrowPhaseInfoBatch; // Class SphereVsCapsuleAlgorithm /** @@ -65,7 +65,7 @@ class SphereVsCapsuleAlgorithm : public NarrowPhaseAlgorithm { SphereVsCapsuleAlgorithm& operator=(const SphereVsCapsuleAlgorithm& algorithm) = delete; /// Compute the narrow-phase collision detection between a sphere and a capsule - bool testCollision(SphereVsCapsuleNarrowPhaseInfoBatch& narrowPhaseInfoBatch, uint batchStartIndex, + bool testCollision(NarrowPhaseInfoBatch& narrowPhaseInfoBatch, uint batchStartIndex, uint batchNbItems, MemoryAllocator& memoryAllocator); }; diff --git a/include/reactphysics3d/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.h b/include/reactphysics3d/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.h index 264e9f5a..41a9a87e 100644 --- a/include/reactphysics3d/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.h +++ b/include/reactphysics3d/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.h @@ -34,6 +34,7 @@ namespace reactphysics3d { // Declarations class ContactPoint; +struct NarrowPhaseInfoBatch; // Class SphereVsConvexPolyhedronAlgorithm /** diff --git a/include/reactphysics3d/collision/narrowphase/SphereVsSphereAlgorithm.h b/include/reactphysics3d/collision/narrowphase/SphereVsSphereAlgorithm.h index e6f68e7a..85193f52 100644 --- a/include/reactphysics3d/collision/narrowphase/SphereVsSphereAlgorithm.h +++ b/include/reactphysics3d/collision/narrowphase/SphereVsSphereAlgorithm.h @@ -34,7 +34,7 @@ namespace reactphysics3d { // Declarations class ContactPoint; -struct SphereVsSphereNarrowPhaseInfoBatch; +struct NarrowPhaseInfoBatch; // Class SphereVsSphereAlgorithm /** @@ -65,7 +65,7 @@ class SphereVsSphereAlgorithm : public NarrowPhaseAlgorithm { SphereVsSphereAlgorithm& operator=(const SphereVsSphereAlgorithm& algorithm) = delete; /// Compute a contact info if the two bounding volume collide - bool testCollision(SphereVsSphereNarrowPhaseInfoBatch& narrowPhaseInfoBatch, uint batchStartIndex, + bool testCollision(NarrowPhaseInfoBatch& narrowPhaseInfoBatch, uint batchStartIndex, uint batchNbItems, MemoryAllocator& memoryAllocator); }; diff --git a/include/reactphysics3d/collision/shapes/AABB.h b/include/reactphysics3d/collision/shapes/AABB.h index 17aea61c..a4d9f921 100644 --- a/include/reactphysics3d/collision/shapes/AABB.h +++ b/include/reactphysics3d/collision/shapes/AABB.h @@ -110,7 +110,10 @@ class AABB { bool testCollisionTriangleAABB(const Vector3* trianglePoints) const; /// Return true if the ray intersects the AABB - bool testRayIntersect(const Ray& ray) const; + bool testRayIntersect(const Vector3& rayOrigin, const Vector3& rayDirectionInv, decimal rayMaxFraction) const; + + /// Compute the intersection of a ray and the AABB + bool raycast(const Ray& ray, Vector3& hitPoint) const; /// Apply a scale factor to the AABB void applyScale(const Vector3& scale); @@ -127,44 +130,44 @@ class AABB { }; // Return the center point of the AABB in world coordinates -inline Vector3 AABB::getCenter() const { +RP3D_FORCE_INLINE Vector3 AABB::getCenter() const { return (mMinCoordinates + mMaxCoordinates) * decimal(0.5); } // Return the minimum coordinates of the AABB -inline const Vector3& AABB::getMin() const { +RP3D_FORCE_INLINE const Vector3& AABB::getMin() const { return mMinCoordinates; } // Set the minimum coordinates of the AABB -inline void AABB::setMin(const Vector3& min) { +RP3D_FORCE_INLINE void AABB::setMin(const Vector3& min) { mMinCoordinates = min; } // Return the maximum coordinates of the AABB -inline const Vector3& AABB::getMax() const { +RP3D_FORCE_INLINE const Vector3& AABB::getMax() const { return mMaxCoordinates; } // Set the maximum coordinates of the AABB -inline void AABB::setMax(const Vector3& max) { +RP3D_FORCE_INLINE void AABB::setMax(const Vector3& max) { mMaxCoordinates = max; } // Return the size of the AABB in the three dimension x, y and z -inline Vector3 AABB::getExtent() const { +RP3D_FORCE_INLINE Vector3 AABB::getExtent() const { return mMaxCoordinates - mMinCoordinates; } // Inflate each side of the AABB by a given size -inline void AABB::inflate(decimal dx, decimal dy, decimal dz) { +RP3D_FORCE_INLINE void AABB::inflate(decimal dx, decimal dy, decimal dz) { mMaxCoordinates += Vector3(dx, dy, dz); mMinCoordinates -= Vector3(dx, dy, dz); } // Return true if the current AABB is overlapping with the AABB in argument. /// Two AABBs overlap if they overlap in the three x, y and z axis at the same time -inline bool AABB::testCollision(const AABB& aabb) const { +RP3D_FORCE_INLINE bool AABB::testCollision(const AABB& aabb) const { if (mMaxCoordinates.x < aabb.mMinCoordinates.x || aabb.mMaxCoordinates.x < mMinCoordinates.x) return false; if (mMaxCoordinates.y < aabb.mMinCoordinates.y || @@ -175,13 +178,13 @@ inline bool AABB::testCollision(const AABB& aabb) const { } // Return the volume of the AABB -inline decimal AABB::getVolume() const { +RP3D_FORCE_INLINE decimal AABB::getVolume() const { const Vector3 diff = mMaxCoordinates - mMinCoordinates; return (diff.x * diff.y * diff.z); } // Return true if the AABB of a triangle intersects the AABB -inline bool AABB::testCollisionTriangleAABB(const Vector3* trianglePoints) const { +RP3D_FORCE_INLINE bool AABB::testCollisionTriangleAABB(const Vector3* trianglePoints) const { if (min3(trianglePoints[0].x, trianglePoints[1].x, trianglePoints[2].x) > mMaxCoordinates.x) return false; if (min3(trianglePoints[0].y, trianglePoints[1].y, trianglePoints[2].y) > mMaxCoordinates.y) return false; @@ -195,7 +198,7 @@ inline bool AABB::testCollisionTriangleAABB(const Vector3* trianglePoints) const } // Return true if a point is inside the AABB -inline bool AABB::contains(const Vector3& point) const { +RP3D_FORCE_INLINE bool AABB::contains(const Vector3& point) const { return (point.x >= mMinCoordinates.x - MACHINE_EPSILON && point.x <= mMaxCoordinates.x + MACHINE_EPSILON && point.y >= mMinCoordinates.y - MACHINE_EPSILON && point.y <= mMaxCoordinates.y + MACHINE_EPSILON && @@ -203,13 +206,13 @@ inline bool AABB::contains(const Vector3& point) const { } // Apply a scale factor to the AABB -inline void AABB::applyScale(const Vector3& scale) { +RP3D_FORCE_INLINE void AABB::applyScale(const Vector3& scale) { mMinCoordinates = mMinCoordinates * scale; mMaxCoordinates = mMaxCoordinates * scale; } // Assignment operator -inline AABB& AABB::operator=(const AABB& aabb) { +RP3D_FORCE_INLINE AABB& AABB::operator=(const AABB& aabb) { if (this != &aabb) { mMinCoordinates = aabb.mMinCoordinates; mMaxCoordinates = aabb.mMaxCoordinates; @@ -217,6 +220,147 @@ inline AABB& AABB::operator=(const AABB& aabb) { return *this; } +// Merge the AABB in parameter with the current one +RP3D_FORCE_INLINE void AABB::mergeWithAABB(const AABB& aabb) { + mMinCoordinates.x = std::min(mMinCoordinates.x, aabb.mMinCoordinates.x); + mMinCoordinates.y = std::min(mMinCoordinates.y, aabb.mMinCoordinates.y); + mMinCoordinates.z = std::min(mMinCoordinates.z, aabb.mMinCoordinates.z); + + mMaxCoordinates.x = std::max(mMaxCoordinates.x, aabb.mMaxCoordinates.x); + mMaxCoordinates.y = std::max(mMaxCoordinates.y, aabb.mMaxCoordinates.y); + mMaxCoordinates.z = std::max(mMaxCoordinates.z, aabb.mMaxCoordinates.z); +} + +// Replace the current AABB with a new AABB that is the union of two AABBs in parameters +RP3D_FORCE_INLINE void AABB::mergeTwoAABBs(const AABB& aabb1, const AABB& aabb2) { + mMinCoordinates.x = std::min(aabb1.mMinCoordinates.x, aabb2.mMinCoordinates.x); + mMinCoordinates.y = std::min(aabb1.mMinCoordinates.y, aabb2.mMinCoordinates.y); + mMinCoordinates.z = std::min(aabb1.mMinCoordinates.z, aabb2.mMinCoordinates.z); + + mMaxCoordinates.x = std::max(aabb1.mMaxCoordinates.x, aabb2.mMaxCoordinates.x); + mMaxCoordinates.y = std::max(aabb1.mMaxCoordinates.y, aabb2.mMaxCoordinates.y); + mMaxCoordinates.z = std::max(aabb1.mMaxCoordinates.z, aabb2.mMaxCoordinates.z); +} + +// Return true if the current AABB contains the AABB given in parameter +RP3D_FORCE_INLINE bool AABB::contains(const AABB& aabb) const { + + bool isInside = true; + isInside = isInside && mMinCoordinates.x <= aabb.mMinCoordinates.x; + isInside = isInside && mMinCoordinates.y <= aabb.mMinCoordinates.y; + isInside = isInside && mMinCoordinates.z <= aabb.mMinCoordinates.z; + + isInside = isInside && mMaxCoordinates.x >= aabb.mMaxCoordinates.x; + isInside = isInside && mMaxCoordinates.y >= aabb.mMaxCoordinates.y; + isInside = isInside && mMaxCoordinates.z >= aabb.mMaxCoordinates.z; + return isInside; +} + +// Create and return an AABB for a triangle +RP3D_FORCE_INLINE AABB AABB::createAABBForTriangle(const Vector3* trianglePoints) { + + Vector3 minCoords(trianglePoints[0].x, trianglePoints[0].y, trianglePoints[0].z); + Vector3 maxCoords(trianglePoints[0].x, trianglePoints[0].y, trianglePoints[0].z); + + if (trianglePoints[1].x < minCoords.x) minCoords.x = trianglePoints[1].x; + if (trianglePoints[1].y < minCoords.y) minCoords.y = trianglePoints[1].y; + if (trianglePoints[1].z < minCoords.z) minCoords.z = trianglePoints[1].z; + + if (trianglePoints[2].x < minCoords.x) minCoords.x = trianglePoints[2].x; + if (trianglePoints[2].y < minCoords.y) minCoords.y = trianglePoints[2].y; + if (trianglePoints[2].z < minCoords.z) minCoords.z = trianglePoints[2].z; + + if (trianglePoints[1].x > maxCoords.x) maxCoords.x = trianglePoints[1].x; + if (trianglePoints[1].y > maxCoords.y) maxCoords.y = trianglePoints[1].y; + if (trianglePoints[1].z > maxCoords.z) maxCoords.z = trianglePoints[1].z; + + if (trianglePoints[2].x > maxCoords.x) maxCoords.x = trianglePoints[2].x; + if (trianglePoints[2].y > maxCoords.y) maxCoords.y = trianglePoints[2].y; + if (trianglePoints[2].z > maxCoords.z) maxCoords.z = trianglePoints[2].z; + + return AABB(minCoords, maxCoords); +} + +// Return true if the ray intersects the AABB +RP3D_FORCE_INLINE bool AABB::testRayIntersect(const Vector3& rayOrigin, const Vector3& rayDirectionInverse, decimal rayMaxFraction) const { + + // This algorithm relies on the IEE floating point properties (division by zero). If the rayDirection is zero, rayDirectionInverse and + // therfore t1 and t2 will be +-INFINITY. If the i coordinate of the ray's origin is inside the AABB (mMinCoordinates[i] < rayOrigin[i] < mMaxCordinates[i)), we have + // t1 = -t2 = +- INFINITY. Since max(n, -INFINITY) = min(n, INFINITY) = n for all n, tMin and tMax will stay unchanged. Secondly, if the i + // coordinate of the ray's origin is outside the box (rayOrigin[i] < mMinCoordinates[i] or rayOrigin[i] > mMaxCoordinates[i]) we have + // t1 = t2 = +- INFINITY and therefore either tMin = +INFINITY or tMax = -INFINITY. One of those values will stay until the end and make the + // method to return false. Unfortunately, if the ray lies exactly on a slab (rayOrigin[i] = mMinCoordinates[i] or rayOrigin[i] = mMaxCoordinates[i]) we + // have t1 = (mMinCoordinates[i] - rayOrigin[i]) * rayDirectionInverse[i] = 0 * INFINITY = NaN which is a problem for the remaining of the algorithm. + // This will cause the method to return true when the ray is not intersecting the AABB and therefore cause to traverse more nodes than necessary in + // the BVH tree. Because this should be rare, it is not really a big issue. + // Reference: https://tavianator.com/2011/ray_box.html + + decimal t1 = (mMinCoordinates[0] - rayOrigin[0]) * rayDirectionInverse[0]; + decimal t2 = (mMaxCoordinates[0] - rayOrigin[0]) * rayDirectionInverse[0]; + + decimal tMin = std::min(t1, t2); + decimal tMax = std::max(t1, t2); + tMax = std::min(tMax, rayMaxFraction); + + for (uint i = 1; i < 3; i++) { + + t1 = (mMinCoordinates[i] - rayOrigin[i]) * rayDirectionInverse[i]; + t2 = (mMaxCoordinates[i] - rayOrigin[i]) * rayDirectionInverse[i]; + + tMin = std::max(tMin, std::min(t1, t2)); + tMax = std::min(tMax, std::max(t1, t2)); + } + + return tMax >= std::max(tMin, decimal(0.0)); +} + +// Compute the intersection of a ray and the AABB +RP3D_FORCE_INLINE bool AABB::raycast(const Ray& ray, Vector3& hitPoint) const { + + decimal tMin = decimal(0.0); + decimal tMax = DECIMAL_LARGEST; + + const decimal epsilon = 0.00001; + + const Vector3 rayDirection = ray.point2 - ray.point1; + + // For all three slabs + for (int i=0; i < 3; i++) { + + // If the ray is parallel to the slab + if (std::abs(rayDirection[i]) < epsilon) { + + // If origin of the ray is not inside the slab, no hit + if (ray.point1[i] < mMinCoordinates[i] || ray.point1[i] > mMaxCoordinates[i]) return false; + } + else { + + decimal rayDirectionInverse = decimal(1.0) / rayDirection[i]; + decimal t1 = (mMinCoordinates[i] - ray.point1[i]) * rayDirectionInverse; + decimal t2 = (mMaxCoordinates[i] - ray.point1[i]) * rayDirectionInverse; + + if (t1 > t2) { + + // Swap t1 and t2 + decimal tTemp = t2; + t2 = t1; + t1 = tTemp; + } + + tMin = std::max(tMin, t1); + tMax = std::min(tMax, t2); + + // Exit with no collision + if (tMin > tMax) return false; + } + } + + // Compute the hit point + hitPoint = ray.point1 + tMin * rayDirection; + + return true; +} + } #endif diff --git a/include/reactphysics3d/collision/shapes/BoxShape.h b/include/reactphysics3d/collision/shapes/BoxShape.h index fc78c353..a94df5b5 100644 --- a/include/reactphysics3d/collision/shapes/BoxShape.h +++ b/include/reactphysics3d/collision/shapes/BoxShape.h @@ -36,6 +36,7 @@ namespace reactphysics3d { // Declarations class CollisionBody; class DefaultAllocator; +class PhysicsCommon; // Class BoxShape /** @@ -53,13 +54,13 @@ class BoxShape : public ConvexPolyhedronShape { /// Half-extents of the box in the x, y and z direction Vector3 mHalfExtents; - /// Half-edge structure of the polyhedron - HalfEdgeStructure mHalfEdgeStructure; + /// Reference to the physics common object + PhysicsCommon& mPhysicsCommon; // -------------------- Methods -------------------- // /// Constructor - BoxShape(const Vector3& halfExtents, MemoryAllocator& allocator); + BoxShape(const Vector3& halfExtents, MemoryAllocator& allocator, PhysicsCommon& physicsCommon); /// Return a local support point in a given direction without the object margin virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction) const override; @@ -140,7 +141,7 @@ class BoxShape : public ConvexPolyhedronShape { /** * @return The vector with the three half-extents of the box shape */ -inline Vector3 BoxShape::getHalfExtents() const { +RP3D_FORCE_INLINE Vector3 BoxShape::getHalfExtents() const { return mHalfExtents; } @@ -150,7 +151,7 @@ inline Vector3 BoxShape::getHalfExtents() const { /** * @param halfExtents The vector with the three half-extents of the box */ -inline void BoxShape::setHalfExtents(const Vector3& halfExtents) { +RP3D_FORCE_INLINE void BoxShape::setHalfExtents(const Vector3& halfExtents) { mHalfExtents = halfExtents; notifyColliderAboutChangedSize(); @@ -162,7 +163,7 @@ inline void BoxShape::setHalfExtents(const Vector3& halfExtents) { * @param min The minimum bounds of the shape in local-space coordinates * @param max The maximum bounds of the shape in local-space coordinates */ -inline void BoxShape::getLocalBounds(Vector3& min, Vector3& max) const { +RP3D_FORCE_INLINE void BoxShape::getLocalBounds(Vector3& min, Vector3& max) const { // Maximum bounds max = mHalfExtents; @@ -172,12 +173,12 @@ inline void BoxShape::getLocalBounds(Vector3& min, Vector3& max) const { } // Return the number of bytes used by the collision shape -inline size_t BoxShape::getSizeInBytes() const { +RP3D_FORCE_INLINE size_t BoxShape::getSizeInBytes() const { return sizeof(BoxShape); } // Return a local support point in a given direction without the object margin -inline Vector3 BoxShape::getLocalSupportPointWithoutMargin(const Vector3& direction) const { +RP3D_FORCE_INLINE Vector3 BoxShape::getLocalSupportPointWithoutMargin(const Vector3& direction) const { return Vector3(direction.x < decimal(0.0) ? -mHalfExtents.x : mHalfExtents.x, direction.y < decimal(0.0) ? -mHalfExtents.y : mHalfExtents.y, @@ -185,49 +186,35 @@ inline Vector3 BoxShape::getLocalSupportPointWithoutMargin(const Vector3& direct } // Return true if a point is inside the collision shape -inline bool BoxShape::testPointInside(const Vector3& localPoint, Collider* collider) const { +RP3D_FORCE_INLINE bool BoxShape::testPointInside(const Vector3& localPoint, Collider* collider) const { return (localPoint.x < mHalfExtents[0] && localPoint.x > -mHalfExtents[0] && localPoint.y < mHalfExtents[1] && localPoint.y > -mHalfExtents[1] && localPoint.z < mHalfExtents[2] && localPoint.z > -mHalfExtents[2]); } // Return the number of faces of the polyhedron -inline uint BoxShape::getNbFaces() const { +RP3D_FORCE_INLINE uint BoxShape::getNbFaces() const { return 6; } -// Return a given face of the polyhedron -inline const HalfEdgeStructure::Face& BoxShape::getFace(uint faceIndex) const { - assert(faceIndex < mHalfEdgeStructure.getNbFaces()); - return mHalfEdgeStructure.getFace(faceIndex); -} - // Return the number of vertices of the polyhedron -inline uint BoxShape::getNbVertices() const { +RP3D_FORCE_INLINE uint BoxShape::getNbVertices() const { return 8; } -// Return a given vertex of the polyhedron -inline HalfEdgeStructure::Vertex BoxShape::getVertex(uint vertexIndex) const { - assert(vertexIndex < getNbVertices()); - return mHalfEdgeStructure.getVertex(vertexIndex); -} - // Return the position of a given vertex -inline Vector3 BoxShape::getVertexPosition(uint vertexIndex) const { +RP3D_FORCE_INLINE Vector3 BoxShape::getVertexPosition(uint vertexIndex) const { assert(vertexIndex < getNbVertices()); - Vector3 extent = getHalfExtents(); - switch(vertexIndex) { - case 0: return Vector3(-extent.x, -extent.y, extent.z); - case 1: return Vector3(extent.x, -extent.y, extent.z); - case 2: return Vector3(extent.x, extent.y, extent.z); - case 3: return Vector3(-extent.x, extent.y, extent.z); - case 4: return Vector3(-extent.x, -extent.y, -extent.z); - case 5: return Vector3(extent.x, -extent.y, -extent.z); - case 6: return Vector3(extent.x, extent.y, -extent.z); - case 7: return Vector3(-extent.x, extent.y, -extent.z); + case 0: return Vector3(-mHalfExtents.x, -mHalfExtents.y, mHalfExtents.z); + case 1: return Vector3(mHalfExtents.x, -mHalfExtents.y, mHalfExtents.z); + case 2: return Vector3(mHalfExtents.x, mHalfExtents.y, mHalfExtents.z); + case 3: return Vector3(-mHalfExtents.x, mHalfExtents.y, mHalfExtents.z); + case 4: return Vector3(-mHalfExtents.x, -mHalfExtents.y, -mHalfExtents.z); + case 5: return Vector3(mHalfExtents.x, -mHalfExtents.y, -mHalfExtents.z); + case 6: return Vector3(mHalfExtents.x, mHalfExtents.y, -mHalfExtents.z); + case 7: return Vector3(-mHalfExtents.x, mHalfExtents.y, -mHalfExtents.z); } assert(false); @@ -235,7 +222,7 @@ inline Vector3 BoxShape::getVertexPosition(uint vertexIndex) const { } // Return the normal vector of a given face of the polyhedron -inline Vector3 BoxShape::getFaceNormal(uint faceIndex) const { +RP3D_FORCE_INLINE Vector3 BoxShape::getFaceNormal(uint faceIndex) const { assert(faceIndex < getNbFaces()); switch(faceIndex) { @@ -252,31 +239,25 @@ inline Vector3 BoxShape::getFaceNormal(uint faceIndex) const { } // Return the centroid of the box -inline Vector3 BoxShape::getCentroid() const { +RP3D_FORCE_INLINE Vector3 BoxShape::getCentroid() const { return Vector3::zero(); } // Compute and return the volume of the collision shape -inline decimal BoxShape::getVolume() const { +RP3D_FORCE_INLINE decimal BoxShape::getVolume() const { return 8 * mHalfExtents.x * mHalfExtents.y * mHalfExtents.z; } // Return the string representation of the shape -inline std::string BoxShape::to_string() const { +RP3D_FORCE_INLINE std::string BoxShape::to_string() const { return "BoxShape{extents=" + mHalfExtents.to_string() + "}"; } // Return the number of half-edges of the polyhedron -inline uint BoxShape::getNbHalfEdges() const { +RP3D_FORCE_INLINE uint BoxShape::getNbHalfEdges() const { return 24; } -// Return a given half-edge of the polyhedron -inline const HalfEdgeStructure::Edge& BoxShape::getHalfEdge(uint edgeIndex) const { - assert(edgeIndex < getNbHalfEdges()); - return mHalfEdgeStructure.getHalfEdge(edgeIndex); -} - } #endif diff --git a/include/reactphysics3d/collision/shapes/CapsuleShape.h b/include/reactphysics3d/collision/shapes/CapsuleShape.h index b9cfbc63..f8a54643 100644 --- a/include/reactphysics3d/collision/shapes/CapsuleShape.h +++ b/include/reactphysics3d/collision/shapes/CapsuleShape.h @@ -126,7 +126,7 @@ class CapsuleShape : public ConvexShape { /** * @return The radius of the capsule shape (in meters) */ -inline decimal CapsuleShape::getRadius() const { +RP3D_FORCE_INLINE decimal CapsuleShape::getRadius() const { return mMargin; } @@ -136,7 +136,7 @@ inline decimal CapsuleShape::getRadius() const { /** * @param radius The radius of the capsule (in meters) */ -inline void CapsuleShape::setRadius(decimal radius) { +RP3D_FORCE_INLINE void CapsuleShape::setRadius(decimal radius) { assert(radius > decimal(0.0)); mMargin = radius; @@ -148,7 +148,7 @@ inline void CapsuleShape::setRadius(decimal radius) { /** * @return The height of the capsule shape (in meters) */ -inline decimal CapsuleShape::getHeight() const { +RP3D_FORCE_INLINE decimal CapsuleShape::getHeight() const { return mHalfHeight + mHalfHeight; } @@ -158,7 +158,7 @@ inline decimal CapsuleShape::getHeight() const { /** * @param height The height of the capsule (in meters) */ -inline void CapsuleShape::setHeight(decimal height) { +RP3D_FORCE_INLINE void CapsuleShape::setHeight(decimal height) { assert(height > decimal(0.0)); mHalfHeight = height * decimal(0.5); @@ -167,7 +167,7 @@ inline void CapsuleShape::setHeight(decimal height) { } // Return the number of bytes used by the collision shape -inline size_t CapsuleShape::getSizeInBytes() const { +RP3D_FORCE_INLINE size_t CapsuleShape::getSizeInBytes() const { return sizeof(CapsuleShape); } @@ -177,7 +177,7 @@ inline size_t CapsuleShape::getSizeInBytes() const { * @param min The minimum bounds of the shape in local-space coordinates * @param max The maximum bounds of the shape in local-space coordinates */ -inline void CapsuleShape::getLocalBounds(Vector3& min, Vector3& max) const { +RP3D_FORCE_INLINE void CapsuleShape::getLocalBounds(Vector3& min, Vector3& max) const { // Maximum bounds max.x = mMargin; @@ -191,12 +191,12 @@ inline void CapsuleShape::getLocalBounds(Vector3& min, Vector3& max) const { } // Compute and return the volume of the collision shape -inline decimal CapsuleShape::getVolume() const { +RP3D_FORCE_INLINE decimal CapsuleShape::getVolume() const { return reactphysics3d::PI_RP3D * mMargin * mMargin * (decimal(4.0) * mMargin / decimal(3.0) + decimal(2.0) * mHalfHeight); } // Return true if the collision shape is a polyhedron -inline bool CapsuleShape::isPolyhedron() const { +RP3D_FORCE_INLINE bool CapsuleShape::isPolyhedron() const { return false; } @@ -207,7 +207,7 @@ inline bool CapsuleShape::isPolyhedron() const { /// Therefore, in this method, we compute the support points of both top and bottom spheres of /// the capsule and return the point with the maximum dot product with the direction vector. Note /// that the object margin is implicitly the radius and height of the capsule. -inline Vector3 CapsuleShape::getLocalSupportPointWithoutMargin(const Vector3& direction) const { +RP3D_FORCE_INLINE Vector3 CapsuleShape::getLocalSupportPointWithoutMargin(const Vector3& direction) const { // Support point top sphere decimal dotProductTop = mHalfHeight * direction.y; @@ -225,7 +225,7 @@ inline Vector3 CapsuleShape::getLocalSupportPointWithoutMargin(const Vector3& di } // Return the string representation of the shape -inline std::string CapsuleShape::to_string() const { +RP3D_FORCE_INLINE std::string CapsuleShape::to_string() const { return "CapsuleShape{halfHeight=" + std::to_string(mHalfHeight) + ", radius=" + std::to_string(getRadius()) + "}"; } diff --git a/include/reactphysics3d/collision/shapes/CollisionShape.h b/include/reactphysics3d/collision/shapes/CollisionShape.h index 0fdca0b4..4c8c5561 100644 --- a/include/reactphysics3d/collision/shapes/CollisionShape.h +++ b/include/reactphysics3d/collision/shapes/CollisionShape.h @@ -30,7 +30,7 @@ #include #include #include -#include +#include /// ReactPhysics3D namespace namespace reactphysics3d { @@ -76,8 +76,8 @@ class CollisionShape { /// Unique identifier of the shape inside an overlapping pair uint32 mId; - /// List of the colliders associated with this shape - List mColliders; + /// Array of the colliders associated with this shape + Array mColliders; #ifdef IS_RP3D_PROFILING_ENABLED @@ -172,7 +172,7 @@ class CollisionShape { /** * @return The name of the collision shape (box, sphere, triangle, ...) */ -inline CollisionShapeName CollisionShape::getName() const { +RP3D_FORCE_INLINE CollisionShapeName CollisionShape::getName() const { return mName; } @@ -180,29 +180,29 @@ inline CollisionShapeName CollisionShape::getName() const { /** * @return The type of the collision shape (sphere, capsule, convex polyhedron, concave mesh) */ -inline CollisionShapeType CollisionShape::getType() const { +RP3D_FORCE_INLINE CollisionShapeType CollisionShape::getType() const { return mType; } // Return the id of the shape -inline uint32 CollisionShape::getId() const { +RP3D_FORCE_INLINE uint32 CollisionShape::getId() const { return mId; } // Assign a new collider to the collision shape -inline void CollisionShape::addCollider(Collider* collider) { +RP3D_FORCE_INLINE void CollisionShape::addCollider(Collider* collider) { mColliders.add(collider); } // Remove an assigned collider from the collision shape -inline void CollisionShape::removeCollider(Collider* collider) { +RP3D_FORCE_INLINE void CollisionShape::removeCollider(Collider* collider) { mColliders.remove(collider); } #ifdef IS_RP3D_PROFILING_ENABLED // Set the profiler -inline void CollisionShape::setProfiler(Profiler* profiler) { +RP3D_FORCE_INLINE void CollisionShape::setProfiler(Profiler* profiler) { mProfiler = profiler; } diff --git a/include/reactphysics3d/collision/shapes/ConcaveMeshShape.h b/include/reactphysics3d/collision/shapes/ConcaveMeshShape.h index 3ecfc5e0..af2112b3 100644 --- a/include/reactphysics3d/collision/shapes/ConcaveMeshShape.h +++ b/include/reactphysics3d/collision/shapes/ConcaveMeshShape.h @@ -29,7 +29,7 @@ // Libraries #include #include -#include +#include namespace reactphysics3d { @@ -72,7 +72,7 @@ class ConcaveMeshRaycastCallback : public DynamicAABBTreeRaycastCallback { private : - List mHitAABBNodes; + Array mHitAABBNodes; const DynamicAABBTree& mDynamicAABBTree; const ConcaveMeshShape& mConcaveMeshShape; Collider* mCollider; @@ -142,10 +142,13 @@ class ConcaveMeshShape : public ConcaveShape { /// if the user did not provide its own vertices normals) Vector3** mComputedVerticesNormals; + /// Reference to the triangle half-edge structure + HalfEdgeStructure& mTriangleHalfEdgeStructure; + // -------------------- Methods -------------------- // /// Constructor - ConcaveMeshShape(TriangleMesh* triangleMesh, MemoryAllocator& allocator, const Vector3& scaling = Vector3(1, 1, 1)); + ConcaveMeshShape(TriangleMesh* triangleMesh, MemoryAllocator& allocator, HalfEdgeStructure& triangleHalfEdgeStructure, const Vector3& scaling = Vector3(1, 1, 1)); /// Raycast method with feedback information virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, Collider* collider, MemoryAllocator& allocator) const override; @@ -156,7 +159,7 @@ class ConcaveMeshShape : public ConcaveShape { /// Insert all the triangles into the dynamic AABB tree void initBVHTree(); - /// Return the three vertices coordinates (in the list outTriangleVertices) of a triangle + /// Return the three vertices coordinates (in the array outTriangleVertices) of a triangle void getTriangleVertices(uint subPart, uint triangleIndex, Vector3* outTriangleVertices) const; /// Return the three vertex normals (in the array outVerticesNormals) of a triangle @@ -166,8 +169,8 @@ class ConcaveMeshShape : public ConcaveShape { uint computeTriangleShapeId(uint subPart, uint triangleIndex) const; /// Compute all the triangles of the mesh that are overlapping with the AABB in parameter - virtual void computeOverlappingTriangles(const AABB& localAABB, List& triangleVertices, - List &triangleVerticesNormals, List& shapeIds, + virtual void computeOverlappingTriangles(const AABB& localAABB, Array& triangleVertices, + Array &triangleVerticesNormals, Array& shapeIds, MemoryAllocator& allocator) const override; /// Destructor @@ -212,7 +215,7 @@ class ConcaveMeshShape : public ConcaveShape { }; // Return the number of bytes used by the collision shape -inline size_t ConcaveMeshShape::getSizeInBytes() const { +RP3D_FORCE_INLINE size_t ConcaveMeshShape::getSizeInBytes() const { return sizeof(ConcaveMeshShape); } @@ -222,7 +225,7 @@ inline size_t ConcaveMeshShape::getSizeInBytes() const { * @param min The minimum bounds of the shape in local-space coordinates * @param max The maximum bounds of the shape in local-space coordinates */ -inline void ConcaveMeshShape::getLocalBounds(Vector3& min, Vector3& max) const { +RP3D_FORCE_INLINE void ConcaveMeshShape::getLocalBounds(Vector3& min, Vector3& max) const { // Get the AABB of the whole tree AABB treeAABB = mDynamicAABBTree.getRootAABB(); @@ -233,7 +236,7 @@ inline void ConcaveMeshShape::getLocalBounds(Vector3& min, Vector3& max) const { // Called when a overlapping node has been found during the call to // DynamicAABBTree:reportAllShapesOverlappingWithAABB() -inline void ConvexTriangleAABBOverlapCallback::notifyOverlappingNode(int nodeId) { +RP3D_FORCE_INLINE void ConvexTriangleAABBOverlapCallback::notifyOverlappingNode(int nodeId) { // Get the node data (triangle index and mesh subpart index) int32* data = mDynamicAABBTree.getNodeDataInt(nodeId); @@ -253,7 +256,7 @@ inline void ConvexTriangleAABBOverlapCallback::notifyOverlappingNode(int nodeId) #ifdef IS_RP3D_PROFILING_ENABLED // Set the profiler -inline void ConcaveMeshShape::setProfiler(Profiler* profiler) { +RP3D_FORCE_INLINE void ConcaveMeshShape::setProfiler(Profiler* profiler) { CollisionShape::setProfiler(profiler); diff --git a/include/reactphysics3d/collision/shapes/ConcaveShape.h b/include/reactphysics3d/collision/shapes/ConcaveShape.h index d4ec18eb..b49f2e27 100644 --- a/include/reactphysics3d/collision/shapes/ConcaveShape.h +++ b/include/reactphysics3d/collision/shapes/ConcaveShape.h @@ -111,8 +111,8 @@ class ConcaveShape : public CollisionShape { virtual bool isPolyhedron() const override; /// Use a callback method on all triangles of the concave shape inside a given AABB - virtual void computeOverlappingTriangles(const AABB& localAABB, List& triangleVertices, - List& triangleVerticesNormals, List& shapeIds, + virtual void computeOverlappingTriangles(const AABB& localAABB, Array& triangleVertices, + Array& triangleVerticesNormals, Array& shapeIds, MemoryAllocator& allocator) const=0; /// Compute and return the volume of the collision shape @@ -120,22 +120,22 @@ class ConcaveShape : public CollisionShape { }; // Return true if the collision shape is convex, false if it is concave -inline bool ConcaveShape::isConvex() const { +RP3D_FORCE_INLINE bool ConcaveShape::isConvex() const { return false; } // Return true if the collision shape is a polyhedron -inline bool ConcaveShape::isPolyhedron() const { +RP3D_FORCE_INLINE bool ConcaveShape::isPolyhedron() const { return true; } // Return true if a point is inside the collision shape -inline bool ConcaveShape::testPointInside(const Vector3& localPoint, Collider* collider) const { +RP3D_FORCE_INLINE bool ConcaveShape::testPointInside(const Vector3& localPoint, Collider* collider) const { return false; } // Return the raycast test type (front, back, front-back) -inline TriangleRaycastSide ConcaveShape::getRaycastTestType() const { +RP3D_FORCE_INLINE TriangleRaycastSide ConcaveShape::getRaycastTestType() const { return mRaycastTestType; } @@ -143,19 +143,19 @@ inline TriangleRaycastSide ConcaveShape::getRaycastTestType() const { /** * @param testType Raycast test type for the triangle (front, back, front-back) */ -inline void ConcaveShape::setRaycastTestType(TriangleRaycastSide testType) { +RP3D_FORCE_INLINE void ConcaveShape::setRaycastTestType(TriangleRaycastSide testType) { mRaycastTestType = testType; } // Return the scale of the shape -inline const Vector3& ConcaveShape::getScale() const { +RP3D_FORCE_INLINE const Vector3& ConcaveShape::getScale() const { return mScale; } // Set the scale of the shape /// Note that you might want to recompute the inertia tensor and center of mass of the body /// after changing the scale of a collision shape -inline void ConcaveShape::setScale(const Vector3& scale) { +RP3D_FORCE_INLINE void ConcaveShape::setScale(const Vector3& scale) { mScale = scale; notifyColliderAboutChangedSize(); @@ -165,7 +165,7 @@ inline void ConcaveShape::setScale(const Vector3& scale) { /** * @param mass Mass to use to compute the inertia tensor of the collision shape */ -inline Vector3 ConcaveShape::getLocalInertiaTensor(decimal mass) const { +RP3D_FORCE_INLINE Vector3 ConcaveShape::getLocalInertiaTensor(decimal mass) const { // Default inertia tensor // Note that this is not very realistic for a concave triangle mesh. diff --git a/include/reactphysics3d/collision/shapes/ConvexMeshShape.h b/include/reactphysics3d/collision/shapes/ConvexMeshShape.h index 0cbb5c43..f8a629e0 100644 --- a/include/reactphysics3d/collision/shapes/ConvexMeshShape.h +++ b/include/reactphysics3d/collision/shapes/ConvexMeshShape.h @@ -147,19 +147,19 @@ class ConvexMeshShape : public ConvexPolyhedronShape { }; // Return the number of bytes used by the collision shape -inline size_t ConvexMeshShape::getSizeInBytes() const { +RP3D_FORCE_INLINE size_t ConvexMeshShape::getSizeInBytes() const { return sizeof(ConvexMeshShape); } // Return the scaling vector -inline const Vector3& ConvexMeshShape::getScale() const { +RP3D_FORCE_INLINE const Vector3& ConvexMeshShape::getScale() const { return mScale; } // Set the scale /// Note that you might want to recompute the inertia tensor and center of mass of the body /// after changing the scale of a collision shape -inline void ConvexMeshShape::setScale(const Vector3& scale) { +RP3D_FORCE_INLINE void ConvexMeshShape::setScale(const Vector3& scale) { mScale = scale; recalculateBounds(); notifyColliderAboutChangedSize(); @@ -170,7 +170,7 @@ inline void ConvexMeshShape::setScale(const Vector3& scale) { * @param min The minimum bounds of the shape in local-space coordinates * @param max The maximum bounds of the shape in local-space coordinates */ -inline void ConvexMeshShape::getLocalBounds(Vector3& min, Vector3& max) const { +RP3D_FORCE_INLINE void ConvexMeshShape::getLocalBounds(Vector3& min, Vector3& max) const { min = mMinBounds; max = mMaxBounds; } @@ -181,7 +181,7 @@ inline void ConvexMeshShape::getLocalBounds(Vector3& min, Vector3& max) const { /** * @param mass Mass to use to compute the inertia tensor of the collision shape */ -inline Vector3 ConvexMeshShape::getLocalInertiaTensor(decimal mass) const { +RP3D_FORCE_INLINE Vector3 ConvexMeshShape::getLocalInertiaTensor(decimal mass) const { const decimal factor = (decimal(1.0) / decimal(3.0)) * mass; const Vector3 realExtent = decimal(0.5) * (mMaxBounds - mMinBounds); assert(realExtent.x > 0 && realExtent.y > 0 && realExtent.z > 0); @@ -192,58 +192,58 @@ inline Vector3 ConvexMeshShape::getLocalInertiaTensor(decimal mass) const { } // Return the number of faces of the polyhedron -inline uint ConvexMeshShape::getNbFaces() const { +RP3D_FORCE_INLINE uint ConvexMeshShape::getNbFaces() const { return mPolyhedronMesh->getHalfEdgeStructure().getNbFaces(); } // Return a given face of the polyhedron -inline const HalfEdgeStructure::Face& ConvexMeshShape::getFace(uint faceIndex) const { +RP3D_FORCE_INLINE const HalfEdgeStructure::Face& ConvexMeshShape::getFace(uint faceIndex) const { assert(faceIndex < getNbFaces()); return mPolyhedronMesh->getHalfEdgeStructure().getFace(faceIndex); } // Return the number of vertices of the polyhedron -inline uint ConvexMeshShape::getNbVertices() const { +RP3D_FORCE_INLINE uint ConvexMeshShape::getNbVertices() const { return mPolyhedronMesh->getHalfEdgeStructure().getNbVertices(); } // Return a given vertex of the polyhedron -inline HalfEdgeStructure::Vertex ConvexMeshShape::getVertex(uint vertexIndex) const { +RP3D_FORCE_INLINE HalfEdgeStructure::Vertex ConvexMeshShape::getVertex(uint vertexIndex) const { assert(vertexIndex < getNbVertices()); return mPolyhedronMesh->getHalfEdgeStructure().getVertex(vertexIndex); } // Return the number of half-edges of the polyhedron -inline uint ConvexMeshShape::getNbHalfEdges() const { +RP3D_FORCE_INLINE uint ConvexMeshShape::getNbHalfEdges() const { return mPolyhedronMesh->getHalfEdgeStructure().getNbHalfEdges(); } // Return a given half-edge of the polyhedron -inline const HalfEdgeStructure::Edge& ConvexMeshShape::getHalfEdge(uint edgeIndex) const { +RP3D_FORCE_INLINE const HalfEdgeStructure::Edge& ConvexMeshShape::getHalfEdge(uint edgeIndex) const { assert(edgeIndex < getNbHalfEdges()); return mPolyhedronMesh->getHalfEdgeStructure().getHalfEdge(edgeIndex); } // Return the position of a given vertex -inline Vector3 ConvexMeshShape::getVertexPosition(uint vertexIndex) const { +RP3D_FORCE_INLINE Vector3 ConvexMeshShape::getVertexPosition(uint vertexIndex) const { assert(vertexIndex < getNbVertices()); return mPolyhedronMesh->getVertex(vertexIndex) * mScale; } // Return the normal vector of a given face of the polyhedron -inline Vector3 ConvexMeshShape::getFaceNormal(uint faceIndex) const { +RP3D_FORCE_INLINE Vector3 ConvexMeshShape::getFaceNormal(uint faceIndex) const { assert(faceIndex < getNbFaces()); return mPolyhedronMesh->getFaceNormal(faceIndex); } // Return the centroid of the polyhedron -inline Vector3 ConvexMeshShape::getCentroid() const { +RP3D_FORCE_INLINE Vector3 ConvexMeshShape::getCentroid() const { return mPolyhedronMesh->getCentroid() * mScale; } // Compute and return the volume of the collision shape -inline decimal ConvexMeshShape::getVolume() const { +RP3D_FORCE_INLINE decimal ConvexMeshShape::getVolume() const { return mPolyhedronMesh->getVolume(); } diff --git a/include/reactphysics3d/collision/shapes/ConvexPolyhedronShape.h b/include/reactphysics3d/collision/shapes/ConvexPolyhedronShape.h index 6a037cf8..68dce6be 100644 --- a/include/reactphysics3d/collision/shapes/ConvexPolyhedronShape.h +++ b/include/reactphysics3d/collision/shapes/ConvexPolyhedronShape.h @@ -94,11 +94,34 @@ class ConvexPolyhedronShape : public ConvexShape { }; // Return true if the collision shape is a polyhedron -inline bool ConvexPolyhedronShape::isPolyhedron() const { +RP3D_FORCE_INLINE bool ConvexPolyhedronShape::isPolyhedron() const { return true; } +// Find and return the index of the polyhedron face with the most anti-parallel face +// normal given a direction vector. This is used to find the incident face on +// a polyhedron of a given reference face of another polyhedron +RP3D_FORCE_INLINE uint ConvexPolyhedronShape::findMostAntiParallelFace(const Vector3& direction) const { + + decimal minDotProduct = DECIMAL_LARGEST; + uint mostAntiParallelFace = 0; + + // For each face of the polyhedron + const uint32 nbFaces = getNbFaces(); + for (uint32 i=0; i < nbFaces; i++) { + + // Get the face normal + const decimal dotProduct = getFaceNormal(i).dot(direction); + if (dotProduct < minDotProduct) { + minDotProduct = dotProduct; + mostAntiParallelFace = i; + } + } + + return mostAntiParallelFace; +} + } #endif diff --git a/include/reactphysics3d/collision/shapes/ConvexShape.h b/include/reactphysics3d/collision/shapes/ConvexShape.h index 9335d662..ddb1c467 100644 --- a/include/reactphysics3d/collision/shapes/ConvexShape.h +++ b/include/reactphysics3d/collision/shapes/ConvexShape.h @@ -83,7 +83,7 @@ class ConvexShape : public CollisionShape { }; // Return true if the collision shape is convex, false if it is concave -inline bool ConvexShape::isConvex() const { +RP3D_FORCE_INLINE bool ConvexShape::isConvex() const { return true; } @@ -91,7 +91,7 @@ inline bool ConvexShape::isConvex() const { /** * @return The margin (in meters) around the collision shape */ -inline decimal ConvexShape::getMargin() const { +RP3D_FORCE_INLINE decimal ConvexShape::getMargin() const { return mMargin; } diff --git a/include/reactphysics3d/collision/shapes/HeightFieldShape.h b/include/reactphysics3d/collision/shapes/HeightFieldShape.h index adb4c947..f706fc8d 100644 --- a/include/reactphysics3d/collision/shapes/HeightFieldShape.h +++ b/include/reactphysics3d/collision/shapes/HeightFieldShape.h @@ -91,14 +91,21 @@ class HeightFieldShape : public ConcaveShape { /// Local AABB of the height field (without scaling) AABB mAABB; + /// Reference to the half-edge structure + HalfEdgeStructure& mTriangleHalfEdgeStructure; + // -------------------- Methods -------------------- // /// Constructor HeightFieldShape(int nbGridColumns, int nbGridRows, decimal minHeight, decimal maxHeight, const void* heightFieldData, HeightDataType dataType, MemoryAllocator& allocator, - int upAxis = 1, decimal integerHeightScale = 1.0f, + HalfEdgeStructure& triangleHalfEdgeStructure, int upAxis = 1, decimal integerHeightScale = 1.0f, const Vector3& scaling = Vector3(1,1,1)); + /// Raycast a single triangle of the height-field + bool raycastTriangle(const Ray& ray, const Vector3& p1, const Vector3& p2, const Vector3& p3, uint shapeId, + Collider *collider, RaycastInfo& raycastInfo, decimal &smallestHitFraction, MemoryAllocator& allocator) const; + /// Raycast method with feedback information virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, Collider* collider, MemoryAllocator& allocator) const override; @@ -122,6 +129,9 @@ class HeightFieldShape : public ConcaveShape { /// Compute the shape Id for a given triangle uint computeTriangleShapeId(uint iIndex, uint jIndex, uint secondTriangleIncrement) const; + /// Compute the first grid cell of the heightfield intersected by a ray + bool computeEnteringRayGridCoordinates(const Ray& ray, int& i, int& j, Vector3& outHitPoint) const; + /// Destructor virtual ~HeightFieldShape() override = default; @@ -152,8 +162,8 @@ class HeightFieldShape : public ConcaveShape { virtual void getLocalBounds(Vector3& min, Vector3& max) const override; /// Use a callback method on all triangles of the concave shape inside a given AABB - virtual void computeOverlappingTriangles(const AABB& localAABB, List& triangleVertices, - List& triangleVerticesNormals, List& shapeIds, + virtual void computeOverlappingTriangles(const AABB& localAABB, Array& triangleVertices, + Array& triangleVerticesNormals, Array& shapeIds, MemoryAllocator& allocator) const override; /// Return the string representation of the shape @@ -167,27 +177,27 @@ class HeightFieldShape : public ConcaveShape { }; // Return the number of rows in the height field -inline int HeightFieldShape::getNbRows() const { +RP3D_FORCE_INLINE int HeightFieldShape::getNbRows() const { return mNbRows; } // Return the number of columns in the height field -inline int HeightFieldShape::getNbColumns() const { +RP3D_FORCE_INLINE int HeightFieldShape::getNbColumns() const { return mNbColumns; } // Return the type of height value in the height field -inline HeightFieldShape::HeightDataType HeightFieldShape::getHeightDataType() const { +RP3D_FORCE_INLINE HeightFieldShape::HeightDataType HeightFieldShape::getHeightDataType() const { return mHeightDataType; } // Return the number of bytes used by the collision shape -inline size_t HeightFieldShape::getSizeInBytes() const { +RP3D_FORCE_INLINE size_t HeightFieldShape::getSizeInBytes() const { return sizeof(HeightFieldShape); } // Return the height of a given (x,y) point in the height field -inline decimal HeightFieldShape::getHeightAt(int x, int y) const { +RP3D_FORCE_INLINE decimal HeightFieldShape::getHeightAt(int x, int y) const { assert(x >= 0 && x < mNbColumns); assert(y >= 0 && y < mNbRows); @@ -201,12 +211,12 @@ inline decimal HeightFieldShape::getHeightAt(int x, int y) const { } // Return the closest inside integer grid value of a given floating grid value -inline int HeightFieldShape::computeIntegerGridValue(decimal value) const { +RP3D_FORCE_INLINE int HeightFieldShape::computeIntegerGridValue(decimal value) const { return (value < decimal(0.0)) ? value - decimal(0.5) : value + decimal(0.5); } // Compute the shape Id for a given triangle -inline uint HeightFieldShape::computeTriangleShapeId(uint iIndex, uint jIndex, uint secondTriangleIncrement) const { +RP3D_FORCE_INLINE uint HeightFieldShape::computeTriangleShapeId(uint iIndex, uint jIndex, uint secondTriangleIncrement) const { return (jIndex * (mNbColumns - 1) + iIndex) * 2 + secondTriangleIncrement; } diff --git a/include/reactphysics3d/collision/shapes/SphereShape.h b/include/reactphysics3d/collision/shapes/SphereShape.h index 97126eec..6db4d4cd 100644 --- a/include/reactphysics3d/collision/shapes/SphereShape.h +++ b/include/reactphysics3d/collision/shapes/SphereShape.h @@ -111,7 +111,7 @@ class SphereShape : public ConvexShape { /** * @return Radius of the sphere */ -inline decimal SphereShape::getRadius() const { +RP3D_FORCE_INLINE decimal SphereShape::getRadius() const { return mMargin; } @@ -121,7 +121,7 @@ inline decimal SphereShape::getRadius() const { /** * @param radius Radius of the sphere */ -inline void SphereShape::setRadius(decimal radius) { +RP3D_FORCE_INLINE void SphereShape::setRadius(decimal radius) { assert(radius > decimal(0.0)); mMargin = radius; @@ -132,7 +132,7 @@ inline void SphereShape::setRadius(decimal radius) { /** * @return False because the sphere shape is not a polyhedron */ -inline bool SphereShape::isPolyhedron() const { +RP3D_FORCE_INLINE bool SphereShape::isPolyhedron() const { return false; } @@ -140,12 +140,12 @@ inline bool SphereShape::isPolyhedron() const { /** * @return The size (in bytes) of the sphere shape */ -inline size_t SphereShape::getSizeInBytes() const { +RP3D_FORCE_INLINE size_t SphereShape::getSizeInBytes() const { return sizeof(SphereShape); } // Return a local support point in a given direction without the object margin -inline Vector3 SphereShape::getLocalSupportPointWithoutMargin(const Vector3& direction) const { +RP3D_FORCE_INLINE Vector3 SphereShape::getLocalSupportPointWithoutMargin(const Vector3& direction) const { // Return the center of the sphere (the radius is taken into account in the object margin) return Vector3(0.0, 0.0, 0.0); @@ -157,7 +157,7 @@ inline Vector3 SphereShape::getLocalSupportPointWithoutMargin(const Vector3& dir * @param min The minimum bounds of the shape in local-space coordinates * @param max The maximum bounds of the shape in local-space coordinates */ -inline void SphereShape::getLocalBounds(Vector3& min, Vector3& max) const { +RP3D_FORCE_INLINE void SphereShape::getLocalBounds(Vector3& min, Vector3& max) const { // Maximum bounds max.x = mMargin; @@ -174,23 +174,23 @@ inline void SphereShape::getLocalBounds(Vector3& min, Vector3& max) const { /** * @param mass Mass to use to compute the inertia tensor of the collision shape */ -inline Vector3 SphereShape::getLocalInertiaTensor(decimal mass) const { +RP3D_FORCE_INLINE Vector3 SphereShape::getLocalInertiaTensor(decimal mass) const { decimal diag = decimal(0.4) * mass * mMargin * mMargin; return Vector3(diag, diag, diag); } // Compute and return the volume of the collision shape -inline decimal SphereShape::getVolume() const { +RP3D_FORCE_INLINE decimal SphereShape::getVolume() const { return decimal(4.0) / decimal(3.0) * reactphysics3d::PI_RP3D * mMargin * mMargin * mMargin; } // Return true if a point is inside the collision shape -inline bool SphereShape::testPointInside(const Vector3& localPoint, Collider* collider) const { +RP3D_FORCE_INLINE bool SphereShape::testPointInside(const Vector3& localPoint, Collider* collider) const { return (localPoint.lengthSquare() < mMargin * mMargin); } // Return the string representation of the shape -inline std::string SphereShape::to_string() const { +RP3D_FORCE_INLINE std::string SphereShape::to_string() const { return "SphereShape{radius=" + std::to_string(getRadius()) + "}"; } diff --git a/include/reactphysics3d/collision/shapes/TriangleShape.h b/include/reactphysics3d/collision/shapes/TriangleShape.h index 9bfc754d..e2e783d5 100644 --- a/include/reactphysics3d/collision/shapes/TriangleShape.h +++ b/include/reactphysics3d/collision/shapes/TriangleShape.h @@ -33,6 +33,9 @@ /// ReactPhysics3D namespace namespace reactphysics3d { +// Forward declarations +class PhysicsCommon; + /// Raycast test side for the triangle enum class TriangleRaycastSide { @@ -73,11 +76,8 @@ class TriangleShape : public ConvexPolyhedronShape { /// Raycast test type for the triangle (front, back, front-back) TriangleRaycastSide mRaycastTestType; - /// Faces information for the two faces of the triangle - HalfEdgeStructure::Face mFaces[2]; - - /// Edges information for the six edges of the triangle - HalfEdgeStructure::Edge mEdges[6]; + /// Reference to triangle half-edge structure + HalfEdgeStructure& mTriangleHalfEdgeStructure; // -------------------- Methods -------------------- // @@ -91,8 +91,7 @@ class TriangleShape : public ConvexPolyhedronShape { virtual bool testPointInside(const Vector3& localPoint, Collider* collider) const override; /// Raycast method with feedback information - virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, Collider* collider, - MemoryAllocator& allocator) const override; + virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, Collider* collider, MemoryAllocator& allocator) const override; /// Return the number of bytes used by the collision shape virtual size_t getSizeInBytes() const override; @@ -108,8 +107,11 @@ class TriangleShape : public ConvexPolyhedronShape { Vector3& outNewLocalContactPointOtherShape, Vector3& outSmoothWorldContactTriangleNormal) const; /// Constructor - TriangleShape(const Vector3* vertices, const Vector3* verticesNormals, - uint shapeId, MemoryAllocator& allocator); + TriangleShape(const Vector3* vertices, const Vector3* verticesNormals, uint shapeId, HalfEdgeStructure& triangleHalfEdgeStructure, + MemoryAllocator& allocator); + + /// Constructor + TriangleShape(const Vector3* vertices, uint shapeId, HalfEdgeStructure& triangleHalfEdgeStructure, MemoryAllocator& allocator); /// Destructor virtual ~TriangleShape() override = default; @@ -188,12 +190,12 @@ class TriangleShape : public ConvexPolyhedronShape { }; // Return the number of bytes used by the collision shape -inline size_t TriangleShape::getSizeInBytes() const { +RP3D_FORCE_INLINE size_t TriangleShape::getSizeInBytes() const { return sizeof(TriangleShape); } // Return a local support point in a given direction without the object margin -inline Vector3 TriangleShape::getLocalSupportPointWithoutMargin(const Vector3& direction) const { +RP3D_FORCE_INLINE Vector3 TriangleShape::getLocalSupportPointWithoutMargin(const Vector3& direction) const { Vector3 dotProducts(direction.dot(mPoints[0]), direction.dot(mPoints[1]), direction.dot(mPoints[2])); return mPoints[dotProducts.getMaxAxis()]; } @@ -204,7 +206,7 @@ inline Vector3 TriangleShape::getLocalSupportPointWithoutMargin(const Vector3& d * @param min The minimum bounds of the shape in local-space coordinates * @param max The maximum bounds of the shape in local-space coordinates */ -inline void TriangleShape::getLocalBounds(Vector3& min, Vector3& max) const { +RP3D_FORCE_INLINE void TriangleShape::getLocalBounds(Vector3& min, Vector3& max) const { const Vector3 xAxis(mPoints[0].x, mPoints[1].x, mPoints[2].x); const Vector3 yAxis(mPoints[0].y, mPoints[1].y, mPoints[2].y); @@ -222,33 +224,27 @@ inline void TriangleShape::getLocalBounds(Vector3& min, Vector3& max) const { * coordinates * @param mass Mass to use to compute the inertia tensor of the collision shape */ -inline Vector3 TriangleShape::getLocalInertiaTensor(decimal mass) const { +RP3D_FORCE_INLINE Vector3 TriangleShape::getLocalInertiaTensor(decimal mass) const { return Vector3(0, 0, 0); } // Return true if a point is inside the collision shape -inline bool TriangleShape::testPointInside(const Vector3& localPoint, Collider* collider) const { +RP3D_FORCE_INLINE bool TriangleShape::testPointInside(const Vector3& localPoint, Collider* collider) const { return false; } // Return the number of faces of the polyhedron -inline uint TriangleShape::getNbFaces() const { +RP3D_FORCE_INLINE uint TriangleShape::getNbFaces() const { return 2; } -// Return a given face of the polyhedron -inline const HalfEdgeStructure::Face& TriangleShape::getFace(uint faceIndex) const { - assert(faceIndex < 2); - return mFaces[faceIndex]; -} - // Return the number of vertices of the polyhedron -inline uint TriangleShape::getNbVertices() const { +RP3D_FORCE_INLINE uint TriangleShape::getNbVertices() const { return 3; } // Return a given vertex of the polyhedron -inline HalfEdgeStructure::Vertex TriangleShape::getVertex(uint vertexIndex) const { +RP3D_FORCE_INLINE HalfEdgeStructure::Vertex TriangleShape::getVertex(uint vertexIndex) const { assert(vertexIndex < 3); HalfEdgeStructure::Vertex vertex(vertexIndex); @@ -260,36 +256,31 @@ inline HalfEdgeStructure::Vertex TriangleShape::getVertex(uint vertexIndex) cons return vertex; } -// Return a given half-edge of the polyhedron -inline const HalfEdgeStructure::Edge& TriangleShape::getHalfEdge(uint edgeIndex) const { - assert(edgeIndex < getNbHalfEdges()); - return mEdges[edgeIndex]; -} - // Return the position of a given vertex -inline Vector3 TriangleShape::getVertexPosition(uint vertexIndex) const { +RP3D_FORCE_INLINE Vector3 TriangleShape::getVertexPosition(uint vertexIndex) const { assert(vertexIndex < 3); return mPoints[vertexIndex]; } // Return the normal vector of a given face of the polyhedron -inline Vector3 TriangleShape::getFaceNormal(uint faceIndex) const { +RP3D_FORCE_INLINE Vector3 TriangleShape::getFaceNormal(uint faceIndex) const { assert(faceIndex < 2); + assert(mNormal.length() > decimal(0.0)); return faceIndex == 0 ? mNormal : -mNormal; } // Return the centroid of the box -inline Vector3 TriangleShape::getCentroid() const { +RP3D_FORCE_INLINE Vector3 TriangleShape::getCentroid() const { return (mPoints[0] + mPoints[1] + mPoints[2]) / decimal(3.0); } // Return the number of half-edges of the polyhedron -inline uint TriangleShape::getNbHalfEdges() const { +RP3D_FORCE_INLINE uint TriangleShape::getNbHalfEdges() const { return 6; } // Return the raycast test type (front, back, front-back) -inline TriangleRaycastSide TriangleShape::getRaycastTestType() const { +RP3D_FORCE_INLINE TriangleRaycastSide TriangleShape::getRaycastTestType() const { return mRaycastTestType; } @@ -297,21 +288,100 @@ inline TriangleRaycastSide TriangleShape::getRaycastTestType() const { /** * @param testType Raycast test type for the triangle (front, back, front-back) */ -inline void TriangleShape::setRaycastTestType(TriangleRaycastSide testType) { +RP3D_FORCE_INLINE void TriangleShape::setRaycastTestType(TriangleRaycastSide testType) { mRaycastTestType = testType; } // Return the string representation of the shape -inline std::string TriangleShape::to_string() const { +RP3D_FORCE_INLINE std::string TriangleShape::to_string() const { return "TriangleShape{v1=" + mPoints[0].to_string() + ", v2=" + mPoints[1].to_string() + "," + "v3=" + mPoints[2].to_string() + "}"; } // Compute and return the volume of the collision shape -inline decimal TriangleShape::getVolume() const { +RP3D_FORCE_INLINE decimal TriangleShape::getVolume() const { return decimal(0.0); } +// Get a smooth contact normal for collision for a triangle of the mesh +/// This is used to avoid the internal edges issue that occurs when a shape is colliding with +/// several triangles of a concave mesh. If the shape collide with an edge of the triangle for instance, +/// the computed contact normal from this triangle edge is not necessarily in the direction of the surface +/// normal of the mesh at this point. The idea to solve this problem is to use the real (smooth) surface +/// normal of the mesh at this point as the contact normal. This technique is described in the chapter 5 +/// of the Game Physics Pearl book by Gino van der Bergen and Dirk Gregorius. The vertices normals of the +/// mesh are either provided by the user or precomputed if the user did not provide them. Note that we only +/// use the interpolated normal if the contact point is on an edge of the triangle. If the contact is in the +/// middle of the triangle, we return the true triangle normal. +RP3D_FORCE_INLINE Vector3 TriangleShape::computeSmoothLocalContactNormalForTriangle(const Vector3& localContactPoint) const { + + assert(mNormal.length() > decimal(0.0)); + + // Compute the barycentric coordinates of the point in the triangle + decimal u, v, w; + computeBarycentricCoordinatesInTriangle(mPoints[0], mPoints[1], mPoints[2], localContactPoint, u, v, w); + + // If the contact is in the middle of the triangle face (not on the edges) + if (u > MACHINE_EPSILON && v > MACHINE_EPSILON && w > MACHINE_EPSILON) { + + // We return the true triangle face normal (not the interpolated one) + return mNormal; + } + + // We compute the contact normal as the barycentric interpolation of the three vertices normals + const Vector3 interpolatedNormal = u * mVerticesNormals[0] + v * mVerticesNormals[1] + w * mVerticesNormals[2]; + + // If the interpolated normal is degenerated + if (interpolatedNormal.lengthSquare() < MACHINE_EPSILON) { + + // Return the original normal + return mNormal; + } + + return interpolatedNormal.getUnit(); +} + +// This method compute the smooth mesh contact with a triangle in case one of the two collision +// shapes is a triangle. The idea in this case is to use a smooth vertex normal of the triangle mesh +// at the contact point instead of the triangle normal to avoid the internal edge collision issue. +// This method will return the new smooth world contact +// normal of the triangle and the the local contact point on the other shape. +RP3D_FORCE_INLINE void TriangleShape::computeSmoothTriangleMeshContact(const CollisionShape* shape1, const CollisionShape* shape2, + Vector3& localContactPointShape1, Vector3& localContactPointShape2, + const Transform& shape1ToWorld, const Transform& shape2ToWorld, + decimal penetrationDepth, Vector3& outSmoothVertexNormal) { + + assert(shape1->getName() != CollisionShapeName::TRIANGLE || shape2->getName() != CollisionShapeName::TRIANGLE); + + // If one the shape is a triangle + bool isShape1Triangle = shape1->getName() == CollisionShapeName::TRIANGLE; + if (isShape1Triangle || shape2->getName() == CollisionShapeName::TRIANGLE) { + + const TriangleShape* triangleShape = isShape1Triangle ? static_cast(shape1): + static_cast(shape2); + + // Compute the smooth triangle mesh contact normal and recompute the local contact point on the other shape + triangleShape->computeSmoothMeshContact(isShape1Triangle ? localContactPointShape1 : localContactPointShape2, + isShape1Triangle ? shape1ToWorld : shape2ToWorld, + isShape1Triangle ? shape2ToWorld.getInverse() : shape1ToWorld.getInverse(), + penetrationDepth, isShape1Triangle, + isShape1Triangle ? localContactPointShape2 : localContactPointShape1, + outSmoothVertexNormal); + } +} + +// Return a given face of the polyhedron +RP3D_FORCE_INLINE const HalfEdgeStructure::Face& TriangleShape::getFace(uint faceIndex) const { + assert(faceIndex < 2); + return mTriangleHalfEdgeStructure.getFace(faceIndex); +} + +// Return a given half-edge of the polyhedron +RP3D_FORCE_INLINE const HalfEdgeStructure::Edge& TriangleShape::getHalfEdge(uint edgeIndex) const { + assert(edgeIndex < getNbHalfEdges()); + return mTriangleHalfEdgeStructure.getHalfEdge(edgeIndex); +} + } #endif diff --git a/include/reactphysics3d/components/BallAndSocketJointComponents.h b/include/reactphysics3d/components/BallAndSocketJointComponents.h index 7778bf10..75ec4081 100644 --- a/include/reactphysics3d/components/BallAndSocketJointComponents.h +++ b/include/reactphysics3d/components/BallAndSocketJointComponents.h @@ -188,140 +188,140 @@ class BallAndSocketJointComponents : public Components { }; // Return a pointer to a given joint -inline BallAndSocketJoint* BallAndSocketJointComponents::getJoint(Entity jointEntity) const { +RP3D_FORCE_INLINE BallAndSocketJoint* BallAndSocketJointComponents::getJoint(Entity jointEntity) const { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); return mJoints[mMapEntityToComponentIndex[jointEntity]]; } // Set the joint pointer to a given joint -inline void BallAndSocketJointComponents::setJoint(Entity jointEntity, BallAndSocketJoint* joint) const { +RP3D_FORCE_INLINE void BallAndSocketJointComponents::setJoint(Entity jointEntity, BallAndSocketJoint* joint) const { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); mJoints[mMapEntityToComponentIndex[jointEntity]] = joint; } // Return the local anchor point of body 1 for a given joint -inline const Vector3& BallAndSocketJointComponents::getLocalAnchorPointBody1(Entity jointEntity) const { +RP3D_FORCE_INLINE const Vector3& BallAndSocketJointComponents::getLocalAnchorPointBody1(Entity jointEntity) const { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); return mLocalAnchorPointBody1[mMapEntityToComponentIndex[jointEntity]]; } // Set the local anchor point of body 1 for a given joint -inline void BallAndSocketJointComponents::setLocalAnchorPointBody1(Entity jointEntity, const Vector3& localAnchoirPointBody1) { +RP3D_FORCE_INLINE void BallAndSocketJointComponents::setLocalAnchorPointBody1(Entity jointEntity, const Vector3& localAnchoirPointBody1) { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); mLocalAnchorPointBody1[mMapEntityToComponentIndex[jointEntity]] = localAnchoirPointBody1; } // Return the local anchor point of body 2 for a given joint -inline const Vector3& BallAndSocketJointComponents::getLocalAnchorPointBody2(Entity jointEntity) const { +RP3D_FORCE_INLINE const Vector3& BallAndSocketJointComponents::getLocalAnchorPointBody2(Entity jointEntity) const { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); return mLocalAnchorPointBody2[mMapEntityToComponentIndex[jointEntity]]; } // Set the local anchor point of body 2 for a given joint -inline void BallAndSocketJointComponents::setLocalAnchorPointBody2(Entity jointEntity, const Vector3& localAnchoirPointBody2) { +RP3D_FORCE_INLINE void BallAndSocketJointComponents::setLocalAnchorPointBody2(Entity jointEntity, const Vector3& localAnchoirPointBody2) { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); mLocalAnchorPointBody2[mMapEntityToComponentIndex[jointEntity]] = localAnchoirPointBody2; } // Return the vector from center of body 1 to anchor point in world-space -inline const Vector3& BallAndSocketJointComponents::getR1World(Entity jointEntity) const { +RP3D_FORCE_INLINE const Vector3& BallAndSocketJointComponents::getR1World(Entity jointEntity) const { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); return mR1World[mMapEntityToComponentIndex[jointEntity]]; } // Set the vector from center of body 1 to anchor point in world-space -inline void BallAndSocketJointComponents::setR1World(Entity jointEntity, const Vector3& r1World) { +RP3D_FORCE_INLINE void BallAndSocketJointComponents::setR1World(Entity jointEntity, const Vector3& r1World) { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); mR1World[mMapEntityToComponentIndex[jointEntity]] = r1World; } // Return the vector from center of body 2 to anchor point in world-space -inline const Vector3& BallAndSocketJointComponents::getR2World(Entity jointEntity) const { +RP3D_FORCE_INLINE const Vector3& BallAndSocketJointComponents::getR2World(Entity jointEntity) const { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); return mR2World[mMapEntityToComponentIndex[jointEntity]]; } // Set the vector from center of body 2 to anchor point in world-space -inline void BallAndSocketJointComponents::setR2World(Entity jointEntity, const Vector3& r2World) { +RP3D_FORCE_INLINE void BallAndSocketJointComponents::setR2World(Entity jointEntity, const Vector3& r2World) { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); mR2World[mMapEntityToComponentIndex[jointEntity]] = r2World; } // Return the inertia tensor of body 1 (in world-space coordinates) -inline const Matrix3x3& BallAndSocketJointComponents::getI1(Entity jointEntity) const { +RP3D_FORCE_INLINE const Matrix3x3& BallAndSocketJointComponents::getI1(Entity jointEntity) const { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); return mI1[mMapEntityToComponentIndex[jointEntity]]; } // Set the inertia tensor of body 1 (in world-space coordinates) -inline void BallAndSocketJointComponents::setI1(Entity jointEntity, const Matrix3x3& i1) { +RP3D_FORCE_INLINE void BallAndSocketJointComponents::setI1(Entity jointEntity, const Matrix3x3& i1) { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); mI1[mMapEntityToComponentIndex[jointEntity]] = i1; } // Return the inertia tensor of body 2 (in world-space coordinates) -inline const Matrix3x3& BallAndSocketJointComponents::getI2(Entity jointEntity) const { +RP3D_FORCE_INLINE const Matrix3x3& BallAndSocketJointComponents::getI2(Entity jointEntity) const { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); return mI2[mMapEntityToComponentIndex[jointEntity]]; } // Set the inertia tensor of body 2 (in world-space coordinates) -inline void BallAndSocketJointComponents::setI2(Entity jointEntity, const Matrix3x3& i2) { +RP3D_FORCE_INLINE void BallAndSocketJointComponents::setI2(Entity jointEntity, const Matrix3x3& i2) { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); mI2[mMapEntityToComponentIndex[jointEntity]] = i2; } // Return the bias vector for the constraint -inline Vector3 &BallAndSocketJointComponents::getBiasVector(Entity jointEntity) { +RP3D_FORCE_INLINE Vector3 &BallAndSocketJointComponents::getBiasVector(Entity jointEntity) { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); return mBiasVector[mMapEntityToComponentIndex[jointEntity]]; } // Set the bias vector for the constraint -inline void BallAndSocketJointComponents::setBiasVector(Entity jointEntity, const Vector3& biasVector) { +RP3D_FORCE_INLINE void BallAndSocketJointComponents::setBiasVector(Entity jointEntity, const Vector3& biasVector) { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); mBiasVector[mMapEntityToComponentIndex[jointEntity]] = biasVector; } // Return the inverse mass matrix K=JM^-1J^-t of the constraint -inline Matrix3x3& BallAndSocketJointComponents::getInverseMassMatrix(Entity jointEntity) { +RP3D_FORCE_INLINE Matrix3x3& BallAndSocketJointComponents::getInverseMassMatrix(Entity jointEntity) { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); return mInverseMassMatrix[mMapEntityToComponentIndex[jointEntity]]; } // Set the inverse mass matrix K=JM^-1J^-t of the constraint -inline void BallAndSocketJointComponents::setInverseMassMatrix(Entity jointEntity, const Matrix3x3& inverseMassMatrix) { +RP3D_FORCE_INLINE void BallAndSocketJointComponents::setInverseMassMatrix(Entity jointEntity, const Matrix3x3& inverseMassMatrix) { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); mInverseMassMatrix[mMapEntityToComponentIndex[jointEntity]] = inverseMassMatrix; } // Return the accumulated impulse -inline Vector3 &BallAndSocketJointComponents::getImpulse(Entity jointEntity) { +RP3D_FORCE_INLINE Vector3 &BallAndSocketJointComponents::getImpulse(Entity jointEntity) { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); return mImpulse[mMapEntityToComponentIndex[jointEntity]]; } // Set the accumulated impulse -inline void BallAndSocketJointComponents::setImpulse(Entity jointEntity, const Vector3& impulse) { +RP3D_FORCE_INLINE void BallAndSocketJointComponents::setImpulse(Entity jointEntity, const Vector3& impulse) { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); mImpulse[mMapEntityToComponentIndex[jointEntity]] = impulse; diff --git a/include/reactphysics3d/components/ColliderComponents.h b/include/reactphysics3d/components/ColliderComponents.h index 86c6c5af..e6c4eb20 100644 --- a/include/reactphysics3d/components/ColliderComponents.h +++ b/include/reactphysics3d/components/ColliderComponents.h @@ -32,6 +32,7 @@ #include #include #include +#include // ReactPhysics3D namespace namespace reactphysics3d { @@ -89,8 +90,8 @@ class ColliderComponents : public Components { /// Array with the local-to-world transforms of the colliders Transform* mLocalToWorldTransforms; - /// Array with the list of involved overlapping pairs for each collider - List* mOverlappingPairs; + /// Array with the involved overlapping pairs for each collider + Array* mOverlappingPairs; /// True if the size of the collision shape associated with the collider /// has been changed by the user @@ -99,6 +100,9 @@ class ColliderComponents : public Components { /// True if the collider is a trigger bool* mIsTrigger; + /// Array with the material of each collider + Material* mMaterials; + // -------------------- Methods -------------------- // @@ -127,14 +131,15 @@ class ColliderComponents : public Components { unsigned short collisionCategoryBits; unsigned short collideWithMaskBits; const Transform& localToWorldTransform; + const Material& material; /// Constructor ColliderComponent(Entity bodyEntity, Collider* collider, AABB localBounds, const Transform& localToBodyTransform, CollisionShape* collisionShape, unsigned short collisionCategoryBits, - unsigned short collideWithMaskBits, const Transform& localToWorldTransform) + unsigned short collideWithMaskBits, const Transform& localToWorldTransform, const Material& material) :bodyEntity(bodyEntity), collider(collider), localBounds(localBounds), localToBodyTransform(localToBodyTransform), collisionShape(collisionShape), collisionCategoryBits(collisionCategoryBits), collideWithMaskBits(collideWithMaskBits), - localToWorldTransform(localToWorldTransform) { + localToWorldTransform(localToWorldTransform), material(material) { } }; @@ -189,8 +194,8 @@ class ColliderComponents : public Components { /// Set the local-to-world transform of a collider void setLocalToWorldTransform(Entity colliderEntity, const Transform& transform); - /// Return a reference to the list of overlapping pairs for a given collider - List& getOverlappingPairs(Entity colliderEntity); + /// Return a reference to the array of overlapping pairs for a given collider + Array& getOverlappingPairs(Entity colliderEntity); /// Return true if the size of collision shape of the collider has been changed by the user bool getHasCollisionShapeChangedSize(Entity colliderEntity) const; @@ -204,16 +209,24 @@ class ColliderComponents : public Components { /// Set whether a collider is a trigger void setIsTrigger(Entity colliderEntity, bool isTrigger); + /// Return a reference to the material of a collider + Material& getMaterial(Entity colliderEntity); + + /// Set the material of a collider + void setMaterial(Entity colliderEntity, const Material& material); + // -------------------- Friendship -------------------- // friend class BroadPhaseSystem; friend class CollisionDetectionSystem; + friend class ContactSolverSystem; friend class DynamicsSystem; friend class OverlappingPairs; + friend class RigidBody; }; // Return the body entity of a given collider -inline Entity ColliderComponents::getBody(Entity colliderEntity) const { +RP3D_FORCE_INLINE Entity ColliderComponents::getBody(Entity colliderEntity) const { assert(mMapEntityToComponentIndex.containsKey(colliderEntity)); @@ -221,7 +234,7 @@ inline Entity ColliderComponents::getBody(Entity colliderEntity) const { } // Return a pointer to a given collider -inline Collider *ColliderComponents::getCollider(Entity colliderEntity) const { +RP3D_FORCE_INLINE Collider *ColliderComponents::getCollider(Entity colliderEntity) const { assert(mMapEntityToComponentIndex.containsKey(colliderEntity)); @@ -229,7 +242,7 @@ inline Collider *ColliderComponents::getCollider(Entity colliderEntity) const { } // Return the local-to-body transform of a collider -inline const Transform& ColliderComponents::getLocalToBodyTransform(Entity colliderEntity) const { +RP3D_FORCE_INLINE const Transform& ColliderComponents::getLocalToBodyTransform(Entity colliderEntity) const { assert(mMapEntityToComponentIndex.containsKey(colliderEntity)); @@ -237,7 +250,7 @@ inline const Transform& ColliderComponents::getLocalToBodyTransform(Entity colli } // Set the local-to-body transform of a collider -inline void ColliderComponents::setLocalToBodyTransform(Entity colliderEntity, const Transform& transform) { +RP3D_FORCE_INLINE void ColliderComponents::setLocalToBodyTransform(Entity colliderEntity, const Transform& transform) { assert(mMapEntityToComponentIndex.containsKey(colliderEntity)); @@ -245,7 +258,7 @@ inline void ColliderComponents::setLocalToBodyTransform(Entity colliderEntity, c } // Return a pointer to the collision shape of a collider -inline CollisionShape* ColliderComponents::getCollisionShape(Entity colliderEntity) const { +RP3D_FORCE_INLINE CollisionShape* ColliderComponents::getCollisionShape(Entity colliderEntity) const { assert(mMapEntityToComponentIndex.containsKey(colliderEntity)); @@ -253,7 +266,7 @@ inline CollisionShape* ColliderComponents::getCollisionShape(Entity colliderEnti } // Return the broad-phase id of a given collider -inline int32 ColliderComponents::getBroadPhaseId(Entity colliderEntity) const { +RP3D_FORCE_INLINE int32 ColliderComponents::getBroadPhaseId(Entity colliderEntity) const { assert(mMapEntityToComponentIndex.containsKey(colliderEntity)); @@ -261,7 +274,7 @@ inline int32 ColliderComponents::getBroadPhaseId(Entity colliderEntity) const { } // Set the broad-phase id of a given collider -inline void ColliderComponents::setBroadPhaseId(Entity colliderEntity, int32 broadPhaseId) { +RP3D_FORCE_INLINE void ColliderComponents::setBroadPhaseId(Entity colliderEntity, int32 broadPhaseId) { assert(mMapEntityToComponentIndex.containsKey(colliderEntity)); @@ -269,7 +282,7 @@ inline void ColliderComponents::setBroadPhaseId(Entity colliderEntity, int32 bro } // Return the collision category bits of a given collider -inline unsigned short ColliderComponents::getCollisionCategoryBits(Entity colliderEntity) const { +RP3D_FORCE_INLINE unsigned short ColliderComponents::getCollisionCategoryBits(Entity colliderEntity) const { assert(mMapEntityToComponentIndex.containsKey(colliderEntity)); @@ -277,7 +290,7 @@ inline unsigned short ColliderComponents::getCollisionCategoryBits(Entity collid } // Return the "collide with" mask bits of a given collider -inline unsigned short ColliderComponents::getCollideWithMaskBits(Entity colliderEntity) const { +RP3D_FORCE_INLINE unsigned short ColliderComponents::getCollideWithMaskBits(Entity colliderEntity) const { assert(mMapEntityToComponentIndex.containsKey(colliderEntity)); @@ -285,7 +298,7 @@ inline unsigned short ColliderComponents::getCollideWithMaskBits(Entity collider } // Set the collision category bits of a given collider -inline void ColliderComponents::setCollisionCategoryBits(Entity colliderEntity, unsigned short collisionCategoryBits) { +RP3D_FORCE_INLINE void ColliderComponents::setCollisionCategoryBits(Entity colliderEntity, unsigned short collisionCategoryBits) { assert(mMapEntityToComponentIndex.containsKey(colliderEntity)); @@ -293,7 +306,7 @@ inline void ColliderComponents::setCollisionCategoryBits(Entity colliderEntity, } // Set the "collide with" mask bits of a given collider -inline void ColliderComponents::setCollideWithMaskBits(Entity colliderEntity, unsigned short collideWithMaskBits) { +RP3D_FORCE_INLINE void ColliderComponents::setCollideWithMaskBits(Entity colliderEntity, unsigned short collideWithMaskBits) { assert(mMapEntityToComponentIndex.containsKey(colliderEntity)); @@ -301,7 +314,7 @@ inline void ColliderComponents::setCollideWithMaskBits(Entity colliderEntity, un } // Return the local-to-world transform of a collider -inline const Transform& ColliderComponents::getLocalToWorldTransform(Entity colliderEntity) const { +RP3D_FORCE_INLINE const Transform& ColliderComponents::getLocalToWorldTransform(Entity colliderEntity) const { assert(mMapEntityToComponentIndex.containsKey(colliderEntity)); @@ -309,15 +322,15 @@ inline const Transform& ColliderComponents::getLocalToWorldTransform(Entity coll } // Set the local-to-world transform of a collider -inline void ColliderComponents::setLocalToWorldTransform(Entity colliderEntity, const Transform& transform) { +RP3D_FORCE_INLINE void ColliderComponents::setLocalToWorldTransform(Entity colliderEntity, const Transform& transform) { assert(mMapEntityToComponentIndex.containsKey(colliderEntity)); mLocalToWorldTransforms[mMapEntityToComponentIndex[colliderEntity]] = transform; } -// Return a reference to the list of overlapping pairs for a given collider -inline List& ColliderComponents::getOverlappingPairs(Entity colliderEntity) { +// Return a reference to the array of overlapping pairs for a given collider +RP3D_FORCE_INLINE Array& ColliderComponents::getOverlappingPairs(Entity colliderEntity) { assert(mMapEntityToComponentIndex.containsKey(colliderEntity)); @@ -325,7 +338,7 @@ inline List& ColliderComponents::getOverlappingPairs(Entity colliderEnti } // Return true if the size of collision shape of the collider has been changed by the user -inline bool ColliderComponents::getHasCollisionShapeChangedSize(Entity colliderEntity) const { +RP3D_FORCE_INLINE bool ColliderComponents::getHasCollisionShapeChangedSize(Entity colliderEntity) const { assert(mMapEntityToComponentIndex.containsKey(colliderEntity)); @@ -333,7 +346,7 @@ inline bool ColliderComponents::getHasCollisionShapeChangedSize(Entity colliderE } // Return true if the size of collision shape of the collider has been changed by the user -inline void ColliderComponents::setHasCollisionShapeChangedSize(Entity colliderEntity, bool hasCollisionShapeChangedSize) { +RP3D_FORCE_INLINE void ColliderComponents::setHasCollisionShapeChangedSize(Entity colliderEntity, bool hasCollisionShapeChangedSize) { assert(mMapEntityToComponentIndex.containsKey(colliderEntity)); @@ -342,7 +355,7 @@ inline void ColliderComponents::setHasCollisionShapeChangedSize(Entity colliderE // Return true if a collider is a trigger -inline bool ColliderComponents::getIsTrigger(Entity colliderEntity) const { +RP3D_FORCE_INLINE bool ColliderComponents::getIsTrigger(Entity colliderEntity) const { assert(mMapEntityToComponentIndex.containsKey(colliderEntity)); @@ -350,13 +363,29 @@ inline bool ColliderComponents::getIsTrigger(Entity colliderEntity) const { } // Set whether a collider is a trigger -inline void ColliderComponents::setIsTrigger(Entity colliderEntity, bool isTrigger) { +RP3D_FORCE_INLINE void ColliderComponents::setIsTrigger(Entity colliderEntity, bool isTrigger) { assert(mMapEntityToComponentIndex.containsKey(colliderEntity)); mIsTrigger[mMapEntityToComponentIndex[colliderEntity]] = isTrigger; } +// Return a reference to the material of a collider +RP3D_FORCE_INLINE Material& ColliderComponents::getMaterial(Entity colliderEntity) { + + assert(mMapEntityToComponentIndex.containsKey(colliderEntity)); + + return mMaterials[mMapEntityToComponentIndex[colliderEntity]]; +} + +// Set the material of a collider +RP3D_FORCE_INLINE void ColliderComponents::setMaterial(Entity colliderEntity, const Material& material) { + + assert(mMapEntityToComponentIndex.containsKey(colliderEntity)); + + mMaterials[mMapEntityToComponentIndex[colliderEntity]] = material; +} + } #endif diff --git a/include/reactphysics3d/components/CollisionBodyComponents.h b/include/reactphysics3d/components/CollisionBodyComponents.h index c97414d5..a9ddd003 100644 --- a/include/reactphysics3d/components/CollisionBodyComponents.h +++ b/include/reactphysics3d/components/CollisionBodyComponents.h @@ -57,8 +57,8 @@ class CollisionBodyComponents : public Components { /// Array of pointers to the corresponding bodies CollisionBody** mBodies; - /// Array with the list of colliders of each body - List* mColliders; + /// Array with the colliders of each body + Array* mColliders; /// Array of boolean values to know if the body is active. bool* mIsActive; @@ -113,8 +113,8 @@ class CollisionBodyComponents : public Components { /// Return a pointer to a body CollisionBody* getBody(Entity bodyEntity); - /// Return the list of colliders of a body - const List& getColliders(Entity bodyEntity) const; + /// Return the array of colliders of a body + const Array& getColliders(Entity bodyEntity) const; /// Return true if the body is active bool getIsActive(Entity bodyEntity) const; @@ -130,7 +130,7 @@ class CollisionBodyComponents : public Components { }; // Add a collider to a body component -inline void CollisionBodyComponents::addColliderToBody(Entity bodyEntity, Entity colliderEntity) { +RP3D_FORCE_INLINE void CollisionBodyComponents::addColliderToBody(Entity bodyEntity, Entity colliderEntity) { assert(mMapEntityToComponentIndex.containsKey(bodyEntity)); @@ -138,7 +138,7 @@ inline void CollisionBodyComponents::addColliderToBody(Entity bodyEntity, Entity } // Remove a collider from a body component -inline void CollisionBodyComponents::removeColliderFromBody(Entity bodyEntity, Entity colliderEntity) { +RP3D_FORCE_INLINE void CollisionBodyComponents::removeColliderFromBody(Entity bodyEntity, Entity colliderEntity) { assert(mMapEntityToComponentIndex.containsKey(bodyEntity)); @@ -146,15 +146,15 @@ inline void CollisionBodyComponents::removeColliderFromBody(Entity bodyEntity, E } // Return a pointer to a body -inline CollisionBody *CollisionBodyComponents::getBody(Entity bodyEntity) { +RP3D_FORCE_INLINE CollisionBody *CollisionBodyComponents::getBody(Entity bodyEntity) { assert(mMapEntityToComponentIndex.containsKey(bodyEntity)); return mBodies[mMapEntityToComponentIndex[bodyEntity]]; } -// Return the list of colliders of a body -inline const List& CollisionBodyComponents::getColliders(Entity bodyEntity) const { +// Return the array of colliders of a body +RP3D_FORCE_INLINE const Array& CollisionBodyComponents::getColliders(Entity bodyEntity) const { assert(mMapEntityToComponentIndex.containsKey(bodyEntity)); @@ -162,7 +162,7 @@ inline const List& CollisionBodyComponents::getColliders(Entity bodyEnti } // Return true if the body is active -inline bool CollisionBodyComponents::getIsActive(Entity bodyEntity) const { +RP3D_FORCE_INLINE bool CollisionBodyComponents::getIsActive(Entity bodyEntity) const { assert(mMapEntityToComponentIndex.containsKey(bodyEntity)); @@ -170,7 +170,7 @@ inline bool CollisionBodyComponents::getIsActive(Entity bodyEntity) const { } // Set the value to know if the body is active -inline void CollisionBodyComponents::setIsActive(Entity bodyEntity, bool isActive) const { +RP3D_FORCE_INLINE void CollisionBodyComponents::setIsActive(Entity bodyEntity, bool isActive) const { assert(mMapEntityToComponentIndex.containsKey(bodyEntity)); @@ -178,7 +178,7 @@ inline void CollisionBodyComponents::setIsActive(Entity bodyEntity, bool isActiv } // Return the user data associated with the body -inline void* CollisionBodyComponents::getUserData(Entity bodyEntity) const { +RP3D_FORCE_INLINE void* CollisionBodyComponents::getUserData(Entity bodyEntity) const { assert(mMapEntityToComponentIndex.containsKey(bodyEntity)); @@ -186,7 +186,7 @@ inline void* CollisionBodyComponents::getUserData(Entity bodyEntity) const { } // Set the user data associated with the body -inline void CollisionBodyComponents::setUserData(Entity bodyEntity, void* userData) const { +RP3D_FORCE_INLINE void CollisionBodyComponents::setUserData(Entity bodyEntity, void* userData) const { assert(mMapEntityToComponentIndex.containsKey(bodyEntity)); diff --git a/include/reactphysics3d/components/Components.h b/include/reactphysics3d/components/Components.h index 71785930..3470ff96 100644 --- a/include/reactphysics3d/components/Components.h +++ b/include/reactphysics3d/components/Components.h @@ -113,6 +113,9 @@ class Components { /// Return true if there is a component for a given entity bool hasComponent(Entity entity) const; + /// Return true if there is a component for a given entiy and if so set the entity index + bool hasComponentGetIndex(Entity entity, uint32& entityIndex) const; + /// Return the number of components uint32 getNbComponents() const; @@ -124,28 +127,41 @@ class Components { }; // Return true if an entity is sleeping -inline bool Components::getIsEntityDisabled(Entity entity) const { +RP3D_FORCE_INLINE bool Components::getIsEntityDisabled(Entity entity) const { assert(hasComponent(entity)); return mMapEntityToComponentIndex[entity] >= mDisabledStartIndex; } // Return true if there is a component for a given entity -inline bool Components::hasComponent(Entity entity) const { +RP3D_FORCE_INLINE bool Components::hasComponent(Entity entity) const { return mMapEntityToComponentIndex.containsKey(entity); } +// Return true if there is a component for a given entiy and if so set the entity index +RP3D_FORCE_INLINE bool Components::hasComponentGetIndex(Entity entity, uint32& entityIndex) const { + + auto it = mMapEntityToComponentIndex.find(entity); + + if (it != mMapEntityToComponentIndex.end()) { + entityIndex = it->second; + return true; + } + + return false; +} + // Return the number of components -inline uint32 Components::getNbComponents() const { +RP3D_FORCE_INLINE uint32 Components::getNbComponents() const { return mNbComponents; } // Return the number of enabled components -inline uint32 Components::getNbEnabledComponents() const { +RP3D_FORCE_INLINE uint32 Components::getNbEnabledComponents() const { return mDisabledStartIndex; } // Return the index in the arrays for a given entity -inline uint32 Components::getEntityIndex(Entity entity) const { +RP3D_FORCE_INLINE uint32 Components::getEntityIndex(Entity entity) const { assert(hasComponent(entity)); return mMapEntityToComponentIndex[entity]; } diff --git a/include/reactphysics3d/components/FixedJointComponents.h b/include/reactphysics3d/components/FixedJointComponents.h index 990a4ba0..a6751d68 100644 --- a/include/reactphysics3d/components/FixedJointComponents.h +++ b/include/reactphysics3d/components/FixedJointComponents.h @@ -224,133 +224,133 @@ class FixedJointComponents : public Components { }; // Return a pointer to a given joint -inline FixedJoint* FixedJointComponents::getJoint(Entity jointEntity) const { +RP3D_FORCE_INLINE FixedJoint* FixedJointComponents::getJoint(Entity jointEntity) const { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); return mJoints[mMapEntityToComponentIndex[jointEntity]]; } // Set the joint pointer to a given joint -inline void FixedJointComponents::setJoint(Entity jointEntity, FixedJoint* joint) const { +RP3D_FORCE_INLINE void FixedJointComponents::setJoint(Entity jointEntity, FixedJoint* joint) const { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); mJoints[mMapEntityToComponentIndex[jointEntity]] = joint; } // Return the local anchor point of body 1 for a given joint -inline const Vector3& FixedJointComponents::getLocalAnchorPointBody1(Entity jointEntity) const { +RP3D_FORCE_INLINE const Vector3& FixedJointComponents::getLocalAnchorPointBody1(Entity jointEntity) const { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); return mLocalAnchorPointBody1[mMapEntityToComponentIndex[jointEntity]]; } // Set the local anchor point of body 1 for a given joint -inline void FixedJointComponents::setLocalAnchorPointBody1(Entity jointEntity, const Vector3& localAnchorPointBody1) { +RP3D_FORCE_INLINE void FixedJointComponents::setLocalAnchorPointBody1(Entity jointEntity, const Vector3& localAnchorPointBody1) { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); mLocalAnchorPointBody1[mMapEntityToComponentIndex[jointEntity]] = localAnchorPointBody1; } // Return the local anchor point of body 2 for a given joint -inline const Vector3& FixedJointComponents::getLocalAnchorPointBody2(Entity jointEntity) const { +RP3D_FORCE_INLINE const Vector3& FixedJointComponents::getLocalAnchorPointBody2(Entity jointEntity) const { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); return mLocalAnchorPointBody2[mMapEntityToComponentIndex[jointEntity]]; } // Set the local anchor point of body 2 for a given joint -inline void FixedJointComponents::setLocalAnchorPointBody2(Entity jointEntity, const Vector3& localAnchorPointBody2) { +RP3D_FORCE_INLINE void FixedJointComponents::setLocalAnchorPointBody2(Entity jointEntity, const Vector3& localAnchorPointBody2) { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); mLocalAnchorPointBody2[mMapEntityToComponentIndex[jointEntity]] = localAnchorPointBody2; } // Return the vector from center of body 1 to anchor point in world-space -inline const Vector3& FixedJointComponents::getR1World(Entity jointEntity) const { +RP3D_FORCE_INLINE const Vector3& FixedJointComponents::getR1World(Entity jointEntity) const { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); return mR1World[mMapEntityToComponentIndex[jointEntity]]; } // Set the vector from center of body 1 to anchor point in world-space -inline void FixedJointComponents::setR1World(Entity jointEntity, const Vector3& r1World) { +RP3D_FORCE_INLINE void FixedJointComponents::setR1World(Entity jointEntity, const Vector3& r1World) { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); mR1World[mMapEntityToComponentIndex[jointEntity]] = r1World; } // Return the vector from center of body 2 to anchor point in world-space -inline const Vector3& FixedJointComponents::getR2World(Entity jointEntity) const { +RP3D_FORCE_INLINE const Vector3& FixedJointComponents::getR2World(Entity jointEntity) const { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); return mR2World[mMapEntityToComponentIndex[jointEntity]]; } // Set the vector from center of body 2 to anchor point in world-space -inline void FixedJointComponents::setR2World(Entity jointEntity, const Vector3& r2World) { +RP3D_FORCE_INLINE void FixedJointComponents::setR2World(Entity jointEntity, const Vector3& r2World) { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); mR2World[mMapEntityToComponentIndex[jointEntity]] = r2World; } // Return the inertia tensor of body 1 (in world-space coordinates) -inline const Matrix3x3& FixedJointComponents::getI1(Entity jointEntity) const { +RP3D_FORCE_INLINE const Matrix3x3& FixedJointComponents::getI1(Entity jointEntity) const { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); return mI1[mMapEntityToComponentIndex[jointEntity]]; } // Set the inertia tensor of body 1 (in world-space coordinates) -inline void FixedJointComponents::setI1(Entity jointEntity, const Matrix3x3& i1) { +RP3D_FORCE_INLINE void FixedJointComponents::setI1(Entity jointEntity, const Matrix3x3& i1) { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); mI1[mMapEntityToComponentIndex[jointEntity]] = i1; } // Return the inertia tensor of body 2 (in world-space coordinates) -inline const Matrix3x3& FixedJointComponents::getI2(Entity jointEntity) const { +RP3D_FORCE_INLINE const Matrix3x3& FixedJointComponents::getI2(Entity jointEntity) const { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); return mI2[mMapEntityToComponentIndex[jointEntity]]; } // Set the inertia tensor of body 2 (in world-space coordinates) -inline void FixedJointComponents::setI2(Entity jointEntity, const Matrix3x3& i2) { +RP3D_FORCE_INLINE void FixedJointComponents::setI2(Entity jointEntity, const Matrix3x3& i2) { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); mI2[mMapEntityToComponentIndex[jointEntity]] = i2; } // Return the translation impulse -inline Vector3& FixedJointComponents::getImpulseTranslation(Entity jointEntity) { +RP3D_FORCE_INLINE Vector3& FixedJointComponents::getImpulseTranslation(Entity jointEntity) { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); return mImpulseTranslation[mMapEntityToComponentIndex[jointEntity]]; } // Set the translation impulse -inline void FixedJointComponents::setImpulseTranslation(Entity jointEntity, const Vector3& impulseTranslation) { +RP3D_FORCE_INLINE void FixedJointComponents::setImpulseTranslation(Entity jointEntity, const Vector3& impulseTranslation) { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); mImpulseTranslation[mMapEntityToComponentIndex[jointEntity]] = impulseTranslation; } // Return the translation impulse -inline Vector3& FixedJointComponents::getImpulseRotation(Entity jointEntity) { +RP3D_FORCE_INLINE Vector3& FixedJointComponents::getImpulseRotation(Entity jointEntity) { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); return mImpulseRotation[mMapEntityToComponentIndex[jointEntity]]; } // Set the translation impulse -inline void FixedJointComponents::setImpulseRotation(Entity jointEntity, const Vector3& impulseTranslation) { +RP3D_FORCE_INLINE void FixedJointComponents::setImpulseRotation(Entity jointEntity, const Vector3& impulseTranslation) { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); mImpulseRotation[mMapEntityToComponentIndex[jointEntity]] = impulseTranslation; } // Return the translation inverse mass matrix of the constraint -inline Matrix3x3& FixedJointComponents::getInverseMassMatrixTranslation(Entity jointEntity) { +RP3D_FORCE_INLINE Matrix3x3& FixedJointComponents::getInverseMassMatrixTranslation(Entity jointEntity) { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); return mInverseMassMatrixTranslation[mMapEntityToComponentIndex[jointEntity]]; @@ -358,63 +358,63 @@ inline Matrix3x3& FixedJointComponents::getInverseMassMatrixTranslation(Entity j // Set the translation inverse mass matrix of the constraint -inline void FixedJointComponents::setInverseMassMatrixTranslation(Entity jointEntity, const Matrix3x3& inverseMassMatrix) { +RP3D_FORCE_INLINE void FixedJointComponents::setInverseMassMatrixTranslation(Entity jointEntity, const Matrix3x3& inverseMassMatrix) { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); mInverseMassMatrixTranslation[mMapEntityToComponentIndex[jointEntity]] = inverseMassMatrix; } // Return the rotation inverse mass matrix of the constraint -inline Matrix3x3& FixedJointComponents::getInverseMassMatrixRotation(Entity jointEntity) { +RP3D_FORCE_INLINE Matrix3x3& FixedJointComponents::getInverseMassMatrixRotation(Entity jointEntity) { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); return mInverseMassMatrixRotation[mMapEntityToComponentIndex[jointEntity]]; } // Set the rotation inverse mass matrix of the constraint -inline void FixedJointComponents::setInverseMassMatrixRotation(Entity jointEntity, const Matrix3x3& inverseMassMatrix) { +RP3D_FORCE_INLINE void FixedJointComponents::setInverseMassMatrixRotation(Entity jointEntity, const Matrix3x3& inverseMassMatrix) { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); mInverseMassMatrixRotation[mMapEntityToComponentIndex[jointEntity]] = inverseMassMatrix; } // Return the translation bias -inline Vector3& FixedJointComponents::getBiasTranslation(Entity jointEntity) { +RP3D_FORCE_INLINE Vector3& FixedJointComponents::getBiasTranslation(Entity jointEntity) { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); return mBiasTranslation[mMapEntityToComponentIndex[jointEntity]]; } // Set the translation impulse -inline void FixedJointComponents::setBiasTranslation(Entity jointEntity, const Vector3 &impulseTranslation) { +RP3D_FORCE_INLINE void FixedJointComponents::setBiasTranslation(Entity jointEntity, const Vector3 &impulseTranslation) { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); mBiasTranslation[mMapEntityToComponentIndex[jointEntity]] = impulseTranslation; } // Return the rotation bias -inline Vector3& FixedJointComponents::getBiasRotation(Entity jointEntity) { +RP3D_FORCE_INLINE Vector3& FixedJointComponents::getBiasRotation(Entity jointEntity) { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); return mBiasRotation[mMapEntityToComponentIndex[jointEntity]]; } // Set the rotation impulse -inline void FixedJointComponents::setBiasRotation(Entity jointEntity, const Vector3& impulseRotation) { +RP3D_FORCE_INLINE void FixedJointComponents::setBiasRotation(Entity jointEntity, const Vector3& impulseRotation) { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); mBiasRotation[mMapEntityToComponentIndex[jointEntity]] = impulseRotation; } // Return the initial orientation difference -inline Quaternion& FixedJointComponents::getInitOrientationDifferenceInv(Entity jointEntity) { +RP3D_FORCE_INLINE Quaternion& FixedJointComponents::getInitOrientationDifferenceInv(Entity jointEntity) { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); return mInitOrientationDifferenceInv[mMapEntityToComponentIndex[jointEntity]]; } // Set the rotation impulse -inline void FixedJointComponents::setInitOrientationDifferenceInv(Entity jointEntity, const Quaternion& initOrientationDifferenceInv) { +RP3D_FORCE_INLINE void FixedJointComponents::setInitOrientationDifferenceInv(Entity jointEntity, const Quaternion& initOrientationDifferenceInv) { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); mInitOrientationDifferenceInv[mMapEntityToComponentIndex[jointEntity]] = initOrientationDifferenceInv; diff --git a/include/reactphysics3d/components/HingeJointComponents.h b/include/reactphysics3d/components/HingeJointComponents.h index d50db707..4ee9402f 100644 --- a/include/reactphysics3d/components/HingeJointComponents.h +++ b/include/reactphysics3d/components/HingeJointComponents.h @@ -415,133 +415,133 @@ class HingeJointComponents : public Components { }; // Return a pointer to a given joint -inline HingeJoint* HingeJointComponents::getJoint(Entity jointEntity) const { +RP3D_FORCE_INLINE HingeJoint* HingeJointComponents::getJoint(Entity jointEntity) const { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); return mJoints[mMapEntityToComponentIndex[jointEntity]]; } // Set the joint pointer to a given joint -inline void HingeJointComponents::setJoint(Entity jointEntity, HingeJoint* joint) const { +RP3D_FORCE_INLINE void HingeJointComponents::setJoint(Entity jointEntity, HingeJoint* joint) const { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); mJoints[mMapEntityToComponentIndex[jointEntity]] = joint; } // Return the local anchor point of body 1 for a given joint -inline const Vector3& HingeJointComponents::getLocalAnchorPointBody1(Entity jointEntity) const { +RP3D_FORCE_INLINE const Vector3& HingeJointComponents::getLocalAnchorPointBody1(Entity jointEntity) const { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); return mLocalAnchorPointBody1[mMapEntityToComponentIndex[jointEntity]]; } // Set the local anchor point of body 1 for a given joint -inline void HingeJointComponents::setLocalAnchorPointBody1(Entity jointEntity, const Vector3& localAnchoirPointBody1) { +RP3D_FORCE_INLINE void HingeJointComponents::setLocalAnchorPointBody1(Entity jointEntity, const Vector3& localAnchoirPointBody1) { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); mLocalAnchorPointBody1[mMapEntityToComponentIndex[jointEntity]] = localAnchoirPointBody1; } // Return the local anchor point of body 2 for a given joint -inline const Vector3& HingeJointComponents::getLocalAnchorPointBody2(Entity jointEntity) const { +RP3D_FORCE_INLINE const Vector3& HingeJointComponents::getLocalAnchorPointBody2(Entity jointEntity) const { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); return mLocalAnchorPointBody2[mMapEntityToComponentIndex[jointEntity]]; } // Set the local anchor point of body 2 for a given joint -inline void HingeJointComponents::setLocalAnchorPointBody2(Entity jointEntity, const Vector3& localAnchoirPointBody2) { +RP3D_FORCE_INLINE void HingeJointComponents::setLocalAnchorPointBody2(Entity jointEntity, const Vector3& localAnchoirPointBody2) { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); mLocalAnchorPointBody2[mMapEntityToComponentIndex[jointEntity]] = localAnchoirPointBody2; } // Return the vector from center of body 1 to anchor point in world-space -inline const Vector3& HingeJointComponents::getR1World(Entity jointEntity) const { +RP3D_FORCE_INLINE const Vector3& HingeJointComponents::getR1World(Entity jointEntity) const { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); return mR1World[mMapEntityToComponentIndex[jointEntity]]; } // Set the vector from center of body 1 to anchor point in world-space -inline void HingeJointComponents::setR1World(Entity jointEntity, const Vector3& r1World) { +RP3D_FORCE_INLINE void HingeJointComponents::setR1World(Entity jointEntity, const Vector3& r1World) { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); mR1World[mMapEntityToComponentIndex[jointEntity]] = r1World; } // Return the vector from center of body 2 to anchor point in world-space -inline const Vector3& HingeJointComponents::getR2World(Entity jointEntity) const { +RP3D_FORCE_INLINE const Vector3& HingeJointComponents::getR2World(Entity jointEntity) const { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); return mR2World[mMapEntityToComponentIndex[jointEntity]]; } // Set the vector from center of body 2 to anchor point in world-space -inline void HingeJointComponents::setR2World(Entity jointEntity, const Vector3& r2World) { +RP3D_FORCE_INLINE void HingeJointComponents::setR2World(Entity jointEntity, const Vector3& r2World) { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); mR2World[mMapEntityToComponentIndex[jointEntity]] = r2World; } // Return the inertia tensor of body 1 (in world-space coordinates) -inline const Matrix3x3& HingeJointComponents::getI1(Entity jointEntity) const { +RP3D_FORCE_INLINE const Matrix3x3& HingeJointComponents::getI1(Entity jointEntity) const { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); return mI1[mMapEntityToComponentIndex[jointEntity]]; } // Set the inertia tensor of body 1 (in world-space coordinates) -inline void HingeJointComponents::setI1(Entity jointEntity, const Matrix3x3& i1) { +RP3D_FORCE_INLINE void HingeJointComponents::setI1(Entity jointEntity, const Matrix3x3& i1) { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); mI1[mMapEntityToComponentIndex[jointEntity]] = i1; } // Return the inertia tensor of body 2 (in world-space coordinates) -inline const Matrix3x3& HingeJointComponents::getI2(Entity jointEntity) const { +RP3D_FORCE_INLINE const Matrix3x3& HingeJointComponents::getI2(Entity jointEntity) const { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); return mI2[mMapEntityToComponentIndex[jointEntity]]; } // Set the inertia tensor of body 2 (in world-space coordinates) -inline void HingeJointComponents::setI2(Entity jointEntity, const Matrix3x3& i2) { +RP3D_FORCE_INLINE void HingeJointComponents::setI2(Entity jointEntity, const Matrix3x3& i2) { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); mI2[mMapEntityToComponentIndex[jointEntity]] = i2; } // Return the translation impulse -inline Vector3& HingeJointComponents::getImpulseTranslation(Entity jointEntity) { +RP3D_FORCE_INLINE Vector3& HingeJointComponents::getImpulseTranslation(Entity jointEntity) { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); return mImpulseTranslation[mMapEntityToComponentIndex[jointEntity]]; } // Set the translation impulse -inline void HingeJointComponents::setImpulseTranslation(Entity jointEntity, const Vector3& impulseTranslation) { +RP3D_FORCE_INLINE void HingeJointComponents::setImpulseTranslation(Entity jointEntity, const Vector3& impulseTranslation) { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); mImpulseTranslation[mMapEntityToComponentIndex[jointEntity]] = impulseTranslation; } // Return the translation impulse -inline Vector2& HingeJointComponents::getImpulseRotation(Entity jointEntity) { +RP3D_FORCE_INLINE Vector2& HingeJointComponents::getImpulseRotation(Entity jointEntity) { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); return mImpulseRotation[mMapEntityToComponentIndex[jointEntity]]; } // Set the translation impulse -inline void HingeJointComponents::setImpulseRotation(Entity jointEntity, const Vector2& impulseTranslation) { +RP3D_FORCE_INLINE void HingeJointComponents::setImpulseRotation(Entity jointEntity, const Vector2& impulseTranslation) { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); mImpulseRotation[mMapEntityToComponentIndex[jointEntity]] = impulseTranslation; } // Return the translation inverse mass matrix of the constraint -inline Matrix3x3& HingeJointComponents::getInverseMassMatrixTranslation(Entity jointEntity) { +RP3D_FORCE_INLINE Matrix3x3& HingeJointComponents::getInverseMassMatrixTranslation(Entity jointEntity) { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); return mInverseMassMatrixTranslation[mMapEntityToComponentIndex[jointEntity]]; @@ -549,91 +549,91 @@ inline Matrix3x3& HingeJointComponents::getInverseMassMatrixTranslation(Entity j // Set the translation inverse mass matrix of the constraint -inline void HingeJointComponents::setInverseMassMatrixTranslation(Entity jointEntity, const Matrix3x3& inverseMassMatrix) { +RP3D_FORCE_INLINE void HingeJointComponents::setInverseMassMatrixTranslation(Entity jointEntity, const Matrix3x3& inverseMassMatrix) { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); mInverseMassMatrixTranslation[mMapEntityToComponentIndex[jointEntity]] = inverseMassMatrix; } // Return the rotation inverse mass matrix of the constraint -inline Matrix2x2& HingeJointComponents::getInverseMassMatrixRotation(Entity jointEntity) { +RP3D_FORCE_INLINE Matrix2x2& HingeJointComponents::getInverseMassMatrixRotation(Entity jointEntity) { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); return mInverseMassMatrixRotation[mMapEntityToComponentIndex[jointEntity]]; } // Set the rotation inverse mass matrix of the constraint -inline void HingeJointComponents::setInverseMassMatrixRotation(Entity jointEntity, const Matrix2x2& inverseMassMatrix) { +RP3D_FORCE_INLINE void HingeJointComponents::setInverseMassMatrixRotation(Entity jointEntity, const Matrix2x2& inverseMassMatrix) { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); mInverseMassMatrixRotation[mMapEntityToComponentIndex[jointEntity]] = inverseMassMatrix; } // Return the translation bias -inline Vector3& HingeJointComponents::getBiasTranslation(Entity jointEntity) { +RP3D_FORCE_INLINE Vector3& HingeJointComponents::getBiasTranslation(Entity jointEntity) { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); return mBiasTranslation[mMapEntityToComponentIndex[jointEntity]]; } // Set the translation impulse -inline void HingeJointComponents::setBiasTranslation(Entity jointEntity, const Vector3 &impulseTranslation) { +RP3D_FORCE_INLINE void HingeJointComponents::setBiasTranslation(Entity jointEntity, const Vector3 &impulseTranslation) { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); mBiasTranslation[mMapEntityToComponentIndex[jointEntity]] = impulseTranslation; } // Return the rotation bias -inline Vector2 &HingeJointComponents::getBiasRotation(Entity jointEntity) { +RP3D_FORCE_INLINE Vector2 &HingeJointComponents::getBiasRotation(Entity jointEntity) { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); return mBiasRotation[mMapEntityToComponentIndex[jointEntity]]; } // Set the rotation impulse -inline void HingeJointComponents::setBiasRotation(Entity jointEntity, const Vector2& impulseRotation) { +RP3D_FORCE_INLINE void HingeJointComponents::setBiasRotation(Entity jointEntity, const Vector2& impulseRotation) { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); mBiasRotation[mMapEntityToComponentIndex[jointEntity]] = impulseRotation; } // Return the initial orientation difference -inline Quaternion& HingeJointComponents::getInitOrientationDifferenceInv(Entity jointEntity) { +RP3D_FORCE_INLINE Quaternion& HingeJointComponents::getInitOrientationDifferenceInv(Entity jointEntity) { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); return mInitOrientationDifferenceInv[mMapEntityToComponentIndex[jointEntity]]; } // Set the rotation impulse -inline void HingeJointComponents::setInitOrientationDifferenceInv(Entity jointEntity, const Quaternion& initOrientationDifferenceInv) { +RP3D_FORCE_INLINE void HingeJointComponents::setInitOrientationDifferenceInv(Entity jointEntity, const Quaternion& initOrientationDifferenceInv) { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); mInitOrientationDifferenceInv[mMapEntityToComponentIndex[jointEntity]] = initOrientationDifferenceInv; } // Return the hinge rotation axis (in local-space coordinates of body 1) -inline Vector3& HingeJointComponents::getHingeLocalAxisBody1(Entity jointEntity) { +RP3D_FORCE_INLINE Vector3& HingeJointComponents::getHingeLocalAxisBody1(Entity jointEntity) { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); return mHingeLocalAxisBody1[mMapEntityToComponentIndex[jointEntity]]; } // Set the hinge rotation axis (in local-space coordinates of body 1) -inline void HingeJointComponents::setHingeLocalAxisBody1(Entity jointEntity, const Vector3& hingeLocalAxisBody1) { +RP3D_FORCE_INLINE void HingeJointComponents::setHingeLocalAxisBody1(Entity jointEntity, const Vector3& hingeLocalAxisBody1) { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); mHingeLocalAxisBody1[mMapEntityToComponentIndex[jointEntity]] = hingeLocalAxisBody1; } // Return the hinge rotation axis (in local-space coordiantes of body 2) -inline Vector3& HingeJointComponents::getHingeLocalAxisBody2(Entity jointEntity) { +RP3D_FORCE_INLINE Vector3& HingeJointComponents::getHingeLocalAxisBody2(Entity jointEntity) { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); return mHingeLocalAxisBody2[mMapEntityToComponentIndex[jointEntity]]; } // Set the hinge rotation axis (in local-space coordiantes of body 2) -inline void HingeJointComponents::setHingeLocalAxisBody2(Entity jointEntity, const Vector3& hingeLocalAxisBody2) { +RP3D_FORCE_INLINE void HingeJointComponents::setHingeLocalAxisBody2(Entity jointEntity, const Vector3& hingeLocalAxisBody2) { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); mHingeLocalAxisBody2[mMapEntityToComponentIndex[jointEntity]] = hingeLocalAxisBody2; @@ -641,56 +641,56 @@ inline void HingeJointComponents::setHingeLocalAxisBody2(Entity jointEntity, con // Return the hinge rotation axis (in world-space coordinates) computed from body 1 -inline Vector3& HingeJointComponents::getA1(Entity jointEntity) { +RP3D_FORCE_INLINE Vector3& HingeJointComponents::getA1(Entity jointEntity) { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); return mA1[mMapEntityToComponentIndex[jointEntity]]; } // Set the hinge rotation axis (in world-space coordinates) computed from body 1 -inline void HingeJointComponents::setA1(Entity jointEntity, const Vector3& a1) { +RP3D_FORCE_INLINE void HingeJointComponents::setA1(Entity jointEntity, const Vector3& a1) { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); mA1[mMapEntityToComponentIndex[jointEntity]] = a1; } // Return the cross product of vector b2 and a1 -inline Vector3& HingeJointComponents::getB2CrossA1(Entity jointEntity) { +RP3D_FORCE_INLINE Vector3& HingeJointComponents::getB2CrossA1(Entity jointEntity) { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); return mB2CrossA1[mMapEntityToComponentIndex[jointEntity]]; } // Set the cross product of vector b2 and a1 -inline void HingeJointComponents::setB2CrossA1(Entity jointEntity, const Vector3& b2CrossA1) { +RP3D_FORCE_INLINE void HingeJointComponents::setB2CrossA1(Entity jointEntity, const Vector3& b2CrossA1) { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); mB2CrossA1[mMapEntityToComponentIndex[jointEntity]] = b2CrossA1; } // Return the cross product of vector c2 and a1; -inline Vector3& HingeJointComponents::getC2CrossA1(Entity jointEntity) { +RP3D_FORCE_INLINE Vector3& HingeJointComponents::getC2CrossA1(Entity jointEntity) { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); return mC2CrossA1[mMapEntityToComponentIndex[jointEntity]]; } // Set the cross product of vector c2 and a1; -inline void HingeJointComponents::setC2CrossA1(Entity jointEntity, const Vector3& c2CrossA1) { +RP3D_FORCE_INLINE void HingeJointComponents::setC2CrossA1(Entity jointEntity, const Vector3& c2CrossA1) { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); mC2CrossA1[mMapEntityToComponentIndex[jointEntity]] = c2CrossA1; } // Return the accumulated impulse for the lower limit constraint -inline decimal HingeJointComponents::getImpulseLowerLimit(Entity jointEntity) const { +RP3D_FORCE_INLINE decimal HingeJointComponents::getImpulseLowerLimit(Entity jointEntity) const { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); return mImpulseLowerLimit[mMapEntityToComponentIndex[jointEntity]]; } // Set the accumulated impulse for the lower limit constraint -inline void HingeJointComponents::setImpulseLowerLimit(Entity jointEntity, decimal impulseLowerLimit) { +RP3D_FORCE_INLINE void HingeJointComponents::setImpulseLowerLimit(Entity jointEntity, decimal impulseLowerLimit) { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); mImpulseLowerLimit[mMapEntityToComponentIndex[jointEntity]] = impulseLowerLimit; @@ -698,14 +698,14 @@ inline void HingeJointComponents::setImpulseLowerLimit(Entity jointEntity, decim // Return the accumulated impulse for the upper limit constraint -inline decimal HingeJointComponents::getImpulseUpperLimit(Entity jointEntity) const { +RP3D_FORCE_INLINE decimal HingeJointComponents::getImpulseUpperLimit(Entity jointEntity) const { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); return mImpulseUpperLimit[mMapEntityToComponentIndex[jointEntity]]; } // Set the accumulated impulse for the upper limit constraint -inline void HingeJointComponents::setImpulseUpperLimit(Entity jointEntity, decimal impulseUpperLimit) const { +RP3D_FORCE_INLINE void HingeJointComponents::setImpulseUpperLimit(Entity jointEntity, decimal impulseUpperLimit) const { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); mImpulseUpperLimit[mMapEntityToComponentIndex[jointEntity]] = impulseUpperLimit; @@ -713,182 +713,182 @@ inline void HingeJointComponents::setImpulseUpperLimit(Entity jointEntity, decim // Return the accumulated impulse for the motor constraint; -inline decimal HingeJointComponents::getImpulseMotor(Entity jointEntity) const { +RP3D_FORCE_INLINE decimal HingeJointComponents::getImpulseMotor(Entity jointEntity) const { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); return mImpulseMotor[mMapEntityToComponentIndex[jointEntity]]; } // Set the accumulated impulse for the motor constraint; -inline void HingeJointComponents::setImpulseMotor(Entity jointEntity, decimal impulseMotor) { +RP3D_FORCE_INLINE void HingeJointComponents::setImpulseMotor(Entity jointEntity, decimal impulseMotor) { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); mImpulseMotor[mMapEntityToComponentIndex[jointEntity]] = impulseMotor; } // Return the inverse of mass matrix K=JM^-1J^t for the limits and motor constraints (1x1 matrix) -inline decimal HingeJointComponents::getInverseMassMatrixLimitMotor(Entity jointEntity) const { +RP3D_FORCE_INLINE decimal HingeJointComponents::getInverseMassMatrixLimitMotor(Entity jointEntity) const { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); return mInverseMassMatrixLimitMotor[mMapEntityToComponentIndex[jointEntity]]; } // Set the inverse of mass matrix K=JM^-1J^t for the limits and motor constraints (1x1 matrix) -inline void HingeJointComponents::setInverseMassMatrixLimitMotor(Entity jointEntity, decimal inverseMassMatrixLimitMotor) { +RP3D_FORCE_INLINE void HingeJointComponents::setInverseMassMatrixLimitMotor(Entity jointEntity, decimal inverseMassMatrixLimitMotor) { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); mInverseMassMatrixLimitMotor[mMapEntityToComponentIndex[jointEntity]] = inverseMassMatrixLimitMotor; } // Return the inverse of mass matrix K=JM^-1J^t for the motor -inline decimal HingeJointComponents::getInverseMassMatrixMotor(Entity jointEntity) { +RP3D_FORCE_INLINE decimal HingeJointComponents::getInverseMassMatrixMotor(Entity jointEntity) { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); return mInverseMassMatrixMotor[mMapEntityToComponentIndex[jointEntity]]; } // Return the inverse of mass matrix K=JM^-1J^t for the motor -inline void HingeJointComponents::setInverseMassMatrixMotor(Entity jointEntity, decimal inverseMassMatrixMotor) { +RP3D_FORCE_INLINE void HingeJointComponents::setInverseMassMatrixMotor(Entity jointEntity, decimal inverseMassMatrixMotor) { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); mInverseMassMatrixMotor[mMapEntityToComponentIndex[jointEntity]] = inverseMassMatrixMotor; } // Return the bias of the lower limit constraint -inline decimal HingeJointComponents::getBLowerLimit(Entity jointEntity) const { +RP3D_FORCE_INLINE decimal HingeJointComponents::getBLowerLimit(Entity jointEntity) const { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); return mBLowerLimit[mMapEntityToComponentIndex[jointEntity]]; } // Set the bias of the lower limit constraint -inline void HingeJointComponents::setBLowerLimit(Entity jointEntity, decimal bLowerLimit) const { +RP3D_FORCE_INLINE void HingeJointComponents::setBLowerLimit(Entity jointEntity, decimal bLowerLimit) const { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); mBLowerLimit[mMapEntityToComponentIndex[jointEntity]] = bLowerLimit; } // Return the bias of the upper limit constraint -inline decimal HingeJointComponents::getBUpperLimit(Entity jointEntity) const { +RP3D_FORCE_INLINE decimal HingeJointComponents::getBUpperLimit(Entity jointEntity) const { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); return mBUpperLimit[mMapEntityToComponentIndex[jointEntity]]; } // Set the bias of the upper limit constraint -inline void HingeJointComponents::setBUpperLimit(Entity jointEntity, decimal bUpperLimit) { +RP3D_FORCE_INLINE void HingeJointComponents::setBUpperLimit(Entity jointEntity, decimal bUpperLimit) { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); mBUpperLimit[mMapEntityToComponentIndex[jointEntity]] = bUpperLimit; } // Return true if the joint limits are enabled -inline bool HingeJointComponents::getIsLimitEnabled(Entity jointEntity) const { +RP3D_FORCE_INLINE bool HingeJointComponents::getIsLimitEnabled(Entity jointEntity) const { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); return mIsLimitEnabled[mMapEntityToComponentIndex[jointEntity]]; } // Set to true if the joint limits are enabled -inline void HingeJointComponents::setIsLimitEnabled(Entity jointEntity, bool isLimitEnabled) { +RP3D_FORCE_INLINE void HingeJointComponents::setIsLimitEnabled(Entity jointEntity, bool isLimitEnabled) { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); mIsLimitEnabled[mMapEntityToComponentIndex[jointEntity]] = isLimitEnabled; } // Return true if the motor of the joint in enabled -inline bool HingeJointComponents::getIsMotorEnabled(Entity jointEntity) const { +RP3D_FORCE_INLINE bool HingeJointComponents::getIsMotorEnabled(Entity jointEntity) const { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); return mIsMotorEnabled[mMapEntityToComponentIndex[jointEntity]]; } // Set to true if the motor of the joint in enabled -inline void HingeJointComponents::setIsMotorEnabled(Entity jointEntity, bool isMotorEnabled) const { +RP3D_FORCE_INLINE void HingeJointComponents::setIsMotorEnabled(Entity jointEntity, bool isMotorEnabled) const { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); mIsMotorEnabled[mMapEntityToComponentIndex[jointEntity]] = isMotorEnabled; } // Return the Lower limit (minimum allowed rotation angle in radian) -inline decimal HingeJointComponents::getLowerLimit(Entity jointEntity) const { +RP3D_FORCE_INLINE decimal HingeJointComponents::getLowerLimit(Entity jointEntity) const { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); return mLowerLimit[mMapEntityToComponentIndex[jointEntity]]; } // Set the Lower limit (minimum allowed rotation angle in radian) -inline void HingeJointComponents::setLowerLimit(Entity jointEntity, decimal lowerLimit) const { +RP3D_FORCE_INLINE void HingeJointComponents::setLowerLimit(Entity jointEntity, decimal lowerLimit) const { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); mLowerLimit[mMapEntityToComponentIndex[jointEntity]] = lowerLimit; } // Return the upper limit (maximum translation distance) -inline decimal HingeJointComponents::getUpperLimit(Entity jointEntity) const { +RP3D_FORCE_INLINE decimal HingeJointComponents::getUpperLimit(Entity jointEntity) const { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); return mUpperLimit[mMapEntityToComponentIndex[jointEntity]]; } // Set the upper limit (maximum translation distance) -inline void HingeJointComponents::setUpperLimit(Entity jointEntity, decimal upperLimit) { +RP3D_FORCE_INLINE void HingeJointComponents::setUpperLimit(Entity jointEntity, decimal upperLimit) { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); mUpperLimit[mMapEntityToComponentIndex[jointEntity]] = upperLimit; } // Return true if the lower limit is violated -inline bool HingeJointComponents::getIsLowerLimitViolated(Entity jointEntity) const { +RP3D_FORCE_INLINE bool HingeJointComponents::getIsLowerLimitViolated(Entity jointEntity) const { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); return mIsLowerLimitViolated[mMapEntityToComponentIndex[jointEntity]]; } // Set to true if the lower limit is violated -inline void HingeJointComponents::setIsLowerLimitViolated(Entity jointEntity, bool isLowerLimitViolated) { +RP3D_FORCE_INLINE void HingeJointComponents::setIsLowerLimitViolated(Entity jointEntity, bool isLowerLimitViolated) { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); mIsLowerLimitViolated[mMapEntityToComponentIndex[jointEntity]] = isLowerLimitViolated; } // Return true if the upper limit is violated -inline bool HingeJointComponents::getIsUpperLimitViolated(Entity jointEntity) const { +RP3D_FORCE_INLINE bool HingeJointComponents::getIsUpperLimitViolated(Entity jointEntity) const { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); return mIsUpperLimitViolated[mMapEntityToComponentIndex[jointEntity]]; } // Set to true if the upper limit is violated -inline void HingeJointComponents::setIsUpperLimitViolated(Entity jointEntity, bool isUpperLimitViolated) const { +RP3D_FORCE_INLINE void HingeJointComponents::setIsUpperLimitViolated(Entity jointEntity, bool isUpperLimitViolated) const { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); mIsUpperLimitViolated[mMapEntityToComponentIndex[jointEntity]] = isUpperLimitViolated; } // Return the motor speed (in rad/s) -inline decimal HingeJointComponents::getMotorSpeed(Entity jointEntity) const { +RP3D_FORCE_INLINE decimal HingeJointComponents::getMotorSpeed(Entity jointEntity) const { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); return mMotorSpeed[mMapEntityToComponentIndex[jointEntity]]; } // Set the motor speed (in rad/s) -inline void HingeJointComponents::setMotorSpeed(Entity jointEntity, decimal motorSpeed) { +RP3D_FORCE_INLINE void HingeJointComponents::setMotorSpeed(Entity jointEntity, decimal motorSpeed) { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); mMotorSpeed[mMapEntityToComponentIndex[jointEntity]] = motorSpeed; } // Return the maximum motor torque (in Newtons) that can be applied to reach to desired motor speed -inline decimal HingeJointComponents::getMaxMotorTorque(Entity jointEntity) const { +RP3D_FORCE_INLINE decimal HingeJointComponents::getMaxMotorTorque(Entity jointEntity) const { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); return mMaxMotorTorque[mMapEntityToComponentIndex[jointEntity]]; } // Set the maximum motor torque (in Newtons) that can be applied to reach to desired motor speed -inline void HingeJointComponents::setMaxMotorTorque(Entity jointEntity, decimal maxMotorTorque) { +RP3D_FORCE_INLINE void HingeJointComponents::setMaxMotorTorque(Entity jointEntity, decimal maxMotorTorque) { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); mMaxMotorTorque[mMapEntityToComponentIndex[jointEntity]] = maxMotorTorque; diff --git a/include/reactphysics3d/components/JointComponents.h b/include/reactphysics3d/components/JointComponents.h index f82744c0..c752f815 100644 --- a/include/reactphysics3d/components/JointComponents.h +++ b/include/reactphysics3d/components/JointComponents.h @@ -138,7 +138,7 @@ class JointComponents : public Components { JointsPositionCorrectionTechnique getPositionCorrectionTechnique(Entity jointEntity) const; /// Set the position correction technique of a joint - void getPositionCorrectionTechnique(Entity jointEntity, JointsPositionCorrectionTechnique positionCorrectionTechnique); + void setPositionCorrectionTechnique(Entity jointEntity, JointsPositionCorrectionTechnique positionCorrectionTechnique); /// Return true if the collision is enabled between the two bodies of a joint bool getIsCollisionEnabled(Entity jointEntity) const; @@ -157,64 +157,69 @@ class JointComponents : public Components { friend class BroadPhaseSystem; friend class ConstraintSolverSystem; friend class PhysicsWorld; + friend class SolveBallAndSocketJointSystem; + friend class SolveFixedJointSystem; + friend class SolveHingeJointSystem; + friend class SolveSliderJointSystem; + }; // Return the entity of the first body of a joint -inline Entity JointComponents::getBody1Entity(Entity jointEntity) const { +RP3D_FORCE_INLINE Entity JointComponents::getBody1Entity(Entity jointEntity) const { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); return mBody1Entities[mMapEntityToComponentIndex[jointEntity]]; } // Return the entity of the second body of a joint -inline Entity JointComponents::getBody2Entity(Entity jointEntity) const { +RP3D_FORCE_INLINE Entity JointComponents::getBody2Entity(Entity jointEntity) const { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); return mBody2Entities[mMapEntityToComponentIndex[jointEntity]]; } // Return a pointer to the joint -inline Joint* JointComponents::getJoint(Entity jointEntity) const { +RP3D_FORCE_INLINE Joint* JointComponents::getJoint(Entity jointEntity) const { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); return mJoints[mMapEntityToComponentIndex[jointEntity]]; } // Return the type of a joint -inline JointType JointComponents::getType(Entity jointEntity) const { +RP3D_FORCE_INLINE JointType JointComponents::getType(Entity jointEntity) const { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); return mTypes[mMapEntityToComponentIndex[jointEntity]]; } // Return the position correction technique of a joint -inline JointsPositionCorrectionTechnique JointComponents::getPositionCorrectionTechnique(Entity jointEntity) const { +RP3D_FORCE_INLINE JointsPositionCorrectionTechnique JointComponents::getPositionCorrectionTechnique(Entity jointEntity) const { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); return mPositionCorrectionTechniques[mMapEntityToComponentIndex[jointEntity]]; } // Set the position correction technique of a joint -inline void JointComponents::getPositionCorrectionTechnique(Entity jointEntity, JointsPositionCorrectionTechnique positionCorrectionTechnique) { +RP3D_FORCE_INLINE void JointComponents::setPositionCorrectionTechnique(Entity jointEntity, JointsPositionCorrectionTechnique positionCorrectionTechnique) { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); mPositionCorrectionTechniques[mMapEntityToComponentIndex[jointEntity]] = positionCorrectionTechnique; } // Return true if the collision is enabled between the two bodies of a joint -inline bool JointComponents::getIsCollisionEnabled(Entity jointEntity) const { +RP3D_FORCE_INLINE bool JointComponents::getIsCollisionEnabled(Entity jointEntity) const { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); return mIsCollisionEnabled[mMapEntityToComponentIndex[jointEntity]]; } // Set whether the collision is enabled between the two bodies of a joint -inline void JointComponents::setIsCollisionEnabled(Entity jointEntity, bool isCollisionEnabled) { +RP3D_FORCE_INLINE void JointComponents::setIsCollisionEnabled(Entity jointEntity, bool isCollisionEnabled) { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); mIsCollisionEnabled[mMapEntityToComponentIndex[jointEntity]] = isCollisionEnabled; } // Return true if the joint has already been added into an island during island creation -inline bool JointComponents::getIsAlreadyInIsland(Entity jointEntity) const { +RP3D_FORCE_INLINE bool JointComponents::getIsAlreadyInIsland(Entity jointEntity) const { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); return mIsAlreadyInIsland[mMapEntityToComponentIndex[jointEntity]]; } // Set to true if the joint has already been added into an island during island creation -inline void JointComponents::setIsAlreadyInIsland(Entity jointEntity, bool isAlreadyInIsland) { +RP3D_FORCE_INLINE void JointComponents::setIsAlreadyInIsland(Entity jointEntity, bool isAlreadyInIsland) { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); mIsAlreadyInIsland[mMapEntityToComponentIndex[jointEntity]] = isAlreadyInIsland; } diff --git a/include/reactphysics3d/components/RigidBodyComponents.h b/include/reactphysics3d/components/RigidBodyComponents.h index 1be18761..69266262 100644 --- a/include/reactphysics3d/components/RigidBodyComponents.h +++ b/include/reactphysics3d/components/RigidBodyComponents.h @@ -109,9 +109,12 @@ class RigidBodyComponents : public Components { /// Array with the inertia tensor of each component Vector3* mLocalInertiaTensors; - /// Array with the inverse of the inertia tensor of each component + /// Array with the inverse of the local inertia tensor of each component Vector3* mInverseInertiaTensorsLocal; + /// Array with the inverse of the world inertia tensor of each component + Matrix3x3* mInverseInertiaTensorsWorld; + /// Array with the constrained linear velocity of each component Vector3* mConstrainedLinearVelocities; @@ -142,8 +145,11 @@ class RigidBodyComponents : public Components { /// Array with the boolean value to know if the body has already been added into an island bool* mIsAlreadyInIsland; - /// For each body, the list of joints entities the body is part of - List* mJoints; + /// For each body, the array of joints entities the body is part of + Array* mJoints; + + /// For each body, the array of the indices of contact pairs in which the body is involved + Array* mContactPairs; // -------------------- Methods -------------------- // @@ -258,6 +264,9 @@ class RigidBodyComponents : public Components { /// Return the inverse local inertia tensor of an entity const Vector3& getInertiaTensorLocalInverse(Entity bodyEntity); + /// Return the inverse world inertia tensor of an entity + const Matrix3x3& getInertiaTensorWorldInverse(Entity bodyEntity); + /// Set the external force of an entity void setExternalForce(Entity bodyEntity, const Vector3& externalForce); @@ -333,8 +342,8 @@ class RigidBodyComponents : public Components { /// Set the value to know if the entity is already in an island void setIsAlreadyInIsland(Entity bodyEntity, bool isAlreadyInIsland); - /// Return the list of joints of a body - const List& getJoints(Entity bodyEntity) const; + /// Return the array of joints of a body + const Array& getJoints(Entity bodyEntity) const; /// Add a joint to a body component void addJointToBody(Entity bodyEntity, Entity jointEntity); @@ -342,10 +351,14 @@ class RigidBodyComponents : public Components { /// Remove a joint from a body component void removeJointFromBody(Entity bodyEntity, Entity jointEntity); + /// A an associated contact pairs into the contact pairs array of the body + void addContacPair(Entity bodyEntity, uint contactPairIndex); + // -------------------- Friendship -------------------- // friend class PhysicsWorld; friend class ContactSolverSystem; + friend class CollisionDetectionSystem; friend class SolveBallAndSocketJointSystem; friend class SolveFixedJointSystem; friend class SolveHingeJointSystem; @@ -358,7 +371,7 @@ class RigidBodyComponents : public Components { }; // Return a pointer to a body rigid -inline RigidBody* RigidBodyComponents::getRigidBody(Entity bodyEntity) { +RP3D_FORCE_INLINE RigidBody* RigidBodyComponents::getRigidBody(Entity bodyEntity) { assert(mMapEntityToComponentIndex.containsKey(bodyEntity)); @@ -366,7 +379,7 @@ inline RigidBody* RigidBodyComponents::getRigidBody(Entity bodyEntity) { } // Return true if the body is allowed to sleep -inline bool RigidBodyComponents::getIsAllowedToSleep(Entity bodyEntity) const { +RP3D_FORCE_INLINE bool RigidBodyComponents::getIsAllowedToSleep(Entity bodyEntity) const { assert(mMapEntityToComponentIndex.containsKey(bodyEntity)); @@ -374,7 +387,7 @@ inline bool RigidBodyComponents::getIsAllowedToSleep(Entity bodyEntity) const { } // Set the value to know if the body is allowed to sleep -inline void RigidBodyComponents::setIsAllowedToSleep(Entity bodyEntity, bool isAllowedToSleep) const { +RP3D_FORCE_INLINE void RigidBodyComponents::setIsAllowedToSleep(Entity bodyEntity, bool isAllowedToSleep) const { assert(mMapEntityToComponentIndex.containsKey(bodyEntity)); @@ -382,7 +395,7 @@ inline void RigidBodyComponents::setIsAllowedToSleep(Entity bodyEntity, bool isA } // Return true if the body is sleeping -inline bool RigidBodyComponents::getIsSleeping(Entity bodyEntity) const { +RP3D_FORCE_INLINE bool RigidBodyComponents::getIsSleeping(Entity bodyEntity) const { assert(mMapEntityToComponentIndex.containsKey(bodyEntity)); @@ -390,7 +403,7 @@ inline bool RigidBodyComponents::getIsSleeping(Entity bodyEntity) const { } // Set the value to know if the body is sleeping -inline void RigidBodyComponents::setIsSleeping(Entity bodyEntity, bool isSleeping) const { +RP3D_FORCE_INLINE void RigidBodyComponents::setIsSleeping(Entity bodyEntity, bool isSleeping) const { assert(mMapEntityToComponentIndex.containsKey(bodyEntity)); @@ -398,7 +411,7 @@ inline void RigidBodyComponents::setIsSleeping(Entity bodyEntity, bool isSleepin } // Return the sleep time -inline decimal RigidBodyComponents::getSleepTime(Entity bodyEntity) const { +RP3D_FORCE_INLINE decimal RigidBodyComponents::getSleepTime(Entity bodyEntity) const { assert(mMapEntityToComponentIndex.containsKey(bodyEntity)); @@ -406,7 +419,7 @@ inline decimal RigidBodyComponents::getSleepTime(Entity bodyEntity) const { } // Set the sleep time -inline void RigidBodyComponents::setSleepTime(Entity bodyEntity, decimal sleepTime) const { +RP3D_FORCE_INLINE void RigidBodyComponents::setSleepTime(Entity bodyEntity, decimal sleepTime) const { assert(mMapEntityToComponentIndex.containsKey(bodyEntity)); @@ -414,7 +427,7 @@ inline void RigidBodyComponents::setSleepTime(Entity bodyEntity, decimal sleepTi } // Return the body type of a body -inline BodyType RigidBodyComponents::getBodyType(Entity bodyEntity) { +RP3D_FORCE_INLINE BodyType RigidBodyComponents::getBodyType(Entity bodyEntity) { assert(mMapEntityToComponentIndex.containsKey(bodyEntity)); @@ -422,7 +435,7 @@ inline BodyType RigidBodyComponents::getBodyType(Entity bodyEntity) { } // Set the body type of a body -inline void RigidBodyComponents::setBodyType(Entity bodyEntity, BodyType bodyType) { +RP3D_FORCE_INLINE void RigidBodyComponents::setBodyType(Entity bodyEntity, BodyType bodyType) { assert(mMapEntityToComponentIndex.containsKey(bodyEntity)); @@ -430,7 +443,7 @@ inline void RigidBodyComponents::setBodyType(Entity bodyEntity, BodyType bodyTyp } // Return the linear velocity of an entity -inline const Vector3& RigidBodyComponents::getLinearVelocity(Entity bodyEntity) const { +RP3D_FORCE_INLINE const Vector3& RigidBodyComponents::getLinearVelocity(Entity bodyEntity) const { assert(mMapEntityToComponentIndex.containsKey(bodyEntity)); @@ -438,7 +451,7 @@ inline const Vector3& RigidBodyComponents::getLinearVelocity(Entity bodyEntity) } // Return the angular velocity of an entity -inline const Vector3& RigidBodyComponents::getAngularVelocity(Entity bodyEntity) const { +RP3D_FORCE_INLINE const Vector3& RigidBodyComponents::getAngularVelocity(Entity bodyEntity) const { assert(mMapEntityToComponentIndex.containsKey(bodyEntity)); @@ -446,7 +459,7 @@ inline const Vector3& RigidBodyComponents::getAngularVelocity(Entity bodyEntity) } // Set the linear velocity of an entity -inline void RigidBodyComponents::setLinearVelocity(Entity bodyEntity, const Vector3& linearVelocity) { +RP3D_FORCE_INLINE void RigidBodyComponents::setLinearVelocity(Entity bodyEntity, const Vector3& linearVelocity) { assert(mMapEntityToComponentIndex.containsKey(bodyEntity)); @@ -454,7 +467,7 @@ inline void RigidBodyComponents::setLinearVelocity(Entity bodyEntity, const Vect } // Set the angular velocity of an entity -inline void RigidBodyComponents::setAngularVelocity(Entity bodyEntity, const Vector3& angularVelocity) { +RP3D_FORCE_INLINE void RigidBodyComponents::setAngularVelocity(Entity bodyEntity, const Vector3& angularVelocity) { assert(mMapEntityToComponentIndex.containsKey(bodyEntity)); @@ -462,7 +475,7 @@ inline void RigidBodyComponents::setAngularVelocity(Entity bodyEntity, const Vec } // Return the external force of an entity -inline const Vector3& RigidBodyComponents::getExternalForce(Entity bodyEntity) const { +RP3D_FORCE_INLINE const Vector3& RigidBodyComponents::getExternalForce(Entity bodyEntity) const { assert(mMapEntityToComponentIndex.containsKey(bodyEntity)); @@ -470,7 +483,7 @@ inline const Vector3& RigidBodyComponents::getExternalForce(Entity bodyEntity) c } // Return the external torque of an entity -inline const Vector3& RigidBodyComponents::getExternalTorque(Entity bodyEntity) const { +RP3D_FORCE_INLINE const Vector3& RigidBodyComponents::getExternalTorque(Entity bodyEntity) const { assert(mMapEntityToComponentIndex.containsKey(bodyEntity)); @@ -478,7 +491,7 @@ inline const Vector3& RigidBodyComponents::getExternalTorque(Entity bodyEntity) } // Return the linear damping factor of an entity -inline decimal RigidBodyComponents::getLinearDamping(Entity bodyEntity) const { +RP3D_FORCE_INLINE decimal RigidBodyComponents::getLinearDamping(Entity bodyEntity) const { assert(mMapEntityToComponentIndex.containsKey(bodyEntity)); @@ -486,7 +499,7 @@ inline decimal RigidBodyComponents::getLinearDamping(Entity bodyEntity) const { } // Return the angular damping factor of an entity -inline decimal RigidBodyComponents::getAngularDamping(Entity bodyEntity) const { +RP3D_FORCE_INLINE decimal RigidBodyComponents::getAngularDamping(Entity bodyEntity) const { assert(mMapEntityToComponentIndex.containsKey(bodyEntity)); @@ -494,7 +507,7 @@ inline decimal RigidBodyComponents::getAngularDamping(Entity bodyEntity) const { } // Return the mass of an entity -inline decimal RigidBodyComponents::getMass(Entity bodyEntity) const { +RP3D_FORCE_INLINE decimal RigidBodyComponents::getMass(Entity bodyEntity) const { assert(mMapEntityToComponentIndex.containsKey(bodyEntity)); @@ -502,7 +515,7 @@ inline decimal RigidBodyComponents::getMass(Entity bodyEntity) const { } // Return the inverse mass of an entity -inline decimal RigidBodyComponents::getMassInverse(Entity bodyEntity) const { +RP3D_FORCE_INLINE decimal RigidBodyComponents::getMassInverse(Entity bodyEntity) const { assert(mMapEntityToComponentIndex.containsKey(bodyEntity)); @@ -510,15 +523,23 @@ inline decimal RigidBodyComponents::getMassInverse(Entity bodyEntity) const { } // Return the inverse local inertia tensor of an entity -inline const Vector3& RigidBodyComponents::getInertiaTensorLocalInverse(Entity bodyEntity) { +RP3D_FORCE_INLINE const Vector3& RigidBodyComponents::getInertiaTensorLocalInverse(Entity bodyEntity) { assert(mMapEntityToComponentIndex.containsKey(bodyEntity)); return mInverseInertiaTensorsLocal[mMapEntityToComponentIndex[bodyEntity]]; } +// Return the inverse world inertia tensor of an entity +RP3D_FORCE_INLINE const Matrix3x3& RigidBodyComponents::getInertiaTensorWorldInverse(Entity bodyEntity) { + + assert(mMapEntityToComponentIndex.containsKey(bodyEntity)); + + return mInverseInertiaTensorsWorld[mMapEntityToComponentIndex[bodyEntity]]; +} + // Set the external force of an entity -inline void RigidBodyComponents::setExternalForce(Entity bodyEntity, const Vector3& externalForce) { +RP3D_FORCE_INLINE void RigidBodyComponents::setExternalForce(Entity bodyEntity, const Vector3& externalForce) { assert(mMapEntityToComponentIndex.containsKey(bodyEntity)); @@ -526,7 +547,7 @@ inline void RigidBodyComponents::setExternalForce(Entity bodyEntity, const Vecto } // Set the external force of an entity -inline void RigidBodyComponents::setExternalTorque(Entity bodyEntity, const Vector3& externalTorque) { +RP3D_FORCE_INLINE void RigidBodyComponents::setExternalTorque(Entity bodyEntity, const Vector3& externalTorque) { assert(mMapEntityToComponentIndex.containsKey(bodyEntity)); @@ -534,7 +555,7 @@ inline void RigidBodyComponents::setExternalTorque(Entity bodyEntity, const Vect } // Set the linear damping factor of an entity -inline void RigidBodyComponents::setLinearDamping(Entity bodyEntity, decimal linearDamping) { +RP3D_FORCE_INLINE void RigidBodyComponents::setLinearDamping(Entity bodyEntity, decimal linearDamping) { assert(mMapEntityToComponentIndex.containsKey(bodyEntity)); @@ -542,7 +563,7 @@ inline void RigidBodyComponents::setLinearDamping(Entity bodyEntity, decimal lin } // Set the angular damping factor of an entity -inline void RigidBodyComponents::setAngularDamping(Entity bodyEntity, decimal angularDamping) { +RP3D_FORCE_INLINE void RigidBodyComponents::setAngularDamping(Entity bodyEntity, decimal angularDamping) { assert(mMapEntityToComponentIndex.containsKey(bodyEntity)); @@ -550,7 +571,7 @@ inline void RigidBodyComponents::setAngularDamping(Entity bodyEntity, decimal an } // Set the mass of an entity -inline void RigidBodyComponents::setMass(Entity bodyEntity, decimal mass) { +RP3D_FORCE_INLINE void RigidBodyComponents::setMass(Entity bodyEntity, decimal mass) { assert(mMapEntityToComponentIndex.containsKey(bodyEntity)); @@ -558,7 +579,7 @@ inline void RigidBodyComponents::setMass(Entity bodyEntity, decimal mass) { } // Set the mass inverse of an entity -inline void RigidBodyComponents::setMassInverse(Entity bodyEntity, decimal inverseMass) { +RP3D_FORCE_INLINE void RigidBodyComponents::setMassInverse(Entity bodyEntity, decimal inverseMass) { assert(mMapEntityToComponentIndex.containsKey(bodyEntity)); @@ -566,7 +587,7 @@ inline void RigidBodyComponents::setMassInverse(Entity bodyEntity, decimal inver } // Return the local inertia tensor of an entity -inline const Vector3& RigidBodyComponents::getLocalInertiaTensor(Entity bodyEntity) { +RP3D_FORCE_INLINE const Vector3& RigidBodyComponents::getLocalInertiaTensor(Entity bodyEntity) { assert(mMapEntityToComponentIndex.containsKey(bodyEntity)); @@ -574,7 +595,7 @@ inline const Vector3& RigidBodyComponents::getLocalInertiaTensor(Entity bodyEnti } // Set the local inertia tensor of an entity -inline void RigidBodyComponents::setLocalInertiaTensor(Entity bodyEntity, const Vector3& inertiaTensorLocal) { +RP3D_FORCE_INLINE void RigidBodyComponents::setLocalInertiaTensor(Entity bodyEntity, const Vector3& inertiaTensorLocal) { assert(mMapEntityToComponentIndex.containsKey(bodyEntity)); @@ -582,7 +603,7 @@ inline void RigidBodyComponents::setLocalInertiaTensor(Entity bodyEntity, const } // Set the inverse local inertia tensor of an entity -inline void RigidBodyComponents::setInverseInertiaTensorLocal(Entity bodyEntity, const Vector3& inertiaTensorLocalInverse) { +RP3D_FORCE_INLINE void RigidBodyComponents::setInverseInertiaTensorLocal(Entity bodyEntity, const Vector3& inertiaTensorLocalInverse) { assert(mMapEntityToComponentIndex.containsKey(bodyEntity)); @@ -590,7 +611,7 @@ inline void RigidBodyComponents::setInverseInertiaTensorLocal(Entity bodyEntity, } // Return the constrained linear velocity of an entity -inline const Vector3& RigidBodyComponents::getConstrainedLinearVelocity(Entity bodyEntity) const { +RP3D_FORCE_INLINE const Vector3& RigidBodyComponents::getConstrainedLinearVelocity(Entity bodyEntity) const { assert(mMapEntityToComponentIndex.containsKey(bodyEntity)); @@ -598,7 +619,7 @@ inline const Vector3& RigidBodyComponents::getConstrainedLinearVelocity(Entity b } // Return the constrained angular velocity of an entity -inline const Vector3& RigidBodyComponents::getConstrainedAngularVelocity(Entity bodyEntity) const { +RP3D_FORCE_INLINE const Vector3& RigidBodyComponents::getConstrainedAngularVelocity(Entity bodyEntity) const { assert(mMapEntityToComponentIndex.containsKey(bodyEntity)); @@ -606,7 +627,7 @@ inline const Vector3& RigidBodyComponents::getConstrainedAngularVelocity(Entity } // Return the split linear velocity of an entity -inline const Vector3& RigidBodyComponents::getSplitLinearVelocity(Entity bodyEntity) const { +RP3D_FORCE_INLINE const Vector3& RigidBodyComponents::getSplitLinearVelocity(Entity bodyEntity) const { assert(mMapEntityToComponentIndex.containsKey(bodyEntity)); @@ -614,7 +635,7 @@ inline const Vector3& RigidBodyComponents::getSplitLinearVelocity(Entity bodyEnt } // Return the split angular velocity of an entity -inline const Vector3& RigidBodyComponents::getSplitAngularVelocity(Entity bodyEntity) const { +RP3D_FORCE_INLINE const Vector3& RigidBodyComponents::getSplitAngularVelocity(Entity bodyEntity) const { assert(mMapEntityToComponentIndex.containsKey(bodyEntity)); @@ -622,7 +643,7 @@ inline const Vector3& RigidBodyComponents::getSplitAngularVelocity(Entity bodyEn } // Return the constrained position of an entity -inline Vector3& RigidBodyComponents::getConstrainedPosition(Entity bodyEntity) { +RP3D_FORCE_INLINE Vector3& RigidBodyComponents::getConstrainedPosition(Entity bodyEntity) { assert(mMapEntityToComponentIndex.containsKey(bodyEntity)); @@ -630,7 +651,7 @@ inline Vector3& RigidBodyComponents::getConstrainedPosition(Entity bodyEntity) { } // Return the constrained orientation of an entity -inline Quaternion& RigidBodyComponents::getConstrainedOrientation(Entity bodyEntity) { +RP3D_FORCE_INLINE Quaternion& RigidBodyComponents::getConstrainedOrientation(Entity bodyEntity) { assert(mMapEntityToComponentIndex.containsKey(bodyEntity)); @@ -638,7 +659,7 @@ inline Quaternion& RigidBodyComponents::getConstrainedOrientation(Entity bodyEnt } // Return the local center of mass of an entity -inline const Vector3& RigidBodyComponents::getCenterOfMassLocal(Entity bodyEntity) { +RP3D_FORCE_INLINE const Vector3& RigidBodyComponents::getCenterOfMassLocal(Entity bodyEntity) { assert(mMapEntityToComponentIndex.containsKey(bodyEntity)); @@ -646,7 +667,7 @@ inline const Vector3& RigidBodyComponents::getCenterOfMassLocal(Entity bodyEntit } // Return the world center of mass of an entity -inline const Vector3& RigidBodyComponents::getCenterOfMassWorld(Entity bodyEntity) { +RP3D_FORCE_INLINE const Vector3& RigidBodyComponents::getCenterOfMassWorld(Entity bodyEntity) { assert(mMapEntityToComponentIndex.containsKey(bodyEntity)); @@ -654,7 +675,7 @@ inline const Vector3& RigidBodyComponents::getCenterOfMassWorld(Entity bodyEntit } // Set the constrained linear velocity of an entity -inline void RigidBodyComponents::setConstrainedLinearVelocity(Entity bodyEntity, const Vector3& constrainedLinearVelocity) { +RP3D_FORCE_INLINE void RigidBodyComponents::setConstrainedLinearVelocity(Entity bodyEntity, const Vector3& constrainedLinearVelocity) { assert(mMapEntityToComponentIndex.containsKey(bodyEntity)); @@ -662,7 +683,7 @@ inline void RigidBodyComponents::setConstrainedLinearVelocity(Entity bodyEntity, } // Set the constrained angular velocity of an entity -inline void RigidBodyComponents::setConstrainedAngularVelocity(Entity bodyEntity, const Vector3& constrainedAngularVelocity) { +RP3D_FORCE_INLINE void RigidBodyComponents::setConstrainedAngularVelocity(Entity bodyEntity, const Vector3& constrainedAngularVelocity) { assert(mMapEntityToComponentIndex.containsKey(bodyEntity)); @@ -670,7 +691,7 @@ inline void RigidBodyComponents::setConstrainedAngularVelocity(Entity bodyEntity } // Set the split linear velocity of an entity -inline void RigidBodyComponents::setSplitLinearVelocity(Entity bodyEntity, const Vector3& splitLinearVelocity) { +RP3D_FORCE_INLINE void RigidBodyComponents::setSplitLinearVelocity(Entity bodyEntity, const Vector3& splitLinearVelocity) { assert(mMapEntityToComponentIndex.containsKey(bodyEntity)); @@ -678,7 +699,7 @@ inline void RigidBodyComponents::setSplitLinearVelocity(Entity bodyEntity, const } // Set the split angular velocity of an entity -inline void RigidBodyComponents::setSplitAngularVelocity(Entity bodyEntity, const Vector3& splitAngularVelocity) { +RP3D_FORCE_INLINE void RigidBodyComponents::setSplitAngularVelocity(Entity bodyEntity, const Vector3& splitAngularVelocity) { assert(mMapEntityToComponentIndex.containsKey(bodyEntity)); @@ -686,7 +707,7 @@ inline void RigidBodyComponents::setSplitAngularVelocity(Entity bodyEntity, cons } // Set the constrained position of an entity -inline void RigidBodyComponents::setConstrainedPosition(Entity bodyEntity, const Vector3& constrainedPosition) { +RP3D_FORCE_INLINE void RigidBodyComponents::setConstrainedPosition(Entity bodyEntity, const Vector3& constrainedPosition) { assert(mMapEntityToComponentIndex.containsKey(bodyEntity)); @@ -694,7 +715,7 @@ inline void RigidBodyComponents::setConstrainedPosition(Entity bodyEntity, const } // Set the constrained orientation of an entity -inline void RigidBodyComponents::setConstrainedOrientation(Entity bodyEntity, const Quaternion& constrainedOrientation) { +RP3D_FORCE_INLINE void RigidBodyComponents::setConstrainedOrientation(Entity bodyEntity, const Quaternion& constrainedOrientation) { assert(mMapEntityToComponentIndex.containsKey(bodyEntity)); @@ -702,7 +723,7 @@ inline void RigidBodyComponents::setConstrainedOrientation(Entity bodyEntity, co } // Set the local center of mass of an entity -inline void RigidBodyComponents::setCenterOfMassLocal(Entity bodyEntity, const Vector3& centerOfMassLocal) { +RP3D_FORCE_INLINE void RigidBodyComponents::setCenterOfMassLocal(Entity bodyEntity, const Vector3& centerOfMassLocal) { assert(mMapEntityToComponentIndex.containsKey(bodyEntity)); @@ -710,7 +731,7 @@ inline void RigidBodyComponents::setCenterOfMassLocal(Entity bodyEntity, const V } // Set the world center of mass of an entity -inline void RigidBodyComponents::setCenterOfMassWorld(Entity bodyEntity, const Vector3& centerOfMassWorld) { +RP3D_FORCE_INLINE void RigidBodyComponents::setCenterOfMassWorld(Entity bodyEntity, const Vector3& centerOfMassWorld) { assert(mMapEntityToComponentIndex.containsKey(bodyEntity)); @@ -718,7 +739,7 @@ inline void RigidBodyComponents::setCenterOfMassWorld(Entity bodyEntity, const V } // Return true if gravity is enabled for this entity -inline bool RigidBodyComponents::getIsGravityEnabled(Entity bodyEntity) const { +RP3D_FORCE_INLINE bool RigidBodyComponents::getIsGravityEnabled(Entity bodyEntity) const { assert(mMapEntityToComponentIndex.containsKey(bodyEntity)); @@ -726,7 +747,7 @@ inline bool RigidBodyComponents::getIsGravityEnabled(Entity bodyEntity) const { } // Return true if the entity is already in an island -inline bool RigidBodyComponents::getIsAlreadyInIsland(Entity bodyEntity) const { +RP3D_FORCE_INLINE bool RigidBodyComponents::getIsAlreadyInIsland(Entity bodyEntity) const { assert(mMapEntityToComponentIndex.containsKey(bodyEntity)); @@ -734,7 +755,7 @@ inline bool RigidBodyComponents::getIsAlreadyInIsland(Entity bodyEntity) const { } // Set the value to know if the gravity is enabled for this entity -inline void RigidBodyComponents::setIsGravityEnabled(Entity bodyEntity, bool isGravityEnabled) { +RP3D_FORCE_INLINE void RigidBodyComponents::setIsGravityEnabled(Entity bodyEntity, bool isGravityEnabled) { assert(mMapEntityToComponentIndex.containsKey(bodyEntity)); @@ -742,33 +763,40 @@ inline void RigidBodyComponents::setIsGravityEnabled(Entity bodyEntity, bool isG } // Set the value to know if the entity is already in an island -inline void RigidBodyComponents::setIsAlreadyInIsland(Entity bodyEntity, bool isAlreadyInIsland) { +RP3D_FORCE_INLINE void RigidBodyComponents::setIsAlreadyInIsland(Entity bodyEntity, bool isAlreadyInIsland) { assert(mMapEntityToComponentIndex.containsKey(bodyEntity)); mIsAlreadyInIsland[mMapEntityToComponentIndex[bodyEntity]] = isAlreadyInIsland; } -// Return the list of joints of a body -inline const List& RigidBodyComponents::getJoints(Entity bodyEntity) const { +// Return the array of joints of a body +RP3D_FORCE_INLINE const Array& RigidBodyComponents::getJoints(Entity bodyEntity) const { assert(mMapEntityToComponentIndex.containsKey(bodyEntity)); return mJoints[mMapEntityToComponentIndex[bodyEntity]]; } // Add a joint to a body component -inline void RigidBodyComponents::addJointToBody(Entity bodyEntity, Entity jointEntity) { +RP3D_FORCE_INLINE void RigidBodyComponents::addJointToBody(Entity bodyEntity, Entity jointEntity) { assert(mMapEntityToComponentIndex.containsKey(bodyEntity)); mJoints[mMapEntityToComponentIndex[bodyEntity]].add(jointEntity); } // Remove a joint from a body component -inline void RigidBodyComponents::removeJointFromBody(Entity bodyEntity, Entity jointEntity) { +RP3D_FORCE_INLINE void RigidBodyComponents::removeJointFromBody(Entity bodyEntity, Entity jointEntity) { assert(mMapEntityToComponentIndex.containsKey(bodyEntity)); mJoints[mMapEntityToComponentIndex[bodyEntity]].remove(jointEntity); } +// A an associated contact pairs into the contact pairs array of the body +RP3D_FORCE_INLINE void RigidBodyComponents::addContacPair(Entity bodyEntity, uint contactPairIndex) { + + assert(mMapEntityToComponentIndex.containsKey(bodyEntity)); + mContactPairs[mMapEntityToComponentIndex[bodyEntity]].add(contactPairIndex); +} + } #endif diff --git a/include/reactphysics3d/components/SliderJointComponents.h b/include/reactphysics3d/components/SliderJointComponents.h index f15388ed..56672259 100644 --- a/include/reactphysics3d/components/SliderJointComponents.h +++ b/include/reactphysics3d/components/SliderJointComponents.h @@ -460,266 +460,266 @@ class SliderJointComponents : public Components { }; // Return a pointer to a given joint -inline SliderJoint* SliderJointComponents::getJoint(Entity jointEntity) const { +RP3D_FORCE_INLINE SliderJoint* SliderJointComponents::getJoint(Entity jointEntity) const { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); return mJoints[mMapEntityToComponentIndex[jointEntity]]; } // Set the joint pointer to a given joint -inline void SliderJointComponents::setJoint(Entity jointEntity, SliderJoint* joint) const { +RP3D_FORCE_INLINE void SliderJointComponents::setJoint(Entity jointEntity, SliderJoint* joint) const { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); mJoints[mMapEntityToComponentIndex[jointEntity]] = joint; } // Return the local anchor point of body 1 for a given joint -inline const Vector3& SliderJointComponents::getLocalAnchorPointBody1(Entity jointEntity) const { +RP3D_FORCE_INLINE const Vector3& SliderJointComponents::getLocalAnchorPointBody1(Entity jointEntity) const { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); return mLocalAnchorPointBody1[mMapEntityToComponentIndex[jointEntity]]; } // Set the local anchor point of body 1 for a given joint -inline void SliderJointComponents::setLocalAnchorPointBody1(Entity jointEntity, const Vector3& localAnchoirPointBody1) { +RP3D_FORCE_INLINE void SliderJointComponents::setLocalAnchorPointBody1(Entity jointEntity, const Vector3& localAnchoirPointBody1) { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); mLocalAnchorPointBody1[mMapEntityToComponentIndex[jointEntity]] = localAnchoirPointBody1; } // Return the local anchor point of body 2 for a given joint -inline const Vector3& SliderJointComponents::getLocalAnchorPointBody2(Entity jointEntity) const { +RP3D_FORCE_INLINE const Vector3& SliderJointComponents::getLocalAnchorPointBody2(Entity jointEntity) const { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); return mLocalAnchorPointBody2[mMapEntityToComponentIndex[jointEntity]]; } // Set the local anchor point of body 2 for a given joint -inline void SliderJointComponents::setLocalAnchorPointBody2(Entity jointEntity, const Vector3& localAnchoirPointBody2) { +RP3D_FORCE_INLINE void SliderJointComponents::setLocalAnchorPointBody2(Entity jointEntity, const Vector3& localAnchoirPointBody2) { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); mLocalAnchorPointBody2[mMapEntityToComponentIndex[jointEntity]] = localAnchoirPointBody2; } // Return the inertia tensor of body 1 (in world-space coordinates) -inline const Matrix3x3& SliderJointComponents::getI1(Entity jointEntity) const { +RP3D_FORCE_INLINE const Matrix3x3& SliderJointComponents::getI1(Entity jointEntity) const { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); return mI1[mMapEntityToComponentIndex[jointEntity]]; } // Set the inertia tensor of body 1 (in world-space coordinates) -inline void SliderJointComponents::setI1(Entity jointEntity, const Matrix3x3& i1) { +RP3D_FORCE_INLINE void SliderJointComponents::setI1(Entity jointEntity, const Matrix3x3& i1) { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); mI1[mMapEntityToComponentIndex[jointEntity]] = i1; } // Return the inertia tensor of body 2 (in world-space coordinates) -inline const Matrix3x3& SliderJointComponents::getI2(Entity jointEntity) const { +RP3D_FORCE_INLINE const Matrix3x3& SliderJointComponents::getI2(Entity jointEntity) const { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); return mI2[mMapEntityToComponentIndex[jointEntity]]; } // Set the inertia tensor of body 2 (in world-space coordinates) -inline void SliderJointComponents::setI2(Entity jointEntity, const Matrix3x3& i2) { +RP3D_FORCE_INLINE void SliderJointComponents::setI2(Entity jointEntity, const Matrix3x3& i2) { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); mI2[mMapEntityToComponentIndex[jointEntity]] = i2; } // Return the translation impulse -inline Vector2& SliderJointComponents::getImpulseTranslation(Entity jointEntity) { +RP3D_FORCE_INLINE Vector2& SliderJointComponents::getImpulseTranslation(Entity jointEntity) { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); return mImpulseTranslation[mMapEntityToComponentIndex[jointEntity]]; } // Set the translation impulse -inline void SliderJointComponents::setImpulseTranslation(Entity jointEntity, const Vector2& impulseTranslation) { +RP3D_FORCE_INLINE void SliderJointComponents::setImpulseTranslation(Entity jointEntity, const Vector2& impulseTranslation) { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); mImpulseTranslation[mMapEntityToComponentIndex[jointEntity]] = impulseTranslation; } // Return the translation impulse -inline Vector3& SliderJointComponents::getImpulseRotation(Entity jointEntity) { +RP3D_FORCE_INLINE Vector3& SliderJointComponents::getImpulseRotation(Entity jointEntity) { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); return mImpulseRotation[mMapEntityToComponentIndex[jointEntity]]; } // Set the translation impulse -inline void SliderJointComponents::setImpulseRotation(Entity jointEntity, const Vector3& impulseTranslation) { +RP3D_FORCE_INLINE void SliderJointComponents::setImpulseRotation(Entity jointEntity, const Vector3& impulseTranslation) { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); mImpulseRotation[mMapEntityToComponentIndex[jointEntity]] = impulseTranslation; } // Return the translation inverse mass matrix of the constraint -inline Matrix2x2& SliderJointComponents::getInverseMassMatrixTranslation(Entity jointEntity) { +RP3D_FORCE_INLINE Matrix2x2& SliderJointComponents::getInverseMassMatrixTranslation(Entity jointEntity) { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); return mInverseMassMatrixTranslation[mMapEntityToComponentIndex[jointEntity]]; } // Set the translation inverse mass matrix of the constraint -inline void SliderJointComponents::setInverseMassMatrixTranslation(Entity jointEntity, const Matrix2x2& inverseMassMatrix) { +RP3D_FORCE_INLINE void SliderJointComponents::setInverseMassMatrixTranslation(Entity jointEntity, const Matrix2x2& inverseMassMatrix) { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); mInverseMassMatrixTranslation[mMapEntityToComponentIndex[jointEntity]] = inverseMassMatrix; } // Return the rotation inverse mass matrix of the constraint -inline Matrix3x3& SliderJointComponents::getInverseMassMatrixRotation(Entity jointEntity) { +RP3D_FORCE_INLINE Matrix3x3& SliderJointComponents::getInverseMassMatrixRotation(Entity jointEntity) { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); return mInverseMassMatrixRotation[mMapEntityToComponentIndex[jointEntity]]; } // Set the rotation inverse mass matrix of the constraint -inline void SliderJointComponents::setInverseMassMatrixRotation(Entity jointEntity, const Matrix3x3& inverseMassMatrix) { +RP3D_FORCE_INLINE void SliderJointComponents::setInverseMassMatrixRotation(Entity jointEntity, const Matrix3x3& inverseMassMatrix) { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); mInverseMassMatrixRotation[mMapEntityToComponentIndex[jointEntity]] = inverseMassMatrix; } // Return the translation bias -inline Vector2& SliderJointComponents::getBiasTranslation(Entity jointEntity) { +RP3D_FORCE_INLINE Vector2& SliderJointComponents::getBiasTranslation(Entity jointEntity) { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); return mBiasTranslation[mMapEntityToComponentIndex[jointEntity]]; } // Set the translation impulse -inline void SliderJointComponents::setBiasTranslation(Entity jointEntity, const Vector2& impulseTranslation) { +RP3D_FORCE_INLINE void SliderJointComponents::setBiasTranslation(Entity jointEntity, const Vector2& impulseTranslation) { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); mBiasTranslation[mMapEntityToComponentIndex[jointEntity]] = impulseTranslation; } // Return the rotation bias -inline Vector3& SliderJointComponents::getBiasRotation(Entity jointEntity) { +RP3D_FORCE_INLINE Vector3& SliderJointComponents::getBiasRotation(Entity jointEntity) { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); return mBiasRotation[mMapEntityToComponentIndex[jointEntity]]; } // Set the rotation impulse -inline void SliderJointComponents::setBiasRotation(Entity jointEntity, const Vector3& impulseRotation) { +RP3D_FORCE_INLINE void SliderJointComponents::setBiasRotation(Entity jointEntity, const Vector3& impulseRotation) { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); mBiasRotation[mMapEntityToComponentIndex[jointEntity]] = impulseRotation; } // Return the initial orientation difference -inline Quaternion& SliderJointComponents::getInitOrientationDifferenceInv(Entity jointEntity) { +RP3D_FORCE_INLINE Quaternion& SliderJointComponents::getInitOrientationDifferenceInv(Entity jointEntity) { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); return mInitOrientationDifferenceInv[mMapEntityToComponentIndex[jointEntity]]; } // Set the rotation impulse -inline void SliderJointComponents::setInitOrientationDifferenceInv(Entity jointEntity, const Quaternion& initOrientationDifferenceInv) { +RP3D_FORCE_INLINE void SliderJointComponents::setInitOrientationDifferenceInv(Entity jointEntity, const Quaternion& initOrientationDifferenceInv) { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); mInitOrientationDifferenceInv[mMapEntityToComponentIndex[jointEntity]] = initOrientationDifferenceInv; } // Return the slider axis (in local-space coordinates of body 1) -inline Vector3& SliderJointComponents::getSliderAxisBody1(Entity jointEntity) { +RP3D_FORCE_INLINE Vector3& SliderJointComponents::getSliderAxisBody1(Entity jointEntity) { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); return mSliderAxisBody1[mMapEntityToComponentIndex[jointEntity]]; } // Set the slider axis (in local-space coordinates of body 1) -inline void SliderJointComponents::setSliderAxisBody1(Entity jointEntity, const Vector3& sliderAxisBody1) { +RP3D_FORCE_INLINE void SliderJointComponents::setSliderAxisBody1(Entity jointEntity, const Vector3& sliderAxisBody1) { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); mSliderAxisBody1[mMapEntityToComponentIndex[jointEntity]] = sliderAxisBody1; } // Retunr the slider axis in world-space coordinates -inline Vector3& SliderJointComponents::getSliderAxisWorld(Entity jointEntity) { +RP3D_FORCE_INLINE Vector3& SliderJointComponents::getSliderAxisWorld(Entity jointEntity) { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); return mSliderAxisWorld[mMapEntityToComponentIndex[jointEntity]]; } // Set the slider axis in world-space coordinates -inline void SliderJointComponents::setSliderAxisWorld(Entity jointEntity, const Vector3& sliderAxisWorld) { +RP3D_FORCE_INLINE void SliderJointComponents::setSliderAxisWorld(Entity jointEntity, const Vector3& sliderAxisWorld) { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); mSliderAxisWorld[mMapEntityToComponentIndex[jointEntity]] = sliderAxisWorld; } // Return the vector r1 in world-space coordinates -inline Vector3& SliderJointComponents::getR1(Entity jointEntity) { +RP3D_FORCE_INLINE Vector3& SliderJointComponents::getR1(Entity jointEntity) { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); return mR1[mMapEntityToComponentIndex[jointEntity]]; } // Set the vector r1 in world-space coordinates -inline void SliderJointComponents::setR1(Entity jointEntity, const Vector3& r1) { +RP3D_FORCE_INLINE void SliderJointComponents::setR1(Entity jointEntity, const Vector3& r1) { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); mR1[mMapEntityToComponentIndex[jointEntity]] = r1; } // Return the vector r2 in world-space coordinates -inline Vector3& SliderJointComponents::getR2(Entity jointEntity) { +RP3D_FORCE_INLINE Vector3& SliderJointComponents::getR2(Entity jointEntity) { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); return mR2[mMapEntityToComponentIndex[jointEntity]]; } // Set the vector r2 in world-space coordinates -inline void SliderJointComponents::setR2(Entity jointEntity, const Vector3& r2) { +RP3D_FORCE_INLINE void SliderJointComponents::setR2(Entity jointEntity, const Vector3& r2) { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); mR2[mMapEntityToComponentIndex[jointEntity]] = r2; } // Return the first vector orthogonal to the slider axis local-space of body 1 -inline Vector3& SliderJointComponents::getN1(Entity jointEntity) { +RP3D_FORCE_INLINE Vector3& SliderJointComponents::getN1(Entity jointEntity) { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); return mN1[mMapEntityToComponentIndex[jointEntity]]; } // Set the first vector orthogonal to the slider axis local-space of body 1 -inline void SliderJointComponents::setN1(Entity jointEntity, const Vector3& n1) { +RP3D_FORCE_INLINE void SliderJointComponents::setN1(Entity jointEntity, const Vector3& n1) { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); mN1[mMapEntityToComponentIndex[jointEntity]] = n1; } // Return the second vector orthogonal to the slider axis and mN1 in local-space of body 1 -inline Vector3& SliderJointComponents::getN2(Entity jointEntity) { +RP3D_FORCE_INLINE Vector3& SliderJointComponents::getN2(Entity jointEntity) { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); return mN2[mMapEntityToComponentIndex[jointEntity]]; } // Set the second vector orthogonal to the slider axis and mN1 in local-space of body 1 -inline void SliderJointComponents::setN2(Entity jointEntity, const Vector3& n2) { +RP3D_FORCE_INLINE void SliderJointComponents::setN2(Entity jointEntity, const Vector3& n2) { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); mN2[mMapEntityToComponentIndex[jointEntity]] = n2; } // Return the accumulated impulse for the lower limit constraint -inline decimal SliderJointComponents::getImpulseLowerLimit(Entity jointEntity) const { +RP3D_FORCE_INLINE decimal SliderJointComponents::getImpulseLowerLimit(Entity jointEntity) const { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); return mImpulseLowerLimit[mMapEntityToComponentIndex[jointEntity]]; } // Set the accumulated impulse for the lower limit constraint -inline void SliderJointComponents::setImpulseLowerLimit(Entity jointEntity, decimal impulseLowerLimit) { +RP3D_FORCE_INLINE void SliderJointComponents::setImpulseLowerLimit(Entity jointEntity, decimal impulseLowerLimit) { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); mImpulseLowerLimit[mMapEntityToComponentIndex[jointEntity]] = impulseLowerLimit; @@ -727,14 +727,14 @@ inline void SliderJointComponents::setImpulseLowerLimit(Entity jointEntity, deci // Return the accumulated impulse for the upper limit constraint -inline decimal SliderJointComponents::getImpulseUpperLimit(Entity jointEntity) const { +RP3D_FORCE_INLINE decimal SliderJointComponents::getImpulseUpperLimit(Entity jointEntity) const { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); return mImpulseUpperLimit[mMapEntityToComponentIndex[jointEntity]]; } // Set the accumulated impulse for the upper limit constraint -inline void SliderJointComponents::setImpulseUpperLimit(Entity jointEntity, decimal impulseUpperLimit) const { +RP3D_FORCE_INLINE void SliderJointComponents::setImpulseUpperLimit(Entity jointEntity, decimal impulseUpperLimit) const { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); mImpulseUpperLimit[mMapEntityToComponentIndex[jointEntity]] = impulseUpperLimit; @@ -742,266 +742,266 @@ inline void SliderJointComponents::setImpulseUpperLimit(Entity jointEntity, deci // Return the accumulated impulse for the motor constraint; -inline decimal SliderJointComponents::getImpulseMotor(Entity jointEntity) const { +RP3D_FORCE_INLINE decimal SliderJointComponents::getImpulseMotor(Entity jointEntity) const { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); return mImpulseMotor[mMapEntityToComponentIndex[jointEntity]]; } // Set the accumulated impulse for the motor constraint; -inline void SliderJointComponents::setImpulseMotor(Entity jointEntity, decimal impulseMotor) { +RP3D_FORCE_INLINE void SliderJointComponents::setImpulseMotor(Entity jointEntity, decimal impulseMotor) { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); mImpulseMotor[mMapEntityToComponentIndex[jointEntity]] = impulseMotor; } // Return the inverse of mass matrix K=JM^-1J^t for the limits (1x1 matrix) -inline decimal SliderJointComponents::getInverseMassMatrixLimit(Entity jointEntity) const { +RP3D_FORCE_INLINE decimal SliderJointComponents::getInverseMassMatrixLimit(Entity jointEntity) const { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); return mInverseMassMatrixLimit[mMapEntityToComponentIndex[jointEntity]]; } // Set the inverse of mass matrix K=JM^-1J^t for the limits (1x1 matrix) -inline void SliderJointComponents::setInverseMassMatrixLimit(Entity jointEntity, decimal inverseMassMatrixLimitMotor) { +RP3D_FORCE_INLINE void SliderJointComponents::setInverseMassMatrixLimit(Entity jointEntity, decimal inverseMassMatrixLimitMotor) { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); mInverseMassMatrixLimit[mMapEntityToComponentIndex[jointEntity]] = inverseMassMatrixLimitMotor; } // Return the inverse of mass matrix K=JM^-1J^t for the motor -inline decimal SliderJointComponents::getInverseMassMatrixMotor(Entity jointEntity) { +RP3D_FORCE_INLINE decimal SliderJointComponents::getInverseMassMatrixMotor(Entity jointEntity) { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); return mInverseMassMatrixMotor[mMapEntityToComponentIndex[jointEntity]]; } // Return the inverse of mass matrix K=JM^-1J^t for the motor -inline void SliderJointComponents::setInverseMassMatrixMotor(Entity jointEntity, decimal inverseMassMatrixMotor) { +RP3D_FORCE_INLINE void SliderJointComponents::setInverseMassMatrixMotor(Entity jointEntity, decimal inverseMassMatrixMotor) { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); mInverseMassMatrixMotor[mMapEntityToComponentIndex[jointEntity]] = inverseMassMatrixMotor; } // Return the bias of the lower limit constraint -inline decimal SliderJointComponents::getBLowerLimit(Entity jointEntity) const { +RP3D_FORCE_INLINE decimal SliderJointComponents::getBLowerLimit(Entity jointEntity) const { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); return mBLowerLimit[mMapEntityToComponentIndex[jointEntity]]; } // Set the bias of the lower limit constraint -inline void SliderJointComponents::setBLowerLimit(Entity jointEntity, decimal bLowerLimit) const { +RP3D_FORCE_INLINE void SliderJointComponents::setBLowerLimit(Entity jointEntity, decimal bLowerLimit) const { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); mBLowerLimit[mMapEntityToComponentIndex[jointEntity]] = bLowerLimit; } // Return the bias of the upper limit constraint -inline decimal SliderJointComponents::getBUpperLimit(Entity jointEntity) const { +RP3D_FORCE_INLINE decimal SliderJointComponents::getBUpperLimit(Entity jointEntity) const { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); return mBUpperLimit[mMapEntityToComponentIndex[jointEntity]]; } // Set the bias of the upper limit constraint -inline void SliderJointComponents::setBUpperLimit(Entity jointEntity, decimal bUpperLimit) { +RP3D_FORCE_INLINE void SliderJointComponents::setBUpperLimit(Entity jointEntity, decimal bUpperLimit) { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); mBUpperLimit[mMapEntityToComponentIndex[jointEntity]] = bUpperLimit; } // Return true if the joint limits are enabled -inline bool SliderJointComponents::getIsLimitEnabled(Entity jointEntity) const { +RP3D_FORCE_INLINE bool SliderJointComponents::getIsLimitEnabled(Entity jointEntity) const { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); return mIsLimitEnabled[mMapEntityToComponentIndex[jointEntity]]; } // Set to true if the joint limits are enabled -inline void SliderJointComponents::setIsLimitEnabled(Entity jointEntity, bool isLimitEnabled) { +RP3D_FORCE_INLINE void SliderJointComponents::setIsLimitEnabled(Entity jointEntity, bool isLimitEnabled) { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); mIsLimitEnabled[mMapEntityToComponentIndex[jointEntity]] = isLimitEnabled; } // Return true if the motor of the joint in enabled -inline bool SliderJointComponents::getIsMotorEnabled(Entity jointEntity) const { +RP3D_FORCE_INLINE bool SliderJointComponents::getIsMotorEnabled(Entity jointEntity) const { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); return mIsMotorEnabled[mMapEntityToComponentIndex[jointEntity]]; } // Set to true if the motor of the joint in enabled -inline void SliderJointComponents::setIsMotorEnabled(Entity jointEntity, bool isMotorEnabled) const { +RP3D_FORCE_INLINE void SliderJointComponents::setIsMotorEnabled(Entity jointEntity, bool isMotorEnabled) const { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); mIsMotorEnabled[mMapEntityToComponentIndex[jointEntity]] = isMotorEnabled; } // Return the Lower limit (minimum allowed rotation angle in radian) -inline decimal SliderJointComponents::getLowerLimit(Entity jointEntity) const { +RP3D_FORCE_INLINE decimal SliderJointComponents::getLowerLimit(Entity jointEntity) const { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); return mLowerLimit[mMapEntityToComponentIndex[jointEntity]]; } // Set the Lower limit (minimum allowed rotation angle in radian) -inline void SliderJointComponents::setLowerLimit(Entity jointEntity, decimal lowerLimit) const { +RP3D_FORCE_INLINE void SliderJointComponents::setLowerLimit(Entity jointEntity, decimal lowerLimit) const { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); mLowerLimit[mMapEntityToComponentIndex[jointEntity]] = lowerLimit; } // Return the upper limit (maximum translation distance) -inline decimal SliderJointComponents::getUpperLimit(Entity jointEntity) const { +RP3D_FORCE_INLINE decimal SliderJointComponents::getUpperLimit(Entity jointEntity) const { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); return mUpperLimit[mMapEntityToComponentIndex[jointEntity]]; } // Set the upper limit (maximum translation distance) -inline void SliderJointComponents::setUpperLimit(Entity jointEntity, decimal upperLimit) { +RP3D_FORCE_INLINE void SliderJointComponents::setUpperLimit(Entity jointEntity, decimal upperLimit) { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); mUpperLimit[mMapEntityToComponentIndex[jointEntity]] = upperLimit; } // Return true if the lower limit is violated -inline bool SliderJointComponents::getIsLowerLimitViolated(Entity jointEntity) const { +RP3D_FORCE_INLINE bool SliderJointComponents::getIsLowerLimitViolated(Entity jointEntity) const { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); return mIsLowerLimitViolated[mMapEntityToComponentIndex[jointEntity]]; } // Set to true if the lower limit is violated -inline void SliderJointComponents::setIsLowerLimitViolated(Entity jointEntity, bool isLowerLimitViolated) { +RP3D_FORCE_INLINE void SliderJointComponents::setIsLowerLimitViolated(Entity jointEntity, bool isLowerLimitViolated) { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); mIsLowerLimitViolated[mMapEntityToComponentIndex[jointEntity]] = isLowerLimitViolated; } // Return true if the upper limit is violated -inline bool SliderJointComponents::getIsUpperLimitViolated(Entity jointEntity) const { +RP3D_FORCE_INLINE bool SliderJointComponents::getIsUpperLimitViolated(Entity jointEntity) const { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); return mIsUpperLimitViolated[mMapEntityToComponentIndex[jointEntity]]; } // Set to true if the upper limit is violated -inline void SliderJointComponents::setIsUpperLimitViolated(Entity jointEntity, bool isUpperLimitViolated) const { +RP3D_FORCE_INLINE void SliderJointComponents::setIsUpperLimitViolated(Entity jointEntity, bool isUpperLimitViolated) const { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); mIsUpperLimitViolated[mMapEntityToComponentIndex[jointEntity]] = isUpperLimitViolated; } // Return the motor speed (in rad/s) -inline decimal SliderJointComponents::getMotorSpeed(Entity jointEntity) const { +RP3D_FORCE_INLINE decimal SliderJointComponents::getMotorSpeed(Entity jointEntity) const { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); return mMotorSpeed[mMapEntityToComponentIndex[jointEntity]]; } // Set the motor speed (in rad/s) -inline void SliderJointComponents::setMotorSpeed(Entity jointEntity, decimal motorSpeed) { +RP3D_FORCE_INLINE void SliderJointComponents::setMotorSpeed(Entity jointEntity, decimal motorSpeed) { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); mMotorSpeed[mMapEntityToComponentIndex[jointEntity]] = motorSpeed; } // Return the maximum motor torque (in Newtons) that can be applied to reach to desired motor speed -inline decimal SliderJointComponents::getMaxMotorForce(Entity jointEntity) const { +RP3D_FORCE_INLINE decimal SliderJointComponents::getMaxMotorForce(Entity jointEntity) const { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); return mMaxMotorForce[mMapEntityToComponentIndex[jointEntity]]; } // Set the maximum motor torque (in Newtons) that can be applied to reach to desired motor speed -inline void SliderJointComponents::setMaxMotorForce(Entity jointEntity, decimal maxMotorForce) { +RP3D_FORCE_INLINE void SliderJointComponents::setMaxMotorForce(Entity jointEntity, decimal maxMotorForce) { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); mMaxMotorForce[mMapEntityToComponentIndex[jointEntity]] = maxMotorForce; } // Return the cross product of r2 and n1 -inline Vector3& SliderJointComponents::getR2CrossN1(Entity jointEntity) { +RP3D_FORCE_INLINE Vector3& SliderJointComponents::getR2CrossN1(Entity jointEntity) { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); return mR2CrossN1[mMapEntityToComponentIndex[jointEntity]]; } // Set the cross product of r2 and n1 -inline void SliderJointComponents::setR2CrossN1(Entity jointEntity, const Vector3& r2CrossN1) { +RP3D_FORCE_INLINE void SliderJointComponents::setR2CrossN1(Entity jointEntity, const Vector3& r2CrossN1) { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); mR2CrossN1[mMapEntityToComponentIndex[jointEntity]] = r2CrossN1; } // Return the cross product of r2 and n2 -inline Vector3& SliderJointComponents::getR2CrossN2(Entity jointEntity) { +RP3D_FORCE_INLINE Vector3& SliderJointComponents::getR2CrossN2(Entity jointEntity) { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); return mR2CrossN2[mMapEntityToComponentIndex[jointEntity]]; } // Set the cross product of r2 and n2 -inline void SliderJointComponents::setR2CrossN2(Entity jointEntity, const Vector3& r2CrossN2) { +RP3D_FORCE_INLINE void SliderJointComponents::setR2CrossN2(Entity jointEntity, const Vector3& r2CrossN2) { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); mR2CrossN2[mMapEntityToComponentIndex[jointEntity]] = r2CrossN2; } // Return the cross product of r2 and the slider axis -inline Vector3& SliderJointComponents::getR2CrossSliderAxis(Entity jointEntity) { +RP3D_FORCE_INLINE Vector3& SliderJointComponents::getR2CrossSliderAxis(Entity jointEntity) { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); return mR2CrossSliderAxis[mMapEntityToComponentIndex[jointEntity]]; } // Set the cross product of r2 and the slider axis -inline void SliderJointComponents::setR2CrossSliderAxis(Entity jointEntity, const Vector3& r2CrossSliderAxis) { +RP3D_FORCE_INLINE void SliderJointComponents::setR2CrossSliderAxis(Entity jointEntity, const Vector3& r2CrossSliderAxis) { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); mR2CrossSliderAxis[mMapEntityToComponentIndex[jointEntity]] = r2CrossSliderAxis; } // Return the cross product of vector (r1 + u) and n1 -inline Vector3& SliderJointComponents::getR1PlusUCrossN1(Entity jointEntity) { +RP3D_FORCE_INLINE Vector3& SliderJointComponents::getR1PlusUCrossN1(Entity jointEntity) { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); return mR1PlusUCrossN1[mMapEntityToComponentIndex[jointEntity]]; } // Set the cross product of vector (r1 + u) and n1 -inline void SliderJointComponents::setR1PlusUCrossN1(Entity jointEntity, const Vector3& r1PlusUCrossN1) { +RP3D_FORCE_INLINE void SliderJointComponents::setR1PlusUCrossN1(Entity jointEntity, const Vector3& r1PlusUCrossN1) { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); mR1PlusUCrossN1[mMapEntityToComponentIndex[jointEntity]] = r1PlusUCrossN1; } // Return the cross product of vector (r1 + u) and n2 -inline Vector3& SliderJointComponents::getR1PlusUCrossN2(Entity jointEntity) { +RP3D_FORCE_INLINE Vector3& SliderJointComponents::getR1PlusUCrossN2(Entity jointEntity) { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); return mR1PlusUCrossN2[mMapEntityToComponentIndex[jointEntity]]; } // Set the cross product of vector (r1 + u) and n2 -inline void SliderJointComponents::setR1PlusUCrossN2(Entity jointEntity, const Vector3& r1PlusUCrossN2) { +RP3D_FORCE_INLINE void SliderJointComponents::setR1PlusUCrossN2(Entity jointEntity, const Vector3& r1PlusUCrossN2) { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); mR1PlusUCrossN2[mMapEntityToComponentIndex[jointEntity]] = r1PlusUCrossN2; } // Return the cross product of vector (r1 + u) and the slider axis -inline Vector3& SliderJointComponents::getR1PlusUCrossSliderAxis(Entity jointEntity) { +RP3D_FORCE_INLINE Vector3& SliderJointComponents::getR1PlusUCrossSliderAxis(Entity jointEntity) { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); return mR1PlusUCrossSliderAxis[mMapEntityToComponentIndex[jointEntity]]; } // Set the cross product of vector (r1 + u) and the slider axis -inline void SliderJointComponents::setR1PlusUCrossSliderAxis(Entity jointEntity, const Vector3& r1PlusUCrossSliderAxis) { +RP3D_FORCE_INLINE void SliderJointComponents::setR1PlusUCrossSliderAxis(Entity jointEntity, const Vector3& r1PlusUCrossSliderAxis) { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); mR1PlusUCrossSliderAxis[mMapEntityToComponentIndex[jointEntity]] = r1PlusUCrossSliderAxis; diff --git a/include/reactphysics3d/components/TransformComponents.h b/include/reactphysics3d/components/TransformComponents.h index a0d5878a..011be27d 100644 --- a/include/reactphysics3d/components/TransformComponents.h +++ b/include/reactphysics3d/components/TransformComponents.h @@ -107,13 +107,13 @@ class TransformComponents : public Components { }; // Return the transform of an entity -inline Transform& TransformComponents::getTransform(Entity bodyEntity) const { +RP3D_FORCE_INLINE Transform& TransformComponents::getTransform(Entity bodyEntity) const { assert(mMapEntityToComponentIndex.containsKey(bodyEntity)); return mTransforms[mMapEntityToComponentIndex[bodyEntity]]; } // Set the transform of an entity -inline void TransformComponents::setTransform(Entity bodyEntity, const Transform& transform) { +RP3D_FORCE_INLINE void TransformComponents::setTransform(Entity bodyEntity, const Transform& transform) { assert(mMapEntityToComponentIndex.containsKey(bodyEntity)); mTransforms[mMapEntityToComponentIndex[bodyEntity]] = transform; } diff --git a/include/reactphysics3d/configuration.h b/include/reactphysics3d/configuration.h index 6283a488..4aca00cf 100644 --- a/include/reactphysics3d/configuration.h +++ b/include/reactphysics3d/configuration.h @@ -35,15 +35,35 @@ #include #include -// Windows platform +// Platforms #if defined(WIN32) ||defined(_WIN32) || defined(_WIN64) ||defined(__WIN32__) || defined(__WINDOWS__) #define WINDOWS_OS -#elif defined(__APPLE__) // Apple platform +#elif defined(__APPLE__) #define APPLE_OS #elif defined(__linux__) || defined(linux) || defined(__linux) // Linux platform #define LINUX_OS #endif +// Compilers +#if defined(_MSC_VER) + #define RP3D_COMPILER_VISUAL_STUDIO +#elif defined(__clang__) + #define RP3D_COMPILER_CLANG +#elif defined(__GNUC__) + #define RP3D_COMPILER_GCC +#else + #define RP3D_COMPILER_UNKNOWN +#endif + +// Force RP3D_FORCE_INLINE macro +#if defined(RP3D_COMPILER_VISUAL_STUDIO) + #define RP3D_FORCE_INLINE __forceinline +#elif defined(RP3D_COMPILER_GCC) || defined(RP3D_COMPILER_CLANG) + #define RP3D_FORCE_INLINE inline __attribute__((always_inline)) +#else + #define RP3D_FORCE_INLINE inline +#endif + /// Namespace reactphysics3d namespace reactphysics3d { @@ -103,6 +123,18 @@ constexpr decimal PI_TIMES_2 = decimal(6.28318530); /// without triggering a large modification of the tree each frame which can be costly constexpr decimal DYNAMIC_TREE_FAT_AABB_INFLATE_PERCENTAGE = decimal(0.08); +/// Maximum number of contact points in a narrow phase info object +constexpr uint8 NB_MAX_CONTACT_POINTS_IN_NARROWPHASE_INFO = 16; + +/// Maximum number of contact manifolds in an overlapping pair +constexpr uint8 NB_MAX_CONTACT_MANIFOLDS = 3; + +/// Maximum number of potential contact manifolds in an overlapping pair +constexpr uint8 NB_MAX_POTENTIAL_CONTACT_MANIFOLDS = 4 * NB_MAX_CONTACT_MANIFOLDS; + +/// Maximum number of contact points in potential contact manifold +constexpr uint8 NB_MAX_CONTACT_POINTS_IN_POTENTIAL_MANIFOLD = 16; + /// Current version of ReactPhysics3D const std::string RP3D_VERSION = std::string("0.8.0"); diff --git a/include/reactphysics3d/constraint/BallAndSocketJoint.h b/include/reactphysics3d/constraint/BallAndSocketJoint.h index fe834a6b..ee671664 100644 --- a/include/reactphysics3d/constraint/BallAndSocketJoint.h +++ b/include/reactphysics3d/constraint/BallAndSocketJoint.h @@ -129,7 +129,7 @@ class BallAndSocketJoint : public Joint { }; // Return the number of bytes used by the joint -inline size_t BallAndSocketJoint::getSizeInBytes() const { +RP3D_FORCE_INLINE size_t BallAndSocketJoint::getSizeInBytes() const { return sizeof(BallAndSocketJoint); } diff --git a/include/reactphysics3d/constraint/ContactPoint.h b/include/reactphysics3d/constraint/ContactPoint.h index 66d321a5..d8491088 100644 --- a/include/reactphysics3d/constraint/ContactPoint.h +++ b/include/reactphysics3d/constraint/ContactPoint.h @@ -144,7 +144,7 @@ class ContactPoint { /** * @return The contact normal */ -inline const Vector3& ContactPoint::getNormal() const { +RP3D_FORCE_INLINE const Vector3& ContactPoint::getNormal() const { return mNormal; } @@ -152,7 +152,7 @@ inline const Vector3& ContactPoint::getNormal() const { /** * @return The contact point on the first collider in the local-space of the collider */ -inline const Vector3& ContactPoint::getLocalPointOnShape1() const { +RP3D_FORCE_INLINE const Vector3& ContactPoint::getLocalPointOnShape1() const { return mLocalPointOnShape1; } @@ -160,7 +160,7 @@ inline const Vector3& ContactPoint::getLocalPointOnShape1() const { /** * @return The contact point on the second collider in the local-space of the collider */ -inline const Vector3& ContactPoint::getLocalPointOnShape2() const { +RP3D_FORCE_INLINE const Vector3& ContactPoint::getLocalPointOnShape2() const { return mLocalPointOnShape2; } @@ -168,12 +168,12 @@ inline const Vector3& ContactPoint::getLocalPointOnShape2() const { /** * @return The penetration impulse */ -inline decimal ContactPoint::getPenetrationImpulse() const { +RP3D_FORCE_INLINE decimal ContactPoint::getPenetrationImpulse() const { return mPenetrationImpulse; } // Return true if the contact point is similar (close enougth) to another given contact point -inline bool ContactPoint::isSimilarWithContactPoint(const ContactPointInfo* localContactPointBody1) const { +RP3D_FORCE_INLINE bool ContactPoint::isSimilarWithContactPoint(const ContactPointInfo* localContactPointBody1) const { return (localContactPointBody1->localPoint1 - mLocalPointOnShape1).lengthSquare() <= (mPersistentContactDistanceThreshold * mPersistentContactDistanceThreshold); } @@ -182,7 +182,7 @@ inline bool ContactPoint::isSimilarWithContactPoint(const ContactPointInfo* loca /** * @param impulse Penetration impulse */ -inline void ContactPoint::setPenetrationImpulse(decimal impulse) { +RP3D_FORCE_INLINE void ContactPoint::setPenetrationImpulse(decimal impulse) { mPenetrationImpulse = impulse; } @@ -190,7 +190,7 @@ inline void ContactPoint::setPenetrationImpulse(decimal impulse) { /** * @return True if it is a resting contact */ -inline bool ContactPoint::getIsRestingContact() const { +RP3D_FORCE_INLINE bool ContactPoint::getIsRestingContact() const { return mIsRestingContact; } @@ -198,7 +198,7 @@ inline bool ContactPoint::getIsRestingContact() const { /** * @param isRestingContact True if it is a resting contact */ -inline void ContactPoint::setIsRestingContact(bool isRestingContact) { +RP3D_FORCE_INLINE void ContactPoint::setIsRestingContact(bool isRestingContact) { mIsRestingContact = isRestingContact; } @@ -206,7 +206,7 @@ inline void ContactPoint::setIsRestingContact(bool isRestingContact) { /** * @return the penetration depth (in meters) */ -inline decimal ContactPoint::getPenetrationDepth() const { +RP3D_FORCE_INLINE decimal ContactPoint::getPenetrationDepth() const { return mPenetrationDepth; } @@ -214,7 +214,7 @@ inline decimal ContactPoint::getPenetrationDepth() const { /** * @return The size of the contact point in memory (in bytes) */ -inline size_t ContactPoint::getSizeInBytes() const { +RP3D_FORCE_INLINE size_t ContactPoint::getSizeInBytes() const { return sizeof(ContactPoint); } diff --git a/include/reactphysics3d/constraint/FixedJoint.h b/include/reactphysics3d/constraint/FixedJoint.h index 84b7508a..fa2ac350 100644 --- a/include/reactphysics3d/constraint/FixedJoint.h +++ b/include/reactphysics3d/constraint/FixedJoint.h @@ -119,7 +119,7 @@ class FixedJoint : public Joint { }; // Return the number of bytes used by the joint -inline size_t FixedJoint::getSizeInBytes() const { +RP3D_FORCE_INLINE size_t FixedJoint::getSizeInBytes() const { return sizeof(FixedJoint); } diff --git a/include/reactphysics3d/constraint/HingeJoint.h b/include/reactphysics3d/constraint/HingeJoint.h index ed583e02..d8c85134 100644 --- a/include/reactphysics3d/constraint/HingeJoint.h +++ b/include/reactphysics3d/constraint/HingeJoint.h @@ -311,6 +311,9 @@ class HingeJoint : public Joint { /// Return the intensity of the current torque applied for the joint motor decimal getMotorTorque(decimal timeStep) const; + /// Return the current hinge angle + decimal getAngle() const; + /// Return a string representation virtual std::string to_string() const override; }; diff --git a/include/reactphysics3d/constraint/Joint.h b/include/reactphysics3d/constraint/Joint.h index e3856830..dc16d311 100644 --- a/include/reactphysics3d/constraint/Joint.h +++ b/include/reactphysics3d/constraint/Joint.h @@ -157,7 +157,7 @@ class Joint { /** * @return The entity of the joint */ -inline Entity Joint::getEntity() const { +RP3D_FORCE_INLINE Entity Joint::getEntity() const { return mEntity; } diff --git a/include/reactphysics3d/constraint/SliderJoint.h b/include/reactphysics3d/constraint/SliderJoint.h index f9f4830f..517716a9 100644 --- a/include/reactphysics3d/constraint/SliderJoint.h +++ b/include/reactphysics3d/constraint/SliderJoint.h @@ -309,7 +309,7 @@ class SliderJoint : public Joint { }; // Return the number of bytes used by the joint -inline size_t SliderJoint::getSizeInBytes() const { +RP3D_FORCE_INLINE size_t SliderJoint::getSizeInBytes() const { return sizeof(SliderJoint); } diff --git a/include/reactphysics3d/containers/List.h b/include/reactphysics3d/containers/Array.h similarity index 69% rename from include/reactphysics3d/containers/List.h rename to include/reactphysics3d/containers/Array.h index a85fb65a..74c9a507 100755 --- a/include/reactphysics3d/containers/List.h +++ b/include/reactphysics3d/containers/Array.h @@ -23,8 +23,8 @@ * * ********************************************************************************/ -#ifndef REACTPHYSICS3D_LIST_H -#define REACTPHYSICS3D_LIST_H +#ifndef REACTPHYSICS3D_ARRAY_H +#define REACTPHYSICS3D_ARRAY_H // Libraries #include @@ -36,25 +36,25 @@ namespace reactphysics3d { -// Class List +// Class Array /** - * This class represents a simple generic list with custom memory allocator. + * This class represents a simple dynamic array with custom memory allocator. */ template -class List { +class Array { private: // -------------------- Attributes -------------------- // - /// Buffer for the list elements - void* mBuffer; + /// Buffer for the array elements + T* mBuffer; - /// Number of elements in the list - size_t mSize; + /// Number of elements in the array + uint32 mSize; - /// Number of allocated elements in the list - size_t mCapacity; + /// Number of allocated elements in the array + uint32 mCapacity; /// Memory allocator MemoryAllocator& mAllocator; @@ -63,15 +63,15 @@ class List { /// Class Iterator /** - * This class represents an iterator for the List + * This class represents an iterator for the array */ class Iterator { private: - size_t mCurrentIndex; + uint32 mCurrentIndex; T* mBuffer; - size_t mSize; + uint32 mSize; public: @@ -88,7 +88,7 @@ class List { Iterator() = default; /// Constructor - Iterator(void* buffer, size_t index, size_t size) + Iterator(void* buffer, uint32 index, uint32 size) :mCurrentIndex(index), mBuffer(static_cast(buffer)), mSize(size) { } @@ -198,7 +198,7 @@ class List { bool operator==(const Iterator& iterator) const { assert(mCurrentIndex >= 0 && mCurrentIndex <= mSize); - // If both iterators points to the end of the list + // If both iterators points to the end of the array if (mCurrentIndex == mSize && iterator.mCurrentIndex == iterator.mSize) { return true; } @@ -212,14 +212,14 @@ class List { } /// Frienship - friend class List; + friend class Array; }; // -------------------- Methods -------------------- // /// Constructor - List(MemoryAllocator& allocator, size_t capacity = 0) + Array(MemoryAllocator& allocator, uint32 capacity = 0) : mBuffer(nullptr), mSize(0), mCapacity(0), mAllocator(allocator) { if (capacity > 0) { @@ -230,43 +230,47 @@ class List { } /// Copy constructor - List(const List& list) : mBuffer(nullptr), mSize(0), mCapacity(0), mAllocator(list.mAllocator) { + Array(const Array& array) : mBuffer(nullptr), mSize(0), mCapacity(0), mAllocator(array.mAllocator) { - // All all the elements of the list to the current one - addRange(list); + // If we need to allocate more memory + if (array.mCapacity > 0) { + reserve(array.mCapacity); + } + + // All all the elements of the array to the current one + addRange(array); } /// Destructor - ~List() { + ~Array() { // If elements have been allocated if (mCapacity > 0) { - // Clear the list + // Clear the array clear(true); } } /// Allocate memory for a given number of elements - void reserve(size_t capacity) { + void reserve(uint32 capacity) { if (capacity <= mCapacity) return; // Allocate memory for the new array void* newMemory = mAllocator.allocate(capacity * sizeof(T)); + T* destination = static_cast(newMemory); if (mBuffer != nullptr) { if (mSize > 0) { // Copy the elements to the new allocated memory location - T* destination = static_cast(newMemory); - T* items = static_cast(mBuffer); - std::uninitialized_copy(items, items + mSize, destination); + std::uninitialized_copy(mBuffer, mBuffer + mSize, destination); // Destruct the previous items - for (size_t i=0; i(mBuffer) + mSize * sizeof(T)) T(element); + // Use the constructor to construct the element + new (reinterpret_cast(mBuffer + mSize)) T(element); mSize++; } - /// Add a given numbers of elements at the end of the list but do not init them - void addWithoutInit(uint nbElements) { + /// Add an element into the array by constructing it directly into the array (in order to avoid a copy) + template + void emplace(Ts&&... args) { // If we need to allocate more memory if (mSize == mCapacity) { + reserve(mCapacity == 0 ? 1 : mCapacity * 2); + } + + // Construct the element directly at its location in the array + new (reinterpret_cast(mBuffer + mSize)) T(std::forward(args)...); + + mSize++; + } + + /// Add a given numbers of elements at the end of the array but do not init them + void addWithoutInit(uint32 nbElements) { + + // If we need to allocate more memory + if ((mSize + nbElements) > mCapacity) { reserve(mCapacity == 0 ? nbElements : (mCapacity + nbElements) * 2); } mSize += nbElements; } - /// Try to find a given item of the list and return an iterator - /// pointing to that element if it exists in the list. Otherwise, + /// Try to find a given item of the array and return an iterator + /// pointing to that element if it exists in the array. Otherwise, /// this method returns the end() iterator Iterator find(const T& element) { - for (uint i=0; i(mBuffer)[i]) { + for (uint32 i=0; i= 0 && index < mSize); + assert(index < mSize); // Call the destructor - (static_cast(mBuffer)[index]).~T(); + mBuffer[index].~T(); mSize--; if (index != mSize) { // Move the elements to fill in the empty slot - char* dest = static_cast(mBuffer) + index * sizeof(T); - char* src = dest + sizeof(T); - std::memmove(static_cast(dest), static_cast(src), (mSize - index) * sizeof(T)); + void* dest = reinterpret_cast(mBuffer + index); + std::uintptr_t src = reinterpret_cast(dest) + sizeof(T); + std::memmove(dest, reinterpret_cast(src), (mSize - index) * sizeof(T)); } // Return an iterator pointing to the element after the removed one return Iterator(mBuffer, index, mSize); } - /// Append another list at the end of the current one - void addRange(const List& list) { + /// Remove an element from the list at a given index and replace it by the last one of the list (if any) + void removeAtAndReplaceByLast(uint32 index) { + + assert(index < mSize); + + mBuffer[index] = mBuffer[mSize - 1]; + + // Call the destructor of the last element + mBuffer[mSize - 1].~T(); + + mSize--; + } + + /// Remove an element from the array at a given index and replace it by the last one of the array (if any) + /// Append another array at the end of the current one + void addRange(const Array& array, uint32 startIndex = 0) { + + assert(startIndex <= array.size()); // If we need to allocate more memory - if (mSize + list.size() > mCapacity) { + if (mSize + (array.size() - startIndex) > mCapacity) { // Allocate memory - reserve(mSize + list.size()); + reserve(mSize + array.size() - startIndex); } - // Add the elements of the list to the current one - for(uint i=0; i(mBuffer) + mSize * sizeof(T)) T(list[i]); + new (reinterpret_cast(mBuffer + mSize)) T(array[i]); mSize++; } } - /// Clear the list + /// Clear the array void clear(bool releaseMemory = false) { // Call the destructor of each element - for (uint i=0; i < mSize; i++) { - (static_cast(mBuffer)[i]).~T(); + for (uint32 i=0; i < mSize; i++) { + mBuffer[i].~T(); } mSize = 0; @@ -392,36 +427,35 @@ class List { } } - /// Return the number of elements in the list - size_t size() const { + /// Return the number of elements in the array + uint32 size() const { return mSize; } - /// Return the capacity of the list - size_t capacity() const { + /// Return the capacity of the array + uint32 capacity() const { return mCapacity; } /// Overloaded index operator - T& operator[](const uint index) { + T& operator[](const uint32 index) { assert(index >= 0 && index < mSize); - return (static_cast(mBuffer)[index]); + return mBuffer[index]; } /// Overloaded const index operator - const T& operator[](const uint index) const { + const T& operator[](const uint32 index) const { assert(index >= 0 && index < mSize); - return (static_cast(mBuffer)[index]); + return mBuffer[index]; } /// Overloaded equality operator - bool operator==(const List& list) const { + bool operator==(const Array& array) const { - if (mSize != list.mSize) return false; + if (mSize != array.mSize) return false; - T* items = static_cast(mBuffer); - for (size_t i=0; i < mSize; i++) { - if (items[i] != list[i]) { + for (uint32 i=0; i < mSize; i++) { + if (mBuffer[i] != array[i]) { return false; } } @@ -430,21 +464,21 @@ class List { } /// Overloaded not equal operator - bool operator!=(const List& list) const { + bool operator!=(const Array& array) const { - return !((*this) == list); + return !((*this) == array); } /// Overloaded assignment operator - List& operator=(const List& list) { + Array& operator=(const Array& array) { - if (this != &list) { + if (this != &array) { // Clear all the elements clear(); - // Add all the elements of the list to the current one - addRange(list); + // Add all the elements of the array to the current one + addRange(array); } return *this; diff --git a/include/reactphysics3d/containers/LinkedList.h b/include/reactphysics3d/containers/LinkedList.h index c2221d4f..914bfaa3 100644 --- a/include/reactphysics3d/containers/LinkedList.h +++ b/include/reactphysics3d/containers/LinkedList.h @@ -93,20 +93,20 @@ class LinkedList { // Return the first element of the list template -inline typename LinkedList::ListElement* LinkedList::getListHead() const { +RP3D_FORCE_INLINE typename LinkedList::ListElement* LinkedList::getListHead() const { return mListHead; } // Insert an element at the beginning of the linked list template -inline void LinkedList::insert(const T& data) { +RP3D_FORCE_INLINE void LinkedList::insert(const T& data) { ListElement* element = new (mAllocator.allocate(sizeof(ListElement))) ListElement(data, mListHead); mListHead = element; } // Remove all the elements of the list template -inline void LinkedList::reset() { +RP3D_FORCE_INLINE void LinkedList::reset() { // Release all the list elements ListElement* element = mListHead; diff --git a/include/reactphysics3d/containers/Map.h b/include/reactphysics3d/containers/Map.h index a486f529..6bb4df00 100755 --- a/include/reactphysics3d/containers/Map.h +++ b/include/reactphysics3d/containers/Map.h @@ -47,202 +47,59 @@ class Map { private: - /// An entry of the map - struct Entry { - - size_t hashCode; // Hash code of the entry - int next; // Index of the next entry - Pair* keyValue; // Pointer to the pair with key and value - - /// Constructor - Entry() { - next = -1; - keyValue = nullptr; - } - - /// Constructor - Entry(size_t hashcode, int nextEntry) { - hashCode = hashcode; - next = nextEntry; - keyValue = nullptr; - } - - /// Copy-constructor - Entry(const Entry& entry) { - hashCode = entry.hashCode; - next = entry.next; - keyValue = entry.keyValue; - } - - /// Destructor - ~Entry() { - - } - - }; - // -------------------- Constants -------------------- // - /// Number of prime numbers in array - static constexpr int NB_PRIMES = 70; + /// Default load factor + static constexpr float DEFAULT_LOAD_FACTOR = 0.75; - /// Array of prime numbers for the size of the map - static const int PRIMES[NB_PRIMES]; - - /// Largest prime number - static int LARGEST_PRIME; + /// Invalid index in the array + static constexpr uint32 INVALID_INDEX = 0xffffffff; // -------------------- Attributes -------------------- // - /// Current number of used entries in the map - int mNbUsedEntries; + /// Total number of allocated entries + uint32 mNbAllocatedEntries; - /// Number of free entries among the used ones - int mNbFreeEntries; + /// Number of items in the set + uint32 mNbEntries; - /// Current capacity of the map - int mCapacity; + /// Number of buckets and size of the hash table (nbEntries = loadFactor * mHashSize) + uint32 mHashSize ; /// Array with all the buckets - int* mBuckets; + uint32* mBuckets; /// Array with all the entries - Entry* mEntries; + Pair* mEntries; + + /// For each entry, index of the next entry at the same bucket + uint32* mNextEntries; /// Memory allocator MemoryAllocator& mAllocator; /// Index to the fist free entry - int mFreeIndex; + uint32 mFreeIndex; // -------------------- Methods -------------------- // - /// Initialize the map - void initialize(int capacity) { - - // Compute the next larger prime size - mCapacity = getPrimeSize(capacity); - assert(mCapacity >= 0); - - // Allocate memory for the buckets - mBuckets = static_cast(mAllocator.allocate(mCapacity * sizeof(int))); - - // Allocate memory for the entries - mEntries = static_cast(mAllocator.allocate(mCapacity * sizeof(Entry))); - - // Initialize the buckets and entries - for (int i=0; i= 0); - } - - /// Expand the capacity of the map - void expand(int newCapacity) { - - assert(newCapacity > mCapacity); - assert(isPrimeNumber(newCapacity)); - - // Allocate memory for the buckets - int* newBuckets = static_cast(mAllocator.allocate(newCapacity * sizeof(int))); - - // Allocate memory for the entries - Entry* newEntries = static_cast(mAllocator.allocate(newCapacity * sizeof(Entry))); - - // Initialize the new buckets - for (int i=0; i 0) { - - // Copy the old entries to the new allocated memory location - std::uninitialized_copy(mEntries, mEntries + mNbUsedEntries, newEntries); - - // Destruct the old entries in the previous location - for (int i=0; i < mNbUsedEntries; i++) { - mEntries[i].~Entry(); - } - } - - // Construct the new entries - for (int i=mNbUsedEntries; i(&newEntries[i])) Entry(); - } - - // For each used entry - for (int i=0; i= 0); - } - /// Return the index of the entry with a given key or -1 if there is no entry with this key int findEntry(const K& key) const { - if (mCapacity > 0) { + if (mHashSize > 0) { const size_t hashCode = Hash()(key); - int bucket = hashCode % mCapacity; + const uint32 bucket = hashCode & (mHashSize - 1); auto keyEqual = KeyEqual(); - for (int i = mBuckets[bucket]; i >= 0; i = mEntries[i].next) { - if (mEntries[i].hashCode == hashCode && keyEqual(mEntries[i].keyValue->first, key)) { + for (uint32 i = mBuckets[bucket]; i != INVALID_INDEX; i = mNextEntries[i]) { + if (Hash()(mEntries[i].first) == hashCode && keyEqual(mEntries[i].first, key)) { return i; } } } - return -1; - } - - /// Return the prime number that is larger or equal to the number in parameter - /// for the size of the map - static int getPrimeSize(int number) { - - // Check if the next larger prime number is in the precomputed array of primes - for (int i = 0; i < NB_PRIMES; i++) { - if (PRIMES[i] >= number) return PRIMES[i]; - } - - // Manually compute the next larger prime number - for (int i = (number | 1); i < std::numeric_limits::max(); i+=2) { - - if (isPrimeNumber(i)) { - return i; - } - } - - return number; + return INVALID_INDEX; } public: @@ -255,36 +112,36 @@ class Map { private: - /// Array of entries - const Entry* mEntries; + /// Pointer to the map + const Map* mMap; - /// Capacity of the map - int mCapacity; - - /// Number of used entries in the map - int mNbUsedEntries; + /// Index of the current bucket + uint32 mCurrentBucketIndex; /// Index of the current entry - int mCurrentEntry; + uint32 mCurrentEntryIndex; /// Advance the iterator void advance() { - // If we are trying to move past the end - assert(mCurrentEntry < mNbUsedEntries); + assert(mCurrentBucketIndex < mMap->mHashSize); + assert(mCurrentEntryIndex < mMap->mNbAllocatedEntries); - for (mCurrentEntry += 1; mCurrentEntry < mNbUsedEntries; mCurrentEntry++) { - - // If the entry is not empty - if (mEntries[mCurrentEntry].keyValue != nullptr) { - - // We have found the next non empty entry - return; - } + // Try the next entry + if (mMap->mNextEntries[mCurrentEntryIndex] != INVALID_INDEX) { + mCurrentEntryIndex = mMap->mNextEntries[mCurrentEntryIndex]; + return; } - // We have not find a non empty entry, we return an iterator to the end - mCurrentEntry = mCapacity; + // Try to move to the next bucket + mCurrentEntryIndex = 0; + mCurrentBucketIndex++; + while(mCurrentBucketIndex < mMap->mHashSize && mMap->mBuckets[mCurrentBucketIndex] == INVALID_INDEX) { + mCurrentBucketIndex++; + } + if (mCurrentBucketIndex < mMap->mHashSize) { + mCurrentEntryIndex = mMap->mBuckets[mCurrentBucketIndex]; + } } public: @@ -300,29 +157,29 @@ class Map { Iterator() = default; /// Constructor - Iterator(const Entry* entries, int capacity, int nbUsedEntries, int currentEntry) - :mEntries(entries), mCapacity(capacity), mNbUsedEntries(nbUsedEntries), mCurrentEntry(currentEntry) { + Iterator(const Map* map, uint32 bucketIndex, uint32 entryIndex) + :mMap(map), mCurrentBucketIndex(bucketIndex), mCurrentEntryIndex(entryIndex) { } /// Copy constructor Iterator(const Iterator& it) - :mEntries(it.mEntries), mCapacity(it.mCapacity), mNbUsedEntries(it.mNbUsedEntries), mCurrentEntry(it.mCurrentEntry) { + :mMap(it.mMap), mCurrentBucketIndex(it.mCurrentBucketIndex), mCurrentEntryIndex(it.mCurrentEntryIndex) { } /// Deferencable reference operator*() const { - assert(mCurrentEntry >= 0 && mCurrentEntry < mNbUsedEntries); - assert(mEntries[mCurrentEntry].keyValue != nullptr); - return *(mEntries[mCurrentEntry].keyValue); + assert(mCurrentEntryIndex < mMap->mNbAllocatedEntries); + assert(mCurrentEntryIndex != INVALID_INDEX); + return mMap->mEntries[mCurrentEntryIndex]; } /// Deferencable pointer operator->() const { - assert(mCurrentEntry >= 0 && mCurrentEntry < mNbUsedEntries); - assert(mEntries[mCurrentEntry].keyValue != nullptr); - return mEntries[mCurrentEntry].keyValue; + assert(mCurrentEntryIndex < mMap->mNbAllocatedEntries); + assert(mCurrentEntryIndex != INVALID_INDEX); + return &(mMap->mEntries[mCurrentEntryIndex]); } /// Post increment (it++) @@ -332,7 +189,7 @@ class Map { } /// Pre increment (++it) - Iterator operator++(int number) { + Iterator operator++(int) { Iterator tmp = *this; advance(); return tmp; @@ -340,69 +197,73 @@ class Map { /// Equality operator (it == end()) bool operator==(const Iterator& iterator) const { - return mCurrentEntry == iterator.mCurrentEntry && mEntries == iterator.mEntries; + return mCurrentBucketIndex == iterator.mCurrentBucketIndex && mCurrentEntryIndex == iterator.mCurrentEntryIndex && mMap == iterator.mMap; } /// Inequality operator (it != end()) bool operator!=(const Iterator& iterator) const { return !(*this == iterator); } + + /// Overloaded assignment operator + Iterator& operator=(const Iterator& it) { + + // Check for self assignment + if (this != &it) { + + mMap = it.mMap; + mCurrentBucketIndex = it.mCurrentBucketIndex; + mCurrentEntryIndex = it.mCurrentEntryIndex; + } + + return *this; + } }; // -------------------- Methods -------------------- // /// Constructor - Map(MemoryAllocator& allocator, size_t capacity = 0) - : mNbUsedEntries(0), mNbFreeEntries(0), mCapacity(0), mBuckets(nullptr), - mEntries(nullptr), mAllocator(allocator), mFreeIndex(-1) { - - // If the largest prime has not been computed yet - if (LARGEST_PRIME == -1) { - - // Compute the largest prime number (largest map capacity) - LARGEST_PRIME = getPrimeSize(PRIMES[NB_PRIMES - 1] + 2); - } + Map(MemoryAllocator& allocator, uint32 capacity = 0) + : mNbAllocatedEntries(0), mNbEntries(0), mHashSize(0), mBuckets(nullptr), + mEntries(nullptr), mNextEntries(nullptr), mAllocator(allocator), mFreeIndex(INVALID_INDEX) { if (capacity > 0) { - initialize(capacity); + reserve(capacity); } } /// Copy constructor Map(const Map& map) - :mNbUsedEntries(map.mNbUsedEntries), mNbFreeEntries(map.mNbFreeEntries), mCapacity(map.mCapacity), - mBuckets(nullptr), mEntries(nullptr), mAllocator(map.mAllocator), mFreeIndex(map.mFreeIndex) { + :mNbAllocatedEntries(map.mNbAllocatedEntries), mNbEntries(map.mNbEntries), mHashSize(map.mHashSize), + mBuckets(nullptr), mEntries(nullptr), mNextEntries(nullptr), mAllocator(map.mAllocator), mFreeIndex(map.mFreeIndex) { - assert(capacity() >= 0); + // Allocate memory for the buckets + mBuckets = static_cast(mAllocator.allocate(mHashSize * sizeof(uint32))); - if (mCapacity > 0) { + // Allocate memory for the entries + mEntries = static_cast*>(mAllocator.allocate(mNbAllocatedEntries * sizeof(Pair))); + mNextEntries = static_cast(mAllocator.allocate(mNbAllocatedEntries * sizeof(uint32))); - // Allocate memory for the buckets - mBuckets = static_cast(mAllocator.allocate(mCapacity * sizeof(int))); + // Copy the buckets array + std::memcpy(mBuckets, map.mBuckets, mHashSize * sizeof(uint32)); - // Allocate memory for the entries - mEntries = static_cast(mAllocator.allocate(mCapacity * sizeof(Entry))); + // Copy the next entries indices + std::memcpy(mNextEntries, map.mNextEntries, mNbAllocatedEntries * sizeof(uint32)); - // Copy the buckets - std::uninitialized_copy(map.mBuckets, map.mBuckets + mCapacity, mBuckets); + // Copy the entries + for (uint32 i=0; i(map.mEntries[entryIndex]); - if (map.mEntries[i].keyValue != nullptr) { - mEntries[i].keyValue = static_cast*>(mAllocator.allocate(sizeof(Pair))); - new (mEntries[i].keyValue) Pair(*(map.mEntries[i].keyValue)); - } + entryIndex = mNextEntries[entryIndex]; } - } - - assert(size() >= 0); - assert((*this) == map); } /// Destructor @@ -412,95 +273,162 @@ class Map { } /// Allocate memory for a given number of elements - void reserve(int capacity) { + void reserve(uint32 capacity) { - if (capacity <= mCapacity) return; + if (capacity <= mHashSize) return; - if (capacity > LARGEST_PRIME && LARGEST_PRIME > mCapacity) { - capacity = LARGEST_PRIME; - } - else { - capacity = getPrimeSize(capacity); - } + if (capacity < 16) capacity = 16; - expand(capacity); + // Make sure we have a power of two size + if (!isPowerOfTwo(capacity)) { + capacity = nextPowerOfTwo32Bits(capacity); + } + + assert(capacity < INVALID_INDEX); + + assert(capacity > mHashSize); + + // Allocate memory for the buckets + uint32* newBuckets = static_cast(mAllocator.allocate(capacity * sizeof(uint32))); + + // Allocate memory for the entries + const uint32 nbAllocatedEntries = capacity * DEFAULT_LOAD_FACTOR; + assert(nbAllocatedEntries > 0); + Pair* newEntries = static_cast*>(mAllocator.allocate(nbAllocatedEntries * sizeof(Pair))); + uint32* newNextEntries = static_cast(mAllocator.allocate(nbAllocatedEntries * sizeof(uint32))); + + assert(newEntries != nullptr); + assert(newNextEntries != nullptr); + + // Initialize the new buckets + for (uint32 i=0; i 0) { + + assert(mNextEntries != nullptr); + + // Copy the free nodes indices in the nextEntries array + std::memcpy(newNextEntries, mNextEntries, mNbAllocatedEntries * sizeof(uint32)); + } + + // Recompute the buckets (hash) with the new hash size + for (uint32 i=0; i(mEntries[entryIndex]); + mEntries[entryIndex].~Pair(); + + entryIndex = mNextEntries[entryIndex]; + } + } + + if (mNbAllocatedEntries > 0) { + + // Release previously allocated memory + mAllocator.release(mBuckets, mHashSize * sizeof(uint32)); + mAllocator.release(mEntries, mNbAllocatedEntries * sizeof(Pair)); + mAllocator.release(mNextEntries, mNbAllocatedEntries * sizeof(uint32)); + } + + // Add the new entries to the free list + for (uint32 i=mNbAllocatedEntries; i < nbAllocatedEntries-1; i++) { + newNextEntries[i] = i + 1; + } + newNextEntries[nbAllocatedEntries - 1] = mFreeIndex; + mFreeIndex = mNbAllocatedEntries; + + mHashSize = capacity; + mNbAllocatedEntries = nbAllocatedEntries; + mBuckets = newBuckets; + mEntries = newEntries; + mNextEntries = newNextEntries; + + assert(mFreeIndex != INVALID_INDEX); } /// Return true if the map contains an item with the given key bool containsKey(const K& key) const { - return findEntry(key) != -1; + return findEntry(key) != INVALID_INDEX; } /// Add an element into the map - void add(const Pair& keyValue, bool insertIfAlreadyPresent = false) { + /// Returns true if the item has been inserted and false otherwise. + bool add(const Pair& keyValue, bool insertIfAlreadyPresent = false) { - if (mCapacity == 0) { - initialize(0); - } + uint32 bucket; - // Compute the hash code of the key + // Compute the hash code of the value const size_t hashCode = Hash()(keyValue.first); - // Compute the corresponding bucket index - int bucket = hashCode % mCapacity; + if (mHashSize > 0) { - auto keyEqual = KeyEqual(); + // Compute the corresponding bucket index + bucket = hashCode & (mHashSize - 1); - // Check if the item is already in the map - for (int i = mBuckets[bucket]; i >= 0; i = mEntries[i].next) { + auto keyEqual = KeyEqual(); - // If there is already an item with the same key in the map - if (mEntries[i].hashCode == hashCode && keyEqual(mEntries[i].keyValue->first, keyValue.first)) { + // Check if the item is already in the set + for (uint32 i = mBuckets[bucket]; i != INVALID_INDEX; i = mNextEntries[i]) { - if (insertIfAlreadyPresent) { + // If there is already an item with the same value in the set + if (Hash()(mEntries[i].first) == hashCode && keyEqual(mEntries[i].first, keyValue.first)) { - // Destruct the previous key/value - mEntries[i].keyValue->~Pair(); + if (insertIfAlreadyPresent) { - // Copy construct the new key/value - new (mEntries[i].keyValue) Pair(keyValue); + // Destruct the previous key/value + mEntries[i].~Pair(); - return; - } - else { - throw std::runtime_error("The key and value pair already exists in the map"); + // Copy construct the new key/value + new (mEntries + i) Pair(keyValue); + + return true; + } + else { + assert(false); + throw std::runtime_error("The key and value pair already exists in the map"); + } } } } size_t entryIndex; - // If there are free entries to use - if (mNbFreeEntries > 0) { - assert(mFreeIndex >= 0); - entryIndex = mFreeIndex; - mFreeIndex = mEntries[entryIndex].next; - mNbFreeEntries--; - } - else { + // If there are no more free entries to use + if (mFreeIndex == INVALID_INDEX) { - // If we need to allocator more entries - if (mNbUsedEntries == mCapacity) { + // Allocate more memory + reserve(mHashSize == 0 ? 16 : mHashSize * 2); - // Allocate more memory - reserve(mCapacity * 2); - - // Recompute the bucket index - bucket = hashCode % mCapacity; - } - - entryIndex = mNbUsedEntries; - mNbUsedEntries++; + // Recompute the bucket index + bucket = hashCode & (mHashSize - 1); } - assert(size() >= 0); - assert(mEntries[entryIndex].keyValue == nullptr); - mEntries[entryIndex].hashCode = hashCode; - mEntries[entryIndex].next = mBuckets[bucket]; - mEntries[entryIndex].keyValue = static_cast*>(mAllocator.allocate(sizeof(Pair))); - assert(mEntries[entryIndex].keyValue != nullptr); - new (mEntries[entryIndex].keyValue) Pair(keyValue); + assert(mNbEntries < mNbAllocatedEntries); + assert(mFreeIndex != INVALID_INDEX); + + // Get the next free entry + entryIndex = mFreeIndex; + mFreeIndex = mNextEntries[entryIndex]; + + mNbEntries++; + + mNextEntries[entryIndex] = mBuckets[bucket]; + new (mEntries + entryIndex) Pair(keyValue); mBuckets[bucket] = entryIndex; + + return true; } /// Remove the element pointed by some iterator @@ -517,110 +445,102 @@ class Map { /// the one that has been removed Iterator remove(const K& key) { - if (mCapacity > 0) { + if (mHashSize > 0) { const size_t hashcode = Hash()(key); - int bucket = hashcode % mCapacity; - int last = -1; auto keyEqual = KeyEqual(); + const uint32 bucket = hashcode & (mHashSize - 1); + uint32 last = INVALID_INDEX; + for (uint32 i = mBuckets[bucket]; i != INVALID_INDEX; last = i, i = mNextEntries[i]) { - for (int i = mBuckets[bucket]; i >= 0; last = i, i = mEntries[i].next) { + // If we have found the item + if (Hash()(mEntries[i].first) == hashcode && keyEqual(mEntries[i].first, key)) { - if (mEntries[i].hashCode == hashcode && keyEqual(mEntries[i].keyValue->first, key)) { - - if (last < 0 ) { - mBuckets[bucket] = mEntries[i].next; + if (last == INVALID_INDEX) { + mBuckets[bucket] = mNextEntries[i]; } else { - mEntries[last].next = mEntries[i].next; + mNextEntries[last] = mNextEntries[i]; } - // Release memory for the key/value pair if any - if (mEntries[i].keyValue != nullptr) { - mEntries[i].keyValue->~Pair(); - mAllocator.release(mEntries[i].keyValue, sizeof(Pair)); - mEntries[i].keyValue = nullptr; - } - assert(mEntries[i].keyValue == nullptr); - mEntries[i].next = mFreeIndex; + uint32 nextEntryIndex = mNextEntries[i]; + uint32 nextBucketIndex = bucket; + + mEntries[i].~Pair(); + mNextEntries[i] = mFreeIndex; mFreeIndex = i; - mNbFreeEntries++; + mNbEntries--; - // Find the next entry to return the iterator - for (i += 1; i < mNbUsedEntries; i++) { - - // If the entry is not empty - if (mEntries[i].keyValue != nullptr) { - - // We have found the next non empty entry - return Iterator(mEntries, mCapacity, mNbUsedEntries, i); + // Find the next entry to return an iterator + if (nextEntryIndex == INVALID_INDEX) { + nextEntryIndex = 0; + nextBucketIndex++; + while(nextBucketIndex < mHashSize && mBuckets[nextBucketIndex] == INVALID_INDEX) { + nextBucketIndex++; + } + if (nextBucketIndex < mHashSize) { + nextEntryIndex = mBuckets[nextBucketIndex]; } } - return end(); + // We have found the next non empty entry + return Iterator(this, nextBucketIndex, nextEntryIndex); } } } - assert(size() >= 0); - - // Return the end iterator return end(); } /// Clear the map void clear(bool releaseMemory = false) { - if (mNbUsedEntries > 0) { + for (uint32 i=0; i~Pair(); - mAllocator.release(mEntries[i].keyValue, sizeof(Pair)); - mEntries[i].keyValue = nullptr; - } + // Destroy the entry + mEntries[entryIndex].~Pair(); + + uint32 nextEntryIndex = mNextEntries[entryIndex]; + + // Add entry to the free list + mNextEntries[entryIndex] = mFreeIndex; + mFreeIndex = entryIndex; + + entryIndex = nextEntryIndex; } - mFreeIndex = -1; - mNbUsedEntries = 0; - mNbFreeEntries = 0; - - assert(size() >= 0); + mBuckets[i] = INVALID_INDEX; } - // If elements have been allocated - if (releaseMemory && mCapacity > 0) { + if (releaseMemory && mNbAllocatedEntries > 0) { - // Destroy the entries - for (int i=0; i < mCapacity; i++) { - mEntries[i].~Entry(); - } + // Release previously allocated memory + mAllocator.release(mBuckets, mHashSize * sizeof(uint32)); + mAllocator.release(mEntries, mNbAllocatedEntries * sizeof(Pair)); + mAllocator.release(mNextEntries, mNbAllocatedEntries * sizeof(uint32)); - // Release memory - mAllocator.release(mBuckets, mCapacity * sizeof(int)); - mAllocator.release(mEntries, mCapacity * sizeof(Entry)); - - mCapacity = 0; mBuckets = nullptr; mEntries = nullptr; + mNextEntries = nullptr; + + mNbAllocatedEntries = 0; + mHashSize = 0; } - assert(size() == 0); - } + mNbEntries = 0; + } /// Return the number of elements in the map - int size() const { - assert(mNbUsedEntries - mNbFreeEntries >= 0); - return mNbUsedEntries - mNbFreeEntries; + uint32 size() const { + return mNbEntries; } /// Return the capacity of the map - int capacity() const { - return mCapacity; + uint32 capacity() const { + return mHashSize; } /// Try to find an item of the map given a key. @@ -628,68 +548,54 @@ class Map { /// an iterator pointing to the end if not found Iterator find(const K& key) const { - int bucket; - int entry = -1; + uint32 bucket; + uint32 entry = INVALID_INDEX; - if (mCapacity > 0) { + if (mHashSize > 0) { const size_t hashCode = Hash()(key); - bucket = hashCode % mCapacity; - + bucket = hashCode & (mHashSize - 1); auto keyEqual = KeyEqual(); - for (int i = mBuckets[bucket]; i >= 0; i = mEntries[i].next) { - if (mEntries[i].hashCode == hashCode && keyEqual(mEntries[i].keyValue->first, key)) { + for (uint32 i = mBuckets[bucket]; i != INVALID_INDEX; i = mNextEntries[i]) { + if (Hash()(mEntries[i].first) == hashCode && keyEqual(mEntries[i].first, key)) { entry = i; break; } } } - if (entry == -1) { + if (entry == INVALID_INDEX) { return end(); } - assert(mEntries[entry].keyValue != nullptr); - - return Iterator(mEntries, mCapacity, mNbUsedEntries, entry); + return Iterator(this, bucket, entry); } /// Overloaded index operator V& operator[](const K& key) { - int entry = -1; + const uint32 entry = findEntry(key); - if (mCapacity > 0) { - entry = findEntry(key); - } - - if (entry == -1) { + if (entry == INVALID_INDEX) { assert(false); throw std::runtime_error("No item with given key has been found in the map"); } - assert(mEntries[entry].keyValue != nullptr); - - return mEntries[entry].keyValue->second; + return mEntries[entry].second; } /// Overloaded index operator const V& operator[](const K& key) const { - int entry = -1; + const uint32 entry = findEntry(key); - if (mCapacity > 0) { - entry = findEntry(key); - } - - if (entry == -1) { + if (entry == INVALID_INDEX) { + assert(false); throw std::runtime_error("No item with given key has been found in the map"); } - assert(mEntries[entry].keyValue != nullptr); - - return mEntries[entry].keyValue->second; + return mEntries[entry].second; } /// Overloaded equality operator @@ -729,40 +635,38 @@ class Map { // Clear the map clear(true); - if (map.mCapacity > 0) { + mNbAllocatedEntries = map.mNbAllocatedEntries; + mNbEntries = map.mNbEntries; + mHashSize = map.mHashSize; + mFreeIndex = map.mFreeIndex; - // Compute the next larger prime size - mCapacity = getPrimeSize(map.mCapacity); - assert(mCapacity >= 0); + // Allocate memory for the buckets + mBuckets = static_cast(mAllocator.allocate(mHashSize * sizeof(uint32))); - // Allocate memory for the buckets - mBuckets = static_cast(mAllocator.allocate(mCapacity * sizeof(int))); + // Allocate memory for the entries + mEntries = static_cast*>(mAllocator.allocate(mNbAllocatedEntries * sizeof(Pair))); + mNextEntries = static_cast(mAllocator.allocate(mNbAllocatedEntries * sizeof(uint32))); - // Allocate memory for the entries - mEntries = static_cast(mAllocator.allocate(mCapacity * sizeof(Entry))); + // Copy the buckets array + std::memcpy(mBuckets, map.mBuckets, mHashSize * sizeof(uint32)); - // Copy the buckets - std::uninitialized_copy(map.mBuckets, map.mBuckets + mCapacity, mBuckets); + // Copy the next entries indices + std::memcpy(mNextEntries, map.mNextEntries, mNbAllocatedEntries * sizeof(uint32)); - // Copy the entries - for (int i=0; i < mCapacity; i++) { + // Copy the entries + for (uint32 i=0; i*>(mAllocator.allocate(sizeof(Pair))); - new (mEntries[i].keyValue) Pair(*(map.mEntries[i].keyValue)); - } + // Copy the entry to the new location and destroy the previous one + new (mEntries + entryIndex) Pair(map.mEntries[entryIndex]); + + entryIndex = mNextEntries[entryIndex]; } - - mNbUsedEntries = map.mNbUsedEntries; - mNbFreeEntries = map.mNbFreeEntries; - mFreeIndex = map.mFreeIndex; } } - assert(size() >= 0); - return *this; } @@ -777,33 +681,28 @@ class Map { } // Find the first used entry - int entry; - for (entry=0; entry < mNbUsedEntries; entry++) { - if (mEntries[entry].keyValue != nullptr) { - return Iterator(mEntries, mCapacity, mNbUsedEntries, entry); - } + uint32 bucketIndex = 0; + while (mBuckets[bucketIndex] == INVALID_INDEX) { + + bucketIndex++; } - assert(false); - return end(); + assert(bucketIndex < mHashSize); + assert(mBuckets[bucketIndex] != INVALID_INDEX); + + return Iterator(this, bucketIndex, mBuckets[bucketIndex]); } /// Return a end iterator Iterator end() const { - return Iterator(mEntries, mCapacity, mNbUsedEntries, mCapacity); + return Iterator(this, mHashSize, 0); } + + // ---------- Friendship ---------- // + + friend class Iterator; }; -template -const int Map::PRIMES[NB_PRIMES] = {3, 7, 11, 17, 23, 29, 37, 47, 59, 71, 89, 107, 131, 163, 197, 239, 293, 353, 431, 521, 631, 761, 919, - 1103, 1327, 1597, 1931, 2333, 2801, 3371, 4049, 4861, 5839, 7013, 8419, 10103, 12143, 14591, - 17519, 21023, 25229, 30293, 36353, 43627, 52361, 62851, 75431, 90523, 108631, 130363, 156437, - 187751, 225307, 270371, 324449, 389357, 467237, 560689, 672827, 807403, 968897, 1162687, 1395263, - 1674319, 2009191, 2411033, 2893249, 3471899, 4166287, 4999559}; - -template -int Map::LARGEST_PRIME = -1; - } #endif diff --git a/include/reactphysics3d/containers/Set.h b/include/reactphysics3d/containers/Set.h index 17088fe3..3be397b8 100755 --- a/include/reactphysics3d/containers/Set.h +++ b/include/reactphysics3d/containers/Set.h @@ -47,197 +47,59 @@ class Set { private: - /// An entry of the set - struct Entry { - - size_t hashCode; // Hash code of the entry - int next; // Index of the next entry - V* value; // Pointer to the value stored in the entry - - /// Constructor - Entry() { - next = -1; - value = nullptr; - } - - /// Constructor - Entry(size_t hashcode, int nextEntry) { - hashCode = hashcode; - next = nextEntry; - value = nullptr; - } - - /// Copy-constructor - Entry(const Entry& entry) { - hashCode = entry.hashCode; - next = entry.next; - value = entry.value; - } - - /// Destructor - ~Entry() { - - } - - }; - // -------------------- Constants -------------------- // - /// Number of prime numbers in array - static constexpr int NB_PRIMES = 70; + /// Default load factor + static constexpr float DEFAULT_LOAD_FACTOR = 0.75; - /// Array of prime numbers for the size of the set - static const int PRIMES[NB_PRIMES]; - - /// Largest prime number - static int LARGEST_PRIME; + /// Invalid index in the array + static constexpr uint32 INVALID_INDEX = 0xffffffff; // -------------------- Attributes -------------------- // - /// Current number of used entries in the set - int mNbUsedEntries; + /// Total number of allocated entries + uint32 mNbAllocatedEntries; - /// Number of free entries among the used ones - int mNbFreeEntries; + /// Number of items in the set + uint32 mNbEntries; - /// Current capacity of the set - int mCapacity; + /// Number of buckets and size of the hash table (nbEntries = loadFactor * mHashSize) + uint32 mHashSize ; /// Array with all the buckets - int* mBuckets; + uint32* mBuckets; - /// Array with all the entries - Entry* mEntries; + /// Array with all the entries (nbEntries = loadFactor * mHashSize) + V* mEntries; + + /// For each entry, index of the next entry at the same bucket + uint32* mNextEntries; /// Memory allocator MemoryAllocator& mAllocator; /// Index to the fist free entry - int mFreeIndex; + uint32 mFreeIndex; // -------------------- Methods -------------------- // - /// Initialize the set - void initialize(int capacity) { - - // Compute the next larger prime size - mCapacity = getPrimeSize(capacity); - - // Allocate memory for the buckets - mBuckets = static_cast(mAllocator.allocate(mCapacity * sizeof(int))); - - // Allocate memory for the entries - mEntries = static_cast(mAllocator.allocate(mCapacity * sizeof(Entry))); - - // Initialize the buckets and entries - for (int i=0; i mCapacity); - assert(isPrimeNumber(newCapacity)); - - // Allocate memory for the buckets - int* newBuckets = static_cast(mAllocator.allocate(newCapacity * sizeof(int))); - - // Allocate memory for the entries - Entry* newEntries = static_cast(mAllocator.allocate(newCapacity * sizeof(Entry))); - - // Initialize the new buckets - for (int i=0; i 0) { - - // Copy the old entries to the new allocated memory location - std::uninitialized_copy(mEntries, mEntries + mNbUsedEntries, newEntries); - - // Destruct the old entries at previous location - for (int i=0; i(&newEntries[i])) Entry(); - } - - // For each used entry - for (int i=0; i 0) { + if (mHashSize > 0) { - size_t hashCode = Hash()(value); - int bucket = hashCode % mCapacity; - auto keyEqual = KeyEqual(); + const size_t hashCode = Hash()(value); + const uint32 bucket = hashCode & (mHashSize - 1); + auto keyEqual = KeyEqual(); - for (int i = mBuckets[bucket]; i >= 0; i = mEntries[i].next) { - if (mEntries[i].hashCode == hashCode && keyEqual(*mEntries[i].value, value)) { + for (uint32 i = mBuckets[bucket]; i != INVALID_INDEX; i = mNextEntries[i]) { + if (Hash()(mEntries[i]) == hashCode && keyEqual(mEntries[i], value)) { return i; } } } - return -1; - } - - /// Return the prime number that is larger or equal to the number in parameter - /// for the size of the set - static int getPrimeSize(int number) { - - // Check if the next larger prime number is in the precomputed array of primes - for (int i = 0; i < NB_PRIMES; i++) { - if (PRIMES[i] >= number) return PRIMES[i]; - } - - // Manually compute the next larger prime number - for (int i = (number | 1); i < std::numeric_limits::max(); i+=2) { - - if (isPrimeNumber(i)) { - return i; - } - } - - return number; + return INVALID_INDEX; } public: @@ -250,36 +112,36 @@ class Set { private: - /// Array of entries - const Entry* mEntries; + /// Pointer to the set + const Set* mSet; - /// Capacity of the map - int mCapacity; - - /// Number of used entries in the map - int mNbUsedEntries; + /// Index of the current bucket + uint32 mCurrentBucketIndex; /// Index of the current entry - int mCurrentEntry; + uint32 mCurrentEntryIndex; /// Advance the iterator void advance() { - // If we are trying to move past the end - assert(mCurrentEntry < mNbUsedEntries); + assert(mCurrentBucketIndex < mSet->mHashSize); + assert(mCurrentEntryIndex < mSet->mNbAllocatedEntries); - for (mCurrentEntry += 1; mCurrentEntry < mNbUsedEntries; mCurrentEntry++) { - - // If the entry is not empty - if (mEntries[mCurrentEntry].value != nullptr) { - - // We have found the next non empty entry - return; - } + // Try the next entry + if (mSet->mNextEntries[mCurrentEntryIndex] != INVALID_INDEX) { + mCurrentEntryIndex = mSet->mNextEntries[mCurrentEntryIndex]; + return; } - // We have not find a non empty entry, we return an iterator to the end - mCurrentEntry = mCapacity; + // Try to move to the next bucket + mCurrentEntryIndex = 0; + mCurrentBucketIndex++; + while(mCurrentBucketIndex < mSet->mHashSize && mSet->mBuckets[mCurrentBucketIndex] == INVALID_INDEX) { + mCurrentBucketIndex++; + } + if (mCurrentBucketIndex < mSet->mHashSize) { + mCurrentEntryIndex = mSet->mBuckets[mCurrentBucketIndex]; + } } public: @@ -295,29 +157,29 @@ class Set { Iterator() = default; /// Constructor - Iterator(const Entry* entries, int capacity, int nbUsedEntries, int currentEntry) - :mEntries(entries), mCapacity(capacity), mNbUsedEntries(nbUsedEntries), mCurrentEntry(currentEntry) { + Iterator(const Set* set, uint32 bucketIndex, uint32 entryIndex) + :mSet(set), mCurrentBucketIndex(bucketIndex), mCurrentEntryIndex(entryIndex) { } /// Copy constructor Iterator(const Iterator& it) - :mEntries(it.mEntries), mCapacity(it.mCapacity), mNbUsedEntries(it.mNbUsedEntries), mCurrentEntry(it.mCurrentEntry) { + :mSet(it.mSet), mCurrentBucketIndex(it.mCurrentBucketIndex), mCurrentEntryIndex(it.mCurrentEntryIndex) { } /// Deferencable reference operator*() const { - assert(mCurrentEntry >= 0 && mCurrentEntry < mNbUsedEntries); - assert(mEntries[mCurrentEntry].value != nullptr); - return *(mEntries[mCurrentEntry].value); + assert(mCurrentEntryIndex < mSet->mNbAllocatedEntries); + assert(mCurrentEntryIndex != INVALID_INDEX); + return mSet->mEntries[mCurrentEntryIndex]; } /// Deferencable pointer operator->() const { - assert(mCurrentEntry >= 0 && mCurrentEntry < mNbUsedEntries); - assert(mEntries[mCurrentEntry].value != nullptr); - return mEntries[mCurrentEntry].value; + assert(mCurrentEntryIndex < mSet->mNbAllocatedEntries); + assert(mCurrentEntryIndex != INVALID_INDEX); + return &(mSet->mEntries[mCurrentEntryIndex]); } /// Post increment (it++) @@ -335,61 +197,71 @@ class Set { /// Equality operator (it == end()) bool operator==(const Iterator& iterator) const { - return mCurrentEntry == iterator.mCurrentEntry && mEntries == iterator.mEntries; + return mCurrentBucketIndex == iterator.mCurrentBucketIndex && mCurrentEntryIndex == iterator.mCurrentEntryIndex && mSet == iterator.mSet; } /// Inequality operator (it != end()) bool operator!=(const Iterator& iterator) const { return !(*this == iterator); } + + /// Overloaded assignment operator + Iterator& operator=(const Iterator& it) { + + // Check for self assignment + if (this != &it) { + + mSet = it.mSet; + mCurrentBucketIndex = it.mCurrentBucketIndex; + mCurrentEntryIndex = it.mCurrentEntryIndex; + } + + return *this; + } }; // -------------------- Methods -------------------- // /// Constructor - Set(MemoryAllocator& allocator, size_t capacity = 0) - : mNbUsedEntries(0), mNbFreeEntries(0), mCapacity(0), mBuckets(nullptr), - mEntries(nullptr), mAllocator(allocator), mFreeIndex(-1) { - - // If the largest prime has not been computed yet - if (LARGEST_PRIME == -1) { - - // Compute the largest prime number (largest map capacity) - LARGEST_PRIME = getPrimeSize(PRIMES[NB_PRIMES - 1] + 2); - } + Set(MemoryAllocator& allocator, uint32 capacity = 0) + : mNbAllocatedEntries(0), mNbEntries(0), mHashSize(0), mBuckets(nullptr), + mEntries(nullptr), mNextEntries(nullptr), mAllocator(allocator), mFreeIndex(INVALID_INDEX) { if (capacity > 0) { - initialize(capacity); + reserve(capacity); } } /// Copy constructor Set(const Set& set) - :mNbUsedEntries(set.mNbUsedEntries), mNbFreeEntries(set.mNbFreeEntries), mCapacity(set.mCapacity), - mBuckets(nullptr), mEntries(nullptr), mAllocator(set.mAllocator), mFreeIndex(set.mFreeIndex) { + :mNbAllocatedEntries(set.mNbAllocatedEntries), mNbEntries(set.mNbEntries), mHashSize(set.mHashSize), + mBuckets(nullptr), mEntries(nullptr), mNextEntries(nullptr), mAllocator(set.mAllocator), mFreeIndex(set.mFreeIndex) { - if (mCapacity > 0) { + // Allocate memory for the buckets + mBuckets = static_cast(mAllocator.allocate(mHashSize * sizeof(uint32))); - // Allocate memory for the buckets - mBuckets = static_cast(mAllocator.allocate(mCapacity * sizeof(int))); + // Allocate memory for the entries + mEntries = static_cast(mAllocator.allocate(mNbAllocatedEntries * sizeof(V))); + mNextEntries = static_cast(mAllocator.allocate(mNbAllocatedEntries * sizeof(uint32))); - // Allocate memory for the entries - mEntries = static_cast(mAllocator.allocate(mCapacity * sizeof(Entry))); + // Copy the buckets array + std::memcpy(mBuckets, set.mBuckets, mHashSize * sizeof(uint32)); - // Copy the buckets - std::uninitialized_copy(set.mBuckets, set.mBuckets + mCapacity, mBuckets); + // Copy the next entries indices + std::memcpy(mNextEntries, set.mNextEntries, mNbAllocatedEntries * sizeof(uint32)); - // Copy the entries - for (int i=0; i < mCapacity; i++) { + // Copy the entries + for (uint32 i=0; i(mAllocator.allocate(sizeof(V))); - new (mEntries[i].value) V(*(set.mEntries[i].value)); - } + // Copy the entry to the new location and destroy the previous one + new (mEntries + entryIndex) V(set.mEntries[entryIndex]); + + entryIndex = mNextEntries[entryIndex]; } } } @@ -401,82 +273,146 @@ class Set { } /// Allocate memory for a given number of elements - void reserve(int capacity) { + void reserve(uint32 capacity) { - if (capacity <= mCapacity) return; + if (capacity <= mHashSize) return; - if (capacity > LARGEST_PRIME && LARGEST_PRIME > mCapacity) { - capacity = LARGEST_PRIME; - } - else { - capacity = getPrimeSize(capacity); - } + if (capacity < 16) capacity = 16; - expand(capacity); + // Make sure we have a power of two size + if (!isPowerOfTwo(capacity)) { + capacity = nextPowerOfTwo32Bits(capacity); + } + + assert(capacity < INVALID_INDEX); + + assert(capacity > mHashSize); + + // Allocate memory for the buckets + uint32* newBuckets = static_cast(mAllocator.allocate(capacity * sizeof(uint32))); + + // Allocate memory for the entries + const uint32 nbAllocatedEntries = capacity * DEFAULT_LOAD_FACTOR; + assert(nbAllocatedEntries > 0); + V* newEntries = static_cast(mAllocator.allocate(nbAllocatedEntries * sizeof(V))); + uint32* newNextEntries = static_cast(mAllocator.allocate(nbAllocatedEntries * sizeof(uint32))); + + assert(newEntries != nullptr); + assert(newNextEntries != nullptr); + + // Initialize the new buckets + for (uint32 i=0; i 0) { + + assert(mNextEntries != nullptr); + + // Copy the free nodes indices in the nextEntries array + std::memcpy(newNextEntries, mNextEntries, mNbAllocatedEntries * sizeof(uint32)); + } + + // Recompute the buckets (hash) with the new hash size + for (uint32 i=0; i 0) { + + // Release previously allocated memory + mAllocator.release(mBuckets, mHashSize * sizeof(uint32)); + mAllocator.release(mEntries, mNbAllocatedEntries * sizeof(V)); + mAllocator.release(mNextEntries, mNbAllocatedEntries * sizeof(uint32)); + } + + // Add the new entries to the free list + for (uint32 i=mNbAllocatedEntries; i < nbAllocatedEntries-1; i++) { + newNextEntries[i] = i + 1; + } + newNextEntries[nbAllocatedEntries - 1] = mFreeIndex; + mFreeIndex = mNbAllocatedEntries; + + mHashSize = capacity; + mNbAllocatedEntries = nbAllocatedEntries; + mBuckets = newBuckets; + mEntries = newEntries; + mNextEntries = newNextEntries; + + assert(mFreeIndex != INVALID_INDEX); } /// Return true if the set contains a given value bool contains(const V& value) const { - return findEntry(value) != -1; + return findEntry(value) != INVALID_INDEX; } /// Add a value into the set. /// Returns true if the item has been inserted and false otherwise. bool add(const V& value) { - if (mCapacity == 0) { - initialize(0); - } + uint32 bucket; // Compute the hash code of the value - size_t hashCode = Hash()(value); + const size_t hashCode = Hash()(value); - // Compute the corresponding bucket index - int bucket = hashCode % mCapacity; + if (mHashSize > 0) { - auto keyEqual = KeyEqual(); + // Compute the corresponding bucket index + bucket = hashCode & (mHashSize - 1); - // Check if the item is already in the set - for (int i = mBuckets[bucket]; i >= 0; i = mEntries[i].next) { + auto keyEqual = KeyEqual(); - // If there is already an item with the same value in the set - if (mEntries[i].hashCode == hashCode && keyEqual(*mEntries[i].value, value)) { + // Check if the item is already in the set + for (uint32 i = mBuckets[bucket]; i != INVALID_INDEX; i = mNextEntries[i]) { - return false; + // If there is already an item with the same value in the set + if (Hash()(mEntries[i]) == hashCode && keyEqual(mEntries[i], value)) { + + return false; + } } } size_t entryIndex; - // If there are free entries to use - if (mNbFreeEntries > 0) { - assert(mFreeIndex >= 0); - entryIndex = mFreeIndex; - mFreeIndex = mEntries[entryIndex].next; - mNbFreeEntries--; - } - else { + // If there are no more free entries to use + if (mFreeIndex == INVALID_INDEX) { - // If we need to allocator more entries - if (mNbUsedEntries == mCapacity) { + // Allocate more memory + reserve(mHashSize == 0 ? 16 : mHashSize * 2); - // Allocate more memory - reserve(mCapacity * 2); - - // Recompute the bucket index - bucket = hashCode % mCapacity; - } - - entryIndex = mNbUsedEntries; - mNbUsedEntries++; + // Recompute the bucket index + bucket = hashCode & (mHashSize - 1); } - assert(mEntries[entryIndex].value == nullptr); - mEntries[entryIndex].hashCode = hashCode; - mEntries[entryIndex].next = mBuckets[bucket]; - mEntries[entryIndex].value = static_cast(mAllocator.allocate(sizeof(V))); - assert(mEntries[entryIndex].value != nullptr); - new (mEntries[entryIndex].value) V(value); + assert(mNbEntries < mNbAllocatedEntries); + assert(mFreeIndex != INVALID_INDEX); + + // Get the next free entry + entryIndex = mFreeIndex; + mFreeIndex = mNextEntries[entryIndex]; + + mNbEntries++; + + mNextEntries[entryIndex] = mBuckets[bucket]; + new (mEntries + entryIndex) V(value); mBuckets[bucket] = entryIndex; return true; @@ -495,46 +431,46 @@ class Set { /// element after the one that has been removed Iterator remove(const V& value) { - if (mCapacity > 0) { + if (mHashSize > 0) { - size_t hashcode = Hash()(value); - auto keyEqual = KeyEqual(); - int bucket = hashcode % mCapacity; - int last = -1; - for (int i = mBuckets[bucket]; i >= 0; last = i, i = mEntries[i].next) { + const size_t hashcode = Hash()(value); + auto keyEqual = KeyEqual(); + const uint32 bucket = hashcode & (mHashSize - 1); + uint32 last = INVALID_INDEX; + for (uint32 i = mBuckets[bucket]; i != INVALID_INDEX; last = i, i = mNextEntries[i]) { - if (mEntries[i].hashCode == hashcode && keyEqual(*mEntries[i].value, value)) { + // If we have found the item + if (Hash()(mEntries[i]) == hashcode && keyEqual(mEntries[i], value)) { - if (last < 0 ) { - mBuckets[bucket] = mEntries[i].next; + if (last == INVALID_INDEX) { + mBuckets[bucket] = mNextEntries[i]; } else { - mEntries[last].next = mEntries[i].next; + mNextEntries[last] = mNextEntries[i]; } - // Release memory for the value if any - if (mEntries[i].value != nullptr) { - mEntries[i].value->~V(); - mAllocator.release(mEntries[i].value, sizeof(V)); - mEntries[i].value = nullptr; - } - assert(mEntries[i].value == nullptr); - mEntries[i].next = mFreeIndex; + uint32 nextEntryIndex = mNextEntries[i]; + uint32 nextBucketIndex = bucket; + + mEntries[i].~V(); + mNextEntries[i] = mFreeIndex; mFreeIndex = i; - mNbFreeEntries++; + mNbEntries--; - // Find the next valid entry to return an iterator - for (i += 1; i < mNbUsedEntries; i++) { - - // If the entry is not empty - if (mEntries[i].value != nullptr) { - - // We have found the next non empty entry - return Iterator(mEntries, mCapacity, mNbUsedEntries, i); + // Find the next entry to return an iterator + if (nextEntryIndex == INVALID_INDEX) { + nextEntryIndex = 0; + nextBucketIndex++; + while(nextBucketIndex < mHashSize && mBuckets[nextBucketIndex] == INVALID_INDEX) { + nextBucketIndex++; + } + if (nextBucketIndex < mHashSize) { + nextEntryIndex = mBuckets[nextBucketIndex]; } } - return end(); + // We have found the next non empty entry + return Iterator(this, nextBucketIndex, nextEntryIndex); } } } @@ -542,67 +478,67 @@ class Set { return end(); } - /// Return a list with all the values of the set - List toList(MemoryAllocator& listAllocator) const { + /// Return an array with all the values of the set + Array toArray(MemoryAllocator& arrayAllocator) const { - List list(listAllocator); + Array array(arrayAllocator); - for (int i=0; i < mCapacity; i++) { - if (mEntries[i].value != nullptr) { - list.add(*(mEntries[i].value)); - } + for (auto it = begin(); it != end(); ++it) { + array.add(*it); } - return list; + return array; } /// Clear the set void clear(bool releaseMemory = false) { - if (mNbUsedEntries > 0) { + for (uint32 i=0; i~V(); - mAllocator.release(mEntries[i].value, sizeof(V)); - mEntries[i].value = nullptr; - } + uint32 entryIndex = mBuckets[i]; + while(entryIndex != INVALID_INDEX) { + + // Destroy the entry + mEntries[entryIndex].~V(); + + uint32 nextEntryIndex = mNextEntries[entryIndex]; + + // Add entry to the free list + mNextEntries[entryIndex] = mFreeIndex; + mFreeIndex = entryIndex; + + entryIndex = nextEntryIndex; } - mFreeIndex = -1; - mNbUsedEntries = 0; - mNbFreeEntries = 0; + mBuckets[i] = INVALID_INDEX; } - // If elements have been allocated - if (releaseMemory && mCapacity > 0) { + if (releaseMemory && mNbAllocatedEntries > 0) { - // Destroy the entries - for (int i=0; i < mCapacity; i++) { - mEntries[i].~Entry(); - } + // Release previously allocated memory + mAllocator.release(mBuckets, mHashSize * sizeof(uint32)); + mAllocator.release(mEntries, mNbAllocatedEntries * sizeof(V)); + mAllocator.release(mNextEntries, mNbAllocatedEntries * sizeof(uint32)); - mAllocator.release(mBuckets, mCapacity * sizeof(int)); - mAllocator.release(mEntries, mCapacity * sizeof(Entry)); - - mCapacity = 0; mBuckets = nullptr; mEntries = nullptr; + mNextEntries = nullptr; + + mNbAllocatedEntries = 0; + mHashSize = 0; } - assert(size() == 0); + mNbEntries = 0; } /// Return the number of elements in the set - int size() const { - return mNbUsedEntries - mNbFreeEntries; + uint32 size() const { + return mNbEntries; } /// Return the capacity of the set - int capacity() const { - return mCapacity; + uint32 capacity() const { + return mHashSize; } /// Try to find an item of the set given a key. @@ -610,30 +546,28 @@ class Set { /// an iterator pointing to the end if not found Iterator find(const V& value) const { - int bucket; - int entry = -1; + uint32 bucket; + uint32 entry = INVALID_INDEX; - if (mCapacity > 0) { + if (mHashSize > 0) { - size_t hashCode = Hash()(value); - bucket = hashCode % mCapacity; - auto keyEqual = KeyEqual(); + const size_t hashCode = Hash()(value); + bucket = hashCode & (mHashSize - 1); + auto keyEqual = KeyEqual(); - for (int i = mBuckets[bucket]; i >= 0; i = mEntries[i].next) { - if (mEntries[i].hashCode == hashCode && keyEqual(*(mEntries[i].value), value)) { + for (uint32 i = mBuckets[bucket]; i != INVALID_INDEX; i = mNextEntries[i]) { + if (Hash()(mEntries[i]) == hashCode && keyEqual(mEntries[i], value)) { entry = i; break; } } } - if (entry == -1) { + if (entry == INVALID_INDEX) { return end(); } - assert(mEntries[entry].value != nullptr); - - return Iterator(mEntries, mCapacity, mNbUsedEntries, entry); + return Iterator(this, bucket, entry); } /// Overloaded equality operator @@ -665,34 +599,35 @@ class Set { // Clear the set clear(true); - if (set.mCapacity > 0) { + mNbAllocatedEntries = set.mNbAllocatedEntries; + mNbEntries = set.mNbEntries; + mHashSize = set.mHashSize; + mFreeIndex = set.mFreeIndex; - // Compute the next larger prime size - mCapacity = getPrimeSize(set.mCapacity); + // Allocate memory for the buckets + mBuckets = static_cast(mAllocator.allocate(mHashSize * sizeof(uint32))); - // Allocate memory for the buckets - mBuckets = static_cast(mAllocator.allocate(mCapacity * sizeof(int))); + // Allocate memory for the entries + mEntries = static_cast(mAllocator.allocate(mNbAllocatedEntries * sizeof(V))); + mNextEntries = static_cast(mAllocator.allocate(mNbAllocatedEntries * sizeof(uint32))); - // Allocate memory for the entries - mEntries = static_cast(mAllocator.allocate(mCapacity * sizeof(Entry))); + // Copy the buckets array + std::memcpy(mBuckets, set.mBuckets, mHashSize * sizeof(uint32)); - // Copy the buckets - std::uninitialized_copy(set.mBuckets, set.mBuckets + mCapacity, mBuckets); + // Copy the next entries indices + std::memcpy(mNextEntries, set.mNextEntries, mNbAllocatedEntries * sizeof(uint32)); - // Copy the entries - for (int i=0; i < mCapacity; i++) { + // Copy the entries + for (uint32 i=0; i(mAllocator.allocate(sizeof(V))); - new (mEntries[i].value) V(*(set.mEntries[i].value)); - } + // Copy the entry to the new location and destroy the previous one + new (mEntries + entryIndex) V(set.mEntries[entryIndex]); + + entryIndex = mNextEntries[entryIndex]; } - - mNbUsedEntries = set.mNbUsedEntries; - mNbFreeEntries = set.mNbFreeEntries; - mFreeIndex = set.mFreeIndex; } } @@ -702,7 +637,7 @@ class Set { /// Return a begin iterator Iterator begin() const { - // If the map is empty + // If the set is empty if (size() == 0) { // Return an iterator to the end @@ -710,33 +645,29 @@ class Set { } // Find the first used entry - int entry; - for (entry=0; entry < mNbUsedEntries; entry++) { - if (mEntries[entry].value != nullptr) { - return Iterator(mEntries, mCapacity, mNbUsedEntries, entry); - } + uint32 bucketIndex = 0; + while (mBuckets[bucketIndex] == INVALID_INDEX) { + + bucketIndex++; } - assert(false); - return end(); + assert(bucketIndex < mHashSize); + assert(mBuckets[bucketIndex] != INVALID_INDEX); + + return Iterator(this, bucketIndex, mBuckets[bucketIndex]); } /// Return a end iterator Iterator end() const { - return Iterator(mEntries, mCapacity, mNbUsedEntries, mCapacity); + + return Iterator(this, mHashSize, 0); } + + // ---------- Friendship ---------- // + + friend class Iterator; }; -template -const int Set::PRIMES[NB_PRIMES] = {3, 7, 11, 17, 23, 29, 37, 47, 59, 71, 89, 107, 131, 163, 197, 239, 293, 353, 431, 521, 631, 761, 919, - 1103, 1327, 1597, 1931, 2333, 2801, 3371, 4049, 4861, 5839, 7013, 8419, 10103, 12143, 14591, - 17519, 21023, 25229, 30293, 36353, 43627, 52361, 62851, 75431, 90523, 108631, 130363, 156437, - 187751, 225307, 270371, 324449, 389357, 467237, 560689, 672827, 807403, 968897, 1162687, 1395263, - 1674319, 2009191, 2411033, 2893249, 3471899, 4166287, 4999559}; - -template -int Set::LARGEST_PRIME = -1; - } #endif diff --git a/include/reactphysics3d/containers/containers_common.h b/include/reactphysics3d/containers/containers_common.h index 84854f90..66b93aac 100644 --- a/include/reactphysics3d/containers/containers_common.h +++ b/include/reactphysics3d/containers/containers_common.h @@ -29,6 +29,7 @@ // Libraries #include #include +#include namespace reactphysics3d { diff --git a/include/reactphysics3d/engine/Entity.h b/include/reactphysics3d/engine/Entity.h index 0b671349..c8eb20ee 100644 --- a/include/reactphysics3d/engine/Entity.h +++ b/include/reactphysics3d/engine/Entity.h @@ -107,23 +107,23 @@ struct Entity { }; // Return the lookup index of the entity in a array -inline uint32 Entity::getIndex() const { +RP3D_FORCE_INLINE uint32 Entity::getIndex() const { return id & ENTITY_INDEX_MASK; } // Return the generation number of the entity -inline uint32 Entity::getGeneration() const { +RP3D_FORCE_INLINE uint32 Entity::getGeneration() const { return (id >> ENTITY_INDEX_BITS) & ENTITY_GENERATION_MASK; } // Equality operator -inline bool Entity::operator==(const Entity& entity) const { +RP3D_FORCE_INLINE bool Entity::operator==(const Entity& entity) const { return entity.id == id; } // Inequality operator -inline bool Entity::operator!=(const Entity& entity) const { +RP3D_FORCE_INLINE bool Entity::operator!=(const Entity& entity) const { return entity.id != id; } diff --git a/include/reactphysics3d/engine/EntityManager.h b/include/reactphysics3d/engine/EntityManager.h index f29ca09d..f2a92bb1 100644 --- a/include/reactphysics3d/engine/EntityManager.h +++ b/include/reactphysics3d/engine/EntityManager.h @@ -28,7 +28,7 @@ // Libraries #include -#include +#include #include #include @@ -45,8 +45,8 @@ class EntityManager { // -------------------- Attributes -------------------- // - /// List storing the generations of the created entities - List mGenerations; + /// Array storing the generations of the created entities + Array mGenerations; /// Deque with the indices of destroyed entities that can be reused Deque mFreeIndices; @@ -71,7 +71,7 @@ class EntityManager { }; // Return true if the entity is still valid (not destroyed) -inline bool EntityManager::isValid(Entity entity) const { +RP3D_FORCE_INLINE bool EntityManager::isValid(Entity entity) const { return mGenerations[entity.getIndex()] == entity.getGeneration(); } diff --git a/include/reactphysics3d/engine/Island.h b/include/reactphysics3d/engine/Island.h index f1b24575..dae4ae51 100644 --- a/include/reactphysics3d/engine/Island.h +++ b/include/reactphysics3d/engine/Island.h @@ -105,35 +105,35 @@ class Island { }; // Add a body into the island -inline void Island::addBody(RigidBody* body) { +RP3D_FORCE_INLINE void Island::addBody(RigidBody* body) { assert(!body->isSleeping()); mBodies[mNbBodies] = body; mNbBodies++; } // Add a contact manifold into the island -inline void Island::addContactManifold(ContactManifold* contactManifold) { +RP3D_FORCE_INLINE void Island::addContactManifold(ContactManifold* contactManifold) { mContactManifolds[mNbContactManifolds] = contactManifold; mNbContactManifolds++; } // Return the number of bodies in the island -inline uint Island::getNbBodies() const { +RP3D_FORCE_INLINE uint Island::getNbBodies() const { return mNbBodies; } // Return the number of contact manifolds in the island -inline uint Island::getNbContactManifolds() const { +RP3D_FORCE_INLINE uint Island::getNbContactManifolds() const { return mNbContactManifolds; } // Return a pointer to the array of bodies -inline RigidBody** Island::getBodies() { +RP3D_FORCE_INLINE RigidBody** Island::getBodies() { return mBodies; } // Return a pointer to the array of contact manifolds -inline ContactManifold** Island::getContactManifolds() { +RP3D_FORCE_INLINE ContactManifold** Island::getContactManifolds() { return mContactManifolds; } diff --git a/include/reactphysics3d/engine/Islands.h b/include/reactphysics3d/engine/Islands.h index 2ffc26a5..b2931222 100644 --- a/include/reactphysics3d/engine/Islands.h +++ b/include/reactphysics3d/engine/Islands.h @@ -28,7 +28,7 @@ // Libraries #include -#include +#include #include #include @@ -46,10 +46,17 @@ struct Islands { private: - // -------------------- Attributes -------------------- // + /// Number of islands in the previous frame + uint32 mNbIslandsPreviousFrame; - /// Reference to the memory allocator - MemoryAllocator& memoryAllocator; + /// Number of items in the bodyEntities array in the previous frame + uint32 mNbBodyEntitiesPreviousFrame; + + /// Maximum number of bodies in a single island in the previous frame + uint32 mNbMaxBodiesInIslandPreviousFrame; + + /// Maximum number of bodies in a single island in the current frame + uint32 mNbMaxBodiesInIslandCurrentFrame; public: @@ -57,20 +64,27 @@ struct Islands { /// For each island, index of the first contact manifold of the island in the array of contact manifolds - List contactManifoldsIndices; + Array contactManifoldsIndices; /// For each island, number of contact manifolds in the island - List nbContactManifolds; + Array nbContactManifolds; - /// For each island, list of all the entities of the bodies in the island - List> bodyEntities; + /// Array of all the entities of the bodies in the islands (stored sequentially) + Array bodyEntities; + + /// For each island we store the starting index of the bodies of that island in the "bodyEntities" array + Array startBodyEntitiesIndex; + + /// For each island, total number of bodies in the island + Array nbBodiesInIsland; // -------------------- Methods -------------------- // /// Constructor Islands(MemoryAllocator& allocator) - :memoryAllocator(allocator), contactManifoldsIndices(allocator), nbContactManifolds(allocator), - bodyEntities(allocator) { + :mNbIslandsPreviousFrame(16), mNbBodyEntitiesPreviousFrame(32), mNbMaxBodiesInIslandPreviousFrame(0), mNbMaxBodiesInIslandCurrentFrame(0), + contactManifoldsIndices(allocator), nbContactManifolds(allocator), + bodyEntities(allocator), startBodyEntitiesIndex(allocator), nbBodiesInIsland(allocator) { } @@ -78,34 +92,76 @@ struct Islands { ~Islands() = default; /// Assignment operator - Islands& operator=(const Islands& island) = default; + Islands& operator=(const Islands& island) = delete; /// Copy-constructor Islands(const Islands& island) = default; /// Return the number of islands uint32 getNbIslands() const { - return static_cast(contactManifoldsIndices.size()); + return contactManifoldsIndices.size(); } /// Add an island and return its index uint32 addIsland(uint32 contactManifoldStartIndex) { - uint32 islandIndex = contactManifoldsIndices.size(); + const uint32 islandIndex = contactManifoldsIndices.size(); contactManifoldsIndices.add(contactManifoldStartIndex); nbContactManifolds.add(0); - bodyEntities.add(List(memoryAllocator)); + startBodyEntitiesIndex.add(bodyEntities.size()); + nbBodiesInIsland.add(0); + + if (islandIndex > 0 && nbBodiesInIsland[islandIndex-1] > mNbMaxBodiesInIslandCurrentFrame) { + mNbMaxBodiesInIslandCurrentFrame = nbBodiesInIsland[islandIndex-1]; + } return islandIndex; } + void addBodyToIsland(Entity bodyEntity) { + + const uint32 islandIndex = contactManifoldsIndices.size(); + assert(islandIndex > 0); + + bodyEntities.add(bodyEntity); + nbBodiesInIsland[islandIndex - 1]++; + } + + /// Reserve memory for the current frame + void reserveMemory() { + + contactManifoldsIndices.reserve(mNbIslandsPreviousFrame); + nbContactManifolds.reserve(mNbIslandsPreviousFrame); + startBodyEntitiesIndex.reserve(mNbIslandsPreviousFrame); + nbBodiesInIsland.reserve(mNbIslandsPreviousFrame); + + bodyEntities.reserve(mNbBodyEntitiesPreviousFrame); + } + /// Clear all the islands void clear() { + const uint32 nbIslands = nbContactManifolds.size(); + + if (nbIslands > 0 && nbBodiesInIsland[nbIslands-1] > mNbMaxBodiesInIslandCurrentFrame) { + mNbMaxBodiesInIslandCurrentFrame = nbBodiesInIsland[nbIslands-1]; + } + + mNbMaxBodiesInIslandPreviousFrame = mNbMaxBodiesInIslandCurrentFrame; + mNbIslandsPreviousFrame = nbIslands; + mNbMaxBodiesInIslandCurrentFrame = 0; + mNbBodyEntitiesPreviousFrame = bodyEntities.size(); + contactManifoldsIndices.clear(true); nbContactManifolds.clear(true); bodyEntities.clear(true); + startBodyEntitiesIndex.clear(true); + nbBodiesInIsland.clear(true); + } + + uint32 getNbMaxBodiesInIslandPreviousFrame() const { + return mNbMaxBodiesInIslandPreviousFrame; } }; diff --git a/include/reactphysics3d/engine/Material.h b/include/reactphysics3d/engine/Material.h index 0882e5b2..e15d2532 100644 --- a/include/reactphysics3d/engine/Material.h +++ b/include/reactphysics3d/engine/Material.h @@ -28,6 +28,7 @@ // Libraries #include +#include #include namespace reactphysics3d { @@ -44,11 +45,8 @@ class Material { // -------------------- Attributes -------------------- // - /// Friction coefficient (positive value) - decimal mFrictionCoefficient; - - /// Rolling resistance factor (positive value) - decimal mRollingResistance; + /// Square root of the friction coefficient + decimal mFrictionCoefficientSqrt; /// Bounciness during collisions (between 0 and 1) where 1 is for a very bouncy body decimal mBounciness; @@ -59,14 +57,7 @@ class Material { // -------------------- Methods -------------------- // /// Constructor - Material(decimal frictionCoefficient, decimal rollingResistance, decimal bounciness, - decimal massDensity = decimal(1.0)); - - /// Copy-constructor - Material(const Material& material); - - /// Destructor - ~Material() = default; + Material(decimal frictionCoefficient, decimal bounciness, decimal massDensity = decimal(1.0)); public : @@ -84,11 +75,8 @@ class Material { /// Set the friction coefficient. void setFrictionCoefficient(decimal frictionCoefficient); - /// Return the rolling resistance factor - decimal getRollingResistance() const; - - /// Set the rolling resistance factor - void setRollingResistance(decimal rollingResistance); + /// Return the square root friction coefficient + decimal getFrictionCoefficientSqrt() const; /// Return the mass density of the collider decimal getMassDensity() const; @@ -99,19 +87,18 @@ class Material { /// Return a string representation for the material std::string to_string() const; - /// Overloaded assignment operator - Material& operator=(const Material& material); - // ---------- Friendship ---------- // friend class Collider; + friend class CollisionBody; + friend class RigidBody; }; // Return the bounciness /** * @return Bounciness factor (between 0 and 1) where 1 is very bouncy */ -inline decimal Material::getBounciness() const { +RP3D_FORCE_INLINE decimal Material::getBounciness() const { return mBounciness; } @@ -121,7 +108,7 @@ inline decimal Material::getBounciness() const { /** * @param bounciness Bounciness factor (between 0 and 1) where 1 is very bouncy */ -inline void Material::setBounciness(decimal bounciness) { +RP3D_FORCE_INLINE void Material::setBounciness(decimal bounciness) { assert(bounciness >= decimal(0.0) && bounciness <= decimal(1.0)); mBounciness = bounciness; } @@ -130,8 +117,8 @@ inline void Material::setBounciness(decimal bounciness) { /** * @return Friction coefficient (positive value) */ -inline decimal Material::getFrictionCoefficient() const { - return mFrictionCoefficient; +RP3D_FORCE_INLINE decimal Material::getFrictionCoefficient() const { + return mFrictionCoefficientSqrt * mFrictionCoefficientSqrt; } // Set the friction coefficient. @@ -140,34 +127,18 @@ inline decimal Material::getFrictionCoefficient() const { /** * @param frictionCoefficient Friction coefficient (positive value) */ -inline void Material::setFrictionCoefficient(decimal frictionCoefficient) { +RP3D_FORCE_INLINE void Material::setFrictionCoefficient(decimal frictionCoefficient) { assert(frictionCoefficient >= decimal(0.0)); - mFrictionCoefficient = frictionCoefficient; + mFrictionCoefficientSqrt = std::sqrt(frictionCoefficient); } -// Return the rolling resistance factor. If this value is larger than zero, -// it will be used to slow down the body when it is rolling -// against another body. -/** - * @return The rolling resistance factor (positive value) - */ -inline decimal Material::getRollingResistance() const { - return mRollingResistance; -} - -// Set the rolling resistance factor. If this value is larger than zero, -// it will be used to slow down the body when it is rolling -// against another body. -/** - * @param rollingResistance The rolling resistance factor - */ -inline void Material::setRollingResistance(decimal rollingResistance) { - assert(rollingResistance >= 0); - mRollingResistance = rollingResistance; +// Return the square root friction coefficient +RP3D_FORCE_INLINE decimal Material::getFrictionCoefficientSqrt() const { + return mFrictionCoefficientSqrt; } // Return the mass density of the collider -inline decimal Material::getMassDensity() const { +RP3D_FORCE_INLINE decimal Material::getMassDensity() const { return mMassDensity; } @@ -175,36 +146,21 @@ inline decimal Material::getMassDensity() const { /** * @param massDensity The mass density of the collider */ -inline void Material::setMassDensity(decimal massDensity) { +RP3D_FORCE_INLINE void Material::setMassDensity(decimal massDensity) { mMassDensity = massDensity; } // Return a string representation for the material -inline std::string Material::to_string() const { +RP3D_FORCE_INLINE std::string Material::to_string() const { std::stringstream ss; - ss << "frictionCoefficient=" << mFrictionCoefficient << std::endl; - ss << "rollingResistance=" << mRollingResistance << std::endl; + ss << "frictionCoefficient=" << (mFrictionCoefficientSqrt * mFrictionCoefficientSqrt) << std::endl; ss << "bounciness=" << mBounciness << std::endl; return ss.str(); } -// Overloaded assignment operator -inline Material& Material::operator=(const Material& material) { - - // Check for self-assignment - if (this != &material) { - mFrictionCoefficient = material.mFrictionCoefficient; - mBounciness = material.mBounciness; - mRollingResistance = material.mRollingResistance; - } - - // Return this material - return *this; -} - } #endif diff --git a/include/reactphysics3d/engine/OverlappingPairs.h b/include/reactphysics3d/engine/OverlappingPairs.h index 13dfad0c..21583ef0 100644 --- a/include/reactphysics3d/engine/OverlappingPairs.h +++ b/include/reactphysics3d/engine/OverlappingPairs.h @@ -54,6 +54,8 @@ class CollisionDispatch; */ struct LastFrameCollisionInfo { + // TODO OPTI : Use bit flags instead of bools here + /// True if we have information about the previous frame bool isValid; @@ -77,9 +79,9 @@ struct LastFrameCollisionInfo { // SAT Algorithm bool satIsAxisFacePolyhedron1; bool satIsAxisFacePolyhedron2; - uint satMinAxisFaceIndex; - uint satMinEdge1Index; - uint satMinEdge2Index; + uint8 satMinAxisFaceIndex; + uint8 satMinEdge1Index; + uint8 satMinEdge2Index; /// Constructor LastFrameCollisionInfo() { @@ -104,80 +106,194 @@ struct LastFrameCollisionInfo { */ class OverlappingPairs { + public: + + struct OverlappingPair { + + /// Ids of the convex vs convex pairs + uint64 pairID; + + /// Broad-phase Id of the first shape + // TODO OPTI : Is this used ? + int32 broadPhaseId1; + + /// Broad-phase Id of the second shape + // TODO OPTI : Is this used ? + int32 broadPhaseId2; + + /// Entity of the first collider of the convex vs convex pairs + Entity collider1; + + /// Entity of the second collider of the convex vs convex pairs + Entity collider2; + + /// True if we need to test if the convex vs convex overlapping pairs of shapes still overlap + bool needToTestOverlap; + + /// Pointer to the narrow-phase algorithm + NarrowPhaseAlgorithmType narrowPhaseAlgorithmType; + + /// True if the colliders of the overlapping pair were colliding in the previous frame + bool collidingInPreviousFrame; + + /// True if the colliders of the overlapping pair are colliding in the current frame + bool collidingInCurrentFrame; + + /// Constructor + OverlappingPair(uint64 pairId, int32 broadPhaseId1, int32 broadPhaseId2, Entity collider1, Entity collider2, + NarrowPhaseAlgorithmType narrowPhaseAlgorithmType) + : pairID(pairId), broadPhaseId1(broadPhaseId1), broadPhaseId2(broadPhaseId2), collider1(collider1) , collider2(collider2), + needToTestOverlap(false), narrowPhaseAlgorithmType(narrowPhaseAlgorithmType), collidingInPreviousFrame(false), + collidingInCurrentFrame(false) { + + } + + /// Destructor + virtual ~OverlappingPair() = default; + }; + + // Overlapping pair between two convex colliders + struct ConvexOverlappingPair : public OverlappingPair { + + /// Temporal coherence collision data for each overlapping collision shapes of this pair. + /// Temporal coherence data store collision information about the last frame. + /// If two convex shapes overlap, we have a single collision data but if one shape is concave, + /// we might have collision data for several overlapping triangles. + LastFrameCollisionInfo lastFrameCollisionInfo; + + /// Constructor + ConvexOverlappingPair(uint64 pairId, int32 broadPhaseId1, int32 broadPhaseId2, Entity collider1, Entity collider2, + NarrowPhaseAlgorithmType narrowPhaseAlgorithmType) + : OverlappingPair(pairId, broadPhaseId1, broadPhaseId2, collider1, collider2, narrowPhaseAlgorithmType) { + + } + }; + + // Overlapping pair between two a convex collider and a concave collider + struct ConcaveOverlappingPair : public OverlappingPair { + + private: + + MemoryAllocator* mPoolAllocator; + + public: + + /// True if the first shape of the pair is convex + bool isShape1Convex; + + /// Temporal coherence collision data for each overlapping collision shapes of this pair. + /// Temporal coherence data store collision information about the last frame. + /// If two convex shapes overlap, we have a single collision data but if one shape is concave, + /// we might have collision data for several overlapping triangles. The key in the map is the + /// shape Ids of the two collision shapes. + Map lastFrameCollisionInfos; + + /// Constructor + ConcaveOverlappingPair(uint64 pairId, int32 broadPhaseId1, int32 broadPhaseId2, Entity collider1, Entity collider2, + NarrowPhaseAlgorithmType narrowPhaseAlgorithmType, + bool isShape1Convex, MemoryAllocator& poolAllocator, MemoryAllocator& heapAllocator) + : OverlappingPair(pairId, broadPhaseId1, broadPhaseId2, collider1, collider2, narrowPhaseAlgorithmType), mPoolAllocator(&poolAllocator), + isShape1Convex(isShape1Convex), lastFrameCollisionInfos(heapAllocator, 16) { + + } + + // Destroy all the LastFrameCollisionInfo objects + void destroyLastFrameCollisionInfos() { + + for (auto it = lastFrameCollisionInfos.begin(); it != lastFrameCollisionInfos.end(); ++it) { + + // Call the destructor + it->second->LastFrameCollisionInfo::~LastFrameCollisionInfo(); + + // Release memory + mPoolAllocator->release(it->second, sizeof(LastFrameCollisionInfo)); + } + + lastFrameCollisionInfos.clear(); + } + + // Add a new last frame collision info if it does not exist for the given shapes already + LastFrameCollisionInfo* addLastFrameInfoIfNecessary(uint32 shapeId1, uint32 shapeId2) { + + uint32 maxShapeId = shapeId1; + uint32 minShapeId = shapeId2; + if (shapeId1 < shapeId2) { + maxShapeId = shapeId2; + minShapeId = shapeId1; + } + + // Try to get the corresponding last frame collision info + const uint64 shapesId = pairNumbers(maxShapeId, minShapeId); + + // If there is no collision info for those two shapes already + auto it = lastFrameCollisionInfos.find(shapesId); + if (it == lastFrameCollisionInfos.end()) { + + LastFrameCollisionInfo* lastFrameInfo = new (mPoolAllocator->allocate(sizeof(LastFrameCollisionInfo))) LastFrameCollisionInfo(); + + // Add it into the map of collision infos + lastFrameCollisionInfos.add(Pair(shapesId, lastFrameInfo)); + + return lastFrameInfo; + } + else { + + // The existing collision info is not obsolete + it->second->isObsolete = false; + + return it->second; + } + } + + /// Clear the obsolete LastFrameCollisionInfo objects + void clearObsoleteLastFrameInfos() { + + // For each last frame collision info + for (auto it = lastFrameCollisionInfos.begin(); it != lastFrameCollisionInfos.end(); ) { + + // If the collision info is obsolete + if (it->second->isObsolete) { + + // Call the destructor + it->second->LastFrameCollisionInfo::~LastFrameCollisionInfo(); + + // Release memory + mPoolAllocator->release(it->second, sizeof(LastFrameCollisionInfo)); + + it = lastFrameCollisionInfos.remove(it); + } + else { // If the collision info is not obsolete + + // Do not delete it but mark it as obsolete + it->second->isObsolete = true; + + ++it; + } + } + } + }; + private: - // -------------------- Constants -------------------- // - - - /// Number of pairs to allocated at the beginning - const uint32 INIT_NB_ALLOCATED_PAIRS = 10; - // -------------------- Attributes -------------------- // - /// Persistent memory allocator - MemoryAllocator& mPersistentAllocator; + /// Pool memory allocator + MemoryAllocator& mPoolAllocator; - /// Memory allocator used to allocated memory for the ContactManifoldInfo and ContactPointInfo - // TODO : Do we need to keep this ? - MemoryAllocator& mTempMemoryAllocator; + /// Heap memory allocator + MemoryAllocator& mHeapAllocator; - /// Current number of components - uint64 mNbPairs; + /// Array of convex vs convex overlapping pairs + Array mConvexPairs; - /// Index in the array of the first convex vs concave pair - uint64 mConcavePairsStartIndex; - - /// Size (in bytes) of a single pair - size_t mPairDataSize; - - /// Number of allocated pairs - uint64 mNbAllocatedPairs; - - /// Allocated memory for all the data of the pairs - void* mBuffer; + /// Array of convex vs concave overlapping pairs + Array mConcavePairs; /// Map a pair id to the internal array index - Map mMapPairIdToPairIndex; + Map mMapConvexPairIdToPairIndex; - /// Ids of the convex vs convex pairs - uint64* mPairIds; - - /// Array with the broad-phase Ids of the first shape - int32* mPairBroadPhaseId1; - - /// Array with the broad-phase Ids of the second shape - int32* mPairBroadPhaseId2; - - /// Array of Entity of the first collider of the convex vs convex pairs - Entity* mColliders1; - - /// Array of Entity of the second collider of the convex vs convex pairs - Entity* mColliders2; - - /// Temporal coherence collision data for each overlapping collision shapes of this pair. - /// Temporal coherence data store collision information about the last frame. - /// If two convex shapes overlap, we have a single collision data but if one shape is concave, - /// we might have collision data for several overlapping triangles. The key in the map is the - /// shape Ids of the two collision shapes. - Map* mLastFrameCollisionInfos; - - /// True if we need to test if the convex vs convex overlapping pairs of shapes still overlap - bool* mNeedToTestOverlap; - - /// True if the overlapping pair is active (at least one body of the pair is active and not static) - bool* mIsActive; - - /// Array with the pointer to the narrow-phase algorithm for each overlapping pair - NarrowPhaseAlgorithmType* mNarrowPhaseAlgorithmType; - - /// True if the first shape of the pair is convex - bool* mIsShape1Convex; - - /// True if the colliders of the overlapping pair were colliding in the previous frame - bool* mCollidingInPreviousFrame; - - /// True if the colliders of the overlapping pair are colliding in the current frame - bool* mCollidingInCurrentFrame; + /// Map a pair id to the internal array index + Map mMapConcavePairIdToPairIndex; /// Reference to the colliders components ColliderComponents& mColliderComponents; @@ -203,15 +319,6 @@ class OverlappingPairs { // -------------------- Methods -------------------- // - /// Allocate memory for a given number of pairs - void allocate(uint64 nbPairsToAllocate); - - /// Compute the index where we need to insert the new pair - uint64 prepareAddPair(bool isConvexVsConvex); - - /// Destroy a pair at a given index - void destroyPair(uint64 index); - // Move a pair from a source to a destination index in the pairs array void movePairToIndex(uint64 srcIndex, uint64 destIndex); @@ -223,8 +330,8 @@ class OverlappingPairs { // -------------------- Methods -------------------- // /// Constructor - OverlappingPairs(MemoryAllocator& persistentMemoryAllocator, MemoryAllocator& temporaryMemoryAllocator, - ColliderComponents& colliderComponents, CollisionBodyComponents& collisionBodyComponents, + OverlappingPairs(MemoryManager& memoryManager, ColliderComponents& colliderComponents, + CollisionBodyComponents& collisionBodyComponents, RigidBodyComponents& rigidBodyComponents, Set& noCollisionPairs, CollisionDispatch& collisionDispatch); @@ -238,46 +345,13 @@ class OverlappingPairs { OverlappingPairs& operator=(const OverlappingPairs& pair) = delete; /// Add an overlapping pair - uint64 addPair(Collider* shape1, Collider* shape2); + uint64 addPair(uint32 collider1Index, uint32 collider2Index, bool isConvexVsConvex); /// Remove a component at a given index void removePair(uint64 pairId); - /// Return the number of pairs - uint64 getNbPairs() const; - - /// Return the number of convex vs convex pairs - uint64 getNbConvexVsConvexPairs() const; - - /// Return the number of convex vs concave pairs - uint64 getNbConvexVsConcavePairs() const; - - /// Return the starting index of the convex vs concave pairs - uint64 getConvexVsConcavePairsStartIndex() const; - - /// Return the entity of the first collider - Entity getCollider1(uint64 pairId) const; - - /// Return the entity of the second collider - Entity getCollider2(uint64 pairId) const; - - /// Notify if a given pair is active or not - void setIsPairActive(uint64 pairId, bool isActive); - - /// Return the index of a given overlapping pair in the internal array - uint64 getPairIndex(uint64 pairId) const; - - /// Return the last frame collision info - LastFrameCollisionInfo* getLastFrameCollisionInfo(uint64, uint64 shapesId); - - /// Return a reference to the temporary memory allocator - MemoryAllocator& getTemporaryAllocator(); - - /// Add a new last frame collision info if it does not exist for the given shapes already - LastFrameCollisionInfo* addLastFrameInfoIfNecessary(uint64 pairIndex, uint32 shapeId1, uint32 shapeId2); - - /// Update whether a given overlapping pair is active or not - void updateOverlappingPairIsActive(uint64 pairId); + /// Remove a pair + void removePair(uint64 pairIndex, bool isConvexVsConvex); /// Delete all the obsolete last frame collision info void clearObsoleteLastFrameCollisionInfos(); @@ -291,11 +365,8 @@ class OverlappingPairs { /// Set if we need to test a given pair for overlap void setNeedToTestOverlap(uint64 pairId, bool needToTestOverlap); - /// Return true if the two colliders of the pair were already colliding the previous frame - bool getCollidingInPreviousFrame(uint64 pairId) const; - - /// Set to true if the two colliders of the pair were already colliding the previous frame - void setCollidingInPreviousFrame(uint64 pairId, bool wereCollidingInPreviousFrame); + /// Return a reference to an overlapping pair + OverlappingPair* getOverlappingPair(uint64 pairId); #ifdef IS_RP3D_PROFILING_ENABLED @@ -310,51 +381,8 @@ class OverlappingPairs { friend class CollisionDetectionSystem; }; -// Return the entity of the first collider -inline Entity OverlappingPairs::getCollider1(uint64 pairId) const { - assert(mMapPairIdToPairIndex.containsKey(pairId)); - assert(mMapPairIdToPairIndex[pairId] < mNbPairs); - return mColliders1[mMapPairIdToPairIndex[pairId]]; -} - -// Return the entity of the second collider -inline Entity OverlappingPairs::getCollider2(uint64 pairId) const { - assert(mMapPairIdToPairIndex.containsKey(pairId)); - assert(mMapPairIdToPairIndex[pairId] < mNbPairs); - return mColliders2[mMapPairIdToPairIndex[pairId]]; -} - -// Notify if a given pair is active or not -inline void OverlappingPairs::setIsPairActive(uint64 pairId, bool isActive) { - - assert(mMapPairIdToPairIndex.containsKey(pairId)); - assert(mMapPairIdToPairIndex[pairId] < mNbPairs); - mIsActive[mMapPairIdToPairIndex[pairId]] = isActive; -} - -// Return the index of a given overlapping pair in the internal array -inline uint64 OverlappingPairs::getPairIndex(uint64 pairId) const { - assert(mMapPairIdToPairIndex.containsKey(pairId)); - return mMapPairIdToPairIndex[pairId]; -} - -// Return the last frame collision info for a given shape id or nullptr if none is found -inline LastFrameCollisionInfo* OverlappingPairs::getLastFrameCollisionInfo(uint64 pairId, uint64 shapesId) { - - assert(mMapPairIdToPairIndex.containsKey(pairId)); - const uint64 index = mMapPairIdToPairIndex[pairId]; - assert(index < mNbPairs); - - Map::Iterator it = mLastFrameCollisionInfos[index].find(shapesId); - if (it != mLastFrameCollisionInfos[index].end()) { - return it->second; - } - - return nullptr; -} - // Return the pair of bodies index -inline bodypair OverlappingPairs::computeBodiesIndexPair(Entity body1Entity, Entity body2Entity) { +RP3D_FORCE_INLINE bodypair OverlappingPairs::computeBodiesIndexPair(Entity body1Entity, Entity body2Entity) { // Construct the pair of body index bodypair indexPair = body1Entity.id < body2Entity.id ? @@ -364,53 +392,39 @@ inline bodypair OverlappingPairs::computeBodiesIndexPair(Entity body1Entity, Ent return indexPair; } -// Return the number of pairs -inline uint64 OverlappingPairs::getNbPairs() const { - return mNbPairs; -} - -// Return the number of convex vs convex pairs -inline uint64 OverlappingPairs::getNbConvexVsConvexPairs() const { - return mConcavePairsStartIndex; -} - -// Return the number of convex vs concave pairs -inline uint64 OverlappingPairs::getNbConvexVsConcavePairs() const { - return mNbPairs - mConcavePairsStartIndex; -} - -// Return the starting index of the convex vs concave pairs -inline uint64 OverlappingPairs::getConvexVsConcavePairsStartIndex() const { - return mConcavePairsStartIndex; -} - -// Return a reference to the temporary memory allocator -inline MemoryAllocator& OverlappingPairs::getTemporaryAllocator() { - return mTempMemoryAllocator; -} - // Set if we need to test a given pair for overlap -inline void OverlappingPairs::setNeedToTestOverlap(uint64 pairId, bool needToTestOverlap) { - assert(mMapPairIdToPairIndex.containsKey(pairId)); - mNeedToTestOverlap[mMapPairIdToPairIndex[pairId]] = needToTestOverlap; +RP3D_FORCE_INLINE void OverlappingPairs::setNeedToTestOverlap(uint64 pairId, bool needToTestOverlap) { + + assert(mMapConvexPairIdToPairIndex.containsKey(pairId) || mMapConcavePairIdToPairIndex.containsKey(pairId)); + + auto it = mMapConvexPairIdToPairIndex.find(pairId); + if (it != mMapConvexPairIdToPairIndex.end()) { + mConvexPairs[it->second].needToTestOverlap = needToTestOverlap; + } + else { + mConcavePairs[mMapConcavePairIdToPairIndex[pairId]].needToTestOverlap = needToTestOverlap; + } } -// Return true if the two colliders of the pair were already colliding the previous frame -inline bool OverlappingPairs::getCollidingInPreviousFrame(uint64 pairId) const { - assert(mMapPairIdToPairIndex.containsKey(pairId)); - return mCollidingInPreviousFrame[mMapPairIdToPairIndex[pairId]]; -} +// Return a reference to an overlapping pair +RP3D_FORCE_INLINE OverlappingPairs::OverlappingPair* OverlappingPairs::getOverlappingPair(uint64 pairId) { -// Set to true if the two colliders of the pair were already colliding the previous frame -inline void OverlappingPairs::setCollidingInPreviousFrame(uint64 pairId, bool wereCollidingInPreviousFrame) { - assert(mMapPairIdToPairIndex.containsKey(pairId)); - mCollidingInPreviousFrame[mMapPairIdToPairIndex[pairId]] = wereCollidingInPreviousFrame; + auto it = mMapConvexPairIdToPairIndex.find(pairId); + if (it != mMapConvexPairIdToPairIndex.end()) { + return &(mConvexPairs[it->second]); + } + it = mMapConcavePairIdToPairIndex.find(pairId); + if (it != mMapConcavePairIdToPairIndex.end()) { + return &(mConcavePairs[it->second]); + } + + return nullptr; } #ifdef IS_RP3D_PROFILING_ENABLED // Set the profiler -inline void OverlappingPairs::setProfiler(Profiler* profiler) { +RP3D_FORCE_INLINE void OverlappingPairs::setProfiler(Profiler* profiler) { mProfiler = profiler; } diff --git a/include/reactphysics3d/engine/PhysicsCommon.h b/include/reactphysics3d/engine/PhysicsCommon.h index 68574f7f..7f685458 100644 --- a/include/reactphysics3d/engine/PhysicsCommon.h +++ b/include/reactphysics3d/engine/PhysicsCommon.h @@ -92,11 +92,56 @@ class PhysicsCommon { /// Set of default loggers Set mDefaultLoggers; + /// Half-edge structure of a box polyhedron + HalfEdgeStructure mBoxShapeHalfEdgeStructure; + + /// Half-edge structure of a triangle shape + HalfEdgeStructure mTriangleShapeHalfEdgeStructure; + // -------------------- Methods -------------------- // + /// Initialization + void init(); + /// Destroy and release everything that has been allocated void release(); + /// Delete an instance of PhysicsWorld + void deletePhysicsWorld(PhysicsWorld* world); + + /// Delete a sphere collision shape + void deleteSphereShape(SphereShape* sphereShape); + + /// Delete a box collision shape + void deleteBoxShape(BoxShape* boxShape); + + /// Delete a capsule collision shape + void deleteCapsuleShape(CapsuleShape* capsuleShape); + + /// Delete a convex mesh shape + void deleteConvexMeshShape(ConvexMeshShape* convexMeshShape); + + /// Delete a height-field shape + void deleteHeightFieldShape(HeightFieldShape* heightFieldShape); + + /// Delete a concave mesh shape + void deleteConcaveMeshShape(ConcaveMeshShape* concaveMeshShape); + + /// Delete a polyhedron mesh + void deletePolyhedronMesh(PolyhedronMesh* polyhedronMesh); + + /// Delete a triangle mesh + void deleteTriangleMesh(TriangleMesh* triangleMesh); + + /// Delete a default logger + void deleteDefaultLogger(DefaultLogger* logger); + + /// Initialize the half-edge structure of a BoxShape + void initBoxShapeHalfEdgeStructure(); + + /// Initialize the static half-edge structure of a TriangleShape + void initTriangleShapeHalfEdgeStructure(); + // If profiling is enabled #ifdef IS_RP3D_PROFILING_ENABLED @@ -106,6 +151,9 @@ class PhysicsCommon { /// Destroy a profiler void destroyProfiler(Profiler* profiler); + /// Delete a profiler + void deleteProfiler(Profiler* profiler); + #endif public : @@ -187,13 +235,19 @@ class PhysicsCommon { /// Set the logger static void setLogger(Logger* logger); + + // ---------- Friendship ---------- // + + friend class BoxShape; + friend class TriangleShape; + friend class PhysicsWorld; }; // Return the current logger /** * @return A pointer to the current logger */ -inline Logger* PhysicsCommon::getLogger() { +RP3D_FORCE_INLINE Logger* PhysicsCommon::getLogger() { return mLogger; } @@ -201,7 +255,7 @@ inline Logger* PhysicsCommon::getLogger() { /** * @param logger A pointer to the logger to use */ -inline void PhysicsCommon::setLogger(Logger* logger) { +RP3D_FORCE_INLINE void PhysicsCommon::setLogger(Logger* logger) { mLogger = logger; } diff --git a/include/reactphysics3d/engine/PhysicsWorld.h b/include/reactphysics3d/engine/PhysicsWorld.h index 49973d96..a6f894eb 100644 --- a/include/reactphysics3d/engine/PhysicsWorld.h +++ b/include/reactphysics3d/engine/PhysicsWorld.h @@ -28,7 +28,7 @@ // Libraries #include -#include +#include #include #include #include @@ -59,6 +59,7 @@ namespace reactphysics3d { // Declarations class Island; class RigidBody; +class PhysicsCommon; struct JointInfo; // Class PhysicsWorld @@ -93,9 +94,6 @@ class PhysicsWorld { /// Velocity threshold for contact velocity restitution decimal restitutionVelocityThreshold; - /// Default rolling resistance - decimal defaultRollingRestistance; - /// True if the sleeping technique is enabled bool isSleepingEnabled; @@ -116,9 +114,6 @@ class PhysicsWorld { /// might enter sleeping mode decimal defaultSleepAngularVelocity; - /// Maximum number of contact manifolds in an overlapping pair - uint nbMaxContactManifolds; - /// This is used to test if two contact manifold are similar (same contact normal) in order to /// merge them. If the cosine of the angle between the normals of the two manifold are larger /// than the value bellow, the manifold are considered to be similar. @@ -132,14 +127,12 @@ class PhysicsWorld { defaultFrictionCoefficient = decimal(0.3); defaultBounciness = decimal(0.5); restitutionVelocityThreshold = decimal(0.5); - defaultRollingRestistance = decimal(0.0); isSleepingEnabled = true; - defaultVelocitySolverNbIterations = 10; - defaultPositionSolverNbIterations = 5; + defaultVelocitySolverNbIterations = 6; + defaultPositionSolverNbIterations = 3; defaultTimeBeforeSleep = 1.0f; defaultSleepLinearVelocity = decimal(0.02); defaultSleepAngularVelocity = decimal(3.0) * (PI_RP3D / decimal(180.0)); - nbMaxContactManifolds = 3; cosAngleSimilarContactManifold = decimal(0.95); } @@ -156,14 +149,12 @@ class PhysicsWorld { ss << "defaultFrictionCoefficient=" << defaultFrictionCoefficient << std::endl; ss << "defaultBounciness=" << defaultBounciness << std::endl; ss << "restitutionVelocityThreshold=" << restitutionVelocityThreshold << std::endl; - ss << "defaultRollingRestistance=" << defaultRollingRestistance << std::endl; ss << "isSleepingEnabled=" << isSleepingEnabled << std::endl; ss << "defaultVelocitySolverNbIterations=" << defaultVelocitySolverNbIterations << std::endl; ss << "defaultPositionSolverNbIterations=" << defaultPositionSolverNbIterations << std::endl; ss << "defaultTimeBeforeSleep=" << defaultTimeBeforeSleep << std::endl; ss << "defaultSleepLinearVelocity=" << defaultSleepLinearVelocity << std::endl; ss << "defaultSleepAngularVelocity=" << defaultSleepAngularVelocity << std::endl; - ss << "nbMaxContactManifolds=" << nbMaxContactManifolds << std::endl; ss << "cosAngleSimilarContactManifold=" << cosAngleSimilarContactManifold << std::endl; return ss.str(); @@ -220,7 +211,7 @@ class PhysicsWorld { CollisionDetectionSystem mCollisionDetection; /// All the collision bodies of the world - List mCollisionBodies; + Array mCollisionBodies; /// Pointer to an event listener object EventListener* mEventListener; @@ -240,6 +231,11 @@ class PhysicsWorld { /// All the islands of bodies of the current frame Islands mIslands; + /// Order in which to process the ContactPairs for contact creation such that + /// all the contact manifolds and contact points of a given island are packed together + /// This array contains the indices of the ContactPairs. + Array mProcessContactPairsOrderIslands; + /// Contact solver system ContactSolverSystem mContactSolverSystem; @@ -259,7 +255,7 @@ class PhysicsWorld { bool mIsSleepingEnabled; /// All the rigid bodies of the physics world - List mRigidBodies; + Array mRigidBodies; /// True if the gravity force is on bool mIsGravityEnabled; @@ -280,7 +276,7 @@ class PhysicsWorld { // -------------------- Methods -------------------- // /// Constructor - PhysicsWorld(MemoryManager& memoryManager, const WorldSettings& worldSettings = WorldSettings(), Profiler* profiler = nullptr); + PhysicsWorld(MemoryManager& memoryManager, PhysicsCommon& physicsCommon, const WorldSettings& worldSettings = WorldSettings(), Profiler* profiler = nullptr); /// Notify the world if a body is disabled (slepping or inactive) or not void setBodyDisabled(Entity entity, bool isDisabled); @@ -303,9 +299,12 @@ class PhysicsWorld { /// Put bodies to sleep if needed. void updateSleepingBodies(decimal timeStep); - /// Add the joint to the list of joints of the two bodies involved in the joint + /// Add the joint to the array of joints of the two bodies involved in the joint void addJointToBodies(Entity body1, Entity body2, Entity joint); + /// Update the world inverse inertia tensors of rigid bodies + void updateBodiesInverseWorldInertiaTensors(); + /// Destructor ~PhysicsWorld(); @@ -491,7 +490,7 @@ class PhysicsWorld { * @param CollisionDispatch Pointer to a collision dispatch object describing * which collision detection algorithm to use for two given collision shapes */ -inline CollisionDispatch& PhysicsWorld::getCollisionDispatch() { +RP3D_FORCE_INLINE CollisionDispatch& PhysicsWorld::getCollisionDispatch() { return mCollisionDetection.getCollisionDispatch(); } @@ -502,7 +501,7 @@ inline CollisionDispatch& PhysicsWorld::getCollisionDispatch() { * @param raycastWithCategoryMaskBits Bits mask corresponding to the category of * bodies to be raycasted */ -inline void PhysicsWorld::raycast(const Ray& ray, +RP3D_FORCE_INLINE void PhysicsWorld::raycast(const Ray& ray, RaycastCallback* raycastCallback, unsigned short raycastWithCategoryMaskBits) const { mCollisionDetection.raycast(raycastCallback, ray, raycastWithCategoryMaskBits); @@ -518,7 +517,7 @@ inline void PhysicsWorld::raycast(const Ray& ray, * @param body2 Pointer to the second body to test * @param callback Pointer to the object with the callback method */ -inline void PhysicsWorld::testCollision(CollisionBody* body1, CollisionBody* body2, CollisionCallback& callback) { +RP3D_FORCE_INLINE void PhysicsWorld::testCollision(CollisionBody* body1, CollisionBody* body2, CollisionCallback& callback) { mCollisionDetection.testCollision(body1, body2, callback); } @@ -531,7 +530,7 @@ inline void PhysicsWorld::testCollision(CollisionBody* body1, CollisionBody* bod * @param body Pointer to the body against which we need to test collision * @param callback Pointer to the object with the callback method to report contacts */ -inline void PhysicsWorld::testCollision(CollisionBody* body, CollisionCallback& callback) { +RP3D_FORCE_INLINE void PhysicsWorld::testCollision(CollisionBody* body, CollisionCallback& callback) { mCollisionDetection.testCollision(body, callback); } @@ -543,7 +542,7 @@ inline void PhysicsWorld::testCollision(CollisionBody* body, CollisionCallback& /** * @param callback Pointer to the object with the callback method to report contacts */ -inline void PhysicsWorld::testCollision(CollisionCallback& callback) { +RP3D_FORCE_INLINE void PhysicsWorld::testCollision(CollisionCallback& callback) { mCollisionDetection.testCollision(callback); } @@ -555,7 +554,7 @@ inline void PhysicsWorld::testCollision(CollisionCallback& callback) { * @param body Pointer to the collision body to test overlap with * @param overlapCallback Pointer to the callback class to report overlap */ -inline void PhysicsWorld::testOverlap(CollisionBody* body, OverlapCallback& overlapCallback) { +RP3D_FORCE_INLINE void PhysicsWorld::testOverlap(CollisionBody* body, OverlapCallback& overlapCallback) { mCollisionDetection.testOverlap(body, overlapCallback); } @@ -566,12 +565,12 @@ inline void PhysicsWorld::testOverlap(CollisionBody* body, OverlapCallback& over /** * @param overlapCallback Pointer to the callback class to report overlap */ -inline void PhysicsWorld::testOverlap(OverlapCallback& overlapCallback) { +RP3D_FORCE_INLINE void PhysicsWorld::testOverlap(OverlapCallback& overlapCallback) { mCollisionDetection.testOverlap(overlapCallback); } // Return a reference to the memory manager of the world -inline MemoryManager& PhysicsWorld::getMemoryManager() { +RP3D_FORCE_INLINE MemoryManager& PhysicsWorld::getMemoryManager() { return mMemoryManager; } @@ -579,7 +578,7 @@ inline MemoryManager& PhysicsWorld::getMemoryManager() { /** * @return Name of the world */ -inline const std::string& PhysicsWorld::getName() const { +RP3D_FORCE_INLINE const std::string& PhysicsWorld::getName() const { return mName; } @@ -589,7 +588,7 @@ inline const std::string& PhysicsWorld::getName() const { /** * @return A pointer to the profiler */ -inline Profiler* PhysicsWorld::getProfiler() { +RP3D_FORCE_INLINE Profiler* PhysicsWorld::getProfiler() { return mProfiler; } @@ -599,7 +598,7 @@ inline Profiler* PhysicsWorld::getProfiler() { /** * @return The number of iterations of the velocity constraint solver */ -inline uint PhysicsWorld::getNbIterationsVelocitySolver() const { +RP3D_FORCE_INLINE uint PhysicsWorld::getNbIterationsVelocitySolver() const { return mNbVelocitySolverIterations; } @@ -607,7 +606,7 @@ inline uint PhysicsWorld::getNbIterationsVelocitySolver() const { /** * @return The number of iterations of the position constraint solver */ -inline uint PhysicsWorld::getNbIterationsPositionSolver() const { +RP3D_FORCE_INLINE uint PhysicsWorld::getNbIterationsPositionSolver() const { return mNbPositionSolverIterations; } @@ -615,7 +614,7 @@ inline uint PhysicsWorld::getNbIterationsPositionSolver() const { /** * @param technique Technique used for the position correction (Baumgarte or Split Impulses) */ -inline void PhysicsWorld::setContactsPositionCorrectionTechnique( +RP3D_FORCE_INLINE void PhysicsWorld::setContactsPositionCorrectionTechnique( ContactsPositionCorrectionTechnique technique) { if (technique == ContactsPositionCorrectionTechnique::BAUMGARTE_CONTACTS) { mContactSolverSystem.setIsSplitImpulseActive(false); @@ -629,7 +628,7 @@ inline void PhysicsWorld::setContactsPositionCorrectionTechnique( /** * @return The current gravity vector (in meter per seconds squared) */ -inline Vector3 PhysicsWorld::getGravity() const { +RP3D_FORCE_INLINE Vector3 PhysicsWorld::getGravity() const { return mConfig.gravity; } @@ -637,7 +636,7 @@ inline Vector3 PhysicsWorld::getGravity() const { /** * @return True if the gravity is enabled in the world */ -inline bool PhysicsWorld::isGravityEnabled() const { +RP3D_FORCE_INLINE bool PhysicsWorld::isGravityEnabled() const { return mIsGravityEnabled; } @@ -645,7 +644,7 @@ inline bool PhysicsWorld::isGravityEnabled() const { /** * @return True if the sleeping technique is enabled and false otherwise */ -inline bool PhysicsWorld::isSleepingEnabled() const { +RP3D_FORCE_INLINE bool PhysicsWorld::isSleepingEnabled() const { return mIsSleepingEnabled; } @@ -653,7 +652,7 @@ inline bool PhysicsWorld::isSleepingEnabled() const { /** * @return The sleep linear velocity (in meters per second) */ -inline decimal PhysicsWorld::getSleepLinearVelocity() const { +RP3D_FORCE_INLINE decimal PhysicsWorld::getSleepLinearVelocity() const { return mSleepLinearVelocity; } @@ -661,7 +660,7 @@ inline decimal PhysicsWorld::getSleepLinearVelocity() const { /** * @return The sleep angular velocity (in radian per second) */ -inline decimal PhysicsWorld::getSleepAngularVelocity() const { +RP3D_FORCE_INLINE decimal PhysicsWorld::getSleepAngularVelocity() const { return mSleepAngularVelocity; } @@ -669,7 +668,7 @@ inline decimal PhysicsWorld::getSleepAngularVelocity() const { /** * @return Time a body is required to stay still before sleeping (in seconds) */ -inline decimal PhysicsWorld::getTimeBeforeSleep() const { +RP3D_FORCE_INLINE decimal PhysicsWorld::getTimeBeforeSleep() const { return mTimeBeforeSleep; } @@ -679,7 +678,7 @@ inline decimal PhysicsWorld::getTimeBeforeSleep() const { * @param eventListener Pointer to the event listener object that will receive * event callbacks during the simulation */ -inline void PhysicsWorld::setEventListener(EventListener* eventListener) { +RP3D_FORCE_INLINE void PhysicsWorld::setEventListener(EventListener* eventListener) { mEventListener = eventListener; } @@ -688,7 +687,7 @@ inline void PhysicsWorld::setEventListener(EventListener* eventListener) { /** * @return The number of collision bodies in the physics world */ -inline uint PhysicsWorld::getNbCollisionBodies() const { +RP3D_FORCE_INLINE uint32 PhysicsWorld::getNbCollisionBodies() const { return mCollisionBodies.size(); } @@ -696,7 +695,7 @@ inline uint PhysicsWorld::getNbCollisionBodies() const { /** * @return The number of rigid bodies in the physics world */ -inline uint PhysicsWorld::getNbRigidBodies() const { +RP3D_FORCE_INLINE uint32 PhysicsWorld::getNbRigidBodies() const { return mRigidBodies.size(); } @@ -704,7 +703,7 @@ inline uint PhysicsWorld::getNbRigidBodies() const { /** * @return True if the debug rendering is enabled and false otherwise */ -inline bool PhysicsWorld::getIsDebugRenderingEnabled() const { +RP3D_FORCE_INLINE bool PhysicsWorld::getIsDebugRenderingEnabled() const { return mIsDebugRenderingEnabled; } @@ -712,7 +711,7 @@ inline bool PhysicsWorld::getIsDebugRenderingEnabled() const { /** * @param isEnabled True if you want to enable the debug rendering and false otherwise */ -inline void PhysicsWorld::setIsDebugRenderingEnabled(bool isEnabled) { +RP3D_FORCE_INLINE void PhysicsWorld::setIsDebugRenderingEnabled(bool isEnabled) { mIsDebugRenderingEnabled = isEnabled; } @@ -720,7 +719,7 @@ inline void PhysicsWorld::setIsDebugRenderingEnabled(bool isEnabled) { /** * @return A reference to the DebugRenderer object of the world */ -inline DebugRenderer& PhysicsWorld::getDebugRenderer() { +RP3D_FORCE_INLINE DebugRenderer& PhysicsWorld::getDebugRenderer() { return mDebugRenderer; } diff --git a/include/reactphysics3d/engine/Timer.h b/include/reactphysics3d/engine/Timer.h index 8db66360..70d887a9 100644 --- a/include/reactphysics3d/engine/Timer.h +++ b/include/reactphysics3d/engine/Timer.h @@ -120,28 +120,28 @@ class Timer { }; // Return the timestep of the physics engine -inline double Timer::getTimeStep() const { +RP3D_FORCE_INLINE double Timer::getTimeStep() const { return mTimeStep; } // Set the timestep of the physics engine -inline void Timer::setTimeStep(double timeStep) { +RP3D_FORCE_INLINE void Timer::setTimeStep(double timeStep) { assert(timeStep > 0.0f); mTimeStep = timeStep; } // Return the current time -inline long double Timer::getPhysicsTime() const { +RP3D_FORCE_INLINE long double Timer::getPhysicsTime() const { return mLastUpdateTime; } // Return if the timer is running -inline bool Timer::getIsRunning() const { +RP3D_FORCE_INLINE bool Timer::getIsRunning() const { return mIsRunning; } // Start the timer -inline void Timer::start() { +RP3D_FORCE_INLINE void Timer::start() { if (!mIsRunning) { // Get the current system time @@ -153,17 +153,17 @@ inline void Timer::start() { } // Stop the timer -inline void Timer::stop() { +RP3D_FORCE_INLINE void Timer::stop() { mIsRunning = false; } // True if it's possible to take a new step -inline bool Timer::isPossibleToTakeStep() const { +RP3D_FORCE_INLINE bool Timer::isPossibleToTakeStep() const { return (mAccumulator >= mTimeStep); } // Take a new step => update the timer by adding the timeStep value to the current time -inline void Timer::nextStep() { +RP3D_FORCE_INLINE void Timer::nextStep() { assert(mIsRunning); // Update the accumulator value @@ -171,12 +171,12 @@ inline void Timer::nextStep() { } // Compute the interpolation factor -inline decimal Timer::computeInterpolationFactor() { +RP3D_FORCE_INLINE decimal Timer::computeInterpolationFactor() { return (decimal(mAccumulator / mTimeStep)); } // Compute the time since the last update() call and add it to the accumulator -inline void Timer::update() { +RP3D_FORCE_INLINE void Timer::update() { // Get the current system time long double currentTime = getCurrentSystemTime(); diff --git a/include/reactphysics3d/mathematics/Matrix2x2.h b/include/reactphysics3d/mathematics/Matrix2x2.h index 29e2a54d..66c651f6 100644 --- a/include/reactphysics3d/mathematics/Matrix2x2.h +++ b/include/reactphysics3d/mathematics/Matrix2x2.h @@ -151,57 +151,57 @@ class Matrix2x2 { }; // Constructor of the class Matrix2x2 -inline Matrix2x2::Matrix2x2() { +RP3D_FORCE_INLINE Matrix2x2::Matrix2x2() { // Initialize all values in the matrix to zero setAllValues(0.0, 0.0, 0.0, 0.0); } // Constructor -inline Matrix2x2::Matrix2x2(decimal value) { +RP3D_FORCE_INLINE Matrix2x2::Matrix2x2(decimal value) { setAllValues(value, value, value, value); } // Constructor with arguments -inline Matrix2x2::Matrix2x2(decimal a1, decimal a2, decimal b1, decimal b2) { +RP3D_FORCE_INLINE Matrix2x2::Matrix2x2(decimal a1, decimal a2, decimal b1, decimal b2) { // Initialize the matrix with the values setAllValues(a1, a2, b1, b2); } // Copy-constructor -inline Matrix2x2::Matrix2x2(const Matrix2x2& matrix) { +RP3D_FORCE_INLINE Matrix2x2::Matrix2x2(const Matrix2x2& matrix) { setAllValues(matrix.mRows[0][0], matrix.mRows[0][1], matrix.mRows[1][0], matrix.mRows[1][1]); } // Method to set all the values in the matrix -inline void Matrix2x2::setAllValues(decimal a1, decimal a2, +RP3D_FORCE_INLINE void Matrix2x2::setAllValues(decimal a1, decimal a2, decimal b1, decimal b2) { mRows[0][0] = a1; mRows[0][1] = a2; mRows[1][0] = b1; mRows[1][1] = b2; } // Set the matrix to zero -inline void Matrix2x2::setToZero() { +RP3D_FORCE_INLINE void Matrix2x2::setToZero() { mRows[0].setToZero(); mRows[1].setToZero(); } // Return a column -inline Vector2 Matrix2x2::getColumn(int i) const { +RP3D_FORCE_INLINE Vector2 Matrix2x2::getColumn(int i) const { assert(i>= 0 && i<2); return Vector2(mRows[0][i], mRows[1][i]); } // Return a row -inline Vector2 Matrix2x2::getRow(int i) const { +RP3D_FORCE_INLINE Vector2 Matrix2x2::getRow(int i) const { assert(i>= 0 && i<2); return mRows[i]; } // Return the transpose matrix -inline Matrix2x2 Matrix2x2::getTranspose() const { +RP3D_FORCE_INLINE Matrix2x2 Matrix2x2::getTranspose() const { // Return the transpose matrix return Matrix2x2(mRows[0][0], mRows[1][0], @@ -209,45 +209,45 @@ inline Matrix2x2 Matrix2x2::getTranspose() const { } // Return the determinant of the matrix -inline decimal Matrix2x2::getDeterminant() const { +RP3D_FORCE_INLINE decimal Matrix2x2::getDeterminant() const { // Compute and return the determinant of the matrix return mRows[0][0] * mRows[1][1] - mRows[1][0] * mRows[0][1]; } // Return the trace of the matrix -inline decimal Matrix2x2::getTrace() const { +RP3D_FORCE_INLINE decimal Matrix2x2::getTrace() const { // Compute and return the trace return (mRows[0][0] + mRows[1][1]); } // Set the matrix to the identity matrix -inline void Matrix2x2::setToIdentity() { +RP3D_FORCE_INLINE void Matrix2x2::setToIdentity() { mRows[0][0] = 1.0; mRows[0][1] = 0.0; mRows[1][0] = 0.0; mRows[1][1] = 1.0; } // Return the 2x2 identity matrix -inline Matrix2x2 Matrix2x2::identity() { +RP3D_FORCE_INLINE Matrix2x2 Matrix2x2::identity() { // Return the isdentity matrix return Matrix2x2(1.0, 0.0, 0.0, 1.0); } // Return the 2x2 zero matrix -inline Matrix2x2 Matrix2x2::zero() { +RP3D_FORCE_INLINE Matrix2x2 Matrix2x2::zero() { return Matrix2x2(0.0, 0.0, 0.0, 0.0); } // Return the matrix with absolute values -inline Matrix2x2 Matrix2x2::getAbsoluteMatrix() const { - return Matrix2x2(fabs(mRows[0][0]), fabs(mRows[0][1]), - fabs(mRows[1][0]), fabs(mRows[1][1])); +RP3D_FORCE_INLINE Matrix2x2 Matrix2x2::getAbsoluteMatrix() const { + return Matrix2x2(std::abs(mRows[0][0]), std::abs(mRows[0][1]), + std::abs(mRows[1][0]), std::abs(mRows[1][1])); } // Overloaded operator for addition -inline Matrix2x2 operator+(const Matrix2x2& matrix1, const Matrix2x2& matrix2) { +RP3D_FORCE_INLINE Matrix2x2 operator+(const Matrix2x2& matrix1, const Matrix2x2& matrix2) { return Matrix2x2(matrix1.mRows[0][0] + matrix2.mRows[0][0], matrix1.mRows[0][1] + matrix2.mRows[0][1], matrix1.mRows[1][0] + matrix2.mRows[1][0], @@ -255,7 +255,7 @@ inline Matrix2x2 operator+(const Matrix2x2& matrix1, const Matrix2x2& matrix2) { } // Overloaded operator for substraction -inline Matrix2x2 operator-(const Matrix2x2& matrix1, const Matrix2x2& matrix2) { +RP3D_FORCE_INLINE Matrix2x2 operator-(const Matrix2x2& matrix1, const Matrix2x2& matrix2) { return Matrix2x2(matrix1.mRows[0][0] - matrix2.mRows[0][0], matrix1.mRows[0][1] - matrix2.mRows[0][1], matrix1.mRows[1][0] - matrix2.mRows[1][0], @@ -263,24 +263,24 @@ inline Matrix2x2 operator-(const Matrix2x2& matrix1, const Matrix2x2& matrix2) { } // Overloaded operator for the negative of the matrix -inline Matrix2x2 operator-(const Matrix2x2& matrix) { +RP3D_FORCE_INLINE Matrix2x2 operator-(const Matrix2x2& matrix) { return Matrix2x2(-matrix.mRows[0][0], -matrix.mRows[0][1], -matrix.mRows[1][0], -matrix.mRows[1][1]); } // Overloaded operator for multiplication with a number -inline Matrix2x2 operator*(decimal nb, const Matrix2x2& matrix) { +RP3D_FORCE_INLINE Matrix2x2 operator*(decimal nb, const Matrix2x2& matrix) { return Matrix2x2(matrix.mRows[0][0] * nb, matrix.mRows[0][1] * nb, matrix.mRows[1][0] * nb, matrix.mRows[1][1] * nb); } // Overloaded operator for multiplication with a matrix -inline Matrix2x2 operator*(const Matrix2x2& matrix, decimal nb) { +RP3D_FORCE_INLINE Matrix2x2 operator*(const Matrix2x2& matrix, decimal nb) { return nb * matrix; } // Overloaded operator for matrix multiplication -inline Matrix2x2 operator*(const Matrix2x2& matrix1, const Matrix2x2& matrix2) { +RP3D_FORCE_INLINE Matrix2x2 operator*(const Matrix2x2& matrix1, const Matrix2x2& matrix2) { return Matrix2x2(matrix1.mRows[0][0] * matrix2.mRows[0][0] + matrix1.mRows[0][1] * matrix2.mRows[1][0], matrix1.mRows[0][0] * matrix2.mRows[0][1] + matrix1.mRows[0][1] * @@ -292,38 +292,38 @@ inline Matrix2x2 operator*(const Matrix2x2& matrix1, const Matrix2x2& matrix2) { } // Overloaded operator for multiplication with a vector -inline Vector2 operator*(const Matrix2x2& matrix, const Vector2& vector) { +RP3D_FORCE_INLINE Vector2 operator*(const Matrix2x2& matrix, const Vector2& vector) { return Vector2(matrix.mRows[0][0]*vector.x + matrix.mRows[0][1]*vector.y, matrix.mRows[1][0]*vector.x + matrix.mRows[1][1]*vector.y); } // Overloaded operator for equality condition -inline bool Matrix2x2::operator==(const Matrix2x2& matrix) const { +RP3D_FORCE_INLINE bool Matrix2x2::operator==(const Matrix2x2& matrix) const { return (mRows[0][0] == matrix.mRows[0][0] && mRows[0][1] == matrix.mRows[0][1] && mRows[1][0] == matrix.mRows[1][0] && mRows[1][1] == matrix.mRows[1][1]); } // Overloaded operator for the is different condition -inline bool Matrix2x2::operator!= (const Matrix2x2& matrix) const { +RP3D_FORCE_INLINE bool Matrix2x2::operator!= (const Matrix2x2& matrix) const { return !(*this == matrix); } // Overloaded operator for addition with assignment -inline Matrix2x2& Matrix2x2::operator+=(const Matrix2x2& matrix) { +RP3D_FORCE_INLINE Matrix2x2& Matrix2x2::operator+=(const Matrix2x2& matrix) { mRows[0][0] += matrix.mRows[0][0]; mRows[0][1] += matrix.mRows[0][1]; mRows[1][0] += matrix.mRows[1][0]; mRows[1][1] += matrix.mRows[1][1]; return *this; } // Overloaded operator for substraction with assignment -inline Matrix2x2& Matrix2x2::operator-=(const Matrix2x2& matrix) { +RP3D_FORCE_INLINE Matrix2x2& Matrix2x2::operator-=(const Matrix2x2& matrix) { mRows[0][0] -= matrix.mRows[0][0]; mRows[0][1] -= matrix.mRows[0][1]; mRows[1][0] -= matrix.mRows[1][0]; mRows[1][1] -= matrix.mRows[1][1]; return *this; } // Overloaded operator for multiplication with a number with assignment -inline Matrix2x2& Matrix2x2::operator*=(decimal nb) { +RP3D_FORCE_INLINE Matrix2x2& Matrix2x2::operator*=(decimal nb) { mRows[0][0] *= nb; mRows[0][1] *= nb; mRows[1][0] *= nb; mRows[1][1] *= nb; return *this; @@ -332,19 +332,19 @@ inline Matrix2x2& Matrix2x2::operator*=(decimal nb) { // Overloaded operator to return a row of the matrix. /// This operator is also used to access a matrix value using the syntax /// matrix[row][col]. -inline const Vector2& Matrix2x2::operator[](int row) const { +RP3D_FORCE_INLINE const Vector2& Matrix2x2::operator[](int row) const { return mRows[row]; } // Overloaded operator to return a row of the matrix. /// This operator is also used to access a matrix value using the syntax /// matrix[row][col]. -inline Vector2& Matrix2x2::operator[](int row) { +RP3D_FORCE_INLINE Vector2& Matrix2x2::operator[](int row) { return mRows[row]; } // Get the string representation -inline std::string Matrix2x2::to_string() const { +RP3D_FORCE_INLINE std::string Matrix2x2::to_string() const { return "Matrix2x2(" + std::to_string(mRows[0][0]) + "," + std::to_string(mRows[0][1]) + "," + std::to_string(mRows[1][0]) + "," + std::to_string(mRows[1][1]) + ")"; } diff --git a/include/reactphysics3d/mathematics/Matrix3x3.h b/include/reactphysics3d/mathematics/Matrix3x3.h index 267a3ebb..0a2661a0 100644 --- a/include/reactphysics3d/mathematics/Matrix3x3.h +++ b/include/reactphysics3d/mathematics/Matrix3x3.h @@ -158,18 +158,18 @@ class Matrix3x3 { }; // Constructor of the class Matrix3x3 -inline Matrix3x3::Matrix3x3() { +RP3D_FORCE_INLINE Matrix3x3::Matrix3x3() { // Initialize all values in the matrix to zero setAllValues(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0); } // Constructor -inline Matrix3x3::Matrix3x3(decimal value) { +RP3D_FORCE_INLINE Matrix3x3::Matrix3x3(decimal value) { setAllValues(value, value, value, value, value, value, value, value, value); } // Constructor with arguments -inline Matrix3x3::Matrix3x3(decimal a1, decimal a2, decimal a3, +RP3D_FORCE_INLINE Matrix3x3::Matrix3x3(decimal a1, decimal a2, decimal a3, decimal b1, decimal b2, decimal b3, decimal c1, decimal c2, decimal c3) { // Initialize the matrix with the values @@ -177,14 +177,14 @@ inline Matrix3x3::Matrix3x3(decimal a1, decimal a2, decimal a3, } // Copy-constructor -inline Matrix3x3::Matrix3x3(const Matrix3x3& matrix) { +RP3D_FORCE_INLINE Matrix3x3::Matrix3x3(const Matrix3x3& matrix) { setAllValues(matrix.mRows[0][0], matrix.mRows[0][1], matrix.mRows[0][2], matrix.mRows[1][0], matrix.mRows[1][1], matrix.mRows[1][2], matrix.mRows[2][0], matrix.mRows[2][1], matrix.mRows[2][2]); } // Method to set all the values in the matrix -inline void Matrix3x3::setAllValues(decimal a1, decimal a2, decimal a3, +RP3D_FORCE_INLINE void Matrix3x3::setAllValues(decimal a1, decimal a2, decimal a3, decimal b1, decimal b2, decimal b3, decimal c1, decimal c2, decimal c3) { mRows[0][0] = a1; mRows[0][1] = a2; mRows[0][2] = a3; @@ -193,26 +193,26 @@ inline void Matrix3x3::setAllValues(decimal a1, decimal a2, decimal a3, } // Set the matrix to zero -inline void Matrix3x3::setToZero() { +RP3D_FORCE_INLINE void Matrix3x3::setToZero() { mRows[0].setToZero(); mRows[1].setToZero(); mRows[2].setToZero(); } // Return a column -inline Vector3 Matrix3x3::getColumn(int i) const { +RP3D_FORCE_INLINE Vector3 Matrix3x3::getColumn(int i) const { assert(i>= 0 && i<3); return Vector3(mRows[0][i], mRows[1][i], mRows[2][i]); } // Return a row -inline Vector3 Matrix3x3::getRow(int i) const { +RP3D_FORCE_INLINE Vector3 Matrix3x3::getRow(int i) const { assert(i>= 0 && i<3); return mRows[i]; } // Return the transpose matrix -inline Matrix3x3 Matrix3x3::getTranspose() const { +RP3D_FORCE_INLINE Matrix3x3 Matrix3x3::getTranspose() const { // Return the transpose matrix return Matrix3x3(mRows[0][0], mRows[1][0], mRows[2][0], @@ -221,7 +221,7 @@ inline Matrix3x3 Matrix3x3::getTranspose() const { } // Return the determinant of the matrix -inline decimal Matrix3x3::getDeterminant() const { +RP3D_FORCE_INLINE decimal Matrix3x3::getDeterminant() const { // Compute and return the determinant of the matrix return (mRows[0][0]*(mRows[1][1]*mRows[2][2]-mRows[2][1]*mRows[1][2]) - @@ -230,44 +230,44 @@ inline decimal Matrix3x3::getDeterminant() const { } // Return the trace of the matrix -inline decimal Matrix3x3::getTrace() const { +RP3D_FORCE_INLINE decimal Matrix3x3::getTrace() const { // Compute and return the trace return (mRows[0][0] + mRows[1][1] + mRows[2][2]); } // Set the matrix to the identity matrix -inline void Matrix3x3::setToIdentity() { +RP3D_FORCE_INLINE void Matrix3x3::setToIdentity() { mRows[0][0] = 1.0; mRows[0][1] = 0.0; mRows[0][2] = 0.0; mRows[1][0] = 0.0; mRows[1][1] = 1.0; mRows[1][2] = 0.0; mRows[2][0] = 0.0; mRows[2][1] = 0.0; mRows[2][2] = 1.0; } // Return the 3x3 identity matrix -inline Matrix3x3 Matrix3x3::identity() { +RP3D_FORCE_INLINE Matrix3x3 Matrix3x3::identity() { return Matrix3x3(1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0); } // Return the 3x3 zero matrix -inline Matrix3x3 Matrix3x3::zero() { +RP3D_FORCE_INLINE Matrix3x3 Matrix3x3::zero() { return Matrix3x3(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0); } // Return a skew-symmetric matrix using a given vector that can be used // to compute cross product with another vector using matrix multiplication -inline Matrix3x3 Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(const Vector3& vector) { +RP3D_FORCE_INLINE Matrix3x3 Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(const Vector3& vector) { return Matrix3x3(0, -vector.z, vector.y, vector.z, 0, -vector.x, -vector.y, vector.x, 0); } // Return the matrix with absolute values -inline Matrix3x3 Matrix3x3::getAbsoluteMatrix() const { - return Matrix3x3(std::fabs(mRows[0][0]), std::fabs(mRows[0][1]), std::fabs(mRows[0][2]), - std::fabs(mRows[1][0]), std::fabs(mRows[1][1]), std::fabs(mRows[1][2]), - std::fabs(mRows[2][0]), std::fabs(mRows[2][1]), std::fabs(mRows[2][2])); +RP3D_FORCE_INLINE Matrix3x3 Matrix3x3::getAbsoluteMatrix() const { + return Matrix3x3(std::abs(mRows[0][0]), std::abs(mRows[0][1]), std::abs(mRows[0][2]), + std::abs(mRows[1][0]), std::abs(mRows[1][1]), std::abs(mRows[1][2]), + std::abs(mRows[2][0]), std::abs(mRows[2][1]), std::abs(mRows[2][2])); } // Overloaded operator for addition -inline Matrix3x3 operator+(const Matrix3x3& matrix1, const Matrix3x3& matrix2) { +RP3D_FORCE_INLINE Matrix3x3 operator+(const Matrix3x3& matrix1, const Matrix3x3& matrix2) { return Matrix3x3(matrix1.mRows[0][0] + matrix2.mRows[0][0], matrix1.mRows[0][1] + matrix2.mRows[0][1], matrix1.mRows[0][2] + matrix2.mRows[0][2], matrix1.mRows[1][0] + matrix2.mRows[1][0], matrix1.mRows[1][1] + @@ -277,7 +277,7 @@ inline Matrix3x3 operator+(const Matrix3x3& matrix1, const Matrix3x3& matrix2) { } // Overloaded operator for substraction -inline Matrix3x3 operator-(const Matrix3x3& matrix1, const Matrix3x3& matrix2) { +RP3D_FORCE_INLINE Matrix3x3 operator-(const Matrix3x3& matrix1, const Matrix3x3& matrix2) { return Matrix3x3(matrix1.mRows[0][0] - matrix2.mRows[0][0], matrix1.mRows[0][1] - matrix2.mRows[0][1], matrix1.mRows[0][2] - matrix2.mRows[0][2], matrix1.mRows[1][0] - matrix2.mRows[1][0], matrix1.mRows[1][1] - @@ -287,26 +287,26 @@ inline Matrix3x3 operator-(const Matrix3x3& matrix1, const Matrix3x3& matrix2) { } // Overloaded operator for the negative of the matrix -inline Matrix3x3 operator-(const Matrix3x3& matrix) { +RP3D_FORCE_INLINE Matrix3x3 operator-(const Matrix3x3& matrix) { return Matrix3x3(-matrix.mRows[0][0], -matrix.mRows[0][1], -matrix.mRows[0][2], -matrix.mRows[1][0], -matrix.mRows[1][1], -matrix.mRows[1][2], -matrix.mRows[2][0], -matrix.mRows[2][1], -matrix.mRows[2][2]); } // Overloaded operator for multiplication with a number -inline Matrix3x3 operator*(decimal nb, const Matrix3x3& matrix) { +RP3D_FORCE_INLINE Matrix3x3 operator*(decimal nb, const Matrix3x3& matrix) { return Matrix3x3(matrix.mRows[0][0] * nb, matrix.mRows[0][1] * nb, matrix.mRows[0][2] * nb, matrix.mRows[1][0] * nb, matrix.mRows[1][1] * nb, matrix.mRows[1][2] * nb, matrix.mRows[2][0] * nb, matrix.mRows[2][1] * nb, matrix.mRows[2][2] * nb); } // Overloaded operator for multiplication with a matrix -inline Matrix3x3 operator*(const Matrix3x3& matrix, decimal nb) { +RP3D_FORCE_INLINE Matrix3x3 operator*(const Matrix3x3& matrix, decimal nb) { return nb * matrix; } // Overloaded operator for matrix multiplication -inline Matrix3x3 operator*(const Matrix3x3& matrix1, const Matrix3x3& matrix2) { +RP3D_FORCE_INLINE Matrix3x3 operator*(const Matrix3x3& matrix1, const Matrix3x3& matrix2) { return Matrix3x3(matrix1.mRows[0][0]*matrix2.mRows[0][0] + matrix1.mRows[0][1] * matrix2.mRows[1][0] + matrix1.mRows[0][2]*matrix2.mRows[2][0], matrix1.mRows[0][0]*matrix2.mRows[0][1] + matrix1.mRows[0][1] * @@ -328,7 +328,7 @@ inline Matrix3x3 operator*(const Matrix3x3& matrix1, const Matrix3x3& matrix2) { } // Overloaded operator for multiplication with a vector -inline Vector3 operator*(const Matrix3x3& matrix, const Vector3& vector) { +RP3D_FORCE_INLINE Vector3 operator*(const Matrix3x3& matrix, const Vector3& vector) { return Vector3(matrix.mRows[0][0]*vector.x + matrix.mRows[0][1]*vector.y + matrix.mRows[0][2]*vector.z, matrix.mRows[1][0]*vector.x + matrix.mRows[1][1]*vector.y + @@ -338,7 +338,7 @@ inline Vector3 operator*(const Matrix3x3& matrix, const Vector3& vector) { } // Overloaded operator for equality condition -inline bool Matrix3x3::operator==(const Matrix3x3& matrix) const { +RP3D_FORCE_INLINE bool Matrix3x3::operator==(const Matrix3x3& matrix) const { return (mRows[0][0] == matrix.mRows[0][0] && mRows[0][1] == matrix.mRows[0][1] && mRows[0][2] == matrix.mRows[0][2] && mRows[1][0] == matrix.mRows[1][0] && mRows[1][1] == matrix.mRows[1][1] && @@ -348,12 +348,12 @@ inline bool Matrix3x3::operator==(const Matrix3x3& matrix) const { } // Overloaded operator for the is different condition -inline bool Matrix3x3::operator!= (const Matrix3x3& matrix) const { +RP3D_FORCE_INLINE bool Matrix3x3::operator!= (const Matrix3x3& matrix) const { return !(*this == matrix); } // Overloaded operator for addition with assignment -inline Matrix3x3& Matrix3x3::operator+=(const Matrix3x3& matrix) { +RP3D_FORCE_INLINE Matrix3x3& Matrix3x3::operator+=(const Matrix3x3& matrix) { mRows[0][0] += matrix.mRows[0][0]; mRows[0][1] += matrix.mRows[0][1]; mRows[0][2] += matrix.mRows[0][2]; mRows[1][0] += matrix.mRows[1][0]; mRows[1][1] += matrix.mRows[1][1]; mRows[1][2] += matrix.mRows[1][2]; @@ -363,7 +363,7 @@ inline Matrix3x3& Matrix3x3::operator+=(const Matrix3x3& matrix) { } // Overloaded operator for substraction with assignment -inline Matrix3x3& Matrix3x3::operator-=(const Matrix3x3& matrix) { +RP3D_FORCE_INLINE Matrix3x3& Matrix3x3::operator-=(const Matrix3x3& matrix) { mRows[0][0] -= matrix.mRows[0][0]; mRows[0][1] -= matrix.mRows[0][1]; mRows[0][2] -= matrix.mRows[0][2]; mRows[1][0] -= matrix.mRows[1][0]; mRows[1][1] -= matrix.mRows[1][1]; mRows[1][2] -= matrix.mRows[1][2]; @@ -373,7 +373,7 @@ inline Matrix3x3& Matrix3x3::operator-=(const Matrix3x3& matrix) { } // Overloaded operator for multiplication with a number with assignment -inline Matrix3x3& Matrix3x3::operator*=(decimal nb) { +RP3D_FORCE_INLINE Matrix3x3& Matrix3x3::operator*=(decimal nb) { mRows[0][0] *= nb; mRows[0][1] *= nb; mRows[0][2] *= nb; mRows[1][0] *= nb; mRows[1][1] *= nb; mRows[1][2] *= nb; mRows[2][0] *= nb; mRows[2][1] *= nb; mRows[2][2] *= nb; @@ -383,19 +383,19 @@ inline Matrix3x3& Matrix3x3::operator*=(decimal nb) { // Overloaded operator to return a row of the matrix. /// This operator is also used to access a matrix value using the syntax /// matrix[row][col]. -inline const Vector3& Matrix3x3::operator[](int row) const { +RP3D_FORCE_INLINE const Vector3& Matrix3x3::operator[](int row) const { return mRows[row]; } // Overloaded operator to return a row of the matrix. /// This operator is also used to access a matrix value using the syntax /// matrix[row][col]. -inline Vector3& Matrix3x3::operator[](int row) { +RP3D_FORCE_INLINE Vector3& Matrix3x3::operator[](int row) { return mRows[row]; } // Get the string representation -inline std::string Matrix3x3::to_string() const { +RP3D_FORCE_INLINE std::string Matrix3x3::to_string() const { return "Matrix3x3(" + std::to_string(mRows[0][0]) + "," + std::to_string(mRows[0][1]) + "," + std::to_string(mRows[0][2]) + "," + std::to_string(mRows[1][0]) + "," + std::to_string(mRows[1][1]) + "," + std::to_string(mRows[1][2]) + "," + std::to_string(mRows[2][0]) + "," + std::to_string(mRows[2][1]) + "," + std::to_string(mRows[2][2]) + ")"; diff --git a/include/reactphysics3d/mathematics/Quaternion.h b/include/reactphysics3d/mathematics/Quaternion.h index 8db5682d..15313474 100644 --- a/include/reactphysics3d/mathematics/Quaternion.h +++ b/include/reactphysics3d/mathematics/Quaternion.h @@ -182,28 +182,28 @@ struct Quaternion { }; // Constructor of the class -inline Quaternion::Quaternion() : x(0.0), y(0.0), z(0.0), w(0.0) { +RP3D_FORCE_INLINE Quaternion::Quaternion() : x(0.0), y(0.0), z(0.0), w(0.0) { } // Constructor with arguments -inline Quaternion::Quaternion(decimal newX, decimal newY, decimal newZ, decimal newW) +RP3D_FORCE_INLINE Quaternion::Quaternion(decimal newX, decimal newY, decimal newZ, decimal newW) :x(newX), y(newY), z(newZ), w(newW) { } // Constructor with the component w and the vector v=(x y z) -inline Quaternion::Quaternion(decimal newW, const Vector3& v) : x(v.x), y(v.y), z(v.z), w(newW) { +RP3D_FORCE_INLINE Quaternion::Quaternion(decimal newW, const Vector3& v) : x(v.x), y(v.y), z(v.z), w(newW) { } // Constructor with the component w and the vector v=(x y z) -inline Quaternion::Quaternion(const Vector3& v, decimal newW) : x(v.x), y(v.y), z(v.z), w(newW) { +RP3D_FORCE_INLINE Quaternion::Quaternion(const Vector3& v, decimal newW) : x(v.x), y(v.y), z(v.z), w(newW) { } // Set all the values -inline void Quaternion::setAllValues(decimal newX, decimal newY, decimal newZ, decimal newW) { +RP3D_FORCE_INLINE void Quaternion::setAllValues(decimal newX, decimal newY, decimal newZ, decimal newW) { x = newX; y = newY; z = newZ; @@ -211,7 +211,7 @@ inline void Quaternion::setAllValues(decimal newX, decimal newY, decimal newZ, d } // Set the quaternion to zero -inline void Quaternion::setToZero() { +RP3D_FORCE_INLINE void Quaternion::setToZero() { x = 0; y = 0; z = 0; @@ -219,7 +219,7 @@ inline void Quaternion::setToZero() { } // Set to the identity quaternion -inline void Quaternion::setToIdentity() { +RP3D_FORCE_INLINE void Quaternion::setToIdentity() { x = 0; y = 0; z = 0; @@ -227,24 +227,24 @@ inline void Quaternion::setToIdentity() { } // Return the vector v=(x y z) of the quaternion -inline Vector3 Quaternion::getVectorV() const { +RP3D_FORCE_INLINE Vector3 Quaternion::getVectorV() const { // Return the vector v return Vector3(x, y, z); } -// Return the length of the quaternion (inline) -inline decimal Quaternion::length() const { +// Return the length of the quaternion (RP3D_FORCE_INLINE) +RP3D_FORCE_INLINE decimal Quaternion::length() const { return std::sqrt(x*x + y*y + z*z + w*w); } // Return the square of the length of the quaternion -inline decimal Quaternion::lengthSquare() const { +RP3D_FORCE_INLINE decimal Quaternion::lengthSquare() const { return x*x + y*y + z*z + w*w; } // Normalize the quaternion -inline void Quaternion::normalize() { +RP3D_FORCE_INLINE void Quaternion::normalize() { decimal l = length(); @@ -258,7 +258,7 @@ inline void Quaternion::normalize() { } // Inverse the quaternion -inline void Quaternion::inverse() { +RP3D_FORCE_INLINE void Quaternion::inverse() { // Use the conjugate of the current quaternion x = -x; @@ -267,7 +267,7 @@ inline void Quaternion::inverse() { } // Return the unit quaternion -inline Quaternion Quaternion::getUnit() const { +RP3D_FORCE_INLINE Quaternion Quaternion::getUnit() const { decimal lengthQuaternion = length(); // Check if the length is not equal to zero @@ -279,60 +279,60 @@ inline Quaternion Quaternion::getUnit() const { } // Return the identity quaternion -inline Quaternion Quaternion::identity() { +RP3D_FORCE_INLINE Quaternion Quaternion::identity() { return Quaternion(0.0, 0.0, 0.0, 1.0); } -// Return the conjugate of the quaternion (inline) -inline Quaternion Quaternion::getConjugate() const { +// Return the conjugate of the quaternion (RP3D_FORCE_INLINE) +RP3D_FORCE_INLINE Quaternion Quaternion::getConjugate() const { return Quaternion(-x, -y, -z, w); } -// Return the inverse of the quaternion (inline) -inline Quaternion Quaternion::getInverse() const { +// Return the inverse of the quaternion (RP3D_FORCE_INLINE) +RP3D_FORCE_INLINE Quaternion Quaternion::getInverse() const { // Return the conjugate quaternion return Quaternion(-x, -y, -z, w); } // Scalar product between two quaternions -inline decimal Quaternion::dot(const Quaternion& quaternion) const { +RP3D_FORCE_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 { +RP3D_FORCE_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 { +RP3D_FORCE_INLINE bool Quaternion::isUnit() const { const decimal length = std::sqrt(x*x + y*y + z*z + w*w); const decimal tolerance = 1e-5f; return std::abs(length - decimal(1.0)) < tolerance; } // Return true if it is a valid quaternion -inline bool Quaternion::isValid() const { +RP3D_FORCE_INLINE bool Quaternion::isValid() const { return isFinite() && isUnit(); } // Overloaded operator for the addition of two quaternions -inline Quaternion Quaternion::operator+(const Quaternion& quaternion) const { +RP3D_FORCE_INLINE Quaternion Quaternion::operator+(const Quaternion& quaternion) const { // Return the result quaternion return Quaternion(x + quaternion.x, y + quaternion.y, z + quaternion.z, w + quaternion.w); } // Overloaded operator for the substraction of two quaternions -inline Quaternion Quaternion::operator-(const Quaternion& quaternion) const { +RP3D_FORCE_INLINE Quaternion Quaternion::operator-(const Quaternion& quaternion) const { // Return the result of the substraction return Quaternion(x - quaternion.x, y - quaternion.y, z - quaternion.z, w - quaternion.w); } // Overloaded operator for addition with assignment -inline Quaternion& Quaternion::operator+=(const Quaternion& quaternion) { +RP3D_FORCE_INLINE Quaternion& Quaternion::operator+=(const Quaternion& quaternion) { x += quaternion.x; y += quaternion.y; z += quaternion.z; @@ -341,7 +341,7 @@ inline Quaternion& Quaternion::operator+=(const Quaternion& quaternion) { } // Overloaded operator for substraction with assignment -inline Quaternion& Quaternion::operator-=(const Quaternion& quaternion) { +RP3D_FORCE_INLINE Quaternion& Quaternion::operator-=(const Quaternion& quaternion) { x -= quaternion.x; y -= quaternion.y; z -= quaternion.z; @@ -350,12 +350,12 @@ inline Quaternion& Quaternion::operator-=(const Quaternion& quaternion) { } // Overloaded operator for the multiplication with a constant -inline Quaternion Quaternion::operator*(decimal nb) const { +RP3D_FORCE_INLINE Quaternion Quaternion::operator*(decimal nb) const { return Quaternion(nb * x, nb * y, nb * z, nb * w); } // Overloaded operator for the multiplication of two quaternions -inline Quaternion Quaternion::operator*(const Quaternion& quaternion) const { +RP3D_FORCE_INLINE Quaternion Quaternion::operator*(const Quaternion& quaternion) const { /* The followin code is equivalent to this return Quaternion(w * quaternion.w - getVectorV().dot(quaternion.getVectorV()), @@ -371,7 +371,7 @@ inline Quaternion Quaternion::operator*(const Quaternion& quaternion) const { // Overloaded operator for the multiplication with a vector. /// This methods rotates a point given the rotation of a quaternion. -inline Vector3 Quaternion::operator*(const Vector3& point) const { +RP3D_FORCE_INLINE Vector3 Quaternion::operator*(const Vector3& point) const { /* The following code is equivalent to this * Quaternion p(point.x, point.y, point.z, 0.0); @@ -388,7 +388,7 @@ inline Vector3 Quaternion::operator*(const Vector3& point) const { } // Overloaded operator for the assignment -inline Quaternion& Quaternion::operator=(const Quaternion& quaternion) { +RP3D_FORCE_INLINE Quaternion& Quaternion::operator=(const Quaternion& quaternion) { // Check for self-assignment if (this != &quaternion) { @@ -403,13 +403,13 @@ inline Quaternion& Quaternion::operator=(const Quaternion& quaternion) { } // Overloaded operator for equality condition -inline bool Quaternion::operator==(const Quaternion& quaternion) const { +RP3D_FORCE_INLINE bool Quaternion::operator==(const Quaternion& quaternion) const { return (x == quaternion.x && y == quaternion.y && z == quaternion.z && w == quaternion.w); } // Get the string representation -inline std::string Quaternion::to_string() const { +RP3D_FORCE_INLINE std::string Quaternion::to_string() const { return "Quaternion(" + std::to_string(x) + "," + std::to_string(y) + "," + std::to_string(z) + "," + std::to_string(w) + ")"; } diff --git a/include/reactphysics3d/mathematics/Transform.h b/include/reactphysics3d/mathematics/Transform.h index aca59848..575eca7f 100644 --- a/include/reactphysics3d/mathematics/Transform.h +++ b/include/reactphysics3d/mathematics/Transform.h @@ -125,62 +125,62 @@ class Transform { }; // Constructor -inline Transform::Transform() : mPosition(Vector3(0.0, 0.0, 0.0)), mOrientation(Quaternion::identity()) { +RP3D_FORCE_INLINE Transform::Transform() : mPosition(Vector3(0.0, 0.0, 0.0)), mOrientation(Quaternion::identity()) { } // Constructor -inline Transform::Transform(const Vector3& position, const Matrix3x3& orientation) +RP3D_FORCE_INLINE Transform::Transform(const Vector3& position, const Matrix3x3& orientation) : mPosition(position), mOrientation(Quaternion(orientation)) { } // Constructor -inline Transform::Transform(const Vector3& position, const Quaternion& orientation) +RP3D_FORCE_INLINE Transform::Transform(const Vector3& position, const Quaternion& orientation) : mPosition(position), mOrientation(orientation) { } // Copy-constructor -inline Transform::Transform(const Transform& transform) +RP3D_FORCE_INLINE Transform::Transform(const Transform& transform) : mPosition(transform.mPosition), mOrientation(transform.mOrientation) { } // Return the position of the transform -inline const Vector3& Transform::getPosition() const { +RP3D_FORCE_INLINE const Vector3& Transform::getPosition() const { return mPosition; } // Set the origin of the transform -inline void Transform::setPosition(const Vector3& position) { +RP3D_FORCE_INLINE void Transform::setPosition(const Vector3& position) { mPosition = position; } // Return the rotation matrix -inline const Quaternion& Transform::getOrientation() const { +RP3D_FORCE_INLINE const Quaternion& Transform::getOrientation() const { return mOrientation; } // Set the rotation matrix of the transform -inline void Transform::setOrientation(const Quaternion& orientation) { +RP3D_FORCE_INLINE void Transform::setOrientation(const Quaternion& orientation) { mOrientation = orientation; } // Set the transform to the identity transform -inline void Transform::setToIdentity() { +RP3D_FORCE_INLINE void Transform::setToIdentity() { mPosition = Vector3(0.0, 0.0, 0.0); mOrientation = Quaternion::identity(); } // Return the inverse of the transform -inline Transform Transform::getInverse() const { +RP3D_FORCE_INLINE Transform Transform::getInverse() const { const Quaternion& invQuaternion = mOrientation.getInverse(); return Transform(invQuaternion * (-mPosition), invQuaternion); } // Return an interpolated transform -inline Transform Transform::interpolateTransforms(const Transform& oldTransform, +RP3D_FORCE_INLINE Transform Transform::interpolateTransforms(const Transform& oldTransform, const Transform& newTransform, decimal interpolationFactor) { @@ -195,22 +195,22 @@ inline Transform Transform::interpolateTransforms(const Transform& oldTransform, } // Return the identity transform -inline Transform Transform::identity() { +RP3D_FORCE_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 { +RP3D_FORCE_INLINE bool Transform::isValid() const { return mPosition.isFinite() && mOrientation.isValid(); } // Return the transformed vector -inline Vector3 Transform::operator*(const Vector3& vector) const { +RP3D_FORCE_INLINE Vector3 Transform::operator*(const Vector3& vector) const { return (mOrientation * vector) + mPosition; } // Operator of multiplication of a transform with another one -inline Transform Transform::operator*(const Transform& transform2) const { +RP3D_FORCE_INLINE Transform Transform::operator*(const Transform& transform2) const { // The following code is equivalent to this //return Transform(mPosition + mOrientation * transform2.mPosition, @@ -239,17 +239,17 @@ inline Transform Transform::operator*(const Transform& transform2) const { } // Return true if the two transforms are equal -inline bool Transform::operator==(const Transform& transform2) const { +RP3D_FORCE_INLINE bool Transform::operator==(const Transform& transform2) const { return (mPosition == transform2.mPosition) && (mOrientation == transform2.mOrientation); } // Return true if the two transforms are different -inline bool Transform::operator!=(const Transform& transform2) const { +RP3D_FORCE_INLINE bool Transform::operator!=(const Transform& transform2) const { return !(*this == transform2); } // Assignment operator -inline Transform& Transform::operator=(const Transform& transform) { +RP3D_FORCE_INLINE Transform& Transform::operator=(const Transform& transform) { if (&transform != this) { mPosition = transform.mPosition; mOrientation = transform.mOrientation; @@ -258,7 +258,7 @@ inline Transform& Transform::operator=(const Transform& transform) { } // Get the string representation -inline std::string Transform::to_string() const { +RP3D_FORCE_INLINE std::string Transform::to_string() const { return "Transform(" + mPosition.to_string() + "," + mOrientation.to_string() + ")"; } diff --git a/include/reactphysics3d/mathematics/Vector2.h b/include/reactphysics3d/mathematics/Vector2.h index 7a088398..a19598a6 100644 --- a/include/reactphysics3d/mathematics/Vector2.h +++ b/include/reactphysics3d/mathematics/Vector2.h @@ -148,6 +148,9 @@ struct Vector2 { /// Return the zero vector static Vector2 zero(); + /// Function to test if two vectors are (almost) equal + static bool approxEqual(const Vector2& vec1, const Vector2& vec2, decimal epsilon = MACHINE_EPSILON); + // -------------------- Friends -------------------- // friend Vector2 operator+(const Vector2& vector1, const Vector2& vector2); @@ -161,50 +164,50 @@ struct Vector2 { }; // Constructor -inline Vector2::Vector2() : x(0.0), y(0.0) { +RP3D_FORCE_INLINE Vector2::Vector2() : x(0.0), y(0.0) { } // Constructor with arguments -inline Vector2::Vector2(decimal newX, decimal newY) : x(newX), y(newY) { +RP3D_FORCE_INLINE Vector2::Vector2(decimal newX, decimal newY) : x(newX), y(newY) { } // Copy-constructor -inline Vector2::Vector2(const Vector2& vector) : x(vector.x), y(vector.y) { +RP3D_FORCE_INLINE Vector2::Vector2(const Vector2& vector) : x(vector.x), y(vector.y) { } // Set the vector to zero -inline void Vector2::setToZero() { +RP3D_FORCE_INLINE void Vector2::setToZero() { x = 0; y = 0; } // Set all the values of the vector -inline void Vector2::setAllValues(decimal newX, decimal newY) { +RP3D_FORCE_INLINE void Vector2::setAllValues(decimal newX, decimal newY) { x = newX; y = newY; } // Return the length of the vector -inline decimal Vector2::length() const { +RP3D_FORCE_INLINE decimal Vector2::length() const { return std::sqrt(x*x + y*y); } // Return the square of the length of the vector -inline decimal Vector2::lengthSquare() const { +RP3D_FORCE_INLINE decimal Vector2::lengthSquare() const { return x*x + y*y; } -// Scalar product of two vectors (inline) -inline decimal Vector2::dot(const Vector2& vector) const { +// Scalar product of two vectors (RP3D_FORCE_INLINE) +RP3D_FORCE_INLINE decimal Vector2::dot(const Vector2& vector) const { return (x*vector.x + y*vector.y); } // Normalize the vector -inline void Vector2::normalize() { +RP3D_FORCE_INLINE void Vector2::normalize() { decimal l = length(); if (l < MACHINE_EPSILON) { return; @@ -214,68 +217,68 @@ inline void Vector2::normalize() { } // Return the corresponding absolute value vector -inline Vector2 Vector2::getAbsoluteVector() const { +RP3D_FORCE_INLINE Vector2 Vector2::getAbsoluteVector() const { return Vector2(std::abs(x), std::abs(y)); } // Return the axis with the minimal value -inline int Vector2::getMinAxis() const { +RP3D_FORCE_INLINE int Vector2::getMinAxis() const { return (x < y ? 0 : 1); } // Return the axis with the maximal value -inline int Vector2::getMaxAxis() const { +RP3D_FORCE_INLINE int Vector2::getMaxAxis() const { return (x < y ? 1 : 0); } // Return true if the vector is unit and false otherwise -inline bool Vector2::isUnit() const { - return approxEqual(lengthSquare(), 1.0); +RP3D_FORCE_INLINE bool Vector2::isUnit() const { + return reactphysics3d::approxEqual(lengthSquare(), decimal(1.0)); } // Return true if the values are not NAN OR INF -inline bool Vector2::isFinite() const { +RP3D_FORCE_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); +RP3D_FORCE_INLINE bool Vector2::isZero() const { + return reactphysics3d::approxEqual(lengthSquare(), decimal(0.0)); } // Overloaded operator for the equality condition -inline bool Vector2::operator== (const Vector2& vector) const { +RP3D_FORCE_INLINE bool Vector2::operator== (const Vector2& vector) const { return (x == vector.x && y == vector.y); } // Overloaded operator for the is different condition -inline bool Vector2::operator!= (const Vector2& vector) const { +RP3D_FORCE_INLINE bool Vector2::operator!= (const Vector2& vector) const { return !(*this == vector); } // Overloaded operator for addition with assignment -inline Vector2& Vector2::operator+=(const Vector2& vector) { +RP3D_FORCE_INLINE Vector2& Vector2::operator+=(const Vector2& vector) { x += vector.x; y += vector.y; return *this; } // Overloaded operator for substraction with assignment -inline Vector2& Vector2::operator-=(const Vector2& vector) { +RP3D_FORCE_INLINE Vector2& Vector2::operator-=(const Vector2& vector) { x -= vector.x; y -= vector.y; return *this; } // Overloaded operator for multiplication with a number with assignment -inline Vector2& Vector2::operator*=(decimal number) { +RP3D_FORCE_INLINE Vector2& Vector2::operator*=(decimal number) { x *= number; y *= number; return *this; } // Overloaded operator for division by a number with assignment -inline Vector2& Vector2::operator/=(decimal number) { +RP3D_FORCE_INLINE Vector2& Vector2::operator/=(decimal number) { assert(number > std::numeric_limits::epsilon()); x /= number; y /= number; @@ -283,60 +286,60 @@ inline Vector2& Vector2::operator/=(decimal number) { } // Overloaded operator for value access -inline decimal& Vector2::operator[] (int index) { +RP3D_FORCE_INLINE decimal& Vector2::operator[] (int index) { return (&x)[index]; } // Overloaded operator for value access -inline const decimal& Vector2::operator[] (int index) const { +RP3D_FORCE_INLINE const decimal& Vector2::operator[] (int index) const { return (&x)[index]; } // Overloaded operator for addition -inline Vector2 operator+(const Vector2& vector1, const Vector2& vector2) { +RP3D_FORCE_INLINE Vector2 operator+(const Vector2& vector1, const Vector2& vector2) { return Vector2(vector1.x + vector2.x, vector1.y + vector2.y); } // Overloaded operator for substraction -inline Vector2 operator-(const Vector2& vector1, const Vector2& vector2) { +RP3D_FORCE_INLINE Vector2 operator-(const Vector2& vector1, const Vector2& vector2) { return Vector2(vector1.x - vector2.x, vector1.y - vector2.y); } // Overloaded operator for the negative of a vector -inline Vector2 operator-(const Vector2& vector) { +RP3D_FORCE_INLINE Vector2 operator-(const Vector2& vector) { return Vector2(-vector.x, -vector.y); } // Overloaded operator for multiplication with a number -inline Vector2 operator*(const Vector2& vector, decimal number) { +RP3D_FORCE_INLINE Vector2 operator*(const Vector2& vector, decimal number) { return Vector2(number * vector.x, number * vector.y); } // Overloaded operator for multiplication of two vectors -inline Vector2 operator*(const Vector2& vector1, const Vector2& vector2) { +RP3D_FORCE_INLINE Vector2 operator*(const Vector2& vector1, const Vector2& vector2) { return Vector2(vector1.x * vector2.x, vector1.y * vector2.y); } // Overloaded operator for division by a number -inline Vector2 operator/(const Vector2& vector, decimal number) { +RP3D_FORCE_INLINE Vector2 operator/(const Vector2& vector, decimal number) { assert(number > MACHINE_EPSILON); return Vector2(vector.x / number, vector.y / number); } // Overload operator for division between two vectors -inline Vector2 operator/(const Vector2& vector1, const Vector2& vector2) { +RP3D_FORCE_INLINE Vector2 operator/(const Vector2& vector1, const Vector2& vector2) { assert(vector2.x > MACHINE_EPSILON); assert(vector2.y > MACHINE_EPSILON); return Vector2(vector1.x / vector2.x, vector1.y / vector2.y); } // Overloaded operator for multiplication with a number -inline Vector2 operator*(decimal number, const Vector2& vector) { +RP3D_FORCE_INLINE Vector2 operator*(decimal number, const Vector2& vector) { return vector * number; } // Assignment operator -inline Vector2& Vector2::operator=(const Vector2& vector) { +RP3D_FORCE_INLINE Vector2& Vector2::operator=(const Vector2& vector) { if (&vector != this) { x = vector.x; y = vector.y; @@ -345,32 +348,37 @@ inline Vector2& Vector2::operator=(const Vector2& vector) { } // Overloaded less than operator for ordering to be used inside std::set for instance -inline bool Vector2::operator<(const Vector2& vector) const { +RP3D_FORCE_INLINE bool Vector2::operator<(const Vector2& vector) const { return (x == vector.x ? y < vector.y : x < vector.x); } // Return a vector taking the minimum components of two vectors -inline Vector2 Vector2::min(const Vector2& vector1, const Vector2& vector2) { +RP3D_FORCE_INLINE Vector2 Vector2::min(const Vector2& vector1, const Vector2& vector2) { return Vector2(std::min(vector1.x, vector2.x), std::min(vector1.y, vector2.y)); } // Return a vector taking the maximum components of two vectors -inline Vector2 Vector2::max(const Vector2& vector1, const Vector2& vector2) { +RP3D_FORCE_INLINE Vector2 Vector2::max(const Vector2& vector1, const Vector2& vector2) { return Vector2(std::max(vector1.x, vector2.x), std::max(vector1.y, vector2.y)); } // Get the string representation -inline std::string Vector2::to_string() const { +RP3D_FORCE_INLINE std::string Vector2::to_string() const { return "Vector2(" + std::to_string(x) + "," + std::to_string(y) + ")"; } // Return the zero vector -inline Vector2 Vector2::zero() { +RP3D_FORCE_INLINE Vector2 Vector2::zero() { return Vector2(0, 0); } +// Function to test if two vectors are (almost) equal +RP3D_FORCE_INLINE bool approxEqual(const Vector2& vec1, const Vector2& vec2, decimal epsilon) { + return approxEqual(vec1.x, vec2.x, epsilon) && approxEqual(vec1.y, vec2.y, epsilon); +} + } #endif diff --git a/include/reactphysics3d/mathematics/Vector3.h b/include/reactphysics3d/mathematics/Vector3.h index ffec1b56..79b47df9 100644 --- a/include/reactphysics3d/mathematics/Vector3.h +++ b/include/reactphysics3d/mathematics/Vector3.h @@ -28,8 +28,11 @@ // Libraries #include -#include +#include +#include #include +#include +#include /// ReactPhysics3D namespace namespace reactphysics3d { @@ -160,6 +163,9 @@ struct Vector3 { /// Return the zero vector static Vector3 zero(); + /// Function to test if two vectors are (almost) equal + static bool approxEqual(const Vector3& vec1, const Vector3& vec2, decimal epsilon = MACHINE_EPSILON); + // -------------------- Friends -------------------- // friend Vector3 operator+(const Vector3& vector1, const Vector3& vector2); @@ -173,58 +179,58 @@ struct Vector3 { }; // Constructor of the struct Vector3 -inline Vector3::Vector3() : x(0.0), y(0.0), z(0.0) { +RP3D_FORCE_INLINE Vector3::Vector3() : x(0.0), y(0.0), z(0.0) { } // Constructor with arguments -inline Vector3::Vector3(decimal newX, decimal newY, decimal newZ) : x(newX), y(newY), z(newZ) { +RP3D_FORCE_INLINE Vector3::Vector3(decimal newX, decimal newY, decimal newZ) : x(newX), y(newY), z(newZ) { } // Copy-constructor -inline Vector3::Vector3(const Vector3& vector) : x(vector.x), y(vector.y), z(vector.z) { +RP3D_FORCE_INLINE Vector3::Vector3(const Vector3& vector) : x(vector.x), y(vector.y), z(vector.z) { } // Set the vector to zero -inline void Vector3::setToZero() { +RP3D_FORCE_INLINE void Vector3::setToZero() { x = 0; y = 0; z = 0; } // Set all the values of the vector -inline void Vector3::setAllValues(decimal newX, decimal newY, decimal newZ) { +RP3D_FORCE_INLINE void Vector3::setAllValues(decimal newX, decimal newY, decimal newZ) { x = newX; y = newY; z = newZ; } // Return the length of the vector -inline decimal Vector3::length() const { +RP3D_FORCE_INLINE decimal Vector3::length() const { return std::sqrt(x*x + y*y + z*z); } // Return the square of the length of the vector -inline decimal Vector3::lengthSquare() const { +RP3D_FORCE_INLINE decimal Vector3::lengthSquare() const { return x*x + y*y + z*z; } -// Scalar product of two vectors (inline) -inline decimal Vector3::dot(const Vector3& vector) const { +// Scalar product of two vectors (RP3D_FORCE_INLINE) +RP3D_FORCE_INLINE decimal Vector3::dot(const Vector3& vector) const { return (x*vector.x + y*vector.y + z*vector.z); } -// Cross product of two vectors (inline) -inline Vector3 Vector3::cross(const Vector3& vector) const { +// Cross product of two vectors (RP3D_FORCE_INLINE) +RP3D_FORCE_INLINE Vector3 Vector3::cross(const Vector3& vector) const { return Vector3(y * vector.z - z * vector.y, z * vector.x - x * vector.z, x * vector.y - y * vector.x); } // Normalize the vector -inline void Vector3::normalize() { +RP3D_FORCE_INLINE void Vector3::normalize() { decimal l = length(); if (l < MACHINE_EPSILON) { return; @@ -235,47 +241,47 @@ inline void Vector3::normalize() { } // Return the corresponding absolute value vector -inline Vector3 Vector3::getAbsoluteVector() const { +RP3D_FORCE_INLINE Vector3 Vector3::getAbsoluteVector() const { return Vector3(std::abs(x), std::abs(y), std::abs(z)); } // Return the axis with the minimal value -inline int Vector3::getMinAxis() const { +RP3D_FORCE_INLINE int Vector3::getMinAxis() const { return (x < y ? (x < z ? 0 : 2) : (y < z ? 1 : 2)); } // Return the axis with the maximal value -inline int Vector3::getMaxAxis() const { +RP3D_FORCE_INLINE int Vector3::getMaxAxis() const { return (x < y ? (y < z ? 2 : 1) : (x < z ? 2 : 0)); } // Return true if the vector is unit and false otherwise -inline bool Vector3::isUnit() const { - return approxEqual(lengthSquare(), 1.0); +RP3D_FORCE_INLINE bool Vector3::isUnit() const { + return reactphysics3d::approxEqual(lengthSquare(), decimal(1.0)); } // Return true if the values are not NAN OR INF -inline bool Vector3::isFinite() const { +RP3D_FORCE_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); +RP3D_FORCE_INLINE bool Vector3::isZero() const { + return reactphysics3d::approxEqual(lengthSquare(), decimal(0.0)); } // Overloaded operator for the equality condition -inline bool Vector3::operator== (const Vector3& vector) const { +RP3D_FORCE_INLINE bool Vector3::operator== (const Vector3& vector) const { return (x == vector.x && y == vector.y && z == vector.z); } // Overloaded operator for the is different condition -inline bool Vector3::operator!= (const Vector3& vector) const { +RP3D_FORCE_INLINE bool Vector3::operator!= (const Vector3& vector) const { return !(*this == vector); } // Overloaded operator for addition with assignment -inline Vector3& Vector3::operator+=(const Vector3& vector) { +RP3D_FORCE_INLINE Vector3& Vector3::operator+=(const Vector3& vector) { x += vector.x; y += vector.y; z += vector.z; @@ -283,7 +289,7 @@ inline Vector3& Vector3::operator+=(const Vector3& vector) { } // Overloaded operator for substraction with assignment -inline Vector3& Vector3::operator-=(const Vector3& vector) { +RP3D_FORCE_INLINE Vector3& Vector3::operator-=(const Vector3& vector) { x -= vector.x; y -= vector.y; z -= vector.z; @@ -291,7 +297,7 @@ inline Vector3& Vector3::operator-=(const Vector3& vector) { } // Overloaded operator for multiplication with a number with assignment -inline Vector3& Vector3::operator*=(decimal number) { +RP3D_FORCE_INLINE Vector3& Vector3::operator*=(decimal number) { x *= number; y *= number; z *= number; @@ -299,7 +305,7 @@ inline Vector3& Vector3::operator*=(decimal number) { } // Overloaded operator for division by a number with assignment -inline Vector3& Vector3::operator/=(decimal number) { +RP3D_FORCE_INLINE Vector3& Vector3::operator/=(decimal number) { assert(number > std::numeric_limits::epsilon()); x /= number; y /= number; @@ -308,43 +314,43 @@ inline Vector3& Vector3::operator/=(decimal number) { } // Overloaded operator for value access -inline decimal& Vector3::operator[] (int index) { +RP3D_FORCE_INLINE decimal& Vector3::operator[] (int index) { return (&x)[index]; } // Overloaded operator for value access -inline const decimal& Vector3::operator[] (int index) const { +RP3D_FORCE_INLINE const decimal& Vector3::operator[] (int index) const { return (&x)[index]; } // Overloaded operator for addition -inline Vector3 operator+(const Vector3& vector1, const Vector3& vector2) { +RP3D_FORCE_INLINE Vector3 operator+(const Vector3& vector1, const Vector3& vector2) { return Vector3(vector1.x + vector2.x, vector1.y + vector2.y, vector1.z + vector2.z); } // Overloaded operator for substraction -inline Vector3 operator-(const Vector3& vector1, const Vector3& vector2) { +RP3D_FORCE_INLINE Vector3 operator-(const Vector3& vector1, const Vector3& vector2) { return Vector3(vector1.x - vector2.x, vector1.y - vector2.y, vector1.z - vector2.z); } // Overloaded operator for the negative of a vector -inline Vector3 operator-(const Vector3& vector) { +RP3D_FORCE_INLINE Vector3 operator-(const Vector3& vector) { return Vector3(-vector.x, -vector.y, -vector.z); } // Overloaded operator for multiplication with a number -inline Vector3 operator*(const Vector3& vector, decimal number) { +RP3D_FORCE_INLINE Vector3 operator*(const Vector3& vector, decimal number) { return Vector3(number * vector.x, number * vector.y, number * vector.z); } // Overloaded operator for division by a number -inline Vector3 operator/(const Vector3& vector, decimal number) { +RP3D_FORCE_INLINE Vector3 operator/(const Vector3& vector, decimal number) { assert(number > MACHINE_EPSILON); return Vector3(vector.x / number, vector.y / number, vector.z / number); } // Overload operator for division between two vectors -inline Vector3 operator/(const Vector3& vector1, const Vector3& vector2) { +RP3D_FORCE_INLINE Vector3 operator/(const Vector3& vector1, const Vector3& vector2) { assert(vector2.x > MACHINE_EPSILON); assert(vector2.y > MACHINE_EPSILON); assert(vector2.z > MACHINE_EPSILON); @@ -352,17 +358,17 @@ inline Vector3 operator/(const Vector3& vector1, const Vector3& vector2) { } // Overloaded operator for multiplication with a number -inline Vector3 operator*(decimal number, const Vector3& vector) { +RP3D_FORCE_INLINE Vector3 operator*(decimal number, const Vector3& vector) { return vector * number; } // Overload operator for multiplication between two vectors -inline Vector3 operator*(const Vector3& vector1, const Vector3& vector2) { +RP3D_FORCE_INLINE Vector3 operator*(const Vector3& vector1, const Vector3& vector2) { return Vector3(vector1.x * vector2.x, vector1.y * vector2.y, vector1.z * vector2.z); } // Assignment operator -inline Vector3& Vector3::operator=(const Vector3& vector) { +RP3D_FORCE_INLINE Vector3& Vector3::operator=(const Vector3& vector) { if (&vector != this) { x = vector.x; y = vector.y; @@ -372,44 +378,50 @@ inline Vector3& Vector3::operator=(const Vector3& vector) { } // Overloaded less than operator for ordering to be used inside std::set for instance -inline bool Vector3::operator<(const Vector3& vector) const { +RP3D_FORCE_INLINE bool Vector3::operator<(const Vector3& vector) const { return (x == vector.x ? (y == vector.y ? z < vector.z : y < vector.y) : x < vector.x); } // Return a vector taking the minimum components of two vectors -inline Vector3 Vector3::min(const Vector3& vector1, const Vector3& vector2) { +RP3D_FORCE_INLINE Vector3 Vector3::min(const Vector3& vector1, const Vector3& vector2) { return Vector3(std::min(vector1.x, vector2.x), std::min(vector1.y, vector2.y), std::min(vector1.z, vector2.z)); } // Return a vector taking the maximum components of two vectors -inline Vector3 Vector3::max(const Vector3& vector1, const Vector3& vector2) { +RP3D_FORCE_INLINE Vector3 Vector3::max(const Vector3& vector1, const Vector3& vector2) { return Vector3(std::max(vector1.x, vector2.x), std::max(vector1.y, vector2.y), std::max(vector1.z, vector2.z)); } // Return the minimum value among the three components of a vector -inline decimal Vector3::getMinValue() const { +RP3D_FORCE_INLINE decimal Vector3::getMinValue() const { return std::min(std::min(x, y), z); } // Return the maximum value among the three components of a vector -inline decimal Vector3::getMaxValue() const { +RP3D_FORCE_INLINE decimal Vector3::getMaxValue() const { return std::max(std::max(x, y), z); } // Get the string representation -inline std::string Vector3::to_string() const { +RP3D_FORCE_INLINE std::string Vector3::to_string() const { return "Vector3(" + std::to_string(x) + "," + std::to_string(y) + "," + std::to_string(z) + ")"; } // Return the zero vector -inline Vector3 Vector3::zero() { +RP3D_FORCE_INLINE Vector3 Vector3::zero() { return Vector3(0, 0, 0); } +// Function to test if two vectors are (almost) equal +RP3D_FORCE_INLINE bool approxEqual(const Vector3& vec1, const Vector3& vec2, decimal epsilon) { + return approxEqual(vec1.x, vec2.x, epsilon) && approxEqual(vec1.y, vec2.y, epsilon) && + approxEqual(vec1.z, vec2.z, epsilon); +} + } #endif diff --git a/include/reactphysics3d/collision/narrowphase/SphereVsSphereNarrowPhaseInfoBatch.h b/include/reactphysics3d/mathematics/mathematics_common.h similarity index 54% rename from include/reactphysics3d/collision/narrowphase/SphereVsSphereNarrowPhaseInfoBatch.h rename to include/reactphysics3d/mathematics/mathematics_common.h index 63792c71..8c1ef6f5 100644 --- a/include/reactphysics3d/collision/narrowphase/SphereVsSphereNarrowPhaseInfoBatch.h +++ b/include/reactphysics3d/mathematics/mathematics_common.h @@ -23,50 +23,28 @@ * * ********************************************************************************/ -#ifndef REACTPHYSICS3D_SPHERE_VS_SPHERE_NARROW_PHASE_INFO_BATCH_H -#define REACTPHYSICS3D_SPHERE_VS_SPHERE_NARROW_PHASE_INFO_BATCH_H +#ifndef REACTPHYSICS3D_MATHEMATICS_COMMON_H +#define REACTPHYSICS3D_MATHEMATICS_COMMON_H // Libraries -#include +#include +#include +#include +#include -/// Namespace ReactPhysics3D +/// ReactPhysics3D namespace namespace reactphysics3d { -// Struct SphereVsSphereNarrowPhaseInfoBatch -/** - * This structure collects all the potential collisions from the middle-phase algorithm - * that have to be tested during narrow-phase collision detection. This class collects all the - * sphere vs sphere collision detection tests. - */ -struct SphereVsSphereNarrowPhaseInfoBatch : public NarrowPhaseInfoBatch { +// ---------- Mathematics functions ---------- // - public: +/// Function to test if two real numbers are (almost) equal +/// We test if two numbers a and b are such that (a-b) are in [-EPSILON; EPSILON] +RP3D_FORCE_INLINE bool approxEqual(decimal a, decimal b, decimal epsilon = MACHINE_EPSILON) { + return (std::abs(a - b) < epsilon); +} - /// List of radiuses for the first spheres - List sphere1Radiuses; - - /// List of radiuses for the second spheres - List sphere2Radiuses; - - /// Constructor - SphereVsSphereNarrowPhaseInfoBatch(MemoryAllocator& allocator, OverlappingPairs& overlappingPairs); - - /// Destructor - virtual ~SphereVsSphereNarrowPhaseInfoBatch() override = default; - - /// Add shapes to be tested during narrow-phase collision detection into the batch - virtual void addNarrowPhaseInfo(uint64 airId, uint64 pairIndex, Entity collider1, Entity collider2, CollisionShape* shape1, - CollisionShape* shape2, const Transform& shape1Transform, - const Transform& shape2Transform, bool needToReportContacts, MemoryAllocator& shapeAllocator) override; - - // Initialize the containers using cached capacity - virtual void reserveMemory() override; - - /// Clear all the objects in the batch - virtual void clear() override; -}; } -#endif +#endif diff --git a/include/reactphysics3d/mathematics/mathematics_functions.h b/include/reactphysics3d/mathematics/mathematics_functions.h index 9ef9ff3a..a163712a 100755 --- a/include/reactphysics3d/mathematics/mathematics_functions.h +++ b/include/reactphysics3d/mathematics/mathematics_functions.h @@ -29,10 +29,11 @@ // Libraries #include #include +#include #include #include #include -#include +#include /// ReactPhysics3D namespace namespace reactphysics3d { @@ -42,98 +43,392 @@ struct Vector2; // ---------- Mathematics functions ---------- // -/// Function to test if two real numbers are (almost) equal -/// We test if two numbers a and b are such that (a-b) are in [-EPSILON; EPSILON] -inline bool approxEqual(decimal a, decimal b, decimal epsilon = MACHINE_EPSILON) { - return (std::fabs(a - b) < epsilon); -} - -/// Function to test if two vectors are (almost) equal -bool approxEqual(const Vector3& vec1, const Vector3& vec2, decimal epsilon = MACHINE_EPSILON); - -/// Function to test if two vectors are (almost) equal -bool approxEqual(const Vector2& vec1, const Vector2& vec2, decimal epsilon = MACHINE_EPSILON); - /// Function that returns the result of the "value" clamped by /// two others values "lowerLimit" and "upperLimit" -inline int clamp(int value, int lowerLimit, int upperLimit) { +RP3D_FORCE_INLINE int clamp(int value, int lowerLimit, int upperLimit) { assert(lowerLimit <= upperLimit); return std::min(std::max(value, lowerLimit), upperLimit); } /// Function that returns the result of the "value" clamped by /// two others values "lowerLimit" and "upperLimit" -inline decimal clamp(decimal value, decimal lowerLimit, decimal upperLimit) { +RP3D_FORCE_INLINE decimal clamp(decimal value, decimal lowerLimit, decimal upperLimit) { assert(lowerLimit <= upperLimit); return std::min(std::max(value, lowerLimit), upperLimit); } /// Return the minimum value among three values -inline decimal min3(decimal a, decimal b, decimal c) { +RP3D_FORCE_INLINE decimal min3(decimal a, decimal b, decimal c) { return std::min(std::min(a, b), c); } /// Return the maximum value among three values -inline decimal max3(decimal a, decimal b, decimal c) { +RP3D_FORCE_INLINE decimal max3(decimal a, decimal b, decimal c) { return std::max(std::max(a, b), c); } /// Return true if two values have the same sign -inline bool sameSign(decimal a, decimal b) { +RP3D_FORCE_INLINE bool sameSign(decimal a, decimal b) { return a * b >= decimal(0.0); } -/// Return true if two vectors are parallel -bool areParallelVectors(const Vector3& vector1, const Vector3& vector2); +// Return true if two vectors are parallel +RP3D_FORCE_INLINE bool areParallelVectors(const Vector3& vector1, const Vector3& vector2) { + return vector1.cross(vector2).lengthSquare() < decimal(0.00001); +} -/// Return true if two vectors are orthogonal -bool areOrthogonalVectors(const Vector3& vector1, const Vector3& vector2); -/// Clamp a vector such that it is no longer than a given maximum length -Vector3 clamp(const Vector3& vector, decimal maxLength); +// Return true if two vectors are orthogonal +RP3D_FORCE_INLINE bool areOrthogonalVectors(const Vector3& vector1, const Vector3& vector2) { + return std::abs(vector1.dot(vector2)) < decimal(0.001); +} + + +// Clamp a vector such that it is no longer than a given maximum length +RP3D_FORCE_INLINE Vector3 clamp(const Vector3& vector, decimal maxLength) { + if (vector.lengthSquare() > maxLength * maxLength) { + return vector.getUnit() * maxLength; + } + return vector; +} // Compute and return a point on segment from "segPointA" and "segPointB" that is closest to point "pointC" -Vector3 computeClosestPointOnSegment(const Vector3& segPointA, const Vector3& segPointB, const Vector3& pointC); +RP3D_FORCE_INLINE Vector3 computeClosestPointOnSegment(const Vector3& segPointA, const Vector3& segPointB, const Vector3& pointC) { + + const Vector3 ab = segPointB - segPointA; + + decimal abLengthSquare = ab.lengthSquare(); + + // If the segment has almost zero length + if (abLengthSquare < MACHINE_EPSILON) { + + // Return one end-point of the segment as the closest point + return segPointA; + } + + // Project point C onto "AB" line + decimal t = (pointC - segPointA).dot(ab) / abLengthSquare; + + // If projected point onto the line is outside the segment, clamp it to the segment + if (t < decimal(0.0)) t = decimal(0.0); + if (t > decimal(1.0)) t = decimal(1.0); + + // Return the closest point on the segment + return segPointA + t * ab; +} // Compute the closest points between two segments -void computeClosestPointBetweenTwoSegments(const Vector3& seg1PointA, const Vector3& seg1PointB, - const Vector3& seg2PointA, const Vector3& seg2PointB, - Vector3& closestPointSeg1, Vector3& closestPointSeg2); +// This method uses the technique described in the book Real-Time +// collision detection by Christer Ericson. +RP3D_FORCE_INLINE void computeClosestPointBetweenTwoSegments(const Vector3& seg1PointA, const Vector3& seg1PointB, + const Vector3& seg2PointA, const Vector3& seg2PointB, + Vector3& closestPointSeg1, Vector3& closestPointSeg2) { -/// Compute the barycentric coordinates u, v, w of a point p inside the triangle (a, b, c) -void computeBarycentricCoordinatesInTriangle(const Vector3& a, const Vector3& b, const Vector3& c, - const Vector3& p, decimal& u, decimal& v, decimal& w); + const Vector3 d1 = seg1PointB - seg1PointA; + const Vector3 d2 = seg2PointB - seg2PointA; + const Vector3 r = seg1PointA - seg2PointA; + decimal a = d1.lengthSquare(); + decimal e = d2.lengthSquare(); + decimal f = d2.dot(r); + decimal s, t; -/// Compute the intersection between a plane and a segment -decimal computePlaneSegmentIntersection(const Vector3& segA, const Vector3& segB, const decimal planeD, const Vector3& planeNormal); + // If both segments degenerate into points + if (a <= MACHINE_EPSILON && e <= MACHINE_EPSILON) { -/// Compute the distance between a point and a line -decimal computePointToLineDistance(const Vector3& linePointA, const Vector3& linePointB, const Vector3& point); + closestPointSeg1 = seg1PointA; + closestPointSeg2 = seg2PointA; + return; + } + if (a <= MACHINE_EPSILON) { // If first segment degenerates into a point -/// Clip a segment against multiple planes and return the clipped segment vertices -List clipSegmentWithPlanes(const Vector3& segA, const Vector3& segB, - const List& planesPoints, - const List& planesNormals, - MemoryAllocator& allocator); + s = decimal(0.0); -/// Clip a polygon against multiple planes and return the clipped polygon vertices -List clipPolygonWithPlanes(const List& polygonVertices, const List& planesPoints, - const List& planesNormals, MemoryAllocator& allocator); + // Compute the closest point on second segment + t = clamp(f / e, decimal(0.0), decimal(1.0)); + } + else { -/// Project a point onto a plane that is given by a point and its unit length normal -Vector3 projectPointOntoPlane(const Vector3& point, const Vector3& planeNormal, const Vector3& planePoint); + decimal c = d1.dot(r); -/// Return the distance between a point and a plane (the plane normal must be normalized) -decimal computePointToPlaneDistance(const Vector3& point, const Vector3& planeNormal, const Vector3& planePoint); + // If the second segment degenerates into a point + if (e <= MACHINE_EPSILON) { -/// Return true if the given number is prime -bool isPrimeNumber(int number); + t = decimal(0.0); + s = clamp(-c / a, decimal(0.0), decimal(1.0)); + } + else { + + decimal b = d1.dot(d2); + decimal denom = a * e - b * b; + + // If the segments are not parallel + if (denom != decimal(0.0)) { + + // Compute the closest point on line 1 to line 2 and + // clamp to first segment. + s = clamp((b * f - c * e) / denom, decimal(0.0), decimal(1.0)); + } + else { + + // Pick an arbitrary point on first segment + s = decimal(0.0); + } + + // Compute the point on line 2 closest to the closest point + // we have just found + t = (b * s + f) / e; + + // If this closest point is inside second segment (t in [0, 1]), we are done. + // Otherwise, we clamp the point to the second segment and compute again the + // closest point on segment 1 + if (t < decimal(0.0)) { + t = decimal(0.0); + s = clamp(-c / a, decimal(0.0), decimal(1.0)); + } + else if (t > decimal(1.0)) { + t = decimal(1.0); + s = clamp((b - c) / a, decimal(0.0), decimal(1.0)); + } + } + } + + // Compute the closest points on both segments + closestPointSeg1 = seg1PointA + d1 * s; + closestPointSeg2 = seg2PointA + d2 * t; +} + +// Compute the barycentric coordinates u, v, w of a point p inside the triangle (a, b, c) +// This method uses the technique described in the book Real-Time collision detection by +// Christer Ericson. +RP3D_FORCE_INLINE void computeBarycentricCoordinatesInTriangle(const Vector3& a, const Vector3& b, const Vector3& c, + const Vector3& p, decimal& u, decimal& v, decimal& w) { + const Vector3 v0 = b - a; + const Vector3 v1 = c - a; + const Vector3 v2 = p - a; + + const decimal d00 = v0.dot(v0); + const decimal d01 = v0.dot(v1); + const decimal d11 = v1.dot(v1); + const decimal d20 = v2.dot(v0); + const decimal d21 = v2.dot(v1); + + const decimal denom = d00 * d11 - d01 * d01; + v = (d11 * d20 - d01 * d21) / denom; + w = (d00 * d21 - d01 * d20) / denom; + u = decimal(1.0) - v - w; +} + +// Compute the intersection between a plane and a segment +// Let the plane define by the equation planeNormal.dot(X) = planeD with X a point on the plane and "planeNormal" the plane normal. This method +// computes the intersection P between the plane and the segment (segA, segB). The method returns the value "t" such +// that P = segA + t * (segB - segA). Note that it only returns a value in [0, 1] if there is an intersection. Otherwise, +// there is no intersection between the plane and the segment. +RP3D_FORCE_INLINE decimal computePlaneSegmentIntersection(const Vector3& segA, const Vector3& segB, const decimal planeD, const Vector3& planeNormal) { + + const decimal parallelEpsilon = decimal(0.0001); + decimal t = decimal(-1); + + const decimal nDotAB = planeNormal.dot(segB - segA); + + // If the segment is not parallel to the plane + if (std::abs(nDotAB) > parallelEpsilon) { + t = (planeD - planeNormal.dot(segA)) / nDotAB; + } + + return t; +} + +// Compute the distance between a point "point" and a line given by the points "linePointA" and "linePointB" +RP3D_FORCE_INLINE decimal computePointToLineDistance(const Vector3& linePointA, const Vector3& linePointB, const Vector3& point) { + + decimal distAB = (linePointB - linePointA).length(); + + if (distAB < MACHINE_EPSILON) { + return (point - linePointA).length(); + } + + return ((point - linePointA).cross(point - linePointB)).length() / distAB; +} + + +// Clip a segment against multiple planes and return the clipped segment vertices +// This method implements the Sutherland–Hodgman clipping algorithm +RP3D_FORCE_INLINE Array clipSegmentWithPlanes(const Vector3& segA, const Vector3& segB, + const Array& planesPoints, + const Array& planesNormals, + MemoryAllocator& allocator) { + assert(planesPoints.size() == planesNormals.size()); + + Array inputVertices(allocator, 2); + Array outputVertices(allocator, 2); + + inputVertices.add(segA); + inputVertices.add(segB); + + // For each clipping plane + const uint32 nbPlanesPoints = planesPoints.size(); + for (uint32 p=0; p < nbPlanesPoints; p++) { + + // If there is no more vertices, stop + if (inputVertices.size() == 0) return inputVertices; + + assert(inputVertices.size() == 2); + + outputVertices.clear(); + + Vector3& v1 = inputVertices[0]; + Vector3& v2 = inputVertices[1]; + + decimal v1DotN = (v1 - planesPoints[p]).dot(planesNormals[p]); + decimal v2DotN = (v2 - planesPoints[p]).dot(planesNormals[p]); + + // If the second vertex is in front of the clippling plane + if (v2DotN >= decimal(0.0)) { + + // If the first vertex is not in front of the clippling plane + if (v1DotN < decimal(0.0)) { + + // The second point we keep is the intersection between the segment v1, v2 and the clipping plane + decimal t = computePlaneSegmentIntersection(v1, v2, planesNormals[p].dot(planesPoints[p]), planesNormals[p]); + + if (t >= decimal(0) && t <= decimal(1.0)) { + outputVertices.add(v1 + t * (v2 - v1)); + } + else { + outputVertices.add(v2); + } + } + else { + outputVertices.add(v1); + } + + // Add the second vertex + outputVertices.add(v2); + } + else { // If the second vertex is behind the clipping plane + + // If the first vertex is in front of the clippling plane + if (v1DotN >= decimal(0.0)) { + + outputVertices.add(v1); + + // The first point we keep is the intersection between the segment v1, v2 and the clipping plane + decimal t = computePlaneSegmentIntersection(v1, v2, -planesNormals[p].dot(planesPoints[p]), -planesNormals[p]); + + if (t >= decimal(0.0) && t <= decimal(1.0)) { + outputVertices.add(v1 + t * (v2 - v1)); + } + } + } + + inputVertices = outputVertices; + } + + return outputVertices; +} + +// Clip a polygon against a single plane and return the clipped polygon vertices +// This method implements the Sutherland–Hodgman polygon clipping algorithm +RP3D_FORCE_INLINE void clipPolygonWithPlane(const Array& polygonVertices, const Vector3& planePoint, + const Vector3& planeNormal, Array& outClippedPolygonVertices) { + + uint32 nbInputVertices = polygonVertices.size(); + + assert(outClippedPolygonVertices.size() == 0); + + uint32 vStartIndex = nbInputVertices - 1; + + const decimal planeNormalDotPlanePoint = planeNormal.dot(planePoint); + + decimal vStartDotN = (polygonVertices[vStartIndex] - planePoint).dot(planeNormal); + + // For each edge of the polygon + for (uint vEndIndex = 0; vEndIndex < nbInputVertices; vEndIndex++) { + + const Vector3& vStart = polygonVertices[vStartIndex]; + const Vector3& vEnd = polygonVertices[vEndIndex]; + + const decimal vEndDotN = (vEnd - planePoint).dot(planeNormal); + + // If the second vertex is in front of the clippling plane + if (vEndDotN >= decimal(0.0)) { + + // If the first vertex is not in front of the clippling plane + if (vStartDotN < decimal(0.0)) { + + // The second point we keep is the intersection between the segment v1, v2 and the clipping plane + const decimal t = computePlaneSegmentIntersection(vStart, vEnd, planeNormalDotPlanePoint, planeNormal); + + if (t >= decimal(0) && t <= decimal(1.0)) { + outClippedPolygonVertices.add(vStart + t * (vEnd - vStart)); + } + else { + outClippedPolygonVertices.add(vEnd); + } + } + + // Add the second vertex + outClippedPolygonVertices.add(vEnd); + } + else { // If the second vertex is behind the clipping plane + + // If the first vertex is in front of the clippling plane + if (vStartDotN >= decimal(0.0)) { + + // The first point we keep is the intersection between the segment v1, v2 and the clipping plane + const decimal t = computePlaneSegmentIntersection(vStart, vEnd, -planeNormalDotPlanePoint, -planeNormal); + + if (t >= decimal(0.0) && t <= decimal(1.0)) { + outClippedPolygonVertices.add(vStart + t * (vEnd - vStart)); + } + else { + outClippedPolygonVertices.add(vStart); + } + } + } + + vStartIndex = vEndIndex; + vStartDotN = vEndDotN; + } +} + +// Project a point onto a plane that is given by a point and its unit length normal +RP3D_FORCE_INLINE Vector3 projectPointOntoPlane(const Vector3& point, const Vector3& unitPlaneNormal, const Vector3& planePoint) { + return point - unitPlaneNormal.dot(point - planePoint) * unitPlaneNormal; +} + +// Return the distance between a point and a plane (the plane normal must be normalized) +RP3D_FORCE_INLINE decimal computePointToPlaneDistance(const Vector3& point, const Vector3& planeNormal, const Vector3& planePoint) { + return planeNormal.dot(point - planePoint); +} + +/// Return true if a number is a power of two +RP3D_FORCE_INLINE bool isPowerOfTwo(uint32 number) { + return number != 0 && !(number & (number -1)); +} + +/// Return the next power of two larger than the number in parameter +RP3D_FORCE_INLINE uint32 nextPowerOfTwo32Bits(uint32 number) { + number--; + number |= number >> 1; + number |= number >> 2; + number |= number >> 4; + number |= number >> 8; + number |= number >> 16; + number++; + number += (number == 0); + return number; +} /// Return an unique integer from two integer numbers (pairing function) /// Here we assume that the two parameter numbers are sorted such that /// number1 = max(number1, number2) /// http://szudzik.com/ElegantPairing.pdf -uint64 pairNumbers(uint32 number1, uint32 number2); +RP3D_FORCE_INLINE uint64 pairNumbers(uint32 number1, uint32 number2) { + assert(number1 == std::max(number1, number2)); + return number1 * number1 + number1 + number2; +} + } diff --git a/include/reactphysics3d/memory/MemoryManager.h b/include/reactphysics3d/memory/MemoryManager.h index 55388e00..a7a5466c 100644 --- a/include/reactphysics3d/memory/MemoryManager.h +++ b/include/reactphysics3d/memory/MemoryManager.h @@ -102,7 +102,7 @@ class MemoryManager { }; // Allocate memory of a given type -inline void* MemoryManager::allocate(AllocationType allocationType, size_t size) { +RP3D_FORCE_INLINE void* MemoryManager::allocate(AllocationType allocationType, size_t size) { switch (allocationType) { case AllocationType::Base: return mBaseAllocator->allocate(size); @@ -115,7 +115,7 @@ inline void* MemoryManager::allocate(AllocationType allocationType, size_t size) } // Release previously allocated memory. -inline void MemoryManager::release(AllocationType allocationType, void* pointer, size_t size) { +RP3D_FORCE_INLINE void MemoryManager::release(AllocationType allocationType, void* pointer, size_t size) { switch (allocationType) { case AllocationType::Base: mBaseAllocator->release(pointer, size); break; @@ -126,22 +126,22 @@ inline void MemoryManager::release(AllocationType allocationType, void* pointer, } // Return the pool allocator -inline PoolAllocator& MemoryManager::getPoolAllocator() { +RP3D_FORCE_INLINE PoolAllocator& MemoryManager::getPoolAllocator() { return mPoolAllocator; } // Return the single frame stack allocator -inline SingleFrameAllocator& MemoryManager::getSingleFrameAllocator() { +RP3D_FORCE_INLINE SingleFrameAllocator& MemoryManager::getSingleFrameAllocator() { return mSingleFrameAllocator; } // Return the heap allocator -inline HeapAllocator& MemoryManager::getHeapAllocator() { +RP3D_FORCE_INLINE HeapAllocator& MemoryManager::getHeapAllocator() { return mHeapAllocator; } // Reset the single frame allocator -inline void MemoryManager::resetFrameAllocator() { +RP3D_FORCE_INLINE void MemoryManager::resetFrameAllocator() { mSingleFrameAllocator.reset(); } diff --git a/include/reactphysics3d/reactphysics3d.h b/include/reactphysics3d/reactphysics3d.h index 2b32999c..918deb73 100644 --- a/include/reactphysics3d/reactphysics3d.h +++ b/include/reactphysics3d/reactphysics3d.h @@ -64,7 +64,7 @@ #include #include #include -#include +#include /// Alias to the ReactPhysics3D namespace namespace rp3d = reactphysics3d; diff --git a/include/reactphysics3d/systems/BroadPhaseSystem.h b/include/reactphysics3d/systems/BroadPhaseSystem.h index e399619f..3cbaa4ec 100644 --- a/include/reactphysics3d/systems/BroadPhaseSystem.h +++ b/include/reactphysics3d/systems/BroadPhaseSystem.h @@ -51,10 +51,10 @@ class AABBOverlapCallback : public DynamicAABBTreeOverlapCallback { public: - List& mOverlappingNodes; + Array& mOverlappingNodes; // Constructor - AABBOverlapCallback(List& overlappingNodes) : mOverlappingNodes(overlappingNodes) { + AABBOverlapCallback(Array& overlappingNodes) : mOverlappingNodes(overlappingNodes) { } @@ -184,7 +184,7 @@ class BroadPhaseSystem { void removeMovedCollider(int broadPhaseID); /// Compute all the overlapping pairs of collision shapes - void computeOverlappingPairs(MemoryManager& memoryManager, List>& overlappingNodes); + void computeOverlappingPairs(MemoryManager& memoryManager, Array>& overlappingNodes); /// Return the collider corresponding to the broad-phase node id in parameter Collider* getColliderForBroadPhaseId(int broadPhaseId) const; @@ -208,27 +208,27 @@ class BroadPhaseSystem { }; // Return the fat AABB of a given broad-phase shape -inline const AABB& BroadPhaseSystem::getFatAABB(int broadPhaseId) const { +RP3D_FORCE_INLINE const AABB& BroadPhaseSystem::getFatAABB(int broadPhaseId) const { return mDynamicAABBTree.getFatAABB(broadPhaseId); } // Remove a collider from the array of colliders that have moved in the last simulation step // and that need to be tested again for broad-phase overlapping. -inline void BroadPhaseSystem::removeMovedCollider(int broadPhaseID) { +RP3D_FORCE_INLINE void BroadPhaseSystem::removeMovedCollider(int broadPhaseID) { // Remove the broad-phase ID from the set mMovedShapes.remove(broadPhaseID); } // Return the collider corresponding to the broad-phase node id in parameter -inline Collider* BroadPhaseSystem::getColliderForBroadPhaseId(int broadPhaseId) const { +RP3D_FORCE_INLINE Collider* BroadPhaseSystem::getColliderForBroadPhaseId(int broadPhaseId) const { return static_cast(mDynamicAABBTree.getNodeDataPointer(broadPhaseId)); } #ifdef IS_RP3D_PROFILING_ENABLED // Set the profiler -inline void BroadPhaseSystem::setProfiler(Profiler* profiler) { +RP3D_FORCE_INLINE void BroadPhaseSystem::setProfiler(Profiler* profiler) { mProfiler = profiler; mDynamicAABBTree.setProfiler(profiler); } diff --git a/include/reactphysics3d/systems/CollisionDetectionSystem.h b/include/reactphysics3d/systems/CollisionDetectionSystem.h index b2d24f39..a978fd3a 100644 --- a/include/reactphysics3d/systems/CollisionDetectionSystem.h +++ b/include/reactphysics3d/systems/CollisionDetectionSystem.h @@ -43,6 +43,7 @@ #include #include #include +#include /// ReactPhysics3D namespace namespace reactphysics3d { @@ -83,6 +84,9 @@ class CollisionDetectionSystem { /// Reference the collider components ColliderComponents& mCollidersComponents; + /// Reference to the rigid bodies components + RigidBodyComponents& mRigidBodyComponents; + /// Collision Detection Dispatch configuration CollisionDispatch mCollisionDispatch; @@ -95,6 +99,9 @@ class CollisionDetectionSystem { /// Broad-phase overlapping pairs OverlappingPairs mOverlappingPairs; + /// Overlapping nodes during broad-phase computation + Array> mBroadPhaseOverlappingNodes; + /// Broad-phase system BroadPhaseSystem mBroadPhaseSystem; @@ -104,67 +111,66 @@ class CollisionDetectionSystem { /// Narrow-phase collision detection input NarrowPhaseInput mNarrowPhaseInput; - /// List of the potential contact points - List mPotentialContactPoints; + /// Array of the potential contact points + Array mPotentialContactPoints; - /// List of the potential contact manifolds - List mPotentialContactManifolds; + /// Array of the potential contact manifolds + Array mPotentialContactManifolds; - /// First list of narrow-phase pair contacts - List mContactPairs1; + /// First array of narrow-phase pair contacts + Array mContactPairs1; - /// Second list of narrow-phase pair contacts - List mContactPairs2; + /// Second array of narrow-phase pair contacts + Array mContactPairs2; - /// Pointer to the list of contact pairs of the previous frame (either mContactPairs1 or mContactPairs2) - List* mPreviousContactPairs; + /// Pointer to the array of contact pairs of the previous frame (either mContactPairs1 or mContactPairs2) + Array* mPreviousContactPairs; - /// Pointer to the list of contact pairs of the current frame (either mContactPairs1 or mContactPairs2) - List* mCurrentContactPairs; + /// Pointer to the array of contact pairs of the current frame (either mContactPairs1 or mContactPairs2) + Array* mCurrentContactPairs; - /// List of lost contact pairs (contact pairs in contact in previous frame but not in the current one) - List mLostContactPairs; - - /// First map of overlapping pair id to the index of the corresponding pair contact - Map mMapPairIdToContactPairIndex1; - - /// Second map of overlapping pair id to the index of the corresponding pair contact - Map mMapPairIdToContactPairIndex2; + /// Array of lost contact pairs (contact pairs in contact in previous frame but not in the current one) + Array mLostContactPairs; /// Pointer to the map of overlappingPairId to the index of contact pair of the previous frame /// (either mMapPairIdToContactPairIndex1 or mMapPairIdToContactPairIndex2) - Map* mPreviousMapPairIdToContactPairIndex; + Map mPreviousMapPairIdToContactPairIndex; - /// Pointer to the map of overlappingPairId to the index of contact pair of the current frame - /// (either mMapPairIdToContactPairIndex1 or mMapPairIdToContactPairIndex2) - Map* mCurrentMapPairIdToContactPairIndex; + /// First array with the contact manifolds + Array mContactManifolds1; - /// First list with the contact manifolds - List mContactManifolds1; + /// Second array with the contact manifolds + Array mContactManifolds2; - /// Second list with the contact manifolds - List mContactManifolds2; + /// Pointer to the array of contact manifolds from the previous frame (either mContactManifolds1 or mContactManifolds2) + Array* mPreviousContactManifolds; - /// Pointer to the list of contact manifolds from the previous frame (either mContactManifolds1 or mContactManifolds2) - List* mPreviousContactManifolds; + /// Pointer to the array of contact manifolds from the current frame (either mContactManifolds1 or mContactManifolds2) + Array* mCurrentContactManifolds; - /// Pointer to the list of contact manifolds from the current frame (either mContactManifolds1 or mContactManifolds2) - List* mCurrentContactManifolds; + /// Second array of contact points (contact points from either the current frame of the previous frame) + Array mContactPoints1; - /// Second list of contact points (contact points from either the current frame of the previous frame) - List mContactPoints1; - - /// Second list of contact points (contact points from either the current frame of the previous frame) - List mContactPoints2; + /// Second array of contact points (contact points from either the current frame of the previous frame) + Array mContactPoints2; /// Pointer to the contact points of the previous frame (either mContactPoints1 or mContactPoints2) - List* mPreviousContactPoints; + Array* mPreviousContactPoints; /// Pointer to the contact points of the current frame (either mContactPoints1 or mContactPoints2) - List* mCurrentContactPoints; + Array* mCurrentContactPoints; - /// Map a body entity to the list of contact pairs in which it is involved - Map> mMapBodyToContactPairs; + /// Array with the indices of all the contact pairs that have at least one CollisionBody + Array mCollisionBodyContactPairsIndices; + + /// Number of potential contact manifolds in the previous frame + uint32 mNbPreviousPotentialContactManifolds; + + /// Number of potential contact points in the previous frame + uint32 mNbPreviousPotentialContactPoints; + + /// Reference to the half-edge structure of the triangle polyhedron + HalfEdgeStructure& mTriangleHalfEdgeStructure; #ifdef IS_RP3D_PROFILING_ENABLED @@ -182,7 +188,7 @@ class CollisionDetectionSystem { void computeMiddlePhase(NarrowPhaseInput& narrowPhaseInput, bool needToReportContacts); // Compute the middle-phase collision detection - void computeMiddlePhaseCollisionSnapshot(List& convexPairs, List& concavePairs, NarrowPhaseInput& narrowPhaseInput, + void computeMiddlePhaseCollisionSnapshot(Array& convexPairs, Array& concavePairs, NarrowPhaseInput& narrowPhaseInput, bool reportContacts); /// Compute the narrow-phase collision detection @@ -195,89 +201,95 @@ class CollisionDetectionSystem { bool computeNarrowPhaseCollisionSnapshot(NarrowPhaseInput& narrowPhaseInput, CollisionCallback& callback); /// Process the potential contacts after narrow-phase collision detection - void computeOverlapSnapshotContactPairs(NarrowPhaseInput& narrowPhaseInput, List& contactPairs) const; + void computeOverlapSnapshotContactPairs(NarrowPhaseInput& narrowPhaseInput, Array& contactPairs) const; /// Convert the potential contact into actual contacts - void computeOverlapSnapshotContactPairs(NarrowPhaseInfoBatch& narrowPhaseInfoBatch, List& contactPairs, + void computeOverlapSnapshotContactPairs(NarrowPhaseInfoBatch& narrowPhaseInfoBatch, Array& contactPairs, Set& setOverlapContactPairId) const; - /// Take a list of overlapping nodes in the broad-phase and create new overlapping pairs if necessary - void updateOverlappingPairs(const List >& overlappingNodes); + /// Take an array of overlapping nodes in the broad-phase and create new overlapping pairs if necessary + void updateOverlappingPairs(const Array >& overlappingNodes); /// Remove pairs that are not overlapping anymore void removeNonOverlappingPairs(); /// Add a lost contact pair (pair of colliders that are not in contact anymore) - void addLostContactPair(uint64 overlappingPairIndex); + void addLostContactPair(OverlappingPairs::OverlappingPair& overlappingPair); /// Execute the narrow-phase collision detection algorithm on batches bool testNarrowPhaseCollision(NarrowPhaseInput& narrowPhaseInput, bool clipWithPreviousAxisIfStillColliding, MemoryAllocator& allocator); /// Compute the concave vs convex middle-phase algorithm for a given pair of bodies - void computeConvexVsConcaveMiddlePhase(uint64 pairIndex, MemoryAllocator& allocator, + void computeConvexVsConcaveMiddlePhase(OverlappingPairs::ConcaveOverlappingPair& overlappingPair, MemoryAllocator& allocator, NarrowPhaseInput& narrowPhaseInput); - /// Swap the previous and current contacts lists + /// Swap the previous and current contacts arrays void swapPreviousAndCurrentContacts(); /// Convert the potential contact into actual contacts void processPotentialContacts(NarrowPhaseInfoBatch& narrowPhaseInfoBatch, - bool updateLastFrameInfo, List& potentialContactPoints, - Map* mapPairIdToContactPairIndex, - List& potentialContactManifolds, List* contactPairs, - Map>& mapBodyToContactPairs); + bool updateLastFrameInfo, Array& potentialContactPoints, + Array& potentialContactManifolds, + Map& mapPairIdToContactPairIndex, Array* contactPairs); /// Process the potential contacts after narrow-phase collision detection - void processAllPotentialContacts(NarrowPhaseInput& narrowPhaseInput, bool updateLastFrameInfo, List& potentialContactPoints, - Map* mapPairIdToContactPairIndex, - List& potentialContactManifolds, List* contactPairs, - Map>& mapBodyToContactPairs); + void processAllPotentialContacts(NarrowPhaseInput& narrowPhaseInput, bool updateLastFrameInfo, Array& potentialContactPoints, + Array& potentialContactManifolds, Array* contactPairs); /// Reduce the potential contact manifolds and contact points of the overlapping pair contacts - void reducePotentialContactManifolds(List* contactPairs, List& potentialContactManifolds, - const List& potentialContactPoints) const; + void reducePotentialContactManifolds(Array* contactPairs, Array& potentialContactManifolds, + const Array& potentialContactPoints) const; /// Create the actual contact manifolds and contacts points (from potential contacts) for a given contact pair void createContacts(); + /// Add the contact pairs to the corresponding bodies + void addContactPairsToBodies(); + + /// Compute the map from contact pairs ids to contact pair for the next frame + void computeMapPreviousContactPairs(); + /// Compute the lost contact pairs (contact pairs in contact in the previous frame but not in the current one) void computeLostContactPairs(); /// Create the actual contact manifolds and contacts points for testCollision() methods - void createSnapshotContacts(List& contactPairs, List &contactManifolds, - List& contactPoints, - List& potentialContactManifolds, - List& potentialContactPoints); + void createSnapshotContacts(Array& contactPairs, Array &contactManifolds, + Array& contactPoints, + Array& potentialContactManifolds, + Array& potentialContactPoints); /// Initialize the current contacts with the contacts from the previous frame (for warmstarting) void initContactsWithPreviousOnes(); /// Reduce the number of contact points of a potential contact manifold void reduceContactPoints(ContactManifoldInfo& manifold, const Transform& shape1ToWorldTransform, - const List& potentialContactPoints) const; + const Array& potentialContactPoints) const; /// Report contacts - void reportContacts(CollisionCallback& callback, List* contactPairs, - List* manifolds, List* contactPoints, List& lostContactPairs); + void reportContacts(CollisionCallback& callback, Array* contactPairs, + Array* manifolds, Array* contactPoints, Array& lostContactPairs); /// Report all triggers - void reportTriggers(EventListener& eventListener, List* contactPairs, List& lostContactPairs); + void reportTriggers(EventListener& eventListener, Array* contactPairs, Array& lostContactPairs); /// Report all contacts for debug rendering - void reportDebugRenderingContacts(List* contactPairs, List* manifolds, List* contactPoints, List& lostContactPairs); + void reportDebugRenderingContacts(Array* contactPairs, Array* manifolds, Array* contactPoints, Array& lostContactPairs); /// Return the largest depth of all the contact points of a potential manifold decimal computePotentialManifoldLargestContactDepth(const ContactManifoldInfo& manifold, - const List& potentialContactPoints) const; + const Array& potentialContactPoints) const; /// Process the potential contacts where one collion is a concave shape void processSmoothMeshContacts(OverlappingPair* pair); /// Filter the overlapping pairs to keep only the pairs where a given body is involved - void filterOverlappingPairs(Entity bodyEntity, List& convexPairs, List& concavePairs) const; + void filterOverlappingPairs(Entity bodyEntity, Array& convexPairs, Array& concavePairs) const; /// Filter the overlapping pairs to keep only the pairs where two given bodies are involved - void filterOverlappingPairs(Entity body1Entity, Entity body2Entity, List& convexPairs, List& concavePairs) const; + void filterOverlappingPairs(Entity body1Entity, Entity body2Entity, Array& convexPairs, Array& concavePairs) const; + + /// Remove an element in an array (and replace it by the last one in the array) + void removeItemAtInArray(uint array[], uint8 index, uint8& arraySize) const; public : @@ -286,7 +298,7 @@ class CollisionDetectionSystem { /// Constructor CollisionDetectionSystem(PhysicsWorld* world, ColliderComponents& collidersComponents, TransformComponents& transformComponents, CollisionBodyComponents& collisionBodyComponents, RigidBodyComponents& rigidBodyComponents, - MemoryManager& memoryManager); + MemoryManager& memoryManager, HalfEdgeStructure& triangleHalfEdgeStructure); /// Destructor ~CollisionDetectionSystem() = default; @@ -380,12 +392,12 @@ class CollisionDetectionSystem { }; // Return a reference to the collision dispatch configuration -inline CollisionDispatch& CollisionDetectionSystem::getCollisionDispatch() { +RP3D_FORCE_INLINE CollisionDispatch& CollisionDetectionSystem::getCollisionDispatch() { return mCollisionDispatch; } // Add a body to the collision detection -inline void CollisionDetectionSystem::addCollider(Collider* collider, const AABB& aabb) { +RP3D_FORCE_INLINE void CollisionDetectionSystem::addCollider(Collider* collider, const AABB& aabb) { // Add the body to the broad-phase mBroadPhaseSystem.addCollider(collider, aabb); @@ -399,19 +411,19 @@ inline void CollisionDetectionSystem::addCollider(Collider* collider, const AABB } // Add a pair of bodies that cannot collide with each other -inline void CollisionDetectionSystem::addNoCollisionPair(Entity body1Entity, Entity body2Entity) { +RP3D_FORCE_INLINE void CollisionDetectionSystem::addNoCollisionPair(Entity body1Entity, Entity body2Entity) { mNoCollisionPairs.add(OverlappingPairs::computeBodiesIndexPair(body1Entity, body2Entity)); } // Remove a pair of bodies that cannot collide with each other -inline void CollisionDetectionSystem::removeNoCollisionPair(Entity body1Entity, Entity body2Entity) { +RP3D_FORCE_INLINE void CollisionDetectionSystem::removeNoCollisionPair(Entity body1Entity, Entity body2Entity) { mNoCollisionPairs.remove(OverlappingPairs::computeBodiesIndexPair(body1Entity, body2Entity)); } // Ask for a collision shape to be tested again during broad-phase. -/// We simply put the shape in the list of collision shape that have moved in the +/// We simply put the shape in the array of collision shape that have moved in the /// previous frame so that it is tested for collision again in the broad-phase. -inline void CollisionDetectionSystem::askForBroadPhaseCollisionCheck(Collider* collider) { +RP3D_FORCE_INLINE void CollisionDetectionSystem::askForBroadPhaseCollisionCheck(Collider* collider) { if (collider->getBroadPhaseId() != -1) { mBroadPhaseSystem.addMovedCollider(collider->getBroadPhaseId(), collider); @@ -419,31 +431,31 @@ inline void CollisionDetectionSystem::askForBroadPhaseCollisionCheck(Collider* c } // Return a pointer to the world -inline PhysicsWorld* CollisionDetectionSystem::getWorld() { +RP3D_FORCE_INLINE PhysicsWorld* CollisionDetectionSystem::getWorld() { return mWorld; } // Return a reference to the memory manager -inline MemoryManager& CollisionDetectionSystem::getMemoryManager() const { +RP3D_FORCE_INLINE MemoryManager& CollisionDetectionSystem::getMemoryManager() const { return mMemoryManager; } // Update a collider (that has moved for instance) -inline void CollisionDetectionSystem::updateCollider(Entity colliderEntity, decimal timeStep) { +RP3D_FORCE_INLINE void CollisionDetectionSystem::updateCollider(Entity colliderEntity, decimal timeStep) { // Update the collider component mBroadPhaseSystem.updateCollider(colliderEntity, timeStep); } // Update all the enabled colliders -inline void CollisionDetectionSystem::updateColliders(decimal timeStep) { +RP3D_FORCE_INLINE void CollisionDetectionSystem::updateColliders(decimal timeStep) { mBroadPhaseSystem.updateColliders(timeStep); } #ifdef IS_RP3D_PROFILING_ENABLED // Set the profiler -inline void CollisionDetectionSystem::setProfiler(Profiler* profiler) { +RP3D_FORCE_INLINE void CollisionDetectionSystem::setProfiler(Profiler* profiler) { mProfiler = profiler; mBroadPhaseSystem.setProfiler(profiler); mCollisionDispatch.setProfiler(profiler); diff --git a/include/reactphysics3d/systems/ConstraintSolverSystem.h b/include/reactphysics3d/systems/ConstraintSolverSystem.h index f3b68d80..626dbe47 100644 --- a/include/reactphysics3d/systems/ConstraintSolverSystem.h +++ b/include/reactphysics3d/systems/ConstraintSolverSystem.h @@ -210,12 +210,15 @@ class ConstraintSolverSystem { #endif + // ---------- Friendship ---------- + + friend class HingeJoint; }; #ifdef IS_RP3D_PROFILING_ENABLED // Set the profiler -inline void ConstraintSolverSystem::setProfiler(Profiler* profiler) { +RP3D_FORCE_INLINE void ConstraintSolverSystem::setProfiler(Profiler* profiler) { mProfiler = profiler; mSolveBallAndSocketJointSystem.setProfiler(profiler); mSolveFixedJointSystem.setProfiler(profiler); diff --git a/include/reactphysics3d/systems/ContactSolverSystem.h b/include/reactphysics3d/systems/ContactSolverSystem.h index 11c4aa31..213e9baa 100644 --- a/include/reactphysics3d/systems/ContactSolverSystem.h +++ b/include/reactphysics3d/systems/ContactSolverSystem.h @@ -30,6 +30,8 @@ #include #include #include +#include +#include /// ReactPhysics3D namespace namespace reactphysics3d { @@ -198,9 +200,6 @@ class ContactSolverSystem { /// Mix friction coefficient for the two bodies decimal frictionCoefficient; - /// Rolling resistance factor between the two bodies - decimal rollingResistanceFactor; - // - Variables used when friction constraints are apply at the center of the manifold-// /// Average normal vector of the contact manifold @@ -239,9 +238,6 @@ class ContactSolverSystem { /// Matrix K for the twist friction constraint decimal inverseTwistFrictionMass; - /// Matrix K for the rolling resistance constraint - Matrix3x3 inverseRollingResistance; - /// First friction direction at contact manifold center Vector3 frictionVector1; @@ -263,9 +259,6 @@ class ContactSolverSystem { /// Twist friction impulse at contact manifold center decimal frictionTwistImpulse; - /// Rolling resistance impulse - Vector3 rollingResistanceImpulse; - /// Number of contact points int8 nbContacts; }; @@ -310,11 +303,11 @@ class ContactSolverSystem { /// Reference to the islands Islands& mIslands; - /// Pointer to the list of contact manifolds from narrow-phase - List* mAllContactManifolds; + /// Pointer to the array of contact manifolds from narrow-phase + Array* mAllContactManifolds; - /// Pointer to the list of contact points from narrow-phase - List* mAllContactPoints; + /// Pointer to the array of contact points from narrow-phase + Array* mAllContactPoints; /// Reference to the body components CollisionBodyComponents& mBodyComponents; @@ -338,14 +331,10 @@ class ContactSolverSystem { // -------------------- Methods -------------------- // /// Compute the collision restitution factor from the restitution factor of each collider - decimal computeMixedRestitutionFactor(Collider* collider1, - Collider* collider2) const; + decimal computeMixedRestitutionFactor(const Material& material1, const Material& material2) const; /// Compute the mixed friction coefficient from the friction coefficient of each collider - decimal computeMixedFrictionCoefficient(Collider* collider1, Collider* collider2) const; - - /// Compute th mixed rolling resistance factor between two colliders - decimal computeMixedRollingResistance(Collider* collider1, Collider* collider2) const; + decimal computeMixedFrictionCoefficient(const Material &material1, const Material &material2) const; /// Compute the two unit orthogonal vectors "t1" and "t2" that span the tangential friction /// plane for a contact manifold. The two vectors have to be @@ -368,7 +357,7 @@ class ContactSolverSystem { ~ContactSolverSystem() = default; /// Initialize the contact constraints - void init(List* contactManifolds, List* contactPoints, decimal timeStep); + void init(Array* contactManifolds, Array* contactPoints, decimal timeStep); /// Initialize the constraint solver for a given island void initializeForIsland(uint islandIndex); @@ -398,19 +387,36 @@ class ContactSolverSystem { }; // Return true if the split impulses position correction technique is used for contacts -inline bool ContactSolverSystem::isSplitImpulseActive() const { +RP3D_FORCE_INLINE bool ContactSolverSystem::isSplitImpulseActive() const { return mIsSplitImpulseActive; } // Activate or Deactivate the split impulses for contacts -inline void ContactSolverSystem::setIsSplitImpulseActive(bool isActive) { +RP3D_FORCE_INLINE void ContactSolverSystem::setIsSplitImpulseActive(bool isActive) { mIsSplitImpulseActive = isActive; } +// Compute the collision restitution factor from the restitution factor of each collider +RP3D_FORCE_INLINE decimal ContactSolverSystem::computeMixedRestitutionFactor(const Material& material1, const Material& material2) const { + + const decimal restitution1 = material1.getBounciness(); + const decimal restitution2 = material2.getBounciness(); + + // Return the largest restitution factor + return (restitution1 > restitution2) ? restitution1 : restitution2; +} + +// Compute the mixed friction coefficient from the friction coefficient of each collider +RP3D_FORCE_INLINE decimal ContactSolverSystem::computeMixedFrictionCoefficient(const Material& material1, const Material& material2) const { + + // Use the geometric mean to compute the mixed friction coefficient + return material1.getFrictionCoefficientSqrt() * material2.getFrictionCoefficientSqrt(); +} + #ifdef IS_RP3D_PROFILING_ENABLED // Set the profiler -inline void ContactSolverSystem::setProfiler(Profiler* profiler) { +RP3D_FORCE_INLINE void ContactSolverSystem::setProfiler(Profiler* profiler) { mProfiler = profiler; } diff --git a/include/reactphysics3d/systems/DynamicsSystem.h b/include/reactphysics3d/systems/DynamicsSystem.h index 76723ae1..ea87d0eb 100644 --- a/include/reactphysics3d/systems/DynamicsSystem.h +++ b/include/reactphysics3d/systems/DynamicsSystem.h @@ -114,7 +114,7 @@ class DynamicsSystem { #ifdef IS_RP3D_PROFILING_ENABLED // Set the profiler -inline void DynamicsSystem::setProfiler(Profiler* profiler) { +RP3D_FORCE_INLINE void DynamicsSystem::setProfiler(Profiler* profiler) { mProfiler = profiler; } diff --git a/include/reactphysics3d/systems/SolveBallAndSocketJointSystem.h b/include/reactphysics3d/systems/SolveBallAndSocketJointSystem.h index bd073781..43b2eec7 100644 --- a/include/reactphysics3d/systems/SolveBallAndSocketJointSystem.h +++ b/include/reactphysics3d/systems/SolveBallAndSocketJointSystem.h @@ -123,20 +123,20 @@ class SolveBallAndSocketJointSystem { #ifdef IS_RP3D_PROFILING_ENABLED // Set the profiler -inline void SolveBallAndSocketJointSystem::setProfiler(Profiler* profiler) { +RP3D_FORCE_INLINE void SolveBallAndSocketJointSystem::setProfiler(Profiler* profiler) { mProfiler = profiler; } #endif // Set the time step -inline void SolveBallAndSocketJointSystem::setTimeStep(decimal timeStep) { +RP3D_FORCE_INLINE void SolveBallAndSocketJointSystem::setTimeStep(decimal timeStep) { assert(timeStep > decimal(0.0)); mTimeStep = timeStep; } // Set to true to enable warm starting -inline void SolveBallAndSocketJointSystem::setIsWarmStartingActive(bool isWarmStartingActive) { +RP3D_FORCE_INLINE void SolveBallAndSocketJointSystem::setIsWarmStartingActive(bool isWarmStartingActive) { mIsWarmStartingActive = isWarmStartingActive; } diff --git a/include/reactphysics3d/systems/SolveFixedJointSystem.h b/include/reactphysics3d/systems/SolveFixedJointSystem.h index 84d77685..79e6c6c2 100644 --- a/include/reactphysics3d/systems/SolveFixedJointSystem.h +++ b/include/reactphysics3d/systems/SolveFixedJointSystem.h @@ -120,20 +120,20 @@ class SolveFixedJointSystem { #ifdef IS_RP3D_PROFILING_ENABLED // Set the profiler -inline void SolveFixedJointSystem::setProfiler(Profiler* profiler) { +RP3D_FORCE_INLINE void SolveFixedJointSystem::setProfiler(Profiler* profiler) { mProfiler = profiler; } #endif // Set the time step -inline void SolveFixedJointSystem::setTimeStep(decimal timeStep) { +RP3D_FORCE_INLINE void SolveFixedJointSystem::setTimeStep(decimal timeStep) { assert(timeStep > decimal(0.0)); mTimeStep = timeStep; } // Set to true to enable warm starting -inline void SolveFixedJointSystem::setIsWarmStartingActive(bool isWarmStartingActive) { +RP3D_FORCE_INLINE void SolveFixedJointSystem::setIsWarmStartingActive(bool isWarmStartingActive) { mIsWarmStartingActive = isWarmStartingActive; } diff --git a/include/reactphysics3d/systems/SolveHingeJointSystem.h b/include/reactphysics3d/systems/SolveHingeJointSystem.h index 05e5f059..d05a68e6 100644 --- a/include/reactphysics3d/systems/SolveHingeJointSystem.h +++ b/include/reactphysics3d/systems/SolveHingeJointSystem.h @@ -92,8 +92,7 @@ class SolveHingeJointSystem { decimal upperLimitAngle) const; /// Compute the current angle around the hinge axis - decimal computeCurrentHingeAngle(Entity jointEntity, const Quaternion& orientationBody1, - const Quaternion& orientationBody2); + decimal computeCurrentHingeAngle(Entity jointEntity, const Quaternion& orientationBody1, const Quaternion& orientationBody2); public : @@ -133,25 +132,29 @@ class SolveHingeJointSystem { #endif + // ---------- Friendship ---------- + + friend class HingeJoint; + }; #ifdef IS_RP3D_PROFILING_ENABLED // Set the profiler -inline void SolveHingeJointSystem::setProfiler(Profiler* profiler) { +RP3D_FORCE_INLINE void SolveHingeJointSystem::setProfiler(Profiler* profiler) { mProfiler = profiler; } #endif // Set the time step -inline void SolveHingeJointSystem::setTimeStep(decimal timeStep) { +RP3D_FORCE_INLINE void SolveHingeJointSystem::setTimeStep(decimal timeStep) { assert(timeStep > decimal(0.0)); mTimeStep = timeStep; } // Set to true to enable warm starting -inline void SolveHingeJointSystem::setIsWarmStartingActive(bool isWarmStartingActive) { +RP3D_FORCE_INLINE void SolveHingeJointSystem::setIsWarmStartingActive(bool isWarmStartingActive) { mIsWarmStartingActive = isWarmStartingActive; } diff --git a/include/reactphysics3d/systems/SolveSliderJointSystem.h b/include/reactphysics3d/systems/SolveSliderJointSystem.h index 9e7d595c..9d60927c 100644 --- a/include/reactphysics3d/systems/SolveSliderJointSystem.h +++ b/include/reactphysics3d/systems/SolveSliderJointSystem.h @@ -124,20 +124,20 @@ class SolveSliderJointSystem { #ifdef IS_RP3D_PROFILING_ENABLED // Set the profiler -inline void SolveSliderJointSystem::setProfiler(Profiler* profiler) { +RP3D_FORCE_INLINE void SolveSliderJointSystem::setProfiler(Profiler* profiler) { mProfiler = profiler; } #endif // Set the time step -inline void SolveSliderJointSystem::setTimeStep(decimal timeStep) { +RP3D_FORCE_INLINE void SolveSliderJointSystem::setTimeStep(decimal timeStep) { assert(timeStep > decimal(0.0)); mTimeStep = timeStep; } // Set to true to enable warm starting -inline void SolveSliderJointSystem::setIsWarmStartingActive(bool isWarmStartingActive) { +RP3D_FORCE_INLINE void SolveSliderJointSystem::setIsWarmStartingActive(bool isWarmStartingActive) { mIsWarmStartingActive = isWarmStartingActive; } diff --git a/include/reactphysics3d/utils/DebugRenderer.h b/include/reactphysics3d/utils/DebugRenderer.h index cae56c8c..c5321f8e 100644 --- a/include/reactphysics3d/utils/DebugRenderer.h +++ b/include/reactphysics3d/utils/DebugRenderer.h @@ -27,7 +27,7 @@ #define REACTPHYSICS3D_DEBUG_RENDERER_H // Libraries -#include +#include #include #include #include @@ -47,7 +47,7 @@ class PhysicsWorld; /** * This class is used to display physics debug information directly into the user application view. * For instance, it is possible to display AABBs of colliders, colliders or contact points. This class - * can be used to get the debug information as lists of basic primitives (points, linges, triangles, ...). + * can be used to get the debug information as arrays of basic primitives (points, linges, triangles, ...). * You can use this to render physics debug information in your simulation on top of your object. Note that * you should use this only for debugging purpose and you should disable it when you compile the final release * version of your application because computing/rendering phyiscs debug information can be expensive. @@ -159,11 +159,11 @@ class DebugRenderer : public EventListener { /// Memory allocator MemoryAllocator& mAllocator; - /// List with all the debug lines - List mLines; + /// Array with all the debug lines + Array mLines; - /// List with all the debug triangles - List mTriangles; + /// Array with all the debug triangles + Array mTriangles; /// 32-bits integer that contains all the flags of debug items to display uint32 mDisplayedDebugItems; @@ -216,8 +216,8 @@ class DebugRenderer : public EventListener { /// Return the number of lines uint32 getNbLines() const; - /// Return a reference to the list of lines - const List& getLines() const; + /// Return a reference to the array of lines + const Array& getLines() const; /// Return a pointer to the array of lines const DebugLine* getLinesArray() const; @@ -225,8 +225,8 @@ class DebugRenderer : public EventListener { /// Return the number of triangles uint32 getNbTriangles() const; - /// Return a reference to the list of triangles - const List& getTriangles() const; + /// Return a reference to the array of triangles + const Array& getTriangles() const; /// Return a pointer to the array of triangles const DebugTriangle* getTrianglesArray() const; @@ -263,15 +263,15 @@ class DebugRenderer : public EventListener { /** * @return The number of lines in the array of lines to draw */ -inline uint32 DebugRenderer::getNbLines() const { +RP3D_FORCE_INLINE uint32 DebugRenderer::getNbLines() const { return mLines.size(); } -// Return a reference to the list of lines +// Return a reference to the array of lines /** - * @return The list of lines to draw + * @return The array of lines to draw */ -inline const List& DebugRenderer::getLines() const { +RP3D_FORCE_INLINE const Array& DebugRenderer::getLines() const { return mLines; } @@ -279,7 +279,7 @@ inline const List& DebugRenderer::getLines() const { /** * @return A pointer to the first element of the lines array to draw */ -inline const DebugRenderer::DebugLine* DebugRenderer::getLinesArray() const { +RP3D_FORCE_INLINE const DebugRenderer::DebugLine* DebugRenderer::getLinesArray() const { return &(mLines[0]); } @@ -287,15 +287,15 @@ inline const DebugRenderer::DebugLine* DebugRenderer::getLinesArray() const { /** * @return The number of triangles in the array of triangles to draw */ -inline uint32 DebugRenderer::getNbTriangles() const { +RP3D_FORCE_INLINE uint32 DebugRenderer::getNbTriangles() const { return mTriangles.size(); } -// Return a reference to the list of triangles +// Return a reference to the array of triangles /** - * @return The list of triangles to draw + * @return The array of triangles to draw */ -inline const List& DebugRenderer::getTriangles() const { +RP3D_FORCE_INLINE const Array& DebugRenderer::getTriangles() const { return mTriangles; } @@ -303,7 +303,7 @@ inline const List& DebugRenderer::getTriangles() c /** * @return A pointer to the first element of the triangles array to draw */ -inline const DebugRenderer::DebugTriangle* DebugRenderer::getTrianglesArray() const { +RP3D_FORCE_INLINE const DebugRenderer::DebugTriangle* DebugRenderer::getTrianglesArray() const { return &(mTriangles[0]); } @@ -312,7 +312,7 @@ inline const DebugRenderer::DebugTriangle* DebugRenderer::getTrianglesArray() co * @param item A debug item * @return True if the given debug item is being displayed and false otherwise */ -inline bool DebugRenderer::getIsDebugItemDisplayed(DebugItem item) const { +RP3D_FORCE_INLINE bool DebugRenderer::getIsDebugItemDisplayed(DebugItem item) const { return mDisplayedDebugItems & static_cast(item); } @@ -321,7 +321,7 @@ inline bool DebugRenderer::getIsDebugItemDisplayed(DebugItem item) const { * @param item A debug item to draw * @param isDisplayed True if the given debug item has to be displayed and false otherwise */ -inline void DebugRenderer::setIsDebugItemDisplayed(DebugItem item, bool isDisplayed) { +RP3D_FORCE_INLINE void DebugRenderer::setIsDebugItemDisplayed(DebugItem item, bool isDisplayed) { const uint32 itemFlag = static_cast(item); uint32 resetBit = ~(itemFlag); mDisplayedDebugItems &= resetBit; @@ -334,7 +334,7 @@ inline void DebugRenderer::setIsDebugItemDisplayed(DebugItem item, bool isDispla /** * @return The radius of the sphere used to display a contact point */ -inline decimal DebugRenderer::getContactPointSphereRadius() const { +RP3D_FORCE_INLINE decimal DebugRenderer::getContactPointSphereRadius() const { return mContactPointSphereRadius; } @@ -342,7 +342,7 @@ inline decimal DebugRenderer::getContactPointSphereRadius() const { /** * @param radius The radius of the sphere used to display a contact point */ -inline void DebugRenderer::setContactPointSphereRadius(decimal radius) { +RP3D_FORCE_INLINE void DebugRenderer::setContactPointSphereRadius(decimal radius) { assert(radius > decimal(0.0)); mContactPointSphereRadius = radius; } @@ -352,7 +352,7 @@ inline void DebugRenderer::setContactPointSphereRadius(decimal radius) { /** * @return The length of the contact normal to display */ -inline decimal DebugRenderer::getContactNormalLength() const { +RP3D_FORCE_INLINE decimal DebugRenderer::getContactNormalLength() const { return mContactNormalLength; } @@ -360,7 +360,7 @@ inline decimal DebugRenderer::getContactNormalLength() const { /** * @param contactNormalLength The length of the contact normal to display */ -inline void DebugRenderer::setContactNormalLength(decimal contactNormalLength) { +RP3D_FORCE_INLINE void DebugRenderer::setContactNormalLength(decimal contactNormalLength) { mContactNormalLength = contactNormalLength; } diff --git a/include/reactphysics3d/utils/DefaultLogger.h b/include/reactphysics3d/utils/DefaultLogger.h index bcc2f450..5f6733ee 100644 --- a/include/reactphysics3d/utils/DefaultLogger.h +++ b/include/reactphysics3d/utils/DefaultLogger.h @@ -28,7 +28,7 @@ // Libraries #include -#include +#include #include #include #include @@ -445,7 +445,7 @@ class DefaultLogger : public Logger { MemoryAllocator& mAllocator; /// All the log destinations - List mDestinations; + Array mDestinations; /// Map a log format to the given formatter object Map mFormatters; diff --git a/include/reactphysics3d/utils/Logger.h b/include/reactphysics3d/utils/Logger.h index ba0144e2..c54ae10e 100644 --- a/include/reactphysics3d/utils/Logger.h +++ b/include/reactphysics3d/utils/Logger.h @@ -27,7 +27,7 @@ #define REACTPHYSICS3D_LOGGER_H // Libraries -#include +#include #include #include #include diff --git a/include/reactphysics3d/utils/Profiler.h b/include/reactphysics3d/utils/Profiler.h index a3ef2a2b..8c46e7ca 100644 --- a/include/reactphysics3d/utils/Profiler.h +++ b/include/reactphysics3d/utils/Profiler.h @@ -34,7 +34,7 @@ #include #include #include -#include +#include /// ReactPhysics3D namespace namespace reactphysics3d { @@ -401,113 +401,113 @@ class ProfileSample { #define RP3D_PROFILE(name, profiler) ProfileSample profileSample(name, profiler) // Return true if we are at the root of the profiler tree -inline bool ProfileNodeIterator::isRoot() { +RP3D_FORCE_INLINE bool ProfileNodeIterator::isRoot() { return (mCurrentParentNode->getParentNode() == nullptr); } // Return true if we are at the end of a branch of the profiler tree -inline bool ProfileNodeIterator::isEnd() { +RP3D_FORCE_INLINE bool ProfileNodeIterator::isEnd() { return (mCurrentChildNode == nullptr); } // Return the name of the current node -inline const char* ProfileNodeIterator::getCurrentName() { +RP3D_FORCE_INLINE const char* ProfileNodeIterator::getCurrentName() { return mCurrentChildNode->getName(); } // Return the total time of the current node -inline long double ProfileNodeIterator::getCurrentTotalTime() { +RP3D_FORCE_INLINE long double ProfileNodeIterator::getCurrentTotalTime() { return mCurrentChildNode->getTotalTime(); } // Return the total number of calls of the current node -inline uint ProfileNodeIterator::getCurrentNbTotalCalls() { +RP3D_FORCE_INLINE uint ProfileNodeIterator::getCurrentNbTotalCalls() { return mCurrentChildNode->getNbTotalCalls(); } // Return the name of the current parent node -inline const char* ProfileNodeIterator::getCurrentParentName() { +RP3D_FORCE_INLINE const char* ProfileNodeIterator::getCurrentParentName() { return mCurrentParentNode->getName(); } // Return the total time of the current parent node -inline long double ProfileNodeIterator::getCurrentParentTotalTime() { +RP3D_FORCE_INLINE long double ProfileNodeIterator::getCurrentParentTotalTime() { return mCurrentParentNode->getTotalTime(); } // Return the total number of calls of the current parent node -inline uint ProfileNodeIterator::getCurrentParentNbTotalCalls() { +RP3D_FORCE_INLINE uint ProfileNodeIterator::getCurrentParentNbTotalCalls() { return mCurrentParentNode->getNbTotalCalls(); } // Go to the first node -inline void ProfileNodeIterator::first() { +RP3D_FORCE_INLINE void ProfileNodeIterator::first() { mCurrentChildNode = mCurrentParentNode->getChildNode(); } // Go to the next node -inline void ProfileNodeIterator::next() { +RP3D_FORCE_INLINE void ProfileNodeIterator::next() { mCurrentChildNode = mCurrentChildNode->getSiblingNode(); } // Return a pointer to the parent node -inline ProfileNode* ProfileNode::getParentNode() { +RP3D_FORCE_INLINE ProfileNode* ProfileNode::getParentNode() { return mParentNode; } // Return a pointer to a sibling node -inline ProfileNode* ProfileNode::getSiblingNode() { +RP3D_FORCE_INLINE ProfileNode* ProfileNode::getSiblingNode() { return mSiblingNode; } // Return a pointer to a child node -inline ProfileNode* ProfileNode::getChildNode() { +RP3D_FORCE_INLINE ProfileNode* ProfileNode::getChildNode() { return mChildNode; } // Return the name of the node -inline const char* ProfileNode::getName() { +RP3D_FORCE_INLINE const char* ProfileNode::getName() { return mName; } // Return the total number of call of the corresponding block of code -inline uint ProfileNode::getNbTotalCalls() const { +RP3D_FORCE_INLINE uint ProfileNode::getNbTotalCalls() const { return mNbTotalCalls; } // Return the total time spent in the block of code -inline long double ProfileNode::getTotalTime() const { +RP3D_FORCE_INLINE long double ProfileNode::getTotalTime() const { return mTotalTime; } // Return the number of frames -inline uint Profiler::getNbFrames() { +RP3D_FORCE_INLINE uint Profiler::getNbFrames() { return mFrameCounter; } // Return the elasped time since the start/reset of the profiling -inline long double Profiler::getElapsedTimeSinceStart() { +RP3D_FORCE_INLINE long double Profiler::getElapsedTimeSinceStart() { long double currentTime = Timer::getCurrentSystemTime() * 1000.0L; return currentTime - mProfilingStartTime; } // Increment the frame counter -inline void Profiler::incrementFrameCounter() { +RP3D_FORCE_INLINE void Profiler::incrementFrameCounter() { mFrameCounter++; } // Return an iterator over the profiler tree starting at the root -inline ProfileNodeIterator* Profiler::getIterator() { +RP3D_FORCE_INLINE ProfileNodeIterator* Profiler::getIterator() { return new ProfileNodeIterator(&mRootNode); } // Destroy a previously allocated iterator -inline void Profiler::destroyIterator(ProfileNodeIterator* iterator) { +RP3D_FORCE_INLINE void Profiler::destroyIterator(ProfileNodeIterator* iterator) { delete iterator; } // Destroy the profiler (release the memory) -inline void Profiler::destroy() { +RP3D_FORCE_INLINE void Profiler::destroy() { mRootNode.destroy(); } diff --git a/src/body/CollisionBody.cpp b/src/body/CollisionBody.cpp index 16279cf7..44513f55 100644 --- a/src/body/CollisionBody.cpp +++ b/src/body/CollisionBody.cpp @@ -89,12 +89,13 @@ Collider* CollisionBody::addCollider(CollisionShape* collisionShape, const Trans // TODO : Maybe this method can directly returns an AABB collisionShape->getLocalBounds(localBoundsMin, localBoundsMax); const Transform localToWorldTransform = mWorld.mTransformComponents.getTransform(mEntity) * transform; - ColliderComponents::ColliderComponent colliderComponent(mEntity, collider, - AABB(localBoundsMin, localBoundsMax), - transform, collisionShape, 0x0001, 0xFFFF, localToWorldTransform); + Material material(mWorld.mConfig.defaultFrictionCoefficient, mWorld.mConfig.defaultBounciness); + ColliderComponents::ColliderComponent colliderComponent(mEntity, collider, AABB(localBoundsMin, localBoundsMax), + transform, collisionShape, 0x0001, 0xFFFF, localToWorldTransform, material); bool isActive = mWorld.mCollisionBodyComponents.getIsActive(mEntity); mWorld.mCollidersComponents.addComponent(colliderEntity, !isActive, colliderComponent); + mWorld.mCollisionBodyComponents.addColliderToBody(mEntity, colliderEntity); // Assign the collider with the collision shape @@ -130,8 +131,8 @@ Collider* CollisionBody::addCollider(CollisionShape* collisionShape, const Trans /** * @return The number of colliders associated with this body */ -uint CollisionBody::getNbColliders() const { - return static_cast(mWorld.mCollisionBodyComponents.getColliders(mEntity).size()); +uint32 CollisionBody::getNbColliders() const { + return mWorld.mCollisionBodyComponents.getColliders(mEntity).size(); } // Return a const pointer to a given collider of the body @@ -199,9 +200,9 @@ void CollisionBody::removeCollider(Collider* collider) { void CollisionBody::removeAllColliders() { // Look for the collider that contains the collision shape in parameter. - // Note that we need to copy the list of collider entities because we are deleting them in a loop. - const List collidersEntities = mWorld.mCollisionBodyComponents.getColliders(mEntity); - for (uint i=0; i < collidersEntities.size(); i++) { + // Note that we need to copy the array of collider entities because we are deleting them in a loop. + const Array collidersEntities = mWorld.mCollisionBodyComponents.getColliders(mEntity); + for (uint32 i=0; i < collidersEntities.size(); i++) { removeCollider(mWorld.mCollidersComponents.getCollider(collidersEntities[i])); } @@ -221,8 +222,9 @@ const Transform& CollisionBody::getTransform() const { void CollisionBody::updateBroadPhaseState(decimal timeStep) const { // For all the colliders of the body - const List& colliderEntities = mWorld.mCollisionBodyComponents.getColliders(mEntity); - for (uint i=0; i < colliderEntities.size(); i++) { + const Array& colliderEntities = mWorld.mCollisionBodyComponents.getColliders(mEntity); + const uint32 nbColliderEntities = colliderEntities.size(); + for (uint32 i=0; i < nbColliderEntities; i++) { // Update the local-to-world transform of the collider mWorld.mCollidersComponents.setLocalToWorldTransform(colliderEntities[i], @@ -251,8 +253,8 @@ void CollisionBody::setIsActive(bool isActive) { const Transform& transform = mWorld.mTransformComponents.getTransform(mEntity); // For each collider of the body - const List& colliderEntities = mWorld.mCollisionBodyComponents.getColliders(mEntity); - for (uint i=0; i < colliderEntities.size(); i++) { + const Array& colliderEntities = mWorld.mCollisionBodyComponents.getColliders(mEntity); + for (uint32 i=0; i < colliderEntities.size(); i++) { Collider* collider = mWorld.mCollidersComponents.getCollider(colliderEntities[i]); @@ -267,8 +269,8 @@ void CollisionBody::setIsActive(bool isActive) { else { // If we have to deactivate the body // For each collider of the body - const List& colliderEntities = mWorld.mCollisionBodyComponents.getColliders(mEntity); - for (uint i=0; i < colliderEntities.size(); i++) { + const Array& colliderEntities = mWorld.mCollisionBodyComponents.getColliders(mEntity); + for (uint32 i=0; i < colliderEntities.size(); i++) { Collider* collider = mWorld.mCollidersComponents.getCollider(colliderEntities[i]); @@ -290,8 +292,9 @@ void CollisionBody::setIsActive(bool isActive) { void CollisionBody::askForBroadPhaseCollisionCheck() const { // For all the colliders of the body - const List& colliderEntities = mWorld.mCollisionBodyComponents.getColliders(mEntity); - for (uint i=0; i < colliderEntities.size(); i++) { + const Array& colliderEntities = mWorld.mCollisionBodyComponents.getColliders(mEntity); + const uint32 nbColliderEntities = colliderEntities.size(); + for (uint32 i=0; i < nbColliderEntities; i++) { Collider* collider = mWorld.mCollidersComponents.getCollider(colliderEntities[i]); @@ -308,8 +311,8 @@ void CollisionBody::askForBroadPhaseCollisionCheck() const { bool CollisionBody::testPointInside(const Vector3& worldPoint) const { // For each collider of the body - const List& colliderEntities = mWorld.mCollisionBodyComponents.getColliders(mEntity); - for (uint i=0; i < colliderEntities.size(); i++) { + const Array& colliderEntities = mWorld.mCollisionBodyComponents.getColliders(mEntity); + for (uint32 i=0; i < colliderEntities.size(); i++) { Collider* collider = mWorld.mCollidersComponents.getCollider(colliderEntities[i]); @@ -337,8 +340,9 @@ bool CollisionBody::raycast(const Ray& ray, RaycastInfo& raycastInfo) { Ray rayTemp(ray); // For each collider of the body - const List& colliderEntities = mWorld.mCollisionBodyComponents.getColliders(mEntity); - for (uint i=0; i < colliderEntities.size(); i++) { + const Array& colliderEntities = mWorld.mCollisionBodyComponents.getColliders(mEntity); + const uint32 nbColliderEntities = colliderEntities.size(); + for (uint32 i=0; i < nbColliderEntities; i++) { Collider* collider = mWorld.mCollidersComponents.getCollider(colliderEntities[i]); @@ -360,7 +364,7 @@ AABB CollisionBody::getAABB() const { AABB bodyAABB; - const List& colliderEntities = mWorld.mCollisionBodyComponents.getColliders(mEntity); + const Array& colliderEntities = mWorld.mCollisionBodyComponents.getColliders(mEntity); if (colliderEntities.size() == 0) return bodyAABB; const Transform& transform = mWorld.mTransformComponents.getTransform(mEntity); @@ -369,7 +373,8 @@ AABB CollisionBody::getAABB() const { collider->getCollisionShape()->computeAABB(bodyAABB, transform * collider->getLocalToBodyTransform()); // For each collider of the body - for (uint i=1; i < colliderEntities.size(); i++) { + const uint32 nbColliderEntities = colliderEntities.size(); + for (uint32 i=1; i < nbColliderEntities; i++) { Collider* collider = mWorld.mCollidersComponents.getCollider(colliderEntities[i]); diff --git a/src/body/RigidBody.cpp b/src/body/RigidBody.cpp index be893ff4..752e5f52 100644 --- a/src/body/RigidBody.cpp +++ b/src/body/RigidBody.cpp @@ -108,11 +108,7 @@ void RigidBody::setType(BodyType type) { setIsSleeping(false); // Update the active status of currently overlapping pairs - updateOverlappingPairs(); - - // Ask the broad-phase to test again the collision shapes of the body for collision - // detection (as if the body has moved) - askForBroadPhaseCollisionCheck(); + resetOverlappingPairs(); // Reset the force and torque on the body mWorld.mRigidBodyComponents.setExternalForce(mEntity, Vector3::zero()); @@ -328,18 +324,18 @@ Vector3 RigidBody::computeCenterOfMass() const { Vector3 centerOfMassLocal(0, 0, 0); // Compute the local center of mass - const List& colliderEntities = mWorld.mCollisionBodyComponents.getColliders(mEntity); - for (uint i=0; i < colliderEntities.size(); i++) { + const Array& colliderEntities = mWorld.mCollisionBodyComponents.getColliders(mEntity); + for (uint32 i=0; i < colliderEntities.size(); i++) { - Collider* collider = mWorld.mCollidersComponents.getCollider(colliderEntities[i]); + const uint colliderIndex = mWorld.mCollidersComponents.getEntityIndex(colliderEntities[i]); - const decimal colliderVolume = mWorld.mCollidersComponents.getCollisionShape(colliderEntities[i])->getVolume(); - const decimal colliderMassDensity = collider->getMaterial().getMassDensity(); + const decimal colliderVolume = mWorld.mCollidersComponents.mCollisionShapes[colliderIndex]->getVolume(); + const decimal colliderMassDensity = mWorld.mCollidersComponents.mMaterials[colliderIndex].getMassDensity(); const decimal colliderMass = colliderVolume * colliderMassDensity; totalMass += colliderMass; - centerOfMassLocal += colliderMass * mWorld.mCollidersComponents.getLocalToBodyTransform(colliderEntities[i]).getPosition(); + centerOfMassLocal += colliderMass * mWorld.mCollidersComponents.mLocalToBodyTransforms[colliderIndex].getPosition(); } if (totalMass > decimal(0.0)) { @@ -360,22 +356,22 @@ void RigidBody::computeMassAndInertiaTensorLocal(Vector3& inertiaTensorLocal, de const Vector3 centerOfMassLocal = mWorld.mRigidBodyComponents.getCenterOfMassLocal(mEntity); // Compute the inertia tensor using all the colliders - const List& colliderEntities = mWorld.mCollisionBodyComponents.getColliders(mEntity); - for (uint i=0; i < colliderEntities.size(); i++) { + const Array& colliderEntities = mWorld.mCollisionBodyComponents.getColliders(mEntity); + for (uint32 i=0; i < colliderEntities.size(); i++) { - Collider* collider = mWorld.mCollidersComponents.getCollider(colliderEntities[i]); + const uint colliderIndex = mWorld.mCollidersComponents.getEntityIndex(colliderEntities[i]); - const decimal colliderVolume = mWorld.mCollidersComponents.getCollisionShape(colliderEntities[i])->getVolume(); - const decimal colliderMassDensity = collider->getMaterial().getMassDensity(); + const decimal colliderVolume = mWorld.mCollidersComponents.mCollisionShapes[colliderIndex]->getVolume(); + const decimal colliderMassDensity = mWorld.mCollidersComponents.mMaterials[colliderIndex].getMassDensity(); const decimal colliderMass = colliderVolume * colliderMassDensity; totalMass += colliderMass; // Get the inertia tensor of the collider in its local-space - Vector3 shapeLocalInertiaTensor = collider->getCollisionShape()->getLocalInertiaTensor(colliderMass); + Vector3 shapeLocalInertiaTensor = mWorld.mCollidersComponents.mCollisionShapes[colliderIndex]->getLocalInertiaTensor(colliderMass); // Convert the collider inertia tensor into the local-space of the body - const Transform& shapeTransform = collider->getLocalToBodyTransform(); + const Transform& shapeTransform = mWorld.mCollidersComponents.mLocalToBodyTransforms[colliderIndex]; Matrix3x3 rotationMatrix = shapeTransform.getOrientation().getMatrix(); Matrix3x3 rotationMatrixTranspose = rotationMatrix.getTranspose(); rotationMatrixTranspose[0] *= shapeLocalInertiaTensor.x; @@ -435,12 +431,13 @@ void RigidBody::updateMassFromColliders() { decimal totalMass = decimal(0.0); // Compute the total mass of the body - const List& colliderEntities = mWorld.mCollisionBodyComponents.getColliders(mEntity); - for (uint i=0; i < colliderEntities.size(); i++) { - Collider* collider = mWorld.mCollidersComponents.getCollider(colliderEntities[i]); + const Array& colliderEntities = mWorld.mCollisionBodyComponents.getColliders(mEntity); + for (uint32 i=0; i < colliderEntities.size(); i++) { - const decimal colliderVolume = mWorld.mCollidersComponents.getCollisionShape(colliderEntities[i])->getVolume(); - const decimal colliderMassDensity = collider->getMaterial().getMassDensity(); + const uint colliderIndex = mWorld.mCollidersComponents.getEntityIndex(colliderEntities[i]); + + const decimal colliderVolume = mWorld.mCollidersComponents.mCollisionShapes[colliderIndex]->getVolume(); + const decimal colliderMassDensity = mWorld.mCollidersComponents.mMaterials[colliderIndex].getMassDensity(); const decimal colliderMass = colliderVolume * colliderMassDensity; @@ -589,8 +586,9 @@ Collider* RigidBody::addCollider(CollisionShape* collisionShape, const Transform // TODO : Maybe this method can directly returns an AABB collisionShape->getLocalBounds(localBoundsMin, localBoundsMax); const Transform localToWorldTransform = mWorld.mTransformComponents.getTransform(mEntity) * transform; + Material material(mWorld.mConfig.defaultFrictionCoefficient, mWorld.mConfig.defaultBounciness); ColliderComponents::ColliderComponent colliderComponent(mEntity, collider, AABB(localBoundsMin, localBoundsMax), - transform, collisionShape, 0x0001, 0xFFFF, localToWorldTransform); + transform, collisionShape, 0x0001, 0xFFFF, localToWorldTransform, material); bool isSleeping = mWorld.mRigidBodyComponents.getIsSleeping(mEntity); mWorld.mCollidersComponents.addComponent(colliderEntity, isSleeping, colliderComponent); @@ -854,7 +852,7 @@ void RigidBody::setIsSleeping(bool isSleeping) { mWorld.setBodyDisabled(mEntity, isSleeping); // Update the currently overlapping pairs - updateOverlappingPairs(); + resetOverlappingPairs(); if (isSleeping) { @@ -869,33 +867,25 @@ void RigidBody::setIsSleeping(bool isSleeping) { (isSleeping ? "true" : "false"), __FILE__, __LINE__); } -// Update whether the current overlapping pairs where this body is involed are active or not -void RigidBody::updateOverlappingPairs() { +// Remove all the overlapping pairs in which this body is involved and ask the broad-phase to recompute pairs in the next frame +void RigidBody::resetOverlappingPairs() { // For each collider of the body - const List& colliderEntities = mWorld.mCollisionBodyComponents.getColliders(mEntity); - for (uint i=0; i < colliderEntities.size(); i++) { + const Array& colliderEntities = mWorld.mCollisionBodyComponents.getColliders(mEntity); + for (uint32 i=0; i < colliderEntities.size(); i++) { // Get the currently overlapping pairs for this collider - List overlappingPairs = mWorld.mCollidersComponents.getOverlappingPairs(colliderEntities[i]); + Array overlappingPairs = mWorld.mCollidersComponents.getOverlappingPairs(colliderEntities[i]); - for (uint j=0; j < overlappingPairs.size(); j++) { + const uint32 nbOverlappingPairs = overlappingPairs.size(); + for (uint32 j=0; j < nbOverlappingPairs; j++) { - mWorld.mCollisionDetection.mOverlappingPairs.updateOverlappingPairIsActive(overlappingPairs[j]); + mWorld.mCollisionDetection.mOverlappingPairs.removePair(overlappingPairs[j]); } } -} -/// Return the inverse of the inertia tensor in world coordinates. -const Matrix3x3 RigidBody::getWorldInertiaTensorInverse(PhysicsWorld& world, Entity bodyEntity) { - - Matrix3x3 orientation = world.mTransformComponents.getTransform(bodyEntity).getOrientation().getMatrix(); - const Vector3& inverseInertiaLocalTensor = world.mRigidBodyComponents.getInertiaTensorLocalInverse(bodyEntity); - Matrix3x3 orientationTranspose = orientation.getTranspose(); - orientationTranspose[0] *= inverseInertiaLocalTensor.x; - orientationTranspose[1] *= inverseInertiaLocalTensor.y; - orientationTranspose[2] *= inverseInertiaLocalTensor.z; - return orientation * orientationTranspose; + // Make sure we recompute the overlapping pairs with this body in the next frame + askForBroadPhaseCollisionCheck(); } // Set whether or not the body is allowed to go to sleep @@ -952,8 +942,8 @@ void RigidBody::setProfiler(Profiler* profiler) { CollisionBody::setProfiler(profiler); // Set the profiler for each collider - const List& colliderEntities = mWorld.mCollisionBodyComponents.getColliders(mEntity); - for (uint i=0; i < colliderEntities.size(); i++) { + const Array& colliderEntities = mWorld.mCollisionBodyComponents.getColliders(mEntity); + for (uint32 i=0; i < colliderEntities.size(); i++) { Collider* collider = mWorld.mCollidersComponents.getCollider(colliderEntities[i]); diff --git a/src/collision/Collider.cpp b/src/collision/Collider.cpp index 1ae22bdb..514f101b 100644 --- a/src/collision/Collider.cpp +++ b/src/collision/Collider.cpp @@ -42,8 +42,7 @@ using namespace reactphysics3d; */ Collider::Collider(Entity entity, CollisionBody* body, MemoryManager& memoryManager) :mMemoryManager(memoryManager), mEntity(entity), mBody(body), - mMaterial(body->mWorld.mConfig.defaultFrictionCoefficient, body->mWorld.mConfig.defaultRollingRestistance, - body->mWorld.mConfig.defaultBounciness), mUserData(nullptr) { + mUserData(nullptr) { } @@ -168,7 +167,7 @@ const Transform& Collider::getLocalToBodyTransform() const { // Raycast method with feedback information /** - * @param ray Ray to use for the raycasting + * @param ray Ray to use for the raycasting in world-space * @param[out] raycastInfo Result of the raycasting that is valid only if the * methods returned true * @return True if the ray hits the collision shape @@ -181,9 +180,7 @@ bool Collider::raycast(const Ray& ray, RaycastInfo& raycastInfo) { // Convert the ray into the local-space of the collision shape const Transform localToWorldTransform = mBody->mWorld.mCollidersComponents.getLocalToWorldTransform(mEntity); const Transform worldToLocalTransform = localToWorldTransform.getInverse(); - Ray rayLocal(worldToLocalTransform * ray.point1, - worldToLocalTransform * ray.point2, - ray.maxFraction); + Ray rayLocal(worldToLocalTransform * ray.point1, worldToLocalTransform * ray.point2, ray.maxFraction); const CollisionShape* collisionShape = mBody->mWorld.mCollidersComponents.getCollisionShape(mEntity); bool isHit = collisionShape->raycast(rayLocal, raycastInfo, this, mMemoryManager.getPoolAllocator()); @@ -223,10 +220,10 @@ void Collider::setHasCollisionShapeChangedSize(bool hasCollisionShapeChangedSize */ void Collider::setMaterial(const Material& material) { - mMaterial = material; + mBody->mWorld.mCollidersComponents.setMaterial(mEntity, material); RP3D_LOG(mBody->mWorld.mConfig.worldName, Logger::Level::Information, Logger::Category::Collider, - "Collider " + std::to_string(mEntity.id) + ": Set Material" + mMaterial.to_string(), __FILE__, __LINE__); + "Collider " + std::to_string(mEntity.id) + ": Set Material" + material.to_string(), __FILE__, __LINE__); } // Return the local to world transform @@ -254,6 +251,14 @@ void Collider::setIsTrigger(bool isTrigger) const { mBody->mWorld.mCollidersComponents.setIsTrigger(mEntity, isTrigger); } +// Return a reference to the material properties of the collider +/** + * @return A reference to the material of the body + */ +Material& Collider::getMaterial() { + return mBody->mWorld.mCollidersComponents.getMaterial(mEntity); +} + #ifdef IS_RP3D_PROFILING_ENABLED diff --git a/src/collision/CollisionCallback.cpp b/src/collision/CollisionCallback.cpp index 88779023..eb792ab5 100644 --- a/src/collision/CollisionCallback.cpp +++ b/src/collision/CollisionCallback.cpp @@ -39,7 +39,7 @@ CollisionCallback::ContactPoint::ContactPoint(const reactphysics3d::ContactPoint // Contact Pair Constructor CollisionCallback::ContactPair::ContactPair(const reactphysics3d::ContactPair& contactPair, - List* contactPoints, PhysicsWorld& world, bool isLostContactPair) + Array* contactPoints, PhysicsWorld& world, bool isLostContactPair) :mContactPair(contactPair), mContactPoints(contactPoints), mWorld(world), mIsLostContactPair(isLostContactPair) { @@ -76,13 +76,15 @@ CollisionCallback::ContactPair::EventType CollisionCallback::ContactPair::getEve } // Constructor -CollisionCallback::CallbackData::CallbackData(List* contactPairs, List* manifolds, - List* contactPoints, List& lostContactPairs, PhysicsWorld& world) +CollisionCallback::CallbackData::CallbackData(Array* contactPairs, Array* manifolds, + Array* contactPoints, Array& lostContactPairs, PhysicsWorld& world) :mContactPairs(contactPairs), mContactManifolds(manifolds), mContactPoints(contactPoints), mLostContactPairs(lostContactPairs), - mContactPairsIndices(world.mMemoryManager.getHeapAllocator()), mLostContactPairsIndices(world.mMemoryManager.getHeapAllocator()), mWorld(world) { + mContactPairsIndices(world.mMemoryManager.getHeapAllocator(), contactPairs->size()), mLostContactPairsIndices(world.mMemoryManager.getHeapAllocator(), lostContactPairs.size()), + mWorld(world) { // Filter the contact pairs to only keep the contact events (not the overlap/trigger events) - for (uint i=0; i < mContactPairs->size(); i++) { + const uint32 nbContactPairs = mContactPairs->size(); + for (uint32 i=0; i < nbContactPairs; i++) { // If the contact pair contains contacts (and is therefore not an overlap/trigger event) if (!(*mContactPairs)[i].isTrigger) { @@ -90,7 +92,8 @@ CollisionCallback::CallbackData::CallbackData(List* } } // Filter the lost contact pairs to only keep the contact events (not the overlap/trigger events) - for (uint i=0; i < mLostContactPairs.size(); i++) { + const uint32 nbLostContactPairs = mLostContactPairs.size(); + for (uint32 i=0; i < nbLostContactPairs; i++) { // If the contact pair contains contacts (and is therefore not an overlap/trigger event) if (!mLostContactPairs[i].isTrigger) { @@ -114,7 +117,7 @@ CollisionCallback::ContactPoint CollisionCallback::ContactPair::getContactPoint( /// Note that the returned ContactPair object is only valid during the call of the CollisionCallback::onContact() /// method. Therefore, you need to get contact data from it and make a copy. Do not make a copy of the ContactPair /// object itself because it won't be valid after the CollisionCallback::onContact() call. -CollisionCallback::ContactPair CollisionCallback::CallbackData::getContactPair(uint index) const { +CollisionCallback::ContactPair CollisionCallback::CallbackData::getContactPair(uint32 index) const { assert(index < getNbContactPairs()); diff --git a/src/collision/ContactManifold.cpp b/src/collision/ContactManifold.cpp index 10c5c957..46f886de 100644 --- a/src/collision/ContactManifold.cpp +++ b/src/collision/ContactManifold.cpp @@ -32,14 +32,9 @@ using namespace reactphysics3d; // Constructor ContactManifold::ContactManifold(Entity bodyEntity1, Entity bodyEntity2, Entity colliderEntity1, Entity colliderEntity2, - uint contactPointsIndex, int8 nbContactPoints) + uint contactPointsIndex, uint8 nbContactPoints) :contactPointsIndex(contactPointsIndex), bodyEntity1(bodyEntity1), bodyEntity2(bodyEntity2), colliderEntity1(colliderEntity1), colliderEntity2(colliderEntity2), nbContactPoints(nbContactPoints), frictionImpulse1(0), frictionImpulse2(0), frictionTwistImpulse(0), isAlreadyInIsland(false) { } - -// Destructor -ContactManifold::~ContactManifold() { - -} diff --git a/src/collision/HalfEdgeStructure.cpp b/src/collision/HalfEdgeStructure.cpp index b1da7ef8..e71e35e1 100644 --- a/src/collision/HalfEdgeStructure.cpp +++ b/src/collision/HalfEdgeStructure.cpp @@ -36,24 +36,26 @@ void HalfEdgeStructure::init() { Map edges(mAllocator); Map nextEdges(mAllocator); - Map mapEdgeToStartVertex(mAllocator); - Map mapEdgeToIndex(mAllocator); - Map mapEdgeIndexToKey(mAllocator); - Map mapFaceIndexToEdgeKey(mAllocator); + Map mapEdgeToStartVertex(mAllocator); + Map mapEdgeToIndex(mAllocator); + Map mapEdgeIndexToKey(mAllocator); + Map mapFaceIndexToEdgeKey(mAllocator); - List currentFaceEdges(mAllocator, mFaces[0].faceVertices.size()); + Array currentFaceEdges(mAllocator, mFaces[0].faceVertices.size()); // For each face - for (uint f=0; f(pairV1V2, v1Index), true); - mapEdgeToStartVertex.add(Pair(pairV2V1, v2Index), true); + mapEdgeToStartVertex.add(Pair(pairV1V2, v1Index), true); + mapEdgeToStartVertex.add(Pair(pairV2V1, v2Index), true); - mapFaceIndexToEdgeKey.add(Pair(f, pairV1V2), true); + mapFaceIndexToEdgeKey.add(Pair(f, pairV1V2), true); auto itEdge = edges.find(pairV2V1); if (itEdge != edges.end()) { - const uint edgeIndex = mEdges.size(); + const uint32 edgeIndex = mEdges.size(); itEdge->second.twinEdgeIndex = edgeIndex + 1; edge.twinEdgeIndex = edgeIndex; - mapEdgeIndexToKey.add(Pair(edgeIndex, pairV2V1)); - mapEdgeIndexToKey.add(Pair(edgeIndex + 1, pairV1V2)); + mapEdgeIndexToKey.add(Pair(edgeIndex, pairV2V1)); + mapEdgeIndexToKey.add(Pair(edgeIndex + 1, pairV1V2)); mVertices[v1Index].edgeIndex = edgeIndex + 1; mVertices[v2Index].edgeIndex = edgeIndex; - mapEdgeToIndex.add(Pair(pairV1V2, edgeIndex + 1)); - mapEdgeToIndex.add(Pair(pairV2V1, edgeIndex)); + mapEdgeToIndex.add(Pair(pairV1V2, edgeIndex + 1)); + mapEdgeToIndex.add(Pair(pairV2V1, edgeIndex)); mEdges.add(itEdge->second); mEdges.add(edge); @@ -107,12 +109,13 @@ void HalfEdgeStructure::init() { } // Set next edges - for (uint i=0; i < mEdges.size(); i++) { + const uint32 nbEdges = mEdges.size(); + for (uint32 i=0; i < nbEdges; i++) { mEdges[i].nextEdgeIndex = mapEdgeToIndex[nextEdges[mapEdgeIndexToKey[i]]]; } // Set face edge - for (uint f=0; f < mFaces.size(); f++) { + for (uint32 f=0; f < nbFaces; f++) { mFaces[f].edgeIndex = mapEdgeToIndex[mapFaceIndexToEdgeKey[f]]; } } diff --git a/src/collision/OverlapCallback.cpp b/src/collision/OverlapCallback.cpp index 5cc9b571..67fbf069 100644 --- a/src/collision/OverlapCallback.cpp +++ b/src/collision/OverlapCallback.cpp @@ -67,12 +67,13 @@ OverlapCallback::OverlapPair::EventType OverlapCallback::OverlapPair::getEventTy } // CollisionCallbackData Constructor -OverlapCallback::CallbackData::CallbackData(List& contactPairs, List& lostContactPairs, bool onlyReportTriggers, PhysicsWorld& world) +OverlapCallback::CallbackData::CallbackData(Array& contactPairs, Array& lostContactPairs, bool onlyReportTriggers, PhysicsWorld& world) :mContactPairs(contactPairs), mLostContactPairs(lostContactPairs), mContactPairsIndices(world.mMemoryManager.getHeapAllocator()), mLostContactPairsIndices(world.mMemoryManager.getHeapAllocator()), mWorld(world) { // Filter the contact pairs to only keep the overlap/trigger events (not the contact events) - for (uint i=0; i < mContactPairs.size(); i++) { + const uint32 nbContactPairs = mContactPairs.size(); + for (uint32 i=0; i < nbContactPairs; i++) { // If the contact pair contains contacts (and is therefore not an overlap/trigger event) if (!onlyReportTriggers || mContactPairs[i].isTrigger) { @@ -80,7 +81,8 @@ OverlapCallback::CallbackData::CallbackData(List& contactPairs, Lis } } // Filter the lost contact pairs to only keep the overlap/trigger events (not the contact events) - for (uint i=0; i < mLostContactPairs.size(); i++) { + const uint32 nbLostContactPairs = mLostContactPairs.size(); + for (uint32 i=0; i < nbLostContactPairs; i++) { // If the contact pair contains contacts (and is therefore not an overlap/trigger event) if (!onlyReportTriggers || mLostContactPairs[i].isTrigger) { diff --git a/src/collision/PolyhedronMesh.cpp b/src/collision/PolyhedronMesh.cpp index 28ae4754..f48f0e46 100644 --- a/src/collision/PolyhedronMesh.cpp +++ b/src/collision/PolyhedronMesh.cpp @@ -27,6 +27,8 @@ #include #include #include +#include +#include #include using namespace reactphysics3d; @@ -37,59 +39,103 @@ using namespace reactphysics3d; * Create a polyhedron mesh given an array of polygons. * @param polygonVertexArray Pointer to the array of polygons and their vertices */ -PolyhedronMesh::PolyhedronMesh(PolygonVertexArray* polygonVertexArray, MemoryAllocator &allocator) +PolyhedronMesh::PolyhedronMesh(PolygonVertexArray* polygonVertexArray, MemoryAllocator& allocator) : mMemoryAllocator(allocator), mHalfEdgeStructure(allocator, polygonVertexArray->getNbFaces(), polygonVertexArray->getNbVertices(), - (polygonVertexArray->getNbFaces() + polygonVertexArray->getNbVertices() - 2) * 2) { + (polygonVertexArray->getNbFaces() + polygonVertexArray->getNbVertices() - 2) * 2), mFacesNormals(nullptr) { mPolygonVertexArray = polygonVertexArray; - - // Create the half-edge structure of the mesh - createHalfEdgeStructure(); - - // Create the face normals array - mFacesNormals = new Vector3[mHalfEdgeStructure.getNbFaces()]; - - // Compute the faces normals - computeFacesNormals(); - - // Compute the centroid - computeCentroid(); } // Destructor PolyhedronMesh::~PolyhedronMesh() { - delete[] mFacesNormals; + + if (mFacesNormals != nullptr) { + + for (uint f=0; f < mHalfEdgeStructure.getNbFaces(); f++) { + mFacesNormals[f].~Vector3(); + } + + mMemoryAllocator.release(mFacesNormals, mHalfEdgeStructure.getNbFaces() * sizeof(Vector3)); + } +} + +/// Static factory method to create a polyhedron mesh. This methods returns null_ptr if the mesh is not valid +PolyhedronMesh* PolyhedronMesh::create(PolygonVertexArray* polygonVertexArray, MemoryAllocator& polyhedronMeshAllocator, MemoryAllocator& dataAllocator) { + + PolyhedronMesh* mesh = new (polyhedronMeshAllocator.allocate(sizeof(PolyhedronMesh))) PolyhedronMesh(polygonVertexArray, dataAllocator); + + // Create the half-edge structure of the mesh + bool isValid = mesh->createHalfEdgeStructure(); + + if (isValid) { + + // Compute the faces normals + mesh->computeFacesNormals(); + + // Compute the centroid + mesh->computeCentroid(); + } + else { + mesh->~PolyhedronMesh(); + polyhedronMeshAllocator.release(mesh, sizeof(PolyhedronMesh)); + mesh = nullptr; + } + + return mesh; } // Create the half-edge structure of the mesh -void PolyhedronMesh::createHalfEdgeStructure() { +/// This method returns true if the mesh is valid or false otherwise +bool PolyhedronMesh::createHalfEdgeStructure() { // For each vertex of the mesh for (uint v=0; v < mPolygonVertexArray->getNbVertices(); v++) { mHalfEdgeStructure.addVertex(v); } + uint32 nbEdges = 0; + // For each polygon face of the mesh for (uint f=0; f < mPolygonVertexArray->getNbFaces(); f++) { // Get the polygon face PolygonVertexArray::PolygonFace* face = mPolygonVertexArray->getPolygonFace(f); - List faceVertices(mMemoryAllocator, face->nbVertices); + Array faceVertices(mMemoryAllocator, face->nbVertices); // For each vertex of the face for (uint v=0; v < face->nbVertices; v++) { faceVertices.add(mPolygonVertexArray->getVertexIndexInFace(f, v)); } + nbEdges += face->nbVertices; + assert(faceVertices.size() >= 3); // Addd the face into the half-edge structure mHalfEdgeStructure.addFace(faceVertices); } + nbEdges /= 2; + + // If the mesh is valid (check Euler formula V + F - E = 2) and does not use duplicated vertices + if (2 + nbEdges - mPolygonVertexArray->getNbFaces() != mPolygonVertexArray->getNbVertices()) { + + RP3D_LOG("PhysicsCommon", Logger::Level::Error, Logger::Category::PhysicCommon, + "Error when creating a PolyhedronMesh: input PolygonVertexArray is not valid. Mesh with duplicated vertices is not supported.", __FILE__, __LINE__); + + assert(false); + + return false; + } + // Initialize the half-edge structure mHalfEdgeStructure.init(); + + // Create the face normals array + mFacesNormals = new (mMemoryAllocator.allocate(mHalfEdgeStructure.getNbFaces() * sizeof(Vector3))) Vector3[mHalfEdgeStructure.getNbFaces()]; + + return true; } /// Return a vertex @@ -131,7 +177,8 @@ Vector3 PolyhedronMesh::getVertex(uint index) const { void PolyhedronMesh::computeFacesNormals() { // For each face - for (uint f=0; f < mHalfEdgeStructure.getNbFaces(); f++) { + const uint32 nbFaces = mHalfEdgeStructure.getNbFaces(); + for (uint f=0; f < nbFaces; f++) { const HalfEdgeStructure::Face& face = mHalfEdgeStructure.getFace(f); assert(face.faceVertices.size() >= 3); @@ -166,7 +213,8 @@ decimal PolyhedronMesh::getFaceArea(uint faceIndex) const { Vector3 v1 = getVertex(face.faceVertices[0]); // For each vertex of the face - for (uint i=2; i < face.faceVertices.size(); i++) { + const uint32 nbFaceVertices = face.faceVertices.size(); + for (uint32 i=2; i < nbFaceVertices; i++) { const Vector3 v2 = getVertex(face.faceVertices[i-1]); const Vector3 v3 = getVertex(face.faceVertices[i]); diff --git a/src/collision/broadphase/DynamicAABBTree.cpp b/src/collision/broadphase/DynamicAABBTree.cpp index a5a7ad21..e01fbe16 100644 --- a/src/collision/broadphase/DynamicAABBTree.cpp +++ b/src/collision/broadphase/DynamicAABBTree.cpp @@ -573,9 +573,9 @@ int32 DynamicAABBTree::balanceSubTreeAtNode(int32 nodeID) { return nodeID; } -/// Take a list of shapes to be tested for broad-phase overlap and return a list of pair of overlapping shapes -void DynamicAABBTree::reportAllShapesOverlappingWithShapes(const List& nodesToTest, size_t startIndex, - size_t endIndex, List>& outOverlappingNodes) const { +/// Take an array of shapes to be tested for broad-phase overlap and return an array of pair of overlapping shapes +void DynamicAABBTree::reportAllShapesOverlappingWithShapes(const Array& nodesToTest, size_t startIndex, + size_t endIndex, Array>& outOverlappingNodes) const { RP3D_PROFILE("DynamicAABBTree::reportAllShapesOverlappingWithAABB()", mProfiler); @@ -609,7 +609,7 @@ void DynamicAABBTree::reportAllShapesOverlappingWithShapes(const List& no // If the node is a leaf if (nodeToVisit->isLeaf()) { - // Add the node in the list of overlapping nodes + // Add the node in the array of overlapping nodes outOverlappingNodes.add(Pair(nodesToTest[i], nodeIDToVisit)); } else { // If the node is not a leaf @@ -626,7 +626,7 @@ void DynamicAABBTree::reportAllShapesOverlappingWithShapes(const List& no } // Report all shapes overlapping with the AABB given in parameter. -void DynamicAABBTree::reportAllShapesOverlappingWithAABB(const AABB& aabb, List& overlappingNodes) const { +void DynamicAABBTree::reportAllShapesOverlappingWithAABB(const AABB& aabb, Array& overlappingNodes) const { RP3D_PROFILE("DynamicAABBTree::reportAllShapesOverlappingWithAABB()", mProfiler); @@ -640,6 +640,9 @@ void DynamicAABBTree::reportAllShapesOverlappingWithAABB(const AABB& aabb, List< // Get the next node ID to visit const int32 nodeIDToVisit = stack.pop(); + assert(nodeIDToVisit >= 0); + assert(nodeIDToVisit < mNbAllocatedNodes); + // Skip it if it is a null node if (nodeIDToVisit == TreeNode::NULL_TREE_NODE) continue; @@ -672,6 +675,10 @@ void DynamicAABBTree::raycast(const Ray& ray, DynamicAABBTreeRaycastCallback& ca decimal maxFraction = ray.maxFraction; + // Compute the inverse ray direction + const Vector3 rayDirection = ray.point2 - ray.point1; + const Vector3 rayDirectionInverse(decimal(1.0) / rayDirection.x, decimal(1.0) / rayDirection.y, decimal(1.0) / rayDirection.z); + Stack stack(mAllocator, 128); stack.push(mRootNodeID); @@ -688,14 +695,14 @@ void DynamicAABBTree::raycast(const Ray& ray, DynamicAABBTreeRaycastCallback& ca // Get the corresponding node const TreeNode* node = mNodes + nodeID; - Ray rayTemp(ray.point1, ray.point2, maxFraction); - // Test if the ray intersects with the current node AABB - if (!node->aabb.testRayIntersect(rayTemp)) continue; + if (!node->aabb.testRayIntersect(ray.point1, rayDirectionInverse, maxFraction)) continue; // If the node is a leaf of the tree if (node->isLeaf()) { + Ray rayTemp(ray.point1, ray.point2, maxFraction); + // Call the callback that will raycast again the broad-phase shape decimal hitFraction = callback.raycastBroadPhaseShape(nodeID, rayTemp); diff --git a/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.cpp b/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.cpp index 4fa4595f..aa201ac8 100755 --- a/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.cpp +++ b/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.cpp @@ -26,7 +26,7 @@ // Libraries #include #include -#include +#include // We want to use the ReactPhysics3D namespace using namespace reactphysics3d; @@ -34,30 +34,37 @@ using namespace reactphysics3d; // Compute the narrow-phase collision detection between two capsules // This technique is based on the "Robust Contact Creation for Physics Simulations" presentation // by Dirk Gregorius. -bool CapsuleVsCapsuleAlgorithm::testCollision(CapsuleVsCapsuleNarrowPhaseInfoBatch& narrowPhaseInfoBatch, uint batchStartIndex, uint batchNbItems, - MemoryAllocator& memoryAllocator) { +bool CapsuleVsCapsuleAlgorithm::testCollision(NarrowPhaseInfoBatch& narrowPhaseInfoBatch, uint batchStartIndex, uint batchNbItems, MemoryAllocator& memoryAllocator) { bool isCollisionFound = false; for (uint batchIndex = batchStartIndex; batchIndex < batchStartIndex + batchNbItems; batchIndex++) { - assert(narrowPhaseInfoBatch.contactPoints[batchIndex].size() == 0); + assert(narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].nbContactPoints == 0); - assert(!narrowPhaseInfoBatch.isColliding[batchIndex]); + assert(!narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].isColliding); // Get the transform from capsule 1 local-space to capsule 2 local-space - const Transform capsule1ToCapsule2SpaceTransform = narrowPhaseInfoBatch.shape2ToWorldTransforms[batchIndex].getInverse() * - narrowPhaseInfoBatch.shape1ToWorldTransforms[batchIndex]; + const Transform capsule1ToCapsule2SpaceTransform = narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].shape2ToWorldTransform.getInverse() * + narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].shape1ToWorldTransform; + + const CapsuleShape* capsuleShape1 = static_cast(narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape1); + const CapsuleShape* capsuleShape2 = static_cast(narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape2); + + const decimal capsule1Height = capsuleShape1->getHeight(); + const decimal capsule2Height = capsuleShape2->getHeight(); + const decimal capsule1Radius = capsuleShape1->getRadius(); + const decimal capsule2Radius = capsuleShape2->getRadius(); // Compute the end-points of the inner segment of the first capsule - const decimal capsule1HalfHeight = narrowPhaseInfoBatch.capsule1Heights[batchIndex] * decimal(0.5); + const decimal capsule1HalfHeight = capsule1Height * decimal(0.5); Vector3 capsule1SegA(0, -capsule1HalfHeight, 0); Vector3 capsule1SegB(0, capsule1HalfHeight, 0); capsule1SegA = capsule1ToCapsule2SpaceTransform * capsule1SegA; capsule1SegB = capsule1ToCapsule2SpaceTransform * capsule1SegB; // Compute the end-points of the inner segment of the second capsule - const decimal capsule2HalfHeight = narrowPhaseInfoBatch.capsule2Heights[batchIndex] * decimal(0.5); + const decimal capsule2HalfHeight = capsule2Height * decimal(0.5); const Vector3 capsule2SegA(0, -capsule2HalfHeight, 0); const Vector3 capsule2SegB(0, capsule2HalfHeight, 0); @@ -66,8 +73,6 @@ bool CapsuleVsCapsuleAlgorithm::testCollision(CapsuleVsCapsuleNarrowPhaseInfoBat const Vector3 seg2 = capsule2SegB - capsule2SegA; // Compute the sum of the radius of the two capsules (virtual spheres) - const decimal capsule1Radius = narrowPhaseInfoBatch.capsule1Radiuses[batchIndex]; - const decimal capsule2Radius = narrowPhaseInfoBatch.capsule2Radiuses[batchIndex]; const decimal sumRadius = capsule1Radius + capsule2Radius; // If the two capsules are parallel (we create two contact points) @@ -95,7 +100,7 @@ bool CapsuleVsCapsuleAlgorithm::testCollision(CapsuleVsCapsuleNarrowPhaseInfoBat // If the segments were overlapping (the clip segment is valid) if (t1 > decimal(0.0) && t2 > decimal(0.0)) { - if (narrowPhaseInfoBatch.reportContacts[batchIndex]) { + if (narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].reportContacts) { // Clip the inner segment of capsule 2 if (t1 > decimal(1.0)) t1 = decimal(1.0); @@ -145,14 +150,14 @@ bool CapsuleVsCapsuleAlgorithm::testCollision(CapsuleVsCapsuleNarrowPhaseInfoBat decimal penetrationDepth = sumRadius - segmentsPerpendicularDistance; - const Vector3 normalWorld = narrowPhaseInfoBatch.shape2ToWorldTransforms[batchIndex].getOrientation() * normalCapsule2SpaceNormalized; + const Vector3 normalWorld = narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].shape2ToWorldTransform.getOrientation() * normalCapsule2SpaceNormalized; // Create the contact info object narrowPhaseInfoBatch.addContactPoint(batchIndex, normalWorld, penetrationDepth, contactPointACapsule1Local, contactPointACapsule2Local); narrowPhaseInfoBatch.addContactPoint(batchIndex, normalWorld, penetrationDepth, contactPointBCapsule1Local, contactPointBCapsule2Local); } - narrowPhaseInfoBatch.isColliding[batchIndex] = true; + narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].isColliding = true; isCollisionFound = true; continue; } @@ -171,7 +176,7 @@ bool CapsuleVsCapsuleAlgorithm::testCollision(CapsuleVsCapsuleNarrowPhaseInfoBat // If the collision shapes overlap if (closestPointsDistanceSquare < sumRadius * sumRadius) { - if (narrowPhaseInfoBatch.reportContacts[batchIndex]) { + if (narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].reportContacts) { // If the distance between the inner segments is not zero if (closestPointsDistanceSquare > MACHINE_EPSILON) { @@ -182,7 +187,7 @@ bool CapsuleVsCapsuleAlgorithm::testCollision(CapsuleVsCapsuleNarrowPhaseInfoBat const Vector3 contactPointCapsule1Local = capsule1ToCapsule2SpaceTransform.getInverse() * (closestPointCapsule1Seg + closestPointsSeg1ToSeg2 * capsule1Radius); const Vector3 contactPointCapsule2Local = closestPointCapsule2Seg - closestPointsSeg1ToSeg2 * capsule2Radius; - const Vector3 normalWorld = narrowPhaseInfoBatch.shape2ToWorldTransforms[batchIndex].getOrientation() * closestPointsSeg1ToSeg2; + const Vector3 normalWorld = narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].shape2ToWorldTransform.getOrientation() * closestPointsSeg1ToSeg2; decimal penetrationDepth = sumRadius - closestPointsDistance; @@ -205,7 +210,7 @@ bool CapsuleVsCapsuleAlgorithm::testCollision(CapsuleVsCapsuleNarrowPhaseInfoBat const Vector3 contactPointCapsule1Local = capsule1ToCapsule2SpaceTransform.getInverse() * (closestPointCapsule1Seg + normalCapsuleSpace2 * capsule1Radius); const Vector3 contactPointCapsule2Local = closestPointCapsule2Seg - normalCapsuleSpace2 * capsule2Radius; - const Vector3 normalWorld = narrowPhaseInfoBatch.shape2ToWorldTransforms[batchIndex].getOrientation() * normalCapsuleSpace2; + const Vector3 normalWorld = narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].shape2ToWorldTransform.getOrientation() * normalCapsuleSpace2; // Create the contact info object narrowPhaseInfoBatch.addContactPoint(batchIndex, normalWorld, sumRadius, contactPointCapsule1Local, contactPointCapsule2Local); @@ -221,7 +226,7 @@ bool CapsuleVsCapsuleAlgorithm::testCollision(CapsuleVsCapsuleNarrowPhaseInfoBat const Vector3 contactPointCapsule1Local = capsule1ToCapsule2SpaceTransform.getInverse() * (closestPointCapsule1Seg + normalCapsuleSpace2 * capsule1Radius); const Vector3 contactPointCapsule2Local = closestPointCapsule2Seg - normalCapsuleSpace2 * capsule2Radius; - const Vector3 normalWorld = narrowPhaseInfoBatch.shape2ToWorldTransforms[batchIndex].getOrientation() * normalCapsuleSpace2; + const Vector3 normalWorld = narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].shape2ToWorldTransform.getOrientation() * normalCapsuleSpace2; // Create the contact info object narrowPhaseInfoBatch.addContactPoint(batchIndex, normalWorld, sumRadius, contactPointCapsule1Local, contactPointCapsule2Local); @@ -229,7 +234,7 @@ bool CapsuleVsCapsuleAlgorithm::testCollision(CapsuleVsCapsuleNarrowPhaseInfoBat } } - narrowPhaseInfoBatch.isColliding[batchIndex] = true; + narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].isColliding = true; isCollisionFound = true; } } diff --git a/src/collision/narrowphase/CapsuleVsCapsuleNarrowPhaseInfoBatch.cpp b/src/collision/narrowphase/CapsuleVsCapsuleNarrowPhaseInfoBatch.cpp deleted file mode 100644 index 6af03018..00000000 --- a/src/collision/narrowphase/CapsuleVsCapsuleNarrowPhaseInfoBatch.cpp +++ /dev/null @@ -1,84 +0,0 @@ -/******************************************************************************** -* ReactPhysics3D physics library, http://www.reactphysics3d.com * -* Copyright (c) 2010-2020 Daniel Chappuis * -********************************************************************************* -* * -* This software is provided 'as-is', without any express or implied warranty. * -* In no event will the authors be held liable for any damages arising from the * -* use of this software. * -* * -* Permission is granted to anyone to use this software for any purpose, * -* including commercial applications, and to alter it and redistribute it * -* freely, subject to the following restrictions: * -* * -* 1. The origin of this software must not be misrepresented; you must not claim * -* that you wrote the original software. If you use this software in a * -* product, an acknowledgment in the product documentation would be * -* appreciated but is not required. * -* * -* 2. Altered source versions must be plainly marked as such, and must not be * -* misrepresented as being the original software. * -* * -* 3. This notice may not be removed or altered from any source distribution. * -* * -********************************************************************************/ - -// Libraries -#include -#include - -using namespace reactphysics3d; - -// Constructor -CapsuleVsCapsuleNarrowPhaseInfoBatch::CapsuleVsCapsuleNarrowPhaseInfoBatch(MemoryAllocator& allocator, - OverlappingPairs& overlappingPairs) - : NarrowPhaseInfoBatch(allocator, overlappingPairs), capsule1Radiuses(allocator), capsule2Radiuses(allocator), - capsule1Heights(allocator), capsule2Heights(allocator) { - -} - -// Add shapes to be tested during narrow-phase collision detection into the batch -void CapsuleVsCapsuleNarrowPhaseInfoBatch::addNarrowPhaseInfo(uint64 pairId, uint64 pairIndex, Entity collider1, Entity collider2, CollisionShape* shape1, CollisionShape* shape2, - const Transform& shape1Transform, const Transform& shape2Transform, bool needToReportContacts, MemoryAllocator &shapeAllocator) { - - NarrowPhaseInfoBatch::addNarrowPhaseInfo(pairId, pairIndex, collider1, collider2, shape1, shape2, shape1Transform, - shape2Transform, needToReportContacts, shapeAllocator); - - assert(shape1->getType() == CollisionShapeType::CAPSULE); - assert(shape2->getType() == CollisionShapeType::CAPSULE); - - const CapsuleShape* capsule1 = static_cast(shape1); - const CapsuleShape* capsule2 = static_cast(shape2); - - capsule1Radiuses.add(capsule1->getRadius()); - capsule2Radiuses.add(capsule2->getRadius()); - capsule1Heights.add(capsule1->getHeight()); - capsule2Heights.add(capsule2->getHeight()); -} - -// Initialize the containers using cached capacity -void CapsuleVsCapsuleNarrowPhaseInfoBatch::reserveMemory() { - - NarrowPhaseInfoBatch::reserveMemory(); - - capsule1Radiuses.reserve(mCachedCapacity); - capsule2Radiuses.reserve(mCachedCapacity); - capsule1Heights.reserve(mCachedCapacity); - capsule2Heights.reserve(mCachedCapacity); -} - -// Clear all the objects in the batch -void CapsuleVsCapsuleNarrowPhaseInfoBatch::clear() { - - // Note that we clear the following containers and we release their allocated memory. Therefore, - // if the memory allocator is a single frame allocator, the memory is deallocated and will be - // allocated in the next frame at a possibly different location in memory (remember that the - // location of the allocated memory of a single frame allocator might change between two frames) - - NarrowPhaseInfoBatch::clear(); - - capsule1Radiuses.clear(true); - capsule2Radiuses.clear(true); - capsule1Heights.clear(true); - capsule2Heights.clear(true); -} diff --git a/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp b/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp index 8e082822..b09df90f 100644 --- a/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp +++ b/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp @@ -59,28 +59,28 @@ bool CapsuleVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfoBatch& nar #endif // Run the GJK algorithm - List gjkResults(memoryAllocator); + Array gjkResults(memoryAllocator); gjkAlgorithm.testCollision(narrowPhaseInfoBatch, batchStartIndex, batchNbItems, gjkResults); assert(gjkResults.size() == batchNbItems); for (uint batchIndex = batchStartIndex; batchIndex < batchStartIndex + batchNbItems; batchIndex++) { // Get the last frame collision info - LastFrameCollisionInfo* lastFrameCollisionInfo = narrowPhaseInfoBatch.lastFrameCollisionInfos[batchIndex]; + LastFrameCollisionInfo* lastFrameCollisionInfo = narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].lastFrameCollisionInfo; lastFrameCollisionInfo->wasUsingGJK = true; lastFrameCollisionInfo->wasUsingSAT = false; - assert(narrowPhaseInfoBatch.collisionShapes1[batchIndex]->getType() == CollisionShapeType::CONVEX_POLYHEDRON || - narrowPhaseInfoBatch.collisionShapes2[batchIndex]->getType() == CollisionShapeType::CONVEX_POLYHEDRON); - assert(narrowPhaseInfoBatch.collisionShapes1[batchIndex]->getType() == CollisionShapeType::CAPSULE || - narrowPhaseInfoBatch.collisionShapes2[batchIndex]->getType() == CollisionShapeType::CAPSULE); + assert(narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape1->getType() == CollisionShapeType::CONVEX_POLYHEDRON || + narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape2->getType() == CollisionShapeType::CONVEX_POLYHEDRON); + assert(narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape1->getType() == CollisionShapeType::CAPSULE || + narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape2->getType() == CollisionShapeType::CAPSULE); // If we have found a contact point inside the margins (shallow penetration) if (gjkResults[batchIndex] == GJKAlgorithm::GJKResult::COLLIDE_IN_MARGIN) { // If we need to report contacts - if (narrowPhaseInfoBatch.reportContacts[batchIndex]) { + if (narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].reportContacts) { bool noContact = false; @@ -89,20 +89,20 @@ bool CapsuleVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfoBatch& nar // two contact points instead of a single one (as in the deep contact case with SAT algorithm) // Get the contact point created by GJK - assert(narrowPhaseInfoBatch.contactPoints[batchIndex].size() > 0); - ContactPointInfo*& contactPoint = narrowPhaseInfoBatch.contactPoints[batchIndex][0]; + assert(narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].nbContactPoints > 0); + ContactPointInfo& contactPoint = narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].contactPoints[0]; - bool isCapsuleShape1 = narrowPhaseInfoBatch.collisionShapes1[batchIndex]->getType() == CollisionShapeType::CAPSULE; + bool isCapsuleShape1 = narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape1->getType() == CollisionShapeType::CAPSULE; // Get the collision shapes - const CapsuleShape* capsuleShape = static_cast(isCapsuleShape1 ? narrowPhaseInfoBatch.collisionShapes1[batchIndex] : narrowPhaseInfoBatch.collisionShapes2[batchIndex]); - const ConvexPolyhedronShape* polyhedron = static_cast(isCapsuleShape1 ? narrowPhaseInfoBatch.collisionShapes2[batchIndex] : narrowPhaseInfoBatch.collisionShapes1[batchIndex]); + const CapsuleShape* capsuleShape = static_cast(isCapsuleShape1 ? narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape1 : narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape2); + const ConvexPolyhedronShape* polyhedron = static_cast(isCapsuleShape1 ? narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape2 : narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape1); // For each face of the polyhedron for (uint f = 0; f < polyhedron->getNbFaces(); f++) { - const Transform polyhedronToWorld = isCapsuleShape1 ? narrowPhaseInfoBatch.shape2ToWorldTransforms[batchIndex] : narrowPhaseInfoBatch.shape1ToWorldTransforms[batchIndex]; - const Transform capsuleToWorld = isCapsuleShape1 ? narrowPhaseInfoBatch.shape1ToWorldTransforms[batchIndex] : narrowPhaseInfoBatch.shape2ToWorldTransforms[batchIndex]; + const Transform polyhedronToWorld = isCapsuleShape1 ? narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].shape2ToWorldTransform : narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].shape1ToWorldTransform; + const Transform capsuleToWorld = isCapsuleShape1 ? narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].shape1ToWorldTransform : narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].shape2ToWorldTransform; // Get the face normal const Vector3 faceNormal = polyhedron->getFaceNormal(f); @@ -113,18 +113,18 @@ bool CapsuleVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfoBatch& nar Vector3 capsuleInnerSegmentDirection = capsuleToWorld.getOrientation() * (capsuleSegB - capsuleSegA); capsuleInnerSegmentDirection.normalize(); - bool isFaceNormalInDirectionOfContactNormal = faceNormalWorld.dot(contactPoint->normal) > decimal(0.0); + bool isFaceNormalInDirectionOfContactNormal = faceNormalWorld.dot(contactPoint.normal) > decimal(0.0); bool isFaceNormalInContactDirection = (isCapsuleShape1 && !isFaceNormalInDirectionOfContactNormal) || (!isCapsuleShape1 && isFaceNormalInDirectionOfContactNormal); // If the polyhedron face normal is orthogonal to the capsule inner segment and parallel to the contact point normal and the face normal // is in direction of the contact normal (from the polyhedron point of view). if (isFaceNormalInContactDirection && areOrthogonalVectors(faceNormalWorld, capsuleInnerSegmentDirection) - && areParallelVectors(faceNormalWorld, contactPoint->normal)) { + && areParallelVectors(faceNormalWorld, contactPoint.normal)) { // Remove the previous contact point computed by GJK narrowPhaseInfoBatch.resetContactPoints(batchIndex); - const Transform capsuleToWorld = isCapsuleShape1 ? narrowPhaseInfoBatch.shape1ToWorldTransforms[batchIndex] : narrowPhaseInfoBatch.shape2ToWorldTransforms[batchIndex]; + const Transform capsuleToWorld = isCapsuleShape1 ? narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].shape1ToWorldTransform : narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].shape2ToWorldTransform; const Transform polyhedronToCapsuleTransform = capsuleToWorld.getInverse() * polyhedronToWorld; // Compute the end-points of the inner segment of the capsule @@ -143,13 +143,13 @@ bool CapsuleVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfoBatch& nar } // Compute and create two contact points - bool contactsFound = satAlgorithm.computeCapsulePolyhedronFaceContactPoints(f, capsuleShape->getRadius(), polyhedron, contactPoint->penetrationDepth, + bool contactsFound = satAlgorithm.computeCapsulePolyhedronFaceContactPoints(f, capsuleShape->getRadius(), polyhedron, contactPoint.penetrationDepth, polyhedronToCapsuleTransform, faceNormalWorld, separatingAxisCapsuleSpace, capsuleSegAPolyhedronSpace, capsuleSegBPolyhedronSpace, narrowPhaseInfoBatch, batchIndex, isCapsuleShape1); if (!contactsFound) { noContact = true; - narrowPhaseInfoBatch.isColliding[batchIndex] = false; + narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].isColliding = false; break; } @@ -166,7 +166,7 @@ bool CapsuleVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfoBatch& nar lastFrameCollisionInfo->wasUsingGJK = false; // Colision found - narrowPhaseInfoBatch.isColliding[batchIndex] = true; + narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].isColliding = true; isCollisionFound = true; continue; } @@ -175,12 +175,12 @@ bool CapsuleVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfoBatch& nar if (gjkResults[batchIndex] == GJKAlgorithm::GJKResult::INTERPENETRATE) { // Run the SAT algorithm to find the separating axis and compute contact point - narrowPhaseInfoBatch.isColliding[batchIndex] = satAlgorithm.testCollisionCapsuleVsConvexPolyhedron(narrowPhaseInfoBatch, batchIndex); + narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].isColliding = satAlgorithm.testCollisionCapsuleVsConvexPolyhedron(narrowPhaseInfoBatch, batchIndex); lastFrameCollisionInfo->wasUsingGJK = false; lastFrameCollisionInfo->wasUsingSAT = true; - if (narrowPhaseInfoBatch.isColliding[batchIndex]) { + if (narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].isColliding) { isCollisionFound = true; } } diff --git a/src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.cpp b/src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.cpp index a44ba93b..9630e975 100644 --- a/src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.cpp +++ b/src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.cpp @@ -35,7 +35,7 @@ using namespace reactphysics3d; // Compute the narrow-phase collision detection between two convex polyhedra // This technique is based on the "Robust Contact Creation for Physics Simulations" presentation // by Dirk Gregorius. -bool ConvexPolyhedronVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfoBatch& narrowPhaseInfoBatch, +bool ConvexPolyhedronVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfoBatch &narrowPhaseInfoBatch, uint batchStartIndex, uint batchNbItems, bool clipWithPreviousAxisIfStillColliding, MemoryAllocator& memoryAllocator) { @@ -54,7 +54,7 @@ bool ConvexPolyhedronVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfoB for (uint batchIndex = batchStartIndex; batchIndex < batchStartIndex + batchNbItems; batchIndex++) { // Get the last frame collision info - LastFrameCollisionInfo* lastFrameCollisionInfo = narrowPhaseInfoBatch.lastFrameCollisionInfos[batchIndex]; + LastFrameCollisionInfo* lastFrameCollisionInfo = narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].lastFrameCollisionInfo; lastFrameCollisionInfo->wasUsingSAT = true; lastFrameCollisionInfo->wasUsingGJK = false; diff --git a/src/collision/narrowphase/GJK/GJKAlgorithm.cpp b/src/collision/narrowphase/GJK/GJKAlgorithm.cpp index 28c3ff0f..d35fd6e0 100644 --- a/src/collision/narrowphase/GJK/GJKAlgorithm.cpp +++ b/src/collision/narrowphase/GJK/GJKAlgorithm.cpp @@ -30,7 +30,7 @@ #include #include #include -#include +#include #include #include #include @@ -48,7 +48,7 @@ using namespace reactphysics3d; /// origin, they we give that simplex polytope to the EPA algorithm which will compute /// the correct penetration depth and contact points between the enlarged objects. void GJKAlgorithm::testCollision(NarrowPhaseInfoBatch& narrowPhaseInfoBatch, uint batchStartIndex, - uint batchNbItems, List& gjkResults) { + uint batchNbItems, Array& gjkResults) { RP3D_PROFILE("GJKAlgorithm::testCollision()", mProfiler); @@ -64,15 +64,15 @@ void GJKAlgorithm::testCollision(NarrowPhaseInfoBatch& narrowPhaseInfoBatch, uin decimal prevDistSquare; bool contactFound = false; - assert(narrowPhaseInfoBatch.collisionShapes1[batchIndex]->isConvex()); - assert(narrowPhaseInfoBatch.collisionShapes2[batchIndex]->isConvex()); + assert(narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape1->isConvex()); + assert(narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape2->isConvex()); - const ConvexShape* shape1 = static_cast(narrowPhaseInfoBatch.collisionShapes1[batchIndex]); - const ConvexShape* shape2 = static_cast(narrowPhaseInfoBatch.collisionShapes2[batchIndex]); + const ConvexShape* shape1 = static_cast(narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape1); + const ConvexShape* shape2 = static_cast(narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape2); // Get the local-space to world-space transforms - const Transform& transform1 = narrowPhaseInfoBatch.shape1ToWorldTransforms[batchIndex]; - const Transform& transform2 = narrowPhaseInfoBatch.shape2ToWorldTransforms[batchIndex]; + const Transform& transform1 = narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].shape1ToWorldTransform; + const Transform& transform2 = narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].shape2ToWorldTransform; // Transform a point from local space of body 2 to local // space of body 1 (the GJK algorithm is done in local space of body 1) @@ -92,7 +92,7 @@ void GJKAlgorithm::testCollision(NarrowPhaseInfoBatch& narrowPhaseInfoBatch, uin VoronoiSimplex simplex; // Get the last collision frame info - LastFrameCollisionInfo* lastFrameCollisionInfo = narrowPhaseInfoBatch.lastFrameCollisionInfos[batchIndex]; + LastFrameCollisionInfo* lastFrameCollisionInfo = narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].lastFrameCollisionInfo; // Get the previous point V (last cached separating axis) Vector3 v; @@ -215,7 +215,7 @@ void GJKAlgorithm::testCollision(NarrowPhaseInfoBatch& narrowPhaseInfoBatch, uin } // If we need to report contacts - if (narrowPhaseInfoBatch.reportContacts[batchIndex]) { + if (narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].reportContacts) { // Compute smooth triangle mesh contact if one of the two collision shapes is a triangle TriangleShape::computeSmoothTriangleMeshContact(shape1, shape2, pA, pB, transform1, transform2, diff --git a/src/collision/narrowphase/NarrowPhaseInfoBatch.cpp b/src/collision/narrowphase/NarrowPhaseInfoBatch.cpp index ff59d32b..9109dd64 100644 --- a/src/collision/narrowphase/NarrowPhaseInfoBatch.cpp +++ b/src/collision/narrowphase/NarrowPhaseInfoBatch.cpp @@ -33,12 +33,8 @@ using namespace reactphysics3d; // Constructor -NarrowPhaseInfoBatch::NarrowPhaseInfoBatch(MemoryAllocator& allocator, OverlappingPairs& overlappingPairs) - : mMemoryAllocator(allocator), mOverlappingPairs(overlappingPairs), overlappingPairIds(allocator), - colliderEntities1(allocator), colliderEntities2(allocator), collisionShapes1(allocator), collisionShapes2(allocator), - shape1ToWorldTransforms(allocator), shape2ToWorldTransforms(allocator), reportContacts(allocator), - isColliding(allocator), contactPoints(allocator), collisionShapeAllocators(allocator), - lastFrameCollisionInfos(allocator) { +NarrowPhaseInfoBatch::NarrowPhaseInfoBatch(OverlappingPairs& overlappingPairs, MemoryAllocator& allocator) + : mMemoryAllocator(allocator), mOverlappingPairs(overlappingPairs), narrowPhaseInfos(allocator){ } @@ -47,100 +43,31 @@ NarrowPhaseInfoBatch::~NarrowPhaseInfoBatch() { clear(); } -// Add shapes to be tested during narrow-phase collision detection into the batch -void NarrowPhaseInfoBatch::addNarrowPhaseInfo(uint64 pairId, uint64 pairIndex, Entity collider1, Entity collider2, CollisionShape* shape1, CollisionShape* shape2, - const Transform& shape1Transform, const Transform& shape2Transform, bool needToReportContacts, - MemoryAllocator& shapeAllocator) { - - overlappingPairIds.add(pairId); - colliderEntities1.add(collider1); - colliderEntities2.add(collider2); - collisionShapes1.add(shape1); - collisionShapes2.add(shape2); - shape1ToWorldTransforms.add(shape1Transform); - shape2ToWorldTransforms.add(shape2Transform); - reportContacts.add(needToReportContacts); - collisionShapeAllocators.add(&shapeAllocator); - contactPoints.add(List(mMemoryAllocator)); - isColliding.add(false); - - // Add a collision info for the two collision shapes into the overlapping pair (if not present yet) - LastFrameCollisionInfo* lastFrameInfo = mOverlappingPairs.addLastFrameInfoIfNecessary(pairIndex, shape1->getId(), shape2->getId()); - lastFrameCollisionInfos.add(lastFrameInfo); -} - -// Add a new contact point -void NarrowPhaseInfoBatch::addContactPoint(uint index, const Vector3& contactNormal, decimal penDepth, - const Vector3& localPt1, const Vector3& localPt2) { - - assert(reportContacts[index]); - assert(penDepth > decimal(0.0)); - - // Get the memory allocator - MemoryAllocator& allocator = mOverlappingPairs.getTemporaryAllocator(); - - // Create the contact point info - ContactPointInfo* contactPointInfo = new (allocator.allocate(sizeof(ContactPointInfo))) - ContactPointInfo(contactNormal, penDepth, localPt1, localPt2); - - // Add it into the list of contact points - contactPoints[index].add(contactPointInfo); -} - -// Reset the remaining contact points -void NarrowPhaseInfoBatch::resetContactPoints(uint index) { - - // Get the memory allocator - MemoryAllocator& allocator = mOverlappingPairs.getTemporaryAllocator(); - - // For each remaining contact point info - for (uint i=0; i < contactPoints[index].size(); i++) { - - ContactPointInfo* contactPoint = contactPoints[index][i]; - - // Call the destructor - contactPoint->~ContactPointInfo(); - - // Delete the current element - allocator.release(contactPoint, sizeof(ContactPointInfo)); - } - - contactPoints[index].clear(); -} - // Initialize the containers using cached capacity void NarrowPhaseInfoBatch::reserveMemory() { - overlappingPairIds.reserve(mCachedCapacity); - colliderEntities1.reserve(mCachedCapacity); - colliderEntities2.reserve(mCachedCapacity); - collisionShapes1.reserve(mCachedCapacity); - collisionShapes2.reserve(mCachedCapacity); - shape1ToWorldTransforms.reserve(mCachedCapacity); - shape2ToWorldTransforms.reserve(mCachedCapacity); - reportContacts.reserve(mCachedCapacity); - collisionShapeAllocators.reserve(mCachedCapacity); - lastFrameCollisionInfos.reserve(mCachedCapacity); - isColliding.reserve(mCachedCapacity); - contactPoints.reserve(mCachedCapacity); + narrowPhaseInfos.reserve(mCachedCapacity); } // Clear all the objects in the batch void NarrowPhaseInfoBatch::clear() { - for (uint i=0; i < overlappingPairIds.size(); i++) { + const uint32 nbNarrowPhaseInfos = narrowPhaseInfos.size(); + for (uint32 i=0; i < nbNarrowPhaseInfos; i++) { - assert(contactPoints[i].size() == 0); + assert(narrowPhaseInfos[i].nbContactPoints == 0); + + // TODO OPTI : Better manage this // Release the memory of the TriangleShape (this memory was allocated in the // MiddlePhaseTriangleCallback::testTriangle() method) - if (collisionShapes1.size() > 0 && collisionShapes1[i]->getName() == CollisionShapeName::TRIANGLE) { - collisionShapes1[i]->~CollisionShape(); - collisionShapeAllocators[i]->release(collisionShapes1[i], sizeof(TriangleShape)); + if (narrowPhaseInfos[i].collisionShape1->getName() == CollisionShapeName::TRIANGLE) { + narrowPhaseInfos[i].collisionShape1->~CollisionShape(); + narrowPhaseInfos[i].collisionShapeAllocator->release(narrowPhaseInfos[i].collisionShape1, sizeof(TriangleShape)); } - if (collisionShapes2.size() > 0 && collisionShapes2[i]->getName() == CollisionShapeName::TRIANGLE) { - collisionShapes2[i]->~CollisionShape(); - collisionShapeAllocators[i]->release(collisionShapes2[i], sizeof(TriangleShape)); + if (narrowPhaseInfos[i].collisionShape2->getName() == CollisionShapeName::TRIANGLE) { + narrowPhaseInfos[i].collisionShape2->~CollisionShape(); + narrowPhaseInfos[i].collisionShapeAllocator->release(narrowPhaseInfos[i].collisionShape2, sizeof(TriangleShape)); } } @@ -149,18 +76,7 @@ void NarrowPhaseInfoBatch::clear() { // allocated in the next frame at a possibly different location in memory (remember that the // location of the allocated memory of a single frame allocator might change between two frames) - mCachedCapacity = overlappingPairIds.size(); + mCachedCapacity = narrowPhaseInfos.capacity(); - overlappingPairIds.clear(true); - colliderEntities1.clear(true); - colliderEntities2.clear(true); - collisionShapes1.clear(true); - collisionShapes2.clear(true); - shape1ToWorldTransforms.clear(true); - shape2ToWorldTransforms.clear(true); - reportContacts.clear(true); - collisionShapeAllocators.clear(true); - lastFrameCollisionInfos.clear(true); - isColliding.clear(true); - contactPoints.clear(true); + narrowPhaseInfos.clear(true); } diff --git a/src/collision/narrowphase/NarrowPhaseInput.cpp b/src/collision/narrowphase/NarrowPhaseInput.cpp index 767fd3c2..fac3d8c3 100644 --- a/src/collision/narrowphase/NarrowPhaseInput.cpp +++ b/src/collision/narrowphase/NarrowPhaseInput.cpp @@ -25,50 +25,18 @@ // Libraries #include -#include using namespace reactphysics3d; /// Constructor NarrowPhaseInput::NarrowPhaseInput(MemoryAllocator& allocator, OverlappingPairs& overlappingPairs) - :mSphereVsSphereBatch(allocator, overlappingPairs), mSphereVsCapsuleBatch(allocator, overlappingPairs), - mCapsuleVsCapsuleBatch(allocator, overlappingPairs), mSphereVsConvexPolyhedronBatch(allocator, overlappingPairs), - mCapsuleVsConvexPolyhedronBatch(allocator, overlappingPairs), - mConvexPolyhedronVsConvexPolyhedronBatch(allocator, overlappingPairs) { + :mSphereVsSphereBatch(overlappingPairs, allocator), mSphereVsCapsuleBatch(overlappingPairs, allocator), + mCapsuleVsCapsuleBatch(overlappingPairs, allocator), mSphereVsConvexPolyhedronBatch(overlappingPairs, allocator), + mCapsuleVsConvexPolyhedronBatch(overlappingPairs, allocator), + mConvexPolyhedronVsConvexPolyhedronBatch(overlappingPairs, allocator) { } -// Add shapes to be tested during narrow-phase collision detection into the batch -void NarrowPhaseInput::addNarrowPhaseTest(uint64 pairId, uint64 pairIndex, Entity collider1, Entity collider2, CollisionShape* shape1, CollisionShape* shape2, - const Transform& shape1Transform, const Transform& shape2Transform, - NarrowPhaseAlgorithmType narrowPhaseAlgorithmType, bool reportContacts, MemoryAllocator& shapeAllocator) { - - switch (narrowPhaseAlgorithmType) { - case NarrowPhaseAlgorithmType::SphereVsSphere: - mSphereVsSphereBatch.addNarrowPhaseInfo(pairId, pairIndex, collider1, collider2, shape1, shape2, shape1Transform, shape2Transform, reportContacts, shapeAllocator); - break; - case NarrowPhaseAlgorithmType::SphereVsCapsule: - mSphereVsCapsuleBatch.addNarrowPhaseInfo(pairId, pairIndex, collider1, collider2, shape1, shape2, shape1Transform, shape2Transform, reportContacts, shapeAllocator); - break; - case NarrowPhaseAlgorithmType::CapsuleVsCapsule: - mCapsuleVsCapsuleBatch.addNarrowPhaseInfo(pairId, pairIndex, collider1, collider2, shape1, shape2, shape1Transform, shape2Transform, reportContacts, shapeAllocator); - break; - case NarrowPhaseAlgorithmType::SphereVsConvexPolyhedron: - mSphereVsConvexPolyhedronBatch.addNarrowPhaseInfo(pairId, pairIndex, collider1, collider2, shape1, shape2, shape1Transform, shape2Transform, reportContacts, shapeAllocator); - break; - case NarrowPhaseAlgorithmType::CapsuleVsConvexPolyhedron: - mCapsuleVsConvexPolyhedronBatch.addNarrowPhaseInfo(pairId, pairIndex, collider1, collider2, shape1, shape2, shape1Transform, shape2Transform, reportContacts, shapeAllocator); - break; - case NarrowPhaseAlgorithmType::ConvexPolyhedronVsConvexPolyhedron: - mConvexPolyhedronVsConvexPolyhedronBatch.addNarrowPhaseInfo(pairId, pairIndex, collider1, collider2, shape1, shape2, shape1Transform, shape2Transform, reportContacts, shapeAllocator); - break; - case NarrowPhaseAlgorithmType::None: - // Must never happen - assert(false); - break; - } -} - /// Reserve memory for the containers with cached capacity void NarrowPhaseInput::reserveMemory() { diff --git a/src/collision/narrowphase/SAT/SATAlgorithm.cpp b/src/collision/narrowphase/SAT/SATAlgorithm.cpp index 4271b900..835ff13a 100644 --- a/src/collision/narrowphase/SAT/SATAlgorithm.cpp +++ b/src/collision/narrowphase/SAT/SATAlgorithm.cpp @@ -55,8 +55,7 @@ SATAlgorithm::SATAlgorithm(bool clipWithPreviousAxisIfStillColliding, MemoryAllo } // Test collision between a sphere and a convex mesh -bool SATAlgorithm::testCollisionSphereVsConvexPolyhedron(NarrowPhaseInfoBatch& narrowPhaseInfoBatch, - uint batchStartIndex, uint batchNbItems) const { +bool SATAlgorithm::testCollisionSphereVsConvexPolyhedron(NarrowPhaseInfoBatch& narrowPhaseInfoBatch, uint batchStartIndex, uint batchNbItems) const { bool isCollisionFound = false; @@ -64,19 +63,19 @@ bool SATAlgorithm::testCollisionSphereVsConvexPolyhedron(NarrowPhaseInfoBatch& n for (uint batchIndex = batchStartIndex; batchIndex < batchStartIndex + batchNbItems; batchIndex++) { - bool isSphereShape1 = narrowPhaseInfoBatch.collisionShapes1[batchIndex]->getType() == CollisionShapeType::SPHERE; + bool isSphereShape1 = narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape1->getType() == CollisionShapeType::SPHERE; - assert(narrowPhaseInfoBatch.collisionShapes1[batchIndex]->getType() == CollisionShapeType::CONVEX_POLYHEDRON || - narrowPhaseInfoBatch.collisionShapes2[batchIndex]->getType() == CollisionShapeType::CONVEX_POLYHEDRON); - assert(narrowPhaseInfoBatch.collisionShapes1[batchIndex]->getType() == CollisionShapeType::SPHERE || - narrowPhaseInfoBatch.collisionShapes2[batchIndex]->getType() == CollisionShapeType::SPHERE); + assert(narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape1->getType() == CollisionShapeType::CONVEX_POLYHEDRON || + narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape2->getType() == CollisionShapeType::CONVEX_POLYHEDRON); + assert(narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape1->getType() == CollisionShapeType::SPHERE || + narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape2->getType() == CollisionShapeType::SPHERE); // Get the capsule collision shapes - const SphereShape* sphere = static_cast(isSphereShape1 ? narrowPhaseInfoBatch.collisionShapes1[batchIndex] : narrowPhaseInfoBatch.collisionShapes2[batchIndex]); - const ConvexPolyhedronShape* polyhedron = static_cast(isSphereShape1 ? narrowPhaseInfoBatch.collisionShapes2[batchIndex] : narrowPhaseInfoBatch.collisionShapes1[batchIndex]); + const SphereShape* sphere = static_cast(isSphereShape1 ? narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape1 : narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape2); + const ConvexPolyhedronShape* polyhedron = static_cast(isSphereShape1 ? narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape2 : narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape1); - const Transform& sphereToWorldTransform = isSphereShape1 ? narrowPhaseInfoBatch.shape1ToWorldTransforms[batchIndex] : narrowPhaseInfoBatch.shape2ToWorldTransforms[batchIndex]; - const Transform& polyhedronToWorldTransform = isSphereShape1 ? narrowPhaseInfoBatch.shape2ToWorldTransforms[batchIndex] : narrowPhaseInfoBatch.shape1ToWorldTransforms[batchIndex]; + const Transform& sphereToWorldTransform = isSphereShape1 ? narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].shape1ToWorldTransform : narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].shape2ToWorldTransform; + const Transform& polyhedronToWorldTransform = isSphereShape1 ? narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].shape2ToWorldTransform : narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].shape1ToWorldTransform; // Get the transform from sphere local-space to polyhedron local-space const Transform worldToPolyhedronTransform = polyhedronToWorldTransform.getInverse(); @@ -115,7 +114,7 @@ bool SATAlgorithm::testCollisionSphereVsConvexPolyhedron(NarrowPhaseInfoBatch& n } // If we need to report contacts - if (narrowPhaseInfoBatch.reportContacts[batchIndex]) { + if (narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].reportContacts) { const Vector3 minFaceNormal = polyhedron->getFaceNormal(minFaceIndex); Vector3 minFaceNormalWorld = polyhedronToWorldTransform.getOrientation() * minFaceNormal; @@ -125,10 +124,10 @@ bool SATAlgorithm::testCollisionSphereVsConvexPolyhedron(NarrowPhaseInfoBatch& n Vector3 normalWorld = isSphereShape1 ? -minFaceNormalWorld : minFaceNormalWorld; // Compute smooth triangle mesh contact if one of the two collision shapes is a triangle - TriangleShape::computeSmoothTriangleMeshContact(narrowPhaseInfoBatch.collisionShapes1[batchIndex], narrowPhaseInfoBatch.collisionShapes2[batchIndex], + TriangleShape::computeSmoothTriangleMeshContact(narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape1, narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape2, isSphereShape1 ? contactPointSphereLocal : contactPointPolyhedronLocal, isSphereShape1 ? contactPointPolyhedronLocal : contactPointSphereLocal, - narrowPhaseInfoBatch.shape1ToWorldTransforms[batchIndex], narrowPhaseInfoBatch.shape2ToWorldTransforms[batchIndex], + narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].shape1ToWorldTransform, narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].shape2ToWorldTransform, minPenetrationDepth, normalWorld); // Create the contact info object @@ -137,7 +136,7 @@ bool SATAlgorithm::testCollisionSphereVsConvexPolyhedron(NarrowPhaseInfoBatch& n isSphereShape1 ? contactPointPolyhedronLocal : contactPointSphereLocal); } - narrowPhaseInfoBatch.isColliding[batchIndex] = true; + narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].isColliding = true; isCollisionFound = true; } @@ -167,19 +166,19 @@ bool SATAlgorithm::testCollisionCapsuleVsConvexPolyhedron(NarrowPhaseInfoBatch& RP3D_PROFILE("SATAlgorithm::testCollisionCapsuleVsConvexPolyhedron()", mProfiler); - bool isCapsuleShape1 = narrowPhaseInfoBatch.collisionShapes1[batchIndex]->getType() == CollisionShapeType::CAPSULE; + bool isCapsuleShape1 = narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape1->getType() == CollisionShapeType::CAPSULE; - assert(narrowPhaseInfoBatch.collisionShapes1[batchIndex]->getType() == CollisionShapeType::CONVEX_POLYHEDRON || - narrowPhaseInfoBatch.collisionShapes2[batchIndex]->getType() == CollisionShapeType::CONVEX_POLYHEDRON); - assert(narrowPhaseInfoBatch.collisionShapes1[batchIndex]->getType() == CollisionShapeType::CAPSULE || - narrowPhaseInfoBatch.collisionShapes2[batchIndex]->getType() == CollisionShapeType::CAPSULE); + assert(narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape1->getType() == CollisionShapeType::CONVEX_POLYHEDRON || + narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape2->getType() == CollisionShapeType::CONVEX_POLYHEDRON); + assert(narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape1->getType() == CollisionShapeType::CAPSULE || + narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape2->getType() == CollisionShapeType::CAPSULE); // Get the collision shapes - const CapsuleShape* capsuleShape = static_cast(isCapsuleShape1 ? narrowPhaseInfoBatch.collisionShapes1[batchIndex] : narrowPhaseInfoBatch.collisionShapes2[batchIndex]); - const ConvexPolyhedronShape* polyhedron = static_cast(isCapsuleShape1 ? narrowPhaseInfoBatch.collisionShapes2[batchIndex] : narrowPhaseInfoBatch.collisionShapes1[batchIndex]); + const CapsuleShape* capsuleShape = static_cast(isCapsuleShape1 ? narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape1 : narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape2); + const ConvexPolyhedronShape* polyhedron = static_cast(isCapsuleShape1 ? narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape2 : narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape1); - const Transform capsuleToWorld = isCapsuleShape1 ? narrowPhaseInfoBatch.shape1ToWorldTransforms[batchIndex] : narrowPhaseInfoBatch.shape2ToWorldTransforms[batchIndex]; - const Transform polyhedronToWorld = isCapsuleShape1 ? narrowPhaseInfoBatch.shape2ToWorldTransforms[batchIndex] : narrowPhaseInfoBatch.shape1ToWorldTransforms[batchIndex]; + const Transform capsuleToWorld = isCapsuleShape1 ? narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].shape1ToWorldTransform : narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].shape2ToWorldTransform; + const Transform polyhedronToWorld = isCapsuleShape1 ? narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].shape2ToWorldTransform : narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].shape1ToWorldTransform; const Transform polyhedronToCapsuleTransform = capsuleToWorld.getInverse() * polyhedronToWorld; @@ -279,7 +278,7 @@ bool SATAlgorithm::testCollisionCapsuleVsConvexPolyhedron(NarrowPhaseInfoBatch& if (isMinPenetrationFaceNormal) { // If we need to report contacts - if (narrowPhaseInfoBatch.reportContacts[batchIndex]) { + if (narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].reportContacts) { return computeCapsulePolyhedronFaceContactPoints(minFaceIndex, capsuleRadius, polyhedron, minPenetrationDepth, polyhedronToCapsuleTransform, normalWorld, separatingAxisCapsuleSpace, @@ -290,7 +289,7 @@ bool SATAlgorithm::testCollisionCapsuleVsConvexPolyhedron(NarrowPhaseInfoBatch& else { // The separating axis is the cross product of a polyhedron edge and the inner capsule segment // If we need to report contacts - if (narrowPhaseInfoBatch.reportContacts[batchIndex]) { + if (narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].reportContacts) { // Compute the closest points between the inner capsule segment and the // edge of the polyhedron in polyhedron local-space @@ -303,10 +302,10 @@ bool SATAlgorithm::testCollisionCapsuleVsConvexPolyhedron(NarrowPhaseInfoBatch& Vector3 contactPointCapsule = (polyhedronToCapsuleTransform * closestPointCapsuleInnerSegment) - separatingAxisCapsuleSpace * capsuleRadius; // Compute smooth triangle mesh contact if one of the two collision shapes is a triangle - TriangleShape::computeSmoothTriangleMeshContact(narrowPhaseInfoBatch.collisionShapes1[batchIndex], narrowPhaseInfoBatch.collisionShapes2[batchIndex], + TriangleShape::computeSmoothTriangleMeshContact(narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape1, narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape2, isCapsuleShape1 ? contactPointCapsule : closestPointPolyhedronEdge, isCapsuleShape1 ? closestPointPolyhedronEdge : contactPointCapsule, - narrowPhaseInfoBatch.shape1ToWorldTransforms[batchIndex], narrowPhaseInfoBatch.shape2ToWorldTransforms[batchIndex], + narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].shape1ToWorldTransform, narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].shape2ToWorldTransform, minPenetrationDepth, normalWorld); // Create the contact point @@ -395,8 +394,8 @@ bool SATAlgorithm::computeCapsulePolyhedronFaceContactPoints(uint referenceFaceI uint firstEdgeIndex = face.edgeIndex; uint edgeIndex = firstEdgeIndex; - List planesPoints(mMemoryAllocator, 2); - List planesNormals(mMemoryAllocator, 2); + Array planesPoints(mMemoryAllocator, 2); + Array planesNormals(mMemoryAllocator, 2); // For each adjacent edge of the separating face of the polyhedron do { @@ -422,7 +421,7 @@ bool SATAlgorithm::computeCapsulePolyhedronFaceContactPoints(uint referenceFaceI } while(edgeIndex != firstEdgeIndex); // First we clip the inner segment of the capsule with the four planes of the adjacent faces - List clipSegment = clipSegmentWithPlanes(capsuleSegAPolyhedronSpace, capsuleSegBPolyhedronSpace, planesPoints, planesNormals, mMemoryAllocator); + Array clipSegment = clipSegmentWithPlanes(capsuleSegAPolyhedronSpace, capsuleSegBPolyhedronSpace, planesPoints, planesNormals, mMemoryAllocator); // Project the two clipped points into the polyhedron face const Vector3 delta = faceNormal * (penetrationDepth - capsuleRadius); @@ -430,7 +429,8 @@ bool SATAlgorithm::computeCapsulePolyhedronFaceContactPoints(uint referenceFaceI bool contactFound = false; // For each of the two clipped points - for (uint i = 0; igetType() == CollisionShapeType::CONVEX_POLYHEDRON); - assert(narrowPhaseInfoBatch.collisionShapes2[batchIndex]->getType() == CollisionShapeType::CONVEX_POLYHEDRON); - assert(narrowPhaseInfoBatch.contactPoints[batchIndex].size() == 0); + assert(narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape1->getType() == CollisionShapeType::CONVEX_POLYHEDRON); + assert(narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape2->getType() == CollisionShapeType::CONVEX_POLYHEDRON); + assert(narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].nbContactPoints == 0); - const ConvexPolyhedronShape* polyhedron1 = static_cast(narrowPhaseInfoBatch.collisionShapes1[batchIndex]); - const ConvexPolyhedronShape* polyhedron2 = static_cast(narrowPhaseInfoBatch.collisionShapes2[batchIndex]); + const ConvexPolyhedronShape* polyhedron1 = static_cast(narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape1); + const ConvexPolyhedronShape* polyhedron2 = static_cast(narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape2); - const Transform polyhedron1ToPolyhedron2 = narrowPhaseInfoBatch.shape2ToWorldTransforms[batchIndex].getInverse() * narrowPhaseInfoBatch.shape1ToWorldTransforms[batchIndex]; + const Transform polyhedron1ToPolyhedron2 = narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].shape2ToWorldTransform.getInverse() * narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].shape1ToWorldTransform; const Transform polyhedron2ToPolyhedron1 = polyhedron1ToPolyhedron2.getInverse(); decimal minPenetrationDepth = DECIMAL_LARGEST; @@ -507,7 +507,7 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseIn Vector3 minEdgeVsEdgeSeparatingAxisPolyhedron2Space; const bool isShape1Triangle = polyhedron1->getName() == CollisionShapeName::TRIANGLE; - LastFrameCollisionInfo* lastFrameCollisionInfo = narrowPhaseInfoBatch.lastFrameCollisionInfos[batchIndex]; + LastFrameCollisionInfo* lastFrameCollisionInfo = narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].lastFrameCollisionInfo; // If the last frame collision info is valid and was also using SAT algorithm if (lastFrameCollisionInfo->isValid && lastFrameCollisionInfo->wasUsingSAT) { @@ -520,7 +520,7 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseIn // was a face normal of polyhedron 1 if (lastFrameCollisionInfo->satIsAxisFacePolyhedron1) { - decimal penetrationDepth = testSingleFaceDirectionPolyhedronVsPolyhedron(polyhedron1, polyhedron2, polyhedron1ToPolyhedron2, + const decimal penetrationDepth = testSingleFaceDirectionPolyhedronVsPolyhedron(polyhedron1, polyhedron2, polyhedron1ToPolyhedron2, lastFrameCollisionInfo->satMinAxisFaceIndex); // If the previous axis was a separating axis and is still a separating axis in this frame @@ -549,7 +549,7 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseIn // The shapes are still overlapping in the previous axis (the contact manifold is not empty). // Therefore, we can return without running the whole SAT algorithm - narrowPhaseInfoBatch.isColliding[batchIndex] = true; + narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].isColliding = true; isCollisionFound = true; continue; } @@ -589,7 +589,7 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseIn // The shapes are still overlapping in the previous axis (the contact manifold is not empty). // Therefore, we can return without running the whole SAT algorithm - narrowPhaseInfoBatch.isColliding[batchIndex] = true; + narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].isColliding = true; isCollisionFound = true; continue; } @@ -651,18 +651,18 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseIn if (t1 >= decimal(0.0) && t1 <= decimal(1) && t2 >= decimal(0.0) && t2 <= decimal(1.0)) { // If we need to report contact points - if (narrowPhaseInfoBatch.reportContacts[batchIndex]) { + if (narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].reportContacts) { // Compute the contact point on polyhedron 1 edge in the local-space of polyhedron 1 Vector3 closestPointPolyhedron1EdgeLocalSpace = polyhedron2ToPolyhedron1 * closestPointPolyhedron1Edge; // Compute the world normal - Vector3 normalWorld = narrowPhaseInfoBatch.shape2ToWorldTransforms[batchIndex].getOrientation() * separatingAxisPolyhedron2Space; + Vector3 normalWorld = narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].shape2ToWorldTransform.getOrientation() * separatingAxisPolyhedron2Space; // Compute smooth triangle mesh contact if one of the two collision shapes is a triangle - TriangleShape::computeSmoothTriangleMeshContact(narrowPhaseInfoBatch.collisionShapes1[batchIndex], narrowPhaseInfoBatch.collisionShapes2[batchIndex], + TriangleShape::computeSmoothTriangleMeshContact(narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape1, narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape2, closestPointPolyhedron1EdgeLocalSpace, closestPointPolyhedron2Edge, - narrowPhaseInfoBatch.shape1ToWorldTransforms[batchIndex], narrowPhaseInfoBatch.shape2ToWorldTransforms[batchIndex], + narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].shape1ToWorldTransform, narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].shape2ToWorldTransform, penetrationDepth, normalWorld); // Create the contact point @@ -673,7 +673,7 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseIn // The shapes are overlapping on the previous axis (the contact manifold is not empty). Therefore // we return without running the whole SAT algorithm - narrowPhaseInfoBatch.isColliding[batchIndex] = true; + narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].isColliding = true; isCollisionFound = true; continue; } @@ -848,7 +848,7 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseIn else { // If we have an edge vs edge contact // If we need to report contacts - if (narrowPhaseInfoBatch.reportContacts[batchIndex]) { + if (narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].reportContacts) { // Compute the closest points between the two edges (in the local-space of poylhedron 2) Vector3 closestPointPolyhedron1Edge, closestPointPolyhedron2Edge; @@ -859,12 +859,12 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseIn Vector3 closestPointPolyhedron1EdgeLocalSpace = polyhedron2ToPolyhedron1 * closestPointPolyhedron1Edge; // Compute the world normal - Vector3 normalWorld = narrowPhaseInfoBatch.shape2ToWorldTransforms[batchIndex].getOrientation() * minEdgeVsEdgeSeparatingAxisPolyhedron2Space; + Vector3 normalWorld = narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].shape2ToWorldTransform.getOrientation() * minEdgeVsEdgeSeparatingAxisPolyhedron2Space; // Compute smooth triangle mesh contact if one of the two collision shapes is a triangle - TriangleShape::computeSmoothTriangleMeshContact(narrowPhaseInfoBatch.collisionShapes1[batchIndex], narrowPhaseInfoBatch.collisionShapes2[batchIndex], + TriangleShape::computeSmoothTriangleMeshContact(narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape1, narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape2, closestPointPolyhedron1EdgeLocalSpace, closestPointPolyhedron2Edge, - narrowPhaseInfoBatch.shape1ToWorldTransforms[batchIndex], narrowPhaseInfoBatch.shape2ToWorldTransforms[batchIndex], + narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].shape1ToWorldTransform, narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].shape2ToWorldTransform, minPenetrationDepth, normalWorld); // Create the contact point @@ -878,7 +878,7 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseIn lastFrameCollisionInfo->satMinEdge2Index = minSeparatingEdge2Index; } - narrowPhaseInfoBatch.isColliding[batchIndex] = true; + narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].isColliding = true; isCollisionFound = true; } @@ -894,17 +894,26 @@ bool SATAlgorithm::computePolyhedronVsPolyhedronFaceContactPoints(bool isMinPene RP3D_PROFILE("SATAlgorithm::computePolyhedronVsPolyhedronFaceContactPoints", mProfiler); - const ConvexPolyhedronShape* referencePolyhedron = isMinPenetrationFaceNormalPolyhedron1 ? polyhedron1 : polyhedron2; - const ConvexPolyhedronShape* incidentPolyhedron = isMinPenetrationFaceNormalPolyhedron1 ? polyhedron2 : polyhedron1; + const ConvexPolyhedronShape* referencePolyhedron; + const ConvexPolyhedronShape* incidentPolyhedron; const Transform& referenceToIncidentTransform = isMinPenetrationFaceNormalPolyhedron1 ? polyhedron1ToPolyhedron2 : polyhedron2ToPolyhedron1; const Transform& incidentToReferenceTransform = isMinPenetrationFaceNormalPolyhedron1 ? polyhedron2ToPolyhedron1 : polyhedron1ToPolyhedron2; + if (isMinPenetrationFaceNormalPolyhedron1) { + referencePolyhedron = polyhedron1; + incidentPolyhedron = polyhedron2; + } + else { + referencePolyhedron = polyhedron2; + incidentPolyhedron = polyhedron1; + } + const Vector3 axisReferenceSpace = referencePolyhedron->getFaceNormal(minFaceIndex); const Vector3 axisIncidentSpace = referenceToIncidentTransform.getOrientation() * axisReferenceSpace; // Compute the world normal - Vector3 normalWorld = isMinPenetrationFaceNormalPolyhedron1 ? narrowPhaseInfoBatch.shape1ToWorldTransforms[batchIndex].getOrientation() * axisReferenceSpace : - -(narrowPhaseInfoBatch.shape2ToWorldTransforms[batchIndex].getOrientation() * axisReferenceSpace); + const Vector3 normalWorld = isMinPenetrationFaceNormalPolyhedron1 ? narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].shape1ToWorldTransform.getOrientation() * axisReferenceSpace : + -(narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].shape2ToWorldTransform.getOrientation() * axisReferenceSpace); // Get the reference face const HalfEdgeStructure::Face& referenceFace = referencePolyhedron->getFace(minFaceIndex); @@ -915,81 +924,101 @@ bool SATAlgorithm::computePolyhedronVsPolyhedronFaceContactPoints(bool isMinPene // Get the incident face const HalfEdgeStructure::Face& incidentFace = incidentPolyhedron->getFace(incidentFaceIndex); - uint nbIncidentFaceVertices = static_cast(incidentFace.faceVertices.size()); - List polygonVertices(mMemoryAllocator, nbIncidentFaceVertices); // Vertices to clip of the incident face - List planesNormals(mMemoryAllocator, nbIncidentFaceVertices); // Normals of the clipping planes - List planesPoints(mMemoryAllocator, nbIncidentFaceVertices); // Points on the clipping planes + const uint32 nbIncidentFaceVertices = incidentFace.faceVertices.size(); + const uint32 nbMaxElements = nbIncidentFaceVertices * 2 * referenceFace.faceVertices.size(); + Array verticesTemp1(mMemoryAllocator, nbMaxElements); + Array verticesTemp2(mMemoryAllocator, nbMaxElements); // Get all the vertices of the incident face (in the reference local-space) - for (uint i=0; i < incidentFace.faceVertices.size(); i++) { + for (uint32 i=0; i < nbIncidentFaceVertices; i++) { const Vector3 faceVertexIncidentSpace = incidentPolyhedron->getVertexPosition(incidentFace.faceVertices[i]); - polygonVertices.add(incidentToReferenceTransform * faceVertexIncidentSpace); + verticesTemp1.add(incidentToReferenceTransform * faceVertexIncidentSpace); } - // Get the reference face clipping planes - uint currentEdgeIndex = referenceFace.edgeIndex; - uint firstEdgeIndex = currentEdgeIndex; + // For each edge of the reference we use it to clip the incident face polygon using Sutherland-Hodgman algorithm + uint32 firstEdgeIndex = referenceFace.edgeIndex; + bool areVertices1Input = false; + uint32 nbOutputVertices; + uint currentEdgeIndex; + + // Get the adjacent edge + const HalfEdgeStructure::Edge* currentEdge = &(referencePolyhedron->getHalfEdge(firstEdgeIndex)); + Vector3 edgeV1 = referencePolyhedron->getVertexPosition(currentEdge->vertexIndex); + do { - // Get the adjacent edge - const HalfEdgeStructure::Edge& edge = referencePolyhedron->getHalfEdge(currentEdgeIndex); + // Switch the input/output arrays of vertices + areVertices1Input = !areVertices1Input; - // Get the twin edge - const HalfEdgeStructure::Edge& twinEdge = referencePolyhedron->getHalfEdge(edge.twinEdgeIndex); + // Get the adjacent edge + const HalfEdgeStructure::Edge* nextEdge = &(referencePolyhedron->getHalfEdge(currentEdge->nextEdgeIndex)); // Compute the edge vertices and edge direction - Vector3 edgeV1 = referencePolyhedron->getVertexPosition(edge.vertexIndex); - Vector3 edgeV2 = referencePolyhedron->getVertexPosition(twinEdge.vertexIndex); - Vector3 edgeDirection = edgeV2 - edgeV1; + const Vector3 edgeV2 = referencePolyhedron->getVertexPosition(nextEdge->vertexIndex); + const Vector3 edgeDirection = edgeV2 - edgeV1; // Compute the normal of the clipping plane for this edge // The clipping plane is perpendicular to the edge direction and the reference face normal - Vector3 clipPlaneNormal = axisReferenceSpace.cross(edgeDirection); + const Vector3 planeNormal = axisReferenceSpace.cross(edgeDirection); - planesNormals.add(clipPlaneNormal); - planesPoints.add(edgeV1); + assert(areVertices1Input && verticesTemp1.size() > 0 || !areVertices1Input); + assert(!areVertices1Input && verticesTemp2.size() > 0 || areVertices1Input); + + // Clip the incident face with one adjacent plane (corresponding to one edge) of the reference face + clipPolygonWithPlane(areVertices1Input ? verticesTemp1 : verticesTemp2, edgeV1, planeNormal, areVertices1Input ? verticesTemp2 : verticesTemp1); + + currentEdgeIndex = currentEdge->nextEdgeIndex; // Go to the next adjacent edge of the reference face - currentEdgeIndex = edge.nextEdgeIndex; + currentEdge = nextEdge; + edgeV1 = edgeV2; - } while (currentEdgeIndex != firstEdgeIndex); + // Clear the input array of vertices before the next loop + if (areVertices1Input) { + verticesTemp1.clear(); + nbOutputVertices = verticesTemp2.size(); + } + else { + verticesTemp2.clear(); + nbOutputVertices = verticesTemp1.size(); + } - assert(planesNormals.size() > 0); - assert(planesNormals.size() == planesPoints.size()); + } while (currentEdgeIndex != firstEdgeIndex && nbOutputVertices > 0); - // Clip the reference faces with the adjacent planes of the reference face - List clipPolygonVertices = clipPolygonWithPlanes(polygonVertices, planesPoints, planesNormals, mMemoryAllocator); + // Reference to the output clipped polygon vertices + Array& clippedPolygonVertices = areVertices1Input ? verticesTemp2 : verticesTemp1; // We only keep the clipped points that are below the reference face const Vector3 referenceFaceVertex = referencePolyhedron->getVertexPosition(referencePolyhedron->getHalfEdge(firstEdgeIndex).vertexIndex); bool contactPointsFound = false; - for (uint i=0; i decimal(0.0)) { contactPointsFound = true; // If we need to report contacts - if (narrowPhaseInfoBatch.reportContacts[batchIndex]) { + if (narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].reportContacts) { Vector3 outWorldNormal = normalWorld; // Convert the clip incident polyhedron vertex into the incident polyhedron local-space - Vector3 contactPointIncidentPolyhedron = referenceToIncidentTransform * clipPolygonVertices[i]; + Vector3 contactPointIncidentPolyhedron = referenceToIncidentTransform * clippedPolygonVertices[i]; // Project the contact point onto the reference face - Vector3 contactPointReferencePolyhedron = projectPointOntoPlane(clipPolygonVertices[i], axisReferenceSpace, referenceFaceVertex); + Vector3 contactPointReferencePolyhedron = projectPointOntoPlane(clippedPolygonVertices[i], axisReferenceSpace, referenceFaceVertex); // Compute smooth triangle mesh contact if one of the two collision shapes is a triangle - TriangleShape::computeSmoothTriangleMeshContact(narrowPhaseInfoBatch.collisionShapes1[batchIndex], narrowPhaseInfoBatch.collisionShapes2[batchIndex], + TriangleShape::computeSmoothTriangleMeshContact(narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape1, narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape2, isMinPenetrationFaceNormalPolyhedron1 ? contactPointReferencePolyhedron : contactPointIncidentPolyhedron, isMinPenetrationFaceNormalPolyhedron1 ? contactPointIncidentPolyhedron : contactPointReferencePolyhedron, - narrowPhaseInfoBatch.shape1ToWorldTransforms[batchIndex], narrowPhaseInfoBatch.shape2ToWorldTransforms[batchIndex], + narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].shape1ToWorldTransform, narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].shape2ToWorldTransform, penetrationDepth, outWorldNormal); // Create a new contact point diff --git a/src/collision/narrowphase/SphereVsCapsuleAlgorithm.cpp b/src/collision/narrowphase/SphereVsCapsuleAlgorithm.cpp index a441cbca..d839f7cb 100755 --- a/src/collision/narrowphase/SphereVsCapsuleAlgorithm.cpp +++ b/src/collision/narrowphase/SphereVsCapsuleAlgorithm.cpp @@ -27,7 +27,7 @@ #include #include #include -#include +#include // We want to use the ReactPhysics3D namespace using namespace reactphysics3d; @@ -35,21 +35,27 @@ using namespace reactphysics3d; // Compute the narrow-phase collision detection between a sphere and a capsule // This technique is based on the "Robust Contact Creation for Physics Simulations" presentation // by Dirk Gregorius. -bool SphereVsCapsuleAlgorithm::testCollision(SphereVsCapsuleNarrowPhaseInfoBatch& narrowPhaseInfoBatch, uint batchStartIndex, uint batchNbItems, - MemoryAllocator& memoryAllocator) { +bool SphereVsCapsuleAlgorithm::testCollision(NarrowPhaseInfoBatch& narrowPhaseInfoBatch, uint batchStartIndex, uint batchNbItems, MemoryAllocator& memoryAllocator) { bool isCollisionFound = false; for (uint batchIndex = batchStartIndex; batchIndex < batchStartIndex + batchNbItems; batchIndex++) { - assert(!narrowPhaseInfoBatch.isColliding[batchIndex]); - assert(narrowPhaseInfoBatch.contactPoints[batchIndex].size() == 0); + assert(!narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].isColliding); + assert(narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].nbContactPoints == 0); - const bool isSphereShape1 = narrowPhaseInfoBatch.isSpheresShape1[batchIndex]; + const bool isSphereShape1 = narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape1->getType() == CollisionShapeType::SPHERE; + + const SphereShape* sphereShape = static_cast(isSphereShape1 ? narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape1 : narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape2); + const CapsuleShape* capsuleShape = static_cast(isSphereShape1 ? narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape2 : narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape1); + + const decimal capsuleHeight = capsuleShape->getHeight(); + const decimal sphereRadius = sphereShape->getRadius(); + const decimal capsuleRadius = capsuleShape->getRadius(); // Get the transform from sphere local-space to capsule local-space - const Transform& sphereToWorldTransform = isSphereShape1 ? narrowPhaseInfoBatch.shape1ToWorldTransforms[batchIndex] : narrowPhaseInfoBatch.shape2ToWorldTransforms[batchIndex]; - const Transform& capsuleToWorldTransform = isSphereShape1 ? narrowPhaseInfoBatch.shape2ToWorldTransforms[batchIndex] : narrowPhaseInfoBatch.shape1ToWorldTransforms[batchIndex]; + const Transform& sphereToWorldTransform = isSphereShape1 ? narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].shape1ToWorldTransform : narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].shape2ToWorldTransform; + const Transform& capsuleToWorldTransform = isSphereShape1 ? narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].shape2ToWorldTransform : narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].shape1ToWorldTransform; const Transform worldToCapsuleTransform = capsuleToWorldTransform.getInverse(); const Transform sphereToCapsuleSpaceTransform = worldToCapsuleTransform * sphereToWorldTransform; @@ -57,7 +63,7 @@ bool SphereVsCapsuleAlgorithm::testCollision(SphereVsCapsuleNarrowPhaseInfoBatch const Vector3 sphereCenter = sphereToCapsuleSpaceTransform.getPosition(); // Compute the end-points of the inner segment of the capsule - const decimal capsuleHalfHeight = narrowPhaseInfoBatch.capsuleHeights[batchIndex] * decimal(0.5); + const decimal capsuleHalfHeight = capsuleHeight * decimal(0.5); const Vector3 capsuleSegA(0, -capsuleHalfHeight, 0); const Vector3 capsuleSegB(0, capsuleHalfHeight, 0); @@ -69,7 +75,7 @@ bool SphereVsCapsuleAlgorithm::testCollision(SphereVsCapsuleNarrowPhaseInfoBatch const decimal sphereSegmentDistanceSquare = sphereCenterToSegment.lengthSquare(); // Compute the sum of the radius of the sphere and the capsule (virtual sphere) - decimal sumRadius = narrowPhaseInfoBatch.sphereRadiuses[batchIndex] + narrowPhaseInfoBatch.capsuleRadiuses[batchIndex]; + decimal sumRadius = sphereRadius + capsuleRadius; // If the collision shapes overlap if (sphereSegmentDistanceSquare < sumRadius * sumRadius) { @@ -80,7 +86,7 @@ bool SphereVsCapsuleAlgorithm::testCollision(SphereVsCapsuleNarrowPhaseInfoBatch Vector3 contactPointCapsuleLocal; // If we need to report contacts - if (narrowPhaseInfoBatch.reportContacts[batchIndex]) { + if (narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].reportContacts) { // If the sphere center is not on the capsule inner segment if (sphereSegmentDistanceSquare > MACHINE_EPSILON) { @@ -88,8 +94,8 @@ bool SphereVsCapsuleAlgorithm::testCollision(SphereVsCapsuleNarrowPhaseInfoBatch decimal sphereSegmentDistance = std::sqrt(sphereSegmentDistanceSquare); sphereCenterToSegment /= sphereSegmentDistance; - contactPointSphereLocal = sphereToCapsuleSpaceTransform.getInverse() * (sphereCenter + sphereCenterToSegment * narrowPhaseInfoBatch.sphereRadiuses[batchIndex]); - contactPointCapsuleLocal = closestPointOnSegment - sphereCenterToSegment * narrowPhaseInfoBatch.capsuleRadiuses[batchIndex]; + contactPointSphereLocal = sphereToCapsuleSpaceTransform.getInverse() * (sphereCenter + sphereCenterToSegment * sphereRadius); + contactPointCapsuleLocal = closestPointOnSegment - sphereCenterToSegment * capsuleRadius; normalWorld = capsuleToWorldTransform.getOrientation() * sphereCenterToSegment; @@ -120,8 +126,8 @@ bool SphereVsCapsuleAlgorithm::testCollision(SphereVsCapsuleNarrowPhaseInfoBatch normalWorld = capsuleToWorldTransform.getOrientation() * normalCapsuleSpace; // Compute the two local contact points - contactPointSphereLocal = sphereToCapsuleSpaceTransform.getInverse() * (sphereCenter + normalCapsuleSpace * narrowPhaseInfoBatch.sphereRadiuses[batchIndex]); - contactPointCapsuleLocal = sphereCenter - normalCapsuleSpace * narrowPhaseInfoBatch.capsuleRadiuses[batchIndex]; + contactPointSphereLocal = sphereToCapsuleSpaceTransform.getInverse() * (sphereCenter + normalCapsuleSpace * sphereRadius); + contactPointCapsuleLocal = sphereCenter - normalCapsuleSpace * capsuleRadius; } if (penetrationDepth <= decimal(0.0)) { @@ -136,7 +142,7 @@ bool SphereVsCapsuleAlgorithm::testCollision(SphereVsCapsuleNarrowPhaseInfoBatch isSphereShape1 ? contactPointCapsuleLocal : contactPointSphereLocal); } - narrowPhaseInfoBatch.isColliding[batchIndex] = true; + narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].isColliding = true; isCollisionFound = true; continue; } diff --git a/src/collision/narrowphase/SphereVsCapsuleNarrowPhaseInfoBatch.cpp b/src/collision/narrowphase/SphereVsCapsuleNarrowPhaseInfoBatch.cpp deleted file mode 100644 index e4433b0d..00000000 --- a/src/collision/narrowphase/SphereVsCapsuleNarrowPhaseInfoBatch.cpp +++ /dev/null @@ -1,88 +0,0 @@ -/******************************************************************************** -* ReactPhysics3D physics library, http://www.reactphysics3d.com * -* Copyright (c) 2010-2020 Daniel Chappuis * -********************************************************************************* -* * -* This software is provided 'as-is', without any express or implied warranty. * -* In no event will the authors be held liable for any damages arising from the * -* use of this software. * -* * -* Permission is granted to anyone to use this software for any purpose, * -* including commercial applications, and to alter it and redistribute it * -* freely, subject to the following restrictions: * -* * -* 1. The origin of this software must not be misrepresented; you must not claim * -* that you wrote the original software. If you use this software in a * -* product, an acknowledgment in the product documentation would be * -* appreciated but is not required. * -* * -* 2. Altered source versions must be plainly marked as such, and must not be * -* misrepresented as being the original software. * -* * -* 3. This notice may not be removed or altered from any source distribution. * -* * -********************************************************************************/ - -// Libraries -#include -#include -#include - -using namespace reactphysics3d; - -// Constructor -SphereVsCapsuleNarrowPhaseInfoBatch::SphereVsCapsuleNarrowPhaseInfoBatch(MemoryAllocator& allocator, - OverlappingPairs& overlappingPairs) - : NarrowPhaseInfoBatch(allocator, overlappingPairs), isSpheresShape1(allocator), sphereRadiuses(allocator), capsuleRadiuses(allocator), - capsuleHeights(allocator) { - -} - -// Add shapes to be tested during narrow-phase collision detection into the batch -void SphereVsCapsuleNarrowPhaseInfoBatch::addNarrowPhaseInfo(uint64 pairId, uint64 pairIndex, Entity collider1, Entity collider2, CollisionShape* shape1, CollisionShape* shape2, - const Transform& shape1Transform, const Transform& shape2Transform, - bool needToReportContacts, MemoryAllocator& shapeAllocator) { - - NarrowPhaseInfoBatch::addNarrowPhaseInfo(pairId, pairIndex, collider1, collider2, shape1, shape2, shape1Transform, - shape2Transform, needToReportContacts, shapeAllocator); - - bool isSphereShape1 = shape1->getType() == CollisionShapeType::SPHERE; - isSpheresShape1.add(isSphereShape1); - - assert(isSphereShape1 || shape1->getType() == CollisionShapeType::CAPSULE); - - // Get the collision shapes - const SphereShape* sphereShape = static_cast(isSphereShape1 ? shape1 : shape2); - const CapsuleShape* capsuleShape = static_cast(isSphereShape1 ? shape2 : shape1); - - sphereRadiuses.add(sphereShape->getRadius()); - capsuleRadiuses.add(capsuleShape->getRadius()); - capsuleHeights.add(capsuleShape->getHeight()); -} - -// Initialize the containers using cached capacity -void SphereVsCapsuleNarrowPhaseInfoBatch::reserveMemory() { - - NarrowPhaseInfoBatch::reserveMemory(); - - isSpheresShape1.reserve(mCachedCapacity); - sphereRadiuses.reserve(mCachedCapacity); - capsuleRadiuses.reserve(mCachedCapacity); - capsuleHeights.reserve(mCachedCapacity); -} - -// Clear all the objects in the batch -void SphereVsCapsuleNarrowPhaseInfoBatch::clear() { - - // Note that we clear the following containers and we release their allocated memory. Therefore, - // if the memory allocator is a single frame allocator, the memory is deallocated and will be - // allocated in the next frame at a possibly different location in memory (remember that the - // location of the allocated memory of a single frame allocator might change between two frames) - - NarrowPhaseInfoBatch::clear(); - - isSpheresShape1.clear(true); - sphereRadiuses.clear(true); - capsuleRadiuses.clear(true); - capsuleHeights.clear(true); -} diff --git a/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.cpp b/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.cpp index 0c0f609b..039aa9bb 100644 --- a/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.cpp +++ b/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.cpp @@ -50,20 +50,20 @@ bool SphereVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfoBatch& narr #endif - List gjkResults(memoryAllocator); + Array gjkResults(memoryAllocator, batchNbItems); gjkAlgorithm.testCollision(narrowPhaseInfoBatch, batchStartIndex, batchNbItems, gjkResults); assert(gjkResults.size() == batchNbItems); // For each item in the batch for (uint batchIndex = batchStartIndex; batchIndex < batchStartIndex + batchNbItems; batchIndex++) { - assert(narrowPhaseInfoBatch.collisionShapes1[batchIndex]->getType() == CollisionShapeType::CONVEX_POLYHEDRON || - narrowPhaseInfoBatch.collisionShapes2[batchIndex]->getType() == CollisionShapeType::CONVEX_POLYHEDRON); - assert(narrowPhaseInfoBatch.collisionShapes1[batchIndex]->getType() == CollisionShapeType::SPHERE || - narrowPhaseInfoBatch.collisionShapes2[batchIndex]->getType() == CollisionShapeType::SPHERE); + assert(narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape1->getType() == CollisionShapeType::CONVEX_POLYHEDRON || + narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape2->getType() == CollisionShapeType::CONVEX_POLYHEDRON); + assert(narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape1->getType() == CollisionShapeType::SPHERE || + narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape2->getType() == CollisionShapeType::SPHERE); // Get the last frame collision info - LastFrameCollisionInfo* lastFrameCollisionInfo = narrowPhaseInfoBatch.lastFrameCollisionInfos[batchIndex]; + LastFrameCollisionInfo* lastFrameCollisionInfo = narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].lastFrameCollisionInfo; lastFrameCollisionInfo->wasUsingGJK = true; lastFrameCollisionInfo->wasUsingSAT = false; @@ -72,7 +72,7 @@ bool SphereVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfoBatch& narr if (gjkResults[batchIndex] == GJKAlgorithm::GJKResult::COLLIDE_IN_MARGIN) { // Return true - narrowPhaseInfoBatch.isColliding[batchIndex] = true; + narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].isColliding = true; isCollisionFound = true; continue; } diff --git a/src/collision/narrowphase/SphereVsSphereAlgorithm.cpp b/src/collision/narrowphase/SphereVsSphereAlgorithm.cpp index 943ae89d..1d488ad5 100755 --- a/src/collision/narrowphase/SphereVsSphereAlgorithm.cpp +++ b/src/collision/narrowphase/SphereVsSphereAlgorithm.cpp @@ -26,74 +26,86 @@ // Libraries #include #include -#include +#include // We want to use the ReactPhysics3D namespace using namespace reactphysics3d; -bool SphereVsSphereAlgorithm::testCollision(SphereVsSphereNarrowPhaseInfoBatch& narrowPhaseInfoBatch, uint batchStartIndex, uint batchNbItems, MemoryAllocator& memoryAllocator) { +bool SphereVsSphereAlgorithm::testCollision(NarrowPhaseInfoBatch& narrowPhaseInfoBatch, uint batchStartIndex, uint batchNbItems, MemoryAllocator& memoryAllocator) { bool isCollisionFound = false; // For each item in the batch for (uint batchIndex = batchStartIndex; batchIndex < batchStartIndex + batchNbItems; batchIndex++) { - assert(narrowPhaseInfoBatch.contactPoints[batchIndex].size() == 0); - assert(!narrowPhaseInfoBatch.isColliding[batchIndex]); + assert(narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].nbContactPoints == 0); + assert(!narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].isColliding); // Get the local-space to world-space transforms - const Transform& transform1 = narrowPhaseInfoBatch.shape1ToWorldTransforms[batchIndex]; - const Transform& transform2 = narrowPhaseInfoBatch.shape2ToWorldTransforms[batchIndex]; + const Transform& transform1 = narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].shape1ToWorldTransform; + const Transform& transform2 = narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].shape2ToWorldTransform; // Compute the distance between the centers Vector3 vectorBetweenCenters = transform2.getPosition() - transform1.getPosition(); decimal squaredDistanceBetweenCenters = vectorBetweenCenters.lengthSquare(); + const SphereShape* sphereShape1 = static_cast(narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape1); + const SphereShape* sphereShape2 = static_cast(narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape2); + + const decimal sphere1Radius = sphereShape1->getRadius(); + const decimal sphere2Radius = sphereShape2->getRadius(); + // Compute the sum of the radius - decimal sumRadiuses = narrowPhaseInfoBatch.sphere1Radiuses[batchIndex] + narrowPhaseInfoBatch.sphere2Radiuses[batchIndex]; + const decimal sumRadiuses = sphere1Radius + sphere2Radius; // Compute the product of the sum of the radius - decimal sumRadiusesProducts = sumRadiuses * sumRadiuses; + const decimal sumRadiusesProducts = sumRadiuses * sumRadiuses; // If the sphere collision shapes intersect if (squaredDistanceBetweenCenters < sumRadiusesProducts) { - // If we need to report contacts - if (narrowPhaseInfoBatch.reportContacts[batchIndex]) { + const decimal penetrationDepth = sumRadiuses - std::sqrt(squaredDistanceBetweenCenters); - const Transform transform1Inverse = transform1.getInverse(); - const Transform transform2Inverse = transform2.getInverse(); + // Make sure the penetration depth is not zero (even if the previous condition test was true the penetration depth can still be + // zero because of precision issue of the computation at the previous line) + if (penetrationDepth > 0) { - decimal penetrationDepth = sumRadiuses - std::sqrt(squaredDistanceBetweenCenters); - Vector3 intersectionOnBody1; - Vector3 intersectionOnBody2; - Vector3 normal; + // If we need to report contacts + if (narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].reportContacts) { - // If the two sphere centers are not at the same position - if (squaredDistanceBetweenCenters > MACHINE_EPSILON) { + const Transform transform1Inverse = transform1.getInverse(); + const Transform transform2Inverse = transform2.getInverse(); - Vector3 centerSphere2InBody1LocalSpace = transform1Inverse * transform2.getPosition(); - Vector3 centerSphere1InBody2LocalSpace = transform2Inverse * transform1.getPosition(); + Vector3 intersectionOnBody1; + Vector3 intersectionOnBody2; + Vector3 normal; - intersectionOnBody1 = narrowPhaseInfoBatch.sphere1Radiuses[batchIndex] * centerSphere2InBody1LocalSpace.getUnit(); - intersectionOnBody2 = narrowPhaseInfoBatch.sphere2Radiuses[batchIndex] * centerSphere1InBody2LocalSpace.getUnit(); - normal = vectorBetweenCenters.getUnit(); - } - else { // If the sphere centers are at the same position (degenerate case) + // If the two sphere centers are not at the same position + if (squaredDistanceBetweenCenters > MACHINE_EPSILON) { - // Take any contact normal direction - normal.setAllValues(0, 1, 0); + const Vector3 centerSphere2InBody1LocalSpace = transform1Inverse * transform2.getPosition(); + const Vector3 centerSphere1InBody2LocalSpace = transform2Inverse * transform1.getPosition(); - intersectionOnBody1 = narrowPhaseInfoBatch.sphere1Radiuses[batchIndex] * (transform1Inverse.getOrientation() * normal); - intersectionOnBody2 = narrowPhaseInfoBatch.sphere2Radiuses[batchIndex] * (transform2Inverse.getOrientation() * normal); + intersectionOnBody1 = sphere1Radius * centerSphere2InBody1LocalSpace.getUnit(); + intersectionOnBody2 = sphere2Radius * centerSphere1InBody2LocalSpace.getUnit(); + normal = vectorBetweenCenters.getUnit(); + } + else { // If the sphere centers are at the same position (degenerate case) + + // Take any contact normal direction + normal.setAllValues(0, 1, 0); + + intersectionOnBody1 = sphere1Radius * (transform1Inverse.getOrientation() * normal); + intersectionOnBody2 = sphere2Radius * (transform2Inverse.getOrientation() * normal); + } + + // Create the contact info object + narrowPhaseInfoBatch.addContactPoint(batchIndex, normal, penetrationDepth, intersectionOnBody1, intersectionOnBody2); } - // Create the contact info object - narrowPhaseInfoBatch.addContactPoint(batchIndex, normal, penetrationDepth, intersectionOnBody1, intersectionOnBody2); + narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].isColliding = true; + isCollisionFound = true; } - - narrowPhaseInfoBatch.isColliding[batchIndex] = true; - isCollisionFound = true; } } diff --git a/src/collision/narrowphase/SphereVsSphereNarrowPhaseInfoBatch.cpp b/src/collision/narrowphase/SphereVsSphereNarrowPhaseInfoBatch.cpp deleted file mode 100644 index 36970cd2..00000000 --- a/src/collision/narrowphase/SphereVsSphereNarrowPhaseInfoBatch.cpp +++ /dev/null @@ -1,77 +0,0 @@ -/******************************************************************************** -* ReactPhysics3D physics library, http://www.reactphysics3d.com * -* Copyright (c) 2010-2020 Daniel Chappuis * -********************************************************************************* -* * -* This software is provided 'as-is', without any express or implied warranty. * -* In no event will the authors be held liable for any damages arising from the * -* use of this software. * -* * -* Permission is granted to anyone to use this software for any purpose, * -* including commercial applications, and to alter it and redistribute it * -* freely, subject to the following restrictions: * -* * -* 1. The origin of this software must not be misrepresented; you must not claim * -* that you wrote the original software. If you use this software in a * -* product, an acknowledgment in the product documentation would be * -* appreciated but is not required. * -* * -* 2. Altered source versions must be plainly marked as such, and must not be * -* misrepresented as being the original software. * -* * -* 3. This notice may not be removed or altered from any source distribution. * -* * -********************************************************************************/ - -// Libraries -#include -#include - -using namespace reactphysics3d; - -// Constructor -SphereVsSphereNarrowPhaseInfoBatch::SphereVsSphereNarrowPhaseInfoBatch(MemoryAllocator& allocator, OverlappingPairs& overlappingPairs) - : NarrowPhaseInfoBatch(allocator, overlappingPairs), sphere1Radiuses(allocator), sphere2Radiuses(allocator) { - -} - -// Add shapes to be tested during narrow-phase collision detection into the batch -void SphereVsSphereNarrowPhaseInfoBatch::addNarrowPhaseInfo(uint64 pairId, uint64 pairIndex, Entity collider1, Entity collider2, CollisionShape* shape1, CollisionShape* shape2, - const Transform& shape1Transform, const Transform& shape2Transform, bool needToReportContacts, MemoryAllocator &shapeAllocator) { - - NarrowPhaseInfoBatch::addNarrowPhaseInfo(pairId, pairIndex, collider1, collider2, shape1, shape2, shape1Transform, - shape2Transform, needToReportContacts, shapeAllocator); - - assert(shape1->getType() == CollisionShapeType::SPHERE); - assert(shape2->getType() == CollisionShapeType::SPHERE); - - const SphereShape* sphere1 = static_cast(shape1); - const SphereShape* sphere2 = static_cast(shape2); - - sphere1Radiuses.add(sphere1->getRadius()); - sphere2Radiuses.add(sphere2->getRadius()); -} - -// Initialize the containers using cached capacity -void SphereVsSphereNarrowPhaseInfoBatch::reserveMemory() { - - NarrowPhaseInfoBatch::reserveMemory(); - - sphere1Radiuses.reserve(mCachedCapacity); - sphere2Radiuses.reserve(mCachedCapacity); -} - -// Clear all the objects in the batch -void SphereVsSphereNarrowPhaseInfoBatch::clear() { - - // Note that we clear the following containers and we release their allocated memory. Therefore, - // if the memory allocator is a single frame allocator, the memory is deallocated and will be - // allocated in the next frame at a possibly different location in memory (remember that the - // location of the allocated memory of a single frame allocator might change between two frames) - - NarrowPhaseInfoBatch::clear(); - - sphere1Radiuses.clear(true); - sphere2Radiuses.clear(true); -} - diff --git a/src/collision/shapes/AABB.cpp b/src/collision/shapes/AABB.cpp index 2d9b29e5..5e9df2f6 100644 --- a/src/collision/shapes/AABB.cpp +++ b/src/collision/shapes/AABB.cpp @@ -42,99 +42,3 @@ AABB::AABB(const AABB& aabb) : mMinCoordinates(aabb.mMinCoordinates), mMaxCoordinates(aabb.mMaxCoordinates) { } - -// Merge the AABB in parameter with the current one -void AABB::mergeWithAABB(const AABB& aabb) { - mMinCoordinates.x = std::min(mMinCoordinates.x, aabb.mMinCoordinates.x); - mMinCoordinates.y = std::min(mMinCoordinates.y, aabb.mMinCoordinates.y); - mMinCoordinates.z = std::min(mMinCoordinates.z, aabb.mMinCoordinates.z); - - mMaxCoordinates.x = std::max(mMaxCoordinates.x, aabb.mMaxCoordinates.x); - mMaxCoordinates.y = std::max(mMaxCoordinates.y, aabb.mMaxCoordinates.y); - mMaxCoordinates.z = std::max(mMaxCoordinates.z, aabb.mMaxCoordinates.z); -} - -// Replace the current AABB with a new AABB that is the union of two AABBs in parameters -void AABB::mergeTwoAABBs(const AABB& aabb1, const AABB& aabb2) { - mMinCoordinates.x = std::min(aabb1.mMinCoordinates.x, aabb2.mMinCoordinates.x); - mMinCoordinates.y = std::min(aabb1.mMinCoordinates.y, aabb2.mMinCoordinates.y); - mMinCoordinates.z = std::min(aabb1.mMinCoordinates.z, aabb2.mMinCoordinates.z); - - mMaxCoordinates.x = std::max(aabb1.mMaxCoordinates.x, aabb2.mMaxCoordinates.x); - mMaxCoordinates.y = std::max(aabb1.mMaxCoordinates.y, aabb2.mMaxCoordinates.y); - mMaxCoordinates.z = std::max(aabb1.mMaxCoordinates.z, aabb2.mMaxCoordinates.z); -} - -// Return true if the current AABB contains the AABB given in parameter -bool AABB::contains(const AABB& aabb) const { - - bool isInside = true; - isInside = isInside && mMinCoordinates.x <= aabb.mMinCoordinates.x; - isInside = isInside && mMinCoordinates.y <= aabb.mMinCoordinates.y; - isInside = isInside && mMinCoordinates.z <= aabb.mMinCoordinates.z; - - isInside = isInside && mMaxCoordinates.x >= aabb.mMaxCoordinates.x; - isInside = isInside && mMaxCoordinates.y >= aabb.mMaxCoordinates.y; - isInside = isInside && mMaxCoordinates.z >= aabb.mMaxCoordinates.z; - return isInside; -} - -// Create and return an AABB for a triangle -AABB AABB::createAABBForTriangle(const Vector3* trianglePoints) { - - Vector3 minCoords(trianglePoints[0].x, trianglePoints[0].y, trianglePoints[0].z); - Vector3 maxCoords(trianglePoints[0].x, trianglePoints[0].y, trianglePoints[0].z); - - if (trianglePoints[1].x < minCoords.x) minCoords.x = trianglePoints[1].x; - if (trianglePoints[1].y < minCoords.y) minCoords.y = trianglePoints[1].y; - if (trianglePoints[1].z < minCoords.z) minCoords.z = trianglePoints[1].z; - - if (trianglePoints[2].x < minCoords.x) minCoords.x = trianglePoints[2].x; - if (trianglePoints[2].y < minCoords.y) minCoords.y = trianglePoints[2].y; - if (trianglePoints[2].z < minCoords.z) minCoords.z = trianglePoints[2].z; - - if (trianglePoints[1].x > maxCoords.x) maxCoords.x = trianglePoints[1].x; - if (trianglePoints[1].y > maxCoords.y) maxCoords.y = trianglePoints[1].y; - if (trianglePoints[1].z > maxCoords.z) maxCoords.z = trianglePoints[1].z; - - if (trianglePoints[2].x > maxCoords.x) maxCoords.x = trianglePoints[2].x; - if (trianglePoints[2].y > maxCoords.y) maxCoords.y = trianglePoints[2].y; - if (trianglePoints[2].z > maxCoords.z) maxCoords.z = trianglePoints[2].z; - - return AABB(minCoords, maxCoords); -} - -// Return true if the ray intersects the AABB -/// This method use the line vs AABB raycasting technique described in -/// Real-time Collision Detection by Christer Ericson. -bool AABB::testRayIntersect(const Ray& ray) const { - - const Vector3 point2 = ray.point1 + ray.maxFraction * (ray.point2 - ray.point1); - const Vector3 e = mMaxCoordinates - mMinCoordinates; - const Vector3 d = point2 - ray.point1; - const Vector3 m = ray.point1 + point2 - mMinCoordinates - mMaxCoordinates; - - // Test if the AABB face normals are separating axis - decimal adx = std::abs(d.x); - if (std::abs(m.x) > e.x + adx) return false; - decimal ady = std::abs(d.y); - if (std::abs(m.y) > e.y + ady) return false; - decimal adz = std::abs(d.z); - if (std::abs(m.z) > e.z + adz) return false; - - // Add in an epsilon term to counteract arithmetic errors when segment is - // (near) parallel to a coordinate axis (see text for detail) - const decimal epsilon = 0.00001; - adx += epsilon; - ady += epsilon; - adz += epsilon; - - // Test if the cross products between face normals and ray direction are - // separating axis - if (std::abs(m.y * d.z - m.z * d.y) > e.y * adz + e.z * ady) return false; - if (std::abs(m.z * d.x - m.x * d.z) > e.x * adz + e.z * adx) return false; - if (std::abs(m.x * d.y - m.y * d.x) > e.x * ady + e.y * adx) return false; - - // No separating axis has been found - return true; -} diff --git a/src/collision/shapes/BoxShape.cpp b/src/collision/shapes/BoxShape.cpp index 60f5a2b2..51335e0a 100644 --- a/src/collision/shapes/BoxShape.cpp +++ b/src/collision/shapes/BoxShape.cpp @@ -27,6 +27,7 @@ #include #include #include +#include #include #include #include @@ -37,46 +38,12 @@ using namespace reactphysics3d; /** * @param halfExtents The vector with the three half-extents of the box */ -BoxShape::BoxShape(const Vector3& halfExtents, MemoryAllocator& allocator) - : ConvexPolyhedronShape(CollisionShapeName::BOX, allocator), mHalfExtents(halfExtents), - mHalfEdgeStructure(allocator, 6, 8, 24) { +BoxShape::BoxShape(const Vector3& halfExtents, MemoryAllocator& allocator, PhysicsCommon& physicsCommon) + : ConvexPolyhedronShape(CollisionShapeName::BOX, allocator), mHalfExtents(halfExtents), mPhysicsCommon(physicsCommon) { assert(halfExtents.x > decimal(0.0)); assert(halfExtents.y > decimal(0.0)); assert(halfExtents.z > decimal(0.0)); - - // Vertices - mHalfEdgeStructure.addVertex(0); - mHalfEdgeStructure.addVertex(1); - mHalfEdgeStructure.addVertex(2); - mHalfEdgeStructure.addVertex(3); - mHalfEdgeStructure.addVertex(4); - mHalfEdgeStructure.addVertex(5); - mHalfEdgeStructure.addVertex(6); - mHalfEdgeStructure.addVertex(7); - - // Faces - List face0(allocator, 4); - face0.add(0); face0.add(1); face0.add(2); face0.add(3); - List face1(allocator, 4); - face1.add(1); face1.add(5); face1.add(6); face1.add(2); - List face2(allocator, 4); - face2.add(4); face2.add(7); face2.add(6); face2.add(5); - List face3(allocator, 4); - face3.add(4); face3.add(0); face3.add(3); face3.add(7); - List face4(allocator, 4); - face4.add(4); face4.add(5); face4.add(1); face4.add(0); - List face5(allocator, 4); - face5.add(2); face5.add(6); face5.add(7); face5.add(3); - - mHalfEdgeStructure.addFace(face0); - mHalfEdgeStructure.addFace(face1); - mHalfEdgeStructure.addFace(face2); - mHalfEdgeStructure.addFace(face3); - mHalfEdgeStructure.addFace(face4); - mHalfEdgeStructure.addFace(face5); - - mHalfEdgeStructure.init(); } // Return the local inertia tensor of the collision shape @@ -155,3 +122,21 @@ bool BoxShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, Collider* colli return true; } + +// Return a given face of the polyhedron +const HalfEdgeStructure::Face& BoxShape::getFace(uint faceIndex) const { + assert(faceIndex < mPhysicsCommon.mBoxShapeHalfEdgeStructure.getNbFaces()); + return mPhysicsCommon.mBoxShapeHalfEdgeStructure.getFace(faceIndex); +} + +// Return a given vertex of the polyhedron +HalfEdgeStructure::Vertex BoxShape::getVertex(uint vertexIndex) const { + assert(vertexIndex < getNbVertices()); + return mPhysicsCommon.mBoxShapeHalfEdgeStructure.getVertex(vertexIndex); +} + +// Return a given half-edge of the polyhedron +const HalfEdgeStructure::Edge& BoxShape::getHalfEdge(uint edgeIndex) const { + assert(edgeIndex < getNbHalfEdges()); + return mPhysicsCommon.mBoxShapeHalfEdgeStructure.getHalfEdge(edgeIndex); +} diff --git a/src/collision/shapes/CollisionShape.cpp b/src/collision/shapes/CollisionShape.cpp index 25f9715e..7e05d428 100644 --- a/src/collision/shapes/CollisionShape.cpp +++ b/src/collision/shapes/CollisionShape.cpp @@ -96,7 +96,8 @@ void CollisionShape::computeAABB(AABB& aabb, const Transform& transform) const { /// Notify all the assign colliders that the size of the collision shape has changed void CollisionShape::notifyColliderAboutChangedSize() { - for (uint i=0; i < mColliders.size(); i++) { + const uint32 nbColliders = mColliders.size(); + for (uint32 i=0; i < nbColliders; i++) { mColliders[i]->setHasCollisionShapeChangedSize(true); } } diff --git a/src/collision/shapes/ConcaveMeshShape.cpp b/src/collision/shapes/ConcaveMeshShape.cpp index c395b0ec..77703c92 100644 --- a/src/collision/shapes/ConcaveMeshShape.cpp +++ b/src/collision/shapes/ConcaveMeshShape.cpp @@ -34,8 +34,8 @@ using namespace reactphysics3d; // Constructor -ConcaveMeshShape::ConcaveMeshShape(TriangleMesh* triangleMesh, MemoryAllocator& allocator, const Vector3& scaling) - : ConcaveShape(CollisionShapeName::TRIANGLE_MESH, allocator, scaling), mDynamicAABBTree(allocator) { +ConcaveMeshShape::ConcaveMeshShape(TriangleMesh* triangleMesh, MemoryAllocator& allocator, HalfEdgeStructure& triangleHalfEdgeStructure, const Vector3& scaling) + : ConcaveShape(CollisionShapeName::TRIANGLE_MESH, allocator, scaling), mDynamicAABBTree(allocator), mTriangleHalfEdgeStructure(triangleHalfEdgeStructure) { mTriangleMesh = triangleMesh; mRaycastTestType = TriangleRaycastSide::FRONT; @@ -127,8 +127,8 @@ uint ConcaveMeshShape::getNbTriangles(uint subPart) const } // Compute all the triangles of the mesh that are overlapping with the AABB in parameter -void ConcaveMeshShape::computeOverlappingTriangles(const AABB& localAABB, List& triangleVertices, - List& triangleVerticesNormals, List& shapeIds, +void ConcaveMeshShape::computeOverlappingTriangles(const AABB& localAABB, Array& triangleVertices, + Array& triangleVerticesNormals, Array& shapeIds, MemoryAllocator& allocator) const { RP3D_PROFILE("ConcaveMeshShape::computeOverlappingTriangles()", mProfiler); @@ -139,12 +139,12 @@ void ConcaveMeshShape::computeOverlappingTriangles(const AABB& localAABB, List overlappingNodes(allocator); + Array overlappingNodes(allocator, 64); mDynamicAABBTree.reportAllShapesOverlappingWithAABB(aabb, overlappingNodes); - const uint nbOverlappingNodes = overlappingNodes.size(); + const uint32 nbOverlappingNodes = overlappingNodes.size(); - // Add space in the list of triangles vertices/normals for the new triangles + // Add space in the array of triangles vertices/normals for the new triangles triangleVertices.addWithoutInit(nbOverlappingNodes * 3); triangleVerticesNormals.addWithoutInit(nbOverlappingNodes * 3); @@ -230,7 +230,7 @@ decimal ConcaveMeshRaycastCallback::raycastBroadPhaseShape(int32 nodeId, const R // Raycast all collision shapes that have been collected void ConcaveMeshRaycastCallback::raycastTriangles() { - List::Iterator it; + Array::Iterator it; decimal smallestHitFraction = mRay.maxFraction; for (it = mHitAABBNodes.begin(); it != mHitAABBNodes.end(); ++it) { @@ -247,7 +247,7 @@ void ConcaveMeshRaycastCallback::raycastTriangles() { mConcaveMeshShape.getTriangleVerticesNormals(data[0], data[1], verticesNormals); // Create a triangle collision shape - TriangleShape triangleShape(trianglePoints, verticesNormals, mConcaveMeshShape.computeTriangleShapeId(data[0], data[1]), mAllocator); + TriangleShape triangleShape(trianglePoints, verticesNormals, mConcaveMeshShape.computeTriangleShapeId(data[0], data[1]), mConcaveMeshShape.mTriangleHalfEdgeStructure, mAllocator); triangleShape.setRaycastTestType(mConcaveMeshShape.getRaycastTestType()); #ifdef IS_RP3D_PROFILING_ENABLED diff --git a/src/collision/shapes/ConvexMeshShape.cpp b/src/collision/shapes/ConvexMeshShape.cpp index b90f11b6..a8912ee2 100644 --- a/src/collision/shapes/ConvexMeshShape.cpp +++ b/src/collision/shapes/ConvexMeshShape.cpp @@ -49,7 +49,7 @@ ConvexMeshShape::ConvexMeshShape(PolyhedronMesh* polyhedronMesh, MemoryAllocator // Return a local support point in a given direction without the object margin. /// If the edges information is not used for collision detection, this method will go through -/// the whole vertices list and pick up the vertex with the largest dot product in the support +/// the whole vertices array and pick up the vertex with the largest dot product in the support /// direction. This is an O(n) process with "n" being the number of vertices in the mesh. /// However, if the edges information is used, we can cache the previous support vertex and use /// it as a start in a hill-climbing (local search) process to find the new support vertex which @@ -231,7 +231,7 @@ std::string ConvexMeshShape::to_string() const { ss << "["; - for (uint v=0; v < face.faceVertices.size(); v++) { + for (uint32 v=0; v < face.faceVertices.size(); v++) { ss << face.faceVertices[v]; if (v != face.faceVertices.size() - 1) { diff --git a/src/collision/shapes/ConvexPolyhedronShape.cpp b/src/collision/shapes/ConvexPolyhedronShape.cpp index bb907a2b..28c81c20 100644 --- a/src/collision/shapes/ConvexPolyhedronShape.cpp +++ b/src/collision/shapes/ConvexPolyhedronShape.cpp @@ -35,25 +35,3 @@ ConvexPolyhedronShape::ConvexPolyhedronShape(CollisionShapeName name, MemoryAllo : ConvexShape(name, CollisionShapeType::CONVEX_POLYHEDRON, allocator) { } - -// Find and return the index of the polyhedron face with the most anti-parallel face -// normal given a direction vector. This is used to find the incident face on -// a polyhedron of a given reference face of another polyhedron -uint ConvexPolyhedronShape::findMostAntiParallelFace(const Vector3& direction) const { - - decimal minDotProduct = DECIMAL_LARGEST; - uint mostAntiParallelFace = 0; - - // For each face of the polyhedron - for (uint i=0; i < getNbFaces(); i++) { - - // Get the face normal - decimal dotProduct = getFaceNormal(i).dot(direction); - if (dotProduct < minDotProduct) { - minDotProduct = dotProduct; - mostAntiParallelFace = i; - } - } - - return mostAntiParallelFace; -} diff --git a/src/collision/shapes/HeightFieldShape.cpp b/src/collision/shapes/HeightFieldShape.cpp index 7c2e99cd..22d76431 100644 --- a/src/collision/shapes/HeightFieldShape.cpp +++ b/src/collision/shapes/HeightFieldShape.cpp @@ -27,6 +27,7 @@ #include #include #include +#include using namespace reactphysics3d; @@ -42,12 +43,13 @@ using namespace reactphysics3d; * @param integerHeightScale Scaling factor used to scale the height values (only when height values type is integer) */ HeightFieldShape::HeightFieldShape(int nbGridColumns, int nbGridRows, decimal minHeight, decimal maxHeight, - const void* heightFieldData, HeightDataType dataType, MemoryAllocator& allocator, int upAxis, + const void* heightFieldData, HeightDataType dataType, MemoryAllocator& allocator, + HalfEdgeStructure& triangleHalfEdgeStructure, int upAxis, decimal integerHeightScale, const Vector3& scaling) : ConcaveShape(CollisionShapeName::HEIGHTFIELD, allocator, scaling), mNbColumns(nbGridColumns), mNbRows(nbGridRows), mWidth(nbGridColumns - 1), mLength(nbGridRows - 1), mMinHeight(minHeight), mMaxHeight(maxHeight), mUpAxis(upAxis), mIntegerHeightScale(integerHeightScale), - mHeightDataType(dataType) { + mHeightDataType(dataType), mTriangleHalfEdgeStructure(triangleHalfEdgeStructure) { assert(nbGridColumns >= 2); assert(nbGridRows >= 2); @@ -91,8 +93,8 @@ void HeightFieldShape::getLocalBounds(Vector3& min, Vector3& max) const { // of the body when need to test and see against which triangles of the height-field we need // to test for collision. We compute the sub-grid points that are inside the other body's AABB // and then for each rectangle in the sub-grid we generate two triangles that we use to test collision. -void HeightFieldShape::computeOverlappingTriangles(const AABB& localAABB, List& triangleVertices, - List& triangleVerticesNormals, List& shapeIds, +void HeightFieldShape::computeOverlappingTriangles(const AABB& localAABB, Array& triangleVertices, + Array& triangleVerticesNormals, Array& shapeIds, MemoryAllocator& allocator) const { RP3D_PROFILE("HeightFieldShape::computeOverlappingTriangles()", mProfiler); @@ -231,64 +233,186 @@ bool HeightFieldShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, Collide RP3D_PROFILE("HeightFieldShape::raycast()", mProfiler); - // Compute the AABB for the ray - const Vector3 rayEnd = ray.point1 + ray.maxFraction * (ray.point2 - ray.point1); - const AABB rayAABB(Vector3::min(ray.point1, rayEnd), Vector3::max(ray.point1, rayEnd)); - - // Compute the triangles overlapping with the ray AABB - List triangleVertices(allocator); - List triangleVerticesNormals(allocator); - List shapeIds(allocator); - computeOverlappingTriangles(rayAABB, triangleVertices, triangleVerticesNormals, shapeIds, allocator); - - assert(triangleVertices.size() == triangleVerticesNormals.size()); - assert(shapeIds.size() == triangleVertices.size() / 3); - assert(triangleVertices.size() % 3 == 0); - assert(triangleVerticesNormals.size() % 3 == 0); + // Apply the concave mesh inverse scale factor because the mesh is stored without scaling + // inside the dynamic AABB tree + const Vector3 inverseScale(decimal(1.0) / mScale.x, decimal(1.0) / mScale.y, decimal(1.0) / mScale.z); + Ray scaledRay(ray.point1 * inverseScale, ray.point2 * inverseScale, ray.maxFraction); bool isHit = false; - decimal smallestHitFraction = ray.maxFraction; - // For each overlapping triangle - for (uint i=0; i < shapeIds.size(); i++) - { - // Create a triangle collision shape - TriangleShape triangleShape(&(triangleVertices[i * 3]), &(triangleVerticesNormals[i * 3]), shapeIds[i], allocator); - triangleShape.setRaycastTestType(getRaycastTestType()); + // Compute the grid coordinates where the ray is entering the AABB of the height field + int i, j; + Vector3 outHitGridPoint; + if (computeEnteringRayGridCoordinates(scaledRay, i, j, outHitGridPoint)) { - #ifdef IS_RP3D_PROFILING_ENABLED + const int nbCellsI = mNbColumns - 1; + const int nbCellsJ = mNbRows - 1; + const Vector3 aabbSize = mAABB.getExtent(); - // Set the profiler to the triangle shape - triangleShape.setProfiler(mProfiler); + const Vector3 rayDirection = scaledRay.point2 - scaledRay.point1; - #endif + int stepI, stepJ; + decimal tMaxI, tMaxJ, nextI, nextJ, tDeltaI, tDeltaJ, sizeI, sizeJ; - // Ray casting test against the collision shape - RaycastInfo triangleRaycastInfo; - bool isTriangleHit = triangleShape.raycast(ray, triangleRaycastInfo, collider, allocator); + switch(mUpAxis) { + case 0 : stepI = rayDirection.y > 0 ? 1 : (rayDirection.y < 0 ? -1 : 0); + stepJ = rayDirection.z > 0 ? 1 : (rayDirection.z < 0 ? -1 : 0); + nextI = stepI >= 0 ? i + 1 : i; + nextJ = stepJ >= 0 ? j + 1 : j; + sizeI = aabbSize.y / nbCellsI; + sizeJ = aabbSize.z / nbCellsJ; + tMaxI = ((nextI * sizeI) - outHitGridPoint.y) / rayDirection.y; + tMaxJ = ((nextJ * sizeJ) - outHitGridPoint.z) / rayDirection.z; + tDeltaI = sizeI / std::abs(rayDirection.y); + tDeltaJ = sizeJ / std::abs(rayDirection.z); + break; + case 1 : stepI = rayDirection.x > 0 ? 1 : (rayDirection.x < 0 ? -1 : 0); + stepJ = rayDirection.z > 0 ? 1 : (rayDirection.z < 0 ? -1 : 0); + nextI = stepI >= 0 ? i + 1 : i; + nextJ = stepJ >= 0 ? j + 1 : j; + sizeI = aabbSize.x / nbCellsI; + sizeJ = aabbSize.z / nbCellsJ; + tMaxI = ((nextI * sizeI) - outHitGridPoint.x) / rayDirection.x; + tMaxJ = ((nextJ * sizeJ) - outHitGridPoint.z) / rayDirection.z; + tDeltaI = sizeI / std::abs(rayDirection.x); + tDeltaJ = sizeJ / std::abs(rayDirection.z); + break; + case 2 : stepI = rayDirection.x > 0 ? 1 : (rayDirection.x < 0 ? -1 : 0); + stepJ = rayDirection.y > 0 ? 1 : (rayDirection.y < 0 ? -1 : 0); + nextI = stepI >= 0 ? i + 1 : i; + nextJ = stepJ >= 0 ? j + 1 : j; + sizeI = aabbSize.x / nbCellsI; + sizeJ = aabbSize.y / nbCellsJ; + tMaxI = ((nextI * sizeI) - outHitGridPoint.x) / rayDirection.x; + tMaxJ = ((nextJ * sizeJ) - outHitGridPoint.y) / rayDirection.y; + tDeltaI = sizeI / std::abs(rayDirection.x); + tDeltaJ = sizeJ / std::abs(rayDirection.y); + break; + } - // If the ray hit the collision shape - if (isTriangleHit && triangleRaycastInfo.hitFraction <= smallestHitFraction) { + decimal smallestHitFraction = ray.maxFraction; - assert(triangleRaycastInfo.hitFraction >= decimal(0.0)); + while (i >= 0 && i < nbCellsI && j >= 0 && j < nbCellsJ) { - raycastInfo.body = triangleRaycastInfo.body; - raycastInfo.collider = triangleRaycastInfo.collider; - raycastInfo.hitFraction = triangleRaycastInfo.hitFraction; - raycastInfo.worldPoint = triangleRaycastInfo.worldPoint; - raycastInfo.worldNormal = triangleRaycastInfo.worldNormal; - raycastInfo.meshSubpart = -1; - raycastInfo.triangleIndex = -1; + // TODO : Remove this + //std::cout << "Cell " << i << ", " << j << std::endl; - smallestHitFraction = triangleRaycastInfo.hitFraction; - isHit = true; + // Compute the four point of the current quad + const Vector3 p1 = getVertexAt(i, j); + const Vector3 p2 = getVertexAt(i, j + 1); + const Vector3 p3 = getVertexAt(i + 1, j); + const Vector3 p4 = getVertexAt(i + 1, j + 1); + + // Raycast against the first triangle of the cell + uint shapeId = computeTriangleShapeId(i, j, 0); + isHit |= raycastTriangle(ray, p1, p2, p3, shapeId, collider, raycastInfo, smallestHitFraction, allocator); + + // Raycast against the second triangle of the cell + shapeId = computeTriangleShapeId(i, j, 1); + isHit |= raycastTriangle(ray, p3, p2, p4, shapeId, collider, raycastInfo, smallestHitFraction, allocator); + + if (stepI == 0 && stepJ == 0) break; + + if (tMaxI < tMaxJ) { + tMaxI += tDeltaI; + i += stepI; + } + else { + tMaxJ += tDeltaJ; + j += stepJ; + } } } return isHit; } +// Raycast a single triangle of the height-field +bool HeightFieldShape::raycastTriangle(const Ray& ray, const Vector3& p1, const Vector3& p2, const Vector3& p3, uint shapeId, + Collider* collider, RaycastInfo& raycastInfo, decimal& smallestHitFraction, MemoryAllocator& allocator) const { + + // Generate the first triangle for the current grid rectangle + Vector3 triangleVertices[3] = {p1, p2, p3}; + + // Create a triangle collision shape + TriangleShape triangleShape(triangleVertices, shapeId, mTriangleHalfEdgeStructure, allocator); + triangleShape.setRaycastTestType(getRaycastTestType()); + +#ifdef IS_RP3D_PROFILING_ENABLED + + + // Set the profiler to the triangle shape + triangleShape.setProfiler(mProfiler); + +#endif + + // Ray casting test against the collision shape + RaycastInfo triangleRaycastInfo; + bool isTriangleHit = triangleShape.raycast(ray, triangleRaycastInfo, collider, allocator); + + // If the ray hit the collision shape + if (isTriangleHit && triangleRaycastInfo.hitFraction <= smallestHitFraction) { + + assert(triangleRaycastInfo.hitFraction >= decimal(0.0)); + + raycastInfo.body = triangleRaycastInfo.body; + raycastInfo.collider = triangleRaycastInfo.collider; + raycastInfo.hitFraction = triangleRaycastInfo.hitFraction; + raycastInfo.worldPoint = triangleRaycastInfo.worldPoint; + raycastInfo.worldNormal = triangleRaycastInfo.worldNormal; + raycastInfo.meshSubpart = -1; + raycastInfo.triangleIndex = -1; + + smallestHitFraction = triangleRaycastInfo.hitFraction; + + return true; + } + + return false; +} + +// Compute the first grid cell of the heightfield intersected by a ray. +/// This method returns true if the ray hit the AABB of the height field and false otherwise +bool HeightFieldShape::computeEnteringRayGridCoordinates(const Ray& ray, int& i, int& j, Vector3& outHitGridPoint) const { + + decimal stepI, stepJ; + const Vector3 aabbSize = mAABB.getExtent(); + + const uint32 nbCellsI = mNbColumns - 1; + const uint32 nbCellsJ = mNbRows - 1; + + if (mAABB.raycast(ray, outHitGridPoint)) { + + // Map the hit point into the grid range [0, mNbColumns - 1], [0, mNbRows - 1] + outHitGridPoint -= mAABB.getMin(); + + switch(mUpAxis) { + case 0 : stepI = aabbSize.y / nbCellsI; + stepJ = aabbSize.z / nbCellsJ; + i = clamp(int(outHitGridPoint.y / stepI), 0, nbCellsI - 1); + j = clamp(int(outHitGridPoint.z / stepJ), 0, nbCellsJ - 1); + break; + case 1 : stepI = aabbSize.x / nbCellsI; + stepJ = aabbSize.z / nbCellsJ; + i = clamp(int(outHitGridPoint.x / stepI), 0, nbCellsI - 1); + j = clamp(int(outHitGridPoint.z / stepJ), 0, nbCellsJ - 1); + break; + case 2 : stepI = aabbSize.x / nbCellsI; + stepJ = aabbSize.y / nbCellsJ; + i = clamp(int(outHitGridPoint.x / stepI), 0, nbCellsI - 1); + j = clamp(int(outHitGridPoint.y / stepJ), 0, nbCellsJ - 1); + break; + } + + assert(i >= 0 && i < nbCellsI); + assert(j >= 0 && j < nbCellsJ); + return true; + } + + return false; +} + // Return the vertex (local-coordinates) of the height field at a given (x,y) position Vector3 HeightFieldShape::getVertexAt(int x, int y) const { diff --git a/src/collision/shapes/TriangleShape.cpp b/src/collision/shapes/TriangleShape.cpp index 2d1b1380..c072ad84 100644 --- a/src/collision/shapes/TriangleShape.cpp +++ b/src/collision/shapes/TriangleShape.cpp @@ -29,6 +29,7 @@ #include #include #include +#include #include #include @@ -36,18 +37,8 @@ using namespace reactphysics3d; // Constructor -/** - * Do not use this constructor. It is supposed to be used internally only. - * Use a ConcaveMeshShape instead. - * @param point1 First point of the triangle - * @param point2 Second point of the triangle - * @param point3 Third point of the triangle - * @param verticesNormals The three vertices normals for smooth mesh collision - * @param margin The collision margin (in meters) around the collision shape - */ -TriangleShape::TriangleShape(const Vector3* vertices, const Vector3* verticesNormals, uint shapeId, - MemoryAllocator& allocator) - : ConvexPolyhedronShape(CollisionShapeName::TRIANGLE, allocator), mFaces{HalfEdgeStructure::Face(allocator), HalfEdgeStructure::Face(allocator)} { +TriangleShape::TriangleShape(const Vector3* vertices, const Vector3* verticesNormals, uint shapeId, HalfEdgeStructure& triangleHalfEdgeStructure, MemoryAllocator& allocator) + : ConvexPolyhedronShape(CollisionShapeName::TRIANGLE, allocator), mTriangleHalfEdgeStructure(triangleHalfEdgeStructure) { mPoints[0] = vertices[0]; mPoints[1] = vertices[1]; @@ -61,97 +52,32 @@ TriangleShape::TriangleShape(const Vector3* vertices, const Vector3* verticesNor mVerticesNormals[1] = verticesNormals[1]; mVerticesNormals[2] = verticesNormals[2]; - // Faces - mFaces[0].faceVertices.reserve(3); - mFaces[0].faceVertices.add(0); - mFaces[0].faceVertices.add(1); - mFaces[0].faceVertices.add(2); - mFaces[0].edgeIndex = 0; - - mFaces[1].faceVertices.reserve(3); - mFaces[1].faceVertices.add(0); - mFaces[1].faceVertices.add(2); - mFaces[1].faceVertices.add(1); - mFaces[1].edgeIndex = 1; - - // Edges - for (uint i=0; i<6; i++) { - switch(i) { - case 0: - mEdges[0].vertexIndex = 0; - mEdges[0].twinEdgeIndex = 1; - mEdges[0].faceIndex = 0; - mEdges[0].nextEdgeIndex = 2; - break; - case 1: - mEdges[1].vertexIndex = 1; - mEdges[1].twinEdgeIndex = 0; - mEdges[1].faceIndex = 1; - mEdges[1].nextEdgeIndex = 5; - break; - case 2: - mEdges[2].vertexIndex = 1; - mEdges[2].twinEdgeIndex = 3; - mEdges[2].faceIndex = 0; - mEdges[2].nextEdgeIndex = 4; - break; - case 3: - mEdges[3].vertexIndex = 2; - mEdges[3].twinEdgeIndex = 2; - mEdges[3].faceIndex = 1; - mEdges[3].nextEdgeIndex = 1; - break; - case 4: - mEdges[4].vertexIndex = 2; - mEdges[4].twinEdgeIndex = 5; - mEdges[4].faceIndex = 0; - mEdges[4].nextEdgeIndex = 0; - break; - case 5: - mEdges[5].vertexIndex = 0; - mEdges[5].twinEdgeIndex = 4; - mEdges[5].faceIndex = 1; - mEdges[5].nextEdgeIndex = 3; - break; - } - } - - mRaycastTestType = TriangleRaycastSide::FRONT; mId = shapeId; } -// This method compute the smooth mesh contact with a triangle in case one of the two collision -// shapes is a triangle. The idea in this case is to use a smooth vertex normal of the triangle mesh -// at the contact point instead of the triangle normal to avoid the internal edge collision issue. -// This method will return the new smooth world contact -// normal of the triangle and the the local contact point on the other shape. -void TriangleShape::computeSmoothTriangleMeshContact(const CollisionShape* shape1, const CollisionShape* shape2, - Vector3& localContactPointShape1, Vector3& localContactPointShape2, - const Transform& shape1ToWorld, const Transform& shape2ToWorld, - decimal penetrationDepth, Vector3& outSmoothVertexNormal) { +// Constructor for raycasting +TriangleShape::TriangleShape(const Vector3* vertices, uint shapeId, HalfEdgeStructure& triangleHalfEdgeStructure, MemoryAllocator& allocator) + : ConvexPolyhedronShape(CollisionShapeName::TRIANGLE, allocator), mTriangleHalfEdgeStructure(triangleHalfEdgeStructure) { - assert(shape1->getName() != CollisionShapeName::TRIANGLE || shape2->getName() != CollisionShapeName::TRIANGLE); + mPoints[0] = vertices[0]; + mPoints[1] = vertices[1]; + mPoints[2] = vertices[2]; - // If one the shape is a triangle - bool isShape1Triangle = shape1->getName() == CollisionShapeName::TRIANGLE; - if (isShape1Triangle || shape2->getName() == CollisionShapeName::TRIANGLE) { + // The normal is not used when creating the triangle shape with this constructor (for raycasting for instance) + mNormal = Vector3(0, 0, 0); - const TriangleShape* triangleShape = isShape1Triangle ? static_cast(shape1): - static_cast(shape2); + // Interpolated normals are not used in this constructor (for raycasting for instance) + mVerticesNormals[0] = mNormal; + mVerticesNormals[1] = mNormal; + mVerticesNormals[2] = mNormal; - // Compute the smooth triangle mesh contact normal and recompute the local contact point on the other shape - triangleShape->computeSmoothMeshContact(isShape1Triangle ? localContactPointShape1 : localContactPointShape2, - isShape1Triangle ? shape1ToWorld : shape2ToWorld, - isShape1Triangle ? shape2ToWorld.getInverse() : shape1ToWorld.getInverse(), - penetrationDepth, isShape1Triangle, - isShape1Triangle ? localContactPointShape2 : localContactPointShape1, - outSmoothVertexNormal); - } + mRaycastTestType = TriangleRaycastSide::FRONT; + + mId = shapeId; } - // This method implements the technique described in Game Physics Pearl book // by Gino van der Bergen and Dirk Gregorius to get smooth triangle mesh collision. The idea is // to replace the contact normal of the triangle shape with the precomputed normal of the triangle @@ -186,42 +112,6 @@ void TriangleShape::computeSmoothMeshContact(Vector3 localContactPointTriangle, outNewLocalContactPointOtherShape.setAllValues(otherShapePoint.x, otherShapePoint.y, otherShapePoint.z); } -// Get a smooth contact normal for collision for a triangle of the mesh -/// This is used to avoid the internal edges issue that occurs when a shape is colliding with -/// several triangles of a concave mesh. If the shape collide with an edge of the triangle for instance, -/// the computed contact normal from this triangle edge is not necessarily in the direction of the surface -/// normal of the mesh at this point. The idea to solve this problem is to use the real (smooth) surface -/// normal of the mesh at this point as the contact normal. This technique is described in the chapter 5 -/// of the Game Physics Pearl book by Gino van der Bergen and Dirk Gregorius. The vertices normals of the -/// mesh are either provided by the user or precomputed if the user did not provide them. Note that we only -/// use the interpolated normal if the contact point is on an edge of the triangle. If the contact is in the -/// middle of the triangle, we return the true triangle normal. -Vector3 TriangleShape::computeSmoothLocalContactNormalForTriangle(const Vector3& localContactPoint) const { - - // Compute the barycentric coordinates of the point in the triangle - decimal u, v, w; - computeBarycentricCoordinatesInTriangle(mPoints[0], mPoints[1], mPoints[2], localContactPoint, u, v, w); - - // If the contact is in the middle of the triangle face (not on the edges) - if (u > MACHINE_EPSILON && v > MACHINE_EPSILON && w > MACHINE_EPSILON) { - - // We return the true triangle face normal (not the interpolated one) - return mNormal; - } - - // We compute the contact normal as the barycentric interpolation of the three vertices normals - Vector3 interpolatedNormal = u * mVerticesNormals[0] + v * mVerticesNormals[1] + w * mVerticesNormals[2]; - - // If the interpolated normal is degenerated - if (interpolatedNormal.lengthSquare() < MACHINE_EPSILON) { - - // Return the original normal - return mNormal; - } - - return interpolatedNormal.getUnit(); -} - // Update the AABB of a body using its collision shape /** * @param[out] aabb The axis-aligned bounding box (AABB) of the collision shape @@ -304,15 +194,16 @@ bool TriangleShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, Collider* if (hitFraction < decimal(0.0) || hitFraction > ray.maxFraction) return false; - Vector3 localHitNormal = (mPoints[1] - mPoints[0]).cross(mPoints[2] - mPoints[0]); - if (localHitNormal.dot(pq) > decimal(0.0)) localHitNormal = -localHitNormal; + // Compute the triangle face normal + Vector3 normal = (mPoints[1] - mPoints[0]).cross(mPoints[2] - mPoints[0]); + normal.normalize(); + normal = normal.dot(pq) > decimal(0.0) ? -normal : normal; raycastInfo.body = collider->getBody(); raycastInfo.collider = collider; raycastInfo.worldPoint = localHitPoint; raycastInfo.hitFraction = hitFraction; - raycastInfo.worldNormal = localHitNormal; + raycastInfo.worldNormal = normal; return true; } - diff --git a/src/components/ColliderComponents.cpp b/src/components/ColliderComponents.cpp index 3c296e58..03cdd37e 100644 --- a/src/components/ColliderComponents.cpp +++ b/src/components/ColliderComponents.cpp @@ -37,8 +37,8 @@ using namespace reactphysics3d; ColliderComponents::ColliderComponents(MemoryAllocator& allocator) :Components(allocator, sizeof(Entity) + sizeof(Entity) + sizeof(Collider*) + sizeof(int32) + sizeof(Transform) + sizeof(CollisionShape*) + sizeof(unsigned short) + - sizeof(unsigned short) + sizeof(Transform) + sizeof(List) + sizeof(bool) + - sizeof(bool)) { + sizeof(unsigned short) + sizeof(Transform) + sizeof(Array) + sizeof(bool) + + sizeof(bool) + sizeof(Material)) { // Allocate memory for the components data allocate(INIT_NB_ALLOCATED_COMPONENTS); @@ -66,9 +66,10 @@ void ColliderComponents::allocate(uint32 nbComponentsToAllocate) { unsigned short* newCollisionCategoryBits = reinterpret_cast(newCollisionShapes + nbComponentsToAllocate); unsigned short* newCollideWithMaskBits = reinterpret_cast(newCollisionCategoryBits + nbComponentsToAllocate); Transform* newLocalToWorldTransforms = reinterpret_cast(newCollideWithMaskBits + nbComponentsToAllocate); - List* newOverlappingPairs = reinterpret_cast*>(newLocalToWorldTransforms + nbComponentsToAllocate); + Array* newOverlappingPairs = reinterpret_cast*>(newLocalToWorldTransforms + nbComponentsToAllocate); bool* hasCollisionShapeChangedSize = reinterpret_cast(newOverlappingPairs + nbComponentsToAllocate); bool* isTrigger = reinterpret_cast(hasCollisionShapeChangedSize + nbComponentsToAllocate); + Material* materials = reinterpret_cast(isTrigger + nbComponentsToAllocate); // If there was already components before if (mNbComponents > 0) { @@ -83,9 +84,10 @@ void ColliderComponents::allocate(uint32 nbComponentsToAllocate) { memcpy(newCollisionCategoryBits, mCollisionCategoryBits, mNbComponents * sizeof(unsigned short)); memcpy(newCollideWithMaskBits, mCollideWithMaskBits, mNbComponents * sizeof(unsigned short)); memcpy(newLocalToWorldTransforms, mLocalToWorldTransforms, mNbComponents * sizeof(Transform)); - memcpy(newOverlappingPairs, mOverlappingPairs, mNbComponents * sizeof(List)); + memcpy(newOverlappingPairs, mOverlappingPairs, mNbComponents * sizeof(Array)); memcpy(hasCollisionShapeChangedSize, mHasCollisionShapeChangedSize, mNbComponents * sizeof(bool)); memcpy(isTrigger, mIsTrigger, mNbComponents * sizeof(bool)); + memcpy(materials, mMaterials, mNbComponents * sizeof(Material)); // Deallocate previous memory mMemoryAllocator.release(mBuffer, mNbAllocatedComponents * mComponentDataSize); @@ -105,6 +107,7 @@ void ColliderComponents::allocate(uint32 nbComponentsToAllocate) { mOverlappingPairs = newOverlappingPairs; mHasCollisionShapeChangedSize = hasCollisionShapeChangedSize; mIsTrigger = isTrigger; + mMaterials = materials; mNbAllocatedComponents = nbComponentsToAllocate; } @@ -125,9 +128,10 @@ void ColliderComponents::addComponent(Entity colliderEntity, bool isSleeping, co new (mCollisionCategoryBits + index) unsigned short(component.collisionCategoryBits); new (mCollideWithMaskBits + index) unsigned short(component.collideWithMaskBits); new (mLocalToWorldTransforms + index) Transform(component.localToWorldTransform); - new (mOverlappingPairs + index) List(mMemoryAllocator); + new (mOverlappingPairs + index) Array(mMemoryAllocator); mHasCollisionShapeChangedSize[index] = false; mIsTrigger[index] = false; + mMaterials[index] = component.material; // Map the entity with the new component lookup index mMapEntityToComponentIndex.add(Pair(colliderEntity, index)); @@ -153,9 +157,10 @@ void ColliderComponents::moveComponentToIndex(uint32 srcIndex, uint32 destIndex) new (mCollisionCategoryBits + destIndex) unsigned short(mCollisionCategoryBits[srcIndex]); new (mCollideWithMaskBits + destIndex) unsigned short(mCollideWithMaskBits[srcIndex]); new (mLocalToWorldTransforms + destIndex) Transform(mLocalToWorldTransforms[srcIndex]); - new (mOverlappingPairs + destIndex) List(mOverlappingPairs[srcIndex]); + new (mOverlappingPairs + destIndex) Array(mOverlappingPairs[srcIndex]); mHasCollisionShapeChangedSize[destIndex] = mHasCollisionShapeChangedSize[srcIndex]; mIsTrigger[destIndex] = mIsTrigger[srcIndex]; + mMaterials[destIndex] = mMaterials[srcIndex]; // Destroy the source component destroyComponent(srcIndex); @@ -181,9 +186,10 @@ void ColliderComponents::swapComponents(uint32 index1, uint32 index2) { unsigned short collisionCategoryBits1 = mCollisionCategoryBits[index1]; unsigned short collideWithMaskBits1 = mCollideWithMaskBits[index1]; Transform localToWorldTransform1 = mLocalToWorldTransforms[index1]; - List overlappingPairs = mOverlappingPairs[index1]; + Array overlappingPairs = mOverlappingPairs[index1]; bool hasCollisionShapeChangedSize = mHasCollisionShapeChangedSize[index1]; bool isTrigger = mIsTrigger[index1]; + Material material = mMaterials[index1]; // Destroy component 1 destroyComponent(index1); @@ -200,9 +206,10 @@ void ColliderComponents::swapComponents(uint32 index1, uint32 index2) { new (mCollisionCategoryBits + index2) unsigned short(collisionCategoryBits1); new (mCollideWithMaskBits + index2) unsigned short(collideWithMaskBits1); new (mLocalToWorldTransforms + index2) Transform(localToWorldTransform1); - new (mOverlappingPairs + index2) List(overlappingPairs); + new (mOverlappingPairs + index2) Array(overlappingPairs); mHasCollisionShapeChangedSize[index2] = hasCollisionShapeChangedSize; mIsTrigger[index2] = isTrigger; + mMaterials[index2] = material; // Update the entity to component index mapping mMapEntityToComponentIndex.add(Pair(colliderEntity1, index2)); @@ -227,5 +234,6 @@ void ColliderComponents::destroyComponent(uint32 index) { mLocalToBodyTransforms[index].~Transform(); mCollisionShapes[index] = nullptr; mLocalToWorldTransforms[index].~Transform(); - mOverlappingPairs[index].~List(); + mOverlappingPairs[index].~Array(); + mMaterials[index].~Material(); } diff --git a/src/components/CollisionBodyComponents.cpp b/src/components/CollisionBodyComponents.cpp index eeadf088..8aee03db 100644 --- a/src/components/CollisionBodyComponents.cpp +++ b/src/components/CollisionBodyComponents.cpp @@ -34,7 +34,7 @@ using namespace reactphysics3d; // Constructor CollisionBodyComponents::CollisionBodyComponents(MemoryAllocator& allocator) - :Components(allocator, sizeof(Entity) + sizeof(CollisionBody*) + sizeof(List) + + :Components(allocator, sizeof(Entity) + sizeof(CollisionBody*) + sizeof(Array) + sizeof(bool) + sizeof(void*)) { // Allocate memory for the components data @@ -56,7 +56,7 @@ void CollisionBodyComponents::allocate(uint32 nbComponentsToAllocate) { // New pointers to components data Entity* newBodiesEntities = static_cast(newBuffer); CollisionBody** newBodies = reinterpret_cast(newBodiesEntities + nbComponentsToAllocate); - List* newColliders = reinterpret_cast*>(newBodies + nbComponentsToAllocate); + Array* newColliders = reinterpret_cast*>(newBodies + nbComponentsToAllocate); bool* newIsActive = reinterpret_cast(newColliders + nbComponentsToAllocate); void** newUserData = reinterpret_cast(newIsActive + nbComponentsToAllocate); @@ -66,7 +66,7 @@ void CollisionBodyComponents::allocate(uint32 nbComponentsToAllocate) { // Copy component data from the previous buffer to the new one memcpy(newBodiesEntities, mBodiesEntities, mNbComponents * sizeof(Entity)); memcpy(newBodies, mBodies, mNbComponents * sizeof(CollisionBody*)); - memcpy(newColliders, mColliders, mNbComponents * sizeof(List)); + memcpy(newColliders, mColliders, mNbComponents * sizeof(Array)); memcpy(newIsActive, mIsActive, mNbComponents * sizeof(bool)); memcpy(newUserData, mUserData, mNbComponents * sizeof(void*)); @@ -92,7 +92,7 @@ void CollisionBodyComponents::addComponent(Entity bodyEntity, bool isSleeping, c // Insert the new component data new (mBodiesEntities + index) Entity(bodyEntity); mBodies[index] = component.body; - new (mColliders + index) List(mMemoryAllocator); + new (mColliders + index) Array(mMemoryAllocator); mIsActive[index] = true; mUserData[index] = nullptr; @@ -114,7 +114,7 @@ void CollisionBodyComponents::moveComponentToIndex(uint32 srcIndex, uint32 destI // Copy the data of the source component to the destination location new (mBodiesEntities + destIndex) Entity(mBodiesEntities[srcIndex]); mBodies[destIndex] = mBodies[srcIndex]; - new (mColliders + destIndex) List(mColliders[srcIndex]); + new (mColliders + destIndex) Array(mColliders[srcIndex]); mIsActive[destIndex] = mIsActive[srcIndex]; mUserData[destIndex] = mUserData[srcIndex]; @@ -135,7 +135,7 @@ void CollisionBodyComponents::swapComponents(uint32 index1, uint32 index2) { // Copy component 1 data Entity entity1(mBodiesEntities[index1]); CollisionBody* body1 = mBodies[index1]; - List colliders1(mColliders[index1]); + Array colliders1(mColliders[index1]); bool isActive1 = mIsActive[index1]; void* userData1 = mUserData[index1]; @@ -146,7 +146,7 @@ void CollisionBodyComponents::swapComponents(uint32 index1, uint32 index2) { // Reconstruct component 1 at component 2 location new (mBodiesEntities + index2) Entity(entity1); - new (mColliders + index2) List(colliders1); + new (mColliders + index2) Array(colliders1); mBodies[index2] = body1; mIsActive[index2] = isActive1; mUserData[index2] = userData1; @@ -170,6 +170,6 @@ void CollisionBodyComponents::destroyComponent(uint32 index) { mBodiesEntities[index].~Entity(); mBodies[index] = nullptr; - mColliders[index].~List(); + mColliders[index].~Array(); mUserData[index] = nullptr; } diff --git a/src/components/FixedJointComponents.cpp b/src/components/FixedJointComponents.cpp index d7c8c129..4ae8552a 100644 --- a/src/components/FixedJointComponents.cpp +++ b/src/components/FixedJointComponents.cpp @@ -163,7 +163,7 @@ void FixedJointComponents::moveComponentToIndex(uint32 srcIndex, uint32 destInde new (mR2World + destIndex) Vector3(mR2World[srcIndex]); new (mI1 + destIndex) Matrix3x3(mI1[srcIndex]); new (mI2 + destIndex) Matrix3x3(mI2[srcIndex]); - new (mImpulseTranslation + destIndex) Vector3(mImpulseRotation[srcIndex]); + new (mImpulseTranslation + destIndex) Vector3(mImpulseTranslation[srcIndex]); new (mImpulseRotation + destIndex) Vector3(mImpulseRotation[srcIndex]); new (mInverseMassMatrixTranslation + destIndex) Matrix3x3(mInverseMassMatrixTranslation[srcIndex]); new (mInverseMassMatrixRotation + destIndex) Matrix3x3(mInverseMassMatrixRotation[srcIndex]); diff --git a/src/components/RigidBodyComponents.cpp b/src/components/RigidBodyComponents.cpp index fa3086e1..ce851b4d 100644 --- a/src/components/RigidBodyComponents.cpp +++ b/src/components/RigidBodyComponents.cpp @@ -40,10 +40,10 @@ RigidBodyComponents::RigidBodyComponents(MemoryAllocator& allocator) sizeof(Vector3) + sizeof(Vector3) + sizeof(Vector3) + sizeof(Vector3) + sizeof(decimal) + sizeof(decimal) + sizeof(decimal) + sizeof(decimal) + sizeof(Vector3) + - sizeof(Vector3) + sizeof(Vector3) + sizeof(Vector3) + + sizeof(Vector3) + + sizeof(Matrix3x3) + sizeof(Vector3) + sizeof(Vector3) + sizeof(Vector3) + sizeof(Vector3) + sizeof(Vector3) + sizeof(Quaternion) + sizeof(Vector3) + sizeof(Vector3) + - sizeof(bool) + sizeof(bool) + sizeof(List)) { + sizeof(bool) + sizeof(bool) + sizeof(Array) + sizeof(Array)) { // Allocate memory for the components data allocate(INIT_NB_ALLOCATED_COMPONENTS); @@ -78,7 +78,8 @@ void RigidBodyComponents::allocate(uint32 nbComponentsToAllocate) { decimal* newInverseMasses = reinterpret_cast(newMasses + nbComponentsToAllocate); Vector3* newInertiaTensorLocal = reinterpret_cast(newInverseMasses + nbComponentsToAllocate); Vector3* newInertiaTensorLocalInverses = reinterpret_cast(newInertiaTensorLocal + nbComponentsToAllocate); - Vector3* newConstrainedLinearVelocities = reinterpret_cast(newInertiaTensorLocalInverses + nbComponentsToAllocate); + Matrix3x3* newInertiaTensorWorldInverses = reinterpret_cast(newInertiaTensorLocalInverses + nbComponentsToAllocate); + Vector3* newConstrainedLinearVelocities = reinterpret_cast(newInertiaTensorWorldInverses + nbComponentsToAllocate); Vector3* newConstrainedAngularVelocities = reinterpret_cast(newConstrainedLinearVelocities + nbComponentsToAllocate); Vector3* newSplitLinearVelocities = reinterpret_cast(newConstrainedAngularVelocities + nbComponentsToAllocate); Vector3* newSplitAngularVelocities = reinterpret_cast(newSplitLinearVelocities + nbComponentsToAllocate); @@ -88,7 +89,8 @@ void RigidBodyComponents::allocate(uint32 nbComponentsToAllocate) { Vector3* newCentersOfMassWorld = reinterpret_cast(newCentersOfMassLocal + nbComponentsToAllocate); bool* newIsGravityEnabled = reinterpret_cast(newCentersOfMassWorld + nbComponentsToAllocate); bool* newIsAlreadyInIsland = reinterpret_cast(newIsGravityEnabled + nbComponentsToAllocate); - List* newJoints = reinterpret_cast*>(newIsAlreadyInIsland + nbComponentsToAllocate); + Array* newJoints = reinterpret_cast*>(newIsAlreadyInIsland + nbComponentsToAllocate); + Array* newContactPairs = reinterpret_cast*>(newJoints + nbComponentsToAllocate); // If there was already components before if (mNbComponents > 0) { @@ -110,6 +112,7 @@ void RigidBodyComponents::allocate(uint32 nbComponentsToAllocate) { memcpy(newInverseMasses, mInverseMasses, mNbComponents * sizeof(decimal)); memcpy(newInertiaTensorLocal, mLocalInertiaTensors, mNbComponents * sizeof(Vector3)); memcpy(newInertiaTensorLocalInverses, mInverseInertiaTensorsLocal, mNbComponents * sizeof(Vector3)); + memcpy(newInertiaTensorWorldInverses, mInverseInertiaTensorsWorld, mNbComponents * sizeof(Matrix3x3)); memcpy(newConstrainedLinearVelocities, mConstrainedLinearVelocities, mNbComponents * sizeof(Vector3)); memcpy(newConstrainedAngularVelocities, mConstrainedAngularVelocities, mNbComponents * sizeof(Vector3)); memcpy(newSplitLinearVelocities, mSplitLinearVelocities, mNbComponents * sizeof(Vector3)); @@ -120,7 +123,8 @@ void RigidBodyComponents::allocate(uint32 nbComponentsToAllocate) { memcpy(newCentersOfMassWorld, mCentersOfMassWorld, mNbComponents * sizeof(Vector3)); memcpy(newIsGravityEnabled, mIsGravityEnabled, mNbComponents * sizeof(bool)); memcpy(newIsAlreadyInIsland, mIsAlreadyInIsland, mNbComponents * sizeof(bool)); - memcpy(newJoints, mJoints, mNbComponents * sizeof(List)); + memcpy(newJoints, mJoints, mNbComponents * sizeof(Array)); + memcpy(newContactPairs, mContactPairs, mNbComponents * sizeof(Array)); // Deallocate previous memory mMemoryAllocator.release(mBuffer, mNbAllocatedComponents * mComponentDataSize); @@ -144,6 +148,7 @@ void RigidBodyComponents::allocate(uint32 nbComponentsToAllocate) { mInverseMasses = newInverseMasses; mLocalInertiaTensors = newInertiaTensorLocal; mInverseInertiaTensorsLocal = newInertiaTensorLocalInverses; + mInverseInertiaTensorsWorld = newInertiaTensorWorldInverses; mConstrainedLinearVelocities = newConstrainedLinearVelocities; mConstrainedAngularVelocities = newConstrainedAngularVelocities; mSplitLinearVelocities = newSplitLinearVelocities; @@ -155,6 +160,7 @@ void RigidBodyComponents::allocate(uint32 nbComponentsToAllocate) { mIsGravityEnabled = newIsGravityEnabled; mIsAlreadyInIsland = newIsAlreadyInIsland; mJoints = newJoints; + mContactPairs = newContactPairs; } // Add a component @@ -180,6 +186,7 @@ void RigidBodyComponents::addComponent(Entity bodyEntity, bool isSleeping, const mInverseMasses[index] = decimal(1.0); new (mLocalInertiaTensors + index) Vector3(1.0, 1.0, 1.0); new (mInverseInertiaTensorsLocal + index) Vector3(1.0, 1.0, 1.0); + new (mInverseInertiaTensorsWorld + index) Matrix3x3(1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0); new (mConstrainedLinearVelocities + index) Vector3(0, 0, 0); new (mConstrainedAngularVelocities + index) Vector3(0, 0, 0); new (mSplitLinearVelocities + index) Vector3(0, 0, 0); @@ -190,7 +197,8 @@ void RigidBodyComponents::addComponent(Entity bodyEntity, bool isSleeping, const new (mCentersOfMassWorld + index) Vector3(component.worldPosition); mIsGravityEnabled[index] = true; mIsAlreadyInIsland[index] = false; - new (mJoints + index) List(mMemoryAllocator); + new (mJoints + index) Array(mMemoryAllocator); + new (mContactPairs + index) Array(mMemoryAllocator); // Map the entity with the new component lookup index mMapEntityToComponentIndex.add(Pair(bodyEntity, index)); @@ -224,6 +232,7 @@ void RigidBodyComponents::moveComponentToIndex(uint32 srcIndex, uint32 destIndex mInverseMasses[destIndex] = mInverseMasses[srcIndex]; new (mLocalInertiaTensors + destIndex) Vector3(mLocalInertiaTensors[srcIndex]); new (mInverseInertiaTensorsLocal + destIndex) Vector3(mInverseInertiaTensorsLocal[srcIndex]); + new (mInverseInertiaTensorsWorld + destIndex) Matrix3x3(mInverseInertiaTensorsWorld[srcIndex]); new (mConstrainedLinearVelocities + destIndex) Vector3(mConstrainedLinearVelocities[srcIndex]); new (mConstrainedAngularVelocities + destIndex) Vector3(mConstrainedAngularVelocities[srcIndex]); new (mSplitLinearVelocities + destIndex) Vector3(mSplitLinearVelocities[srcIndex]); @@ -234,7 +243,8 @@ void RigidBodyComponents::moveComponentToIndex(uint32 srcIndex, uint32 destIndex new (mCentersOfMassWorld + destIndex) Vector3(mCentersOfMassWorld[srcIndex]); mIsGravityEnabled[destIndex] = mIsGravityEnabled[srcIndex]; mIsAlreadyInIsland[destIndex] = mIsAlreadyInIsland[srcIndex]; - new (mJoints + destIndex) List(mJoints[srcIndex]); + new (mJoints + destIndex) Array(mJoints[srcIndex]); + new (mContactPairs + destIndex) Array(mContactPairs[srcIndex]); // Destroy the source component destroyComponent(srcIndex); @@ -267,6 +277,7 @@ void RigidBodyComponents::swapComponents(uint32 index1, uint32 index2) { decimal inverseMass1 = mInverseMasses[index1]; Vector3 inertiaTensorLocal1 = mLocalInertiaTensors[index1]; Vector3 inertiaTensorLocalInverse1 = mInverseInertiaTensorsLocal[index1]; + Matrix3x3 inertiaTensorWorldInverse1 = mInverseInertiaTensorsWorld[index1]; Vector3 constrainedLinearVelocity1(mConstrainedLinearVelocities[index1]); Vector3 constrainedAngularVelocity1(mConstrainedAngularVelocities[index1]); Vector3 splitLinearVelocity1(mSplitLinearVelocities[index1]); @@ -277,7 +288,8 @@ void RigidBodyComponents::swapComponents(uint32 index1, uint32 index2) { Vector3 centerOfMassWorld1 = mCentersOfMassWorld[index1]; bool isGravityEnabled1 = mIsGravityEnabled[index1]; bool isAlreadyInIsland1 = mIsAlreadyInIsland[index1]; - List joints1 = mJoints[index1]; + Array joints1 = mJoints[index1]; + Array contactPairs1 = mContactPairs[index1]; // Destroy component 1 destroyComponent(index1); @@ -301,6 +313,7 @@ void RigidBodyComponents::swapComponents(uint32 index1, uint32 index2) { mInverseMasses[index2] = inverseMass1; mLocalInertiaTensors[index2] = inertiaTensorLocal1; mInverseInertiaTensorsLocal[index2] = inertiaTensorLocalInverse1; + mInverseInertiaTensorsWorld[index2] = inertiaTensorWorldInverse1; new (mConstrainedLinearVelocities + index2) Vector3(constrainedLinearVelocity1); new (mConstrainedAngularVelocities + index2) Vector3(constrainedAngularVelocity1); new (mSplitLinearVelocities + index2) Vector3(splitLinearVelocity1); @@ -311,7 +324,8 @@ void RigidBodyComponents::swapComponents(uint32 index1, uint32 index2) { mCentersOfMassWorld[index2] = centerOfMassWorld1; mIsGravityEnabled[index2] = isGravityEnabled1; mIsAlreadyInIsland[index2] = isAlreadyInIsland1; - new (mJoints + index2) List(joints1); + new (mJoints + index2) Array(joints1); + new (mContactPairs + index2) Array(contactPairs1); // Update the entity to component index mapping mMapEntityToComponentIndex.add(Pair(entity1, index2)); @@ -338,6 +352,7 @@ void RigidBodyComponents::destroyComponent(uint32 index) { mExternalTorques[index].~Vector3(); mLocalInertiaTensors[index].~Vector3(); mInverseInertiaTensorsLocal[index].~Vector3(); + mInverseInertiaTensorsWorld[index].~Matrix3x3(); mConstrainedLinearVelocities[index].~Vector3(); mConstrainedAngularVelocities[index].~Vector3(); mSplitLinearVelocities[index].~Vector3(); @@ -346,5 +361,6 @@ void RigidBodyComponents::destroyComponent(uint32 index) { mConstrainedOrientations[index].~Quaternion(); mCentersOfMassLocal[index].~Vector3(); mCentersOfMassWorld[index].~Vector3(); - mJoints[index].~List(); + mJoints[index].~Array(); + mContactPairs[index].~Array(); } diff --git a/src/constraint/ContactPoint.cpp b/src/constraint/ContactPoint.cpp index 6f0ba719..97667ed8 100644 --- a/src/constraint/ContactPoint.cpp +++ b/src/constraint/ContactPoint.cpp @@ -36,7 +36,7 @@ ContactPoint::ContactPoint(const ContactPointInfo* contactInfo, decimal persiste mPenetrationDepth(contactInfo->penetrationDepth), mLocalPointOnShape1(contactInfo->localPoint1), mLocalPointOnShape2(contactInfo->localPoint2), - mIsRestingContact(false), mIsObsolete(false), mNext(nullptr), mPrevious(nullptr), + mIsRestingContact(false), mIsObsolete(false), mPersistentContactDistanceThreshold(persistentContactDistanceThreshold) { assert(mPenetrationDepth > decimal(0.0)); diff --git a/src/constraint/HingeJoint.cpp b/src/constraint/HingeJoint.cpp index fbebd900..b1542096 100644 --- a/src/constraint/HingeJoint.cpp +++ b/src/constraint/HingeJoint.cpp @@ -228,7 +228,7 @@ decimal HingeJoint::getMaxAngleLimit() const { /** * @return The current speed of the joint motor (in radian per second) */ - decimal HingeJoint::getMotorSpeed() const { +decimal HingeJoint::getMotorSpeed() const { return mWorld.mHingeJointsComponents.getMotorSpeed(mEntity); } @@ -236,7 +236,7 @@ decimal HingeJoint::getMaxAngleLimit() const { /** * @return The maximum torque of the joint motor (in Newtons) */ - decimal HingeJoint::getMaxMotorTorque() const { +decimal HingeJoint::getMaxMotorTorque() const { return mWorld.mHingeJointsComponents.getMaxMotorTorque(mEntity); } @@ -245,10 +245,27 @@ decimal HingeJoint::getMaxAngleLimit() const { * @param timeStep The current time step (in seconds) * @return The intensity of the current torque (in Newtons) of the joint motor */ - decimal HingeJoint::getMotorTorque(decimal timeStep) const { +decimal HingeJoint::getMotorTorque(decimal timeStep) const { return mWorld.mHingeJointsComponents.getImpulseMotor(mEntity) / timeStep; } +// Return the current hinge angle +/** + * @return The current hinge angle (in radians) + */ +decimal HingeJoint::getAngle() const { + + // Get the bodies entities + const Entity body1Entity = mWorld.mJointsComponents.getBody1Entity(mEntity); + const Entity body2Entity = mWorld.mJointsComponents.getBody2Entity(mEntity); + + const Quaternion& orientationBody1 = mWorld.mTransformComponents.getTransform(body1Entity).getOrientation(); + const Quaternion& orientationBody2 = mWorld.mTransformComponents.getTransform(body2Entity).getOrientation(); + + // Compute the current angle around the hinge axis + return mWorld.mConstraintSolverSystem.mSolveHingeJointSystem.computeCurrentHingeAngle(mEntity, orientationBody1, orientationBody2); +} + // Return the number of bytes used by the joint size_t HingeJoint::getSizeInBytes() const { return sizeof(HingeJoint); diff --git a/src/engine/EntityManager.cpp b/src/engine/EntityManager.cpp index e7cd4231..15b6aca7 100644 --- a/src/engine/EntityManager.cpp +++ b/src/engine/EntityManager.cpp @@ -53,7 +53,7 @@ Entity EntityManager::createEntity() { mGenerations.add(0); // Create a new indice - index = static_cast(mGenerations.size()) - 1; + index = mGenerations.size() - 1; assert(index < (1 << Entity::ENTITY_INDEX_BITS)); } diff --git a/src/engine/Material.cpp b/src/engine/Material.cpp index bb2aaa35..dee9c015 100644 --- a/src/engine/Material.cpp +++ b/src/engine/Material.cpp @@ -29,15 +29,7 @@ using namespace reactphysics3d; // Constructor -Material::Material(decimal frictionCoefficient, decimal rollingResistance, decimal bounciness, decimal massDensity) - : mFrictionCoefficient(frictionCoefficient), mRollingResistance(rollingResistance), mBounciness(bounciness), - mMassDensity(massDensity) { - -} - -// Copy-constructor -Material::Material(const Material& material) - : mFrictionCoefficient(material.mFrictionCoefficient), mRollingResistance(material.mRollingResistance), - mBounciness(material.mBounciness) { +Material::Material(decimal frictionCoefficient, decimal bounciness, decimal massDensity) + : mFrictionCoefficientSqrt(std::sqrt(frictionCoefficient)), mBounciness(bounciness), mMassDensity(massDensity) { } diff --git a/src/engine/OverlappingPairs.cpp b/src/engine/OverlappingPairs.cpp index 2121143e..372e7231 100644 --- a/src/engine/OverlappingPairs.cpp +++ b/src/engine/OverlappingPairs.cpp @@ -30,498 +30,194 @@ #include #include #include +#include using namespace reactphysics3d; // Constructor -OverlappingPairs::OverlappingPairs(MemoryAllocator& persistentMemoryAllocator, MemoryAllocator& temporaryMemoryAllocator, ColliderComponents &colliderComponents, +OverlappingPairs::OverlappingPairs(MemoryManager& memoryManager, ColliderComponents& colliderComponents, CollisionBodyComponents& collisionBodyComponents, RigidBodyComponents& rigidBodyComponents, Set &noCollisionPairs, CollisionDispatch &collisionDispatch) - : mPersistentAllocator(persistentMemoryAllocator), mTempMemoryAllocator(temporaryMemoryAllocator), - mNbPairs(0), mConcavePairsStartIndex(0), mPairDataSize(sizeof(uint64) + sizeof(int32) + sizeof(int32) + sizeof(Entity) + - sizeof(Entity) + sizeof(Map) + - sizeof(bool) + sizeof(bool) + sizeof(NarrowPhaseAlgorithmType) + - sizeof(bool) + sizeof(bool) + sizeof(bool)), - mNbAllocatedPairs(0), mBuffer(nullptr), - mMapPairIdToPairIndex(persistentMemoryAllocator), + : mPoolAllocator(memoryManager.getPoolAllocator()), mHeapAllocator(memoryManager.getHeapAllocator()), mConvexPairs(memoryManager.getHeapAllocator()), + mConcavePairs(memoryManager.getHeapAllocator()), mMapConvexPairIdToPairIndex(memoryManager.getHeapAllocator()), mMapConcavePairIdToPairIndex(memoryManager.getHeapAllocator()), mColliderComponents(colliderComponents), mCollisionBodyComponents(collisionBodyComponents), mRigidBodyComponents(rigidBodyComponents), mNoCollisionPairs(noCollisionPairs), mCollisionDispatch(collisionDispatch) { - // Allocate memory for the components data - allocate(INIT_NB_ALLOCATED_PAIRS); } // Destructor OverlappingPairs::~OverlappingPairs() { - // If there are allocated pairs - if (mNbAllocatedPairs > 0) { + // Destroy the convex pairs + while (mConvexPairs.size() > 0) { - // Destroy all the remaining pairs - for (uint32 i = 0; i < mNbPairs; i++) { - - // Remove all the remaining last frame collision info - for (auto it = mLastFrameCollisionInfos[i].begin(); it != mLastFrameCollisionInfos[i].end(); ++it) { - - // Call the constructor - it->second->~LastFrameCollisionInfo(); - - // Release memory - mPersistentAllocator.release(it->second, sizeof(LastFrameCollisionInfo)); - } - - // Remove the involved overlapping pair to the two colliders - assert(mColliderComponents.getOverlappingPairs(mColliders1[i]).find(mPairIds[i]) != mColliderComponents.getOverlappingPairs(mColliders1[i]).end()); - assert(mColliderComponents.getOverlappingPairs(mColliders2[i]).find(mPairIds[i]) != mColliderComponents.getOverlappingPairs(mColliders2[i]).end()); - mColliderComponents.getOverlappingPairs(mColliders1[i]).remove(mPairIds[i]); - mColliderComponents.getOverlappingPairs(mColliders2[i]).remove(mPairIds[i]); - - destroyPair(i); - } - - // Size for the data of a single pair (in bytes) - const size_t totalSizeBytes = mNbAllocatedPairs * mPairDataSize; - - // Release the allocated memory - mPersistentAllocator.release(mBuffer, totalSizeBytes); - } -} - -// Compute the index where we need to insert the new pair -uint64 OverlappingPairs::prepareAddPair(bool isConvexVsConvex) { - - // If we need to allocate more components - if (mNbPairs == mNbAllocatedPairs) { - allocate(mNbAllocatedPairs * 2); + removePair(mConvexPairs.size() - 1, true); } - uint64 index; + // Destroy the concave pairs + while (mConcavePairs.size() > 0) { - // If the pair to add is not convex vs convex or there are no concave pairs yet - if (!isConvexVsConvex) { - - // Add the component at the end of the array - index = mNbPairs; + removePair(mConcavePairs.size() - 1, false); } - // If the pair to add is convex vs convex - else { - - // If there already are convex vs concave pairs - if (mConcavePairsStartIndex != mNbPairs) { - - // Move the first convex vs concave pair to the end of the array - movePairToIndex(mConcavePairsStartIndex, mNbPairs); - } - - index = mConcavePairsStartIndex; - - mConcavePairsStartIndex++; - } - - return index; } // Remove a component at a given index void OverlappingPairs::removePair(uint64 pairId) { - RP3D_PROFILE("OverlappingPairs::removePair()", mProfiler); + assert(mMapConvexPairIdToPairIndex.containsKey(pairId) || mMapConcavePairIdToPairIndex.containsKey(pairId)); - assert(mMapPairIdToPairIndex.containsKey(pairId)); - - uint64 index = mMapPairIdToPairIndex[pairId]; - assert(index < mNbPairs); - - // We want to keep the arrays tightly packed. Therefore, when a pair is removed, - // we replace it with the last element of the array. But we need to make sure that convex - // and concave pairs stay grouped together. - - // Remove all the remaining last frame collision info - for (auto it = mLastFrameCollisionInfos[index].begin(); it != mLastFrameCollisionInfos[index].end(); ++it) { - - // Call the constructor - it->second->~LastFrameCollisionInfo(); - - // Release memory - mPersistentAllocator.release(it->second, sizeof(LastFrameCollisionInfo)); + auto it = mMapConvexPairIdToPairIndex.find(pairId); + if (it != mMapConvexPairIdToPairIndex.end()) { + removePair(it->second, true); } - - // Remove the involved overlapping pair to the two colliders - assert(mColliderComponents.getOverlappingPairs(mColliders1[index]).find(pairId) != mColliderComponents.getOverlappingPairs(mColliders1[index]).end()); - assert(mColliderComponents.getOverlappingPairs(mColliders2[index]).find(pairId) != mColliderComponents.getOverlappingPairs(mColliders2[index]).end()); - mColliderComponents.getOverlappingPairs(mColliders1[index]).remove(pairId); - mColliderComponents.getOverlappingPairs(mColliders2[index]).remove(pairId); - - // Destroy the pair - destroyPair(index); - - // If the pair to remove is convex vs concave - if (index >= mConcavePairsStartIndex) { - - // If the pair is not the last one - if (index != mNbPairs - 1) { - - // We replace it by the last convex vs concave pair - movePairToIndex(mNbPairs - 1, index); - } + else { + removePair(mMapConcavePairIdToPairIndex[pairId], false); } - else { // If the pair to remove is convex vs convex - - // If it not the last convex vs convex pair - if (index != mConcavePairsStartIndex - 1) { - - // We replace it by the last convex vs convex pair - movePairToIndex(mConcavePairsStartIndex - 1, index); - } - - // If there are convex vs concave pairs at the end - if (mConcavePairsStartIndex != mNbPairs) { - - // We replace the last convex vs convex pair by the last convex vs concave pair - movePairToIndex(mNbPairs - 1, mConcavePairsStartIndex - 1); - } - - mConcavePairsStartIndex--; - } - - mNbPairs--; - - assert(mConcavePairsStartIndex <= mNbPairs); - assert(mNbPairs == static_cast(mMapPairIdToPairIndex.size())); } -// Allocate memory for a given number of pairs -void OverlappingPairs::allocate(uint64 nbPairsToAllocate) { +// Remove a component at a given index +void OverlappingPairs::removePair(uint64 pairIndex, bool isConvexVsConvex) { - assert(nbPairsToAllocate > mNbAllocatedPairs); + RP3D_PROFILE("OverlappingPairs::removePair()", mProfiler); - // Size for the data of a single component (in bytes) - const size_t totalSizeBytes = nbPairsToAllocate * mPairDataSize; + if (isConvexVsConvex) { - // Allocate memory - void* newBuffer = mPersistentAllocator.allocate(totalSizeBytes); - assert(newBuffer != nullptr); + const uint32 nbConvexPairs = mConvexPairs.size(); - // New pointers to components data - uint64* newPairIds = static_cast(newBuffer); - int32* newPairBroadPhaseId1 = reinterpret_cast(newPairIds + nbPairsToAllocate); - int32* newPairBroadPhaseId2 = reinterpret_cast(newPairBroadPhaseId1 + nbPairsToAllocate); - Entity* newColliders1 = reinterpret_cast(newPairBroadPhaseId2 + nbPairsToAllocate); - Entity* newColliders2 = reinterpret_cast(newColliders1 + nbPairsToAllocate); - Map* newLastFrameCollisionInfos = reinterpret_cast*>(newColliders2 + nbPairsToAllocate); - bool* newNeedToTestOverlap = reinterpret_cast(newLastFrameCollisionInfos + nbPairsToAllocate); - bool* newIsActive = reinterpret_cast(newNeedToTestOverlap + nbPairsToAllocate); - NarrowPhaseAlgorithmType* newNarrowPhaseAlgorithmType = reinterpret_cast(newIsActive + nbPairsToAllocate); - bool* newIsShape1Convex = reinterpret_cast(newNarrowPhaseAlgorithmType + nbPairsToAllocate); - bool* wereCollidingInPreviousFrame = reinterpret_cast(newIsShape1Convex + nbPairsToAllocate); - bool* areCollidingInCurrentFrame = reinterpret_cast(wereCollidingInPreviousFrame + nbPairsToAllocate); + assert(pairIndex < nbConvexPairs); - // If there was already pairs before - if (mNbPairs > 0) { + // Remove the involved overlapping pair from the two colliders + assert(mColliderComponents.getOverlappingPairs(mConvexPairs[pairIndex].collider1).find(mConvexPairs[pairIndex].pairID) != mColliderComponents.getOverlappingPairs(mConvexPairs[pairIndex].collider1).end()); + assert(mColliderComponents.getOverlappingPairs(mConvexPairs[pairIndex].collider2).find(mConvexPairs[pairIndex].pairID) != mColliderComponents.getOverlappingPairs(mConvexPairs[pairIndex].collider2).end()); + mColliderComponents.getOverlappingPairs(mConvexPairs[pairIndex].collider1).remove(mConvexPairs[pairIndex].pairID); + mColliderComponents.getOverlappingPairs(mConvexPairs[pairIndex].collider2).remove(mConvexPairs[pairIndex].pairID); - // Copy component data from the previous buffer to the new one - memcpy(newPairIds, mPairIds, mNbPairs * sizeof(uint64)); - memcpy(newPairBroadPhaseId1, mPairBroadPhaseId1, mNbPairs * sizeof(int32)); - memcpy(newPairBroadPhaseId2, mPairBroadPhaseId2, mNbPairs * sizeof(int32)); - memcpy(newColliders1, mColliders1, mNbPairs * sizeof(Entity)); - memcpy(newColliders2, mColliders2, mNbPairs * sizeof(Entity)); - memcpy(newLastFrameCollisionInfos, mLastFrameCollisionInfos, mNbPairs * sizeof(Map)); - memcpy(newNeedToTestOverlap, mNeedToTestOverlap, mNbPairs * sizeof(bool)); - memcpy(newIsActive, mIsActive, mNbPairs * sizeof(bool)); - memcpy(newNarrowPhaseAlgorithmType, mNarrowPhaseAlgorithmType, mNbPairs * sizeof(NarrowPhaseAlgorithmType)); - memcpy(newIsShape1Convex, mIsShape1Convex, mNbPairs * sizeof(bool)); - memcpy(wereCollidingInPreviousFrame, mCollidingInPreviousFrame, mNbPairs * sizeof(bool)); - memcpy(areCollidingInCurrentFrame, mCollidingInCurrentFrame, mNbPairs * sizeof(bool)); + assert(mMapConvexPairIdToPairIndex[mConvexPairs[pairIndex].pairID] == pairIndex); + mMapConvexPairIdToPairIndex.remove(mConvexPairs[pairIndex].pairID); - // Deallocate previous memory - mPersistentAllocator.release(mBuffer, mNbAllocatedPairs * mPairDataSize); + // Change the mapping between the pairId and the index in the convex pairs array if we swap the last item with the one to remove + if (mConvexPairs.size() > 1 && pairIndex < (nbConvexPairs - 1)) { + + mMapConvexPairIdToPairIndex[mConvexPairs[nbConvexPairs - 1].pairID] = pairIndex; + } + + // We want to keep the arrays tightly packed. Therefore, when a pair is removed, + // we replace it with the last element of the array. + mConvexPairs.removeAtAndReplaceByLast(pairIndex); } + else { - mBuffer = newBuffer; - mPairIds = newPairIds; - mPairBroadPhaseId1 = newPairBroadPhaseId1; - mPairBroadPhaseId2 = newPairBroadPhaseId2; - mColliders1 = newColliders1; - mColliders2 = newColliders2; - mLastFrameCollisionInfos = newLastFrameCollisionInfos; - mNeedToTestOverlap = newNeedToTestOverlap; - mIsActive = newIsActive; - mNarrowPhaseAlgorithmType = newNarrowPhaseAlgorithmType; - mIsShape1Convex = newIsShape1Convex; - mCollidingInPreviousFrame = wereCollidingInPreviousFrame; - mCollidingInCurrentFrame = areCollidingInCurrentFrame; + const uint32 nbConcavePairs = mConcavePairs.size(); - mNbAllocatedPairs = nbPairsToAllocate; + assert(pairIndex < nbConcavePairs); + + // Remove the involved overlapping pair to the two colliders + assert(mColliderComponents.getOverlappingPairs(mConcavePairs[pairIndex].collider1).find(mConcavePairs[pairIndex].pairID) != mColliderComponents.getOverlappingPairs(mConcavePairs[pairIndex].collider1).end()); + assert(mColliderComponents.getOverlappingPairs(mConcavePairs[pairIndex].collider2).find(mConcavePairs[pairIndex].pairID) != mColliderComponents.getOverlappingPairs(mConcavePairs[pairIndex].collider2).end()); + mColliderComponents.getOverlappingPairs(mConcavePairs[pairIndex].collider1).remove(mConcavePairs[pairIndex].pairID); + mColliderComponents.getOverlappingPairs(mConcavePairs[pairIndex].collider2).remove(mConcavePairs[pairIndex].pairID); + + assert(mMapConcavePairIdToPairIndex[mConcavePairs[pairIndex].pairID] == pairIndex); + mMapConcavePairIdToPairIndex.remove(mConcavePairs[pairIndex].pairID); + + // Destroy all the LastFrameCollisionInfo objects + mConcavePairs[pairIndex].destroyLastFrameCollisionInfos(); + + // Change the mapping between the pairId and the index in the convex pairs array if we swap the last item with the one to remove + if (mConcavePairs.size() > 1 && pairIndex < (nbConcavePairs - 1)) { + + mMapConcavePairIdToPairIndex[mConcavePairs[nbConcavePairs - 1].pairID] = pairIndex; + } + + // We want to keep the arrays tightly packed. Therefore, when a pair is removed, + // we replace it with the last element of the array. + mConcavePairs.removeAtAndReplaceByLast(pairIndex); + } } // Add an overlapping pair -uint64 OverlappingPairs::addPair(Collider* shape1, Collider* shape2) { +uint64 OverlappingPairs::addPair(uint32 collider1Index, uint32 collider2Index, bool isConvexVsConvex) { RP3D_PROFILE("OverlappingPairs::addPair()", mProfiler); - const Entity collider1 = shape1->getEntity(); - const Entity collider2 = shape2->getEntity(); - - const uint collider1Index = mColliderComponents.getEntityIndex(collider1); - const uint collider2Index = mColliderComponents.getEntityIndex(collider2); + assert(mColliderComponents.mBroadPhaseIds[collider1Index] >= 0 && mColliderComponents.mBroadPhaseIds[collider2Index] >= 0); const CollisionShape* collisionShape1 = mColliderComponents.mCollisionShapes[collider1Index]; const CollisionShape* collisionShape2 = mColliderComponents.mCollisionShapes[collider2Index]; - const bool isShape1Convex = collisionShape1->isConvex(); - const bool isShape2Convex = collisionShape2->isConvex(); - const bool isConvexVsConvex = isShape1Convex && isShape2Convex; + const Entity collider1Entity = mColliderComponents.mCollidersEntities[collider1Index]; + const Entity collider2Entity = mColliderComponents.mCollidersEntities[collider2Index]; - // Prepare to add new pair (allocate memory if necessary and compute insertion index) - uint64 index = prepareAddPair(isConvexVsConvex); - - const uint32 broadPhase1Id = static_cast(shape1->getBroadPhaseId()); - const uint32 broadPhase2Id = static_cast(shape2->getBroadPhaseId()); + const uint32 broadPhase1Id = static_cast(mColliderComponents.mBroadPhaseIds[collider1Index]); + const uint32 broadPhase2Id = static_cast(mColliderComponents.mBroadPhaseIds[collider2Index]); // Compute a unique id for the overlapping pair const uint64 pairId = pairNumbers(std::max(broadPhase1Id, broadPhase2Id), std::min(broadPhase1Id, broadPhase2Id)); - assert(!mMapPairIdToPairIndex.containsKey(pairId)); - // Select the narrow phase algorithm to use according to the two collision shapes - NarrowPhaseAlgorithmType algorithmType; if (isConvexVsConvex) { - algorithmType = mCollisionDispatch.selectNarrowPhaseAlgorithm(collisionShape1->getType(), collisionShape2->getType()); + assert(!mMapConvexPairIdToPairIndex.containsKey(pairId)); + NarrowPhaseAlgorithmType algorithmType = mCollisionDispatch.selectNarrowPhaseAlgorithm(collisionShape1->getType(), collisionShape2->getType()); + + // Map the entity with the new component lookup index + mMapConvexPairIdToPairIndex.add(Pair(pairId, mConvexPairs.size())); + + // Create and add a new convex pair + mConvexPairs.emplace(pairId, broadPhase1Id, broadPhase2Id, collider1Entity, collider2Entity, algorithmType); } else { - algorithmType = mCollisionDispatch.selectNarrowPhaseAlgorithm(isShape1Convex ? collisionShape1->getType() : collisionShape2->getType(), + const bool isShape1Convex = collisionShape1->isConvex(); + + assert(!mMapConcavePairIdToPairIndex.containsKey(pairId)); + NarrowPhaseAlgorithmType algorithmType = mCollisionDispatch.selectNarrowPhaseAlgorithm(isShape1Convex ? collisionShape1->getType() : collisionShape2->getType(), CollisionShapeType::CONVEX_POLYHEDRON); + // Map the entity with the new component lookup index + mMapConcavePairIdToPairIndex.add(Pair(pairId, mConcavePairs.size())); + + // Create and add a new concave pair + mConcavePairs.emplace(pairId, broadPhase1Id, broadPhase2Id, collider1Entity, collider2Entity, algorithmType, + isShape1Convex, mPoolAllocator, mHeapAllocator); } - // Insert the new component data - new (mPairIds + index) uint64(pairId); - new (mPairBroadPhaseId1 + index) int32(shape1->getBroadPhaseId()); - new (mPairBroadPhaseId2 + index) int32(shape2->getBroadPhaseId()); - new (mColliders1 + index) Entity(shape1->getEntity()); - new (mColliders2 + index) Entity(shape2->getEntity()); - new (mLastFrameCollisionInfos + index) Map(mPersistentAllocator); - new (mNeedToTestOverlap + index) bool(false); - new (mIsActive + index) bool(true); - new (mNarrowPhaseAlgorithmType + index) NarrowPhaseAlgorithmType(algorithmType); - new (mIsShape1Convex + index) bool(isShape1Convex); - new (mCollidingInPreviousFrame + index) bool(false); - new (mCollidingInCurrentFrame + index) bool(false); - - // Map the entity with the new component lookup index - mMapPairIdToPairIndex.add(Pair(pairId, index)); - // Add the involved overlapping pair to the two colliders assert(mColliderComponents.mOverlappingPairs[collider1Index].find(pairId) == mColliderComponents.mOverlappingPairs[collider1Index].end()); assert(mColliderComponents.mOverlappingPairs[collider2Index].find(pairId) == mColliderComponents.mOverlappingPairs[collider2Index].end()); mColliderComponents.mOverlappingPairs[collider1Index].add(pairId); mColliderComponents.mOverlappingPairs[collider2Index].add(pairId); - mNbPairs++; - - assert(mConcavePairsStartIndex <= mNbPairs); - assert(mNbPairs == static_cast(mMapPairIdToPairIndex.size())); - - updateOverlappingPairIsActive(pairId); - return pairId; } -// Move a pair from a source to a destination index in the pairs array -// The destination location must contain a constructed object -void OverlappingPairs::movePairToIndex(uint64 srcIndex, uint64 destIndex) { - - const uint64 pairId = mPairIds[srcIndex]; - - // Copy the data of the source pair to the destination location - mPairIds[destIndex] = mPairIds[srcIndex]; - mPairBroadPhaseId1[destIndex] = mPairBroadPhaseId1[srcIndex]; - mPairBroadPhaseId2[destIndex] = mPairBroadPhaseId2[srcIndex]; - new (mColliders1 + destIndex) Entity(mColliders1[srcIndex]); - new (mColliders2 + destIndex) Entity(mColliders2[srcIndex]); - new (mLastFrameCollisionInfos + destIndex) Map(mLastFrameCollisionInfos[srcIndex]); - mNeedToTestOverlap[destIndex] = mNeedToTestOverlap[srcIndex]; - mIsActive[destIndex] = mIsActive[srcIndex]; - new (mNarrowPhaseAlgorithmType + destIndex) NarrowPhaseAlgorithmType(mNarrowPhaseAlgorithmType[srcIndex]); - mIsShape1Convex[destIndex] = mIsShape1Convex[srcIndex]; - mCollidingInPreviousFrame[destIndex] = mCollidingInPreviousFrame[srcIndex]; - mCollidingInCurrentFrame[destIndex] = mCollidingInCurrentFrame[srcIndex]; - - // Destroy the source pair - destroyPair(srcIndex); - - assert(!mMapPairIdToPairIndex.containsKey(pairId)); - - // Update the pairId to pair index mapping - mMapPairIdToPairIndex.add(Pair(pairId, destIndex)); - - assert(mMapPairIdToPairIndex[mPairIds[destIndex]] == destIndex); -} - -// Swap two pairs in the array -void OverlappingPairs::swapPairs(uint64 index1, uint64 index2) { - - // Copy pair 1 data - uint64 pairId = mPairIds[index1]; - int32 pairBroadPhaseId1 = mPairBroadPhaseId1[index1]; - int32 pairBroadPhaseId2 = mPairBroadPhaseId2[index1]; - Entity collider1 = mColliders1[index1]; - Entity collider2 = mColliders2[index1]; - Map lastFrameCollisionInfo(mLastFrameCollisionInfos[index1]); - bool needTestOverlap = mNeedToTestOverlap[index1]; - bool isActive = mIsActive[index1]; - NarrowPhaseAlgorithmType narrowPhaseAlgorithmType = mNarrowPhaseAlgorithmType[index1]; - bool isShape1Convex = mIsShape1Convex[index1]; - bool wereCollidingInPreviousFrame = mCollidingInPreviousFrame[index1]; - bool areCollidingInCurrentFrame = mCollidingInCurrentFrame[index1]; - - // Destroy pair 1 - destroyPair(index1); - - movePairToIndex(index2, index1); - - // Reconstruct pair 1 at pair 2 location - mPairIds[index2] = pairId; - mPairBroadPhaseId1[index2] = pairBroadPhaseId1; - mPairBroadPhaseId2[index2] = pairBroadPhaseId2; - new (mColliders1 + index2) Entity(collider1); - new (mColliders2 + index2) Entity(collider2); - new (mLastFrameCollisionInfos + index2) Map(lastFrameCollisionInfo); - mNeedToTestOverlap[index2] = needTestOverlap; - mIsActive[index2] = isActive; - new (mNarrowPhaseAlgorithmType + index2) NarrowPhaseAlgorithmType(narrowPhaseAlgorithmType); - mIsShape1Convex[index2] = isShape1Convex; - mCollidingInPreviousFrame[index2] = wereCollidingInPreviousFrame; - mCollidingInCurrentFrame[index2] = areCollidingInCurrentFrame; - - // Update the pairID to pair index mapping - mMapPairIdToPairIndex.add(Pair(pairId, index2)); - - assert(mMapPairIdToPairIndex[mPairIds[index1]] == index1); - assert(mMapPairIdToPairIndex[mPairIds[index2]] == index2); - assert(mNbPairs == static_cast(mMapPairIdToPairIndex.size())); -} - -// Destroy a pair at a given index -void OverlappingPairs::destroyPair(uint64 index) { - - assert(index < mNbPairs); - - assert(mMapPairIdToPairIndex[mPairIds[index]] == index); - - mMapPairIdToPairIndex.remove(mPairIds[index]); - - mColliders1[index].~Entity(); - mColliders2[index].~Entity(); - mLastFrameCollisionInfos[index].~Map(); - mNarrowPhaseAlgorithmType[index].~NarrowPhaseAlgorithmType(); -} - -// Update whether a given overlapping pair is active or not -void OverlappingPairs::updateOverlappingPairIsActive(uint64 pairId) { - - assert(mMapPairIdToPairIndex.containsKey(pairId)); - - const uint64 pairIndex = mMapPairIdToPairIndex[pairId]; - - const Entity collider1 = mColliders1[pairIndex]; - const Entity collider2 = mColliders2[pairIndex]; - - const Entity body1 = mColliderComponents.getBody(collider1); - const Entity body2 = mColliderComponents.getBody(collider2); - - const bool isBody1Enabled = !mCollisionBodyComponents.getIsEntityDisabled(body1); - const bool isBody2Enabled = !mCollisionBodyComponents.getIsEntityDisabled(body2); - const bool isBody1Static = mRigidBodyComponents.hasComponent(body1) && - mRigidBodyComponents.getBodyType(body1) == BodyType::STATIC; - const bool isBody2Static = mRigidBodyComponents.hasComponent(body2) && - mRigidBodyComponents.getBodyType(body2) == BodyType::STATIC; - - const bool isBody1Active = isBody1Enabled && !isBody1Static; - const bool isBody2Active = isBody2Enabled && !isBody2Static; - - // Check if the bodies are in the set of bodies that cannot collide between each other - bodypair bodiesIndex = OverlappingPairs::computeBodiesIndexPair(body1, body2); - bool bodiesCanCollide = !mNoCollisionPairs.contains(bodiesIndex); - - mIsActive[pairIndex] = bodiesCanCollide && (isBody1Active || isBody2Active); -} - -// Add a new last frame collision info if it does not exist for the given shapes already -LastFrameCollisionInfo* OverlappingPairs::addLastFrameInfoIfNecessary(uint64 pairIndex, uint32 shapeId1, uint32 shapeId2) { - - RP3D_PROFILE("OverlappingPairs::addLastFrameInfoIfNecessary()", mProfiler); - - assert(pairIndex < mNbPairs); - - uint32 maxShapeId = shapeId1; - uint32 minShapeId = shapeId2; - if (shapeId1 < shapeId2) { - maxShapeId = shapeId2; - minShapeId = shapeId1; - } - - // Try to get the corresponding last frame collision info - const uint64 shapesId = pairNumbers(maxShapeId, minShapeId); - - // If there is no collision info for those two shapes already - auto it = mLastFrameCollisionInfos[pairIndex].find(shapesId); - if (it == mLastFrameCollisionInfos[pairIndex].end()) { - - // Create a new collision info - LastFrameCollisionInfo* collisionInfo = new (mPersistentAllocator.allocate(sizeof(LastFrameCollisionInfo))) - LastFrameCollisionInfo(); - - // Add it into the map of collision infos - mLastFrameCollisionInfos[pairIndex].add(Pair(shapesId, collisionInfo)); - - return collisionInfo; - } - else { - - // The existing collision info is not obsolete - it->second->isObsolete = false; - - return it->second; - } -} - // Delete all the obsolete last frame collision info void OverlappingPairs::clearObsoleteLastFrameCollisionInfos() { RP3D_PROFILE("OverlappingPairs::clearObsoleteLastFrameCollisionInfos()", mProfiler); - // For each overlapping pair - for (uint64 i=0; i < mNbPairs; i++) { + // For each concave overlapping pair + const uint64 nbConcavePairs = mConcavePairs.size(); + for (uint64 i=0; i < nbConcavePairs; i++) { - // For each collision info - for (auto it = mLastFrameCollisionInfos[i].begin(); it != mLastFrameCollisionInfos[i].end(); ) { - - // If the collision info is obsolete - if (it->second->isObsolete) { - - // Delete it - it->second->~LastFrameCollisionInfo(); - mPersistentAllocator.release(it->second, sizeof(LastFrameCollisionInfo)); - - it = mLastFrameCollisionInfos[i].remove(it); - } - else { // If the collision info is not obsolete - - // Do not delete it but mark it as obsolete - it->second->isObsolete = true; - - ++it; - } - } + mConcavePairs[i].clearObsoleteLastFrameInfos(); } } // Set the collidingInPreviousFrame value with the collidinginCurrentFrame value for each pair void OverlappingPairs::updateCollidingInPreviousFrame() { - // For each overlapping pair - for (uint64 i=0; i < mNbPairs; i++) { + RP3D_PROFILE("OverlappingPairs::updateCollidingInPreviousFrame()", mProfiler); - mCollidingInPreviousFrame[i] = mCollidingInCurrentFrame[i]; + // For each convex overlapping pair + const uint64 nbConvexPairs = mConvexPairs.size(); + for (uint64 i=0; i < nbConvexPairs; i++) { + + mConvexPairs[i].collidingInPreviousFrame = mConvexPairs[i].collidingInCurrentFrame; + } + + // For each concave overlapping pair + const uint64 nbConcavePairs = mConcavePairs.size(); + for (uint64 i=0; i < nbConcavePairs; i++) { + + mConcavePairs[i].collidingInPreviousFrame = mConcavePairs[i].collidingInCurrentFrame; } } diff --git a/src/engine/PhysicsCommon.cpp b/src/engine/PhysicsCommon.cpp index 3147c702..7d774fa2 100644 --- a/src/engine/PhysicsCommon.cpp +++ b/src/engine/PhysicsCommon.cpp @@ -42,8 +42,11 @@ PhysicsCommon::PhysicsCommon(MemoryAllocator* baseMemoryAllocator) mConvexMeshShapes(mMemoryManager.getHeapAllocator()), mConcaveMeshShapes(mMemoryManager.getHeapAllocator()), mHeightFieldShapes(mMemoryManager.getHeapAllocator()), mPolyhedronMeshes(mMemoryManager.getHeapAllocator()), mTriangleMeshes(mMemoryManager.getHeapAllocator()), - mProfilers(mMemoryManager.getHeapAllocator()), mDefaultLoggers(mMemoryManager.getHeapAllocator()) { + mProfilers(mMemoryManager.getHeapAllocator()), mDefaultLoggers(mMemoryManager.getHeapAllocator()), + mBoxShapeHalfEdgeStructure(mMemoryManager.getHeapAllocator(), 6, 8, 24), + mTriangleShapeHalfEdgeStructure(mMemoryManager.getHeapAllocator(), 2, 3, 6) { + init(); } // Destructor @@ -53,58 +56,139 @@ PhysicsCommon::~PhysicsCommon() { release(); } +/// Initialization +void PhysicsCommon::init() { + + // Initialize the static half-edge structure for the BoxShape collision shape + initBoxShapeHalfEdgeStructure(); + + // Initialize the static half-edge structure for the TriangleShape collision shape + initTriangleShapeHalfEdgeStructure(); +} + +// Initialize the static half-edge structure of a BoxShape +void PhysicsCommon::initBoxShapeHalfEdgeStructure() { + + // Vertices + mBoxShapeHalfEdgeStructure.addVertex(0); + mBoxShapeHalfEdgeStructure.addVertex(1); + mBoxShapeHalfEdgeStructure.addVertex(2); + mBoxShapeHalfEdgeStructure.addVertex(3); + mBoxShapeHalfEdgeStructure.addVertex(4); + mBoxShapeHalfEdgeStructure.addVertex(5); + mBoxShapeHalfEdgeStructure.addVertex(6); + mBoxShapeHalfEdgeStructure.addVertex(7); + + MemoryAllocator& allocator = mMemoryManager.getHeapAllocator(); + + // Faces + Array face0(allocator, 4); + face0.add(0); face0.add(1); face0.add(2); face0.add(3); + Array face1(allocator, 4); + face1.add(1); face1.add(5); face1.add(6); face1.add(2); + Array face2(allocator, 4); + face2.add(4); face2.add(7); face2.add(6); face2.add(5); + Array face3(allocator, 4); + face3.add(4); face3.add(0); face3.add(3); face3.add(7); + Array face4(allocator, 4); + face4.add(4); face4.add(5); face4.add(1); face4.add(0); + Array face5(allocator, 4); + face5.add(2); face5.add(6); face5.add(7); face5.add(3); + + mBoxShapeHalfEdgeStructure.addFace(face0); + mBoxShapeHalfEdgeStructure.addFace(face1); + mBoxShapeHalfEdgeStructure.addFace(face2); + mBoxShapeHalfEdgeStructure.addFace(face3); + mBoxShapeHalfEdgeStructure.addFace(face4); + mBoxShapeHalfEdgeStructure.addFace(face5); + + mBoxShapeHalfEdgeStructure.init(); +} + +// Initialize the static half-edge structure of a TriangleShape +void PhysicsCommon::initTriangleShapeHalfEdgeStructure() { + + // Vertices + mTriangleShapeHalfEdgeStructure.addVertex(0); + mTriangleShapeHalfEdgeStructure.addVertex(1); + mTriangleShapeHalfEdgeStructure.addVertex(2); + + MemoryAllocator& allocator = mMemoryManager.getHeapAllocator(); + + // Faces + Array face0(allocator, 3); + face0.add(0); face0.add(1); face0.add(2); + Array face1(allocator, 3); + face1.add(0); face1.add(2); face1.add(1); + + mTriangleShapeHalfEdgeStructure.addFace(face0); + mTriangleShapeHalfEdgeStructure.addFace(face1); + + mTriangleShapeHalfEdgeStructure.init(); +} + // Destroy and release everything that has been allocated void PhysicsCommon::release() { // Destroy the physics worlds for (auto it = mPhysicsWorlds.begin(); it != mPhysicsWorlds.end(); ++it) { - destroyPhysicsWorld(*it); + deletePhysicsWorld(*it); } + mPhysicsWorlds.clear(); // Destroy the sphere shapes for (auto it = mSphereShapes.begin(); it != mSphereShapes.end(); ++it) { - destroySphereShape(*it); + deleteSphereShape(*it); } + mSphereShapes.clear(); // Destroy the box shapes for (auto it = mBoxShapes.begin(); it != mBoxShapes.end(); ++it) { - destroyBoxShape(*it); + deleteBoxShape(*it); } + mBoxShapes.clear(); // Destroy the capsule shapes for (auto it = mCapsuleShapes.begin(); it != mCapsuleShapes.end(); ++it) { - destroyCapsuleShape(*it); + deleteCapsuleShape(*it); } + mCapsuleShapes.clear(); // Destroy the convex mesh shapes for (auto it = mConvexMeshShapes.begin(); it != mConvexMeshShapes.end(); ++it) { - destroyConvexMeshShape(*it); + deleteConvexMeshShape(*it); } + mConvexMeshShapes.clear(); // Destroy the heigh-field shapes for (auto it = mHeightFieldShapes.begin(); it != mHeightFieldShapes.end(); ++it) { - destroyHeightFieldShape(*it); + deleteHeightFieldShape(*it); } + mHeightFieldShapes.clear(); // Destroy the concave mesh shapes for (auto it = mConcaveMeshShapes.begin(); it != mConcaveMeshShapes.end(); ++it) { - destroyConcaveMeshShape(*it); + deleteConcaveMeshShape(*it); } + mConcaveMeshShapes.clear(); // Destroy the polyhedron mesh for (auto it = mPolyhedronMeshes.begin(); it != mPolyhedronMeshes.end(); ++it) { - destroyPolyhedronMesh(*it); + deletePolyhedronMesh(*it); } + mPolyhedronMeshes.clear(); // Destroy the triangle mesh for (auto it = mTriangleMeshes.begin(); it != mTriangleMeshes.end(); ++it) { - destroyTriangleMesh(*it); + deleteTriangleMesh(*it); } + mTriangleMeshes.clear(); // Destroy the default loggers for (auto it = mDefaultLoggers.begin(); it != mDefaultLoggers.end(); ++it) { - destroyDefaultLogger(*it); + deleteDefaultLogger(*it); } + mDefaultLoggers.clear(); // If profiling is enabled #ifdef IS_RP3D_PROFILING_ENABLED @@ -112,8 +196,9 @@ void PhysicsCommon::release() { // Destroy the profilers for (auto it = mProfilers.begin(); it != mProfilers.end(); ++it) { - destroyProfiler(*it); + deleteProfiler(*it); } + mProfilers.clear(); #endif @@ -138,7 +223,7 @@ PhysicsWorld* PhysicsCommon::createPhysicsWorld(const PhysicsWorld::WorldSetting #endif - PhysicsWorld* world = new(mMemoryManager.allocate(MemoryManager::AllocationType::Heap, sizeof(PhysicsWorld))) PhysicsWorld(mMemoryManager, worldSettings, profiler); + PhysicsWorld* world = new(mMemoryManager.allocate(MemoryManager::AllocationType::Heap, sizeof(PhysicsWorld))) PhysicsWorld(mMemoryManager, *this, worldSettings, profiler); mPhysicsWorlds.add(world); @@ -151,13 +236,22 @@ PhysicsWorld* PhysicsCommon::createPhysicsWorld(const PhysicsWorld::WorldSetting */ void PhysicsCommon::destroyPhysicsWorld(PhysicsWorld* world) { + deletePhysicsWorld(world); + + mPhysicsWorlds.remove(world); +} + +// Delete an instance of PhysicsWorld +/** + * @param world A pointer to the physics world to destroy + */ +void PhysicsCommon::deletePhysicsWorld(PhysicsWorld* world) { + // Call the destructor of the world world->~PhysicsWorld(); // Release allocated memory mMemoryManager.release(MemoryManager::AllocationType::Heap, world, sizeof(PhysicsWorld)); - - mPhysicsWorlds.remove(world); } // Create and return a sphere collision shape @@ -185,6 +279,17 @@ SphereShape* PhysicsCommon::createSphereShape(const decimal radius) { */ void PhysicsCommon::destroySphereShape(SphereShape* sphereShape) { + deleteSphereShape(sphereShape); + + mSphereShapes.remove(sphereShape); +} + +// Delete a sphere collision shape +/** + * @param sphereShape A pointer to the sphere collision shape to destroy + */ +void PhysicsCommon::deleteSphereShape(SphereShape* sphereShape) { + // If the shape is still part of some colliders if (sphereShape->mColliders.size() > 0) { @@ -197,8 +302,6 @@ void PhysicsCommon::destroySphereShape(SphereShape* sphereShape) { // Release allocated memory mMemoryManager.release(MemoryManager::AllocationType::Pool, sphereShape, sizeof(SphereShape)); - - mSphereShapes.remove(sphereShape); } // Create and return a box collision shape @@ -213,7 +316,7 @@ BoxShape* PhysicsCommon::createBoxShape(const Vector3& halfExtents) { RP3D_LOG("PhysicsCommon", Logger::Level::Error, Logger::Category::PhysicCommon, "Error when creating a BoxShape: the half extents must be positive values", __FILE__, __LINE__); } - BoxShape* shape = new (mMemoryManager.allocate(MemoryManager::AllocationType::Pool, sizeof(BoxShape))) BoxShape(halfExtents, mMemoryManager.getHeapAllocator()); + BoxShape* shape = new (mMemoryManager.allocate(MemoryManager::AllocationType::Pool, sizeof(BoxShape))) BoxShape(halfExtents, mMemoryManager.getHeapAllocator(), *this); mBoxShapes.add(shape); @@ -226,6 +329,17 @@ BoxShape* PhysicsCommon::createBoxShape(const Vector3& halfExtents) { */ void PhysicsCommon::destroyBoxShape(BoxShape* boxShape) { + deleteBoxShape(boxShape); + + mBoxShapes.remove(boxShape); +} + +// Delete a box collision shape +/** + * @param boxShape A pointer to the box shape to destroy + */ +void PhysicsCommon::deleteBoxShape(BoxShape* boxShape) { + // If the shape is still part of some colliders if (boxShape->mColliders.size() > 0) { @@ -238,8 +352,6 @@ void PhysicsCommon::destroyBoxShape(BoxShape* boxShape) { // Release allocated memory mMemoryManager.release(MemoryManager::AllocationType::Pool, boxShape, sizeof(BoxShape)); - - mBoxShapes.remove(boxShape); } // Create and return a capsule shape @@ -275,6 +387,17 @@ CapsuleShape* PhysicsCommon::createCapsuleShape(decimal radius, decimal height) */ void PhysicsCommon::destroyCapsuleShape(CapsuleShape* capsuleShape) { + deleteCapsuleShape(capsuleShape); + + mCapsuleShapes.remove(capsuleShape); +} + +// Delete a capsule collision shape +/** + * @param capsuleShape A pointer to the capsule shape to destroy + */ +void PhysicsCommon::deleteCapsuleShape(CapsuleShape* capsuleShape) { + // If the shape is still part of some colliders if (capsuleShape->mColliders.size() > 0) { @@ -287,8 +410,6 @@ void PhysicsCommon::destroyCapsuleShape(CapsuleShape* capsuleShape) { // Release allocated memory mMemoryManager.release(MemoryManager::AllocationType::Pool, capsuleShape, sizeof(CapsuleShape)); - - mCapsuleShapes.remove(capsuleShape); } // Create and return a convex mesh shape @@ -312,6 +433,17 @@ ConvexMeshShape* PhysicsCommon::createConvexMeshShape(PolyhedronMesh* polyhedron */ void PhysicsCommon::destroyConvexMeshShape(ConvexMeshShape* convexMeshShape) { + deleteConvexMeshShape(convexMeshShape); + + mConvexMeshShapes.remove(convexMeshShape); +} + +// Delete a convex mesh shape +/** + * @param convexMeshShape A pointer to the convex mesh shape to destroy + */ +void PhysicsCommon::deleteConvexMeshShape(ConvexMeshShape* convexMeshShape) { + // If the shape is still part of some colliders if (convexMeshShape->mColliders.size() > 0) { @@ -324,8 +456,6 @@ void PhysicsCommon::destroyConvexMeshShape(ConvexMeshShape* convexMeshShape) { // Release allocated memory mMemoryManager.release(MemoryManager::AllocationType::Pool, convexMeshShape, sizeof(ConvexMeshShape)); - - mConvexMeshShapes.remove(convexMeshShape); } // Create and return a height-field shape @@ -345,7 +475,7 @@ HeightFieldShape* PhysicsCommon::createHeightFieldShape(int nbGridColumns, int n int upAxis, decimal integerHeightScale, const Vector3& scaling) { HeightFieldShape* shape = new (mMemoryManager.allocate(MemoryManager::AllocationType::Pool, sizeof(HeightFieldShape))) HeightFieldShape(nbGridColumns, nbGridRows, minHeight, maxHeight, - heightFieldData, dataType, mMemoryManager.getHeapAllocator(), upAxis, integerHeightScale, scaling); + heightFieldData, dataType, mMemoryManager.getHeapAllocator(), mTriangleShapeHalfEdgeStructure, upAxis, integerHeightScale, scaling); mHeightFieldShapes.add(shape); @@ -358,6 +488,17 @@ HeightFieldShape* PhysicsCommon::createHeightFieldShape(int nbGridColumns, int n */ void PhysicsCommon::destroyHeightFieldShape(HeightFieldShape* heightFieldShape) { + deleteHeightFieldShape(heightFieldShape); + + mHeightFieldShapes.remove(heightFieldShape); +} + +// Delete a height-field shape +/** + * @param heightFieldShape A pointer to the height field shape to destroy + */ +void PhysicsCommon::deleteHeightFieldShape(HeightFieldShape* heightFieldShape) { + // If the shape is still part of some colliders if (heightFieldShape->mColliders.size() > 0) { @@ -370,8 +511,6 @@ void PhysicsCommon::destroyHeightFieldShape(HeightFieldShape* heightFieldShape) // Release allocated memory mMemoryManager.release(MemoryManager::AllocationType::Pool, heightFieldShape, sizeof(HeightFieldShape)); - - mHeightFieldShapes.remove(heightFieldShape); } // Create and return a concave mesh shape @@ -382,7 +521,8 @@ void PhysicsCommon::destroyHeightFieldShape(HeightFieldShape* heightFieldShape) */ ConcaveMeshShape* PhysicsCommon::createConcaveMeshShape(TriangleMesh* triangleMesh, const Vector3& scaling) { - ConcaveMeshShape* shape = new (mMemoryManager.allocate(MemoryManager::AllocationType::Pool, sizeof(ConcaveMeshShape))) ConcaveMeshShape(triangleMesh, mMemoryManager.getHeapAllocator(), scaling); + ConcaveMeshShape* shape = new (mMemoryManager.allocate(MemoryManager::AllocationType::Pool, sizeof(ConcaveMeshShape))) ConcaveMeshShape(triangleMesh, + mMemoryManager.getHeapAllocator(), mTriangleShapeHalfEdgeStructure, scaling); mConcaveMeshShapes.add(shape); @@ -395,6 +535,17 @@ ConcaveMeshShape* PhysicsCommon::createConcaveMeshShape(TriangleMesh* triangleMe */ void PhysicsCommon::destroyConcaveMeshShape(ConcaveMeshShape* concaveMeshShape) { + deleteConcaveMeshShape(concaveMeshShape); + + mConcaveMeshShapes.remove(concaveMeshShape); +} + +// Delete a concave mesh shape +/** + * @param concaveMeshShape A pointer to the concave mesh shape to destroy + */ +void PhysicsCommon::deleteConcaveMeshShape(ConcaveMeshShape* concaveMeshShape) { + // If the shape is still part of some colliders if (concaveMeshShape->mColliders.size() > 0) { @@ -407,20 +558,23 @@ void PhysicsCommon::destroyConcaveMeshShape(ConcaveMeshShape* concaveMeshShape) // Release allocated memory mMemoryManager.release(MemoryManager::AllocationType::Pool, concaveMeshShape, sizeof(ConcaveMeshShape)); - - mConcaveMeshShapes.remove(concaveMeshShape); } // Create a polyhedron mesh /** * @param polygonVertexArray A pointer to the polygon vertex array to use to create the polyhedron mesh - * @return A pointer to the created polyhedron mesh + * @return A pointer to the created polyhedron mesh or nullptr if the mesh is not valid */ PolyhedronMesh* PhysicsCommon::createPolyhedronMesh(PolygonVertexArray* polygonVertexArray) { - PolyhedronMesh* mesh = new (mMemoryManager.allocate(MemoryManager::AllocationType::Pool, sizeof(PolyhedronMesh))) PolyhedronMesh(polygonVertexArray, mMemoryManager.getHeapAllocator()); + // Create the polyhedron mesh + PolyhedronMesh* mesh = PolyhedronMesh::create(polygonVertexArray, mMemoryManager.getPoolAllocator(), mMemoryManager.getHeapAllocator()); - mPolyhedronMeshes.add(mesh); + // If the mesh is valid + if (mesh != nullptr) { + + mPolyhedronMeshes.add(mesh); + } return mesh; } @@ -431,13 +585,22 @@ PolyhedronMesh* PhysicsCommon::createPolyhedronMesh(PolygonVertexArray* polygonV */ void PhysicsCommon::destroyPolyhedronMesh(PolyhedronMesh* polyhedronMesh) { + deletePolyhedronMesh(polyhedronMesh); + + mPolyhedronMeshes.remove(polyhedronMesh); +} + +// Delete a polyhedron mesh +/** + * @param polyhedronMesh A pointer to the polyhedron mesh to destroy + */ +void PhysicsCommon::deletePolyhedronMesh(PolyhedronMesh* polyhedronMesh) { + // Call the destructor of the shape polyhedronMesh->~PolyhedronMesh(); // Release allocated memory mMemoryManager.release(MemoryManager::AllocationType::Pool, polyhedronMesh, sizeof(PolyhedronMesh)); - - mPolyhedronMeshes.remove(polyhedronMesh); } // Create a triangle mesh @@ -459,13 +622,22 @@ TriangleMesh* PhysicsCommon::createTriangleMesh() { */ void PhysicsCommon::destroyTriangleMesh(TriangleMesh* triangleMesh) { + deleteTriangleMesh(triangleMesh); + + mTriangleMeshes.remove(triangleMesh); +} + +// Delete a triangle mesh +/** + * @param A pointer to the triangle mesh to destroy + */ +void PhysicsCommon::deleteTriangleMesh(TriangleMesh* triangleMesh) { + // Call the destructor of the shape triangleMesh->~TriangleMesh(); // Release allocated memory mMemoryManager.release(MemoryManager::AllocationType::Pool, triangleMesh, sizeof(TriangleMesh)); - - mTriangleMeshes.remove(triangleMesh); } // Create and return a new logger @@ -487,13 +659,22 @@ DefaultLogger* PhysicsCommon::createDefaultLogger() { */ void PhysicsCommon::destroyDefaultLogger(DefaultLogger* logger) { + deleteDefaultLogger(logger); + + mDefaultLoggers.remove(logger); +} + +// Delete a logger +/** + * @param A pointer to the default logger to destroy + */ +void PhysicsCommon::deleteDefaultLogger(DefaultLogger* logger) { + // Call the destructor of the logger logger->~DefaultLogger(); // Release allocated memory mMemoryManager.release(MemoryManager::AllocationType::Pool, logger, sizeof(DefaultLogger)); - - mDefaultLoggers.remove(logger); } // If profiling is enabled @@ -514,13 +695,18 @@ Profiler* PhysicsCommon::createProfiler() { // Destroy a profiler void PhysicsCommon::destroyProfiler(Profiler* profiler) { + deleteProfiler(profiler); + + mProfilers.remove(profiler); +} + +// Delete a profiler +void PhysicsCommon::deleteProfiler(Profiler* profiler) { + // Call the destructor of the profiler profiler->~Profiler(); // Release allocated memory mMemoryManager.release(MemoryManager::AllocationType::Pool, profiler, sizeof(Profiler)); - - mProfilers.remove(profiler); } - #endif diff --git a/src/engine/PhysicsWorld.cpp b/src/engine/PhysicsWorld.cpp index 19e55eab..a6fe538d 100644 --- a/src/engine/PhysicsWorld.cpp +++ b/src/engine/PhysicsWorld.cpp @@ -50,16 +50,16 @@ uint PhysicsWorld::mNbWorlds = 0; * @param worldSettings The settings of the world * @param profiler Pointer to the profiler */ -PhysicsWorld::PhysicsWorld(MemoryManager& memoryManager, const WorldSettings& worldSettings, Profiler* profiler) +PhysicsWorld::PhysicsWorld(MemoryManager& memoryManager, PhysicsCommon& physicsCommon, const WorldSettings& worldSettings, Profiler* profiler) : mMemoryManager(memoryManager), mConfig(worldSettings), mEntityManager(mMemoryManager.getHeapAllocator()), mDebugRenderer(mMemoryManager.getHeapAllocator()), mCollisionBodyComponents(mMemoryManager.getHeapAllocator()), mRigidBodyComponents(mMemoryManager.getHeapAllocator()), mTransformComponents(mMemoryManager.getHeapAllocator()), mCollidersComponents(mMemoryManager.getHeapAllocator()), mJointsComponents(mMemoryManager.getHeapAllocator()), mBallAndSocketJointsComponents(mMemoryManager.getHeapAllocator()), mFixedJointsComponents(mMemoryManager.getHeapAllocator()), mHingeJointsComponents(mMemoryManager.getHeapAllocator()), mSliderJointsComponents(mMemoryManager.getHeapAllocator()), mCollisionDetection(this, mCollidersComponents, mTransformComponents, mCollisionBodyComponents, mRigidBodyComponents, - mMemoryManager), + mMemoryManager, physicsCommon.mTriangleShapeHalfEdgeStructure), mCollisionBodies(mMemoryManager.getHeapAllocator()), mEventListener(nullptr), - mName(worldSettings.worldName), mIslands(mMemoryManager.getSingleFrameAllocator()), + mName(worldSettings.worldName), mIslands(mMemoryManager.getSingleFrameAllocator()), mProcessContactPairsOrderIslands(mMemoryManager.getSingleFrameAllocator()), mContactSolverSystem(mMemoryManager, *this, mIslands, mCollisionBodyComponents, mRigidBodyComponents, mCollidersComponents, mConfig.restitutionVelocityThreshold), mConstraintSolverSystem(*this, mIslands, mRigidBodyComponents, mTransformComponents, mJointsComponents, @@ -217,7 +217,7 @@ void PhysicsWorld::destroyCollisionBody(CollisionBody* collisionBody) { // Call the destructor of the collision body collisionBody->~CollisionBody(); - // Remove the collision body from the list of bodies + // Remove the collision body from the array of bodies mCollisionBodies.remove(collisionBody); // Free the object from the memory allocator @@ -233,39 +233,37 @@ void PhysicsWorld::setBodyDisabled(Entity bodyEntity, bool isDisabled) { mCollisionBodyComponents.setIsEntityDisabled(bodyEntity, isDisabled); mTransformComponents.setIsEntityDisabled(bodyEntity, isDisabled); - if (mRigidBodyComponents.hasComponent(bodyEntity)) { - mRigidBodyComponents.setIsEntityDisabled(bodyEntity, isDisabled); - } + assert(mRigidBodyComponents.hasComponent(bodyEntity)); + + mRigidBodyComponents.setIsEntityDisabled(bodyEntity, isDisabled); // For each collider of the body - const List& collidersEntities = mCollisionBodyComponents.getColliders(bodyEntity); - for (uint i=0; i < collidersEntities.size(); i++) { + const Array& collidersEntities = mCollisionBodyComponents.getColliders(bodyEntity); + const uint32 nbColliderEntities = collidersEntities.size(); + for (uint32 i=0; i < nbColliderEntities; i++) { mCollidersComponents.setIsEntityDisabled(collidersEntities[i], isDisabled); } // Disable the joints of the body if necessary - if (mRigidBodyComponents.hasComponent(bodyEntity)) { + // For each joint of the body + const Array& joints = mRigidBodyComponents.getJoints(bodyEntity); + const uint32 nbJoints = joints.size(); + for(uint32 i=0; i < nbJoints; i++) { - // For each joint of the body - const List& joints = mRigidBodyComponents.getJoints(bodyEntity); - for(uint32 i=0; i < joints.size(); i++) { + const Entity body1Entity = mJointsComponents.getBody1Entity(joints[i]); + const Entity body2Entity = mJointsComponents.getBody2Entity(joints[i]); - const Entity body1Entity = mJointsComponents.getBody1Entity(joints[i]); - const Entity body2Entity = mJointsComponents.getBody2Entity(joints[i]); + // If both bodies of the joint are disabled + if (mRigidBodyComponents.getIsEntityDisabled(body1Entity) && mRigidBodyComponents.getIsEntityDisabled(body2Entity)) { - // If both bodies of the joint are disabled - if (mRigidBodyComponents.getIsEntityDisabled(body1Entity) && - mRigidBodyComponents.getIsEntityDisabled(body2Entity)) { + // We disable the joint + setJointDisabled(joints[i], true); + } + else { - // We disable the joint - setJointDisabled(joints[i], true); - } - else { - - // Enable the joint - setJointDisabled(joints[i], false); - } + // Enable the joint + setJointDisabled(joints[i], false); } } } @@ -342,9 +340,15 @@ void PhysicsWorld::update(decimal timeStep) { // Create the islands createIslands(); + // Create the actual narrow-phase contacts + mCollisionDetection.createContacts(); + // Report the contacts to the user mCollisionDetection.reportContactsAndTriggers(); + // Recompute the inverse inertia tensors of rigid bodies + updateBodiesInverseWorldInertiaTensors(); + // Disable the joints for pair of sleeping bodies disableJointsOfSleepingBodies(); @@ -374,6 +378,8 @@ void PhysicsWorld::update(decimal timeStep) { // Reset the islands mIslands.clear(); + mProcessContactPairsOrderIslands.clear(true); + // Generate debug rendering primitives (if enabled) if (mIsDebugRenderingEnabled) { mDebugRenderer.computeDebugRenderingPrimitives(*this); @@ -383,6 +389,15 @@ void PhysicsWorld::update(decimal timeStep) { mMemoryManager.resetFrameAllocator(); } +// Update the world inverse inertia tensors of rigid bodies +void PhysicsWorld::updateBodiesInverseWorldInertiaTensors() { + + for (uint i=0; i < mRigidBodyComponents.getNbEnabledComponents(); i++) { + const Matrix3x3 orientation = mTransformComponents.getTransform(mRigidBodyComponents.mBodiesEntities[i]).getOrientation().getMatrix(); + + RigidBody::computeWorldInertiaTensorInverse(orientation, mRigidBodyComponents.mInverseInertiaTensorsLocal[i], mRigidBodyComponents.mInverseInertiaTensorsWorld[i]); + } +} // Solve the contacts and constraints void PhysicsWorld::solveContactsAndConstraints(decimal timeStep) { @@ -505,8 +520,9 @@ void PhysicsWorld::destroyRigidBody(RigidBody* rigidBody) { rigidBody->removeAllColliders(); // Destroy all the joints in which the rigid body to be destroyed is involved - const List& joints = mRigidBodyComponents.getJoints(rigidBody->getEntity()); - for (uint32 i=0; i < joints.size(); i++) { + const Array& joints = mRigidBodyComponents.getJoints(rigidBody->getEntity()); + const uint32 nbJoints = joints.size(); + for (uint32 i=0; i < nbJoints; i++) { destroyJoint(mJointsComponents.getJoint(joints[i])); } @@ -519,7 +535,7 @@ void PhysicsWorld::destroyRigidBody(RigidBody* rigidBody) { // Call the destructor of the rigid body rigidBody->~RigidBody(); - // Remove the rigid body from the list of rigid bodies + // Remove the rigid body from the array of rigid bodies mRigidBodies.remove(rigidBody); // Free the object from the memory allocator @@ -643,7 +659,7 @@ Joint* PhysicsWorld::createJoint(const JointInfo& jointInfo) { RP3D_LOG(mConfig.worldName, Logger::Level::Information, Logger::Category::Joint, "Joint " + std::to_string(newJoint->getEntity().id) + ": " + newJoint->to_string(), __FILE__, __LINE__); - // Add the joint into the joint list of the bodies involved in the joint + // Add the joint into the joint array of the bodies involved in the joint addJointToBodies(jointInfo.body1->getEntity(), jointInfo.body2->getEntity(), entity); // Return the pointer to the created joint @@ -675,7 +691,7 @@ void PhysicsWorld::destroyJoint(Joint* joint) { body1->setIsSleeping(false); body2->setIsSleeping(false); - // Remove the joint from the joint list of the bodies involved in the joint + // Remove the joint from the joint array of the bodies involved in the joint mRigidBodyComponents.removeJointFromBody(body1->getEntity(), joint->getEntity()); mRigidBodyComponents.removeJointFromBody(body2->getEntity(), joint->getEntity()); @@ -719,7 +735,7 @@ void PhysicsWorld::setNbIterationsVelocitySolver(uint nbIterations) { "Physics World: Set nb iterations velocity solver to " + std::to_string(nbIterations), __FILE__, __LINE__); } -// Add the joint to the list of joints of the two bodies involved in the joint +// Add the joint to the array of joints of the two bodies involved in the joint void PhysicsWorld::addJointToBodies(Entity body1, Entity body2, Entity joint) { mRigidBodyComponents.addJointToBody(body1, joint); @@ -747,17 +763,26 @@ void PhysicsWorld::createIslands() { RP3D_PROFILE("PhysicsWorld::createIslands()", mProfiler); - // Reset all the isAlreadyInIsland variables of bodies and joints - for (uint b=0; b < mRigidBodyComponents.getNbComponents(); b++) { + assert(mProcessContactPairsOrderIslands.size() == 0); + // Reset all the isAlreadyInIsland variables of bodies and joints + const uint32 nbRigidBodyComponents = mRigidBodyComponents.getNbComponents(); + for (uint b=0; b < nbRigidBodyComponents; b++) { mRigidBodyComponents.mIsAlreadyInIsland[b] = false; } - for (uint32 i=0; i < mJointsComponents.getNbComponents(); i++) { + const uint32 nbJointsComponents = mJointsComponents.getNbComponents(); + for (uint32 i=0; i < nbJointsComponents; i++) { mJointsComponents.mIsAlreadyInIsland[i] = false; } + // Reserve memory for the islands + mIslands.reserveMemory(); + // Create a stack for the bodies to visit during the Depth First Search - Stack bodyEntityIndicesToVisit(mMemoryManager.getSingleFrameAllocator()); + Stack bodyEntitiesToVisit(mMemoryManager.getSingleFrameAllocator(), mIslands.getNbMaxBodiesInIslandPreviousFrame()); + + // Array of static bodies added to the current island (used to reset the isAlreadyInIsland variable of static bodies) + Array staticBodiesAddedToIsland(mMemoryManager.getSingleFrameAllocator(), 16); uint nbTotalManifolds = 0; @@ -771,107 +796,127 @@ void PhysicsWorld::createIslands() { if (mRigidBodyComponents.mBodyTypes[b] == BodyType::STATIC) continue; // Reset the stack of bodies to visit - bodyEntityIndicesToVisit.clear(); + bodyEntitiesToVisit.clear(); // Add the body into the stack of bodies to visit mRigidBodyComponents.mIsAlreadyInIsland[b] = true; - bodyEntityIndicesToVisit.push(mRigidBodyComponents.mBodiesEntities[b]); + bodyEntitiesToVisit.push(mRigidBodyComponents.mBodiesEntities[b]); // Create the new island uint32 islandIndex = mIslands.addIsland(nbTotalManifolds); // While there are still some bodies to visit in the stack - while (bodyEntityIndicesToVisit.size() > 0) { + while (bodyEntitiesToVisit.size() > 0) { - // Get the next body to visit from the stack - const Entity bodyToVisitEntity = bodyEntityIndicesToVisit.pop(); + // Get the body entity + const Entity bodyToVisitEntity = bodyEntitiesToVisit.pop(); // Add the body into the island - mIslands.bodyEntities[islandIndex].add(bodyToVisitEntity); + mIslands.addBodyToIsland(bodyToVisitEntity); - RigidBody* rigidBodyToVisit = static_cast(mCollisionBodyComponents.getBody(bodyToVisitEntity)); + RigidBody* rigidBodyToVisit = mRigidBodyComponents.getRigidBody(bodyToVisitEntity); - // Awake the body if it is sleeping + // Awake the body if it is sleeping (note that this called might change the body index in the mRigidBodyComponents array) rigidBodyToVisit->setIsSleeping(false); - // If the current body is static, we do not want to perform the DFS search across that body - if (rigidBodyToVisit->getType() == BodyType::STATIC) continue; + // Compute the body index in the array (Note that it could have changed because of the previous call to rigidBodyToVisit->setIsSleeping(false)) + const uint32 bodyToVisitIndex = mRigidBodyComponents.getEntityIndex(bodyToVisitEntity); + + // If the currenbodyEntityIndicesToVisitt body is static, we do not want to perform the DFS search across that body + if (mRigidBodyComponents.mBodyTypes[bodyToVisitIndex] == BodyType::STATIC) { + + staticBodiesAddedToIsland.add(bodyToVisitEntity); + + // Go to the next body + continue; + } // If the body is involved in contacts with other bodies - auto itBodyContactPairs = mCollisionDetection.mMapBodyToContactPairs.find(bodyToVisitEntity); - if (itBodyContactPairs != mCollisionDetection.mMapBodyToContactPairs.end()) { + // For each contact pair in which the current body is involded + const uint32 nbBodyContactPairs = mRigidBodyComponents.mContactPairs[bodyToVisitIndex].size(); + for (uint32 p=0; p < nbBodyContactPairs; p++) { - // For each contact pair in which the current body is involded - List& contactPairs = itBodyContactPairs->second; - for (uint p=0; p < contactPairs.size(); p++) { + const uint32 contactPairIndex = mRigidBodyComponents.mContactPairs[bodyToVisitIndex][p]; + ContactPair& pair = (*mCollisionDetection.mCurrentContactPairs)[contactPairIndex]; - ContactPair& pair = (*mCollisionDetection.mCurrentContactPairs)[contactPairs[p]]; + // Check if the current contact pair has already been added into an island + if (pair.isAlreadyInIsland) continue; - // Check if the current contact pair has already been added into an island - if (pair.isAlreadyInIsland) continue; + const Entity otherBodyEntity = pair.body1Entity == bodyToVisitEntity ? pair.body2Entity : pair.body1Entity; - // If the colliding body is a RigidBody (and not a CollisionBody) and is not a trigger - if (mRigidBodyComponents.hasComponent(pair.body1Entity) && mRigidBodyComponents.hasComponent(pair.body2Entity) - && !mCollidersComponents.getIsTrigger(pair.collider1Entity) && !mCollidersComponents.getIsTrigger(pair.collider2Entity)) { + // If the colliding body is a RigidBody (and not a CollisionBody) and is not a trigger + uint32 otherBodyIndex; + if (mRigidBodyComponents.hasComponentGetIndex(otherBodyEntity, otherBodyIndex) + && !mCollidersComponents.getIsTrigger(pair.collider1Entity) && !mCollidersComponents.getIsTrigger(pair.collider2Entity)) { - assert(pair.potentialContactManifoldsIndices.size() > 0); - nbTotalManifolds += pair.potentialContactManifoldsIndices.size(); + mProcessContactPairsOrderIslands.add(contactPairIndex); - // Add the contact manifold into the island - mIslands.nbContactManifolds[islandIndex] += pair.potentialContactManifoldsIndices.size(); - pair.isAlreadyInIsland = true; + assert(pair.nbPotentialContactManifolds > 0); + nbTotalManifolds += pair.nbPotentialContactManifolds; - const Entity otherBodyEntity = pair.body1Entity == bodyToVisitEntity ? pair.body2Entity : pair.body1Entity; + // Add the contact manifold into the island + mIslands.nbContactManifolds[islandIndex] += pair.nbPotentialContactManifolds; + pair.isAlreadyInIsland = true; - // Check if the other body has already been added to the island - if (mRigidBodyComponents.getIsAlreadyInIsland(otherBodyEntity)) continue; + // Check if the other body has already been added to the island + if (mRigidBodyComponents.mIsAlreadyInIsland[otherBodyIndex]) continue; - // Insert the other body into the stack of bodies to visit - bodyEntityIndicesToVisit.push(otherBodyEntity); - mRigidBodyComponents.setIsAlreadyInIsland(otherBodyEntity, true); - } - else { + // Insert the other body into the stack of bodies to visit + bodyEntitiesToVisit.push(otherBodyEntity); + mRigidBodyComponents.mIsAlreadyInIsland[otherBodyIndex] = true; + } + else { - // Add the contact pair index in the list of contact pairs that won't be part of islands - pair.isAlreadyInIsland = true; - } + // Add the contact pair index in the array of contact pairs that won't be part of islands + pair.isAlreadyInIsland = true; } } // For each joint in which the current body is involved - const List& joints = mRigidBodyComponents.getJoints(rigidBodyToVisit->getEntity()); - for (uint32 i=0; i < joints.size(); i++) { + const Array& joints = mRigidBodyComponents.getJoints(rigidBodyToVisit->getEntity()); + const uint32 nbBodyJoints = joints.size(); + for (uint32 i=0; i < nbBodyJoints; i++) { + + const uint32 jointComponentIndex = mJointsComponents.getEntityIndex(joints[i]); // Check if the current joint has already been added into an island - if (mJointsComponents.getIsAlreadyInIsland(joints[i])) continue; + if (mJointsComponents.mIsAlreadyInIsland[jointComponentIndex]) continue; // Add the joint into the island - mJointsComponents.setIsAlreadyInIsland(joints[i], true); + mJointsComponents.mIsAlreadyInIsland[jointComponentIndex] = true; - const Entity body1Entity = mJointsComponents.getBody1Entity(joints[i]); - const Entity body2Entity = mJointsComponents.getBody2Entity(joints[i]); + const Entity body1Entity = mJointsComponents.mBody1Entities[jointComponentIndex]; + const Entity body2Entity = mJointsComponents.mBody2Entities[jointComponentIndex]; const Entity otherBodyEntity = body1Entity == bodyToVisitEntity ? body2Entity : body1Entity; + const uint32 otherBodyIndex = mRigidBodyComponents.getEntityIndex(otherBodyEntity); + // Check if the other body has already been added to the island - if (mRigidBodyComponents.getIsAlreadyInIsland(otherBodyEntity)) continue; + if (mRigidBodyComponents.mIsAlreadyInIsland[otherBodyIndex]) continue; // Insert the other body into the stack of bodies to visit - bodyEntityIndicesToVisit.push(otherBodyEntity); - mRigidBodyComponents.setIsAlreadyInIsland(otherBodyEntity, true); + bodyEntitiesToVisit.push(otherBodyEntity); + mRigidBodyComponents.mIsAlreadyInIsland[otherBodyIndex] = true; } } // Reset the isAlreadyIsland variable of the static bodies so that they // can also be included in the other islands - for (uint j=0; j < mRigidBodyComponents.getNbEnabledComponents(); j++) { + const uint32 nbStaticBodiesAddedToIsland = staticBodiesAddedToIsland.size(); + for (uint32 j=0; j < nbStaticBodiesAddedToIsland; j++) { - if (mRigidBodyComponents.mBodyTypes[j] == BodyType::STATIC) { - mRigidBodyComponents.mIsAlreadyInIsland[j] = false; - } + assert(mRigidBodyComponents.getBodyType(staticBodiesAddedToIsland[j]) == BodyType::STATIC); + mRigidBodyComponents.setIsAlreadyInIsland(staticBodiesAddedToIsland[j], false); } + + staticBodiesAddedToIsland.clear(); } - mCollisionDetection.mMapBodyToContactPairs.clear(true); + // Clear the associated contacts pairs of rigid bodies + const uint32 nbRigidBodyEnabledComponents = mRigidBodyComponents.getNbEnabledComponents(); + for (uint b=0; b < nbRigidBodyEnabledComponents; b++) { + mRigidBodyComponents.mContactPairs[b].clear(); + } } // Put bodies to sleep if needed. @@ -885,35 +930,35 @@ void PhysicsWorld::updateSleepingBodies(decimal timeStep) { const decimal sleepAngularVelocitySquare = mSleepAngularVelocity * mSleepAngularVelocity; // For each island of the world - for (uint i=0; i sleepLinearVelocitySquare || - mRigidBodyComponents.getAngularVelocity(bodyEntity).lengthSquare() > sleepAngularVelocitySquare || - !mRigidBodyComponents.getIsAllowedToSleep(bodyEntity)) { + if (mRigidBodyComponents.mLinearVelocities[bodyIndex].lengthSquare() > sleepLinearVelocitySquare || + mRigidBodyComponents.mAngularVelocities[bodyIndex].lengthSquare() > sleepAngularVelocitySquare || + !mRigidBodyComponents.mIsAllowedToSleep[bodyIndex]) { // Reset the sleep time of the body - mRigidBodyComponents.setSleepTime(bodyEntity, decimal(0.0)); + mRigidBodyComponents.mSleepTimes[bodyIndex] = decimal(0.0); minSleepTime = decimal(0.0); } else { // If the body velocity is below the sleeping velocity threshold // Increase the sleep time - decimal sleepTime = mRigidBodyComponents.getSleepTime(bodyEntity); - mRigidBodyComponents.setSleepTime(bodyEntity, sleepTime + timeStep); - sleepTime = mRigidBodyComponents.getSleepTime(bodyEntity); - if (sleepTime < minSleepTime) { - minSleepTime = sleepTime; + mRigidBodyComponents.mSleepTimes[bodyIndex] += timeStep; + if (mRigidBodyComponents.mSleepTimes[bodyIndex] < minSleepTime) { + minSleepTime = mRigidBodyComponents.mSleepTimes[bodyIndex]; } } } @@ -924,9 +969,9 @@ void PhysicsWorld::updateSleepingBodies(decimal timeStep) { if (minSleepTime >= mTimeBeforeSleep) { // Put all the bodies of the island to sleep - for (uint b=0; b < mIslands.bodyEntities[i].size(); b++) { + for (uint b=0; b < mIslands.nbBodiesInIsland[i]; b++) { - const Entity bodyEntity = mIslands.bodyEntities[i][b]; + const Entity bodyEntity = mIslands.bodyEntities[mIslands.startBodyEntitiesIndex[i] + b]; RigidBody* body = mRigidBodyComponents.getRigidBody(bodyEntity); body->setIsSleeping(true); } @@ -947,7 +992,7 @@ void PhysicsWorld::enableSleeping(bool isSleepingEnabled) { if (!mIsSleepingEnabled) { // For each body of the world - List::Iterator it; + Array::Iterator it; for (it = mRigidBodies.begin(); it != mRigidBodies.end(); ++it) { // Wake up the rigid body diff --git a/src/mathematics/Matrix3x3.cpp b/src/mathematics/Matrix3x3.cpp index 2cca4a2c..18f96a1b 100644 --- a/src/mathematics/Matrix3x3.cpp +++ b/src/mathematics/Matrix3x3.cpp @@ -65,6 +65,3 @@ Matrix3x3 Matrix3x3::getInverse() const { // Return the inverse matrix return (invDeterminant * tempMatrix); } - - - diff --git a/src/mathematics/Quaternion.cpp b/src/mathematics/Quaternion.cpp index d1451a3d..a8918f93 100644 --- a/src/mathematics/Quaternion.cpp +++ b/src/mathematics/Quaternion.cpp @@ -106,7 +106,7 @@ Quaternion::Quaternion(const Matrix3x3& matrix) { // Compute the quaternion x = decimal(0.5) * r; y = (matrix[0][1] + matrix[1][0]) * s; - z = (matrix[2][0] - matrix[0][2]) * s; + z = (matrix[2][0] + matrix[0][2]) * s; w = (matrix[2][1] - matrix[1][2]) * s; } } diff --git a/src/mathematics/Vector3.cpp b/src/mathematics/Vector3.cpp index 5509e853..b6bfca54 100644 --- a/src/mathematics/Vector3.cpp +++ b/src/mathematics/Vector3.cpp @@ -48,7 +48,7 @@ Vector3 Vector3::getOneUnitOrthogonalVector() const { assert(length() > MACHINE_EPSILON); // Get the minimum element of the vector - Vector3 vectorAbs(std::fabs(x), std::fabs(y), std::fabs(z)); + Vector3 vectorAbs(std::abs(x), std::abs(y), std::abs(z)); int minElement = vectorAbs.getMinAxis(); if (minElement == 0) { diff --git a/src/mathematics/mathematics_functions.cpp b/src/mathematics/mathematics_functions.cpp deleted file mode 100755 index 66c3efda..00000000 --- a/src/mathematics/mathematics_functions.cpp +++ /dev/null @@ -1,415 +0,0 @@ -/******************************************************************************** -* ReactPhysics3D physics library, http://www.reactphysics3d.com * -* Copyright (c) 2010-2020 Daniel Chappuis * -********************************************************************************* -* * -* This software is provided 'as-is', without any express or implied warranty. * -* In no event will the authors be held liable for any damages arising from the * -* use of this software. * -* * -* Permission is granted to anyone to use this software for any purpose, * -* including commercial applications, and to alter it and redistribute it * -* freely, subject to the following restrictions: * -* * -* 1. The origin of this software must not be misrepresented; you must not claim * -* that you wrote the original software. If you use this software in a * -* product, an acknowledgment in the product documentation would be * -* appreciated but is not required. * -* * -* 2. Altered source versions must be plainly marked as such, and must not be * -* misrepresented as being the original software. * -* * -* 3. This notice may not be removed or altered from any source distribution. * -* * -********************************************************************************/ - -// Libraries -#include -#include -#include -#include - -using namespace reactphysics3d; - - -// Function to test if two vectors are (almost) equal -bool reactphysics3d::approxEqual(const Vector3& vec1, const Vector3& vec2, decimal epsilon) { - return approxEqual(vec1.x, vec2.x, epsilon) && approxEqual(vec1.y, vec2.y, epsilon) && - approxEqual(vec1.z, vec2.z, epsilon); -} - -// Function to test if two vectors are (almost) equal -bool reactphysics3d::approxEqual(const Vector2& vec1, const Vector2& vec2, decimal epsilon) { - return approxEqual(vec1.x, vec2.x, epsilon) && approxEqual(vec1.y, vec2.y, epsilon); -} - -// Compute the barycentric coordinates u, v, w of a point p inside the triangle (a, b, c) -// This method uses the technique described in the book Real-Time collision detection by -// Christer Ericson. -void reactphysics3d::computeBarycentricCoordinatesInTriangle(const Vector3& a, const Vector3& b, const Vector3& c, - const Vector3& p, decimal& u, decimal& v, decimal& w) { - const Vector3 v0 = b - a; - const Vector3 v1 = c - a; - const Vector3 v2 = p - a; - - decimal d00 = v0.dot(v0); - decimal d01 = v0.dot(v1); - decimal d11 = v1.dot(v1); - decimal d20 = v2.dot(v0); - decimal d21 = v2.dot(v1); - - decimal denom = d00 * d11 - d01 * d01; - v = (d11 * d20 - d01 * d21) / denom; - w = (d00 * d21 - d01 * d20) / denom; - u = decimal(1.0) - v - w; -} - -// Clamp a vector such that it is no longer than a given maximum length -Vector3 reactphysics3d::clamp(const Vector3& vector, decimal maxLength) { - if (vector.lengthSquare() > maxLength * maxLength) { - return vector.getUnit() * maxLength; - } - return vector; -} - -// Return true if two vectors are parallel -bool reactphysics3d::areParallelVectors(const Vector3& vector1, const Vector3& vector2) { - return vector1.cross(vector2).lengthSquare() < decimal(0.00001); -} - -// Return true if two vectors are orthogonal -bool reactphysics3d::areOrthogonalVectors(const Vector3& vector1, const Vector3& vector2) { - return std::abs(vector1.dot(vector2)) < decimal(0.001); -} - -// Compute and return a point on segment from "segPointA" and "segPointB" that is closest to point "pointC" -Vector3 reactphysics3d::computeClosestPointOnSegment(const Vector3& segPointA, const Vector3& segPointB, const Vector3& pointC) { - - const Vector3 ab = segPointB - segPointA; - - decimal abLengthSquare = ab.lengthSquare(); - - // If the segment has almost zero length - if (abLengthSquare < MACHINE_EPSILON) { - - // Return one end-point of the segment as the closest point - return segPointA; - } - - // Project point C onto "AB" line - decimal t = (pointC - segPointA).dot(ab) / abLengthSquare; - - // If projected point onto the line is outside the segment, clamp it to the segment - if (t < decimal(0.0)) t = decimal(0.0); - if (t > decimal(1.0)) t = decimal(1.0); - - // Return the closest point on the segment - return segPointA + t * ab; -} - -// Compute the closest points between two segments -// This method uses the technique described in the book Real-Time -// collision detection by Christer Ericson. -void reactphysics3d::computeClosestPointBetweenTwoSegments(const Vector3& seg1PointA, const Vector3& seg1PointB, - const Vector3& seg2PointA, const Vector3& seg2PointB, - Vector3& closestPointSeg1, Vector3& closestPointSeg2) { - - const Vector3 d1 = seg1PointB - seg1PointA; - const Vector3 d2 = seg2PointB - seg2PointA; - const Vector3 r = seg1PointA - seg2PointA; - decimal a = d1.lengthSquare(); - decimal e = d2.lengthSquare(); - decimal f = d2.dot(r); - decimal s, t; - - // If both segments degenerate into points - if (a <= MACHINE_EPSILON && e <= MACHINE_EPSILON) { - - closestPointSeg1 = seg1PointA; - closestPointSeg2 = seg2PointA; - return; - } - if (a <= MACHINE_EPSILON) { // If first segment degenerates into a point - - s = decimal(0.0); - - // Compute the closest point on second segment - t = clamp(f / e, decimal(0.0), decimal(1.0)); - } - else { - - decimal c = d1.dot(r); - - // If the second segment degenerates into a point - if (e <= MACHINE_EPSILON) { - - t = decimal(0.0); - s = clamp(-c / a, decimal(0.0), decimal(1.0)); - } - else { - - decimal b = d1.dot(d2); - decimal denom = a * e - b * b; - - // If the segments are not parallel - if (denom != decimal(0.0)) { - - // Compute the closest point on line 1 to line 2 and - // clamp to first segment. - s = clamp((b * f - c * e) / denom, decimal(0.0), decimal(1.0)); - } - else { - - // Pick an arbitrary point on first segment - s = decimal(0.0); - } - - // Compute the point on line 2 closest to the closest point - // we have just found - t = (b * s + f) / e; - - // If this closest point is inside second segment (t in [0, 1]), we are done. - // Otherwise, we clamp the point to the second segment and compute again the - // closest point on segment 1 - if (t < decimal(0.0)) { - t = decimal(0.0); - s = clamp(-c / a, decimal(0.0), decimal(1.0)); - } - else if (t > decimal(1.0)) { - t = decimal(1.0); - s = clamp((b - c) / a, decimal(0.0), decimal(1.0)); - } - } - } - - // Compute the closest points on both segments - closestPointSeg1 = seg1PointA + d1 * s; - closestPointSeg2 = seg2PointA + d2 * t; -} - -// Compute the intersection between a plane and a segment -// Let the plane define by the equation planeNormal.dot(X) = planeD with X a point on the plane and "planeNormal" the plane normal. This method -// computes the intersection P between the plane and the segment (segA, segB). The method returns the value "t" such -// that P = segA + t * (segB - segA). Note that it only returns a value in [0, 1] if there is an intersection. Otherwise, -// there is no intersection between the plane and the segment. -decimal reactphysics3d::computePlaneSegmentIntersection(const Vector3& segA, const Vector3& segB, const decimal planeD, const Vector3& planeNormal) { - - const decimal parallelEpsilon = decimal(0.0001); - decimal t = decimal(-1); - - decimal nDotAB = planeNormal.dot(segB - segA); - - // If the segment is not parallel to the plane - if (std::abs(nDotAB) > parallelEpsilon) { - t = (planeD - planeNormal.dot(segA)) / nDotAB; - } - - return t; -} - -// Compute the distance between a point "point" and a line given by the points "linePointA" and "linePointB" -decimal reactphysics3d::computePointToLineDistance(const Vector3& linePointA, const Vector3& linePointB, const Vector3& point) { - - decimal distAB = (linePointB - linePointA).length(); - - if (distAB < MACHINE_EPSILON) { - return (point - linePointA).length(); - } - - return ((point - linePointA).cross(point - linePointB)).length() / distAB; -} - -// Clip a segment against multiple planes and return the clipped segment vertices -// This method implements the Sutherland–Hodgman clipping algorithm -List reactphysics3d::clipSegmentWithPlanes(const Vector3& segA, const Vector3& segB, - const List& planesPoints, - const List& planesNormals, - MemoryAllocator& allocator) { - assert(planesPoints.size() == planesNormals.size()); - - List inputVertices(allocator, 2); - List outputVertices(allocator, 2); - - inputVertices.add(segA); - inputVertices.add(segB); - - // For each clipping plane - for (uint p=0; p= decimal(0.0)) { - - // If the first vertex is not in front of the clippling plane - if (v1DotN < decimal(0.0)) { - - // The second point we keep is the intersection between the segment v1, v2 and the clipping plane - decimal t = computePlaneSegmentIntersection(v1, v2, planesNormals[p].dot(planesPoints[p]), planesNormals[p]); - - if (t >= decimal(0) && t <= decimal(1.0)) { - outputVertices.add(v1 + t * (v2 - v1)); - } - else { - outputVertices.add(v2); - } - } - else { - outputVertices.add(v1); - } - - // Add the second vertex - outputVertices.add(v2); - } - else { // If the second vertex is behind the clipping plane - - // If the first vertex is in front of the clippling plane - if (v1DotN >= decimal(0.0)) { - - outputVertices.add(v1); - - // The first point we keep is the intersection between the segment v1, v2 and the clipping plane - decimal t = computePlaneSegmentIntersection(v1, v2, -planesNormals[p].dot(planesPoints[p]), -planesNormals[p]); - - if (t >= decimal(0.0) && t <= decimal(1.0)) { - outputVertices.add(v1 + t * (v2 - v1)); - } - } - } - - inputVertices = outputVertices; - } - - return outputVertices; -} - -// Clip a polygon against multiple planes and return the clipped polygon vertices -// This method implements the Sutherland–Hodgman clipping algorithm -List reactphysics3d::clipPolygonWithPlanes(const List& polygonVertices, const List& planesPoints, - const List& planesNormals, MemoryAllocator& allocator) { - - assert(planesPoints.size() == planesNormals.size()); - - uint nbMaxElements = polygonVertices.size() + planesPoints.size(); - List inputVertices(allocator, nbMaxElements); - List outputVertices(allocator, nbMaxElements); - - inputVertices.addRange(polygonVertices); - - // For each clipping plane - for (uint p=0; p= decimal(0.0)) { - - // If the first vertex is not in front of the clippling plane - if (v1DotN < decimal(0.0)) { - - // The second point we keep is the intersection between the segment v1, v2 and the clipping plane - decimal t = computePlaneSegmentIntersection(v1, v2, planesNormals[p].dot(planesPoints[p]), planesNormals[p]); - - if (t >= decimal(0) && t <= decimal(1.0)) { - outputVertices.add(v1 + t * (v2 - v1)); - } - else { - outputVertices.add(v2); - } - } - - // Add the second vertex - outputVertices.add(v2); - } - else { // If the second vertex is behind the clipping plane - - // If the first vertex is in front of the clippling plane - if (v1DotN >= decimal(0.0)) { - - // The first point we keep is the intersection between the segment v1, v2 and the clipping plane - decimal t = computePlaneSegmentIntersection(v1, v2, -planesNormals[p].dot(planesPoints[p]), -planesNormals[p]); - - if (t >= decimal(0.0) && t <= decimal(1.0)) { - outputVertices.add(v1 + t * (v2 - v1)); - } - else { - outputVertices.add(v1); - } - } - } - - vStart = vEnd; - } - - inputVertices = outputVertices; - } - - return outputVertices; -} - -// Project a point onto a plane that is given by a point and its unit length normal -Vector3 reactphysics3d::projectPointOntoPlane(const Vector3& point, const Vector3& unitPlaneNormal, const Vector3& planePoint) { - return point - unitPlaneNormal.dot(point - planePoint) * unitPlaneNormal; -} - -// Return the distance between a point and a plane (the plane normal must be normalized) -decimal reactphysics3d::computePointToPlaneDistance(const Vector3& point, const Vector3& planeNormal, const Vector3& planePoint) { - return planeNormal.dot(point - planePoint); -} - -// Return true if the given number is prime -bool reactphysics3d::isPrimeNumber(int number) { - - // If it's a odd number - if ((number & 1) != 0) { - - int limit = static_cast(std::sqrt(number)); - - for (int divisor = 3; divisor <= limit; divisor += 2) { - - // If we have found a divisor - if ((number % divisor) == 0) { - - // It is not a prime number - return false; - } - } - - return true; - } - - return number == 2; -} - -// Return an unique integer from two integer numbers (pairing function) -/// Here we assume that the two parameter numbers are sorted such that -/// number1 = max(number1, number2) -/// http://szudzik.com/ElegantPairing.pdf -uint64 reactphysics3d::pairNumbers(uint32 number1, uint32 number2) { - assert(number1 == std::max(number1, number2)); - return number1 * number1 + number1 + number2; -} - diff --git a/src/systems/BroadPhaseSystem.cpp b/src/systems/BroadPhaseSystem.cpp index 53a2993e..678c99ec 100644 --- a/src/systems/BroadPhaseSystem.cpp +++ b/src/systems/BroadPhaseSystem.cpp @@ -37,9 +37,9 @@ using namespace reactphysics3d; // Constructor BroadPhaseSystem::BroadPhaseSystem(CollisionDetectionSystem& collisionDetection, ColliderComponents& collidersComponents, TransformComponents& transformComponents, RigidBodyComponents& rigidBodyComponents) - :mDynamicAABBTree(collisionDetection.getMemoryManager().getPoolAllocator(), DYNAMIC_TREE_FAT_AABB_INFLATE_PERCENTAGE), + :mDynamicAABBTree(collisionDetection.getMemoryManager().getHeapAllocator(), DYNAMIC_TREE_FAT_AABB_INFLATE_PERCENTAGE), mCollidersComponents(collidersComponents), mTransformsComponents(transformComponents), - mRigidBodyComponents(rigidBodyComponents), mMovedShapes(collisionDetection.getMemoryManager().getPoolAllocator()), + mRigidBodyComponents(rigidBodyComponents), mMovedShapes(collisionDetection.getMemoryManager().getHeapAllocator()), mCollisionDetection(collisionDetection) { #ifdef IS_RP3D_PROFILING_ENABLED @@ -67,13 +67,16 @@ bool BroadPhaseSystem::testOverlappingShapes(int32 shape1BroadPhaseId, int32 sha } // Ray casting method -void BroadPhaseSystem::raycast(const Ray& ray, RaycastTest& raycastTest, - unsigned short raycastWithCategoryMaskBits) const { +void BroadPhaseSystem::raycast(const Ray& ray, RaycastTest& raycastTest, unsigned short raycastWithCategoryMaskBits) const { RP3D_PROFILE("BroadPhaseSystem::raycast()", mProfiler); BroadPhaseRaycastCallback broadPhaseRaycastCallback(mDynamicAABBTree, raycastWithCategoryMaskBits, raycastTest); + // Compute the inverse ray direction + const Vector3 rayDirection = ray.point2 - ray.point1; + const Vector3 rayDirectionInverse(decimal(1.0) / rayDirection.x, decimal(1.0) / rayDirection.y, decimal(1.0) / rayDirection.z); + mDynamicAABBTree.raycast(ray, broadPhaseRaycastCallback); } @@ -206,12 +209,12 @@ void BroadPhaseSystem::addMovedCollider(int broadPhaseID, Collider* collider) { } // Compute all the overlapping pairs of collision shapes -void BroadPhaseSystem::computeOverlappingPairs(MemoryManager& memoryManager, List>& overlappingNodes) { +void BroadPhaseSystem::computeOverlappingPairs(MemoryManager& memoryManager, Array>& overlappingNodes) { RP3D_PROFILE("BroadPhaseSystem::computeOverlappingPairs()", mProfiler); - // Get the list of the colliders that have moved or have been created in the last frame - List shapesToTest = mMovedShapes.toList(memoryManager.getPoolAllocator()); + // Get the array of the colliders that have moved or have been created in the last frame + Array shapesToTest = mMovedShapes.toArray(memoryManager.getHeapAllocator()); // Ask the dynamic AABB tree to report all collision shapes that overlap with the shapes to test mDynamicAABBTree.reportAllShapesOverlappingWithShapes(shapesToTest, 0, shapesToTest.size(), overlappingNodes); diff --git a/src/systems/CollisionDetectionSystem.cpp b/src/systems/CollisionDetectionSystem.cpp index 24f9660c..2124f812 100644 --- a/src/systems/CollisionDetectionSystem.cpp +++ b/src/systems/CollisionDetectionSystem.cpp @@ -49,25 +49,26 @@ using namespace reactphysics3d; using namespace std; // Constructor -CollisionDetectionSystem::CollisionDetectionSystem(PhysicsWorld* world, ColliderComponents& collidersComponents, TransformComponents& transformComponents, - CollisionBodyComponents& collisionBodyComponents, RigidBodyComponents& rigidBodyComponents, MemoryManager& memoryManager) - : mMemoryManager(memoryManager), mCollidersComponents(collidersComponents), +CollisionDetectionSystem::CollisionDetectionSystem(PhysicsWorld* world, ColliderComponents& collidersComponents, TransformComponents& transformComponents, + CollisionBodyComponents& collisionBodyComponents, RigidBodyComponents& rigidBodyComponents, + MemoryManager& memoryManager, HalfEdgeStructure& triangleHalfEdgeStructure) + : mMemoryManager(memoryManager), mCollidersComponents(collidersComponents), mRigidBodyComponents(rigidBodyComponents), mCollisionDispatch(mMemoryManager.getPoolAllocator()), mWorld(world), mNoCollisionPairs(mMemoryManager.getPoolAllocator()), - mOverlappingPairs(mMemoryManager.getPoolAllocator(), mMemoryManager.getSingleFrameAllocator(), mCollidersComponents, - collisionBodyComponents, rigidBodyComponents, mNoCollisionPairs, mCollisionDispatch), + mOverlappingPairs(mMemoryManager, mCollidersComponents, collisionBodyComponents, rigidBodyComponents, + mNoCollisionPairs, mCollisionDispatch), + mBroadPhaseOverlappingNodes(mMemoryManager.getHeapAllocator(), 32), mBroadPhaseSystem(*this, mCollidersComponents, transformComponents, rigidBodyComponents), mMapBroadPhaseIdToColliderEntity(memoryManager.getPoolAllocator()), mNarrowPhaseInput(mMemoryManager.getSingleFrameAllocator(), mOverlappingPairs), mPotentialContactPoints(mMemoryManager.getSingleFrameAllocator()), mPotentialContactManifolds(mMemoryManager.getSingleFrameAllocator()), mContactPairs1(mMemoryManager.getPoolAllocator()), mContactPairs2(mMemoryManager.getPoolAllocator()), mPreviousContactPairs(&mContactPairs1), mCurrentContactPairs(&mContactPairs2), - mLostContactPairs(mMemoryManager.getSingleFrameAllocator()), mMapPairIdToContactPairIndex1(mMemoryManager.getPoolAllocator()), - mMapPairIdToContactPairIndex2(mMemoryManager.getPoolAllocator()), mPreviousMapPairIdToContactPairIndex(&mMapPairIdToContactPairIndex1), - mCurrentMapPairIdToContactPairIndex(&mMapPairIdToContactPairIndex2), mContactManifolds1(mMemoryManager.getPoolAllocator()), - mContactManifolds2(mMemoryManager.getPoolAllocator()), mPreviousContactManifolds(&mContactManifolds1), mCurrentContactManifolds(&mContactManifolds2), - mContactPoints1(mMemoryManager.getPoolAllocator()), - mContactPoints2(mMemoryManager.getPoolAllocator()), mPreviousContactPoints(&mContactPoints1), - mCurrentContactPoints(&mContactPoints2), mMapBodyToContactPairs(mMemoryManager.getSingleFrameAllocator()) { + mLostContactPairs(mMemoryManager.getSingleFrameAllocator()), mPreviousMapPairIdToContactPairIndex(mMemoryManager.getHeapAllocator()), + mContactManifolds1(mMemoryManager.getPoolAllocator()), mContactManifolds2(mMemoryManager.getPoolAllocator()), + mPreviousContactManifolds(&mContactManifolds1), mCurrentContactManifolds(&mContactManifolds2), + mContactPoints1(mMemoryManager.getPoolAllocator()), mContactPoints2(mMemoryManager.getPoolAllocator()), + mPreviousContactPoints(&mContactPoints1), mCurrentContactPoints(&mContactPoints2), mCollisionBodyContactPairsIndices(mMemoryManager.getSingleFrameAllocator()), + mNbPreviousPotentialContactManifolds(0), mNbPreviousPotentialContactPoints(0), mTriangleHalfEdgeStructure(triangleHalfEdgeStructure) { #ifdef IS_RP3D_PROFILING_ENABLED @@ -100,17 +101,20 @@ void CollisionDetectionSystem::computeBroadPhase() { RP3D_PROFILE("CollisionDetectionSystem::computeBroadPhase()", mProfiler); + assert(mBroadPhaseOverlappingNodes.size() == 0); + // Ask the broad-phase to compute all the shapes overlapping with the shapes that // have moved or have been added in the last frame. This call can only add new // overlapping pairs in the collision detection. - List> overlappingNodes(mMemoryManager.getPoolAllocator(), 32); - mBroadPhaseSystem.computeOverlappingPairs(mMemoryManager, overlappingNodes); + mBroadPhaseSystem.computeOverlappingPairs(mMemoryManager, mBroadPhaseOverlappingNodes); // Create new overlapping pairs if necessary - updateOverlappingPairs(overlappingNodes); + updateOverlappingPairs(mBroadPhaseOverlappingNodes); // Remove non overlapping pairs removeNonOverlappingPairs(); + + mBroadPhaseOverlappingNodes.clear(); } // Remove pairs that are not overlapping anymore @@ -118,25 +122,55 @@ void CollisionDetectionSystem::removeNonOverlappingPairs() { RP3D_PROFILE("CollisionDetectionSystem::removeNonOverlappingPairs()", mProfiler); - for (uint64 i=0; i < mOverlappingPairs.getNbPairs(); i++) { + // For each convex pairs + for (uint64 i=0; i < mOverlappingPairs.mConvexPairs.size(); i++) { + + OverlappingPairs::ConvexOverlappingPair& overlappingPair = mOverlappingPairs.mConvexPairs[i]; // Check if we need to test overlap. If so, test if the two shapes are still overlapping. // Otherwise, we destroy the overlapping pair - if (mOverlappingPairs.mNeedToTestOverlap[i]) { + if (overlappingPair.needToTestOverlap) { - if(mBroadPhaseSystem.testOverlappingShapes(mOverlappingPairs.mPairBroadPhaseId1[i], mOverlappingPairs.mPairBroadPhaseId2[i])) { - mOverlappingPairs.mNeedToTestOverlap[i] = false; + if(mBroadPhaseSystem.testOverlappingShapes(overlappingPair.broadPhaseId1, overlappingPair.broadPhaseId2)) { + overlappingPair.needToTestOverlap = false; } else { // If the two colliders of the pair were colliding in the previous frame - if (mOverlappingPairs.mCollidingInPreviousFrame[i]) { + if (overlappingPair.collidingInPreviousFrame) { // Create a new lost contact pair - addLostContactPair(i); + addLostContactPair(overlappingPair); } - mOverlappingPairs.removePair(mOverlappingPairs.mPairIds[i]); + mOverlappingPairs.removePair(i, true); + i--; + } + } + } + + // For each concave pairs + for (uint64 i=0; i < mOverlappingPairs.mConcavePairs.size(); i++) { + + OverlappingPairs::ConcaveOverlappingPair& overlappingPair = mOverlappingPairs.mConcavePairs[i]; + + // Check if we need to test overlap. If so, test if the two shapes are still overlapping. + // Otherwise, we destroy the overlapping pair + if (overlappingPair.needToTestOverlap) { + + if(mBroadPhaseSystem.testOverlappingShapes(overlappingPair.broadPhaseId1, overlappingPair.broadPhaseId2)) { + overlappingPair.needToTestOverlap = false; + } + else { + + // If the two colliders of the pair were colliding in the previous frame + if (overlappingPair.collidingInPreviousFrame) { + + // Create a new lost contact pair + addLostContactPair(overlappingPair); + } + + mOverlappingPairs.removePair(i, false); i--; } } @@ -144,31 +178,32 @@ void CollisionDetectionSystem::removeNonOverlappingPairs() { } // Add a lost contact pair (pair of colliders that are not in contact anymore) -void CollisionDetectionSystem::addLostContactPair(uint64 overlappingPairIndex) { +void CollisionDetectionSystem::addLostContactPair(OverlappingPairs::OverlappingPair& overlappingPair) { - const Entity collider1Entity = mOverlappingPairs.mColliders1[overlappingPairIndex]; - const Entity collider2Entity = mOverlappingPairs.mColliders2[overlappingPairIndex]; + const uint32 collider1Index = mCollidersComponents.getEntityIndex(overlappingPair.collider1); + const uint32 collider2Index = mCollidersComponents.getEntityIndex(overlappingPair.collider2); - const Entity body1Entity = mCollidersComponents.getBody(collider1Entity); - const Entity body2Entity = mCollidersComponents.getBody(collider2Entity); + const Entity body1Entity = mCollidersComponents.mBodiesEntities[collider1Index]; + const Entity body2Entity = mCollidersComponents.mBodiesEntities[collider2Index]; - const bool isCollider1Trigger = mCollidersComponents.getIsTrigger(collider1Entity); - const bool isCollider2Trigger = mCollidersComponents.getIsTrigger(collider2Entity); + const bool isCollider1Trigger = mCollidersComponents.mIsTrigger[collider1Index]; + const bool isCollider2Trigger = mCollidersComponents.mIsTrigger[collider2Index]; const bool isTrigger = isCollider1Trigger || isCollider2Trigger; // Create a lost contact pair - ContactPair lostContactPair(mOverlappingPairs.mPairIds[overlappingPairIndex], body1Entity, body2Entity, collider1Entity, collider2Entity, mLostContactPairs.size(), - true, isTrigger, mMemoryManager.getHeapAllocator()); + ContactPair lostContactPair(overlappingPair.pairID, body1Entity, body2Entity, overlappingPair.collider1, overlappingPair.collider2, mLostContactPairs.size(), + true, isTrigger); mLostContactPairs.add(lostContactPair); } -// Take a list of overlapping nodes in the broad-phase and create new overlapping pairs if necessary -void CollisionDetectionSystem::updateOverlappingPairs(const List>& overlappingNodes) { +// Take an array of overlapping nodes in the broad-phase and create new overlapping pairs if necessary +void CollisionDetectionSystem::updateOverlappingPairs(const Array>& overlappingNodes) { RP3D_PROFILE("CollisionDetectionSystem::updateOverlappingPairs()", mProfiler); // For each overlapping pair of nodes - for (uint i=0; i < overlappingNodes.size(); i++) { + const uint32 nbOverlappingNodes = overlappingNodes.size(); + for (uint32 i=0; i < nbOverlappingNodes; i++) { Pair nodePair = overlappingNodes[i]; @@ -192,39 +227,66 @@ void CollisionDetectionSystem::updateOverlappingPairs(const ListgetCollisionShape()->isConvex() || shape2->getCollisionShape()->isConvex()) { + const unsigned short shape1CollideWithMaskBits = mCollidersComponents.mCollideWithMaskBits[collider1Index]; + const unsigned short shape2CollideWithMaskBits = mCollidersComponents.mCollideWithMaskBits[collider2Index]; - // Add the new overlapping pair - mOverlappingPairs.addPair(shape1, shape2); + const unsigned short shape1CollisionCategoryBits = mCollidersComponents.mCollisionCategoryBits[collider1Index]; + const unsigned short shape2CollisionCategoryBits = mCollidersComponents.mCollisionCategoryBits[collider2Index]; + + // Check if the collision filtering allows collision between the two shapes + if ((shape1CollideWithMaskBits & shape2CollisionCategoryBits) != 0 && + (shape1CollisionCategoryBits & shape2CollideWithMaskBits) != 0) { + + Collider* shape1 = mCollidersComponents.mColliders[collider1Index]; + Collider* shape2 = mCollidersComponents.mColliders[collider2Index]; + + // Check that at least one collision shape is convex + const bool isShape1Convex = shape1->getCollisionShape()->isConvex(); + const bool isShape2Convex = shape2->getCollisionShape()->isConvex(); + if (isShape1Convex || isShape2Convex) { + + // Add the new overlapping pair + mOverlappingPairs.addPair(collider1Index, collider2Index, isShape1Convex && isShape2Convex); + } + } + } + else { + + // We do not need to test the pair for overlap because it has just been reported that they still overlap + overlappingPair->needToTestOverlap = false; } } } - else { - - // We do not need to test the pair for overlap because it has just been reported that they still overlap - mOverlappingPairs.mNeedToTestOverlap[it->second] = false; - } } } } @@ -242,62 +304,61 @@ void CollisionDetectionSystem::computeMiddlePhase(NarrowPhaseInput& narrowPhaseI mOverlappingPairs.clearObsoleteLastFrameCollisionInfos(); // For each possible convex vs convex pair of bodies - for (uint64 i=0; i < mOverlappingPairs.getNbConvexVsConvexPairs(); i++) { + const uint64 nbConvexVsConvexPairs = mOverlappingPairs.mConvexPairs.size(); + for (uint64 i=0; i < nbConvexVsConvexPairs; i++) { - assert(mCollidersComponents.getBroadPhaseId(mOverlappingPairs.mColliders1[i]) != -1); - assert(mCollidersComponents.getBroadPhaseId(mOverlappingPairs.mColliders2[i]) != -1); - assert(mCollidersComponents.getBroadPhaseId(mOverlappingPairs.mColliders1[i]) != mCollidersComponents.getBroadPhaseId(mOverlappingPairs.mColliders2[i])); + OverlappingPairs::ConvexOverlappingPair& overlappingPair = mOverlappingPairs.mConvexPairs[i]; - // Check that at least one body is enabled (active and awake) and not static - if (mOverlappingPairs.mIsActive[i]) { + assert(mCollidersComponents.getBroadPhaseId(overlappingPair.collider1) != -1); + assert(mCollidersComponents.getBroadPhaseId(overlappingPair.collider2) != -1); + assert(mCollidersComponents.getBroadPhaseId(overlappingPair.collider1) != mCollidersComponents.getBroadPhaseId(overlappingPair.collider2)); - const Entity collider1Entity = mOverlappingPairs.mColliders1[i]; - const Entity collider2Entity = mOverlappingPairs.mColliders2[i]; - const uint collider1Index = mCollidersComponents.getEntityIndex(collider1Entity); - const uint collider2Index = mCollidersComponents.getEntityIndex(collider2Entity); + const Entity collider1Entity = overlappingPair.collider1; + const Entity collider2Entity = overlappingPair.collider2; - CollisionShape* collisionShape1 = mCollidersComponents.mCollisionShapes[collider1Index]; - CollisionShape* collisionShape2 = mCollidersComponents.mCollisionShapes[collider2Index]; + const uint collider1Index = mCollidersComponents.getEntityIndex(collider1Entity); + const uint collider2Index = mCollidersComponents.getEntityIndex(collider2Entity); - NarrowPhaseAlgorithmType algorithmType = mOverlappingPairs.mNarrowPhaseAlgorithmType[i]; + CollisionShape* collisionShape1 = mCollidersComponents.mCollisionShapes[collider1Index]; + CollisionShape* collisionShape2 = mCollidersComponents.mCollisionShapes[collider2Index]; - const bool isCollider1Trigger = mCollidersComponents.mIsTrigger[collider1Index]; - const bool isCollider2Trigger = mCollidersComponents.mIsTrigger[collider2Index]; - const bool reportContacts = needToReportContacts && !isCollider1Trigger && !isCollider2Trigger; + NarrowPhaseAlgorithmType algorithmType = overlappingPair.narrowPhaseAlgorithmType; - // No middle-phase is necessary, simply create a narrow phase info - // for the narrow-phase collision detection - narrowPhaseInput.addNarrowPhaseTest(mOverlappingPairs.mPairIds[i], i, collider1Entity, collider2Entity, collisionShape1, collisionShape2, - mCollidersComponents.mLocalToWorldTransforms[collider1Index], - mCollidersComponents.mLocalToWorldTransforms[collider2Index], - algorithmType, reportContacts, mMemoryManager.getSingleFrameAllocator()); + const bool isCollider1Trigger = mCollidersComponents.mIsTrigger[collider1Index]; + const bool isCollider2Trigger = mCollidersComponents.mIsTrigger[collider2Index]; + const bool reportContacts = needToReportContacts && !isCollider1Trigger && !isCollider2Trigger; - mOverlappingPairs.mCollidingInCurrentFrame[i] = false; - } + // No middle-phase is necessary, simply create a narrow phase info + // for the narrow-phase collision detection + narrowPhaseInput.addNarrowPhaseTest(overlappingPair.pairID, collider1Entity, collider2Entity, collisionShape1, collisionShape2, + mCollidersComponents.mLocalToWorldTransforms[collider1Index], + mCollidersComponents.mLocalToWorldTransforms[collider2Index], + algorithmType, reportContacts, &overlappingPair.lastFrameCollisionInfo, + mMemoryManager.getSingleFrameAllocator()); + + overlappingPair.collidingInCurrentFrame = false; } // For each possible convex vs concave pair of bodies - const uint64 convexVsConcaveStartIndex = mOverlappingPairs.getConvexVsConcavePairsStartIndex(); - for (uint64 i=convexVsConcaveStartIndex; i < convexVsConcaveStartIndex + mOverlappingPairs.getNbConvexVsConcavePairs(); i++) { + const uint64 nbConcavePairs = mOverlappingPairs.mConcavePairs.size(); + for (uint64 i=0; i < nbConcavePairs; i++) { - assert(mCollidersComponents.getBroadPhaseId(mOverlappingPairs.mColliders1[i]) != -1); - assert(mCollidersComponents.getBroadPhaseId(mOverlappingPairs.mColliders2[i]) != -1); - assert(mCollidersComponents.getBroadPhaseId(mOverlappingPairs.mColliders1[i]) != mCollidersComponents.getBroadPhaseId(mOverlappingPairs.mColliders2[i])); + OverlappingPairs::ConcaveOverlappingPair& overlappingPair = mOverlappingPairs.mConcavePairs[i]; - // Check that at least one body is enabled (active and awake) and not static - if (mOverlappingPairs.mIsActive[i]) { + assert(mCollidersComponents.getBroadPhaseId(overlappingPair.collider1) != -1); + assert(mCollidersComponents.getBroadPhaseId(overlappingPair.collider2) != -1); + assert(mCollidersComponents.getBroadPhaseId(overlappingPair.collider1) != mCollidersComponents.getBroadPhaseId(overlappingPair.collider2)); - computeConvexVsConcaveMiddlePhase(i, mMemoryManager.getSingleFrameAllocator(), narrowPhaseInput); + computeConvexVsConcaveMiddlePhase(overlappingPair, mMemoryManager.getSingleFrameAllocator(), narrowPhaseInput); - mOverlappingPairs.mCollidingInCurrentFrame[i] = false; - } + overlappingPair.collidingInCurrentFrame = false; } } // Compute the middle-phase collision detection -void CollisionDetectionSystem::computeMiddlePhaseCollisionSnapshot(List& convexPairs, List& concavePairs, NarrowPhaseInput& narrowPhaseInput, - bool reportContacts) { +void CollisionDetectionSystem::computeMiddlePhaseCollisionSnapshot(Array& convexPairs, Array& concavePairs, + NarrowPhaseInput& narrowPhaseInput, bool reportContacts) { RP3D_PROFILE("CollisionDetectionSystem::computeMiddlePhase()", mProfiler); @@ -308,15 +369,16 @@ void CollisionDetectionSystem::computeMiddlePhaseCollisionSnapshot(List& mOverlappingPairs.clearObsoleteLastFrameCollisionInfos(); // For each possible convex vs convex pair of bodies - for (uint64 p=0; p < convexPairs.size(); p++) { + const uint64 nbConvexPairs = convexPairs.size(); + for (uint64 p=0; p < nbConvexPairs; p++) { const uint64 pairId = convexPairs[p]; - const uint64 pairIndex = mOverlappingPairs.mMapPairIdToPairIndex[pairId]; - assert(pairIndex < mOverlappingPairs.getNbPairs()); + const uint64 pairIndex = mOverlappingPairs.mMapConvexPairIdToPairIndex[pairId]; + assert(pairIndex < mOverlappingPairs.mConvexPairs.size()); - const Entity collider1Entity = mOverlappingPairs.mColliders1[pairIndex]; - const Entity collider2Entity = mOverlappingPairs.mColliders2[pairIndex]; + const Entity collider1Entity = mOverlappingPairs.mConvexPairs[pairIndex].collider1; + const Entity collider2Entity = mOverlappingPairs.mConvexPairs[pairIndex].collider2; const uint collider1Index = mCollidersComponents.getEntityIndex(collider1Entity); const uint collider2Index = mCollidersComponents.getEntityIndex(collider2Entity); @@ -328,38 +390,39 @@ void CollisionDetectionSystem::computeMiddlePhaseCollisionSnapshot(List& CollisionShape* collisionShape1 = mCollidersComponents.mCollisionShapes[collider1Index]; CollisionShape* collisionShape2 = mCollidersComponents.mCollisionShapes[collider2Index]; - NarrowPhaseAlgorithmType algorithmType = mOverlappingPairs.mNarrowPhaseAlgorithmType[pairIndex]; + NarrowPhaseAlgorithmType algorithmType = mOverlappingPairs.mConvexPairs[pairIndex].narrowPhaseAlgorithmType; // No middle-phase is necessary, simply create a narrow phase info // for the narrow-phase collision detection - narrowPhaseInput.addNarrowPhaseTest(pairId, pairIndex, collider1Entity, collider2Entity, collisionShape1, collisionShape2, + narrowPhaseInput.addNarrowPhaseTest(pairId, collider1Entity, collider2Entity, collisionShape1, collisionShape2, mCollidersComponents.mLocalToWorldTransforms[collider1Index], mCollidersComponents.mLocalToWorldTransforms[collider2Index], - algorithmType, reportContacts, mMemoryManager.getSingleFrameAllocator()); + algorithmType, reportContacts, &mOverlappingPairs.mConvexPairs[pairIndex].lastFrameCollisionInfo, mMemoryManager.getSingleFrameAllocator()); } // For each possible convex vs concave pair of bodies - for (uint p=0; p < concavePairs.size(); p++) { + const uint nbConcavePairs = concavePairs.size(); + for (uint p=0; p < nbConcavePairs; p++) { const uint64 pairId = concavePairs[p]; - const uint64 pairIndex = mOverlappingPairs.mMapPairIdToPairIndex[pairId]; + const uint64 pairIndex = mOverlappingPairs.mMapConcavePairIdToPairIndex[pairId]; - assert(mCollidersComponents.getBroadPhaseId(mOverlappingPairs.mColliders1[pairIndex]) != -1); - assert(mCollidersComponents.getBroadPhaseId(mOverlappingPairs.mColliders2[pairIndex]) != -1); - assert(mCollidersComponents.getBroadPhaseId(mOverlappingPairs.mColliders1[pairIndex]) != mCollidersComponents.getBroadPhaseId(mOverlappingPairs.mColliders2[pairIndex])); + assert(mCollidersComponents.getBroadPhaseId(mOverlappingPairs.mConcavePairs[pairIndex].collider1) != -1); + assert(mCollidersComponents.getBroadPhaseId(mOverlappingPairs.mConcavePairs[pairIndex].collider2) != -1); + assert(mCollidersComponents.getBroadPhaseId(mOverlappingPairs.mConcavePairs[pairIndex].collider1) != mCollidersComponents.getBroadPhaseId(mOverlappingPairs.mConcavePairs[pairIndex].collider2)); - computeConvexVsConcaveMiddlePhase(pairIndex, mMemoryManager.getSingleFrameAllocator(), narrowPhaseInput); + computeConvexVsConcaveMiddlePhase(mOverlappingPairs.mConcavePairs[pairIndex], mMemoryManager.getSingleFrameAllocator(), narrowPhaseInput); } } // Compute the concave vs convex middle-phase algorithm for a given pair of bodies -void CollisionDetectionSystem::computeConvexVsConcaveMiddlePhase(uint64 pairIndex, MemoryAllocator& allocator, NarrowPhaseInput& narrowPhaseInput) { +void CollisionDetectionSystem::computeConvexVsConcaveMiddlePhase(OverlappingPairs::ConcaveOverlappingPair& overlappingPair, MemoryAllocator& allocator, NarrowPhaseInput& narrowPhaseInput) { RP3D_PROFILE("CollisionDetectionSystem::computeConvexVsConcaveMiddlePhase()", mProfiler); - const Entity collider1 = mOverlappingPairs.mColliders1[pairIndex]; - const Entity collider2 = mOverlappingPairs.mColliders2[pairIndex]; + const Entity collider1 = overlappingPair.collider1; + const Entity collider2 = overlappingPair.collider2; const uint collider1Index = mCollidersComponents.getEntityIndex(collider1); const uint collider2Index = mCollidersComponents.getEntityIndex(collider2); @@ -372,8 +435,7 @@ void CollisionDetectionSystem::computeConvexVsConcaveMiddlePhase(uint64 pairInde // Collision shape 1 is convex, collision shape 2 is concave ConvexShape* convexShape; ConcaveShape* concaveShape; - const bool isShape1Convex = mOverlappingPairs.mIsShape1Convex[pairIndex]; - if (isShape1Convex) { + if (overlappingPair.isShape1Convex) { convexShape = static_cast(mCollidersComponents.mCollisionShapes[collider1Index]); concaveShape = static_cast(mCollidersComponents.mCollisionShapes[collider2Index]); convexToConcaveTransform = shape2LocalToWorldTransform.getInverse() * shape1LocalToWorldTransform; @@ -386,16 +448,16 @@ void CollisionDetectionSystem::computeConvexVsConcaveMiddlePhase(uint64 pairInde assert(convexShape->isConvex()); assert(!concaveShape->isConvex()); - assert(mOverlappingPairs.mNarrowPhaseAlgorithmType[pairIndex] != NarrowPhaseAlgorithmType::None); + assert(overlappingPair.narrowPhaseAlgorithmType != NarrowPhaseAlgorithmType::None); // Compute the convex shape AABB in the local-space of the convex shape AABB aabb; convexShape->computeAABB(aabb, convexToConcaveTransform); // Compute the concave shape triangles that are overlapping with the convex mesh AABB - List triangleVertices(allocator); - List triangleVerticesNormals(allocator); - List shapeIds(allocator); + Array triangleVertices(allocator, 64); + Array triangleVerticesNormals(allocator, 64); + Array shapeIds(allocator, 64); concaveShape->computeOverlappingTriangles(aabb, triangleVertices, triangleVerticesNormals, shapeIds, allocator); assert(triangleVertices.size() == triangleVerticesNormals.size()); @@ -407,13 +469,24 @@ void CollisionDetectionSystem::computeConvexVsConcaveMiddlePhase(uint64 pairInde const bool isCollider2Trigger = mCollidersComponents.mIsTrigger[collider2Index]; const bool reportContacts = !isCollider1Trigger && !isCollider2Trigger; + CollisionShape* shape1; + CollisionShape* shape2; + + if (overlappingPair.isShape1Convex) { + shape1 = convexShape; + } + else { + shape2 = convexShape; + } + // For each overlapping triangle - for (uint i=0; i < shapeIds.size(); i++) - { + const uint32 nbShapeIds = shapeIds.size(); + for (uint32 i=0; i < nbShapeIds; i++) { + // Create a triangle collision shape (the allocated memory for the TriangleShape will be released in the // destructor of the corresponding NarrowPhaseInfo. TriangleShape* triangleShape = new (allocator.allocate(sizeof(TriangleShape))) - TriangleShape(&(triangleVertices[i * 3]), &(triangleVerticesNormals[i * 3]), shapeIds[i], allocator); + TriangleShape(&(triangleVertices[i * 3]), &(triangleVerticesNormals[i * 3]), shapeIds[i], mTriangleHalfEdgeStructure, allocator); #ifdef IS_RP3D_PROFILING_ENABLED @@ -423,11 +496,20 @@ void CollisionDetectionSystem::computeConvexVsConcaveMiddlePhase(uint64 pairInde #endif + if (overlappingPair.isShape1Convex) { + shape2 = triangleShape; + } + else { + shape1 = triangleShape; + } + + // Add a collision info for the two collision shapes into the overlapping pair (if not present yet) + LastFrameCollisionInfo* lastFrameInfo = overlappingPair.addLastFrameInfoIfNecessary(shape1->getId(), shape2->getId()); + // Create a narrow phase info for the narrow-phase collision detection - narrowPhaseInput.addNarrowPhaseTest(mOverlappingPairs.mPairIds[pairIndex], pairIndex, collider1, collider2, isShape1Convex ? convexShape : triangleShape, - isShape1Convex ? triangleShape : convexShape, - shape1LocalToWorldTransform, shape2LocalToWorldTransform, - mOverlappingPairs.mNarrowPhaseAlgorithmType[pairIndex], reportContacts, allocator); + narrowPhaseInput.addNarrowPhaseTest(overlappingPair.pairID, collider1, collider2, shape1, shape2, + shape1LocalToWorldTransform, shape2LocalToWorldTransform, + overlappingPair.narrowPhaseAlgorithmType, reportContacts, lastFrameInfo, allocator); } } @@ -446,9 +528,9 @@ bool CollisionDetectionSystem::testNarrowPhaseCollision(NarrowPhaseInput& narrow ConvexPolyhedronVsConvexPolyhedronAlgorithm* convexPolyVsConvexPolyAlgo = mCollisionDispatch.getConvexPolyhedronVsConvexPolyhedronAlgorithm(); // get the narrow-phase batches to test for collision for contacts - SphereVsSphereNarrowPhaseInfoBatch& sphereVsSphereBatchContacts = narrowPhaseInput.getSphereVsSphereBatch(); - SphereVsCapsuleNarrowPhaseInfoBatch& sphereVsCapsuleBatchContacts = narrowPhaseInput.getSphereVsCapsuleBatch(); - CapsuleVsCapsuleNarrowPhaseInfoBatch& capsuleVsCapsuleBatchContacts = narrowPhaseInput.getCapsuleVsCapsuleBatch(); + NarrowPhaseInfoBatch& sphereVsSphereBatchContacts = narrowPhaseInput.getSphereVsSphereBatch(); + NarrowPhaseInfoBatch& sphereVsCapsuleBatchContacts = narrowPhaseInput.getSphereVsCapsuleBatch(); + NarrowPhaseInfoBatch& capsuleVsCapsuleBatchContacts = narrowPhaseInput.getCapsuleVsCapsuleBatch(); NarrowPhaseInfoBatch& sphereVsConvexPolyhedronBatchContacts = narrowPhaseInput.getSphereVsConvexPolyhedronBatch(); NarrowPhaseInfoBatch& capsuleVsConvexPolyhedronBatchContacts = narrowPhaseInput.getCapsuleVsConvexPolyhedronBatch(); NarrowPhaseInfoBatch& convexPolyhedronVsConvexPolyhedronBatchContacts = narrowPhaseInput.getConvexPolyhedronVsConvexPolyhedronBatch(); @@ -478,14 +560,13 @@ bool CollisionDetectionSystem::testNarrowPhaseCollision(NarrowPhaseInput& narrow // Process the potential contacts after narrow-phase collision detection void CollisionDetectionSystem::processAllPotentialContacts(NarrowPhaseInput& narrowPhaseInput, bool updateLastFrameInfo, - List& potentialContactPoints, - Map* mapPairIdToContactPairIndex, - List& potentialContactManifolds, - List* contactPairs, - Map>& mapBodyToContactPairs) { + Array& potentialContactPoints, + Array& potentialContactManifolds, + Array* contactPairs) { assert(contactPairs->size() == 0); - assert(mapPairIdToContactPairIndex->size() == 0); + + Map mapPairIdToContactPairIndex(mMemoryManager.getHeapAllocator(), mPreviousMapPairIdToContactPairIndex.size()); // get the narrow-phase batches to test for collision NarrowPhaseInfoBatch& sphereVsSphereBatch = narrowPhaseInput.getSphereVsSphereBatch(); @@ -496,18 +577,13 @@ void CollisionDetectionSystem::processAllPotentialContacts(NarrowPhaseInput& nar NarrowPhaseInfoBatch& convexPolyhedronVsConvexPolyhedronBatch = narrowPhaseInput.getConvexPolyhedronVsConvexPolyhedronBatch(); // Process the potential contacts - processPotentialContacts(sphereVsSphereBatch, updateLastFrameInfo, potentialContactPoints, mapPairIdToContactPairIndex, - potentialContactManifolds, contactPairs, mapBodyToContactPairs); - processPotentialContacts(sphereVsCapsuleBatch, updateLastFrameInfo, potentialContactPoints, mapPairIdToContactPairIndex, - potentialContactManifolds, contactPairs, mapBodyToContactPairs); - processPotentialContacts(capsuleVsCapsuleBatch, updateLastFrameInfo, potentialContactPoints, mapPairIdToContactPairIndex, - potentialContactManifolds, contactPairs, mapBodyToContactPairs); - processPotentialContacts(sphereVsConvexPolyhedronBatch, updateLastFrameInfo, potentialContactPoints, mapPairIdToContactPairIndex, - potentialContactManifolds, contactPairs, mapBodyToContactPairs); - processPotentialContacts(capsuleVsConvexPolyhedronBatch, updateLastFrameInfo, potentialContactPoints, mapPairIdToContactPairIndex, - potentialContactManifolds, contactPairs, mapBodyToContactPairs); - processPotentialContacts(convexPolyhedronVsConvexPolyhedronBatch, updateLastFrameInfo, potentialContactPoints, mapPairIdToContactPairIndex, - potentialContactManifolds, contactPairs, mapBodyToContactPairs); + processPotentialContacts(sphereVsSphereBatch, updateLastFrameInfo, potentialContactPoints, potentialContactManifolds, mapPairIdToContactPairIndex, contactPairs); + processPotentialContacts(sphereVsCapsuleBatch, updateLastFrameInfo, potentialContactPoints, potentialContactManifolds, mapPairIdToContactPairIndex, contactPairs); + processPotentialContacts(capsuleVsCapsuleBatch, updateLastFrameInfo, potentialContactPoints, potentialContactManifolds, mapPairIdToContactPairIndex, contactPairs); + processPotentialContacts(sphereVsConvexPolyhedronBatch, updateLastFrameInfo, potentialContactPoints, potentialContactManifolds, mapPairIdToContactPairIndex, contactPairs); + processPotentialContacts(capsuleVsConvexPolyhedronBatch, updateLastFrameInfo, potentialContactPoints, potentialContactManifolds, mapPairIdToContactPairIndex, contactPairs); + processPotentialContacts(convexPolyhedronVsConvexPolyhedronBatch, updateLastFrameInfo, potentialContactPoints, + potentialContactManifolds, mapPairIdToContactPairIndex, contactPairs); } // Compute the narrow-phase collision detection @@ -517,26 +593,65 @@ void CollisionDetectionSystem::computeNarrowPhase() { MemoryAllocator& allocator = mMemoryManager.getSingleFrameAllocator(); - // Swap the previous and current contacts lists + // Swap the previous and current contacts arrays swapPreviousAndCurrentContacts(); + mPotentialContactManifolds.reserve(mNbPreviousPotentialContactManifolds); + mPotentialContactPoints.reserve(mNbPreviousPotentialContactPoints); + // Test the narrow-phase collision detection on the batches to be tested testNarrowPhaseCollision(mNarrowPhaseInput, true, allocator); // Process all the potential contacts after narrow-phase collision - processAllPotentialContacts(mNarrowPhaseInput, true, mPotentialContactPoints, mCurrentMapPairIdToContactPairIndex, - mPotentialContactManifolds, mCurrentContactPairs, mMapBodyToContactPairs); + processAllPotentialContacts(mNarrowPhaseInput, true, mPotentialContactPoints, + mPotentialContactManifolds, mCurrentContactPairs); // Reduce the number of contact points in the manifolds reducePotentialContactManifolds(mCurrentContactPairs, mPotentialContactManifolds, mPotentialContactPoints); + // Add the contact pairs to the bodies + addContactPairsToBodies(); + assert(mCurrentContactManifolds->size() == 0); assert(mCurrentContactPoints->size() == 0); +} - // Create the actual narrow-phase contacts - createContacts(); +// Add the contact pairs to the corresponding bodies +void CollisionDetectionSystem::addContactPairsToBodies() { - mNarrowPhaseInput.clear(); + const uint32 nbContactPairs = mCurrentContactPairs->size(); + for (uint32 p=0 ; p < nbContactPairs; p++) { + + ContactPair& contactPair = (*mCurrentContactPairs)[p]; + + const bool isBody1Rigid = mRigidBodyComponents.hasComponent(contactPair.body1Entity); + const bool isBody2Rigid = mRigidBodyComponents.hasComponent(contactPair.body2Entity); + + // Add the associated contact pair to both bodies of the pair (used to create islands later) + if (isBody1Rigid) { + mRigidBodyComponents.addContacPair(contactPair.body1Entity, p); + } + if (isBody2Rigid) { + mRigidBodyComponents.addContacPair(contactPair.body2Entity, p); + } + + // If at least of body is a CollisionBody + if (!isBody1Rigid || !isBody2Rigid) { + + // Add the pair index to the array of pairs with CollisionBody + mCollisionBodyContactPairsIndices.add(p); + } + } +} + +// Compute the map from contact pairs ids to contact pair for the next frame +void CollisionDetectionSystem::computeMapPreviousContactPairs() { + + mPreviousMapPairIdToContactPairIndex.clear(); + const uint32 nbCurrentContactPairs = mCurrentContactPairs->size(); + for (uint32 i=0; i < nbCurrentContactPairs; i++) { + mPreviousMapPairIdToContactPairIndex.add(Pair((*mCurrentContactPairs)[i].pairId, i)); + } } // Compute the narrow-phase collision detection for the testOverlap() methods. @@ -552,8 +667,8 @@ bool CollisionDetectionSystem::computeNarrowPhaseOverlapSnapshot(NarrowPhaseInpu if (collisionFound && callback != nullptr) { // Compute the overlapping colliders - List contactPairs(allocator); - List lostContactPairs(allocator); // Always empty in this case (snapshot) + Array contactPairs(allocator); + Array lostContactPairs(allocator); // Always empty in this case (snapshot) computeOverlapSnapshotContactPairs(narrowPhaseInput, contactPairs); // Report overlapping colliders @@ -565,7 +680,7 @@ bool CollisionDetectionSystem::computeNarrowPhaseOverlapSnapshot(NarrowPhaseInpu } // Process the potential overlapping bodies for the testOverlap() methods -void CollisionDetectionSystem::computeOverlapSnapshotContactPairs(NarrowPhaseInput& narrowPhaseInput, List& contactPairs) const { +void CollisionDetectionSystem::computeOverlapSnapshotContactPairs(NarrowPhaseInput& narrowPhaseInput, Array& contactPairs) const { Set setOverlapContactPairId(mMemoryManager.getHeapAllocator()); @@ -590,9 +705,10 @@ void CollisionDetectionSystem::computeOverlapSnapshotContactPairs(NarrowPhaseInp void CollisionDetectionSystem::notifyOverlappingPairsToTestOverlap(Collider* collider) { // Get the overlapping pairs involved with this collider - List& overlappingPairs = mCollidersComponents.getOverlappingPairs(collider->getEntity()); + Array& overlappingPairs = mCollidersComponents.getOverlappingPairs(collider->getEntity()); - for (uint i=0; i < overlappingPairs.size(); i++) { + const uint32 nbPairs = overlappingPairs.size(); + for (uint32 i=0; i < nbPairs; i++) { // Notify that the overlapping pair needs to be testbed for overlap mOverlappingPairs.setNeedToTestOverlap(overlappingPairs[i], true); @@ -600,7 +716,7 @@ void CollisionDetectionSystem::notifyOverlappingPairsToTestOverlap(Collider* col } // Convert the potential overlapping bodies for the testOverlap() methods -void CollisionDetectionSystem::computeOverlapSnapshotContactPairs(NarrowPhaseInfoBatch& narrowPhaseInfoBatch, List& contactPairs, +void CollisionDetectionSystem::computeOverlapSnapshotContactPairs(NarrowPhaseInfoBatch& narrowPhaseInfoBatch, Array& contactPairs, Set& setOverlapContactPairId) const { RP3D_PROFILE("CollisionDetectionSystem::computeSnapshotContactPairs()", mProfiler); @@ -609,13 +725,13 @@ void CollisionDetectionSystem::computeOverlapSnapshotContactPairs(NarrowPhaseInf for(uint i=0; i < narrowPhaseInfoBatch.getNbObjects(); i++) { // If there is a collision - if (narrowPhaseInfoBatch.isColliding[i]) { + if (narrowPhaseInfoBatch.narrowPhaseInfos[i].isColliding) { // If the contact pair does not already exist - if (!setOverlapContactPairId.contains(narrowPhaseInfoBatch.overlappingPairIds[i])) { + if (!setOverlapContactPairId.contains(narrowPhaseInfoBatch.narrowPhaseInfos[i].overlappingPairId)) { - const Entity collider1Entity = narrowPhaseInfoBatch.colliderEntities1[i]; - const Entity collider2Entity = narrowPhaseInfoBatch.colliderEntities2[i]; + const Entity collider1Entity = narrowPhaseInfoBatch.narrowPhaseInfos[i].colliderEntity1; + const Entity collider2Entity = narrowPhaseInfoBatch.narrowPhaseInfos[i].colliderEntity2; const uint32 collider1Index = mCollidersComponents.getEntityIndex(collider1Entity); const uint32 collider2Index = mCollidersComponents.getEntityIndex(collider2Entity); @@ -626,11 +742,10 @@ void CollisionDetectionSystem::computeOverlapSnapshotContactPairs(NarrowPhaseInf const bool isTrigger = mCollidersComponents.mIsTrigger[collider1Index] || mCollidersComponents.mIsTrigger[collider2Index]; // Create a new contact pair - ContactPair contactPair(narrowPhaseInfoBatch.overlappingPairIds[i], body1Entity, body2Entity, collider1Entity, collider2Entity, - contactPairs.size(), isTrigger, false, mMemoryManager.getHeapAllocator()); + ContactPair contactPair(narrowPhaseInfoBatch.narrowPhaseInfos[i].overlappingPairId, body1Entity, body2Entity, collider1Entity, collider2Entity, contactPairs.size(), isTrigger, false); contactPairs.add(contactPair); - setOverlapContactPairId.add(narrowPhaseInfoBatch.overlappingPairIds[i]); + setOverlapContactPairId.add(narrowPhaseInfoBatch.narrowPhaseInfos[i].overlappingPairId); } } @@ -652,25 +767,21 @@ bool CollisionDetectionSystem::computeNarrowPhaseCollisionSnapshot(NarrowPhaseIn // If collision has been found, create contacts if (collisionFound) { - List potentialContactPoints(allocator); - List potentialContactManifolds(allocator); - Map mapPairIdToContactPairIndex(allocator); - List contactPairs(allocator); - List lostContactPairs(allocator); // Not used during collision snapshots - List contactManifolds(allocator); - List contactPoints(allocator); - Map> mapBodyToContactPairs(allocator); + Array potentialContactPoints(allocator); + Array potentialContactManifolds(allocator); + Array contactPairs(allocator); + Array lostContactPairs(allocator); // Not used during collision snapshots + Array contactManifolds(allocator); + Array contactPoints(allocator); // Process all the potential contacts after narrow-phase collision - processAllPotentialContacts(narrowPhaseInput, true, potentialContactPoints, &mapPairIdToContactPairIndex, potentialContactManifolds, - &contactPairs, mapBodyToContactPairs); + processAllPotentialContacts(narrowPhaseInput, true, potentialContactPoints, potentialContactManifolds, &contactPairs); // Reduce the number of contact points in the manifolds reducePotentialContactManifolds(&contactPairs, potentialContactManifolds, potentialContactPoints); // Create the actual contact manifolds and contact points - createSnapshotContacts(contactPairs, contactManifolds, contactPoints, potentialContactManifolds, - potentialContactPoints); + createSnapshotContacts(contactPairs, contactManifolds, contactPoints, potentialContactManifolds, potentialContactPoints); // Report the contacts to the user reportContacts(callback, &contactPairs, &contactManifolds, &contactPoints, lostContactPairs); @@ -679,7 +790,7 @@ bool CollisionDetectionSystem::computeNarrowPhaseCollisionSnapshot(NarrowPhaseIn return collisionFound; } -// Swap the previous and current contacts lists +// Swap the previous and current contacts arrays void CollisionDetectionSystem::swapPreviousAndCurrentContacts() { if (mPreviousContactPairs == &mContactPairs1) { @@ -687,24 +798,20 @@ void CollisionDetectionSystem::swapPreviousAndCurrentContacts() { mPreviousContactPairs = &mContactPairs2; mPreviousContactManifolds = &mContactManifolds2; mPreviousContactPoints = &mContactPoints2; - mPreviousMapPairIdToContactPairIndex = &mMapPairIdToContactPairIndex2; mCurrentContactPairs = &mContactPairs1; mCurrentContactManifolds = &mContactManifolds1; mCurrentContactPoints = &mContactPoints1; - mCurrentMapPairIdToContactPairIndex = &mMapPairIdToContactPairIndex1; } else { mPreviousContactPairs = &mContactPairs1; mPreviousContactManifolds = &mContactManifolds1; mPreviousContactPoints = &mContactPoints1; - mPreviousMapPairIdToContactPairIndex = &mMapPairIdToContactPairIndex1; mCurrentContactPairs = &mContactPairs2; mCurrentContactManifolds = &mContactManifolds2; mCurrentContactPoints = &mContactPoints2; - mCurrentMapPairIdToContactPairIndex = &mMapPairIdToContactPairIndex2; } } @@ -716,44 +823,49 @@ void CollisionDetectionSystem::createContacts() { mCurrentContactManifolds->reserve(mCurrentContactPairs->size()); mCurrentContactPoints->reserve(mCurrentContactManifolds->size()); - // For each contact pair - for (uint p=0; p < (*mCurrentContactPairs).size(); p++) { + // We go through all the contact pairs and add the pairs with a least a CollisionBody at the end of the + // mProcessContactPairsOrderIslands array because those pairs have not been added during the islands + // creation (only the pairs with two RigidBodies are added during island creation) + mWorld->mProcessContactPairsOrderIslands.addRange(mCollisionBodyContactPairsIndices); - ContactPair& contactPair = (*mCurrentContactPairs)[p]; + assert(mWorld->mProcessContactPairsOrderIslands.size() == (*mCurrentContactPairs).size()); + + // Process the contact pairs in the order defined by the islands such that the contact manifolds and + // contact points of a given island are packed together in the array of manifolds and contact points + const uint32 nbContactPairsToProcess = mWorld->mProcessContactPairsOrderIslands.size(); + for (uint p=0; p < nbContactPairsToProcess; p++) { + + uint32 contactPairIndex = mWorld->mProcessContactPairsOrderIslands[p]; + + ContactPair& contactPair = (*mCurrentContactPairs)[contactPairIndex]; contactPair.contactManifoldsIndex = mCurrentContactManifolds->size(); - contactPair.nbContactManifolds = contactPair.potentialContactManifoldsIndices.size(); + contactPair.nbContactManifolds = contactPair.nbPotentialContactManifolds; contactPair.contactPointsIndex = mCurrentContactPoints->size(); // For each potential contact manifold of the pair - for (uint m=0; m < contactPair.potentialContactManifoldsIndices.size(); m++) { + for (uint m=0; m < contactPair.nbPotentialContactManifolds; m++) { ContactManifoldInfo& potentialManifold = mPotentialContactManifolds[contactPair.potentialContactManifoldsIndices[m]]; // Start index and number of contact points for this manifold - const uint contactPointsIndex = mCurrentContactPoints->size(); - const int8 nbContactPoints = static_cast(potentialManifold.potentialContactPointsIndices.size()); + const uint32 contactPointsIndex = mCurrentContactPoints->size(); + const int8 nbContactPoints = static_cast(potentialManifold.nbPotentialContactPoints); contactPair.nbToTalContactPoints += nbContactPoints; - // We create a new contact manifold - ContactManifold contactManifold(contactPair.body1Entity, contactPair.body2Entity, contactPair.collider1Entity, - contactPair.collider2Entity, contactPointsIndex, nbContactPoints); + // Create and add the contact manifold + mCurrentContactManifolds->emplace(contactPair.body1Entity, contactPair.body2Entity, contactPair.collider1Entity, + contactPair.collider2Entity, contactPointsIndex, nbContactPoints); - // Add the contact manifold - mCurrentContactManifolds->add(contactManifold); - - assert(potentialManifold.potentialContactPointsIndices.size() > 0); + assert(potentialManifold.nbPotentialContactPoints > 0); // For each contact point of the manifold - for (uint c=0; c < potentialManifold.potentialContactPointsIndices.size(); c++) { + for (uint c=0; c < potentialManifold.nbPotentialContactPoints; c++) { ContactPointInfo& potentialContactPoint = mPotentialContactPoints[potentialManifold.potentialContactPointsIndices[c]]; - // Create a new contact point - ContactPoint contactPoint(potentialContactPoint, mWorld->mConfig.persistentContactDistanceThreshold); - - // Add the contact point - mCurrentContactPoints->add(contactPoint); + // Create and add the contact point + mCurrentContactPoints->emplace(potentialContactPoint, mWorld->mConfig.persistentContactDistanceThreshold); } } } @@ -767,38 +879,64 @@ void CollisionDetectionSystem::createContacts() { mPreviousContactPoints->clear(); mPreviousContactManifolds->clear(); mPreviousContactPairs->clear(); - mPreviousMapPairIdToContactPairIndex->clear(); + + mNbPreviousPotentialContactManifolds = mPotentialContactManifolds.capacity(); + mNbPreviousPotentialContactPoints = mPotentialContactPoints.capacity(); // Reset the potential contacts mPotentialContactPoints.clear(true); mPotentialContactManifolds.clear(true); + + // Compute the map from contact pairs ids to contact pair for the next frame + computeMapPreviousContactPairs(); + + mCollisionBodyContactPairsIndices.clear(true); + + mNarrowPhaseInput.clear(); } // Compute the lost contact pairs (contact pairs in contact in the previous frame but not in the current one) void CollisionDetectionSystem::computeLostContactPairs() { - // For each overlapping pair - for (uint i=0; i < mOverlappingPairs.getNbPairs(); i++) { + // For each convex pair + const uint32 nbConvexPairs = mOverlappingPairs.mConvexPairs.size(); + for (uint32 i=0; i < nbConvexPairs; i++) { // If the two colliders of the pair were colliding in the previous frame but not in the current one - if (mOverlappingPairs.mCollidingInPreviousFrame[i] && !mOverlappingPairs.mCollidingInCurrentFrame[i]) { + if (mOverlappingPairs.mConvexPairs[i].collidingInPreviousFrame && !mOverlappingPairs.mConvexPairs[i].collidingInCurrentFrame) { // If both bodies still exist - if (mCollidersComponents.hasComponent(mOverlappingPairs.mColliders1[i]) && mCollidersComponents.hasComponent(mOverlappingPairs.mColliders2[i])) { + if (mCollidersComponents.hasComponent(mOverlappingPairs.mConvexPairs[i].collider1) && mCollidersComponents.hasComponent(mOverlappingPairs.mConvexPairs[i].collider2)) { // Create a lost contact pair - addLostContactPair(i); + addLostContactPair(mOverlappingPairs.mConvexPairs[i]); + } + } + } + + // For each convex pair + const uint32 nbConcavePairs = mOverlappingPairs.mConcavePairs.size(); + for (uint32 i=0; i < nbConcavePairs; i++) { + + // If the two colliders of the pair were colliding in the previous frame but not in the current one + if (mOverlappingPairs.mConcavePairs[i].collidingInPreviousFrame && !mOverlappingPairs.mConcavePairs[i].collidingInCurrentFrame) { + + // If both bodies still exist + if (mCollidersComponents.hasComponent(mOverlappingPairs.mConcavePairs[i].collider1) && mCollidersComponents.hasComponent(mOverlappingPairs.mConcavePairs[i].collider2)) { + + // Create a lost contact pair + addLostContactPair(mOverlappingPairs.mConcavePairs[i]); } } } } // Create the actual contact manifolds and contacts points for testCollision() methods -void CollisionDetectionSystem::createSnapshotContacts(List& contactPairs, - List& contactManifolds, - List& contactPoints, - List& potentialContactManifolds, - List& potentialContactPoints) { +void CollisionDetectionSystem::createSnapshotContacts(Array& contactPairs, + Array& contactManifolds, + Array& contactPoints, + Array& potentialContactManifolds, + Array& potentialContactPoints) { RP3D_PROFILE("CollisionDetectionSystem::createSnapshotContacts()", mProfiler); @@ -806,36 +944,34 @@ void CollisionDetectionSystem::createSnapshotContacts(List& contact contactPoints.reserve(contactManifolds.size()); // For each contact pair - for (uint p=0; p < contactPairs.size(); p++) { + const uint32 nbContactPairs = contactPairs.size(); + for (uint32 p=0; p < nbContactPairs; p++) { ContactPair& contactPair = contactPairs[p]; - assert(contactPair.potentialContactManifoldsIndices.size() > 0); + assert(contactPair.nbPotentialContactManifolds > 0); contactPair.contactManifoldsIndex = contactManifolds.size(); - contactPair.nbContactManifolds = contactPair.potentialContactManifoldsIndices.size(); + contactPair.nbContactManifolds = contactPair.nbPotentialContactManifolds; contactPair.contactPointsIndex = contactPoints.size(); // For each potential contact manifold of the pair - for (uint m=0; m < contactPair.potentialContactManifoldsIndices.size(); m++) { + for (uint32 m=0; m < contactPair.nbPotentialContactManifolds; m++) { ContactManifoldInfo& potentialManifold = potentialContactManifolds[contactPair.potentialContactManifoldsIndices[m]]; // Start index and number of contact points for this manifold - const uint contactPointsIndex = contactPoints.size(); - const int8 nbContactPoints = static_cast(potentialManifold.potentialContactPointsIndices.size()); + const uint32 contactPointsIndex = contactPoints.size(); + const uint8 nbContactPoints = potentialManifold.nbPotentialContactPoints; contactPair.nbToTalContactPoints += nbContactPoints; - // We create a new contact manifold - ContactManifold contactManifold(contactPair.body1Entity, contactPair.body2Entity, contactPair.collider1Entity, - contactPair.collider2Entity, contactPointsIndex, nbContactPoints); + // Create and add the contact manifold + contactManifolds.emplace(contactPair.body1Entity, contactPair.body2Entity, contactPair.collider1Entity, + contactPair.collider2Entity, contactPointsIndex, nbContactPoints); - // Add the contact manifold - contactManifolds.add(contactManifold); - - assert(potentialManifold.potentialContactPointsIndices.size() > 0); + assert(potentialManifold.nbPotentialContactPoints > 0); // For each contact point of the manifold - for (uint c=0; c < potentialManifold.potentialContactPointsIndices.size(); c++) { + for (uint32 c=0; c < potentialManifold.nbPotentialContactPoints; c++) { ContactPointInfo& potentialContactPoint = potentialContactPoints[potentialManifold.potentialContactPointsIndices[c]]; @@ -852,16 +988,19 @@ void CollisionDetectionSystem::createSnapshotContacts(List& contact // Initialize the current contacts with the contacts from the previous frame (for warmstarting) void CollisionDetectionSystem::initContactsWithPreviousOnes() { + const decimal persistentContactDistThresholdSqr = mWorld->mConfig.persistentContactDistanceThreshold * mWorld->mConfig.persistentContactDistanceThreshold; + // For each contact pair of the current frame - for (uint i=0; i < mCurrentContactPairs->size(); i++) { + const uint32 nbCurrentContactPairs = mCurrentContactPairs->size(); + for (uint32 i=0; i < nbCurrentContactPairs; i++) { ContactPair& currentContactPair = (*mCurrentContactPairs)[i]; // Find the corresponding contact pair in the previous frame (if any) - auto itPrevContactPair = mPreviousMapPairIdToContactPairIndex->find(currentContactPair.pairId); + auto itPrevContactPair = mPreviousMapPairIdToContactPairIndex.find(currentContactPair.pairId); // If we have found a corresponding contact pair in the previous frame - if (itPrevContactPair != mPreviousMapPairIdToContactPairIndex->end()) { + if (itPrevContactPair != mPreviousMapPairIdToContactPairIndex.end()) { const uint previousContactPairIndex = itPrevContactPair->second; ContactPair& previousContactPair = (*mPreviousContactPairs)[previousContactPairIndex]; @@ -898,7 +1037,6 @@ void CollisionDetectionSystem::initContactsWithPreviousOnes() { currentContactManifold.frictionImpulse1 = previousContactManifold.frictionImpulse1; currentContactManifold.frictionImpulse2 = previousContactManifold.frictionImpulse2; currentContactManifold.frictionTwistImpulse = previousContactManifold.frictionTwistImpulse; - currentContactManifold.rollingResistanceImpulse = previousContactManifold.rollingResistanceImpulse; break; } @@ -916,16 +1054,18 @@ void CollisionDetectionSystem::initContactsWithPreviousOnes() { assert(c < mCurrentContactPoints->size()); ContactPoint& currentContactPoint = (*mCurrentContactPoints)[c]; + const Vector3& currentContactPointLocalShape1 = currentContactPoint.getLocalPointOnShape1(); + // Find a similar contact point among the contact points from the previous frame (for warmstarting) const uint previousContactPointsIndex = previousContactPair.contactPointsIndex; const uint previousNbContactPoints = previousContactPair.nbToTalContactPoints; for (uint p=previousContactPointsIndex; p < previousContactPointsIndex + previousNbContactPoints; p++) { - ContactPoint& previousContactPoint = (*mPreviousContactPoints)[p]; + const ContactPoint& previousContactPoint = (*mPreviousContactPoints)[p]; // If the previous contact point is very close to th current one - const decimal distSquare = (currentContactPoint.getLocalPointOnShape1() - previousContactPoint.getLocalPointOnShape1()).lengthSquare(); - if (distSquare <= mWorld->mConfig.persistentContactDistanceThreshold * mWorld->mConfig.persistentContactDistanceThreshold) { + const decimal distSquare = (currentContactPointLocalShape1 - previousContactPoint.getLocalPointOnShape1()).lengthSquare(); + if (distSquare <= persistentContactDistThresholdSqr) { // Transfer data from the previous contact point to the current one currentContactPoint.setPenetrationImpulse(previousContactPoint.getPenetrationImpulse()); @@ -948,10 +1088,10 @@ void CollisionDetectionSystem::removeCollider(Collider* collider) { assert(mMapBroadPhaseIdToColliderEntity.containsKey(colliderBroadPhaseId)); // Remove all the overlapping pairs involving this collider - List& overlappingPairs = mCollidersComponents.getOverlappingPairs(collider->getEntity()); + Array& overlappingPairs = mCollidersComponents.getOverlappingPairs(collider->getEntity()); while(overlappingPairs.size() > 0) { - // TODO : Remove all the contact manifold of the overlapping pair from the contact manifolds list of the two bodies involved + // TODO : Remove all the contact manifold of the overlapping pair from the contact manifolds array of the two bodies involved // Remove the overlapping pair mOverlappingPairs.removePair(overlappingPairs[0]); @@ -964,9 +1104,7 @@ void CollisionDetectionSystem::removeCollider(Collider* collider) { } // Ray casting method -void CollisionDetectionSystem::raycast(RaycastCallback* raycastCallback, - const Ray& ray, - unsigned short raycastWithCategoryMaskBits) const { +void CollisionDetectionSystem::raycast(RaycastCallback* raycastCallback, const Ray& ray, unsigned short raycastWithCategoryMaskBits) const { RP3D_PROFILE("CollisionDetectionSystem::raycast()", mProfiler); @@ -979,145 +1117,183 @@ void CollisionDetectionSystem::raycast(RaycastCallback* raycastCallback, // Convert the potential contact into actual contacts void CollisionDetectionSystem::processPotentialContacts(NarrowPhaseInfoBatch& narrowPhaseInfoBatch, bool updateLastFrameInfo, - List& potentialContactPoints, - Map* mapPairIdToContactPairIndex, - List& potentialContactManifolds, - List* contactPairs, - Map>& mapBodyToContactPairs) { + Array& potentialContactPoints, + Array& potentialContactManifolds, + Map& mapPairIdToContactPairIndex, + Array* contactPairs) { RP3D_PROFILE("CollisionDetectionSystem::processPotentialContacts()", mProfiler); - // For each narrow phase info object - for(uint i=0; i < narrowPhaseInfoBatch.getNbObjects(); i++) { + const uint nbObjects = narrowPhaseInfoBatch.getNbObjects(); - if (updateLastFrameInfo) { - narrowPhaseInfoBatch.lastFrameCollisionInfos[i]->wasColliding = narrowPhaseInfoBatch.isColliding[i]; + if (updateLastFrameInfo) { + + // For each narrow phase info object + for(uint i=0; i < nbObjects; i++) { + + narrowPhaseInfoBatch.narrowPhaseInfos[i].lastFrameCollisionInfo->wasColliding = narrowPhaseInfoBatch.narrowPhaseInfos[i].isColliding; // The previous frame collision info is now valid - narrowPhaseInfoBatch.lastFrameCollisionInfos[i]->isValid = true; + narrowPhaseInfoBatch.narrowPhaseInfos[i].lastFrameCollisionInfo->isValid = true; } + } - const uint64 pairId = narrowPhaseInfoBatch.overlappingPairIds[i]; - const uint64 pairIndex = mOverlappingPairs.mMapPairIdToPairIndex[pairId]; + // For each narrow phase info object + for(uint i=0; i < nbObjects; i++) { // If the two colliders are colliding - if (narrowPhaseInfoBatch.isColliding[i]) { + if (narrowPhaseInfoBatch.narrowPhaseInfos[i].isColliding) { - mOverlappingPairs.mCollidingInCurrentFrame[pairIndex] = true; + const uint64 pairId = narrowPhaseInfoBatch.narrowPhaseInfos[i].overlappingPairId; + OverlappingPairs::OverlappingPair* overlappingPair = mOverlappingPairs.getOverlappingPair(pairId); + assert(overlappingPair != nullptr); - // If there is not already a contact pair for this overlapping pair - auto it = mapPairIdToContactPairIndex->find(pairId); - ContactPair* pairContact = nullptr; - if (it == mapPairIdToContactPairIndex->end()) { + overlappingPair->collidingInCurrentFrame = true; + + const Entity collider1Entity = narrowPhaseInfoBatch.narrowPhaseInfos[i].colliderEntity1; + const Entity collider2Entity = narrowPhaseInfoBatch.narrowPhaseInfos[i].colliderEntity2; + + const uint32 collider1Index = mCollidersComponents.getEntityIndex(collider1Entity); + const uint32 collider2Index = mCollidersComponents.getEntityIndex(collider2Entity); + + const Entity body1Entity = mCollidersComponents.mBodiesEntities[collider1Index]; + const Entity body2Entity = mCollidersComponents.mBodiesEntities[collider2Index]; + + // If we have a convex vs convex collision (if we consider the base collision shapes of the colliders) + if (mCollidersComponents.mCollisionShapes[collider1Index]->isConvex() && mCollidersComponents.mCollisionShapes[collider2Index]->isConvex()) { // Create a new ContactPair - const Entity collider1Entity = narrowPhaseInfoBatch.colliderEntities1[i]; - const Entity collider2Entity = narrowPhaseInfoBatch.colliderEntities2[i]; - - const uint32 collider1Index = mCollidersComponents.getEntityIndex(collider1Entity); - const uint32 collider2Index = mCollidersComponents.getEntityIndex(collider2Entity); - - const Entity body1Entity = mCollidersComponents.mBodiesEntities[collider1Index]; - const Entity body2Entity = mCollidersComponents.mBodiesEntities[collider2Index]; - const bool isTrigger = mCollidersComponents.mIsTrigger[collider1Index] || mCollidersComponents.mIsTrigger[collider2Index]; assert(!mWorld->mCollisionBodyComponents.getIsEntityDisabled(body1Entity) || !mWorld->mCollisionBodyComponents.getIsEntityDisabled(body2Entity)); - const uint newContactPairIndex = contactPairs->size(); - ContactPair overlappingPairContact(pairId, body1Entity, body2Entity, - collider1Entity, collider2Entity, - newContactPairIndex, mOverlappingPairs.getCollidingInPreviousFrame(pairId), isTrigger, mMemoryManager.getHeapAllocator()); - contactPairs->add(overlappingPairContact); - pairContact = &((*contactPairs)[newContactPairIndex]); - mapPairIdToContactPairIndex->add(Pair(pairId, newContactPairIndex)); + const uint32 newContactPairIndex = contactPairs->size(); - auto itbodyContactPairs = mapBodyToContactPairs.find(body1Entity); - if (itbodyContactPairs != mapBodyToContactPairs.end()) { - itbodyContactPairs->second.add(newContactPairIndex); - } - else { - List contactPairs(mMemoryManager.getPoolAllocator(), 1); - contactPairs.add(newContactPairIndex); - mapBodyToContactPairs.add(Pair>(body1Entity, contactPairs)); - } - itbodyContactPairs = mapBodyToContactPairs.find(body2Entity); - if (itbodyContactPairs != mapBodyToContactPairs.end()) { - itbodyContactPairs->second.add(newContactPairIndex); - } - else { - List contactPairs(mMemoryManager.getPoolAllocator(), 1); - contactPairs.add(newContactPairIndex); - mapBodyToContactPairs.add(Pair>(body2Entity, contactPairs)); - } - } - else { // If a ContactPair already exists for this overlapping pair, we use this one + contactPairs->emplace(pairId, body1Entity, body2Entity, collider1Entity, collider2Entity, + newContactPairIndex, overlappingPair->collidingInPreviousFrame, isTrigger); - assert(it->first == pairId); + ContactPair* pairContact = &((*contactPairs)[newContactPairIndex]); - const uint pairContactIndex = it->second; - pairContact = &((*contactPairs)[pairContactIndex]); - } + // Create a new potential contact manifold for the overlapping pair + uint32 contactManifoldIndex = static_cast(potentialContactManifolds.size()); + potentialContactManifolds.emplace(pairId); + ContactManifoldInfo& contactManifoldInfo = potentialContactManifolds[contactManifoldIndex]; - assert(pairContact != nullptr); + const uint32 contactPointIndexStart = static_cast(potentialContactPoints.size()); - // Add the potential contacts - for (uint j=0; j < narrowPhaseInfoBatch.contactPoints[i].size(); j++) { + // Add the potential contacts + for (uint j=0; j < narrowPhaseInfoBatch.narrowPhaseInfos[i].nbContactPoints; j++) { - const ContactPointInfo& contactPoint = *(narrowPhaseInfoBatch.contactPoints[i][j]); - - // Add the contact point to the list of potential contact points - const uint contactPointIndex = static_cast(potentialContactPoints.size()); - - potentialContactPoints.add(contactPoint); - - bool similarManifoldFound = false; - - // For each contact manifold of the overlapping pair - for (uint m=0; m < pairContact->potentialContactManifoldsIndices.size(); m++) { - - uint contactManifoldIndex = pairContact->potentialContactManifoldsIndices[m]; - - // Get the first contact point of the current manifold - assert(potentialContactManifolds[contactManifoldIndex].potentialContactPointsIndices.size() > 0); - const uint manifoldContactPointIndex = potentialContactManifolds[contactManifoldIndex].potentialContactPointsIndices[0]; - const ContactPointInfo& manifoldContactPoint = potentialContactPoints[manifoldContactPointIndex]; - - // If we have found a corresponding manifold for the new contact point - // (a manifold with a similar contact normal direction) - if (manifoldContactPoint.normal.dot(contactPoint.normal) >= mWorld->mConfig.cosAngleSimilarContactManifold) { + if (contactManifoldInfo.nbPotentialContactPoints < NB_MAX_CONTACT_POINTS_IN_POTENTIAL_MANIFOLD) { // Add the contact point to the manifold - potentialContactManifolds[contactManifoldIndex].potentialContactPointsIndices.add(contactPointIndex); + contactManifoldInfo.potentialContactPointsIndices[contactManifoldInfo.nbPotentialContactPoints] = contactPointIndexStart + j; + contactManifoldInfo.nbPotentialContactPoints++; - similarManifoldFound = true; + // Add the contact point to the array of potential contact points + const ContactPointInfo& contactPoint = narrowPhaseInfoBatch.narrowPhaseInfos[i].contactPoints[j]; - break; + potentialContactPoints.add(contactPoint); } } - // If we have not found a manifold with a similar contact normal for the contact point - if (!similarManifoldFound) { + // Add the contact manifold to the overlapping pair contact + assert(pairId == contactManifoldInfo.pairId); + pairContact->potentialContactManifoldsIndices[0] = contactManifoldIndex; + pairContact->nbPotentialContactManifolds = 1; + } + else { - // Create a new contact manifold for the overlapping pair - ContactManifoldInfo contactManifoldInfo(pairId, mMemoryManager.getPoolAllocator()); + // If there is not already a contact pair for this overlapping pair + auto it = mapPairIdToContactPairIndex.find(pairId); + ContactPair* pairContact = nullptr; + if (it == mapPairIdToContactPairIndex.end()) { - // Add the contact point to the manifold - contactManifoldInfo.potentialContactPointsIndices.add(contactPointIndex); + // Create a new ContactPair - assert(pairContact != nullptr); + const bool isTrigger = mCollidersComponents.mIsTrigger[collider1Index] || mCollidersComponents.mIsTrigger[collider2Index]; - // Add the potential contact manifold - uint contactManifoldIndex = static_cast(potentialContactManifolds.size()); - potentialContactManifolds.add(contactManifoldInfo); + assert(!mWorld->mCollisionBodyComponents.getIsEntityDisabled(body1Entity) || !mWorld->mCollisionBodyComponents.getIsEntityDisabled(body2Entity)); - // Add the contact manifold to the overlapping pair contact - assert(pairContact->pairId == contactManifoldInfo.pairId); - pairContact->potentialContactManifoldsIndices.add(contactManifoldIndex); + const uint32 newContactPairIndex = contactPairs->size(); + contactPairs->emplace(pairId, body1Entity, body2Entity, collider1Entity, collider2Entity, + newContactPairIndex, overlappingPair->collidingInPreviousFrame , isTrigger); + pairContact = &((*contactPairs)[newContactPairIndex]); + mapPairIdToContactPairIndex.add(Pair(pairId, newContactPairIndex)); + + } + else { // If a ContactPair already exists for this overlapping pair, we use this one + + assert(it->first == pairId); + + const uint pairContactIndex = it->second; + pairContact = &((*contactPairs)[pairContactIndex]); } - assert(pairContact->potentialContactManifoldsIndices.size() > 0); + assert(pairContact != nullptr); + + // Add the potential contacts + for (uint j=0; j < narrowPhaseInfoBatch.narrowPhaseInfos[i].nbContactPoints; j++) { + + const ContactPointInfo& contactPoint = narrowPhaseInfoBatch.narrowPhaseInfos[i].contactPoints[j]; + + // Add the contact point to the array of potential contact points + const uint32 contactPointIndex = potentialContactPoints.size(); + + potentialContactPoints.add(contactPoint); + + bool similarManifoldFound = false; + + // For each contact manifold of the overlapping pair + for (uint m=0; m < pairContact->nbPotentialContactManifolds; m++) { + + uint contactManifoldIndex = pairContact->potentialContactManifoldsIndices[m]; + + if (potentialContactManifolds[contactManifoldIndex].nbPotentialContactPoints < NB_MAX_CONTACT_POINTS_IN_POTENTIAL_MANIFOLD) { + + // Get the first contact point of the current manifold + assert(potentialContactManifolds[contactManifoldIndex].nbPotentialContactPoints > 0); + const uint manifoldContactPointIndex = potentialContactManifolds[contactManifoldIndex].potentialContactPointsIndices[0]; + const ContactPointInfo& manifoldContactPoint = potentialContactPoints[manifoldContactPointIndex]; + + // If we have found a corresponding manifold for the new contact point + // (a manifold with a similar contact normal direction) + if (manifoldContactPoint.normal.dot(contactPoint.normal) >= mWorld->mConfig.cosAngleSimilarContactManifold) { + + // Add the contact point to the manifold + potentialContactManifolds[contactManifoldIndex].potentialContactPointsIndices[potentialContactManifolds[contactManifoldIndex].nbPotentialContactPoints] = contactPointIndex; + potentialContactManifolds[contactManifoldIndex].nbPotentialContactPoints++; + + similarManifoldFound = true; + + break; + } + } + } + + // If we have not found a manifold with a similar contact normal for the contact point + if (!similarManifoldFound && pairContact->nbPotentialContactManifolds < NB_MAX_POTENTIAL_CONTACT_MANIFOLDS) { + + // Create a new potential contact manifold for the overlapping pair + uint32 contactManifoldIndex = potentialContactManifolds.size(); + potentialContactManifolds.emplace(pairId); + ContactManifoldInfo& contactManifoldInfo = potentialContactManifolds[contactManifoldIndex]; + + // Add the contact point to the manifold + contactManifoldInfo.potentialContactPointsIndices[0] = contactPointIndex; + contactManifoldInfo.nbPotentialContactPoints = 1; + + assert(pairContact != nullptr); + + // Add the contact manifold to the overlapping pair contact + assert(pairContact->pairId == contactManifoldInfo.pairId); + pairContact->potentialContactManifoldsIndices[pairContact->nbPotentialContactManifolds] = contactManifoldIndex; + pairContact->nbPotentialContactManifolds++; + } + + assert(pairContact->nbPotentialContactManifolds > 0); + } } narrowPhaseInfoBatch.resetContactPoints(i); @@ -1126,24 +1302,25 @@ void CollisionDetectionSystem::processPotentialContacts(NarrowPhaseInfoBatch& na } // Clear the obsolete manifolds and contact points and reduce the number of contacts points of the remaining manifolds -void CollisionDetectionSystem::reducePotentialContactManifolds(List* contactPairs, - List& potentialContactManifolds, - const List& potentialContactPoints) const { +void CollisionDetectionSystem::reducePotentialContactManifolds(Array* contactPairs, + Array& potentialContactManifolds, + const Array& potentialContactPoints) const { RP3D_PROFILE("CollisionDetectionSystem::reducePotentialContactManifolds()", mProfiler); // Reduce the number of potential contact manifolds in a contact pair - for (uint i=0; i < contactPairs->size(); i++) { + const uint32 nbContactPairs = contactPairs->size(); + for (uint32 i=0; i < nbContactPairs; i++) { ContactPair& contactPair = (*contactPairs)[i]; // While there are too many manifolds in the contact pair - while(contactPair.potentialContactManifoldsIndices.size() > mWorld->mConfig.nbMaxContactManifolds) { + while(contactPair.nbPotentialContactManifolds > NB_MAX_CONTACT_MANIFOLDS) { // Look for a manifold with the smallest contact penetration depth. decimal minDepth = DECIMAL_LARGEST; int minDepthManifoldIndex = -1; - for (uint j=0; j < contactPair.potentialContactManifoldsIndices.size(); j++) { + for (uint32 j=0; j < contactPair.nbPotentialContactManifolds; j++) { ContactManifoldInfo& manifold = potentialContactManifolds[contactPair.potentialContactManifoldsIndices[j]]; @@ -1158,45 +1335,43 @@ void CollisionDetectionSystem::reducePotentialContactManifolds(List // Remove the non optimal manifold assert(minDepthManifoldIndex >= 0); - contactPair.potentialContactManifoldsIndices.removeAt(minDepthManifoldIndex); + contactPair.removePotentialManifoldAtIndex(minDepthManifoldIndex); } } // Reduce the number of potential contact points in the manifolds - for (uint i=0; i < contactPairs->size(); i++) { + for (uint32 i=0; i < nbContactPairs; i++) { const ContactPair& pairContact = (*contactPairs)[i]; // For each potential contact manifold - for (uint j=0; j < pairContact.potentialContactManifoldsIndices.size(); j++) { + for (uint32 j=0; j < pairContact.nbPotentialContactManifolds; j++) { ContactManifoldInfo& manifold = potentialContactManifolds[pairContact.potentialContactManifoldsIndices[j]]; // If there are two many contact points in the manifold - if (manifold.potentialContactPointsIndices.size() > MAX_CONTACT_POINTS_IN_MANIFOLD) { + if (manifold.nbPotentialContactPoints > MAX_CONTACT_POINTS_IN_MANIFOLD) { - Entity collider1 = mOverlappingPairs.mColliders1[mOverlappingPairs.mMapPairIdToPairIndex[manifold.pairId]]; - - Transform shape1LocalToWorldTransoform = mCollidersComponents.getLocalToWorldTransform(collider1); + Transform shape1LocalToWorldTransoform = mCollidersComponents.getLocalToWorldTransform(pairContact.collider1Entity); // Reduce the number of contact points in the manifold reduceContactPoints(manifold, shape1LocalToWorldTransoform, potentialContactPoints); } - assert(manifold.potentialContactPointsIndices.size() <= MAX_CONTACT_POINTS_IN_MANIFOLD); + assert(manifold.nbPotentialContactPoints <= MAX_CONTACT_POINTS_IN_MANIFOLD); } } } // Return the largest depth of all the contact points of a potential manifold decimal CollisionDetectionSystem::computePotentialManifoldLargestContactDepth(const ContactManifoldInfo& manifold, - const List& potentialContactPoints) const { + const Array& potentialContactPoints) const { decimal largestDepth = 0.0f; - assert(manifold.potentialContactPointsIndices.size() > 0); + assert(manifold.nbPotentialContactPoints > 0); - for (uint i=0; i < manifold.potentialContactPointsIndices.size(); i++) { + for (uint32 i=0; i < manifold.nbPotentialContactPoints; i++) { decimal depth = potentialContactPoints[manifold.potentialContactPointsIndices[i]].penetrationDepth; if (depth > largestDepth) { @@ -1212,16 +1387,20 @@ decimal CollisionDetectionSystem::computePotentialManifoldLargestContactDepth(co // "Contacts Creation" GDC presentation. This method will reduce the number of // contact points to a maximum of 4 points (but it can be less). void CollisionDetectionSystem::reduceContactPoints(ContactManifoldInfo& manifold, const Transform& shape1ToWorldTransform, - const List& potentialContactPoints) const { + const Array& potentialContactPoints) const { - assert(manifold.potentialContactPointsIndices.size() > MAX_CONTACT_POINTS_IN_MANIFOLD); + assert(manifold.nbPotentialContactPoints > MAX_CONTACT_POINTS_IN_MANIFOLD); // The following algorithm only works to reduce to a maximum of 4 contact points assert(MAX_CONTACT_POINTS_IN_MANIFOLD == 4); - // List of the candidate contact points indices in the manifold. Every time that we have found a - // point we want to keep, we will remove it from this list - List candidatePointsIndices(manifold.potentialContactPointsIndices); + // Array of the candidate contact points indices in the manifold. Every time that we have found a + // point we want to keep, we will remove it from this array + uint candidatePointsIndices[NB_MAX_CONTACT_POINTS_IN_POTENTIAL_MANIFOLD]; + uint8 nbCandidatePoints = manifold.nbPotentialContactPoints; + for (uint8 i=0 ; i < manifold.nbPotentialContactPoints; i++) { + candidatePointsIndices[i] = manifold.potentialContactPointsIndices[i]; + } int8 nbReducedPoints = 0; @@ -1245,7 +1424,7 @@ void CollisionDetectionSystem::reduceContactPoints(ContactManifoldInfo& manifold const Vector3 searchDirection(1, 1, 1); decimal maxDotProduct = DECIMAL_SMALLEST; uint elementIndexToKeep = 0; - for (uint i=0; i < candidatePointsIndices.size(); i++) { + for (uint i=0; i < nbCandidatePoints; i++) { const ContactPointInfo& element = potentialContactPoints[candidatePointsIndices[i]]; decimal dotProduct = searchDirection.dot(element.localPoint1); @@ -1256,7 +1435,8 @@ void CollisionDetectionSystem::reduceContactPoints(ContactManifoldInfo& manifold } } pointsToKeepIndices[0] = candidatePointsIndices[elementIndexToKeep]; - candidatePointsIndices.removeAt(elementIndexToKeep); + removeItemAtInArray(candidatePointsIndices, elementIndexToKeep, nbCandidatePoints); + //candidatePointsIndices.removeAt(elementIndexToKeep); assert(nbReducedPoints == 1); // Compute the second contact point we need to keep. @@ -1264,7 +1444,7 @@ void CollisionDetectionSystem::reduceContactPoints(ContactManifoldInfo& manifold decimal maxDistance = decimal(0.0); elementIndexToKeep = 0; - for (uint i=0; i < candidatePointsIndices.size(); i++) { + for (uint i=0; i < nbCandidatePoints; i++) { const ContactPointInfo& element = potentialContactPoints[candidatePointsIndices[i]]; const ContactPointInfo& pointToKeep0 = potentialContactPoints[pointsToKeepIndices[0]]; @@ -1280,7 +1460,7 @@ void CollisionDetectionSystem::reduceContactPoints(ContactManifoldInfo& manifold } pointsToKeepIndices[1] = candidatePointsIndices[elementIndexToKeep]; - candidatePointsIndices.removeAt(elementIndexToKeep); + removeItemAtInArray(candidatePointsIndices, elementIndexToKeep, nbCandidatePoints); assert(nbReducedPoints == 2); // Compute the third contact point we need to keep. @@ -1293,7 +1473,7 @@ void CollisionDetectionSystem::reduceContactPoints(ContactManifoldInfo& manifold decimal minArea = decimal(0.0); decimal maxArea = decimal(0.0); bool isPreviousAreaPositive = true; - for (uint i=0; i < candidatePointsIndices.size(); i++) { + for (uint i=0; i < nbCandidatePoints; i++) { const ContactPointInfo& element = potentialContactPoints[candidatePointsIndices[i]]; const ContactPointInfo& pointToKeep0 = potentialContactPoints[pointsToKeepIndices[0]]; @@ -1322,12 +1502,12 @@ void CollisionDetectionSystem::reduceContactPoints(ContactManifoldInfo& manifold if (maxArea > (-minArea)) { isPreviousAreaPositive = true; pointsToKeepIndices[2] = candidatePointsIndices[thirdPointMaxAreaIndex]; - candidatePointsIndices.removeAt(thirdPointMaxAreaIndex); + removeItemAtInArray(candidatePointsIndices, thirdPointMaxAreaIndex, nbCandidatePoints); } else { isPreviousAreaPositive = false; pointsToKeepIndices[2] = candidatePointsIndices[thirdPointMinAreaIndex]; - candidatePointsIndices.removeAt(thirdPointMinAreaIndex); + removeItemAtInArray(candidatePointsIndices, thirdPointMinAreaIndex, nbCandidatePoints); } nbReducedPoints = 3; @@ -1340,7 +1520,7 @@ void CollisionDetectionSystem::reduceContactPoints(ContactManifoldInfo& manifold decimal area; // For each remaining candidate points - for (uint i=0; i < candidatePointsIndices.size(); i++) { + for (uint i=0; i < nbCandidatePoints; i++) { const ContactPointInfo& element = potentialContactPoints[candidatePointsIndices[i]]; @@ -1377,14 +1557,22 @@ void CollisionDetectionSystem::reduceContactPoints(ContactManifoldInfo& manifold } } pointsToKeepIndices[3] = candidatePointsIndices[elementIndexToKeep]; - candidatePointsIndices.removeAt(elementIndexToKeep); + removeItemAtInArray(candidatePointsIndices, elementIndexToKeep, nbCandidatePoints); // Only keep the four selected contact points in the manifold - manifold.potentialContactPointsIndices.clear(); - manifold.potentialContactPointsIndices.add(pointsToKeepIndices[0]); - manifold.potentialContactPointsIndices.add(pointsToKeepIndices[1]); - manifold.potentialContactPointsIndices.add(pointsToKeepIndices[2]); - manifold.potentialContactPointsIndices.add(pointsToKeepIndices[3]); + manifold.potentialContactPointsIndices[0] = pointsToKeepIndices[0]; + manifold.potentialContactPointsIndices[1] = pointsToKeepIndices[1]; + manifold.potentialContactPointsIndices[2] = pointsToKeepIndices[2]; + manifold.potentialContactPointsIndices[3] = pointsToKeepIndices[3]; + manifold.nbPotentialContactPoints = 4; +} + +// Remove an element in an array (and replace it by the last one in the array) +void CollisionDetectionSystem::removeItemAtInArray(uint array[], uint8 index, uint8& arraySize) const { + assert(index < arraySize); + assert(arraySize > 0); + array[index] = array[arraySize - 1]; + arraySize--; } // Report contacts and triggers @@ -1409,8 +1597,8 @@ void CollisionDetectionSystem::reportContactsAndTriggers() { } // Report all contacts to the user -void CollisionDetectionSystem::reportContacts(CollisionCallback& callback, List* contactPairs, - List* manifolds, List* contactPoints, List& lostContactPairs) { +void CollisionDetectionSystem::reportContacts(CollisionCallback& callback, Array* contactPairs, + Array* manifolds, Array* contactPoints, Array& lostContactPairs) { RP3D_PROFILE("CollisionDetectionSystem::reportContacts()", mProfiler); @@ -1425,7 +1613,7 @@ void CollisionDetectionSystem::reportContacts(CollisionCallback& callback, List< } // Report all triggers to the user -void CollisionDetectionSystem::reportTriggers(EventListener& eventListener, List* contactPairs, List& lostContactPairs) { +void CollisionDetectionSystem::reportTriggers(EventListener& eventListener, Array* contactPairs, Array& lostContactPairs) { RP3D_PROFILE("CollisionDetectionSystem::reportTriggers()", mProfiler); @@ -1440,7 +1628,7 @@ void CollisionDetectionSystem::reportTriggers(EventListener& eventListener, List } // Report all contacts for debug rendering -void CollisionDetectionSystem::reportDebugRenderingContacts(List* contactPairs, List* manifolds, List* contactPoints, List& lostContactPairs) { +void CollisionDetectionSystem::reportDebugRenderingContacts(Array* contactPairs, Array* manifolds, Array* contactPoints, Array& lostContactPairs) { RP3D_PROFILE("CollisionDetectionSystem::reportDebugRenderingContacts()", mProfiler); @@ -1463,8 +1651,8 @@ bool CollisionDetectionSystem::testOverlap(CollisionBody* body1, CollisionBody* computeBroadPhase(); // Filter the overlapping pairs to get only the ones with the selected body involved - List convexPairs(mMemoryManager.getPoolAllocator()); - List concavePairs(mMemoryManager.getPoolAllocator()); + Array convexPairs(mMemoryManager.getPoolAllocator()); + Array concavePairs(mMemoryManager.getPoolAllocator()); filterOverlappingPairs(body1->getEntity(), body2->getEntity(), convexPairs, concavePairs); if (convexPairs.size() > 0 || concavePairs.size() > 0) { @@ -1503,8 +1691,8 @@ void CollisionDetectionSystem::testOverlap(CollisionBody* body, OverlapCallback& computeBroadPhase(); // Filter the overlapping pairs to get only the ones with the selected body involved - List convexPairs(mMemoryManager.getPoolAllocator()); - List concavePairs(mMemoryManager.getPoolAllocator()); + Array convexPairs(mMemoryManager.getPoolAllocator()); + Array concavePairs(mMemoryManager.getPoolAllocator()); filterOverlappingPairs(body->getEntity(), convexPairs, concavePairs); if (convexPairs.size() > 0 || concavePairs.size() > 0) { @@ -1526,8 +1714,8 @@ void CollisionDetectionSystem::testCollision(CollisionBody* body1, CollisionBody computeBroadPhase(); // Filter the overlapping pairs to get only the ones with the selected body involved - List convexPairs(mMemoryManager.getPoolAllocator()); - List concavePairs(mMemoryManager.getPoolAllocator()); + Array convexPairs(mMemoryManager.getPoolAllocator()); + Array concavePairs(mMemoryManager.getPoolAllocator()); filterOverlappingPairs(body1->getEntity(), body2->getEntity(), convexPairs, concavePairs); if (convexPairs.size() > 0 || concavePairs.size() > 0) { @@ -1549,8 +1737,8 @@ void CollisionDetectionSystem::testCollision(CollisionBody* body, CollisionCallb computeBroadPhase(); // Filter the overlapping pairs to get only the ones with the selected body involved - List convexPairs(mMemoryManager.getPoolAllocator()); - List concavePairs(mMemoryManager.getPoolAllocator()); + Array convexPairs(mMemoryManager.getPoolAllocator()); + Array concavePairs(mMemoryManager.getPoolAllocator()); filterOverlappingPairs(body->getEntity(), convexPairs, concavePairs); if (convexPairs.size() > 0 || concavePairs.size() > 0) { @@ -1579,42 +1767,59 @@ void CollisionDetectionSystem::testCollision(CollisionCallback& callback) { } // Filter the overlapping pairs to keep only the pairs where a given body is involved -void CollisionDetectionSystem::filterOverlappingPairs(Entity bodyEntity, List& convexPairs, List& concavePairs) const { +void CollisionDetectionSystem::filterOverlappingPairs(Entity bodyEntity, Array& convexPairs, Array& concavePairs) const { - // For each possible collision pair of bodies - for (uint i=0; i < mOverlappingPairs.getNbPairs(); i++) { + // For each convex pairs + const uint32 nbConvexPairs = mOverlappingPairs.mConvexPairs.size(); + for (uint32 i=0; i < nbConvexPairs; i++) { - if (mCollidersComponents.getBody(mOverlappingPairs.mColliders1[i]) == bodyEntity || - mCollidersComponents.getBody(mOverlappingPairs.mColliders2[i]) == bodyEntity) { + if (mCollidersComponents.getBody(mOverlappingPairs.mConvexPairs[i].collider1) == bodyEntity || + mCollidersComponents.getBody(mOverlappingPairs.mConvexPairs[i].collider2) == bodyEntity) { - if (i < mOverlappingPairs.getNbConvexVsConvexPairs()) { - convexPairs.add(mOverlappingPairs.mPairIds[i]); - } - else { - concavePairs.add(mOverlappingPairs.mPairIds[i]); - } + convexPairs.add(mOverlappingPairs.mConvexPairs[i].pairID); + } + } + + // For each concave pairs + const uint32 nbConcavePairs = mOverlappingPairs.mConcavePairs.size(); + for (uint32 i=0; i < nbConcavePairs; i++) { + + if (mCollidersComponents.getBody(mOverlappingPairs.mConcavePairs[i].collider1) == bodyEntity || + mCollidersComponents.getBody(mOverlappingPairs.mConcavePairs[i].collider2) == bodyEntity) { + + concavePairs.add(mOverlappingPairs.mConcavePairs[i].pairID); } } } // Filter the overlapping pairs to keep only the pairs where two given bodies are involved -void CollisionDetectionSystem::filterOverlappingPairs(Entity body1Entity, Entity body2Entity, List& convexPairs, List& concavePairs) const { +void CollisionDetectionSystem::filterOverlappingPairs(Entity body1Entity, Entity body2Entity, Array& convexPairs, Array& concavePairs) const { - // For each possible collision pair of bodies - for (uint i=0; i < mOverlappingPairs.getNbPairs(); i++) { + // For each convex pair + const uint32 nbConvexPairs = mOverlappingPairs.mConvexPairs.size(); + for (uint32 i=0; i < nbConvexPairs; i++) { - const Entity collider1Body = mCollidersComponents.getBody(mOverlappingPairs.mColliders1[i]); - const Entity collider2Body = mCollidersComponents.getBody(mOverlappingPairs.mColliders2[i]); + const Entity collider1Body = mCollidersComponents.getBody(mOverlappingPairs.mConvexPairs[i].collider1); + const Entity collider2Body = mCollidersComponents.getBody(mOverlappingPairs.mConvexPairs[i].collider2); if ((collider1Body == body1Entity && collider2Body == body2Entity) || (collider1Body == body2Entity && collider2Body == body1Entity)) { - if (i < mOverlappingPairs.getNbConvexVsConvexPairs()) { - convexPairs.add(mOverlappingPairs.mPairIds[i]); - } - else { - concavePairs.add(mOverlappingPairs.mPairIds[i]); - } + convexPairs.add(mOverlappingPairs.mConvexPairs[i].pairID); + } + } + + // For each concave pair + const uint nbConcavePairs = mOverlappingPairs.mConcavePairs.size(); + for (uint i=0; i < nbConcavePairs; i++) { + + const Entity collider1Body = mCollidersComponents.getBody(mOverlappingPairs.mConcavePairs[i].collider1); + const Entity collider2Body = mCollidersComponents.getBody(mOverlappingPairs.mConcavePairs[i].collider2); + + if ((collider1Body == body1Entity && collider2Body == body2Entity) || + (collider1Body == body2Entity && collider2Body == body1Entity)) { + + concavePairs.add(mOverlappingPairs.mConcavePairs[i].pairID); } } } diff --git a/src/systems/ContactSolverSystem.cpp b/src/systems/ContactSolverSystem.cpp index f384a78a..18139b8f 100644 --- a/src/systems/ContactSolverSystem.cpp +++ b/src/systems/ContactSolverSystem.cpp @@ -34,6 +34,7 @@ #include #include #include +#include using namespace reactphysics3d; using namespace std; @@ -61,7 +62,7 @@ ContactSolverSystem::ContactSolverSystem(MemoryManager& memoryManager, PhysicsWo } // Initialize the contact constraints -void ContactSolverSystem::init(List* contactManifolds, List* contactPoints, decimal timeStep) { +void ContactSolverSystem::init(Array* contactManifolds, Array* contactPoints, decimal timeStep) { mAllContactManifolds = contactManifolds; mAllContactPoints = contactPoints; @@ -70,8 +71,8 @@ void ContactSolverSystem::init(List* contactManifolds, Listsize(); - uint nbContactPoints = mAllContactPoints->size(); + const uint32 nbContactManifolds = mAllContactManifolds->size(); + const uint32 nbContactPoints = mAllContactPoints->size(); mNbContactManifolds = 0; mNbContactPoints = 0; @@ -90,7 +91,8 @@ void ContactSolverSystem::init(List* contactManifolds, List 0) { initializeForIsland(i); @@ -113,31 +115,27 @@ void ContactSolverSystem::initializeForIsland(uint islandIndex) { RP3D_PROFILE("ContactSolver::initializeForIsland()", mProfiler); - assert(mIslands.bodyEntities[islandIndex].size() > 0); + assert(mIslands.nbBodiesInIsland[islandIndex] > 0); assert(mIslands.nbContactManifolds[islandIndex] > 0); // For each contact manifold of the island - uint contactManifoldsIndex = mIslands.contactManifoldsIndices[islandIndex]; - uint nbContactManifolds = mIslands.nbContactManifolds[islandIndex]; + const uint contactManifoldsIndex = mIslands.contactManifoldsIndices[islandIndex]; + const uint nbContactManifolds = mIslands.nbContactManifolds[islandIndex]; for (uint m=contactManifoldsIndex; m < contactManifoldsIndex + nbContactManifolds; m++) { ContactManifold& externalManifold = (*mAllContactManifolds)[m]; assert(externalManifold.nbContactPoints > 0); - // Get the two bodies of the contact - RigidBody* body1 = static_cast(mBodyComponents.getBody(externalManifold.bodyEntity1)); - RigidBody* body2 = static_cast(mBodyComponents.getBody(externalManifold.bodyEntity2)); - assert(body1 != nullptr); - assert(body2 != nullptr); - assert(!mBodyComponents.getIsEntityDisabled(externalManifold.bodyEntity1)); - assert(!mBodyComponents.getIsEntityDisabled(externalManifold.bodyEntity2)); - const uint rigidBodyIndex1 = mRigidBodyComponents.getEntityIndex(externalManifold.bodyEntity1); const uint rigidBodyIndex2 = mRigidBodyComponents.getEntityIndex(externalManifold.bodyEntity2); - Collider* collider1 = mColliderComponents.getCollider(externalManifold.colliderEntity1); - Collider* collider2 = mColliderComponents.getCollider(externalManifold.colliderEntity2); + // Get the two bodies of the contact + assert(!mBodyComponents.getIsEntityDisabled(externalManifold.bodyEntity1)); + assert(!mBodyComponents.getIsEntityDisabled(externalManifold.bodyEntity2)); + + const uint collider1Index = mColliderComponents.getEntityIndex(externalManifold.colliderEntity1); + const uint collider2Index = mColliderComponents.getEntityIndex(externalManifold.colliderEntity2); // Get the position of the two bodies const Vector3& x1 = mRigidBodyComponents.mCentersOfMassWorld[rigidBodyIndex1]; @@ -147,13 +145,12 @@ void ContactSolverSystem::initializeForIsland(uint islandIndex) { new (mContactConstraints + mNbContactManifolds) ContactManifoldSolver(); mContactConstraints[mNbContactManifolds].rigidBodyComponentIndexBody1 = rigidBodyIndex1; mContactConstraints[mNbContactManifolds].rigidBodyComponentIndexBody2 = rigidBodyIndex2; - mContactConstraints[mNbContactManifolds].inverseInertiaTensorBody1 = RigidBody::getWorldInertiaTensorInverse(mWorld, externalManifold.bodyEntity1); - mContactConstraints[mNbContactManifolds].inverseInertiaTensorBody2 = RigidBody::getWorldInertiaTensorInverse(mWorld, externalManifold.bodyEntity2); + mContactConstraints[mNbContactManifolds].inverseInertiaTensorBody1 = mRigidBodyComponents.mInverseInertiaTensorsWorld[rigidBodyIndex1]; + mContactConstraints[mNbContactManifolds].inverseInertiaTensorBody2 = mRigidBodyComponents.mInverseInertiaTensorsWorld[rigidBodyIndex2]; mContactConstraints[mNbContactManifolds].massInverseBody1 = mRigidBodyComponents.mInverseMasses[rigidBodyIndex1]; mContactConstraints[mNbContactManifolds].massInverseBody2 = mRigidBodyComponents.mInverseMasses[rigidBodyIndex2]; mContactConstraints[mNbContactManifolds].nbContacts = externalManifold.nbContactPoints; - mContactConstraints[mNbContactManifolds].frictionCoefficient = computeMixedFrictionCoefficient(collider1, collider2); - mContactConstraints[mNbContactManifolds].rollingResistanceFactor = computeMixedRollingResistance(collider1, collider2); + mContactConstraints[mNbContactManifolds].frictionCoefficient = computeMixedFrictionCoefficient(mColliderComponents.mMaterials[collider1Index], mColliderComponents.mMaterials[collider2Index]); mContactConstraints[mNbContactManifolds].externalContactManifold = &externalManifold; mContactConstraints[mNbContactManifolds].normal.setToZero(); mContactConstraints[mNbContactManifolds].frictionPointBody1.setToZero(); @@ -165,21 +162,25 @@ void ContactSolverSystem::initializeForIsland(uint islandIndex) { const Vector3& v2 = mRigidBodyComponents.mLinearVelocities[rigidBodyIndex2]; const Vector3& w2 = mRigidBodyComponents.mAngularVelocities[rigidBodyIndex2]; + const Transform& collider1LocalToWorldTransform = mColliderComponents.mLocalToWorldTransforms[collider1Index]; + const Transform& collider2LocalToWorldTransform = mColliderComponents.mLocalToWorldTransforms[collider2Index]; + // For each contact point of the contact manifold assert(externalManifold.nbContactPoints > 0); - uint contactPointsStartIndex = externalManifold.contactPointsIndex; - uint nbContactPoints = static_cast(externalManifold.nbContactPoints); + const uint contactPointsStartIndex = externalManifold.contactPointsIndex; + const uint nbContactPoints = static_cast(externalManifold.nbContactPoints); for (uint c=contactPointsStartIndex; c < contactPointsStartIndex + nbContactPoints; c++) { ContactPoint& externalContact = (*mAllContactPoints)[c]; - // Get the contact point on the two bodies - Vector3 p1 = mColliderComponents.getLocalToWorldTransform(externalManifold.colliderEntity1) * externalContact.getLocalPointOnShape1(); - Vector3 p2 = mColliderComponents.getLocalToWorldTransform(externalManifold.colliderEntity2) * externalContact.getLocalPointOnShape2(); - new (mContactPoints + mNbContactPoints) ContactPointSolver(); mContactPoints[mNbContactPoints].externalContact = &externalContact; mContactPoints[mNbContactPoints].normal = externalContact.getNormal(); + + // Get the contact point on the two bodies + const Vector3 p1 = collider1LocalToWorldTransform * externalContact.getLocalPointOnShape1(); + const Vector3 p2 = collider2LocalToWorldTransform * externalContact.getLocalPointOnShape2(); + mContactPoints[mNbContactPoints].r1.x = p1.x - x1.x; mContactPoints[mNbContactPoints].r1.y = p1.y - x1.y; mContactPoints[mNbContactPoints].r1.z = p1.z - x1.z; @@ -241,7 +242,7 @@ void ContactSolverSystem::initializeForIsland(uint islandIndex) { decimal deltaVDotN = deltaV.x * mContactPoints[mNbContactPoints].normal.x + deltaV.y * mContactPoints[mNbContactPoints].normal.y + deltaV.z * mContactPoints[mNbContactPoints].normal.z; - const decimal restitutionFactor = computeMixedRestitutionFactor(collider1, collider2); + const decimal restitutionFactor = computeMixedRestitutionFactor(mColliderComponents.mMaterials[collider1Index], mColliderComponents.mMaterials[collider2Index]); if (deltaVDotN < -mRestitutionVelocityThreshold) { mContactPoints[mNbContactPoints].restitutionBias = restitutionFactor * deltaVDotN; } @@ -253,8 +254,8 @@ void ContactSolverSystem::initializeForIsland(uint islandIndex) { mNbContactPoints++; } - mContactConstraints[mNbContactManifolds].frictionPointBody1 /=static_cast(mContactConstraints[mNbContactManifolds].nbContacts); - mContactConstraints[mNbContactManifolds].frictionPointBody2 /=static_cast(mContactConstraints[mNbContactManifolds].nbContacts); + mContactConstraints[mNbContactManifolds].frictionPointBody1 /= static_cast(mContactConstraints[mNbContactManifolds].nbContacts); + mContactConstraints[mNbContactManifolds].frictionPointBody2 /= static_cast(mContactConstraints[mNbContactManifolds].nbContacts); mContactConstraints[mNbContactManifolds].r1Friction.x = mContactConstraints[mNbContactManifolds].frictionPointBody1.x - x1.x; mContactConstraints[mNbContactManifolds].r1Friction.y = mContactConstraints[mNbContactManifolds].frictionPointBody1.y - x1.y; mContactConstraints[mNbContactManifolds].r1Friction.z = mContactConstraints[mNbContactManifolds].frictionPointBody1.z - x1.z; @@ -269,24 +270,6 @@ void ContactSolverSystem::initializeForIsland(uint islandIndex) { mContactConstraints[mNbContactManifolds].friction2Impulse = externalManifold.frictionImpulse2; mContactConstraints[mNbContactManifolds].frictionTwistImpulse = externalManifold.frictionTwistImpulse; - // Compute the inverse K matrix for the rolling resistance constraint - bool isBody1DynamicType = body1->getType() == BodyType::DYNAMIC; - bool isBody2DynamicType = body2->getType() == BodyType::DYNAMIC; - mContactConstraints[mNbContactManifolds].inverseRollingResistance.setToZero(); - if (mContactConstraints[mNbContactManifolds].rollingResistanceFactor > 0 && (isBody1DynamicType || isBody2DynamicType)) { - - mContactConstraints[mNbContactManifolds].inverseRollingResistance = mContactConstraints[mNbContactManifolds].inverseInertiaTensorBody1 + mContactConstraints[mNbContactManifolds].inverseInertiaTensorBody2; - decimal det = mContactConstraints[mNbContactManifolds].inverseRollingResistance.getDeterminant(); - - // If the matrix is not inversible - if (approxEqual(det, decimal(0.0))) { - mContactConstraints[mNbContactManifolds].inverseRollingResistance.setToZero(); - } - else { - mContactConstraints[mNbContactManifolds].inverseRollingResistance = mContactConstraints[mNbContactManifolds].inverseRollingResistance.getInverse(); - } - } - mContactConstraints[mNbContactManifolds].normal.normalize(); // deltaVFrictionPoint = v2 + w2.cross(mContactConstraints[mNbContactManifolds].r2Friction) - @@ -352,10 +335,12 @@ void ContactSolverSystem::warmStart() { for (short int i=0; i SLOP) biasPenetrationDepth = -(beta/mTimeStep) * - max(0.0f, float(mContactPoints[contactPointIndex].penetrationDepth - SLOP)); + std::max(0.0f, float(mContactPoints[contactPointIndex].penetrationDepth - SLOP)); decimal b = biasPenetrationDepth + mContactPoints[contactPointIndex].restitutionBias; // Compute the Lagrange multiplier lambda @@ -554,22 +539,22 @@ void ContactSolverSystem::solve() { mContactPoints[contactPointIndex].normal.z * deltaLambda); // Update the velocities of the body 1 by applying the impulse P - mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1].x -= mContactConstraints[c].massInverseBody1 * linearImpulse.x; - mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1].y -= mContactConstraints[c].massInverseBody1 * linearImpulse.y; - mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1].z -= mContactConstraints[c].massInverseBody1 * linearImpulse.z; + mRigidBodyComponents.mConstrainedLinearVelocities[rigidBody1Index].x -= mContactConstraints[c].massInverseBody1 * linearImpulse.x; + mRigidBodyComponents.mConstrainedLinearVelocities[rigidBody1Index].y -= mContactConstraints[c].massInverseBody1 * linearImpulse.y; + mRigidBodyComponents.mConstrainedLinearVelocities[rigidBody1Index].z -= mContactConstraints[c].massInverseBody1 * linearImpulse.z; - mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1].x -= mContactPoints[contactPointIndex].i1TimesR1CrossN.x * deltaLambda; - mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1].y -= mContactPoints[contactPointIndex].i1TimesR1CrossN.y * deltaLambda; - mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1].z -= mContactPoints[contactPointIndex].i1TimesR1CrossN.z * deltaLambda; + mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody1Index].x -= mContactPoints[contactPointIndex].i1TimesR1CrossN.x * deltaLambda; + mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody1Index].y -= mContactPoints[contactPointIndex].i1TimesR1CrossN.y * deltaLambda; + mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody1Index].z -= mContactPoints[contactPointIndex].i1TimesR1CrossN.z * deltaLambda; // Update the velocities of the body 2 by applying the impulse P - mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2].x += mContactConstraints[c].massInverseBody2 * linearImpulse.x; - mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2].y += mContactConstraints[c].massInverseBody2 * linearImpulse.y; - mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2].z += mContactConstraints[c].massInverseBody2 * linearImpulse.z; + mRigidBodyComponents.mConstrainedLinearVelocities[rigidBody2Index].x += mContactConstraints[c].massInverseBody2 * linearImpulse.x; + mRigidBodyComponents.mConstrainedLinearVelocities[rigidBody2Index].y += mContactConstraints[c].massInverseBody2 * linearImpulse.y; + mRigidBodyComponents.mConstrainedLinearVelocities[rigidBody2Index].z += mContactConstraints[c].massInverseBody2 * linearImpulse.z; - mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2].x += mContactPoints[contactPointIndex].i2TimesR2CrossN.x * deltaLambda; - mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2].y += mContactPoints[contactPointIndex].i2TimesR2CrossN.y * deltaLambda; - mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2].z += mContactPoints[contactPointIndex].i2TimesR2CrossN.z * deltaLambda; + mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody2Index].x += mContactPoints[contactPointIndex].i2TimesR2CrossN.x * deltaLambda; + mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody2Index].y += mContactPoints[contactPointIndex].i2TimesR2CrossN.y * deltaLambda; + mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody2Index].z += mContactPoints[contactPointIndex].i2TimesR2CrossN.z * deltaLambda; sumPenetrationImpulse += mContactPoints[contactPointIndex].penetrationImpulse; @@ -577,10 +562,10 @@ void ContactSolverSystem::solve() { if (mIsSplitImpulseActive) { // Split impulse (position correction) - const Vector3& v1Split = mRigidBodyComponents.mSplitLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1]; - const Vector3& w1Split = mRigidBodyComponents.mSplitAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1]; - const Vector3& v2Split = mRigidBodyComponents.mSplitLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2]; - const Vector3& w2Split = mRigidBodyComponents.mSplitAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2]; + const Vector3& v1Split = mRigidBodyComponents.mSplitLinearVelocities[rigidBody1Index]; + const Vector3& w1Split = mRigidBodyComponents.mSplitAngularVelocities[rigidBody1Index]; + const Vector3& v2Split = mRigidBodyComponents.mSplitLinearVelocities[rigidBody2Index]; + const Vector3& w2Split = mRigidBodyComponents.mSplitAngularVelocities[rigidBody2Index]; //Vector3 deltaVSplit = v2Split + w2Split.cross(mContactPoints[contactPointIndex].r2) - v1Split - w1Split.cross(mContactPoints[contactPointIndex].r1); Vector3 deltaVSplit(v2Split.x + w2Split.y * mContactPoints[contactPointIndex].r2.z - w2Split.z * mContactPoints[contactPointIndex].r2.y - v1Split.x - @@ -605,22 +590,22 @@ void ContactSolverSystem::solve() { mContactPoints[contactPointIndex].normal.z * deltaLambdaSplit); // Update the velocities of the body 1 by applying the impulse P - mRigidBodyComponents.mSplitLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1].x -= mContactConstraints[c].massInverseBody1 * linearImpulse.x; - mRigidBodyComponents.mSplitLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1].y -= mContactConstraints[c].massInverseBody1 * linearImpulse.y; - mRigidBodyComponents.mSplitLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1].z -= mContactConstraints[c].massInverseBody1 * linearImpulse.z; + mRigidBodyComponents.mSplitLinearVelocities[rigidBody1Index].x -= mContactConstraints[c].massInverseBody1 * linearImpulse.x; + mRigidBodyComponents.mSplitLinearVelocities[rigidBody1Index].y -= mContactConstraints[c].massInverseBody1 * linearImpulse.y; + mRigidBodyComponents.mSplitLinearVelocities[rigidBody1Index].z -= mContactConstraints[c].massInverseBody1 * linearImpulse.z; - mRigidBodyComponents.mSplitAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1].x -= mContactPoints[contactPointIndex].i1TimesR1CrossN.x * deltaLambdaSplit; - mRigidBodyComponents.mSplitAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1].y -= mContactPoints[contactPointIndex].i1TimesR1CrossN.y * deltaLambdaSplit; - mRigidBodyComponents.mSplitAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1].z -= mContactPoints[contactPointIndex].i1TimesR1CrossN.z * deltaLambdaSplit; + mRigidBodyComponents.mSplitAngularVelocities[rigidBody1Index].x -= mContactPoints[contactPointIndex].i1TimesR1CrossN.x * deltaLambdaSplit; + mRigidBodyComponents.mSplitAngularVelocities[rigidBody1Index].y -= mContactPoints[contactPointIndex].i1TimesR1CrossN.y * deltaLambdaSplit; + mRigidBodyComponents.mSplitAngularVelocities[rigidBody1Index].z -= mContactPoints[contactPointIndex].i1TimesR1CrossN.z * deltaLambdaSplit; // Update the velocities of the body 1 by applying the impulse P - mRigidBodyComponents.mSplitLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2].x += mContactConstraints[c].massInverseBody2 * linearImpulse.x; - mRigidBodyComponents.mSplitLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2].y += mContactConstraints[c].massInverseBody2 * linearImpulse.y; - mRigidBodyComponents.mSplitLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2].z += mContactConstraints[c].massInverseBody2 * linearImpulse.z; + mRigidBodyComponents.mSplitLinearVelocities[rigidBody2Index].x += mContactConstraints[c].massInverseBody2 * linearImpulse.x; + mRigidBodyComponents.mSplitLinearVelocities[rigidBody2Index].y += mContactConstraints[c].massInverseBody2 * linearImpulse.y; + mRigidBodyComponents.mSplitLinearVelocities[rigidBody2Index].z += mContactConstraints[c].massInverseBody2 * linearImpulse.z; - mRigidBodyComponents.mSplitAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2].x += mContactPoints[contactPointIndex].i2TimesR2CrossN.x * deltaLambdaSplit; - mRigidBodyComponents.mSplitAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2].y += mContactPoints[contactPointIndex].i2TimesR2CrossN.y * deltaLambdaSplit; - mRigidBodyComponents.mSplitAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2].z += mContactPoints[contactPointIndex].i2TimesR2CrossN.z * deltaLambdaSplit; + mRigidBodyComponents.mSplitAngularVelocities[rigidBody2Index].x += mContactPoints[contactPointIndex].i2TimesR2CrossN.x * deltaLambdaSplit; + mRigidBodyComponents.mSplitAngularVelocities[rigidBody2Index].y += mContactPoints[contactPointIndex].i2TimesR2CrossN.y * deltaLambdaSplit; + mRigidBodyComponents.mSplitAngularVelocities[rigidBody2Index].z += mContactPoints[contactPointIndex].i2TimesR2CrossN.z * deltaLambdaSplit; } contactPointIndex++; @@ -664,18 +649,24 @@ void ContactSolverSystem::solve() { // Update the velocities of the body 1 by applying the impulse P - mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1].x -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.x; - mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1].y -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.y; - mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1].z -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.z; + mRigidBodyComponents.mConstrainedLinearVelocities[rigidBody1Index].x -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.x; + mRigidBodyComponents.mConstrainedLinearVelocities[rigidBody1Index].y -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.y; + mRigidBodyComponents.mConstrainedLinearVelocities[rigidBody1Index].z -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.z; - mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1] += mContactConstraints[c].inverseInertiaTensorBody1 * angularImpulseBody1; + Vector3 angularVelocity1 = mContactConstraints[c].inverseInertiaTensorBody1 * angularImpulseBody1; + mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody1Index].x += angularVelocity1.x; + mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody1Index].y += angularVelocity1.y; + mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody1Index].z += angularVelocity1.z; // Update the velocities of the body 2 by applying the impulse P - mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2].x += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.x; - mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2].y += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.y; - mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2].z += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.z; + mRigidBodyComponents.mConstrainedLinearVelocities[rigidBody2Index].x += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.x; + mRigidBodyComponents.mConstrainedLinearVelocities[rigidBody2Index].y += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.y; + mRigidBodyComponents.mConstrainedLinearVelocities[rigidBody2Index].z += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.z; - mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2] += mContactConstraints[c].inverseInertiaTensorBody2 * angularImpulseBody2; + Vector3 angularVelocity2 = mContactConstraints[c].inverseInertiaTensorBody2 * angularImpulseBody2; + mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody2Index].x += angularVelocity2.x; + mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody2Index].y += angularVelocity2.y; + mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody2Index].z += angularVelocity2.z; // ------ Second friction constraint at the center of the contact manifold ----- // @@ -713,16 +704,24 @@ void ContactSolverSystem::solve() { angularImpulseBody2.z = mContactConstraints[c].r2CrossT2.z * deltaLambda; // Update the velocities of the body 1 by applying the impulse P - mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1].x -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.x; - mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1].y -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.y; - mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1].z -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.z; - mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1] += mContactConstraints[c].inverseInertiaTensorBody1 * angularImpulseBody1; + mRigidBodyComponents.mConstrainedLinearVelocities[rigidBody1Index].x -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.x; + mRigidBodyComponents.mConstrainedLinearVelocities[rigidBody1Index].y -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.y; + mRigidBodyComponents.mConstrainedLinearVelocities[rigidBody1Index].z -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.z; + + angularVelocity1 = mContactConstraints[c].inverseInertiaTensorBody1 * angularImpulseBody1; + mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody1Index].x += angularVelocity1.x; + mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody1Index].y += angularVelocity1.y; + mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody1Index].z += angularVelocity1.z; // Update the velocities of the body 2 by applying the impulse P - mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2].x += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.x; - mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2].y += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.y; - mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2].z += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.z; - mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2] += mContactConstraints[c].inverseInertiaTensorBody2 * angularImpulseBody2; + mRigidBodyComponents.mConstrainedLinearVelocities[rigidBody2Index].x += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.x; + mRigidBodyComponents.mConstrainedLinearVelocities[rigidBody2Index].y += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.y; + mRigidBodyComponents.mConstrainedLinearVelocities[rigidBody2Index].z += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.z; + + angularVelocity2 = mContactConstraints[c].inverseInertiaTensorBody2 * angularImpulseBody2; + mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody2Index].x += angularVelocity2.x; + mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody2Index].y += angularVelocity2.y; + mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody2Index].z += angularVelocity2.z; // ------ Twist friction constraint at the center of the contact manifol ------ // @@ -745,57 +744,19 @@ void ContactSolverSystem::solve() { angularImpulseBody2.z = mContactConstraints[c].normal.z * deltaLambda; // Update the velocities of the body 1 by applying the impulse P - mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1] -= mContactConstraints[c].inverseInertiaTensorBody1 * angularImpulseBody2; + angularVelocity1 = mContactConstraints[c].inverseInertiaTensorBody1 * angularImpulseBody2; + mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody1Index].x -= angularVelocity1.x; + mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody1Index].y -= angularVelocity1.y; + mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody1Index].z -= angularVelocity1.z; // Update the velocities of the body 1 by applying the impulse P - mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2] += mContactConstraints[c].inverseInertiaTensorBody2 * angularImpulseBody2; - - // --------- Rolling resistance constraint at the center of the contact manifold --------- // - - if (mContactConstraints[c].rollingResistanceFactor > 0) { - - // Compute J*v - const Vector3 JvRolling = w2 - w1; - - // Compute the Lagrange multiplier lambda - Vector3 deltaLambdaRolling = mContactConstraints[c].inverseRollingResistance * (-JvRolling); - decimal rollingLimit = mContactConstraints[c].rollingResistanceFactor * sumPenetrationImpulse; - Vector3 lambdaTempRolling = mContactConstraints[c].rollingResistanceImpulse; - mContactConstraints[c].rollingResistanceImpulse = clamp(mContactConstraints[c].rollingResistanceImpulse + - deltaLambdaRolling, rollingLimit); - deltaLambdaRolling = mContactConstraints[c].rollingResistanceImpulse - lambdaTempRolling; - - // Update the velocities of the body 1 by applying the impulse P - mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1] -= mContactConstraints[c].inverseInertiaTensorBody1 * deltaLambdaRolling; - - // Update the velocities of the body 2 by applying the impulse P - mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2] += mContactConstraints[c].inverseInertiaTensorBody2 * deltaLambdaRolling; - } + angularVelocity2 = mContactConstraints[c].inverseInertiaTensorBody2 * angularImpulseBody2; + mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody2Index].x += angularVelocity2.x; + mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody2Index].y += angularVelocity2.y; + mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody2Index].z += angularVelocity2.z; } } -// Compute the collision restitution factor from the restitution factor of each collider -decimal ContactSolverSystem::computeMixedRestitutionFactor(Collider* collider1, Collider* collider2) const { - decimal restitution1 = collider1->getMaterial().getBounciness(); - decimal restitution2 = collider2->getMaterial().getBounciness(); - - // Return the largest restitution factor - return (restitution1 > restitution2) ? restitution1 : restitution2; -} - -// Compute the mixed friction coefficient from the friction coefficient of each collider -decimal ContactSolverSystem::computeMixedFrictionCoefficient(Collider* collider1, Collider* collider2) const { - - // Use the geometric mean to compute the mixed friction coefficient - return std::sqrt(collider1->getMaterial().getFrictionCoefficient() * - collider2->getMaterial().getFrictionCoefficient()); -} - -// Compute th mixed rolling resistance factor between two colliders -inline decimal ContactSolverSystem::computeMixedRollingResistance(Collider* collider1, Collider* collider2) const { - return decimal(0.5f) * (collider1->getMaterial().getRollingResistance() + collider2->getMaterial().getRollingResistance()); -} - // Store the computed impulses to use them to // warm start the solver at the next iteration void ContactSolverSystem::storeImpulses() { @@ -817,7 +778,6 @@ void ContactSolverSystem::storeImpulses() { mContactConstraints[c].externalContactManifold->frictionImpulse1 = mContactConstraints[c].friction1Impulse; mContactConstraints[c].externalContactManifold->frictionImpulse2 = mContactConstraints[c].friction2Impulse; mContactConstraints[c].externalContactManifold->frictionTwistImpulse = mContactConstraints[c].frictionTwistImpulse; - mContactConstraints[c].externalContactManifold->rollingResistanceImpulse = mContactConstraints[c].rollingResistanceImpulse; mContactConstraints[c].externalContactManifold->frictionVector1 = mContactConstraints[c].frictionVector1; mContactConstraints[c].externalContactManifold->frictionVector2 = mContactConstraints[c].frictionVector2; } @@ -825,22 +785,21 @@ void ContactSolverSystem::storeImpulses() { // Compute the two unit orthogonal vectors "t1" and "t2" that span the tangential friction plane // for a contact manifold. The two vectors have to be such that : t1 x t2 = contactNormal. -void ContactSolverSystem::computeFrictionVectors(const Vector3& deltaVelocity, - ContactManifoldSolver& contact) const { +void ContactSolverSystem::computeFrictionVectors(const Vector3& deltaVelocity, ContactManifoldSolver& contact) const { RP3D_PROFILE("ContactSolver::computeFrictionVectors()", mProfiler); assert(contact.normal.length() > decimal(0.0)); // Compute the velocity difference vector in the tangential plane - Vector3 normalVelocity(deltaVelocity.x * contact.normal.x * contact.normal.x, + const Vector3 normalVelocity(deltaVelocity.x * contact.normal.x * contact.normal.x, deltaVelocity.y * contact.normal.y * contact.normal.y, deltaVelocity.z * contact.normal.z * contact.normal.z); - Vector3 tangentVelocity(deltaVelocity.x - normalVelocity.x, deltaVelocity.y - normalVelocity.y, + const Vector3 tangentVelocity(deltaVelocity.x - normalVelocity.x, deltaVelocity.y - normalVelocity.y, deltaVelocity.z - normalVelocity.z); // If the velocty difference in the tangential plane is not zero - decimal lengthTangentVelocity = tangentVelocity.length(); + const decimal lengthTangentVelocity = tangentVelocity.length(); if (lengthTangentVelocity > MACHINE_EPSILON) { // Compute the first friction vector in the direction of the tangent @@ -855,5 +814,5 @@ void ContactSolverSystem::computeFrictionVectors(const Vector3& deltaVelocity, // The second friction vector is computed by the cross product of the first // friction vector and the contact normal - contact.frictionVector2 = contact.normal.cross(contact.frictionVector1).getUnit(); + contact.frictionVector2 = contact.normal.cross(contact.frictionVector1); } diff --git a/src/systems/DynamicsSystem.cpp b/src/systems/DynamicsSystem.cpp index fae61e82..409c9dac 100644 --- a/src/systems/DynamicsSystem.cpp +++ b/src/systems/DynamicsSystem.cpp @@ -47,7 +47,8 @@ void DynamicsSystem::integrateRigidBodiesPositions(decimal timeStep, bool isSpli const decimal isSplitImpulseFactor = isSplitImpulseActive ? decimal(1.0) : decimal(0.0); - for (uint32 i=0; i < mRigidBodyComponents.getNbEnabledComponents(); i++) { + const uint32 nbRigidBodyComponents = mRigidBodyComponents.getNbEnabledComponents(); + for (uint32 i=0; i < nbRigidBodyComponents; i++) { // Get the constrained velocity Vector3 newLinVelocity = mRigidBodyComponents.mConstrainedLinearVelocities[i]; @@ -74,7 +75,8 @@ void DynamicsSystem::updateBodiesState() { RP3D_PROFILE("DynamicsSystem::updateBodiesState()", mProfiler); - for (uint32 i=0; i < mRigidBodyComponents.getNbEnabledComponents(); i++) { + const uint32 nbRigidBodyComponents = mRigidBodyComponents.getNbEnabledComponents(); + for (uint32 i=0; i < nbRigidBodyComponents; i++) { // Update the linear and angular velocity of the body mRigidBodyComponents.mLinearVelocities[i] = mRigidBodyComponents.mConstrainedLinearVelocities[i]; @@ -89,7 +91,7 @@ void DynamicsSystem::updateBodiesState() { } // Update the position of the body (using the new center of mass and new orientation) - for (uint32 i=0; i < mRigidBodyComponents.getNbEnabledComponents(); i++) { + for (uint32 i=0; i < nbRigidBodyComponents; i++) { Transform& transform = mTransformComponents.getTransform(mRigidBodyComponents.mBodiesEntities[i]); const Vector3& centerOfMassWorld = mRigidBodyComponents.mCentersOfMassWorld[i]; @@ -98,7 +100,8 @@ void DynamicsSystem::updateBodiesState() { } // Update the local-to-world transform of the colliders - for (uint32 i=0; i < mColliderComponents.getNbEnabledComponents(); i++) { + const uint32 nbColliderComponents = mColliderComponents.getNbEnabledComponents(); + for (uint32 i=0; i < nbColliderComponents; i++) { // Update the local-to-world transform of the collider mColliderComponents.mLocalToWorldTransforms[i] = mTransformComponents.getTransform(mColliderComponents.mBodiesEntities[i]) * @@ -119,7 +122,8 @@ void DynamicsSystem::integrateRigidBodiesVelocities(decimal timeStep) { resetSplitVelocities(); // Integration component velocities using force/torque - for (uint32 i=0; i < mRigidBodyComponents.getNbEnabledComponents(); i++) { + const uint32 nbEnabledRigidBodyComponents = mRigidBodyComponents.getNbEnabledComponents(); + for (uint32 i=0; i < nbEnabledRigidBodyComponents; i++) { assert(mRigidBodyComponents.mSplitLinearVelocities[i] == Vector3(0, 0, 0)); assert(mRigidBodyComponents.mSplitAngularVelocities[i] == Vector3(0, 0, 0)); @@ -130,14 +134,15 @@ void DynamicsSystem::integrateRigidBodiesVelocities(decimal timeStep) { // Integrate the external force to get the new velocity of the body mRigidBodyComponents.mConstrainedLinearVelocities[i] = linearVelocity + timeStep * mRigidBodyComponents.mInverseMasses[i] * mRigidBodyComponents.mExternalForces[i]; - mRigidBodyComponents.mConstrainedAngularVelocities[i] = angularVelocity + timeStep * - RigidBody::getWorldInertiaTensorInverse(mWorld, mRigidBodyComponents.mBodiesEntities[i]) * mRigidBodyComponents.mExternalTorques[i]; + mRigidBodyComponents.mConstrainedAngularVelocities[i] = angularVelocity + timeStep * mRigidBodyComponents.mInverseInertiaTensorsWorld[i] * + mRigidBodyComponents.mExternalTorques[i]; } // Apply gravity force if (mIsGravityEnabled) { - for (uint32 i=0; i < mRigidBodyComponents.getNbEnabledComponents(); i++) { + const uint32 nbRigidBodyComponents = mRigidBodyComponents.getNbEnabledComponents(); + for (uint32 i=0; i < nbRigidBodyComponents; i++) { // If the gravity has to be applied to this rigid body if (mRigidBodyComponents.mIsGravityEnabled[i]) { @@ -162,7 +167,8 @@ void DynamicsSystem::integrateRigidBodiesVelocities(decimal timeStep) { // Using Taylor Serie for e^(-x) : e^x ~ 1 + x + x^2/2! + ... // => e^(-x) ~ 1 - x // => v2 = v1 * (1 - c * dt) - for (uint32 i=0; i < mRigidBodyComponents.getNbEnabledComponents(); i++) { + const uint32 nbRigidBodyComponents = mRigidBodyComponents.getNbEnabledComponents(); + for (uint32 i=0; i < nbRigidBodyComponents; i++) { const decimal linDampingFactor = mRigidBodyComponents.mLinearDampings[i]; const decimal angDampingFactor = mRigidBodyComponents.mAngularDampings[i]; @@ -177,7 +183,8 @@ void DynamicsSystem::integrateRigidBodiesVelocities(decimal timeStep) { void DynamicsSystem::resetBodiesForceAndTorque() { // For each body of the world - for (uint32 i=0; i < mRigidBodyComponents.getNbComponents(); i++) { + const uint32 nbRigidBodyComponents = mRigidBodyComponents.getNbComponents(); + for (uint32 i=0; i < nbRigidBodyComponents; i++) { mRigidBodyComponents.mExternalForces[i].setToZero(); mRigidBodyComponents.mExternalTorques[i].setToZero(); } @@ -186,7 +193,8 @@ void DynamicsSystem::resetBodiesForceAndTorque() { // Reset the split velocities of the bodies void DynamicsSystem::resetSplitVelocities() { - for(uint32 i=0; i < mRigidBodyComponents.getNbEnabledComponents(); i++) { + const uint32 nbRigidBodyComponents = mRigidBodyComponents.getNbEnabledComponents(); + for(uint32 i=0; i < nbRigidBodyComponents; i++) { mRigidBodyComponents.mSplitLinearVelocities[i].setToZero(); mRigidBodyComponents.mSplitAngularVelocities[i].setToZero(); } diff --git a/src/systems/SolveBallAndSocketJointSystem.cpp b/src/systems/SolveBallAndSocketJointSystem.cpp index f481d319..b4902b98 100644 --- a/src/systems/SolveBallAndSocketJointSystem.cpp +++ b/src/systems/SolveBallAndSocketJointSystem.cpp @@ -47,31 +47,25 @@ SolveBallAndSocketJointSystem::SolveBallAndSocketJointSystem(PhysicsWorld& world // Initialize before solving the constraint void SolveBallAndSocketJointSystem::initBeforeSolve() { + const decimal biasFactor = (BETA / mTimeStep); + // For each joint - for (uint32 i=0; i < mBallAndSocketJointComponents.getNbEnabledComponents(); i++) { + const uint32 nbJoints = mBallAndSocketJointComponents.getNbEnabledComponents(); + for (uint32 i=0; i < nbJoints; i++) { const Entity jointEntity = mBallAndSocketJointComponents.mJointEntities[i]; + const uint32 jointIndex = mJointComponents.getEntityIndex(jointEntity); // Get the bodies entities - const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity); - const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity); + const Entity body1Entity = mJointComponents.mBody1Entities[jointIndex]; + const Entity body2Entity = mJointComponents.mBody2Entities[jointIndex]; assert(!mRigidBodyComponents.getIsEntityDisabled(body1Entity)); assert(!mRigidBodyComponents.getIsEntityDisabled(body2Entity)); // Get the inertia tensor of bodies - mBallAndSocketJointComponents.mI1[i] = RigidBody::getWorldInertiaTensorInverse(mWorld, body1Entity); - mBallAndSocketJointComponents.mI2[i] = RigidBody::getWorldInertiaTensorInverse(mWorld, body2Entity); - } - - // For each joint - for (uint32 i=0; i < mBallAndSocketJointComponents.getNbEnabledComponents(); i++) { - - const Entity jointEntity = mBallAndSocketJointComponents.mJointEntities[i]; - - // Get the bodies entities - const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity); - const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity); + mBallAndSocketJointComponents.mI1[i] = mRigidBodyComponents.getInertiaTensorWorldInverse(body1Entity); + mBallAndSocketJointComponents.mI2[i] = mRigidBodyComponents.getInertiaTensorWorldInverse(body2Entity); const Quaternion& orientationBody1 = mTransformComponents.getTransform(body1Entity).getOrientation(); const Quaternion& orientationBody2 = mTransformComponents.getTransform(body2Entity).getOrientation(); @@ -79,12 +73,6 @@ void SolveBallAndSocketJointSystem::initBeforeSolve() { // Compute the vector from body center to the anchor point in world-space mBallAndSocketJointComponents.mR1World[i] = orientationBody1 * mBallAndSocketJointComponents.mLocalAnchorPointBody1[i]; mBallAndSocketJointComponents.mR2World[i] = orientationBody2 * mBallAndSocketJointComponents.mLocalAnchorPointBody2[i]; - } - - // For each joint - for (uint32 i=0; i < mBallAndSocketJointComponents.getNbEnabledComponents(); i++) { - - const Entity jointEntity = mBallAndSocketJointComponents.mJointEntities[i]; // Compute the corresponding skew-symmetric matrices const Vector3& r1World = mBallAndSocketJointComponents.mR1World[i]; @@ -92,10 +80,6 @@ void SolveBallAndSocketJointSystem::initBeforeSolve() { Matrix3x3 skewSymmetricMatrixU1 = Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(r1World); Matrix3x3 skewSymmetricMatrixU2 = Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(r2World); - // Get the bodies entities - const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity); - const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity); - const uint32 componentIndexBody1 = mRigidBodyComponents.getEntityIndex(body1Entity); const uint32 componentIndexBody2 = mRigidBodyComponents.getEntityIndex(body2Entity); @@ -117,37 +101,18 @@ void SolveBallAndSocketJointSystem::initBeforeSolve() { mRigidBodyComponents.mBodyTypes[componentIndexBody2] == BodyType::DYNAMIC) { mBallAndSocketJointComponents.mInverseMassMatrix[i] = massMatrix.getInverse(); } - } - - const decimal biasFactor = (BETA / mTimeStep); - - // For each joint - for (uint32 i=0; i < mBallAndSocketJointComponents.getNbEnabledComponents(); i++) { - - const Entity jointEntity = mBallAndSocketJointComponents.mJointEntities[i]; - - // Get the bodies entities - const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity); - const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity); - - const Vector3& r1World = mBallAndSocketJointComponents.mR1World[i]; - const Vector3& r2World = mBallAndSocketJointComponents.mR2World[i]; const Vector3& x1 = mRigidBodyComponents.getCenterOfMassWorld(body1Entity); const Vector3& x2 = mRigidBodyComponents.getCenterOfMassWorld(body2Entity); // Compute the bias "b" of the constraint mBallAndSocketJointComponents.mBiasVector[i].setToZero(); - if (mJointComponents.getPositionCorrectionTechnique(jointEntity) == JointsPositionCorrectionTechnique::BAUMGARTE_JOINTS) { + if (mJointComponents.mPositionCorrectionTechniques[jointIndex] == JointsPositionCorrectionTechnique::BAUMGARTE_JOINTS) { mBallAndSocketJointComponents.mBiasVector[i] = biasFactor * (x2 + r2World - x1 - r1World); } - } - // If warm-starting is not enabled - if (!mIsWarmStartingActive) { - - // For each joint - for (uint32 i=0; i < mBallAndSocketJointComponents.getNbEnabledComponents(); i++) { + // If warm-starting is not enabled + if (!mIsWarmStartingActive) { // Reset the accumulated impulse mBallAndSocketJointComponents.mImpulse[i].setToZero(); @@ -159,12 +124,14 @@ void SolveBallAndSocketJointSystem::initBeforeSolve() { void SolveBallAndSocketJointSystem::warmstart() { // For each joint component - for (uint32 i=0; i < mBallAndSocketJointComponents.getNbEnabledComponents(); i++) { + const uint32 nbJoints = mBallAndSocketJointComponents.getNbEnabledComponents(); + for (uint32 i=0; i < nbJoints; i++) { const Entity jointEntity = mBallAndSocketJointComponents.mJointEntities[i]; + const uint32 jointIndex = mJointComponents.getEntityIndex(jointEntity); - const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity); - const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity); + const Entity body1Entity = mJointComponents.mBody1Entities[jointIndex]; + const Entity body2Entity = mJointComponents.mBody2Entities[jointIndex]; const uint32 componentIndexBody1 = mRigidBodyComponents.getEntityIndex(body1Entity); const uint32 componentIndexBody2 = mRigidBodyComponents.getEntityIndex(body2Entity); @@ -202,12 +169,14 @@ void SolveBallAndSocketJointSystem::warmstart() { void SolveBallAndSocketJointSystem::solveVelocityConstraint() { // For each joint component - for (uint32 i=0; i < mBallAndSocketJointComponents.getNbEnabledComponents(); i++) { + const uint32 nbJoints = mBallAndSocketJointComponents.getNbEnabledComponents(); + for (uint32 i=0; i < nbJoints; i++) { const Entity jointEntity = mBallAndSocketJointComponents.mJointEntities[i]; + const uint32 jointIndex = mJointComponents.getEntityIndex(jointEntity); - const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity); - const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity); + const Entity body1Entity = mJointComponents.mBody1Entities[jointIndex]; + const Entity body2Entity = mJointComponents.mBody2Entities[jointIndex]; const uint32 componentIndexBody1 = mRigidBodyComponents.getEntityIndex(body1Entity); const uint32 componentIndexBody2 = mRigidBodyComponents.getEntityIndex(body2Entity); @@ -249,56 +218,38 @@ void SolveBallAndSocketJointSystem::solveVelocityConstraint() { void SolveBallAndSocketJointSystem::solvePositionConstraint() { // For each joint component - for (uint32 i=0; i < mBallAndSocketJointComponents.getNbEnabledComponents(); i++) { + const uint32 nbEnabledJoints = mBallAndSocketJointComponents.getNbEnabledComponents(); + for (uint32 i=0; i < nbEnabledJoints; i++) { const Entity jointEntity = mBallAndSocketJointComponents.mJointEntities[i]; + const uint32 jointIndex = mJointComponents.getEntityIndex(jointEntity); // If the error position correction technique is not the non-linear-gauss-seidel, we do // do not execute this method - if (mJointComponents.getPositionCorrectionTechnique(jointEntity) != JointsPositionCorrectionTechnique::NON_LINEAR_GAUSS_SEIDEL) continue; + if (mJointComponents.mPositionCorrectionTechniques[jointIndex] != JointsPositionCorrectionTechnique::NON_LINEAR_GAUSS_SEIDEL) continue; - const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity); - const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity); - - // Recompute the inverse inertia tensors - mBallAndSocketJointComponents.mI1[i] = RigidBody::getWorldInertiaTensorInverse(mWorld, body1Entity); - mBallAndSocketJointComponents.mI2[i] = RigidBody::getWorldInertiaTensorInverse(mWorld, body2Entity); - } - - // For each joint component - for (uint32 i=0; i < mBallAndSocketJointComponents.getNbEnabledComponents(); i++) { - - const Entity jointEntity = mBallAndSocketJointComponents.mJointEntities[i]; - - // If the error position correction technique is not the non-linear-gauss-seidel, we do - // do not execute this method - if (mJointComponents.getPositionCorrectionTechnique(jointEntity) != JointsPositionCorrectionTechnique::NON_LINEAR_GAUSS_SEIDEL) continue; - - const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity); - const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity); - - // Compute the vector from body center to the anchor point in world-space - mBallAndSocketJointComponents.mR1World[i] = mRigidBodyComponents.getConstrainedOrientation(body1Entity) * - mBallAndSocketJointComponents.mLocalAnchorPointBody1[i]; - mBallAndSocketJointComponents.mR2World[i] = mRigidBodyComponents.getConstrainedOrientation(body2Entity) * - mBallAndSocketJointComponents.mLocalAnchorPointBody2[i]; - } - - // For each joint component - for (uint32 i=0; i < mBallAndSocketJointComponents.getNbEnabledComponents(); i++) { - - const Entity jointEntity = mBallAndSocketJointComponents.mJointEntities[i]; - - // If the error position correction technique is not the non-linear-gauss-seidel, we do - // do not execute this method - if (mJointComponents.getPositionCorrectionTechnique(jointEntity) != JointsPositionCorrectionTechnique::NON_LINEAR_GAUSS_SEIDEL) continue; - - const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity); - const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity); + const Entity body1Entity = mJointComponents.mBody1Entities[jointIndex]; + const Entity body2Entity = mJointComponents.mBody2Entities[jointIndex]; const uint32 componentIndexBody1 = mRigidBodyComponents.getEntityIndex(body1Entity); const uint32 componentIndexBody2 = mRigidBodyComponents.getEntityIndex(body2Entity); + Quaternion& q1 = mRigidBodyComponents.mConstrainedOrientations[componentIndexBody1]; + Quaternion& q2 = mRigidBodyComponents.mConstrainedOrientations[componentIndexBody2]; + + // Recompute the world inverse inertia tensors + RigidBody::computeWorldInertiaTensorInverse(q1.getMatrix(), mRigidBodyComponents.mInverseInertiaTensorsLocal[componentIndexBody1], + mBallAndSocketJointComponents.mI1[i]); + + RigidBody::computeWorldInertiaTensorInverse(q2.getMatrix(), mRigidBodyComponents.mInverseInertiaTensorsLocal[componentIndexBody2], + mBallAndSocketJointComponents.mI2[i]); + + // Compute the vector from body center to the anchor point in world-space + mBallAndSocketJointComponents.mR1World[i] = mRigidBodyComponents.mConstrainedOrientations[componentIndexBody1] * + mBallAndSocketJointComponents.mLocalAnchorPointBody1[i]; + mBallAndSocketJointComponents.mR2World[i] = mRigidBodyComponents.mConstrainedOrientations[componentIndexBody2] * + mBallAndSocketJointComponents.mLocalAnchorPointBody2[i]; + const Vector3& r1World = mBallAndSocketJointComponents.mR1World[i]; const Vector3& r2World = mBallAndSocketJointComponents.mR2World[i]; @@ -322,29 +273,10 @@ void SolveBallAndSocketJointSystem::solvePositionConstraint() { mRigidBodyComponents.mBodyTypes[componentIndexBody2] == BodyType::DYNAMIC) { mBallAndSocketJointComponents.mInverseMassMatrix[i] = massMatrix.getInverse(); } - } - - // For each joint component - for (uint32 i=0; i < mBallAndSocketJointComponents.getNbEnabledComponents(); i++) { - - const Entity jointEntity = mBallAndSocketJointComponents.mJointEntities[i]; - - // If the error position correction technique is not the non-linear-gauss-seidel, we do - // do not execute this method - if (mJointComponents.getPositionCorrectionTechnique(jointEntity) != JointsPositionCorrectionTechnique::NON_LINEAR_GAUSS_SEIDEL) continue; - - const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity); - const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity); - - const uint32 componentIndexBody1 = mRigidBodyComponents.getEntityIndex(body1Entity); - const uint32 componentIndexBody2 = mRigidBodyComponents.getEntityIndex(body2Entity); Vector3& x1 = mRigidBodyComponents.mConstrainedPositions[componentIndexBody1]; Vector3& x2 = mRigidBodyComponents.mConstrainedPositions[componentIndexBody2]; - const Vector3& r1World = mBallAndSocketJointComponents.mR1World[i]; - const Vector3& r2World = mBallAndSocketJointComponents.mR2World[i]; - // Compute the constraint error (value of the C(x) function) const Vector3 constraintError = (x2 + r2World - x1 - r1World); @@ -357,17 +289,10 @@ void SolveBallAndSocketJointSystem::solvePositionConstraint() { const Vector3 linearImpulseBody1 = -lambda; const Vector3 angularImpulseBody1 = lambda.cross(r1World); - // Get the inverse mass and inverse inertia tensors of the bodies - const decimal inverseMassBody1 = mRigidBodyComponents.mInverseMasses[componentIndexBody1]; - const decimal inverseMassBody2 = mRigidBodyComponents.mInverseMasses[componentIndexBody2]; - // Compute the pseudo velocity of body 1 const Vector3 v1 = inverseMassBody1 * linearImpulseBody1; const Vector3 w1 = mBallAndSocketJointComponents.mI1[i] * angularImpulseBody1; - Quaternion& q1 = mRigidBodyComponents.mConstrainedOrientations[componentIndexBody1]; - Quaternion& q2 = mRigidBodyComponents.mConstrainedOrientations[componentIndexBody2]; - // Update the body center of mass and orientation of body 1 x1 += v1; q1 += Quaternion(0, w1) * q1 * decimal(0.5); diff --git a/src/systems/SolveFixedJointSystem.cpp b/src/systems/SolveFixedJointSystem.cpp index 7143bb43..e120bfcc 100644 --- a/src/systems/SolveFixedJointSystem.cpp +++ b/src/systems/SolveFixedJointSystem.cpp @@ -47,31 +47,25 @@ SolveFixedJointSystem::SolveFixedJointSystem(PhysicsWorld& world, RigidBodyCompo // Initialize before solving the constraint void SolveFixedJointSystem::initBeforeSolve() { + const decimal biasFactor = BETA / mTimeStep; + // For each joint - for (uint32 i=0; i < mFixedJointComponents.getNbEnabledComponents(); i++) { + const uint32 nbJoints = mFixedJointComponents.getNbEnabledComponents(); + for (uint32 i=0; i < nbJoints; i++) { const Entity jointEntity = mFixedJointComponents.mJointEntities[i]; + const uint32 jointIndex = mJointComponents.getEntityIndex(jointEntity); // Get the bodies entities - const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity); - const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity); + const Entity body1Entity = mJointComponents.mBody1Entities[jointIndex]; + const Entity body2Entity = mJointComponents.mBody2Entities[jointIndex]; assert(!mRigidBodyComponents.getIsEntityDisabled(body1Entity)); assert(!mRigidBodyComponents.getIsEntityDisabled(body2Entity)); // Get the inertia tensor of bodies - mFixedJointComponents.mI1[i] = RigidBody::getWorldInertiaTensorInverse(mWorld, body1Entity); - mFixedJointComponents.mI2[i] = RigidBody::getWorldInertiaTensorInverse(mWorld, body2Entity); - } - - // For each joint - for (uint32 i=0; i < mFixedJointComponents.getNbEnabledComponents(); i++) { - - const Entity jointEntity = mFixedJointComponents.mJointEntities[i]; - - // Get the bodies entities - const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity); - const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity); + mFixedJointComponents.mI1[i] = mRigidBodyComponents.getInertiaTensorWorldInverse(body1Entity); + mFixedJointComponents.mI2[i] = mRigidBodyComponents.getInertiaTensorWorldInverse(body2Entity); const Quaternion& orientationBody1 = mTransformComponents.getTransform(body1Entity).getOrientation(); const Quaternion& orientationBody2 = mTransformComponents.getTransform(body2Entity).getOrientation(); @@ -79,16 +73,6 @@ void SolveFixedJointSystem::initBeforeSolve() { // Compute the vector from body center to the anchor point in world-space mFixedJointComponents.mR1World[i] = orientationBody1 * mFixedJointComponents.mLocalAnchorPointBody1[i]; mFixedJointComponents.mR2World[i] = orientationBody2 * mFixedJointComponents.mLocalAnchorPointBody2[i]; - } - - // For each joint - for (uint32 i=0; i < mFixedJointComponents.getNbEnabledComponents(); i++) { - - const Entity jointEntity = mFixedJointComponents.mJointEntities[i]; - - // Get the bodies entities - const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity); - const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity); // Compute the corresponding skew-symmetric matrices Matrix3x3 skewSymmetricMatrixU1 = Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mFixedJointComponents.mR1World[i]); @@ -113,18 +97,6 @@ void SolveFixedJointSystem::initBeforeSolve() { mRigidBodyComponents.mBodyTypes[componentIndexBody2] == BodyType::DYNAMIC) { mFixedJointComponents.mInverseMassMatrixTranslation[i] = massMatrix.getInverse(); } - } - - const decimal biasFactor = BETA / mTimeStep; - - // For each joint - for (uint32 i=0; i < mFixedJointComponents.getNbEnabledComponents(); i++) { - - const Entity jointEntity = mFixedJointComponents.mJointEntities[i]; - - // Get the bodies entities - const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity); - const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity); // Get the bodies positions and orientations const Vector3& x1 = mRigidBodyComponents.getCenterOfMassWorld(body1Entity); @@ -135,19 +107,9 @@ void SolveFixedJointSystem::initBeforeSolve() { // Compute the bias "b" of the constraint for the 3 translation constraints mFixedJointComponents.mBiasTranslation[i].setToZero(); - if (mJointComponents.getPositionCorrectionTechnique(jointEntity) == JointsPositionCorrectionTechnique::BAUMGARTE_JOINTS) { + if (mJointComponents.mPositionCorrectionTechniques[jointIndex] == JointsPositionCorrectionTechnique::BAUMGARTE_JOINTS) { mFixedJointComponents.mBiasTranslation[i] = biasFactor * (x2 + r2World - x1 - r1World); } - } - - // For each joint - for (uint32 i=0; i < mFixedJointComponents.getNbEnabledComponents(); i++) { - - const Entity jointEntity = mFixedJointComponents.mJointEntities[i]; - - // Get the bodies entities - const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity); - const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity); // Compute the inverse of the mass matrix K=JM^-1J^t for the 3 rotation contraints (3x3 matrix) mFixedJointComponents.mInverseMassMatrixRotation[i] = mFixedJointComponents.mI1[i] + mFixedJointComponents.mI2[i]; @@ -155,34 +117,17 @@ void SolveFixedJointSystem::initBeforeSolve() { mRigidBodyComponents.getBodyType(body2Entity) == BodyType::DYNAMIC) { mFixedJointComponents.mInverseMassMatrixRotation[i] = mFixedJointComponents.mInverseMassMatrixRotation[i].getInverse(); } - } - - // For each joint - for (uint32 i=0; i < mFixedJointComponents.getNbEnabledComponents(); i++) { - - const Entity jointEntity = mFixedJointComponents.mJointEntities[i]; - - // Get the bodies entities - const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity); - const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity); // Compute the bias "b" for the 3 rotation constraints mFixedJointComponents.mBiasRotation[i].setToZero(); - const Quaternion& orientationBody1 = mTransformComponents.getTransform(body1Entity).getOrientation(); - const Quaternion& orientationBody2 = mTransformComponents.getTransform(body2Entity).getOrientation(); - - if (mJointComponents.getPositionCorrectionTechnique(jointEntity) == JointsPositionCorrectionTechnique::BAUMGARTE_JOINTS) { + if (mJointComponents.mPositionCorrectionTechniques[jointIndex] == JointsPositionCorrectionTechnique::BAUMGARTE_JOINTS) { const Quaternion qError = orientationBody2 * mFixedJointComponents.mInitOrientationDifferenceInv[i] * orientationBody1.getInverse(); mFixedJointComponents.mBiasRotation[i] = biasFactor * decimal(2.0) * qError.getVectorV(); } - } - // If warm-starting is not enabled - if (!mIsWarmStartingActive) { - - // For each joint - for (uint32 i=0; i < mFixedJointComponents.getNbEnabledComponents(); i++) { + // If warm-starting is not enabled + if (!mIsWarmStartingActive) { // Reset the accumulated impulses mFixedJointComponents.mImpulseTranslation[i].setToZero(); @@ -195,13 +140,15 @@ void SolveFixedJointSystem::initBeforeSolve() { void SolveFixedJointSystem::warmstart() { // For each joint - for (uint32 i=0; i < mFixedJointComponents.getNbEnabledComponents(); i++) { + const uint32 nbJoints = mFixedJointComponents.getNbEnabledComponents(); + for (uint32 i=0; i < nbJoints; i++) { const Entity jointEntity = mFixedJointComponents.mJointEntities[i]; + const uint32 jointIndex = mJointComponents.getEntityIndex(jointEntity); // Get the bodies entities - const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity); - const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity); + const Entity body1Entity = mJointComponents.mBody1Entities[jointIndex]; + const Entity body2Entity = mJointComponents.mBody2Entities[jointIndex]; const uint32 componentIndexBody1 = mRigidBodyComponents.getEntityIndex(body1Entity); const uint32 componentIndexBody2 = mRigidBodyComponents.getEntityIndex(body2Entity); @@ -253,13 +200,15 @@ void SolveFixedJointSystem::warmstart() { void SolveFixedJointSystem::solveVelocityConstraint() { // For each joint - for (uint32 i=0; i < mFixedJointComponents.getNbEnabledComponents(); i++) { + const uint32 nbJoints = mFixedJointComponents.getNbEnabledComponents(); + for (uint32 i=0; i < nbJoints; i++) { const Entity jointEntity = mFixedJointComponents.mJointEntities[i]; + const uint32 jointIndex = mJointComponents.getEntityIndex(jointEntity); // Get the bodies entities - const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity); - const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity); + const Entity body1Entity = mJointComponents.mBody1Entities[jointIndex]; + const Entity body2Entity = mJointComponents.mBody2Entities[jointIndex]; const uint32 componentIndexBody1 = mRigidBodyComponents.getEntityIndex(body1Entity); const uint32 componentIndexBody2 = mRigidBodyComponents.getEntityIndex(body2Entity); @@ -334,61 +283,38 @@ void SolveFixedJointSystem::solveVelocityConstraint() { void SolveFixedJointSystem::solvePositionConstraint() { // For each joint - for (uint32 i=0; i < mFixedJointComponents.getNbEnabledComponents(); i++) { + const uint32 nbEnabledJoints = mFixedJointComponents.getNbEnabledComponents(); + for (uint32 i=0; i < nbEnabledJoints; i++) { const Entity jointEntity = mFixedJointComponents.mJointEntities[i]; + const uint32 jointIndex = mJointComponents.getEntityIndex(jointEntity); // If the error position correction technique is not the non-linear-gauss-seidel, we do // do not execute this method - if (mJointComponents.getPositionCorrectionTechnique(jointEntity) != JointsPositionCorrectionTechnique::NON_LINEAR_GAUSS_SEIDEL) continue; + if (mJointComponents.mPositionCorrectionTechniques[jointIndex] != JointsPositionCorrectionTechnique::NON_LINEAR_GAUSS_SEIDEL) continue; // Get the bodies entities - const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity); - const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity); - - // Recompute the inverse inertia tensors - mFixedJointComponents.mI1[i] = RigidBody::getWorldInertiaTensorInverse(mWorld, body1Entity); - mFixedJointComponents.mI2[i] = RigidBody::getWorldInertiaTensorInverse(mWorld, body2Entity); - } - - // For each joint - for (uint32 i=0; i < mFixedJointComponents.getNbEnabledComponents(); i++) { - - const Entity jointEntity = mFixedJointComponents.mJointEntities[i]; - - // If the error position correction technique is not the non-linear-gauss-seidel, we do - // do not execute this method - if (mJointComponents.getPositionCorrectionTechnique(jointEntity) != JointsPositionCorrectionTechnique::NON_LINEAR_GAUSS_SEIDEL) continue; - - // Get the bodies entities - const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity); - const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity); - - // Get the bodies positions and orientations - const Quaternion& q1 = mRigidBodyComponents.getConstrainedOrientation(body1Entity); - const Quaternion& q2 = mRigidBodyComponents.getConstrainedOrientation(body2Entity); - - // Compute the vector from body center to the anchor point in world-space - mFixedJointComponents.mR1World[i] = q1 * mFixedJointComponents.getLocalAnchorPointBody1(jointEntity); - mFixedJointComponents.mR2World[i] = q2 * mFixedJointComponents.getLocalAnchorPointBody2(jointEntity); - } - - // For each joint - for (uint32 i=0; i < mFixedJointComponents.getNbEnabledComponents(); i++) { - - const Entity jointEntity = mFixedJointComponents.mJointEntities[i]; - - // If the error position correction technique is not the non-linear-gauss-seidel, we do - // do not execute this method - if (mJointComponents.getPositionCorrectionTechnique(jointEntity) != JointsPositionCorrectionTechnique::NON_LINEAR_GAUSS_SEIDEL) continue; - - // Get the bodies entities - const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity); - const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity); + const Entity body1Entity = mJointComponents.mBody1Entities[jointIndex]; + const Entity body2Entity = mJointComponents.mBody2Entities[jointIndex]; const uint32 componentIndexBody1 = mRigidBodyComponents.getEntityIndex(body1Entity); const uint32 componentIndexBody2 = mRigidBodyComponents.getEntityIndex(body2Entity); + // Get the bodies positions and orientations + Quaternion& q1 = mRigidBodyComponents.mConstrainedOrientations[componentIndexBody1]; + Quaternion& q2 = mRigidBodyComponents.mConstrainedOrientations[componentIndexBody2]; + + // Recompute the world inverse inertia tensors + RigidBody::computeWorldInertiaTensorInverse(q1.getMatrix(), mRigidBodyComponents.getInertiaTensorLocalInverse(body1Entity), + mFixedJointComponents.mI1[i]); + + RigidBody::computeWorldInertiaTensorInverse(q2.getMatrix(), mRigidBodyComponents.getInertiaTensorLocalInverse(body2Entity), + mFixedJointComponents.mI2[i]); + + // Compute the vector from body center to the anchor point in world-space + mFixedJointComponents.mR1World[i] = q1 * mFixedJointComponents.mLocalAnchorPointBody1[i]; + mFixedJointComponents.mR2World[i] = q2 * mFixedJointComponents.mLocalAnchorPointBody2[i]; + // Get the inverse mass and inverse inertia tensors of the bodies decimal inverseMassBody1 = mRigidBodyComponents.mInverseMasses[componentIndexBody1]; decimal inverseMassBody2 = mRigidBodyComponents.mInverseMasses[componentIndexBody2]; @@ -414,32 +340,9 @@ void SolveFixedJointSystem::solvePositionConstraint() { mRigidBodyComponents.mBodyTypes[componentIndexBody2] == BodyType::DYNAMIC) { mFixedJointComponents.mInverseMassMatrixTranslation[i] = massMatrix.getInverse(); } - } - - // For each joint - for (uint32 i=0; i < mFixedJointComponents.getNbEnabledComponents(); i++) { - - const Entity jointEntity = mFixedJointComponents.mJointEntities[i]; - - // If the error position correction technique is not the non-linear-gauss-seidel, we do - // do not execute this method - if (mJointComponents.getPositionCorrectionTechnique(jointEntity) != JointsPositionCorrectionTechnique::NON_LINEAR_GAUSS_SEIDEL) continue; - - // Get the bodies entities - const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity); - const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity); - - const Vector3& r1World = mFixedJointComponents.mR1World[i]; - const Vector3& r2World = mFixedJointComponents.mR2World[i]; - - const uint32 componentIndexBody1 = mRigidBodyComponents.getEntityIndex(body1Entity); - const uint32 componentIndexBody2 = mRigidBodyComponents.getEntityIndex(body2Entity); Vector3& x1 = mRigidBodyComponents.mConstrainedPositions[componentIndexBody1]; Vector3& x2 = mRigidBodyComponents.mConstrainedPositions[componentIndexBody2]; - Quaternion& q1 = mRigidBodyComponents.mConstrainedOrientations[componentIndexBody1]; - Quaternion& q2 = mRigidBodyComponents.mConstrainedOrientations[componentIndexBody2]; - // Compute position error for the 3 translation constraints const Vector3 errorTranslation = x2 + r2World - x1 - r1World; @@ -450,11 +353,9 @@ void SolveFixedJointSystem::solvePositionConstraint() { Vector3 linearImpulseBody1 = -lambdaTranslation; Vector3 angularImpulseBody1 = lambdaTranslation.cross(r1World); - const decimal inverseMassBody1 = mRigidBodyComponents.mInverseMasses[componentIndexBody1]; - // Compute the pseudo velocity of body 1 const Vector3 v1 = inverseMassBody1 * linearImpulseBody1; - Vector3 w1 = mFixedJointComponents.mI2[i] * angularImpulseBody1; + Vector3 w1 = mFixedJointComponents.mI1[i] * angularImpulseBody1; // Update the body position/orientation of body 1 x1 += v1; @@ -464,8 +365,6 @@ void SolveFixedJointSystem::solvePositionConstraint() { // Compute the impulse of body 2 Vector3 angularImpulseBody2 = -lambdaTranslation.cross(r2World); - const decimal inverseMassBody2 = mRigidBodyComponents.mInverseMasses[componentIndexBody2]; - // Compute the pseudo velocity of body 2 const Vector3 v2 = inverseMassBody2 * lambdaTranslation; Vector3 w2 = mFixedJointComponents.mI2[i] * angularImpulseBody2; diff --git a/src/systems/SolveHingeJointSystem.cpp b/src/systems/SolveHingeJointSystem.cpp index a1582750..5a061f3f 100644 --- a/src/systems/SolveHingeJointSystem.cpp +++ b/src/systems/SolveHingeJointSystem.cpp @@ -47,31 +47,28 @@ SolveHingeJointSystem::SolveHingeJointSystem(PhysicsWorld& world, RigidBodyCompo // Initialize before solving the constraint void SolveHingeJointSystem::initBeforeSolve() { + const decimal biasFactor = (BETA / mTimeStep); + // For each joint - for (uint32 i=0; i < mHingeJointComponents.getNbEnabledComponents(); i++) { + const uint32 nbJoints = mHingeJointComponents.getNbEnabledComponents(); + for (uint32 i=0; i < nbJoints; i++) { const Entity jointEntity = mHingeJointComponents.mJointEntities[i]; + const uint32 jointIndex = mJointComponents.getEntityIndex(jointEntity); // Get the bodies entities - const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity); - const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity); + const Entity body1Entity = mJointComponents.mBody1Entities[jointIndex]; + const Entity body2Entity = mJointComponents.mBody2Entities[jointIndex]; + + const uint32 componentIndexBody1 = mRigidBodyComponents.getEntityIndex(body1Entity); + const uint32 componentIndexBody2 = mRigidBodyComponents.getEntityIndex(body2Entity); assert(!mRigidBodyComponents.getIsEntityDisabled(body1Entity)); assert(!mRigidBodyComponents.getIsEntityDisabled(body2Entity)); // Get the inertia tensor of bodies - mHingeJointComponents.mI1[i] = RigidBody::getWorldInertiaTensorInverse(mWorld, body1Entity); - mHingeJointComponents.mI2[i] = RigidBody::getWorldInertiaTensorInverse(mWorld, body2Entity); - } - - // For each joint - for (uint32 i=0; i < mHingeJointComponents.getNbEnabledComponents(); i++) { - - const Entity jointEntity = mHingeJointComponents.mJointEntities[i]; - - // Get the bodies entities - const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity); - const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity); + mHingeJointComponents.mI1[i] = mRigidBodyComponents.mInverseInertiaTensorsWorld[componentIndexBody1]; + mHingeJointComponents.mI2[i] = mRigidBodyComponents.mInverseInertiaTensorsWorld[componentIndexBody2]; const Quaternion& orientationBody1 = mTransformComponents.getTransform(body1Entity).getOrientation(); const Quaternion& orientationBody2 = mTransformComponents.getTransform(body2Entity).getOrientation(); @@ -79,21 +76,6 @@ void SolveHingeJointSystem::initBeforeSolve() { // Compute the vector from body center to the anchor point in world-space mHingeJointComponents.mR1World[i] = orientationBody1 * mHingeJointComponents.mLocalAnchorPointBody1[i]; mHingeJointComponents.mR2World[i] = orientationBody2 * mHingeJointComponents.mLocalAnchorPointBody2[i]; - } - - const decimal biasFactor = (BETA / mTimeStep); - - // For each joint - for (uint32 i=0; i < mHingeJointComponents.getNbEnabledComponents(); i++) { - - const Entity jointEntity = mHingeJointComponents.mJointEntities[i]; - - // Get the bodies entities - const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity); - const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity); - - const Quaternion& orientationBody1 = mTransformComponents.getTransform(body1Entity).getOrientation(); - const Quaternion& orientationBody2 = mTransformComponents.getTransform(body2Entity).getOrientation(); // Compute vectors needed in the Jacobian Vector3& a1 = mHingeJointComponents.mA1[i]; @@ -109,27 +91,14 @@ void SolveHingeJointSystem::initBeforeSolve() { // Compute the bias "b" of the rotation constraints mHingeJointComponents.mBiasRotation[i].setToZero(); - if (mJointComponents.getPositionCorrectionTechnique(jointEntity) == JointsPositionCorrectionTechnique::BAUMGARTE_JOINTS) { + if (mJointComponents.mPositionCorrectionTechniques[jointIndex] == JointsPositionCorrectionTechnique::BAUMGARTE_JOINTS) { mHingeJointComponents.mBiasRotation[i] = biasFactor * Vector2(a1.dot(b2), a1.dot(c2)); } - } - - // For each joint - for (uint32 i=0; i < mHingeJointComponents.getNbEnabledComponents(); i++) { - - const Entity jointEntity = mHingeJointComponents.mJointEntities[i]; - - // Get the bodies entities - const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity); - const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity); // Compute the corresponding skew-symmetric matrices Matrix3x3 skewSymmetricMatrixU1= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mHingeJointComponents.mR1World[i]); Matrix3x3 skewSymmetricMatrixU2= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mHingeJointComponents.mR2World[i]); - const uint32 componentIndexBody1 = mRigidBodyComponents.getEntityIndex(body1Entity); - const uint32 componentIndexBody2 = mRigidBodyComponents.getEntityIndex(body2Entity); - // Compute the inverse mass matrix K=JM^-1J^t for the 3 translation constraints (3x3 matrix) decimal body1MassInverse = mRigidBodyComponents.mInverseMasses[componentIndexBody1]; decimal body2MassInverse = mRigidBodyComponents.mInverseMasses[componentIndexBody2]; @@ -145,36 +114,16 @@ void SolveHingeJointSystem::initBeforeSolve() { mRigidBodyComponents.mBodyTypes[componentIndexBody2] == BodyType::DYNAMIC) { mHingeJointComponents.mInverseMassMatrixTranslation[i] = massMatrix.getInverse(); } - } - - // For each joint - for (uint32 i=0; i < mHingeJointComponents.getNbEnabledComponents(); i++) { - - const Entity jointEntity = mHingeJointComponents.mJointEntities[i]; - - // Get the bodies entities - const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity); - const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity); // Get the bodies positions and orientations - const Vector3& x1 = mRigidBodyComponents.getCenterOfMassWorld(body1Entity); - const Vector3& x2 = mRigidBodyComponents.getCenterOfMassWorld(body2Entity); + const Vector3& x1 = mRigidBodyComponents.mCentersOfMassWorld[componentIndexBody1]; + const Vector3& x2 = mRigidBodyComponents.mCentersOfMassWorld[componentIndexBody2]; // Compute the bias "b" of the translation constraints mHingeJointComponents.mBiasTranslation[i].setToZero(); - if (mJointComponents.getPositionCorrectionTechnique(jointEntity) == JointsPositionCorrectionTechnique::BAUMGARTE_JOINTS) { + if (mJointComponents.mPositionCorrectionTechniques[jointIndex] == JointsPositionCorrectionTechnique::BAUMGARTE_JOINTS) { mHingeJointComponents.mBiasTranslation[i] = biasFactor * (x2 + mHingeJointComponents.mR2World[i] - x1 - mHingeJointComponents.mR1World[i]); } - } - - // For each joint - for (uint32 i=0; i < mHingeJointComponents.getNbEnabledComponents(); i++) { - - const Entity jointEntity = mHingeJointComponents.mJointEntities[i]; - - // Get the bodies entities - const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity); - const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity); const Matrix3x3& i1 = mHingeJointComponents.mI1[i]; const Matrix3x3& i2 = mHingeJointComponents.mI2[i]; @@ -192,17 +141,13 @@ void SolveHingeJointSystem::initBeforeSolve() { const decimal el22 = c2CrossA1.dot(i1C2CrossA1) + c2CrossA1.dot(i2C2CrossA1); const Matrix2x2 matrixKRotation(el11, el12, el21, el22); mHingeJointComponents.mInverseMassMatrixRotation[i].setToZero(); - if (mRigidBodyComponents.getBodyType(body1Entity) == BodyType::DYNAMIC || - mRigidBodyComponents.getBodyType(body2Entity) == BodyType::DYNAMIC) { + if (mRigidBodyComponents.mBodyTypes[componentIndexBody1] == BodyType::DYNAMIC || + mRigidBodyComponents.mBodyTypes[componentIndexBody2] == BodyType::DYNAMIC) { mHingeJointComponents.mInverseMassMatrixRotation[i] = matrixKRotation.getInverse(); } - } - // If warm-starting is not enabled - if (!mIsWarmStartingActive) { - - // For each joint - for (uint32 i=0; i < mHingeJointComponents.getNbEnabledComponents(); i++) { + // If warm-starting is not enabled + if (!mIsWarmStartingActive) { // Reset all the accumulated impulses mHingeJointComponents.mImpulseTranslation[i].setToZero(); @@ -211,19 +156,6 @@ void SolveHingeJointSystem::initBeforeSolve() { mHingeJointComponents.mImpulseUpperLimit[i] = decimal(0.0); mHingeJointComponents.mImpulseMotor[i] = decimal(0.0); } - } - - // For each joint - for (uint32 i=0; i < mHingeJointComponents.getNbEnabledComponents(); i++) { - - const Entity jointEntity = mHingeJointComponents.mJointEntities[i]; - - // Get the bodies entities - const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity); - const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity); - - const Quaternion& orientationBody1 = mTransformComponents.getTransform(body1Entity).getOrientation(); - const Quaternion& orientationBody2 = mTransformComponents.getTransform(body2Entity).getOrientation(); // Compute the current angle around the hinge axis decimal hingeAngle = computeCurrentHingeAngle(jointEntity, orientationBody1, orientationBody2); @@ -261,13 +193,13 @@ void SolveHingeJointSystem::initBeforeSolve() { // Compute the bias "b" of the lower limit constraint mHingeJointComponents.mBLowerLimit[i] = decimal(0.0); - if (mJointComponents.getPositionCorrectionTechnique(jointEntity) == JointsPositionCorrectionTechnique::BAUMGARTE_JOINTS) { + if (mJointComponents.mPositionCorrectionTechniques[jointIndex] == JointsPositionCorrectionTechnique::BAUMGARTE_JOINTS) { mHingeJointComponents.mBLowerLimit[i] = biasFactor * lowerLimitError; } // Compute the bias "b" of the upper limit constraint mHingeJointComponents.mBUpperLimit[i] = decimal(0.0); - if (mJointComponents.getPositionCorrectionTechnique(jointEntity) == JointsPositionCorrectionTechnique::BAUMGARTE_JOINTS) { + if (mJointComponents.mPositionCorrectionTechniques[jointIndex] == JointsPositionCorrectionTechnique::BAUMGARTE_JOINTS) { mHingeJointComponents.mBUpperLimit[i] = biasFactor * upperLimitError; } } @@ -279,13 +211,15 @@ void SolveHingeJointSystem::initBeforeSolve() { void SolveHingeJointSystem::warmstart() { // For each joint component - for (uint32 i=0; i < mHingeJointComponents.getNbEnabledComponents(); i++) { + const uint32 nbJoints = mHingeJointComponents.getNbEnabledComponents(); + for (uint32 i=0; i < nbJoints; i++) { const Entity jointEntity = mHingeJointComponents.mJointEntities[i]; + const uint32 jointIndex = mJointComponents.getEntityIndex(jointEntity); // Get the bodies entities - const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity); - const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity); + const Entity body1Entity = mJointComponents.mBody1Entities[jointIndex]; + const Entity body2Entity = mJointComponents.mBody2Entities[jointIndex]; const uint32 componentIndexBody1 = mRigidBodyComponents.getEntityIndex(body1Entity); const uint32 componentIndexBody2 = mRigidBodyComponents.getEntityIndex(body2Entity); @@ -357,13 +291,15 @@ void SolveHingeJointSystem::warmstart() { void SolveHingeJointSystem::solveVelocityConstraint() { // For each joint component - for (uint32 i=0; i < mHingeJointComponents.getNbEnabledComponents(); i++) { + const uint32 nbJoints = mHingeJointComponents.getNbEnabledComponents(); + for (uint32 i=0; i < nbJoints; i++) { const Entity jointEntity = mHingeJointComponents.mJointEntities[i]; + const uint32 jointIndex = mJointComponents.getEntityIndex(jointEntity); // Get the bodies entities - const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity); - const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity); + const Entity body1Entity = mJointComponents.mBody1Entities[jointIndex]; + const Entity body2Entity = mJointComponents.mBody2Entities[jointIndex]; const uint32 componentIndexBody1 = mRigidBodyComponents.getEntityIndex(body1Entity); const uint32 componentIndexBody2 = mRigidBodyComponents.getEntityIndex(body2Entity); @@ -528,53 +464,35 @@ void SolveHingeJointSystem::solveVelocityConstraint() { void SolveHingeJointSystem::solvePositionConstraint() { // For each joint component - for (uint32 i=0; i < mHingeJointComponents.getNbEnabledComponents(); i++) { + const uint32 nbEnabledJoints = mHingeJointComponents.getNbEnabledComponents(); + for (uint32 i=0; i < nbEnabledJoints; i++) { const Entity jointEntity = mHingeJointComponents.mJointEntities[i]; + const uint32 jointIndex = mJointComponents.getEntityIndex(jointEntity); // If the error position correction technique is not the non-linear-gauss-seidel, we do not execute this method - if (mJointComponents.getPositionCorrectionTechnique(jointEntity) != JointsPositionCorrectionTechnique::NON_LINEAR_GAUSS_SEIDEL) continue; + if (mJointComponents.mPositionCorrectionTechniques[jointIndex] != JointsPositionCorrectionTechnique::NON_LINEAR_GAUSS_SEIDEL) continue; // Get the bodies entities - Entity body1Entity = mJointComponents.getBody1Entity(jointEntity); - Entity body2Entity = mJointComponents.getBody2Entity(jointEntity); + Entity body1Entity = mJointComponents.mBody1Entities[jointIndex]; + Entity body2Entity = mJointComponents.mBody2Entities[jointIndex]; - // Recompute the inverse inertia tensors - mHingeJointComponents.mI1[i] = RigidBody::getWorldInertiaTensorInverse(mWorld, body1Entity); - mHingeJointComponents.mI2[i] = RigidBody::getWorldInertiaTensorInverse(mWorld, body2Entity); - } + const uint32 componentIndexBody1 = mRigidBodyComponents.getEntityIndex(body1Entity); + const uint32 componentIndexBody2 = mRigidBodyComponents.getEntityIndex(body2Entity); - // For each joint component - for (uint32 i=0; i < mHingeJointComponents.getNbEnabledComponents(); i++) { + Quaternion& q1 = mRigidBodyComponents.mConstrainedOrientations[componentIndexBody1]; + Quaternion& q2 = mRigidBodyComponents.mConstrainedOrientations[componentIndexBody2]; - const Entity jointEntity = mHingeJointComponents.mJointEntities[i]; + // Recompute the world inverse inertia tensors + RigidBody::computeWorldInertiaTensorInverse(q1.getMatrix(), mRigidBodyComponents.mInverseInertiaTensorsLocal[componentIndexBody1], + mHingeJointComponents.mI1[i]); - // If the error position correction technique is not the non-linear-gauss-seidel, we do not execute this method - if (mJointComponents.getPositionCorrectionTechnique(jointEntity) != JointsPositionCorrectionTechnique::NON_LINEAR_GAUSS_SEIDEL) continue; - - // Get the bodies entities - const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity); - const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity); - - const Quaternion& q1 = mRigidBodyComponents.getConstrainedOrientation(body1Entity); - const Quaternion& q2 = mRigidBodyComponents.getConstrainedOrientation(body2Entity); + RigidBody::computeWorldInertiaTensorInverse(q2.getMatrix(), mRigidBodyComponents.mInverseInertiaTensorsLocal[componentIndexBody2], + mHingeJointComponents.mI2[i]); // Compute the vector from body center to the anchor point in world-space mHingeJointComponents.mR1World[i] = q1 * mHingeJointComponents.mLocalAnchorPointBody1[i]; mHingeJointComponents.mR2World[i] = q2 * mHingeJointComponents.mLocalAnchorPointBody2[i]; - } - - // For each joint component - for (uint32 i=0; i < mHingeJointComponents.getNbEnabledComponents(); i++) { - - const Entity jointEntity = mHingeJointComponents.mJointEntities[i]; - - // If the error position correction technique is not the non-linear-gauss-seidel, we do not execute this method - if (mJointComponents.getPositionCorrectionTechnique(jointEntity) != JointsPositionCorrectionTechnique::NON_LINEAR_GAUSS_SEIDEL) continue; - - // Get the bodies entities - const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity); - const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity); // Compute the corresponding skew-symmetric matrices Matrix3x3 skewSymmetricMatrixU1 = Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mHingeJointComponents.mR1World[i]); @@ -582,9 +500,6 @@ void SolveHingeJointSystem::solvePositionConstraint() { // --------------- Translation Constraints --------------- // - const uint32 componentIndexBody1 = mRigidBodyComponents.getEntityIndex(body1Entity); - const uint32 componentIndexBody2 = mRigidBodyComponents.getEntityIndex(body2Entity); - // Compute the matrix K=JM^-1J^t (3x3 matrix) for the 3 translation constraints const decimal body1InverseMass = mRigidBodyComponents.mInverseMasses[componentIndexBody1]; const decimal body2InverseMass = mRigidBodyComponents.mInverseMasses[componentIndexBody2]; @@ -599,25 +514,6 @@ void SolveHingeJointSystem::solvePositionConstraint() { mRigidBodyComponents.mBodyTypes[componentIndexBody2] == BodyType::DYNAMIC) { mHingeJointComponents.mInverseMassMatrixTranslation[i] = massMatrix.getInverse(); } - } - - // For each joint component - for (uint32 i=0; i < mHingeJointComponents.getNbEnabledComponents(); i++) { - - const Entity jointEntity = mHingeJointComponents.mJointEntities[i]; - - // If the error position correction technique is not the non-linear-gauss-seidel, we do not execute this method - if (mJointComponents.getPositionCorrectionTechnique(jointEntity) != JointsPositionCorrectionTechnique::NON_LINEAR_GAUSS_SEIDEL) continue; - - // Get the bodies entities - const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity); - const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity); - - const uint32 componentIndexBody1 = mRigidBodyComponents.getEntityIndex(body1Entity); - const uint32 componentIndexBody2 = mRigidBodyComponents.getEntityIndex(body2Entity); - - Quaternion& q1 = mRigidBodyComponents.mConstrainedOrientations[componentIndexBody1]; - Quaternion& q2 = mRigidBodyComponents.mConstrainedOrientations[componentIndexBody2]; Vector3& b2CrossA1 = mHingeJointComponents.mB2CrossA1[i]; Vector3& c2CrossA1 = mHingeJointComponents.mC2CrossA1[i]; @@ -722,22 +618,6 @@ void SolveHingeJointSystem::solvePositionConstraint() { // Update the body position/orientation of body 2 q2 += Quaternion(0, w2) * q2 * decimal(0.5); q2.normalize(); - } - - // For each joint component - for (uint32 i=0; i < mHingeJointComponents.getNbEnabledComponents(); i++) { - - const Entity jointEntity = mHingeJointComponents.mJointEntities[i]; - - // If the error position correction technique is not the non-linear-gauss-seidel, we do not execute this method - if (mJointComponents.getPositionCorrectionTechnique(jointEntity) != JointsPositionCorrectionTechnique::NON_LINEAR_GAUSS_SEIDEL) continue; - - // Get the bodies entities - const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity); - const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity); - - Quaternion& q1 = mRigidBodyComponents.getConstrainedOrientation(body1Entity); - Quaternion& q2 = mRigidBodyComponents.getConstrainedOrientation(body2Entity); // Compute the current angle around the hinge axis const decimal hingeAngle = computeCurrentHingeAngle(jointEntity, q1, q2); @@ -847,13 +727,13 @@ decimal SolveHingeJointSystem::computeCorrespondingAngleNearLimits(decimal input return inputAngle; } else if (inputAngle > upperLimitAngle) { - decimal diffToUpperLimit = std::fabs(computeNormalizedAngle(inputAngle - upperLimitAngle)); - decimal diffToLowerLimit = std::fabs(computeNormalizedAngle(inputAngle - lowerLimitAngle)); + decimal diffToUpperLimit = std::abs(computeNormalizedAngle(inputAngle - upperLimitAngle)); + decimal diffToLowerLimit = std::abs(computeNormalizedAngle(inputAngle - lowerLimitAngle)); return (diffToUpperLimit > diffToLowerLimit) ? (inputAngle - PI_TIMES_2) : inputAngle; } else if (inputAngle < lowerLimitAngle) { - decimal diffToUpperLimit = std::fabs(computeNormalizedAngle(upperLimitAngle - inputAngle)); - decimal diffToLowerLimit = std::fabs(computeNormalizedAngle(lowerLimitAngle - inputAngle)); + decimal diffToUpperLimit = std::abs(computeNormalizedAngle(upperLimitAngle - inputAngle)); + decimal diffToLowerLimit = std::abs(computeNormalizedAngle(lowerLimitAngle - inputAngle)); return (diffToUpperLimit > diffToLowerLimit) ? inputAngle : (inputAngle + PI_TIMES_2); } else { diff --git a/src/systems/SolveSliderJointSystem.cpp b/src/systems/SolveSliderJointSystem.cpp index d68645f7..851be91f 100644 --- a/src/systems/SolveSliderJointSystem.cpp +++ b/src/systems/SolveSliderJointSystem.cpp @@ -47,31 +47,28 @@ SolveSliderJointSystem::SolveSliderJointSystem(PhysicsWorld& world, RigidBodyCom // Initialize before solving the constraint void SolveSliderJointSystem::initBeforeSolve() { + const decimal biasFactor = (BETA / mTimeStep); + // For each joint - for (uint32 i=0; i < mSliderJointComponents.getNbEnabledComponents(); i++) { + const uint32 nbJoints = mSliderJointComponents.getNbEnabledComponents(); + for (uint32 i=0; i < nbJoints; i++) { const Entity jointEntity = mSliderJointComponents.mJointEntities[i]; + const uint32 jointIndex = mJointComponents.getEntityIndex(jointEntity); // Get the bodies entities - const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity); - const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity); + const Entity body1Entity = mJointComponents.mBody1Entities[jointIndex]; + const Entity body2Entity = mJointComponents.mBody2Entities[jointIndex]; + + const uint32 componentIndexBody1 = mRigidBodyComponents.getEntityIndex(body1Entity); + const uint32 componentIndexBody2 = mRigidBodyComponents.getEntityIndex(body2Entity); assert(!mRigidBodyComponents.getIsEntityDisabled(body1Entity)); assert(!mRigidBodyComponents.getIsEntityDisabled(body2Entity)); // Get the inertia tensor of bodies - mSliderJointComponents.mI1[i] = RigidBody::getWorldInertiaTensorInverse(mWorld, body1Entity); - mSliderJointComponents.mI2[i] = RigidBody::getWorldInertiaTensorInverse(mWorld, body2Entity); - } - - // For each joint - for (uint32 i=0; i < mSliderJointComponents.getNbEnabledComponents(); i++) { - - const Entity jointEntity = mSliderJointComponents.mJointEntities[i]; - - // Get the bodies entities - const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity); - const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity); + mSliderJointComponents.mI1[i] = mRigidBodyComponents.mInverseInertiaTensorsWorld[componentIndexBody1]; + mSliderJointComponents.mI2[i] = mRigidBodyComponents.mInverseInertiaTensorsWorld[componentIndexBody2]; const Quaternion& orientationBody1 = mTransformComponents.getTransform(body1Entity).getOrientation(); const Quaternion& orientationBody2 = mTransformComponents.getTransform(body2Entity).getOrientation(); @@ -79,41 +76,13 @@ void SolveSliderJointSystem::initBeforeSolve() { // Vector from body center to the anchor point mSliderJointComponents.mR1[i] = orientationBody1 * mSliderJointComponents.mLocalAnchorPointBody1[i]; mSliderJointComponents.mR2[i] = orientationBody2 * mSliderJointComponents.mLocalAnchorPointBody2[i]; - } - - // For each joint - for (uint32 i=0; i < mSliderJointComponents.getNbEnabledComponents(); i++) { - - const Entity jointEntity = mSliderJointComponents.mJointEntities[i]; - - const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity); - const Quaternion& orientationBody1 = mTransformComponents.getTransform(body1Entity).getOrientation(); // Compute the two orthogonal vectors to the slider axis in world-space mSliderJointComponents.mSliderAxisWorld[i] = orientationBody1 * mSliderJointComponents.mSliderAxisBody1[i]; mSliderJointComponents.mSliderAxisWorld[i].normalize(); - } - - // For each joint - for (uint32 i=0; i < mSliderJointComponents.getNbEnabledComponents(); i++) { mSliderJointComponents.mN1[i] = mSliderJointComponents.mSliderAxisWorld[i].getOneUnitOrthogonalVector(); mSliderJointComponents.mN2[i] = mSliderJointComponents.mSliderAxisWorld[i].cross(mSliderJointComponents.mN1[i]); - } - - const decimal biasFactor = (BETA / mTimeStep); - - // For each joint - for (uint32 i=0; i < mSliderJointComponents.getNbEnabledComponents(); i++) { - - const Entity jointEntity = mSliderJointComponents.mJointEntities[i]; - - // Get the bodies entities - const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity); - const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity); - - const uint32 componentIndexBody1 = mRigidBodyComponents.getEntityIndex(body1Entity); - const uint32 componentIndexBody2 = mRigidBodyComponents.getEntityIndex(body2Entity); const Vector3& x1 = mRigidBodyComponents.mCentersOfMassWorld[componentIndexBody1]; const Vector3& x2 = mRigidBodyComponents.mCentersOfMassWorld[componentIndexBody2]; @@ -147,7 +116,7 @@ void SolveSliderJointSystem::initBeforeSolve() { // Compute the bias "b" of the translation constraint mSliderJointComponents.mBiasTranslation[i].setToZero(); - if (mJointComponents.getPositionCorrectionTechnique(jointEntity) == JointsPositionCorrectionTechnique::BAUMGARTE_JOINTS) { + if (mJointComponents.mPositionCorrectionTechniques[jointIndex] == JointsPositionCorrectionTechnique::BAUMGARTE_JOINTS) { mSliderJointComponents.mBiasTranslation[i].x = u.dot(mSliderJointComponents.mN1[i]); mSliderJointComponents.mBiasTranslation[i].y = u.dot(mSliderJointComponents.mN2[i]); mSliderJointComponents.mBiasTranslation[i] *= biasFactor; @@ -173,35 +142,21 @@ void SolveSliderJointSystem::initBeforeSolve() { // Compute the bias "b" of the lower limit constraint mSliderJointComponents.mBLowerLimit[i] = decimal(0.0); - if (mJointComponents.getPositionCorrectionTechnique(jointEntity) == JointsPositionCorrectionTechnique::BAUMGARTE_JOINTS) { + if (mJointComponents.mPositionCorrectionTechniques[jointIndex] == JointsPositionCorrectionTechnique::BAUMGARTE_JOINTS) { mSliderJointComponents.mBLowerLimit[i] = biasFactor * lowerLimitError; } // Compute the bias "b" of the upper limit constraint mSliderJointComponents.mBUpperLimit[i] = decimal(0.0); - if (mJointComponents.getPositionCorrectionTechnique(jointEntity) == JointsPositionCorrectionTechnique::BAUMGARTE_JOINTS) { + if (mJointComponents.mPositionCorrectionTechniques[jointIndex] == JointsPositionCorrectionTechnique::BAUMGARTE_JOINTS) { mSliderJointComponents.mBUpperLimit[i] = biasFactor * upperLimitError; } } - } - - // For each joint - for (uint32 i=0; i < mSliderJointComponents.getNbEnabledComponents(); i++) { // Compute the cross products used in the Jacobians mSliderJointComponents.mR2CrossN1[i] = mSliderJointComponents.mR2[i].cross(mSliderJointComponents.mN1[i]); mSliderJointComponents.mR2CrossN2[i] = mSliderJointComponents.mR2[i].cross(mSliderJointComponents.mN2[i]); mSliderJointComponents.mR2CrossSliderAxis[i] = mSliderJointComponents.mR2[i].cross(mSliderJointComponents.mSliderAxisWorld[i]); - } - - // For each joint - for (uint32 i=0; i < mSliderJointComponents.getNbEnabledComponents(); i++) { - - const Entity jointEntity = mSliderJointComponents.mJointEntities[i]; - - // Get the bodies entities - const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity); - const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity); const Vector3& r2CrossN1 = mSliderJointComponents.mR2CrossN1[i]; const Vector3& r2CrossN2 = mSliderJointComponents.mR2CrossN2[i]; @@ -211,9 +166,6 @@ void SolveSliderJointSystem::initBeforeSolve() { const Matrix3x3& i1 = mSliderJointComponents.mI1[i]; const Matrix3x3& i2 = mSliderJointComponents.mI2[i]; - const uint32 componentIndexBody1 = mRigidBodyComponents.getEntityIndex(body1Entity); - const uint32 componentIndexBody2 = mRigidBodyComponents.getEntityIndex(body2Entity); - // Compute the inverse of the mass matrix K=JM^-1J^t for the 2 translation // constraints (2x2 matrix) const decimal body1MassInverse = mRigidBodyComponents.mInverseMasses[componentIndexBody1]; @@ -247,23 +199,10 @@ void SolveSliderJointSystem::initBeforeSolve() { mSliderJointComponents.mInverseMassMatrixRotation[i] = mSliderJointComponents.mInverseMassMatrixRotation[i].getInverse(); } - } - - // For each joint - for (uint32 i=0; i < mSliderJointComponents.getNbEnabledComponents(); i++) { - - const Entity jointEntity = mSliderJointComponents.mJointEntities[i]; - - // Get the bodies entities - const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity); - const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity); - - const Quaternion& orientationBody1 = mTransformComponents.getTransform(body1Entity).getOrientation(); - const Quaternion& orientationBody2 = mTransformComponents.getTransform(body2Entity).getOrientation(); // Compute the bias "b" of the rotation constraint mSliderJointComponents.mBiasRotation[i].setToZero(); - if (mJointComponents.getPositionCorrectionTechnique(jointEntity) == JointsPositionCorrectionTechnique::BAUMGARTE_JOINTS) { + if (mJointComponents.mPositionCorrectionTechniques[jointIndex] == JointsPositionCorrectionTechnique::BAUMGARTE_JOINTS) { const Quaternion qError = orientationBody2 * mSliderJointComponents.mInitOrientationDifferenceInv[i] * orientationBody1.getInverse(); mSliderJointComponents.mBiasRotation[i] = biasFactor * decimal(2.0) * qError.getVectorV(); } @@ -271,8 +210,8 @@ void SolveSliderJointSystem::initBeforeSolve() { // If the motor is enabled if (mSliderJointComponents.mIsMotorEnabled[i]) { - const decimal body1MassInverse = mRigidBodyComponents.getMassInverse(body1Entity); - const decimal body2MassInverse = mRigidBodyComponents.getMassInverse(body2Entity); + const decimal body1MassInverse = mRigidBodyComponents.mInverseMasses[componentIndexBody1]; + const decimal body2MassInverse = mRigidBodyComponents.mInverseMasses[componentIndexBody2]; const decimal sumInverseMass = body1MassInverse + body2MassInverse; // Compute the inverse of mass matrix K=JM^-1J^t for the motor (1x1 matrix) @@ -280,13 +219,9 @@ void SolveSliderJointSystem::initBeforeSolve() { mSliderJointComponents.mInverseMassMatrixMotor[i] = (mSliderJointComponents.mInverseMassMatrixMotor[i] > decimal(0.0)) ? decimal(1.0) / mSliderJointComponents.mInverseMassMatrixMotor[i] : decimal(0.0); } - } - // If warm-starting is not enabled - if (!mIsWarmStartingActive) { - - // For each joint - for (uint32 i=0; i < mSliderJointComponents.getNbEnabledComponents(); i++) { + // If warm-starting is not enabled + if (!mIsWarmStartingActive) { // Reset all the accumulated impulses mSliderJointComponents.mImpulseTranslation[i].setToZero(); @@ -302,13 +237,15 @@ void SolveSliderJointSystem::initBeforeSolve() { void SolveSliderJointSystem::warmstart() { // For each joint component - for (uint32 i=0; i < mSliderJointComponents.getNbEnabledComponents(); i++) { + const uint32 nbJoints = mSliderJointComponents.getNbEnabledComponents(); + for (uint32 i=0; i < nbJoints; i++) { const Entity jointEntity = mSliderJointComponents.mJointEntities[i]; + const uint32 jointIndex = mJointComponents.getEntityIndex(jointEntity); // Get the bodies entities - const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity); - const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity); + const Entity body1Entity = mJointComponents.mBody1Entities[jointIndex]; + const Entity body2Entity = mJointComponents.mBody2Entities[jointIndex]; const uint32 componentIndexBody1 = mRigidBodyComponents.getEntityIndex(body1Entity); const uint32 componentIndexBody2 = mRigidBodyComponents.getEntityIndex(body2Entity); @@ -380,13 +317,15 @@ void SolveSliderJointSystem::warmstart() { void SolveSliderJointSystem::solveVelocityConstraint() { // For each joint component - for (uint32 i=0; i < mSliderJointComponents.getNbEnabledComponents(); i++) { + const uint32 nbJoints = mSliderJointComponents.getNbEnabledComponents(); + for (uint32 i=0; i < nbJoints; i++) { const Entity jointEntity = mSliderJointComponents.mJointEntities[i]; + const uint32 jointIndex = mJointComponents.getEntityIndex(jointEntity); // Get the bodies entities - const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity); - const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity); + const Entity body1Entity = mJointComponents.mBody1Entities[jointIndex]; + const Entity body2Entity = mJointComponents.mBody2Entities[jointIndex]; const uint32 componentIndexBody1 = mRigidBodyComponents.getEntityIndex(body1Entity); const uint32 componentIndexBody2 = mRigidBodyComponents.getEntityIndex(body2Entity); @@ -441,25 +380,9 @@ void SolveSliderJointSystem::solveVelocityConstraint() { // Apply the impulse to the body 2 v2 += inverseMassBody2 * linearImpulseBody2; w2 += i2 * angularImpulseBody2; - } - - // For each joint component - for (uint32 i=0; i < mSliderJointComponents.getNbEnabledComponents(); i++) { - - const Entity jointEntity = mSliderJointComponents.mJointEntities[i]; - - // Get the bodies entities - const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity); - const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity); - - const uint32 componentIndexBody1 = mRigidBodyComponents.getEntityIndex(body1Entity); - const uint32 componentIndexBody2 = mRigidBodyComponents.getEntityIndex(body2Entity); // --------------- Rotation Constraints --------------- // - Vector3& w1 = mRigidBodyComponents.mConstrainedAngularVelocities[componentIndexBody1]; - Vector3& w2 = mRigidBodyComponents.mConstrainedAngularVelocities[componentIndexBody2]; - // Compute J*v for the 3 rotation constraints const Vector3 JvRotation = w2 - w1; @@ -469,35 +392,16 @@ void SolveSliderJointSystem::solveVelocityConstraint() { mSliderJointComponents.mImpulseRotation[i] += deltaLambda2; // Compute the impulse P=J^T * lambda for the 3 rotation constraints of body 1 - Vector3 angularImpulseBody1 = -deltaLambda2; + angularImpulseBody1 = -deltaLambda2; - // Apply the impulse to the body to body 1 + // Apply the impulse to the body 1 w1 += mSliderJointComponents.mI1[i] * angularImpulseBody1; // Compute the impulse P=J^T * lambda for the 3 rotation constraints of body 2 - Vector3 angularImpulseBody2 = deltaLambda2; + angularImpulseBody2 = deltaLambda2; // Apply the impulse to the body 2 w2 += mSliderJointComponents.mI2[i] * angularImpulseBody2; - } - - // For each joint component - for (uint32 i=0; i < mSliderJointComponents.getNbEnabledComponents(); i++) { - - const Entity jointEntity = mSliderJointComponents.mJointEntities[i]; - - // Get the bodies entities - const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity); - const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity); - - const uint32 componentIndexBody1 = mRigidBodyComponents.getEntityIndex(body1Entity); - const uint32 componentIndexBody2 = mRigidBodyComponents.getEntityIndex(body2Entity); - - Vector3& v1 = mRigidBodyComponents.mConstrainedLinearVelocities[componentIndexBody1]; - Vector3& v2 = mRigidBodyComponents.mConstrainedLinearVelocities[componentIndexBody2]; - - decimal inverseMassBody1 = mRigidBodyComponents.mInverseMasses[componentIndexBody1]; - decimal inverseMassBody2 = mRigidBodyComponents.mInverseMasses[componentIndexBody2]; const Vector3& r2CrossSliderAxis = mSliderJointComponents.mR2CrossSliderAxis[i]; const Vector3& r1PlusUCrossSliderAxis = mSliderJointComponents.mR1PlusUCrossSliderAxis[i]; @@ -607,59 +511,36 @@ void SolveSliderJointSystem::solveVelocityConstraint() { void SolveSliderJointSystem::solvePositionConstraint() { // For each joint component - for (uint32 i=0; i < mSliderJointComponents.getNbEnabledComponents(); i++) { + const uint32 nbEnabledJoints = mSliderJointComponents.getNbEnabledComponents(); + for (uint32 i=0; i < nbEnabledJoints; i++) { const Entity jointEntity = mSliderJointComponents.mJointEntities[i]; + const uint32 jointIndex = mJointComponents.getEntityIndex(jointEntity); // If the error position correction technique is not the non-linear-gauss-seidel, we do // do not execute this method - if (mJointComponents.getPositionCorrectionTechnique(jointEntity) != JointsPositionCorrectionTechnique::NON_LINEAR_GAUSS_SEIDEL) return; + if (mJointComponents.mPositionCorrectionTechniques[jointIndex] != JointsPositionCorrectionTechnique::NON_LINEAR_GAUSS_SEIDEL) return; // Get the bodies entities - const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity); - const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity); + const Entity body1Entity = mJointComponents.mBody1Entities[jointIndex]; + const Entity body2Entity = mJointComponents.mBody2Entities[jointIndex]; - // Recompute the inverse inertia tensors - mSliderJointComponents.mI1[i] = RigidBody::getWorldInertiaTensorInverse(mWorld, body1Entity); - mSliderJointComponents.mI2[i] = RigidBody::getWorldInertiaTensorInverse(mWorld, body2Entity); - } + const uint32 componentIndexBody1 = mRigidBodyComponents.getEntityIndex(body1Entity); + const uint32 componentIndexBody2 = mRigidBodyComponents.getEntityIndex(body2Entity); - // For each joint component - for (uint32 i=0; i < mSliderJointComponents.getNbEnabledComponents(); i++) { + Quaternion& q1 = mRigidBodyComponents.mConstrainedOrientations[componentIndexBody1]; + Quaternion& q2 = mRigidBodyComponents.mConstrainedOrientations[componentIndexBody2]; - const Entity jointEntity = mSliderJointComponents.mJointEntities[i]; + // Recompute the world inverse inertia tensors + RigidBody::computeWorldInertiaTensorInverse(q1.getMatrix(), mRigidBodyComponents.mInverseInertiaTensorsLocal[componentIndexBody1], + mSliderJointComponents.mI1[i]); - // If the error position correction technique is not the non-linear-gauss-seidel, we do - // do not execute this method - if (mJointComponents.getPositionCorrectionTechnique(jointEntity) != JointsPositionCorrectionTechnique::NON_LINEAR_GAUSS_SEIDEL) return; - - // Get the bodies entities - const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity); - const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity); - - const Quaternion& q1 = mRigidBodyComponents.getConstrainedOrientation(body1Entity); - const Quaternion& q2 = mRigidBodyComponents.getConstrainedOrientation(body2Entity); + RigidBody::computeWorldInertiaTensorInverse(q2.getMatrix(), mRigidBodyComponents.mInverseInertiaTensorsLocal[componentIndexBody2], + mSliderJointComponents.mI2[i]); // Vector from body center to the anchor point mSliderJointComponents.mR1[i] = q1 * mSliderJointComponents.mLocalAnchorPointBody1[i]; mSliderJointComponents.mR2[i] = q2 * mSliderJointComponents.mLocalAnchorPointBody2[i]; - } - - // For each joint component - for (uint32 i=0; i < mSliderJointComponents.getNbEnabledComponents(); i++) { - - const Entity jointEntity = mSliderJointComponents.mJointEntities[i]; - - // If the error position correction technique is not the non-linear-gauss-seidel, we do - // do not execute this method - if (mJointComponents.getPositionCorrectionTechnique(jointEntity) != JointsPositionCorrectionTechnique::NON_LINEAR_GAUSS_SEIDEL) return; - - // Get the bodies entities - const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity); - const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity); - - const uint32 componentIndexBody1 = mRigidBodyComponents.getEntityIndex(body1Entity); - const uint32 componentIndexBody2 = mRigidBodyComponents.getEntityIndex(body2Entity); // Get the inverse mass and inverse inertia tensors of the bodies const decimal inverseMassBody1 = mRigidBodyComponents.mInverseMasses[componentIndexBody1]; @@ -677,9 +558,6 @@ void SolveSliderJointSystem::solvePositionConstraint() { // Compute the vector u (difference between anchor points) const Vector3 u = x2 + r2 - x1 - r1; - Quaternion& q1 = mRigidBodyComponents.mConstrainedOrientations[componentIndexBody1]; - Quaternion& q2 = mRigidBodyComponents.mConstrainedOrientations[componentIndexBody2]; - // Compute the two orthogonal vectors to the slider axis in world-space mSliderJointComponents.mSliderAxisWorld[i] = q1 * mSliderJointComponents.mSliderAxisBody1[i]; mSliderJointComponents.mSliderAxisWorld[i].normalize(); @@ -768,30 +646,6 @@ void SolveSliderJointSystem::solvePositionConstraint() { x2 += v2; q2 += Quaternion(0, w2) * q2 * decimal(0.5); q2.normalize(); - } - - // For each joint component - for (uint32 i=0; i < mSliderJointComponents.getNbEnabledComponents(); i++) { - - const Entity jointEntity = mSliderJointComponents.mJointEntities[i]; - - // If the error position correction technique is not the non-linear-gauss-seidel, we do - // do not execute this method - if (mJointComponents.getPositionCorrectionTechnique(jointEntity) != JointsPositionCorrectionTechnique::NON_LINEAR_GAUSS_SEIDEL) return; - - // Get the bodies entities - const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity); - const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity); - - const uint32 componentIndexBody1 = mRigidBodyComponents.getEntityIndex(body1Entity); - const uint32 componentIndexBody2 = mRigidBodyComponents.getEntityIndex(body2Entity); - - Quaternion& q1 = mRigidBodyComponents.mConstrainedOrientations[componentIndexBody1]; - Quaternion& q2 = mRigidBodyComponents.mConstrainedOrientations[componentIndexBody2]; - - // Get the velocities - Vector3& w1 = mRigidBodyComponents.mConstrainedAngularVelocities[componentIndexBody1]; - Vector3& w2 = mRigidBodyComponents.mConstrainedAngularVelocities[componentIndexBody2]; // --------------- Rotation Constraints --------------- // @@ -835,7 +689,7 @@ void SolveSliderJointSystem::solvePositionConstraint() { Vector3 lambdaRotation = mSliderJointComponents.mInverseMassMatrixRotation[i] * (-errorRotation); // Compute the impulse P=J^T * lambda for the 3 rotation constraints of body 1 - Vector3 angularImpulseBody1 = -lambdaRotation; + angularImpulseBody1 = -lambdaRotation; // Apply the impulse to the body 1 w1 = mSliderJointComponents.mI1[i] * angularImpulseBody1; @@ -845,7 +699,7 @@ void SolveSliderJointSystem::solvePositionConstraint() { q1.normalize(); // Compute the impulse P=J^T * lambda for the 3 rotation constraints of body 2 - Vector3 angularImpulseBody2 = lambdaRotation; + angularImpulseBody2 = lambdaRotation; // Apply the impulse to the body 2 w2 = mSliderJointComponents.mI2[i] * angularImpulseBody2; diff --git a/src/utils/DebugRenderer.cpp b/src/utils/DebugRenderer.cpp index 845fd8b3..2105ea84 100644 --- a/src/utils/DebugRenderer.cpp +++ b/src/utils/DebugRenderer.cpp @@ -280,7 +280,8 @@ void DebugRenderer::drawConvexMesh(const Transform& transform, const ConvexMeshS assert(face.faceVertices.size() >= 3); // Perform a fan triangulation of the convex polygon face - for (uint32 v = 2; v < face.faceVertices.size(); v++) { + const uint32 nbFaceVertices = face.faceVertices.size(); + for (uint32 v = 2; v < nbFaceVertices; v++) { uint v1Index = face.faceVertices[v - 2]; uint v2Index = face.faceVertices[v - 1]; @@ -463,7 +464,7 @@ void DebugRenderer::onContact(const CollisionCallback::CallbackData& callbackDat if (getIsDebugItemDisplayed(DebugItem::CONTACT_POINT)) { // Contact point - drawSphere(point, DEFAULT_CONTACT_POINT_SPHERE_RADIUS, mMapDebugItemWithColor[DebugItem::CONTACT_POINT]); + drawSphere(point, mContactPointSphereRadius, mMapDebugItemWithColor[DebugItem::CONTACT_POINT]); } if (getIsDebugItemDisplayed(DebugItem::CONTACT_NORMAL)) { diff --git a/src/utils/DefaultLogger.cpp b/src/utils/DefaultLogger.cpp index bb1a2dd2..cd3b2d35 100644 --- a/src/utils/DefaultLogger.cpp +++ b/src/utils/DefaultLogger.cpp @@ -79,7 +79,7 @@ void DefaultLogger::addStreamDestination(std::ostream& outputStream, uint logLev void DefaultLogger::removeAllDestinations() { // Delete all the destinations - for (uint i=0; igetSizeBytes(); diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index 804f7cb8..c125f2ac 100755 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -15,7 +15,7 @@ set (RP3D_TESTS_HEADERS "tests/collision/TestPointInside.h" "tests/collision/TestRaycast.h" "tests/collision/TestTriangleVertexArray.h" - "tests/containers/TestList.h" + "tests/containers/TestArray.h" "tests/containers/TestMap.h" "tests/containers/TestSet.h" "tests/containers/TestStack.h" diff --git a/test/main.cpp b/test/main.cpp index 6a965039..0346ee57 100644 --- a/test/main.cpp +++ b/test/main.cpp @@ -39,7 +39,7 @@ #include "tests/collision/TestDynamicAABBTree.h" #include "tests/collision/TestHalfEdgeStructure.h" #include "tests/collision/TestTriangleVertexArray.h" -#include "tests/containers/TestList.h" +#include "tests/containers/TestArray.h" #include "tests/containers/TestMap.h" #include "tests/containers/TestSet.h" #include "tests/containers/TestDeque.h" @@ -53,9 +53,9 @@ int main() { // ---------- Containers tests ---------- // - testSuite.addTest(new TestList("List")); - testSuite.addTest(new TestMap("Map")); testSuite.addTest(new TestSet("Set")); + testSuite.addTest(new TestArray("Array")); + testSuite.addTest(new TestMap("Map")); testSuite.addTest(new TestDeque("Deque")); testSuite.addTest(new TestStack("Stack")); diff --git a/test/tests/collision/TestAABB.h b/test/tests/collision/TestAABB.h index d51972dd..2383d87e 100644 --- a/test/tests/collision/TestAABB.h +++ b/test/tests/collision/TestAABB.h @@ -286,14 +286,31 @@ class TestAABB : public Test { Ray ray7(Vector3(-4, 6, -100), Vector3(-4, 6, -11), 0.6); Ray ray8(Vector3(-403, -432, -100), Vector3(134, 643, 23)); - rp3d_test(mAABB1.testRayIntersect(ray1)); - rp3d_test(!mAABB1.testRayIntersect(ray2)); - rp3d_test(mAABB1.testRayIntersect(ray3)); - rp3d_test(mAABB1.testRayIntersect(ray4)); - rp3d_test(mAABB1.testRayIntersect(ray5)); - rp3d_test(mAABB1.testRayIntersect(ray6)); - rp3d_test(!mAABB1.testRayIntersect(ray7)); - rp3d_test(!mAABB1.testRayIntersect(ray8)); + const Vector3 ray1Direction = ray1.point2 - ray1.point1; + const Vector3 ray1DirectionInv(decimal(1.0) / ray1Direction.x, decimal(1.0) / ray1Direction.y, decimal(1.0) / ray1Direction.z); + const Vector3 ray2Direction = ray2.point2 - ray2.point1; + const Vector3 ray2DirectionInv(decimal(1.0) / ray2Direction.x, decimal(1.0) / ray2Direction.y, decimal(1.0) / ray2Direction.z); + const Vector3 ray3Direction = ray3.point2 - ray3.point1; + const Vector3 ray3DirectionInv(decimal(1.0) / ray3Direction.x, decimal(1.0) / ray3Direction.y, decimal(1.0) / ray3Direction.z); + const Vector3 ray4Direction = ray4.point2 - ray4.point1; + const Vector3 ray4DirectionInv(decimal(1.0) / ray4Direction.x, decimal(1.0) / ray4Direction.y, decimal(1.0) / ray4Direction.z); + const Vector3 ray5Direction = ray5.point2 - ray5.point1; + const Vector3 ray5DirectionInv(decimal(1.0) / ray5Direction.x, decimal(1.0) / ray5Direction.y, decimal(1.0) / ray5Direction.z); + const Vector3 ray6Direction = ray6.point2 - ray6.point1; + const Vector3 ray6DirectionInv(decimal(1.0) / ray6Direction.x, decimal(1.0) / ray6Direction.y, decimal(1.0) / ray6Direction.z); + const Vector3 ray7Direction = ray7.point2 - ray7.point1; + const Vector3 ray7DirectionInv(decimal(1.0) / ray7Direction.x, decimal(1.0) / ray7Direction.y, decimal(1.0) / ray7Direction.z); + const Vector3 ray8Direction = ray8.point2 - ray8.point1; + const Vector3 ray8DirectionInv(decimal(1.0) / ray8Direction.x, decimal(1.0) / ray8Direction.y, decimal(1.0) / ray8Direction.z); + + rp3d_test(mAABB1.testRayIntersect(ray1.point1, ray1DirectionInv, decimal(1.0))); + rp3d_test(!mAABB1.testRayIntersect(ray2.point1, ray2DirectionInv, decimal(1.0))); + rp3d_test(mAABB1.testRayIntersect(ray3.point1, ray3DirectionInv, decimal(1.0))); + rp3d_test(mAABB1.testRayIntersect(ray4.point1, ray4DirectionInv, decimal(1.0))); + rp3d_test(mAABB1.testRayIntersect(ray5.point1, ray5DirectionInv, decimal(1.0))); + rp3d_test(mAABB1.testRayIntersect(ray6.point1, ray6DirectionInv, decimal(1.0))); + rp3d_test(!mAABB1.testRayIntersect(ray7.point1, ray7DirectionInv, decimal(1.0))); + rp3d_test(!mAABB1.testRayIntersect(ray8.point1, ray8DirectionInv, decimal(1.0))); } }; diff --git a/test/tests/collision/TestCollisionWorld.h b/test/tests/collision/TestCollisionWorld.h index 4c02d49c..583d7b1a 100644 --- a/test/tests/collision/TestCollisionWorld.h +++ b/test/tests/collision/TestCollisionWorld.h @@ -610,7 +610,7 @@ class TestCollisionWorld : public Test { Transform initTransform2 = mSphereBody2->getTransform(); Transform transform1(Vector3(10, 20, 50), Quaternion::identity()); - Transform transform2(Vector3(17, 20, 50), Quaternion::fromEulerAngles(rp3d::PI / 8.0f, rp3d::PI / 4.0f, rp3d::PI / 16.0f)); + Transform transform2(Vector3(17, 20, 50), Quaternion::fromEulerAngles(rp3d::PI_RP3D / 8.0f, rp3d::PI_RP3D / 4.0f, rp3d::PI_RP3D / 16.0f)); // Move spheres to collide with each other mSphereBody1->setTransform(transform1); @@ -2200,7 +2200,7 @@ class TestCollisionWorld : public Test { *********************************************************************************/ Transform transform1(Vector3(10, 20, 50), Quaternion::identity()); - Transform transform2(Vector3(17, 21, 50), Quaternion::fromEulerAngles(0, 0, rp3d::PI * 0.5f)); + Transform transform2(Vector3(17, 21, 50), Quaternion::fromEulerAngles(0, 0, rp3d::PI_RP3D * 0.5f)); // Move spheres to collide with each other mBoxBody1->setTransform(transform1); @@ -2315,7 +2315,7 @@ class TestCollisionWorld : public Test { *********************************************************************************/ Transform transform1(Vector3(10, 20, 50), Quaternion::identity()); - Transform transform2(Vector3(17, 21, 50), Quaternion::fromEulerAngles(0, 0, rp3d::PI * 0.5f)); + Transform transform2(Vector3(17, 21, 50), Quaternion::fromEulerAngles(0, 0, rp3d::PI_RP3D * 0.5f)); // Move spheres to collide with each other mConvexMeshBody1->setTransform(transform1); @@ -2626,7 +2626,7 @@ class TestCollisionWorld : public Test { *********************************************************************************/ Transform transform1(Vector3(10, 20, 50), Quaternion::identity()); - Transform transform2(Vector3(16, 23, 50), Quaternion::fromEulerAngles(0, 0, rp3d::PI * 0.5f)); + Transform transform2(Vector3(16, 23, 50), Quaternion::fromEulerAngles(0, 0, rp3d::PI_RP3D * 0.5f)); // Move spheres to collide with each other mCapsuleBody1->setTransform(transform1); @@ -2732,7 +2732,7 @@ class TestCollisionWorld : public Test { *********************************************************************************/ transform1 = Transform(Vector3(10, 20, 50), Quaternion::identity()); - transform2 = Transform(Vector3(10, 27, 50), Quaternion::fromEulerAngles(0, 0, rp3d::PI * 0.5f)); + transform2 = Transform(Vector3(10, 27, 50), Quaternion::fromEulerAngles(0, 0, rp3d::PI_RP3D * 0.5f)); // Move spheres to collide with each other mCapsuleBody1->setTransform(transform1); diff --git a/test/tests/collision/TestDynamicAABBTree.h b/test/tests/collision/TestDynamicAABBTree.h index 69b876be..22fc4155 100755 --- a/test/tests/collision/TestDynamicAABBTree.h +++ b/test/tests/collision/TestDynamicAABBTree.h @@ -126,7 +126,7 @@ class TestDynamicAABBTree : public Test { } - bool isOverlapping(int nodeId, const List& overlappingNodes) const { + bool isOverlapping(int nodeId, const Array& overlappingNodes) const { return std::find(overlappingNodes.begin(), overlappingNodes.end(), nodeId) != overlappingNodes.end(); } @@ -223,7 +223,7 @@ class TestDynamicAABBTree : public Test { // ---------- Tests ---------- // - List overlappingNodes(mAllocator); + Array overlappingNodes(mAllocator); // AABB overlapping nothing overlappingNodes.clear(); diff --git a/test/tests/collision/TestHalfEdgeStructure.h b/test/tests/collision/TestHalfEdgeStructure.h index 447c1c97..b6f7e834 100644 --- a/test/tests/collision/TestHalfEdgeStructure.h +++ b/test/tests/collision/TestHalfEdgeStructure.h @@ -71,17 +71,17 @@ class TestHalfEdgeStructure : public Test { cubeStructure.addVertex(7); // Faces - List face0(mAllocator, 4); + Array face0(mAllocator, 4); face0.add(0); face0.add(1); face0.add(2); face0.add(3); - List face1(mAllocator, 4); + Array face1(mAllocator, 4); face1.add(1); face1.add(5); face1.add(6); face1.add(2); - List face2(mAllocator, 4); + Array face2(mAllocator, 4); face2.add(5); face2.add(4); face2.add(7); face2.add(6); - List face3(mAllocator, 4); + Array face3(mAllocator, 4); face3.add(4); face3.add(0); face3.add(3); face3.add(7); - List face4(mAllocator, 4); + Array face4(mAllocator, 4); face4.add(0); face4.add(4); face4.add(5); face4.add(1); - List face5(mAllocator, 4); + Array face5(mAllocator, 4); face5.add(2); face5.add(6); face5.add(7); face5.add(3); cubeStructure.addFace(face0); @@ -188,13 +188,13 @@ class TestHalfEdgeStructure : public Test { tetrahedron.addVertex(3); // Faces - List face0(mAllocator, 3); + Array face0(mAllocator, 3); face0.add(0); face0.add(1); face0.add(2); - List face1(mAllocator, 3); + Array face1(mAllocator, 3); face1.add(0); face1.add(3); face1.add(1); - List face2(mAllocator, 3); + Array face2(mAllocator, 3); face2.add(1); face2.add(3); face2.add(2); - List face3(mAllocator, 3); + Array face3(mAllocator, 3); face3.add(0); face3.add(2); face3.add(3); tetrahedron.addFace(face0); diff --git a/test/tests/collision/TestPointInside.h b/test/tests/collision/TestPointInside.h index 9170a2d8..63fac628 100644 --- a/test/tests/collision/TestPointInside.h +++ b/test/tests/collision/TestPointInside.h @@ -101,7 +101,7 @@ class TestPointInside : public Test { // Body transform Vector3 position(-3, 2, 7); - Quaternion orientation = Quaternion::fromEulerAngles(PI / 5, PI / 6, PI / 7); + Quaternion orientation = Quaternion::fromEulerAngles(PI_RP3D / 5, PI_RP3D / 6, PI_RP3D / 7); mBodyTransform = Transform(position, orientation); // Create the bodies @@ -117,7 +117,7 @@ class TestPointInside : public Test { // Collision shape transform Vector3 shapePosition(1, -4, -3); - Quaternion shapeOrientation = Quaternion::fromEulerAngles(3 * PI / 6 , -PI / 8, PI / 3); + Quaternion shapeOrientation = Quaternion::fromEulerAngles(3 * PI_RP3D / 6 , -PI_RP3D / 8, PI_RP3D / 3); mShapeTransform = Transform(shapePosition, shapeOrientation); // Compute the the transform from a local shape point to world-space @@ -167,7 +167,7 @@ class TestPointInside : public Test { // Compound shape is a capsule and a sphere Vector3 positionShape2(Vector3(4, 2, -3)); - Quaternion orientationShape2 = Quaternion::fromEulerAngles(-3 * PI / 8, 1.5 * PI/ 3, PI / 13); + Quaternion orientationShape2 = Quaternion::fromEulerAngles(-3 * PI_RP3D / 8, 1.5 * PI_RP3D/ 3, PI_RP3D / 13); Transform shapeTransform2(positionShape2, orientationShape2); mLocalShape2ToWorld = mBodyTransform * shapeTransform2; mCompoundBody->addCollider(mCapsuleShape, mShapeTransform); diff --git a/test/tests/collision/TestRaycast.h b/test/tests/collision/TestRaycast.h index 51f6f7de..f2b0a248 100644 --- a/test/tests/collision/TestRaycast.h +++ b/test/tests/collision/TestRaycast.h @@ -176,7 +176,7 @@ class TestRaycast : public Test { // Body transform Vector3 position(-3, 2, 7); - Quaternion orientation = Quaternion::fromEulerAngles(PI / 5, PI / 6, PI / 7); + Quaternion orientation = Quaternion::fromEulerAngles(PI_RP3D / 5, PI_RP3D / 6, PI_RP3D / 7); mBodyTransform = Transform(position, orientation); // Create the bodies @@ -191,7 +191,7 @@ class TestRaycast : public Test { // Collision shape transform Vector3 shapePosition(1, -4, -3); - Quaternion shapeOrientation = Quaternion::fromEulerAngles(3 * PI / 6 , -PI / 8, PI / 3); + Quaternion shapeOrientation = Quaternion::fromEulerAngles(3 * PI_RP3D / 6 , -PI_RP3D / 8, PI_RP3D / 3); mShapeTransform = Transform(shapePosition, shapeOrientation); // Compute the the transform from a local shape point to world-space @@ -243,7 +243,7 @@ class TestRaycast : public Test { // Compound shape is a cylinder and a sphere Vector3 positionShape2(Vector3(4, 2, -3)); - Quaternion orientationShape2 = Quaternion::fromEulerAngles(-3 *PI / 8, 1.5 * PI/ 3, PI / 13); + Quaternion orientationShape2 = Quaternion::fromEulerAngles(-3 * PI_RP3D / 8, 1.5 * PI_RP3D/ 3, PI_RP3D / 13); Transform shapeTransform2(positionShape2, orientationShape2); mLocalShape2ToWorld = mBodyTransform * shapeTransform2; mCompoundCapsuleCollider = mCompoundBody->addCollider(mCapsuleShape, mShapeTransform); diff --git a/test/tests/containers/TestArray.h b/test/tests/containers/TestArray.h new file mode 100644 index 00000000..8faaa8dd --- /dev/null +++ b/test/tests/containers/TestArray.h @@ -0,0 +1,449 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://www.reactphysics3d.com * +* Copyright (c) 2010-2016 Daniel Chappuis * +********************************************************************************* +* * +* This software is provided 'as-is', without any express or implied warranty. * +* In no event will the authors be held liable for any damages arising from the * +* use of this software. * +* * +* Permission is granted to anyone to use this software for any purpose, * +* including commercial applications, and to alter it and redistribute it * +* freely, subject to the following restrictions: * +* * +* 1. The origin of this software must not be misrepresented; you must not claim * +* that you wrote the original software. If you use this software in a * +* product, an acknowledgment in the product documentation would be * +* appreciated but is not required. * +* * +* 2. Altered source versions must be plainly marked as such, and must not be * +* misrepresented as being the original software. * +* * +* 3. This notice may not be removed or altered from any source distribution. * +* * +********************************************************************************/ + +#ifndef TEST_ARRAY_H +#define TEST_ARRAY_H + +// Libraries +#include "Test.h" +#include +#include + +/// Reactphysics3D namespace +namespace reactphysics3d { + +// Class TestArray +/** + * Unit test for the Array class + */ +class TestArray : public Test { + + private : + + // ---------- Atributes ---------- // + + DefaultAllocator mAllocator; + + public : + + // ---------- Methods ---------- // + + /// Constructor + TestArray(const std::string& name) : Test(name) { + + } + + /// Run the tests + void run() { + + testConstructors(); + testAddRemoveClear(); + testAssignment(); + testIndexing(); + testFind(); + testEquality(); + testReserve(); + testIterators(); + } + + void testConstructors() { + + // ----- Constructors ----- // + + Array array1(mAllocator); + rp3d_test(array1.capacity() == 0); + rp3d_test(array1.size() == 0); + + Array array2(mAllocator, 100); + rp3d_test(array2.capacity() == 100); + rp3d_test(array2.size() == 0); + + Array array3(mAllocator); + array3.add(1); + array3.add(2); + array3.add(3); + rp3d_test(array3.capacity() == 4); + rp3d_test(array3.size() == 3); + + // ----- Copy Constructors ----- // + + Array array4(array1); + rp3d_test(array4.capacity() == 0); + rp3d_test(array4.size() == 0); + + Array array5(array3); + rp3d_test(array5.capacity() == array3.capacity()); + rp3d_test(array5.size() == array3.size()); + for (uint i=0; i arra6(mAllocator, 20); + rp3d_test(arra6.capacity() == 20); + for (uint i=0; i<20; i++) { + arra6.add("test"); + } + rp3d_test(arra6.capacity() == 20); + arra6.add("test"); + rp3d_test(arra6.capacity() == 40); + } + + void testAddRemoveClear() { + + // ----- Test add() ----- // + + Array array1(mAllocator); + array1.add(4); + rp3d_test(array1.size() == 1); + rp3d_test(array1[0] == 4); + array1.add(9); + rp3d_test(array1.size() == 2); + rp3d_test(array1[0] == 4); + rp3d_test(array1[1] == 9); + + const int arraySize = 15; + int arrayTest[arraySize] = {3, 145, -182, 34, 12, 95, -1834, 4143, -111, -111, 4343, 234, 22983, -3432, 753}; + Array array2(mAllocator); + for (uint i=0; i array3(mAllocator); + array3.add(1); + array3.add(2); + array3.add(3); + array3.add(4); + + auto it = array3.removeAt(3); + rp3d_test(array3.size() == 3); + rp3d_test(array3.capacity() == 4); + rp3d_test(it == array3.end()); + rp3d_test(array3[0] == 1); + rp3d_test(array3[1] == 2); + rp3d_test(array3[2] == 3); + + it = array3.removeAt(1); + rp3d_test(array3.size() == 2); + rp3d_test(array3.capacity() == 4); + rp3d_test(array3[0] == 1); + rp3d_test(array3[1] == 3); + rp3d_test(*it == 3); + + array3.removeAt(0); + rp3d_test(array3.size() == 1); + rp3d_test(array3.capacity() == 4); + rp3d_test(array3[0] == 3); + + it = array3.removeAt(0); + rp3d_test(array3.size() == 0); + rp3d_test(array3.capacity() == 4); + rp3d_test(it == array3.end()); + + array3.add(1); + array3.add(2); + array3.add(3); + it = array3.begin(); + array3.remove(it); + rp3d_test(array3.size() == 2); + rp3d_test(array3[0] == 2); + rp3d_test(array3[1] == 3); + it = array3.find(3); + array3.remove(it); + rp3d_test(array3.size() == 1); + rp3d_test(array3[0] == 2); + + array3.add(5); + array3.add(6); + array3.add(7); + it = array3.remove(7); + rp3d_test(it == array3.end()); + rp3d_test(array3.size() == 3); + it = array3.remove(5); + rp3d_test((*it) == 6); + + // ----- Test addRange() ----- // + + Array array4(mAllocator); + array4.add(1); + array4.add(2); + array4.add(3); + + Array array5(mAllocator); + array5.add(4); + array5.add(5); + + Array array6(mAllocator); + array6.addRange(array5); + rp3d_test(array6.size() == array5.size()); + rp3d_test(array6[0] == 4); + rp3d_test(array6[1] == 5); + + array4.addRange(array5); + rp3d_test(array4.size() == 3 + array5.size()); + rp3d_test(array4[0] == 1); + rp3d_test(array4[1] == 2); + rp3d_test(array4[2] == 3); + rp3d_test(array4[3] == 4); + rp3d_test(array4[4] == 5); + + // ----- Test clear() ----- // + + Array array7(mAllocator); + array7.add("test1"); + array7.add("test2"); + array7.add("test3"); + array7.clear(); + rp3d_test(array7.size() == 0); + array7.add("new"); + rp3d_test(array7.size() == 1); + rp3d_test(array7[0] == "new"); + + // ----- Test removeAtAndReplaceByLast() ----- // + + Array array8(mAllocator); + array8.add(1); + array8.add(2); + array8.add(3); + array8.add(4); + array8.removeAtAndReplaceByLast(1); + rp3d_test(array8.size() == 3); + rp3d_test(array8[0] == 1); + rp3d_test(array8[1] == 4); + rp3d_test(array8[2] == 3); + array8.removeAtAndReplaceByLast(2); + rp3d_test(array8.size() == 2); + rp3d_test(array8[0] == 1); + rp3d_test(array8[1] == 4); + array8.removeAtAndReplaceByLast(0); + rp3d_test(array8.size() == 1); + rp3d_test(array8[0] == 4); + array8.removeAtAndReplaceByLast(0); + rp3d_test(array8.size() == 0); + } + + void testAssignment() { + + Array array1(mAllocator); + array1.add(1); + array1.add(2); + array1.add(3); + + Array array2(mAllocator); + array2.add(5); + array2.add(6); + + Array array3(mAllocator); + Array array4(mAllocator); + array4.add(1); + array4.add(2); + + Array array5(mAllocator); + array5.add(1); + array5.add(2); + array5.add(3); + + array3 = array2; + rp3d_test(array2.size() == array3.size()); + rp3d_test(array2[0] == array3[0]); + rp3d_test(array2[1] == array3[1]); + + array4 = array1; + rp3d_test(array4.size() == array1.size()) + rp3d_test(array4[0] == array1[0]); + rp3d_test(array4[1] == array1[1]); + rp3d_test(array4[2] == array1[2]); + + array5 = array2; + rp3d_test(array5.size() == array2.size()); + rp3d_test(array5[0] == array2[0]); + rp3d_test(array5[1] == array2[1]); + } + + void testIndexing() { + + Array array1(mAllocator); + array1.add(1); + array1.add(2); + array1.add(3); + + rp3d_test(array1[0] == 1); + rp3d_test(array1[1] == 2); + rp3d_test(array1[2] == 3); + + array1[0] = 6; + array1[1] = 7; + array1[2] = 8; + + rp3d_test(array1[0] == 6); + rp3d_test(array1[1] == 7); + rp3d_test(array1[2] == 8); + + const int a = array1[0]; + const int b = array1[1]; + rp3d_test(a == 6); + rp3d_test(b == 7); + + array1[0]++; + array1[1]++; + rp3d_test(array1[0] == 7); + rp3d_test(array1[1] == 8); + } + + void testFind() { + + Array array1(mAllocator); + array1.add(1); + array1.add(2); + array1.add(3); + array1.add(4); + array1.add(5); + + rp3d_test(array1.find(1) == array1.begin()); + rp3d_test(*(array1.find(2)) == 2); + rp3d_test(*(array1.find(5)) == 5); + } + + void testEquality() { + + Array array1(mAllocator); + array1.add(1); + array1.add(2); + array1.add(3); + + Array array2(mAllocator); + array2.add(1); + array2.add(2); + + Array array3(mAllocator); + array3.add(1); + array3.add(2); + array3.add(3); + + Array array4(mAllocator); + array4.add(1); + array4.add(5); + array4.add(3); + + rp3d_test(array1 == array1); + rp3d_test(array1 != array2); + rp3d_test(array1 == array3); + rp3d_test(array1 != array4); + rp3d_test(array2 != array4); + } + + void testReserve() { + + Array array1(mAllocator); + array1.reserve(10); + rp3d_test(array1.size() == 0); + rp3d_test(array1.capacity() == 10); + array1.add(1); + array1.add(2); + rp3d_test(array1.capacity() == 10); + rp3d_test(array1.size() == 2); + rp3d_test(array1[0] == 1); + rp3d_test(array1[1] == 2); + + array1.reserve(1); + rp3d_test(array1.capacity() == 10); + + array1.reserve(100); + rp3d_test(array1.capacity() == 100); + rp3d_test(array1[0] == 1); + rp3d_test(array1[1] == 2); + } + + void testIterators() { + + Array array1(mAllocator); + + rp3d_test(array1.begin() == array1.end()); + + array1.add(5); + array1.add(6); + array1.add(8); + array1.add(-1); + + Array::Iterator itBegin = array1.begin(); + Array::Iterator itEnd = array1.end(); + Array::Iterator it = array1.begin(); + + rp3d_test(itBegin < itEnd); + rp3d_test(itBegin <= itEnd); + rp3d_test(itEnd > itBegin); + rp3d_test(itEnd >= itBegin); + + rp3d_test(itBegin == it); + rp3d_test(*it == 5); + rp3d_test(*(it++) == 5); + rp3d_test(*it == 6); + rp3d_test(*(it--) == 6); + rp3d_test(*it == 5); + rp3d_test(*(++it) == 6); + rp3d_test(*it == 6); + rp3d_test(*(--it) == 5); + rp3d_test(*it == 5); + rp3d_test(it == itBegin); + + it = array1.end(); + rp3d_test(it == itEnd); + it--; + rp3d_test(*it == -1); + it++; + rp3d_test(it == itEnd); + + Array array2(mAllocator); + for (auto it = array1.begin(); it != array1.end(); ++it) { + array2.add(*it); + } + + rp3d_test(array1 == array2); + + it = itBegin; + rp3d_test(*(it + 2) == 8); + it += 2; + rp3d_test(*it == 8); + rp3d_test(*(it - 2) == 5); + it -= 2; + rp3d_test(*it == 5); + rp3d_test((itEnd - itBegin) == 4); + + it = itBegin; + *it = 19; + rp3d_test(*it == 19); + } + + }; + +} + +#endif diff --git a/test/tests/containers/TestList.h b/test/tests/containers/TestList.h deleted file mode 100644 index 5190e341..00000000 --- a/test/tests/containers/TestList.h +++ /dev/null @@ -1,427 +0,0 @@ -/******************************************************************************** -* ReactPhysics3D physics library, http://www.reactphysics3d.com * -* Copyright (c) 2010-2016 Daniel Chappuis * -********************************************************************************* -* * -* This software is provided 'as-is', without any express or implied warranty. * -* In no event will the authors be held liable for any damages arising from the * -* use of this software. * -* * -* Permission is granted to anyone to use this software for any purpose, * -* including commercial applications, and to alter it and redistribute it * -* freely, subject to the following restrictions: * -* * -* 1. The origin of this software must not be misrepresented; you must not claim * -* that you wrote the original software. If you use this software in a * -* product, an acknowledgment in the product documentation would be * -* appreciated but is not required. * -* * -* 2. Altered source versions must be plainly marked as such, and must not be * -* misrepresented as being the original software. * -* * -* 3. This notice may not be removed or altered from any source distribution. * -* * -********************************************************************************/ - -#ifndef TEST_LIST_H -#define TEST_LIST_H - -// Libraries -#include "Test.h" -#include -#include - -/// Reactphysics3D namespace -namespace reactphysics3d { - -// Class TestList -/** - * Unit test for the List class - */ -class TestList : public Test { - - private : - - // ---------- Atributes ---------- // - - DefaultAllocator mAllocator; - - public : - - // ---------- Methods ---------- // - - /// Constructor - TestList(const std::string& name) : Test(name) { - - } - - /// Run the tests - void run() { - - testConstructors(); - testAddRemoveClear(); - testAssignment(); - testIndexing(); - testFind(); - testEquality(); - testReserve(); - testIterators(); - } - - void testConstructors() { - - // ----- Constructors ----- // - - List list1(mAllocator); - rp3d_test(list1.capacity() == 0); - rp3d_test(list1.size() == 0); - - List list2(mAllocator, 100); - rp3d_test(list2.capacity() == 100); - rp3d_test(list2.size() == 0); - - List list3(mAllocator); - list3.add(1); - list3.add(2); - list3.add(3); - rp3d_test(list3.capacity() == 4); - rp3d_test(list3.size() == 3); - - // ----- Copy Constructors ----- // - - List list4(list1); - rp3d_test(list4.capacity() == 0); - rp3d_test(list4.size() == 0); - - List list5(list3); - rp3d_test(list5.capacity() == list3.size()); - rp3d_test(list5.size() == list3.size()); - for (uint i=0; i list6(mAllocator, 20); - rp3d_test(list6.capacity() == 20); - for (uint i=0; i<20; i++) { - list6.add("test"); - } - rp3d_test(list6.capacity() == 20); - list6.add("test"); - rp3d_test(list6.capacity() == 40); - } - - void testAddRemoveClear() { - - // ----- Test add() ----- // - - List list1(mAllocator); - list1.add(4); - rp3d_test(list1.size() == 1); - rp3d_test(list1[0] == 4); - list1.add(9); - rp3d_test(list1.size() == 2); - rp3d_test(list1[0] == 4); - rp3d_test(list1[1] == 9); - - const int arraySize = 15; - int arrayTest[arraySize] = {3, 145, -182, 34, 12, 95, -1834, 4143, -111, -111, 4343, 234, 22983, -3432, 753}; - List list2(mAllocator); - for (uint i=0; i list3(mAllocator); - list3.add(1); - list3.add(2); - list3.add(3); - list3.add(4); - - auto it = list3.removeAt(3); - rp3d_test(list3.size() == 3); - rp3d_test(list3.capacity() == 4); - rp3d_test(it == list3.end()); - rp3d_test(list3[0] == 1); - rp3d_test(list3[1] == 2); - rp3d_test(list3[2] == 3); - - it = list3.removeAt(1); - rp3d_test(list3.size() == 2); - rp3d_test(list3.capacity() == 4); - rp3d_test(list3[0] == 1); - rp3d_test(list3[1] == 3); - rp3d_test(*it == 3); - - list3.removeAt(0); - rp3d_test(list3.size() == 1); - rp3d_test(list3.capacity() == 4); - rp3d_test(list3[0] == 3); - - it = list3.removeAt(0); - rp3d_test(list3.size() == 0); - rp3d_test(list3.capacity() == 4); - rp3d_test(it == list3.end()); - - list3.add(1); - list3.add(2); - list3.add(3); - it = list3.begin(); - list3.remove(it); - rp3d_test(list3.size() == 2); - rp3d_test(list3[0] == 2); - rp3d_test(list3[1] == 3); - it = list3.find(3); - list3.remove(it); - rp3d_test(list3.size() == 1); - rp3d_test(list3[0] == 2); - - list3.add(5); - list3.add(6); - list3.add(7); - it = list3.remove(7); - rp3d_test(it == list3.end()); - rp3d_test(list3.size() == 3); - it = list3.remove(5); - rp3d_test((*it) == 6); - - // ----- Test addRange() ----- // - - List list4(mAllocator); - list4.add(1); - list4.add(2); - list4.add(3); - - List list5(mAllocator); - list5.add(4); - list5.add(5); - - List list6(mAllocator); - list6.addRange(list5); - rp3d_test(list6.size() == list5.size()); - rp3d_test(list6[0] == 4); - rp3d_test(list6[1] == 5); - - list4.addRange(list5); - rp3d_test(list4.size() == 3 + list5.size()); - rp3d_test(list4[0] == 1); - rp3d_test(list4[1] == 2); - rp3d_test(list4[2] == 3); - rp3d_test(list4[3] == 4); - rp3d_test(list4[4] == 5); - - // ----- Test clear() ----- // - - List list7(mAllocator); - list7.add("test1"); - list7.add("test2"); - list7.add("test3"); - list7.clear(); - rp3d_test(list7.size() == 0); - list7.add("new"); - rp3d_test(list7.size() == 1); - rp3d_test(list7[0] == "new"); - } - - void testAssignment() { - - List list1(mAllocator); - list1.add(1); - list1.add(2); - list1.add(3); - - List list2(mAllocator); - list2.add(5); - list2.add(6); - - List list3(mAllocator); - List list4(mAllocator); - list4.add(1); - list4.add(2); - - List list5(mAllocator); - list5.add(1); - list5.add(2); - list5.add(3); - - list3 = list2; - rp3d_test(list2.size() == list3.size()); - rp3d_test(list2[0] == list3[0]); - rp3d_test(list2[1] == list3[1]); - - list4 = list1; - rp3d_test(list4.size() == list1.size()) - rp3d_test(list4[0] == list1[0]); - rp3d_test(list4[1] == list1[1]); - rp3d_test(list4[2] == list1[2]); - - list5 = list2; - rp3d_test(list5.size() == list2.size()); - rp3d_test(list5[0] == list2[0]); - rp3d_test(list5[1] == list2[1]); - } - - void testIndexing() { - - List list1(mAllocator); - list1.add(1); - list1.add(2); - list1.add(3); - - rp3d_test(list1[0] == 1); - rp3d_test(list1[1] == 2); - rp3d_test(list1[2] == 3); - - list1[0] = 6; - list1[1] = 7; - list1[2] = 8; - - rp3d_test(list1[0] == 6); - rp3d_test(list1[1] == 7); - rp3d_test(list1[2] == 8); - - const int a = list1[0]; - const int b = list1[1]; - rp3d_test(a == 6); - rp3d_test(b == 7); - - list1[0]++; - list1[1]++; - rp3d_test(list1[0] == 7); - rp3d_test(list1[1] == 8); - } - - void testFind() { - - List list1(mAllocator); - list1.add(1); - list1.add(2); - list1.add(3); - list1.add(4); - list1.add(5); - - rp3d_test(list1.find(1) == list1.begin()); - rp3d_test(*(list1.find(2)) == 2); - rp3d_test(*(list1.find(5)) == 5); - } - - void testEquality() { - - List list1(mAllocator); - list1.add(1); - list1.add(2); - list1.add(3); - - List list2(mAllocator); - list2.add(1); - list2.add(2); - - List list3(mAllocator); - list3.add(1); - list3.add(2); - list3.add(3); - - List list4(mAllocator); - list4.add(1); - list4.add(5); - list4.add(3); - - rp3d_test(list1 == list1); - rp3d_test(list1 != list2); - rp3d_test(list1 == list3); - rp3d_test(list1 != list4); - rp3d_test(list2 != list4); - } - - void testReserve() { - - List list1(mAllocator); - list1.reserve(10); - rp3d_test(list1.size() == 0); - rp3d_test(list1.capacity() == 10); - list1.add(1); - list1.add(2); - rp3d_test(list1.capacity() == 10); - rp3d_test(list1.size() == 2); - rp3d_test(list1[0] == 1); - rp3d_test(list1[1] == 2); - - list1.reserve(1); - rp3d_test(list1.capacity() == 10); - - list1.reserve(100); - rp3d_test(list1.capacity() == 100); - rp3d_test(list1[0] == 1); - rp3d_test(list1[1] == 2); - } - - void testIterators() { - - List list1(mAllocator); - - rp3d_test(list1.begin() == list1.end()); - - list1.add(5); - list1.add(6); - list1.add(8); - list1.add(-1); - - List::Iterator itBegin = list1.begin(); - List::Iterator itEnd = list1.end(); - List::Iterator it = list1.begin(); - - rp3d_test(itBegin < itEnd); - rp3d_test(itBegin <= itEnd); - rp3d_test(itEnd > itBegin); - rp3d_test(itEnd >= itBegin); - - rp3d_test(itBegin == it); - rp3d_test(*it == 5); - rp3d_test(*(it++) == 5); - rp3d_test(*it == 6); - rp3d_test(*(it--) == 6); - rp3d_test(*it == 5); - rp3d_test(*(++it) == 6); - rp3d_test(*it == 6); - rp3d_test(*(--it) == 5); - rp3d_test(*it == 5); - rp3d_test(it == itBegin); - - it = list1.end(); - rp3d_test(it == itEnd); - it--; - rp3d_test(*it == -1); - it++; - rp3d_test(it == itEnd); - - List list2(mAllocator); - for (auto it = list1.begin(); it != list1.end(); ++it) { - list2.add(*it); - } - - rp3d_test(list1 == list2); - - it = itBegin; - rp3d_test(*(it + 2) == 8); - it += 2; - rp3d_test(*it == 8); - rp3d_test(*(it - 2) == 5); - it -= 2; - rp3d_test(*it == 5); - rp3d_test((itEnd - itBegin) == 4); - - it = itBegin; - *it = 19; - rp3d_test(*it == 19); - } - - }; - -} - -#endif diff --git a/test/tests/containers/TestSet.h b/test/tests/containers/TestSet.h index 0d1f4091..6d6c488c 100644 --- a/test/tests/containers/TestSet.h +++ b/test/tests/containers/TestSet.h @@ -432,18 +432,18 @@ class TestSet : public Test { set1.add(3); set1.add(4); - List list1 = set1.toList(mAllocator); - rp3d_test(list1.size() == 4); - rp3d_test(list1.find(1) != list1.end()); - rp3d_test(list1.find(2) != list1.end()); - rp3d_test(list1.find(3) != list1.end()); - rp3d_test(list1.find(4) != list1.end()); - rp3d_test(list1.find(5) == list1.end()); - rp3d_test(list1.find(6) == list1.end()); + Array array1 = set1.toArray(mAllocator); + rp3d_test(array1.size() == 4); + rp3d_test(array1.find(1) != array1.end()); + rp3d_test(array1.find(2) != array1.end()); + rp3d_test(array1.find(3) != array1.end()); + rp3d_test(array1.find(4) != array1.end()); + rp3d_test(array1.find(5) == array1.end()); + rp3d_test(array1.find(6) == array1.end()); Set set2(mAllocator); - List list2 = set2.toList(mAllocator); - rp3d_test(list2.size() == 0); + Array array2 = set2.toArray(mAllocator); + rp3d_test(array2.size() == 0); } }; diff --git a/test/tests/mathematics/TestMathematicsFunctions.h b/test/tests/mathematics/TestMathematicsFunctions.h index 5a8c5623..7bd92432 100644 --- a/test/tests/mathematics/TestMathematicsFunctions.h +++ b/test/tests/mathematics/TestMathematicsFunctions.h @@ -27,8 +27,9 @@ #define TEST_MATHEMATICS_FUNCTIONS_H // Libraries -#include +#include #include +#include /// Reactphysics3D namespace namespace reactphysics3d { @@ -186,12 +187,12 @@ class TestMathematicsFunctions : public Test { segmentVertices.push_back(Vector3(-6, 3, 0)); segmentVertices.push_back(Vector3(8, 3, 0)); - List planesNormals(mAllocator, 2); - List planesPoints(mAllocator, 2); + Array planesNormals(mAllocator, 2); + Array planesPoints(mAllocator, 2); planesNormals.add(Vector3(-1, 0, 0)); planesPoints.add(Vector3(4, 0, 0)); - List clipSegmentVertices = clipSegmentWithPlanes(segmentVertices[0], segmentVertices[1], + Array clipSegmentVertices = clipSegmentWithPlanes(segmentVertices[0], segmentVertices[1], planesPoints, planesNormals, mAllocator); rp3d_test(clipSegmentVertices.size() == 2); rp3d_test(approxEqual(clipSegmentVertices[0].x, -6, 0.000001)); @@ -234,15 +235,16 @@ class TestMathematicsFunctions : public Test { clipSegmentVertices = clipSegmentWithPlanes(segmentVertices[0], segmentVertices[1], planesPoints, planesNormals, mAllocator); rp3d_test(clipSegmentVertices.size() == 0); + /* TODO : UNCOMMENT THIS // Test clipPolygonWithPlanes() - List polygonVertices(mAllocator); + Array polygonVertices(mAllocator); polygonVertices.add(Vector3(-4, 2, 0)); polygonVertices.add(Vector3(7, 2, 0)); polygonVertices.add(Vector3(7, 4, 0)); polygonVertices.add(Vector3(-4, 4, 0)); - List polygonPlanesNormals(mAllocator); - List polygonPlanesPoints(mAllocator); + Array polygonPlanesNormals(mAllocator); + Array polygonPlanesPoints(mAllocator); polygonPlanesNormals.add(Vector3(1, 0, 0)); polygonPlanesPoints.add(Vector3(0, 0, 0)); polygonPlanesNormals.add(Vector3(0, 1, 0)); @@ -252,7 +254,8 @@ class TestMathematicsFunctions : public Test { polygonPlanesNormals.add(Vector3(0, -1, 0)); polygonPlanesPoints.add(Vector3(10, 5, 0)); - List clipPolygonVertices = clipPolygonWithPlanes(polygonVertices, polygonPlanesPoints, polygonPlanesNormals, mAllocator); + Array clipPolygonVertices(mAllocator); + clipPolygonWithPlanes(polygonVertices, polygonPlanesPoints, polygonPlanesNormals, clipPolygonVertices, mAllocator); rp3d_test(clipPolygonVertices.size() == 4); rp3d_test(approxEqual(clipPolygonVertices[0].x, 0, 0.000001)); rp3d_test(approxEqual(clipPolygonVertices[0].y, 2, 0.000001)); @@ -266,7 +269,35 @@ class TestMathematicsFunctions : public Test { rp3d_test(approxEqual(clipPolygonVertices[3].x, 0, 0.000001)); rp3d_test(approxEqual(clipPolygonVertices[3].y, 4, 0.000001)); rp3d_test(approxEqual(clipPolygonVertices[3].z, 0, 0.000001)); + */ + // Test isPowerOfTwo() + rp3d_test(!isPowerOfTwo(0)); + rp3d_test(!isPowerOfTwo(3)); + rp3d_test(!isPowerOfTwo(144)); + rp3d_test(!isPowerOfTwo(13)); + rp3d_test(!isPowerOfTwo(18)); + rp3d_test(!isPowerOfTwo(1000)); + + rp3d_test(isPowerOfTwo(1)); + rp3d_test(isPowerOfTwo(2)); + rp3d_test(isPowerOfTwo(4)); + rp3d_test(isPowerOfTwo(8)); + rp3d_test(isPowerOfTwo(256)); + rp3d_test(isPowerOfTwo(1024)); + rp3d_test(isPowerOfTwo(2048)); + + // Test nextPowerOfTwo32Bits() + rp3d_test(nextPowerOfTwo32Bits(0) == 1); + rp3d_test(nextPowerOfTwo32Bits(1) == 1); + rp3d_test(nextPowerOfTwo32Bits(2) == 2); + rp3d_test(nextPowerOfTwo32Bits(3) == 4); + rp3d_test(nextPowerOfTwo32Bits(5) == 8); + rp3d_test(nextPowerOfTwo32Bits(6) == 8); + rp3d_test(nextPowerOfTwo32Bits(7) == 8); + rp3d_test(nextPowerOfTwo32Bits(1000) == 1024); + rp3d_test(nextPowerOfTwo32Bits(129) == 256); + rp3d_test(nextPowerOfTwo32Bits(260) == 512); } }; diff --git a/test/tests/mathematics/TestQuaternion.h b/test/tests/mathematics/TestQuaternion.h index 3a25cba9..edade07b 100644 --- a/test/tests/mathematics/TestQuaternion.h +++ b/test/tests/mathematics/TestQuaternion.h @@ -56,8 +56,8 @@ class TestQuaternion : public Test { /// Constructor TestQuaternion(const std::string& name) : Test(name), mIdentity(Quaternion::identity()) { - decimal sinA = sin(decimal(PI/8.0)); - decimal cosA = cos(decimal(PI/8.0)); + decimal sinA = sin(decimal(PI_RP3D/8.0)); + decimal cosA = cos(decimal(PI_RP3D/8.0)); Vector3 vector(2, 3, 4); vector.normalize(); mQuaternion1 = Quaternion(vector.x * sinA, vector.y * sinA, vector.z * sinA, cosA); @@ -90,9 +90,24 @@ class TestQuaternion : public Test { rp3d_test(approxEqual(quaternion4.z, mQuaternion1.z)); rp3d_test(approxEqual(quaternion4.w, mQuaternion1.w)); + Matrix3x3 original(0.001743,-0.968608,0.248589,-0.614229,-0.197205,-0.764090,0.789126,-0.151359,-0.595290); + Matrix3x3 converted = Quaternion(original).getMatrix(); + rp3d_test(approxEqual(original[0][0], converted[0][0], 0.0001)); + rp3d_test(approxEqual(original[0][1], converted[0][1], 0.0001)); + rp3d_test(approxEqual(original[0][2], converted[0][2], 0.0001)); + rp3d_test(approxEqual(original[1][0], converted[1][0], 0.0001)); + rp3d_test(approxEqual(original[1][1], converted[1][1], 0.0001)); + rp3d_test(approxEqual(original[1][2], converted[1][2], 0.0001)); + rp3d_test(approxEqual(original[2][0], converted[2][0], 0.0001)); + rp3d_test(approxEqual(original[2][1], converted[2][1], 0.0001)); + rp3d_test(approxEqual(original[2][2], converted[2][2], 0.0001)); + + std::cout << original.to_string() << std::endl; + std::cout << converted.to_string() << std::endl; + // Test conversion from Euler angles to quaternion - const decimal PI_OVER_2 = PI * decimal(0.5); + const decimal PI_OVER_2 = PI_RP3D * decimal(0.5); const decimal PI_OVER_4 = PI_OVER_2 * decimal(0.5); Quaternion quaternion5 = Quaternion::fromEulerAngles(PI_OVER_2, 0, 0); Quaternion quaternionTest5(std::sin(PI_OVER_4), 0, 0, std::cos(PI_OVER_4)); @@ -189,7 +204,7 @@ class TestQuaternion : public Test { Vector3 originalAxis = Vector3(2, 3, 4).getUnit(); mQuaternion1.getRotationAngleAxis(angle, axis); rp3d_test(approxEqual(axis.x, originalAxis.x)); - rp3d_test(approxEqual(angle, decimal(PI/4.0), decimal(10e-6))); + rp3d_test(approxEqual(angle, decimal(PI_RP3D/4.0), decimal(10e-6))); // Test the method that returns the corresponding matrix Matrix3x3 matrix = mQuaternion1.getMatrix(); @@ -207,14 +222,14 @@ class TestQuaternion : public Test { Quaternion test2 = Quaternion::slerp(quatStart, quatEnd, 1.0); rp3d_test(test1 == quatStart); rp3d_test(test2 == quatEnd); - decimal sinA = sin(decimal(PI/4.0)); - decimal cosA = cos(decimal(PI/4.0)); + decimal sinA = sin(decimal(PI_RP3D/4.0)); + decimal cosA = cos(decimal(PI_RP3D/4.0)); Quaternion quat(sinA, 0, 0, cosA); Quaternion test3 = Quaternion::slerp(mIdentity, quat, decimal(0.5)); - rp3d_test(approxEqual(test3.x, sin(decimal(PI/8.0)))); + rp3d_test(approxEqual(test3.x, sin(decimal(PI_RP3D/8.0)))); rp3d_test(approxEqual(test3.y, 0.0)); rp3d_test(approxEqual(test3.z, 0.0)); - rp3d_test(approxEqual(test3.w, cos(decimal(PI/8.0)), decimal(10e-6))); + rp3d_test(approxEqual(test3.w, cos(decimal(PI_RP3D/8.0)), decimal(10e-6))); } /// Test overloaded operators diff --git a/test/tests/mathematics/TestTransform.h b/test/tests/mathematics/TestTransform.h index cb020df9..74006034 100644 --- a/test/tests/mathematics/TestTransform.h +++ b/test/tests/mathematics/TestTransform.h @@ -65,12 +65,12 @@ class TestTransform : public Test { Vector3 unitVec(1, 1, 1); unitVec.normalize(); - decimal sinA = std::sin(PI/8.0f); - decimal cosA = std::cos(PI/8.0f); + decimal sinA = std::sin(PI_RP3D/8.0f); + decimal cosA = std::cos(PI_RP3D/8.0f); mTransform1 = Transform(Vector3(4, 5, 6), Quaternion(sinA * unitVec, cosA)); - decimal sinB = std::sin(PI/3.0f); - decimal cosB = std::cos(PI/3.0f); + decimal sinB = std::sin(PI_RP3D/3.0f); + decimal cosB = std::cos(PI_RP3D/3.0f); mTransform2 = Transform(Vector3(8, 45, -6), Quaternion(sinB * unitVec, cosB)); } @@ -162,10 +162,10 @@ class TestTransform : public Test { rp3d_test(transformStart == mTransform1); rp3d_test(transformEnd == mTransform2); - decimal sinA = sin(PI/3.0f); - decimal cosA = cos(PI/3.0f); - decimal sinB = sin(PI/6.0f); - decimal cosB = cos(PI/6.0f); + decimal sinA = sin(PI_RP3D/3.0f); + decimal cosA = cos(PI_RP3D/3.0f); + decimal sinB = sin(PI_RP3D/6.0f); + decimal cosB = cos(PI_RP3D/6.0f); Transform transform1(Vector3(4, 5, 6), Quaternion::identity()); Transform transform2(Vector3(8, 11, 16), Quaternion(sinA, sinA, sinA, cosA)); Transform transform = Transform::interpolateTransforms(transform1, transform2, 0.5); diff --git a/testbed/CMakeLists.txt b/testbed/CMakeLists.txt index 29459aaf..7305e9cf 100755 --- a/testbed/CMakeLists.txt +++ b/testbed/CMakeLists.txt @@ -8,6 +8,7 @@ project(Testbed) set(NANOGUI_BUILD_EXAMPLES OFF CACHE BOOL " " FORCE) set(NANOGUI_BUILD_PYTHON OFF CACHE BOOL " " FORCE) set(NANOGUI_INSTALL OFF CACHE BOOL " " FORCE) +set(NANOGUI_BACKEND OpenGL CACHE BOOL " " FORCE) # ---- Make sure to recursively clone all the git submodules for external libraries (nanogui) --- # find_package(Git QUIET) @@ -123,6 +124,12 @@ set(COMMON_SOURCES # Examples scenes source files set(SCENES_SOURCES + scenes/ballandsocketjointsnet/BallAndSocketJointsNetScene.h + scenes/ballandsocketjointsnet/BallAndSocketJointsNetScene.cpp + scenes/ballandsocketjointschain/BallAndSocketJointsChainScene.h + scenes/ballandsocketjointschain/BallAndSocketJointsChainScene.cpp + scenes/hingejointschain/HingeJointsChainScene.h + scenes/hingejointschain/HingeJointsChainScene.cpp scenes/cubes/CubesScene.h scenes/cubes/CubesScene.cpp scenes/joints/JointsScene.h diff --git a/testbed/scenes/ballandsocketjointschain/BallAndSocketJointsChainScene.cpp b/testbed/scenes/ballandsocketjointschain/BallAndSocketJointsChainScene.cpp new file mode 100644 index 00000000..8653d1c2 --- /dev/null +++ b/testbed/scenes/ballandsocketjointschain/BallAndSocketJointsChainScene.cpp @@ -0,0 +1,159 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://www.reactphysics3d.com * +* Copyright (c) 2010-2016 Daniel Chappuis * +********************************************************************************* +* * +* This software is provided 'as-is', without any express or implied warranty. * +* In no event will the authors be held liable for any damages arising from the * +* use of this software. * +* * +* Permission is granted to anyone to use this software for any purpose, * +* including commercial applications, and to alter it and redistribute it * +* freely, subject to the following restrictions: * +* * +* 1. The origin of this software must not be misrepresented; you must not claim * +* that you wrote the original software. If you use this software in a * +* product, an acknowledgment in the product documentation would be * +* appreciated but is not required. * +* * +* 2. Altered source versions must be plainly marked as such, and must not be * +* misrepresented as being the original software. * +* * +* 3. This notice may not be removed or altered from any source distribution. * +* * +********************************************************************************/ + +// Libraries +#include "BallAndSocketJointsChainScene.h" +#include + +// Namespaces +using namespace openglframework; +using namespace ballandsocketjointschainscene; + +// Constructor +BallAndSocketJointsChainScene::BallAndSocketJointsChainScene(const std::string& name, EngineSettings& settings) + : SceneDemo(name, settings, true, SCENE_RADIUS) { + + std::string meshFolderPath("meshes/"); + + // Compute the radius and the center of the scene + openglframework::Vector3 center(0, 0, 0); + + // Set the center of the scene + setScenePosition(center, SCENE_RADIUS); + + // Gravity vector in the physics world + rp3d::Vector3 gravity(0, rp3d::decimal(-9.81), 0); + + rp3d::PhysicsWorld::WorldSettings worldSettings; + worldSettings.worldName = name; + + // Logger + rp3d::DefaultLogger* defaultLogger = mPhysicsCommon.createDefaultLogger(); + uint logLevel = static_cast(rp3d::Logger::Level::Information) | static_cast(rp3d::Logger::Level::Warning) | + static_cast(rp3d::Logger::Level::Error); + defaultLogger->addFileDestination("rp3d_log_" + name + ".html", logLevel, rp3d::DefaultLogger::Format::HTML); + mPhysicsCommon.setLogger(defaultLogger); + + // Create the physics world for the physics simulation + rp3d::PhysicsWorld* physicsWorld = mPhysicsCommon.createPhysicsWorld(worldSettings); + physicsWorld->setEventListener(this); + mPhysicsWorld = physicsWorld; + + // Create all the spheres of the scene + for (int i=0; isetColor(mObjectColorDemo); + mSpheres[i]->setSleepingColor(mSleepingColorDemo); + + // Change the material properties of the rigid body + rp3d::Material& material = mSpheres[i]->getCollider()->getMaterial(); + material.setBounciness(rp3d::decimal(0.0)); + + if (i == 0) { + mSpheres[i]->getRigidBody()->setType(rp3d::BodyType::STATIC); + } + + // Add the sphere the list of sphere in the scene + mPhysicsObjects.push_back(mSpheres[i]); + } + + // Set the position of the spheres before the joints creation + reset(); + + // Create the Ball-and-Socket joints + createJoints(); + + // Get the physics engine parameters + mEngineSettings.isGravityEnabled = mPhysicsWorld->isGravityEnabled(); + rp3d::Vector3 gravityVector = mPhysicsWorld->getGravity(); + mEngineSettings.gravity = openglframework::Vector3(gravityVector.x, gravityVector.y, gravityVector.z); + mEngineSettings.isSleepingEnabled = mPhysicsWorld->isSleepingEnabled(); + mEngineSettings.sleepLinearVelocity = mPhysicsWorld->getSleepLinearVelocity(); + mEngineSettings.sleepAngularVelocity = mPhysicsWorld->getSleepAngularVelocity(); + mEngineSettings.nbPositionSolverIterations = mPhysicsWorld->getNbIterationsPositionSolver(); + mEngineSettings.nbVelocitySolverIterations = mPhysicsWorld->getNbIterationsVelocitySolver(); + mEngineSettings.timeBeforeSleep = mPhysicsWorld->getTimeBeforeSleep(); +} + +// Destructor +BallAndSocketJointsChainScene::~BallAndSocketJointsChainScene() { + + // Destroy the joints + for (uint i=0; i < mBallAndSocketJoints.size(); i++) { + + mPhysicsWorld->destroyJoint(mBallAndSocketJoints[i]); + } + + // Destroy all the rigid bodies of the scene + for (int i=0; idestroyRigidBody(mSpheres[i]->getRigidBody()); + } + + // Destroy the physics world + mPhysicsCommon.destroyPhysicsWorld(mPhysicsWorld); +} + +// Create the joints +void BallAndSocketJointsChainScene::createJoints() { + + for (int i=0; i < NB_SPHERES-1; i++) { + + // Create the joint info object + rp3d::RigidBody* body1 = mSpheres[i]->getRigidBody(); + rp3d::RigidBody* body2 = mSpheres[i+1]->getRigidBody(); + rp3d::Vector3 body1Position = body1->getTransform().getPosition(); + rp3d::Vector3 body2Position = body2->getTransform().getPosition(); + const rp3d::Vector3 anchorPointWorldSpace = body1Position; + rp3d::BallAndSocketJointInfo jointInfo(body1, body2, anchorPointWorldSpace); + jointInfo.isCollisionEnabled = false; + rp3d::BallAndSocketJoint* joint = dynamic_cast( mPhysicsWorld->createJoint(jointInfo)); + mBallAndSocketJoints.push_back(joint); + } +} + +// Reset the scene +void BallAndSocketJointsChainScene::reset() { + + SceneDemo::reset(); + + const float space = 0.5f; + + const rp3d::Quaternion initOrientation = rp3d::Quaternion::identity(); + + for (int i=0; isetTransform(transform); + } +} diff --git a/include/reactphysics3d/collision/narrowphase/CapsuleVsCapsuleNarrowPhaseInfoBatch.h b/testbed/scenes/ballandsocketjointschain/BallAndSocketJointsChainScene.h similarity index 51% rename from include/reactphysics3d/collision/narrowphase/CapsuleVsCapsuleNarrowPhaseInfoBatch.h rename to testbed/scenes/ballandsocketjointschain/BallAndSocketJointsChainScene.h index 2312a2e2..c3bbdebd 100644 --- a/include/reactphysics3d/collision/narrowphase/CapsuleVsCapsuleNarrowPhaseInfoBatch.h +++ b/testbed/scenes/ballandsocketjointschain/BallAndSocketJointsChainScene.h @@ -1,6 +1,6 @@ /******************************************************************************** * ReactPhysics3D physics library, http://www.reactphysics3d.com * -* Copyright (c) 2010-2020 Daniel Chappuis * +* Copyright (c) 2010-2016 Daniel Chappuis * ********************************************************************************* * * * This software is provided 'as-is', without any express or implied warranty. * @@ -23,56 +23,54 @@ * * ********************************************************************************/ -#ifndef REACTPHYSICS3D_CAPSULE_VS_CAPSULE_NARROW_PHASE_INFO_BATCH_H -#define REACTPHYSICS3D_CAPSULE_VS_CAPSULE_NARROW_PHASE_INFO_BATCH_H +#ifndef BALL_AND_SOCKET_JOINTS_CHAIN_SCENE_H +#define BALL_AND_SOCKET_JOINTS_CHAIN_SCENE_H // Libraries -#include +#include "openglframework.h" +#include +#include "Sphere.h" +#include "SceneDemo.h" -/// Namespace ReactPhysics3D -namespace reactphysics3d { +namespace ballandsocketjointschainscene { -// Struct CapsuleVsCapsuleNarrowPhaseInfoBatch -/** - * This structure collects all the potential collisions from the middle-phase algorithm - * that have to be tested during narrow-phase collision detection. This class collects all the - * capsule vs capsule collision detection tests. - */ -struct CapsuleVsCapsuleNarrowPhaseInfoBatch : public NarrowPhaseInfoBatch { +// Constants +const float SCENE_RADIUS = 60.0f; +const float SPHERE_RADIUS = 1.0f; +const int NB_SPHERES = 20; + +// Class BallAndSocketJointsChain scene +class BallAndSocketJointsChainScene : public SceneDemo { + + protected : + + // -------------------- Attributes -------------------- // + + /// Spheres in the Ball-And-Socket joint net + Sphere* mSpheres[NB_SPHERES]; + + /// Ball-And-Socket joints of the chain + std::vector mBallAndSocketJoints; + + // -------------------- Methods -------------------- // + + /// Create the joints + void createJoints(); public: - /// List of radiuses for the first capsules - List capsule1Radiuses; - - /// List of radiuses for the second capsules - List capsule2Radiuses; - - /// List of heights for the first capsules - List capsule1Heights; - - /// List of heights for the second capsules - List capsule2Heights; + // -------------------- Methods -------------------- // /// Constructor - CapsuleVsCapsuleNarrowPhaseInfoBatch(MemoryAllocator& allocator, OverlappingPairs& overlappingPairs); + BallAndSocketJointsChainScene(const std::string& name, EngineSettings& settings); /// Destructor - virtual ~CapsuleVsCapsuleNarrowPhaseInfoBatch() override = default; + virtual ~BallAndSocketJointsChainScene() override ; - /// Add shapes to be tested during narrow-phase collision detection into the batch - virtual void addNarrowPhaseInfo(uint64 pairId, uint64 pairIndex, Entity collider1, Entity collider2, CollisionShape* shape1, - CollisionShape* shape2, const Transform& shape1Transform, - const Transform& shape2Transform, bool needToReportContacts, MemoryAllocator& shapeAllocator) override; - - // Initialize the containers using cached capacity - virtual void reserveMemory() override; - - /// Clear all the objects in the batch - virtual void clear() override; + /// Reset the scene + virtual void reset() override; }; } #endif - diff --git a/testbed/scenes/ballandsocketjointsnet/BallAndSocketJointsNetScene.cpp b/testbed/scenes/ballandsocketjointsnet/BallAndSocketJointsNetScene.cpp new file mode 100644 index 00000000..af38b7e5 --- /dev/null +++ b/testbed/scenes/ballandsocketjointsnet/BallAndSocketJointsNetScene.cpp @@ -0,0 +1,202 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://www.reactphysics3d.com * +* Copyright (c) 2010-2016 Daniel Chappuis * +********************************************************************************* +* * +* This software is provided 'as-is', without any express or implied warranty. * +* In no event will the authors be held liable for any damages arising from the * +* use of this software. * +* * +* Permission is granted to anyone to use this software for any purpose, * +* including commercial applications, and to alter it and redistribute it * +* freely, subject to the following restrictions: * +* * +* 1. The origin of this software must not be misrepresented; you must not claim * +* that you wrote the original software. If you use this software in a * +* product, an acknowledgment in the product documentation would be * +* appreciated but is not required. * +* * +* 2. Altered source versions must be plainly marked as such, and must not be * +* misrepresented as being the original software. * +* * +* 3. This notice may not be removed or altered from any source distribution. * +* * +********************************************************************************/ + +// Libraries +#include "BallAndSocketJointsNetScene.h" +#include + +// Namespaces +using namespace openglframework; +using namespace ballandsocketjointsnetscene; + +// Constructor +BallAndSocketJointsNetScene::BallAndSocketJointsNetScene(const std::string& name, EngineSettings& settings) + : SceneDemo(name, settings, true, SCENE_RADIUS) { + + std::string meshFolderPath("meshes/"); + + // Compute the radius and the center of the scene + openglframework::Vector3 center(0, 5, 0); + + // Set the center of the scene + setScenePosition(center, SCENE_RADIUS); + + // Gravity vector in the physics world + rp3d::Vector3 gravity(0, rp3d::decimal(-9.81), 0); + + rp3d::PhysicsWorld::WorldSettings worldSettings; + worldSettings.worldName = name; + + // Logger + rp3d::DefaultLogger* defaultLogger = mPhysicsCommon.createDefaultLogger(); + uint logLevel = static_cast(rp3d::Logger::Level::Information) | static_cast(rp3d::Logger::Level::Warning) | + static_cast(rp3d::Logger::Level::Error); + defaultLogger->addFileDestination("rp3d_log_" + name + ".html", logLevel, rp3d::DefaultLogger::Format::HTML); + mPhysicsCommon.setLogger(defaultLogger); + + // Create the physics world for the physics simulation + rp3d::PhysicsWorld* physicsWorld = mPhysicsCommon.createPhysicsWorld(worldSettings); + physicsWorld->setEventListener(this); + mPhysicsWorld = physicsWorld; + + // Create all the spheres of the scene + for (int i=0; isetColor(mObjectColorDemo); + sphere->setSleepingColor(mSleepingColorDemo); + + // Change the material properties of the rigid body + rp3d::Material& material = sphere->getCollider()->getMaterial(); + material.setBounciness(rp3d::decimal(0.0)); + + // Add the sphere the list of sphere in the scene + mNetSpheres[i][j] = sphere; + mPhysicsObjects.push_back(sphere); + } + } + + // Set the position of the spheres before the joints creation + reset(); + + // Create the Ball-and-Socket joints + createJoints(); + + // Create the main sphere + mMainSphere = new Sphere(true, 7, mPhysicsCommon, mPhysicsWorld, meshFolderPath); + mMainSphere->setColor(mObjectColorDemo); + mMainSphere->setSleepingColor(mSleepingColorDemo); + rp3d::Vector3 initPosition(0, 0, 0); + rp3d::Quaternion initOrientation = rp3d::Quaternion::identity(); + rp3d::Transform transform(initPosition, initOrientation); + mMainSphere->setTransform(transform); + mMainSphere->getRigidBody()->setType(rp3d::BodyType::STATIC); + rp3d::Material& material = mMainSphere->getCollider()->getMaterial(); + material.setBounciness(rp3d::decimal(0.0)); + mPhysicsObjects.push_back(mMainSphere); + + // Get the physics engine parameters + mEngineSettings.isGravityEnabled = mPhysicsWorld->isGravityEnabled(); + rp3d::Vector3 gravityVector = mPhysicsWorld->getGravity(); + mEngineSettings.gravity = openglframework::Vector3(gravityVector.x, gravityVector.y, gravityVector.z); + mEngineSettings.isSleepingEnabled = mPhysicsWorld->isSleepingEnabled(); + mEngineSettings.sleepLinearVelocity = mPhysicsWorld->getSleepLinearVelocity(); + mEngineSettings.sleepAngularVelocity = mPhysicsWorld->getSleepAngularVelocity(); + mEngineSettings.nbPositionSolverIterations = mPhysicsWorld->getNbIterationsPositionSolver(); + mEngineSettings.nbVelocitySolverIterations = mPhysicsWorld->getNbIterationsVelocitySolver(); + mEngineSettings.timeBeforeSleep = mPhysicsWorld->getTimeBeforeSleep(); +} + +// Destructor +BallAndSocketJointsNetScene::~BallAndSocketJointsNetScene() { + + // Destroy the joints + for (uint i=0; i < mBallAndSocketJoints.size(); i++) { + + mPhysicsWorld->destroyJoint(mBallAndSocketJoints[i]); + } + + // Destroy all the rigid bodies of the scene + for (int i=0; idestroyRigidBody(mNetSpheres[i][j]->getRigidBody()); + } + } + mPhysicsWorld->destroyRigidBody(mMainSphere->getRigidBody()); + + // Destroy the physics world + mPhysicsCommon.destroyPhysicsWorld(mPhysicsWorld); +} + +// Create the joints +void BallAndSocketJointsNetScene::createJoints() { + + for (int i=0; i 0) { + + // Create the joint info object + rp3d::RigidBody* body1 = mNetSpheres[i-1][j]->getRigidBody(); + rp3d::RigidBody* body2 = mNetSpheres[i][j]->getRigidBody(); + rp3d::Vector3 body1Position = body1->getTransform().getPosition(); + rp3d::Vector3 body2Position = body2->getTransform().getPosition(); + //const rp3d::Vector3 anchorPointWorldSpace = 0.5 * (body1Position + body2Position); + const rp3d::Vector3 anchorPointWorldSpace = body2Position; + rp3d::BallAndSocketJointInfo jointInfo(body1, body2, anchorPointWorldSpace); + jointInfo.isCollisionEnabled = false; + rp3d::BallAndSocketJoint* joint = dynamic_cast( mPhysicsWorld->createJoint(jointInfo)); + mBallAndSocketJoints.push_back(joint); + } + + if (j > 0) { + + // Create the joint info object + rp3d::RigidBody* body1 = mNetSpheres[i][j-1]->getRigidBody(); + rp3d::RigidBody* body2 = mNetSpheres[i][j]->getRigidBody(); + rp3d::Vector3 body1Position = body1->getTransform().getPosition(); + rp3d::Vector3 body2Position = body2->getTransform().getPosition(); + const rp3d::Vector3 anchorPointWorldSpace = body2Position; + rp3d::BallAndSocketJointInfo jointInfo(body1, body2, anchorPointWorldSpace); + jointInfo.isCollisionEnabled = false; + rp3d::BallAndSocketJoint* joint = dynamic_cast( mPhysicsWorld->createJoint(jointInfo)); + mBallAndSocketJoints.push_back(joint); + } + } + } +} + +// Reset the scene +void BallAndSocketJointsNetScene::reset() { + + SceneDemo::reset(); + + const float space = 0.5f; + const float startX = -(NB_ROWS_NET_SPHERES / 2.0f * (2.0 * SPHERE_RADIUS + space)); + const float startZ = -(NB_ROWS_NET_SPHERES / 2.0f * (2.0 * SPHERE_RADIUS + space)); + + const rp3d::Quaternion initOrientation = rp3d::Quaternion::identity(); + + for (int i=0; isetTransform(transform); + } + } +} diff --git a/include/reactphysics3d/collision/narrowphase/SphereVsCapsuleNarrowPhaseInfoBatch.h b/testbed/scenes/ballandsocketjointsnet/BallAndSocketJointsNetScene.h similarity index 51% rename from include/reactphysics3d/collision/narrowphase/SphereVsCapsuleNarrowPhaseInfoBatch.h rename to testbed/scenes/ballandsocketjointsnet/BallAndSocketJointsNetScene.h index 4841a3cf..4e9e4826 100644 --- a/include/reactphysics3d/collision/narrowphase/SphereVsCapsuleNarrowPhaseInfoBatch.h +++ b/testbed/scenes/ballandsocketjointsnet/BallAndSocketJointsNetScene.h @@ -1,6 +1,6 @@ /******************************************************************************** * ReactPhysics3D physics library, http://www.reactphysics3d.com * -* Copyright (c) 2010-2020 Daniel Chappuis * +* Copyright (c) 2010-2016 Daniel Chappuis * ********************************************************************************* * * * This software is provided 'as-is', without any express or implied warranty. * @@ -23,56 +23,57 @@ * * ********************************************************************************/ -#ifndef REACTPHYSICS3D_SPHERE_VS_CAPSULE_NARROW_PHASE_INFO_BATCH_H -#define REACTPHYSICS3D_SPHERE_VS_CAPSULE_NARROW_PHASE_INFO_BATCH_H +#ifndef BALL_AND_SOCKET_JOINTS_NET_SCENE_H +#define BALL_AND_SOCKET_JOINTS_NET_SCENE_H // Libraries -#include +#include "openglframework.h" +#include +#include "Sphere.h" +#include "SceneDemo.h" -/// Namespace ReactPhysics3D -namespace reactphysics3d { +namespace ballandsocketjointsnetscene { -// Struct SphereVsCapsuleNarrowPhaseInfoBatch -/** - * This structure collects all the potential collisions from the middle-phase algorithm - * that have to be tested during narrow-phase collision detection. This class collects all the - * sphere vs capsule collision detection tests. - */ -struct SphereVsCapsuleNarrowPhaseInfoBatch : public NarrowPhaseInfoBatch { +// Constants +const float SCENE_RADIUS = 40.0f; +const float SPHERE_RADIUS = 0.5f; +const int NB_ROWS_NET_SPHERES = 20; + +// Class JointsScene +class BallAndSocketJointsNetScene : public SceneDemo { + + protected : + + // -------------------- Attributes -------------------- // + + /// Spheres in the Ball-And-Socket joint net + Sphere* mNetSpheres[NB_ROWS_NET_SPHERES][NB_ROWS_NET_SPHERES]; + + /// Main sphere + Sphere* mMainSphere; + + /// Ball-And-Socket joints of the chain + std::vector mBallAndSocketJoints; + + // -------------------- Methods -------------------- // + + /// Create the joints + void createJoints(); public: - /// List of boolean values to know if the the sphere is the first or second shape - List isSpheresShape1; - - /// List of radiuses for the spheres - List sphereRadiuses; - - /// List of radiuses for the capsules - List capsuleRadiuses; - - /// List of heights for the capsules - List capsuleHeights; + // -------------------- Methods -------------------- // /// Constructor - SphereVsCapsuleNarrowPhaseInfoBatch(MemoryAllocator& allocator, OverlappingPairs& overlappingPairs); + BallAndSocketJointsNetScene(const std::string& name, EngineSettings& settings); /// Destructor - virtual ~SphereVsCapsuleNarrowPhaseInfoBatch() override = default; + virtual ~BallAndSocketJointsNetScene() override ; - /// Add shapes to be tested during narrow-phase collision detection into the batch - virtual void addNarrowPhaseInfo(uint64 pairId, uint64 pairIndex, Entity collider1, Entity collider2, CollisionShape* shape1, - CollisionShape* shape2, const Transform& shape1Transform, - const Transform& shape2Transform, bool needToReportContacts, MemoryAllocator& shapeAllocator) override; - - // Initialize the containers using cached capacity - virtual void reserveMemory() override; - - /// Clear all the objects in the batch - virtual void clear() override; + /// Reset the scene + virtual void reset() override; }; } #endif - diff --git a/testbed/scenes/collisionshapes/CollisionShapesScene.cpp b/testbed/scenes/collisionshapes/CollisionShapesScene.cpp index 0cc97c8e..5a5b92e4 100644 --- a/testbed/scenes/collisionshapes/CollisionShapesScene.cpp +++ b/testbed/scenes/collisionshapes/CollisionShapesScene.cpp @@ -107,9 +107,6 @@ CollisionShapesScene::CollisionShapesScene(const std::string& name, EngineSettin // Create a sphere and a corresponding rigid in the physics world Sphere* sphere = new Sphere(true, SPHERE_RADIUS, mPhysicsCommon, mPhysicsWorld, meshFolderPath); - // Add some rolling resistance - sphere->getCollider()->getMaterial().setRollingResistance(rp3d::decimal(0.08)); - // Set the box color sphere->setColor(mObjectColorDemo); sphere->setSleepingColor(mSleepingColorDemo); @@ -130,8 +127,6 @@ CollisionShapesScene::CollisionShapesScene(const std::string& name, EngineSettin Capsule* capsule = new Capsule(true, CAPSULE_RADIUS, CAPSULE_HEIGHT, mPhysicsCommon, mPhysicsWorld, meshFolderPath); - capsule->getCollider()->getMaterial().setRollingResistance(rp3d::decimal(0.08f)); - // Set the box color capsule->setColor(mObjectColorDemo); capsule->setSleepingColor(mSleepingColorDemo); diff --git a/testbed/scenes/concavemesh/ConcaveMeshScene.cpp b/testbed/scenes/concavemesh/ConcaveMeshScene.cpp index e08ef540..c0c1b3e0 100644 --- a/testbed/scenes/concavemesh/ConcaveMeshScene.cpp +++ b/testbed/scenes/concavemesh/ConcaveMeshScene.cpp @@ -108,9 +108,6 @@ ConcaveMeshScene::ConcaveMeshScene(const std::string& name, EngineSettings& sett // Create a sphere and a corresponding rigid in the physics world Sphere* sphere = new Sphere(true, SPHERE_RADIUS, mPhysicsCommon, mPhysicsWorld, meshFolderPath); - // Add some rolling resistance - sphere->getCollider()->getMaterial().setRollingResistance(rp3d::decimal(0.08)); - // Set the box color sphere->setColor(mObjectColorDemo); sphere->setSleepingColor(mSleepingColorDemo); @@ -131,8 +128,6 @@ ConcaveMeshScene::ConcaveMeshScene(const std::string& name, EngineSettings& sett Capsule* capsule = new Capsule(true, CAPSULE_RADIUS, CAPSULE_HEIGHT, mPhysicsCommon, mPhysicsWorld, meshFolderPath); - capsule->getCollider()->getMaterial().setRollingResistance(rp3d::decimal(0.08)); - // Set the box color capsule->setColor(mObjectColorDemo); capsule->setSleepingColor(mSleepingColorDemo); diff --git a/testbed/scenes/cubestack/CubeStackScene.cpp b/testbed/scenes/cubestack/CubeStackScene.cpp index b45f4977..40b2b66d 100644 --- a/testbed/scenes/cubestack/CubeStackScene.cpp +++ b/testbed/scenes/cubestack/CubeStackScene.cpp @@ -145,6 +145,8 @@ void CubeStackScene::reset() { 0); box->setTransform(rp3d::Transform(position, rp3d::Quaternion::identity())); + box->getRigidBody()->setLinearVelocity(rp3d::Vector3::zero()); + box->getRigidBody()->setAngularVelocity(rp3d::Vector3::zero()); index++; } diff --git a/testbed/scenes/heightfield/HeightFieldScene.cpp b/testbed/scenes/heightfield/HeightFieldScene.cpp index 53ac20c7..4620ca54 100644 --- a/testbed/scenes/heightfield/HeightFieldScene.cpp +++ b/testbed/scenes/heightfield/HeightFieldScene.cpp @@ -107,9 +107,6 @@ HeightFieldScene::HeightFieldScene(const std::string& name, EngineSettings& sett // Create a sphere and a corresponding rigid in the physics world Sphere* sphere = new Sphere(true, SPHERE_RADIUS, mPhysicsCommon, mPhysicsWorld, meshFolderPath); - // Add some rolling resistance - sphere->getCollider()->getMaterial().setRollingResistance(0.08f); - // Set the box color sphere->setColor(mObjectColorDemo); sphere->setSleepingColor(mSleepingColorDemo); @@ -129,8 +126,6 @@ HeightFieldScene::HeightFieldScene(const std::string& name, EngineSettings& sett // Create a cylinder and a corresponding rigid in the physics world Capsule* capsule = new Capsule(true, CAPSULE_RADIUS, CAPSULE_HEIGHT, mPhysicsCommon, mPhysicsWorld, meshFolderPath); - capsule->getCollider()->getMaterial().setRollingResistance(0.08f); - // Set the box color capsule->setColor(mObjectColorDemo); capsule->setSleepingColor(mSleepingColorDemo); diff --git a/testbed/scenes/hingejointschain/HingeJointsChainScene.cpp b/testbed/scenes/hingejointschain/HingeJointsChainScene.cpp new file mode 100644 index 00000000..e6ebc47f --- /dev/null +++ b/testbed/scenes/hingejointschain/HingeJointsChainScene.cpp @@ -0,0 +1,159 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://www.reactphysics3d.com * +* Copyright (c) 2010-2016 Daniel Chappuis * +********************************************************************************* +* * +* This software is provided 'as-is', without any express or implied warranty. * +* In no event will the authors be held liable for any damages arising from the * +* use of this software. * +* * +* Permission is granted to anyone to use this software for any purpose, * +* including commercial applications, and to alter it and redistribute it * +* freely, subject to the following restrictions: * +* * +* 1. The origin of this software must not be misrepresented; you must not claim * +* that you wrote the original software. If you use this software in a * +* product, an acknowledgment in the product documentation would be * +* appreciated but is not required. * +* * +* 2. Altered source versions must be plainly marked as such, and must not be * +* misrepresented as being the original software. * +* * +* 3. This notice may not be removed or altered from any source distribution. * +* * +********************************************************************************/ + +// Libraries +#include "HingeJointsChainScene.h" +#include + +// Namespaces +using namespace openglframework; +using namespace hingejointschainscene; + +// Constructor +HingeJointsChainScene::HingeJointsChainScene(const std::string& name, EngineSettings& settings) + : SceneDemo(name, settings, true, SCENE_RADIUS) { + + std::string meshFolderPath("meshes/"); + + // Compute the radius and the center of the scene + openglframework::Vector3 center(0, 0, 0); + + // Set the center of the scene + setScenePosition(center, SCENE_RADIUS); + + // Gravity vector in the physics world + rp3d::Vector3 gravity(0, rp3d::decimal(-9.81), 0); + + rp3d::PhysicsWorld::WorldSettings worldSettings; + worldSettings.worldName = name; + + // Logger + rp3d::DefaultLogger* defaultLogger = mPhysicsCommon.createDefaultLogger(); + uint logLevel = static_cast(rp3d::Logger::Level::Information) | static_cast(rp3d::Logger::Level::Warning) | + static_cast(rp3d::Logger::Level::Error); + defaultLogger->addFileDestination("rp3d_log_" + name + ".html", logLevel, rp3d::DefaultLogger::Format::HTML); + mPhysicsCommon.setLogger(defaultLogger); + + // Create the physics world for the physics simulation + rp3d::PhysicsWorld* physicsWorld = mPhysicsCommon.createPhysicsWorld(worldSettings); + physicsWorld->setEventListener(this); + mPhysicsWorld = physicsWorld; + + // Create all the boxes of the scene + for (int i=0; isetColor(mObjectColorDemo); + mBoxes[i]->setSleepingColor(mSleepingColorDemo); + + // Change the material properties of the rigid body + rp3d::Material& material = mBoxes[i]->getCollider()->getMaterial(); + material.setBounciness(rp3d::decimal(0.0)); + + if (i == 0) { + mBoxes[i]->getRigidBody()->setType(rp3d::BodyType::STATIC); + } + + // Add the box the list of boxes in the scene + mPhysicsObjects.push_back(mBoxes[i]); + } + + // Set the position of the boxes before the joints creation + reset(); + + // Create the Ball-and-Socket joints + createJoints(); + + // Get the physics engine parameters + mEngineSettings.isGravityEnabled = mPhysicsWorld->isGravityEnabled(); + rp3d::Vector3 gravityVector = mPhysicsWorld->getGravity(); + mEngineSettings.gravity = openglframework::Vector3(gravityVector.x, gravityVector.y, gravityVector.z); + mEngineSettings.isSleepingEnabled = mPhysicsWorld->isSleepingEnabled(); + mEngineSettings.sleepLinearVelocity = mPhysicsWorld->getSleepLinearVelocity(); + mEngineSettings.sleepAngularVelocity = mPhysicsWorld->getSleepAngularVelocity(); + mEngineSettings.nbPositionSolverIterations = mPhysicsWorld->getNbIterationsPositionSolver(); + mEngineSettings.nbVelocitySolverIterations = mPhysicsWorld->getNbIterationsVelocitySolver(); + mEngineSettings.timeBeforeSleep = mPhysicsWorld->getTimeBeforeSleep(); +} + +// Destructor +HingeJointsChainScene::~HingeJointsChainScene() { + + // Destroy the joints + for (uint i=0; i < mHingeJoints.size(); i++) { + + mPhysicsWorld->destroyJoint(mHingeJoints[i]); + } + + // Destroy all the rigid bodies of the scene + for (int i=0; idestroyRigidBody(mBoxes[i]->getRigidBody()); + } + + // Destroy the physics world + mPhysicsCommon.destroyPhysicsWorld(mPhysicsWorld); +} + +// Create the joints +void HingeJointsChainScene::createJoints() { + + for (int i=0; i < NB_BOXES-1; i++) { + + // Create the joint info object + rp3d::RigidBody* body1 = mBoxes[i]->getRigidBody(); + rp3d::RigidBody* body2 = mBoxes[i+1]->getRigidBody(); + rp3d::Vector3 body1Position = body1->getTransform().getPosition(); + rp3d::Vector3 body2Position = body2->getTransform().getPosition(); + const rp3d::Vector3 anchorPointWorldSpace = body1Position + rp3d::Vector3(BOX_SIZE.x / 2.0f, 0, 0); + rp3d::HingeJointInfo jointInfo(body1, body2, anchorPointWorldSpace, rp3d::Vector3(0, 0, 1)); + jointInfo.isCollisionEnabled = false; + rp3d::HingeJoint* joint = dynamic_cast(mPhysicsWorld->createJoint(jointInfo)); + mHingeJoints.push_back(joint); + } +} + +// Reset the scene +void HingeJointsChainScene::reset() { + + SceneDemo::reset(); + + const float space = 0.3f; + + const rp3d::Quaternion initOrientation = rp3d::Quaternion::identity(); + + for (int i=0; isetTransform(transform); + } +} diff --git a/testbed/scenes/hingejointschain/HingeJointsChainScene.h b/testbed/scenes/hingejointschain/HingeJointsChainScene.h new file mode 100644 index 00000000..ad37b612 --- /dev/null +++ b/testbed/scenes/hingejointschain/HingeJointsChainScene.h @@ -0,0 +1,76 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://www.reactphysics3d.com * +* Copyright (c) 2010-2016 Daniel Chappuis * +********************************************************************************* +* * +* This software is provided 'as-is', without any express or implied warranty. * +* In no event will the authors be held liable for any damages arising from the * +* use of this software. * +* * +* Permission is granted to anyone to use this software for any purpose, * +* including commercial applications, and to alter it and redistribute it * +* freely, subject to the following restrictions: * +* * +* 1. The origin of this software must not be misrepresented; you must not claim * +* that you wrote the original software. If you use this software in a * +* product, an acknowledgment in the product documentation would be * +* appreciated but is not required. * +* * +* 2. Altered source versions must be plainly marked as such, and must not be * +* misrepresented as being the original software. * +* * +* 3. This notice may not be removed or altered from any source distribution. * +* * +********************************************************************************/ + +#ifndef HINGE_JOINTS_CHAIN_SCENE_H +#define HINGE_JOINTS_CHAIN_SCENE_H + +// Libraries +#include "openglframework.h" +#include +#include "Box.h" +#include "SceneDemo.h" + +namespace hingejointschainscene { + +// Constants +const float SCENE_RADIUS = 60.0f; +const openglframework::Vector3 BOX_SIZE = openglframework::Vector3(2, 1, 1); +const int NB_BOXES = 20; + +// Class HingeJointsChain scene +class HingeJointsChainScene : public SceneDemo { + + protected : + + // -------------------- Attributes -------------------- // + + /// Boxes + Box* mBoxes[NB_BOXES]; + + /// Hinge joints of the chain + std::vector mHingeJoints; + + // -------------------- Methods -------------------- // + + /// Create the joints + void createJoints(); + + public: + + // -------------------- Methods -------------------- // + + /// Constructor + HingeJointsChainScene(const std::string& name, EngineSettings& settings); + + /// Destructor + virtual ~HingeJointsChainScene() override ; + + /// Reset the scene + virtual void reset() override; +}; + +} + +#endif diff --git a/testbed/scenes/pile/PileScene.cpp b/testbed/scenes/pile/PileScene.cpp index 6698bc6e..636fe087 100644 --- a/testbed/scenes/pile/PileScene.cpp +++ b/testbed/scenes/pile/PileScene.cpp @@ -105,9 +105,6 @@ PileScene::PileScene(const std::string& name, EngineSettings& settings) // Create a sphere and a corresponding rigid in the physics world Sphere* sphere = new Sphere(true, SPHERE_RADIUS, mPhysicsCommon, mPhysicsWorld, meshFolderPath); - // Add some rolling resistance - sphere->getCollider()->getMaterial().setRollingResistance(rp3d::decimal(0.08)); - // Set the box color sphere->setColor(mObjectColorDemo); sphere->setSleepingColor(mSleepingColorDemo); @@ -128,8 +125,6 @@ PileScene::PileScene(const std::string& name, EngineSettings& settings) Capsule* capsule = new Capsule(true, CAPSULE_RADIUS, CAPSULE_HEIGHT, mPhysicsCommon, mPhysicsWorld, meshFolderPath); - capsule->getCollider()->getMaterial().setRollingResistance(rp3d::decimal(0.08f)); - // Set the box color capsule->setColor(mObjectColorDemo); capsule->setSleepingColor(mSleepingColorDemo); diff --git a/testbed/src/TestbedApplication.cpp b/testbed/src/TestbedApplication.cpp index b5d1dcd4..9a467044 100755 --- a/testbed/src/TestbedApplication.cpp +++ b/testbed/src/TestbedApplication.cpp @@ -39,6 +39,9 @@ #include "cubestack/CubeStackScene.h" #include "pile/PileScene.h" #include "boxtower/BoxTowerScene.h" +#include "ballandsocketjointsnet/BallAndSocketJointsNetScene.h" +#include "ballandsocketjointschain/BallAndSocketJointsChainScene.h" +#include "hingejointschain/HingeJointsChainScene.h" using namespace openglframework; using namespace jointsscene; @@ -51,6 +54,9 @@ using namespace collisiondetectionscene; using namespace cubestackscene; using namespace pilescene; using namespace boxtowerscene; +using namespace ballandsocketjointsnetscene; +using namespace ballandsocketjointschainscene; +using namespace hingejointschainscene; // Initialization of static variables const float TestbedApplication::SCROLL_SENSITIVITY = 0.08f; @@ -97,12 +103,15 @@ void TestbedApplication::init() { glGetIntegerv(GL_MAJOR_VERSION, &glMajorVersion); glGetIntegerv(GL_MINOR_VERSION, &glMinorVersion); +#ifdef GL_DEBUG_OUTPUT + if (glMajorVersion > 4 || (glMajorVersion == 4 && glMinorVersion >= 3)) { // Enable OpenGL error reporting glEnable(GL_DEBUG_OUTPUT); glDebugMessageCallback(onOpenGLError, 0); } +#endif mIsInitialized = true; } @@ -145,11 +154,23 @@ void TestbedApplication::createScenes() { // Pile scene PileScene* pileScene = new PileScene("Pile", mEngineSettings); mScenes.push_back(pileScene); - assert(mScenes.size() > 0); // Box Tower scene BoxTowerScene* boxTowerScene = new BoxTowerScene("Box Tower", mEngineSettings); mScenes.push_back(boxTowerScene); + + // Ball and Socket joints Net scene + BallAndSocketJointsNetScene* ballAndSocketJointsNetScene = new BallAndSocketJointsNetScene("BallAndSocket Joints Net", mEngineSettings); + mScenes.push_back(ballAndSocketJointsNetScene); + + // Ball and Socket joints chain scene + BallAndSocketJointsChainScene* ballAndSocketJointsChainScene = new BallAndSocketJointsChainScene("BallAndSocket Joints Chain", mEngineSettings); + mScenes.push_back(ballAndSocketJointsChainScene); + + // Hinge joints chain scene + HingeJointsChainScene* hingeJointsChainScene = new HingeJointsChainScene("Hinge Joints Chain", mEngineSettings); + mScenes.push_back(hingeJointsChainScene); + assert(mScenes.size() > 0); const int firstSceneIndex = 0; @@ -314,6 +335,7 @@ void TestbedApplication::notifyEngineSetttingsChanged() { void GLAPIENTRY TestbedApplication::onOpenGLError(GLenum source, GLenum type, GLuint id, GLenum severity, GLsizei length, const GLchar* message, const void* userParam ) { +#ifdef GL_DEBUG_OUTPUT if (type == GL_DEBUG_TYPE_ERROR) { /* fprintf( stderr, "GL CALLBACK: %s type = 0x%x, severity = 0x%x, message = %s\n", @@ -321,6 +343,8 @@ void GLAPIENTRY TestbedApplication::onOpenGLError(GLenum source, GLenum type, GL type, severity, message ); */ } +#endif + } // Compute the FPS