diff --git a/ephysics/body/Body.cpp b/ephysics/body/Body.cpp index 6ab38dc..6e9515f 100644 --- a/ephysics/body/Body.cpp +++ b/ephysics/body/Body.cpp @@ -1,27 +1,8 @@ -/******************************************************************************** -* 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. * -* * -********************************************************************************/ +/** @file + * @author Daniel Chappuis + * @copyright 2010-2016 Daniel Chappuis + * @license BSD 3 clauses (see license file) + */ // Libraries #include @@ -35,8 +16,8 @@ using namespace reactphysics3d; * @param id ID of the new body */ Body::Body(bodyindex id) - : mID(id), mIsAlreadyInIsland(false), mIsAllowedToSleep(true), mIsActive(true), - mIsSleeping(false), mSleepTime(0), mUserData(NULL) { + : mID(id), mIsAlreadyInIsland(false), mIsAllowedToSleep(true), mIsActive(true), + mIsSleeping(false), mSleepTime(0), mUserData(NULL) { } diff --git a/ephysics/body/Body.h b/ephysics/body/Body.h index 02a3428..fd5a700 100644 --- a/ephysics/body/Body.h +++ b/ephysics/body/Body.h @@ -1,30 +1,9 @@ -/******************************************************************************** -* 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 REACTPHYSICS3D_BODY_H -#define REACTPHYSICS3D_BODY_H +/** @file + * @author Daniel Chappuis + * @copyright 2010-2016 Daniel Chappuis + * @license BSD 3 clauses (see license file) + */ +#pragma once // Libraries #include @@ -43,98 +22,98 @@ namespace reactphysics3d { */ class Body { - protected : + protected : - // -------------------- Attributes -------------------- // + // -------------------- Attributes -------------------- // - /// ID of the body - bodyindex mID; + /// ID of the body + bodyindex mID; - /// True if the body has already been added in an island (for sleeping technique) - bool mIsAlreadyInIsland; + /// True if the body has already been added in an island (for sleeping technique) + bool mIsAlreadyInIsland; - /// True if the body is allowed to go to sleep for better efficiency - bool mIsAllowedToSleep; + /// True if the body is allowed to go to sleep for better efficiency + bool mIsAllowedToSleep; - /// True if the body is active. - /// An inactive body does not participate in collision detection, - /// is not simulated and will not be hit in a ray casting query. - /// A body is active by default. If you set this - /// value to "false", all the proxy shapes of this body will be - /// removed from the broad-phase. If you set this value to "true", - /// all the proxy shapes will be added to the broad-phase. A joint - /// connected to an inactive body will also be inactive. - bool mIsActive; + /// True if the body is active. + /// An inactive body does not participate in collision detection, + /// is not simulated and will not be hit in a ray casting query. + /// A body is active by default. If you set this + /// value to "false", all the proxy shapes of this body will be + /// removed from the broad-phase. If you set this value to "true", + /// all the proxy shapes will be added to the broad-phase. A joint + /// connected to an inactive body will also be inactive. + bool mIsActive; - /// True if the body is sleeping (for sleeping technique) - bool mIsSleeping; + /// True if the body is sleeping (for sleeping technique) + bool mIsSleeping; - /// Elapsed time since the body velocity was bellow the sleep velocity - decimal mSleepTime; + /// Elapsed time since the body velocity was bellow the sleep velocity + float mSleepTime; - /// Pointer that can be used to attach user data to the body - void* mUserData; + /// Pointer that can be used to attach user data to the body + void* mUserData; - // -------------------- Methods -------------------- // + // -------------------- Methods -------------------- // - /// Private copy-constructor - Body(const Body& body); + /// Private copy-constructor + Body(const Body& body); - /// Private assignment operator - Body& operator=(const Body& body); + /// Private assignment operator + Body& operator=(const Body& body); - public : + public : - // -------------------- Methods -------------------- // + // -------------------- Methods -------------------- // - /// Constructor - Body(bodyindex id); + /// Constructor + Body(bodyindex id); - /// Destructor - virtual ~Body(); + /// Destructor + virtual ~Body(); - /// Return the ID of the body - bodyindex getID() const; + /// Return the ID of the body + bodyindex getID() const; - /// Return whether or not the body is allowed to sleep - bool isAllowedToSleep() const; + /// Return whether or not the body is allowed to sleep + bool isAllowedToSleep() const; - /// Set whether or not the body is allowed to go to sleep - void setIsAllowedToSleep(bool isAllowedToSleep); + /// Set whether or not the body is allowed to go to sleep + void setIsAllowedToSleep(bool isAllowedToSleep); - /// Set the variable to know whether or not the body is sleeping - virtual void setIsSleeping(bool isSleeping); + /// Set the variable to know whether or not the body is sleeping + virtual void setIsSleeping(bool isSleeping); - /// Return whether or not the body is sleeping - bool isSleeping() const; + /// Return whether or not the body is sleeping + bool isSleeping() const; - /// Return true if the body is active - bool isActive() const; + /// Return true if the body is active + bool isActive() const; - /// Set whether or not the body is active - virtual void setIsActive(bool isActive); + /// Set whether or not the body is active + virtual void setIsActive(bool isActive); - /// Return a pointer to the user data attached to this body - void* getUserData() const; + /// Return a pointer to the user data attached to this body + void* getUserData() const; - /// Attach user data to this body - void setUserData(void* userData); + /// Attach user data to this body + void setUserData(void* userData); - /// Smaller than operator - bool operator<(const Body& body2) const; + /// Smaller than operator + bool operator<(const Body& body2) const; - /// Larger than operator - bool operator>(const Body& body2) const; + /// Larger than operator + bool operator>(const Body& body2) const; - /// Equal operator - bool operator==(const Body& body2) const; + /// Equal operator + bool operator==(const Body& body2) const; - /// Not equal operator - bool operator!=(const Body& body2) const; + /// Not equal operator + bool operator!=(const Body& body2) const; - // -------------------- Friendship -------------------- // + // -------------------- Friendship -------------------- // - friend class DynamicsWorld; + friend class DynamicsWorld; }; // Return the id of the body @@ -142,7 +121,7 @@ class Body { * @return The ID of the body */ inline bodyindex Body::getID() const { - return mID; + return mID; } // Return whether or not the body is allowed to sleep @@ -150,7 +129,7 @@ inline bodyindex Body::getID() const { * @return True if the body is allowed to sleep and false otherwise */ inline bool Body::isAllowedToSleep() const { - return mIsAllowedToSleep; + return mIsAllowedToSleep; } // Set whether or not the body is allowed to go to sleep @@ -158,9 +137,9 @@ inline bool Body::isAllowedToSleep() const { * @param isAllowedToSleep True if the body is allowed to sleep */ inline void Body::setIsAllowedToSleep(bool isAllowedToSleep) { - mIsAllowedToSleep = isAllowedToSleep; + mIsAllowedToSleep = isAllowedToSleep; - if (!mIsAllowedToSleep) setIsSleeping(false); + if (!mIsAllowedToSleep) setIsSleeping(false); } // Return whether or not the body is sleeping @@ -168,7 +147,7 @@ inline void Body::setIsAllowedToSleep(bool isAllowedToSleep) { * @return True if the body is currently sleeping and false otherwise */ inline bool Body::isSleeping() const { - return mIsSleeping; + return mIsSleeping; } // Return true if the body is active @@ -176,7 +155,7 @@ inline bool Body::isSleeping() const { * @return True if the body currently active and false otherwise */ inline bool Body::isActive() const { - return mIsActive; + return mIsActive; } // Set whether or not the body is active @@ -184,22 +163,22 @@ inline bool Body::isActive() const { * @param isActive True if you want to activate the body */ inline void Body::setIsActive(bool isActive) { - mIsActive = isActive; + mIsActive = isActive; } // Set the variable to know whether or not the body is sleeping inline void Body::setIsSleeping(bool isSleeping) { - if (isSleeping) { - mSleepTime = decimal(0.0); - } - else { - if (mIsSleeping) { - mSleepTime = decimal(0.0); - } - } + if (isSleeping) { + mSleepTime = float(0.0); + } + else { + if (mIsSleeping) { + mSleepTime = float(0.0); + } + } - mIsSleeping = isSleeping; + mIsSleeping = isSleeping; } // Return a pointer to the user data attached to this body @@ -207,7 +186,7 @@ inline void Body::setIsSleeping(bool isSleeping) { * @return A pointer to the user data you have attached to the body */ inline void* Body::getUserData() const { - return mUserData; + return mUserData; } // Attach user data to this body @@ -215,29 +194,27 @@ inline void* Body::getUserData() const { * @param userData A pointer to the user data you want to attach to the body */ inline void Body::setUserData(void* userData) { - mUserData = userData; + mUserData = userData; } // Smaller than operator inline bool Body::operator<(const Body& body2) const { - return (mID < body2.mID); + return (mID < body2.mID); } // Larger than operator inline bool Body::operator>(const Body& body2) const { - return (mID > body2.mID); + return (mID > body2.mID); } // Equal operator inline bool Body::operator==(const Body& body2) const { - return (mID == body2.mID); + return (mID == body2.mID); } - + // Not equal operator inline bool Body::operator!=(const Body& body2) const { - return (mID != body2.mID); -} + return (mID != body2.mID); +} } - - #endif diff --git a/ephysics/body/CollisionBody.cpp b/ephysics/body/CollisionBody.cpp index 35fa8b8..a8ba77b 100644 --- a/ephysics/body/CollisionBody.cpp +++ b/ephysics/body/CollisionBody.cpp @@ -1,27 +1,8 @@ -/******************************************************************************** -* 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. * -* * -********************************************************************************/ +/** @file + * @author Daniel Chappuis + * @copyright 2010-2016 Daniel Chappuis + * @license BSD 3 clauses (see license file) + */ // Libraries #include @@ -38,17 +19,17 @@ using namespace reactphysics3d; * @param id ID of the body */ CollisionBody::CollisionBody(const Transform& transform, CollisionWorld& world, bodyindex id) - : Body(id), mType(DYNAMIC), mTransform(transform), mProxyCollisionShapes(NULL), - mNbCollisionShapes(0), mContactManifoldsList(NULL), mWorld(world) { + : Body(id), mType(DYNAMIC), mTransform(transform), mProxyCollisionShapes(NULL), + mNbCollisionShapes(0), mContactManifoldsList(NULL), mWorld(world) { } // Destructor CollisionBody::~CollisionBody() { - assert(mContactManifoldsList == NULL); + assert(mContactManifoldsList == NULL); - // Remove all the proxy collision shapes of the body - removeAllCollisionShapes(); + // Remove all the proxy collision shapes of the body + removeAllCollisionShapes(); } // Add a collision shape to the body. Note that you can share a collision @@ -62,38 +43,38 @@ CollisionBody::~CollisionBody() { /** * @param collisionShape A pointer to the collision shape you want to add to the body * @param transform The transformation of the collision shape that transforms the - * local-space of the collision shape into the local-space of the body + * local-space of the collision shape int32_to the local-space of the body * @return A pointer to the proxy shape that has been created to link the body to - * the new collision shape you have added. + * the new collision shape you have added. */ ProxyShape* CollisionBody::addCollisionShape(CollisionShape* collisionShape, - const Transform& transform) { + const Transform& transform) { - // Create a new proxy collision shape to attach the collision shape to the body - ProxyShape* proxyShape = new (mWorld.mMemoryAllocator.allocate( - sizeof(ProxyShape))) ProxyShape(this, collisionShape, - transform, decimal(1)); + // Create a new proxy collision shape to attach the collision shape to the body + ProxyShape* proxyShape = new (mWorld.mMemoryAllocator.allocate( + sizeof(ProxyShape))) ProxyShape(this, collisionShape, + transform, float(1)); - // Add it to the list of proxy collision shapes of the body - if (mProxyCollisionShapes == NULL) { - mProxyCollisionShapes = proxyShape; - } - else { - proxyShape->mNext = mProxyCollisionShapes; - mProxyCollisionShapes = proxyShape; - } + // Add it to the list of proxy collision shapes of the body + if (mProxyCollisionShapes == NULL) { + mProxyCollisionShapes = proxyShape; + } + else { + proxyShape->mNext = mProxyCollisionShapes; + mProxyCollisionShapes = proxyShape; + } - // Compute the world-space AABB of the new collision shape - AABB aabb; - collisionShape->computeAABB(aabb, mTransform * transform); + // Compute the world-space AABB of the new collision shape + AABB aabb; + collisionShape->computeAABB(aabb, mTransform * transform); - // Notify the collision detection about this new collision shape - mWorld.mCollisionDetection.addProxyCollisionShape(proxyShape, aabb); + // Notify the collision detection about this new collision shape + mWorld.mCollisionDetection.addProxyCollisionShape(proxyShape, aabb); - mNbCollisionShapes++; + mNbCollisionShapes++; - // Return a pointer to the collision shape - return proxyShape; + // Return a pointer to the collision shape + return proxyShape; } // Remove a collision shape from the body @@ -105,109 +86,109 @@ ProxyShape* CollisionBody::addCollisionShape(CollisionShape* collisionShape, */ void CollisionBody::removeCollisionShape(const ProxyShape* proxyShape) { - ProxyShape* current = mProxyCollisionShapes; + ProxyShape* current = mProxyCollisionShapes; - // If the the first proxy shape is the one to remove - if (current == proxyShape) { - mProxyCollisionShapes = current->mNext; + // If the the first proxy shape is the one to remove + if (current == proxyShape) { + mProxyCollisionShapes = current->mNext; - if (mIsActive) { - mWorld.mCollisionDetection.removeProxyCollisionShape(current); - } + if (mIsActive) { + mWorld.mCollisionDetection.removeProxyCollisionShape(current); + } - current->~ProxyShape(); - mWorld.mMemoryAllocator.release(current, sizeof(ProxyShape)); - mNbCollisionShapes--; - return; - } + current->~ProxyShape(); + mWorld.mMemoryAllocator.release(current, sizeof(ProxyShape)); + mNbCollisionShapes--; + return; + } - // Look for the proxy shape that contains the collision shape in parameter - while(current->mNext != NULL) { + // Look for the proxy shape that contains the collision shape in parameter + while(current->mNext != NULL) { - // If we have found the collision shape to remove - if (current->mNext == proxyShape) { + // If we have found the collision shape to remove + if (current->mNext == proxyShape) { - // Remove the proxy collision shape - ProxyShape* elementToRemove = current->mNext; - current->mNext = elementToRemove->mNext; + // Remove the proxy collision shape + ProxyShape* elementToRemove = current->mNext; + current->mNext = elementToRemove->mNext; - if (mIsActive) { - mWorld.mCollisionDetection.removeProxyCollisionShape(elementToRemove); - } + if (mIsActive) { + mWorld.mCollisionDetection.removeProxyCollisionShape(elementToRemove); + } - elementToRemove->~ProxyShape(); - mWorld.mMemoryAllocator.release(elementToRemove, sizeof(ProxyShape)); - mNbCollisionShapes--; - return; - } + elementToRemove->~ProxyShape(); + mWorld.mMemoryAllocator.release(elementToRemove, sizeof(ProxyShape)); + mNbCollisionShapes--; + return; + } - // Get the next element in the list - current = current->mNext; - } + // Get the next element in the list + current = current->mNext; + } } // Remove all the collision shapes void CollisionBody::removeAllCollisionShapes() { - ProxyShape* current = mProxyCollisionShapes; + ProxyShape* current = mProxyCollisionShapes; - // Look for the proxy shape that contains the collision shape in parameter - while(current != NULL) { + // Look for the proxy shape that contains the collision shape in parameter + while(current != NULL) { - // Remove the proxy collision shape - ProxyShape* nextElement = current->mNext; + // Remove the proxy collision shape + ProxyShape* nextElement = current->mNext; - if (mIsActive) { - mWorld.mCollisionDetection.removeProxyCollisionShape(current); - } + if (mIsActive) { + mWorld.mCollisionDetection.removeProxyCollisionShape(current); + } - current->~ProxyShape(); - mWorld.mMemoryAllocator.release(current, sizeof(ProxyShape)); + current->~ProxyShape(); + mWorld.mMemoryAllocator.release(current, sizeof(ProxyShape)); - // Get the next element in the list - current = nextElement; - } + // Get the next element in the list + current = nextElement; + } - mProxyCollisionShapes = NULL; + mProxyCollisionShapes = NULL; } // Reset the contact manifold lists void CollisionBody::resetContactManifoldsList() { - // Delete the linked list of contact manifolds of that body - ContactManifoldListElement* currentElement = mContactManifoldsList; - while (currentElement != NULL) { - ContactManifoldListElement* nextElement = currentElement->next; + // Delete the linked list of contact manifolds of that body + ContactManifoldListElement* currentElement = mContactManifoldsList; + while (currentElement != NULL) { + ContactManifoldListElement* nextElement = currentElement->next; - // Delete the current element - currentElement->~ContactManifoldListElement(); - mWorld.mMemoryAllocator.release(currentElement, sizeof(ContactManifoldListElement)); + // Delete the current element + currentElement->~ContactManifoldListElement(); + mWorld.mMemoryAllocator.release(currentElement, sizeof(ContactManifoldListElement)); - currentElement = nextElement; - } - mContactManifoldsList = NULL; + currentElement = nextElement; + } + mContactManifoldsList = NULL; } // Update the broad-phase state for this body (because it has moved for instance) void CollisionBody::updateBroadPhaseState() const { - // For all the proxy collision shapes of the body - for (ProxyShape* shape = mProxyCollisionShapes; shape != NULL; shape = shape->mNext) { + // For all the proxy collision shapes of the body + for (ProxyShape* shape = mProxyCollisionShapes; shape != NULL; shape = shape->mNext) { - // Update the proxy - updateProxyShapeInBroadPhase(shape); - } + // Update the proxy + updateProxyShapeInBroadPhase(shape); + } } // Update the broad-phase state of a proxy collision shape of the body void CollisionBody::updateProxyShapeInBroadPhase(ProxyShape* proxyShape, bool forceReinsert) const { - // Recompute the world-space AABB of the collision shape - AABB aabb; - proxyShape->getCollisionShape()->computeAABB(aabb, mTransform * proxyShape->getLocalToBodyTransform()); + // Recompute the world-space AABB of the collision shape + AABB aabb; + proxyShape->getCollisionShape()->computeAABB(aabb, mTransform * proxyShape->getLocalToBodyTransform()); - // Update the broad-phase state for the proxy collision shape - mWorld.mCollisionDetection.updateProxyCollisionShape(proxyShape, aabb, Vector3(0, 0, 0), forceReinsert); + // Update the broad-phase state for the proxy collision shape + mWorld.mCollisionDetection.updateProxyCollisionShape(proxyShape, aabb, Vector3(0, 0, 0), forceReinsert); } // Set whether or not the body is active @@ -216,68 +197,68 @@ void CollisionBody::updateProxyShapeInBroadPhase(ProxyShape* proxyShape, bool fo */ void CollisionBody::setIsActive(bool isActive) { - // If the state does not change - if (mIsActive == isActive) return; + // If the state does not change + if (mIsActive == isActive) return; - Body::setIsActive(isActive); + Body::setIsActive(isActive); - // If we have to activate the body - if (isActive) { + // If we have to activate the body + if (isActive) { - // For each proxy shape of the body - for (ProxyShape* shape = mProxyCollisionShapes; shape != NULL; shape = shape->mNext) { + // For each proxy shape of the body + for (ProxyShape* shape = mProxyCollisionShapes; shape != NULL; shape = shape->mNext) { - // Compute the world-space AABB of the new collision shape - AABB aabb; - shape->getCollisionShape()->computeAABB(aabb, mTransform * shape->mLocalToBodyTransform); + // Compute the world-space AABB of the new collision shape + AABB aabb; + shape->getCollisionShape()->computeAABB(aabb, mTransform * shape->mLocalToBodyTransform); - // Add the proxy shape to the collision detection - mWorld.mCollisionDetection.addProxyCollisionShape(shape, aabb); - } - } - else { // If we have to deactivate the body + // Add the proxy shape to the collision detection + mWorld.mCollisionDetection.addProxyCollisionShape(shape, aabb); + } + } + else { // If we have to deactivate the body - // For each proxy shape of the body - for (ProxyShape* shape = mProxyCollisionShapes; shape != NULL; shape = shape->mNext) { + // For each proxy shape of the body + for (ProxyShape* shape = mProxyCollisionShapes; shape != NULL; shape = shape->mNext) { - // Remove the proxy shape from the collision detection - mWorld.mCollisionDetection.removeProxyCollisionShape(shape); - } + // Remove the proxy shape from the collision detection + mWorld.mCollisionDetection.removeProxyCollisionShape(shape); + } - // Reset the contact manifold list of the body - resetContactManifoldsList(); - } + // Reset the contact manifold list of the body + resetContactManifoldsList(); + } } // Ask the broad-phase to test again the collision shapes of the body for collision // (as if the body has moved). void CollisionBody::askForBroadPhaseCollisionCheck() const { - // For all the proxy collision shapes of the body - for (ProxyShape* shape = mProxyCollisionShapes; shape != NULL; shape = shape->mNext) { + // For all the proxy collision shapes of the body + for (ProxyShape* shape = mProxyCollisionShapes; shape != NULL; shape = shape->mNext) { - mWorld.mCollisionDetection.askForBroadPhaseCollisionCheck(shape); - } + mWorld.mCollisionDetection.askForBroadPhaseCollisionCheck(shape); + } } // Reset the mIsAlreadyInIsland variable of the body and contact manifolds. /// This method also returns the number of contact manifolds of the body. -int CollisionBody::resetIsAlreadyInIslandAndCountManifolds() { +int32_t CollisionBody::resetIsAlreadyInIslandAndCountManifolds() { - mIsAlreadyInIsland = false; + mIsAlreadyInIsland = false; - int nbManifolds = 0; + int32_t nbManifolds = 0; - // Reset the mIsAlreadyInIsland variable of the contact manifolds for - // this body - ContactManifoldListElement* currentElement = mContactManifoldsList; - while (currentElement != NULL) { - currentElement->contactManifold->mIsAlreadyInIsland = false; - currentElement = currentElement->next; - nbManifolds++; - } + // Reset the mIsAlreadyInIsland variable of the contact manifolds for + // this body + ContactManifoldListElement* currentElement = mContactManifoldsList; + while (currentElement != NULL) { + currentElement->contactManifold->mIsAlreadyInIsland = false; + currentElement = currentElement->next; + nbManifolds++; + } - return nbManifolds; + return nbManifolds; } // Return true if a point is inside the collision body @@ -288,14 +269,14 @@ int CollisionBody::resetIsAlreadyInIslandAndCountManifolds() { */ bool CollisionBody::testPointInside(const Vector3& worldPoint) const { - // For each collision shape of the body - for (ProxyShape* shape = mProxyCollisionShapes; shape != NULL; shape = shape->mNext) { + // For each collision shape of the body + for (ProxyShape* shape = mProxyCollisionShapes; shape != NULL; shape = shape->mNext) { - // Test if the point is inside the collision shape - if (shape->testPointInside(worldPoint)) return true; - } + // Test if the point is inside the collision shape + if (shape->testPointInside(worldPoint)) return true; + } - return false; + return false; } // Raycast method with feedback information @@ -303,28 +284,28 @@ bool CollisionBody::testPointInside(const Vector3& worldPoint) const { /** * @param ray The ray used to raycast agains the body * @param[out] raycastInfo Structure that contains the result of the raycasting -* (valid only if the method returned true) +* (valid only if the method returned true) * @return True if the ray hit the body and false otherwise */ bool CollisionBody::raycast(const Ray& ray, RaycastInfo& raycastInfo) { - // If the body is not active, it cannot be hit by rays - if (!mIsActive) return false; + // If the body is not active, it cannot be hit by rays + if (!mIsActive) return false; - bool isHit = false; - Ray rayTemp(ray); + bool isHit = false; + Ray rayTemp(ray); - // For each collision shape of the body - for (ProxyShape* shape = mProxyCollisionShapes; shape != NULL; shape = shape->mNext) { + // For each collision shape of the body + for (ProxyShape* shape = mProxyCollisionShapes; shape != NULL; shape = shape->mNext) { - // Test if the ray hits the collision shape - if (shape->raycast(rayTemp, raycastInfo)) { - rayTemp.maxFraction = raycastInfo.hitFraction; - isHit = true; - } - } + // Test if the ray hits the collision shape + if (shape->raycast(rayTemp, raycastInfo)) { + rayTemp.maxFraction = raycastInfo.hitFraction; + isHit = true; + } + } - return isHit; + return isHit; } // Compute and return the AABB of the body by merging all proxy shapes AABBs @@ -333,22 +314,22 @@ bool CollisionBody::raycast(const Ray& ray, RaycastInfo& raycastInfo) { */ AABB CollisionBody::getAABB() const { - AABB bodyAABB; + AABB bodyAABB; - if (mProxyCollisionShapes == NULL) return bodyAABB; + if (mProxyCollisionShapes == NULL) return bodyAABB; - mProxyCollisionShapes->getCollisionShape()->computeAABB(bodyAABB, mTransform * mProxyCollisionShapes->getLocalToBodyTransform()); + mProxyCollisionShapes->getCollisionShape()->computeAABB(bodyAABB, mTransform * mProxyCollisionShapes->getLocalToBodyTransform()); - // For each proxy shape of the body - for (ProxyShape* shape = mProxyCollisionShapes->mNext; shape != NULL; shape = shape->mNext) { + // For each proxy shape of the body + for (ProxyShape* shape = mProxyCollisionShapes->mNext; shape != NULL; shape = shape->mNext) { - // Compute the world-space AABB of the collision shape - AABB aabb; - shape->getCollisionShape()->computeAABB(aabb, mTransform * shape->getLocalToBodyTransform()); + // Compute the world-space AABB of the collision shape + AABB aabb; + shape->getCollisionShape()->computeAABB(aabb, mTransform * shape->getLocalToBodyTransform()); - // Merge the proxy shape AABB with the current body AABB - bodyAABB.mergeWithAABB(aabb); - } + // Merge the proxy shape AABB with the current body AABB + bodyAABB.mergeWithAABB(aabb); + } - return bodyAABB; + return bodyAABB; } diff --git a/ephysics/body/CollisionBody.h b/ephysics/body/CollisionBody.h index 86dc06d..6fba5f7 100644 --- a/ephysics/body/CollisionBody.h +++ b/ephysics/body/CollisionBody.h @@ -1,30 +1,9 @@ -/******************************************************************************** -* 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 REACTPHYSICS3D_COLLISION_BODY_H -#define REACTPHYSICS3D_COLLISION_BODY_H +/** @file + * @author Daniel Chappuis + * @copyright 2010-2016 Daniel Chappuis + * @license BSD 3 clauses (see license file) + */ +#pragma once // Libraries #include @@ -47,13 +26,13 @@ class CollisionWorld; /// Enumeration for the type of a body /// STATIC : A static body has infinite mass, zero velocity but the position can be -/// changed manually. A static body does not collide with other static or kinematic bodies. +/// changed manually. A static body does not collide with other static or kinematic bodies. /// KINEMATIC : A kinematic body has infinite mass, the velocity can be changed manually and its -/// position is computed by the physics engine. A kinematic body does not collide with -/// other static or kinematic bodies. +/// position is computed by the physics engine. A kinematic body does not collide with +/// other static or kinematic bodies. /// DYNAMIC : A dynamic body has non-zero mass, non-zero velocity determined by forces and its -/// position is determined by the physics engine. A dynamic body can collide with other -/// dynamic, static or kinematic bodies. +/// position is determined by the physics engine. A dynamic body can collide with other +/// dynamic, static or kinematic bodies. enum BodyType {STATIC, KINEMATIC, DYNAMIC}; // Class CollisionBody @@ -63,125 +42,125 @@ enum BodyType {STATIC, KINEMATIC, DYNAMIC}; */ class CollisionBody : public Body { - protected : + protected : - // -------------------- Attributes -------------------- // + // -------------------- Attributes -------------------- // - /// Type of body (static, kinematic or dynamic) - BodyType mType; + /// Type of body (static, kinematic or dynamic) + BodyType mType; - /// Position and orientation of the body - Transform mTransform; + /// Position and orientation of the body + Transform mTransform; - /// First element of the linked list of proxy collision shapes of this body - ProxyShape* mProxyCollisionShapes; + /// First element of the linked list of proxy collision shapes of this body + ProxyShape* mProxyCollisionShapes; - /// Number of collision shapes - uint mNbCollisionShapes; + /// Number of collision shapes + uint32_t mNbCollisionShapes; - /// First element of the linked list of contact manifolds involving this body - ContactManifoldListElement* mContactManifoldsList; + /// First element of the linked list of contact manifolds involving this body + ContactManifoldListElement* mContactManifoldsList; - /// Reference to the world the body belongs to - CollisionWorld& mWorld; + /// Reference to the world the body belongs to + CollisionWorld& mWorld; - // -------------------- Methods -------------------- // + // -------------------- Methods -------------------- // - /// Private copy-constructor - CollisionBody(const CollisionBody& body); + /// Private copy-constructor + CollisionBody(const CollisionBody& body); - /// Private assignment operator - CollisionBody& operator=(const CollisionBody& body); + /// Private assignment operator + CollisionBody& operator=(const CollisionBody& body); - /// Reset the contact manifold lists - void resetContactManifoldsList(); + /// Reset the contact manifold lists + void resetContactManifoldsList(); - /// Remove all the collision shapes - void removeAllCollisionShapes(); + /// Remove all the collision shapes + void removeAllCollisionShapes(); - /// Update the broad-phase state for this body (because it has moved for instance) - virtual void updateBroadPhaseState() const; + /// Update the broad-phase state for this body (because it has moved for instance) + virtual void updateBroadPhaseState() const; - /// Update the broad-phase state of a proxy collision shape of the body - void updateProxyShapeInBroadPhase(ProxyShape* proxyShape, bool forceReinsert = false) const; + /// Update the broad-phase state of a proxy collision shape of the body + void updateProxyShapeInBroadPhase(ProxyShape* proxyShape, bool forceReinsert = false) const; - /// Ask the broad-phase to test again the collision shapes of the body for collision - /// (as if the body has moved). - void askForBroadPhaseCollisionCheck() const; + /// Ask the broad-phase to test again the collision shapes of the body for collision + /// (as if the body has moved). + void askForBroadPhaseCollisionCheck() const; - /// Reset the mIsAlreadyInIsland variable of the body and contact manifolds - int resetIsAlreadyInIslandAndCountManifolds(); + /// Reset the mIsAlreadyInIsland variable of the body and contact manifolds + int32_t resetIsAlreadyInIslandAndCountManifolds(); - public : + public : - // -------------------- Methods -------------------- // + // -------------------- Methods -------------------- // - /// Constructor - CollisionBody(const Transform& transform, CollisionWorld& world, bodyindex id); + /// Constructor + CollisionBody(const Transform& transform, CollisionWorld& world, bodyindex id); - /// Destructor - virtual ~CollisionBody(); + /// Destructor + virtual ~CollisionBody(); - /// Return the type of the body - BodyType getType() const; + /// Return the type of the body + BodyType getType() const; - /// Set the type of the body - void setType(BodyType type); + /// Set the type of the body + void setType(BodyType type); - /// Set whether or not the body is active - virtual void setIsActive(bool isActive); + /// Set whether or not the body is active + virtual void setIsActive(bool isActive); - /// Return the current position and orientation - const Transform& getTransform() const; + /// Return the current position and orientation + const Transform& getTransform() const; - /// Set the current position and orientation - virtual void setTransform(const Transform& transform); + /// Set the current position and orientation + virtual void setTransform(const Transform& transform); - /// Add a collision shape to the body. - virtual ProxyShape* addCollisionShape(CollisionShape* collisionShape, - const Transform& transform); + /// Add a collision shape to the body. + virtual ProxyShape* addCollisionShape(CollisionShape* collisionShape, + const Transform& transform); - /// Remove a collision shape from the body - virtual void removeCollisionShape(const ProxyShape* proxyShape); + /// Remove a collision shape from the body + virtual void removeCollisionShape(const ProxyShape* proxyShape); - /// Return the first element of the linked list of contact manifolds involving this body - const ContactManifoldListElement* getContactManifoldsList() const; + /// Return the first element of the linked list of contact manifolds involving this body + const ContactManifoldListElement* getContactManifoldsList() const; - /// Return true if a point is inside the collision body - bool testPointInside(const Vector3& worldPoint) const; + /// Return true if a point is inside the collision body + bool testPointInside(const Vector3& worldPoint) const; - /// Raycast method with feedback information - bool raycast(const Ray& ray, RaycastInfo& raycastInfo); + /// Raycast method with feedback information + bool raycast(const Ray& ray, RaycastInfo& raycastInfo); - /// Compute and return the AABB of the body by merging all proxy shapes AABBs - AABB getAABB() const; + /// Compute and return the AABB of the body by merging all proxy shapes AABBs + AABB getAABB() const; - /// Return the linked list of proxy shapes of that body - ProxyShape* getProxyShapesList(); + /// Return the linked list of proxy shapes of that body + ProxyShape* getProxyShapesList(); - /// Return the linked list of proxy shapes of that body - const ProxyShape* getProxyShapesList() const; + /// Return the linked list of proxy shapes of that body + const ProxyShape* getProxyShapesList() const; - /// Return the world-space coordinates of a point given the local-space coordinates of the body - Vector3 getWorldPoint(const Vector3& localPoint) const; + /// Return the world-space coordinates of a point given the local-space coordinates of the body + Vector3 getWorldPoint(const Vector3& localPoint) const; - /// Return the world-space vector of a vector given in local-space coordinates of the body - Vector3 getWorldVector(const Vector3& localVector) const; + /// Return the world-space vector of a vector given in local-space coordinates of the body + Vector3 getWorldVector(const Vector3& localVector) const; - /// Return the body local-space coordinates of a point given in the world-space coordinates - Vector3 getLocalPoint(const Vector3& worldPoint) const; + /// Return the body local-space coordinates of a point given in the world-space coordinates + Vector3 getLocalPoint(const Vector3& worldPoint) const; - /// Return the body local-space coordinates of a vector given in the world-space coordinates - Vector3 getLocalVector(const Vector3& worldVector) const; + /// Return the body local-space coordinates of a vector given in the world-space coordinates + Vector3 getLocalVector(const Vector3& worldVector) const; - // -------------------- Friendship -------------------- // + // -------------------- Friendship -------------------- // - friend class CollisionWorld; - friend class DynamicsWorld; - friend class CollisionDetection; - friend class BroadPhaseAlgorithm; - friend class ConvexMeshShape; - friend class ProxyShape; + friend class CollisionWorld; + friend class DynamicsWorld; + friend class CollisionDetection; + friend class BroadPhaseAlgorithm; + friend class ConvexMeshShape; + friend class ProxyShape; }; // Return the type of the body @@ -189,80 +168,80 @@ class CollisionBody : public Body { * @return the type of the body (STATIC, KINEMATIC, DYNAMIC) */ inline BodyType CollisionBody::getType() const { - return mType; + return mType; } // Set the type of the body /// The type of the body can either STATIC, KINEMATIC or DYNAMIC as described bellow: /// STATIC : A static body has infinite mass, zero velocity but the position can be -/// changed manually. A static body does not collide with other static or kinematic bodies. +/// changed manually. A static body does not collide with other static or kinematic bodies. /// KINEMATIC : A kinematic body has infinite mass, the velocity can be changed manually and its -/// position is computed by the physics engine. A kinematic body does not collide with -/// other static or kinematic bodies. +/// position is computed by the physics engine. A kinematic body does not collide with +/// other static or kinematic bodies. /// DYNAMIC : A dynamic body has non-zero mass, non-zero velocity determined by forces and its -/// position is determined by the physics engine. A dynamic body can collide with other -/// dynamic, static or kinematic bodies. +/// position is determined by the physics engine. A dynamic body can collide with other +/// dynamic, static or kinematic bodies. /** * @param type The type of the body (STATIC, KINEMATIC, DYNAMIC) */ inline void CollisionBody::setType(BodyType type) { - mType = type; + mType = type; - if (mType == STATIC) { + if (mType == STATIC) { - // Update the broad-phase state of the body - updateBroadPhaseState(); - } + // Update the broad-phase state of the body + updateBroadPhaseState(); + } } // Return the current position and orientation /** * @return The current transformation of the body that transforms the local-space - * of the body into world-space + * of the body int32_to world-space */ inline const Transform& CollisionBody::getTransform() const { - return mTransform; + return mTransform; } // Set the current position and orientation /** * @param transform The transformation of the body that transforms the local-space - * of the body into world-space + * of the body int32_to world-space */ inline void CollisionBody::setTransform(const Transform& transform) { - // Update the transform of the body - mTransform = transform; + // Update the transform of the body + mTransform = transform; - // Update the broad-phase state of the body - updateBroadPhaseState(); + // Update the broad-phase state of the body + updateBroadPhaseState(); } // Return the first element of the linked list of contact manifolds involving this body /** * @return A pointer to the first element of the linked-list with the contact - * manifolds of this body + * manifolds of this body */ inline const ContactManifoldListElement* CollisionBody::getContactManifoldsList() const { - return mContactManifoldsList; + return mContactManifoldsList; } // Return the linked list of proxy shapes of that body /** * @return The pointer of the first proxy shape of the linked-list of all the -* proxy shapes of the body +* proxy shapes of the body */ inline ProxyShape* CollisionBody::getProxyShapesList() { - return mProxyCollisionShapes; + return mProxyCollisionShapes; } // Return the linked list of proxy shapes of that body /** * @return The pointer of the first proxy shape of the linked-list of all the -* proxy shapes of the body +* proxy shapes of the body */ inline const ProxyShape* CollisionBody::getProxyShapesList() const { - return mProxyCollisionShapes; + return mProxyCollisionShapes; } // Return the world-space coordinates of a point given the local-space coordinates of the body @@ -271,7 +250,7 @@ inline const ProxyShape* CollisionBody::getProxyShapesList() const { * @return The point in world-space coordinates */ inline Vector3 CollisionBody::getWorldPoint(const Vector3& localPoint) const { - return mTransform * localPoint; + return mTransform * localPoint; } // Return the world-space vector of a vector given in local-space coordinates of the body @@ -280,7 +259,7 @@ inline Vector3 CollisionBody::getWorldPoint(const Vector3& localPoint) const { * @return The vector in world-space coordinates */ inline Vector3 CollisionBody::getWorldVector(const Vector3& localVector) const { - return mTransform.getOrientation() * localVector; + return mTransform.getOrientation() * localVector; } // Return the body local-space coordinates of a point given in the world-space coordinates @@ -289,7 +268,7 @@ inline Vector3 CollisionBody::getWorldVector(const Vector3& localVector) const { * @return The point in the local-space coordinates of the body */ inline Vector3 CollisionBody::getLocalPoint(const Vector3& worldPoint) const { - return mTransform.getInverse() * worldPoint; + return mTransform.getInverse() * worldPoint; } // Return the body local-space coordinates of a vector given in the world-space coordinates @@ -298,9 +277,7 @@ inline Vector3 CollisionBody::getLocalPoint(const Vector3& worldPoint) const { * @return The vector in the local-space coordinates of the body */ inline Vector3 CollisionBody::getLocalVector(const Vector3& worldVector) const { - return mTransform.getOrientation().getInverse() * worldVector; + return mTransform.getOrientation().getInverse() * worldVector; } } - - #endif diff --git a/ephysics/body/RigidBody.cpp b/ephysics/body/RigidBody.cpp index a5af3d9..c109a86 100644 --- a/ephysics/body/RigidBody.cpp +++ b/ephysics/body/RigidBody.cpp @@ -1,27 +1,8 @@ -/******************************************************************************** -* 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. * -* * -********************************************************************************/ +/** @file + * @author Daniel Chappuis + * @copyright 2010-2016 Daniel Chappuis + * @license BSD 3 clauses (see license file) + */ // Libraries #include @@ -39,163 +20,163 @@ using namespace reactphysics3d; * @param id The ID of the body */ RigidBody::RigidBody(const Transform& transform, CollisionWorld& world, bodyindex id) - : CollisionBody(transform, world, id), mInitMass(decimal(1.0)), - mCenterOfMassLocal(0, 0, 0), mCenterOfMassWorld(transform.getPosition()), - mIsGravityEnabled(true), mLinearDamping(decimal(0.0)), mAngularDamping(decimal(0.0)), - mJointsList(NULL) { + : CollisionBody(transform, world, id), mInitMass(float(1.0)), + mCenterOfMassLocal(0, 0, 0), mCenterOfMassWorld(transform.getPosition()), + mIsGravityEnabled(true), mLinearDamping(float(0.0)), mAngularDamping(float(0.0)), + mJointsList(NULL) { - // Compute the inverse mass - mMassInverse = decimal(1.0) / mInitMass; + // Compute the inverse mass + mMassInverse = float(1.0) / mInitMass; } // Destructor RigidBody::~RigidBody() { - assert(mJointsList == NULL); + assert(mJointsList == NULL); } // Set the type of the body /// The type of the body can either STATIC, KINEMATIC or DYNAMIC as described bellow: /// STATIC : A static body has infinite mass, zero velocity but the position can be -/// changed manually. A static body does not collide with other static or kinematic bodies. +/// changed manually. A static body does not collide with other static or kinematic bodies. /// KINEMATIC : A kinematic body has infinite mass, the velocity can be changed manually and its -/// position is computed by the physics engine. A kinematic body does not collide with -/// other static or kinematic bodies. +/// position is computed by the physics engine. A kinematic body does not collide with +/// other static or kinematic bodies. /// DYNAMIC : A dynamic body has non-zero mass, non-zero velocity determined by forces and its -/// position is determined by the physics engine. A dynamic body can collide with other -/// dynamic, static or kinematic bodies. +/// position is determined by the physics engine. A dynamic body can collide with other +/// dynamic, static or kinematic bodies. /** * @param type The type of the body (STATIC, KINEMATIC, DYNAMIC) */ void RigidBody::setType(BodyType type) { - if (mType == type) return; + if (mType == type) return; - CollisionBody::setType(type); + CollisionBody::setType(type); - // Recompute the total mass, center of mass and inertia tensor - recomputeMassInformation(); + // Recompute the total mass, center of mass and inertia tensor + recomputeMassInformation(); - // If it is a static body - if (mType == STATIC) { + // If it is a static body + if (mType == STATIC) { - // Reset the velocity to zero - mLinearVelocity.setToZero(); - mAngularVelocity.setToZero(); - } + // Reset the velocity to zero + mLinearVelocity.setToZero(); + mAngularVelocity.setToZero(); + } - // If it is a static or a kinematic body - if (mType == STATIC || mType == KINEMATIC) { + // If it is a static or a kinematic body + if (mType == STATIC || mType == KINEMATIC) { - // Reset the inverse mass and inverse inertia tensor to zero - mMassInverse = decimal(0.0); - mInertiaTensorLocal.setToZero(); - mInertiaTensorLocalInverse.setToZero(); + // Reset the inverse mass and inverse inertia tensor to zero + mMassInverse = float(0.0); + mInertiaTensorLocal.setToZero(); + mInertiaTensorLocalInverse.setToZero(); - } - else { // If it is a dynamic body - mMassInverse = decimal(1.0) / mInitMass; - mInertiaTensorLocalInverse = mInertiaTensorLocal.getInverse(); - } + } + else { // If it is a dynamic body + mMassInverse = float(1.0) / mInitMass; + mInertiaTensorLocalInverse = mInertiaTensorLocal.getInverse(); + } - // Awake the body - setIsSleeping(false); + // Awake the body + setIsSleeping(false); - // Remove all the contacts with this body - resetContactManifoldsList(); + // Remove all the contacts with this body + resetContactManifoldsList(); - // Ask the broad-phase to test again the collision shapes of the body for collision - // detection (as if the body has moved) - askForBroadPhaseCollisionCheck(); + // Ask the broad-phase to test again the collision shapes of the body for collision + // detection (as if the body has moved) + askForBroadPhaseCollisionCheck(); - // Reset the force and torque on the body - mExternalForce.setToZero(); - mExternalTorque.setToZero(); + // Reset the force and torque on the body + mExternalForce.setToZero(); + mExternalTorque.setToZero(); } // Set the local inertia tensor of the body (in local-space coordinates) /** * @param inertiaTensorLocal The 3x3 inertia tensor matrix of the body in local-space - * coordinates + * coordinates */ void RigidBody::setInertiaTensorLocal(const Matrix3x3& inertiaTensorLocal) { - if (mType != DYNAMIC) return; + if (mType != DYNAMIC) return; - mInertiaTensorLocal = inertiaTensorLocal; + mInertiaTensorLocal = inertiaTensorLocal; - // Compute the inverse local inertia tensor - mInertiaTensorLocalInverse = mInertiaTensorLocal.getInverse(); + // Compute the inverse local inertia tensor + mInertiaTensorLocalInverse = mInertiaTensorLocal.getInverse(); } // Set the local center of mass of the body (in local-space coordinates) /** * @param centerOfMassLocal The center of mass of the body in local-space - * coordinates + * coordinates */ void RigidBody::setCenterOfMassLocal(const Vector3& centerOfMassLocal) { - if (mType != DYNAMIC) return; + if (mType != DYNAMIC) return; - const Vector3 oldCenterOfMass = mCenterOfMassWorld; - mCenterOfMassLocal = centerOfMassLocal; + const Vector3 oldCenterOfMass = mCenterOfMassWorld; + mCenterOfMassLocal = centerOfMassLocal; - // Compute the center of mass in world-space coordinates - mCenterOfMassWorld = mTransform * mCenterOfMassLocal; + // Compute the center of mass in world-space coordinates + mCenterOfMassWorld = mTransform * mCenterOfMassLocal; - // Update the linear velocity of the center of mass - mLinearVelocity += mAngularVelocity.cross(mCenterOfMassWorld - oldCenterOfMass); + // Update the linear velocity of the center of mass + mLinearVelocity += mAngularVelocity.cross(mCenterOfMassWorld - oldCenterOfMass); } // Set the mass of the rigid body /** * @param mass The mass (in kilograms) of the body */ -void RigidBody::setMass(decimal mass) { +void RigidBody::setMass(float mass) { - if (mType != DYNAMIC) return; + if (mType != DYNAMIC) return; - mInitMass = mass; + mInitMass = mass; - if (mInitMass > decimal(0.0)) { - mMassInverse = decimal(1.0) / mInitMass; - } - else { - mInitMass = decimal(1.0); - mMassInverse = decimal(1.0); - } + if (mInitMass > float(0.0)) { + mMassInverse = float(1.0) / mInitMass; + } + else { + mInitMass = float(1.0); + mMassInverse = float(1.0); + } } // Remove a joint from the joints list void RigidBody::removeJointFromJointsList(MemoryAllocator& memoryAllocator, const Joint* joint) { - assert(joint != NULL); - assert(mJointsList != NULL); + assert(joint != NULL); + assert(mJointsList != NULL); - // Remove the joint from the linked list of the joints of the first body - if (mJointsList->joint == joint) { // If the first element is the one to remove - JointListElement* elementToRemove = mJointsList; - mJointsList = elementToRemove->next; - elementToRemove->~JointListElement(); - memoryAllocator.release(elementToRemove, sizeof(JointListElement)); - } - else { // If the element to remove is not the first one in the list - JointListElement* currentElement = mJointsList; - while (currentElement->next != NULL) { - if (currentElement->next->joint == joint) { - JointListElement* elementToRemove = currentElement->next; - currentElement->next = elementToRemove->next; - elementToRemove->~JointListElement(); - memoryAllocator.release(elementToRemove, sizeof(JointListElement)); - break; - } - currentElement = currentElement->next; - } - } + // Remove the joint from the linked list of the joints of the first body + if (mJointsList->joint == joint) { // If the first element is the one to remove + JointListElement* elementToRemove = mJointsList; + mJointsList = elementToRemove->next; + elementToRemove->~JointListElement(); + memoryAllocator.release(elementToRemove, sizeof(JointListElement)); + } + else { // If the element to remove is not the first one in the list + JointListElement* currentElement = mJointsList; + while (currentElement->next != NULL) { + if (currentElement->next->joint == joint) { + JointListElement* elementToRemove = currentElement->next; + currentElement->next = elementToRemove->next; + elementToRemove->~JointListElement(); + memoryAllocator.release(elementToRemove, sizeof(JointListElement)); + break; + } + currentElement = currentElement->next; + } + } } // Add a collision shape to the body. -/// When you add a collision shape to the body, an internal copy of this -/// collision shape will be created internally. Therefore, you can delete it +/// When you add a collision shape to the body, an int32_ternal copy of this +/// collision shape will be created int32_ternally. Therefore, you can delete it /// right after calling this method or use it later to add it to another body. /// This method will return a pointer to a new proxy shape. A proxy shape is /// an object that links a collision shape and a given body. You can use the @@ -204,46 +185,46 @@ void RigidBody::removeJointFromJointsList(MemoryAllocator& memoryAllocator, cons /** * @param collisionShape The collision shape you want to add to the body * @param transform The transformation of the collision shape that transforms the - * local-space of the collision shape into the local-space of the body + * local-space of the collision shape int32_to the local-space of the body * @param mass Mass (in kilograms) of the collision shape you want to add * @return A pointer to the proxy shape that has been created to link the body to - * the new collision shape you have added. + * the new collision shape you have added. */ ProxyShape* RigidBody::addCollisionShape(CollisionShape* collisionShape, - const Transform& transform, - decimal mass) { + const Transform& transform, + float mass) { - assert(mass > decimal(0.0)); + assert(mass > float(0.0)); - // Create a new proxy collision shape to attach the collision shape to the body - ProxyShape* proxyShape = new (mWorld.mMemoryAllocator.allocate( - sizeof(ProxyShape))) ProxyShape(this, collisionShape, - transform, mass); + // Create a new proxy collision shape to attach the collision shape to the body + ProxyShape* proxyShape = new (mWorld.mMemoryAllocator.allocate( + sizeof(ProxyShape))) ProxyShape(this, collisionShape, + transform, mass); - // Add it to the list of proxy collision shapes of the body - if (mProxyCollisionShapes == NULL) { - mProxyCollisionShapes = proxyShape; - } - else { - proxyShape->mNext = mProxyCollisionShapes; - mProxyCollisionShapes = proxyShape; - } + // Add it to the list of proxy collision shapes of the body + if (mProxyCollisionShapes == NULL) { + mProxyCollisionShapes = proxyShape; + } + else { + proxyShape->mNext = mProxyCollisionShapes; + mProxyCollisionShapes = proxyShape; + } - // Compute the world-space AABB of the new collision shape - AABB aabb; - collisionShape->computeAABB(aabb, mTransform * transform); + // Compute the world-space AABB of the new collision shape + AABB aabb; + collisionShape->computeAABB(aabb, mTransform * transform); - // Notify the collision detection about this new collision shape - mWorld.mCollisionDetection.addProxyCollisionShape(proxyShape, aabb); + // Notify the collision detection about this new collision shape + mWorld.mCollisionDetection.addProxyCollisionShape(proxyShape, aabb); - mNbCollisionShapes++; + mNbCollisionShapes++; - // Recompute the center of mass, total mass and inertia tensor of the body with the new - // collision shape - recomputeMassInformation(); + // Recompute the center of mass, total mass and inertia tensor of the body with the new + // collision shape + recomputeMassInformation(); - // Return a pointer to the proxy collision shape - return proxyShape; + // Return a pointer to the proxy collision shape + return proxyShape; } // Remove a collision shape from the body @@ -255,11 +236,11 @@ ProxyShape* RigidBody::addCollisionShape(CollisionShape* collisionShape, */ void RigidBody::removeCollisionShape(const ProxyShape* proxyShape) { - // Remove the collision shape - CollisionBody::removeCollisionShape(proxyShape); + // Remove the collision shape + CollisionBody::removeCollisionShape(proxyShape); - // Recompute the total mass, center of mass and inertia tensor - recomputeMassInformation(); + // Recompute the total mass, center of mass and inertia tensor + recomputeMassInformation(); } // Set the linear velocity of the rigid body. @@ -268,16 +249,16 @@ void RigidBody::removeCollisionShape(const ProxyShape* proxyShape) { */ void RigidBody::setLinearVelocity(const Vector3& linearVelocity) { - // If it is a static body, we do nothing - if (mType == STATIC) return; + // If it is a static body, we do nothing + if (mType == STATIC) return; - // Update the linear velocity of the current body state - mLinearVelocity = linearVelocity; + // Update the linear velocity of the current body state + mLinearVelocity = linearVelocity; - // If the linear velocity is not zero, awake the body - if (mLinearVelocity.lengthSquare() > decimal(0.0)) { - setIsSleeping(false); - } + // If the linear velocity is not zero, awake the body + if (mLinearVelocity.lengthSquare() > float(0.0)) { + setIsSleeping(false); + } } // Set the angular velocity. @@ -286,129 +267,129 @@ void RigidBody::setLinearVelocity(const Vector3& linearVelocity) { */ void RigidBody::setAngularVelocity(const Vector3& angularVelocity) { - // If it is a static body, we do nothing - if (mType == STATIC) return; + // If it is a static body, we do nothing + if (mType == STATIC) return; - // Set the angular velocity - mAngularVelocity = angularVelocity; + // Set the angular velocity + mAngularVelocity = angularVelocity; - // If the velocity is not zero, awake the body - if (mAngularVelocity.lengthSquare() > decimal(0.0)) { - setIsSleeping(false); - } + // If the velocity is not zero, awake the body + if (mAngularVelocity.lengthSquare() > float(0.0)) { + setIsSleeping(false); + } } // Set the current position and orientation /** * @param transform The transformation of the body that transforms the local-space - * of the body into world-space + * of the body int32_to world-space */ void RigidBody::setTransform(const Transform& transform) { - // Update the transform of the body - mTransform = transform; + // Update the transform of the body + mTransform = transform; - const Vector3 oldCenterOfMass = mCenterOfMassWorld; + const Vector3 oldCenterOfMass = mCenterOfMassWorld; - // Compute the new center of mass in world-space coordinates - mCenterOfMassWorld = mTransform * mCenterOfMassLocal; + // Compute the new center of mass in world-space coordinates + mCenterOfMassWorld = mTransform * mCenterOfMassLocal; - // Update the linear velocity of the center of mass - mLinearVelocity += mAngularVelocity.cross(mCenterOfMassWorld - oldCenterOfMass); + // Update the linear velocity of the center of mass + mLinearVelocity += mAngularVelocity.cross(mCenterOfMassWorld - oldCenterOfMass); - // Update the broad-phase state of the body - updateBroadPhaseState(); + // Update the broad-phase state of the body + updateBroadPhaseState(); } // Recompute the center of mass, total mass and inertia tensor of the body using all // the collision shapes attached to the body. void RigidBody::recomputeMassInformation() { - mInitMass = decimal(0.0); - mMassInverse = decimal(0.0); - mInertiaTensorLocal.setToZero(); - mInertiaTensorLocalInverse.setToZero(); - mCenterOfMassLocal.setToZero(); + mInitMass = float(0.0); + mMassInverse = float(0.0); + mInertiaTensorLocal.setToZero(); + mInertiaTensorLocalInverse.setToZero(); + mCenterOfMassLocal.setToZero(); - // If it is STATIC or KINEMATIC body - if (mType == STATIC || mType == KINEMATIC) { - mCenterOfMassWorld = mTransform.getPosition(); - return; - } + // If it is STATIC or KINEMATIC body + if (mType == STATIC || mType == KINEMATIC) { + mCenterOfMassWorld = mTransform.getPosition(); + return; + } - assert(mType == DYNAMIC); + assert(mType == DYNAMIC); - // Compute the total mass of the body - for (ProxyShape* shape = mProxyCollisionShapes; shape != NULL; shape = shape->mNext) { - mInitMass += shape->getMass(); - mCenterOfMassLocal += shape->getLocalToBodyTransform().getPosition() * shape->getMass(); - } + // Compute the total mass of the body + for (ProxyShape* shape = mProxyCollisionShapes; shape != NULL; shape = shape->mNext) { + mInitMass += shape->getMass(); + mCenterOfMassLocal += shape->getLocalToBodyTransform().getPosition() * shape->getMass(); + } - if (mInitMass > decimal(0.0)) { - mMassInverse = decimal(1.0) / mInitMass; - } - else { - mInitMass = decimal(1.0); - mMassInverse = decimal(1.0); - } + if (mInitMass > float(0.0)) { + mMassInverse = float(1.0) / mInitMass; + } + else { + mInitMass = float(1.0); + mMassInverse = float(1.0); + } - // Compute the center of mass - const Vector3 oldCenterOfMass = mCenterOfMassWorld; - mCenterOfMassLocal *= mMassInverse; - mCenterOfMassWorld = mTransform * mCenterOfMassLocal; + // Compute the center of mass + const Vector3 oldCenterOfMass = mCenterOfMassWorld; + mCenterOfMassLocal *= mMassInverse; + mCenterOfMassWorld = mTransform * mCenterOfMassLocal; - // Compute the total mass and inertia tensor using all the collision shapes - for (ProxyShape* shape = mProxyCollisionShapes; shape != NULL; shape = shape->mNext) { + // Compute the total mass and inertia tensor using all the collision shapes + for (ProxyShape* shape = mProxyCollisionShapes; shape != NULL; shape = shape->mNext) { - // Get the inertia tensor of the collision shape in its local-space - Matrix3x3 inertiaTensor; - shape->getCollisionShape()->computeLocalInertiaTensor(inertiaTensor, shape->getMass()); + // Get the inertia tensor of the collision shape in its local-space + Matrix3x3 inertiaTensor; + shape->getCollisionShape()->computeLocalInertiaTensor(inertiaTensor, shape->getMass()); - // Convert the collision shape inertia tensor into the local-space of the body - const Transform& shapeTransform = shape->getLocalToBodyTransform(); - Matrix3x3 rotationMatrix = shapeTransform.getOrientation().getMatrix(); - inertiaTensor = rotationMatrix * inertiaTensor * rotationMatrix.getTranspose(); + // Convert the collision shape inertia tensor int32_to the local-space of the body + const Transform& shapeTransform = shape->getLocalToBodyTransform(); + Matrix3x3 rotationMatrix = shapeTransform.getOrientation().getMatrix(); + inertiaTensor = rotationMatrix * inertiaTensor * rotationMatrix.getTranspose(); - // Use the parallel axis theorem to convert the inertia tensor w.r.t the collision shape - // center into a inertia tensor w.r.t to the body origin. - Vector3 offset = shapeTransform.getPosition() - mCenterOfMassLocal; - decimal offsetSquare = offset.lengthSquare(); - Matrix3x3 offsetMatrix; - offsetMatrix[0].setAllValues(offsetSquare, decimal(0.0), decimal(0.0)); - offsetMatrix[1].setAllValues(decimal(0.0), offsetSquare, decimal(0.0)); - offsetMatrix[2].setAllValues(decimal(0.0), decimal(0.0), offsetSquare); - offsetMatrix[0] += offset * (-offset.x); - offsetMatrix[1] += offset * (-offset.y); - offsetMatrix[2] += offset * (-offset.z); - offsetMatrix *= shape->getMass(); + // Use the parallel axis theorem to convert the inertia tensor w.r.t the collision shape + // center int32_to a inertia tensor w.r.t to the body origin. + Vector3 offset = shapeTransform.getPosition() - mCenterOfMassLocal; + float offsetSquare = offset.lengthSquare(); + Matrix3x3 offsetMatrix; + offsetMatrix[0].setAllValues(offsetSquare, float(0.0), float(0.0)); + offsetMatrix[1].setAllValues(float(0.0), offsetSquare, float(0.0)); + offsetMatrix[2].setAllValues(float(0.0), float(0.0), offsetSquare); + offsetMatrix[0] += offset * (-offset.x); + offsetMatrix[1] += offset * (-offset.y); + offsetMatrix[2] += offset * (-offset.z); + offsetMatrix *= shape->getMass(); - mInertiaTensorLocal += inertiaTensor + offsetMatrix; - } + mInertiaTensorLocal += inertiaTensor + offsetMatrix; + } - // Compute the local inverse inertia tensor - mInertiaTensorLocalInverse = mInertiaTensorLocal.getInverse(); + // Compute the local inverse inertia tensor + mInertiaTensorLocalInverse = mInertiaTensorLocal.getInverse(); - // Update the linear velocity of the center of mass - mLinearVelocity += mAngularVelocity.cross(mCenterOfMassWorld - oldCenterOfMass); + // Update the linear velocity of the center of mass + mLinearVelocity += mAngularVelocity.cross(mCenterOfMassWorld - oldCenterOfMass); } // Update the broad-phase state for this body (because it has moved for instance) void RigidBody::updateBroadPhaseState() const { - PROFILE("RigidBody::updateBroadPhaseState()"); + PROFILE("RigidBody::updateBroadPhaseState()"); - DynamicsWorld& world = static_cast(mWorld); + DynamicsWorld& world = static_cast(mWorld); const Vector3 displacement = world.mTimeStep * mLinearVelocity; - // For all the proxy collision shapes of the body - for (ProxyShape* shape = mProxyCollisionShapes; shape != NULL; shape = shape->mNext) { + // For all the proxy collision shapes of the body + for (ProxyShape* shape = mProxyCollisionShapes; shape != NULL; shape = shape->mNext) { - // Recompute the world-space AABB of the collision shape - AABB aabb; - shape->getCollisionShape()->computeAABB(aabb, mTransform *shape->getLocalToBodyTransform()); + // Recompute the world-space AABB of the collision shape + AABB aabb; + shape->getCollisionShape()->computeAABB(aabb, mTransform *shape->getLocalToBodyTransform()); - // Update the broad-phase state for the proxy collision shape - mWorld.mCollisionDetection.updateProxyCollisionShape(shape, aabb, displacement); - } + // Update the broad-phase state for the proxy collision shape + mWorld.mCollisionDetection.updateProxyCollisionShape(shape, aabb, displacement); + } } diff --git a/ephysics/body/RigidBody.h b/ephysics/body/RigidBody.h index df3f42f..7b47c2e 100644 --- a/ephysics/body/RigidBody.h +++ b/ephysics/body/RigidBody.h @@ -1,30 +1,9 @@ -/******************************************************************************** -* 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 REACTPHYSICS3D_RIGID_BODY_H -#define REACTPHYSICS3D_RIGID_BODY_H +/** @file + * @author Daniel Chappuis + * @copyright 2010-2016 Daniel Chappuis + * @license BSD 3 clauses (see license file) + */ +#pragma once // Libraries #include @@ -50,193 +29,193 @@ class DynamicsWorld; */ class RigidBody : public CollisionBody { - protected : + protected : - // -------------------- Attributes -------------------- // + // -------------------- Attributes -------------------- // - /// Intial mass of the body - decimal mInitMass; + /// Intial mass of the body + float mInitMass; - /// Center of mass of the body in local-space coordinates. - /// The center of mass can therefore be different from the body origin - Vector3 mCenterOfMassLocal; + /// Center of mass of the body in local-space coordinates. + /// The center of mass can therefore be different from the body origin + Vector3 mCenterOfMassLocal; - /// Center of mass of the body in world-space coordinates - Vector3 mCenterOfMassWorld; + /// Center of mass of the body in world-space coordinates + Vector3 mCenterOfMassWorld; - /// Linear velocity of the body - Vector3 mLinearVelocity; + /// Linear velocity of the body + Vector3 mLinearVelocity; - /// Angular velocity of the body - Vector3 mAngularVelocity; + /// Angular velocity of the body + Vector3 mAngularVelocity; - /// Current external force on the body - Vector3 mExternalForce; + /// Current external force on the body + Vector3 mExternalForce; - /// Current external torque on the body - Vector3 mExternalTorque; + /// Current external torque on the body + Vector3 mExternalTorque; - /// Local inertia tensor of the body (in local-space) with respect to the - /// center of mass of the body - Matrix3x3 mInertiaTensorLocal; + /// Local inertia tensor of the body (in local-space) with respect to the + /// center of mass of the body + Matrix3x3 mInertiaTensorLocal; - /// Inverse of the inertia tensor of the body - Matrix3x3 mInertiaTensorLocalInverse; + /// Inverse of the inertia tensor of the body + Matrix3x3 mInertiaTensorLocalInverse; - /// Inverse of the mass of the body - decimal mMassInverse; + /// Inverse of the mass of the body + float mMassInverse; - /// True if the gravity needs to be applied to this rigid body - bool mIsGravityEnabled; + /// True if the gravity needs to be applied to this rigid body + bool mIsGravityEnabled; - /// Material properties of the rigid body - Material mMaterial; + /// Material properties of the rigid body + Material mMaterial; - /// Linear velocity damping factor - decimal mLinearDamping; + /// Linear velocity damping factor + float mLinearDamping; - /// Angular velocity damping factor - decimal mAngularDamping; + /// Angular velocity damping factor + float mAngularDamping; - /// First element of the linked list of joints involving this body - JointListElement* mJointsList; + /// First element of the linked list of joints involving this body + JointListElement* mJointsList; - // -------------------- Methods -------------------- // + // -------------------- Methods -------------------- // - /// Private copy-constructor - RigidBody(const RigidBody& body); + /// Private copy-constructor + RigidBody(const RigidBody& body); - /// Private assignment operator - RigidBody& operator=(const RigidBody& body); + /// Private assignment operator + RigidBody& operator=(const RigidBody& body); - /// Remove a joint from the joints list - void removeJointFromJointsList(MemoryAllocator& memoryAllocator, const Joint* joint); + /// Remove a joint from the joints list + void removeJointFromJointsList(MemoryAllocator& memoryAllocator, const Joint* joint); - /// Update the transform of the body after a change of the center of mass - void updateTransformWithCenterOfMass(); + /// Update the transform of the body after a change of the center of mass + void updateTransformWithCenterOfMass(); - /// Update the broad-phase state for this body (because it has moved for instance) - virtual void updateBroadPhaseState() const; + /// Update the broad-phase state for this body (because it has moved for instance) + virtual void updateBroadPhaseState() const; - public : + public : - // -------------------- Methods -------------------- // + // -------------------- Methods -------------------- // - /// Constructor - RigidBody(const Transform& transform, CollisionWorld& world, bodyindex id); + /// Constructor + RigidBody(const Transform& transform, CollisionWorld& world, bodyindex id); - /// Destructor - virtual ~RigidBody(); + /// Destructor + virtual ~RigidBody(); - /// Set the type of the body (static, kinematic or dynamic) - void setType(BodyType type); + /// Set the type of the body (static, kinematic or dynamic) + void setType(BodyType type); - /// Set the current position and orientation - virtual void setTransform(const Transform& transform); + /// Set the current position and orientation + virtual void setTransform(const Transform& transform); - /// Return the mass of the body - decimal getMass() const; + /// Return the mass of the body + float getMass() const; - /// Return the linear velocity - Vector3 getLinearVelocity() const; + /// Return the linear velocity + Vector3 getLinearVelocity() const; - /// Set the linear velocity of the body. - void setLinearVelocity(const Vector3& linearVelocity); + /// Set the linear velocity of the body. + void setLinearVelocity(const Vector3& linearVelocity); - /// Return the angular velocity - Vector3 getAngularVelocity() const; + /// Return the angular velocity + Vector3 getAngularVelocity() const; - /// Set the angular velocity. - void setAngularVelocity(const Vector3& angularVelocity); + /// Set the angular velocity. + void setAngularVelocity(const Vector3& angularVelocity); - /// Set the variable to know whether or not the body is sleeping - virtual void setIsSleeping(bool isSleeping); + /// Set the variable to know whether or not the body is sleeping + virtual void setIsSleeping(bool isSleeping); - /// Return the local inertia tensor of the body (in body coordinates) - const Matrix3x3& getInertiaTensorLocal() const; + /// Return the local inertia tensor of the body (in body coordinates) + const Matrix3x3& getInertiaTensorLocal() const; - /// Set the local inertia tensor of the body (in body coordinates) - void setInertiaTensorLocal(const Matrix3x3& inertiaTensorLocal); + /// Set the local inertia tensor of the body (in body coordinates) + void setInertiaTensorLocal(const Matrix3x3& inertiaTensorLocal); - /// Set the local center of mass of the body (in local-space coordinates) - void setCenterOfMassLocal(const Vector3& centerOfMassLocal); + /// Set the local center of mass of the body (in local-space coordinates) + void setCenterOfMassLocal(const Vector3& centerOfMassLocal); - /// Set the mass of the rigid body - void setMass(decimal mass); + /// Set the mass of the rigid body + void setMass(float mass); - /// Return the inertia tensor in world coordinates. - Matrix3x3 getInertiaTensorWorld() const; + /// Return the inertia tensor in world coordinates. + Matrix3x3 getInertiaTensorWorld() const; - /// Return the inverse of the inertia tensor in world coordinates. - Matrix3x3 getInertiaTensorInverseWorld() const; + /// Return the inverse of the inertia tensor in world coordinates. + Matrix3x3 getInertiaTensorInverseWorld() const; - /// Return true if the gravity needs to be applied to this rigid body - bool isGravityEnabled() const; + /// Return true if the gravity needs to be applied to this rigid body + bool isGravityEnabled() const; - /// Set the variable to know if the gravity is applied to this rigid body - void enableGravity(bool isEnabled); + /// Set the variable to know if the gravity is applied to this rigid body + void enableGravity(bool isEnabled); - /// Return a reference to the material properties of the rigid body - Material& getMaterial(); + /// Return a reference to the material properties of the rigid body + Material& getMaterial(); - /// Set a new material for this rigid body - void setMaterial(const Material& material); + /// Set a new material for this rigid body + void setMaterial(const Material& material); - /// Return the linear velocity damping factor - decimal getLinearDamping() const; + /// Return the linear velocity damping factor + float getLinearDamping() const; - /// Set the linear damping factor - void setLinearDamping(decimal linearDamping); + /// Set the linear damping factor + void setLinearDamping(float linearDamping); - /// Return the angular velocity damping factor - decimal getAngularDamping() const; + /// Return the angular velocity damping factor + float getAngularDamping() const; - /// Set the angular damping factor - void setAngularDamping(decimal angularDamping); + /// Set the angular damping factor + void setAngularDamping(float angularDamping); - /// Return the first element of the linked list of joints involving this body - const JointListElement* getJointsList() const; + /// Return the first element of the linked list of joints involving this body + const JointListElement* getJointsList() const; - /// Return the first element of the linked list of joints involving this body - JointListElement* getJointsList(); + /// Return the first element of the linked list of joints involving this body + JointListElement* getJointsList(); - /// Apply an external force to the body at its center of mass. - void applyForceToCenterOfMass(const Vector3& force); + /// Apply an external force to the body at its center of mass. + void applyForceToCenterOfMass(const Vector3& force); - /// Apply an external force to the body at a given point (in world-space coordinates). - void applyForce(const Vector3& force, const Vector3& point); + /// Apply an external force to the body at a given point (in world-space coordinates). + void applyForce(const Vector3& force, const Vector3& point); - /// Apply an external torque to the body. - void applyTorque(const Vector3& torque); + /// Apply an external torque to the body. + void applyTorque(const Vector3& torque); - /// Add a collision shape to the body. - virtual ProxyShape* addCollisionShape(CollisionShape* collisionShape, - const Transform& transform, - decimal mass); + /// Add a collision shape to the body. + virtual ProxyShape* addCollisionShape(CollisionShape* collisionShape, + const Transform& transform, + float mass); - /// Remove a collision shape from the body - virtual void removeCollisionShape(const ProxyShape* proxyShape); + /// Remove a collision shape from the body + virtual void removeCollisionShape(const ProxyShape* proxyShape); - /// Recompute the center of mass, total mass and inertia tensor of the body using all - /// the collision shapes attached to the body. - void recomputeMassInformation(); + /// Recompute the center of mass, total mass and inertia tensor of the body using all + /// the collision shapes attached to the body. + void recomputeMassInformation(); - // -------------------- Friendship -------------------- // + // -------------------- Friendship -------------------- // - friend class DynamicsWorld; - friend class ContactSolver; - friend class BallAndSocketJoint; - friend class SliderJoint; - friend class HingeJoint; - friend class FixedJoint; + friend class DynamicsWorld; + friend class ContactSolver; + friend class BallAndSocketJoint; + friend class SliderJoint; + friend class HingeJoint; + friend class FixedJoint; }; // Method that return the mass of the body /** * @return The mass (in kilograms) of the body */ -inline decimal RigidBody::getMass() const { - return mInitMass; +inline float RigidBody::getMass() const { + return mInitMass; } // Return the linear velocity @@ -244,7 +223,7 @@ inline decimal RigidBody::getMass() const { * @return The linear velocity vector of the body */ inline Vector3 RigidBody::getLinearVelocity() const { - return mLinearVelocity; + return mLinearVelocity; } // Return the angular velocity of the body @@ -252,7 +231,7 @@ inline Vector3 RigidBody::getLinearVelocity() const { * @return The angular velocity vector of the body */ inline Vector3 RigidBody::getAngularVelocity() const { - return mAngularVelocity; + return mAngularVelocity; } // Return the local inertia tensor of the body (in local-space coordinates) @@ -260,7 +239,7 @@ inline Vector3 RigidBody::getAngularVelocity() const { * @return The 3x3 inertia tensor matrix of the body (in local-space coordinates) */ inline const Matrix3x3& RigidBody::getInertiaTensorLocal() const { - return mInertiaTensorLocal; + return mInertiaTensorLocal; } // Return the inertia tensor in world coordinates. @@ -274,9 +253,9 @@ inline const Matrix3x3& RigidBody::getInertiaTensorLocal() const { */ inline Matrix3x3 RigidBody::getInertiaTensorWorld() const { - // Compute and return the inertia tensor in world coordinates - return mTransform.getOrientation().getMatrix() * mInertiaTensorLocal * - mTransform.getOrientation().getMatrix().getTranspose(); + // Compute and return the inertia tensor in world coordinates + return mTransform.getOrientation().getMatrix() * mInertiaTensorLocal * + mTransform.getOrientation().getMatrix().getTranspose(); } // Return the inverse of the inertia tensor in world coordinates. @@ -287,16 +266,16 @@ inline Matrix3x3 RigidBody::getInertiaTensorWorld() const { /// current orientation quaternion of the body /** * @return The 3x3 inverse inertia tensor matrix of the body in world-space - * coordinates + * coordinates */ inline Matrix3x3 RigidBody::getInertiaTensorInverseWorld() const { - // TODO : DO NOT RECOMPUTE THE MATRIX MULTIPLICATION EVERY TIME. WE NEED TO STORE THE - // INVERSE WORLD TENSOR IN THE CLASS AND UPLDATE IT WHEN THE ORIENTATION OF THE BODY CHANGES + // TODO : DO NOT RECOMPUTE THE MATRIX MULTIPLICATION EVERY TIME. WE NEED TO STORE THE + // INVERSE WORLD TENSOR IN THE CLASS AND UPLDATE IT WHEN THE ORIENTATION OF THE BODY CHANGES - // Compute and return the inertia tensor in world coordinates - return mTransform.getOrientation().getMatrix() * mInertiaTensorLocalInverse * - mTransform.getOrientation().getMatrix().getTranspose(); + // Compute and return the inertia tensor in world coordinates + return mTransform.getOrientation().getMatrix() * mInertiaTensorLocalInverse * + mTransform.getOrientation().getMatrix().getTranspose(); } // Return true if the gravity needs to be applied to this rigid body @@ -304,7 +283,7 @@ inline Matrix3x3 RigidBody::getInertiaTensorInverseWorld() const { * @return True if the gravity is applied to the body */ inline bool RigidBody::isGravityEnabled() const { - return mIsGravityEnabled; + return mIsGravityEnabled; } // Set the variable to know if the gravity is applied to this rigid body @@ -312,7 +291,7 @@ inline bool RigidBody::isGravityEnabled() const { * @param isEnabled True if you want the gravity to be applied to this body */ inline void RigidBody::enableGravity(bool isEnabled) { - mIsGravityEnabled = isEnabled; + mIsGravityEnabled = isEnabled; } // Return a reference to the material properties of the rigid body @@ -320,7 +299,7 @@ inline void RigidBody::enableGravity(bool isEnabled) { * @return A reference to the material of the body */ inline Material& RigidBody::getMaterial() { - return mMaterial; + return mMaterial; } // Set a new material for this rigid body @@ -328,15 +307,15 @@ inline Material& RigidBody::getMaterial() { * @param material The material you want to set to the body */ inline void RigidBody::setMaterial(const Material& material) { - mMaterial = material; + mMaterial = material; } // Return the linear velocity damping factor /** * @return The linear damping factor of this body */ -inline decimal RigidBody::getLinearDamping() const { - return mLinearDamping; +inline float RigidBody::getLinearDamping() const { + return mLinearDamping; } // Set the linear damping factor. This is the ratio of the linear velocity @@ -344,17 +323,17 @@ inline decimal RigidBody::getLinearDamping() const { /** * @param linearDamping The linear damping factor of this body */ -inline void RigidBody::setLinearDamping(decimal linearDamping) { - assert(linearDamping >= decimal(0.0)); - mLinearDamping = linearDamping; +inline void RigidBody::setLinearDamping(float linearDamping) { + assert(linearDamping >= float(0.0)); + mLinearDamping = linearDamping; } // Return the angular velocity damping factor /** * @return The angular damping factor of this body */ -inline decimal RigidBody::getAngularDamping() const { - return mAngularDamping; +inline float RigidBody::getAngularDamping() const { + return mAngularDamping; } // Set the angular damping factor. This is the ratio of the angular velocity @@ -362,9 +341,9 @@ inline decimal RigidBody::getAngularDamping() const { /** * @param angularDamping The angular damping factor of this body */ -inline void RigidBody::setAngularDamping(decimal angularDamping) { - assert(angularDamping >= decimal(0.0)); - mAngularDamping = angularDamping; +inline void RigidBody::setAngularDamping(float angularDamping) { + assert(angularDamping >= float(0.0)); + mAngularDamping = angularDamping; } // Return the first element of the linked list of joints involving this body @@ -372,7 +351,7 @@ inline void RigidBody::setAngularDamping(decimal angularDamping) { * @return The first element of the linked-list of all the joints involving this body */ inline const JointListElement* RigidBody::getJointsList() const { - return mJointsList; + return mJointsList; } // Return the first element of the linked list of joints involving this body @@ -380,20 +359,20 @@ inline const JointListElement* RigidBody::getJointsList() const { * @return The first element of the linked-list of all the joints involving this body */ inline JointListElement* RigidBody::getJointsList() { - return mJointsList; + return mJointsList; } // Set the variable to know whether or not the body is sleeping inline void RigidBody::setIsSleeping(bool isSleeping) { - if (isSleeping) { - mLinearVelocity.setToZero(); - mAngularVelocity.setToZero(); - mExternalForce.setToZero(); - mExternalTorque.setToZero(); - } + if (isSleeping) { + mLinearVelocity.setToZero(); + mAngularVelocity.setToZero(); + mExternalForce.setToZero(); + mExternalTorque.setToZero(); + } - Body::setIsSleeping(isSleeping); + Body::setIsSleeping(isSleeping); } // Apply an external force to the body at its center of mass. @@ -406,16 +385,16 @@ inline void RigidBody::setIsSleeping(bool isSleeping) { */ inline void RigidBody::applyForceToCenterOfMass(const Vector3& force) { - // If it is not a dynamic body, we do nothing - if (mType != DYNAMIC) return; + // If it is not a dynamic body, we do nothing + if (mType != DYNAMIC) return; - // Awake the body if it was sleeping - if (mIsSleeping) { - setIsSleeping(false); - } + // Awake the body if it was sleeping + if (mIsSleeping) { + setIsSleeping(false); + } - // Add the force - mExternalForce += force; + // Add the force + mExternalForce += force; } // Apply an external force to the body at a given point (in world-space coordinates). @@ -431,17 +410,17 @@ inline void RigidBody::applyForceToCenterOfMass(const Vector3& force) { */ inline void RigidBody::applyForce(const Vector3& force, const Vector3& point) { - // If it is not a dynamic body, we do nothing - if (mType != DYNAMIC) return; + // If it is not a dynamic body, we do nothing + if (mType != DYNAMIC) return; - // Awake the body if it was sleeping - if (mIsSleeping) { - setIsSleeping(false); - } + // Awake the body if it was sleeping + if (mIsSleeping) { + setIsSleeping(false); + } - // Add the force and torque - mExternalForce += force; - mExternalTorque += (point - mCenterOfMassWorld).cross(force); + // Add the force and torque + mExternalForce += force; + mExternalTorque += (point - mCenterOfMassWorld).cross(force); } // Apply an external torque to the body. @@ -454,25 +433,24 @@ inline void RigidBody::applyForce(const Vector3& force, const Vector3& point) { */ inline void RigidBody::applyTorque(const Vector3& torque) { - // If it is not a dynamic body, we do nothing - if (mType != DYNAMIC) return; + // If it is not a dynamic body, we do nothing + if (mType != DYNAMIC) return; - // Awake the body if it was sleeping - if (mIsSleeping) { - setIsSleeping(false); - } + // Awake the body if it was sleeping + if (mIsSleeping) { + setIsSleeping(false); + } - // Add the torque - mExternalTorque += torque; + // Add the torque + mExternalTorque += torque; } /// Update the transform of the body after a change of the center of mass inline void RigidBody::updateTransformWithCenterOfMass() { - // Translate the body according to the translation of the center of mass position - mTransform.setPosition(mCenterOfMassWorld - mTransform.getOrientation() * mCenterOfMassLocal); + // Translate the body according to the translation of the center of mass position + mTransform.setPosition(mCenterOfMassWorld - mTransform.getOrientation() * mCenterOfMassLocal); } } - #endif diff --git a/ephysics/collision/CollisionDetection.cpp b/ephysics/collision/CollisionDetection.cpp index 6678dc8..5a2d45a 100644 --- a/ephysics/collision/CollisionDetection.cpp +++ b/ephysics/collision/CollisionDetection.cpp @@ -1,27 +1,8 @@ -/******************************************************************************** -* 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. * -* * -********************************************************************************/ +/** @file + * @author Daniel Chappuis + * @copyright 2010-2016 Daniel Chappuis + * @license BSD 3 clauses (see license file) + */ // Libraries #include @@ -42,15 +23,15 @@ using namespace std; // Constructor CollisionDetection::CollisionDetection(CollisionWorld* world, MemoryAllocator& memoryAllocator) - : mMemoryAllocator(memoryAllocator), - mWorld(world), mBroadPhaseAlgorithm(*this), - mIsCollisionShapesAdded(false) { + : mMemoryAllocator(memoryAllocator), + mWorld(world), mBroadPhaseAlgorithm(*this), + mIsCollisionShapesAdded(false) { - // Set the default collision dispatch configuration - setCollisionDispatch(&mDefaultCollisionDispatch); + // Set the default collision dispatch configuration + setCollisionDispatch(&mDefaultCollisionDispatch); - // Fill-in the collision detection matrix with algorithms - fillInCollisionMatrix(); + // Fill-in the collision detection matrix with algorithms + fillInCollisionMatrix(); } // Destructor @@ -61,458 +42,458 @@ CollisionDetection::~CollisionDetection() { // Compute the collision detection void CollisionDetection::computeCollisionDetection() { - PROFILE("CollisionDetection::computeCollisionDetection()"); - - // Compute the broad-phase collision detection - computeBroadPhase(); - - // Compute the narrow-phase collision detection - computeNarrowPhase(); + PROFILE("CollisionDetection::computeCollisionDetection()"); + + // Compute the broad-phase collision detection + computeBroadPhase(); + + // Compute the narrow-phase collision detection + computeNarrowPhase(); } // Compute the collision detection void CollisionDetection::testCollisionBetweenShapes(CollisionCallback* callback, - const std::set& shapes1, - const std::set& shapes2) { + const std::set& shapes1, + const std::set& shapes2) { - // Compute the broad-phase collision detection - computeBroadPhase(); + // Compute the broad-phase collision detection + computeBroadPhase(); - // Delete all the contact points in the currently overlapping pairs - clearContactPoints(); + // Delete all the contact points in the currently overlapping pairs + clearContactPoints(); - // Compute the narrow-phase collision detection among given sets of shapes - computeNarrowPhaseBetweenShapes(callback, shapes1, shapes2); + // Compute the narrow-phase collision detection among given sets of shapes + computeNarrowPhaseBetweenShapes(callback, shapes1, shapes2); } // Report collision between two sets of shapes void CollisionDetection::reportCollisionBetweenShapes(CollisionCallback* callback, - const std::set& shapes1, - const std::set& shapes2) { + const std::set& shapes1, + const std::set& shapes2) { - // For each possible collision pair of bodies - map::iterator it; - for (it = mOverlappingPairs.begin(); it != mOverlappingPairs.end(); ++it) { + // For each possible collision pair of bodies + map::iterator it; + for (it = mOverlappingPairs.begin(); it != mOverlappingPairs.end(); ++it) { - OverlappingPair* pair = it->second; + OverlappingPair* pair = it->second; - const ProxyShape* shape1 = pair->getShape1(); - const ProxyShape* shape2 = pair->getShape2(); + const ProxyShape* shape1 = pair->getShape1(); + const ProxyShape* shape2 = pair->getShape2(); - assert(shape1->mBroadPhaseID != shape2->mBroadPhaseID); + assert(shape1->mBroadPhaseID != shape2->mBroadPhaseID); - // If both shapes1 and shapes2 sets are non-empty, we check that - // shape1 is among on set and shape2 is among the other one - if (!shapes1.empty() && !shapes2.empty() && - (shapes1.count(shape1->mBroadPhaseID) == 0 || shapes2.count(shape2->mBroadPhaseID) == 0) && - (shapes1.count(shape2->mBroadPhaseID) == 0 || shapes2.count(shape1->mBroadPhaseID) == 0)) { - continue; - } - if (!shapes1.empty() && shapes2.empty() && - shapes1.count(shape1->mBroadPhaseID) == 0 && shapes1.count(shape2->mBroadPhaseID) == 0) - { - continue; - } - if (!shapes2.empty() && shapes1.empty() && - shapes2.count(shape1->mBroadPhaseID) == 0 && shapes2.count(shape2->mBroadPhaseID) == 0) - { - continue; - } + // If both shapes1 and shapes2 sets are non-empty, we check that + // shape1 is among on set and shape2 is among the other one + if (!shapes1.empty() && !shapes2.empty() && + (shapes1.count(shape1->mBroadPhaseID) == 0 || shapes2.count(shape2->mBroadPhaseID) == 0) && + (shapes1.count(shape2->mBroadPhaseID) == 0 || shapes2.count(shape1->mBroadPhaseID) == 0)) { + continue; + } + if (!shapes1.empty() && shapes2.empty() && + shapes1.count(shape1->mBroadPhaseID) == 0 && shapes1.count(shape2->mBroadPhaseID) == 0) + { + continue; + } + if (!shapes2.empty() && shapes1.empty() && + shapes2.count(shape1->mBroadPhaseID) == 0 && shapes2.count(shape2->mBroadPhaseID) == 0) + { + continue; + } - // For each contact manifold set of the overlapping pair - const ContactManifoldSet& manifoldSet = pair->getContactManifoldSet(); - for (int j=0; jgetContactManifoldSet(); + for (int32_t j=0; jgetNbContactPoints(); i++) { + // For each contact manifold of the manifold set + for (uint32_t i=0; igetNbContactPoints(); i++) { - ContactPoint* contactPoint = manifold->getContactPoint(i); + ContactPoint* contactPoint = manifold->getContactPoint(i); - // Create the contact info object for the contact - ContactPointInfo contactInfo(manifold->getShape1(), manifold->getShape2(), - manifold->getShape1()->getCollisionShape(), - manifold->getShape2()->getCollisionShape(), - contactPoint->getNormal(), - contactPoint->getPenetrationDepth(), - contactPoint->getLocalPointOnBody1(), - contactPoint->getLocalPointOnBody2()); + // Create the contact info object for the contact + ContactPointInfo contactInfo(manifold->getShape1(), manifold->getShape2(), + manifold->getShape1()->getCollisionShape(), + manifold->getShape2()->getCollisionShape(), + contactPoint->getNormal(), + contactPoint->getPenetrationDepth(), + contactPoint->getLocalPointOnBody1(), + contactPoint->getLocalPointOnBody2()); - // Notify the collision callback about this new contact - if (callback != NULL) callback->notifyContact(contactInfo); - } - } - } + // Notify the collision callback about this new contact + if (callback != NULL) callback->notifyContact(contactInfo); + } + } + } } // Compute the broad-phase collision detection void CollisionDetection::computeBroadPhase() { - PROFILE("CollisionDetection::computeBroadPhase()"); + PROFILE("CollisionDetection::computeBroadPhase()"); - // If new collision shapes have been added to bodies - if (mIsCollisionShapesAdded) { + // If new collision shapes have been added to bodies + if (mIsCollisionShapesAdded) { - // Ask the broad-phase to recompute the overlapping pairs of collision - // shapes. This call can only add new overlapping pairs in the collision - // detection. - mBroadPhaseAlgorithm.computeOverlappingPairs(); - } + // Ask the broad-phase to recompute the overlapping pairs of collision + // shapes. This call can only add new overlapping pairs in the collision + // detection. + mBroadPhaseAlgorithm.computeOverlappingPairs(); + } } // Compute the narrow-phase collision detection void CollisionDetection::computeNarrowPhase() { - PROFILE("CollisionDetection::computeNarrowPhase()"); + PROFILE("CollisionDetection::computeNarrowPhase()"); - // Clear the set of overlapping pairs in narrow-phase contact - mContactOverlappingPairs.clear(); - - // For each possible collision pair of bodies - map::iterator it; - for (it = mOverlappingPairs.begin(); it != mOverlappingPairs.end(); ) { + // Clear the set of overlapping pairs in narrow-phase contact + mContactOverlappingPairs.clear(); + + // For each possible collision pair of bodies + map::iterator it; + for (it = mOverlappingPairs.begin(); it != mOverlappingPairs.end(); ) { - OverlappingPair* pair = it->second; + OverlappingPair* pair = it->second; - ProxyShape* shape1 = pair->getShape1(); - ProxyShape* shape2 = pair->getShape2(); + ProxyShape* shape1 = pair->getShape1(); + ProxyShape* shape2 = pair->getShape2(); - assert(shape1->mBroadPhaseID != shape2->mBroadPhaseID); + assert(shape1->mBroadPhaseID != shape2->mBroadPhaseID); - // Check if the collision filtering allows collision between the two shapes and - // that the two shapes are still overlapping. Otherwise, we destroy the - // overlapping pair - if (((shape1->getCollideWithMaskBits() & shape2->getCollisionCategoryBits()) == 0 || - (shape1->getCollisionCategoryBits() & shape2->getCollideWithMaskBits()) == 0) || - !mBroadPhaseAlgorithm.testOverlappingShapes(shape1, shape2)) { + // Check if the collision filtering allows collision between the two shapes and + // that the two shapes are still overlapping. Otherwise, we destroy the + // overlapping pair + if (((shape1->getCollideWithMaskBits() & shape2->getCollisionCategoryBits()) == 0 || + (shape1->getCollisionCategoryBits() & shape2->getCollideWithMaskBits()) == 0) || + !mBroadPhaseAlgorithm.testOverlappingShapes(shape1, shape2)) { - std::map::iterator itToRemove = it; - ++it; + std::map::iterator itToRemove = it; + ++it; - // 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 list of the two bodies involved - // Destroy the overlapping pair - itToRemove->second->~OverlappingPair(); - mWorld->mMemoryAllocator.release(itToRemove->second, sizeof(OverlappingPair)); - mOverlappingPairs.erase(itToRemove); - continue; - } - else { - ++it; - } + // Destroy the overlapping pair + itToRemove->second->~OverlappingPair(); + mWorld->mMemoryAllocator.release(itToRemove->second, sizeof(OverlappingPair)); + mOverlappingPairs.erase(itToRemove); + continue; + } + else { + ++it; + } - CollisionBody* const body1 = shape1->getBody(); - CollisionBody* const body2 = shape2->getBody(); - - // Update the contact cache of the overlapping pair - pair->update(); + CollisionBody* const body1 = shape1->getBody(); + CollisionBody* const body2 = shape2->getBody(); + + // Update the contact cache of the overlapping pair + pair->update(); - // Check that at least one body is awake and not static - bool isBody1Active = !body1->isSleeping() && body1->getType() != STATIC; - bool isBody2Active = !body2->isSleeping() && body2->getType() != STATIC; - if (!isBody1Active && !isBody2Active) continue; + // Check that at least one body is awake and not static + bool isBody1Active = !body1->isSleeping() && body1->getType() != STATIC; + bool isBody2Active = !body2->isSleeping() && body2->getType() != STATIC; + if (!isBody1Active && !isBody2Active) continue; - // Check if the bodies are in the set of bodies that cannot collide between each other - bodyindexpair bodiesIndex = OverlappingPair::computeBodiesIndexPair(body1, body2); - if (mNoCollisionPairs.count(bodiesIndex) > 0) continue; - - // Select the narrow phase algorithm to use according to the two collision shapes - const CollisionShapeType shape1Type = shape1->getCollisionShape()->getType(); - const CollisionShapeType shape2Type = shape2->getCollisionShape()->getType(); - NarrowPhaseAlgorithm* narrowPhaseAlgorithm = mCollisionMatrix[shape1Type][shape2Type]; + // Check if the bodies are in the set of bodies that cannot collide between each other + bodyindexpair bodiesIndex = OverlappingPair::computeBodiesIndexPair(body1, body2); + if (mNoCollisionPairs.count(bodiesIndex) > 0) continue; + + // Select the narrow phase algorithm to use according to the two collision shapes + const CollisionShapeType shape1Type = shape1->getCollisionShape()->getType(); + const CollisionShapeType shape2Type = shape2->getCollisionShape()->getType(); + NarrowPhaseAlgorithm* narrowPhaseAlgorithm = mCollisionMatrix[shape1Type][shape2Type]; - // If there is no collision algorithm between those two kinds of shapes - if (narrowPhaseAlgorithm == NULL) continue; - - // Notify the narrow-phase algorithm about the overlapping pair we are going to test - narrowPhaseAlgorithm->setCurrentOverlappingPair(pair); + // If there is no collision algorithm between those two kinds of shapes + if (narrowPhaseAlgorithm == NULL) continue; + + // Notify the narrow-phase algorithm about the overlapping pair we are going to test + narrowPhaseAlgorithm->setCurrentOverlappingPair(pair); - // Create the CollisionShapeInfo objects - CollisionShapeInfo shape1Info(shape1, shape1->getCollisionShape(), shape1->getLocalToWorldTransform(), - pair, shape1->getCachedCollisionData()); - CollisionShapeInfo shape2Info(shape2, shape2->getCollisionShape(), shape2->getLocalToWorldTransform(), - pair, shape2->getCachedCollisionData()); - - // Use the narrow-phase collision detection algorithm to check - // if there really is a collision. If a collision occurs, the - // notifyContact() callback method will be called. - narrowPhaseAlgorithm->testCollision(shape1Info, shape2Info, this); - } + // Create the CollisionShapeInfo objects + CollisionShapeInfo shape1Info(shape1, shape1->getCollisionShape(), shape1->getLocalToWorldTransform(), + pair, shape1->getCachedCollisionData()); + CollisionShapeInfo shape2Info(shape2, shape2->getCollisionShape(), shape2->getLocalToWorldTransform(), + pair, shape2->getCachedCollisionData()); + + // Use the narrow-phase collision detection algorithm to check + // if there really is a collision. If a collision occurs, the + // notifyContact() callback method will be called. + narrowPhaseAlgorithm->testCollision(shape1Info, shape2Info, this); + } - // Add all the contact manifolds (between colliding bodies) to the bodies - addAllContactManifoldsToBodies(); + // Add all the contact manifolds (between colliding bodies) to the bodies + addAllContactManifoldsToBodies(); } // Compute the narrow-phase collision detection void CollisionDetection::computeNarrowPhaseBetweenShapes(CollisionCallback* callback, - const std::set& shapes1, - const std::set& shapes2) { + const std::set& shapes1, + const std::set& shapes2) { - mContactOverlappingPairs.clear(); + mContactOverlappingPairs.clear(); - // For each possible collision pair of bodies - map::iterator it; - for (it = mOverlappingPairs.begin(); it != mOverlappingPairs.end(); ) { + // For each possible collision pair of bodies + map::iterator it; + for (it = mOverlappingPairs.begin(); it != mOverlappingPairs.end(); ) { - OverlappingPair* pair = it->second; + OverlappingPair* pair = it->second; - ProxyShape* shape1 = pair->getShape1(); - ProxyShape* shape2 = pair->getShape2(); + ProxyShape* shape1 = pair->getShape1(); + ProxyShape* shape2 = pair->getShape2(); - assert(shape1->mBroadPhaseID != shape2->mBroadPhaseID); + assert(shape1->mBroadPhaseID != shape2->mBroadPhaseID); - // If both shapes1 and shapes2 sets are non-empty, we check that - // shape1 is among on set and shape2 is among the other one - if (!shapes1.empty() && !shapes2.empty() && - (shapes1.count(shape1->mBroadPhaseID) == 0 || shapes2.count(shape2->mBroadPhaseID) == 0) && - (shapes1.count(shape2->mBroadPhaseID) == 0 || shapes2.count(shape1->mBroadPhaseID) == 0)) { - ++it; - continue; - } - if (!shapes1.empty() && shapes2.empty() && - shapes1.count(shape1->mBroadPhaseID) == 0 && shapes1.count(shape2->mBroadPhaseID) == 0) - { - ++it; - continue; - } - if (!shapes2.empty() && shapes1.empty() && - shapes2.count(shape1->mBroadPhaseID) == 0 && shapes2.count(shape2->mBroadPhaseID) == 0) - { - ++it; - continue; - } + // If both shapes1 and shapes2 sets are non-empty, we check that + // shape1 is among on set and shape2 is among the other one + if (!shapes1.empty() && !shapes2.empty() && + (shapes1.count(shape1->mBroadPhaseID) == 0 || shapes2.count(shape2->mBroadPhaseID) == 0) && + (shapes1.count(shape2->mBroadPhaseID) == 0 || shapes2.count(shape1->mBroadPhaseID) == 0)) { + ++it; + continue; + } + if (!shapes1.empty() && shapes2.empty() && + shapes1.count(shape1->mBroadPhaseID) == 0 && shapes1.count(shape2->mBroadPhaseID) == 0) + { + ++it; + continue; + } + if (!shapes2.empty() && shapes1.empty() && + shapes2.count(shape1->mBroadPhaseID) == 0 && shapes2.count(shape2->mBroadPhaseID) == 0) + { + ++it; + continue; + } - // Check if the collision filtering allows collision between the two shapes and - // that the two shapes are still overlapping. Otherwise, we destroy the - // overlapping pair - if (((shape1->getCollideWithMaskBits() & shape2->getCollisionCategoryBits()) == 0 || - (shape1->getCollisionCategoryBits() & shape2->getCollideWithMaskBits()) == 0) || - !mBroadPhaseAlgorithm.testOverlappingShapes(shape1, shape2)) { + // Check if the collision filtering allows collision between the two shapes and + // that the two shapes are still overlapping. Otherwise, we destroy the + // overlapping pair + if (((shape1->getCollideWithMaskBits() & shape2->getCollisionCategoryBits()) == 0 || + (shape1->getCollisionCategoryBits() & shape2->getCollideWithMaskBits()) == 0) || + !mBroadPhaseAlgorithm.testOverlappingShapes(shape1, shape2)) { - std::map::iterator itToRemove = it; - ++it; + std::map::iterator itToRemove = it; + ++it; - // 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 list of the two bodies involved - // Destroy the overlapping pair - itToRemove->second->~OverlappingPair(); - mWorld->mMemoryAllocator.release(itToRemove->second, sizeof(OverlappingPair)); - mOverlappingPairs.erase(itToRemove); - continue; - } - else { - ++it; - } + // Destroy the overlapping pair + itToRemove->second->~OverlappingPair(); + mWorld->mMemoryAllocator.release(itToRemove->second, sizeof(OverlappingPair)); + mOverlappingPairs.erase(itToRemove); + continue; + } + else { + ++it; + } - CollisionBody* const body1 = shape1->getBody(); - CollisionBody* const body2 = shape2->getBody(); + CollisionBody* const body1 = shape1->getBody(); + CollisionBody* const body2 = shape2->getBody(); - // Update the contact cache of the overlapping pair - pair->update(); + // Update the contact cache of the overlapping pair + pair->update(); - // Check if the two bodies are allowed to collide, otherwise, we do not test for collision - if (body1->getType() != DYNAMIC && body2->getType() != DYNAMIC) continue; - bodyindexpair bodiesIndex = OverlappingPair::computeBodiesIndexPair(body1, body2); - if (mNoCollisionPairs.count(bodiesIndex) > 0) continue; + // Check if the two bodies are allowed to collide, otherwise, we do not test for collision + if (body1->getType() != DYNAMIC && body2->getType() != DYNAMIC) continue; + bodyindexpair bodiesIndex = OverlappingPair::computeBodiesIndexPair(body1, body2); + if (mNoCollisionPairs.count(bodiesIndex) > 0) continue; - // Check if the two bodies are sleeping, if so, we do no test collision between them - if (body1->isSleeping() && body2->isSleeping()) continue; + // Check if the two bodies are sleeping, if so, we do no test collision between them + if (body1->isSleeping() && body2->isSleeping()) continue; - // Select the narrow phase algorithm to use according to the two collision shapes - const CollisionShapeType shape1Type = shape1->getCollisionShape()->getType(); - const CollisionShapeType shape2Type = shape2->getCollisionShape()->getType(); - NarrowPhaseAlgorithm* narrowPhaseAlgorithm = mCollisionMatrix[shape1Type][shape2Type]; + // Select the narrow phase algorithm to use according to the two collision shapes + const CollisionShapeType shape1Type = shape1->getCollisionShape()->getType(); + const CollisionShapeType shape2Type = shape2->getCollisionShape()->getType(); + NarrowPhaseAlgorithm* narrowPhaseAlgorithm = mCollisionMatrix[shape1Type][shape2Type]; - // If there is no collision algorithm between those two kinds of shapes - if (narrowPhaseAlgorithm == NULL) continue; + // If there is no collision algorithm between those two kinds of shapes + if (narrowPhaseAlgorithm == NULL) continue; - // Notify the narrow-phase algorithm about the overlapping pair we are going to test - narrowPhaseAlgorithm->setCurrentOverlappingPair(pair); + // Notify the narrow-phase algorithm about the overlapping pair we are going to test + narrowPhaseAlgorithm->setCurrentOverlappingPair(pair); - // Create the CollisionShapeInfo objects - CollisionShapeInfo shape1Info(shape1, shape1->getCollisionShape(), shape1->getLocalToWorldTransform(), - pair, shape1->getCachedCollisionData()); - CollisionShapeInfo shape2Info(shape2, shape2->getCollisionShape(), shape2->getLocalToWorldTransform(), - pair, shape2->getCachedCollisionData()); + // Create the CollisionShapeInfo objects + CollisionShapeInfo shape1Info(shape1, shape1->getCollisionShape(), shape1->getLocalToWorldTransform(), + pair, shape1->getCachedCollisionData()); + CollisionShapeInfo shape2Info(shape2, shape2->getCollisionShape(), shape2->getLocalToWorldTransform(), + pair, shape2->getCachedCollisionData()); - TestCollisionBetweenShapesCallback narrowPhaseCallback(callback); + TestCollisionBetweenShapesCallback narrowPhaseCallback(callback); - // Use the narrow-phase collision detection algorithm to check - // if there really is a collision - narrowPhaseAlgorithm->testCollision(shape1Info, shape2Info, &narrowPhaseCallback); - } + // Use the narrow-phase collision detection algorithm to check + // if there really is a collision + narrowPhaseAlgorithm->testCollision(shape1Info, shape2Info, &narrowPhaseCallback); + } - // Add all the contact manifolds (between colliding bodies) to the bodies - addAllContactManifoldsToBodies(); + // Add all the contact manifolds (between colliding bodies) to the bodies + addAllContactManifoldsToBodies(); } // Allow the broadphase to notify the collision detection about an overlapping pair. /// This method is called by the broad-phase collision detection algorithm void CollisionDetection::broadPhaseNotifyOverlappingPair(ProxyShape* shape1, ProxyShape* shape2) { - assert(shape1->mBroadPhaseID != shape2->mBroadPhaseID); + assert(shape1->mBroadPhaseID != shape2->mBroadPhaseID); - // If the two proxy collision shapes are from the same body, skip it - if (shape1->getBody()->getID() == shape2->getBody()->getID()) return; + // If the two proxy collision shapes are from the same body, skip it + if (shape1->getBody()->getID() == shape2->getBody()->getID()) return; - // Check if the collision filtering allows collision between the two shapes - if ((shape1->getCollideWithMaskBits() & shape2->getCollisionCategoryBits()) == 0 || - (shape1->getCollisionCategoryBits() & shape2->getCollideWithMaskBits()) == 0) return; + // Check if the collision filtering allows collision between the two shapes + if ((shape1->getCollideWithMaskBits() & shape2->getCollisionCategoryBits()) == 0 || + (shape1->getCollisionCategoryBits() & shape2->getCollideWithMaskBits()) == 0) return; - // Compute the overlapping pair ID - overlappingpairid pairID = OverlappingPair::computeID(shape1, shape2); + // Compute the overlapping pair ID + overlappingpairid pairID = OverlappingPair::computeID(shape1, shape2); - // Check if the overlapping pair already exists - if (mOverlappingPairs.find(pairID) != mOverlappingPairs.end()) return; + // Check if the overlapping pair already exists + if (mOverlappingPairs.find(pairID) != mOverlappingPairs.end()) return; - // Compute the maximum number of contact manifolds for this pair - int nbMaxManifolds = CollisionShape::computeNbMaxContactManifolds(shape1->getCollisionShape()->getType(), - shape2->getCollisionShape()->getType()); + // Compute the maximum number of contact manifolds for this pair + int32_t nbMaxManifolds = CollisionShape::computeNbMaxContactManifolds(shape1->getCollisionShape()->getType(), + shape2->getCollisionShape()->getType()); - // Create the overlapping pair and add it into the set of overlapping pairs - OverlappingPair* newPair = new (mWorld->mMemoryAllocator.allocate(sizeof(OverlappingPair))) - OverlappingPair(shape1, shape2, nbMaxManifolds, mWorld->mMemoryAllocator); - assert(newPair != NULL); + // Create the overlapping pair and add it int32_to the set of overlapping pairs + OverlappingPair* newPair = new (mWorld->mMemoryAllocator.allocate(sizeof(OverlappingPair))) + OverlappingPair(shape1, shape2, nbMaxManifolds, mWorld->mMemoryAllocator); + assert(newPair != NULL); #ifndef NDEBUG - std::pair::iterator, bool> check = + std::pair::iterator, bool> check = #endif - mOverlappingPairs.insert(make_pair(pairID, newPair)); - assert(check.second); + mOverlappingPairs.insert(make_pair(pairID, newPair)); + assert(check.second); - // Wake up the two bodies - shape1->getBody()->setIsSleeping(false); - shape2->getBody()->setIsSleeping(false); + // Wake up the two bodies + shape1->getBody()->setIsSleeping(false); + shape2->getBody()->setIsSleeping(false); } // Remove a body from the collision detection void CollisionDetection::removeProxyCollisionShape(ProxyShape* proxyShape) { - // Remove all the overlapping pairs involving this proxy shape - std::map::iterator it; - for (it = mOverlappingPairs.begin(); it != mOverlappingPairs.end(); ) { - if (it->second->getShape1()->mBroadPhaseID == proxyShape->mBroadPhaseID|| - it->second->getShape2()->mBroadPhaseID == proxyShape->mBroadPhaseID) { - std::map::iterator itToRemove = it; - ++it; + // Remove all the overlapping pairs involving this proxy shape + std::map::iterator it; + for (it = mOverlappingPairs.begin(); it != mOverlappingPairs.end(); ) { + if (it->second->getShape1()->mBroadPhaseID == proxyShape->mBroadPhaseID|| + it->second->getShape2()->mBroadPhaseID == proxyShape->mBroadPhaseID) { + std::map::iterator itToRemove = it; + ++it; - // 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 list of the two bodies involved - // Destroy the overlapping pair - itToRemove->second->~OverlappingPair(); - mWorld->mMemoryAllocator.release(itToRemove->second, sizeof(OverlappingPair)); - mOverlappingPairs.erase(itToRemove); - } - else { - ++it; - } - } + // Destroy the overlapping pair + itToRemove->second->~OverlappingPair(); + mWorld->mMemoryAllocator.release(itToRemove->second, sizeof(OverlappingPair)); + mOverlappingPairs.erase(itToRemove); + } + else { + ++it; + } + } - // Remove the body from the broad-phase - mBroadPhaseAlgorithm.removeProxyCollisionShape(proxyShape); + // Remove the body from the broad-phase + mBroadPhaseAlgorithm.removeProxyCollisionShape(proxyShape); } // Called by a narrow-phase collision algorithm when a new contact has been found void CollisionDetection::notifyContact(OverlappingPair* overlappingPair, const ContactPointInfo& contactInfo) { - // If it is the first contact since the pairs are overlapping - if (overlappingPair->getNbContactPoints() == 0) { + // If it is the first contact since the pairs are overlapping + if (overlappingPair->getNbContactPoints() == 0) { - // Trigger a callback event - if (mWorld->mEventListener != NULL) mWorld->mEventListener->beginContact(contactInfo); - } + // Trigger a callback event + if (mWorld->mEventListener != NULL) mWorld->mEventListener->beginContact(contactInfo); + } - // Create a new contact - createContact(overlappingPair, contactInfo); + // Create a new contact + createContact(overlappingPair, contactInfo); - // Trigger a callback event for the new contact - if (mWorld->mEventListener != NULL) mWorld->mEventListener->newContact(contactInfo); + // Trigger a callback event for the new contact + if (mWorld->mEventListener != NULL) mWorld->mEventListener->newContact(contactInfo); } // Create a new contact void CollisionDetection::createContact(OverlappingPair* overlappingPair, - const ContactPointInfo& contactInfo) { + const ContactPointInfo& contactInfo) { - // Create a new contact - ContactPoint* contact = new (mWorld->mMemoryAllocator.allocate(sizeof(ContactPoint))) - ContactPoint(contactInfo); + // Create a new contact + ContactPoint* contact = new (mWorld->mMemoryAllocator.allocate(sizeof(ContactPoint))) + ContactPoint(contactInfo); - // Add the contact to the contact manifold set of the corresponding overlapping pair - overlappingPair->addContact(contact); + // Add the contact to the contact manifold set of the corresponding overlapping pair + overlappingPair->addContact(contact); - // Add the overlapping pair into the set of pairs in contact during narrow-phase - overlappingpairid pairId = OverlappingPair::computeID(overlappingPair->getShape1(), - overlappingPair->getShape2()); - mContactOverlappingPairs[pairId] = overlappingPair; + // Add the overlapping pair int32_to the set of pairs in contact during narrow-phase + overlappingpairid pairId = OverlappingPair::computeID(overlappingPair->getShape1(), + overlappingPair->getShape2()); + mContactOverlappingPairs[pairId] = overlappingPair; } void CollisionDetection::addAllContactManifoldsToBodies() { - // For each overlapping pairs in contact during the narrow-phase - std::map::iterator it; - for (it = mContactOverlappingPairs.begin(); it != mContactOverlappingPairs.end(); ++it) { + // For each overlapping pairs in contact during the narrow-phase + std::map::iterator it; + for (it = mContactOverlappingPairs.begin(); it != mContactOverlappingPairs.end(); ++it) { - // Add all the contact manifolds of the pair into the list of contact manifolds - // of the two bodies involved in the contact - addContactManifoldToBody(it->second); - } + // Add all the contact manifolds of the pair int32_to the list of contact manifolds + // of the two bodies involved in the contact + addContactManifoldToBody(it->second); + } } // Add a contact manifold to the linked list of contact manifolds of the two bodies involved // in the corresponding contact void CollisionDetection::addContactManifoldToBody(OverlappingPair* pair) { - assert(pair != NULL); + assert(pair != NULL); - CollisionBody* body1 = pair->getShape1()->getBody(); - CollisionBody* body2 = pair->getShape2()->getBody(); - const ContactManifoldSet& manifoldSet = pair->getContactManifoldSet(); + CollisionBody* body1 = pair->getShape1()->getBody(); + CollisionBody* body2 = pair->getShape2()->getBody(); + const ContactManifoldSet& manifoldSet = pair->getContactManifoldSet(); - // For each contact manifold in the set of manifolds in the pair - for (int i=0; igetNbContactPoints() > 0); + assert(contactManifold->getNbContactPoints() > 0); - // Add the contact manifold at the beginning of the linked - // list of contact manifolds of the first body - void* allocatedMemory1 = mWorld->mMemoryAllocator.allocate(sizeof(ContactManifoldListElement)); - ContactManifoldListElement* listElement1 = new (allocatedMemory1) - ContactManifoldListElement(contactManifold, - body1->mContactManifoldsList); - body1->mContactManifoldsList = listElement1; + // Add the contact manifold at the beginning of the linked + // list of contact manifolds of the first body + void* allocatedMemory1 = mWorld->mMemoryAllocator.allocate(sizeof(ContactManifoldListElement)); + ContactManifoldListElement* listElement1 = new (allocatedMemory1) + ContactManifoldListElement(contactManifold, + body1->mContactManifoldsList); + body1->mContactManifoldsList = listElement1; - // Add the contact manifold at the beginning of the linked - // list of the contact manifolds of the second body - void* allocatedMemory2 = mWorld->mMemoryAllocator.allocate(sizeof(ContactManifoldListElement)); - ContactManifoldListElement* listElement2 = new (allocatedMemory2) - ContactManifoldListElement(contactManifold, - body2->mContactManifoldsList); - body2->mContactManifoldsList = listElement2; - } + // Add the contact manifold at the beginning of the linked + // list of the contact manifolds of the second body + void* allocatedMemory2 = mWorld->mMemoryAllocator.allocate(sizeof(ContactManifoldListElement)); + ContactManifoldListElement* listElement2 = new (allocatedMemory2) + ContactManifoldListElement(contactManifold, + body2->mContactManifoldsList); + body2->mContactManifoldsList = listElement2; + } } // Delete all the contact points in the currently overlapping pairs void CollisionDetection::clearContactPoints() { - // For each overlapping pair - std::map::iterator it; - for (it = mOverlappingPairs.begin(); it != mOverlappingPairs.end(); ++it) { - it->second->clearContactPoints(); - } + // For each overlapping pair + std::map::iterator it; + for (it = mOverlappingPairs.begin(); it != mOverlappingPairs.end(); ++it) { + it->second->clearContactPoints(); + } } // Fill-in the collision detection matrix void CollisionDetection::fillInCollisionMatrix() { - // For each possible type of collision shape - for (int i=0; iselectAlgorithm(i, j); - } - } + // For each possible type of collision shape + for (int32_t i=0; iselectAlgorithm(i, j); + } + } } // Return the world event listener @@ -527,6 +508,6 @@ MemoryAllocator& CollisionDetection::getWorldMemoryAllocator() { // Called by a narrow-phase collision algorithm when a new contact has been found void TestCollisionBetweenShapesCallback::notifyContact(OverlappingPair* overlappingPair, - const ContactPointInfo& contactInfo) { - mCollisionCallback->notifyContact(contactInfo); + const ContactPointInfo& contactInfo) { + mCollisionCallback->notifyContact(contactInfo); } diff --git a/ephysics/collision/CollisionDetection.h b/ephysics/collision/CollisionDetection.h index 09bbab0..1156663 100644 --- a/ephysics/collision/CollisionDetection.h +++ b/ephysics/collision/CollisionDetection.h @@ -1,30 +1,9 @@ -/******************************************************************************** -* 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 REACTPHYSICS3D_COLLISION_DETECTION_H -#define REACTPHYSICS3D_COLLISION_DETECTION_H +/** @file + * @author Daniel Chappuis + * @copyright 2010-2016 Daniel Chappuis + * @license BSD 3 clauses (see license file) + */ +#pragma once // Libraries #include @@ -50,21 +29,21 @@ class CollisionCallback; // Class TestCollisionBetweenShapesCallback class TestCollisionBetweenShapesCallback : public NarrowPhaseCallback { - private: + private: - CollisionCallback* mCollisionCallback; + CollisionCallback* mCollisionCallback; - public: + public: - // Constructor - TestCollisionBetweenShapesCallback(CollisionCallback* callback) - : mCollisionCallback(callback) { + // Constructor + TestCollisionBetweenShapesCallback(CollisionCallback* callback) + : mCollisionCallback(callback) { - } + } - // Called by a narrow-phase collision algorithm when a new contact has been found - virtual void notifyContact(OverlappingPair* overlappingPair, - const ContactPointInfo& contactInfo); + // Called by a narrow-phase collision algorithm when a new contact has been found + virtual void notifyContact(OverlappingPair* overlappingPair, + const ContactPointInfo& contactInfo); }; // Class CollisionDetection @@ -76,243 +55,241 @@ class TestCollisionBetweenShapesCallback : public NarrowPhaseCallback { */ class CollisionDetection : public NarrowPhaseCallback { - private : + private : - // -------------------- Attributes -------------------- // + // -------------------- Attributes -------------------- // - /// Collision Detection Dispatch configuration - CollisionDispatch* mCollisionDispatch; + /// Collision Detection Dispatch configuration + CollisionDispatch* mCollisionDispatch; - /// Default collision dispatch configuration - DefaultCollisionDispatch mDefaultCollisionDispatch; + /// Default collision dispatch configuration + DefaultCollisionDispatch mDefaultCollisionDispatch; - /// Collision detection matrix (algorithms to use) - NarrowPhaseAlgorithm* mCollisionMatrix[NB_COLLISION_SHAPE_TYPES][NB_COLLISION_SHAPE_TYPES]; + /// Collision detection matrix (algorithms to use) + NarrowPhaseAlgorithm* mCollisionMatrix[NB_COLLISION_SHAPE_TYPES][NB_COLLISION_SHAPE_TYPES]; - /// Reference to the memory allocator - MemoryAllocator& mMemoryAllocator; + /// Reference to the memory allocator + MemoryAllocator& mMemoryAllocator; - /// Pointer to the physics world - CollisionWorld* mWorld; + /// Pointer to the physics world + CollisionWorld* mWorld; - /// Broad-phase overlapping pairs - std::map mOverlappingPairs; + /// Broad-phase overlapping pairs + std::map mOverlappingPairs; - /// Overlapping pairs in contact (during the current Narrow-phase collision detection) - std::map mContactOverlappingPairs; + /// Overlapping pairs in contact (during the current Narrow-phase collision detection) + std::map mContactOverlappingPairs; - /// Broad-phase algorithm - BroadPhaseAlgorithm mBroadPhaseAlgorithm; + /// Broad-phase algorithm + BroadPhaseAlgorithm mBroadPhaseAlgorithm; - /// Narrow-phase GJK algorithm - // TODO : Delete this - GJKAlgorithm mNarrowPhaseGJKAlgorithm; + /// Narrow-phase GJK algorithm + // TODO : Delete this + GJKAlgorithm mNarrowPhaseGJKAlgorithm; - /// Set of pair of bodies that cannot collide between each other - std::set mNoCollisionPairs; + /// Set of pair of bodies that cannot collide between each other + std::set mNoCollisionPairs; - /// True if some collision shapes have been added previously - bool mIsCollisionShapesAdded; + /// True if some collision shapes have been added previously + bool mIsCollisionShapesAdded; - // -------------------- Methods -------------------- // + // -------------------- Methods -------------------- // - /// Private copy-constructor - CollisionDetection(const CollisionDetection& collisionDetection); + /// Private copy-constructor + CollisionDetection(const CollisionDetection& collisionDetection); - /// Private assignment operator - CollisionDetection& operator=(const CollisionDetection& collisionDetection); + /// Private assignment operator + CollisionDetection& operator=(const CollisionDetection& collisionDetection); - /// Compute the broad-phase collision detection - void computeBroadPhase(); + /// Compute the broad-phase collision detection + void computeBroadPhase(); - /// Compute the narrow-phase collision detection - void computeNarrowPhase(); + /// Compute the narrow-phase collision detection + void computeNarrowPhase(); - /// Add a contact manifold to the linked list of contact manifolds of the two bodies - /// involed in the corresponding contact. - void addContactManifoldToBody(OverlappingPair* pair); + /// Add a contact manifold to the linked list of contact manifolds of the two bodies + /// involed in the corresponding contact. + void addContactManifoldToBody(OverlappingPair* pair); - /// Delete all the contact points in the currently overlapping pairs - void clearContactPoints(); + /// Delete all the contact points in the currently overlapping pairs + void clearContactPoints(); - /// Fill-in the collision detection matrix - void fillInCollisionMatrix(); + /// Fill-in the collision detection matrix + void fillInCollisionMatrix(); - /// Add all the contact manifold of colliding pairs to their bodies - void addAllContactManifoldsToBodies(); + /// Add all the contact manifold of colliding pairs to their bodies + void addAllContactManifoldsToBodies(); - public : + public : - // -------------------- Methods -------------------- // + // -------------------- Methods -------------------- // - /// Constructor - CollisionDetection(CollisionWorld* world, MemoryAllocator& memoryAllocator); + /// Constructor + CollisionDetection(CollisionWorld* world, MemoryAllocator& memoryAllocator); - /// Destructor - ~CollisionDetection(); + /// Destructor + ~CollisionDetection(); - /// Set the collision dispatch configuration - void setCollisionDispatch(CollisionDispatch* collisionDispatch); + /// Set the collision dispatch configuration + void setCollisionDispatch(CollisionDispatch* collisionDispatch); - /// Return the Narrow-phase collision detection algorithm to use between two types of shapes - NarrowPhaseAlgorithm* getCollisionAlgorithm(CollisionShapeType shape1Type, - CollisionShapeType shape2Type) const; + /// Return the Narrow-phase collision detection algorithm to use between two types of shapes + NarrowPhaseAlgorithm* getCollisionAlgorithm(CollisionShapeType shape1Type, + CollisionShapeType shape2Type) const; - /// Add a proxy collision shape to the collision detection - void addProxyCollisionShape(ProxyShape* proxyShape, const AABB& aabb); + /// Add a proxy collision shape to the collision detection + void addProxyCollisionShape(ProxyShape* proxyShape, const AABB& aabb); - /// Remove a proxy collision shape from the collision detection - void removeProxyCollisionShape(ProxyShape* proxyShape); + /// Remove a proxy collision shape from the collision detection + void removeProxyCollisionShape(ProxyShape* proxyShape); - /// Update a proxy collision shape (that has moved for instance) - void updateProxyCollisionShape(ProxyShape* shape, const AABB& aabb, - const Vector3& displacement = Vector3(0, 0, 0), bool forceReinsert = false); + /// Update a proxy collision shape (that has moved for instance) + void updateProxyCollisionShape(ProxyShape* shape, const AABB& aabb, + const Vector3& displacement = Vector3(0, 0, 0), bool forceReinsert = false); - /// Add a pair of bodies that cannot collide with each other - void addNoCollisionPair(CollisionBody* body1, CollisionBody* body2); + /// Add a pair of bodies that cannot collide with each other + void addNoCollisionPair(CollisionBody* body1, CollisionBody* body2); - /// Remove a pair of bodies that cannot collide with each other - void removeNoCollisionPair(CollisionBody* body1, CollisionBody* body2); + /// Remove a pair of bodies that cannot collide with each other + void removeNoCollisionPair(CollisionBody* body1, CollisionBody* body2); - /// Ask for a collision shape to be tested again during broad-phase. - void askForBroadPhaseCollisionCheck(ProxyShape* shape); + /// Ask for a collision shape to be tested again during broad-phase. + void askForBroadPhaseCollisionCheck(ProxyShape* shape); - /// Compute the collision detection - void computeCollisionDetection(); + /// Compute the collision detection + void computeCollisionDetection(); - /// Compute the collision detection - void testCollisionBetweenShapes(CollisionCallback* callback, - const std::set& shapes1, - const std::set& shapes2); + /// Compute the collision detection + void testCollisionBetweenShapes(CollisionCallback* callback, + const std::set& shapes1, + const std::set& shapes2); - /// Report collision between two sets of shapes - void reportCollisionBetweenShapes(CollisionCallback* callback, - const std::set& shapes1, - const std::set& shapes2) ; + /// Report collision between two sets of shapes + void reportCollisionBetweenShapes(CollisionCallback* callback, + const std::set& shapes1, + const std::set& shapes2) ; - /// Ray casting method - void raycast(RaycastCallback* raycastCallback, const Ray& ray, - unsigned short raycastWithCategoryMaskBits) const; + /// Ray casting method + void raycast(RaycastCallback* raycastCallback, const Ray& ray, + unsigned short raycastWithCategoryMaskBits) const; - /// Test if the AABBs of two bodies overlap - bool testAABBOverlap(const CollisionBody* body1, - const CollisionBody* body2) const; + /// Test if the AABBs of two bodies overlap + bool testAABBOverlap(const CollisionBody* body1, + const CollisionBody* body2) const; - /// Test if the AABBs of two proxy shapes overlap - bool testAABBOverlap(const ProxyShape* shape1, - const ProxyShape* shape2) const; + /// Test if the AABBs of two proxy shapes overlap + bool testAABBOverlap(const ProxyShape* shape1, + const ProxyShape* shape2) const; - /// Allow the broadphase to notify the collision detection about an overlapping pair. - void broadPhaseNotifyOverlappingPair(ProxyShape* shape1, ProxyShape* shape2); + /// Allow the broadphase to notify the collision detection about an overlapping pair. + void broadPhaseNotifyOverlappingPair(ProxyShape* shape1, ProxyShape* shape2); - /// Compute the narrow-phase collision detection - void computeNarrowPhaseBetweenShapes(CollisionCallback* callback, - const std::set& shapes1, - const std::set& shapes2); + /// Compute the narrow-phase collision detection + void computeNarrowPhaseBetweenShapes(CollisionCallback* callback, + const std::set& shapes1, + const std::set& shapes2); - /// Return a pointer to the world - CollisionWorld* getWorld(); + /// Return a pointer to the world + CollisionWorld* getWorld(); - /// Return the world event listener - EventListener* getWorldEventListener(); + /// Return the world event listener + EventListener* getWorldEventListener(); - /// Return a reference to the world memory allocator - MemoryAllocator& getWorldMemoryAllocator(); + /// Return a reference to the world memory allocator + MemoryAllocator& getWorldMemoryAllocator(); - /// Called by a narrow-phase collision algorithm when a new contact has been found - virtual void notifyContact(OverlappingPair* overlappingPair, const ContactPointInfo& contactInfo); + /// Called by a narrow-phase collision algorithm when a new contact has been found + virtual void notifyContact(OverlappingPair* overlappingPair, const ContactPointInfo& contactInfo); - /// Create a new contact - void createContact(OverlappingPair* overlappingPair, const ContactPointInfo& contactInfo); + /// Create a new contact + void createContact(OverlappingPair* overlappingPair, const ContactPointInfo& contactInfo); - // -------------------- Friendship -------------------- // + // -------------------- Friendship -------------------- // - friend class DynamicsWorld; - friend class ConvexMeshShape; + friend class DynamicsWorld; + friend class ConvexMeshShape; }; // Return the Narrow-phase collision detection algorithm to use between two types of shapes inline NarrowPhaseAlgorithm* CollisionDetection::getCollisionAlgorithm(CollisionShapeType shape1Type, - CollisionShapeType shape2Type) const { - return mCollisionMatrix[shape1Type][shape2Type]; + CollisionShapeType shape2Type) const { + return mCollisionMatrix[shape1Type][shape2Type]; } // Set the collision dispatch configuration inline void CollisionDetection::setCollisionDispatch(CollisionDispatch* collisionDispatch) { - mCollisionDispatch = collisionDispatch; + mCollisionDispatch = collisionDispatch; - mCollisionDispatch->init(this, &mMemoryAllocator); + mCollisionDispatch->init(this, &mMemoryAllocator); - // Fill-in the collision matrix with the new algorithms to use - fillInCollisionMatrix(); + // Fill-in the collision matrix with the new algorithms to use + fillInCollisionMatrix(); } // Add a body to the collision detection inline void CollisionDetection::addProxyCollisionShape(ProxyShape* proxyShape, - const AABB& aabb) { - - // Add the body to the broad-phase - mBroadPhaseAlgorithm.addProxyCollisionShape(proxyShape, aabb); + const AABB& aabb) { + + // Add the body to the broad-phase + mBroadPhaseAlgorithm.addProxyCollisionShape(proxyShape, aabb); - mIsCollisionShapesAdded = true; + mIsCollisionShapesAdded = true; } // Add a pair of bodies that cannot collide with each other inline void CollisionDetection::addNoCollisionPair(CollisionBody* body1, - CollisionBody* body2) { - mNoCollisionPairs.insert(OverlappingPair::computeBodiesIndexPair(body1, body2)); + CollisionBody* body2) { + mNoCollisionPairs.insert(OverlappingPair::computeBodiesIndexPair(body1, body2)); } // Remove a pair of bodies that cannot collide with each other inline void CollisionDetection::removeNoCollisionPair(CollisionBody* body1, - CollisionBody* body2) { - mNoCollisionPairs.erase(OverlappingPair::computeBodiesIndexPair(body1, body2)); + CollisionBody* body2) { + mNoCollisionPairs.erase(OverlappingPair::computeBodiesIndexPair(body1, body2)); } // 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 /// previous frame so that it is tested for collision again in the broad-phase. inline void CollisionDetection::askForBroadPhaseCollisionCheck(ProxyShape* shape) { - mBroadPhaseAlgorithm.addMovedCollisionShape(shape->mBroadPhaseID); + mBroadPhaseAlgorithm.addMovedCollisionShape(shape->mBroadPhaseID); } // Update a proxy collision shape (that has moved for instance) inline void CollisionDetection::updateProxyCollisionShape(ProxyShape* shape, const AABB& aabb, - const Vector3& displacement, bool forceReinsert) { - mBroadPhaseAlgorithm.updateProxyCollisionShape(shape, aabb, displacement); + const Vector3& displacement, bool forceReinsert) { + mBroadPhaseAlgorithm.updateProxyCollisionShape(shape, aabb, displacement); } // Ray casting method inline void CollisionDetection::raycast(RaycastCallback* raycastCallback, - const Ray& ray, - unsigned short raycastWithCategoryMaskBits) const { + const Ray& ray, + unsigned short raycastWithCategoryMaskBits) const { - PROFILE("CollisionDetection::raycast()"); + PROFILE("CollisionDetection::raycast()"); - RaycastTest rayCastTest(raycastCallback); + RaycastTest rayCastTest(raycastCallback); - // Ask the broad-phase algorithm to call the testRaycastAgainstShape() - // callback method for each proxy shape hit by the ray in the broad-phase - mBroadPhaseAlgorithm.raycast(ray, rayCastTest, raycastWithCategoryMaskBits); + // Ask the broad-phase algorithm to call the testRaycastAgainstShape() + // callback method for each proxy shape hit by the ray in the broad-phase + mBroadPhaseAlgorithm.raycast(ray, rayCastTest, raycastWithCategoryMaskBits); } // Test if the AABBs of two proxy shapes overlap inline bool CollisionDetection::testAABBOverlap(const ProxyShape* shape1, - const ProxyShape* shape2) const { + const ProxyShape* shape2) const { - // If one of the shape's body is not active, we return no overlap - if (!shape1->getBody()->isActive() || !shape2->getBody()->isActive()) { - return false; - } + // If one of the shape's body is not active, we return no overlap + if (!shape1->getBody()->isActive() || !shape2->getBody()->isActive()) { + return false; + } - return mBroadPhaseAlgorithm.testOverlappingShapes(shape1, shape2); + return mBroadPhaseAlgorithm.testOverlappingShapes(shape1, shape2); } // Return a pointer to the world inline CollisionWorld* CollisionDetection::getWorld() { - return mWorld; + return mWorld; } } - -#endif diff --git a/ephysics/collision/CollisionShapeInfo.h b/ephysics/collision/CollisionShapeInfo.h index 69081a1..e149510 100644 --- a/ephysics/collision/CollisionShapeInfo.h +++ b/ephysics/collision/CollisionShapeInfo.h @@ -1,30 +1,9 @@ -/******************************************************************************** -* 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 REACTPHYSICS3D_COLLISION_SHAPE_INFO_H -#define REACTPHYSICS3D_COLLISION_SHAPE_INFO_H +/** @file + * @author Daniel Chappuis + * @copyright 2010-2016 Daniel Chappuis + * @license BSD 3 clauses (see license file) + */ +#pragma once // Libraries #include @@ -41,35 +20,34 @@ class OverlappingPair; */ struct CollisionShapeInfo { - public: + public: - /// Broadphase overlapping pair - OverlappingPair* overlappingPair; + /// Broadphase overlapping pair + OverlappingPair* overlappingPair; - /// Proxy shape - ProxyShape* proxyShape; + /// Proxy shape + ProxyShape* proxyShape; - /// Pointer to the collision shape - const CollisionShape* collisionShape; + /// Pointer to the collision shape + const CollisionShape* collisionShape; - /// Transform that maps from collision shape local-space to world-space - Transform shapeToWorldTransform; + /// Transform that maps from collision shape local-space to world-space + Transform shapeToWorldTransform; - /// Cached collision data of the proxy shape - void** cachedCollisionData; + /// Cached collision data of the proxy shape + void** cachedCollisionData; - /// Constructor - CollisionShapeInfo(ProxyShape* proxyCollisionShape, const CollisionShape* shape, - const Transform& shapeLocalToWorldTransform, OverlappingPair* pair, - void** cachedData) - : overlappingPair(pair), proxyShape(proxyCollisionShape), collisionShape(shape), - shapeToWorldTransform(shapeLocalToWorldTransform), - cachedCollisionData(cachedData) { + /// Constructor + CollisionShapeInfo(ProxyShape* proxyCollisionShape, const CollisionShape* shape, + const Transform& shapeLocalToWorldTransform, OverlappingPair* pair, + void** cachedData) + : overlappingPair(pair), proxyShape(proxyCollisionShape), collisionShape(shape), + shapeToWorldTransform(shapeLocalToWorldTransform), + cachedCollisionData(cachedData) { - } + } }; } -#endif diff --git a/ephysics/collision/ContactManifold.cpp b/ephysics/collision/ContactManifold.cpp index 7df4fe6..ae6a68a 100644 --- a/ephysics/collision/ContactManifold.cpp +++ b/ephysics/collision/ContactManifold.cpp @@ -1,27 +1,9 @@ -/******************************************************************************** -* 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. * -* * -********************************************************************************/ +/** @file + * @author Daniel Chappuis + * @copyright 2010-2016 Daniel Chappuis + * @license BSD 3 clauses (see license file) + */ + // Libraries #include @@ -31,71 +13,71 @@ using namespace reactphysics3d; // Constructor ContactManifold::ContactManifold(ProxyShape* shape1, ProxyShape* shape2, - MemoryAllocator& memoryAllocator, short normalDirectionId) - : mShape1(shape1), mShape2(shape2), mNormalDirectionId(normalDirectionId), - mNbContactPoints(0), mFrictionImpulse1(0.0), mFrictionImpulse2(0.0), - mFrictionTwistImpulse(0.0), mIsAlreadyInIsland(false), - mMemoryAllocator(memoryAllocator) { - + MemoryAllocator& memoryAllocator, short normalDirectionId) + : mShape1(shape1), mShape2(shape2), mNormalDirectionId(normalDirectionId), + mNbContactPoints(0), mFrictionImpulse1(0.0), mFrictionImpulse2(0.0), + mFrictionTwistImpulse(0.0), mIsAlreadyInIsland(false), + mMemoryAllocator(memoryAllocator) { + } // Destructor ContactManifold::~ContactManifold() { - clear(); + clear(); } // Add a contact point in the manifold void ContactManifold::addContactPoint(ContactPoint* contact) { - // For contact already in the manifold - for (uint i=0; igetWorldPointOnBody1() - - contact->getWorldPointOnBody1()).lengthSquare(); - if (distance <= PERSISTENT_CONTACT_DIST_THRESHOLD*PERSISTENT_CONTACT_DIST_THRESHOLD) { + // already in the manifold. + float distance = (mContactPoints[i]->getWorldPointOnBody1() - + contact->getWorldPointOnBody1()).lengthSquare(); + if (distance <= PERSISTENT_CONTACT_DIST_THRESHOLD*PERSISTENT_CONTACT_DIST_THRESHOLD) { - // Delete the new contact - contact->~ContactPoint(); - mMemoryAllocator.release(contact, sizeof(ContactPoint)); + // Delete the new contact + contact->~ContactPoint(); + mMemoryAllocator.release(contact, sizeof(ContactPoint)); - assert(mNbContactPoints > 0); + assert(mNbContactPoints > 0); - return; + return; } } - - // If the contact manifold is full - if (mNbContactPoints == MAX_CONTACT_POINTS_IN_MANIFOLD) { - int indexMaxPenetration = getIndexOfDeepestPenetration(contact); - int indexToRemove = getIndexToRemove(indexMaxPenetration, contact->getLocalPointOnBody1()); - removeContactPoint(indexToRemove); - } + + // If the contact manifold is full + if (mNbContactPoints == MAX_CONTACT_POINTS_IN_MANIFOLD) { + int32_t indexMaxPenetration = getIndexOfDeepestPenetration(contact); + int32_t indexToRemove = getIndexToRemove(indexMaxPenetration, contact->getLocalPointOnBody1()); + removeContactPoint(indexToRemove); + } - // Add the new contact point in the manifold - mContactPoints[mNbContactPoints] = contact; - mNbContactPoints++; + // Add the new contact point in the manifold + mContactPoints[mNbContactPoints] = contact; + mNbContactPoints++; - assert(mNbContactPoints > 0); + assert(mNbContactPoints > 0); } // Remove a contact point from the manifold -void ContactManifold::removeContactPoint(uint index) { - assert(index < mNbContactPoints); - assert(mNbContactPoints > 0); +void ContactManifold::removeContactPoint(uint32_t index) { + assert(index < mNbContactPoints); + assert(mNbContactPoints > 0); - // Call the destructor explicitly and tell the memory allocator that + // Call the destructor explicitly and tell the memory allocator that // the corresponding memory block is now free - mContactPoints[index]->~ContactPoint(); - mMemoryAllocator.release(mContactPoints[index], sizeof(ContactPoint)); + mContactPoints[index]->~ContactPoint(); + mMemoryAllocator.release(mContactPoints[index], sizeof(ContactPoint)); - // If we don't remove the last index - if (index < mNbContactPoints - 1) { - mContactPoints[index] = mContactPoints[mNbContactPoints - 1]; - } + // If we don't remove the last index + if (index < mNbContactPoints - 1) { + mContactPoints[index] = mContactPoints[mNbContactPoints - 1]; + } - mNbContactPoints--; + mNbContactPoints--; } // Update the contact manifold @@ -106,68 +88,68 @@ void ContactManifold::removeContactPoint(uint index) { /// contact normal. void ContactManifold::update(const Transform& transform1, const Transform& transform2) { - if (mNbContactPoints == 0) return; + if (mNbContactPoints == 0) return; - // Update the world coordinates and penetration depth of the contact points in the manifold - for (uint i=0; isetWorldPointOnBody1(transform1 * - mContactPoints[i]->getLocalPointOnBody1()); - mContactPoints[i]->setWorldPointOnBody2(transform2 * - mContactPoints[i]->getLocalPointOnBody2()); - mContactPoints[i]->setPenetrationDepth((mContactPoints[i]->getWorldPointOnBody1() - - mContactPoints[i]->getWorldPointOnBody2()).dot(mContactPoints[i]->getNormal())); - } + // Update the world coordinates and penetration depth of the contact points in the manifold + for (uint32_t i=0; isetWorldPointOnBody1(transform1 * + mContactPoints[i]->getLocalPointOnBody1()); + mContactPoints[i]->setWorldPointOnBody2(transform2 * + mContactPoints[i]->getLocalPointOnBody2()); + mContactPoints[i]->setPenetrationDepth((mContactPoints[i]->getWorldPointOnBody1() - + mContactPoints[i]->getWorldPointOnBody2()).dot(mContactPoints[i]->getNormal())); + } - const decimal squarePersistentContactThreshold = PERSISTENT_CONTACT_DIST_THRESHOLD * - PERSISTENT_CONTACT_DIST_THRESHOLD; + const float squarePersistentContactThreshold = PERSISTENT_CONTACT_DIST_THRESHOLD * + PERSISTENT_CONTACT_DIST_THRESHOLD; - // Remove the contact points that don't represent very well the contact manifold - for (int i=static_cast(mNbContactPoints)-1; i>=0; i--) { - assert(i < static_cast(mNbContactPoints)); + // Remove the contact points that don't represent very well the contact manifold + for (int32_t i=static_cast(mNbContactPoints)-1; i>=0; i--) { + assert(i < static_cast(mNbContactPoints)); - // Compute the distance between contact points in the normal direction - decimal distanceNormal = -mContactPoints[i]->getPenetrationDepth(); - - // If the contacts points are too far from each other in the normal direction - if (distanceNormal > squarePersistentContactThreshold) { - removeContactPoint(i); - } - else { - // Compute the distance of the two contact points in the plane - // orthogonal to the contact normal - Vector3 projOfPoint1 = mContactPoints[i]->getWorldPointOnBody1() + - mContactPoints[i]->getNormal() * distanceNormal; - Vector3 projDifference = mContactPoints[i]->getWorldPointOnBody2() - projOfPoint1; + // Compute the distance between contact points in the normal direction + float distanceNormal = -mContactPoints[i]->getPenetrationDepth(); + + // If the contacts points are too far from each other in the normal direction + if (distanceNormal > squarePersistentContactThreshold) { + removeContactPoint(i); + } + else { + // Compute the distance of the two contact points in the plane + // orthogonal to the contact normal + Vector3 projOfPoint1 = mContactPoints[i]->getWorldPointOnBody1() + + mContactPoints[i]->getNormal() * distanceNormal; + Vector3 projDifference = mContactPoints[i]->getWorldPointOnBody2() - projOfPoint1; - // If the orthogonal distance is larger than the valid distance - // threshold, we remove the contact - if (projDifference.lengthSquare() > squarePersistentContactThreshold) { - removeContactPoint(i); - } - } - } + // If the orthogonal distance is larger than the valid distance + // threshold, we remove the contact + if (projDifference.lengthSquare() > squarePersistentContactThreshold) { + removeContactPoint(i); + } + } + } } // Return the index of the contact point with the larger penetration depth. /// This corresponding contact will be kept in the cache. The method returns -1 is /// the new contact is the deepest. -int ContactManifold::getIndexOfDeepestPenetration(ContactPoint* newContact) const { - assert(mNbContactPoints == MAX_CONTACT_POINTS_IN_MANIFOLD); - int indexMaxPenetrationDepth = -1; - decimal maxPenetrationDepth = newContact->getPenetrationDepth(); +int32_t ContactManifold::getIndexOfDeepestPenetration(ContactPoint* newContact) const { + assert(mNbContactPoints == MAX_CONTACT_POINTS_IN_MANIFOLD); + int32_t indexMaxPenetrationDepth = -1; + float maxPenetrationDepth = newContact->getPenetrationDepth(); - // For each contact in the cache - for (uint i=0; igetPenetrationDepth() > maxPenetrationDepth) { - maxPenetrationDepth = mContactPoints[i]->getPenetrationDepth(); - indexMaxPenetrationDepth = i; - } - } + // If the current contact has a larger penetration depth + if (mContactPoints[i]->getPenetrationDepth() > maxPenetrationDepth) { + maxPenetrationDepth = mContactPoints[i]->getPenetrationDepth(); + indexMaxPenetrationDepth = i; + } + } - // Return the index of largest penetration depth - return indexMaxPenetrationDepth; + // Return the index of largest penetration depth + return indexMaxPenetrationDepth; } // Return the index that will be removed. @@ -180,84 +162,84 @@ int ContactManifold::getIndexOfDeepestPenetration(ContactPoint* newContact) cons /// only estimate it because we do not compute the actual diagonals of the quadrialteral. Therefore, /// this is only a guess that is faster to compute. This idea comes from the Bullet Physics library /// by Erwin Coumans (http://wwww.bulletphysics.org). -int ContactManifold::getIndexToRemove(int indexMaxPenetration, const Vector3& newPoint) const { +int32_t ContactManifold::getIndexToRemove(int32_t indexMaxPenetration, const Vector3& newPoint) const { - assert(mNbContactPoints == MAX_CONTACT_POINTS_IN_MANIFOLD); + assert(mNbContactPoints == MAX_CONTACT_POINTS_IN_MANIFOLD); - decimal area0 = 0.0; // Area with contact 1,2,3 and newPoint - decimal area1 = 0.0; // Area with contact 0,2,3 and newPoint - decimal area2 = 0.0; // Area with contact 0,1,3 and newPoint - decimal area3 = 0.0; // Area with contact 0,1,2 and newPoint + float area0 = 0.0; // Area with contact 1,2,3 and newPoint + float area1 = 0.0; // Area with contact 0,2,3 and newPoint + float area2 = 0.0; // Area with contact 0,1,3 and newPoint + float area3 = 0.0; // Area with contact 0,1,2 and newPoint - if (indexMaxPenetration != 0) { - // Compute the area - Vector3 vector1 = newPoint - mContactPoints[1]->getLocalPointOnBody1(); - Vector3 vector2 = mContactPoints[3]->getLocalPointOnBody1() - - mContactPoints[2]->getLocalPointOnBody1(); - Vector3 crossProduct = vector1.cross(vector2); - area0 = crossProduct.lengthSquare(); - } - if (indexMaxPenetration != 1) { - // Compute the area - Vector3 vector1 = newPoint - mContactPoints[0]->getLocalPointOnBody1(); - Vector3 vector2 = mContactPoints[3]->getLocalPointOnBody1() - - mContactPoints[2]->getLocalPointOnBody1(); - Vector3 crossProduct = vector1.cross(vector2); - area1 = crossProduct.lengthSquare(); - } - if (indexMaxPenetration != 2) { - // Compute the area - Vector3 vector1 = newPoint - mContactPoints[0]->getLocalPointOnBody1(); - Vector3 vector2 = mContactPoints[3]->getLocalPointOnBody1() - - mContactPoints[1]->getLocalPointOnBody1(); - Vector3 crossProduct = vector1.cross(vector2); - area2 = crossProduct.lengthSquare(); - } - if (indexMaxPenetration != 3) { - // Compute the area - Vector3 vector1 = newPoint - mContactPoints[0]->getLocalPointOnBody1(); - Vector3 vector2 = mContactPoints[2]->getLocalPointOnBody1() - - mContactPoints[1]->getLocalPointOnBody1(); - Vector3 crossProduct = vector1.cross(vector2); - area3 = crossProduct.lengthSquare(); - } - - // Return the index of the contact to remove - return getMaxArea(area0, area1, area2, area3); + if (indexMaxPenetration != 0) { + // Compute the area + Vector3 vector1 = newPoint - mContactPoints[1]->getLocalPointOnBody1(); + Vector3 vector2 = mContactPoints[3]->getLocalPointOnBody1() - + mContactPoints[2]->getLocalPointOnBody1(); + Vector3 crossProduct = vector1.cross(vector2); + area0 = crossProduct.lengthSquare(); + } + if (indexMaxPenetration != 1) { + // Compute the area + Vector3 vector1 = newPoint - mContactPoints[0]->getLocalPointOnBody1(); + Vector3 vector2 = mContactPoints[3]->getLocalPointOnBody1() - + mContactPoints[2]->getLocalPointOnBody1(); + Vector3 crossProduct = vector1.cross(vector2); + area1 = crossProduct.lengthSquare(); + } + if (indexMaxPenetration != 2) { + // Compute the area + Vector3 vector1 = newPoint - mContactPoints[0]->getLocalPointOnBody1(); + Vector3 vector2 = mContactPoints[3]->getLocalPointOnBody1() - + mContactPoints[1]->getLocalPointOnBody1(); + Vector3 crossProduct = vector1.cross(vector2); + area2 = crossProduct.lengthSquare(); + } + if (indexMaxPenetration != 3) { + // Compute the area + Vector3 vector1 = newPoint - mContactPoints[0]->getLocalPointOnBody1(); + Vector3 vector2 = mContactPoints[2]->getLocalPointOnBody1() - + mContactPoints[1]->getLocalPointOnBody1(); + Vector3 crossProduct = vector1.cross(vector2); + area3 = crossProduct.lengthSquare(); + } + + // Return the index of the contact to remove + return getMaxArea(area0, area1, area2, area3); } // Return the index of maximum area -int ContactManifold::getMaxArea(decimal area0, decimal area1, decimal area2, decimal area3) const { - if (area0 < area1) { - if (area1 < area2) { - if (area2 < area3) return 3; - else return 2; - } - else { - if (area1 < area3) return 3; - else return 1; - } - } - else { - if (area0 < area2) { - if (area2 < area3) return 3; - else return 2; - } - else { - if (area0 < area3) return 3; - else return 0; - } - } +int32_t ContactManifold::getMaxArea(float area0, float area1, float area2, float area3) const { + if (area0 < area1) { + if (area1 < area2) { + if (area2 < area3) return 3; + else return 2; + } + else { + if (area1 < area3) return 3; + else return 1; + } + } + else { + if (area0 < area2) { + if (area2 < area3) return 3; + else return 2; + } + else { + if (area0 < area3) return 3; + else return 0; + } + } } // Clear the contact manifold void ContactManifold::clear() { - for (uint i=0; i~ContactPoint(); - mMemoryAllocator.release(mContactPoints[i], sizeof(ContactPoint)); - } - mNbContactPoints = 0; + mContactPoints[i]->~ContactPoint(); + mMemoryAllocator.release(mContactPoints[i], sizeof(ContactPoint)); + } + mNbContactPoints = 0; } diff --git a/ephysics/collision/ContactManifold.h b/ephysics/collision/ContactManifold.h index 809331a..1499a32 100644 --- a/ephysics/collision/ContactManifold.h +++ b/ephysics/collision/ContactManifold.h @@ -1,30 +1,9 @@ -/******************************************************************************** -* 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 REACTPHYSICS3D_CONTACT_MANIFOLD_H -#define REACTPHYSICS3D_CONTACT_MANIFOLD_H +/** @file + * @author Daniel Chappuis + * @copyright 2010-2016 Daniel Chappuis + * @license BSD 3 clauses (see license file) + */ +#pragma once // Libraries #include @@ -37,7 +16,7 @@ namespace reactphysics3d { // Constants -const uint MAX_CONTACT_POINTS_IN_MANIFOLD = 4; // Maximum number of contacts in the manifold +const uint32_t MAX_CONTACT_POINTS_IN_MANIFOLD = 4; // Maximum number of contacts in the manifold // Class declarations class ContactManifold; @@ -48,24 +27,24 @@ class ContactManifold; */ struct ContactManifoldListElement { - public: + public: - // -------------------- Attributes -------------------- // + // -------------------- Attributes -------------------- // - /// Pointer to the actual contact manifold - ContactManifold* contactManifold; + /// Pointer to the actual contact manifold + ContactManifold* contactManifold; - /// Next element of the list - ContactManifoldListElement* next; + /// Next element of the list + ContactManifoldListElement* next; - // -------------------- Methods -------------------- // + // -------------------- Methods -------------------- // - /// Constructor - ContactManifoldListElement(ContactManifold* initContactManifold, - ContactManifoldListElement* initNext) - :contactManifold(initContactManifold), next(initNext) { + /// Constructor + ContactManifoldListElement(ContactManifold* initContactManifold, + ContactManifoldListElement* initNext) + :contactManifold(initContactManifold), next(initNext) { - } + } }; // Class ContactManifold @@ -85,280 +64,280 @@ struct ContactManifoldListElement { */ class ContactManifold { - private: + private: - // -------------------- Attributes -------------------- // + // -------------------- Attributes -------------------- // - /// Pointer to the first proxy shape of the contact - ProxyShape* mShape1; + /// Pointer to the first proxy shape of the contact + ProxyShape* mShape1; - /// Pointer to the second proxy shape of the contact - ProxyShape* mShape2; + /// Pointer to the second proxy shape of the contact + ProxyShape* mShape2; - /// Contact points in the manifold - ContactPoint* mContactPoints[MAX_CONTACT_POINTS_IN_MANIFOLD]; + /// Contact points in the manifold + ContactPoint* mContactPoints[MAX_CONTACT_POINTS_IN_MANIFOLD]; - /// Normal direction Id (Unique Id representing the normal direction) - short int mNormalDirectionId; + /// Normal direction Id (Unique Id representing the normal direction) + int16_t mNormalDirectionId; - /// Number of contacts in the cache - uint mNbContactPoints; + /// Number of contacts in the cache + uint32_t mNbContactPoints; - /// First friction vector of the contact manifold - Vector3 mFrictionVector1; + /// First friction vector of the contact manifold + Vector3 mFrictionVector1; - /// Second friction vector of the contact manifold - Vector3 mFrictionVector2; + /// Second friction vector of the contact manifold + Vector3 mFrictionVector2; - /// First friction constraint accumulated impulse - decimal mFrictionImpulse1; + /// First friction constraint accumulated impulse + float mFrictionImpulse1; - /// Second friction constraint accumulated impulse - decimal mFrictionImpulse2; + /// Second friction constraint accumulated impulse + float mFrictionImpulse2; - /// Twist friction constraint accumulated impulse - decimal mFrictionTwistImpulse; + /// Twist friction constraint accumulated impulse + float mFrictionTwistImpulse; - /// Accumulated rolling resistance impulse - Vector3 mRollingResistanceImpulse; + /// Accumulated rolling resistance impulse + Vector3 mRollingResistanceImpulse; - /// True if the contact manifold has already been added into an island - bool mIsAlreadyInIsland; + /// True if the contact manifold has already been added int32_to an island + bool mIsAlreadyInIsland; - /// Reference to the memory allocator - MemoryAllocator& mMemoryAllocator; + /// Reference to the memory allocator + MemoryAllocator& mMemoryAllocator; - // -------------------- Methods -------------------- // + // -------------------- Methods -------------------- // - /// Private copy-constructor - ContactManifold(const ContactManifold& contactManifold); + /// Private copy-constructor + ContactManifold(const ContactManifold& contactManifold); - /// Private assignment operator - ContactManifold& operator=(const ContactManifold& contactManifold); + /// Private assignment operator + ContactManifold& operator=(const ContactManifold& contactManifold); - /// Return the index of maximum area - int getMaxArea(decimal area0, decimal area1, decimal area2, decimal area3) const; + /// Return the index of maximum area + int32_t getMaxArea(float area0, float area1, float area2, float area3) const; - /// Return the index of the contact with the larger penetration depth. - int getIndexOfDeepestPenetration(ContactPoint* newContact) const; + /// Return the index of the contact with the larger penetration depth. + int32_t getIndexOfDeepestPenetration(ContactPoint* newContact) const; - /// Return the index that will be removed. - int getIndexToRemove(int indexMaxPenetration, const Vector3& newPoint) const; + /// Return the index that will be removed. + int32_t getIndexToRemove(int32_t indexMaxPenetration, const Vector3& newPoint) const; - /// Remove a contact point from the manifold - void removeContactPoint(uint index); + /// Remove a contact point from the manifold + void removeContactPoint(uint32_t index); - /// Return true if the contact manifold has already been added into an island - bool isAlreadyInIsland() const; - - public: + /// Return true if the contact manifold has already been added int32_to an island + bool isAlreadyInIsland() const; + + public: - // -------------------- Methods -------------------- // + // -------------------- Methods -------------------- // - /// Constructor - ContactManifold(ProxyShape* shape1, ProxyShape* shape2, - MemoryAllocator& memoryAllocator, short int normalDirectionId); + /// Constructor + ContactManifold(ProxyShape* shape1, ProxyShape* shape2, + MemoryAllocator& memoryAllocator, int16_t normalDirectionId); - /// Destructor - ~ContactManifold(); + /// Destructor + ~ContactManifold(); - /// Return a pointer to the first proxy shape of the contact - ProxyShape* getShape1() const; + /// Return a pointer to the first proxy shape of the contact + ProxyShape* getShape1() const; - /// Return a pointer to the second proxy shape of the contact - ProxyShape* getShape2() const; + /// Return a pointer to the second proxy shape of the contact + ProxyShape* getShape2() const; - /// Return a pointer to the first body of the contact manifold - CollisionBody* getBody1() const; + /// Return a pointer to the first body of the contact manifold + CollisionBody* getBody1() const; - /// Return a pointer to the second body of the contact manifold - CollisionBody* getBody2() const; + /// Return a pointer to the second body of the contact manifold + CollisionBody* getBody2() const; - /// Return the normal direction Id - short int getNormalDirectionId() const; + /// Return the normal direction Id + int16_t getNormalDirectionId() const; - /// Add a contact point to the manifold - void addContactPoint(ContactPoint* contact); + /// Add a contact point to the manifold + void addContactPoint(ContactPoint* contact); - /// Update the contact manifold. - void update(const Transform& transform1, const Transform& transform2); + /// Update the contact manifold. + void update(const Transform& transform1, const Transform& transform2); - /// Clear the contact manifold - void clear(); + /// Clear the contact manifold + void clear(); - /// Return the number of contact points in the manifold - uint getNbContactPoints() const; + /// Return the number of contact points in the manifold + uint32_t getNbContactPoints() const; - /// Return the first friction vector at the center of the contact manifold - const Vector3& getFrictionVector1() const; + /// Return the first friction vector at the center of the contact manifold + const Vector3& getFrictionVector1() const; - /// set the first friction vector at the center of the contact manifold - void setFrictionVector1(const Vector3& mFrictionVector1); + /// set the first friction vector at the center of the contact manifold + void setFrictionVector1(const Vector3& mFrictionVector1); - /// Return the second friction vector at the center of the contact manifold - const Vector3& getFrictionVector2() const; + /// Return the second friction vector at the center of the contact manifold + const Vector3& getFrictionVector2() const; - /// set the second friction vector at the center of the contact manifold - void setFrictionVector2(const Vector3& mFrictionVector2); + /// set the second friction vector at the center of the contact manifold + void setFrictionVector2(const Vector3& mFrictionVector2); - /// Return the first friction accumulated impulse - decimal getFrictionImpulse1() const; + /// Return the first friction accumulated impulse + float getFrictionImpulse1() const; - /// Set the first friction accumulated impulse - void setFrictionImpulse1(decimal frictionImpulse1); + /// Set the first friction accumulated impulse + void setFrictionImpulse1(float frictionImpulse1); - /// Return the second friction accumulated impulse - decimal getFrictionImpulse2() const; + /// Return the second friction accumulated impulse + float getFrictionImpulse2() const; - /// Set the second friction accumulated impulse - void setFrictionImpulse2(decimal frictionImpulse2); + /// Set the second friction accumulated impulse + void setFrictionImpulse2(float frictionImpulse2); - /// Return the friction twist accumulated impulse - decimal getFrictionTwistImpulse() const; + /// Return the friction twist accumulated impulse + float getFrictionTwistImpulse() const; - /// Set the friction twist accumulated impulse - void setFrictionTwistImpulse(decimal frictionTwistImpulse); + /// Set the friction twist accumulated impulse + void setFrictionTwistImpulse(float frictionTwistImpulse); - /// Set the accumulated rolling resistance impulse - void setRollingResistanceImpulse(const Vector3& rollingResistanceImpulse); + /// Set the accumulated rolling resistance impulse + void setRollingResistanceImpulse(const Vector3& rollingResistanceImpulse); - /// Return a contact point of the manifold - ContactPoint* getContactPoint(uint index) const; + /// Return a contact point of the manifold + ContactPoint* getContactPoint(uint32_t index) const; - /// Return the normalized averaged normal vector - Vector3 getAverageContactNormal() const; + /// Return the normalized averaged normal vector + Vector3 getAverageContactNormal() const; - /// Return the largest depth of all the contact points - decimal getLargestContactDepth() const; + /// Return the largest depth of all the contact points + float getLargestContactDepth() const; - // -------------------- Friendship -------------------- // + // -------------------- Friendship -------------------- // - friend class DynamicsWorld; - friend class Island; - friend class CollisionBody; + friend class DynamicsWorld; + friend class Island; + friend class CollisionBody; }; // Return a pointer to the first proxy shape of the contact inline ProxyShape* ContactManifold::getShape1() const { - return mShape1; + return mShape1; } // Return a pointer to the second proxy shape of the contact inline ProxyShape* ContactManifold::getShape2() const { - return mShape2; + return mShape2; } // Return a pointer to the first body of the contact manifold inline CollisionBody* ContactManifold::getBody1() const { - return mShape1->getBody(); + return mShape1->getBody(); } // Return a pointer to the second body of the contact manifold inline CollisionBody* ContactManifold::getBody2() const { - return mShape2->getBody(); + return mShape2->getBody(); } // Return the normal direction Id -inline short int ContactManifold::getNormalDirectionId() const { - return mNormalDirectionId; +inline int16_t ContactManifold::getNormalDirectionId() const { + return mNormalDirectionId; } // Return the number of contact points in the manifold -inline uint ContactManifold::getNbContactPoints() const { - return mNbContactPoints; +inline uint32_t ContactManifold::getNbContactPoints() const { + return mNbContactPoints; } // Return the first friction vector at the center of the contact manifold inline const Vector3& ContactManifold::getFrictionVector1() const { - return mFrictionVector1; + return mFrictionVector1; } // set the first friction vector at the center of the contact manifold inline void ContactManifold::setFrictionVector1(const Vector3& frictionVector1) { - mFrictionVector1 = frictionVector1; + mFrictionVector1 = frictionVector1; } // Return the second friction vector at the center of the contact manifold inline const Vector3& ContactManifold::getFrictionVector2() const { - return mFrictionVector2; + return mFrictionVector2; } // set the second friction vector at the center of the contact manifold inline void ContactManifold::setFrictionVector2(const Vector3& frictionVector2) { - mFrictionVector2 = frictionVector2; + mFrictionVector2 = frictionVector2; } // Return the first friction accumulated impulse -inline decimal ContactManifold::getFrictionImpulse1() const { - return mFrictionImpulse1; +inline float ContactManifold::getFrictionImpulse1() const { + return mFrictionImpulse1; } // Set the first friction accumulated impulse -inline void ContactManifold::setFrictionImpulse1(decimal frictionImpulse1) { - mFrictionImpulse1 = frictionImpulse1; +inline void ContactManifold::setFrictionImpulse1(float frictionImpulse1) { + mFrictionImpulse1 = frictionImpulse1; } // Return the second friction accumulated impulse -inline decimal ContactManifold::getFrictionImpulse2() const { - return mFrictionImpulse2; +inline float ContactManifold::getFrictionImpulse2() const { + return mFrictionImpulse2; } // Set the second friction accumulated impulse -inline void ContactManifold::setFrictionImpulse2(decimal frictionImpulse2) { - mFrictionImpulse2 = frictionImpulse2; +inline void ContactManifold::setFrictionImpulse2(float frictionImpulse2) { + mFrictionImpulse2 = frictionImpulse2; } // Return the friction twist accumulated impulse -inline decimal ContactManifold::getFrictionTwistImpulse() const { - return mFrictionTwistImpulse; +inline float ContactManifold::getFrictionTwistImpulse() const { + return mFrictionTwistImpulse; } // Set the friction twist accumulated impulse -inline void ContactManifold::setFrictionTwistImpulse(decimal frictionTwistImpulse) { - mFrictionTwistImpulse = frictionTwistImpulse; +inline void ContactManifold::setFrictionTwistImpulse(float frictionTwistImpulse) { + mFrictionTwistImpulse = frictionTwistImpulse; } // Set the accumulated rolling resistance impulse inline void ContactManifold::setRollingResistanceImpulse(const Vector3& rollingResistanceImpulse) { - mRollingResistanceImpulse = rollingResistanceImpulse; + mRollingResistanceImpulse = rollingResistanceImpulse; } // Return a contact point of the manifold -inline ContactPoint* ContactManifold::getContactPoint(uint index) const { - assert(index < mNbContactPoints); - return mContactPoints[index]; +inline ContactPoint* ContactManifold::getContactPoint(uint32_t index) const { + assert(index < mNbContactPoints); + return mContactPoints[index]; } -// Return true if the contact manifold has already been added into an island +// Return true if the contact manifold has already been added int32_to an island inline bool ContactManifold::isAlreadyInIsland() const { - return mIsAlreadyInIsland; + return mIsAlreadyInIsland; } // Return the normalized averaged normal vector inline Vector3 ContactManifold::getAverageContactNormal() const { - Vector3 averageNormal; + Vector3 averageNormal; - for (uint i=0; igetNormal(); - } + for (uint32_t i=0; igetNormal(); + } - return averageNormal.getUnit(); + return averageNormal.getUnit(); } // Return the largest depth of all the contact points -inline decimal ContactManifold::getLargestContactDepth() const { - decimal largestDepth = 0.0f; +inline float ContactManifold::getLargestContactDepth() const { + float largestDepth = 0.0f; - for (uint i=0; igetPenetrationDepth(); - if (depth > largestDepth) { - largestDepth = depth; - } - } + for (uint32_t i=0; igetPenetrationDepth(); + if (depth > largestDepth) { + largestDepth = depth; + } + } - return largestDepth; + return largestDepth; } } -#endif + diff --git a/ephysics/collision/ContactManifoldSet.cpp b/ephysics/collision/ContactManifoldSet.cpp index 4d75a70..6685424 100644 --- a/ephysics/collision/ContactManifoldSet.cpp +++ b/ephysics/collision/ContactManifoldSet.cpp @@ -1,27 +1,8 @@ -/******************************************************************************** -* 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. * -* * -********************************************************************************/ +/** @file + * @author Daniel Chappuis + * @copyright 2010-2016 Daniel Chappuis + * @license BSD 3 clauses (see license file) + */ // Libraries #include @@ -30,207 +11,207 @@ using namespace reactphysics3d; // Constructor ContactManifoldSet::ContactManifoldSet(ProxyShape* shape1, ProxyShape* shape2, - MemoryAllocator& memoryAllocator, int nbMaxManifolds) - : mNbMaxManifolds(nbMaxManifolds), mNbManifolds(0), mShape1(shape1), - mShape2(shape2), mMemoryAllocator(memoryAllocator) { - assert(nbMaxManifolds >= 1); + MemoryAllocator& memoryAllocator, int32_t nbMaxManifolds) + : mNbMaxManifolds(nbMaxManifolds), mNbManifolds(0), mShape1(shape1), + mShape2(shape2), mMemoryAllocator(memoryAllocator) { + assert(nbMaxManifolds >= 1); } // Destructor ContactManifoldSet::~ContactManifoldSet() { - // Clear all the contact manifolds - clear(); + // Clear all the contact manifolds + clear(); } // Add a contact point to the manifold set void ContactManifoldSet::addContactPoint(ContactPoint* contact) { - // Compute an Id corresponding to the normal direction (using a cubemap) - short int normalDirectionId = computeCubemapNormalId(contact->getNormal()); + // Compute an Id corresponding to the normal direction (using a cubemap) + int16_t normalDirectionId = computeCubemapNormalId(contact->getNormal()); - // If there is no contact manifold yet - if (mNbManifolds == 0) { + // If there is no contact manifold yet + if (mNbManifolds == 0) { - createManifold(normalDirectionId); - mManifolds[0]->addContactPoint(contact); - assert(mManifolds[mNbManifolds-1]->getNbContactPoints() > 0); - for (int i=0; igetNbContactPoints() > 0); - } + createManifold(normalDirectionId); + mManifolds[0]->addContactPoint(contact); + assert(mManifolds[mNbManifolds-1]->getNbContactPoints() > 0); + for (int32_t i=0; igetNbContactPoints() > 0); + } - return; - } + return; + } - // Select the manifold with the most similar normal (if exists) - int similarManifoldIndex = 0; - if (mNbMaxManifolds > 1) { - similarManifoldIndex = selectManifoldWithSimilarNormal(normalDirectionId); - } + // Select the manifold with the most similar normal (if exists) + int32_t similarManifoldIndex = 0; + if (mNbMaxManifolds > 1) { + similarManifoldIndex = selectManifoldWithSimilarNormal(normalDirectionId); + } - // If a similar manifold has been found - if (similarManifoldIndex != -1) { + // If a similar manifold has been found + if (similarManifoldIndex != -1) { - // Add the contact point to that similar manifold - mManifolds[similarManifoldIndex]->addContactPoint(contact); - assert(mManifolds[similarManifoldIndex]->getNbContactPoints() > 0); + // Add the contact point to that similar manifold + mManifolds[similarManifoldIndex]->addContactPoint(contact); + assert(mManifolds[similarManifoldIndex]->getNbContactPoints() > 0); - return; - } + return; + } - // If the maximum number of manifold has not been reached yet - if (mNbManifolds < mNbMaxManifolds) { + // If the maximum number of manifold has not been reached yet + if (mNbManifolds < mNbMaxManifolds) { - // Create a new manifold for the contact point - createManifold(normalDirectionId); - mManifolds[mNbManifolds-1]->addContactPoint(contact); - for (int i=0; igetNbContactPoints() > 0); - } + // Create a new manifold for the contact point + createManifold(normalDirectionId); + mManifolds[mNbManifolds-1]->addContactPoint(contact); + for (int32_t i=0; igetNbContactPoints() > 0); + } - return; - } + return; + } - // The contact point will be in a new contact manifold, we now have too much - // manifolds condidates. We need to remove one. We choose to keep the manifolds - // with the largest contact depth among their points - int smallestDepthIndex = -1; - decimal minDepth = contact->getPenetrationDepth(); - assert(mNbManifolds == mNbMaxManifolds); - for (int i=0; igetLargestContactDepth(); - if (depth < minDepth) { - minDepth = depth; - smallestDepthIndex = i; - } - } + // The contact point will be in a new contact manifold, we now have too much + // manifolds condidates. We need to remove one. We choose to keep the manifolds + // with the largest contact depth among their points + int32_t smallestDepthIndex = -1; + float minDepth = contact->getPenetrationDepth(); + assert(mNbManifolds == mNbMaxManifolds); + for (int32_t i=0; igetLargestContactDepth(); + if (depth < minDepth) { + minDepth = depth; + smallestDepthIndex = i; + } + } - // If we do not want to keep to new manifold (not created yet) with the - // new contact point - if (smallestDepthIndex == -1) { + // If we do not want to keep to new manifold (not created yet) with the + // new contact point + if (smallestDepthIndex == -1) { - // Delete the new contact - contact->~ContactPoint(); - mMemoryAllocator.release(contact, sizeof(ContactPoint)); + // Delete the new contact + contact->~ContactPoint(); + mMemoryAllocator.release(contact, sizeof(ContactPoint)); - return; - } + return; + } - assert(smallestDepthIndex >= 0 && smallestDepthIndex < mNbManifolds); + assert(smallestDepthIndex >= 0 && smallestDepthIndex < mNbManifolds); - // Here we need to replace an existing manifold with a new one (that contains - // the new contact point) - removeManifold(smallestDepthIndex); - createManifold(normalDirectionId); - mManifolds[mNbManifolds-1]->addContactPoint(contact); - assert(mManifolds[mNbManifolds-1]->getNbContactPoints() > 0); - for (int i=0; igetNbContactPoints() > 0); - } + // Here we need to replace an existing manifold with a new one (that contains + // the new contact point) + removeManifold(smallestDepthIndex); + createManifold(normalDirectionId); + mManifolds[mNbManifolds-1]->addContactPoint(contact); + assert(mManifolds[mNbManifolds-1]->getNbContactPoints() > 0); + for (int32_t i=0; igetNbContactPoints() > 0); + } - return; + return; } // Return the index of the contact manifold with a similar average normal. // If no manifold has close enough average normal, it returns -1 -int ContactManifoldSet::selectManifoldWithSimilarNormal(short int normalDirectionId) const { +int32_t ContactManifoldSet::selectManifoldWithSimilarNormal(int16_t normalDirectionId) const { - // Return the Id of the manifold with the same normal direction id (if exists) - for (int i=0; igetNormalDirectionId()) { - return i; - } - } + // Return the Id of the manifold with the same normal direction id (if exists) + for (int32_t i=0; igetNormalDirectionId()) { + return i; + } + } - return -1; + return -1; } -// Map the normal vector into a cubemap face bucket (a face contains 4x4 buckets) -// Each face of the cube is divided into 4x4 buckets. This method maps the -// normal vector into of the of the bucket and returns a unique Id for the bucket -short int ContactManifoldSet::computeCubemapNormalId(const Vector3& normal) const { +// Map the normal vector int32_to a cubemap face bucket (a face contains 4x4 buckets) +// Each face of the cube is divided int32_to 4x4 buckets. This method maps the +// normal vector int32_to of the of the bucket and returns a unique Id for the bucket +int16_t ContactManifoldSet::computeCubemapNormalId(const Vector3& normal) const { - assert(normal.lengthSquare() > MACHINE_EPSILON); + assert(normal.lengthSquare() > MACHINE_EPSILON); - int faceNo; - decimal u, v; - decimal max = max3(fabs(normal.x), fabs(normal.y), fabs(normal.z)); - Vector3 normalScaled = normal / max; + int32_t faceNo; + float u, v; + float max = max3(fabs(normal.x), fabs(normal.y), fabs(normal.z)); + Vector3 normalScaled = normal / max; - if (normalScaled.x >= normalScaled.y && normalScaled.x >= normalScaled.z) { - faceNo = normalScaled.x > 0 ? 0 : 1; - u = normalScaled.y; - v = normalScaled.z; - } - else if (normalScaled.y >= normalScaled.x && normalScaled.y >= normalScaled.z) { - faceNo = normalScaled.y > 0 ? 2 : 3; - u = normalScaled.x; - v = normalScaled.z; - } - else { - faceNo = normalScaled.z > 0 ? 4 : 5; - u = normalScaled.x; - v = normalScaled.y; - } + if (normalScaled.x >= normalScaled.y && normalScaled.x >= normalScaled.z) { + faceNo = normalScaled.x > 0 ? 0 : 1; + u = normalScaled.y; + v = normalScaled.z; + } + else if (normalScaled.y >= normalScaled.x && normalScaled.y >= normalScaled.z) { + faceNo = normalScaled.y > 0 ? 2 : 3; + u = normalScaled.x; + v = normalScaled.z; + } + else { + faceNo = normalScaled.z > 0 ? 4 : 5; + u = normalScaled.x; + v = normalScaled.y; + } - int indexU = floor(((u + 1)/2) * CONTACT_CUBEMAP_FACE_NB_SUBDIVISIONS); - int indexV = floor(((v + 1)/2) * CONTACT_CUBEMAP_FACE_NB_SUBDIVISIONS); - if (indexU == CONTACT_CUBEMAP_FACE_NB_SUBDIVISIONS) indexU--; - if (indexV == CONTACT_CUBEMAP_FACE_NB_SUBDIVISIONS) indexV--; + int32_t indexU = floor(((u + 1)/2) * CONTACT_CUBEMAP_FACE_NB_SUBDIVISIONS); + int32_t indexV = floor(((v + 1)/2) * CONTACT_CUBEMAP_FACE_NB_SUBDIVISIONS); + if (indexU == CONTACT_CUBEMAP_FACE_NB_SUBDIVISIONS) indexU--; + if (indexV == CONTACT_CUBEMAP_FACE_NB_SUBDIVISIONS) indexV--; - const int nbSubDivInFace = CONTACT_CUBEMAP_FACE_NB_SUBDIVISIONS * CONTACT_CUBEMAP_FACE_NB_SUBDIVISIONS; - return faceNo * 200 + indexU * nbSubDivInFace + indexV; + const int32_t nbSubDivInFace = CONTACT_CUBEMAP_FACE_NB_SUBDIVISIONS * CONTACT_CUBEMAP_FACE_NB_SUBDIVISIONS; + return faceNo * 200 + indexU * nbSubDivInFace + indexV; } // Update the contact manifolds void ContactManifoldSet::update() { - for (int i=mNbManifolds-1; i>=0; i--) { + for (int32_t i=mNbManifolds-1; i>=0; i--) { - // Update the contact manifold - mManifolds[i]->update(mShape1->getBody()->getTransform() * mShape1->getLocalToBodyTransform(), - mShape2->getBody()->getTransform() * mShape2->getLocalToBodyTransform()); + // Update the contact manifold + mManifolds[i]->update(mShape1->getBody()->getTransform() * mShape1->getLocalToBodyTransform(), + mShape2->getBody()->getTransform() * mShape2->getLocalToBodyTransform()); - // Remove the contact manifold if has no contact points anymore - if (mManifolds[i]->getNbContactPoints() == 0) { - removeManifold(i); - } - } + // Remove the contact manifold if has no contact points anymore + if (mManifolds[i]->getNbContactPoints() == 0) { + removeManifold(i); + } + } } // Clear the contact manifold set void ContactManifoldSet::clear() { - // Destroy all the contact manifolds - for (int i=mNbManifolds-1; i>=0; i--) { - removeManifold(i); - } + // Destroy all the contact manifolds + for (int32_t i=mNbManifolds-1; i>=0; i--) { + removeManifold(i); + } - assert(mNbManifolds == 0); + assert(mNbManifolds == 0); } // Create a new contact manifold and add it to the set -void ContactManifoldSet::createManifold(short int normalDirectionId) { - assert(mNbManifolds < mNbMaxManifolds); +void ContactManifoldSet::createManifold(int16_t normalDirectionId) { + assert(mNbManifolds < mNbMaxManifolds); - mManifolds[mNbManifolds] = new (mMemoryAllocator.allocate(sizeof(ContactManifold))) - ContactManifold(mShape1, mShape2, mMemoryAllocator, normalDirectionId); - mNbManifolds++; + mManifolds[mNbManifolds] = new (mMemoryAllocator.allocate(sizeof(ContactManifold))) + ContactManifold(mShape1, mShape2, mMemoryAllocator, normalDirectionId); + mNbManifolds++; } // Remove a contact manifold from the set -void ContactManifoldSet::removeManifold(int index) { +void ContactManifoldSet::removeManifold(int32_t index) { - assert(mNbManifolds > 0); - assert(index >= 0 && index < mNbManifolds); + assert(mNbManifolds > 0); + assert(index >= 0 && index < mNbManifolds); - // Delete the new contact - mManifolds[index]->~ContactManifold(); - mMemoryAllocator.release(mManifolds[index], sizeof(ContactManifold)); + // Delete the new contact + mManifolds[index]->~ContactManifold(); + mMemoryAllocator.release(mManifolds[index], sizeof(ContactManifold)); - for (int i=index; (i+1) < mNbManifolds; i++) { - mManifolds[i] = mManifolds[i+1]; - } + for (int32_t i=index; (i+1) < mNbManifolds; i++) { + mManifolds[i] = mManifolds[i+1]; + } - mNbManifolds--; + mNbManifolds--; } diff --git a/ephysics/collision/ContactManifoldSet.h b/ephysics/collision/ContactManifoldSet.h index 8baf6eb..5b5f988 100644 --- a/ephysics/collision/ContactManifoldSet.h +++ b/ephysics/collision/ContactManifoldSet.h @@ -1,30 +1,9 @@ -/******************************************************************************** -* 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 REACTPHYSICS3D_CONTACT_MANIFOLD_SET_H -#define REACTPHYSICS3D_CONTACT_MANIFOLD_SET_H +/** @file + * @author Daniel Chappuis + * @copyright 2010-2016 Daniel Chappuis + * @license BSD 3 clauses (see license file) + */ +#pragma once // Libraries #include @@ -32,8 +11,8 @@ namespace reactphysics3d { // Constants -const int MAX_MANIFOLDS_IN_CONTACT_MANIFOLD_SET = 3; // Maximum number of contact manifolds in the set -const int CONTACT_CUBEMAP_FACE_NB_SUBDIVISIONS = 3; // N Number for the N x N subdivisions of the cubemap +const int32_t MAX_MANIFOLDS_IN_CONTACT_MANIFOLD_SET = 3; // Maximum number of contact manifolds in the set +const int32_t CONTACT_CUBEMAP_FACE_NB_SUBDIVISIONS = 3; // N Number for the N x N subdivisions of the cubemap // Class ContactManifoldSet /** @@ -44,111 +23,109 @@ const int CONTACT_CUBEMAP_FACE_NB_SUBDIVISIONS = 3; // N Number for the N x N */ class ContactManifoldSet { - private: + private: - // -------------------- Attributes -------------------- // + // -------------------- Attributes -------------------- // - /// Maximum number of contact manifolds in the set - int mNbMaxManifolds; + /// Maximum number of contact manifolds in the set + int32_t mNbMaxManifolds; - /// Current number of contact manifolds in the set - int mNbManifolds; + /// Current number of contact manifolds in the set + int32_t mNbManifolds; - /// Pointer to the first proxy shape of the contact - ProxyShape* mShape1; + /// Pointer to the first proxy shape of the contact + ProxyShape* mShape1; - /// Pointer to the second proxy shape of the contact - ProxyShape* mShape2; + /// Pointer to the second proxy shape of the contact + ProxyShape* mShape2; - /// Reference to the memory allocator - MemoryAllocator& mMemoryAllocator; + /// Reference to the memory allocator + MemoryAllocator& mMemoryAllocator; - /// Contact manifolds of the set - ContactManifold* mManifolds[MAX_MANIFOLDS_IN_CONTACT_MANIFOLD_SET]; + /// Contact manifolds of the set + ContactManifold* mManifolds[MAX_MANIFOLDS_IN_CONTACT_MANIFOLD_SET]; - // -------------------- Methods -------------------- // + // -------------------- Methods -------------------- // - /// Create a new contact manifold and add it to the set - void createManifold(short normalDirectionId); + /// Create a new contact manifold and add it to the set + void createManifold(short normalDirectionId); - /// Remove a contact manifold from the set - void removeManifold(int index); + /// Remove a contact manifold from the set + void removeManifold(int32_t index); - // Return the index of the contact manifold with a similar average normal. - int selectManifoldWithSimilarNormal(short int normalDirectionId) const; + // Return the index of the contact manifold with a similar average normal. + int32_t selectManifoldWithSimilarNormal(int16_t normalDirectionId) const; - // Map the normal vector into a cubemap face bucket (a face contains 4x4 buckets) - // Each face of the cube is divided into 4x4 buckets. This method maps the - // normal vector into of the of the bucket and returns a unique Id for the bucket - short int computeCubemapNormalId(const Vector3& normal) const; + // Map the normal vector int32_to a cubemap face bucket (a face contains 4x4 buckets) + // Each face of the cube is divided int32_to 4x4 buckets. This method maps the + // normal vector int32_to of the of the bucket and returns a unique Id for the bucket + int16_t computeCubemapNormalId(const Vector3& normal) const; - public: + public: - // -------------------- Methods -------------------- // + // -------------------- Methods -------------------- // - /// Constructor - ContactManifoldSet(ProxyShape* shape1, ProxyShape* shape2, - MemoryAllocator& memoryAllocator, int nbMaxManifolds); + /// Constructor + ContactManifoldSet(ProxyShape* shape1, ProxyShape* shape2, + MemoryAllocator& memoryAllocator, int32_t nbMaxManifolds); - /// Destructor - ~ContactManifoldSet(); + /// Destructor + ~ContactManifoldSet(); - /// Return the first proxy shape - ProxyShape* getShape1() const; + /// Return the first proxy shape + ProxyShape* getShape1() const; - /// Return the second proxy shape - ProxyShape* getShape2() const; + /// Return the second proxy shape + ProxyShape* getShape2() const; - /// Add a contact point to the manifold set - void addContactPoint(ContactPoint* contact); + /// Add a contact point to the manifold set + void addContactPoint(ContactPoint* contact); - /// Update the contact manifolds - void update(); + /// Update the contact manifolds + void update(); - /// Clear the contact manifold set - void clear(); + /// Clear the contact manifold set + void clear(); - /// Return the number of manifolds in the set - int getNbContactManifolds() const; + /// Return the number of manifolds in the set + int32_t getNbContactManifolds() const; - /// Return a given contact manifold - ContactManifold* getContactManifold(int index) const; + /// Return a given contact manifold + ContactManifold* getContactManifold(int32_t index) const; - /// Return the total number of contact points in the set of manifolds - int getTotalNbContactPoints() const; + /// Return the total number of contact points in the set of manifolds + int32_t getTotalNbContactPoints() const; }; // Return the first proxy shape inline ProxyShape* ContactManifoldSet::getShape1() const { - return mShape1; + return mShape1; } // Return the second proxy shape inline ProxyShape* ContactManifoldSet::getShape2() const { - return mShape2; + return mShape2; } // Return the number of manifolds in the set -inline int ContactManifoldSet::getNbContactManifolds() const { - return mNbManifolds; +inline int32_t ContactManifoldSet::getNbContactManifolds() const { + return mNbManifolds; } // Return a given contact manifold -inline ContactManifold* ContactManifoldSet::getContactManifold(int index) const { - assert(index >= 0 && index < mNbManifolds); - return mManifolds[index]; +inline ContactManifold* ContactManifoldSet::getContactManifold(int32_t index) const { + assert(index >= 0 && index < mNbManifolds); + return mManifolds[index]; } // Return the total number of contact points in the set of manifolds -inline int ContactManifoldSet::getTotalNbContactPoints() const { - int nbPoints = 0; - for (int i=0; igetNbContactPoints(); - } - return nbPoints; +inline int32_t ContactManifoldSet::getTotalNbContactPoints() const { + int32_t nbPoints = 0; + for (int32_t i=0; igetNbContactPoints(); + } + return nbPoints; } } -#endif - diff --git a/ephysics/collision/ProxyShape.cpp b/ephysics/collision/ProxyShape.cpp index 3bce712..d4a3b7d 100644 --- a/ephysics/collision/ProxyShape.cpp +++ b/ephysics/collision/ProxyShape.cpp @@ -1,27 +1,8 @@ -/******************************************************************************** -* 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. * -* * -********************************************************************************/ +/** @file + * @author Daniel Chappuis + * @copyright 2010-2016 Daniel Chappuis + * @license BSD 3 clauses (see license file) + */ // Libraries #include @@ -35,20 +16,20 @@ using namespace reactphysics3d; * @param transform Transformation from collision shape local-space to body local-space * @param mass Mass of the collision shape (in kilograms) */ -ProxyShape::ProxyShape(CollisionBody* body, CollisionShape* shape, const Transform& transform, decimal mass) - :mBody(body), mCollisionShape(shape), mLocalToBodyTransform(transform), mMass(mass), - mNext(NULL), mBroadPhaseID(-1), mCachedCollisionData(NULL), mUserData(NULL), - mCollisionCategoryBits(0x0001), mCollideWithMaskBits(0xFFFF) { +ProxyShape::ProxyShape(CollisionBody* body, CollisionShape* shape, const Transform& transform, float mass) + :mBody(body), mCollisionShape(shape), mLocalToBodyTransform(transform), mMass(mass), + mNext(NULL), mBroadPhaseID(-1), mCachedCollisionData(NULL), mUserData(NULL), + mCollisionCategoryBits(0x0001), mCollideWithMaskBits(0xFFFF) { } // Destructor ProxyShape::~ProxyShape() { - // Release the cached collision data memory - if (mCachedCollisionData != NULL) { - free(mCachedCollisionData); - } + // Release the cached collision data memory + if (mCachedCollisionData != NULL) { + free(mCachedCollisionData); + } } // Return true if a point is inside the collision shape @@ -57,36 +38,36 @@ ProxyShape::~ProxyShape() { * @return True if the point is inside the collision shape */ bool ProxyShape::testPointInside(const Vector3& worldPoint) { - const Transform localToWorld = mBody->getTransform() * mLocalToBodyTransform; - const Vector3 localPoint = localToWorld.getInverse() * worldPoint; - return mCollisionShape->testPointInside(localPoint, this); + const Transform localToWorld = mBody->getTransform() * mLocalToBodyTransform; + const Vector3 localPoint = localToWorld.getInverse() * worldPoint; + return mCollisionShape->testPointInside(localPoint, this); } // Raycast method with feedback information /** * @param ray Ray to use for the raycasting * @param[out] raycastInfo Result of the raycasting that is valid only if the - * methods returned true + * methods returned true * @return True if the ray hit the collision shape */ bool ProxyShape::raycast(const Ray& ray, RaycastInfo& raycastInfo) { - // If the corresponding body is not active, it cannot be hit by rays - if (!mBody->isActive()) return false; + // If the corresponding body is not active, it cannot be hit by rays + if (!mBody->isActive()) return false; - // Convert the ray into the local-space of the collision shape - const Transform localToWorldTransform = getLocalToWorldTransform(); - const Transform worldToLocalTransform = localToWorldTransform.getInverse(); - Ray rayLocal(worldToLocalTransform * ray.point1, - worldToLocalTransform * ray.point2, - ray.maxFraction); + // Convert the ray int32_to the local-space of the collision shape + const Transform localToWorldTransform = getLocalToWorldTransform(); + const Transform worldToLocalTransform = localToWorldTransform.getInverse(); + Ray rayLocal(worldToLocalTransform * ray.point1, + worldToLocalTransform * ray.point2, + ray.maxFraction); - bool isHit = mCollisionShape->raycast(rayLocal, raycastInfo, this); + bool isHit = mCollisionShape->raycast(rayLocal, raycastInfo, this); - // Convert the raycast info into world-space - raycastInfo.worldPoint = localToWorldTransform * raycastInfo.worldPoint; - raycastInfo.worldNormal = localToWorldTransform.getOrientation() * raycastInfo.worldNormal; - raycastInfo.worldNormal.normalize(); + // Convert the raycast info int32_to world-space + raycastInfo.worldPoint = localToWorldTransform * raycastInfo.worldPoint; + raycastInfo.worldNormal = localToWorldTransform.getOrientation() * raycastInfo.worldNormal; + raycastInfo.worldNormal.normalize(); - return isHit; + return isHit; } diff --git a/ephysics/collision/ProxyShape.h b/ephysics/collision/ProxyShape.h index 9eff8a8..2242020 100644 --- a/ephysics/collision/ProxyShape.h +++ b/ephysics/collision/ProxyShape.h @@ -1,30 +1,9 @@ -/******************************************************************************** -* 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 REACTPHYSICS3D_PROXY_SHAPE_H -#define REACTPHYSICS3D_PROXY_SHAPE_H +/** @file + * @author Daniel Chappuis + * @copyright 2010-2016 Daniel Chappuis + * @license BSD 3 clauses (see license file) + */ +#pragma once // Libraries #include @@ -44,150 +23,150 @@ namespace reactphysics3d { */ class ProxyShape { - protected: + protected: - // -------------------- Attributes -------------------- // + // -------------------- Attributes -------------------- // - /// Pointer to the parent body - CollisionBody* mBody; + /// Pointer to the parent body + CollisionBody* mBody; - /// Internal collision shape - CollisionShape* mCollisionShape; + /// Internal collision shape + CollisionShape* mCollisionShape; - /// Local-space to parent body-space transform (does not change over time) - Transform mLocalToBodyTransform; + /// Local-space to parent body-space transform (does not change over time) + Transform mLocalToBodyTransform; - /// Mass (in kilogramms) of the corresponding collision shape - decimal mMass; + /// Mass (in kilogramms) of the corresponding collision shape + float mMass; - /// Pointer to the next proxy shape of the body (linked list) - ProxyShape* mNext; + /// Pointer to the next proxy shape of the body (linked list) + ProxyShape* mNext; - /// Broad-phase ID (node ID in the dynamic AABB tree) - int mBroadPhaseID; + /// Broad-phase ID (node ID in the dynamic AABB tree) + int32_t mBroadPhaseID; - /// Cached collision data - void* mCachedCollisionData; + /// Cached collision data + void* mCachedCollisionData; - /// Pointer to user data - void* mUserData; + /// Pointer to user data + void* mUserData; - /// Bits used to define the collision category of this shape. - /// You can set a single bit to one to define a category value for this - /// shape. This value is one (0x0001) by default. This variable can be used - /// together with the mCollideWithMaskBits variable so that given - /// categories of shapes collide with each other and do not collide with - /// other categories. - unsigned short mCollisionCategoryBits; + /// Bits used to define the collision category of this shape. + /// You can set a single bit to one to define a category value for this + /// shape. This value is one (0x0001) by default. This variable can be used + /// together with the mCollideWithMaskBits variable so that given + /// categories of shapes collide with each other and do not collide with + /// other categories. + unsigned short mCollisionCategoryBits; - /// Bits mask used to state which collision categories this shape can - /// collide with. This value is 0xFFFF by default. It means that this - /// proxy shape will collide with every collision categories by default. - unsigned short mCollideWithMaskBits; + /// Bits mask used to state which collision categories this shape can + /// collide with. This value is 0xFFFF by default. It means that this + /// proxy shape will collide with every collision categories by default. + unsigned short mCollideWithMaskBits; - // -------------------- Methods -------------------- // + // -------------------- Methods -------------------- // - /// Private copy-constructor - ProxyShape(const ProxyShape& proxyShape); + /// Private copy-constructor + ProxyShape(const ProxyShape& proxyShape); - /// Private assignment operator - ProxyShape& operator=(const ProxyShape& proxyShape); + /// Private assignment operator + ProxyShape& operator=(const ProxyShape& proxyShape); - public: + public: - // -------------------- Methods -------------------- // + // -------------------- Methods -------------------- // - /// Constructor - ProxyShape(CollisionBody* body, CollisionShape* shape, - const Transform& transform, decimal mass); + /// Constructor + ProxyShape(CollisionBody* body, CollisionShape* shape, + const Transform& transform, float mass); - /// Destructor - virtual ~ProxyShape(); + /// Destructor + virtual ~ProxyShape(); - /// Return the collision shape - const CollisionShape* getCollisionShape() const; + /// Return the collision shape + const CollisionShape* getCollisionShape() const; - /// Return the parent body - CollisionBody* getBody() const; + /// Return the parent body + CollisionBody* getBody() const; - /// Return the mass of the collision shape - decimal getMass() const; + /// Return the mass of the collision shape + float getMass() const; - /// Return a pointer to the user data attached to this body - void* getUserData() const; + /// Return a pointer to the user data attached to this body + void* getUserData() const; - /// Attach user data to this body - void setUserData(void* userData); + /// Attach user data to this body + void setUserData(void* userData); - /// Return the local to parent body transform - const Transform& getLocalToBodyTransform() const; + /// Return the local to parent body transform + const Transform& getLocalToBodyTransform() const; - /// Set the local to parent body transform - void setLocalToBodyTransform(const Transform& transform); + /// Set the local to parent body transform + void setLocalToBodyTransform(const Transform& transform); - /// Return the local to world transform - const Transform getLocalToWorldTransform() const; + /// Return the local to world transform + const Transform getLocalToWorldTransform() const; - /// Return true if a point is inside the collision shape - bool testPointInside(const Vector3& worldPoint); + /// Return true if a point is inside the collision shape + bool testPointInside(const Vector3& worldPoint); - /// Raycast method with feedback information - bool raycast(const Ray& ray, RaycastInfo& raycastInfo); + /// Raycast method with feedback information + bool raycast(const Ray& ray, RaycastInfo& raycastInfo); - /// Return the collision bits mask - unsigned short getCollideWithMaskBits() const; + /// Return the collision bits mask + unsigned short getCollideWithMaskBits() const; - /// Set the collision bits mask - void setCollideWithMaskBits(unsigned short collideWithMaskBits); + /// Set the collision bits mask + void setCollideWithMaskBits(unsigned short collideWithMaskBits); - /// Return the collision category bits - unsigned short getCollisionCategoryBits() const; + /// Return the collision category bits + unsigned short getCollisionCategoryBits() const; - /// Set the collision category bits - void setCollisionCategoryBits(unsigned short collisionCategoryBits); + /// Set the collision category bits + void setCollisionCategoryBits(unsigned short collisionCategoryBits); - /// Return the next proxy shape in the linked list of proxy shapes - ProxyShape* getNext(); + /// Return the next proxy shape in the linked list of proxy shapes + ProxyShape* getNext(); - /// Return the next proxy shape in the linked list of proxy shapes - const ProxyShape* getNext() const; + /// Return the next proxy shape in the linked list of proxy shapes + const ProxyShape* getNext() const; - /// Return the pointer to the cached collision data - void** getCachedCollisionData(); + /// Return the pointer to the cached collision data + void** getCachedCollisionData(); - /// Return the local scaling vector of the collision shape - Vector3 getLocalScaling() const; + /// Return the local scaling vector of the collision shape + Vector3 getLocalScaling() const; - /// Set the local scaling vector of the collision shape - virtual void setLocalScaling(const Vector3& scaling); + /// Set the local scaling vector of the collision shape + virtual void setLocalScaling(const Vector3& scaling); - // -------------------- Friendship -------------------- // + // -------------------- Friendship -------------------- // - friend class OverlappingPair; - friend class CollisionBody; - friend class RigidBody; - friend class BroadPhaseAlgorithm; - friend class DynamicAABBTree; - friend class CollisionDetection; - friend class CollisionWorld; - friend class DynamicsWorld; - friend class EPAAlgorithm; - friend class GJKAlgorithm; - friend class ConvexMeshShape; + friend class OverlappingPair; + friend class CollisionBody; + friend class RigidBody; + friend class BroadPhaseAlgorithm; + friend class DynamicAABBTree; + friend class CollisionDetection; + friend class CollisionWorld; + friend class DynamicsWorld; + friend class EPAAlgorithm; + friend class GJKAlgorithm; + friend class ConvexMeshShape; }; // Return the pointer to the cached collision data inline void** ProxyShape::getCachedCollisionData() { - return &mCachedCollisionData; + return &mCachedCollisionData; } // Return the collision shape /** - * @return Pointer to the internal collision shape + * @return Pointer to the int32_ternal collision shape */ inline const CollisionShape* ProxyShape::getCollisionShape() const { - return mCollisionShape; + return mCollisionShape; } // Return the parent body @@ -195,23 +174,23 @@ inline const CollisionShape* ProxyShape::getCollisionShape() const { * @return Pointer to the parent body */ inline CollisionBody* ProxyShape::getBody() const { - return mBody; + return mBody; } // Return the mass of the collision shape /** * @return Mass of the collision shape (in kilograms) */ -inline decimal ProxyShape::getMass() const { - return mMass; +inline float ProxyShape::getMass() const { + return mMass; } // Return a pointer to the user data attached to this body /** - * @return A pointer to the user data stored into the proxy shape + * @return A pointer to the user data stored int32_to the proxy shape */ inline void* ProxyShape::getUserData() const { - return mUserData; + return mUserData; } // Attach user data to this body @@ -219,36 +198,36 @@ inline void* ProxyShape::getUserData() const { * @param userData Pointer to the user data you want to store within the proxy shape */ inline void ProxyShape::setUserData(void* userData) { - mUserData = userData; + mUserData = userData; } // Return the local to parent body transform /** * @return The transformation that transforms the local-space of the collision shape - * to the local-space of the parent body + * to the local-space of the parent body */ inline const Transform& ProxyShape::getLocalToBodyTransform() const { - return mLocalToBodyTransform; + return mLocalToBodyTransform; } // Set the local to parent body transform inline void ProxyShape::setLocalToBodyTransform(const Transform& transform) { - mLocalToBodyTransform = transform; + mLocalToBodyTransform = transform; - mBody->setIsSleeping(false); + mBody->setIsSleeping(false); - // Notify the body that the proxy shape has to be updated in the broad-phase - mBody->updateProxyShapeInBroadPhase(this, true); + // Notify the body that the proxy shape has to be updated in the broad-phase + mBody->updateProxyShapeInBroadPhase(this, true); } // Return the local to world transform /** * @return The transformation that transforms the local-space of the collision - * shape to the world-space + * shape to the world-space */ inline const Transform ProxyShape::getLocalToWorldTransform() const { - return mBody->mTransform * mLocalToBodyTransform; + return mBody->mTransform * mLocalToBodyTransform; } // Return the next proxy shape in the linked list of proxy shapes @@ -256,7 +235,7 @@ inline const Transform ProxyShape::getLocalToWorldTransform() const { * @return Pointer to the next proxy shape in the linked list of proxy shapes */ inline ProxyShape* ProxyShape::getNext() { - return mNext; + return mNext; } // Return the next proxy shape in the linked list of proxy shapes @@ -264,7 +243,7 @@ inline ProxyShape* ProxyShape::getNext() { * @return Pointer to the next proxy shape in the linked list of proxy shapes */ inline const ProxyShape* ProxyShape::getNext() const { - return mNext; + return mNext; } // Return the collision category bits @@ -272,7 +251,7 @@ inline const ProxyShape* ProxyShape::getNext() const { * @return The collision category bits mask of the proxy shape */ inline unsigned short ProxyShape::getCollisionCategoryBits() const { - return mCollisionCategoryBits; + return mCollisionCategoryBits; } // Set the collision category bits @@ -280,7 +259,7 @@ inline unsigned short ProxyShape::getCollisionCategoryBits() const { * @param collisionCategoryBits The collision category bits mask of the proxy shape */ inline void ProxyShape::setCollisionCategoryBits(unsigned short collisionCategoryBits) { - mCollisionCategoryBits = collisionCategoryBits; + mCollisionCategoryBits = collisionCategoryBits; } // Return the collision bits mask @@ -288,7 +267,7 @@ inline void ProxyShape::setCollisionCategoryBits(unsigned short collisionCategor * @return The bits mask that specifies with which collision category this shape will collide */ inline unsigned short ProxyShape::getCollideWithMaskBits() const { - return mCollideWithMaskBits; + return mCollideWithMaskBits; } // Set the collision bits mask @@ -296,7 +275,7 @@ inline unsigned short ProxyShape::getCollideWithMaskBits() const { * @param collideWithMaskBits The bits mask that specifies with which collision category this shape will collide */ inline void ProxyShape::setCollideWithMaskBits(unsigned short collideWithMaskBits) { - mCollideWithMaskBits = collideWithMaskBits; + mCollideWithMaskBits = collideWithMaskBits; } // Return the local scaling vector of the collision shape @@ -304,7 +283,7 @@ inline void ProxyShape::setCollideWithMaskBits(unsigned short collideWithMaskBit * @return The local scaling vector */ inline Vector3 ProxyShape::getLocalScaling() const { - return mCollisionShape->getScaling(); + return mCollisionShape->getScaling(); } // Set the local scaling vector of the collision shape @@ -313,15 +292,14 @@ inline Vector3 ProxyShape::getLocalScaling() const { */ inline void ProxyShape::setLocalScaling(const Vector3& scaling) { - // Set the local scaling of the collision shape - mCollisionShape->setLocalScaling(scaling); + // Set the local scaling of the collision shape + mCollisionShape->setLocalScaling(scaling); - mBody->setIsSleeping(false); + mBody->setIsSleeping(false); - // Notify the body that the proxy shape has to be updated in the broad-phase - mBody->updateProxyShapeInBroadPhase(this, true); + // Notify the body that the proxy shape has to be updated in the broad-phase + mBody->updateProxyShapeInBroadPhase(this, true); } } -#endif diff --git a/ephysics/collision/RaycastInfo.cpp b/ephysics/collision/RaycastInfo.cpp index a70d006..7e9b5d5 100644 --- a/ephysics/collision/RaycastInfo.cpp +++ b/ephysics/collision/RaycastInfo.cpp @@ -1,49 +1,30 @@ -/******************************************************************************** -* 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. * -* * -********************************************************************************/ +/** @file + * @author Daniel Chappuis + * @copyright 2010-2016 Daniel Chappuis + * @license BSD 3 clauses (see license file) + */ // Libraries -#include + #include #include using namespace reactphysics3d; // Ray cast test against a proxy shape -decimal RaycastTest::raycastAgainstShape(ProxyShape* shape, const Ray& ray) { +float RaycastTest::raycastAgainstShape(ProxyShape* shape, const Ray& ray) { - // Ray casting test against the collision shape - RaycastInfo raycastInfo; - bool isHit = shape->raycast(ray, raycastInfo); + // Ray casting test against the collision shape + RaycastInfo raycastInfo; + bool isHit = shape->raycast(ray, raycastInfo); - // If the ray hit the collision shape - if (isHit) { + // If the ray hit the collision shape + if (isHit) { - // Report the hit to the user and return the - // user hit fraction value - return userCallback->notifyRaycastHit(raycastInfo); - } + // Report the hit to the user and return the + // user hit fraction value + return userCallback->notifyRaycastHit(raycastInfo); + } - return ray.maxFraction; + return ray.maxFraction; } diff --git a/ephysics/collision/RaycastInfo.h b/ephysics/collision/RaycastInfo.h index ae9c16b..93d7c85 100644 --- a/ephysics/collision/RaycastInfo.h +++ b/ephysics/collision/RaycastInfo.h @@ -1,30 +1,9 @@ -/******************************************************************************** -* 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 REACTPHYSICS3D_RAYCAST_INFO_H -#define REACTPHYSICS3D_RAYCAST_INFO_H +/** @file + * @author Daniel Chappuis + * @copyright 2010-2016 Daniel Chappuis + * @license BSD 3 clauses (see license file) + */ +#pragma once // Libraries #include @@ -44,53 +23,53 @@ class CollisionShape; */ struct RaycastInfo { - private: + private: - // -------------------- Methods -------------------- // + // -------------------- Methods -------------------- // - /// Private copy constructor - RaycastInfo(const RaycastInfo& raycastInfo); + /// Private copy constructor + RaycastInfo(const RaycastInfo& raycastInfo); - /// Private assignment operator - RaycastInfo& operator=(const RaycastInfo& raycastInfo); + /// Private assignment operator + RaycastInfo& operator=(const RaycastInfo& raycastInfo); - public: + public: - // -------------------- Attributes -------------------- // + // -------------------- Attributes -------------------- // - /// Hit point in world-space coordinates - Vector3 worldPoint; + /// Hit point in world-space coordinates + Vector3 worldPoint; - /// Surface normal at hit point in world-space coordinates - Vector3 worldNormal; + /// Surface normal at hit point in world-space coordinates + Vector3 worldNormal; - /// Fraction distance of the hit point between point1 and point2 of the ray - /// The hit point "p" is such that p = point1 + hitFraction * (point2 - point1) - decimal hitFraction; + /// Fraction distance of the hit point between point1 and point2 of the ray + /// The hit point "p" is such that p = point1 + hitFraction * (point2 - point1) + float hitFraction; - /// Mesh subpart index that has been hit (only used for triangles mesh and -1 otherwise) - int meshSubpart; + /// Mesh subpart index that has been hit (only used for triangles mesh and -1 otherwise) + int32_t meshSubpart; - /// Hit triangle index (only used for triangles mesh and -1 otherwise) - int triangleIndex; + /// Hit triangle index (only used for triangles mesh and -1 otherwise) + int32_t triangleIndex; - /// Pointer to the hit collision body - CollisionBody* body; + /// Pointer to the hit collision body + CollisionBody* body; - /// Pointer to the hit proxy collision shape - ProxyShape* proxyShape; + /// Pointer to the hit proxy collision shape + ProxyShape* proxyShape; - // -------------------- Methods -------------------- // + // -------------------- Methods -------------------- // - /// Constructor - RaycastInfo() : meshSubpart(-1), triangleIndex(-1), body(NULL), proxyShape(NULL) { + /// Constructor + RaycastInfo() : meshSubpart(-1), triangleIndex(-1), body(NULL), proxyShape(NULL) { - } + } - /// Destructor - virtual ~RaycastInfo() { + /// Destructor + virtual ~RaycastInfo() { - } + } }; // Class RaycastCallback @@ -102,51 +81,50 @@ struct RaycastInfo { */ class RaycastCallback { - public: + public: - // -------------------- Methods -------------------- // + // -------------------- Methods -------------------- // - /// Destructor - virtual ~RaycastCallback() { + /// Destructor + virtual ~RaycastCallback() { - } + } - /// This method will be called for each ProxyShape that is hit by the - /// ray. You cannot make any assumptions about the order of the - /// calls. You should use the return value to control the continuation - /// of the ray. The returned value is the next maxFraction value to use. - /// If you return a fraction of 0.0, it means that the raycast should - /// terminate. If you return a fraction of 1.0, it indicates that the - /// ray is not clipped and the ray cast should continue as if no hit - /// occurred. If you return the fraction in the parameter (hitFraction - /// value in the RaycastInfo object), the current ray will be clipped - /// to this fraction in the next queries. If you return -1.0, it will - /// ignore this ProxyShape and continue the ray cast. - /** - * @param raycastInfo Information about the raycast hit - * @return Value that controls the continuation of the ray after a hit - */ - virtual decimal notifyRaycastHit(const RaycastInfo& raycastInfo)=0; + /// This method will be called for each ProxyShape that is hit by the + /// ray. You cannot make any assumptions about the order of the + /// calls. You should use the return value to control the continuation + /// of the ray. The returned value is the next maxFraction value to use. + /// If you return a fraction of 0.0, it means that the raycast should + /// terminate. If you return a fraction of 1.0, it indicates that the + /// ray is not clipped and the ray cast should continue as if no hit + /// occurred. If you return the fraction in the parameter (hitFraction + /// value in the RaycastInfo object), the current ray will be clipped + /// to this fraction in the next queries. If you return -1.0, it will + /// ignore this ProxyShape and continue the ray cast. + /** + * @param raycastInfo Information about the raycast hit + * @return Value that controls the continuation of the ray after a hit + */ + virtual float notifyRaycastHit(const RaycastInfo& raycastInfo)=0; }; /// Structure RaycastTest struct RaycastTest { - public: + public: - /// User callback class - RaycastCallback* userCallback; + /// User callback class + RaycastCallback* userCallback; - /// Constructor - RaycastTest(RaycastCallback* callback) { - userCallback = callback; - } + /// Constructor + RaycastTest(RaycastCallback* callback) { + userCallback = callback; + } - /// Ray cast test against a proxy shape - decimal raycastAgainstShape(ProxyShape* shape, const Ray& ray); + /// Ray cast test against a proxy shape + float raycastAgainstShape(ProxyShape* shape, const Ray& ray); }; } -#endif diff --git a/ephysics/collision/TriangleMesh.cpp b/ephysics/collision/TriangleMesh.cpp index 647154d..5d68963 100644 --- a/ephysics/collision/TriangleMesh.cpp +++ b/ephysics/collision/TriangleMesh.cpp @@ -1,27 +1,8 @@ -/******************************************************************************** -* 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. * -* * -********************************************************************************/ +/** @file + * @author Daniel Chappuis + * @copyright 2010-2016 Daniel Chappuis + * @license BSD 3 clauses (see license file) + */ // Libraries #include diff --git a/ephysics/collision/TriangleMesh.h b/ephysics/collision/TriangleMesh.h index 8c1bbcd..386433f 100644 --- a/ephysics/collision/TriangleMesh.h +++ b/ephysics/collision/TriangleMesh.h @@ -1,30 +1,9 @@ -/******************************************************************************** -* 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 REACTPHYSICS3D_TRIANGLE_MESH_H -#define REACTPHYSICS3D_TRIANGLE_MESH_H +/** @file + * @author Daniel Chappuis + * @copyright 2010-2016 Daniel Chappuis + * @license BSD 3 clauses (see license file) + */ +#pragma once // Libraries #include @@ -43,46 +22,45 @@ namespace reactphysics3d { */ class TriangleMesh { - protected: + protected: - /// All the triangle arrays of the mesh (one triangle array per part) - std::vector mTriangleArrays; + /// All the triangle arrays of the mesh (one triangle array per part) + std::vector mTriangleArrays; - public: + public: - /// Constructor - TriangleMesh(); + /// Constructor + TriangleMesh(); - /// Destructor - virtual ~TriangleMesh(); + /// Destructor + virtual ~TriangleMesh(); - /// Add a subpart of the mesh - void addSubpart(TriangleVertexArray* triangleVertexArray); + /// Add a subpart of the mesh + void addSubpart(TriangleVertexArray* triangleVertexArray); - /// Return a pointer to a given subpart (triangle vertex array) of the mesh - TriangleVertexArray* getSubpart(uint indexSubpart) const; + /// Return a pointer to a given subpart (triangle vertex array) of the mesh + TriangleVertexArray* getSubpart(uint32_t indexSubpart) const; - /// Return the number of subparts of the mesh - uint getNbSubparts() const; + /// Return the number of subparts of the mesh + uint32_t getNbSubparts() const; }; // Add a subpart of the mesh inline void TriangleMesh::addSubpart(TriangleVertexArray* triangleVertexArray) { - mTriangleArrays.push_back(triangleVertexArray ); + mTriangleArrays.push_back(triangleVertexArray ); } // Return a pointer to a given subpart (triangle vertex array) of the mesh -inline TriangleVertexArray* TriangleMesh::getSubpart(uint indexSubpart) const { +inline TriangleVertexArray* TriangleMesh::getSubpart(uint32_t indexSubpart) const { assert(indexSubpart < mTriangleArrays.size()); return mTriangleArrays[indexSubpart]; } // Return the number of subparts of the mesh -inline uint TriangleMesh::getNbSubparts() const { - return mTriangleArrays.size(); +inline uint32_t TriangleMesh::getNbSubparts() const { + return mTriangleArrays.size(); } } -#endif diff --git a/ephysics/collision/TriangleVertexArray.cpp b/ephysics/collision/TriangleVertexArray.cpp index e1e60b7..b0fe3be 100644 --- a/ephysics/collision/TriangleVertexArray.cpp +++ b/ephysics/collision/TriangleVertexArray.cpp @@ -1,27 +1,8 @@ -/******************************************************************************** -* 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. * -* * -********************************************************************************/ +/** @file + * @author Daniel Chappuis + * @copyright 2010-2016 Daniel Chappuis + * @license BSD 3 clauses (see license file) + */ // Libraries #include @@ -29,7 +10,7 @@ using namespace reactphysics3d; // Constructor -/// Note that your data will not be copied into the TriangleVertexArray and +/// Note that your data will not be copied int32_to the TriangleVertexArray and /// therefore, you need to make sure that those data are always valid during /// the lifetime of the TriangleVertexArray. /** @@ -40,19 +21,19 @@ using namespace reactphysics3d; * @param indexesStart Pointer to the first triangle index * @param indexesStride Number of bytes between the beginning of two consecutive triangle indices * @param vertexDataType Type of data for the vertices (float, double) - * @param indexDataType Type of data for the indices (short, int) + * @param indexDataType Type of data for the indices (short, int32_t) */ -TriangleVertexArray::TriangleVertexArray(uint nbVertices, void* verticesStart, int verticesStride, - uint nbTriangles, void* indexesStart, int indexesStride, - VertexDataType vertexDataType, IndexDataType indexDataType) { - m_numberVertices = nbVertices; - m_verticesStart = reinterpret_cast(verticesStart); - m_verticesStride = verticesStride; - mNbTriangles = nbTriangles; - mIndicesStart = reinterpret_cast(indexesStart); - mIndicesStride = indexesStride; - mVertexDataType = vertexDataType; - mIndexDataType = indexDataType; +TriangleVertexArray::TriangleVertexArray(uint32_t nbVertices, void* verticesStart, int32_t verticesStride, + uint32_t nbTriangles, void* indexesStart, int32_t indexesStride, + VertexDataType vertexDataType, IndexDataType indexDataType) { + m_numberVertices = nbVertices; + m_verticesStart = reinterpret_cast(verticesStart); + m_verticesStride = verticesStride; + mNbTriangles = nbTriangles; + mIndicesStart = reinterpret_cast(indexesStart); + mIndicesStride = indexesStride; + mVertexDataType = vertexDataType; + mIndexDataType = indexDataType; } // Destructor diff --git a/ephysics/collision/TriangleVertexArray.h b/ephysics/collision/TriangleVertexArray.h index 070f7e3..daf3c4b 100644 --- a/ephysics/collision/TriangleVertexArray.h +++ b/ephysics/collision/TriangleVertexArray.h @@ -1,30 +1,9 @@ -/******************************************************************************** -* 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 REACTPHYSICS3D_TRIANGLE_VERTEX_ARRAY_H -#define REACTPHYSICS3D_TRIANGLE_VERTEX_ARRAY_H +/** @file + * @author Daniel Chappuis + * @copyright 2010-2016 Daniel Chappuis + * @license BSD 3 clauses (see license file) + */ +#pragma once // Libraries #include @@ -36,87 +15,87 @@ namespace reactphysics3d { * This class is used to describe the vertices and faces of a triangular mesh. * A TriangleVertexArray represents a continuous array of vertices and indexes * of a triangular mesh. When you create a TriangleVertexArray, no data is copied - * into the array. It only stores pointer to the data. The purpose is to allow + * int32_to the array. It only stores pointer to the data. The purpose is to allow * the user to share vertices data between the physics engine and the rendering * part. Therefore, make sure that the data pointed by a TriangleVertexArray * remains valid during the TriangleVertexArray life. */ class TriangleVertexArray { - public: + public: - /// Data type for the vertices in the array - enum VertexDataType {VERTEX_FLOAT_TYPE, VERTEX_DOUBLE_TYPE}; + /// Data type for the vertices in the array + enum VertexDataType {VERTEX_FLOAT_TYPE, VERTEX_DOUBLE_TYPE}; - /// Data type for the indices in the array - enum IndexDataType {INDEX_INTEGER_TYPE, INDEX_SHORT_TYPE}; + /// Data type for the indices in the array + enum IndexDataType {INDEX_INTEGER_TYPE, INDEX_SHORT_TYPE}; - protected: + protected: - /// Number of vertices in the array - uint m_numberVertices; + /// Number of vertices in the array + uint32_t m_numberVertices; - /// Pointer to the first vertex value in the array - unsigned char* m_verticesStart; + /// Pointer to the first vertex value in the array + unsigned char* m_verticesStart; - /// Stride (number of bytes) between the beginning of two vertices - /// values in the array - int m_verticesStride; + /// Stride (number of bytes) between the beginning of two vertices + /// values in the array + int32_t m_verticesStride; - /// Number of triangles in the array - uint mNbTriangles; + /// Number of triangles in the array + uint32_t mNbTriangles; - /// Pointer to the first vertex index of the array - unsigned char* mIndicesStart; + /// Pointer to the first vertex index of the array + unsigned char* mIndicesStart; - /// Stride (number of bytes) between the beginning of two indices in - /// the array - int mIndicesStride; + /// Stride (number of bytes) between the beginning of two indices in + /// the array + int32_t mIndicesStride; - /// Data type of the vertices in the array - VertexDataType mVertexDataType; + /// Data type of the vertices in the array + VertexDataType mVertexDataType; - /// Data type of the indices in the array - IndexDataType mIndexDataType; + /// Data type of the indices in the array + IndexDataType mIndexDataType; - public: + public: - /// Constructor - TriangleVertexArray(uint nbVertices, void* verticesStart, int verticesStride, - uint nbTriangles, void* indexesStart, int indexesStride, - VertexDataType vertexDataType, IndexDataType indexDataType); + /// Constructor + TriangleVertexArray(uint32_t nbVertices, void* verticesStart, int32_t verticesStride, + uint32_t nbTriangles, void* indexesStart, int32_t indexesStride, + VertexDataType vertexDataType, IndexDataType indexDataType); - /// Destructor - virtual ~TriangleVertexArray(); + /// Destructor + virtual ~TriangleVertexArray(); - /// Return the vertex data type - VertexDataType getVertexDataType() const; + /// Return the vertex data type + VertexDataType getVertexDataType() const; - /// Return the index data type - IndexDataType getIndexDataType() const; + /// Return the index data type + IndexDataType getIndexDataType() const; - /// Return the number of vertices - uint getNbVertices() const; + /// Return the number of vertices + uint32_t getNbVertices() const; - /// Return the number of triangles - uint getNbTriangles() const; + /// Return the number of triangles + uint32_t getNbTriangles() const; - /// Return the vertices stride (number of bytes) - int getVerticesStride() const; + /// Return the vertices stride (number of bytes) + int32_t getVerticesStride() const; - /// Return the indices stride (number of bytes) - int getIndicesStride() const; + /// Return the indices stride (number of bytes) + int32_t getIndicesStride() const; - /// Return the pointer to the start of the vertices array - unsigned char* getVerticesStart() const; + /// Return the pointer to the start of the vertices array + unsigned char* getVerticesStart() const; - /// Return the pointer to the start of the indices array - unsigned char* getIndicesStart() const; + /// Return the pointer to the start of the indices array + unsigned char* getIndicesStart() const; }; // Return the vertex data type inline TriangleVertexArray::VertexDataType TriangleVertexArray::getVertexDataType() const { - return mVertexDataType; + return mVertexDataType; } // Return the index data type @@ -125,36 +104,35 @@ inline TriangleVertexArray::IndexDataType TriangleVertexArray::getIndexDataType( } // Return the number of vertices -inline uint TriangleVertexArray::getNbVertices() const { - return m_numberVertices; +inline uint32_t TriangleVertexArray::getNbVertices() const { + return m_numberVertices; } // Return the number of triangles -inline uint TriangleVertexArray::getNbTriangles() const { - return mNbTriangles; +inline uint32_t TriangleVertexArray::getNbTriangles() const { + return mNbTriangles; } // Return the vertices stride (number of bytes) -inline int TriangleVertexArray::getVerticesStride() const { - return m_verticesStride; +inline int32_t TriangleVertexArray::getVerticesStride() const { + return m_verticesStride; } // Return the indices stride (number of bytes) -inline int TriangleVertexArray::getIndicesStride() const { - return mIndicesStride; +inline int32_t TriangleVertexArray::getIndicesStride() const { + return mIndicesStride; } // Return the pointer to the start of the vertices array inline unsigned char* TriangleVertexArray::getVerticesStart() const { - return m_verticesStart; + return m_verticesStart; } // Return the pointer to the start of the indices array inline unsigned char* TriangleVertexArray::getIndicesStart() const { - return mIndicesStart; + return mIndicesStart; } } -#endif diff --git a/ephysics/collision/broadphase/BroadPhaseAlgorithm.cpp b/ephysics/collision/broadphase/BroadPhaseAlgorithm.cpp index abe046a..fcac0b7 100644 --- a/ephysics/collision/broadphase/BroadPhaseAlgorithm.cpp +++ b/ephysics/collision/broadphase/BroadPhaseAlgorithm.cpp @@ -1,27 +1,8 @@ -/******************************************************************************** -* 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. * -* * -********************************************************************************/ +/** @file + * @author Daniel Chappuis + * @copyright 2010-2016 Daniel Chappuis + * @license BSD 3 clauses (see license file) + */ // Libraries #include @@ -33,260 +14,260 @@ using namespace reactphysics3d; // Constructor BroadPhaseAlgorithm::BroadPhaseAlgorithm(CollisionDetection& collisionDetection) - :m_dynamicAABBTree(DYNAMIC_TREE_AABB_GAP), mNbMovedShapes(0), mNbAllocatedMovedShapes(8), - mNbNonUsedMovedShapes(0), mNbPotentialPairs(0), mNbAllocatedPotentialPairs(8), - mCollisionDetection(collisionDetection) { + :m_dynamicAABBTree(DYNAMIC_TREE_AABB_GAP), mNbMovedShapes(0), mNbAllocatedMovedShapes(8), + mNbNonUsedMovedShapes(0), mNbPotentialPairs(0), mNbAllocatedPotentialPairs(8), + mCollisionDetection(collisionDetection) { - // Allocate memory for the array of non-static proxy shapes IDs - mMovedShapes = (int*) malloc(mNbAllocatedMovedShapes * sizeof(int)); - assert(mMovedShapes != NULL); + // Allocate memory for the array of non-static proxy shapes IDs + mMovedShapes = (int32_t*) malloc(mNbAllocatedMovedShapes * sizeof(int32_t)); + assert(mMovedShapes != NULL); - // Allocate memory for the array of potential overlapping pairs - mPotentialPairs = (BroadPhasePair*) malloc(mNbAllocatedPotentialPairs * sizeof(BroadPhasePair)); - assert(mPotentialPairs != NULL); + // Allocate memory for the array of potential overlapping pairs + mPotentialPairs = (BroadPhasePair*) malloc(mNbAllocatedPotentialPairs * sizeof(BroadPhasePair)); + assert(mPotentialPairs != NULL); } // Destructor BroadPhaseAlgorithm::~BroadPhaseAlgorithm() { - // Release the memory for the array of non-static proxy shapes IDs - free(mMovedShapes); + // Release the memory for the array of non-static proxy shapes IDs + free(mMovedShapes); - // Release the memory for the array of potential overlapping pairs - free(mPotentialPairs); + // Release the memory for the array of potential overlapping pairs + free(mPotentialPairs); } // Add a collision shape in the array of shapes that have moved in the last simulation step // and that need to be tested again for broad-phase overlapping. -void BroadPhaseAlgorithm::addMovedCollisionShape(int broadPhaseID) { +void BroadPhaseAlgorithm::addMovedCollisionShape(int32_t broadPhaseID) { - // Allocate more elements in the array of shapes that have moved if necessary - if (mNbAllocatedMovedShapes == mNbMovedShapes) { - mNbAllocatedMovedShapes *= 2; - int* oldArray = mMovedShapes; - mMovedShapes = (int*) malloc(mNbAllocatedMovedShapes * sizeof(int)); - assert(mMovedShapes != NULL); - memcpy(mMovedShapes, oldArray, mNbMovedShapes * sizeof(int)); - free(oldArray); - } + // Allocate more elements in the array of shapes that have moved if necessary + if (mNbAllocatedMovedShapes == mNbMovedShapes) { + mNbAllocatedMovedShapes *= 2; + int32_t* oldArray = mMovedShapes; + mMovedShapes = (int32_t*) malloc(mNbAllocatedMovedShapes * sizeof(int32_t)); + assert(mMovedShapes != NULL); + memcpy(mMovedShapes, oldArray, mNbMovedShapes * sizeof(int32_t)); + free(oldArray); + } - // Store the broad-phase ID into the array of shapes that have moved - assert(mNbMovedShapes < mNbAllocatedMovedShapes); - assert(mMovedShapes != NULL); - mMovedShapes[mNbMovedShapes] = broadPhaseID; - mNbMovedShapes++; + // Store the broad-phase ID int32_to the array of shapes that have moved + assert(mNbMovedShapes < mNbAllocatedMovedShapes); + assert(mMovedShapes != NULL); + mMovedShapes[mNbMovedShapes] = broadPhaseID; + mNbMovedShapes++; } // Remove a collision shape from the array of shapes that have moved in the last simulation step // and that need to be tested again for broad-phase overlapping. -void BroadPhaseAlgorithm::removeMovedCollisionShape(int broadPhaseID) { +void BroadPhaseAlgorithm::removeMovedCollisionShape(int32_t broadPhaseID) { - assert(mNbNonUsedMovedShapes <= mNbMovedShapes); + assert(mNbNonUsedMovedShapes <= mNbMovedShapes); - // If less than the quarter of allocated elements of the non-static shapes IDs array - // are used, we release some allocated memory - if ((mNbMovedShapes - mNbNonUsedMovedShapes) < mNbAllocatedMovedShapes / 4 && - mNbAllocatedMovedShapes > 8) { + // If less than the quarter of allocated elements of the non-static shapes IDs array + // are used, we release some allocated memory + if ((mNbMovedShapes - mNbNonUsedMovedShapes) < mNbAllocatedMovedShapes / 4 && + mNbAllocatedMovedShapes > 8) { - mNbAllocatedMovedShapes /= 2; - int* oldArray = mMovedShapes; - mMovedShapes = (int*) malloc(mNbAllocatedMovedShapes * sizeof(int)); - assert(mMovedShapes != NULL); - uint nbElements = 0; - for (uint i=0; imBroadPhaseID = nodeId; + // Set the broad-phase ID of the proxy shape + proxyShape->mBroadPhaseID = nodeId; - // Add the collision shape into the array of bodies that have moved (or have been created) - // during the last simulation step - addMovedCollisionShape(proxyShape->mBroadPhaseID); + // Add the collision shape int32_to the array of bodies that have moved (or have been created) + // during the last simulation step + addMovedCollisionShape(proxyShape->mBroadPhaseID); } // Remove a proxy collision shape from the broad-phase collision detection void BroadPhaseAlgorithm::removeProxyCollisionShape(ProxyShape* proxyShape) { - int broadPhaseID = proxyShape->mBroadPhaseID; + int32_t broadPhaseID = proxyShape->mBroadPhaseID; - // Remove the collision shape from the dynamic AABB tree - m_dynamicAABBTree.removeObject(broadPhaseID); + // Remove the collision shape from the dynamic AABB tree + m_dynamicAABBTree.removeObject(broadPhaseID); - // Remove the collision shape into the array of shapes that have moved (or have been created) - // during the last simulation step - removeMovedCollisionShape(broadPhaseID); + // Remove the collision shape int32_to the array of shapes that have moved (or have been created) + // during the last simulation step + removeMovedCollisionShape(broadPhaseID); } // Notify the broad-phase that a collision shape has moved and need to be updated void BroadPhaseAlgorithm::updateProxyCollisionShape(ProxyShape* proxyShape, const AABB& aabb, - const Vector3& displacement, bool forceReinsert) { + const Vector3& displacement, bool forceReinsert) { - int broadPhaseID = proxyShape->mBroadPhaseID; + int32_t broadPhaseID = proxyShape->mBroadPhaseID; - assert(broadPhaseID >= 0); + assert(broadPhaseID >= 0); - // Update the dynamic AABB tree according to the movement of the collision shape - bool hasBeenReInserted = m_dynamicAABBTree.updateObject(broadPhaseID, aabb, displacement, forceReinsert); + // Update the dynamic AABB tree according to the movement of the collision shape + bool hasBeenReInserted = m_dynamicAABBTree.updateObject(broadPhaseID, aabb, displacement, forceReinsert); - // If the collision shape has moved out of its fat AABB (and therefore has been reinserted - // into the tree). - if (hasBeenReInserted) { + // If the collision shape has moved out of its fat AABB (and therefore has been reinserted + // int32_to the tree). + if (hasBeenReInserted) { - // Add the collision shape into the array of shapes that have moved (or have been created) - // during the last simulation step - addMovedCollisionShape(broadPhaseID); - } + // Add the collision shape int32_to the array of shapes that have moved (or have been created) + // during the last simulation step + addMovedCollisionShape(broadPhaseID); + } } // Compute all the overlapping pairs of collision shapes void BroadPhaseAlgorithm::computeOverlappingPairs() { - // Reset the potential overlapping pairs - mNbPotentialPairs = 0; + // Reset the potential overlapping pairs + mNbPotentialPairs = 0; - // For all collision shapes that have moved (or have been created) during the - // last simulation step - for (uint i=0; icollisionShape1ID != pair->collisionShape2ID); + assert(pair->collisionShape1ID != pair->collisionShape2ID); - // Get the two collision shapes of the pair - ProxyShape* shape1 = static_cast(m_dynamicAABBTree.getNodeDataPointer(pair->collisionShape1ID)); - ProxyShape* shape2 = static_cast(m_dynamicAABBTree.getNodeDataPointer(pair->collisionShape2ID)); + // Get the two collision shapes of the pair + ProxyShape* shape1 = static_cast(m_dynamicAABBTree.getNodeDataPointer(pair->collisionShape1ID)); + ProxyShape* shape2 = static_cast(m_dynamicAABBTree.getNodeDataPointer(pair->collisionShape2ID)); - // Notify the collision detection about the overlapping pair - mCollisionDetection.broadPhaseNotifyOverlappingPair(shape1, shape2); + // Notify the collision detection about the overlapping pair + mCollisionDetection.broadPhaseNotifyOverlappingPair(shape1, shape2); - // Skip the duplicate overlapping pairs - while (i < mNbPotentialPairs) { + // Skip the duplicate overlapping pairs + while (i < mNbPotentialPairs) { - // Get the next pair - BroadPhasePair* nextPair = mPotentialPairs + i; + // Get the next pair + BroadPhasePair* nextPair = mPotentialPairs + i; - // If the next pair is different from the previous one, we stop skipping pairs - if (nextPair->collisionShape1ID != pair->collisionShape1ID || - nextPair->collisionShape2ID != pair->collisionShape2ID) { - break; - } - i++; - } - } + // If the next pair is different from the previous one, we stop skipping pairs + if (nextPair->collisionShape1ID != pair->collisionShape1ID || + nextPair->collisionShape2ID != pair->collisionShape2ID) { + break; + } + i++; + } + } - // If the number of potential overlapping pairs is less than the quarter of allocated - // number of overlapping pairs - if (mNbPotentialPairs < mNbAllocatedPotentialPairs / 4 && mNbPotentialPairs > 8) { + // If the number of potential overlapping pairs is less than the quarter of allocated + // number of overlapping pairs + if (mNbPotentialPairs < mNbAllocatedPotentialPairs / 4 && mNbPotentialPairs > 8) { - // Reduce the number of allocated potential overlapping pairs - BroadPhasePair* oldPairs = mPotentialPairs; - mNbAllocatedPotentialPairs /= 2; - mPotentialPairs = (BroadPhasePair*) malloc(mNbAllocatedPotentialPairs * sizeof(BroadPhasePair)); - assert(mPotentialPairs); - memcpy(mPotentialPairs, oldPairs, mNbPotentialPairs * sizeof(BroadPhasePair)); - free(oldPairs); - } + // Reduce the number of allocated potential overlapping pairs + BroadPhasePair* oldPairs = mPotentialPairs; + mNbAllocatedPotentialPairs /= 2; + mPotentialPairs = (BroadPhasePair*) malloc(mNbAllocatedPotentialPairs * sizeof(BroadPhasePair)); + assert(mPotentialPairs); + memcpy(mPotentialPairs, oldPairs, mNbPotentialPairs * sizeof(BroadPhasePair)); + free(oldPairs); + } } // Notify the broad-phase about a potential overlapping pair in the dynamic AABB tree -void BroadPhaseAlgorithm::notifyOverlappingNodes(int node1ID, int node2ID) { +void BroadPhaseAlgorithm::notifyOverlappingNodes(int32_t node1ID, int32_t node2ID) { - // If both the nodes are the same, we do not create store the overlapping pair - if (node1ID == node2ID) return; + // If both the nodes are the same, we do not create store the overlapping pair + if (node1ID == node2ID) return; - // If we need to allocate more memory for the array of potential overlapping pairs - if (mNbPotentialPairs == mNbAllocatedPotentialPairs) { + // If we need to allocate more memory for the array of potential overlapping pairs + if (mNbPotentialPairs == mNbAllocatedPotentialPairs) { - // Allocate more memory for the array of potential pairs - BroadPhasePair* oldPairs = mPotentialPairs; - mNbAllocatedPotentialPairs *= 2; - mPotentialPairs = (BroadPhasePair*) malloc(mNbAllocatedPotentialPairs * sizeof(BroadPhasePair)); - assert(mPotentialPairs); - memcpy(mPotentialPairs, oldPairs, mNbPotentialPairs * sizeof(BroadPhasePair)); - free(oldPairs); - } + // Allocate more memory for the array of potential pairs + BroadPhasePair* oldPairs = mPotentialPairs; + mNbAllocatedPotentialPairs *= 2; + mPotentialPairs = (BroadPhasePair*) malloc(mNbAllocatedPotentialPairs * sizeof(BroadPhasePair)); + assert(mPotentialPairs); + memcpy(mPotentialPairs, oldPairs, mNbPotentialPairs * sizeof(BroadPhasePair)); + free(oldPairs); + } - // Add the new potential pair into the array of potential overlapping pairs - mPotentialPairs[mNbPotentialPairs].collisionShape1ID = std::min(node1ID, node2ID); - mPotentialPairs[mNbPotentialPairs].collisionShape2ID = std::max(node1ID, node2ID); - mNbPotentialPairs++; + // Add the new potential pair int32_to the array of potential overlapping pairs + mPotentialPairs[mNbPotentialPairs].collisionShape1ID = std::min(node1ID, node2ID); + mPotentialPairs[mNbPotentialPairs].collisionShape2ID = std::max(node1ID, node2ID); + mNbPotentialPairs++; } // Called when a overlapping node has been found during the call to // DynamicAABBTree:reportAllShapesOverlappingWithAABB() -void AABBOverlapCallback::notifyOverlappingNode(int nodeId) { +void AABBOverlapCallback::notifyOverlappingNode(int32_t nodeId) { - mBroadPhaseAlgorithm.notifyOverlappingNodes(mReferenceNodeId, nodeId); + mBroadPhaseAlgorithm.notifyOverlappingNodes(mReferenceNodeId, nodeId); } // Called for a broad-phase shape that has to be tested for raycast -decimal BroadPhaseRaycastCallback::raycastBroadPhaseShape(int32 nodeId, const Ray& ray) { +float BroadPhaseRaycastCallback::raycastBroadPhaseShape(int32_t nodeId, const Ray& ray) { - decimal hitFraction = decimal(-1.0); + float hitFraction = float(-1.0); - // Get the proxy shape from the node - ProxyShape* proxyShape = static_cast(m_dynamicAABBTree.getNodeDataPointer(nodeId)); + // Get the proxy shape from the node + ProxyShape* proxyShape = static_cast(m_dynamicAABBTree.getNodeDataPointer(nodeId)); - // Check if the raycast filtering mask allows raycast against this shape - if ((m_raycastWithCategoryMaskBits & proxyShape->getCollisionCategoryBits()) != 0) { + // Check if the raycast filtering mask allows raycast against this shape + if ((m_raycastWithCategoryMaskBits & proxyShape->getCollisionCategoryBits()) != 0) { - // Ask the collision detection to perform a ray cast test against - // the proxy shape of this node because the ray is overlapping - // with the shape in the broad-phase - hitFraction = m_raycastTest.raycastAgainstShape(proxyShape, ray); - } + // Ask the collision detection to perform a ray cast test against + // the proxy shape of this node because the ray is overlapping + // with the shape in the broad-phase + hitFraction = m_raycastTest.raycastAgainstShape(proxyShape, ray); + } - return hitFraction; + return hitFraction; } diff --git a/ephysics/collision/broadphase/BroadPhaseAlgorithm.h b/ephysics/collision/broadphase/BroadPhaseAlgorithm.h index 6c2297f..eb07845 100644 --- a/ephysics/collision/broadphase/BroadPhaseAlgorithm.h +++ b/ephysics/collision/broadphase/BroadPhaseAlgorithm.h @@ -1,30 +1,9 @@ -/******************************************************************************** -* 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 REACTPHYSICS3D_BROAD_PHASE_ALGORITHM_H -#define REACTPHYSICS3D_BROAD_PHASE_ALGORITHM_H +/** @file + * @author Daniel Chappuis + * @copyright 2010-2016 Daniel Chappuis + * @license BSD 3 clauses (see license file) + */ +#pragma once // Libraries #include @@ -47,40 +26,40 @@ class BroadPhaseAlgorithm; */ struct BroadPhasePair { - // -------------------- Attributes -------------------- // + // -------------------- Attributes -------------------- // - /// Broad-phase ID of the first collision shape - int collisionShape1ID; + /// Broad-phase ID of the first collision shape + int32_t collisionShape1ID; - /// Broad-phase ID of the second collision shape - int collisionShape2ID; + /// Broad-phase ID of the second collision shape + int32_t collisionShape2ID; - // -------------------- Methods -------------------- // + // -------------------- Methods -------------------- // - /// Method used to compare two pairs for sorting algorithm - static bool smallerThan(const BroadPhasePair& pair1, const BroadPhasePair& pair2); + /// Method used to compare two pairs for sorting algorithm + static bool smallerThan(const BroadPhasePair& pair1, const BroadPhasePair& pair2); }; // class AABBOverlapCallback class AABBOverlapCallback : public DynamicAABBTreeOverlapCallback { - private: + private: - BroadPhaseAlgorithm& mBroadPhaseAlgorithm; + BroadPhaseAlgorithm& mBroadPhaseAlgorithm; - int mReferenceNodeId; + int32_t mReferenceNodeId; - public: + public: - // Constructor - AABBOverlapCallback(BroadPhaseAlgorithm& broadPhaseAlgo, int referenceNodeId) - : mBroadPhaseAlgorithm(broadPhaseAlgo), mReferenceNodeId(referenceNodeId) { + // Constructor + AABBOverlapCallback(BroadPhaseAlgorithm& broadPhaseAlgo, int32_t referenceNodeId) + : mBroadPhaseAlgorithm(broadPhaseAlgo), mReferenceNodeId(referenceNodeId) { - } + } - // Called when a overlapping node has been found during the call to - // DynamicAABBTree:reportAllShapesOverlappingWithAABB() - virtual void notifyOverlappingNode(int nodeId); + // Called when a overlapping node has been found during the call to + // DynamicAABBTree:reportAllShapesOverlappingWithAABB() + virtual void notifyOverlappingNode(int32_t nodeId); }; @@ -91,26 +70,26 @@ class AABBOverlapCallback : public DynamicAABBTreeOverlapCallback { */ class BroadPhaseRaycastCallback : public DynamicAABBTreeRaycastCallback { - private : + private : - const DynamicAABBTree& m_dynamicAABBTree; + const DynamicAABBTree& m_dynamicAABBTree; - unsigned short m_raycastWithCategoryMaskBits; + unsigned short m_raycastWithCategoryMaskBits; - RaycastTest& m_raycastTest; + RaycastTest& m_raycastTest; - public: + public: - // Constructor - BroadPhaseRaycastCallback(const DynamicAABBTree& dynamicAABBTree, unsigned short raycastWithCategoryMaskBits, - RaycastTest& raycastTest) - : m_dynamicAABBTree(dynamicAABBTree), m_raycastWithCategoryMaskBits(raycastWithCategoryMaskBits), - m_raycastTest(raycastTest) { + // Constructor + BroadPhaseRaycastCallback(const DynamicAABBTree& dynamicAABBTree, unsigned short raycastWithCategoryMaskBits, + RaycastTest& raycastTest) + : m_dynamicAABBTree(dynamicAABBTree), m_raycastWithCategoryMaskBits(raycastWithCategoryMaskBits), + m_raycastTest(raycastTest) { - } + } - // Called for a broad-phase shape that has to be tested for raycast - virtual decimal raycastBroadPhaseShape(int32 nodeId, const Ray& ray); + // Called for a broad-phase shape that has to be tested for raycast + virtual float raycastBroadPhaseShape(int32_t nodeId, const Ray& ray); }; @@ -124,125 +103,125 @@ class BroadPhaseRaycastCallback : public DynamicAABBTreeRaycastCallback { */ class BroadPhaseAlgorithm { - protected : + protected : - // -------------------- Attributes -------------------- // + // -------------------- Attributes -------------------- // - /// Dynamic AABB tree - DynamicAABBTree m_dynamicAABBTree; + /// Dynamic AABB tree + DynamicAABBTree m_dynamicAABBTree; - /// Array with the broad-phase IDs of all collision shapes that have moved (or have been - /// created) during the last simulation step. Those are the shapes that need to be tested - /// for overlapping in the next simulation step. - int* mMovedShapes; + /// Array with the broad-phase IDs of all collision shapes that have moved (or have been + /// created) during the last simulation step. Those are the shapes that need to be tested + /// for overlapping in the next simulation step. + int32_t* mMovedShapes; - /// Number of collision shapes in the array of shapes that have moved during the last - /// simulation step. - uint mNbMovedShapes; + /// Number of collision shapes in the array of shapes that have moved during the last + /// simulation step. + uint32_t mNbMovedShapes; - /// Number of allocated elements for the array of shapes that have moved during the last - /// simulation step. - uint mNbAllocatedMovedShapes; + /// Number of allocated elements for the array of shapes that have moved during the last + /// simulation step. + uint32_t mNbAllocatedMovedShapes; - /// Number of non-used elements in the array of shapes that have moved during the last - /// simulation step. - uint mNbNonUsedMovedShapes; + /// Number of non-used elements in the array of shapes that have moved during the last + /// simulation step. + uint32_t mNbNonUsedMovedShapes; - /// Temporary array of potential overlapping pairs (with potential duplicates) - BroadPhasePair* mPotentialPairs; + /// Temporary array of potential overlapping pairs (with potential duplicates) + BroadPhasePair* mPotentialPairs; - /// Number of potential overlapping pairs - uint mNbPotentialPairs; + /// Number of potential overlapping pairs + uint32_t mNbPotentialPairs; - /// Number of allocated elements for the array of potential overlapping pairs - uint mNbAllocatedPotentialPairs; + /// Number of allocated elements for the array of potential overlapping pairs + uint32_t mNbAllocatedPotentialPairs; - /// Reference to the collision detection object - CollisionDetection& mCollisionDetection; - - // -------------------- Methods -------------------- // + /// Reference to the collision detection object + CollisionDetection& mCollisionDetection; + + // -------------------- Methods -------------------- // - /// Private copy-constructor - BroadPhaseAlgorithm(const BroadPhaseAlgorithm& algorithm); + /// Private copy-constructor + BroadPhaseAlgorithm(const BroadPhaseAlgorithm& algorithm); - /// Private assignment operator - BroadPhaseAlgorithm& operator=(const BroadPhaseAlgorithm& algorithm); + /// Private assignment operator + BroadPhaseAlgorithm& operator=(const BroadPhaseAlgorithm& algorithm); - public : + public : - // -------------------- Methods -------------------- // + // -------------------- Methods -------------------- // - /// Constructor - BroadPhaseAlgorithm(CollisionDetection& collisionDetection); + /// Constructor + BroadPhaseAlgorithm(CollisionDetection& collisionDetection); - /// Destructor - virtual ~BroadPhaseAlgorithm(); - - /// Add a proxy collision shape into the broad-phase collision detection - void addProxyCollisionShape(ProxyShape* proxyShape, const AABB& aabb); + /// Destructor + virtual ~BroadPhaseAlgorithm(); + + /// Add a proxy collision shape int32_to the broad-phase collision detection + void addProxyCollisionShape(ProxyShape* proxyShape, const AABB& aabb); - /// Remove a proxy collision shape from the broad-phase collision detection - void removeProxyCollisionShape(ProxyShape* proxyShape); + /// Remove a proxy collision shape from the broad-phase collision detection + void removeProxyCollisionShape(ProxyShape* proxyShape); - /// Notify the broad-phase that a collision shape has moved and need to be updated - void updateProxyCollisionShape(ProxyShape* proxyShape, const AABB& aabb, - const Vector3& displacement, bool forceReinsert = false); + /// Notify the broad-phase that a collision shape has moved and need to be updated + void updateProxyCollisionShape(ProxyShape* proxyShape, const AABB& aabb, + const Vector3& displacement, bool forceReinsert = false); - /// Add a collision shape in the array of shapes that have moved in the last simulation step - /// and that need to be tested again for broad-phase overlapping. - void addMovedCollisionShape(int broadPhaseID); + /// Add a collision shape in the array of shapes that have moved in the last simulation step + /// and that need to be tested again for broad-phase overlapping. + void addMovedCollisionShape(int32_t broadPhaseID); - /// Remove a collision shape from the array of shapes that have moved in the last simulation - /// step and that need to be tested again for broad-phase overlapping. - void removeMovedCollisionShape(int broadPhaseID); + /// Remove a collision shape from the array of shapes that have moved in the last simulation + /// step and that need to be tested again for broad-phase overlapping. + void removeMovedCollisionShape(int32_t broadPhaseID); - /// Notify the broad-phase about a potential overlapping pair in the dynamic AABB tree - void notifyOverlappingNodes(int broadPhaseId1, int broadPhaseId2); + /// Notify the broad-phase about a potential overlapping pair in the dynamic AABB tree + void notifyOverlappingNodes(int32_t broadPhaseId1, int32_t broadPhaseId2); - /// Compute all the overlapping pairs of collision shapes - void computeOverlappingPairs(); + /// Compute all the overlapping pairs of collision shapes + void computeOverlappingPairs(); - /// Return true if the two broad-phase collision shapes are overlapping - bool testOverlappingShapes(const ProxyShape* shape1, const ProxyShape* shape2) const; + /// Return true if the two broad-phase collision shapes are overlapping + bool testOverlappingShapes(const ProxyShape* shape1, const ProxyShape* shape2) const; - /// Ray casting method - void raycast(const Ray& ray, RaycastTest& raycastTest, - unsigned short raycastWithCategoryMaskBits) const; + /// Ray casting method + void raycast(const Ray& ray, RaycastTest& raycastTest, + unsigned short raycastWithCategoryMaskBits) const; }; // Method used to compare two pairs for sorting algorithm inline bool BroadPhasePair::smallerThan(const BroadPhasePair& pair1, const BroadPhasePair& pair2) { - if (pair1.collisionShape1ID < pair2.collisionShape1ID) return true; - if (pair1.collisionShape1ID == pair2.collisionShape1ID) { - return pair1.collisionShape2ID < pair2.collisionShape2ID; - } - return false; + if (pair1.collisionShape1ID < pair2.collisionShape1ID) return true; + if (pair1.collisionShape1ID == pair2.collisionShape1ID) { + return pair1.collisionShape2ID < pair2.collisionShape2ID; + } + return false; } // Return true if the two broad-phase collision shapes are overlapping inline bool BroadPhaseAlgorithm::testOverlappingShapes(const ProxyShape* shape1, - const ProxyShape* shape2) const { - // Get the two AABBs of the collision shapes - const AABB& aabb1 = m_dynamicAABBTree.getFatAABB(shape1->mBroadPhaseID); - const AABB& aabb2 = m_dynamicAABBTree.getFatAABB(shape2->mBroadPhaseID); + const ProxyShape* shape2) const { + // Get the two AABBs of the collision shapes + const AABB& aabb1 = m_dynamicAABBTree.getFatAABB(shape1->mBroadPhaseID); + const AABB& aabb2 = m_dynamicAABBTree.getFatAABB(shape2->mBroadPhaseID); - // Check if the two AABBs are overlapping - return aabb1.testCollision(aabb2); + // Check if the two AABBs are overlapping + return aabb1.testCollision(aabb2); } // Ray casting method inline void BroadPhaseAlgorithm::raycast(const Ray& ray, RaycastTest& raycastTest, - unsigned short raycastWithCategoryMaskBits) const { + unsigned short raycastWithCategoryMaskBits) const { - PROFILE("BroadPhaseAlgorithm::raycast()"); + PROFILE("BroadPhaseAlgorithm::raycast()"); - BroadPhaseRaycastCallback broadPhaseRaycastCallback(m_dynamicAABBTree, raycastWithCategoryMaskBits, raycastTest); + BroadPhaseRaycastCallback broadPhaseRaycastCallback(m_dynamicAABBTree, raycastWithCategoryMaskBits, raycastTest); - m_dynamicAABBTree.raycast(ray, broadPhaseRaycastCallback); + m_dynamicAABBTree.raycast(ray, broadPhaseRaycastCallback); } } -#endif + diff --git a/ephysics/collision/broadphase/DynamicAABBTree.cpp b/ephysics/collision/broadphase/DynamicAABBTree.cpp index c739803..2d09a83 100644 --- a/ephysics/collision/broadphase/DynamicAABBTree.cpp +++ b/ephysics/collision/broadphase/DynamicAABBTree.cpp @@ -1,27 +1,8 @@ -/******************************************************************************** -* 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. * -* * -********************************************************************************/ +/** @file + * @author Daniel Chappuis + * @copyright 2010-2016 Daniel Chappuis + * @license BSD 3 clauses (see license file) + */ // Libraries #include @@ -32,664 +13,664 @@ using namespace reactphysics3d; // Initialization of static variables -const int TreeNode::NULL_TREE_NODE = -1; +const int32_t TreeNode::NULL_TREE_NODE = -1; // Constructor -DynamicAABBTree::DynamicAABBTree(decimal extraAABBGap) : mExtraAABBGap(extraAABBGap) { +DynamicAABBTree::DynamicAABBTree(float extraAABBGap) : mExtraAABBGap(extraAABBGap) { - init(); + init(); } // Destructor DynamicAABBTree::~DynamicAABBTree() { - // Free the allocated memory for the nodes - free(mNodes); + // Free the allocated memory for the nodes + free(mNodes); } // Initialize the tree void DynamicAABBTree::init() { - mRootNodeID = TreeNode::NULL_TREE_NODE; - mNbNodes = 0; - mNbAllocatedNodes = 8; + mRootNodeID = TreeNode::NULL_TREE_NODE; + mNbNodes = 0; + mNbAllocatedNodes = 8; - // Allocate memory for the nodes of the tree - mNodes = (TreeNode*) malloc(mNbAllocatedNodes * sizeof(TreeNode)); - assert(mNodes); - memset(mNodes, 0, mNbAllocatedNodes * sizeof(TreeNode)); + // Allocate memory for the nodes of the tree + mNodes = (TreeNode*) malloc(mNbAllocatedNodes * sizeof(TreeNode)); + assert(mNodes); + memset(mNodes, 0, mNbAllocatedNodes * sizeof(TreeNode)); - // Initialize the allocated nodes - for (int i=0; i 0); - assert(nodeID >= 0 && nodeID < mNbAllocatedNodes); - assert(mNodes[nodeID].height >= 0); - mNodes[nodeID].nextNodeID = mFreeNodeID; - mNodes[nodeID].height = -1; - mFreeNodeID = nodeID; - mNbNodes--; + assert(mNbNodes > 0); + assert(nodeID >= 0 && nodeID < mNbAllocatedNodes); + assert(mNodes[nodeID].height >= 0); + mNodes[nodeID].nextNodeID = mFreeNodeID; + mNodes[nodeID].height = -1; + mFreeNodeID = nodeID; + mNbNodes--; } -// Internally add an object into the tree -int DynamicAABBTree::addObjectInternal(const AABB& aabb) { +// Internally add an object int32_to the tree +int32_t DynamicAABBTree::addObjectInternal(const AABB& aabb) { - // Get the next available node (or allocate new ones if necessary) - int nodeID = allocateNode(); + // Get the next available node (or allocate new ones if necessary) + int32_t nodeID = allocateNode(); - // Create the fat aabb to use in the tree - const Vector3 gap(mExtraAABBGap, mExtraAABBGap, mExtraAABBGap); - mNodes[nodeID].aabb.setMin(aabb.getMin() - gap); - mNodes[nodeID].aabb.setMax(aabb.getMax() + gap); + // Create the fat aabb to use in the tree + const Vector3 gap(mExtraAABBGap, mExtraAABBGap, mExtraAABBGap); + mNodes[nodeID].aabb.setMin(aabb.getMin() - gap); + mNodes[nodeID].aabb.setMax(aabb.getMax() + gap); - // Set the height of the node in the tree - mNodes[nodeID].height = 0; + // Set the height of the node in the tree + mNodes[nodeID].height = 0; - // Insert the new leaf node in the tree - insertLeafNode(nodeID); - assert(mNodes[nodeID].isLeaf()); + // Insert the new leaf node in the tree + insertLeafNode(nodeID); + assert(mNodes[nodeID].isLeaf()); - assert(nodeID >= 0); + assert(nodeID >= 0); - // Return the Id of the node - return nodeID; + // Return the Id of the node + return nodeID; } // Remove an object from the tree -void DynamicAABBTree::removeObject(int nodeID) { +void DynamicAABBTree::removeObject(int32_t nodeID) { - assert(nodeID >= 0 && nodeID < mNbAllocatedNodes); - assert(mNodes[nodeID].isLeaf()); + assert(nodeID >= 0 && nodeID < mNbAllocatedNodes); + assert(mNodes[nodeID].isLeaf()); - // Remove the node from the tree - removeLeafNode(nodeID); - releaseNode(nodeID); + // Remove the node from the tree + removeLeafNode(nodeID); + releaseNode(nodeID); } // Update the dynamic tree after an object has moved. /// If the new AABB of the object that has moved is still inside its fat AABB, then -/// nothing is done. Otherwise, the corresponding node is removed and reinserted into the tree. -/// The method returns true if the object has been reinserted into the tree. The "displacement" +/// nothing is done. Otherwise, the corresponding node is removed and reinserted int32_to the tree. +/// The method returns true if the object has been reinserted int32_to the tree. The "displacement" /// argument is the linear velocity of the AABB multiplied by the elapsed time between two /// frames. If the "forceReinsert" parameter is true, we force a removal and reinsertion of the node /// (this can be useful if the shape AABB has become much smaller than the previous one for instance). -bool DynamicAABBTree::updateObject(int nodeID, const AABB& newAABB, const Vector3& displacement, bool forceReinsert) { +bool DynamicAABBTree::updateObject(int32_t nodeID, const AABB& newAABB, const Vector3& displacement, bool forceReinsert) { - PROFILE("DynamicAABBTree::updateObject()"); + PROFILE("DynamicAABBTree::updateObject()"); - assert(nodeID >= 0 && nodeID < mNbAllocatedNodes); - assert(mNodes[nodeID].isLeaf()); - assert(mNodes[nodeID].height >= 0); + assert(nodeID >= 0 && nodeID < mNbAllocatedNodes); + assert(mNodes[nodeID].isLeaf()); + assert(mNodes[nodeID].height >= 0); - // If the new AABB is still inside the fat AABB of the node - if (!forceReinsert && mNodes[nodeID].aabb.contains(newAABB)) { - return false; - } + // If the new AABB is still inside the fat AABB of the node + if (!forceReinsert && mNodes[nodeID].aabb.contains(newAABB)) { + return false; + } - // If the new AABB is outside the fat AABB, we remove the corresponding node - removeLeafNode(nodeID); + // If the new AABB is outside the fat AABB, we remove the corresponding node + removeLeafNode(nodeID); - // Compute the fat AABB by inflating the AABB with a constant gap - mNodes[nodeID].aabb = newAABB; - const Vector3 gap(mExtraAABBGap, mExtraAABBGap, mExtraAABBGap); - mNodes[nodeID].aabb.mMinCoordinates -= gap; - mNodes[nodeID].aabb.mMaxCoordinates += gap; + // Compute the fat AABB by inflating the AABB with a constant gap + mNodes[nodeID].aabb = newAABB; + const Vector3 gap(mExtraAABBGap, mExtraAABBGap, mExtraAABBGap); + mNodes[nodeID].aabb.mMinCoordinates -= gap; + mNodes[nodeID].aabb.mMaxCoordinates += gap; - // Inflate the fat AABB in direction of the linear motion of the AABB - if (displacement.x < decimal(0.0)) { - mNodes[nodeID].aabb.mMinCoordinates.x += DYNAMIC_TREE_AABB_LIN_GAP_MULTIPLIER *displacement.x; - } - else { - mNodes[nodeID].aabb.mMaxCoordinates.x += DYNAMIC_TREE_AABB_LIN_GAP_MULTIPLIER *displacement.x; - } - if (displacement.y < decimal(0.0)) { - mNodes[nodeID].aabb.mMinCoordinates.y += DYNAMIC_TREE_AABB_LIN_GAP_MULTIPLIER *displacement.y; - } - else { - mNodes[nodeID].aabb.mMaxCoordinates.y += DYNAMIC_TREE_AABB_LIN_GAP_MULTIPLIER *displacement.y; - } - if (displacement.z < decimal(0.0)) { - mNodes[nodeID].aabb.mMinCoordinates.z += DYNAMIC_TREE_AABB_LIN_GAP_MULTIPLIER *displacement.z; - } - else { - mNodes[nodeID].aabb.mMaxCoordinates.z += DYNAMIC_TREE_AABB_LIN_GAP_MULTIPLIER *displacement.z; - } + // Inflate the fat AABB in direction of the linear motion of the AABB + if (displacement.x < float(0.0)) { + mNodes[nodeID].aabb.mMinCoordinates.x += DYNAMIC_TREE_AABB_LIN_GAP_MULTIPLIER *displacement.x; + } + else { + mNodes[nodeID].aabb.mMaxCoordinates.x += DYNAMIC_TREE_AABB_LIN_GAP_MULTIPLIER *displacement.x; + } + if (displacement.y < float(0.0)) { + mNodes[nodeID].aabb.mMinCoordinates.y += DYNAMIC_TREE_AABB_LIN_GAP_MULTIPLIER *displacement.y; + } + else { + mNodes[nodeID].aabb.mMaxCoordinates.y += DYNAMIC_TREE_AABB_LIN_GAP_MULTIPLIER *displacement.y; + } + if (displacement.z < float(0.0)) { + mNodes[nodeID].aabb.mMinCoordinates.z += DYNAMIC_TREE_AABB_LIN_GAP_MULTIPLIER *displacement.z; + } + else { + mNodes[nodeID].aabb.mMaxCoordinates.z += DYNAMIC_TREE_AABB_LIN_GAP_MULTIPLIER *displacement.z; + } - assert(mNodes[nodeID].aabb.contains(newAABB)); + assert(mNodes[nodeID].aabb.contains(newAABB)); - // Reinsert the node into the tree - insertLeafNode(nodeID); + // Reinsert the node int32_to the tree + insertLeafNode(nodeID); - return true; + return true; } // Insert a leaf node in the tree. The process of inserting a new leaf node // in the dynamic tree is described in the book "Introduction to Game Physics // with Box2D" by Ian Parberry. -void DynamicAABBTree::insertLeafNode(int nodeID) { +void DynamicAABBTree::insertLeafNode(int32_t nodeID) { - // If the tree is empty - if (mRootNodeID == TreeNode::NULL_TREE_NODE) { - mRootNodeID = nodeID; - mNodes[mRootNodeID].parentID = TreeNode::NULL_TREE_NODE; - return; - } + // If the tree is empty + if (mRootNodeID == TreeNode::NULL_TREE_NODE) { + mRootNodeID = nodeID; + mNodes[mRootNodeID].parentID = TreeNode::NULL_TREE_NODE; + return; + } - assert(mRootNodeID != TreeNode::NULL_TREE_NODE); + assert(mRootNodeID != TreeNode::NULL_TREE_NODE); - // Find the best sibling node for the new node - AABB newNodeAABB = mNodes[nodeID].aabb; - int currentNodeID = mRootNodeID; - while (!mNodes[currentNodeID].isLeaf()) { + // Find the best sibling node for the new node + AABB newNodeAABB = mNodes[nodeID].aabb; + int32_t currentNodeID = mRootNodeID; + while (!mNodes[currentNodeID].isLeaf()) { - int leftChild = mNodes[currentNodeID].children[0]; - int rightChild = mNodes[currentNodeID].children[1]; + int32_t leftChild = mNodes[currentNodeID].children[0]; + int32_t rightChild = mNodes[currentNodeID].children[1]; - // Compute the merged AABB - decimal volumeAABB = mNodes[currentNodeID].aabb.getVolume(); - AABB mergedAABBs; - mergedAABBs.mergeTwoAABBs(mNodes[currentNodeID].aabb, newNodeAABB); - decimal mergedVolume = mergedAABBs.getVolume(); + // Compute the merged AABB + float volumeAABB = mNodes[currentNodeID].aabb.getVolume(); + AABB mergedAABBs; + mergedAABBs.mergeTwoAABBs(mNodes[currentNodeID].aabb, newNodeAABB); + float mergedVolume = mergedAABBs.getVolume(); - // Compute the cost of making the current node the sibbling of the new node - decimal costS = decimal(2.0) * mergedVolume; + // Compute the cost of making the current node the sibbling of the new node + float costS = float(2.0) * mergedVolume; - // Compute the minimum cost of pushing the new node further down the tree (inheritance cost) - decimal costI = decimal(2.0) * (mergedVolume - volumeAABB); + // Compute the minimum cost of pushing the new node further down the tree (inheritance cost) + float costI = float(2.0) * (mergedVolume - volumeAABB); - // Compute the cost of descending into the left child - decimal costLeft; - AABB currentAndLeftAABB; - currentAndLeftAABB.mergeTwoAABBs(newNodeAABB, mNodes[leftChild].aabb); - if (mNodes[leftChild].isLeaf()) { // If the left child is a leaf - costLeft = currentAndLeftAABB.getVolume() + costI; - } - else { - decimal leftChildVolume = mNodes[leftChild].aabb.getVolume(); - costLeft = costI + currentAndLeftAABB.getVolume() - leftChildVolume; - } + // Compute the cost of descending int32_to the left child + float costLeft; + AABB currentAndLeftAABB; + currentAndLeftAABB.mergeTwoAABBs(newNodeAABB, mNodes[leftChild].aabb); + if (mNodes[leftChild].isLeaf()) { // If the left child is a leaf + costLeft = currentAndLeftAABB.getVolume() + costI; + } + else { + float leftChildVolume = mNodes[leftChild].aabb.getVolume(); + costLeft = costI + currentAndLeftAABB.getVolume() - leftChildVolume; + } - // Compute the cost of descending into the right child - decimal costRight; - AABB currentAndRightAABB; - currentAndRightAABB.mergeTwoAABBs(newNodeAABB, mNodes[rightChild].aabb); - if (mNodes[rightChild].isLeaf()) { // If the right child is a leaf - costRight = currentAndRightAABB.getVolume() + costI; - } - else { - decimal rightChildVolume = mNodes[rightChild].aabb.getVolume(); - costRight = costI + currentAndRightAABB.getVolume() - rightChildVolume; - } + // Compute the cost of descending int32_to the right child + float costRight; + AABB currentAndRightAABB; + currentAndRightAABB.mergeTwoAABBs(newNodeAABB, mNodes[rightChild].aabb); + if (mNodes[rightChild].isLeaf()) { // If the right child is a leaf + costRight = currentAndRightAABB.getVolume() + costI; + } + else { + float rightChildVolume = mNodes[rightChild].aabb.getVolume(); + costRight = costI + currentAndRightAABB.getVolume() - rightChildVolume; + } - // If the cost of making the current node a sibbling of the new node is smaller than - // the cost of going down into the left or right child - if (costS < costLeft && costS < costRight) break; + // If the cost of making the current node a sibbling of the new node is smaller than + // the cost of going down int32_to the left or right child + if (costS < costLeft && costS < costRight) break; - // It is cheaper to go down into a child of the current node, choose the best child - if (costLeft < costRight) { - currentNodeID = leftChild; - } - else { - currentNodeID = rightChild; - } - } + // It is cheaper to go down int32_to a child of the current node, choose the best child + if (costLeft < costRight) { + currentNodeID = leftChild; + } + else { + currentNodeID = rightChild; + } + } - int siblingNode = currentNodeID; + int32_t siblingNode = currentNodeID; - // Create a new parent for the new node and the sibling node - int oldParentNode = mNodes[siblingNode].parentID; - int newParentNode = allocateNode(); - mNodes[newParentNode].parentID = oldParentNode; - mNodes[newParentNode].aabb.mergeTwoAABBs(mNodes[siblingNode].aabb, newNodeAABB); - mNodes[newParentNode].height = mNodes[siblingNode].height + 1; - assert(mNodes[newParentNode].height > 0); + // Create a new parent for the new node and the sibling node + int32_t oldParentNode = mNodes[siblingNode].parentID; + int32_t newParentNode = allocateNode(); + mNodes[newParentNode].parentID = oldParentNode; + mNodes[newParentNode].aabb.mergeTwoAABBs(mNodes[siblingNode].aabb, newNodeAABB); + mNodes[newParentNode].height = mNodes[siblingNode].height + 1; + assert(mNodes[newParentNode].height > 0); - // If the sibling node was not the root node - if (oldParentNode != TreeNode::NULL_TREE_NODE) { - assert(!mNodes[oldParentNode].isLeaf()); - if (mNodes[oldParentNode].children[0] == siblingNode) { - mNodes[oldParentNode].children[0] = newParentNode; - } - else { - mNodes[oldParentNode].children[1] = newParentNode; - } - mNodes[newParentNode].children[0] = siblingNode; - mNodes[newParentNode].children[1] = nodeID; - mNodes[siblingNode].parentID = newParentNode; - mNodes[nodeID].parentID = newParentNode; - } - else { // If the sibling node was the root node - mNodes[newParentNode].children[0] = siblingNode; - mNodes[newParentNode].children[1] = nodeID; - mNodes[siblingNode].parentID = newParentNode; - mNodes[nodeID].parentID = newParentNode; - mRootNodeID = newParentNode; - } + // If the sibling node was not the root node + if (oldParentNode != TreeNode::NULL_TREE_NODE) { + assert(!mNodes[oldParentNode].isLeaf()); + if (mNodes[oldParentNode].children[0] == siblingNode) { + mNodes[oldParentNode].children[0] = newParentNode; + } + else { + mNodes[oldParentNode].children[1] = newParentNode; + } + mNodes[newParentNode].children[0] = siblingNode; + mNodes[newParentNode].children[1] = nodeID; + mNodes[siblingNode].parentID = newParentNode; + mNodes[nodeID].parentID = newParentNode; + } + else { // If the sibling node was the root node + mNodes[newParentNode].children[0] = siblingNode; + mNodes[newParentNode].children[1] = nodeID; + mNodes[siblingNode].parentID = newParentNode; + mNodes[nodeID].parentID = newParentNode; + mRootNodeID = newParentNode; + } - // Move up in the tree to change the AABBs that have changed - currentNodeID = mNodes[nodeID].parentID; - assert(!mNodes[currentNodeID].isLeaf()); - while (currentNodeID != TreeNode::NULL_TREE_NODE) { + // Move up in the tree to change the AABBs that have changed + currentNodeID = mNodes[nodeID].parentID; + assert(!mNodes[currentNodeID].isLeaf()); + while (currentNodeID != TreeNode::NULL_TREE_NODE) { - // Balance the sub-tree of the current node if it is not balanced - currentNodeID = balanceSubTreeAtNode(currentNodeID); - assert(mNodes[nodeID].isLeaf()); + // Balance the sub-tree of the current node if it is not balanced + currentNodeID = balanceSubTreeAtNode(currentNodeID); + assert(mNodes[nodeID].isLeaf()); - assert(!mNodes[currentNodeID].isLeaf()); - int leftChild = mNodes[currentNodeID].children[0]; - int rightChild = mNodes[currentNodeID].children[1]; - assert(leftChild != TreeNode::NULL_TREE_NODE); - assert(rightChild != TreeNode::NULL_TREE_NODE); + assert(!mNodes[currentNodeID].isLeaf()); + int32_t leftChild = mNodes[currentNodeID].children[0]; + int32_t rightChild = mNodes[currentNodeID].children[1]; + assert(leftChild != TreeNode::NULL_TREE_NODE); + assert(rightChild != TreeNode::NULL_TREE_NODE); - // Recompute the height of the node in the tree - mNodes[currentNodeID].height = std::max(mNodes[leftChild].height, - mNodes[rightChild].height) + 1; - assert(mNodes[currentNodeID].height > 0); + // Recompute the height of the node in the tree + mNodes[currentNodeID].height = std::max(mNodes[leftChild].height, + mNodes[rightChild].height) + 1; + assert(mNodes[currentNodeID].height > 0); - // Recompute the AABB of the node - mNodes[currentNodeID].aabb.mergeTwoAABBs(mNodes[leftChild].aabb, mNodes[rightChild].aabb); + // Recompute the AABB of the node + mNodes[currentNodeID].aabb.mergeTwoAABBs(mNodes[leftChild].aabb, mNodes[rightChild].aabb); - currentNodeID = mNodes[currentNodeID].parentID; - } + currentNodeID = mNodes[currentNodeID].parentID; + } - assert(mNodes[nodeID].isLeaf()); + assert(mNodes[nodeID].isLeaf()); } // Remove a leaf node from the tree -void DynamicAABBTree::removeLeafNode(int nodeID) { +void DynamicAABBTree::removeLeafNode(int32_t nodeID) { - assert(nodeID >= 0 && nodeID < mNbAllocatedNodes); - assert(mNodes[nodeID].isLeaf()); + assert(nodeID >= 0 && nodeID < mNbAllocatedNodes); + assert(mNodes[nodeID].isLeaf()); - // If we are removing the root node (root node is a leaf in this case) - if (mRootNodeID == nodeID) { - mRootNodeID = TreeNode::NULL_TREE_NODE; - return; - } + // If we are removing the root node (root node is a leaf in this case) + if (mRootNodeID == nodeID) { + mRootNodeID = TreeNode::NULL_TREE_NODE; + return; + } - int parentNodeID = mNodes[nodeID].parentID; - int grandParentNodeID = mNodes[parentNodeID].parentID; - int siblingNodeID; - if (mNodes[parentNodeID].children[0] == nodeID) { - siblingNodeID = mNodes[parentNodeID].children[1]; - } - else { - siblingNodeID = mNodes[parentNodeID].children[0]; - } + int32_t parentNodeID = mNodes[nodeID].parentID; + int32_t grandParentNodeID = mNodes[parentNodeID].parentID; + int32_t siblingNodeID; + if (mNodes[parentNodeID].children[0] == nodeID) { + siblingNodeID = mNodes[parentNodeID].children[1]; + } + else { + siblingNodeID = mNodes[parentNodeID].children[0]; + } - // If the parent of the node to remove is not the root node - if (grandParentNodeID != TreeNode::NULL_TREE_NODE) { + // If the parent of the node to remove is not the root node + if (grandParentNodeID != TreeNode::NULL_TREE_NODE) { - // Destroy the parent node - if (mNodes[grandParentNodeID].children[0] == parentNodeID) { - mNodes[grandParentNodeID].children[0] = siblingNodeID; - } - else { - assert(mNodes[grandParentNodeID].children[1] == parentNodeID); - mNodes[grandParentNodeID].children[1] = siblingNodeID; - } - mNodes[siblingNodeID].parentID = grandParentNodeID; - releaseNode(parentNodeID); + // Destroy the parent node + if (mNodes[grandParentNodeID].children[0] == parentNodeID) { + mNodes[grandParentNodeID].children[0] = siblingNodeID; + } + else { + assert(mNodes[grandParentNodeID].children[1] == parentNodeID); + mNodes[grandParentNodeID].children[1] = siblingNodeID; + } + mNodes[siblingNodeID].parentID = grandParentNodeID; + releaseNode(parentNodeID); - // Now, we need to recompute the AABBs of the node on the path back to the root - // and make sure that the tree is still balanced - int currentNodeID = grandParentNodeID; - while(currentNodeID != TreeNode::NULL_TREE_NODE) { + // Now, we need to recompute the AABBs of the node on the path back to the root + // and make sure that the tree is still balanced + int32_t currentNodeID = grandParentNodeID; + while(currentNodeID != TreeNode::NULL_TREE_NODE) { - // Balance the current sub-tree if necessary - currentNodeID = balanceSubTreeAtNode(currentNodeID); + // Balance the current sub-tree if necessary + currentNodeID = balanceSubTreeAtNode(currentNodeID); - assert(!mNodes[currentNodeID].isLeaf()); + assert(!mNodes[currentNodeID].isLeaf()); - // Get the two children of the current node - int leftChildID = mNodes[currentNodeID].children[0]; - int rightChildID = mNodes[currentNodeID].children[1]; + // Get the two children of the current node + int32_t leftChildID = mNodes[currentNodeID].children[0]; + int32_t rightChildID = mNodes[currentNodeID].children[1]; - // Recompute the AABB and the height of the current node - mNodes[currentNodeID].aabb.mergeTwoAABBs(mNodes[leftChildID].aabb, - mNodes[rightChildID].aabb); - mNodes[currentNodeID].height = std::max(mNodes[leftChildID].height, - mNodes[rightChildID].height) + 1; - assert(mNodes[currentNodeID].height > 0); + // Recompute the AABB and the height of the current node + mNodes[currentNodeID].aabb.mergeTwoAABBs(mNodes[leftChildID].aabb, + mNodes[rightChildID].aabb); + mNodes[currentNodeID].height = std::max(mNodes[leftChildID].height, + mNodes[rightChildID].height) + 1; + assert(mNodes[currentNodeID].height > 0); - currentNodeID = mNodes[currentNodeID].parentID; - } - } - else { // If the parent of the node to remove is the root node + currentNodeID = mNodes[currentNodeID].parentID; + } + } + else { // If the parent of the node to remove is the root node - // The sibling node becomes the new root node - mRootNodeID = siblingNodeID; - mNodes[siblingNodeID].parentID = TreeNode::NULL_TREE_NODE; - releaseNode(parentNodeID); - } + // The sibling node becomes the new root node + mRootNodeID = siblingNodeID; + mNodes[siblingNodeID].parentID = TreeNode::NULL_TREE_NODE; + releaseNode(parentNodeID); + } } // Balance the sub-tree of a given node using left or right rotations. /// The rotation schemes are described in the book "Introduction to Game Physics /// with Box2D" by Ian Parberry. This method returns the new root node ID. -int DynamicAABBTree::balanceSubTreeAtNode(int nodeID) { +int32_t DynamicAABBTree::balanceSubTreeAtNode(int32_t nodeID) { - assert(nodeID != TreeNode::NULL_TREE_NODE); + assert(nodeID != TreeNode::NULL_TREE_NODE); - TreeNode* nodeA = mNodes + nodeID; + TreeNode* nodeA = mNodes + nodeID; - // If the node is a leaf or the height of A's sub-tree is less than 2 - if (nodeA->isLeaf() || nodeA->height < 2) { + // If the node is a leaf or the height of A's sub-tree is less than 2 + if (nodeA->isLeaf() || nodeA->height < 2) { - // Do not perform any rotation - return nodeID; - } + // Do not perform any rotation + return nodeID; + } - // Get the two children nodes - int nodeBID = nodeA->children[0]; - int nodeCID = nodeA->children[1]; - assert(nodeBID >= 0 && nodeBID < mNbAllocatedNodes); - assert(nodeCID >= 0 && nodeCID < mNbAllocatedNodes); - TreeNode* nodeB = mNodes + nodeBID; - TreeNode* nodeC = mNodes + nodeCID; + // Get the two children nodes + int32_t nodeBID = nodeA->children[0]; + int32_t nodeCID = nodeA->children[1]; + assert(nodeBID >= 0 && nodeBID < mNbAllocatedNodes); + assert(nodeCID >= 0 && nodeCID < mNbAllocatedNodes); + TreeNode* nodeB = mNodes + nodeBID; + TreeNode* nodeC = mNodes + nodeCID; - // Compute the factor of the left and right sub-trees - int balanceFactor = nodeC->height - nodeB->height; + // Compute the factor of the left and right sub-trees + int32_t balanceFactor = nodeC->height - nodeB->height; - // If the right node C is 2 higher than left node B - if (balanceFactor > 1) { + // If the right node C is 2 higher than left node B + if (balanceFactor > 1) { - assert(!nodeC->isLeaf()); + assert(!nodeC->isLeaf()); - int nodeFID = nodeC->children[0]; - int nodeGID = nodeC->children[1]; - assert(nodeFID >= 0 && nodeFID < mNbAllocatedNodes); - assert(nodeGID >= 0 && nodeGID < mNbAllocatedNodes); - TreeNode* nodeF = mNodes + nodeFID; - TreeNode* nodeG = mNodes + nodeGID; + int32_t nodeFID = nodeC->children[0]; + int32_t nodeGID = nodeC->children[1]; + assert(nodeFID >= 0 && nodeFID < mNbAllocatedNodes); + assert(nodeGID >= 0 && nodeGID < mNbAllocatedNodes); + TreeNode* nodeF = mNodes + nodeFID; + TreeNode* nodeG = mNodes + nodeGID; - nodeC->children[0] = nodeID; - nodeC->parentID = nodeA->parentID; - nodeA->parentID = nodeCID; + nodeC->children[0] = nodeID; + nodeC->parentID = nodeA->parentID; + nodeA->parentID = nodeCID; - if (nodeC->parentID != TreeNode::NULL_TREE_NODE) { + if (nodeC->parentID != TreeNode::NULL_TREE_NODE) { - if (mNodes[nodeC->parentID].children[0] == nodeID) { - mNodes[nodeC->parentID].children[0] = nodeCID; - } - else { - assert(mNodes[nodeC->parentID].children[1] == nodeID); - mNodes[nodeC->parentID].children[1] = nodeCID; - } - } - else { - mRootNodeID = nodeCID; - } + if (mNodes[nodeC->parentID].children[0] == nodeID) { + mNodes[nodeC->parentID].children[0] = nodeCID; + } + else { + assert(mNodes[nodeC->parentID].children[1] == nodeID); + mNodes[nodeC->parentID].children[1] = nodeCID; + } + } + else { + mRootNodeID = nodeCID; + } - assert(!nodeC->isLeaf()); - assert(!nodeA->isLeaf()); + assert(!nodeC->isLeaf()); + assert(!nodeA->isLeaf()); - // If the right node C was higher than left node B because of the F node - if (nodeF->height > nodeG->height) { + // If the right node C was higher than left node B because of the F node + if (nodeF->height > nodeG->height) { - nodeC->children[1] = nodeFID; - nodeA->children[1] = nodeGID; - nodeG->parentID = nodeID; + nodeC->children[1] = nodeFID; + nodeA->children[1] = nodeGID; + nodeG->parentID = nodeID; - // Recompute the AABB of node A and C - nodeA->aabb.mergeTwoAABBs(nodeB->aabb, nodeG->aabb); - nodeC->aabb.mergeTwoAABBs(nodeA->aabb, nodeF->aabb); + // Recompute the AABB of node A and C + nodeA->aabb.mergeTwoAABBs(nodeB->aabb, nodeG->aabb); + nodeC->aabb.mergeTwoAABBs(nodeA->aabb, nodeF->aabb); - // Recompute the height of node A and C - nodeA->height = std::max(nodeB->height, nodeG->height) + 1; - nodeC->height = std::max(nodeA->height, nodeF->height) + 1; - assert(nodeA->height > 0); - assert(nodeC->height > 0); - } - else { // If the right node C was higher than left node B because of node G - nodeC->children[1] = nodeGID; - nodeA->children[1] = nodeFID; - nodeF->parentID = nodeID; + // Recompute the height of node A and C + nodeA->height = std::max(nodeB->height, nodeG->height) + 1; + nodeC->height = std::max(nodeA->height, nodeF->height) + 1; + assert(nodeA->height > 0); + assert(nodeC->height > 0); + } + else { // If the right node C was higher than left node B because of node G + nodeC->children[1] = nodeGID; + nodeA->children[1] = nodeFID; + nodeF->parentID = nodeID; - // Recompute the AABB of node A and C - nodeA->aabb.mergeTwoAABBs(nodeB->aabb, nodeF->aabb); - nodeC->aabb.mergeTwoAABBs(nodeA->aabb, nodeG->aabb); + // Recompute the AABB of node A and C + nodeA->aabb.mergeTwoAABBs(nodeB->aabb, nodeF->aabb); + nodeC->aabb.mergeTwoAABBs(nodeA->aabb, nodeG->aabb); - // Recompute the height of node A and C - nodeA->height = std::max(nodeB->height, nodeF->height) + 1; - nodeC->height = std::max(nodeA->height, nodeG->height) + 1; - assert(nodeA->height > 0); - assert(nodeC->height > 0); - } + // Recompute the height of node A and C + nodeA->height = std::max(nodeB->height, nodeF->height) + 1; + nodeC->height = std::max(nodeA->height, nodeG->height) + 1; + assert(nodeA->height > 0); + assert(nodeC->height > 0); + } - // Return the new root of the sub-tree - return nodeCID; - } + // Return the new root of the sub-tree + return nodeCID; + } - // If the left node B is 2 higher than right node C - if (balanceFactor < -1) { + // If the left node B is 2 higher than right node C + if (balanceFactor < -1) { - assert(!nodeB->isLeaf()); + assert(!nodeB->isLeaf()); - int nodeFID = nodeB->children[0]; - int nodeGID = nodeB->children[1]; - assert(nodeFID >= 0 && nodeFID < mNbAllocatedNodes); - assert(nodeGID >= 0 && nodeGID < mNbAllocatedNodes); - TreeNode* nodeF = mNodes + nodeFID; - TreeNode* nodeG = mNodes + nodeGID; + int32_t nodeFID = nodeB->children[0]; + int32_t nodeGID = nodeB->children[1]; + assert(nodeFID >= 0 && nodeFID < mNbAllocatedNodes); + assert(nodeGID >= 0 && nodeGID < mNbAllocatedNodes); + TreeNode* nodeF = mNodes + nodeFID; + TreeNode* nodeG = mNodes + nodeGID; - nodeB->children[0] = nodeID; - nodeB->parentID = nodeA->parentID; - nodeA->parentID = nodeBID; + nodeB->children[0] = nodeID; + nodeB->parentID = nodeA->parentID; + nodeA->parentID = nodeBID; - if (nodeB->parentID != TreeNode::NULL_TREE_NODE) { + if (nodeB->parentID != TreeNode::NULL_TREE_NODE) { - if (mNodes[nodeB->parentID].children[0] == nodeID) { - mNodes[nodeB->parentID].children[0] = nodeBID; - } - else { - assert(mNodes[nodeB->parentID].children[1] == nodeID); - mNodes[nodeB->parentID].children[1] = nodeBID; - } - } - else { - mRootNodeID = nodeBID; - } + if (mNodes[nodeB->parentID].children[0] == nodeID) { + mNodes[nodeB->parentID].children[0] = nodeBID; + } + else { + assert(mNodes[nodeB->parentID].children[1] == nodeID); + mNodes[nodeB->parentID].children[1] = nodeBID; + } + } + else { + mRootNodeID = nodeBID; + } - assert(!nodeB->isLeaf()); - assert(!nodeA->isLeaf()); + assert(!nodeB->isLeaf()); + assert(!nodeA->isLeaf()); - // If the left node B was higher than right node C because of the F node - if (nodeF->height > nodeG->height) { + // If the left node B was higher than right node C because of the F node + if (nodeF->height > nodeG->height) { - nodeB->children[1] = nodeFID; - nodeA->children[0] = nodeGID; - nodeG->parentID = nodeID; + nodeB->children[1] = nodeFID; + nodeA->children[0] = nodeGID; + nodeG->parentID = nodeID; - // Recompute the AABB of node A and B - nodeA->aabb.mergeTwoAABBs(nodeC->aabb, nodeG->aabb); - nodeB->aabb.mergeTwoAABBs(nodeA->aabb, nodeF->aabb); + // Recompute the AABB of node A and B + nodeA->aabb.mergeTwoAABBs(nodeC->aabb, nodeG->aabb); + nodeB->aabb.mergeTwoAABBs(nodeA->aabb, nodeF->aabb); - // Recompute the height of node A and B - nodeA->height = std::max(nodeC->height, nodeG->height) + 1; - nodeB->height = std::max(nodeA->height, nodeF->height) + 1; - assert(nodeA->height > 0); - assert(nodeB->height > 0); - } - else { // If the left node B was higher than right node C because of node G - nodeB->children[1] = nodeGID; - nodeA->children[0] = nodeFID; - nodeF->parentID = nodeID; + // Recompute the height of node A and B + nodeA->height = std::max(nodeC->height, nodeG->height) + 1; + nodeB->height = std::max(nodeA->height, nodeF->height) + 1; + assert(nodeA->height > 0); + assert(nodeB->height > 0); + } + else { // If the left node B was higher than right node C because of node G + nodeB->children[1] = nodeGID; + nodeA->children[0] = nodeFID; + nodeF->parentID = nodeID; - // Recompute the AABB of node A and B - nodeA->aabb.mergeTwoAABBs(nodeC->aabb, nodeF->aabb); - nodeB->aabb.mergeTwoAABBs(nodeA->aabb, nodeG->aabb); + // Recompute the AABB of node A and B + nodeA->aabb.mergeTwoAABBs(nodeC->aabb, nodeF->aabb); + nodeB->aabb.mergeTwoAABBs(nodeA->aabb, nodeG->aabb); - // Recompute the height of node A and B - nodeA->height = std::max(nodeC->height, nodeF->height) + 1; - nodeB->height = std::max(nodeA->height, nodeG->height) + 1; - assert(nodeA->height > 0); - assert(nodeB->height > 0); - } + // Recompute the height of node A and B + nodeA->height = std::max(nodeC->height, nodeF->height) + 1; + nodeB->height = std::max(nodeA->height, nodeG->height) + 1; + assert(nodeA->height > 0); + assert(nodeB->height > 0); + } - // Return the new root of the sub-tree - return nodeBID; - } + // Return the new root of the sub-tree + return nodeBID; + } - // If the sub-tree is balanced, return the current root node - return nodeID; + // If the sub-tree is balanced, return the current root node + return nodeID; } /// Report all shapes overlapping with the AABB given in parameter. void DynamicAABBTree::reportAllShapesOverlappingWithAABB(const AABB& aabb, - DynamicAABBTreeOverlapCallback& callback) const { + DynamicAABBTreeOverlapCallback& callback) const { - // Create a stack with the nodes to visit - Stack stack; - stack.push(mRootNodeID); + // Create a stack with the nodes to visit + Stack stack; + stack.push(mRootNodeID); - // While there are still nodes to visit - while(stack.getNbElements() > 0) { + // While there are still nodes to visit + while(stack.getNbElements() > 0) { - // Get the next node ID to visit - int nodeIDToVisit = stack.pop(); + // Get the next node ID to visit + int32_t nodeIDToVisit = stack.pop(); - // Skip it if it is a null node - if (nodeIDToVisit == TreeNode::NULL_TREE_NODE) continue; + // Skip it if it is a null node + if (nodeIDToVisit == TreeNode::NULL_TREE_NODE) continue; - // Get the corresponding node - const TreeNode* nodeToVisit = mNodes + nodeIDToVisit; + // Get the corresponding node + const TreeNode* nodeToVisit = mNodes + nodeIDToVisit; - // If the AABB in parameter overlaps with the AABB of the node to visit - if (aabb.testCollision(nodeToVisit->aabb)) { + // If the AABB in parameter overlaps with the AABB of the node to visit + if (aabb.testCollision(nodeToVisit->aabb)) { - // If the node is a leaf - if (nodeToVisit->isLeaf()) { + // If the node is a leaf + if (nodeToVisit->isLeaf()) { - // Notify the broad-phase about a new potential overlapping pair - callback.notifyOverlappingNode(nodeIDToVisit); - } - else { // If the node is not a leaf + // Notify the broad-phase about a new potential overlapping pair + callback.notifyOverlappingNode(nodeIDToVisit); + } + else { // If the node is not a leaf - // We need to visit its children - stack.push(nodeToVisit->children[0]); - stack.push(nodeToVisit->children[1]); - } - } - } + // We need to visit its children + stack.push(nodeToVisit->children[0]); + stack.push(nodeToVisit->children[1]); + } + } + } } // Ray casting method void DynamicAABBTree::raycast(const Ray& ray, DynamicAABBTreeRaycastCallback &callback) const { - PROFILE("DynamicAABBTree::raycast()"); + PROFILE("DynamicAABBTree::raycast()"); - decimal maxFraction = ray.maxFraction; + float maxFraction = ray.maxFraction; - Stack stack; - stack.push(mRootNodeID); + Stack stack; + stack.push(mRootNodeID); - // Walk through the tree from the root looking for proxy shapes - // that overlap with the ray AABB - while (stack.getNbElements() > 0) { + // Walk through the tree from the root looking for proxy shapes + // that overlap with the ray AABB + while (stack.getNbElements() > 0) { - // Get the next node in the stack - int nodeID = stack.pop(); + // Get the next node in the stack + int32_t nodeID = stack.pop(); - // If it is a null node, skip it - if (nodeID == TreeNode::NULL_TREE_NODE) continue; + // If it is a null node, skip it + if (nodeID == TreeNode::NULL_TREE_NODE) continue; - // Get the corresponding node - const TreeNode* node = mNodes + nodeID; + // Get the corresponding node + const TreeNode* node = mNodes + nodeID; - Ray rayTemp(ray.point1, ray.point2, maxFraction); + Ray rayTemp(ray.point1, ray.point2, maxFraction); - // Test if the ray intersects with the current node AABB - if (!node->aabb.testRayIntersect(rayTemp)) continue; + // Test if the ray int32_tersects with the current node AABB + if (!node->aabb.testRayIntersect(rayTemp)) continue; - // If the node is a leaf of the tree - if (node->isLeaf()) { + // If the node is a leaf of the tree + if (node->isLeaf()) { - // Call the callback that will raycast again the broad-phase shape - decimal hitFraction = callback.raycastBroadPhaseShape(nodeID, rayTemp); + // Call the callback that will raycast again the broad-phase shape + float hitFraction = callback.raycastBroadPhaseShape(nodeID, rayTemp); - // If the user returned a hitFraction of zero, it means that - // the raycasting should stop here - if (hitFraction == decimal(0.0)) { - return; - } + // If the user returned a hitFraction of zero, it means that + // the raycasting should stop here + if (hitFraction == float(0.0)) { + return; + } - // If the user returned a positive fraction - if (hitFraction > decimal(0.0)) { + // If the user returned a positive fraction + if (hitFraction > float(0.0)) { - // We update the maxFraction value and the ray - // AABB using the new maximum fraction - if (hitFraction < maxFraction) { - maxFraction = hitFraction; - } - } + // We update the maxFraction value and the ray + // AABB using the new maximum fraction + if (hitFraction < maxFraction) { + maxFraction = hitFraction; + } + } - // If the user returned a negative fraction, we continue - // the raycasting as if the proxy shape did not exist - } - else { // If the node has children + // If the user returned a negative fraction, we continue + // the raycasting as if the proxy shape did not exist + } + else { // If the node has children - // Push its children in the stack of nodes to explore - stack.push(node->children[0]); - stack.push(node->children[1]); - } - } + // Push its children in the stack of nodes to explore + stack.push(node->children[0]); + stack.push(node->children[1]); + } + } } #ifndef NDEBUG @@ -697,96 +678,96 @@ void DynamicAABBTree::raycast(const Ray& ray, DynamicAABBTreeRaycastCallback &ca // Check if the tree structure is valid (for debugging purpose) void DynamicAABBTree::check() const { - // Recursively check each node - checkNode(mRootNodeID); + // Recursively check each node + checkNode(mRootNodeID); - int nbFreeNodes = 0; - int freeNodeID = mFreeNodeID; + int32_t nbFreeNodes = 0; + int32_t freeNodeID = mFreeNodeID; - // Check the free nodes - while(freeNodeID != TreeNode::NULL_TREE_NODE) { - assert(0 <= freeNodeID && freeNodeID < mNbAllocatedNodes); - freeNodeID = mNodes[freeNodeID].nextNodeID; - nbFreeNodes++; - } + // Check the free nodes + while(freeNodeID != TreeNode::NULL_TREE_NODE) { + assert(0 <= freeNodeID && freeNodeID < mNbAllocatedNodes); + freeNodeID = mNodes[freeNodeID].nextNodeID; + nbFreeNodes++; + } - assert(mNbNodes + nbFreeNodes == mNbAllocatedNodes); + assert(mNbNodes + nbFreeNodes == mNbAllocatedNodes); } // Check if the node structure is valid (for debugging purpose) -void DynamicAABBTree::checkNode(int nodeID) const { +void DynamicAABBTree::checkNode(int32_t nodeID) const { - if (nodeID == TreeNode::NULL_TREE_NODE) return; + if (nodeID == TreeNode::NULL_TREE_NODE) return; - // If it is the root - if (nodeID == mRootNodeID) { - assert(mNodes[nodeID].parentID == TreeNode::NULL_TREE_NODE); - } + // If it is the root + if (nodeID == mRootNodeID) { + assert(mNodes[nodeID].parentID == TreeNode::NULL_TREE_NODE); + } - // Get the children nodes - TreeNode* pNode = mNodes + nodeID; - assert(!pNode->isLeaf()); - int leftChild = pNode->children[0]; - int rightChild = pNode->children[1]; + // Get the children nodes + TreeNode* pNode = mNodes + nodeID; + assert(!pNode->isLeaf()); + int32_t leftChild = pNode->children[0]; + int32_t rightChild = pNode->children[1]; - assert(pNode->height >= 0); - assert(pNode->aabb.getVolume() > 0); + assert(pNode->height >= 0); + assert(pNode->aabb.getVolume() > 0); - // If the current node is a leaf - if (pNode->isLeaf()) { + // If the current node is a leaf + if (pNode->isLeaf()) { - // Check that there are no children - assert(leftChild == TreeNode::NULL_TREE_NODE); - assert(rightChild == TreeNode::NULL_TREE_NODE); - assert(pNode->height == 0); - } - else { + // Check that there are no children + assert(leftChild == TreeNode::NULL_TREE_NODE); + assert(rightChild == TreeNode::NULL_TREE_NODE); + assert(pNode->height == 0); + } + else { - // Check that the children node IDs are valid - assert(0 <= leftChild && leftChild < mNbAllocatedNodes); - assert(0 <= rightChild && rightChild < mNbAllocatedNodes); + // Check that the children node IDs are valid + assert(0 <= leftChild && leftChild < mNbAllocatedNodes); + assert(0 <= rightChild && rightChild < mNbAllocatedNodes); - // Check that the children nodes have the correct parent node - assert(mNodes[leftChild].parentID == nodeID); - assert(mNodes[rightChild].parentID == nodeID); + // Check that the children nodes have the correct parent node + assert(mNodes[leftChild].parentID == nodeID); + assert(mNodes[rightChild].parentID == nodeID); - // Check the height of node - int height = 1 + std::max(mNodes[leftChild].height, mNodes[rightChild].height); - assert(mNodes[nodeID].height == height); + // Check the height of node + int32_t height = 1 + std::max(mNodes[leftChild].height, mNodes[rightChild].height); + assert(mNodes[nodeID].height == height); - // Check the AABB of the node - AABB aabb; - aabb.mergeTwoAABBs(mNodes[leftChild].aabb, mNodes[rightChild].aabb); - assert(aabb.getMin() == mNodes[nodeID].aabb.getMin()); - assert(aabb.getMax() == mNodes[nodeID].aabb.getMax()); + // Check the AABB of the node + AABB aabb; + aabb.mergeTwoAABBs(mNodes[leftChild].aabb, mNodes[rightChild].aabb); + assert(aabb.getMin() == mNodes[nodeID].aabb.getMin()); + assert(aabb.getMax() == mNodes[nodeID].aabb.getMax()); - // Recursively check the children nodes - checkNode(leftChild); - checkNode(rightChild); - } + // Recursively check the children nodes + checkNode(leftChild); + checkNode(rightChild); + } } // Compute the height of the tree -int DynamicAABBTree::computeHeight() { +int32_t DynamicAABBTree::computeHeight() { return computeHeight(mRootNodeID); } // Compute the height of a given node in the tree -int DynamicAABBTree::computeHeight(int nodeID) { - assert(nodeID >= 0 && nodeID < mNbAllocatedNodes); - TreeNode* node = mNodes + nodeID; +int32_t DynamicAABBTree::computeHeight(int32_t nodeID) { + assert(nodeID >= 0 && nodeID < mNbAllocatedNodes); + TreeNode* node = mNodes + nodeID; - // If the node is a leaf, its height is zero - if (node->isLeaf()) { - return 0; - } + // If the node is a leaf, its height is zero + if (node->isLeaf()) { + return 0; + } - // Compute the height of the left and right sub-tree - int leftHeight = computeHeight(node->children[0]); - int rightHeight = computeHeight(node->children[1]); + // Compute the height of the left and right sub-tree + int32_t leftHeight = computeHeight(node->children[0]); + int32_t rightHeight = computeHeight(node->children[1]); - // Return the height of the node - return 1 + std::max(leftHeight, rightHeight); + // Return the height of the node + return 1 + std::max(leftHeight, rightHeight); } #endif diff --git a/ephysics/collision/broadphase/DynamicAABBTree.h b/ephysics/collision/broadphase/DynamicAABBTree.h index 01a9212..942ccc2 100644 --- a/ephysics/collision/broadphase/DynamicAABBTree.h +++ b/ephysics/collision/broadphase/DynamicAABBTree.h @@ -1,30 +1,9 @@ -/******************************************************************************** -* 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 REACTPHYSICS3D_DYNAMIC_AABB_TREE_H -#define REACTPHYSICS3D_DYNAMIC_AABB_TREE_H +/** @file + * @author Daniel Chappuis + * @copyright 2010-2016 Daniel Chappuis + * @license BSD 3 clauses (see license file) + */ +#pragma once // Libraries #include @@ -46,47 +25,47 @@ struct RaycastTest; */ struct TreeNode { - // -------------------- Constants -------------------- // + // -------------------- Constants -------------------- // - /// Null tree node constant - const static int NULL_TREE_NODE; + /// Null tree node constant + const static int32_t NULL_TREE_NODE; - // -------------------- Attributes -------------------- // + // -------------------- Attributes -------------------- // - // A node is either in the tree (has a parent) or in the free nodes list - // (has a next node) - union { + // A node is either in the tree (has a parent) or in the free nodes list + // (has a next node) + union { - /// Parent node ID - int32 parentID; + /// Parent node ID + int32_t parentID; - /// Next allocated node ID - int32 nextNodeID; - }; + /// Next allocated node ID + int32_t nextNodeID; + }; - // A node is either a leaf (has data) or is an internal node (has children) - union { + // A node is either a leaf (has data) or is an int32_ternal node (has children) + union { - /// Left and right child of the node (children[0] = left child) - int32 children[2]; + /// Left and right child of the node (children[0] = left child) + int32_t children[2]; - /// Two pieces of data stored at that node (in case the node is a leaf) - union { - int32 dataInt[2]; - void* dataPointer; - }; - }; + /// Two pieces of data stored at that node (in case the node is a leaf) + union { + int32_t dataInt[2]; + void* dataPointer; + }; + }; - /// Height of the node in the tree - int16 height; + /// Height of the node in the tree + int16_t height; - /// Fat axis aligned bounding box (AABB) corresponding to the node - AABB aabb; + /// Fat axis aligned bounding box (AABB) corresponding to the node + AABB aabb; - // -------------------- Methods -------------------- // + // -------------------- Methods -------------------- // - /// Return true if the node is a leaf of the tree - bool isLeaf() const; + /// Return true if the node is a leaf of the tree + bool isLeaf() const; }; // Class DynamicAABBTreeOverlapCallback @@ -96,12 +75,12 @@ struct TreeNode { */ class DynamicAABBTreeOverlapCallback { - public : - virtual ~DynamicAABBTreeOverlapCallback() = default; + public : + virtual ~DynamicAABBTreeOverlapCallback() = default; - // Called when a overlapping node has been found during the call to - // DynamicAABBTree:reportAllShapesOverlappingWithAABB() - virtual void notifyOverlappingNode(int nodeId)=0; + // Called when a overlapping node has been found during the call to + // DynamicAABBTree:reportAllShapesOverlappingWithAABB() + virtual void notifyOverlappingNode(int32_t nodeId)=0; }; // Class DynamicAABBTreeRaycastCallback @@ -111,11 +90,11 @@ class DynamicAABBTreeOverlapCallback { */ class DynamicAABBTreeRaycastCallback { - public: - virtual ~DynamicAABBTreeRaycastCallback() = default; + public: + virtual ~DynamicAABBTreeRaycastCallback() = default; - // Called when the AABB of a leaf node is hit by a ray - virtual decimal raycastBroadPhaseShape(int32 nodeId, const Ray& ray)=0; + // Called when the AABB of a leaf node is hit by a ray + virtual float raycastBroadPhaseShape(int32_t nodeId, const Ray& ray)=0; }; @@ -129,166 +108,164 @@ class DynamicAABBTreeRaycastCallback { */ class DynamicAABBTree { - private: + private: - // -------------------- Attributes -------------------- // + // -------------------- Attributes -------------------- // - /// Pointer to the memory location of the nodes of the tree - TreeNode* mNodes; + /// Pointer to the memory location of the nodes of the tree + TreeNode* mNodes; - /// ID of the root node of the tree - int mRootNodeID; + /// ID of the root node of the tree + int32_t mRootNodeID; - /// ID of the first node of the list of free (allocated) nodes in the tree that we can use - int mFreeNodeID; + /// ID of the first node of the list of free (allocated) nodes in the tree that we can use + int32_t mFreeNodeID; - /// Number of allocated nodes in the tree - int mNbAllocatedNodes; + /// Number of allocated nodes in the tree + int32_t mNbAllocatedNodes; - /// Number of nodes in the tree - int mNbNodes; + /// Number of nodes in the tree + int32_t mNbNodes; - /// Extra AABB Gap used to allow the collision shape to move a little bit - /// without triggering a large modification of the tree which can be costly - decimal mExtraAABBGap; + /// Extra AABB Gap used to allow the collision shape to move a little bit + /// without triggering a large modification of the tree which can be costly + float mExtraAABBGap; - // -------------------- Methods -------------------- // + // -------------------- Methods -------------------- // - /// Allocate and return a node to use in the tree - int allocateNode(); + /// Allocate and return a node to use in the tree + int32_t allocateNode(); - /// Release a node - void releaseNode(int nodeID); + /// Release a node + void releaseNode(int32_t nodeID); - /// Insert a leaf node in the tree - void insertLeafNode(int nodeID); + /// Insert a leaf node in the tree + void insertLeafNode(int32_t nodeID); - /// Remove a leaf node from the tree - void removeLeafNode(int nodeID); + /// Remove a leaf node from the tree + void removeLeafNode(int32_t nodeID); - /// Balance the sub-tree of a given node using left or right rotations. - int balanceSubTreeAtNode(int nodeID); + /// Balance the sub-tree of a given node using left or right rotations. + int32_t balanceSubTreeAtNode(int32_t nodeID); - /// Compute the height of a given node in the tree - int computeHeight(int nodeID); + /// Compute the height of a given node in the tree + int32_t computeHeight(int32_t nodeID); - /// Internally add an object into the tree - int addObjectInternal(const AABB& aabb); + /// Internally add an object int32_to the tree + int32_t addObjectInternal(const AABB& aabb); - /// Initialize the tree - void init(); + /// Initialize the tree + void init(); #ifndef NDEBUG - /// Check if the tree structure is valid (for debugging purpose) - void check() const; + /// Check if the tree structure is valid (for debugging purpose) + void check() const; - /// Check if the node structure is valid (for debugging purpose) - void checkNode(int nodeID) const; + /// Check if the node structure is valid (for debugging purpose) + void checkNode(int32_t nodeID) const; #endif - public: + public: - // -------------------- Methods -------------------- // + // -------------------- Methods -------------------- // - /// Constructor - DynamicAABBTree(decimal extraAABBGap = decimal(0.0)); + /// Constructor + DynamicAABBTree(float extraAABBGap = float(0.0)); - /// Destructor - virtual ~DynamicAABBTree(); + /// Destructor + virtual ~DynamicAABBTree(); - /// Add an object into the tree (where node data are two integers) - int addObject(const AABB& aabb, int32 data1, int32 data2); + /// Add an object int32_to the tree (where node data are two int32_tegers) + int32_t addObject(const AABB& aabb, int32_t data1, int32_t data2); - /// Add an object into the tree (where node data is a pointer) - int addObject(const AABB& aabb, void* data); + /// Add an object int32_to the tree (where node data is a pointer) + int32_t addObject(const AABB& aabb, void* data); - /// Remove an object from the tree - void removeObject(int nodeID); + /// Remove an object from the tree + void removeObject(int32_t nodeID); - /// Update the dynamic tree after an object has moved. - bool updateObject(int nodeID, const AABB& newAABB, const Vector3& displacement, bool forceReinsert = false); + /// Update the dynamic tree after an object has moved. + bool updateObject(int32_t nodeID, const AABB& newAABB, const Vector3& displacement, bool forceReinsert = false); - /// Return the fat AABB corresponding to a given node ID - const AABB& getFatAABB(int nodeID) const; + /// Return the fat AABB corresponding to a given node ID + const AABB& getFatAABB(int32_t nodeID) const; - /// Return the pointer to the data array of a given leaf node of the tree - int32* getNodeDataInt(int nodeID) const; + /// Return the pointer to the data array of a given leaf node of the tree + int32_t* getNodeDataInt(int32_t nodeID) const; - /// Return the data pointer of a given leaf node of the tree - void* getNodeDataPointer(int nodeID) const; + /// Return the data pointer of a given leaf node of the tree + void* getNodeDataPointer(int32_t nodeID) const; - /// Report all shapes overlapping with the AABB given in parameter. - void reportAllShapesOverlappingWithAABB(const AABB& aabb, - DynamicAABBTreeOverlapCallback& callback) const; + /// Report all shapes overlapping with the AABB given in parameter. + void reportAllShapesOverlappingWithAABB(const AABB& aabb, + DynamicAABBTreeOverlapCallback& callback) const; - /// Ray casting method - void raycast(const Ray& ray, DynamicAABBTreeRaycastCallback& callback) const; + /// Ray casting method + void raycast(const Ray& ray, DynamicAABBTreeRaycastCallback& callback) const; - /// Compute the height of the tree - int computeHeight(); + /// Compute the height of the tree + int32_t computeHeight(); - /// Return the root AABB of the tree - AABB getRootAABB() const; + /// Return the root AABB of the tree + AABB getRootAABB() const; - /// Clear all the nodes and reset the tree - void reset(); + /// Clear all the nodes and reset the tree + void reset(); }; // Return true if the node is a leaf of the tree inline bool TreeNode::isLeaf() const { - return (height == 0); + return (height == 0); } // Return the fat AABB corresponding to a given node ID -inline const AABB& DynamicAABBTree::getFatAABB(int nodeID) const { - assert(nodeID >= 0 && nodeID < mNbAllocatedNodes); - return mNodes[nodeID].aabb; +inline const AABB& DynamicAABBTree::getFatAABB(int32_t 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(int nodeID) const { - assert(nodeID >= 0 && nodeID < mNbAllocatedNodes); - assert(mNodes[nodeID].isLeaf()); - return mNodes[nodeID].dataInt; +inline int32_t* DynamicAABBTree::getNodeDataInt(int32_t 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(int nodeID) const { - assert(nodeID >= 0 && nodeID < mNbAllocatedNodes); - assert(mNodes[nodeID].isLeaf()); - return mNodes[nodeID].dataPointer; +inline void* DynamicAABBTree::getNodeDataPointer(int32_t 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 { - return getFatAABB(mRootNodeID); + return getFatAABB(mRootNodeID); } -// Add an object into the tree. This method creates a new leaf node in the tree and +// Add an object int32_to the tree. This method creates a new leaf node in the tree and // returns the ID of the corresponding node. -inline int DynamicAABBTree::addObject(const AABB& aabb, int32 data1, int32 data2) { +inline int32_t DynamicAABBTree::addObject(const AABB& aabb, int32_t data1, int32_t data2) { - int nodeId = addObjectInternal(aabb); + int32_t nodeId = addObjectInternal(aabb); - mNodes[nodeId].dataInt[0] = data1; - mNodes[nodeId].dataInt[1] = data2; + mNodes[nodeId].dataInt[0] = data1; + mNodes[nodeId].dataInt[1] = data2; - return nodeId; + return nodeId; } -// Add an object into the tree. This method creates a new leaf node in the tree and +// Add an object int32_to the tree. This method creates a new leaf node in the tree and // returns the ID of the corresponding node. -inline int DynamicAABBTree::addObject(const AABB& aabb, void* data) { +inline int32_t DynamicAABBTree::addObject(const AABB& aabb, void* data) { - int nodeId = addObjectInternal(aabb); + int32_t nodeId = addObjectInternal(aabb); - mNodes[nodeId].dataPointer = data; + mNodes[nodeId].dataPointer = data; - return nodeId; + return nodeId; } } - -#endif diff --git a/ephysics/collision/narrowphase/CollisionDispatch.h b/ephysics/collision/narrowphase/CollisionDispatch.h index c093864..a032cd8 100644 --- a/ephysics/collision/narrowphase/CollisionDispatch.h +++ b/ephysics/collision/narrowphase/CollisionDispatch.h @@ -1,30 +1,9 @@ -/******************************************************************************** -* 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 REACTPHYSICS3D_COLLISION_DISPATCH_H -#define REACTPHYSICS3D_COLLISION_DISPATCH_H +/** @file + * @author Daniel Chappuis + * @copyright 2010-2016 Daniel Chappuis + * @license BSD 3 clauses (see license file) + */ +#pragma once // Libraries #include @@ -39,29 +18,27 @@ namespace reactphysics3d { */ class CollisionDispatch { - protected: + protected: - public: + public: - /// Constructor - CollisionDispatch() {} + /// Constructor + CollisionDispatch() {} - /// Destructor - virtual ~CollisionDispatch() {} + /// Destructor + virtual ~CollisionDispatch() {} - /// Initialize the collision dispatch configuration - virtual void init(CollisionDetection* collisionDetection, - MemoryAllocator* memoryAllocator) { + /// Initialize the collision dispatch configuration + virtual void init(CollisionDetection* collisionDetection, + MemoryAllocator* memoryAllocator) { + } - } - - /// Select and return the narrow-phase collision detection algorithm to - /// use between two types of collision shapes. - virtual NarrowPhaseAlgorithm* selectAlgorithm(int shape1Type, - int shape2Type)=0; + /// Select and return the narrow-phase collision detection algorithm to + /// use between two types of collision shapes. + virtual NarrowPhaseAlgorithm* selectAlgorithm(int32_t shape1Type, + int32_t shape2Type)=0; }; } -#endif diff --git a/ephysics/collision/narrowphase/ConcaveVsConvexAlgorithm.cpp b/ephysics/collision/narrowphase/ConcaveVsConvexAlgorithm.cpp index 56db041..8ef4f6a 100644 --- a/ephysics/collision/narrowphase/ConcaveVsConvexAlgorithm.cpp +++ b/ephysics/collision/narrowphase/ConcaveVsConvexAlgorithm.cpp @@ -1,27 +1,8 @@ -/******************************************************************************** -* 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. * -* * -********************************************************************************/ +/** @file + * @author Daniel Chappuis + * @copyright 2010-2016 Daniel Chappuis + * @license BSD 3 clauses (see license file) + */ // Libraries #include @@ -45,247 +26,247 @@ ConcaveVsConvexAlgorithm::~ConcaveVsConvexAlgorithm() { // Return true and compute a contact info if the two bounding volumes collide void ConcaveVsConvexAlgorithm::testCollision(const CollisionShapeInfo& shape1Info, - const CollisionShapeInfo& shape2Info, - NarrowPhaseCallback* narrowPhaseCallback) { + const CollisionShapeInfo& shape2Info, + NarrowPhaseCallback* narrowPhaseCallback) { - ProxyShape* convexProxyShape; - ProxyShape* concaveProxyShape; - const ConvexShape* convexShape; - const ConcaveShape* concaveShape; + ProxyShape* convexProxyShape; + ProxyShape* concaveProxyShape; + const ConvexShape* convexShape; + const ConcaveShape* concaveShape; - // Collision shape 1 is convex, collision shape 2 is concave - if (shape1Info.collisionShape->isConvex()) { - convexProxyShape = shape1Info.proxyShape; - convexShape = static_cast(shape1Info.collisionShape); - concaveProxyShape = shape2Info.proxyShape; - concaveShape = static_cast(shape2Info.collisionShape); - } - else { // Collision shape 2 is convex, collision shape 1 is concave - convexProxyShape = shape2Info.proxyShape; - convexShape = static_cast(shape2Info.collisionShape); - concaveProxyShape = shape1Info.proxyShape; - concaveShape = static_cast(shape1Info.collisionShape); - } + // Collision shape 1 is convex, collision shape 2 is concave + if (shape1Info.collisionShape->isConvex()) { + convexProxyShape = shape1Info.proxyShape; + convexShape = static_cast(shape1Info.collisionShape); + concaveProxyShape = shape2Info.proxyShape; + concaveShape = static_cast(shape2Info.collisionShape); + } + else { // Collision shape 2 is convex, collision shape 1 is concave + convexProxyShape = shape2Info.proxyShape; + convexShape = static_cast(shape2Info.collisionShape); + concaveProxyShape = shape1Info.proxyShape; + concaveShape = static_cast(shape1Info.collisionShape); + } - // Set the parameters of the callback object - ConvexVsTriangleCallback convexVsTriangleCallback; - convexVsTriangleCallback.setCollisionDetection(mCollisionDetection); - convexVsTriangleCallback.setConvexShape(convexShape); - convexVsTriangleCallback.setConcaveShape(concaveShape); - convexVsTriangleCallback.setProxyShapes(convexProxyShape, concaveProxyShape); - convexVsTriangleCallback.setOverlappingPair(shape1Info.overlappingPair); + // Set the parameters of the callback object + ConvexVsTriangleCallback convexVsTriangleCallback; + convexVsTriangleCallback.setCollisionDetection(mCollisionDetection); + convexVsTriangleCallback.setConvexShape(convexShape); + convexVsTriangleCallback.setConcaveShape(concaveShape); + convexVsTriangleCallback.setProxyShapes(convexProxyShape, concaveProxyShape); + convexVsTriangleCallback.setOverlappingPair(shape1Info.overlappingPair); - // Compute the convex shape AABB in the local-space of the convex shape - AABB aabb; - convexShape->computeAABB(aabb, convexProxyShape->getLocalToWorldTransform()); + // Compute the convex shape AABB in the local-space of the convex shape + AABB aabb; + convexShape->computeAABB(aabb, convexProxyShape->getLocalToWorldTransform()); - // If smooth mesh collision is enabled for the concave mesh - if (concaveShape->getIsSmoothMeshCollisionEnabled()) { + // If smooth mesh collision is enabled for the concave mesh + if (concaveShape->getIsSmoothMeshCollisionEnabled()) { - std::vector contactPoints; + std::vector contactPoints; - SmoothCollisionNarrowPhaseCallback smoothNarrowPhaseCallback(contactPoints); + SmoothCollisionNarrowPhaseCallback smoothNarrowPhaseCallback(contactPoints); - convexVsTriangleCallback.setNarrowPhaseCallback(&smoothNarrowPhaseCallback); + convexVsTriangleCallback.setNarrowPhaseCallback(&smoothNarrowPhaseCallback); - // Call the convex vs triangle callback for each triangle of the concave shape - concaveShape->testAllTriangles(convexVsTriangleCallback, aabb); + // Call the convex vs triangle callback for each triangle of the concave shape + concaveShape->testAllTriangles(convexVsTriangleCallback, aabb); - // Run the smooth mesh collision algorithm - processSmoothMeshCollision(shape1Info.overlappingPair, contactPoints, narrowPhaseCallback); - } - else { + // Run the smooth mesh collision algorithm + processSmoothMeshCollision(shape1Info.overlappingPair, contactPoints, narrowPhaseCallback); + } + else { - convexVsTriangleCallback.setNarrowPhaseCallback(narrowPhaseCallback); + convexVsTriangleCallback.setNarrowPhaseCallback(narrowPhaseCallback); - // Call the convex vs triangle callback for each triangle of the concave shape - concaveShape->testAllTriangles(convexVsTriangleCallback, aabb); - } + // Call the convex vs triangle callback for each triangle of the concave shape + concaveShape->testAllTriangles(convexVsTriangleCallback, aabb); + } } // Test collision between a triangle and the convex mesh shape void ConvexVsTriangleCallback::testTriangle(const Vector3* trianglePoints) { - // Create a triangle collision shape - decimal margin = mConcaveShape->getTriangleMargin(); - TriangleShape triangleShape(trianglePoints[0], trianglePoints[1], trianglePoints[2], margin); + // Create a triangle collision shape + float margin = mConcaveShape->getTriangleMargin(); + TriangleShape triangleShape(trianglePoints[0], trianglePoints[1], trianglePoints[2], margin); - // Select the collision algorithm to use between the triangle and the convex shape - NarrowPhaseAlgorithm* algo = mCollisionDetection->getCollisionAlgorithm(triangleShape.getType(), - mConvexShape->getType()); + // Select the collision algorithm to use between the triangle and the convex shape + NarrowPhaseAlgorithm* algo = mCollisionDetection->getCollisionAlgorithm(triangleShape.getType(), + mConvexShape->getType()); - // If there is no collision algorithm between those two kinds of shapes - if (algo == NULL) return; + // If there is no collision algorithm between those two kinds of shapes + if (algo == NULL) return; - // Notify the narrow-phase algorithm about the overlapping pair we are going to test - algo->setCurrentOverlappingPair(mOverlappingPair); + // Notify the narrow-phase algorithm about the overlapping pair we are going to test + algo->setCurrentOverlappingPair(mOverlappingPair); - // Create the CollisionShapeInfo objects - CollisionShapeInfo shapeConvexInfo(mConvexProxyShape, mConvexShape, mConvexProxyShape->getLocalToWorldTransform(), - mOverlappingPair, mConvexProxyShape->getCachedCollisionData()); - CollisionShapeInfo shapeConcaveInfo(mConcaveProxyShape, &triangleShape, - mConcaveProxyShape->getLocalToWorldTransform(), - mOverlappingPair, mConcaveProxyShape->getCachedCollisionData()); + // Create the CollisionShapeInfo objects + CollisionShapeInfo shapeConvexInfo(mConvexProxyShape, mConvexShape, mConvexProxyShape->getLocalToWorldTransform(), + mOverlappingPair, mConvexProxyShape->getCachedCollisionData()); + CollisionShapeInfo shapeConcaveInfo(mConcaveProxyShape, &triangleShape, + mConcaveProxyShape->getLocalToWorldTransform(), + mOverlappingPair, mConcaveProxyShape->getCachedCollisionData()); - // Use the collision algorithm to test collision between the triangle and the other convex shape - algo->testCollision(shapeConvexInfo, shapeConcaveInfo, mNarrowPhaseCallback); + // Use the collision algorithm to test collision between the triangle and the other convex shape + algo->testCollision(shapeConvexInfo, shapeConcaveInfo, mNarrowPhaseCallback); } // Process the concave triangle mesh collision using the smooth mesh collision algorithm described // by Pierre Terdiman (http://www.codercorner.com/MeshContacts.pdf). This is used to avoid the collision -// issue with some internal edges. +// issue with some int32_ternal edges. void ConcaveVsConvexAlgorithm::processSmoothMeshCollision(OverlappingPair* overlappingPair, - std::vector contactPoints, - NarrowPhaseCallback* narrowPhaseCallback) { + std::vector contactPoints, + NarrowPhaseCallback* narrowPhaseCallback) { - // Set with the triangle vertices already processed to void further contacts with same triangle - std::unordered_multimap processTriangleVertices; + // Set with the triangle vertices already processed to void further contacts with same triangle + std::unordered_multimap processTriangleVertices; - // Sort the list of narrow-phase contacts according to their penetration depth - std::sort(contactPoints.begin(), contactPoints.end(), ContactsDepthCompare()); + // Sort the list of narrow-phase contacts according to their penetration depth + std::sort(contactPoints.begin(), contactPoints.end(), ContactsDepthCompare()); - // For each contact point (from smaller penetration depth to larger) - std::vector::const_iterator it; - for (it = contactPoints.begin(); it != contactPoints.end(); ++it) { + // For each contact point (from smaller penetration depth to larger) + std::vector::const_iterator it; + for (it = contactPoints.begin(); it != contactPoints.end(); ++it) { - const SmoothMeshContactInfo info = *it; - const Vector3& contactPoint = info.isFirstShapeTriangle ? info.contactInfo.localPoint1 : info.contactInfo.localPoint2; + const SmoothMeshContactInfo info = *it; + const Vector3& contactPoint = info.isFirstShapeTriangle ? info.contactInfo.localPoint1 : info.contactInfo.localPoint2; - // Compute the barycentric coordinates of the point in the triangle - decimal u, v, w; - computeBarycentricCoordinatesInTriangle(info.triangleVertices[0], - info.triangleVertices[1], - info.triangleVertices[2], - contactPoint, u, v, w); - int nbZeros = 0; - bool isUZero = approxEqual(u, 0, 0.0001); - bool isVZero = approxEqual(v, 0, 0.0001); - bool isWZero = approxEqual(w, 0, 0.0001); - if (isUZero) nbZeros++; - if (isVZero) nbZeros++; - if (isWZero) nbZeros++; + // Compute the barycentric coordinates of the point in the triangle + float u, v, w; + computeBarycentricCoordinatesInTriangle(info.triangleVertices[0], + info.triangleVertices[1], + info.triangleVertices[2], + contactPoint, u, v, w); + int32_t nbZeros = 0; + bool isUZero = approxEqual(u, 0, 0.0001); + bool isVZero = approxEqual(v, 0, 0.0001); + bool isWZero = approxEqual(w, 0, 0.0001); + if (isUZero) nbZeros++; + if (isVZero) nbZeros++; + if (isWZero) nbZeros++; - // If it is a vertex contact - if (nbZeros == 2) { + // If it is a vertex contact + if (nbZeros == 2) { - Vector3 contactVertex = !isUZero ? info.triangleVertices[0] : (!isVZero ? info.triangleVertices[1] : info.triangleVertices[2]); + Vector3 contactVertex = !isUZero ? info.triangleVertices[0] : (!isVZero ? info.triangleVertices[1] : info.triangleVertices[2]); - // Check that this triangle vertex has not been processed yet - if (!hasVertexBeenProcessed(processTriangleVertices, contactVertex)) { + // Check that this triangle vertex has not been processed yet + if (!hasVertexBeenProcessed(processTriangleVertices, contactVertex)) { - // Keep the contact as it is and report it - narrowPhaseCallback->notifyContact(overlappingPair, info.contactInfo); - } - } - else if (nbZeros == 1) { // If it is an edge contact + // Keep the contact as it is and report it + narrowPhaseCallback->notifyContact(overlappingPair, info.contactInfo); + } + } + else if (nbZeros == 1) { // If it is an edge contact - Vector3 contactVertex1 = isUZero ? info.triangleVertices[1] : (isVZero ? info.triangleVertices[0] : info.triangleVertices[0]); - Vector3 contactVertex2 = isUZero ? info.triangleVertices[2] : (isVZero ? info.triangleVertices[2] : info.triangleVertices[1]); + Vector3 contactVertex1 = isUZero ? info.triangleVertices[1] : (isVZero ? info.triangleVertices[0] : info.triangleVertices[0]); + Vector3 contactVertex2 = isUZero ? info.triangleVertices[2] : (isVZero ? info.triangleVertices[2] : info.triangleVertices[1]); - // Check that this triangle edge has not been processed yet - if (!hasVertexBeenProcessed(processTriangleVertices, contactVertex1) && - !hasVertexBeenProcessed(processTriangleVertices, contactVertex2)) { + // Check that this triangle edge has not been processed yet + if (!hasVertexBeenProcessed(processTriangleVertices, contactVertex1) && + !hasVertexBeenProcessed(processTriangleVertices, contactVertex2)) { - // Keep the contact as it is and report it - narrowPhaseCallback->notifyContact(overlappingPair, info.contactInfo); - } + // Keep the contact as it is and report it + narrowPhaseCallback->notifyContact(overlappingPair, info.contactInfo); + } - } - else { // If it is a face contact + } + else { // If it is a face contact - ContactPointInfo newContactInfo(info.contactInfo); + ContactPointInfo newContactInfo(info.contactInfo); - ProxyShape* firstShape; - ProxyShape* secondShape; - if (info.isFirstShapeTriangle) { - firstShape = overlappingPair->getShape1(); - secondShape = overlappingPair->getShape2(); - } - else { - firstShape = overlappingPair->getShape2(); - secondShape = overlappingPair->getShape1(); - } + ProxyShape* firstShape; + ProxyShape* secondShape; + if (info.isFirstShapeTriangle) { + firstShape = overlappingPair->getShape1(); + secondShape = overlappingPair->getShape2(); + } + else { + firstShape = overlappingPair->getShape2(); + secondShape = overlappingPair->getShape1(); + } - // We use the triangle normal as the contact normal - Vector3 a = info.triangleVertices[1] - info.triangleVertices[0]; - Vector3 b = info.triangleVertices[2] - info.triangleVertices[0]; - Vector3 localNormal = a.cross(b); - newContactInfo.normal = firstShape->getLocalToWorldTransform().getOrientation() * localNormal; - Vector3 firstLocalPoint = info.isFirstShapeTriangle ? info.contactInfo.localPoint1 : info.contactInfo.localPoint2; - Vector3 firstWorldPoint = firstShape->getLocalToWorldTransform() * firstLocalPoint; - newContactInfo.normal.normalize(); - if (newContactInfo.normal.dot(info.contactInfo.normal) < 0) { - newContactInfo.normal = -newContactInfo.normal; - } + // We use the triangle normal as the contact normal + Vector3 a = info.triangleVertices[1] - info.triangleVertices[0]; + Vector3 b = info.triangleVertices[2] - info.triangleVertices[0]; + Vector3 localNormal = a.cross(b); + newContactInfo.normal = firstShape->getLocalToWorldTransform().getOrientation() * localNormal; + Vector3 firstLocalPoint = info.isFirstShapeTriangle ? info.contactInfo.localPoint1 : info.contactInfo.localPoint2; + Vector3 firstWorldPoint = firstShape->getLocalToWorldTransform() * firstLocalPoint; + newContactInfo.normal.normalize(); + if (newContactInfo.normal.dot(info.contactInfo.normal) < 0) { + newContactInfo.normal = -newContactInfo.normal; + } - // We recompute the contact point on the second body with the new normal as described in - // the Smooth Mesh Contacts with GJK of the Game Physics Pearls book (from Gino van Den Bergen and - // Dirk Gregorius) to avoid adding torque - Transform worldToLocalSecondPoint = secondShape->getLocalToWorldTransform().getInverse(); - if (info.isFirstShapeTriangle) { - Vector3 newSecondWorldPoint = firstWorldPoint + newContactInfo.normal; - newContactInfo.localPoint2 = worldToLocalSecondPoint * newSecondWorldPoint; - } - else { - Vector3 newSecondWorldPoint = firstWorldPoint - newContactInfo.normal; - newContactInfo.localPoint1 = worldToLocalSecondPoint * newSecondWorldPoint; - } + // We recompute the contact point on the second body with the new normal as described in + // the Smooth Mesh Contacts with GJK of the Game Physics Pearls book (from Gino van Den Bergen and + // Dirk Gregorius) to avoid adding torque + Transform worldToLocalSecondPoint = secondShape->getLocalToWorldTransform().getInverse(); + if (info.isFirstShapeTriangle) { + Vector3 newSecondWorldPoint = firstWorldPoint + newContactInfo.normal; + newContactInfo.localPoint2 = worldToLocalSecondPoint * newSecondWorldPoint; + } + else { + Vector3 newSecondWorldPoint = firstWorldPoint - newContactInfo.normal; + newContactInfo.localPoint1 = worldToLocalSecondPoint * newSecondWorldPoint; + } - // Report the contact - narrowPhaseCallback->notifyContact(overlappingPair, newContactInfo); - } + // Report the contact + narrowPhaseCallback->notifyContact(overlappingPair, newContactInfo); + } - // Add the three vertices of the triangle to the set of processed - // triangle vertices - addProcessedVertex(processTriangleVertices, info.triangleVertices[0]); - addProcessedVertex(processTriangleVertices, info.triangleVertices[1]); - addProcessedVertex(processTriangleVertices, info.triangleVertices[2]); - } + // Add the three vertices of the triangle to the set of processed + // triangle vertices + addProcessedVertex(processTriangleVertices, info.triangleVertices[0]); + addProcessedVertex(processTriangleVertices, info.triangleVertices[1]); + addProcessedVertex(processTriangleVertices, info.triangleVertices[2]); + } } // Return true if the vertex is in the set of already processed vertices -bool ConcaveVsConvexAlgorithm::hasVertexBeenProcessed(const std::unordered_multimap& processTriangleVertices, const Vector3& vertex) const { +bool ConcaveVsConvexAlgorithm::hasVertexBeenProcessed(const std::unordered_multimap& processTriangleVertices, const Vector3& vertex) const { - int key = int(vertex.x * vertex.y * vertex.z); + int32_t key = int32_t(vertex.x * vertex.y * vertex.z); - auto range = processTriangleVertices.equal_range(key); - for (auto it = range.first; it != range.second; ++it) { - if (vertex.x == it->second.x && vertex.y == it->second.y && vertex.z == it->second.z) return true; - } + auto range = processTriangleVertices.equal_range(key); + for (auto it = range.first; it != range.second; ++it) { + if (vertex.x == it->second.x && vertex.y == it->second.y && vertex.z == it->second.z) return true; + } - return false; + return false; } // Called by a narrow-phase collision algorithm when a new contact has been found void SmoothCollisionNarrowPhaseCallback::notifyContact(OverlappingPair* overlappingPair, - const ContactPointInfo& contactInfo) { - Vector3 triangleVertices[3]; - bool isFirstShapeTriangle; + const ContactPointInfo& contactInfo) { + Vector3 triangleVertices[3]; + bool isFirstShapeTriangle; - // If the collision shape 1 is the triangle - if (contactInfo.collisionShape1->getType() == TRIANGLE) { - assert(contactInfo.collisionShape2->getType() != TRIANGLE); + // If the collision shape 1 is the triangle + if (contactInfo.collisionShape1->getType() == TRIANGLE) { + assert(contactInfo.collisionShape2->getType() != TRIANGLE); - const TriangleShape* triangleShape = static_cast(contactInfo.collisionShape1); - triangleVertices[0] = triangleShape->getVertex(0); - triangleVertices[1] = triangleShape->getVertex(1); - triangleVertices[2] = triangleShape->getVertex(2); + const TriangleShape* triangleShape = static_cast(contactInfo.collisionShape1); + triangleVertices[0] = triangleShape->getVertex(0); + triangleVertices[1] = triangleShape->getVertex(1); + triangleVertices[2] = triangleShape->getVertex(2); - isFirstShapeTriangle = true; - } - else { // If the collision shape 2 is the triangle - assert(contactInfo.collisionShape2->getType() == TRIANGLE); + isFirstShapeTriangle = true; + } + else { // If the collision shape 2 is the triangle + assert(contactInfo.collisionShape2->getType() == TRIANGLE); - const TriangleShape* triangleShape = static_cast(contactInfo.collisionShape2); - triangleVertices[0] = triangleShape->getVertex(0); - triangleVertices[1] = triangleShape->getVertex(1); - triangleVertices[2] = triangleShape->getVertex(2); + const TriangleShape* triangleShape = static_cast(contactInfo.collisionShape2); + triangleVertices[0] = triangleShape->getVertex(0); + triangleVertices[1] = triangleShape->getVertex(1); + triangleVertices[2] = triangleShape->getVertex(2); - isFirstShapeTriangle = false; - } - SmoothMeshContactInfo smoothContactInfo(contactInfo, isFirstShapeTriangle, triangleVertices[0], triangleVertices[1], triangleVertices[2]); + isFirstShapeTriangle = false; + } + SmoothMeshContactInfo smoothContactInfo(contactInfo, isFirstShapeTriangle, triangleVertices[0], triangleVertices[1], triangleVertices[2]); - // Add the narrow-phase contact into the list of contact to process for - // smooth mesh collision - mContactPoints.push_back(smoothContactInfo); + // Add the narrow-phase contact int32_to the list of contact to process for + // smooth mesh collision + mContactPoints.push_back(smoothContactInfo); } diff --git a/ephysics/collision/narrowphase/ConcaveVsConvexAlgorithm.h b/ephysics/collision/narrowphase/ConcaveVsConvexAlgorithm.h index f0597d9..ab179f6 100644 --- a/ephysics/collision/narrowphase/ConcaveVsConvexAlgorithm.h +++ b/ephysics/collision/narrowphase/ConcaveVsConvexAlgorithm.h @@ -1,30 +1,9 @@ -/******************************************************************************** -* 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 REACTPHYSICS3D_CONCAVE_VS_CONVEX_ALGORITHM_H -#define REACTPHYSICS3D_CONCAVE_VS_CONVEX_ALGORITHM_H +/** @file + * @author Daniel Chappuis + * @copyright 2010-2016 Daniel Chappuis + * @license BSD 3 clauses (see license file) + */ +#pragma once // Libraries #include @@ -43,68 +22,68 @@ namespace reactphysics3d { */ class ConvexVsTriangleCallback : public TriangleCallback { - protected: + protected: - /// Pointer to the collision detection object - CollisionDetection* mCollisionDetection; + /// Pointer to the collision detection object + CollisionDetection* mCollisionDetection; - /// Narrow-phase collision callback - NarrowPhaseCallback* mNarrowPhaseCallback; + /// Narrow-phase collision callback + NarrowPhaseCallback* mNarrowPhaseCallback; - /// Convex collision shape to test collision with - const ConvexShape* mConvexShape; + /// Convex collision shape to test collision with + const ConvexShape* mConvexShape; - /// Concave collision shape - const ConcaveShape* mConcaveShape; + /// Concave collision shape + const ConcaveShape* mConcaveShape; - /// Proxy shape of the convex collision shape - ProxyShape* mConvexProxyShape; + /// Proxy shape of the convex collision shape + ProxyShape* mConvexProxyShape; - /// Proxy shape of the concave collision shape - ProxyShape* mConcaveProxyShape; + /// Proxy shape of the concave collision shape + ProxyShape* mConcaveProxyShape; - /// Broadphase overlapping pair - OverlappingPair* mOverlappingPair; + /// Broadphase overlapping pair + OverlappingPair* mOverlappingPair; - /// Used to sort ContactPointInfos according to their penetration depth - static bool contactsDepthCompare(const ContactPointInfo& contact1, - const ContactPointInfo& contact2); + /// Used to sort ContactPointInfos according to their penetration depth + static bool contactsDepthCompare(const ContactPointInfo& contact1, + const ContactPointInfo& contact2); - public: + public: - /// Set the collision detection pointer - void setCollisionDetection(CollisionDetection* collisionDetection) { - mCollisionDetection = collisionDetection; - } + /// Set the collision detection pointer + void setCollisionDetection(CollisionDetection* collisionDetection) { + mCollisionDetection = collisionDetection; + } - /// Set the narrow-phase collision callback - void setNarrowPhaseCallback(NarrowPhaseCallback* callback) { - mNarrowPhaseCallback = callback; - } + /// Set the narrow-phase collision callback + void setNarrowPhaseCallback(NarrowPhaseCallback* callback) { + mNarrowPhaseCallback = callback; + } - /// Set the convex collision shape to test collision with - void setConvexShape(const ConvexShape* convexShape) { - mConvexShape = convexShape; - } + /// Set the convex collision shape to test collision with + void setConvexShape(const ConvexShape* convexShape) { + mConvexShape = convexShape; + } - /// Set the concave collision shape - void setConcaveShape(const ConcaveShape* concaveShape) { - mConcaveShape = concaveShape; - } + /// Set the concave collision shape + void setConcaveShape(const ConcaveShape* concaveShape) { + mConcaveShape = concaveShape; + } - /// Set the broadphase overlapping pair - void setOverlappingPair(OverlappingPair* overlappingPair) { - mOverlappingPair = overlappingPair; - } + /// Set the broadphase overlapping pair + void setOverlappingPair(OverlappingPair* overlappingPair) { + mOverlappingPair = overlappingPair; + } - /// Set the proxy shapes of the two collision shapes - void setProxyShapes(ProxyShape* convexProxyShape, ProxyShape* concaveProxyShape) { - mConvexProxyShape = convexProxyShape; - mConcaveProxyShape = concaveProxyShape; - } + /// Set the proxy shapes of the two collision shapes + void setProxyShapes(ProxyShape* convexProxyShape, ProxyShape* concaveProxyShape) { + mConvexProxyShape = convexProxyShape; + mConcaveProxyShape = concaveProxyShape; + } - /// Test collision between a triangle and the convex mesh shape - virtual void testTriangle(const Vector3* trianglePoints); + /// Test collision between a triangle and the convex mesh shape + virtual void testTriangle(const Vector3* trianglePoints); }; // Class SmoothMeshContactInfo @@ -114,35 +93,35 @@ class ConvexVsTriangleCallback : public TriangleCallback { */ class SmoothMeshContactInfo { - public: + public: - ContactPointInfo contactInfo; - bool isFirstShapeTriangle; - Vector3 triangleVertices[3]; + ContactPointInfo contactInfo; + bool isFirstShapeTriangle; + Vector3 triangleVertices[3]; - /// Constructor - SmoothMeshContactInfo(const ContactPointInfo& contact, bool firstShapeTriangle, const Vector3& trianglePoint1, - const Vector3& trianglePoint2, const Vector3& trianglePoint3) - : contactInfo(contact) { - isFirstShapeTriangle = firstShapeTriangle; - triangleVertices[0] = trianglePoint1; - triangleVertices[1] = trianglePoint2; - triangleVertices[2] = trianglePoint3; - } + /// Constructor + SmoothMeshContactInfo(const ContactPointInfo& contact, bool firstShapeTriangle, const Vector3& trianglePoint1, + const Vector3& trianglePoint2, const Vector3& trianglePoint3) + : contactInfo(contact) { + isFirstShapeTriangle = firstShapeTriangle; + triangleVertices[0] = trianglePoint1; + triangleVertices[1] = trianglePoint2; + triangleVertices[2] = trianglePoint3; + } }; struct ContactsDepthCompare { - bool operator()(const SmoothMeshContactInfo& contact1, const SmoothMeshContactInfo& contact2) - { - return contact1.contactInfo.penetrationDepth < contact2.contactInfo.penetrationDepth; - } + bool operator()(const SmoothMeshContactInfo& contact1, const SmoothMeshContactInfo& contact2) + { + return contact1.contactInfo.penetrationDepth < contact2.contactInfo.penetrationDepth; + } }; /// Method used to compare two smooth mesh contact info to sort them //inline static bool contactsDepthCompare(const SmoothMeshContactInfo& contact1, -// const SmoothMeshContactInfo& contact2) { -// return contact1.contactInfo.penetrationDepth < contact2.contactInfo.penetrationDepth; +// const SmoothMeshContactInfo& contact2) { +// return contact1.contactInfo.penetrationDepth < contact2.contactInfo.penetrationDepth; //} // Class SmoothCollisionNarrowPhaseCallback @@ -153,23 +132,23 @@ struct ContactsDepthCompare { */ class SmoothCollisionNarrowPhaseCallback : public NarrowPhaseCallback { - private: + private: - std::vector& mContactPoints; + std::vector& mContactPoints; - public: + public: - // Constructor - SmoothCollisionNarrowPhaseCallback(std::vector& contactPoints) - : mContactPoints(contactPoints) { + // Constructor + SmoothCollisionNarrowPhaseCallback(std::vector& contactPoints) + : mContactPoints(contactPoints) { - } + } - /// Called by a narrow-phase collision algorithm when a new contact has been found - virtual void notifyContact(OverlappingPair* overlappingPair, - const ContactPointInfo& contactInfo); + /// Called by a narrow-phase collision algorithm when a new contact has been found + virtual void notifyContact(OverlappingPair* overlappingPair, + const ContactPointInfo& contactInfo); }; @@ -182,53 +161,51 @@ class SmoothCollisionNarrowPhaseCallback : public NarrowPhaseCallback { */ class ConcaveVsConvexAlgorithm : public NarrowPhaseAlgorithm { - protected : + protected : - // -------------------- Attributes -------------------- // + // -------------------- Attributes -------------------- // - // -------------------- Methods -------------------- // + // -------------------- Methods -------------------- // - /// Private copy-constructor - ConcaveVsConvexAlgorithm(const ConcaveVsConvexAlgorithm& algorithm); + /// Private copy-constructor + ConcaveVsConvexAlgorithm(const ConcaveVsConvexAlgorithm& algorithm); - /// Private assignment operator - ConcaveVsConvexAlgorithm& operator=(const ConcaveVsConvexAlgorithm& algorithm); + /// Private assignment operator + ConcaveVsConvexAlgorithm& operator=(const ConcaveVsConvexAlgorithm& algorithm); - /// Process the concave triangle mesh collision using the smooth mesh collision algorithm - void processSmoothMeshCollision(OverlappingPair* overlappingPair, - std::vector contactPoints, - NarrowPhaseCallback* narrowPhaseCallback); + /// Process the concave triangle mesh collision using the smooth mesh collision algorithm + void processSmoothMeshCollision(OverlappingPair* overlappingPair, + std::vector contactPoints, + NarrowPhaseCallback* narrowPhaseCallback); - /// Add a triangle vertex into the set of processed triangles - void addProcessedVertex(std::unordered_multimap& processTriangleVertices, - const Vector3& vertex); + /// Add a triangle vertex int32_to the set of processed triangles + void addProcessedVertex(std::unordered_multimap& processTriangleVertices, + const Vector3& vertex); - /// Return true if the vertex is in the set of already processed vertices - bool hasVertexBeenProcessed(const std::unordered_multimap& processTriangleVertices, - const Vector3& vertex) const; + /// Return true if the vertex is in the set of already processed vertices + bool hasVertexBeenProcessed(const std::unordered_multimap& processTriangleVertices, + const Vector3& vertex) const; - public : + public : - // -------------------- Methods -------------------- // + // -------------------- Methods -------------------- // - /// Constructor - ConcaveVsConvexAlgorithm(); + /// Constructor + ConcaveVsConvexAlgorithm(); - /// Destructor - virtual ~ConcaveVsConvexAlgorithm(); + /// Destructor + virtual ~ConcaveVsConvexAlgorithm(); - /// Compute a contact info if the two bounding volume collide - virtual void testCollision(const CollisionShapeInfo& shape1Info, - const CollisionShapeInfo& shape2Info, - NarrowPhaseCallback* narrowPhaseCallback); + /// Compute a contact info if the two bounding volume collide + virtual void testCollision(const CollisionShapeInfo& shape1Info, + const CollisionShapeInfo& shape2Info, + NarrowPhaseCallback* narrowPhaseCallback); }; -// Add a triangle vertex into the set of processed triangles -inline void ConcaveVsConvexAlgorithm::addProcessedVertex(std::unordered_multimap& processTriangleVertices, const Vector3& vertex) { - processTriangleVertices.insert(std::make_pair(int(vertex.x * vertex.y * vertex.z), vertex)); +// Add a triangle vertex int32_to the set of processed triangles +inline void ConcaveVsConvexAlgorithm::addProcessedVertex(std::unordered_multimap& processTriangleVertices, const Vector3& vertex) { + processTriangleVertices.insert(std::make_pair(int32_t(vertex.x * vertex.y * vertex.z), vertex)); } } -#endif - diff --git a/ephysics/collision/narrowphase/DefaultCollisionDispatch.cpp b/ephysics/collision/narrowphase/DefaultCollisionDispatch.cpp index 4e361df..4d254a5 100644 --- a/ephysics/collision/narrowphase/DefaultCollisionDispatch.cpp +++ b/ephysics/collision/narrowphase/DefaultCollisionDispatch.cpp @@ -1,27 +1,8 @@ -/******************************************************************************** -* 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. * -* * -********************************************************************************/ +/** @file + * @author Daniel Chappuis + * @copyright 2010-2016 Daniel Chappuis + * @license BSD 3 clauses (see license file) + */ // Libraries #include @@ -41,35 +22,30 @@ DefaultCollisionDispatch::~DefaultCollisionDispatch() { /// Initialize the collision dispatch configuration void DefaultCollisionDispatch::init(CollisionDetection* collisionDetection, - MemoryAllocator* memoryAllocator) { + MemoryAllocator* memoryAllocator) { - // Initialize the collision algorithms - mSphereVsSphereAlgorithm.init(collisionDetection, memoryAllocator); - mGJKAlgorithm.init(collisionDetection, memoryAllocator); - mConcaveVsConvexAlgorithm.init(collisionDetection, memoryAllocator); + // Initialize the collision algorithms + mSphereVsSphereAlgorithm.init(collisionDetection, memoryAllocator); + mGJKAlgorithm.init(collisionDetection, memoryAllocator); + mConcaveVsConvexAlgorithm.init(collisionDetection, memoryAllocator); } // Select and return the narrow-phase collision detection algorithm to // use between two types of collision shapes. -NarrowPhaseAlgorithm* DefaultCollisionDispatch::selectAlgorithm(int type1, int type2) { - - CollisionShapeType shape1Type = static_cast(type1); - CollisionShapeType shape2Type = static_cast(type2); - - // Sphere vs Sphere algorithm - if (shape1Type == SPHERE && shape2Type == SPHERE) { - return &mSphereVsSphereAlgorithm; - } - // Concave vs Convex algorithm - else if ((!CollisionShape::isConvex(shape1Type) && CollisionShape::isConvex(shape2Type)) || - (!CollisionShape::isConvex(shape2Type) && CollisionShape::isConvex(shape1Type))) { - return &mConcaveVsConvexAlgorithm; - } - // Convex vs Convex algorithm (GJK algorithm) - else if (CollisionShape::isConvex(shape1Type) && CollisionShape::isConvex(shape2Type)) { - return &mGJKAlgorithm; - } - else { - return NULL; - } +NarrowPhaseAlgorithm* DefaultCollisionDispatch::selectAlgorithm(int32_t type1, int32_t type2) { + CollisionShapeType shape1Type = static_cast(type1); + CollisionShapeType shape2Type = static_cast(type2); + // Sphere vs Sphere algorithm + if (shape1Type == SPHERE && shape2Type == SPHERE) { + return &mSphereVsSphereAlgorithm; + } else if ( (!CollisionShape::isConvex(shape1Type) && CollisionShape::isConvex(shape2Type)) + || (!CollisionShape::isConvex(shape2Type) && CollisionShape::isConvex(shape1Type))) { + // Concave vs Convex algorithm + return &mConcaveVsConvexAlgorithm; + } else if (CollisionShape::isConvex(shape1Type) && CollisionShape::isConvex(shape2Type)) { + // Convex vs Convex algorithm (GJK algorithm) + return &mGJKAlgorithm; + } else { + return nullptr; + } } diff --git a/ephysics/collision/narrowphase/DefaultCollisionDispatch.h b/ephysics/collision/narrowphase/DefaultCollisionDispatch.h index 4802769..b329137 100644 --- a/ephysics/collision/narrowphase/DefaultCollisionDispatch.h +++ b/ephysics/collision/narrowphase/DefaultCollisionDispatch.h @@ -1,30 +1,9 @@ -/******************************************************************************** -* 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 REACTPHYSICS3D_DEFAULT_COLLISION_DISPATCH_H -#define REACTPHYSICS3D_DEFAULT_COLLISION_DISPATCH_H +/** @file + * @author Daniel Chappuis + * @copyright 2010-2016 Daniel Chappuis + * @license BSD 3 clauses (see license file) + */ +#pragma once // Libraries #include @@ -42,37 +21,34 @@ namespace reactphysics3d { */ class DefaultCollisionDispatch : public CollisionDispatch { - protected: + protected: - /// Sphere vs Sphere collision algorithm - SphereVsSphereAlgorithm mSphereVsSphereAlgorithm; + /// Sphere vs Sphere collision algorithm + SphereVsSphereAlgorithm mSphereVsSphereAlgorithm; - /// Concave vs Convex collision algorithm - ConcaveVsConvexAlgorithm mConcaveVsConvexAlgorithm; + /// Concave vs Convex collision algorithm + ConcaveVsConvexAlgorithm mConcaveVsConvexAlgorithm; - /// GJK Algorithm - GJKAlgorithm mGJKAlgorithm; + /// GJK Algorithm + GJKAlgorithm mGJKAlgorithm; - public: + public: - /// Constructor - DefaultCollisionDispatch(); + /// Constructor + DefaultCollisionDispatch(); - /// Destructor - virtual ~DefaultCollisionDispatch(); + /// Destructor + virtual ~DefaultCollisionDispatch(); - /// Initialize the collision dispatch configuration - virtual void init(CollisionDetection* collisionDetection, - MemoryAllocator* memoryAllocator); + /// Initialize the collision dispatch configuration + virtual void init(CollisionDetection* collisionDetection, + MemoryAllocator* memoryAllocator); - /// Select and return the narrow-phase collision detection algorithm to - /// use between two types of collision shapes. - virtual NarrowPhaseAlgorithm* selectAlgorithm(int type1, int type2); + /// Select and return the narrow-phase collision detection algorithm to + /// use between two types of collision shapes. + virtual NarrowPhaseAlgorithm* selectAlgorithm(int32_t type1, int32_t type2); }; } -#endif - - diff --git a/ephysics/collision/narrowphase/EPA/EPAAlgorithm.cpp b/ephysics/collision/narrowphase/EPA/EPAAlgorithm.cpp index c642739..c3e93f4 100644 --- a/ephysics/collision/narrowphase/EPA/EPAAlgorithm.cpp +++ b/ephysics/collision/narrowphase/EPA/EPAAlgorithm.cpp @@ -1,27 +1,8 @@ -/******************************************************************************** -* 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. * -* * -********************************************************************************/ +/** @file + * @author Daniel Chappuis + * @copyright 2010-2016 Daniel Chappuis + * @license BSD 3 clauses (see license file) + */ // Libraries #include @@ -39,401 +20,401 @@ EPAAlgorithm::EPAAlgorithm() { // Destructor EPAAlgorithm::~EPAAlgorithm() { - + } // Decide if the origin is in the tetrahedron. /// Return 0 if the origin is in the tetrahedron and return the number (1,2,3 or 4) of /// the vertex that is wrong if the origin is not in the tetrahedron -int EPAAlgorithm::isOriginInTetrahedron(const Vector3& p1, const Vector3& p2, - const Vector3& p3, const Vector3& p4) const { +int32_t EPAAlgorithm::isOriginInTetrahedron(const Vector3& p1, const Vector3& p2, + const Vector3& p3, const Vector3& p4) const { - // Check vertex 1 - Vector3 normal1 = (p2-p1).cross(p3-p1); - if ((normal1.dot(p1) > 0.0) == (normal1.dot(p4) > 0.0)) { - return 4; - } + // Check vertex 1 + Vector3 normal1 = (p2-p1).cross(p3-p1); + if ((normal1.dot(p1) > 0.0) == (normal1.dot(p4) > 0.0)) { + return 4; + } - // Check vertex 2 - Vector3 normal2 = (p4-p2).cross(p3-p2); - if ((normal2.dot(p2) > 0.0) == (normal2.dot(p1) > 0.0)) { - return 1; - } + // Check vertex 2 + Vector3 normal2 = (p4-p2).cross(p3-p2); + if ((normal2.dot(p2) > 0.0) == (normal2.dot(p1) > 0.0)) { + return 1; + } - // Check vertex 3 - Vector3 normal3 = (p4-p3).cross(p1-p3); - if ((normal3.dot(p3) > 0.0) == (normal3.dot(p2) > 0.0)) { - return 2; - } + // Check vertex 3 + Vector3 normal3 = (p4-p3).cross(p1-p3); + if ((normal3.dot(p3) > 0.0) == (normal3.dot(p2) > 0.0)) { + return 2; + } - // Check vertex 4 - Vector3 normal4 = (p2-p4).cross(p1-p4); - if ((normal4.dot(p4) > 0.0) == (normal4.dot(p3) > 0.0)) { - return 3; - } + // Check vertex 4 + Vector3 normal4 = (p2-p4).cross(p1-p4); + if ((normal4.dot(p4) > 0.0) == (normal4.dot(p3) > 0.0)) { + return 3; + } - // The origin is in the tetrahedron, we return 0 - return 0; + // The origin is in the tetrahedron, we return 0 + return 0; } // Compute the penetration depth with the EPA algorithm. /// This method computes the penetration depth and contact points between two /// enlarged objects (with margin) where the original objects (without margin) -/// intersect. An initial simplex that contains origin has been computed with +/// int32_tersect. An initial simplex that contains origin has been computed with /// GJK algorithm. The EPA Algorithm will extend this simplex polytope to find /// the correct penetration depth void EPAAlgorithm::computePenetrationDepthAndContactPoints(const Simplex& simplex, - CollisionShapeInfo shape1Info, - const Transform& transform1, - CollisionShapeInfo shape2Info, - const Transform& transform2, - Vector3& v, - NarrowPhaseCallback* narrowPhaseCallback) { + CollisionShapeInfo shape1Info, + const Transform& transform1, + CollisionShapeInfo shape2Info, + const Transform& transform2, + Vector3& v, + NarrowPhaseCallback* narrowPhaseCallback) { - PROFILE("EPAAlgorithm::computePenetrationDepthAndContactPoints()"); + PROFILE("EPAAlgorithm::computePenetrationDepthAndContactPoints()"); - assert(shape1Info.collisionShape->isConvex()); - assert(shape2Info.collisionShape->isConvex()); + assert(shape1Info.collisionShape->isConvex()); + assert(shape2Info.collisionShape->isConvex()); - const ConvexShape* shape1 = static_cast(shape1Info.collisionShape); - const ConvexShape* shape2 = static_cast(shape2Info.collisionShape); + const ConvexShape* shape1 = static_cast(shape1Info.collisionShape); + const ConvexShape* shape2 = static_cast(shape2Info.collisionShape); - void** shape1CachedCollisionData = shape1Info.cachedCollisionData; - void** shape2CachedCollisionData = shape2Info.cachedCollisionData; + void** shape1CachedCollisionData = shape1Info.cachedCollisionData; + void** shape2CachedCollisionData = shape2Info.cachedCollisionData; - Vector3 suppPointsA[MAX_SUPPORT_POINTS]; // Support points of object A in local coordinates - Vector3 suppPointsB[MAX_SUPPORT_POINTS]; // Support points of object B in local coordinates - Vector3 points[MAX_SUPPORT_POINTS]; // Current points - TrianglesStore triangleStore; // Store the triangles - TriangleEPA* triangleHeap[MAX_FACETS]; // Heap that contains the face - // candidate of the EPA algorithm + Vector3 suppPointsA[MAX_SUPPORT_POINTS]; // Support points of object A in local coordinates + Vector3 suppPointsB[MAX_SUPPORT_POINTS]; // Support points of object B in local coordinates + Vector3 points[MAX_SUPPORT_POINTS]; // Current points + TrianglesStore triangleStore; // Store the triangles + TriangleEPA* triangleHeap[MAX_FACETS]; // Heap that contains the face + // candidate of the EPA algorithm - // 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) - Transform body2Tobody1 = transform1.getInverse() * transform2; + // 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) + Transform body2Tobody1 = transform1.getInverse() * transform2; - // Matrix that transform a direction from local - // space of body 1 into local space of body 2 - Quaternion rotateToBody2 = transform2.getOrientation().getInverse() * - transform1.getOrientation(); + // Matrix that transform a direction from local + // space of body 1 int32_to local space of body 2 + Quaternion rotateToBody2 = transform2.getOrientation().getInverse() * + transform1.getOrientation(); - // Get the simplex computed previously by the GJK algorithm - unsigned int nbVertices = simplex.getSimplex(suppPointsA, suppPointsB, points); + // Get the simplex computed previously by the GJK algorithm + uint32_t nbVertices = simplex.getSimplex(suppPointsA, suppPointsB, points); - // Compute the tolerance - decimal tolerance = MACHINE_EPSILON * simplex.getMaxLengthSquareOfAPoint(); + // Compute the tolerance + float tolerance = MACHINE_EPSILON * simplex.getMaxLengthSquareOfAPoint(); - // Number of triangles in the polytope - unsigned int nbTriangles = 0; + // Number of triangles in the polytope + uint32_t nbTriangles = 0; - // Clear the storing of triangles - triangleStore.clear(); + // Clear the storing of triangles + triangleStore.clear(); - // Select an action according to the number of points in the simplex - // computed with GJK algorithm in order to obtain an initial polytope for - // The EPA algorithm. - switch(nbVertices) { - case 1: - // Only one point in the simplex (which should be the origin). - // We have a touching contact with zero penetration depth. - // We drop that kind of contact. Therefore, we return false - return; + // Select an action according to the number of points in the simplex + // computed with GJK algorithm in order to obtain an initial polytope for + // The EPA algorithm. + switch(nbVertices) { + case 1: + // Only one point in the simplex (which should be the origin). + // We have a touching contact with zero penetration depth. + // We drop that kind of contact. Therefore, we return false + return; - case 2: { - // The simplex returned by GJK is a line segment d containing the origin. - // We add two additional support points to construct a hexahedron (two tetrahedron - // glued together with triangle faces. The idea is to compute three different vectors - // v1, v2 and v3 that are orthogonal to the segment d. The three vectors are relatively - // rotated of 120 degree around the d segment. The the three new points to - // construct the polytope are the three support points in those three directions - // v1, v2 and v3. + case 2: { + // The simplex returned by GJK is a line segment d containing the origin. + // We add two additional support points to construct a hexahedron (two tetrahedron + // glued together with triangle faces. The idea is to compute three different vectors + // v1, v2 and v3 that are orthogonal to the segment d. The three vectors are relatively + // rotated of 120 degree around the d segment. The the three new points to + // construct the polytope are the three support points in those three directions + // v1, v2 and v3. - // Direction of the segment - Vector3 d = (points[1] - points[0]).getUnit(); + // Direction of the segment + Vector3 d = (points[1] - points[0]).getUnit(); - // Choose the coordinate axis from the minimal absolute component of the vector d - int minAxis = d.getAbsoluteVector().getMinAxis(); + // Choose the coordinate axis from the minimal absolute component of the vector d + int32_t minAxis = d.getAbsoluteVector().getMinAxis(); - // Compute sin(60) - const decimal sin60 = decimal(sqrt(3.0)) * decimal(0.5); + // Compute sin(60) + const float sin60 = float(sqrt(3.0)) * float(0.5); - // Create a rotation quaternion to rotate the vector v1 to get the vectors - // v2 and v3 - Quaternion rotationQuat(d.x * sin60, d.y * sin60, d.z * sin60, 0.5); + // Create a rotation quaternion to rotate the vector v1 to get the vectors + // v2 and v3 + Quaternion rotationQuat(d.x * sin60, d.y * sin60, d.z * sin60, 0.5); - // Compute the vector v1, v2, v3 - Vector3 v1 = d.cross(Vector3(minAxis == 0, minAxis == 1, minAxis == 2)); - Vector3 v2 = rotationQuat * v1; - Vector3 v3 = rotationQuat * v2; + // Compute the vector v1, v2, v3 + Vector3 v1 = d.cross(Vector3(minAxis == 0, minAxis == 1, minAxis == 2)); + Vector3 v2 = rotationQuat * v1; + Vector3 v3 = rotationQuat * v2; - // Compute the support point in the direction of v1 - suppPointsA[2] = shape1->getLocalSupportPointWithMargin(v1, shape1CachedCollisionData); - suppPointsB[2] = body2Tobody1 * - shape2->getLocalSupportPointWithMargin(rotateToBody2 * (-v1), shape2CachedCollisionData); - points[2] = suppPointsA[2] - suppPointsB[2]; + // Compute the support point in the direction of v1 + suppPointsA[2] = shape1->getLocalSupportPointWithMargin(v1, shape1CachedCollisionData); + suppPointsB[2] = body2Tobody1 * + shape2->getLocalSupportPointWithMargin(rotateToBody2 * (-v1), shape2CachedCollisionData); + points[2] = suppPointsA[2] - suppPointsB[2]; - // Compute the support point in the direction of v2 - suppPointsA[3] = shape1->getLocalSupportPointWithMargin(v2, shape1CachedCollisionData); - suppPointsB[3] = body2Tobody1 * - shape2->getLocalSupportPointWithMargin(rotateToBody2 * (-v2), shape2CachedCollisionData); - points[3] = suppPointsA[3] - suppPointsB[3]; + // Compute the support point in the direction of v2 + suppPointsA[3] = shape1->getLocalSupportPointWithMargin(v2, shape1CachedCollisionData); + suppPointsB[3] = body2Tobody1 * + shape2->getLocalSupportPointWithMargin(rotateToBody2 * (-v2), shape2CachedCollisionData); + points[3] = suppPointsA[3] - suppPointsB[3]; - // Compute the support point in the direction of v3 - suppPointsA[4] = shape1->getLocalSupportPointWithMargin(v3, shape1CachedCollisionData); - suppPointsB[4] = body2Tobody1 * - shape2->getLocalSupportPointWithMargin(rotateToBody2 * (-v3), shape2CachedCollisionData); - points[4] = suppPointsA[4] - suppPointsB[4]; + // Compute the support point in the direction of v3 + suppPointsA[4] = shape1->getLocalSupportPointWithMargin(v3, shape1CachedCollisionData); + suppPointsB[4] = body2Tobody1 * + shape2->getLocalSupportPointWithMargin(rotateToBody2 * (-v3), shape2CachedCollisionData); + points[4] = suppPointsA[4] - suppPointsB[4]; - // Now we have an hexahedron (two tetrahedron glued together). We can simply keep the - // tetrahedron that contains the origin in order that the initial polytope of the - // EPA algorithm is a tetrahedron, which is simpler to deal with. + // Now we have an hexahedron (two tetrahedron glued together). We can simply keep the + // tetrahedron that contains the origin in order that the initial polytope of the + // EPA algorithm is a tetrahedron, which is simpler to deal with. - // If the origin is in the tetrahedron of points 0, 2, 3, 4 - if (isOriginInTetrahedron(points[0], points[2], points[3], points[4]) == 0) { - // We use the point 4 instead of point 1 for the initial tetrahedron - suppPointsA[1] = suppPointsA[4]; - suppPointsB[1] = suppPointsB[4]; - points[1] = points[4]; - } - // If the origin is in the tetrahedron of points 1, 2, 3, 4 - else if (isOriginInTetrahedron(points[1], points[2], points[3], points[4]) == 0) { - // We use the point 4 instead of point 0 for the initial tetrahedron - suppPointsA[0] = suppPointsA[4]; - suppPointsB[0] = suppPointsB[4]; - points[0] = points[4]; - } - else { - // The origin is not in the initial polytope - return; - } + // If the origin is in the tetrahedron of points 0, 2, 3, 4 + if (isOriginInTetrahedron(points[0], points[2], points[3], points[4]) == 0) { + // We use the point 4 instead of point 1 for the initial tetrahedron + suppPointsA[1] = suppPointsA[4]; + suppPointsB[1] = suppPointsB[4]; + points[1] = points[4]; + } + // If the origin is in the tetrahedron of points 1, 2, 3, 4 + else if (isOriginInTetrahedron(points[1], points[2], points[3], points[4]) == 0) { + // We use the point 4 instead of point 0 for the initial tetrahedron + suppPointsA[0] = suppPointsA[4]; + suppPointsB[0] = suppPointsB[4]; + points[0] = points[4]; + } + else { + // The origin is not in the initial polytope + return; + } - // The polytope contains now 4 vertices - nbVertices = 4; - } - case 4: { - // The simplex computed by the GJK algorithm is a tetrahedron. Here we check - // if this tetrahedron contains the origin. If it is the case, we keep it and - // otherwise we remove the wrong vertex of the tetrahedron and go in the case - // where the GJK algorithm compute a simplex of three vertices. + // The polytope contains now 4 vertices + nbVertices = 4; + } + case 4: { + // The simplex computed by the GJK algorithm is a tetrahedron. Here we check + // if this tetrahedron contains the origin. If it is the case, we keep it and + // otherwise we remove the wrong vertex of the tetrahedron and go in the case + // where the GJK algorithm compute a simplex of three vertices. - // Check if the tetrahedron contains the origin (or wich is the wrong vertex otherwise) - int badVertex = isOriginInTetrahedron(points[0], points[1], points[2], points[3]); + // Check if the tetrahedron contains the origin (or wich is the wrong vertex otherwise) + int32_t badVertex = isOriginInTetrahedron(points[0], points[1], points[2], points[3]); - // If the origin is in the tetrahedron - if (badVertex == 0) { - // The tetrahedron is a correct initial polytope for the EPA algorithm. - // Therefore, we construct the tetrahedron. + // If the origin is in the tetrahedron + if (badVertex == 0) { + // The tetrahedron is a correct initial polytope for the EPA algorithm. + // Therefore, we construct the tetrahedron. - // Comstruct the 4 triangle faces of the tetrahedron - TriangleEPA* face0 = triangleStore.newTriangle(points, 0, 1, 2); - TriangleEPA* face1 = triangleStore.newTriangle(points, 0, 3, 1); - TriangleEPA* face2 = triangleStore.newTriangle(points, 0, 2, 3); - TriangleEPA* face3 = triangleStore.newTriangle(points, 1, 3, 2); + // Comstruct the 4 triangle faces of the tetrahedron + TriangleEPA* face0 = triangleStore.newTriangle(points, 0, 1, 2); + TriangleEPA* face1 = triangleStore.newTriangle(points, 0, 3, 1); + TriangleEPA* face2 = triangleStore.newTriangle(points, 0, 2, 3); + TriangleEPA* face3 = triangleStore.newTriangle(points, 1, 3, 2); - // If the constructed tetrahedron is not correct - if (!((face0 != NULL) && (face1 != NULL) && (face2 != NULL) && (face3 != NULL) - && face0->getDistSquare() > 0.0 && face1->getDistSquare() > 0.0 - && face2->getDistSquare() > 0.0 && face3->getDistSquare() > 0.0)) { - return; - } + // If the constructed tetrahedron is not correct + if (!((face0 != NULL) && (face1 != NULL) && (face2 != NULL) && (face3 != NULL) + && face0->getDistSquare() > 0.0 && face1->getDistSquare() > 0.0 + && face2->getDistSquare() > 0.0 && face3->getDistSquare() > 0.0)) { + return; + } - // Associate the edges of neighbouring triangle faces - link(EdgeEPA(face0, 0), EdgeEPA(face1, 2)); - link(EdgeEPA(face0, 1), EdgeEPA(face3, 2)); - link(EdgeEPA(face0, 2), EdgeEPA(face2, 0)); - link(EdgeEPA(face1, 0), EdgeEPA(face2, 2)); - link(EdgeEPA(face1, 1), EdgeEPA(face3, 0)); - link(EdgeEPA(face2, 1), EdgeEPA(face3, 1)); + // Associate the edges of neighbouring triangle faces + link(EdgeEPA(face0, 0), EdgeEPA(face1, 2)); + link(EdgeEPA(face0, 1), EdgeEPA(face3, 2)); + link(EdgeEPA(face0, 2), EdgeEPA(face2, 0)); + link(EdgeEPA(face1, 0), EdgeEPA(face2, 2)); + link(EdgeEPA(face1, 1), EdgeEPA(face3, 0)); + link(EdgeEPA(face2, 1), EdgeEPA(face3, 1)); - // Add the triangle faces in the candidate heap - addFaceCandidate(face0, triangleHeap, nbTriangles, DECIMAL_LARGEST); - addFaceCandidate(face1, triangleHeap, nbTriangles, DECIMAL_LARGEST); - addFaceCandidate(face2, triangleHeap, nbTriangles, DECIMAL_LARGEST); - addFaceCandidate(face3, triangleHeap, nbTriangles, DECIMAL_LARGEST); + // Add the triangle faces in the candidate heap + addFaceCandidate(face0, triangleHeap, nbTriangles, DECIMAL_LARGEST); + addFaceCandidate(face1, triangleHeap, nbTriangles, DECIMAL_LARGEST); + addFaceCandidate(face2, triangleHeap, nbTriangles, DECIMAL_LARGEST); + addFaceCandidate(face3, triangleHeap, nbTriangles, DECIMAL_LARGEST); - break; - } + break; + } - // The tetrahedron contains a wrong vertex (the origin is not inside the tetrahedron) - // Remove the wrong vertex and continue to the next case with the - // three remaining vertices - if (badVertex < 4) { + // The tetrahedron contains a wrong vertex (the origin is not inside the tetrahedron) + // Remove the wrong vertex and continue to the next case with the + // three remaining vertices + if (badVertex < 4) { - suppPointsA[badVertex-1] = suppPointsA[3]; - suppPointsB[badVertex-1] = suppPointsB[3]; - points[badVertex-1] = points[3]; - } + suppPointsA[badVertex-1] = suppPointsA[3]; + suppPointsB[badVertex-1] = suppPointsB[3]; + points[badVertex-1] = points[3]; + } - // We have removed the wrong vertex - nbVertices = 3; - } - case 3: { - // The GJK algorithm returned a triangle that contains the origin. - // We need two new vertices to create two tetrahedron. The two new - // vertices are the support points in the "n" and "-n" direction - // where "n" is the normal of the triangle. Then, we use only the - // tetrahedron that contains the origin. + // We have removed the wrong vertex + nbVertices = 3; + } + case 3: { + // The GJK algorithm returned a triangle that contains the origin. + // We need two new vertices to create two tetrahedron. The two new + // vertices are the support points in the "n" and "-n" direction + // where "n" is the normal of the triangle. Then, we use only the + // tetrahedron that contains the origin. - // Compute the normal of the triangle - Vector3 v1 = points[1] - points[0]; - Vector3 v2 = points[2] - points[0]; - Vector3 n = v1.cross(v2); + // Compute the normal of the triangle + Vector3 v1 = points[1] - points[0]; + Vector3 v2 = points[2] - points[0]; + Vector3 n = v1.cross(v2); - // Compute the two new vertices to obtain a hexahedron - suppPointsA[3] = shape1->getLocalSupportPointWithMargin(n, shape1CachedCollisionData); - suppPointsB[3] = body2Tobody1 * - shape2->getLocalSupportPointWithMargin(rotateToBody2 * (-n), shape2CachedCollisionData); - points[3] = suppPointsA[3] - suppPointsB[3]; - suppPointsA[4] = shape1->getLocalSupportPointWithMargin(-n, shape1CachedCollisionData); - suppPointsB[4] = body2Tobody1 * - shape2->getLocalSupportPointWithMargin(rotateToBody2 * n, shape2CachedCollisionData); - points[4] = suppPointsA[4] - suppPointsB[4]; + // Compute the two new vertices to obtain a hexahedron + suppPointsA[3] = shape1->getLocalSupportPointWithMargin(n, shape1CachedCollisionData); + suppPointsB[3] = body2Tobody1 * + shape2->getLocalSupportPointWithMargin(rotateToBody2 * (-n), shape2CachedCollisionData); + points[3] = suppPointsA[3] - suppPointsB[3]; + suppPointsA[4] = shape1->getLocalSupportPointWithMargin(-n, shape1CachedCollisionData); + suppPointsB[4] = body2Tobody1 * + shape2->getLocalSupportPointWithMargin(rotateToBody2 * n, shape2CachedCollisionData); + points[4] = suppPointsA[4] - suppPointsB[4]; - TriangleEPA* face0 = NULL; - TriangleEPA* face1 = NULL; - TriangleEPA* face2 = NULL; - TriangleEPA* face3 = NULL; + TriangleEPA* face0 = NULL; + TriangleEPA* face1 = NULL; + TriangleEPA* face2 = NULL; + TriangleEPA* face3 = NULL; - // If the origin is in the first tetrahedron - if (isOriginInTetrahedron(points[0], points[1], - points[2], points[3]) == 0) { - // The tetrahedron is a correct initial polytope for the EPA algorithm. - // Therefore, we construct the tetrahedron. + // If the origin is in the first tetrahedron + if (isOriginInTetrahedron(points[0], points[1], + points[2], points[3]) == 0) { + // The tetrahedron is a correct initial polytope for the EPA algorithm. + // Therefore, we construct the tetrahedron. - // Comstruct the 4 triangle faces of the tetrahedron - face0 = triangleStore.newTriangle(points, 0, 1, 2); - face1 = triangleStore.newTriangle(points, 0, 3, 1); - face2 = triangleStore.newTriangle(points, 0, 2, 3); - face3 = triangleStore.newTriangle(points, 1, 3, 2); - } - else if (isOriginInTetrahedron(points[0], points[1], - points[2], points[4]) == 0) { + // Comstruct the 4 triangle faces of the tetrahedron + face0 = triangleStore.newTriangle(points, 0, 1, 2); + face1 = triangleStore.newTriangle(points, 0, 3, 1); + face2 = triangleStore.newTriangle(points, 0, 2, 3); + face3 = triangleStore.newTriangle(points, 1, 3, 2); + } + else if (isOriginInTetrahedron(points[0], points[1], + points[2], points[4]) == 0) { - // The tetrahedron is a correct initial polytope for the EPA algorithm. - // Therefore, we construct the tetrahedron. + // The tetrahedron is a correct initial polytope for the EPA algorithm. + // Therefore, we construct the tetrahedron. - // Comstruct the 4 triangle faces of the tetrahedron - face0 = triangleStore.newTriangle(points, 0, 1, 2); - face1 = triangleStore.newTriangle(points, 0, 4, 1); - face2 = triangleStore.newTriangle(points, 0, 2, 4); - face3 = triangleStore.newTriangle(points, 1, 4, 2); - } - else { - return; - } + // Comstruct the 4 triangle faces of the tetrahedron + face0 = triangleStore.newTriangle(points, 0, 1, 2); + face1 = triangleStore.newTriangle(points, 0, 4, 1); + face2 = triangleStore.newTriangle(points, 0, 2, 4); + face3 = triangleStore.newTriangle(points, 1, 4, 2); + } + else { + return; + } - // If the constructed tetrahedron is not correct - if (!((face0 != NULL) && (face1 != NULL) && (face2 != NULL) && (face3 != NULL) - && face0->getDistSquare() > 0.0 && face1->getDistSquare() > 0.0 - && face2->getDistSquare() > 0.0 && face3->getDistSquare() > 0.0)) { - return; - } + // If the constructed tetrahedron is not correct + if (!((face0 != NULL) && (face1 != NULL) && (face2 != NULL) && (face3 != NULL) + && face0->getDistSquare() > 0.0 && face1->getDistSquare() > 0.0 + && face2->getDistSquare() > 0.0 && face3->getDistSquare() > 0.0)) { + return; + } - // Associate the edges of neighbouring triangle faces - link(EdgeEPA(face0, 0), EdgeEPA(face1, 2)); - link(EdgeEPA(face0, 1), EdgeEPA(face3, 2)); - link(EdgeEPA(face0, 2), EdgeEPA(face2, 0)); - link(EdgeEPA(face1, 0), EdgeEPA(face2, 2)); - link(EdgeEPA(face1, 1), EdgeEPA(face3, 0)); - link(EdgeEPA(face2, 1), EdgeEPA(face3, 1)); + // Associate the edges of neighbouring triangle faces + link(EdgeEPA(face0, 0), EdgeEPA(face1, 2)); + link(EdgeEPA(face0, 1), EdgeEPA(face3, 2)); + link(EdgeEPA(face0, 2), EdgeEPA(face2, 0)); + link(EdgeEPA(face1, 0), EdgeEPA(face2, 2)); + link(EdgeEPA(face1, 1), EdgeEPA(face3, 0)); + link(EdgeEPA(face2, 1), EdgeEPA(face3, 1)); - // Add the triangle faces in the candidate heap - addFaceCandidate(face0, triangleHeap, nbTriangles, DECIMAL_LARGEST); - addFaceCandidate(face1, triangleHeap, nbTriangles, DECIMAL_LARGEST); - addFaceCandidate(face2, triangleHeap, nbTriangles, DECIMAL_LARGEST); - addFaceCandidate(face3, triangleHeap, nbTriangles, DECIMAL_LARGEST); + // Add the triangle faces in the candidate heap + addFaceCandidate(face0, triangleHeap, nbTriangles, DECIMAL_LARGEST); + addFaceCandidate(face1, triangleHeap, nbTriangles, DECIMAL_LARGEST); + addFaceCandidate(face2, triangleHeap, nbTriangles, DECIMAL_LARGEST); + addFaceCandidate(face3, triangleHeap, nbTriangles, DECIMAL_LARGEST); - nbVertices = 4; + nbVertices = 4; - } - break; - } + } + break; + } - // At this point, we have a polytope that contains the origin. Therefore, we - // can run the EPA algorithm. + // At this point, we have a polytope that contains the origin. Therefore, we + // can run the EPA algorithm. - if (nbTriangles == 0) { - return; - } + if (nbTriangles == 0) { + return; + } - TriangleEPA* triangle = 0; - decimal upperBoundSquarePenDepth = DECIMAL_LARGEST; + TriangleEPA* triangle = 0; + float upperBoundSquarePenDepth = DECIMAL_LARGEST; - do { - triangle = triangleHeap[0]; + do { + triangle = triangleHeap[0]; - // Get the next candidate face (the face closest to the origin) - std::pop_heap(&triangleHeap[0], &triangleHeap[nbTriangles], mTriangleComparison); - nbTriangles--; + // Get the next candidate face (the face closest to the origin) + std::pop_heap(&triangleHeap[0], &triangleHeap[nbTriangles], mTriangleComparison); + nbTriangles--; - // If the candidate face in the heap is not obsolete - if (!triangle->getIsObsolete()) { - // If we have reached the maximum number of support points - if (nbVertices == MAX_SUPPORT_POINTS) { - assert(false); - break; - } + // If the candidate face in the heap is not obsolete + if (!triangle->getIsObsolete()) { + // If we have reached the maximum number of support points + if (nbVertices == MAX_SUPPORT_POINTS) { + assert(false); + break; + } - // Compute the support point of the Minkowski - // difference (A-B) in the closest point direction - suppPointsA[nbVertices] = shape1->getLocalSupportPointWithMargin( - triangle->getClosestPoint(), shape1CachedCollisionData); - suppPointsB[nbVertices] = body2Tobody1 * - shape2->getLocalSupportPointWithMargin(rotateToBody2 * - (-triangle->getClosestPoint()), shape2CachedCollisionData); - points[nbVertices] = suppPointsA[nbVertices] - suppPointsB[nbVertices]; + // Compute the support point of the Minkowski + // difference (A-B) in the closest point direction + suppPointsA[nbVertices] = shape1->getLocalSupportPointWithMargin( + triangle->getClosestPoint(), shape1CachedCollisionData); + suppPointsB[nbVertices] = body2Tobody1 * + shape2->getLocalSupportPointWithMargin(rotateToBody2 * + (-triangle->getClosestPoint()), shape2CachedCollisionData); + points[nbVertices] = suppPointsA[nbVertices] - suppPointsB[nbVertices]; - int indexNewVertex = nbVertices; - nbVertices++; + int32_t indexNewVertex = nbVertices; + nbVertices++; - // Update the upper bound of the penetration depth - decimal wDotv = points[indexNewVertex].dot(triangle->getClosestPoint()); - assert(wDotv > 0.0); - decimal wDotVSquare = wDotv * wDotv / triangle->getDistSquare(); - if (wDotVSquare < upperBoundSquarePenDepth) { - upperBoundSquarePenDepth = wDotVSquare; - } + // Update the upper bound of the penetration depth + float wDotv = points[indexNewVertex].dot(triangle->getClosestPoint()); + assert(wDotv > 0.0); + float wDotVSquare = wDotv * wDotv / triangle->getDistSquare(); + if (wDotVSquare < upperBoundSquarePenDepth) { + upperBoundSquarePenDepth = wDotVSquare; + } - // Compute the error - decimal error = wDotv - triangle->getDistSquare(); - if (error <= std::max(tolerance, REL_ERROR_SQUARE * wDotv) || - points[indexNewVertex] == points[(*triangle)[0]] || - points[indexNewVertex] == points[(*triangle)[1]] || - points[indexNewVertex] == points[(*triangle)[2]]) { - break; - } + // Compute the error + float error = wDotv - triangle->getDistSquare(); + if (error <= std::max(tolerance, REL_ERROR_SQUARE * wDotv) || + points[indexNewVertex] == points[(*triangle)[0]] || + points[indexNewVertex] == points[(*triangle)[1]] || + points[indexNewVertex] == points[(*triangle)[2]]) { + break; + } - // Now, we compute the silhouette cast by the new vertex. The current triangle - // face will not be in the convex hull. We start the local recursive silhouette - // algorithm from the current triangle face. - int i = triangleStore.getNbTriangles(); - if (!triangle->computeSilhouette(points, indexNewVertex, triangleStore)) { - break; - } + // Now, we compute the silhouette cast by the new vertex. The current triangle + // face will not be in the convex hull. We start the local recursive silhouette + // algorithm from the current triangle face. + int32_t i = triangleStore.getNbTriangles(); + if (!triangle->computeSilhouette(points, indexNewVertex, triangleStore)) { + break; + } - // Add all the new triangle faces computed with the silhouette algorithm - // to the candidates list of faces of the current polytope - while(i != triangleStore.getNbTriangles()) { - TriangleEPA* newTriangle = &triangleStore[i]; - addFaceCandidate(newTriangle, triangleHeap, nbTriangles, upperBoundSquarePenDepth); - i++; - } - } - } while(nbTriangles > 0 && triangleHeap[0]->getDistSquare() <= upperBoundSquarePenDepth); + // Add all the new triangle faces computed with the silhouette algorithm + // to the candidates list of faces of the current polytope + while(i != triangleStore.getNbTriangles()) { + TriangleEPA* newTriangle = &triangleStore[i]; + addFaceCandidate(newTriangle, triangleHeap, nbTriangles, upperBoundSquarePenDepth); + i++; + } + } + } while(nbTriangles > 0 && triangleHeap[0]->getDistSquare() <= upperBoundSquarePenDepth); - // Compute the contact info - v = transform1.getOrientation() * triangle->getClosestPoint(); - Vector3 pALocal = triangle->computeClosestPointOfObject(suppPointsA); - Vector3 pBLocal = body2Tobody1.getInverse() * triangle->computeClosestPointOfObject(suppPointsB); - Vector3 normal = v.getUnit(); - decimal penetrationDepth = v.length(); - assert(penetrationDepth > 0.0); + // Compute the contact info + v = transform1.getOrientation() * triangle->getClosestPoint(); + Vector3 pALocal = triangle->computeClosestPointOfObject(suppPointsA); + Vector3 pBLocal = body2Tobody1.getInverse() * triangle->computeClosestPointOfObject(suppPointsB); + Vector3 normal = v.getUnit(); + float penetrationDepth = v.length(); + assert(penetrationDepth > 0.0); - if (normal.lengthSquare() < MACHINE_EPSILON) return; - - // Create the contact info object - ContactPointInfo contactInfo(shape1Info.proxyShape, shape2Info.proxyShape, shape1Info.collisionShape, - shape2Info.collisionShape, normal, penetrationDepth, pALocal, pBLocal); + if (normal.lengthSquare() < MACHINE_EPSILON) return; + + // Create the contact info object + ContactPointInfo contactInfo(shape1Info.proxyShape, shape2Info.proxyShape, shape1Info.collisionShape, + shape2Info.collisionShape, normal, penetrationDepth, pALocal, pBLocal); - narrowPhaseCallback->notifyContact(shape1Info.overlappingPair, contactInfo); + narrowPhaseCallback->notifyContact(shape1Info.overlappingPair, contactInfo); } diff --git a/ephysics/collision/narrowphase/EPA/EPAAlgorithm.h b/ephysics/collision/narrowphase/EPA/EPAAlgorithm.h index e0b6427..aa71a19 100644 --- a/ephysics/collision/narrowphase/EPA/EPAAlgorithm.h +++ b/ephysics/collision/narrowphase/EPA/EPAAlgorithm.h @@ -1,30 +1,9 @@ -/******************************************************************************** -* 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 REACTPHYSICS3D_EPA_ALGORITHM_H -#define REACTPHYSICS3D_EPA_ALGORITHM_H +/** @file + * @author Daniel Chappuis + * @copyright 2010-2016 Daniel Chappuis + * @license BSD 3 clauses (see license file) + */ +#pragma once // Libraries #include @@ -43,10 +22,10 @@ namespace reactphysics3d { // ---------- Constants ---------- // /// Maximum number of support points of the polytope -const unsigned int MAX_SUPPORT_POINTS = 100; +const uint32_t MAX_SUPPORT_POINTS = 100; /// Maximum number of facets of the polytope -const unsigned int MAX_FACETS = 200; +const uint32_t MAX_FACETS = 200; // Class TriangleComparison @@ -58,12 +37,12 @@ const unsigned int MAX_FACETS = 200; */ class TriangleComparison { - public: + public: - /// Comparison operator - bool operator()(const TriangleEPA* face1, const TriangleEPA* face2) { - return (face1->getDistSquare() > face2->getDistSquare()); - } + /// Comparison operator + bool operator()(const TriangleEPA* face1, const TriangleEPA* face2) { + return (face1->getDistSquare() > face2->getDistSquare()); + } }; @@ -72,7 +51,7 @@ class TriangleComparison { * This class is the implementation of the Expanding Polytope Algorithm (EPA). * The EPA algorithm computes the penetration depth and contact points between * two enlarged objects (with margin) where the original objects (without margin) - * intersect. The penetration depth of a pair of intersecting objects A and B is + * int32_tersect. The penetration depth of a pair of int32_tersecting objects A and B is * the length of a point on the boundary of the Minkowski sum (A-B) closest to the * origin. The goal of the EPA algorithm is to start with an initial simplex polytope * that contains the origin and expend it in order to find the point on the boundary @@ -83,79 +62,77 @@ class TriangleComparison { */ class EPAAlgorithm { - private: + private: - // -------------------- Attributes -------------------- // + // -------------------- Attributes -------------------- // - /// Reference to the memory allocator - MemoryAllocator* mMemoryAllocator; + /// Reference to the memory allocator + MemoryAllocator* mMemoryAllocator; - /// Triangle comparison operator - TriangleComparison mTriangleComparison; - - // -------------------- Methods -------------------- // + /// Triangle comparison operator + TriangleComparison mTriangleComparison; + + // -------------------- Methods -------------------- // - /// Private copy-constructor - EPAAlgorithm(const EPAAlgorithm& algorithm); + /// Private copy-constructor + EPAAlgorithm(const EPAAlgorithm& algorithm); - /// Private assignment operator - EPAAlgorithm& operator=(const EPAAlgorithm& algorithm); + /// Private assignment operator + EPAAlgorithm& operator=(const EPAAlgorithm& algorithm); - /// Add a triangle face in the candidate triangle heap - void addFaceCandidate(TriangleEPA* triangle, TriangleEPA** heap, uint& nbTriangles, - decimal upperBoundSquarePenDepth); + /// Add a triangle face in the candidate triangle heap + void addFaceCandidate(TriangleEPA* triangle, TriangleEPA** heap, uint32_t& nbTriangles, + float upperBoundSquarePenDepth); - /// Decide if the origin is in the tetrahedron. - int isOriginInTetrahedron(const Vector3& p1, const Vector3& p2, - const Vector3& p3, const Vector3& p4) const; + /// Decide if the origin is in the tetrahedron. + int32_t isOriginInTetrahedron(const Vector3& p1, const Vector3& p2, + const Vector3& p3, const Vector3& p4) const; - public: + public: - // -------------------- Methods -------------------- // + // -------------------- Methods -------------------- // - /// Constructor - EPAAlgorithm(); + /// Constructor + EPAAlgorithm(); - /// Destructor - ~EPAAlgorithm(); + /// Destructor + ~EPAAlgorithm(); - /// Initalize the algorithm - void init(MemoryAllocator* memoryAllocator); + /// Initalize the algorithm + void init(MemoryAllocator* memoryAllocator); - /// Compute the penetration depth with EPA algorithm. - void computePenetrationDepthAndContactPoints(const Simplex& simplex, - CollisionShapeInfo shape1Info, - const Transform& transform1, - CollisionShapeInfo shape2Info, - const Transform& transform2, - Vector3& v, - NarrowPhaseCallback* narrowPhaseCallback); + /// Compute the penetration depth with EPA algorithm. + void computePenetrationDepthAndContactPoints(const Simplex& simplex, + CollisionShapeInfo shape1Info, + const Transform& transform1, + CollisionShapeInfo shape2Info, + const Transform& transform2, + Vector3& v, + NarrowPhaseCallback* narrowPhaseCallback); }; // Add a triangle face in the candidate triangle heap in the EPA algorithm inline void EPAAlgorithm::addFaceCandidate(TriangleEPA* triangle, TriangleEPA** heap, - uint& nbTriangles, decimal upperBoundSquarePenDepth) { - - // If the closest point of the affine hull of triangle - // points is internal to the triangle and if the distance - // of the closest point from the origin is at most the - // penetration depth upper bound - if (triangle->isClosestPointInternalToTriangle() && - triangle->getDistSquare() <= upperBoundSquarePenDepth) { + uint32_t& nbTriangles, float upperBoundSquarePenDepth) { + + // If the closest point of the affine hull of triangle + // points is int32_ternal to the triangle and if the distance + // of the closest point from the origin is at most the + // penetration depth upper bound + if (triangle->isClosestPointInternalToTriangle() && + triangle->getDistSquare() <= upperBoundSquarePenDepth) { - // Add the triangle face to the list of candidates - heap[nbTriangles] = triangle; - nbTriangles++; - std::push_heap(&heap[0], &heap[nbTriangles], mTriangleComparison); - } + // Add the triangle face to the list of candidates + heap[nbTriangles] = triangle; + nbTriangles++; + std::push_heap(&heap[0], &heap[nbTriangles], mTriangleComparison); + } } // Initalize the algorithm inline void EPAAlgorithm::init(MemoryAllocator* memoryAllocator) { - mMemoryAllocator = memoryAllocator; + mMemoryAllocator = memoryAllocator; } } -#endif - diff --git a/ephysics/collision/narrowphase/EPA/EdgeEPA.cpp b/ephysics/collision/narrowphase/EPA/EdgeEPA.cpp index 7a05307..1dd2fdc 100644 --- a/ephysics/collision/narrowphase/EPA/EdgeEPA.cpp +++ b/ephysics/collision/narrowphase/EPA/EdgeEPA.cpp @@ -1,27 +1,8 @@ -/******************************************************************************** -* 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. * -* * -********************************************************************************/ +/** @file + * @author Daniel Chappuis + * @copyright 2010-2016 Daniel Chappuis + * @license BSD 3 clauses (see license file) + */ // Libraries #include @@ -35,19 +16,19 @@ using namespace reactphysics3d; // Constructor EdgeEPA::EdgeEPA() { - + } // Constructor -EdgeEPA::EdgeEPA(TriangleEPA* ownerTriangle, int index) - : mOwnerTriangle(ownerTriangle), mIndex(index) { - assert(index >= 0 && index < 3); +EdgeEPA::EdgeEPA(TriangleEPA* ownerTriangle, int32_t index) + : mOwnerTriangle(ownerTriangle), mIndex(index) { + assert(index >= 0 && index < 3); } // Copy-constructor EdgeEPA::EdgeEPA(const EdgeEPA& edge) { - mOwnerTriangle = edge.mOwnerTriangle; - mIndex = edge.mIndex; + mOwnerTriangle = edge.mOwnerTriangle; + mIndex = edge.mIndex; } // Destructor @@ -56,81 +37,81 @@ EdgeEPA::~EdgeEPA() { } // Return the index of the source vertex of the edge (vertex starting the edge) -uint EdgeEPA::getSourceVertexIndex() const { - return (*mOwnerTriangle)[mIndex]; +uint32_t EdgeEPA::getSourceVertexIndex() const { + return (*mOwnerTriangle)[mIndex]; } // Return the index of the target vertex of the edge (vertex ending the edge) -uint EdgeEPA::getTargetVertexIndex() const { - return (*mOwnerTriangle)[indexOfNextCounterClockwiseEdge(mIndex)]; +uint32_t EdgeEPA::getTargetVertexIndex() const { + return (*mOwnerTriangle)[indexOfNextCounterClockwiseEdge(mIndex)]; } // Execute the recursive silhouette algorithm from this edge -bool EdgeEPA::computeSilhouette(const Vector3* vertices, uint indexNewVertex, - TrianglesStore& triangleStore) { - // If the edge has not already been visited - if (!mOwnerTriangle->getIsObsolete()) { +bool EdgeEPA::computeSilhouette(const Vector3* vertices, uint32_t indexNewVertex, + TrianglesStore& triangleStore) { + // If the edge has not already been visited + if (!mOwnerTriangle->getIsObsolete()) { - // If the triangle of this edge is not visible from the given point - if (!mOwnerTriangle->isVisibleFromVertex(vertices, indexNewVertex)) { - TriangleEPA* triangle = triangleStore.newTriangle(vertices, indexNewVertex, - getTargetVertexIndex(), - getSourceVertexIndex()); + // If the triangle of this edge is not visible from the given point + if (!mOwnerTriangle->isVisibleFromVertex(vertices, indexNewVertex)) { + TriangleEPA* triangle = triangleStore.newTriangle(vertices, indexNewVertex, + getTargetVertexIndex(), + getSourceVertexIndex()); - // If the triangle has been created - if (triangle != NULL) { - halfLink(EdgeEPA(triangle, 1), *this); - return true; - } + // If the triangle has been created + if (triangle != NULL) { + halfLink(EdgeEPA(triangle, 1), *this); + return true; + } - return false; - } - else { + return false; + } + else { - // The current triangle is visible and therefore obsolete - mOwnerTriangle->setIsObsolete(true); + // The current triangle is visible and therefore obsolete + mOwnerTriangle->setIsObsolete(true); - int backup = triangleStore.getNbTriangles(); + int32_t backup = triangleStore.getNbTriangles(); - if(!mOwnerTriangle->getAdjacentEdge(indexOfNextCounterClockwiseEdge( - this->mIndex)).computeSilhouette(vertices, - indexNewVertex, - triangleStore)) { - mOwnerTriangle->setIsObsolete(false); + if(!mOwnerTriangle->getAdjacentEdge(indexOfNextCounterClockwiseEdge( + this->mIndex)).computeSilhouette(vertices, + indexNewVertex, + triangleStore)) { + mOwnerTriangle->setIsObsolete(false); - TriangleEPA* triangle = triangleStore.newTriangle(vertices, indexNewVertex, - getTargetVertexIndex(), - getSourceVertexIndex()); + TriangleEPA* triangle = triangleStore.newTriangle(vertices, indexNewVertex, + getTargetVertexIndex(), + getSourceVertexIndex()); - // If the triangle has been created - if (triangle != NULL) { - halfLink(EdgeEPA(triangle, 1), *this); - return true; - } + // If the triangle has been created + if (triangle != NULL) { + halfLink(EdgeEPA(triangle, 1), *this); + return true; + } - return false; - } - else if (!mOwnerTriangle->getAdjacentEdge(indexOfPreviousCounterClockwiseEdge( - this->mIndex)).computeSilhouette(vertices, - indexNewVertex, - triangleStore)) { - mOwnerTriangle->setIsObsolete(false); + return false; + } + else if (!mOwnerTriangle->getAdjacentEdge(indexOfPreviousCounterClockwiseEdge( + this->mIndex)).computeSilhouette(vertices, + indexNewVertex, + triangleStore)) { + mOwnerTriangle->setIsObsolete(false); - triangleStore.setNbTriangles(backup); + triangleStore.setNbTriangles(backup); - TriangleEPA* triangle = triangleStore.newTriangle(vertices, indexNewVertex, - getTargetVertexIndex(), - getSourceVertexIndex()); + TriangleEPA* triangle = triangleStore.newTriangle(vertices, indexNewVertex, + getTargetVertexIndex(), + getSourceVertexIndex()); - if (triangle != NULL) { - halfLink(EdgeEPA(triangle, 1), *this); - return true; - } + if (triangle != NULL) { + halfLink(EdgeEPA(triangle, 1), *this); + return true; + } - return false; - } - } - } + return false; + } + } + } - return true; + return true; } diff --git a/ephysics/collision/narrowphase/EPA/EdgeEPA.h b/ephysics/collision/narrowphase/EPA/EdgeEPA.h index e4497aa..fb42910 100644 --- a/ephysics/collision/narrowphase/EPA/EdgeEPA.h +++ b/ephysics/collision/narrowphase/EPA/EdgeEPA.h @@ -1,30 +1,9 @@ -/******************************************************************************** -* 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 REACTPHYSICS3D_EDGE_EPA_H -#define REACTPHYSICS3D_EDGE_EPA_H +/** @file + * @author Daniel Chappuis + * @copyright 2010-2016 Daniel Chappuis + * @license BSD 3 clauses (see license file) + */ +#pragma once // Libraries @@ -43,80 +22,79 @@ class TrianglesStore; */ class EdgeEPA { - private: + private: - // -------------------- Attributes -------------------- // + // -------------------- Attributes -------------------- // - /// Pointer to the triangle that contains this edge - TriangleEPA* mOwnerTriangle; + /// Pointer to the triangle that contains this edge + TriangleEPA* mOwnerTriangle; - /// Index of the edge in the triangle (between 0 and 2). - /// The edge with index i connect triangle vertices i and (i+1 % 3) - int mIndex; + /// Index of the edge in the triangle (between 0 and 2). + /// The edge with index i connect triangle vertices i and (i+1 % 3) + int32_t mIndex; - public: + public: - // -------------------- Methods -------------------- // + // -------------------- Methods -------------------- // - /// Constructor - EdgeEPA(); + /// Constructor + EdgeEPA(); - /// Constructor - EdgeEPA(TriangleEPA* ownerTriangle, int index); + /// Constructor + EdgeEPA(TriangleEPA* ownerTriangle, int32_t index); - /// Copy-constructor - EdgeEPA(const EdgeEPA& edge); + /// Copy-constructor + EdgeEPA(const EdgeEPA& edge); - /// Destructor - ~EdgeEPA(); + /// Destructor + ~EdgeEPA(); - /// Return the pointer to the owner triangle - TriangleEPA* getOwnerTriangle() const; + /// Return the pointer to the owner triangle + TriangleEPA* getOwnerTriangle() const; - /// Return the index of the edge in the triangle - int getIndex() const; + /// Return the index of the edge in the triangle + int32_t getIndex() const; - /// Return index of the source vertex of the edge - uint getSourceVertexIndex() const; + /// Return index of the source vertex of the edge + uint32_t getSourceVertexIndex() const; - /// Return the index of the target vertex of the edge - uint getTargetVertexIndex() const; + /// Return the index of the target vertex of the edge + uint32_t getTargetVertexIndex() const; - /// Execute the recursive silhouette algorithm from this edge - bool computeSilhouette(const Vector3* vertices, uint index, TrianglesStore& triangleStore); + /// Execute the recursive silhouette algorithm from this edge + bool computeSilhouette(const Vector3* vertices, uint32_t index, TrianglesStore& triangleStore); - /// Assignment operator - EdgeEPA& operator=(const EdgeEPA& edge); + /// Assignment operator + EdgeEPA& operator=(const EdgeEPA& edge); }; // Return the pointer to the owner triangle inline TriangleEPA* EdgeEPA::getOwnerTriangle() const { - return mOwnerTriangle; + return mOwnerTriangle; } // Return the edge index -inline int EdgeEPA::getIndex() const { - return mIndex; +inline int32_t EdgeEPA::getIndex() const { + return mIndex; } // Assignment operator inline EdgeEPA& EdgeEPA::operator=(const EdgeEPA& edge) { - mOwnerTriangle = edge.mOwnerTriangle; - mIndex = edge.mIndex; - return *this; + mOwnerTriangle = edge.mOwnerTriangle; + mIndex = edge.mIndex; + return *this; } // Return the index of the next counter-clockwise edge of the ownver triangle -inline int indexOfNextCounterClockwiseEdge(int i) { - return (i + 1) % 3; +inline int32_t indexOfNextCounterClockwiseEdge(int32_t i) { + return (i + 1) % 3; } // Return the index of the previous counter-clockwise edge of the ownver triangle -inline int indexOfPreviousCounterClockwiseEdge(int i) { - return (i + 2) % 3; +inline int32_t indexOfPreviousCounterClockwiseEdge(int32_t i) { + return (i + 2) % 3; } } -#endif diff --git a/ephysics/collision/narrowphase/EPA/TriangleEPA.cpp b/ephysics/collision/narrowphase/EPA/TriangleEPA.cpp index 22270de..d87825f 100644 --- a/ephysics/collision/narrowphase/EPA/TriangleEPA.cpp +++ b/ephysics/collision/narrowphase/EPA/TriangleEPA.cpp @@ -1,28 +1,8 @@ -/******************************************************************************** -* 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. * -* * -********************************************************************************/ - +/** @file + * @author Daniel Chappuis + * @copyright 2010-2016 Daniel Chappuis + * @license BSD 3 clauses (see license file) + */ // Libraries #include @@ -34,15 +14,15 @@ using namespace reactphysics3d; // Constructor TriangleEPA::TriangleEPA() { - + } // Constructor -TriangleEPA::TriangleEPA(uint indexVertex1, uint indexVertex2, uint indexVertex3) - : mIsObsolete(false) { - mIndicesVertices[0] = indexVertex1; - mIndicesVertices[1] = indexVertex2; - mIndicesVertices[2] = indexVertex3; +TriangleEPA::TriangleEPA(uint32_t indexVertex1, uint32_t indexVertex2, uint32_t indexVertex3) + : mIsObsolete(false) { + mIndicesVertices[0] = indexVertex1; + mIndicesVertices[1] = indexVertex2; + mIndicesVertices[2] = indexVertex3; } // Destructor @@ -52,50 +32,50 @@ TriangleEPA::~TriangleEPA() { // Compute the point v closest to the origin of this triangle bool TriangleEPA::computeClosestPoint(const Vector3* vertices) { - const Vector3& p0 = vertices[mIndicesVertices[0]]; + const Vector3& p0 = vertices[mIndicesVertices[0]]; - Vector3 v1 = vertices[mIndicesVertices[1]] - p0; - Vector3 v2 = vertices[mIndicesVertices[2]] - p0; - decimal v1Dotv1 = v1.dot(v1); - decimal v1Dotv2 = v1.dot(v2); - decimal v2Dotv2 = v2.dot(v2); - decimal p0Dotv1 = p0.dot(v1); - decimal p0Dotv2 = p0.dot(v2); + Vector3 v1 = vertices[mIndicesVertices[1]] - p0; + Vector3 v2 = vertices[mIndicesVertices[2]] - p0; + float v1Dotv1 = v1.dot(v1); + float v1Dotv2 = v1.dot(v2); + float v2Dotv2 = v2.dot(v2); + float p0Dotv1 = p0.dot(v1); + float p0Dotv2 = p0.dot(v2); - // Compute determinant - mDet = v1Dotv1 * v2Dotv2 - v1Dotv2 * v1Dotv2; + // Compute determinant + mDet = v1Dotv1 * v2Dotv2 - v1Dotv2 * v1Dotv2; - // Compute lambda values - mLambda1 = p0Dotv2 * v1Dotv2 - p0Dotv1 * v2Dotv2; - mLambda2 = p0Dotv1 * v1Dotv2 - p0Dotv2 * v1Dotv1; + // Compute lambda values + mLambda1 = p0Dotv2 * v1Dotv2 - p0Dotv1 * v2Dotv2; + mLambda2 = p0Dotv1 * v1Dotv2 - p0Dotv2 * v1Dotv1; - // If the determinant is positive - if (mDet > 0.0) { - // Compute the closest point v - mClosestPoint = p0 + decimal(1.0) / mDet * (mLambda1 * v1 + mLambda2 * v2); + // If the determinant is positive + if (mDet > 0.0) { + // Compute the closest point v + mClosestPoint = p0 + float(1.0) / mDet * (mLambda1 * v1 + mLambda2 * v2); - // Compute the square distance of closest point to the origin - mDistSquare = mClosestPoint.dot(mClosestPoint); + // Compute the square distance of closest point to the origin + mDistSquare = mClosestPoint.dot(mClosestPoint); - return true; - } + return true; + } - return false; + return false; } /// Link an edge with another one. It means that the current edge of a triangle will /// be associated with the edge of another triangle in order that both triangles /// are neighbour along both edges). bool reactphysics3d::link(const EdgeEPA& edge0, const EdgeEPA& edge1) { - bool isPossible = (edge0.getSourceVertexIndex() == edge1.getTargetVertexIndex() && - edge0.getTargetVertexIndex() == edge1.getSourceVertexIndex()); + bool isPossible = (edge0.getSourceVertexIndex() == edge1.getTargetVertexIndex() && + edge0.getTargetVertexIndex() == edge1.getSourceVertexIndex()); - if (isPossible) { - edge0.getOwnerTriangle()->mAdjacentEdges[edge0.getIndex()] = edge1; - edge1.getOwnerTriangle()->mAdjacentEdges[edge1.getIndex()] = edge0; - } + if (isPossible) { + edge0.getOwnerTriangle()->mAdjacentEdges[edge0.getIndex()] = edge1; + edge1.getOwnerTriangle()->mAdjacentEdges[edge1.getIndex()] = edge0; + } - return isPossible; + return isPossible; } /// Make an half link of an edge with another one from another triangle. An half-link @@ -103,11 +83,11 @@ bool reactphysics3d::link(const EdgeEPA& edge0, const EdgeEPA& edge1) { /// adjacent edge of "edge0" but not the opposite. The opposite edge connection will /// be made later. void reactphysics3d::halfLink(const EdgeEPA& edge0, const EdgeEPA& edge1) { - assert(edge0.getSourceVertexIndex() == edge1.getTargetVertexIndex() && - edge0.getTargetVertexIndex() == edge1.getSourceVertexIndex()); + assert(edge0.getSourceVertexIndex() == edge1.getTargetVertexIndex() && + edge0.getTargetVertexIndex() == edge1.getSourceVertexIndex()); - // Link - edge0.getOwnerTriangle()->mAdjacentEdges[edge0.getIndex()] = edge1; + // Link + edge0.getOwnerTriangle()->mAdjacentEdges[edge0.getIndex()] = edge1; } // Execute the recursive silhouette algorithm from this triangle face. @@ -121,35 +101,35 @@ void reactphysics3d::halfLink(const EdgeEPA& edge0, const EdgeEPA& edge1) { /// face from the new vertex, computes the silhouette and create the new faces from the new vertex in /// order that we always have a convex polytope. The faces visible from the new vertex are set /// obselete and will not be considered as being a candidate face in the future. -bool TriangleEPA::computeSilhouette(const Vector3* vertices, uint indexNewVertex, - TrianglesStore& triangleStore) { - - uint first = triangleStore.getNbTriangles(); +bool TriangleEPA::computeSilhouette(const Vector3* vertices, uint32_t indexNewVertex, + TrianglesStore& triangleStore) { + + uint32_t first = triangleStore.getNbTriangles(); - // Mark the current triangle as obsolete because it - setIsObsolete(true); + // Mark the current triangle as obsolete because it + setIsObsolete(true); - // Execute recursively the silhouette algorithm for the adjacent edges of neighboring - // triangles of the current triangle - bool result = mAdjacentEdges[0].computeSilhouette(vertices, indexNewVertex, triangleStore) && - mAdjacentEdges[1].computeSilhouette(vertices, indexNewVertex, triangleStore) && - mAdjacentEdges[2].computeSilhouette(vertices, indexNewVertex, triangleStore); + // Execute recursively the silhouette algorithm for the adjacent edges of neighboring + // triangles of the current triangle + bool result = mAdjacentEdges[0].computeSilhouette(vertices, indexNewVertex, triangleStore) && + mAdjacentEdges[1].computeSilhouette(vertices, indexNewVertex, triangleStore) && + mAdjacentEdges[2].computeSilhouette(vertices, indexNewVertex, triangleStore); - if (result) { - int i,j; + if (result) { + int32_t i,j; - // For each triangle face that contains the new vertex and an edge of the silhouette - for (i=first, j=triangleStore.getNbTriangles()-1; - i != triangleStore.getNbTriangles(); j = i++) { - TriangleEPA* triangle = &triangleStore[i]; - halfLink(triangle->getAdjacentEdge(1), EdgeEPA(triangle, 1)); + // For each triangle face that contains the new vertex and an edge of the silhouette + for (i=first, j=triangleStore.getNbTriangles()-1; + i != triangleStore.getNbTriangles(); j = i++) { + TriangleEPA* triangle = &triangleStore[i]; + halfLink(triangle->getAdjacentEdge(1), EdgeEPA(triangle, 1)); - if (!link(EdgeEPA(triangle, 0), EdgeEPA(&triangleStore[j], 2))) { - return false; - } - } + if (!link(EdgeEPA(triangle, 0), EdgeEPA(&triangleStore[j], 2))) { + return false; + } + } - } + } - return result; + return result; } diff --git a/ephysics/collision/narrowphase/EPA/TriangleEPA.h b/ephysics/collision/narrowphase/EPA/TriangleEPA.h index fad7b50..773469e 100644 --- a/ephysics/collision/narrowphase/EPA/TriangleEPA.h +++ b/ephysics/collision/narrowphase/EPA/TriangleEPA.h @@ -1,30 +1,9 @@ -/******************************************************************************** -* 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 REACTPHYSICS3D_TRIANGLE_EPA_H -#define REACTPHYSICS3D_TRIANGLE_EPA_H +/** @file + * @author Daniel Chappuis + * @copyright 2010-2016 Daniel Chappuis + * @license BSD 3 clauses (see license file) + */ +#pragma once // Libraries #include @@ -46,154 +25,152 @@ void halfLink(const EdgeEPA& edge0, const EdgeEPA& edge1); */ class TriangleEPA { - private: + private: - // -------------------- Attributes -------------------- // + // -------------------- Attributes -------------------- // - /// Indices of the vertices y_i of the triangle - uint mIndicesVertices[3]; + /// Indices of the vertices y_i of the triangle + uint32_t mIndicesVertices[3]; - /// Three adjacent edges of the triangle (edges of other triangles) - EdgeEPA mAdjacentEdges[3]; + /// Three adjacent edges of the triangle (edges of other triangles) + EdgeEPA mAdjacentEdges[3]; - /// True if the triangle face is visible from the new support point - bool mIsObsolete; + /// True if the triangle face is visible from the new support point + bool mIsObsolete; - /// Determinant - decimal mDet; + /// Determinant + float mDet; - /// Point v closest to the origin on the affine hull of the triangle - Vector3 mClosestPoint; + /// Point v closest to the origin on the affine hull of the triangle + Vector3 mClosestPoint; - /// Lambda1 value such that v = lambda0 * y_0 + lambda1 * y_1 + lambda2 * y_2 - decimal mLambda1; + /// Lambda1 value such that v = lambda0 * y_0 + lambda1 * y_1 + lambda2 * y_2 + float mLambda1; - /// Lambda1 value such that v = lambda0 * y_0 + lambda1 * y_1 + lambda2 * y_2 - decimal mLambda2; + /// Lambda1 value such that v = lambda0 * y_0 + lambda1 * y_1 + lambda2 * y_2 + float mLambda2; - /// Square distance of the point closest point v to the origin - decimal mDistSquare; + /// Square distance of the point closest point v to the origin + float mDistSquare; - // -------------------- Methods -------------------- // + // -------------------- Methods -------------------- // - /// Private copy-constructor - TriangleEPA(const TriangleEPA& triangle); + /// Private copy-constructor + TriangleEPA(const TriangleEPA& triangle); - /// Private assignment operator - TriangleEPA& operator=(const TriangleEPA& triangle); + /// Private assignment operator + TriangleEPA& operator=(const TriangleEPA& triangle); - public: + public: - // -------------------- Methods -------------------- // + // -------------------- Methods -------------------- // - /// Constructor - TriangleEPA(); + /// Constructor + TriangleEPA(); - /// Constructor - TriangleEPA(uint v1, uint v2, uint v3); + /// Constructor + TriangleEPA(uint32_t v1, uint32_t v2, uint32_t v3); - /// Destructor - ~TriangleEPA(); + /// Destructor + ~TriangleEPA(); - /// Return an adjacent edge of the triangle - EdgeEPA& getAdjacentEdge(int index); + /// Return an adjacent edge of the triangle + EdgeEPA& getAdjacentEdge(int32_t index); - /// Set an adjacent edge of the triangle - void setAdjacentEdge(int index, EdgeEPA& edge); + /// Set an adjacent edge of the triangle + void setAdjacentEdge(int32_t index, EdgeEPA& edge); - /// Return the square distance of the closest point to origin - decimal getDistSquare() const; + /// Return the square distance of the closest point to origin + float getDistSquare() const; - /// Set the isObsolete value - void setIsObsolete(bool isObsolete); + /// Set the isObsolete value + void setIsObsolete(bool isObsolete); - /// Return true if the triangle face is obsolete - bool getIsObsolete() const; + /// Return true if the triangle face is obsolete + bool getIsObsolete() const; - /// Return the point closest to the origin - const Vector3& getClosestPoint() const; + /// Return the point closest to the origin + const Vector3& getClosestPoint() const; - // Return true if the closest point on affine hull is inside the triangle - bool isClosestPointInternalToTriangle() const; + // Return true if the closest point on affine hull is inside the triangle + bool isClosestPointInternalToTriangle() const; - /// Return true if the triangle is visible from a given vertex - bool isVisibleFromVertex(const Vector3* vertices, uint index) const; + /// Return true if the triangle is visible from a given vertex + bool isVisibleFromVertex(const Vector3* vertices, uint32_t index) const; - /// Compute the point v closest to the origin of this triangle - bool computeClosestPoint(const Vector3* vertices); + /// Compute the point v closest to the origin of this triangle + bool computeClosestPoint(const Vector3* vertices); - /// Compute the point of an object closest to the origin - Vector3 computeClosestPointOfObject(const Vector3* supportPointsOfObject) const; + /// Compute the point of an object closest to the origin + Vector3 computeClosestPointOfObject(const Vector3* supportPointsOfObject) const; - /// Execute the recursive silhouette algorithm from this triangle face. - bool computeSilhouette(const Vector3* vertices, uint index, TrianglesStore& triangleStore); + /// Execute the recursive silhouette algorithm from this triangle face. + bool computeSilhouette(const Vector3* vertices, uint32_t index, TrianglesStore& triangleStore); - /// Access operator - uint operator[](int i) const; + /// Access operator + uint32_t operator[](int32_t i) const; - /// Associate two edges - friend bool link(const EdgeEPA& edge0, const EdgeEPA& edge1); + /// Associate two edges + friend bool link(const EdgeEPA& edge0, const EdgeEPA& edge1); - /// Make a half-link between two edges - friend void halfLink(const EdgeEPA& edge0, const EdgeEPA& edge1); + /// Make a half-link between two edges + friend void halfLink(const EdgeEPA& edge0, const EdgeEPA& edge1); }; // Return an edge of the triangle -inline EdgeEPA& TriangleEPA::getAdjacentEdge(int index) { - assert(index >= 0 && index < 3); - return mAdjacentEdges[index]; +inline EdgeEPA& TriangleEPA::getAdjacentEdge(int32_t index) { + assert(index >= 0 && index < 3); + return mAdjacentEdges[index]; } // Set an adjacent edge of the triangle -inline void TriangleEPA::setAdjacentEdge(int index, EdgeEPA& edge) { - assert(index >=0 && index < 3); - mAdjacentEdges[index] = edge; +inline void TriangleEPA::setAdjacentEdge(int32_t index, EdgeEPA& edge) { + assert(index >=0 && index < 3); + mAdjacentEdges[index] = edge; } // Return the square distance of the closest point to origin -inline decimal TriangleEPA::getDistSquare() const { - return mDistSquare; +inline float TriangleEPA::getDistSquare() const { + return mDistSquare; } // Set the isObsolete value inline void TriangleEPA::setIsObsolete(bool isObsolete) { - mIsObsolete = isObsolete; + mIsObsolete = isObsolete; } // Return true if the triangle face is obsolete inline bool TriangleEPA::getIsObsolete() const { - return mIsObsolete; + return mIsObsolete; } // Return the point closest to the origin inline const Vector3& TriangleEPA::getClosestPoint() const { - return mClosestPoint; + return mClosestPoint; } // Return true if the closest point on affine hull is inside the triangle inline bool TriangleEPA::isClosestPointInternalToTriangle() const { - return (mLambda1 >= 0.0 && mLambda2 >= 0.0 && (mLambda1 + mLambda2) <= mDet); + return (mLambda1 >= 0.0 && mLambda2 >= 0.0 && (mLambda1 + mLambda2) <= mDet); } // Return true if the triangle is visible from a given vertex -inline bool TriangleEPA::isVisibleFromVertex(const Vector3* vertices, uint index) const { - Vector3 closestToVert = vertices[index] - mClosestPoint; - return (mClosestPoint.dot(closestToVert) > 0.0); +inline bool TriangleEPA::isVisibleFromVertex(const Vector3* vertices, uint32_t index) const { + Vector3 closestToVert = vertices[index] - mClosestPoint; + return (mClosestPoint.dot(closestToVert) > 0.0); } // Compute the point of an object closest to the origin inline Vector3 TriangleEPA::computeClosestPointOfObject(const Vector3* supportPointsOfObject) const{ - const Vector3& p0 = supportPointsOfObject[mIndicesVertices[0]]; - return p0 + decimal(1.0)/mDet * (mLambda1 * (supportPointsOfObject[mIndicesVertices[1]] - p0) + - mLambda2 * (supportPointsOfObject[mIndicesVertices[2]] - p0)); + const Vector3& p0 = supportPointsOfObject[mIndicesVertices[0]]; + return p0 + float(1.0)/mDet * (mLambda1 * (supportPointsOfObject[mIndicesVertices[1]] - p0) + + mLambda2 * (supportPointsOfObject[mIndicesVertices[2]] - p0)); } // Access operator -inline uint TriangleEPA::operator[](int i) const { - assert(i >= 0 && i <3); - return mIndicesVertices[i]; +inline uint32_t TriangleEPA::operator[](int32_t i) const { + assert(i >= 0 && i <3); + return mIndicesVertices[i]; } } - -#endif diff --git a/ephysics/collision/narrowphase/EPA/TrianglesStore.cpp b/ephysics/collision/narrowphase/EPA/TrianglesStore.cpp index 344a54f..9c4a1fc 100644 --- a/ephysics/collision/narrowphase/EPA/TrianglesStore.cpp +++ b/ephysics/collision/narrowphase/EPA/TrianglesStore.cpp @@ -1,27 +1,8 @@ -/******************************************************************************** -* 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. * -* * -********************************************************************************/ +/** @file + * @author Daniel Chappuis + * @copyright 2010-2016 Daniel Chappuis + * @license BSD 3 clauses (see license file) + */ // Libraries #include @@ -31,7 +12,7 @@ using namespace reactphysics3d; // Constructor TrianglesStore::TrianglesStore() : mNbTriangles(0) { - + } // Destructor diff --git a/ephysics/collision/narrowphase/EPA/TrianglesStore.h b/ephysics/collision/narrowphase/EPA/TrianglesStore.h index 6295cb5..28f4daa 100644 --- a/ephysics/collision/narrowphase/EPA/TrianglesStore.h +++ b/ephysics/collision/narrowphase/EPA/TrianglesStore.h @@ -1,30 +1,9 @@ -/******************************************************************************** -* 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 REACTPHYSICS3D_TRIANGLES_STORE_H -#define REACTPHYSICS3D_TRIANGLES_STORE_H +/** @file + * @author Daniel Chappuis + * @copyright 2010-2016 Daniel Chappuis + * @license BSD 3 clauses (see license file) + */ +#pragma once #include @@ -36,7 +15,7 @@ namespace reactphysics3d { // Constants -const unsigned int MAX_TRIANGLES = 200; // Maximum number of triangles +const uint32_t MAX_TRIANGLES = 200; // Maximum number of triangles // Class TriangleStore /** @@ -44,98 +23,97 @@ const unsigned int MAX_TRIANGLES = 200; // Maximum number of triangles */ class TrianglesStore { - private: + private: - // -------------------- Attributes -------------------- // + // -------------------- Attributes -------------------- // - /// Triangles - TriangleEPA mTriangles[MAX_TRIANGLES]; + /// Triangles + TriangleEPA mTriangles[MAX_TRIANGLES]; - /// Number of triangles - int mNbTriangles; + /// Number of triangles + int32_t mNbTriangles; - // -------------------- Methods -------------------- // + // -------------------- Methods -------------------- // - /// Private copy-constructor - TrianglesStore(const TrianglesStore& triangleStore); + /// Private copy-constructor + TrianglesStore(const TrianglesStore& triangleStore); - /// Private assignment operator - TrianglesStore& operator=(const TrianglesStore& triangleStore); - - public: + /// Private assignment operator + TrianglesStore& operator=(const TrianglesStore& triangleStore); + + public: - // -------------------- Methods -------------------- // + // -------------------- Methods -------------------- // - /// Constructor - TrianglesStore(); + /// Constructor + TrianglesStore(); - /// Destructor - ~TrianglesStore(); + /// Destructor + ~TrianglesStore(); - /// Clear all the storage - void clear(); + /// Clear all the storage + void clear(); - /// Return the number of triangles - int getNbTriangles() const; + /// Return the number of triangles + int32_t getNbTriangles() const; - /// Set the number of triangles - void setNbTriangles(int backup); + /// Set the number of triangles + void setNbTriangles(int32_t backup); - /// Return the last triangle - TriangleEPA& last(); + /// Return the last triangle + TriangleEPA& last(); - /// Create a new triangle - TriangleEPA* newTriangle(const Vector3* vertices, uint v0, uint v1, uint v2); + /// Create a new triangle + TriangleEPA* newTriangle(const Vector3* vertices, uint32_t v0, uint32_t v1, uint32_t v2); - /// Access operator - TriangleEPA& operator[](int i); + /// Access operator + TriangleEPA& operator[](int32_t i); }; // Clear all the storage inline void TrianglesStore::clear() { - mNbTriangles = 0; + mNbTriangles = 0; } // Return the number of triangles -inline int TrianglesStore::getNbTriangles() const { - return mNbTriangles; +inline int32_t TrianglesStore::getNbTriangles() const { + return mNbTriangles; } -inline void TrianglesStore::setNbTriangles(int backup) { - mNbTriangles = backup; +inline void TrianglesStore::setNbTriangles(int32_t backup) { + mNbTriangles = backup; } // Return the last triangle inline TriangleEPA& TrianglesStore::last() { - assert(mNbTriangles > 0); - return mTriangles[mNbTriangles - 1]; + assert(mNbTriangles > 0); + return mTriangles[mNbTriangles - 1]; } // Create a new triangle inline TriangleEPA* TrianglesStore::newTriangle(const Vector3* vertices, - uint v0,uint v1, uint v2) { - TriangleEPA* newTriangle = NULL; + uint32_t v0,uint32_t v1, uint32_t v2) { + TriangleEPA* newTriangle = NULL; - // If we have not reached the maximum number of triangles - if (mNbTriangles != MAX_TRIANGLES) { - newTriangle = &mTriangles[mNbTriangles++]; - new (newTriangle) TriangleEPA(v0, v1, v2); - if (!newTriangle->computeClosestPoint(vertices)) { - mNbTriangles--; - newTriangle = NULL; - } - } + // If we have not reached the maximum number of triangles + if (mNbTriangles != MAX_TRIANGLES) { + newTriangle = &mTriangles[mNbTriangles++]; + new (newTriangle) TriangleEPA(v0, v1, v2); + if (!newTriangle->computeClosestPoint(vertices)) { + mNbTriangles--; + newTriangle = NULL; + } + } - // Return the new triangle - return newTriangle; + // Return the new triangle + return newTriangle; } // Access operator -inline TriangleEPA& TrianglesStore::operator[](int i) { - return mTriangles[i]; +inline TriangleEPA& TrianglesStore::operator[](int32_t i) { + return mTriangles[i]; } } -#endif diff --git a/ephysics/collision/narrowphase/GJK/GJKAlgorithm.cpp b/ephysics/collision/narrowphase/GJK/GJKAlgorithm.cpp index 56fb929..f853a22 100644 --- a/ephysics/collision/narrowphase/GJK/GJKAlgorithm.cpp +++ b/ephysics/collision/narrowphase/GJK/GJKAlgorithm.cpp @@ -1,27 +1,8 @@ -/******************************************************************************** -* 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. * -* * -********************************************************************************/ +/** @file + * @author Daniel Chappuis + * @copyright 2010-2016 Daniel Chappuis + * @license BSD 3 clauses (see license file) + */ // Libraries #include @@ -49,375 +30,375 @@ GJKAlgorithm::~GJKAlgorithm() { // Compute a contact info if the two collision shapes collide. /// This method implements the Hybrid Technique for computing the penetration depth by -/// running the GJK algorithm on original objects (without margin). If the shapes intersect +/// running the GJK algorithm on original objects (without margin). If the shapes int32_tersect /// only in the margins, the method compute the penetration depth and contact points -/// (of enlarged objects). If the original objects (without margin) intersect, we +/// (of enlarged objects). If the original objects (without margin) int32_tersect, we /// call the computePenetrationDepthForEnlargedObjects() method that run the GJK /// algorithm on the enlarged object to obtain a simplex polytope that contains the /// 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(const CollisionShapeInfo& shape1Info, - const CollisionShapeInfo& shape2Info, - NarrowPhaseCallback* narrowPhaseCallback) { + const CollisionShapeInfo& shape2Info, + NarrowPhaseCallback* narrowPhaseCallback) { - PROFILE("GJKAlgorithm::testCollision()"); - - Vector3 suppA; // Support point of object A - Vector3 suppB; // Support point of object B - Vector3 w; // Support point of Minkowski difference A-B - Vector3 pA; // Closest point of object A - Vector3 pB; // Closest point of object B - decimal vDotw; - decimal prevDistSquare; + PROFILE("GJKAlgorithm::testCollision()"); + + Vector3 suppA; // Support point of object A + Vector3 suppB; // Support point of object B + Vector3 w; // Support point of Minkowski difference A-B + Vector3 pA; // Closest point of object A + Vector3 pB; // Closest point of object B + float vDotw; + float prevDistSquare; - assert(shape1Info.collisionShape->isConvex()); - assert(shape2Info.collisionShape->isConvex()); + assert(shape1Info.collisionShape->isConvex()); + assert(shape2Info.collisionShape->isConvex()); - const ConvexShape* shape1 = static_cast(shape1Info.collisionShape); - const ConvexShape* shape2 = static_cast(shape2Info.collisionShape); + const ConvexShape* shape1 = static_cast(shape1Info.collisionShape); + const ConvexShape* shape2 = static_cast(shape2Info.collisionShape); - void** shape1CachedCollisionData = shape1Info.cachedCollisionData; - void** shape2CachedCollisionData = shape2Info.cachedCollisionData; + void** shape1CachedCollisionData = shape1Info.cachedCollisionData; + void** shape2CachedCollisionData = shape2Info.cachedCollisionData; - // Get the local-space to world-space transforms - const Transform transform1 = shape1Info.shapeToWorldTransform; - const Transform transform2 = shape2Info.shapeToWorldTransform; + // Get the local-space to world-space transforms + const Transform transform1 = shape1Info.shapeToWorldTransform; + const Transform transform2 = shape2Info.shapeToWorldTransform; - // 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) - Transform body2Tobody1 = transform1.getInverse() * transform2; + // 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) + Transform body2Tobody1 = transform1.getInverse() * transform2; - // Matrix that transform a direction from local - // space of body 1 into local space of body 2 - Matrix3x3 rotateToBody2 = transform2.getOrientation().getMatrix().getTranspose() * - transform1.getOrientation().getMatrix(); + // Matrix that transform a direction from local + // space of body 1 int32_to local space of body 2 + Matrix3x3 rotateToBody2 = transform2.getOrientation().getMatrix().getTranspose() * + transform1.getOrientation().getMatrix(); - // Initialize the margin (sum of margins of both objects) - decimal margin = shape1->getMargin() + shape2->getMargin(); - decimal marginSquare = margin * margin; - assert(margin > 0.0); + // Initialize the margin (sum of margins of both objects) + float margin = shape1->getMargin() + shape2->getMargin(); + float marginSquare = margin * margin; + assert(margin > 0.0); - // Create a simplex set - Simplex simplex; + // Create a simplex set + Simplex simplex; - // Get the previous point V (last cached separating axis) - Vector3 v = mCurrentOverlappingPair->getCachedSeparatingAxis(); + // Get the previous point V (last cached separating axis) + Vector3 v = mCurrentOverlappingPair->getCachedSeparatingAxis(); - // Initialize the upper bound for the square distance - decimal distSquare = DECIMAL_LARGEST; - - do { - - // Compute the support points for original objects (without margins) A and B - suppA = shape1->getLocalSupportPointWithoutMargin(-v, shape1CachedCollisionData); - suppB = body2Tobody1 * - shape2->getLocalSupportPointWithoutMargin(rotateToBody2 * v, shape2CachedCollisionData); + // Initialize the upper bound for the square distance + float distSquare = DECIMAL_LARGEST; + + do { + + // Compute the support points for original objects (without margins) A and B + suppA = shape1->getLocalSupportPointWithoutMargin(-v, shape1CachedCollisionData); + suppB = body2Tobody1 * + shape2->getLocalSupportPointWithoutMargin(rotateToBody2 * v, shape2CachedCollisionData); - // Compute the support point for the Minkowski difference A-B - w = suppA - suppB; - - vDotw = v.dot(w); - - // If the enlarge objects (with margins) do not intersect - if (vDotw > 0.0 && vDotw * vDotw > distSquare * marginSquare) { - - // Cache the current separating axis for frame coherence - mCurrentOverlappingPair->setCachedSeparatingAxis(v); - - // No intersection, we return - return; - } + // Compute the support point for the Minkowski difference A-B + w = suppA - suppB; + + vDotw = v.dot(w); + + // If the enlarge objects (with margins) do not int32_tersect + if (vDotw > 0.0 && vDotw * vDotw > distSquare * marginSquare) { + + // Cache the current separating axis for frame coherence + mCurrentOverlappingPair->setCachedSeparatingAxis(v); + + // No int32_tersection, we return + return; + } - // If the objects intersect only in the margins - if (simplex.isPointInSimplex(w) || distSquare - vDotw <= distSquare * REL_ERROR_SQUARE) { + // If the objects int32_tersect only in the margins + if (simplex.isPointInSimplex(w) || distSquare - vDotw <= distSquare * REL_ERROR_SQUARE) { - // Compute the closet points of both objects (without the margins) - simplex.computeClosestPointsOfAandB(pA, pB); + // Compute the closet points of both objects (without the margins) + simplex.computeClosestPointsOfAandB(pA, pB); - // Project those two points on the margins to have the closest points of both - // object with the margins - decimal dist = sqrt(distSquare); - assert(dist > 0.0); - pA = (pA - (shape1->getMargin() / dist) * v); - pB = body2Tobody1.getInverse() * (pB + (shape2->getMargin() / dist) * v); + // Project those two points on the margins to have the closest points of both + // object with the margins + float dist = sqrt(distSquare); + assert(dist > 0.0); + pA = (pA - (shape1->getMargin() / dist) * v); + pB = body2Tobody1.getInverse() * (pB + (shape2->getMargin() / dist) * v); - // Compute the contact info - Vector3 normal = transform1.getOrientation() * (-v.getUnit()); - decimal penetrationDepth = margin - dist; + // Compute the contact info + Vector3 normal = transform1.getOrientation() * (-v.getUnit()); + float penetrationDepth = margin - dist; // Reject the contact if the penetration depth is negative (due too numerical errors) - if (penetrationDepth <= 0.0) return; + if (penetrationDepth <= 0.0) return; - // Create the contact info object - ContactPointInfo contactInfo(shape1Info.proxyShape, shape2Info.proxyShape, shape1Info.collisionShape, - shape2Info.collisionShape, normal, penetrationDepth, pA, pB); + // Create the contact info object + ContactPointInfo contactInfo(shape1Info.proxyShape, shape2Info.proxyShape, shape1Info.collisionShape, + shape2Info.collisionShape, normal, penetrationDepth, pA, pB); - narrowPhaseCallback->notifyContact(shape1Info.overlappingPair, contactInfo); + narrowPhaseCallback->notifyContact(shape1Info.overlappingPair, contactInfo); - // There is an intersection, therefore we return - return; - } + // There is an int32_tersection, therefore we return + return; + } - // Add the new support point to the simplex - simplex.addPoint(w, suppA, suppB); + // Add the new support point to the simplex + simplex.addPoint(w, suppA, suppB); - // If the simplex is affinely dependent - if (simplex.isAffinelyDependent()) { + // If the simplex is affinely dependent + if (simplex.isAffinelyDependent()) { - // Compute the closet points of both objects (without the margins) - simplex.computeClosestPointsOfAandB(pA, pB); + // Compute the closet points of both objects (without the margins) + simplex.computeClosestPointsOfAandB(pA, pB); - // Project those two points on the margins to have the closest points of both - // object with the margins - decimal dist = sqrt(distSquare); - assert(dist > 0.0); - pA = (pA - (shape1->getMargin() / dist) * v); - pB = body2Tobody1.getInverse() * (pB + (shape2->getMargin() / dist) * v); + // Project those two points on the margins to have the closest points of both + // object with the margins + float dist = sqrt(distSquare); + assert(dist > 0.0); + pA = (pA - (shape1->getMargin() / dist) * v); + pB = body2Tobody1.getInverse() * (pB + (shape2->getMargin() / dist) * v); - // Compute the contact info - Vector3 normal = transform1.getOrientation() * (-v.getUnit()); - decimal penetrationDepth = margin - dist; + // Compute the contact info + Vector3 normal = transform1.getOrientation() * (-v.getUnit()); + float penetrationDepth = margin - dist; // Reject the contact if the penetration depth is negative (due too numerical errors) - if (penetrationDepth <= 0.0) return; + if (penetrationDepth <= 0.0) return; - // Create the contact info object - ContactPointInfo contactInfo(shape1Info.proxyShape, shape2Info.proxyShape, shape1Info.collisionShape, - shape2Info.collisionShape, normal, penetrationDepth, pA, pB); + // Create the contact info object + ContactPointInfo contactInfo(shape1Info.proxyShape, shape2Info.proxyShape, shape1Info.collisionShape, + shape2Info.collisionShape, normal, penetrationDepth, pA, pB); - narrowPhaseCallback->notifyContact(shape1Info.overlappingPair, contactInfo); + narrowPhaseCallback->notifyContact(shape1Info.overlappingPair, contactInfo); - // There is an intersection, therefore we return - return; - } + // There is an int32_tersection, therefore we return + return; + } - // Compute the point of the simplex closest to the origin - // If the computation of the closest point fail - if (!simplex.computeClosestPoint(v)) { + // Compute the point of the simplex closest to the origin + // If the computation of the closest point fail + if (!simplex.computeClosestPoint(v)) { - // Compute the closet points of both objects (without the margins) - simplex.computeClosestPointsOfAandB(pA, pB); + // Compute the closet points of both objects (without the margins) + simplex.computeClosestPointsOfAandB(pA, pB); - // Project those two points on the margins to have the closest points of both - // object with the margins - decimal dist = sqrt(distSquare); - assert(dist > 0.0); - pA = (pA - (shape1->getMargin() / dist) * v); - pB = body2Tobody1.getInverse() * (pB + (shape2->getMargin() / dist) * v); + // Project those two points on the margins to have the closest points of both + // object with the margins + float dist = sqrt(distSquare); + assert(dist > 0.0); + pA = (pA - (shape1->getMargin() / dist) * v); + pB = body2Tobody1.getInverse() * (pB + (shape2->getMargin() / dist) * v); - // Compute the contact info - Vector3 normal = transform1.getOrientation() * (-v.getUnit()); - decimal penetrationDepth = margin - dist; + // Compute the contact info + Vector3 normal = transform1.getOrientation() * (-v.getUnit()); + float penetrationDepth = margin - dist; // Reject the contact if the penetration depth is negative (due too numerical errors) - if (penetrationDepth <= 0.0) return; + if (penetrationDepth <= 0.0) return; - // Create the contact info object - ContactPointInfo contactInfo(shape1Info.proxyShape, shape2Info.proxyShape, shape1Info.collisionShape, - shape2Info.collisionShape, normal, penetrationDepth, pA, pB); + // Create the contact info object + ContactPointInfo contactInfo(shape1Info.proxyShape, shape2Info.proxyShape, shape1Info.collisionShape, + shape2Info.collisionShape, normal, penetrationDepth, pA, pB); - narrowPhaseCallback->notifyContact(shape1Info.overlappingPair, contactInfo); + narrowPhaseCallback->notifyContact(shape1Info.overlappingPair, contactInfo); - // There is an intersection, therefore we return - return; - } + // There is an int32_tersection, therefore we return + return; + } - // Store and update the squared distance of the closest point - prevDistSquare = distSquare; - distSquare = v.lengthSquare(); + // Store and update the squared distance of the closest point + prevDistSquare = distSquare; + distSquare = v.lengthSquare(); - // If the distance to the closest point doesn't improve a lot - if (prevDistSquare - distSquare <= MACHINE_EPSILON * prevDistSquare) { - simplex.backupClosestPointInSimplex(v); - - // Get the new squared distance - distSquare = v.lengthSquare(); + // If the distance to the closest point doesn't improve a lot + if (prevDistSquare - distSquare <= MACHINE_EPSILON * prevDistSquare) { + simplex.backupClosestPointInSimplex(v); + + // Get the new squared distance + distSquare = v.lengthSquare(); - // Compute the closet points of both objects (without the margins) - simplex.computeClosestPointsOfAandB(pA, pB); + // Compute the closet points of both objects (without the margins) + simplex.computeClosestPointsOfAandB(pA, pB); - // Project those two points on the margins to have the closest points of both - // object with the margins - decimal dist = sqrt(distSquare); - assert(dist > 0.0); - pA = (pA - (shape1->getMargin() / dist) * v); - pB = body2Tobody1.getInverse() * (pB + (shape2->getMargin() / dist) * v); + // Project those two points on the margins to have the closest points of both + // object with the margins + float dist = sqrt(distSquare); + assert(dist > 0.0); + pA = (pA - (shape1->getMargin() / dist) * v); + pB = body2Tobody1.getInverse() * (pB + (shape2->getMargin() / dist) * v); - // Compute the contact info - Vector3 normal = transform1.getOrientation() * (-v.getUnit()); - decimal penetrationDepth = margin - dist; + // Compute the contact info + Vector3 normal = transform1.getOrientation() * (-v.getUnit()); + float penetrationDepth = margin - dist; // Reject the contact if the penetration depth is negative (due too numerical errors) - if (penetrationDepth <= 0.0) return; + if (penetrationDepth <= 0.0) return; - // Create the contact info object - ContactPointInfo contactInfo(shape1Info.proxyShape, shape2Info.proxyShape, shape1Info.collisionShape, - shape2Info.collisionShape, normal, penetrationDepth, pA, pB); + // Create the contact info object + ContactPointInfo contactInfo(shape1Info.proxyShape, shape2Info.proxyShape, shape1Info.collisionShape, + shape2Info.collisionShape, normal, penetrationDepth, pA, pB); - narrowPhaseCallback->notifyContact(shape1Info.overlappingPair, contactInfo); + narrowPhaseCallback->notifyContact(shape1Info.overlappingPair, contactInfo); - // There is an intersection, therefore we return - return; - } - } while(!simplex.isFull() && distSquare > MACHINE_EPSILON * - simplex.getMaxLengthSquareOfAPoint()); + // There is an int32_tersection, therefore we return + return; + } + } while(!simplex.isFull() && distSquare > MACHINE_EPSILON * + simplex.getMaxLengthSquareOfAPoint()); - // The objects (without margins) intersect. Therefore, we run the GJK algorithm - // again but on the enlarged objects to compute a simplex polytope that contains - // the origin. Then, we give that simplex polytope to the EPA algorithm to compute - // the correct penetration depth and contact points between the enlarged objects. - return computePenetrationDepthForEnlargedObjects(shape1Info, transform1, shape2Info, - transform2, narrowPhaseCallback, v); + // The objects (without margins) int32_tersect. Therefore, we run the GJK algorithm + // again but on the enlarged objects to compute a simplex polytope that contains + // the origin. Then, we give that simplex polytope to the EPA algorithm to compute + // the correct penetration depth and contact points between the enlarged objects. + return computePenetrationDepthForEnlargedObjects(shape1Info, transform1, shape2Info, + transform2, narrowPhaseCallback, v); } /// This method runs the GJK algorithm on the two enlarged objects (with margin) /// to compute a simplex polytope that contains the origin. The two objects are -/// assumed to intersect in the original objects (without margin). Therefore such +/// assumed to int32_tersect in the original objects (without margin). Therefore such /// a polytope must exist. Then, we give that polytope to the EPA algorithm to /// compute the correct penetration depth and contact points of the enlarged objects. void GJKAlgorithm::computePenetrationDepthForEnlargedObjects(const CollisionShapeInfo& shape1Info, - const Transform& transform1, - const CollisionShapeInfo& shape2Info, - const Transform& transform2, - NarrowPhaseCallback* narrowPhaseCallback, - Vector3& v) { - PROFILE("GJKAlgorithm::computePenetrationDepthForEnlargedObjects()"); + const Transform& transform1, + const CollisionShapeInfo& shape2Info, + const Transform& transform2, + NarrowPhaseCallback* narrowPhaseCallback, + Vector3& v) { + PROFILE("GJKAlgorithm::computePenetrationDepthForEnlargedObjects()"); - Simplex simplex; - Vector3 suppA; - Vector3 suppB; - Vector3 w; - decimal vDotw; - decimal distSquare = DECIMAL_LARGEST; - decimal prevDistSquare; + Simplex simplex; + Vector3 suppA; + Vector3 suppB; + Vector3 w; + float vDotw; + float distSquare = DECIMAL_LARGEST; + float prevDistSquare; - assert(shape1Info.collisionShape->isConvex()); - assert(shape2Info.collisionShape->isConvex()); + assert(shape1Info.collisionShape->isConvex()); + assert(shape2Info.collisionShape->isConvex()); - const ConvexShape* shape1 = static_cast(shape1Info.collisionShape); - const ConvexShape* shape2 = static_cast(shape2Info.collisionShape); + const ConvexShape* shape1 = static_cast(shape1Info.collisionShape); + const ConvexShape* shape2 = static_cast(shape2Info.collisionShape); - void** shape1CachedCollisionData = shape1Info.cachedCollisionData; - void** shape2CachedCollisionData = shape2Info.cachedCollisionData; + void** shape1CachedCollisionData = shape1Info.cachedCollisionData; + void** shape2CachedCollisionData = shape2Info.cachedCollisionData; - // 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) - Transform body2ToBody1 = transform1.getInverse() * transform2; + // 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) + Transform body2ToBody1 = transform1.getInverse() * transform2; - // Matrix that transform a direction from local space of body 1 into local space of body 2 - Matrix3x3 rotateToBody2 = transform2.getOrientation().getMatrix().getTranspose() * - transform1.getOrientation().getMatrix(); - - do { - // Compute the support points for the enlarged object A and B - suppA = shape1->getLocalSupportPointWithMargin(-v, shape1CachedCollisionData); - suppB = body2ToBody1 * shape2->getLocalSupportPointWithMargin(rotateToBody2 * v, shape2CachedCollisionData); + // Matrix that transform a direction from local space of body 1 int32_to local space of body 2 + Matrix3x3 rotateToBody2 = transform2.getOrientation().getMatrix().getTranspose() * + transform1.getOrientation().getMatrix(); + + do { + // Compute the support points for the enlarged object A and B + suppA = shape1->getLocalSupportPointWithMargin(-v, shape1CachedCollisionData); + suppB = body2ToBody1 * shape2->getLocalSupportPointWithMargin(rotateToBody2 * v, shape2CachedCollisionData); - // Compute the support point for the Minkowski difference A-B - w = suppA - suppB; + // Compute the support point for the Minkowski difference A-B + w = suppA - suppB; - vDotw = v.dot(w); + vDotw = v.dot(w); - // If the enlarge objects do not intersect - if (vDotw > 0.0) { + // If the enlarge objects do not int32_tersect + if (vDotw > 0.0) { - // No intersection, we return - return; - } + // No int32_tersection, we return + return; + } - // Add the new support point to the simplex - simplex.addPoint(w, suppA, suppB); + // Add the new support point to the simplex + simplex.addPoint(w, suppA, suppB); - if (simplex.isAffinelyDependent()) { - return; - } + if (simplex.isAffinelyDependent()) { + return; + } - if (!simplex.computeClosestPoint(v)) { - return; - } + if (!simplex.computeClosestPoint(v)) { + return; + } - // Store and update the square distance - prevDistSquare = distSquare; - distSquare = v.lengthSquare(); + // Store and update the square distance + prevDistSquare = distSquare; + distSquare = v.lengthSquare(); - if (prevDistSquare - distSquare <= MACHINE_EPSILON * prevDistSquare) { - return; - } + if (prevDistSquare - distSquare <= MACHINE_EPSILON * prevDistSquare) { + return; + } - } while(!simplex.isFull() && distSquare > MACHINE_EPSILON * - simplex.getMaxLengthSquareOfAPoint()); + } while(!simplex.isFull() && distSquare > MACHINE_EPSILON * + simplex.getMaxLengthSquareOfAPoint()); - // Give the simplex computed with GJK algorithm to the EPA algorithm - // which will compute the correct penetration depth and contact points - // between the two enlarged objects - return mAlgoEPA.computePenetrationDepthAndContactPoints(simplex, shape1Info, - transform1, shape2Info, transform2, - v, narrowPhaseCallback); + // Give the simplex computed with GJK algorithm to the EPA algorithm + // which will compute the correct penetration depth and contact points + // between the two enlarged objects + return mAlgoEPA.computePenetrationDepthAndContactPoints(simplex, shape1Info, + transform1, shape2Info, transform2, + v, narrowPhaseCallback); } // Use the GJK Algorithm to find if a point is inside a convex collision shape bool GJKAlgorithm::testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) { - Vector3 suppA; // Support point of object A - Vector3 w; // Support point of Minkowski difference A-B - decimal prevDistSquare; + Vector3 suppA; // Support point of object A + Vector3 w; // Support point of Minkowski difference A-B + float prevDistSquare; - assert(proxyShape->getCollisionShape()->isConvex()); + assert(proxyShape->getCollisionShape()->isConvex()); - const ConvexShape* shape = static_cast(proxyShape->getCollisionShape()); + const ConvexShape* shape = static_cast(proxyShape->getCollisionShape()); - void** shapeCachedCollisionData = proxyShape->getCachedCollisionData(); + void** shapeCachedCollisionData = proxyShape->getCachedCollisionData(); - // Support point of object B (object B is a single point) - const Vector3 suppB(localPoint); + // Support point of object B (object B is a single point) + const Vector3 suppB(localPoint); - // Create a simplex set - Simplex simplex; + // Create a simplex set + Simplex simplex; - // Initial supporting direction - Vector3 v(1, 1, 1); + // Initial supporting direction + Vector3 v(1, 1, 1); - // Initialize the upper bound for the square distance - decimal distSquare = DECIMAL_LARGEST; + // Initialize the upper bound for the square distance + float distSquare = DECIMAL_LARGEST; - do { + do { - // Compute the support points for original objects (without margins) A and B - suppA = shape->getLocalSupportPointWithoutMargin(-v, shapeCachedCollisionData); + // Compute the support points for original objects (without margins) A and B + suppA = shape->getLocalSupportPointWithoutMargin(-v, shapeCachedCollisionData); - // Compute the support point for the Minkowski difference A-B - w = suppA - suppB; + // Compute the support point for the Minkowski difference A-B + w = suppA - suppB; - // Add the new support point to the simplex - simplex.addPoint(w, suppA, suppB); + // Add the new support point to the simplex + simplex.addPoint(w, suppA, suppB); - // If the simplex is affinely dependent - if (simplex.isAffinelyDependent()) { + // If the simplex is affinely dependent + if (simplex.isAffinelyDependent()) { - return false; - } + return false; + } - // Compute the point of the simplex closest to the origin - // If the computation of the closest point fail - if (!simplex.computeClosestPoint(v)) { + // Compute the point of the simplex closest to the origin + // If the computation of the closest point fail + if (!simplex.computeClosestPoint(v)) { - return false; - } + return false; + } - // Store and update the squared distance of the closest point - prevDistSquare = distSquare; - distSquare = v.lengthSquare(); + // Store and update the squared distance of the closest point + prevDistSquare = distSquare; + distSquare = v.lengthSquare(); - // If the distance to the closest point doesn't improve a lot - if (prevDistSquare - distSquare <= MACHINE_EPSILON * prevDistSquare) { + // If the distance to the closest point doesn't improve a lot + if (prevDistSquare - distSquare <= MACHINE_EPSILON * prevDistSquare) { - return false; - } - } while(!simplex.isFull() && distSquare > MACHINE_EPSILON * - simplex.getMaxLengthSquareOfAPoint()); + return false; + } + } while(!simplex.isFull() && distSquare > MACHINE_EPSILON * + simplex.getMaxLengthSquareOfAPoint()); - // The point is inside the collision shape - return true; + // The point is inside the collision shape + return true; } @@ -426,103 +407,103 @@ bool GJKAlgorithm::testPointInside(const Vector3& localPoint, ProxyShape* proxyS /// "Ray Casting against General Convex Objects with Application to Continuous Collision Detection". bool GJKAlgorithm::raycast(const Ray& ray, ProxyShape* proxyShape, RaycastInfo& raycastInfo) { - assert(proxyShape->getCollisionShape()->isConvex()); + assert(proxyShape->getCollisionShape()->isConvex()); - const ConvexShape* shape = static_cast(proxyShape->getCollisionShape()); + const ConvexShape* shape = static_cast(proxyShape->getCollisionShape()); - void** shapeCachedCollisionData = proxyShape->getCachedCollisionData(); + void** shapeCachedCollisionData = proxyShape->getCachedCollisionData(); - Vector3 suppA; // Current lower bound point on the ray (starting at ray's origin) - Vector3 suppB; // Support point on the collision shape - const decimal machineEpsilonSquare = MACHINE_EPSILON * MACHINE_EPSILON; - const decimal epsilon = decimal(0.0001); + Vector3 suppA; // Current lower bound point on the ray (starting at ray's origin) + Vector3 suppB; // Support point on the collision shape + const float machineEpsilonSquare = MACHINE_EPSILON * MACHINE_EPSILON; + const float epsilon = float(0.0001); - // Convert the ray origin and direction into the local-space of the collision shape - Vector3 rayDirection = ray.point2 - ray.point1; + // Convert the ray origin and direction int32_to the local-space of the collision shape + Vector3 rayDirection = ray.point2 - ray.point1; - // If the points of the segment are two close, return no hit - if (rayDirection.lengthSquare() < machineEpsilonSquare) return false; + // If the points of the segment are two close, return no hit + if (rayDirection.lengthSquare() < machineEpsilonSquare) return false; - Vector3 w; + Vector3 w; - // Create a simplex set - Simplex simplex; + // Create a simplex set + Simplex simplex; - Vector3 n(decimal(0.0), decimal(0.0), decimal(0.0)); - decimal lambda = decimal(0.0); - suppA = ray.point1; // Current lower bound point on the ray (starting at ray's origin) - suppB = shape->getLocalSupportPointWithoutMargin(rayDirection, shapeCachedCollisionData); - Vector3 v = suppA - suppB; - decimal vDotW, vDotR; - decimal distSquare = v.lengthSquare(); - int nbIterations = 0; + Vector3 n(float(0.0), float(0.0), float(0.0)); + float lambda = float(0.0); + suppA = ray.point1; // Current lower bound point on the ray (starting at ray's origin) + suppB = shape->getLocalSupportPointWithoutMargin(rayDirection, shapeCachedCollisionData); + Vector3 v = suppA - suppB; + float vDotW, vDotR; + float distSquare = v.lengthSquare(); + int32_t nbIterations = 0; - // GJK Algorithm loop - while (distSquare > epsilon && nbIterations < MAX_ITERATIONS_GJK_RAYCAST) { + // GJK Algorithm loop + while (distSquare > epsilon && nbIterations < MAX_ITERATIONS_GJK_RAYCAST) { - // Compute the support points - suppB = shape->getLocalSupportPointWithoutMargin(v, shapeCachedCollisionData); - w = suppA - suppB; + // Compute the support points + suppB = shape->getLocalSupportPointWithoutMargin(v, shapeCachedCollisionData); + w = suppA - suppB; - vDotW = v.dot(w); + vDotW = v.dot(w); - if (vDotW > decimal(0)) { + if (vDotW > float(0)) { - vDotR = v.dot(rayDirection); + vDotR = v.dot(rayDirection); - if (vDotR >= -machineEpsilonSquare) { - return false; - } - else { + if (vDotR >= -machineEpsilonSquare) { + return false; + } + else { - // We have found a better lower bound for the hit point along the ray - lambda = lambda - vDotW / vDotR; - suppA = ray.point1 + lambda * rayDirection; - w = suppA - suppB; - n = v; - } - } + // We have found a better lower bound for the hit point along the ray + lambda = lambda - vDotW / vDotR; + suppA = ray.point1 + lambda * rayDirection; + w = suppA - suppB; + n = v; + } + } - // Add the new support point to the simplex - if (!simplex.isPointInSimplex(w)) { - simplex.addPoint(w, suppA, suppB); - } + // Add the new support point to the simplex + if (!simplex.isPointInSimplex(w)) { + simplex.addPoint(w, suppA, suppB); + } - // Compute the closest point - if (simplex.computeClosestPoint(v)) { + // Compute the closest point + if (simplex.computeClosestPoint(v)) { - distSquare = v.lengthSquare(); - } - else { - distSquare = decimal(0.0); - } + distSquare = v.lengthSquare(); + } + else { + distSquare = float(0.0); + } - // If the current lower bound distance is larger than the maximum raycasting distance - if (lambda > ray.maxFraction) return false; + // If the current lower bound distance is larger than the maximum raycasting distance + if (lambda > ray.maxFraction) return false; - nbIterations++; - } + nbIterations++; + } - // If the origin was inside the shape, we return no hit - if (lambda < MACHINE_EPSILON) return false; + // If the origin was inside the shape, we return no hit + if (lambda < MACHINE_EPSILON) return false; - // Compute the closet points of both objects (without the margins) - Vector3 pointA; - Vector3 pointB; - simplex.computeClosestPointsOfAandB(pointA, pointB); + // Compute the closet points of both objects (without the margins) + Vector3 pointA; + Vector3 pointB; + simplex.computeClosestPointsOfAandB(pointA, pointB); - // A raycast hit has been found, we fill in the raycast info - raycastInfo.hitFraction = lambda; - raycastInfo.worldPoint = pointB; - raycastInfo.body = proxyShape->getBody(); - raycastInfo.proxyShape = proxyShape; + // A raycast hit has been found, we fill in the raycast info + raycastInfo.hitFraction = lambda; + raycastInfo.worldPoint = pointB; + raycastInfo.body = proxyShape->getBody(); + raycastInfo.proxyShape = proxyShape; - if (n.lengthSquare() >= machineEpsilonSquare) { // The normal vector is valid - raycastInfo.worldNormal = n; - } - else { // Degenerated normal vector, we return a zero normal vector - raycastInfo.worldNormal = Vector3(decimal(0), decimal(0), decimal(0)); - } + if (n.lengthSquare() >= machineEpsilonSquare) { // The normal vector is valid + raycastInfo.worldNormal = n; + } + else { // Degenerated normal vector, we return a zero normal vector + raycastInfo.worldNormal = Vector3(float(0), float(0), float(0)); + } - return true; + return true; } diff --git a/ephysics/collision/narrowphase/GJK/GJKAlgorithm.h b/ephysics/collision/narrowphase/GJK/GJKAlgorithm.h index 401c47d..7ea1809 100644 --- a/ephysics/collision/narrowphase/GJK/GJKAlgorithm.h +++ b/ephysics/collision/narrowphase/GJK/GJKAlgorithm.h @@ -1,30 +1,9 @@ -/******************************************************************************** -* 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 REACTPHYSICS3D_GJK_ALGORITHM_H -#define REACTPHYSICS3D_GJK_ALGORITHM_H +/** @file + * @author Daniel Chappuis + * @copyright 2010-2016 Daniel Chappuis + * @license BSD 3 clauses (see license file) + */ +#pragma once // Libraries #include @@ -37,9 +16,9 @@ namespace reactphysics3d { // Constants -const decimal REL_ERROR = decimal(1.0e-3); -const decimal REL_ERROR_SQUARE = REL_ERROR * REL_ERROR; -const int MAX_ITERATIONS_GJK_RAYCAST = 32; +const float REL_ERROR = float(1.0e-3); +const float REL_ERROR_SQUARE = REL_ERROR * REL_ERROR; +const int32_t MAX_ITERATIONS_GJK_RAYCAST = 32; // Class GJKAlgorithm /** @@ -49,9 +28,9 @@ const int MAX_ITERATIONS_GJK_RAYCAST = 32; * "Collision Detection in Interactive 3D Environments" by Gino van den Bergen. * This method implements the Hybrid Technique for calculating the * penetration depth. The two objects are enlarged with a small margin. If - * the object intersects in their margins, the penetration depth is quickly + * the object int32_tersects in their margins, the penetration depth is quickly * computed using the GJK algorithm on the original objects (without margin). - * If the original objects (without margin) intersect, we run again the GJK + * If the original objects (without margin) int32_tersect, we run again the GJK * algorithm on the enlarged objects (with margin) to compute simplex * polytope that contains the origin and give it to the EPA (Expanding * Polytope Algorithm) to compute the correct penetration depth between the @@ -59,62 +38,61 @@ const int MAX_ITERATIONS_GJK_RAYCAST = 32; */ class GJKAlgorithm : public NarrowPhaseAlgorithm { - private : + private : - // -------------------- Attributes -------------------- // + // -------------------- Attributes -------------------- // - /// EPA Algorithm - EPAAlgorithm mAlgoEPA; + /// EPA Algorithm + EPAAlgorithm mAlgoEPA; - // -------------------- Methods -------------------- // + // -------------------- Methods -------------------- // - /// Private copy-constructor - GJKAlgorithm(const GJKAlgorithm& algorithm); + /// Private copy-constructor + GJKAlgorithm(const GJKAlgorithm& algorithm); - /// Private assignment operator - GJKAlgorithm& operator=(const GJKAlgorithm& algorithm); + /// Private assignment operator + GJKAlgorithm& operator=(const GJKAlgorithm& algorithm); - /// Compute the penetration depth for enlarged objects. - void computePenetrationDepthForEnlargedObjects(const CollisionShapeInfo& shape1Info, - const Transform& transform1, - const CollisionShapeInfo& shape2Info, - const Transform& transform2, - NarrowPhaseCallback* narrowPhaseCallback, - Vector3& v); + /// Compute the penetration depth for enlarged objects. + void computePenetrationDepthForEnlargedObjects(const CollisionShapeInfo& shape1Info, + const Transform& transform1, + const CollisionShapeInfo& shape2Info, + const Transform& transform2, + NarrowPhaseCallback* narrowPhaseCallback, + Vector3& v); - public : + public : - // -------------------- Methods -------------------- // + // -------------------- Methods -------------------- // - /// Constructor - GJKAlgorithm(); + /// Constructor + GJKAlgorithm(); - /// Destructor - ~GJKAlgorithm(); + /// Destructor + ~GJKAlgorithm(); - /// Initalize the algorithm - virtual void init(CollisionDetection* collisionDetection, - MemoryAllocator* memoryAllocator); + /// Initalize the algorithm + virtual void init(CollisionDetection* collisionDetection, + MemoryAllocator* memoryAllocator); - /// Compute a contact info if the two bounding volumes collide. - virtual void testCollision(const CollisionShapeInfo& shape1Info, - const CollisionShapeInfo& shape2Info, - NarrowPhaseCallback* narrowPhaseCallback); + /// Compute a contact info if the two bounding volumes collide. + virtual void testCollision(const CollisionShapeInfo& shape1Info, + const CollisionShapeInfo& shape2Info, + NarrowPhaseCallback* narrowPhaseCallback); - /// Use the GJK Algorithm to find if a point is inside a convex collision shape - bool testPointInside(const Vector3& localPoint, ProxyShape* proxyShape); + /// Use the GJK Algorithm to find if a point is inside a convex collision shape + bool testPointInside(const Vector3& localPoint, ProxyShape* proxyShape); - /// Ray casting algorithm agains a convex collision shape using the GJK Algorithm - bool raycast(const Ray& ray, ProxyShape* proxyShape, RaycastInfo& raycastInfo); + /// Ray casting algorithm agains a convex collision shape using the GJK Algorithm + bool raycast(const Ray& ray, ProxyShape* proxyShape, RaycastInfo& raycastInfo); }; // Initalize the algorithm inline void GJKAlgorithm::init(CollisionDetection* collisionDetection, - MemoryAllocator* memoryAllocator) { - NarrowPhaseAlgorithm::init(collisionDetection, memoryAllocator); - mAlgoEPA.init(memoryAllocator); + MemoryAllocator* memoryAllocator) { + NarrowPhaseAlgorithm::init(collisionDetection, memoryAllocator); + mAlgoEPA.init(memoryAllocator); } } -#endif diff --git a/ephysics/collision/narrowphase/GJK/Simplex.cpp b/ephysics/collision/narrowphase/GJK/Simplex.cpp index ca817e6..a6721e9 100644 --- a/ephysics/collision/narrowphase/GJK/Simplex.cpp +++ b/ephysics/collision/narrowphase/GJK/Simplex.cpp @@ -1,27 +1,8 @@ -/******************************************************************************** -* 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. * -* * -********************************************************************************/ +/** @file + * @author Daniel Chappuis + * @copyright 2010-2016 Daniel Chappuis + * @license BSD 3 clauses (see license file) + */ // Libraries #include @@ -40,278 +21,278 @@ Simplex::~Simplex() { } -// Add a new support point of (A-B) into the simplex +// Add a new support point of (A-B) int32_to the simplex /// suppPointA : support point of object A in a direction -v /// suppPointB : support point of object B in a direction v -/// point : support point of object (A-B) => point = suppPointA - suppPointB +/// point : support point of object (A-B) => point = suppPointA - suppPointB void Simplex::addPoint(const Vector3& point, const Vector3& suppPointA, const Vector3& suppPointB) { - assert(!isFull()); + assert(!isFull()); - mLastFound = 0; - mLastFoundBit = 0x1; + mLastFound = 0; + mLastFoundBit = 0x1; - // Look for the bit corresponding to one of the four point that is not in - // the current simplex - while (overlap(mBitsCurrentSimplex, mLastFoundBit)) { - mLastFound++; - mLastFoundBit <<= 1; - } + // Look for the bit corresponding to one of the four point that is not in + // the current simplex + while (overlap(mBitsCurrentSimplex, mLastFoundBit)) { + mLastFound++; + mLastFoundBit <<= 1; + } - assert(mLastFound < 4); + assert(mLastFound < 4); - // Add the point into the simplex - mPoints[mLastFound] = point; - mPointsLengthSquare[mLastFound] = point.dot(point); - mAllBits = mBitsCurrentSimplex | mLastFoundBit; + // Add the point int32_to the simplex + mPoints[mLastFound] = point; + mPointsLengthSquare[mLastFound] = point.dot(point); + mAllBits = mBitsCurrentSimplex | mLastFoundBit; - // Update the cached values - updateCache(); - - // Compute the cached determinant values - computeDeterminants(); - - // Add the support points of objects A and B - mSuppPointsA[mLastFound] = suppPointA; - mSuppPointsB[mLastFound] = suppPointB; + // Update the cached values + updateCache(); + + // Compute the cached determinant values + computeDeterminants(); + + // Add the support points of objects A and B + mSuppPointsA[mLastFound] = suppPointA; + mSuppPointsB[mLastFound] = suppPointB; } // Return true if the point is in the simplex bool Simplex::isPointInSimplex(const Vector3& point) const { - int i; - Bits bit; + int32_t i; + Bits bit; - // For each four possible points in the simplex - for (i=0, bit = 0x1; i<4; i++, bit <<= 1) { - // Check if the current point is in the simplex - if (overlap(mAllBits, bit) && point == mPoints[i]) return true; - } + // For each four possible points in the simplex + for (i=0, bit = 0x1; i<4; i++, bit <<= 1) { + // Check if the current point is in the simplex + if (overlap(mAllBits, bit) && point == mPoints[i]) return true; + } - return false; + return false; } // Update the cached values used during the GJK algorithm void Simplex::updateCache() { - int i; - Bits bit; + int32_t i; + Bits bit; - // For each of the four possible points of the simplex - for (i=0, bit = 0x1; i<4; i++, bit <<= 1) { - // If the current points is in the simplex - if (overlap(mBitsCurrentSimplex, bit)) { - - // Compute the distance between two points in the possible simplex set - mDiffLength[i][mLastFound] = mPoints[i] - mPoints[mLastFound]; - mDiffLength[mLastFound][i] = -mDiffLength[i][mLastFound]; + // For each of the four possible points of the simplex + for (i=0, bit = 0x1; i<4; i++, bit <<= 1) { + // If the current points is in the simplex + if (overlap(mBitsCurrentSimplex, bit)) { + + // Compute the distance between two points in the possible simplex set + mDiffLength[i][mLastFound] = mPoints[i] - mPoints[mLastFound]; + mDiffLength[mLastFound][i] = -mDiffLength[i][mLastFound]; - // Compute the squared length of the vector - // distances from points in the possible simplex set - mNormSquare[i][mLastFound] = mNormSquare[mLastFound][i] = - mDiffLength[i][mLastFound].dot(mDiffLength[i][mLastFound]); - } - } + // Compute the squared length of the vector + // distances from points in the possible simplex set + mNormSquare[i][mLastFound] = mNormSquare[mLastFound][i] = + mDiffLength[i][mLastFound].dot(mDiffLength[i][mLastFound]); + } + } } // Return the points of the simplex -unsigned int Simplex::getSimplex(Vector3* suppPointsA, Vector3* suppPointsB, - Vector3* points) const { - unsigned int nbVertices = 0; - int i; - Bits bit; +uint32_t Simplex::getSimplex(Vector3* suppPointsA, Vector3* suppPointsB, + Vector3* points) const { + uint32_t nbVertices = 0; + int32_t i; + Bits bit; - // For each four point in the possible simplex - for (i=0, bit=0x1; i<4; i++, bit <<=1) { + // For each four point in the possible simplex + for (i=0, bit=0x1; i<4; i++, bit <<=1) { - // If the current point is in the simplex - if (overlap(mBitsCurrentSimplex, bit)) { + // If the current point is in the simplex + if (overlap(mBitsCurrentSimplex, bit)) { - // Store the points - suppPointsA[nbVertices] = this->mSuppPointsA[nbVertices]; - suppPointsB[nbVertices] = this->mSuppPointsB[nbVertices]; - points[nbVertices] = this->mPoints[nbVertices]; + // Store the points + suppPointsA[nbVertices] = this->mSuppPointsA[nbVertices]; + suppPointsB[nbVertices] = this->mSuppPointsB[nbVertices]; + points[nbVertices] = this->mPoints[nbVertices]; - nbVertices++; - } - } + nbVertices++; + } + } - // Return the number of points in the simplex - return nbVertices; + // Return the number of points in the simplex + return nbVertices; } // Compute the cached determinant values void Simplex::computeDeterminants() { - mDet[mLastFoundBit][mLastFound] = 1.0; + mDet[mLastFoundBit][mLastFound] = 1.0; - // If the current simplex is not empty - if (!isEmpty()) { - int i; - Bits bitI; + // If the current simplex is not empty + if (!isEmpty()) { + int32_t i; + Bits bitI; - // For each possible four points in the simplex set - for (i=0, bitI = 0x1; i<4; i++, bitI <<= 1) { + // For each possible four points in the simplex set + for (i=0, bitI = 0x1; i<4; i++, bitI <<= 1) { - // If the current point is in the simplex - if (overlap(mBitsCurrentSimplex, bitI)) { - Bits bit2 = bitI | mLastFoundBit; + // If the current point is in the simplex + if (overlap(mBitsCurrentSimplex, bitI)) { + Bits bit2 = bitI | mLastFoundBit; - mDet[bit2][i] = mDiffLength[mLastFound][i].dot(mPoints[mLastFound]); - mDet[bit2][mLastFound] = mDiffLength[i][mLastFound].dot(mPoints[i]); - + mDet[bit2][i] = mDiffLength[mLastFound][i].dot(mPoints[mLastFound]); + mDet[bit2][mLastFound] = mDiffLength[i][mLastFound].dot(mPoints[i]); + - int j; - Bits bitJ; + int32_t j; + Bits bitJ; - for (j=0, bitJ = 0x1; j 0 for each "i" in I_x and -/// 2. delta(X U {y_j})_j <= 0 for each "j" not in I_x_ +/// 1. delta(X)_i > 0 for each "i" in I_x and +/// 2. delta(X U {y_j})_j <= 0 for each "j" not in I_x_ bool Simplex::isValidSubset(Bits subset) const { - int i; - Bits bit; + int32_t i; + Bits bit; - // For each four point in the possible simplex set - for (i=0, bit=0x1; i<4; i++, bit <<= 1) { - if (overlap(mAllBits, bit)) { - // If the current point is in the subset - if (overlap(subset, bit)) { - // If one delta(X)_i is smaller or equal to zero - if (mDet[subset][i] <= 0.0) { - // The subset is not valid - return false; - } - } - // If the point is not in the subset and the value delta(X U {y_j})_j - // is bigger than zero - else if (mDet[subset | bit][i] > 0.0) { - // The subset is not valid - return false; - } - } - } + // For each four point in the possible simplex set + for (i=0, bit=0x1; i<4; i++, bit <<= 1) { + if (overlap(mAllBits, bit)) { + // If the current point is in the subset + if (overlap(subset, bit)) { + // If one delta(X)_i is smaller or equal to zero + if (mDet[subset][i] <= 0.0) { + // The subset is not valid + return false; + } + } + // If the point is not in the subset and the value delta(X U {y_j})_j + // is bigger than zero + else if (mDet[subset | bit][i] > 0.0) { + // The subset is not valid + return false; + } + } + } - return true; + return true; } // Compute the closest points "pA" and "pB" of object A and B. /// The points are computed as follows : -/// pA = sum(lambda_i * a_i) where "a_i" are the support points of object A -/// pB = sum(lambda_i * b_i) where "b_i" are the support points of object B -/// with lambda_i = deltaX_i / deltaX +/// pA = sum(lambda_i * a_i) where "a_i" are the support points of object A +/// pB = sum(lambda_i * b_i) where "b_i" are the support points of object B +/// with lambda_i = deltaX_i / deltaX void Simplex::computeClosestPointsOfAandB(Vector3& pA, Vector3& pB) const { - decimal deltaX = 0.0; - pA.setAllValues(0.0, 0.0, 0.0); - pB.setAllValues(0.0, 0.0, 0.0); - int i; - Bits bit; + float deltaX = 0.0; + pA.setAllValues(0.0, 0.0, 0.0); + pB.setAllValues(0.0, 0.0, 0.0); + int32_t i; + Bits bit; - // For each four points in the possible simplex set - for (i=0, bit=0x1; i<4; i++, bit <<= 1) { - // If the current point is part of the simplex - if (overlap(mBitsCurrentSimplex, bit)) { - deltaX += mDet[mBitsCurrentSimplex][i]; - pA += mDet[mBitsCurrentSimplex][i] * mSuppPointsA[i]; - pB += mDet[mBitsCurrentSimplex][i] * mSuppPointsB[i]; - } - } + // For each four points in the possible simplex set + for (i=0, bit=0x1; i<4; i++, bit <<= 1) { + // If the current point is part of the simplex + if (overlap(mBitsCurrentSimplex, bit)) { + deltaX += mDet[mBitsCurrentSimplex][i]; + pA += mDet[mBitsCurrentSimplex][i] * mSuppPointsA[i]; + pB += mDet[mBitsCurrentSimplex][i] * mSuppPointsB[i]; + } + } - assert(deltaX > 0.0); - decimal factor = decimal(1.0) / deltaX; - pA *= factor; - pB *= factor; + assert(deltaX > 0.0); + float factor = float(1.0) / deltaX; + pA *= factor; + pB *= factor; } // Compute the closest point "v" to the origin of the current simplex. @@ -319,76 +300,76 @@ void Simplex::computeClosestPointsOfAandB(Vector3& pA, Vector3& pB) const { /// "v" of simplex that is closest to the origin. The method returns true /// if a closest point has been found. bool Simplex::computeClosestPoint(Vector3& v) { - Bits subset; + Bits subset; - // For each possible simplex set - for (subset=mBitsCurrentSimplex; subset != 0x0; subset--) { - // If the simplex is a subset of the current simplex and is valid for the Johnson's - // algorithm test - if (isSubset(subset, mBitsCurrentSimplex) && isValidSubset(subset | mLastFoundBit)) { - mBitsCurrentSimplex = subset | mLastFoundBit; // Add the last added point to the current simplex - v = computeClosestPointForSubset(mBitsCurrentSimplex); // Compute the closest point in the simplex - return true; - } - } + // For each possible simplex set + for (subset=mBitsCurrentSimplex; subset != 0x0; subset--) { + // If the simplex is a subset of the current simplex and is valid for the Johnson's + // algorithm test + if (isSubset(subset, mBitsCurrentSimplex) && isValidSubset(subset | mLastFoundBit)) { + mBitsCurrentSimplex = subset | mLastFoundBit; // Add the last added point to the current simplex + v = computeClosestPointForSubset(mBitsCurrentSimplex); // Compute the closest point in the simplex + return true; + } + } - // If the simplex that contains only the last added point is valid for the Johnson's algorithm test - if (isValidSubset(mLastFoundBit)) { - mBitsCurrentSimplex = mLastFoundBit; // Set the current simplex to the set that contains only the last added point - mMaxLengthSquare = mPointsLengthSquare[mLastFound]; // Update the maximum square length - v = mPoints[mLastFound]; // The closest point of the simplex "v" is the last added point - return true; - } + // If the simplex that contains only the last added point is valid for the Johnson's algorithm test + if (isValidSubset(mLastFoundBit)) { + mBitsCurrentSimplex = mLastFoundBit; // Set the current simplex to the set that contains only the last added point + mMaxLengthSquare = mPointsLengthSquare[mLastFound]; // Update the maximum square length + v = mPoints[mLastFound]; // The closest point of the simplex "v" is the last added point + return true; + } - // The algorithm failed to found a point - return false; + // The algorithm failed to found a point + return false; } // Backup the closest point void Simplex::backupClosestPointInSimplex(Vector3& v) { - decimal minDistSquare = DECIMAL_LARGEST; - Bits bit; + float minDistSquare = DECIMAL_LARGEST; + Bits bit; - for (bit = mAllBits; bit != 0x0; bit--) { - if (isSubset(bit, mAllBits) && isProperSubset(bit)) { - Vector3 u = computeClosestPointForSubset(bit); - decimal distSquare = u.dot(u); - if (distSquare < minDistSquare) { - minDistSquare = distSquare; - mBitsCurrentSimplex = bit; - v = u; - } - } - } + for (bit = mAllBits; bit != 0x0; bit--) { + if (isSubset(bit, mAllBits) && isProperSubset(bit)) { + Vector3 u = computeClosestPointForSubset(bit); + float distSquare = u.dot(u); + if (distSquare < minDistSquare) { + minDistSquare = distSquare; + mBitsCurrentSimplex = bit; + v = u; + } + } + } } // Return the closest point "v" in the convex hull of the points in the subset // represented by the bits "subset" Vector3 Simplex::computeClosestPointForSubset(Bits subset) { - Vector3 v(0.0, 0.0, 0.0); // Closet point v = sum(lambda_i * points[i]) - mMaxLengthSquare = 0.0; - decimal deltaX = 0.0; // deltaX = sum of all det[subset][i] - int i; - Bits bit; + Vector3 v(0.0, 0.0, 0.0); // Closet point v = sum(lambda_i * points[i]) + mMaxLengthSquare = 0.0; + float deltaX = 0.0; // deltaX = sum of all det[subset][i] + int32_t i; + Bits bit; - // For each four point in the possible simplex set - for (i=0, bit=0x1; i<4; i++, bit <<= 1) { - // If the current point is in the subset - if (overlap(subset, bit)) { - // deltaX = sum of all det[subset][i] - deltaX += mDet[subset][i]; + // For each four point in the possible simplex set + for (i=0, bit=0x1; i<4; i++, bit <<= 1) { + // If the current point is in the subset + if (overlap(subset, bit)) { + // deltaX = sum of all det[subset][i] + deltaX += mDet[subset][i]; - if (mMaxLengthSquare < mPointsLengthSquare[i]) { - mMaxLengthSquare = mPointsLengthSquare[i]; - } + if (mMaxLengthSquare < mPointsLengthSquare[i]) { + mMaxLengthSquare = mPointsLengthSquare[i]; + } - // Closest point v = sum(lambda_i * points[i]) - v += mDet[subset][i] * mPoints[i]; - } - } + // Closest point v = sum(lambda_i * points[i]) + v += mDet[subset][i] * mPoints[i]; + } + } - assert(deltaX > 0.0); + assert(deltaX > 0.0); - // Return the closet point "v" in the convex hull for the given subset - return (decimal(1.0) / deltaX) * v; + // Return the closet point "v" in the convex hull for the given subset + return (float(1.0) / deltaX) * v; } diff --git a/ephysics/collision/narrowphase/GJK/Simplex.h b/ephysics/collision/narrowphase/GJK/Simplex.h index 733060b..e7208cb 100644 --- a/ephysics/collision/narrowphase/GJK/Simplex.h +++ b/ephysics/collision/narrowphase/GJK/Simplex.h @@ -1,30 +1,9 @@ -/******************************************************************************** -* 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 REACTPHYSICS3D_SIMPLEX_H -#define REACTPHYSICS3D_SIMPLEX_H +/** @file + * @author Daniel Chappuis + * @copyright 2010-2016 Daniel Chappuis + * @license BSD 3 clauses (see license file) + */ +#pragma once // Libraries #include @@ -34,7 +13,7 @@ namespace reactphysics3d { // Type definitions -typedef unsigned int Bits; +typedef uint32_t Bits; // Class Simplex /** @@ -47,143 +26,142 @@ typedef unsigned int Bits; */ class Simplex { - private: + private: - // -------------------- Attributes -------------------- // + // -------------------- Attributes -------------------- // - /// Current points - Vector3 mPoints[4]; + /// Current points + Vector3 mPoints[4]; - /// pointsLengthSquare[i] = (points[i].length)^2 - decimal mPointsLengthSquare[4]; + /// pointsLengthSquare[i] = (points[i].length)^2 + float mPointsLengthSquare[4]; - /// Maximum length of pointsLengthSquare[i] - decimal mMaxLengthSquare; + /// Maximum length of pointsLengthSquare[i] + float mMaxLengthSquare; - /// Support points of object A in local coordinates - Vector3 mSuppPointsA[4]; + /// Support points of object A in local coordinates + Vector3 mSuppPointsA[4]; - /// Support points of object B in local coordinates - Vector3 mSuppPointsB[4]; + /// Support points of object B in local coordinates + Vector3 mSuppPointsB[4]; - /// diff[i][j] contains points[i] - points[j] - Vector3 mDiffLength[4][4]; + /// diff[i][j] contains points[i] - points[j] + Vector3 mDiffLength[4][4]; - /// Cached determinant values - decimal mDet[16][4]; + /// Cached determinant values + float mDet[16][4]; - /// norm[i][j] = (diff[i][j].length())^2 - decimal mNormSquare[4][4]; + /// norm[i][j] = (diff[i][j].length())^2 + float mNormSquare[4][4]; - /// 4 bits that identify the current points of the simplex - /// For instance, 0101 means that points[1] and points[3] are in the simplex - Bits mBitsCurrentSimplex; + /// 4 bits that identify the current points of the simplex + /// For instance, 0101 means that points[1] and points[3] are in the simplex + Bits mBitsCurrentSimplex; - /// Number between 1 and 4 that identify the last found support point - Bits mLastFound; + /// Number between 1 and 4 that identify the last found support point + Bits mLastFound; - /// Position of the last found support point (lastFoundBit = 0x1 << lastFound) - Bits mLastFoundBit; + /// Position of the last found support point (lastFoundBit = 0x1 << lastFound) + Bits mLastFoundBit; - /// allBits = bitsCurrentSimplex | lastFoundBit; - Bits mAllBits; + /// allBits = bitsCurrentSimplex | lastFoundBit; + Bits mAllBits; - // -------------------- Methods -------------------- // + // -------------------- Methods -------------------- // - /// Private copy-constructor - Simplex(const Simplex& simplex); + /// Private copy-constructor + Simplex(const Simplex& simplex); - /// Private assignment operator - Simplex& operator=(const Simplex& simplex); + /// Private assignment operator + Simplex& operator=(const Simplex& simplex); - /// Return true if some bits of "a" overlap with bits of "b" - bool overlap(Bits a, Bits b) const; + /// Return true if some bits of "a" overlap with bits of "b" + bool overlap(Bits a, Bits b) const; - /// Return true if the bits of "b" is a subset of the bits of "a" - bool isSubset(Bits a, Bits b) const; + /// Return true if the bits of "b" is a subset of the bits of "a" + bool isSubset(Bits a, Bits b) const; - /// Return true if the subset is a valid one for the closest point computation. - bool isValidSubset(Bits subset) const; + /// Return true if the subset is a valid one for the closest point computation. + bool isValidSubset(Bits subset) const; - /// Return true if the subset is a proper subset. - bool isProperSubset(Bits subset) const; + /// Return true if the subset is a proper subset. + bool isProperSubset(Bits subset) const; - /// Update the cached values used during the GJK algorithm - void updateCache(); + /// Update the cached values used during the GJK algorithm + void updateCache(); - /// Compute the cached determinant values - void computeDeterminants(); + /// Compute the cached determinant values + void computeDeterminants(); - /// Return the closest point "v" in the convex hull of a subset of points - Vector3 computeClosestPointForSubset(Bits subset); + /// Return the closest point "v" in the convex hull of a subset of points + Vector3 computeClosestPointForSubset(Bits subset); - public: + public: - // -------------------- Methods -------------------- // + // -------------------- Methods -------------------- // - /// Constructor - Simplex(); + /// Constructor + Simplex(); - /// Destructor - ~Simplex(); + /// Destructor + ~Simplex(); - /// Return true if the simplex contains 4 points - bool isFull() const; + /// Return true if the simplex contains 4 points + bool isFull() const; - /// Return true if the simplex is empty - bool isEmpty() const; + /// Return true if the simplex is empty + bool isEmpty() const; - /// Return the points of the simplex - unsigned int getSimplex(Vector3* mSuppPointsA, Vector3* mSuppPointsB, - Vector3* mPoints) const; + /// Return the points of the simplex + uint32_t getSimplex(Vector3* mSuppPointsA, Vector3* mSuppPointsB, + Vector3* mPoints) const; - /// Return the maximum squared length of a point - decimal getMaxLengthSquareOfAPoint() const; + /// Return the maximum squared length of a point + float getMaxLengthSquareOfAPoint() const; - /// Add a new support point of (A-B) into the simplex. - void addPoint(const Vector3& point, const Vector3& suppPointA, const Vector3& suppPointB); + /// Add a new support point of (A-B) int32_to the simplex. + void addPoint(const Vector3& point, const Vector3& suppPointA, const Vector3& suppPointB); - /// Return true if the point is in the simplex - bool isPointInSimplex(const Vector3& point) const; + /// Return true if the point is in the simplex + bool isPointInSimplex(const Vector3& point) const; - /// Return true if the set is affinely dependent - bool isAffinelyDependent() const; + /// Return true if the set is affinely dependent + bool isAffinelyDependent() const; - /// Backup the closest point - void backupClosestPointInSimplex(Vector3& point); + /// Backup the closest point + void backupClosestPointInSimplex(Vector3& point); - /// Compute the closest points "pA" and "pB" of object A and B. - void computeClosestPointsOfAandB(Vector3& pA, Vector3& pB) const; + /// Compute the closest points "pA" and "pB" of object A and B. + void computeClosestPointsOfAandB(Vector3& pA, Vector3& pB) const; - /// Compute the closest point to the origin of the current simplex. - bool computeClosestPoint(Vector3& v); + /// Compute the closest point to the origin of the current simplex. + bool computeClosestPoint(Vector3& v); }; // Return true if some bits of "a" overlap with bits of "b" inline bool Simplex::overlap(Bits a, Bits b) const { - return ((a & b) != 0x0); + return ((a & b) != 0x0); } // Return true if the bits of "b" is a subset of the bits of "a" inline bool Simplex::isSubset(Bits a, Bits b) const { - return ((a & b) == a); + return ((a & b) == a); } // Return true if the simplex contains 4 points inline bool Simplex::isFull() const { - return (mBitsCurrentSimplex == 0xf); + return (mBitsCurrentSimplex == 0xf); } // Return true if the simple is empty inline bool Simplex::isEmpty() const { - return (mBitsCurrentSimplex == 0x0); + return (mBitsCurrentSimplex == 0x0); } // Return the maximum squared length of a point -inline decimal Simplex::getMaxLengthSquareOfAPoint() const { - return mMaxLengthSquare; +inline float Simplex::getMaxLengthSquareOfAPoint() const { + return mMaxLengthSquare; } } -#endif diff --git a/ephysics/collision/narrowphase/NarrowPhaseAlgorithm.cpp b/ephysics/collision/narrowphase/NarrowPhaseAlgorithm.cpp index 0c86a5d..064ac2f 100644 --- a/ephysics/collision/narrowphase/NarrowPhaseAlgorithm.cpp +++ b/ephysics/collision/narrowphase/NarrowPhaseAlgorithm.cpp @@ -1,27 +1,8 @@ -/******************************************************************************** -* 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. * -* * -********************************************************************************/ +/** @file + * @author Daniel Chappuis + * @copyright 2010-2016 Daniel Chappuis + * @license BSD 3 clauses (see license file) + */ // Libraries #include @@ -31,7 +12,7 @@ using namespace reactphysics3d; // Constructor NarrowPhaseAlgorithm::NarrowPhaseAlgorithm() - : mMemoryAllocator(NULL), mCurrentOverlappingPair(NULL) { + : mMemoryAllocator(NULL), mCurrentOverlappingPair(NULL) { } @@ -42,6 +23,6 @@ NarrowPhaseAlgorithm::~NarrowPhaseAlgorithm() { // Initalize the algorithm void NarrowPhaseAlgorithm::init(CollisionDetection* collisionDetection, MemoryAllocator* memoryAllocator) { - mCollisionDetection = collisionDetection; - mMemoryAllocator = memoryAllocator; + mCollisionDetection = collisionDetection; + mMemoryAllocator = memoryAllocator; } diff --git a/ephysics/collision/narrowphase/NarrowPhaseAlgorithm.h b/ephysics/collision/narrowphase/NarrowPhaseAlgorithm.h index 8b78584..aba5419 100644 --- a/ephysics/collision/narrowphase/NarrowPhaseAlgorithm.h +++ b/ephysics/collision/narrowphase/NarrowPhaseAlgorithm.h @@ -1,30 +1,9 @@ -/******************************************************************************** -* 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 REACTPHYSICS3D_NARROW_PHASE_ALGORITHM_H -#define REACTPHYSICS3D_NARROW_PHASE_ALGORITHM_H +/** @file + * @author Daniel Chappuis + * @copyright 2010-2016 Daniel Chappuis + * @license BSD 3 clauses (see license file) + */ +#pragma once // Libraries #include @@ -45,12 +24,12 @@ class CollisionDetection; */ class NarrowPhaseCallback { - public: - virtual ~NarrowPhaseCallback() = default; + public: + virtual ~NarrowPhaseCallback() = default; - /// Called by a narrow-phase collision algorithm when a new contact has been found - virtual void notifyContact(OverlappingPair* overlappingPair, - const ContactPointInfo& contactInfo)=0; + /// Called by a narrow-phase collision algorithm when a new contact has been found + virtual void notifyContact(OverlappingPair* overlappingPair, + const ContactPointInfo& contactInfo)=0; }; @@ -62,56 +41,54 @@ class NarrowPhaseCallback { */ class NarrowPhaseAlgorithm { - protected : + protected : - // -------------------- Attributes -------------------- // + // -------------------- Attributes -------------------- // - /// Pointer to the collision detection object - CollisionDetection* mCollisionDetection; + /// Pointer to the collision detection object + CollisionDetection* mCollisionDetection; - /// Pointer to the memory allocator - MemoryAllocator* mMemoryAllocator; + /// Pointer to the memory allocator + MemoryAllocator* mMemoryAllocator; - /// Overlapping pair of the bodies currently tested for collision - OverlappingPair* mCurrentOverlappingPair; - - // -------------------- Methods -------------------- // + /// Overlapping pair of the bodies currently tested for collision + OverlappingPair* mCurrentOverlappingPair; + + // -------------------- Methods -------------------- // - /// Private copy-constructor - NarrowPhaseAlgorithm(const NarrowPhaseAlgorithm& algorithm); + /// Private copy-constructor + NarrowPhaseAlgorithm(const NarrowPhaseAlgorithm& algorithm); - /// Private assignment operator - NarrowPhaseAlgorithm& operator=(const NarrowPhaseAlgorithm& algorithm); + /// Private assignment operator + NarrowPhaseAlgorithm& operator=(const NarrowPhaseAlgorithm& algorithm); - public : + public : - // -------------------- Methods -------------------- // + // -------------------- Methods -------------------- // - /// Constructor - NarrowPhaseAlgorithm(); + /// Constructor + NarrowPhaseAlgorithm(); - /// Destructor - virtual ~NarrowPhaseAlgorithm(); + /// Destructor + virtual ~NarrowPhaseAlgorithm(); - /// Initalize the algorithm - virtual void init(CollisionDetection* collisionDetection, MemoryAllocator* memoryAllocator); - - /// Set the current overlapping pair of bodies - void setCurrentOverlappingPair(OverlappingPair* overlappingPair); + /// Initalize the algorithm + virtual void init(CollisionDetection* collisionDetection, MemoryAllocator* memoryAllocator); + + /// Set the current overlapping pair of bodies + void setCurrentOverlappingPair(OverlappingPair* overlappingPair); - /// Compute a contact info if the two bounding volume collide - virtual void testCollision(const CollisionShapeInfo& shape1Info, - const CollisionShapeInfo& shape2Info, - NarrowPhaseCallback* narrowPhaseCallback)=0; + /// Compute a contact info if the two bounding volume collide + virtual void testCollision(const CollisionShapeInfo& shape1Info, + const CollisionShapeInfo& shape2Info, + NarrowPhaseCallback* narrowPhaseCallback)=0; }; // Set the current overlapping pair of bodies inline void NarrowPhaseAlgorithm::setCurrentOverlappingPair(OverlappingPair* overlappingPair) { - mCurrentOverlappingPair = overlappingPair; -} + mCurrentOverlappingPair = overlappingPair; +} } -#endif - diff --git a/ephysics/collision/narrowphase/SphereVsSphereAlgorithm.cpp b/ephysics/collision/narrowphase/SphereVsSphereAlgorithm.cpp index 9b03bcf..1558864 100644 --- a/ephysics/collision/narrowphase/SphereVsSphereAlgorithm.cpp +++ b/ephysics/collision/narrowphase/SphereVsSphereAlgorithm.cpp @@ -1,27 +1,8 @@ -/******************************************************************************** -* 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. * -* * -********************************************************************************/ +/** @file + * @author Daniel Chappuis + * @copyright 2010-2016 Daniel Chappuis + * @license BSD 3 clauses (see license file) + */ // Libraries #include @@ -31,50 +12,45 @@ using namespace reactphysics3d; // Constructor -SphereVsSphereAlgorithm::SphereVsSphereAlgorithm() : NarrowPhaseAlgorithm() { - +SphereVsSphereAlgorithm::SphereVsSphereAlgorithm() : + NarrowPhaseAlgorithm() { + } // Destructor SphereVsSphereAlgorithm::~SphereVsSphereAlgorithm() { - -} + +} void SphereVsSphereAlgorithm::testCollision(const CollisionShapeInfo& shape1Info, const CollisionShapeInfo& shape2Info, NarrowPhaseCallback* narrowPhaseCallback) { - - // Get the sphere collision shapes - const SphereShape* sphereShape1 = static_cast(shape1Info.collisionShape); - const SphereShape* sphereShape2 = static_cast(shape2Info.collisionShape); - - // Get the local-space to world-space transforms - const Transform& transform1 = shape1Info.shapeToWorldTransform; - const Transform& transform2 = shape2Info.shapeToWorldTransform; - - // Compute the distance between the centers - Vector3 vectorBetweenCenters = transform2.getPosition() - transform1.getPosition(); - decimal squaredDistanceBetweenCenters = vectorBetweenCenters.lengthSquare(); - - // Compute the sum of the radius - decimal sumRadius = sphereShape1->getRadius() + sphereShape2->getRadius(); - - // If the sphere collision shapes intersect - if (squaredDistanceBetweenCenters <= sumRadius * sumRadius) { - Vector3 centerSphere2InBody1LocalSpace = transform1.getInverse() * transform2.getPosition(); - Vector3 centerSphere1InBody2LocalSpace = transform2.getInverse() * transform1.getPosition(); - Vector3 intersectionOnBody1 = sphereShape1->getRadius() * - centerSphere2InBody1LocalSpace.getUnit(); - Vector3 intersectionOnBody2 = sphereShape2->getRadius() * - centerSphere1InBody2LocalSpace.getUnit(); - decimal penetrationDepth = sumRadius - std::sqrt(squaredDistanceBetweenCenters); - - // Create the contact info object - ContactPointInfo contactInfo(shape1Info.proxyShape, shape2Info.proxyShape, shape1Info.collisionShape, - shape2Info.collisionShape, vectorBetweenCenters.getUnit(), penetrationDepth, - intersectionOnBody1, intersectionOnBody2); - - // Notify about the new contact - narrowPhaseCallback->notifyContact(shape1Info.overlappingPair, contactInfo); - } + // Get the sphere collision shapes + const SphereShape* sphereShape1 = static_cast(shape1Info.collisionShape); + const SphereShape* sphereShape2 = static_cast(shape2Info.collisionShape); + // Get the local-space to world-space transforms + const Transform& transform1 = shape1Info.shapeToWorldTransform; + const Transform& transform2 = shape2Info.shapeToWorldTransform; + // Compute the distance between the centers + Vector3 vectorBetweenCenters = transform2.getPosition() - transform1.getPosition(); + float squaredDistanceBetweenCenters = vectorBetweenCenters.lengthSquare(); + // Compute the sum of the radius + float sumRadius = sphereShape1->getRadius() + sphereShape2->getRadius(); + // If the sphere collision shapes int32_tersect + if (squaredDistanceBetweenCenters <= sumRadius * sumRadius) { + Vector3 centerSphere2InBody1LocalSpace = transform1.getInverse() * transform2.getPosition(); + Vector3 centerSphere1InBody2LocalSpace = transform2.getInverse() * transform1.getPosition(); + Vector3 int32_tersectionOnBody1 = sphereShape1->getRadius() * + centerSphere2InBody1LocalSpace.getUnit(); + Vector3 int32_tersectionOnBody2 = sphereShape2->getRadius() * + centerSphere1InBody2LocalSpace.getUnit(); + float penetrationDepth = sumRadius - std::sqrt(squaredDistanceBetweenCenters); + + // Create the contact info object + ContactPointInfo contactInfo(shape1Info.proxyShape, shape2Info.proxyShape, shape1Info.collisionShape, + shape2Info.collisionShape, vectorBetweenCenters.getUnit(), penetrationDepth, + int32_tersectionOnBody1, int32_tersectionOnBody2); + // Notify about the new contact + narrowPhaseCallback->notifyContact(shape1Info.overlappingPair, contactInfo); + } } diff --git a/ephysics/collision/narrowphase/SphereVsSphereAlgorithm.h b/ephysics/collision/narrowphase/SphereVsSphereAlgorithm.h index 01dc50b..f82e3ea 100644 --- a/ephysics/collision/narrowphase/SphereVsSphereAlgorithm.h +++ b/ephysics/collision/narrowphase/SphereVsSphereAlgorithm.h @@ -1,30 +1,9 @@ -/******************************************************************************** -* 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 REACTPHYSICS3D_SPHERE_VS_SPHERE_ALGORITHM_H -#define REACTPHYSICS3D_SPHERE_VS_SPHERE_ALGORITHM_H +/** @file + * @author Daniel Chappuis + * @copyright 2010-2016 Daniel Chappuis + * @license BSD 3 clauses (see license file) + */ +#pragma once // Libraries #include @@ -42,33 +21,32 @@ namespace reactphysics3d { */ class SphereVsSphereAlgorithm : public NarrowPhaseAlgorithm { - protected : + protected : - // -------------------- Methods -------------------- // + // -------------------- Methods -------------------- // - /// Private copy-constructor - SphereVsSphereAlgorithm(const SphereVsSphereAlgorithm& algorithm); + /// Private copy-constructor + SphereVsSphereAlgorithm(const SphereVsSphereAlgorithm& algorithm); - /// Private assignment operator - SphereVsSphereAlgorithm& operator=(const SphereVsSphereAlgorithm& algorithm); - - public : + /// Private assignment operator + SphereVsSphereAlgorithm& operator=(const SphereVsSphereAlgorithm& algorithm); + + public : - // -------------------- Methods -------------------- // + // -------------------- Methods -------------------- // - /// Constructor - SphereVsSphereAlgorithm(); + /// Constructor + SphereVsSphereAlgorithm(); - /// Destructor - virtual ~SphereVsSphereAlgorithm(); + /// Destructor + virtual ~SphereVsSphereAlgorithm(); - /// Compute a contact info if the two bounding volume collide - virtual void testCollision(const CollisionShapeInfo& shape1Info, - const CollisionShapeInfo& shape2Info, - NarrowPhaseCallback* narrowPhaseCallback); + /// Compute a contact info if the two bounding volume collide + virtual void testCollision(const CollisionShapeInfo& shape1Info, + const CollisionShapeInfo& shape2Info, + NarrowPhaseCallback* narrowPhaseCallback); }; } -#endif diff --git a/ephysics/collision/shapes/AABB.cpp b/ephysics/collision/shapes/AABB.cpp index 63e6930..d6fdae7 100644 --- a/ephysics/collision/shapes/AABB.cpp +++ b/ephysics/collision/shapes/AABB.cpp @@ -1,27 +1,9 @@ -/******************************************************************************** -* 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. * -* * -********************************************************************************/ +/** @file + * @author Daniel Chappuis + * @copyright 2010-2016 Daniel Chappuis + * @license BSD 3 clauses (see license file) + */ + // Libraries #include @@ -38,13 +20,13 @@ AABB::AABB() { // Constructor AABB::AABB(const Vector3& minCoordinates, const Vector3& maxCoordinates) - :mMinCoordinates(minCoordinates), mMaxCoordinates(maxCoordinates) { + :mMinCoordinates(minCoordinates), mMaxCoordinates(maxCoordinates) { } // Copy-constructor AABB::AABB(const AABB& aabb) - : mMinCoordinates(aabb.mMinCoordinates), mMaxCoordinates(aabb.mMaxCoordinates) { + : mMinCoordinates(aabb.mMinCoordinates), mMaxCoordinates(aabb.mMaxCoordinates) { } @@ -55,96 +37,96 @@ AABB::~AABB() { // 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); + 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); + 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); + 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); + 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; + 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; + 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); + 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[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[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[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; + 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 AABB(minCoords, maxCoords); } -// Return true if the ray intersects the AABB +// Return true if the ray int32_tersects 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; + 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; + // Test if the AABB face normals are separating axis + float adx = std::abs(d.x); + if (std::abs(m.x) > e.x + adx) return false; + float ady = std::abs(d.y); + if (std::abs(m.y) > e.y + ady) return false; + float 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; + // Add in an epsilon term to counteract arithmetic errors when segment is + // (near) parallel to a coordinate axis (see text for detail) + const float 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; + // 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; + // No separating axis has been found + return true; } diff --git a/ephysics/collision/shapes/AABB.h b/ephysics/collision/shapes/AABB.h index 013bf58..ccfa874 100644 --- a/ephysics/collision/shapes/AABB.h +++ b/ephysics/collision/shapes/AABB.h @@ -1,37 +1,16 @@ -/******************************************************************************** -* 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 REACTPHYSICS3D_AABB_H -#define REACTPHYSICS3D_AABB_H +/** @file + * @author Daniel Chappuis + * @copyright 2010-2016 Daniel Chappuis + * @license BSD 3 clauses (see license file) + */ +#pragma once // Libraries #include /// ReactPhysics3D namespace namespace reactphysics3d { - + // Class AABB /** * This class represents a bounding volume of type "Axis Aligned @@ -41,111 +20,111 @@ namespace reactphysics3d { */ class AABB { - private : + private : - // -------------------- Attributes -------------------- // + // -------------------- Attributes -------------------- // - /// Minimum world coordinates of the AABB on the x,y and z axis - Vector3 mMinCoordinates; + /// Minimum world coordinates of the AABB on the x,y and z axis + Vector3 mMinCoordinates; - /// Maximum world coordinates of the AABB on the x,y and z axis - Vector3 mMaxCoordinates; + /// Maximum world coordinates of the AABB on the x,y and z axis + Vector3 mMaxCoordinates; - public : + public : - // -------------------- Methods -------------------- // + // -------------------- Methods -------------------- // - /// Constructor - AABB(); + /// Constructor + AABB(); - /// Constructor - AABB(const Vector3& minCoordinates, const Vector3& maxCoordinates); + /// Constructor + AABB(const Vector3& minCoordinates, const Vector3& maxCoordinates); - /// Copy-constructor - AABB(const AABB& aabb); + /// Copy-constructor + AABB(const AABB& aabb); - /// Destructor - ~AABB(); + /// Destructor + ~AABB(); - /// Return the center point - Vector3 getCenter() const; + /// Return the center point + Vector3 getCenter() const; - /// Return the minimum coordinates of the AABB - const Vector3& getMin() const; + /// Return the minimum coordinates of the AABB + const Vector3& getMin() const; - /// Set the minimum coordinates of the AABB - void setMin(const Vector3& min); + /// Set the minimum coordinates of the AABB + void setMin(const Vector3& min); - /// Return the maximum coordinates of the AABB - const Vector3& getMax() const; + /// Return the maximum coordinates of the AABB + const Vector3& getMax() const; - /// Set the maximum coordinates of the AABB - void setMax(const Vector3& max); + /// Set the maximum coordinates of the AABB + void setMax(const Vector3& max); - /// Return the size of the AABB in the three dimension x, y and z - Vector3 getExtent() const; + /// Return the size of the AABB in the three dimension x, y and z + Vector3 getExtent() const; - /// Inflate each side of the AABB by a given size - void inflate(decimal dx, decimal dy, decimal dz); + /// Inflate each side of the AABB by a given size + void inflate(float dx, float dy, float dz); - /// Return true if the current AABB is overlapping with the AABB in argument - bool testCollision(const AABB& aabb) const; + /// Return true if the current AABB is overlapping with the AABB in argument + bool testCollision(const AABB& aabb) const; - /// Return the volume of the AABB - decimal getVolume() const; + /// Return the volume of the AABB + float getVolume() const; - /// Merge the AABB in parameter with the current one - void mergeWithAABB(const AABB& aabb); + /// Merge the AABB in parameter with the current one + void mergeWithAABB(const AABB& aabb); - /// Replace the current AABB with a new AABB that is the union of two AABBs in parameters - void mergeTwoAABBs(const AABB& aabb1, const AABB& aabb2); + /// Replace the current AABB with a new AABB that is the union of two AABBs in parameters + void mergeTwoAABBs(const AABB& aabb1, const AABB& aabb2); - /// Return true if the current AABB contains the AABB given in parameter - bool contains(const AABB& aabb) const; + /// Return true if the current AABB contains the AABB given in parameter + bool contains(const AABB& aabb) const; - /// Return true if a point is inside the AABB - bool contains(const Vector3& point) const; + /// Return true if a point is inside the AABB + bool contains(const Vector3& point) const; - /// Return true if the AABB of a triangle intersects the AABB - bool testCollisionTriangleAABB(const Vector3* trianglePoints) const; + /// Return true if the AABB of a triangle int32_tersects the AABB + bool testCollisionTriangleAABB(const Vector3* trianglePoints) const; - /// Return true if the ray intersects the AABB - bool testRayIntersect(const Ray& ray) const; + /// Return true if the ray int32_tersects the AABB + bool testRayIntersect(const Ray& ray) const; - /// Create and return an AABB for a triangle - static AABB createAABBForTriangle(const Vector3* trianglePoints); + /// Create and return an AABB for a triangle + static AABB createAABBForTriangle(const Vector3* trianglePoints); - /// Assignment operator - AABB& operator=(const AABB& aabb); + /// Assignment operator + AABB& operator=(const AABB& aabb); - // -------------------- Friendship -------------------- // + // -------------------- Friendship -------------------- // - friend class DynamicAABBTree; + friend class DynamicAABBTree; }; // Return the center point of the AABB in world coordinates inline Vector3 AABB::getCenter() const { - return (mMinCoordinates + mMaxCoordinates) * decimal(0.5); + return (mMinCoordinates + mMaxCoordinates) * float(0.5); } // Return the minimum coordinates of the AABB inline const Vector3& AABB::getMin() const { - return mMinCoordinates; + return mMinCoordinates; } // Set the minimum coordinates of the AABB inline void AABB::setMin(const Vector3& min) { - mMinCoordinates = min; + mMinCoordinates = min; } // Return the maximum coordinates of the AABB inline const Vector3& AABB::getMax() const { - return mMaxCoordinates; + return mMaxCoordinates; } // Set the maximum coordinates of the AABB inline void AABB::setMax(const Vector3& max) { - mMaxCoordinates = max; + mMaxCoordinates = max; } // Return the size of the AABB in the three dimension x, y and z @@ -154,60 +133,58 @@ inline Vector3 AABB::getExtent() const { } // Inflate each side of the AABB by a given size -inline void AABB::inflate(decimal dx, decimal dy, decimal dz) { - mMaxCoordinates += Vector3(dx, dy, dz); - mMinCoordinates -= Vector3(dx, dy, dz); +inline void AABB::inflate(float dx, float dy, float 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 { - if (mMaxCoordinates.x < aabb.mMinCoordinates.x || - aabb.mMaxCoordinates.x < mMinCoordinates.x) return false; - if (mMaxCoordinates.y < aabb.mMinCoordinates.y || - aabb.mMaxCoordinates.y < mMinCoordinates.y) return false; - if (mMaxCoordinates.z < aabb.mMinCoordinates.z|| - aabb.mMaxCoordinates.z < mMinCoordinates.z) return false; - return true; + if (mMaxCoordinates.x < aabb.mMinCoordinates.x || + aabb.mMaxCoordinates.x < mMinCoordinates.x) return false; + if (mMaxCoordinates.y < aabb.mMinCoordinates.y || + aabb.mMaxCoordinates.y < mMinCoordinates.y) return false; + if (mMaxCoordinates.z < aabb.mMinCoordinates.z|| + aabb.mMaxCoordinates.z < mMinCoordinates.z) return false; + return true; } // Return the volume of the AABB -inline decimal AABB::getVolume() const { - const Vector3 diff = mMaxCoordinates - mMinCoordinates; - return (diff.x * diff.y * diff.z); +inline float 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 +// Return true if the AABB of a triangle int32_tersects the AABB 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; - if (min3(trianglePoints[0].z, trianglePoints[1].z, trianglePoints[2].z) > mMaxCoordinates.z) return false; + 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; + if (min3(trianglePoints[0].z, trianglePoints[1].z, trianglePoints[2].z) > mMaxCoordinates.z) return false; - if (max3(trianglePoints[0].x, trianglePoints[1].x, trianglePoints[2].x) < mMinCoordinates.x) return false; - if (max3(trianglePoints[0].y, trianglePoints[1].y, trianglePoints[2].y) < mMinCoordinates.y) return false; - if (max3(trianglePoints[0].z, trianglePoints[1].z, trianglePoints[2].z) < mMinCoordinates.z) return false; + if (max3(trianglePoints[0].x, trianglePoints[1].x, trianglePoints[2].x) < mMinCoordinates.x) return false; + if (max3(trianglePoints[0].y, trianglePoints[1].y, trianglePoints[2].y) < mMinCoordinates.y) return false; + if (max3(trianglePoints[0].z, trianglePoints[1].z, trianglePoints[2].z) < mMinCoordinates.z) return false; - return true; + return true; } // Return true if a point is inside the AABB 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 && - point.z >= mMinCoordinates.z - MACHINE_EPSILON && point.z <= mMaxCoordinates.z + MACHINE_EPSILON); + 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 && + point.z >= mMinCoordinates.z - MACHINE_EPSILON && point.z <= mMaxCoordinates.z + MACHINE_EPSILON); } // Assignment operator inline AABB& AABB::operator=(const AABB& aabb) { - if (this != &aabb) { - mMinCoordinates = aabb.mMinCoordinates; - mMaxCoordinates = aabb.mMaxCoordinates; - } - return *this; + if (this != &aabb) { + mMinCoordinates = aabb.mMinCoordinates; + mMaxCoordinates = aabb.mMaxCoordinates; + } + return *this; } } - -#endif diff --git a/ephysics/collision/shapes/BoxShape.cpp b/ephysics/collision/shapes/BoxShape.cpp index 1d9bacd..e3e8b64 100644 --- a/ephysics/collision/shapes/BoxShape.cpp +++ b/ephysics/collision/shapes/BoxShape.cpp @@ -1,27 +1,8 @@ -/******************************************************************************** -* ReactPhysics3D physics library, http://www.reactphysics3d.com * -* Copyright (c) 2010-2015 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. * -* * -********************************************************************************/ +/** @file + * @author Daniel Chappuis + * @copyright 2010-2016 Daniel Chappuis + * @license BSD 3 clauses (see license file) + */ // Libraries #include @@ -37,11 +18,11 @@ using namespace reactphysics3d; * @param extent The vector with the three extents of the box (in meters) * @param margin The collision margin (in meters) around the collision shape */ -BoxShape::BoxShape(const Vector3& extent, decimal margin) - : ConvexShape(BOX, margin), mExtent(extent - Vector3(margin, margin, margin)) { - assert(extent.x > decimal(0.0) && extent.x > margin); - assert(extent.y > decimal(0.0) && extent.y > margin); - assert(extent.z > decimal(0.0) && extent.z > margin); +BoxShape::BoxShape(const Vector3& extent, float margin) + : ConvexShape(BOX, margin), mExtent(extent - Vector3(margin, margin, margin)) { + assert(extent.x > float(0.0) && extent.x > margin); + assert(extent.y > float(0.0) && extent.y > margin); + assert(extent.z > float(0.0) && extent.z > margin); } // Destructor @@ -52,81 +33,81 @@ BoxShape::~BoxShape() { // Return the local inertia tensor of the collision shape /** * @param[out] tensor The 3x3 inertia tensor matrix of the shape in local-space - * coordinates + * coordinates * @param mass Mass to use to compute the inertia tensor of the collision shape */ -void BoxShape::computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const { - decimal factor = (decimal(1.0) / decimal(3.0)) * mass; - Vector3 realExtent = mExtent + Vector3(mMargin, mMargin, mMargin); - decimal xSquare = realExtent.x * realExtent.x; - decimal ySquare = realExtent.y * realExtent.y; - decimal zSquare = realExtent.z * realExtent.z; - tensor.setAllValues(factor * (ySquare + zSquare), 0.0, 0.0, - 0.0, factor * (xSquare + zSquare), 0.0, - 0.0, 0.0, factor * (xSquare + ySquare)); +void BoxShape::computeLocalInertiaTensor(Matrix3x3& tensor, float mass) const { + float factor = (float(1.0) / float(3.0)) * mass; + Vector3 realExtent = mExtent + Vector3(mMargin, mMargin, mMargin); + float xSquare = realExtent.x * realExtent.x; + float ySquare = realExtent.y * realExtent.y; + float zSquare = realExtent.z * realExtent.z; + tensor.setAllValues(factor * (ySquare + zSquare), 0.0, 0.0, + 0.0, factor * (xSquare + zSquare), 0.0, + 0.0, 0.0, factor * (xSquare + ySquare)); } // Raycast method with feedback information bool BoxShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const { - Vector3 rayDirection = ray.point2 - ray.point1; - decimal tMin = DECIMAL_SMALLEST; - decimal tMax = DECIMAL_LARGEST; - Vector3 normalDirection(decimal(0), decimal(0), decimal(0)); - Vector3 currentNormal; + Vector3 rayDirection = ray.point2 - ray.point1; + float tMin = DECIMAL_SMALLEST; + float tMax = DECIMAL_LARGEST; + Vector3 normalDirection(float(0), float(0), float(0)); + Vector3 currentNormal; - // For each of the three slabs - for (int i=0; i<3; i++) { + // For each of the three slabs + for (int32_t i=0; i<3; i++) { - // If ray is parallel to the slab - if (std::abs(rayDirection[i]) < MACHINE_EPSILON) { + // If ray is parallel to the slab + if (std::abs(rayDirection[i]) < MACHINE_EPSILON) { - // If the ray's origin is not inside the slab, there is no hit - if (ray.point1[i] > mExtent[i] || ray.point1[i] < -mExtent[i]) return false; - } - else { + // If the ray's origin is not inside the slab, there is no hit + if (ray.point1[i] > mExtent[i] || ray.point1[i] < -mExtent[i]) return false; + } + else { - // Compute the intersection of the ray with the near and far plane of the slab - decimal oneOverD = decimal(1.0) / rayDirection[i]; - decimal t1 = (-mExtent[i] - ray.point1[i]) * oneOverD; - decimal t2 = (mExtent[i] - ray.point1[i]) * oneOverD; - currentNormal[0] = (i == 0) ? -mExtent[i] : decimal(0.0); - currentNormal[1] = (i == 1) ? -mExtent[i] : decimal(0.0); - currentNormal[2] = (i == 2) ? -mExtent[i] : decimal(0.0); + // Compute the int32_tersection of the ray with the near and far plane of the slab + float oneOverD = float(1.0) / rayDirection[i]; + float t1 = (-mExtent[i] - ray.point1[i]) * oneOverD; + float t2 = (mExtent[i] - ray.point1[i]) * oneOverD; + currentNormal[0] = (i == 0) ? -mExtent[i] : float(0.0); + currentNormal[1] = (i == 1) ? -mExtent[i] : float(0.0); + currentNormal[2] = (i == 2) ? -mExtent[i] : float(0.0); - // Swap t1 and t2 if need so that t1 is intersection with near plane and - // t2 with far plane - if (t1 > t2) { - std::swap(t1, t2); - currentNormal = -currentNormal; - } + // Swap t1 and t2 if need so that t1 is int32_tersection with near plane and + // t2 with far plane + if (t1 > t2) { + std::swap(t1, t2); + currentNormal = -currentNormal; + } - // Compute the intersection of the of slab intersection interval with previous slabs - if (t1 > tMin) { - tMin = t1; - normalDirection = currentNormal; - } - tMax = std::min(tMax, t2); + // Compute the int32_tersection of the of slab int32_tersection int32_terval with previous slabs + if (t1 > tMin) { + tMin = t1; + normalDirection = currentNormal; + } + tMax = std::min(tMax, t2); - // If tMin is larger than the maximum raycasting fraction, we return no hit - if (tMin > ray.maxFraction) return false; + // If tMin is larger than the maximum raycasting fraction, we return no hit + if (tMin > ray.maxFraction) return false; - // If the slabs intersection is empty, there is no hit - if (tMin > tMax) return false; - } - } + // If the slabs int32_tersection is empty, there is no hit + if (tMin > tMax) return false; + } + } - // If tMin is negative, we return no hit - if (tMin < decimal(0.0) || tMin > ray.maxFraction) return false; + // If tMin is negative, we return no hit + if (tMin < float(0.0) || tMin > ray.maxFraction) return false; - // The ray intersects the three slabs, we compute the hit point - Vector3 localHitPoint = ray.point1 + tMin * rayDirection; + // The ray int32_tersects the three slabs, we compute the hit point + Vector3 localHitPoint = ray.point1 + tMin * rayDirection; - raycastInfo.body = proxyShape->getBody(); - raycastInfo.proxyShape = proxyShape; - raycastInfo.hitFraction = tMin; - raycastInfo.worldPoint = localHitPoint; - raycastInfo.worldNormal = normalDirection; + raycastInfo.body = proxyShape->getBody(); + raycastInfo.proxyShape = proxyShape; + raycastInfo.hitFraction = tMin; + raycastInfo.worldPoint = localHitPoint; + raycastInfo.worldNormal = normalDirection; - return true; + return true; } diff --git a/ephysics/collision/shapes/BoxShape.h b/ephysics/collision/shapes/BoxShape.h index 15cc66a..21c1f38 100644 --- a/ephysics/collision/shapes/BoxShape.h +++ b/ephysics/collision/shapes/BoxShape.h @@ -1,30 +1,9 @@ -/******************************************************************************** -* 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 REACTPHYSICS3D_BOX_SHAPE_H -#define REACTPHYSICS3D_BOX_SHAPE_H +/** @file + * @author Daniel Chappuis + * @copyright 2010-2016 Daniel Chappuis + * @license BSD 3 clauses (see license file) + */ +#pragma once // Libraries #include @@ -52,55 +31,55 @@ namespace reactphysics3d { */ class BoxShape : public ConvexShape { - protected : + protected : - // -------------------- Attributes -------------------- // + // -------------------- Attributes -------------------- // - /// Extent sizes of the box in the x, y and z direction - Vector3 mExtent; + /// Extent sizes of the box in the x, y and z direction + Vector3 mExtent; - // -------------------- Methods -------------------- // + // -------------------- Methods -------------------- // - /// Private copy-constructor - BoxShape(const BoxShape& shape); + /// Private copy-constructor + BoxShape(const BoxShape& shape); - /// Private assignment operator - BoxShape& operator=(const BoxShape& shape); + /// Private assignment operator + BoxShape& operator=(const BoxShape& shape); - /// Return a local support point in a given direction without the object margin - virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction, - void** cachedCollisionData) const; + /// Return a local support point in a given direction without the object margin + virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction, + void** cachedCollisionData) const; - /// Return true if a point is inside the collision shape - virtual bool testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const; + /// Return true if a point is inside the collision shape + virtual bool testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const; - /// Raycast method with feedback information - virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const; + /// Raycast method with feedback information + virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const; - /// Return the number of bytes used by the collision shape - virtual size_t getSizeInBytes() const; + /// Return the number of bytes used by the collision shape + virtual size_t getSizeInBytes() const; - public : + public : - // -------------------- Methods -------------------- // + // -------------------- Methods -------------------- // - /// Constructor - BoxShape(const Vector3& extent, decimal margin = OBJECT_MARGIN); + /// Constructor + BoxShape(const Vector3& extent, float margin = OBJECT_MARGIN); - /// Destructor - virtual ~BoxShape(); + /// Destructor + virtual ~BoxShape(); - /// Return the extents of the box - Vector3 getExtent() const; + /// Return the extents of the box + Vector3 getExtent() const; - /// Set the scaling vector of the collision shape - virtual void setLocalScaling(const Vector3& scaling); + /// Set the scaling vector of the collision shape + virtual void setLocalScaling(const Vector3& scaling); - /// Return the local bounds of the shape in x, y and z directions - virtual void getLocalBounds(Vector3& min, Vector3& max) const; + /// Return the local bounds of the shape in x, y and z directions + virtual void getLocalBounds(Vector3& min, Vector3& max) const; - /// Return the local inertia tensor of the collision shape - virtual void computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const; + /// Return the local inertia tensor of the collision shape + virtual void computeLocalInertiaTensor(Matrix3x3& tensor, float mass) const; }; // Return the extents of the box @@ -108,15 +87,15 @@ class BoxShape : public ConvexShape { * @return The vector with the three extents of the box shape (in meters) */ inline Vector3 BoxShape::getExtent() const { - return mExtent + Vector3(mMargin, mMargin, mMargin); + return mExtent + Vector3(mMargin, mMargin, mMargin); } // Set the scaling vector of the collision shape inline void BoxShape::setLocalScaling(const Vector3& scaling) { - mExtent = (mExtent / mScaling) * scaling; + mExtent = (mExtent / mScaling) * scaling; - CollisionShape::setLocalScaling(scaling); + CollisionShape::setLocalScaling(scaling); } // Return the local bounds of the shape in x, y and z directions @@ -127,34 +106,32 @@ inline void BoxShape::setLocalScaling(const Vector3& scaling) { */ inline void BoxShape::getLocalBounds(Vector3& min, Vector3& max) const { - // Maximum bounds - max = mExtent + Vector3(mMargin, mMargin, mMargin); + // Maximum bounds + max = mExtent + Vector3(mMargin, mMargin, mMargin); - // Minimum bounds - min = -max; + // Minimum bounds + min = -max; } // Return the number of bytes used by the collision shape inline size_t BoxShape::getSizeInBytes() const { - return sizeof(BoxShape); + return sizeof(BoxShape); } // Return a local support point in a given direction without the objec margin inline Vector3 BoxShape::getLocalSupportPointWithoutMargin(const Vector3& direction, - void** cachedCollisionData) const { + void** cachedCollisionData) const { - return Vector3(direction.x < 0.0 ? -mExtent.x : mExtent.x, - direction.y < 0.0 ? -mExtent.y : mExtent.y, - direction.z < 0.0 ? -mExtent.z : mExtent.z); + return Vector3(direction.x < 0.0 ? -mExtent.x : mExtent.x, + direction.y < 0.0 ? -mExtent.y : mExtent.y, + direction.z < 0.0 ? -mExtent.z : mExtent.z); } // Return true if a point is inside the collision shape inline bool BoxShape::testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const { - return (localPoint.x < mExtent[0] && localPoint.x > -mExtent[0] && - localPoint.y < mExtent[1] && localPoint.y > -mExtent[1] && - localPoint.z < mExtent[2] && localPoint.z > -mExtent[2]); + return (localPoint.x < mExtent[0] && localPoint.x > -mExtent[0] && + localPoint.y < mExtent[1] && localPoint.y > -mExtent[1] && + localPoint.z < mExtent[2] && localPoint.z > -mExtent[2]); } } - -#endif diff --git a/ephysics/collision/shapes/CapsuleShape.cpp b/ephysics/collision/shapes/CapsuleShape.cpp index bfcbd85..49a02c7 100644 --- a/ephysics/collision/shapes/CapsuleShape.cpp +++ b/ephysics/collision/shapes/CapsuleShape.cpp @@ -1,27 +1,8 @@ -/******************************************************************************** -* 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. * -* * -********************************************************************************/ +/** @file + * @author Daniel Chappuis + * @copyright 2010-2016 Daniel Chappuis + * @license BSD 3 clauses (see license file) + */ // Libraries #include @@ -36,10 +17,10 @@ using namespace reactphysics3d; * @param radius The radius of the capsule (in meters) * @param height The height of the capsule (in meters) */ -CapsuleShape::CapsuleShape(decimal radius, decimal height) - : ConvexShape(CAPSULE, radius), mHalfHeight(height * decimal(0.5)) { - assert(radius > decimal(0.0)); - assert(height > decimal(0.0)); +CapsuleShape::CapsuleShape(float radius, float height) + : ConvexShape(CAPSULE, radius), mHalfHeight(height * float(0.5)) { + assert(radius > float(0.0)); + assert(height > float(0.0)); } // Destructor @@ -50,232 +31,232 @@ CapsuleShape::~CapsuleShape() { // Return the local inertia tensor of the capsule /** * @param[out] tensor The 3x3 inertia tensor matrix of the shape in local-space - * coordinates + * coordinates * @param mass Mass to use to compute the inertia tensor of the collision shape */ -void CapsuleShape::computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const { +void CapsuleShape::computeLocalInertiaTensor(Matrix3x3& tensor, float mass) const { // The inertia tensor formula for a capsule can be found in : Game Engine Gems, Volume 1 - decimal height = mHalfHeight + mHalfHeight; - decimal radiusSquare = mMargin * mMargin; - decimal heightSquare = height * height; - decimal radiusSquareDouble = radiusSquare + radiusSquare; - decimal factor1 = decimal(2.0) * mMargin / (decimal(4.0) * mMargin + decimal(3.0) * height); - decimal factor2 = decimal(3.0) * height / (decimal(4.0) * mMargin + decimal(3.0) * height); - decimal sum1 = decimal(0.4) * radiusSquareDouble; - decimal sum2 = decimal(0.75) * height * mMargin + decimal(0.5) * heightSquare; - decimal sum3 = decimal(0.25) * radiusSquare + decimal(1.0 / 12.0) * heightSquare; - decimal IxxAndzz = factor1 * mass * (sum1 + sum2) + factor2 * mass * sum3; - decimal Iyy = factor1 * mass * sum1 + factor2 * mass * decimal(0.25) * radiusSquareDouble; - tensor.setAllValues(IxxAndzz, 0.0, 0.0, - 0.0, Iyy, 0.0, - 0.0, 0.0, IxxAndzz); + float height = mHalfHeight + mHalfHeight; + float radiusSquare = mMargin * mMargin; + float heightSquare = height * height; + float radiusSquareDouble = radiusSquare + radiusSquare; + float factor1 = float(2.0) * mMargin / (float(4.0) * mMargin + float(3.0) * height); + float factor2 = float(3.0) * height / (float(4.0) * mMargin + float(3.0) * height); + float sum1 = float(0.4) * radiusSquareDouble; + float sum2 = float(0.75) * height * mMargin + float(0.5) * heightSquare; + float sum3 = float(0.25) * radiusSquare + float(1.0 / 12.0) * heightSquare; + float IxxAndzz = factor1 * mass * (sum1 + sum2) + factor2 * mass * sum3; + float Iyy = factor1 * mass * sum1 + factor2 * mass * float(0.25) * radiusSquareDouble; + tensor.setAllValues(IxxAndzz, 0.0, 0.0, + 0.0, Iyy, 0.0, + 0.0, 0.0, IxxAndzz); } // Return true if a point is inside the collision shape bool CapsuleShape::testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const { - const decimal diffYCenterSphere1 = localPoint.y - mHalfHeight; - const decimal diffYCenterSphere2 = localPoint.y + mHalfHeight; - const decimal xSquare = localPoint.x * localPoint.x; - const decimal zSquare = localPoint.z * localPoint.z; - const decimal squareRadius = mMargin * mMargin; + const float diffYCenterSphere1 = localPoint.y - mHalfHeight; + const float diffYCenterSphere2 = localPoint.y + mHalfHeight; + const float xSquare = localPoint.x * localPoint.x; + const float zSquare = localPoint.z * localPoint.z; + const float squareRadius = mMargin * mMargin; - // Return true if the point is inside the cylinder or one of the two spheres of the capsule - return ((xSquare + zSquare) < squareRadius && - localPoint.y < mHalfHeight && localPoint.y > -mHalfHeight) || - (xSquare + zSquare + diffYCenterSphere1 * diffYCenterSphere1) < squareRadius || - (xSquare + zSquare + diffYCenterSphere2 * diffYCenterSphere2) < squareRadius; + // Return true if the point is inside the cylinder or one of the two spheres of the capsule + return ((xSquare + zSquare) < squareRadius && + localPoint.y < mHalfHeight && localPoint.y > -mHalfHeight) || + (xSquare + zSquare + diffYCenterSphere1 * diffYCenterSphere1) < squareRadius || + (xSquare + zSquare + diffYCenterSphere2 * diffYCenterSphere2) < squareRadius; } // Raycast method with feedback information bool CapsuleShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const { - const Vector3 n = ray.point2 - ray.point1; + const Vector3 n = ray.point2 - ray.point1; - const decimal epsilon = decimal(0.01); - Vector3 p(decimal(0), -mHalfHeight, decimal(0)); - Vector3 q(decimal(0), mHalfHeight, decimal(0)); - Vector3 d = q - p; - Vector3 m = ray.point1 - p; - decimal t; + const float epsilon = float(0.01); + Vector3 p(float(0), -mHalfHeight, float(0)); + Vector3 q(float(0), mHalfHeight, float(0)); + Vector3 d = q - p; + Vector3 m = ray.point1 - p; + float t; - decimal mDotD = m.dot(d); - decimal nDotD = n.dot(d); - decimal dDotD = d.dot(d); + float mDotD = m.dot(d); + float nDotD = n.dot(d); + float dDotD = d.dot(d); - // Test if the segment is outside the cylinder - decimal vec1DotD = (ray.point1 - Vector3(decimal(0.0), -mHalfHeight - mMargin, decimal(0.0))).dot(d); - if (vec1DotD < decimal(0.0) && vec1DotD + nDotD < decimal(0.0)) return false; - decimal ddotDExtraCaps = decimal(2.0) * mMargin * d.y; - if (vec1DotD > dDotD + ddotDExtraCaps && vec1DotD + nDotD > dDotD + ddotDExtraCaps) return false; + // Test if the segment is outside the cylinder + float vec1DotD = (ray.point1 - Vector3(float(0.0), -mHalfHeight - mMargin, float(0.0))).dot(d); + if (vec1DotD < float(0.0) && vec1DotD + nDotD < float(0.0)) return false; + float ddotDExtraCaps = float(2.0) * mMargin * d.y; + if (vec1DotD > dDotD + ddotDExtraCaps && vec1DotD + nDotD > dDotD + ddotDExtraCaps) return false; - decimal nDotN = n.dot(n); - decimal mDotN = m.dot(n); + float nDotN = n.dot(n); + float mDotN = m.dot(n); - decimal a = dDotD * nDotN - nDotD * nDotD; - decimal k = m.dot(m) - mMargin * mMargin; - decimal c = dDotD * k - mDotD * mDotD; + float a = dDotD * nDotN - nDotD * nDotD; + float k = m.dot(m) - mMargin * mMargin; + float c = dDotD * k - mDotD * mDotD; - // If the ray is parallel to the capsule axis - if (std::abs(a) < epsilon) { + // If the ray is parallel to the capsule axis + if (std::abs(a) < epsilon) { - // If the origin is outside the surface of the capusle's cylinder, we return no hit - if (c > decimal(0.0)) return false; + // If the origin is outside the surface of the capusle's cylinder, we return no hit + if (c > float(0.0)) return false; - // Here we know that the segment intersect an endcap of the capsule + // Here we know that the segment int32_tersect an endcap of the capsule - // If the ray intersects with the "p" endcap of the capsule - if (mDotD < decimal(0.0)) { + // If the ray int32_tersects with the "p" endcap of the capsule + if (mDotD < float(0.0)) { - // Check intersection between the ray and the "p" sphere endcap of the capsule - Vector3 hitLocalPoint; - decimal hitFraction; - if (raycastWithSphereEndCap(ray.point1, ray.point2, p, ray.maxFraction, hitLocalPoint, hitFraction)) { - raycastInfo.body = proxyShape->getBody(); - raycastInfo.proxyShape = proxyShape; - raycastInfo.hitFraction = hitFraction; - raycastInfo.worldPoint = hitLocalPoint; - Vector3 normalDirection = hitLocalPoint - p; - raycastInfo.worldNormal = normalDirection; + // Check int32_tersection between the ray and the "p" sphere endcap of the capsule + Vector3 hitLocalPoint; + float hitFraction; + if (raycastWithSphereEndCap(ray.point1, ray.point2, p, ray.maxFraction, hitLocalPoint, hitFraction)) { + raycastInfo.body = proxyShape->getBody(); + raycastInfo.proxyShape = proxyShape; + raycastInfo.hitFraction = hitFraction; + raycastInfo.worldPoint = hitLocalPoint; + Vector3 normalDirection = hitLocalPoint - p; + raycastInfo.worldNormal = normalDirection; - return true; - } + return true; + } - return false; - } - else if (mDotD > dDotD) { // If the ray intersects with the "q" endcap of the cylinder + return false; + } + else if (mDotD > dDotD) { // If the ray int32_tersects with the "q" endcap of the cylinder - // Check intersection between the ray and the "q" sphere endcap of the capsule - Vector3 hitLocalPoint; - decimal hitFraction; - if (raycastWithSphereEndCap(ray.point1, ray.point2, q, ray.maxFraction, hitLocalPoint, hitFraction)) { - raycastInfo.body = proxyShape->getBody(); - raycastInfo.proxyShape = proxyShape; - raycastInfo.hitFraction = hitFraction; - raycastInfo.worldPoint = hitLocalPoint; - Vector3 normalDirection = hitLocalPoint - q; - raycastInfo.worldNormal = normalDirection; + // Check int32_tersection between the ray and the "q" sphere endcap of the capsule + Vector3 hitLocalPoint; + float hitFraction; + if (raycastWithSphereEndCap(ray.point1, ray.point2, q, ray.maxFraction, hitLocalPoint, hitFraction)) { + raycastInfo.body = proxyShape->getBody(); + raycastInfo.proxyShape = proxyShape; + raycastInfo.hitFraction = hitFraction; + raycastInfo.worldPoint = hitLocalPoint; + Vector3 normalDirection = hitLocalPoint - q; + raycastInfo.worldNormal = normalDirection; - return true; - } + return true; + } - return false; - } - else { // If the origin is inside the cylinder, we return no hit - return false; - } - } - decimal b = dDotD * mDotN - nDotD * mDotD; - decimal discriminant = b * b - a * c; + return false; + } + else { // If the origin is inside the cylinder, we return no hit + return false; + } + } + float b = dDotD * mDotN - nDotD * mDotD; + float discriminant = b * b - a * c; - // If the discriminant is negative, no real roots and therfore, no hit - if (discriminant < decimal(0.0)) return false; + // If the discriminant is negative, no real roots and therfore, no hit + if (discriminant < float(0.0)) return false; - // Compute the smallest root (first intersection along the ray) - decimal t0 = t = (-b - std::sqrt(discriminant)) / a; + // Compute the smallest root (first int32_tersection along the ray) + float t0 = t = (-b - std::sqrt(discriminant)) / a; - // If the intersection is outside the finite cylinder of the capsule on "p" endcap side - decimal value = mDotD + t * nDotD; - if (value < decimal(0.0)) { + // If the int32_tersection is outside the finite cylinder of the capsule on "p" endcap side + float value = mDotD + t * nDotD; + if (value < float(0.0)) { - // Check intersection between the ray and the "p" sphere endcap of the capsule - Vector3 hitLocalPoint; - decimal hitFraction; - if (raycastWithSphereEndCap(ray.point1, ray.point2, p, ray.maxFraction, hitLocalPoint, hitFraction)) { - raycastInfo.body = proxyShape->getBody(); - raycastInfo.proxyShape = proxyShape; - raycastInfo.hitFraction = hitFraction; - raycastInfo.worldPoint = hitLocalPoint; - Vector3 normalDirection = hitLocalPoint - p; - raycastInfo.worldNormal = normalDirection; + // Check int32_tersection between the ray and the "p" sphere endcap of the capsule + Vector3 hitLocalPoint; + float hitFraction; + if (raycastWithSphereEndCap(ray.point1, ray.point2, p, ray.maxFraction, hitLocalPoint, hitFraction)) { + raycastInfo.body = proxyShape->getBody(); + raycastInfo.proxyShape = proxyShape; + raycastInfo.hitFraction = hitFraction; + raycastInfo.worldPoint = hitLocalPoint; + Vector3 normalDirection = hitLocalPoint - p; + raycastInfo.worldNormal = normalDirection; - return true; - } + return true; + } - return false; - } - else if (value > dDotD) { // If the intersection is outside the finite cylinder on the "q" side + return false; + } + else if (value > dDotD) { // If the int32_tersection is outside the finite cylinder on the "q" side - // Check intersection between the ray and the "q" sphere endcap of the capsule - Vector3 hitLocalPoint; - decimal hitFraction; - if (raycastWithSphereEndCap(ray.point1, ray.point2, q, ray.maxFraction, hitLocalPoint, hitFraction)) { - raycastInfo.body = proxyShape->getBody(); - raycastInfo.proxyShape = proxyShape; - raycastInfo.hitFraction = hitFraction; - raycastInfo.worldPoint = hitLocalPoint; - Vector3 normalDirection = hitLocalPoint - q; - raycastInfo.worldNormal = normalDirection; + // Check int32_tersection between the ray and the "q" sphere endcap of the capsule + Vector3 hitLocalPoint; + float hitFraction; + if (raycastWithSphereEndCap(ray.point1, ray.point2, q, ray.maxFraction, hitLocalPoint, hitFraction)) { + raycastInfo.body = proxyShape->getBody(); + raycastInfo.proxyShape = proxyShape; + raycastInfo.hitFraction = hitFraction; + raycastInfo.worldPoint = hitLocalPoint; + Vector3 normalDirection = hitLocalPoint - q; + raycastInfo.worldNormal = normalDirection; - return true; - } + return true; + } - return false; - } + return false; + } - t = t0; + t = t0; - // If the intersection is behind the origin of the ray or beyond the maximum - // raycasting distance, we return no hit - if (t < decimal(0.0) || t > ray.maxFraction) return false; + // If the int32_tersection is behind the origin of the ray or beyond the maximum + // raycasting distance, we return no hit + if (t < float(0.0) || t > ray.maxFraction) return false; - // Compute the hit information - Vector3 localHitPoint = ray.point1 + t * n; - raycastInfo.body = proxyShape->getBody(); - raycastInfo.proxyShape = proxyShape; - raycastInfo.hitFraction = t; - raycastInfo.worldPoint = localHitPoint; - Vector3 v = localHitPoint - p; - Vector3 w = (v.dot(d) / d.lengthSquare()) * d; - Vector3 normalDirection = (localHitPoint - (p + w)).getUnit(); - raycastInfo.worldNormal = normalDirection; + // Compute the hit information + Vector3 localHitPoint = ray.point1 + t * n; + raycastInfo.body = proxyShape->getBody(); + raycastInfo.proxyShape = proxyShape; + raycastInfo.hitFraction = t; + raycastInfo.worldPoint = localHitPoint; + Vector3 v = localHitPoint - p; + Vector3 w = (v.dot(d) / d.lengthSquare()) * d; + Vector3 normalDirection = (localHitPoint - (p + w)).getUnit(); + raycastInfo.worldNormal = normalDirection; - return true; + return true; } // Raycasting method between a ray one of the two spheres end cap of the capsule bool CapsuleShape::raycastWithSphereEndCap(const Vector3& point1, const Vector3& point2, - const Vector3& sphereCenter, decimal maxFraction, - Vector3& hitLocalPoint, decimal& hitFraction) const { + const Vector3& sphereCenter, float maxFraction, + Vector3& hitLocalPoint, float& hitFraction) const { - const Vector3 m = point1 - sphereCenter; - decimal c = m.dot(m) - mMargin * mMargin; + const Vector3 m = point1 - sphereCenter; + float c = m.dot(m) - mMargin * mMargin; - // If the origin of the ray is inside the sphere, we return no intersection - if (c < decimal(0.0)) return false; + // If the origin of the ray is inside the sphere, we return no int32_tersection + if (c < float(0.0)) return false; - const Vector3 rayDirection = point2 - point1; - decimal b = m.dot(rayDirection); + const Vector3 rayDirection = point2 - point1; + float b = m.dot(rayDirection); - // If the origin of the ray is outside the sphere and the ray - // is pointing away from the sphere, there is no intersection - if (b > decimal(0.0)) return false; + // If the origin of the ray is outside the sphere and the ray + // is pointing away from the sphere, there is no int32_tersection + if (b > float(0.0)) return false; - decimal raySquareLength = rayDirection.lengthSquare(); + float raySquareLength = rayDirection.lengthSquare(); - // Compute the discriminant of the quadratic equation - decimal discriminant = b * b - raySquareLength * c; + // Compute the discriminant of the quadratic equation + float discriminant = b * b - raySquareLength * c; - // If the discriminant is negative or the ray length is very small, there is no intersection - if (discriminant < decimal(0.0) || raySquareLength < MACHINE_EPSILON) return false; + // If the discriminant is negative or the ray length is very small, there is no int32_tersection + if (discriminant < float(0.0) || raySquareLength < MACHINE_EPSILON) return false; - // Compute the solution "t" closest to the origin - decimal t = -b - std::sqrt(discriminant); + // Compute the solution "t" closest to the origin + float t = -b - std::sqrt(discriminant); - assert(t >= decimal(0.0)); + assert(t >= float(0.0)); - // If the hit point is withing the segment ray fraction - if (t < maxFraction * raySquareLength) { + // If the hit point is withing the segment ray fraction + if (t < maxFraction * raySquareLength) { - // Compute the intersection information - t /= raySquareLength; - hitFraction = t; - hitLocalPoint = point1 + t * rayDirection; + // Compute the int32_tersection information + t /= raySquareLength; + hitFraction = t; + hitLocalPoint = point1 + t * rayDirection; - return true; - } + return true; + } - return false; + return false; } diff --git a/ephysics/collision/shapes/CapsuleShape.h b/ephysics/collision/shapes/CapsuleShape.h index fc2f080..1fa995e 100644 --- a/ephysics/collision/shapes/CapsuleShape.h +++ b/ephysics/collision/shapes/CapsuleShape.h @@ -1,30 +1,9 @@ -/******************************************************************************** -* 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 REACTPHYSICS3D_CAPSULE_SHAPE_H -#define REACTPHYSICS3D_CAPSULE_SHAPE_H +/** @file + * @author Daniel Chappuis + * @copyright 2010-2016 Daniel Chappuis + * @license BSD 3 clauses (see license file) + */ +#pragma once // Libraries #include @@ -46,93 +25,93 @@ namespace reactphysics3d { */ class CapsuleShape : public ConvexShape { - protected : + protected : - // -------------------- Attributes -------------------- // + // -------------------- Attributes -------------------- // - /// Half height of the capsule (height = distance between the centers of the two spheres) - decimal mHalfHeight; + /// Half height of the capsule (height = distance between the centers of the two spheres) + float mHalfHeight; - // -------------------- Methods -------------------- // + // -------------------- Methods -------------------- // - /// Private copy-constructor - CapsuleShape(const CapsuleShape& shape); + /// Private copy-constructor + CapsuleShape(const CapsuleShape& shape); - /// Private assignment operator - CapsuleShape& operator=(const CapsuleShape& shape); + /// Private assignment operator + CapsuleShape& operator=(const CapsuleShape& shape); - /// Return a local support point in a given direction without the object margin - virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction, - void** cachedCollisionData) const; + /// Return a local support point in a given direction without the object margin + virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction, + void** cachedCollisionData) const; - /// Return true if a point is inside the collision shape - virtual bool testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const; + /// Return true if a point is inside the collision shape + virtual bool testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const; - /// Raycast method with feedback information - virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const; + /// Raycast method with feedback information + virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const; - /// Raycasting method between a ray one of the two spheres end cap of the capsule - bool raycastWithSphereEndCap(const Vector3& point1, const Vector3& point2, - const Vector3& sphereCenter, decimal maxFraction, - Vector3& hitLocalPoint, decimal& hitFraction) const; + /// Raycasting method between a ray one of the two spheres end cap of the capsule + bool raycastWithSphereEndCap(const Vector3& point1, const Vector3& point2, + const Vector3& sphereCenter, float maxFraction, + Vector3& hitLocalPoint, float& hitFraction) const; - /// Return the number of bytes used by the collision shape - virtual size_t getSizeInBytes() const; + /// Return the number of bytes used by the collision shape + virtual size_t getSizeInBytes() const; - public : + public : - // -------------------- Methods -------------------- // + // -------------------- Methods -------------------- // - /// Constructor - CapsuleShape(decimal radius, decimal height); + /// Constructor + CapsuleShape(float radius, float height); - /// Destructor - virtual ~CapsuleShape(); + /// Destructor + virtual ~CapsuleShape(); - /// Return the radius of the capsule - decimal getRadius() const; + /// Return the radius of the capsule + float getRadius() const; - /// Return the height of the capsule - decimal getHeight() const; + /// Return the height of the capsule + float getHeight() const; - /// Set the scaling vector of the collision shape - virtual void setLocalScaling(const Vector3& scaling); + /// Set the scaling vector of the collision shape + virtual void setLocalScaling(const Vector3& scaling); - /// Return the local bounds of the shape in x, y and z directions - virtual void getLocalBounds(Vector3& min, Vector3& max) const; + /// Return the local bounds of the shape in x, y and z directions + virtual void getLocalBounds(Vector3& min, Vector3& max) const; - /// Return the local inertia tensor of the collision shape - virtual void computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const; + /// Return the local inertia tensor of the collision shape + virtual void computeLocalInertiaTensor(Matrix3x3& tensor, float mass) const; }; // Get the radius of the capsule /** * @return The radius of the capsule shape (in meters) */ -inline decimal CapsuleShape::getRadius() const { - return mMargin; +inline float CapsuleShape::getRadius() const { + return mMargin; } // Return the height of the capsule /** * @return The height of the capsule shape (in meters) */ -inline decimal CapsuleShape::getHeight() const { - return mHalfHeight + mHalfHeight; +inline float CapsuleShape::getHeight() const { + return mHalfHeight + mHalfHeight; } // Set the scaling vector of the collision shape inline void CapsuleShape::setLocalScaling(const Vector3& scaling) { - mHalfHeight = (mHalfHeight / mScaling.y) * scaling.y; - mMargin = (mMargin / mScaling.x) * scaling.x; + mHalfHeight = (mHalfHeight / mScaling.y) * scaling.y; + mMargin = (mMargin / mScaling.x) * scaling.x; - CollisionShape::setLocalScaling(scaling); + CollisionShape::setLocalScaling(scaling); } // Return the number of bytes used by the collision shape inline size_t CapsuleShape::getSizeInBytes() const { - return sizeof(CapsuleShape); + return sizeof(CapsuleShape); } // Return the local bounds of the shape in x, y and z directions @@ -143,15 +122,15 @@ inline size_t CapsuleShape::getSizeInBytes() const { */ inline void CapsuleShape::getLocalBounds(Vector3& min, Vector3& max) const { - // Maximum bounds - max.x = mMargin; - max.y = mHalfHeight + mMargin; - max.z = mMargin; + // Maximum bounds + max.x = mMargin; + max.y = mHalfHeight + mMargin; + max.z = mMargin; - // Minimum bounds - min.x = -mMargin; - min.y = -max.y; - min.z = min.x; + // Minimum bounds + min.x = -mMargin; + min.y = -max.y; + min.z = min.x; } // Return a local support point in a given direction without the object margin. @@ -162,23 +141,21 @@ inline void CapsuleShape::getLocalBounds(Vector3& min, Vector3& max) const { /// 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, - void** cachedCollisionData) const { + void** cachedCollisionData) const { - // Support point top sphere - decimal dotProductTop = mHalfHeight * direction.y; + // Support point top sphere + float dotProductTop = mHalfHeight * direction.y; - // Support point bottom sphere - decimal dotProductBottom = -mHalfHeight * direction.y; + // Support point bottom sphere + float dotProductBottom = -mHalfHeight * direction.y; - // Return the point with the maximum dot product - if (dotProductTop > dotProductBottom) { - return Vector3(0, mHalfHeight, 0); - } - else { - return Vector3(0, -mHalfHeight, 0); - } + // Return the point with the maximum dot product + if (dotProductTop > dotProductBottom) { + return Vector3(0, mHalfHeight, 0); + } + else { + return Vector3(0, -mHalfHeight, 0); + } } } - -#endif diff --git a/ephysics/collision/shapes/CollisionShape.cpp b/ephysics/collision/shapes/CollisionShape.cpp index fe56181..2445555 100644 --- a/ephysics/collision/shapes/CollisionShape.cpp +++ b/ephysics/collision/shapes/CollisionShape.cpp @@ -1,27 +1,8 @@ -/******************************************************************************** -* 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. * -* * -********************************************************************************/ +/** @file + * @author Daniel Chappuis + * @copyright 2010-2016 Daniel Chappuis + * @license BSD 3 clauses (see license file) + */ // Libraries #include @@ -33,7 +14,7 @@ using namespace reactphysics3d; // Constructor CollisionShape::CollisionShape(CollisionShapeType type) : mType(type), mScaling(1.0, 1.0, 1.0) { - + } // Destructor @@ -44,32 +25,32 @@ CollisionShape::~CollisionShape() { // Compute the world-space AABB of the collision shape given a transform /** * @param[out] aabb The axis-aligned bounding box (AABB) of the collision shape - * computed in world-space coordinates + * computed in world-space coordinates * @param transform Transform used to compute the AABB of the collision shape */ void CollisionShape::computeAABB(AABB& aabb, const Transform& transform) const { - PROFILE("CollisionShape::computeAABB()"); + PROFILE("CollisionShape::computeAABB()"); - // Get the local bounds in x,y and z direction - Vector3 minBounds; - Vector3 maxBounds; - getLocalBounds(minBounds, maxBounds); + // Get the local bounds in x,y and z direction + Vector3 minBounds; + Vector3 maxBounds; + getLocalBounds(minBounds, maxBounds); - // Rotate the local bounds according to the orientation of the body - Matrix3x3 worldAxis = transform.getOrientation().getMatrix().getAbsoluteMatrix(); - Vector3 worldMinBounds(worldAxis.getColumn(0).dot(minBounds), - worldAxis.getColumn(1).dot(minBounds), - worldAxis.getColumn(2).dot(minBounds)); - Vector3 worldMaxBounds(worldAxis.getColumn(0).dot(maxBounds), - worldAxis.getColumn(1).dot(maxBounds), - worldAxis.getColumn(2).dot(maxBounds)); + // Rotate the local bounds according to the orientation of the body + Matrix3x3 worldAxis = transform.getOrientation().getMatrix().getAbsoluteMatrix(); + Vector3 worldMinBounds(worldAxis.getColumn(0).dot(minBounds), + worldAxis.getColumn(1).dot(minBounds), + worldAxis.getColumn(2).dot(minBounds)); + Vector3 worldMaxBounds(worldAxis.getColumn(0).dot(maxBounds), + worldAxis.getColumn(1).dot(maxBounds), + worldAxis.getColumn(2).dot(maxBounds)); - // Compute the minimum and maximum coordinates of the rotated extents - Vector3 minCoordinates = transform.getPosition() + worldMinBounds; - Vector3 maxCoordinates = transform.getPosition() + worldMaxBounds; + // Compute the minimum and maximum coordinates of the rotated extents + Vector3 minCoordinates = transform.getPosition() + worldMinBounds; + Vector3 maxCoordinates = transform.getPosition() + worldMaxBounds; - // Update the AABB with the new minimum and maximum coordinates - aabb.setMin(minCoordinates); - aabb.setMax(maxCoordinates); + // Update the AABB with the new minimum and maximum coordinates + aabb.setMin(minCoordinates); + aabb.setMax(maxCoordinates); } diff --git a/ephysics/collision/shapes/CollisionShape.h b/ephysics/collision/shapes/CollisionShape.h index 327a46d..1e20475 100644 --- a/ephysics/collision/shapes/CollisionShape.h +++ b/ephysics/collision/shapes/CollisionShape.h @@ -1,30 +1,9 @@ -/******************************************************************************** -* 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 REACTPHYSICS3D_COLLISION_SHAPE_H -#define REACTPHYSICS3D_COLLISION_SHAPE_H +/** @file + * @author Daniel Chappuis + * @copyright 2010-2016 Daniel Chappuis + * @license BSD 3 clauses (see license file) + */ +#pragma once // Libraries #include @@ -38,11 +17,11 @@ /// ReactPhysics3D namespace namespace reactphysics3d { - + /// Type of the collision shape enum CollisionShapeType {TRIANGLE, BOX, SPHERE, CONE, CYLINDER, - CAPSULE, CONVEX_MESH, CONCAVE_MESH, HEIGHTFIELD}; -const int NB_COLLISION_SHAPE_TYPES = 9; + CAPSULE, CONVEX_MESH, CONCAVE_MESH, HEIGHTFIELD}; +const int32_t NB_COLLISION_SHAPE_TYPES = 9; // Declarations class ProxyShape; @@ -54,76 +33,76 @@ class CollisionBody; * body that is used during the narrow-phase collision detection. */ class CollisionShape { - - protected : + + protected : - // -------------------- Attributes -------------------- // + // -------------------- Attributes -------------------- // - /// Type of the collision shape - CollisionShapeType mType; + /// Type of the collision shape + CollisionShapeType mType; - /// Scaling vector of the collision shape - Vector3 mScaling; - - // -------------------- Methods -------------------- // + /// Scaling vector of the collision shape + Vector3 mScaling; + + // -------------------- Methods -------------------- // - /// Private copy-constructor - CollisionShape(const CollisionShape& shape); + /// Private copy-constructor + CollisionShape(const CollisionShape& shape); - /// Private assignment operator - CollisionShape& operator=(const CollisionShape& shape); + /// Private assignment operator + CollisionShape& operator=(const CollisionShape& shape); - /// Return true if a point is inside the collision shape - virtual bool testPointInside(const Vector3& worldPoint, ProxyShape* proxyShape) const=0; + /// Return true if a point is inside the collision shape + virtual bool testPointInside(const Vector3& worldPoint, ProxyShape* proxyShape) const=0; - /// Raycast method with feedback information - virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const=0; + /// Raycast method with feedback information + virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const=0; - /// Return the number of bytes used by the collision shape - virtual size_t getSizeInBytes() const = 0; + /// Return the number of bytes used by the collision shape + virtual size_t getSizeInBytes() const = 0; - public : + public : - // -------------------- Methods -------------------- // + // -------------------- Methods -------------------- // - /// Constructor - CollisionShape(CollisionShapeType type); + /// Constructor + CollisionShape(CollisionShapeType type); - /// Destructor - virtual ~CollisionShape(); + /// Destructor + virtual ~CollisionShape(); - /// Return the type of the collision shapes - CollisionShapeType getType() const; + /// Return the type of the collision shapes + CollisionShapeType getType() const; - /// Return true if the collision shape is convex, false if it is concave - virtual bool isConvex() const=0; + /// Return true if the collision shape is convex, false if it is concave + virtual bool isConvex() const=0; - /// Return the local bounds of the shape in x, y and z directions - virtual void getLocalBounds(Vector3& min, Vector3& max) const=0; + /// Return the local bounds of the shape in x, y and z directions + virtual void getLocalBounds(Vector3& min, Vector3& max) const=0; - /// Return the scaling vector of the collision shape - Vector3 getScaling() const; + /// Return the scaling vector of the collision shape + Vector3 getScaling() const; - /// Set the local scaling vector of the collision shape - virtual void setLocalScaling(const Vector3& scaling); + /// Set the local scaling vector of the collision shape + virtual void setLocalScaling(const Vector3& scaling); - /// Return the local inertia tensor of the collision shapes - virtual void computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const=0; + /// Return the local inertia tensor of the collision shapes + virtual void computeLocalInertiaTensor(Matrix3x3& tensor, float mass) const=0; - /// Compute the world-space AABB of the collision shape given a transform - virtual void computeAABB(AABB& aabb, const Transform& transform) const; + /// Compute the world-space AABB of the collision shape given a transform + virtual void computeAABB(AABB& aabb, const Transform& transform) const; - /// Return true if the collision shape type is a convex shape - static bool isConvex(CollisionShapeType shapeType); + /// Return true if the collision shape type is a convex shape + static bool isConvex(CollisionShapeType shapeType); - /// Return the maximum number of contact manifolds in an overlapping pair given two shape types - static int computeNbMaxContactManifolds(CollisionShapeType shapeType1, - CollisionShapeType shapeType2); + /// Return the maximum number of contact manifolds in an overlapping pair given two shape types + static int32_t computeNbMaxContactManifolds(CollisionShapeType shapeType1, + CollisionShapeType shapeType2); - // -------------------- Friendship -------------------- // + // -------------------- Friendship -------------------- // - friend class ProxyShape; - friend class CollisionWorld; + friend class ProxyShape; + friend class CollisionWorld; }; // Return the type of the collision shape @@ -131,37 +110,36 @@ class CollisionShape { * @return The type of the collision shape (box, sphere, cylinder, ...) */ inline CollisionShapeType CollisionShape::getType() const { - return mType; + return mType; } // Return true if the collision shape type is a convex shape inline bool CollisionShape::isConvex(CollisionShapeType shapeType) { - return shapeType != CONCAVE_MESH && shapeType != HEIGHTFIELD; + return shapeType != CONCAVE_MESH && shapeType != HEIGHTFIELD; } // Return the scaling vector of the collision shape inline Vector3 CollisionShape::getScaling() const { - return mScaling; + return mScaling; } // Set the scaling vector of the collision shape inline void CollisionShape::setLocalScaling(const Vector3& scaling) { - mScaling = scaling; + mScaling = scaling; } // Return the maximum number of contact manifolds allowed in an overlapping // pair wit the given two collision shape types -inline int CollisionShape::computeNbMaxContactManifolds(CollisionShapeType shapeType1, - CollisionShapeType shapeType2) { - // If both shapes are convex - if (isConvex(shapeType1) && isConvex(shapeType2)) { - return NB_MAX_CONTACT_MANIFOLDS_CONVEX_SHAPE; - } // If there is at least one concave shape - else { - return NB_MAX_CONTACT_MANIFOLDS_CONCAVE_SHAPE; - } +inline int32_t CollisionShape::computeNbMaxContactManifolds(CollisionShapeType shapeType1, + CollisionShapeType shapeType2) { + // If both shapes are convex + if (isConvex(shapeType1) && isConvex(shapeType2)) { + return NB_MAX_CONTACT_MANIFOLDS_CONVEX_SHAPE; + } // If there is at least one concave shape + else { + return NB_MAX_CONTACT_MANIFOLDS_CONCAVE_SHAPE; + } } } -#endif diff --git a/ephysics/collision/shapes/ConcaveMeshShape.cpp b/ephysics/collision/shapes/ConcaveMeshShape.cpp index 2d69d4b..e9c39e7 100644 --- a/ephysics/collision/shapes/ConcaveMeshShape.cpp +++ b/ephysics/collision/shapes/ConcaveMeshShape.cpp @@ -1,27 +1,8 @@ -/******************************************************************************** -* 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. * -* * -********************************************************************************/ +/** @file + * @author Daniel Chappuis + * @copyright 2010-2016 Daniel Chappuis + * @license BSD 3 clauses (see license file) + */ // Libraries #include @@ -34,7 +15,7 @@ ConcaveMeshShape::ConcaveMeshShape(TriangleMesh* triangleMesh): ConcaveShape(CONCAVE_MESH) { m_triangleMesh = triangleMesh; m_raycastTestType = FRONT; - // Insert all the triangles into the dynamic AABB tree + // Insert all the triangles int32_to the dynamic AABB tree initBVHTree(); } @@ -43,29 +24,29 @@ ConcaveMeshShape::~ConcaveMeshShape() { } -// Insert all the triangles into the dynamic AABB tree +// Insert all the triangles int32_to the dynamic AABB tree void ConcaveMeshShape::initBVHTree() { - // TODO : Try to randomly add the triangles into the tree to obtain a better tree + // TODO : Try to randomly add the triangles int32_to the tree to obtain a better tree // For each sub-part of the mesh - for (uint subPart=0; subPartgetNbSubparts(); subPart++) { + for (uint32_t subPart=0; subPartgetNbSubparts(); subPart++) { // Get the triangle vertex array of the current sub-part TriangleVertexArray* triangleVertexArray = m_triangleMesh->getSubpart(subPart); TriangleVertexArray::VertexDataType vertexType = triangleVertexArray->getVertexDataType(); TriangleVertexArray::IndexDataType indexType = triangleVertexArray->getIndexDataType(); unsigned char* verticesStart = triangleVertexArray->getVerticesStart(); unsigned char* indicesStart = triangleVertexArray->getIndicesStart(); - int vertexStride = triangleVertexArray->getVerticesStride(); - int indexStride = triangleVertexArray->getIndicesStride(); + int32_t vertexStride = triangleVertexArray->getVerticesStride(); + int32_t indexStride = triangleVertexArray->getIndicesStride(); // For each triangle of the concave mesh - for (uint triangleIndex=0; triangleIndexgetNbTriangles(); triangleIndex++) { + for (uint32_t triangleIndex=0; triangleIndexgetNbTriangles(); triangleIndex++) { void* vertexIndexPointer = (indicesStart + triangleIndex * 3 * indexStride); Vector3 trianglePoints[3]; // For each vertex of the triangle - for (int k=0; k < 3; k++) { + for (int32_t k=0; k < 3; k++) { // Get the index of the current vertex in the triangle - int vertexIndex = 0; + int32_t vertexIndex = 0; if (indexType == TriangleVertexArray::INDEX_INTEGER_TYPE) { - vertexIndex = ((uint*)vertexIndexPointer)[k]; + vertexIndex = ((uint32_t*)vertexIndexPointer)[k]; } else if (indexType == TriangleVertexArray::INDEX_SHORT_TYPE) { vertexIndex = ((unsigned short*)vertexIndexPointer)[k]; } else { @@ -74,14 +55,14 @@ void ConcaveMeshShape::initBVHTree() { // Get the vertices components of the triangle if (vertexType == TriangleVertexArray::VERTEX_FLOAT_TYPE) { const float* vertices = (float*)(verticesStart + vertexIndex * vertexStride); - trianglePoints[k][0] = decimal(vertices[0]) * mScaling.x; - trianglePoints[k][1] = decimal(vertices[1]) * mScaling.y; - trianglePoints[k][2] = decimal(vertices[2]) * mScaling.z; + trianglePoints[k][0] = float(vertices[0]) * mScaling.x; + trianglePoints[k][1] = float(vertices[1]) * mScaling.y; + trianglePoints[k][2] = float(vertices[2]) * mScaling.z; } else if (vertexType == TriangleVertexArray::VERTEX_DOUBLE_TYPE) { const double* vertices = (double*)(verticesStart + vertexIndex * vertexStride); - trianglePoints[k][0] = decimal(vertices[0]) * mScaling.x; - trianglePoints[k][1] = decimal(vertices[1]) * mScaling.y; - trianglePoints[k][2] = decimal(vertices[2]) * mScaling.z; + trianglePoints[k][0] = float(vertices[0]) * mScaling.x; + trianglePoints[k][1] = float(vertices[1]) * mScaling.y; + trianglePoints[k][2] = float(vertices[2]) * mScaling.z; } else { assert(false); } @@ -89,7 +70,7 @@ void ConcaveMeshShape::initBVHTree() { // Create the AABB for the triangle AABB aabb = AABB::createAABBForTriangle(trianglePoints); aabb.inflate(m_triangleMargin, m_triangleMargin, m_triangleMargin); - // Add the AABB with the index of the triangle into the dynamic AABB tree + // Add the AABB with the index of the triangle int32_to the dynamic AABB tree m_dynamicAABBTree.addObject(aabb, subPart, triangleIndex); } } @@ -97,7 +78,7 @@ void ConcaveMeshShape::initBVHTree() { // Return the three vertices coordinates (in the array outTriangleVertices) of a triangle // given the start vertex index pointer of the triangle -void ConcaveMeshShape::getTriangleVerticesWithIndexPointer(int32 subPart, int32 triangleIndex, Vector3* outTriangleVertices) const { +void ConcaveMeshShape::getTriangleVerticesWithIndexPointer(int32_t subPart, int32_t triangleIndex, Vector3* outTriangleVertices) const { // Get the triangle vertex array of the current sub-part TriangleVertexArray* triangleVertexArray = m_triangleMesh->getSubpart(subPart); if (triangleVertexArray == nullptr) { @@ -107,15 +88,15 @@ void ConcaveMeshShape::getTriangleVerticesWithIndexPointer(int32 subPart, int32 TriangleVertexArray::IndexDataType indexType = triangleVertexArray->getIndexDataType(); unsigned char* verticesStart = triangleVertexArray->getVerticesStart(); unsigned char* indicesStart = triangleVertexArray->getIndicesStart(); - int vertexStride = triangleVertexArray->getVerticesStride(); - int indexStride = triangleVertexArray->getIndicesStride(); + int32_t vertexStride = triangleVertexArray->getVerticesStride(); + int32_t indexStride = triangleVertexArray->getIndicesStride(); void* vertexIndexPointer = (indicesStart + triangleIndex * 3 * indexStride); // For each vertex of the triangle - for (int k=0; k < 3; k++) { + for (int32_t k=0; k < 3; k++) { // Get the index of the current vertex in the triangle - int vertexIndex = 0; + int32_t vertexIndex = 0; if (indexType == TriangleVertexArray::INDEX_INTEGER_TYPE) { - vertexIndex = ((uint*)vertexIndexPointer)[k]; + vertexIndex = ((uint32_t*)vertexIndexPointer)[k]; } else if (indexType == TriangleVertexArray::INDEX_SHORT_TYPE) { vertexIndex = ((unsigned short*)vertexIndexPointer)[k]; } else { @@ -125,14 +106,14 @@ void ConcaveMeshShape::getTriangleVerticesWithIndexPointer(int32 subPart, int32 // Get the vertices components of the triangle if (vertexType == TriangleVertexArray::VERTEX_FLOAT_TYPE) { const float* vertices = (float*)(verticesStart + vertexIndex * vertexStride); - outTriangleVertices[k][0] = decimal(vertices[0]) * mScaling.x; - outTriangleVertices[k][1] = decimal(vertices[1]) * mScaling.y; - outTriangleVertices[k][2] = decimal(vertices[2]) * mScaling.z; + outTriangleVertices[k][0] = float(vertices[0]) * mScaling.x; + outTriangleVertices[k][1] = float(vertices[1]) * mScaling.y; + outTriangleVertices[k][2] = float(vertices[2]) * mScaling.z; } else if (vertexType == TriangleVertexArray::VERTEX_DOUBLE_TYPE) { const double* vertices = (double*)(verticesStart + vertexIndex * vertexStride); - outTriangleVertices[k][0] = decimal(vertices[0]) * mScaling.x; - outTriangleVertices[k][1] = decimal(vertices[1]) * mScaling.y; - outTriangleVertices[k][2] = decimal(vertices[2]) * mScaling.z; + outTriangleVertices[k][0] = float(vertices[0]) * mScaling.x; + outTriangleVertices[k][1] = float(vertices[1]) * mScaling.y; + outTriangleVertices[k][2] = float(vertices[2]) * mScaling.z; } else { assert(false); } @@ -163,24 +144,24 @@ bool ConcaveMeshShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxySh } // Collect all the AABB nodes that are hit by the ray in the Dynamic AABB Tree -decimal ConcaveMeshRaycastCallback::raycastBroadPhaseShape(int32 nodeId, const Ray& ray) { - // Add the id of the hit AABB node into +float ConcaveMeshRaycastCallback::raycastBroadPhaseShape(int32_t nodeId, const Ray& ray) { + // Add the id of the hit AABB node int32_to m_hitAABBNodes.push_back(nodeId); return ray.maxFraction; } // Raycast all collision shapes that have been collected void ConcaveMeshRaycastCallback::raycastTriangles() { - std::vector::const_iterator it; - decimal smallestHitFraction = m_ray.maxFraction; + std::vector::const_iterator it; + float smallestHitFraction = m_ray.maxFraction; for (it = m_hitAABBNodes.begin(); it != m_hitAABBNodes.end(); ++it) { // Get the node data (triangle index and mesh subpart index) - int32* data = m_dynamicAABBTree.getNodeDataInt(*it); + int32_t* data = m_dynamicAABBTree.getNodeDataInt(*it); // Get the triangle vertices for this node from the concave mesh shape Vector3 trianglePoints[3]; m_concaveMeshShape.getTriangleVerticesWithIndexPointer(data[0], data[1], trianglePoints); // Create a triangle collision shape - decimal margin = m_concaveMeshShape.getTriangleMargin(); + float margin = m_concaveMeshShape.getTriangleMargin(); TriangleShape triangleShape(trianglePoints[0], trianglePoints[1], trianglePoints[2], margin); triangleShape.setRaycastTestType(m_concaveMeshShape.getRaycastTestType()); // Ray casting test against the collision shape @@ -188,7 +169,7 @@ void ConcaveMeshRaycastCallback::raycastTriangles() { bool isTriangleHit = triangleShape.raycast(m_ray, raycastInfo, m_proxyShape); // If the ray hit the collision shape if (isTriangleHit && raycastInfo.hitFraction <= smallestHitFraction) { - assert(raycastInfo.hitFraction >= decimal(0.0)); + assert(raycastInfo.hitFraction >= float(0.0)); m_raycastInfo.body = raycastInfo.body; m_raycastInfo.proxyShape = raycastInfo.proxyShape; m_raycastInfo.hitFraction = raycastInfo.hitFraction; diff --git a/ephysics/collision/shapes/ConcaveMeshShape.h b/ephysics/collision/shapes/ConcaveMeshShape.h index 96ee311..5f93ca9 100644 --- a/ephysics/collision/shapes/ConcaveMeshShape.h +++ b/ephysics/collision/shapes/ConcaveMeshShape.h @@ -1,30 +1,9 @@ -/******************************************************************************** -* 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 REACTPHYSICS3D_CONCAVE_MESH_SHAPE_H -#define REACTPHYSICS3D_CONCAVE_MESH_SHAPE_H +/** @file + * @author Daniel Chappuis + * @copyright 2010-2016 Daniel Chappuis + * @license BSD 3 clauses (see license file) + */ +#pragma once // Libraries #include @@ -40,64 +19,64 @@ class ConcaveMeshShape; // class ConvexTriangleAABBOverlapCallback class ConvexTriangleAABBOverlapCallback : public DynamicAABBTreeOverlapCallback { - private: + private: - TriangleCallback& m_triangleTestCallback; + TriangleCallback& m_triangleTestCallback; - // Reference to the concave mesh shape - const ConcaveMeshShape& m_concaveMeshShape; + // Reference to the concave mesh shape + const ConcaveMeshShape& m_concaveMeshShape; - // Reference to the Dynamic AABB tree - const DynamicAABBTree& m_dynamicAABBTree; + // Reference to the Dynamic AABB tree + const DynamicAABBTree& m_dynamicAABBTree; - public: + public: - // Constructor - ConvexTriangleAABBOverlapCallback(TriangleCallback& triangleCallback, const ConcaveMeshShape& concaveShape, - const DynamicAABBTree& dynamicAABBTree) - : m_triangleTestCallback(triangleCallback), m_concaveMeshShape(concaveShape), m_dynamicAABBTree(dynamicAABBTree) { + // Constructor + ConvexTriangleAABBOverlapCallback(TriangleCallback& triangleCallback, const ConcaveMeshShape& concaveShape, + const DynamicAABBTree& dynamicAABBTree) + : m_triangleTestCallback(triangleCallback), m_concaveMeshShape(concaveShape), m_dynamicAABBTree(dynamicAABBTree) { - } + } - // Called when a overlapping node has been found during the call to - // DynamicAABBTree:reportAllShapesOverlappingWithAABB() - virtual void notifyOverlappingNode(int nodeId); + // Called when a overlapping node has been found during the call to + // DynamicAABBTree:reportAllShapesOverlappingWithAABB() + virtual void notifyOverlappingNode(int32_t nodeId); }; /// Class ConcaveMeshRaycastCallback class ConcaveMeshRaycastCallback : public DynamicAABBTreeRaycastCallback { - private : + private : - std::vector m_hitAABBNodes; - const DynamicAABBTree& m_dynamicAABBTree; - const ConcaveMeshShape& m_concaveMeshShape; - ProxyShape* m_proxyShape; - RaycastInfo& m_raycastInfo; - const Ray& m_ray; - bool mIsHit; + std::vector m_hitAABBNodes; + const DynamicAABBTree& m_dynamicAABBTree; + const ConcaveMeshShape& m_concaveMeshShape; + ProxyShape* m_proxyShape; + RaycastInfo& m_raycastInfo; + const Ray& m_ray; + bool mIsHit; - public: + public: - // Constructor - ConcaveMeshRaycastCallback(const DynamicAABBTree& dynamicAABBTree, const ConcaveMeshShape& concaveMeshShape, - ProxyShape* proxyShape, RaycastInfo& raycastInfo, const Ray& ray) - : m_dynamicAABBTree(dynamicAABBTree), m_concaveMeshShape(concaveMeshShape), m_proxyShape(proxyShape), - m_raycastInfo(raycastInfo), m_ray(ray), mIsHit(false) { + // Constructor + ConcaveMeshRaycastCallback(const DynamicAABBTree& dynamicAABBTree, const ConcaveMeshShape& concaveMeshShape, + ProxyShape* proxyShape, RaycastInfo& raycastInfo, const Ray& ray) + : m_dynamicAABBTree(dynamicAABBTree), m_concaveMeshShape(concaveMeshShape), m_proxyShape(proxyShape), + m_raycastInfo(raycastInfo), m_ray(ray), mIsHit(false) { - } + } - /// Collect all the AABB nodes that are hit by the ray in the Dynamic AABB Tree - virtual decimal raycastBroadPhaseShape(int32 nodeId, const Ray& ray); + /// Collect all the AABB nodes that are hit by the ray in the Dynamic AABB Tree + virtual float raycastBroadPhaseShape(int32_t nodeId, const Ray& ray); - /// Raycast all collision shapes that have been collected - void raycastTriangles(); + /// Raycast all collision shapes that have been collected + void raycastTriangles(); - /// Return true if a raycast hit has been found - bool getIsHit() const { - return mIsHit; - } + /// Return true if a raycast hit has been found + bool getIsHit() const { + return mIsHit; + } }; // Class ConcaveMeshShape @@ -108,67 +87,67 @@ class ConcaveMeshRaycastCallback : public DynamicAABBTreeRaycastCallback { */ class ConcaveMeshShape : public ConcaveShape { - protected: + protected: - // -------------------- Attributes -------------------- // + // -------------------- Attributes -------------------- // - /// Triangle mesh - TriangleMesh* m_triangleMesh; + /// Triangle mesh + TriangleMesh* m_triangleMesh; - /// Dynamic AABB tree to accelerate collision with the triangles - DynamicAABBTree m_dynamicAABBTree; + /// Dynamic AABB tree to accelerate collision with the triangles + DynamicAABBTree m_dynamicAABBTree; - // -------------------- Methods -------------------- // + // -------------------- Methods -------------------- // - /// Private copy-constructor - ConcaveMeshShape(const ConcaveMeshShape& shape); + /// Private copy-constructor + ConcaveMeshShape(const ConcaveMeshShape& shape); - /// Private assignment operator - ConcaveMeshShape& operator=(const ConcaveMeshShape& shape); + /// Private assignment operator + ConcaveMeshShape& operator=(const ConcaveMeshShape& shape); - /// Raycast method with feedback information - virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const; + /// Raycast method with feedback information + virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const; - /// Return the number of bytes used by the collision shape - virtual size_t getSizeInBytes() const; + /// Return the number of bytes used by the collision shape + virtual size_t getSizeInBytes() const; - /// Insert all the triangles into the dynamic AABB tree - void initBVHTree(); + /// Insert all the triangles int32_to the dynamic AABB tree + void initBVHTree(); - /// Return the three vertices coordinates (in the array outTriangleVertices) of a triangle - /// given the start vertex index pointer of the triangle. - void getTriangleVerticesWithIndexPointer(int32 subPart, int32 triangleIndex, - Vector3* outTriangleVertices) const; + /// Return the three vertices coordinates (in the array outTriangleVertices) of a triangle + /// given the start vertex index pointer of the triangle. + void getTriangleVerticesWithIndexPointer(int32_t subPart, int32_t triangleIndex, + Vector3* outTriangleVertices) const; - public: + public: - /// Constructor - ConcaveMeshShape(TriangleMesh* triangleMesh); + /// Constructor + ConcaveMeshShape(TriangleMesh* triangleMesh); - /// Destructor - ~ConcaveMeshShape(); + /// Destructor + ~ConcaveMeshShape(); - /// Return the local bounds of the shape in x, y and z directions. - virtual void getLocalBounds(Vector3& min, Vector3& max) const; + /// Return the local bounds of the shape in x, y and z directions. + virtual void getLocalBounds(Vector3& min, Vector3& max) const; - /// Set the local scaling vector of the collision shape - virtual void setLocalScaling(const Vector3& scaling); + /// Set the local scaling vector of the collision shape + virtual void setLocalScaling(const Vector3& scaling); - /// Return the local inertia tensor of the collision shape - virtual void computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const; + /// Return the local inertia tensor of the collision shape + virtual void computeLocalInertiaTensor(Matrix3x3& tensor, float mass) const; - /// Use a callback method on all triangles of the concave shape inside a given AABB - virtual void testAllTriangles(TriangleCallback& callback, const AABB& localAABB) const; + /// Use a callback method on all triangles of the concave shape inside a given AABB + virtual void testAllTriangles(TriangleCallback& callback, const AABB& localAABB) const; - // ---------- Friendship ----------- // + // ---------- Friendship ----------- // - friend class ConvexTriangleAABBOverlapCallback; - friend class ConcaveMeshRaycastCallback; + friend class ConvexTriangleAABBOverlapCallback; + friend class ConcaveMeshRaycastCallback; }; // Return the number of bytes used by the collision shape inline size_t ConcaveMeshShape::getSizeInBytes() const { - return sizeof(ConcaveMeshShape); + return sizeof(ConcaveMeshShape); } // Return the local bounds of the shape in x, y and z directions. @@ -179,57 +158,55 @@ inline size_t ConcaveMeshShape::getSizeInBytes() const { */ inline void ConcaveMeshShape::getLocalBounds(Vector3& min, Vector3& max) const { - // Get the AABB of the whole tree - AABB treeAABB = m_dynamicAABBTree.getRootAABB(); + // Get the AABB of the whole tree + AABB treeAABB = m_dynamicAABBTree.getRootAABB(); - min = treeAABB.getMin(); - max = treeAABB.getMax(); + min = treeAABB.getMin(); + max = treeAABB.getMax(); } // Set the local scaling vector of the collision shape inline void ConcaveMeshShape::setLocalScaling(const Vector3& scaling) { - CollisionShape::setLocalScaling(scaling); + CollisionShape::setLocalScaling(scaling); - // Reset the Dynamic AABB Tree - m_dynamicAABBTree.reset(); + // Reset the Dynamic AABB Tree + m_dynamicAABBTree.reset(); - // Rebuild Dynamic AABB Tree here - initBVHTree(); + // Rebuild Dynamic AABB Tree here + initBVHTree(); } // Return the local inertia tensor of the shape /** * @param[out] tensor The 3x3 inertia tensor matrix of the shape in local-space - * coordinates + * coordinates * @param mass Mass to use to compute the inertia tensor of the collision shape */ -inline void ConcaveMeshShape::computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const { +inline void ConcaveMeshShape::computeLocalInertiaTensor(Matrix3x3& tensor, float mass) const { - // Default inertia tensor - // Note that this is not very realistic for a concave triangle mesh. - // However, in most cases, it will only be used static bodies and therefore, - // the inertia tensor is not used. - tensor.setAllValues(mass, 0, 0, - 0, mass, 0, - 0, 0, mass); + // Default inertia tensor + // Note that this is not very realistic for a concave triangle mesh. + // However, in most cases, it will only be used static bodies and therefore, + // the inertia tensor is not used. + tensor.setAllValues(mass, 0, 0, + 0, mass, 0, + 0, 0, mass); } // Called when a overlapping node has been found during the call to // DynamicAABBTree:reportAllShapesOverlappingWithAABB() -inline void ConvexTriangleAABBOverlapCallback::notifyOverlappingNode(int nodeId) { +inline void ConvexTriangleAABBOverlapCallback::notifyOverlappingNode(int32_t nodeId) { - // Get the node data (triangle index and mesh subpart index) - int32* data = m_dynamicAABBTree.getNodeDataInt(nodeId); + // Get the node data (triangle index and mesh subpart index) + int32_t* data = m_dynamicAABBTree.getNodeDataInt(nodeId); - // Get the triangle vertices for this node from the concave mesh shape - Vector3 trianglePoints[3]; - m_concaveMeshShape.getTriangleVerticesWithIndexPointer(data[0], data[1], trianglePoints); + // Get the triangle vertices for this node from the concave mesh shape + Vector3 trianglePoints[3]; + m_concaveMeshShape.getTriangleVerticesWithIndexPointer(data[0], data[1], trianglePoints); - // Call the callback to test narrow-phase collision with this triangle - m_triangleTestCallback.testTriangle(trianglePoints); + // Call the callback to test narrow-phase collision with this triangle + m_triangleTestCallback.testTriangle(trianglePoints); } } -#endif - diff --git a/ephysics/collision/shapes/ConcaveShape.cpp b/ephysics/collision/shapes/ConcaveShape.cpp index 4591ab4..b03749d 100644 --- a/ephysics/collision/shapes/ConcaveShape.cpp +++ b/ephysics/collision/shapes/ConcaveShape.cpp @@ -1,27 +1,8 @@ -/******************************************************************************** -* 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. * -* * -********************************************************************************/ +/** @file + * @author Daniel Chappuis + * @copyright 2010-2016 Daniel Chappuis + * @license BSD 3 clauses (see license file) + */ // Libraries #include @@ -32,8 +13,8 @@ using namespace reactphysics3d; // Constructor ConcaveShape::ConcaveShape(CollisionShapeType type) - : CollisionShape(type), m_isSmoothMeshCollisionEnabled(false), - m_triangleMargin(0), m_raycastTestType(FRONT) { + : CollisionShape(type), m_isSmoothMeshCollisionEnabled(false), + m_triangleMargin(0), m_raycastTestType(FRONT) { } diff --git a/ephysics/collision/shapes/ConcaveShape.h b/ephysics/collision/shapes/ConcaveShape.h index 7515d9c..621b567 100644 --- a/ephysics/collision/shapes/ConcaveShape.h +++ b/ephysics/collision/shapes/ConcaveShape.h @@ -1,30 +1,9 @@ -/******************************************************************************** -* 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 REACTPHYSICS3D_CONCAVE_SHAPE_H -#define REACTPHYSICS3D_CONCAVE_SHAPE_H +/** @file + * @author Daniel Chappuis + * @copyright 2010-2016 Daniel Chappuis + * @license BSD 3 clauses (see license file) + */ +#pragma once // Libraries #include @@ -40,11 +19,11 @@ namespace reactphysics3d { */ class TriangleCallback { - public: - virtual ~TriangleCallback() = default; + public: + virtual ~TriangleCallback() = default; - /// Report a triangle - virtual void testTriangle(const Vector3* trianglePoints)=0; + /// Report a triangle + virtual void testTriangle(const Vector3* trianglePoints)=0; }; @@ -56,93 +35,93 @@ class TriangleCallback { */ class ConcaveShape : public CollisionShape { - protected : + protected : - // -------------------- Attributes -------------------- // + // -------------------- Attributes -------------------- // - /// True if the smooth mesh collision algorithm is enabled - bool m_isSmoothMeshCollisionEnabled; + /// True if the smooth mesh collision algorithm is enabled + bool m_isSmoothMeshCollisionEnabled; - // Margin use for collision detection for each triangle - decimal m_triangleMargin; + // Margin use for collision detection for each triangle + float m_triangleMargin; - /// Raycast test type for the triangle (front, back, front-back) - TriangleRaycastSide m_raycastTestType; + /// Raycast test type for the triangle (front, back, front-back) + TriangleRaycastSide m_raycastTestType; - // -------------------- Methods -------------------- // + // -------------------- Methods -------------------- // - /// Private copy-constructor - ConcaveShape(const ConcaveShape& shape); + /// Private copy-constructor + ConcaveShape(const ConcaveShape& shape); - /// Private assignment operator - ConcaveShape& operator=(const ConcaveShape& shape); + /// Private assignment operator + ConcaveShape& operator=(const ConcaveShape& shape); - /// Return true if a point is inside the collision shape - virtual bool testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const; + /// Return true if a point is inside the collision shape + virtual bool testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const; - public : + public : - // -------------------- Methods -------------------- // + // -------------------- Methods -------------------- // - /// Constructor - ConcaveShape(CollisionShapeType type); + /// Constructor + ConcaveShape(CollisionShapeType type); - /// Destructor - virtual ~ConcaveShape(); + /// Destructor + virtual ~ConcaveShape(); - /// Return the triangle margin - decimal getTriangleMargin() const; + /// Return the triangle margin + float getTriangleMargin() const; - /// Return the raycast test type (front, back, front-back) - TriangleRaycastSide getRaycastTestType() const; + /// Return the raycast test type (front, back, front-back) + TriangleRaycastSide getRaycastTestType() const; - // Set the raycast test type (front, back, front-back) - void setRaycastTestType(TriangleRaycastSide testType); + // Set the raycast test type (front, back, front-back) + void setRaycastTestType(TriangleRaycastSide testType); - /// Return true if the collision shape is convex, false if it is concave - virtual bool isConvex() const; + /// Return true if the collision shape is convex, false if it is concave + virtual bool isConvex() const; - /// Use a callback method on all triangles of the concave shape inside a given AABB - virtual void testAllTriangles(TriangleCallback& callback, const AABB& localAABB) const=0; + /// Use a callback method on all triangles of the concave shape inside a given AABB + virtual void testAllTriangles(TriangleCallback& callback, const AABB& localAABB) const=0; - /// Return true if the smooth mesh collision is enabled - bool getIsSmoothMeshCollisionEnabled() const; + /// Return true if the smooth mesh collision is enabled + bool getIsSmoothMeshCollisionEnabled() const; - /// Enable/disable the smooth mesh collision algorithm - void setIsSmoothMeshCollisionEnabled(bool isEnabled); + /// Enable/disable the smooth mesh collision algorithm + void setIsSmoothMeshCollisionEnabled(bool isEnabled); }; // Return the triangle margin -inline decimal ConcaveShape::getTriangleMargin() const { - return m_triangleMargin; +inline float ConcaveShape::getTriangleMargin() const { + return m_triangleMargin; } /// Return true if the collision shape is convex, false if it is concave inline bool ConcaveShape::isConvex() const { - return false; + return false; } // Return true if a point is inside the collision shape inline bool ConcaveShape::testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const { - return false; + return false; } // Return true if the smooth mesh collision is enabled inline bool ConcaveShape::getIsSmoothMeshCollisionEnabled() const { - return m_isSmoothMeshCollisionEnabled; + return m_isSmoothMeshCollisionEnabled; } // Enable/disable the smooth mesh collision algorithm -/// Smooth mesh collision is used to avoid collisions against some internal edges +/// Smooth mesh collision is used to avoid collisions against some int32_ternal edges /// of the triangle mesh. If it is enabled, collsions with the mesh will be smoother /// but collisions computation is a bit more expensive. inline void ConcaveShape::setIsSmoothMeshCollisionEnabled(bool isEnabled) { - m_isSmoothMeshCollisionEnabled = isEnabled; + m_isSmoothMeshCollisionEnabled = isEnabled; } // Return the raycast test type (front, back, front-back) inline TriangleRaycastSide ConcaveShape::getRaycastTestType() const { - return m_raycastTestType; + return m_raycastTestType; } // Set the raycast test type (front, back, front-back) @@ -150,10 +129,9 @@ inline TriangleRaycastSide ConcaveShape::getRaycastTestType() const { * @param testType Raycast test type for the triangle (front, back, front-back) */ inline void ConcaveShape::setRaycastTestType(TriangleRaycastSide testType) { - m_raycastTestType = testType; + m_raycastTestType = testType; } } -#endif diff --git a/ephysics/collision/shapes/ConeShape.cpp b/ephysics/collision/shapes/ConeShape.cpp index 2f4def3..068a0bd 100644 --- a/ephysics/collision/shapes/ConeShape.cpp +++ b/ephysics/collision/shapes/ConeShape.cpp @@ -1,27 +1,8 @@ -/******************************************************************************** -* 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. * -* * -********************************************************************************/ +/** @file + * @author Daniel Chappuis + * @copyright 2010-2016 Daniel Chappuis + * @license BSD 3 clauses (see license file) + */ // Libraries #include @@ -37,13 +18,13 @@ using namespace reactphysics3d; * @param height Height of the cone (in meters) * @param margin Collision margin (in meters) around the collision shape */ -ConeShape::ConeShape(decimal radius, decimal height, decimal margin) - : ConvexShape(CONE, margin), mRadius(radius), mHalfHeight(height * decimal(0.5)) { - assert(mRadius > decimal(0.0)); - assert(mHalfHeight > decimal(0.0)); - - // Compute the sine of the semi-angle at the apex point - mSinTheta = mRadius / (sqrt(mRadius * mRadius + height * height)); +ConeShape::ConeShape(float radius, float height, float margin) + : ConvexShape(CONE, margin), mRadius(radius), mHalfHeight(height * float(0.5)) { + assert(mRadius > float(0.0)); + assert(mHalfHeight > float(0.0)); + + // Compute the sine of the semi-angle at the apex point + mSinTheta = mRadius / (sqrt(mRadius * mRadius + height * height)); } // Destructor @@ -53,27 +34,27 @@ ConeShape::~ConeShape() { // Return a local support point in a given direction without the object margin Vector3 ConeShape::getLocalSupportPointWithoutMargin(const Vector3& direction, - void** cachedCollisionData) const { + void** cachedCollisionData) const { - const Vector3& v = direction; - decimal sinThetaTimesLengthV = mSinTheta * v.length(); - Vector3 supportPoint; + const Vector3& v = direction; + float sinThetaTimesLengthV = mSinTheta * v.length(); + Vector3 supportPoint; - if (v.y > sinThetaTimesLengthV) { - supportPoint = Vector3(0.0, mHalfHeight, 0.0); - } - else { - decimal projectedLength = sqrt(v.x * v.x + v.z * v.z); - if (projectedLength > MACHINE_EPSILON) { - decimal d = mRadius / projectedLength; - supportPoint = Vector3(v.x * d, -mHalfHeight, v.z * d); - } - else { - supportPoint = Vector3(0.0, -mHalfHeight, 0.0); - } - } + if (v.y > sinThetaTimesLengthV) { + supportPoint = Vector3(0.0, mHalfHeight, 0.0); + } + else { + float projectedLength = sqrt(v.x * v.x + v.z * v.z); + if (projectedLength > MACHINE_EPSILON) { + float d = mRadius / projectedLength; + supportPoint = Vector3(v.x * d, -mHalfHeight, v.z * d); + } + else { + supportPoint = Vector3(0.0, -mHalfHeight, 0.0); + } + } - return supportPoint; + return supportPoint; } // Raycast method with feedback information @@ -82,133 +63,133 @@ Vector3 ConeShape::getLocalSupportPointWithoutMargin(const Vector3& direction, // http://www.geometrictools.com/Documentation/IntersectionLineCone.pdf bool ConeShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const { - const Vector3 r = ray.point2 - ray.point1; + const Vector3 r = ray.point2 - ray.point1; - const decimal epsilon = decimal(0.00001); - Vector3 V(0, mHalfHeight, 0); - Vector3 centerBase(0, -mHalfHeight, 0); - Vector3 axis(0, decimal(-1.0), 0); - decimal heightSquare = decimal(4.0) * mHalfHeight * mHalfHeight; - decimal cosThetaSquare = heightSquare / (heightSquare + mRadius * mRadius); - decimal factor = decimal(1.0) - cosThetaSquare; - Vector3 delta = ray.point1 - V; - decimal c0 = -cosThetaSquare * delta.x * delta.x + factor * delta.y * delta.y - - cosThetaSquare * delta.z * delta.z; - decimal c1 = -cosThetaSquare * delta.x * r.x + factor * delta.y * r.y - cosThetaSquare * delta.z * r.z; - decimal c2 = -cosThetaSquare * r.x * r.x + factor * r.y * r.y - cosThetaSquare * r.z * r.z; - decimal tHit[] = {decimal(-1.0), decimal(-1.0), decimal(-1.0)}; - Vector3 localHitPoint[3]; - Vector3 localNormal[3]; + const float epsilon = float(0.00001); + Vector3 V(0, mHalfHeight, 0); + Vector3 centerBase(0, -mHalfHeight, 0); + Vector3 axis(0, float(-1.0), 0); + float heightSquare = float(4.0) * mHalfHeight * mHalfHeight; + float cosThetaSquare = heightSquare / (heightSquare + mRadius * mRadius); + float factor = float(1.0) - cosThetaSquare; + Vector3 delta = ray.point1 - V; + float c0 = -cosThetaSquare * delta.x * delta.x + factor * delta.y * delta.y - + cosThetaSquare * delta.z * delta.z; + float c1 = -cosThetaSquare * delta.x * r.x + factor * delta.y * r.y - cosThetaSquare * delta.z * r.z; + float c2 = -cosThetaSquare * r.x * r.x + factor * r.y * r.y - cosThetaSquare * r.z * r.z; + float tHit[] = {float(-1.0), float(-1.0), float(-1.0)}; + Vector3 localHitPoint[3]; + Vector3 localNormal[3]; - // If c2 is different from zero - if (std::abs(c2) > MACHINE_EPSILON) { - decimal gamma = c1 * c1 - c0 * c2; + // If c2 is different from zero + if (std::abs(c2) > MACHINE_EPSILON) { + float gamma = c1 * c1 - c0 * c2; - // If there is no real roots in the quadratic equation - if (gamma < decimal(0.0)) { - return false; - } - else if (gamma > decimal(0.0)) { // The equation has two real roots + // If there is no real roots in the quadratic equation + if (gamma < float(0.0)) { + return false; + } + else if (gamma > float(0.0)) { // The equation has two real roots - // Compute two intersections - decimal sqrRoot = std::sqrt(gamma); - tHit[0] = (-c1 - sqrRoot) / c2; - tHit[1] = (-c1 + sqrRoot) / c2; - } - else { // If the equation has a single real root + // Compute two int32_tersections + float sqrRoot = std::sqrt(gamma); + tHit[0] = (-c1 - sqrRoot) / c2; + tHit[1] = (-c1 + sqrRoot) / c2; + } + else { // If the equation has a single real root - // Compute the intersection - tHit[0] = -c1 / c2; - } - } - else { // If c2 == 0 + // Compute the int32_tersection + tHit[0] = -c1 / c2; + } + } + else { // If c2 == 0 - // If c2 = 0 and c1 != 0 - if (std::abs(c1) > MACHINE_EPSILON) { - tHit[0] = -c0 / (decimal(2.0) * c1); - } - else { // If c2 = c1 = 0 + // If c2 = 0 and c1 != 0 + if (std::abs(c1) > MACHINE_EPSILON) { + tHit[0] = -c0 / (float(2.0) * c1); + } + else { // If c2 = c1 = 0 - // If c0 is different from zero, no solution and if c0 = 0, we have a - // degenerate case, the whole ray is contained in the cone side - // but we return no hit in this case - return false; - } - } + // If c0 is different from zero, no solution and if c0 = 0, we have a + // degenerate case, the whole ray is contained in the cone side + // but we return no hit in this case + return false; + } + } - // If the origin of the ray is inside the cone, we return no hit - if (testPointInside(ray.point1, NULL)) return false; + // If the origin of the ray is inside the cone, we return no hit + if (testPointInside(ray.point1, NULL)) return false; - localHitPoint[0] = ray.point1 + tHit[0] * r; - localHitPoint[1] = ray.point1 + tHit[1] * r; + localHitPoint[0] = ray.point1 + tHit[0] * r; + localHitPoint[1] = ray.point1 + tHit[1] * r; - // Only keep hit points in one side of the double cone (the cone we are interested in) - if (axis.dot(localHitPoint[0] - V) < decimal(0.0)) { - tHit[0] = decimal(-1.0); - } - if (axis.dot(localHitPoint[1] - V) < decimal(0.0)) { - tHit[1] = decimal(-1.0); - } + // Only keep hit points in one side of the double cone (the cone we are int32_terested in) + if (axis.dot(localHitPoint[0] - V) < float(0.0)) { + tHit[0] = float(-1.0); + } + if (axis.dot(localHitPoint[1] - V) < float(0.0)) { + tHit[1] = float(-1.0); + } - // Only keep hit points that are within the correct height of the cone - if (localHitPoint[0].y < decimal(-mHalfHeight)) { - tHit[0] = decimal(-1.0); - } - if (localHitPoint[1].y < decimal(-mHalfHeight)) { - tHit[1] = decimal(-1.0); - } + // Only keep hit points that are within the correct height of the cone + if (localHitPoint[0].y < float(-mHalfHeight)) { + tHit[0] = float(-1.0); + } + if (localHitPoint[1].y < float(-mHalfHeight)) { + tHit[1] = float(-1.0); + } - // If the ray is in direction of the base plane of the cone - if (r.y > epsilon) { + // If the ray is in direction of the base plane of the cone + if (r.y > epsilon) { - // Compute the intersection with the base plane of the cone - tHit[2] = (-ray.point1.y - mHalfHeight) / (r.y); + // Compute the int32_tersection with the base plane of the cone + tHit[2] = (-ray.point1.y - mHalfHeight) / (r.y); - // Only keep this intersection if it is inside the cone radius - localHitPoint[2] = ray.point1 + tHit[2] * r; + // Only keep this int32_tersection if it is inside the cone radius + localHitPoint[2] = ray.point1 + tHit[2] * r; - if ((localHitPoint[2] - centerBase).lengthSquare() > mRadius * mRadius) { - tHit[2] = decimal(-1.0); - } + if ((localHitPoint[2] - centerBase).lengthSquare() > mRadius * mRadius) { + tHit[2] = float(-1.0); + } - // Compute the normal direction - localNormal[2] = axis; - } + // Compute the normal direction + localNormal[2] = axis; + } - // Find the smallest positive t value - int hitIndex = -1; - decimal t = DECIMAL_LARGEST; - for (int i=0; i<3; i++) { - if (tHit[i] < decimal(0.0)) continue; - if (tHit[i] < t) { - hitIndex = i; - t = tHit[hitIndex]; - } - } + // Find the smallest positive t value + int32_t hitIndex = -1; + float t = DECIMAL_LARGEST; + for (int32_t i=0; i<3; i++) { + if (tHit[i] < float(0.0)) continue; + if (tHit[i] < t) { + hitIndex = i; + t = tHit[hitIndex]; + } + } - if (hitIndex < 0) return false; - if (t > ray.maxFraction) return false; + if (hitIndex < 0) return false; + if (t > ray.maxFraction) return false; - // Compute the normal direction for hit against side of the cone - if (hitIndex != 2) { - decimal h = decimal(2.0) * mHalfHeight; - decimal value1 = (localHitPoint[hitIndex].x * localHitPoint[hitIndex].x + - localHitPoint[hitIndex].z * localHitPoint[hitIndex].z); - decimal rOverH = mRadius / h; - decimal value2 = decimal(1.0) + rOverH * rOverH; - decimal factor = decimal(1.0) / std::sqrt(value1 * value2); - decimal x = localHitPoint[hitIndex].x * factor; - decimal z = localHitPoint[hitIndex].z * factor; - localNormal[hitIndex].x = x; - localNormal[hitIndex].y = std::sqrt(x * x + z * z) * rOverH; - localNormal[hitIndex].z = z; - } + // Compute the normal direction for hit against side of the cone + if (hitIndex != 2) { + float h = float(2.0) * mHalfHeight; + float value1 = (localHitPoint[hitIndex].x * localHitPoint[hitIndex].x + + localHitPoint[hitIndex].z * localHitPoint[hitIndex].z); + float rOverH = mRadius / h; + float value2 = float(1.0) + rOverH * rOverH; + float factor = float(1.0) / std::sqrt(value1 * value2); + float x = localHitPoint[hitIndex].x * factor; + float z = localHitPoint[hitIndex].z * factor; + localNormal[hitIndex].x = x; + localNormal[hitIndex].y = std::sqrt(x * x + z * z) * rOverH; + localNormal[hitIndex].z = z; + } - raycastInfo.body = proxyShape->getBody(); - raycastInfo.proxyShape = proxyShape; - raycastInfo.hitFraction = t; - raycastInfo.worldPoint = localHitPoint[hitIndex]; - raycastInfo.worldNormal = localNormal[hitIndex]; + raycastInfo.body = proxyShape->getBody(); + raycastInfo.proxyShape = proxyShape; + raycastInfo.hitFraction = t; + raycastInfo.worldPoint = localHitPoint[hitIndex]; + raycastInfo.worldNormal = localNormal[hitIndex]; - return true; + return true; } diff --git a/ephysics/collision/shapes/ConeShape.h b/ephysics/collision/shapes/ConeShape.h index 4771563..522e3c5 100644 --- a/ephysics/collision/shapes/ConeShape.h +++ b/ephysics/collision/shapes/ConeShape.h @@ -1,30 +1,9 @@ -/******************************************************************************** -* 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 REACTPHYSICS3D_CONE_SHAPE_H -#define REACTPHYSICS3D_CONE_SHAPE_H +/** @file + * @author Daniel Chappuis + * @copyright 2010-2016 Daniel Chappuis + * @license BSD 3 clauses (see license file) + */ +#pragma once // Libraries #include @@ -51,94 +30,94 @@ namespace reactphysics3d { */ class ConeShape : public ConvexShape { - protected : + protected : - // -------------------- Attributes -------------------- // + // -------------------- Attributes -------------------- // - /// Radius of the base - decimal mRadius; + /// Radius of the base + float mRadius; - /// Half height of the cone - decimal mHalfHeight; + /// Half height of the cone + float mHalfHeight; - /// sine of the semi angle at the apex point - decimal mSinTheta; + /// sine of the semi angle at the apex point + float mSinTheta; - // -------------------- Methods -------------------- // + // -------------------- Methods -------------------- // - /// Private copy-constructor - ConeShape(const ConeShape& shape); + /// Private copy-constructor + ConeShape(const ConeShape& shape); - /// Private assignment operator - ConeShape& operator=(const ConeShape& shape); + /// Private assignment operator + ConeShape& operator=(const ConeShape& shape); - /// Return a local support point in a given direction without the object margin - virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction, - void** cachedCollisionData) const; + /// Return a local support point in a given direction without the object margin + virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction, + void** cachedCollisionData) const; - /// Return true if a point is inside the collision shape - virtual bool testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const; + /// Return true if a point is inside the collision shape + virtual bool testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const; - /// Raycast method with feedback information - virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const; + /// Raycast method with feedback information + virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const; - /// Return the number of bytes used by the collision shape - virtual size_t getSizeInBytes() const; - - public : + /// Return the number of bytes used by the collision shape + virtual size_t getSizeInBytes() const; + + public : - // -------------------- Methods -------------------- // + // -------------------- Methods -------------------- // - /// Constructor - ConeShape(decimal mRadius, decimal height, decimal margin = OBJECT_MARGIN); + /// Constructor + ConeShape(float mRadius, float height, float margin = OBJECT_MARGIN); - /// Destructor - virtual ~ConeShape(); + /// Destructor + virtual ~ConeShape(); - /// Return the radius - decimal getRadius() const; + /// Return the radius + float getRadius() const; - /// Return the height - decimal getHeight() const; + /// Return the height + float getHeight() const; - /// Set the scaling vector of the collision shape - virtual void setLocalScaling(const Vector3& scaling); + /// Set the scaling vector of the collision shape + virtual void setLocalScaling(const Vector3& scaling); - /// Return the local bounds of the shape in x, y and z directions - virtual void getLocalBounds(Vector3& min, Vector3& max) const; + /// Return the local bounds of the shape in x, y and z directions + virtual void getLocalBounds(Vector3& min, Vector3& max) const; - /// Return the local inertia tensor of the collision shape - virtual void computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const; + /// Return the local inertia tensor of the collision shape + virtual void computeLocalInertiaTensor(Matrix3x3& tensor, float mass) const; }; // Return the radius /** * @return Radius of the cone (in meters) */ -inline decimal ConeShape::getRadius() const { - return mRadius; +inline float ConeShape::getRadius() const { + return mRadius; } // Return the height /** * @return Height of the cone (in meters) */ -inline decimal ConeShape::getHeight() const { - return decimal(2.0) * mHalfHeight; +inline float ConeShape::getHeight() const { + return float(2.0) * mHalfHeight; } // Set the scaling vector of the collision shape inline void ConeShape::setLocalScaling(const Vector3& scaling) { - mHalfHeight = (mHalfHeight / mScaling.y) * scaling.y; - mRadius = (mRadius / mScaling.x) * scaling.x; + mHalfHeight = (mHalfHeight / mScaling.y) * scaling.y; + mRadius = (mRadius / mScaling.x) * scaling.x; - CollisionShape::setLocalScaling(scaling); + CollisionShape::setLocalScaling(scaling); } // Return the number of bytes used by the collision shape inline size_t ConeShape::getSizeInBytes() const { - return sizeof(ConeShape); + return sizeof(ConeShape); } // Return the local bounds of the shape in x, y and z directions @@ -148,39 +127,37 @@ inline size_t ConeShape::getSizeInBytes() const { */ inline void ConeShape::getLocalBounds(Vector3& min, Vector3& max) const { - // Maximum bounds - max.x = mRadius + mMargin; - max.y = mHalfHeight + mMargin; - max.z = max.x; + // Maximum bounds + max.x = mRadius + mMargin; + max.y = mHalfHeight + mMargin; + max.z = max.x; - // Minimum bounds - min.x = -max.x; - min.y = -max.y; - min.z = min.x; + // Minimum bounds + min.x = -max.x; + min.y = -max.y; + min.z = min.x; } // Return the local inertia tensor of the collision shape /** * @param[out] tensor The 3x3 inertia tensor matrix of the shape in local-space - * coordinates + * coordinates * @param mass Mass to use to compute the inertia tensor of the collision shape */ -inline void ConeShape::computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const { - decimal rSquare = mRadius * mRadius; - decimal diagXZ = decimal(0.15) * mass * (rSquare + mHalfHeight); - tensor.setAllValues(diagXZ, 0.0, 0.0, - 0.0, decimal(0.3) * mass * rSquare, - 0.0, 0.0, 0.0, diagXZ); +inline void ConeShape::computeLocalInertiaTensor(Matrix3x3& tensor, float mass) const { + float rSquare = mRadius * mRadius; + float diagXZ = float(0.15) * mass * (rSquare + mHalfHeight); + tensor.setAllValues(diagXZ, 0.0, 0.0, + 0.0, float(0.3) * mass * rSquare, + 0.0, 0.0, 0.0, diagXZ); } // Return true if a point is inside the collision shape inline bool ConeShape::testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const { - const decimal radiusHeight = mRadius * (-localPoint.y + mHalfHeight) / - (mHalfHeight * decimal(2.0)); - return (localPoint.y < mHalfHeight && localPoint.y > -mHalfHeight) && - (localPoint.x * localPoint.x + localPoint.z * localPoint.z < radiusHeight *radiusHeight); + const float radiusHeight = mRadius * (-localPoint.y + mHalfHeight) / + (mHalfHeight * float(2.0)); + return (localPoint.y < mHalfHeight && localPoint.y > -mHalfHeight) && + (localPoint.x * localPoint.x + localPoint.z * localPoint.z < radiusHeight *radiusHeight); } } - -#endif diff --git a/ephysics/collision/shapes/ConvexMeshShape.cpp b/ephysics/collision/shapes/ConvexMeshShape.cpp index 8297540..6ce9993 100644 --- a/ephysics/collision/shapes/ConvexMeshShape.cpp +++ b/ephysics/collision/shapes/ConvexMeshShape.cpp @@ -1,27 +1,8 @@ -/******************************************************************************** -* 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. * -* * -********************************************************************************/ +/** @file + * @author Daniel Chappuis + * @copyright 2010-2016 Daniel Chappuis + * @license BSD 3 clauses (see license file) + */ // Libraries #include @@ -31,112 +12,112 @@ using namespace reactphysics3d; // Constructor to initialize with an array of 3D vertices. -/// This method creates an internal copy of the input vertices. +/// This method creates an int32_ternal copy of the input vertices. /** * @param arrayVertices Array with the vertices of the convex mesh * @param nbVertices Number of vertices in the convex mesh * @param stride Stride between the beginning of two elements in the vertices array * @param margin Collision margin (in meters) around the collision shape */ -ConvexMeshShape::ConvexMeshShape(const decimal* arrayVertices, uint nbVertices, int stride, decimal margin) - : ConvexShape(CONVEX_MESH, margin), m_numberVertices(nbVertices), m_minBounds(0, 0, 0), - m_maxBounds(0, 0, 0), m_isEdgesInformationUsed(false) { - assert(nbVertices > 0); - assert(stride > 0); +ConvexMeshShape::ConvexMeshShape(const float* arrayVertices, uint32_t nbVertices, int32_t stride, float margin) + : ConvexShape(CONVEX_MESH, margin), m_numberVertices(nbVertices), m_minBounds(0, 0, 0), + m_maxBounds(0, 0, 0), m_isEdgesInformationUsed(false) { + assert(nbVertices > 0); + assert(stride > 0); - const unsigned char* vertexPointer = (const unsigned char*) arrayVertices; + const unsigned char* vertexPointer = (const unsigned char*) arrayVertices; - // Copy all the vertices into the internal array - for (uint i=0; igetVertexDataType(); - TriangleVertexArray::IndexDataType indexType = triangleVertexArray->getIndexDataType(); - unsigned char* verticesStart = triangleVertexArray->getVerticesStart(); - unsigned char* indicesStart = triangleVertexArray->getIndicesStart(); - int vertexStride = triangleVertexArray->getVerticesStride(); - int indexStride = triangleVertexArray->getIndicesStride(); + TriangleVertexArray::VertexDataType vertexType = triangleVertexArray->getVertexDataType(); + TriangleVertexArray::IndexDataType indexType = triangleVertexArray->getIndexDataType(); + unsigned char* verticesStart = triangleVertexArray->getVerticesStart(); + unsigned char* indicesStart = triangleVertexArray->getIndicesStart(); + int32_t vertexStride = triangleVertexArray->getVerticesStride(); + int32_t indexStride = triangleVertexArray->getIndicesStride(); - // For each vertex of the mesh - for (uint v = 0; v < triangleVertexArray->getNbVertices(); v++) { + // For each vertex of the mesh + for (uint32_t v = 0; v < triangleVertexArray->getNbVertices(); v++) { - // Get the vertices components of the triangle - if (vertexType == TriangleVertexArray::VERTEX_FLOAT_TYPE) { - const float* vertices = (float*)(verticesStart + v * vertexStride); + // Get the vertices components of the triangle + if (vertexType == TriangleVertexArray::VERTEX_FLOAT_TYPE) { + const float* vertices = (float*)(verticesStart + v * vertexStride); - Vector3 vertex(vertices[0], vertices[1], vertices[2] ); - vertex = vertex * mScaling; - m_vertices.push_back(vertex); - } - else if (vertexType == TriangleVertexArray::VERTEX_DOUBLE_TYPE) { - const double* vertices = (double*)(verticesStart + v * vertexStride); + Vector3 vertex(vertices[0], vertices[1], vertices[2] ); + vertex = vertex * mScaling; + m_vertices.push_back(vertex); + } + else if (vertexType == TriangleVertexArray::VERTEX_DOUBLE_TYPE) { + const double* vertices = (double*)(verticesStart + v * vertexStride); - Vector3 vertex(vertices[0], vertices[1], vertices[2] ); - vertex = vertex * mScaling; - m_vertices.push_back(vertex); - } - } + Vector3 vertex(vertices[0], vertices[1], vertices[2] ); + vertex = vertex * mScaling; + m_vertices.push_back(vertex); + } + } - // If we need to use the edges information of the mesh - if (m_isEdgesInformationUsed) { + // If we need to use the edges information of the mesh + if (m_isEdgesInformationUsed) { - // For each triangle of the mesh - for (uint triangleIndex=0; triangleIndexgetNbTriangles(); triangleIndex++) { + // For each triangle of the mesh + for (uint32_t triangleIndex=0; triangleIndexgetNbTriangles(); triangleIndex++) { - void* vertexIndexPointer = (indicesStart + triangleIndex * 3 * indexStride); + void* vertexIndexPointer = (indicesStart + triangleIndex * 3 * indexStride); - uint vertexIndex[3] = {0, 0, 0}; + uint32_t vertexIndex[3] = {0, 0, 0}; - // For each vertex of the triangle - for (int k=0; k < 3; k++) { + // For each vertex of the triangle + for (int32_t k=0; k < 3; k++) { - // Get the index of the current vertex in the triangle - if (indexType == TriangleVertexArray::INDEX_INTEGER_TYPE) { - vertexIndex[k] = ((uint*)vertexIndexPointer)[k]; - } - else if (indexType == TriangleVertexArray::INDEX_SHORT_TYPE) { - vertexIndex[k] = ((unsigned short*)vertexIndexPointer)[k]; - } - else { - assert(false); - } - } + // Get the index of the current vertex in the triangle + if (indexType == TriangleVertexArray::INDEX_INTEGER_TYPE) { + vertexIndex[k] = ((uint32_t*)vertexIndexPointer)[k]; + } + else if (indexType == TriangleVertexArray::INDEX_SHORT_TYPE) { + vertexIndex[k] = ((unsigned short*)vertexIndexPointer)[k]; + } + else { + assert(false); + } + } - // Add information about the edges - addEdge(vertexIndex[0], vertexIndex[1]); - addEdge(vertexIndex[0], vertexIndex[2]); - addEdge(vertexIndex[1], vertexIndex[2]); - } - } + // Add information about the edges + addEdge(vertexIndex[0], vertexIndex[1]); + addEdge(vertexIndex[0], vertexIndex[2]); + addEdge(vertexIndex[1], vertexIndex[2]); + } + } - m_numberVertices = m_vertices.size(); - recalculateBounds(); + m_numberVertices = m_vertices.size(); + recalculateBounds(); } // Constructor. /// If you use this constructor, you will need to set the vertices manually one by one using /// the addVertex() method. -ConvexMeshShape::ConvexMeshShape(decimal margin) - : ConvexShape(CONVEX_MESH, margin), m_numberVertices(0), m_minBounds(0, 0, 0), - m_maxBounds(0, 0, 0), m_isEdgesInformationUsed(false) { +ConvexMeshShape::ConvexMeshShape(float margin) + : ConvexShape(CONVEX_MESH, margin), m_numberVertices(0), m_minBounds(0, 0, 0), + m_maxBounds(0, 0, 0), m_isEdgesInformationUsed(false) { } @@ -154,115 +135,115 @@ ConvexMeshShape::~ConvexMeshShape() { /// will be in most of the cases very close to the previous one. Using hill-climbing, this method /// runs in almost constant time. Vector3 ConvexMeshShape::getLocalSupportPointWithoutMargin(const Vector3& direction, - void** cachedCollisionData) const { + void** cachedCollisionData) const { - assert(m_numberVertices == m_vertices.size()); - assert(cachedCollisionData != NULL); + assert(m_numberVertices == m_vertices.size()); + assert(cachedCollisionData != NULL); - // Allocate memory for the cached collision data if not allocated yet - if ((*cachedCollisionData) == NULL) { - *cachedCollisionData = (int*) malloc(sizeof(int)); - *((int*)(*cachedCollisionData)) = 0; - } + // Allocate memory for the cached collision data if not allocated yet + if ((*cachedCollisionData) == NULL) { + *cachedCollisionData = (int32_t*) malloc(sizeof(int32_t)); + *((int32_t*)(*cachedCollisionData)) = 0; + } - // If the edges information is used to speed up the collision detection - if (m_isEdgesInformationUsed) { + // If the edges information is used to speed up the collision detection + if (m_isEdgesInformationUsed) { - assert(m_edgesAdjacencyList.size() == m_numberVertices); + assert(m_edgesAdjacencyList.size() == m_numberVertices); - uint maxVertex = *((int*)(*cachedCollisionData)); - decimal maxDotProduct = direction.dot(m_vertices[maxVertex]); - bool isOptimal; + uint32_t maxVertex = *((int32_t*)(*cachedCollisionData)); + float maxDotProduct = direction.dot(m_vertices[maxVertex]); + bool isOptimal; - // Perform hill-climbing (local search) - do { - isOptimal = true; + // Perform hill-climbing (local search) + do { + isOptimal = true; - assert(m_edgesAdjacencyList.at(maxVertex).size() > 0); + assert(m_edgesAdjacencyList.at(maxVertex).size() > 0); - // For all neighbors of the current vertex - std::set::const_iterator it; - std::set::const_iterator itBegin = m_edgesAdjacencyList.at(maxVertex).begin(); - std::set::const_iterator itEnd = m_edgesAdjacencyList.at(maxVertex).end(); - for (it = itBegin; it != itEnd; ++it) { + // For all neighbors of the current vertex + std::set::const_iterator it; + std::set::const_iterator itBegin = m_edgesAdjacencyList.at(maxVertex).begin(); + std::set::const_iterator itEnd = m_edgesAdjacencyList.at(maxVertex).end(); + for (it = itBegin; it != itEnd; ++it) { - // Compute the dot product - decimal dotProduct = direction.dot(m_vertices[*it]); + // Compute the dot product + float dotProduct = direction.dot(m_vertices[*it]); - // If the current vertex is a better vertex (larger dot product) - if (dotProduct > maxDotProduct) { - maxVertex = *it; - maxDotProduct = dotProduct; - isOptimal = false; - } - } + // If the current vertex is a better vertex (larger dot product) + if (dotProduct > maxDotProduct) { + maxVertex = *it; + maxDotProduct = dotProduct; + isOptimal = false; + } + } - } while(!isOptimal); + } while(!isOptimal); - // Cache the support vertex - *((int*)(*cachedCollisionData)) = maxVertex; + // Cache the support vertex + *((int32_t*)(*cachedCollisionData)) = maxVertex; - // Return the support vertex - return m_vertices[maxVertex] * mScaling; - } - else { // If the edges information is not used + // Return the support vertex + return m_vertices[maxVertex] * mScaling; + } + else { // If the edges information is not used - double maxDotProduct = DECIMAL_SMALLEST; - uint indexMaxDotProduct = 0; + double maxDotProduct = DECIMAL_SMALLEST; + uint32_t indexMaxDotProduct = 0; - // For each vertex of the mesh - for (uint i=0; i maxDotProduct) { - indexMaxDotProduct = i; - maxDotProduct = dotProduct; - } - } + // If the current dot product is larger than the maximum one + if (dotProduct > maxDotProduct) { + indexMaxDotProduct = i; + maxDotProduct = dotProduct; + } + } - assert(maxDotProduct >= decimal(0.0)); + assert(maxDotProduct >= float(0.0)); - // Return the vertex with the largest dot product in the support direction - return m_vertices[indexMaxDotProduct] * mScaling; - } + // Return the vertex with the largest dot product in the support direction + return m_vertices[indexMaxDotProduct] * mScaling; + } } // Recompute the bounds of the mesh void ConvexMeshShape::recalculateBounds() { - // TODO : Only works if the local origin is inside the mesh - // => Make it more robust (init with first vertex of mesh instead) + // TODO : Only works if the local origin is inside the mesh + // => Make it more robust (init with first vertex of mesh instead) - m_minBounds.setToZero(); - m_maxBounds.setToZero(); + m_minBounds.setToZero(); + m_maxBounds.setToZero(); - // For each vertex of the mesh - for (uint i=0; i m_maxBounds.x) m_maxBounds.x = m_vertices[i].x; - if (m_vertices[i].x < m_minBounds.x) m_minBounds.x = m_vertices[i].x; + if (m_vertices[i].x > m_maxBounds.x) m_maxBounds.x = m_vertices[i].x; + if (m_vertices[i].x < m_minBounds.x) m_minBounds.x = m_vertices[i].x; - if (m_vertices[i].y > m_maxBounds.y) m_maxBounds.y = m_vertices[i].y; - if (m_vertices[i].y < m_minBounds.y) m_minBounds.y = m_vertices[i].y; + if (m_vertices[i].y > m_maxBounds.y) m_maxBounds.y = m_vertices[i].y; + if (m_vertices[i].y < m_minBounds.y) m_minBounds.y = m_vertices[i].y; - if (m_vertices[i].z > m_maxBounds.z) m_maxBounds.z = m_vertices[i].z; - if (m_vertices[i].z < m_minBounds.z) m_minBounds.z = m_vertices[i].z; - } + if (m_vertices[i].z > m_maxBounds.z) m_maxBounds.z = m_vertices[i].z; + if (m_vertices[i].z < m_minBounds.z) m_minBounds.z = m_vertices[i].z; + } - // Apply the local scaling factor - m_maxBounds = m_maxBounds * mScaling; - m_minBounds = m_minBounds * mScaling; + // Apply the local scaling factor + m_maxBounds = m_maxBounds * mScaling; + m_minBounds = m_minBounds * mScaling; - // Add the object margin to the bounds - m_maxBounds += Vector3(mMargin, mMargin, mMargin); - m_minBounds -= Vector3(mMargin, mMargin, mMargin); + // Add the object margin to the bounds + m_maxBounds += Vector3(mMargin, mMargin, mMargin); + m_minBounds -= Vector3(mMargin, mMargin, mMargin); } // Raycast method with feedback information bool ConvexMeshShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const { - return proxyShape->mBody->mWorld.mCollisionDetection.mNarrowPhaseGJKAlgorithm.raycast( - ray, proxyShape, raycastInfo); + return proxyShape->mBody->mWorld.mCollisionDetection.mNarrowPhaseGJKAlgorithm.raycast( + ray, proxyShape, raycastInfo); } diff --git a/ephysics/collision/shapes/ConvexMeshShape.h b/ephysics/collision/shapes/ConvexMeshShape.h index 1980bea..220d938 100644 --- a/ephysics/collision/shapes/ConvexMeshShape.h +++ b/ephysics/collision/shapes/ConvexMeshShape.h @@ -1,30 +1,9 @@ -/******************************************************************************** -* 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 REACTPHYSICS3D_CONVEX_MESH_SHAPE_H -#define REACTPHYSICS3D_CONVEX_MESH_SHAPE_H +/** @file + * @author Daniel Chappuis + * @copyright 2010-2016 Daniel Chappuis + * @license BSD 3 clauses (see license file) + */ +#pragma once // Libraries #include @@ -60,103 +39,103 @@ class CollisionWorld; */ class ConvexMeshShape : public ConvexShape { - protected : + protected : - // -------------------- Attributes -------------------- // + // -------------------- Attributes -------------------- // - /// Array with the vertices of the mesh - std::vector m_vertices; + /// Array with the vertices of the mesh + std::vector m_vertices; - /// Number of vertices in the mesh - uint m_numberVertices; + /// Number of vertices in the mesh + uint32_t m_numberVertices; - /// Mesh minimum bounds in the three local x, y and z directions - Vector3 m_minBounds; + /// Mesh minimum bounds in the three local x, y and z directions + Vector3 m_minBounds; - /// Mesh maximum bounds in the three local x, y and z directions - Vector3 m_maxBounds; + /// Mesh maximum bounds in the three local x, y and z directions + Vector3 m_maxBounds; - /// True if the shape contains the edges of the convex mesh in order to - /// make the collision detection faster - bool m_isEdgesInformationUsed; + /// True if the shape contains the edges of the convex mesh in order to + /// make the collision detection faster + bool m_isEdgesInformationUsed; - /// Adjacency list representing the edges of the mesh - std::map > m_edgesAdjacencyList; + /// Adjacency list representing the edges of the mesh + std::map > m_edgesAdjacencyList; - // -------------------- Methods -------------------- // + // -------------------- Methods -------------------- // - /// Private copy-constructor - ConvexMeshShape(const ConvexMeshShape& shape); + /// Private copy-constructor + ConvexMeshShape(const ConvexMeshShape& shape); - /// Private assignment operator - ConvexMeshShape& operator=(const ConvexMeshShape& shape); + /// Private assignment operator + ConvexMeshShape& operator=(const ConvexMeshShape& shape); - /// Recompute the bounds of the mesh - void recalculateBounds(); + /// Recompute the bounds of the mesh + void recalculateBounds(); - /// Set the scaling vector of the collision shape - virtual void setLocalScaling(const Vector3& scaling); + /// Set the scaling vector of the collision shape + virtual void setLocalScaling(const Vector3& scaling); - /// Return a local support point in a given direction without the object margin. - virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction, - void** cachedCollisionData) const; + /// Return a local support point in a given direction without the object margin. + virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction, + void** cachedCollisionData) const; - /// Return true if a point is inside the collision shape - virtual bool testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const; + /// Return true if a point is inside the collision shape + virtual bool testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const; - /// Raycast method with feedback information - virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const; + /// Raycast method with feedback information + virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const; - /// Return the number of bytes used by the collision shape - virtual size_t getSizeInBytes() const; + /// Return the number of bytes used by the collision shape + virtual size_t getSizeInBytes() const; - public : + public : - // -------------------- Methods -------------------- // + // -------------------- Methods -------------------- // - /// Constructor to initialize with an array of 3D vertices. - ConvexMeshShape(const decimal* arrayVertices, uint nbVertices, int stride, - decimal margin = OBJECT_MARGIN); + /// Constructor to initialize with an array of 3D vertices. + ConvexMeshShape(const float* arrayVertices, uint32_t nbVertices, int32_t stride, + float margin = OBJECT_MARGIN); - /// Constructor to initialize with a triangle vertex array - ConvexMeshShape(TriangleVertexArray* triangleVertexArray, bool isEdgesInformationUsed = true, - decimal margin = OBJECT_MARGIN); + /// Constructor to initialize with a triangle vertex array + ConvexMeshShape(TriangleVertexArray* triangleVertexArray, bool isEdgesInformationUsed = true, + float margin = OBJECT_MARGIN); - /// Constructor. - ConvexMeshShape(decimal margin = OBJECT_MARGIN); + /// Constructor. + ConvexMeshShape(float margin = OBJECT_MARGIN); - /// Destructor - virtual ~ConvexMeshShape(); + /// Destructor + virtual ~ConvexMeshShape(); - /// Return the local bounds of the shape in x, y and z directions - virtual void getLocalBounds(Vector3& min, Vector3& max) const; + /// Return the local bounds of the shape in x, y and z directions + virtual void getLocalBounds(Vector3& min, Vector3& max) const; - /// Return the local inertia tensor of the collision shape. - virtual void computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const; + /// Return the local inertia tensor of the collision shape. + virtual void computeLocalInertiaTensor(Matrix3x3& tensor, float mass) const; - /// Add a vertex into the convex mesh - void addVertex(const Vector3& vertex); + /// Add a vertex int32_to the convex mesh + void addVertex(const Vector3& vertex); - /// Add an edge into the convex mesh by specifying the two vertex indices of the edge. - void addEdge(uint v1, uint v2); + /// Add an edge int32_to the convex mesh by specifying the two vertex indices of the edge. + void addEdge(uint32_t v1, uint32_t v2); - /// Return true if the edges information is used to speed up the collision detection - bool isEdgesInformationUsed() const; + /// Return true if the edges information is used to speed up the collision detection + bool isEdgesInformationUsed() const; - /// Set the variable to know if the edges information is used to speed up the - /// collision detection - void setIsEdgesInformationUsed(bool isEdgesUsed); + /// Set the variable to know if the edges information is used to speed up the + /// collision detection + void setIsEdgesInformationUsed(bool isEdgesUsed); }; /// Set the scaling vector of the collision shape inline void ConvexMeshShape::setLocalScaling(const Vector3& scaling) { - ConvexShape::setLocalScaling(scaling); - recalculateBounds(); + ConvexShape::setLocalScaling(scaling); + recalculateBounds(); } // Return the number of bytes used by the collision shape inline size_t ConvexMeshShape::getSizeInBytes() const { - return sizeof(ConvexMeshShape); + return sizeof(ConvexMeshShape); } // Return the local bounds of the shape in x, y and z directions @@ -165,8 +144,8 @@ inline size_t ConvexMeshShape::getSizeInBytes() const { * @param max The maximum bounds of the shape in local-space coordinates */ inline void ConvexMeshShape::getLocalBounds(Vector3& min, Vector3& max) const { - min = m_minBounds; - max = m_maxBounds; + min = m_minBounds; + max = m_maxBounds; } // Return the local inertia tensor of the collision shape. @@ -174,63 +153,63 @@ inline void ConvexMeshShape::getLocalBounds(Vector3& min, Vector3& max) const { /// of its bounding box. /** * @param[out] tensor The 3x3 inertia tensor matrix of the shape in local-space -* coordinates +* coordinates * @param mass Mass to use to compute the inertia tensor of the collision shape */ -inline void ConvexMeshShape::computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const { - decimal factor = (decimal(1.0) / decimal(3.0)) * mass; - Vector3 realExtent = decimal(0.5) * (m_maxBounds - m_minBounds); - assert(realExtent.x > 0 && realExtent.y > 0 && realExtent.z > 0); - decimal xSquare = realExtent.x * realExtent.x; - decimal ySquare = realExtent.y * realExtent.y; - decimal zSquare = realExtent.z * realExtent.z; - tensor.setAllValues(factor * (ySquare + zSquare), 0.0, 0.0, - 0.0, factor * (xSquare + zSquare), 0.0, - 0.0, 0.0, factor * (xSquare + ySquare)); +inline void ConvexMeshShape::computeLocalInertiaTensor(Matrix3x3& tensor, float mass) const { + float factor = (float(1.0) / float(3.0)) * mass; + Vector3 realExtent = float(0.5) * (m_maxBounds - m_minBounds); + assert(realExtent.x > 0 && realExtent.y > 0 && realExtent.z > 0); + float xSquare = realExtent.x * realExtent.x; + float ySquare = realExtent.y * realExtent.y; + float zSquare = realExtent.z * realExtent.z; + tensor.setAllValues(factor * (ySquare + zSquare), 0.0, 0.0, + 0.0, factor * (xSquare + zSquare), 0.0, + 0.0, 0.0, factor * (xSquare + ySquare)); } -// Add a vertex into the convex mesh +// Add a vertex int32_to the convex mesh /** * @param vertex Vertex to be added */ inline void ConvexMeshShape::addVertex(const Vector3& vertex) { - // Add the vertex in to vertices array - m_vertices.push_back(vertex); - m_numberVertices++; + // Add the vertex in to vertices array + m_vertices.push_back(vertex); + m_numberVertices++; - // Update the bounds of the mesh - if (vertex.x * mScaling.x > m_maxBounds.x) m_maxBounds.x = vertex.x * mScaling.x; - if (vertex.x * mScaling.x < m_minBounds.x) m_minBounds.x = vertex.x * mScaling.x; - if (vertex.y * mScaling.y > m_maxBounds.y) m_maxBounds.y = vertex.y * mScaling.y; - if (vertex.y * mScaling.y < m_minBounds.y) m_minBounds.y = vertex.y * mScaling.y; - if (vertex.z * mScaling.z > m_maxBounds.z) m_maxBounds.z = vertex.z * mScaling.z; - if (vertex.z * mScaling.z < m_minBounds.z) m_minBounds.z = vertex.z * mScaling.z; + // Update the bounds of the mesh + if (vertex.x * mScaling.x > m_maxBounds.x) m_maxBounds.x = vertex.x * mScaling.x; + if (vertex.x * mScaling.x < m_minBounds.x) m_minBounds.x = vertex.x * mScaling.x; + if (vertex.y * mScaling.y > m_maxBounds.y) m_maxBounds.y = vertex.y * mScaling.y; + if (vertex.y * mScaling.y < m_minBounds.y) m_minBounds.y = vertex.y * mScaling.y; + if (vertex.z * mScaling.z > m_maxBounds.z) m_maxBounds.z = vertex.z * mScaling.z; + if (vertex.z * mScaling.z < m_minBounds.z) m_minBounds.z = vertex.z * mScaling.z; } -// Add an edge into the convex mesh by specifying the two vertex indices of the edge. +// Add an edge int32_to the convex mesh by specifying the two vertex indices of the edge. /// Note that the vertex indices start at zero and need to correspond to the order of /// the vertices in the vertices array in the constructor or the order of the calls -/// of the addVertex() methods that you use to add vertices into the convex mesh. +/// of the addVertex() methods that you use to add vertices int32_to the convex mesh. /** * @param v1 Index of the first vertex of the edge to add * @param v2 Index of the second vertex of the edge to add */ -inline void ConvexMeshShape::addEdge(uint v1, uint v2) { +inline void ConvexMeshShape::addEdge(uint32_t v1, uint32_t v2) { - // If the entry for vertex v1 does not exist in the adjacency list - if (m_edgesAdjacencyList.count(v1) == 0) { - m_edgesAdjacencyList.insert(std::make_pair(v1, std::set())); - } + // If the entry for vertex v1 does not exist in the adjacency list + if (m_edgesAdjacencyList.count(v1) == 0) { + m_edgesAdjacencyList.insert(std::make_pair(v1, std::set())); + } - // If the entry for vertex v2 does not exist in the adjacency list - if (m_edgesAdjacencyList.count(v2) == 0) { - m_edgesAdjacencyList.insert(std::make_pair(v2, std::set())); - } + // If the entry for vertex v2 does not exist in the adjacency list + if (m_edgesAdjacencyList.count(v2) == 0) { + m_edgesAdjacencyList.insert(std::make_pair(v2, std::set())); + } - // Add the edge in the adjacency list - m_edgesAdjacencyList[v1].insert(v2); - m_edgesAdjacencyList[v2].insert(v1); + // Add the edge in the adjacency list + m_edgesAdjacencyList[v1].insert(v2); + m_edgesAdjacencyList[v2].insert(v1); } // Return true if the edges information is used to speed up the collision detection @@ -238,28 +217,26 @@ inline void ConvexMeshShape::addEdge(uint v1, uint v2) { * @return True if the edges information is used and false otherwise */ inline bool ConvexMeshShape::isEdgesInformationUsed() const { - return m_isEdgesInformationUsed; + return m_isEdgesInformationUsed; } // Set the variable to know if the edges information is used to speed up the // collision detection /** * @param isEdgesUsed True if you want to use the edges information to speed up - * the collision detection with the convex mesh shape + * the collision detection with the convex mesh shape */ inline void ConvexMeshShape::setIsEdgesInformationUsed(bool isEdgesUsed) { - m_isEdgesInformationUsed = isEdgesUsed; + m_isEdgesInformationUsed = isEdgesUsed; } // Return true if a point is inside the collision shape inline bool ConvexMeshShape::testPointInside(const Vector3& localPoint, - ProxyShape* proxyShape) const { + ProxyShape* proxyShape) const { - // Use the GJK algorithm to test if the point is inside the convex mesh - return proxyShape->mBody->mWorld.mCollisionDetection. - mNarrowPhaseGJKAlgorithm.testPointInside(localPoint, proxyShape); + // Use the GJK algorithm to test if the point is inside the convex mesh + return proxyShape->mBody->mWorld.mCollisionDetection. + mNarrowPhaseGJKAlgorithm.testPointInside(localPoint, proxyShape); } } - -#endif diff --git a/ephysics/collision/shapes/ConvexShape.cpp b/ephysics/collision/shapes/ConvexShape.cpp index 30fccc1..5a2c8f0 100644 --- a/ephysics/collision/shapes/ConvexShape.cpp +++ b/ephysics/collision/shapes/ConvexShape.cpp @@ -1,27 +1,8 @@ -/******************************************************************************** -* 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. * -* * -********************************************************************************/ +/** @file + * @author Daniel Chappuis + * @copyright 2010-2016 Daniel Chappuis + * @license BSD 3 clauses (see license file) + */ // Libraries #include @@ -31,8 +12,8 @@ using namespace reactphysics3d; // Constructor -ConvexShape::ConvexShape(CollisionShapeType type, decimal margin) - : CollisionShape(type), mMargin(margin) { +ConvexShape::ConvexShape(CollisionShapeType type, float margin) + : CollisionShape(type), mMargin(margin) { } @@ -43,20 +24,20 @@ ConvexShape::~ConvexShape() { // Return a local support point in a given direction with the object margin Vector3 ConvexShape::getLocalSupportPointWithMargin(const Vector3& direction, - void** cachedCollisionData) const { + void** cachedCollisionData) const { - // Get the support point without margin - Vector3 supportPoint = getLocalSupportPointWithoutMargin(direction, cachedCollisionData); + // Get the support point without margin + Vector3 supportPoint = getLocalSupportPointWithoutMargin(direction, cachedCollisionData); - if (mMargin != decimal(0.0)) { + if (mMargin != float(0.0)) { - // Add the margin to the support point - Vector3 unitVec(0.0, -1.0, 0.0); - if (direction.lengthSquare() > MACHINE_EPSILON * MACHINE_EPSILON) { - unitVec = direction.getUnit(); - } - supportPoint += unitVec * mMargin; - } + // Add the margin to the support point + Vector3 unitVec(0.0, -1.0, 0.0); + if (direction.lengthSquare() > MACHINE_EPSILON * MACHINE_EPSILON) { + unitVec = direction.getUnit(); + } + supportPoint += unitVec * mMargin; + } - return supportPoint; + return supportPoint; } diff --git a/ephysics/collision/shapes/ConvexShape.h b/ephysics/collision/shapes/ConvexShape.h index bb37c1a..fd876da 100644 --- a/ephysics/collision/shapes/ConvexShape.h +++ b/ephysics/collision/shapes/ConvexShape.h @@ -1,30 +1,9 @@ -/******************************************************************************** -* 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 REACTPHYSICS3D_CONVEX_SHAPE_H -#define REACTPHYSICS3D_CONVEX_SHAPE_H +/** @file + * @author Daniel Chappuis + * @copyright 2010-2016 Daniel Chappuis + * @license BSD 3 clauses (see license file) + */ +#pragma once // Libraries #include @@ -39,68 +18,67 @@ namespace reactphysics3d { */ class ConvexShape : public CollisionShape { - protected : + protected : - // -------------------- Attributes -------------------- // + // -------------------- Attributes -------------------- // - /// Margin used for the GJK collision detection algorithm - decimal mMargin; + /// Margin used for the GJK collision detection algorithm + float mMargin; - // -------------------- Methods -------------------- // + // -------------------- Methods -------------------- // - /// Private copy-constructor - ConvexShape(const ConvexShape& shape); + /// Private copy-constructor + ConvexShape(const ConvexShape& shape); - /// Private assignment operator - ConvexShape& operator=(const ConvexShape& shape); + /// Private assignment operator + ConvexShape& operator=(const ConvexShape& shape); - // Return a local support point in a given direction with the object margin - Vector3 getLocalSupportPointWithMargin(const Vector3& direction, - void** cachedCollisionData) const; + // Return a local support point in a given direction with the object margin + Vector3 getLocalSupportPointWithMargin(const Vector3& direction, + void** cachedCollisionData) const; - /// Return a local support point in a given direction without the object margin - virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction, - void** cachedCollisionData) const=0; + /// Return a local support point in a given direction without the object margin + virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction, + void** cachedCollisionData) const=0; - /// Return true if a point is inside the collision shape - virtual bool testPointInside(const Vector3& worldPoint, ProxyShape* proxyShape) const=0; + /// Return true if a point is inside the collision shape + virtual bool testPointInside(const Vector3& worldPoint, ProxyShape* proxyShape) const=0; - public : + public : - // -------------------- Methods -------------------- // + // -------------------- Methods -------------------- // - /// Constructor - ConvexShape(CollisionShapeType type, decimal margin); + /// Constructor + ConvexShape(CollisionShapeType type, float margin); - /// Destructor - virtual ~ConvexShape(); + /// Destructor + virtual ~ConvexShape(); - /// Return the current object margin - decimal getMargin() const; + /// Return the current object margin + float getMargin() const; - /// Return true if the collision shape is convex, false if it is concave - virtual bool isConvex() const; + /// Return true if the collision shape is convex, false if it is concave + virtual bool isConvex() const; - // -------------------- Friendship -------------------- // + // -------------------- Friendship -------------------- // - friend class GJKAlgorithm; - friend class EPAAlgorithm; + friend class GJKAlgorithm; + friend class EPAAlgorithm; }; /// Return true if the collision shape is convex, false if it is concave inline bool ConvexShape::isConvex() const { - return true; + return true; } // Return the current collision shape margin /** * @return The margin (in meters) around the collision shape */ -inline decimal ConvexShape::getMargin() const { - return mMargin; +inline float ConvexShape::getMargin() const { + return mMargin; } } -#endif diff --git a/ephysics/collision/shapes/CylinderShape.cpp b/ephysics/collision/shapes/CylinderShape.cpp index 5fb9167..6fab781 100644 --- a/ephysics/collision/shapes/CylinderShape.cpp +++ b/ephysics/collision/shapes/CylinderShape.cpp @@ -1,27 +1,8 @@ -/******************************************************************************** -* 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. * -* * -********************************************************************************/ +/** @file + * @author Daniel Chappuis + * @copyright 2010-2016 Daniel Chappuis + * @license BSD 3 clauses (see license file) + */ // Libraries #include @@ -36,11 +17,11 @@ using namespace reactphysics3d; * @param height Height of the cylinder (in meters) * @param margin Collision margin (in meters) around the collision shape */ -CylinderShape::CylinderShape(decimal radius, decimal height, decimal margin) - : ConvexShape(CYLINDER, margin), mRadius(radius), - mHalfHeight(height/decimal(2.0)) { - assert(radius > decimal(0.0)); - assert(height > decimal(0.0)); +CylinderShape::CylinderShape(float radius, float height, float margin) + : ConvexShape(CYLINDER, margin), mRadius(radius), + mHalfHeight(height/float(2.0)) { + assert(radius > float(0.0)); + assert(height > float(0.0)); } // Destructor @@ -50,24 +31,24 @@ CylinderShape::~CylinderShape() { // Return a local support point in a given direction without the object margin Vector3 CylinderShape::getLocalSupportPointWithoutMargin(const Vector3& direction, - void** cachedCollisionData) const { + void** cachedCollisionData) const { - Vector3 supportPoint(0.0, 0.0, 0.0); - decimal uDotv = direction.y; - Vector3 w(direction.x, 0.0, direction.z); - decimal lengthW = sqrt(direction.x * direction.x + direction.z * direction.z); + Vector3 supportPoint(0.0, 0.0, 0.0); + float uDotv = direction.y; + Vector3 w(direction.x, 0.0, direction.z); + float lengthW = sqrt(direction.x * direction.x + direction.z * direction.z); - if (lengthW > MACHINE_EPSILON) { - if (uDotv < 0.0) supportPoint.y = -mHalfHeight; - else supportPoint.y = mHalfHeight; - supportPoint += (mRadius / lengthW) * w; - } - else { - if (uDotv < 0.0) supportPoint.y = -mHalfHeight; - else supportPoint.y = mHalfHeight; - } + if (lengthW > MACHINE_EPSILON) { + if (uDotv < 0.0) supportPoint.y = -mHalfHeight; + else supportPoint.y = mHalfHeight; + supportPoint += (mRadius / lengthW) * w; + } + else { + if (uDotv < 0.0) supportPoint.y = -mHalfHeight; + else supportPoint.y = mHalfHeight; + } - return supportPoint; + return supportPoint; } // Raycast method with feedback information @@ -75,162 +56,162 @@ Vector3 CylinderShape::getLocalSupportPointWithoutMargin(const Vector3& directio /// Morgan Kaufmann. bool CylinderShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const { - const Vector3 n = ray.point2 - ray.point1; + const Vector3 n = ray.point2 - ray.point1; - const decimal epsilon = decimal(0.01); - Vector3 p(decimal(0), -mHalfHeight, decimal(0)); - Vector3 q(decimal(0), mHalfHeight, decimal(0)); - Vector3 d = q - p; - Vector3 m = ray.point1 - p; - decimal t; + const float epsilon = float(0.01); + Vector3 p(float(0), -mHalfHeight, float(0)); + Vector3 q(float(0), mHalfHeight, float(0)); + Vector3 d = q - p; + Vector3 m = ray.point1 - p; + float t; - decimal mDotD = m.dot(d); - decimal nDotD = n.dot(d); - decimal dDotD = d.dot(d); + float mDotD = m.dot(d); + float nDotD = n.dot(d); + float dDotD = d.dot(d); - // Test if the segment is outside the cylinder - if (mDotD < decimal(0.0) && mDotD + nDotD < decimal(0.0)) return false; - if (mDotD > dDotD && mDotD + nDotD > dDotD) return false; + // Test if the segment is outside the cylinder + if (mDotD < float(0.0) && mDotD + nDotD < float(0.0)) return false; + if (mDotD > dDotD && mDotD + nDotD > dDotD) return false; - decimal nDotN = n.dot(n); - decimal mDotN = m.dot(n); + float nDotN = n.dot(n); + float mDotN = m.dot(n); - decimal a = dDotD * nDotN - nDotD * nDotD; - decimal k = m.dot(m) - mRadius * mRadius; - decimal c = dDotD * k - mDotD * mDotD; + float a = dDotD * nDotN - nDotD * nDotD; + float k = m.dot(m) - mRadius * mRadius; + float c = dDotD * k - mDotD * mDotD; - // If the ray is parallel to the cylinder axis - if (std::abs(a) < epsilon) { + // If the ray is parallel to the cylinder axis + if (std::abs(a) < epsilon) { - // If the origin is outside the surface of the cylinder, we return no hit - if (c > decimal(0.0)) return false; + // If the origin is outside the surface of the cylinder, we return no hit + if (c > float(0.0)) return false; - // Here we know that the segment intersect an endcap of the cylinder + // Here we know that the segment int32_tersect an endcap of the cylinder - // If the ray intersects with the "p" endcap of the cylinder - if (mDotD < decimal(0.0)) { + // If the ray int32_tersects with the "p" endcap of the cylinder + if (mDotD < float(0.0)) { - t = -mDotN / nDotN; + t = -mDotN / nDotN; - // If the intersection is behind the origin of the ray or beyond the maximum - // raycasting distance, we return no hit - if (t < decimal(0.0) || t > ray.maxFraction) return false; + // If the int32_tersection is behind the origin of the ray or beyond the maximum + // raycasting distance, we return no hit + if (t < float(0.0) || t > ray.maxFraction) return false; - // Compute the hit information - Vector3 localHitPoint = ray.point1 + t * n; - raycastInfo.body = proxyShape->getBody(); - raycastInfo.proxyShape = proxyShape; - raycastInfo.hitFraction = t; - raycastInfo.worldPoint = localHitPoint; - Vector3 normalDirection(0, decimal(-1), 0); - raycastInfo.worldNormal = normalDirection; + // Compute the hit information + Vector3 localHitPoint = ray.point1 + t * n; + raycastInfo.body = proxyShape->getBody(); + raycastInfo.proxyShape = proxyShape; + raycastInfo.hitFraction = t; + raycastInfo.worldPoint = localHitPoint; + Vector3 normalDirection(0, float(-1), 0); + raycastInfo.worldNormal = normalDirection; - return true; - } - else if (mDotD > dDotD) { // If the ray intersects with the "q" endcap of the cylinder + return true; + } + else if (mDotD > dDotD) { // If the ray int32_tersects with the "q" endcap of the cylinder - t = (nDotD - mDotN) / nDotN; + t = (nDotD - mDotN) / nDotN; - // If the intersection is behind the origin of the ray or beyond the maximum - // raycasting distance, we return no hit - if (t < decimal(0.0) || t > ray.maxFraction) return false; + // If the int32_tersection is behind the origin of the ray or beyond the maximum + // raycasting distance, we return no hit + if (t < float(0.0) || t > ray.maxFraction) return false; - // Compute the hit information - Vector3 localHitPoint = ray.point1 + t * n; - raycastInfo.body = proxyShape->getBody(); - raycastInfo.proxyShape = proxyShape; - raycastInfo.hitFraction = t; - raycastInfo.worldPoint = localHitPoint; - Vector3 normalDirection(0, decimal(1.0), 0); - raycastInfo.worldNormal = normalDirection; + // Compute the hit information + Vector3 localHitPoint = ray.point1 + t * n; + raycastInfo.body = proxyShape->getBody(); + raycastInfo.proxyShape = proxyShape; + raycastInfo.hitFraction = t; + raycastInfo.worldPoint = localHitPoint; + Vector3 normalDirection(0, float(1.0), 0); + raycastInfo.worldNormal = normalDirection; - return true; - } - else { // If the origin is inside the cylinder, we return no hit - return false; - } - } - decimal b = dDotD * mDotN - nDotD * mDotD; - decimal discriminant = b * b - a * c; + return true; + } + else { // If the origin is inside the cylinder, we return no hit + return false; + } + } + float b = dDotD * mDotN - nDotD * mDotD; + float discriminant = b * b - a * c; - // If the discriminant is negative, no real roots and therfore, no hit - if (discriminant < decimal(0.0)) return false; + // If the discriminant is negative, no real roots and therfore, no hit + if (discriminant < float(0.0)) return false; - // Compute the smallest root (first intersection along the ray) - decimal t0 = t = (-b - std::sqrt(discriminant)) / a; + // Compute the smallest root (first int32_tersection along the ray) + float t0 = t = (-b - std::sqrt(discriminant)) / a; - // If the intersection is outside the cylinder on "p" endcap side - decimal value = mDotD + t * nDotD; - if (value < decimal(0.0)) { + // If the int32_tersection is outside the cylinder on "p" endcap side + float value = mDotD + t * nDotD; + if (value < float(0.0)) { - // If the ray is pointing away from the "p" endcap, we return no hit - if (nDotD <= decimal(0.0)) return false; + // If the ray is pointing away from the "p" endcap, we return no hit + if (nDotD <= float(0.0)) return false; - // Compute the intersection against the "p" endcap (intersection agains whole plane) - t = -mDotD / nDotD; + // Compute the int32_tersection against the "p" endcap (int32_tersection agains whole plane) + t = -mDotD / nDotD; - // Keep the intersection if the it is inside the cylinder radius - if (k + t * (decimal(2.0) * mDotN + t) > decimal(0.0)) return false; + // Keep the int32_tersection if the it is inside the cylinder radius + if (k + t * (float(2.0) * mDotN + t) > float(0.0)) return false; - // If the intersection is behind the origin of the ray or beyond the maximum - // raycasting distance, we return no hit - if (t < decimal(0.0) || t > ray.maxFraction) return false; + // If the int32_tersection is behind the origin of the ray or beyond the maximum + // raycasting distance, we return no hit + if (t < float(0.0) || t > ray.maxFraction) return false; - // Compute the hit information - Vector3 localHitPoint = ray.point1 + t * n; - raycastInfo.body = proxyShape->getBody(); - raycastInfo.proxyShape = proxyShape; - raycastInfo.hitFraction = t; - raycastInfo.worldPoint = localHitPoint; - Vector3 normalDirection(0, decimal(-1.0), 0); - raycastInfo.worldNormal = normalDirection; + // Compute the hit information + Vector3 localHitPoint = ray.point1 + t * n; + raycastInfo.body = proxyShape->getBody(); + raycastInfo.proxyShape = proxyShape; + raycastInfo.hitFraction = t; + raycastInfo.worldPoint = localHitPoint; + Vector3 normalDirection(0, float(-1.0), 0); + raycastInfo.worldNormal = normalDirection; - return true; - } - else if (value > dDotD) { // If the intersection is outside the cylinder on the "q" side + return true; + } + else if (value > dDotD) { // If the int32_tersection is outside the cylinder on the "q" side - // If the ray is pointing away from the "q" endcap, we return no hit - if (nDotD >= decimal(0.0)) return false; + // If the ray is pointing away from the "q" endcap, we return no hit + if (nDotD >= float(0.0)) return false; - // Compute the intersection against the "q" endcap (intersection against whole plane) - t = (dDotD - mDotD) / nDotD; + // Compute the int32_tersection against the "q" endcap (int32_tersection against whole plane) + t = (dDotD - mDotD) / nDotD; - // Keep the intersection if it is inside the cylinder radius - if (k + dDotD - decimal(2.0) * mDotD + t * (decimal(2.0) * (mDotN - nDotD) + t) > - decimal(0.0)) return false; + // Keep the int32_tersection if it is inside the cylinder radius + if (k + dDotD - float(2.0) * mDotD + t * (float(2.0) * (mDotN - nDotD) + t) > + float(0.0)) return false; - // If the intersection is behind the origin of the ray or beyond the maximum - // raycasting distance, we return no hit - if (t < decimal(0.0) || t > ray.maxFraction) return false; + // If the int32_tersection is behind the origin of the ray or beyond the maximum + // raycasting distance, we return no hit + if (t < float(0.0) || t > ray.maxFraction) return false; - // Compute the hit information - Vector3 localHitPoint = ray.point1 + t * n; - raycastInfo.body = proxyShape->getBody(); - raycastInfo.proxyShape = proxyShape; - raycastInfo.hitFraction = t; - raycastInfo.worldPoint = localHitPoint; - Vector3 normalDirection(0, decimal(1.0), 0); - raycastInfo.worldNormal = normalDirection; + // Compute the hit information + Vector3 localHitPoint = ray.point1 + t * n; + raycastInfo.body = proxyShape->getBody(); + raycastInfo.proxyShape = proxyShape; + raycastInfo.hitFraction = t; + raycastInfo.worldPoint = localHitPoint; + Vector3 normalDirection(0, float(1.0), 0); + raycastInfo.worldNormal = normalDirection; - return true; - } + return true; + } - t = t0; + t = t0; - // If the intersection is behind the origin of the ray or beyond the maximum - // raycasting distance, we return no hit - if (t < decimal(0.0) || t > ray.maxFraction) return false; + // If the int32_tersection is behind the origin of the ray or beyond the maximum + // raycasting distance, we return no hit + if (t < float(0.0) || t > ray.maxFraction) return false; - // Compute the hit information - Vector3 localHitPoint = ray.point1 + t * n; - raycastInfo.body = proxyShape->getBody(); - raycastInfo.proxyShape = proxyShape; - raycastInfo.hitFraction = t; - raycastInfo.worldPoint = localHitPoint; - Vector3 v = localHitPoint - p; - Vector3 w = (v.dot(d) / d.lengthSquare()) * d; - Vector3 normalDirection = (localHitPoint - (p + w)); - raycastInfo.worldNormal = normalDirection; + // Compute the hit information + Vector3 localHitPoint = ray.point1 + t * n; + raycastInfo.body = proxyShape->getBody(); + raycastInfo.proxyShape = proxyShape; + raycastInfo.hitFraction = t; + raycastInfo.worldPoint = localHitPoint; + Vector3 v = localHitPoint - p; + Vector3 w = (v.dot(d) / d.lengthSquare()) * d; + Vector3 normalDirection = (localHitPoint - (p + w)); + raycastInfo.worldNormal = normalDirection; - return true; + return true; } diff --git a/ephysics/collision/shapes/CylinderShape.h b/ephysics/collision/shapes/CylinderShape.h index a095129..b5e425c 100644 --- a/ephysics/collision/shapes/CylinderShape.h +++ b/ephysics/collision/shapes/CylinderShape.h @@ -1,30 +1,9 @@ -/******************************************************************************** -* 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 REACTPHYSICS3D_CYLINDER_SHAPE_H -#define REACTPHYSICS3D_CYLINDER_SHAPE_H +/** @file + * @author Daniel Chappuis + * @copyright 2010-2016 Daniel Chappuis + * @license BSD 3 clauses (see license file) + */ +#pragma once // Libraries #include @@ -51,91 +30,91 @@ namespace reactphysics3d { */ class CylinderShape : public ConvexShape { - protected : + protected : - // -------------------- Attributes -------------------- // + // -------------------- Attributes -------------------- // - /// Radius of the base - decimal mRadius; + /// Radius of the base + float mRadius; - /// Half height of the cylinder - decimal mHalfHeight; + /// Half height of the cylinder + float mHalfHeight; - // -------------------- Methods -------------------- // + // -------------------- Methods -------------------- // - /// Private copy-constructor - CylinderShape(const CylinderShape& shape); + /// Private copy-constructor + CylinderShape(const CylinderShape& shape); - /// Private assignment operator - CylinderShape& operator=(const CylinderShape& shape); + /// Private assignment operator + CylinderShape& operator=(const CylinderShape& shape); - /// Return a local support point in a given direction without the object margin - virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction, - void** cachedCollisionData) const; + /// Return a local support point in a given direction without the object margin + virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction, + void** cachedCollisionData) const; - /// Return true if a point is inside the collision shape - virtual bool testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const; + /// Return true if a point is inside the collision shape + virtual bool testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const; - /// Raycast method with feedback information - virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const; + /// Raycast method with feedback information + virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const; - /// Return the number of bytes used by the collision shape - virtual size_t getSizeInBytes() const; + /// Return the number of bytes used by the collision shape + virtual size_t getSizeInBytes() const; - public : + public : - // -------------------- Methods -------------------- // + // -------------------- Methods -------------------- // - /// Constructor - CylinderShape(decimal radius, decimal height, decimal margin = OBJECT_MARGIN); + /// Constructor + CylinderShape(float radius, float height, float margin = OBJECT_MARGIN); - /// Destructor - virtual ~CylinderShape(); + /// Destructor + virtual ~CylinderShape(); - /// Return the radius - decimal getRadius() const; + /// Return the radius + float getRadius() const; - /// Return the height - decimal getHeight() const; + /// Return the height + float getHeight() const; - /// Set the scaling vector of the collision shape - virtual void setLocalScaling(const Vector3& scaling); + /// Set the scaling vector of the collision shape + virtual void setLocalScaling(const Vector3& scaling); - /// Return the local bounds of the shape in x, y and z directions - virtual void getLocalBounds(Vector3& min, Vector3& max) const; + /// Return the local bounds of the shape in x, y and z directions + virtual void getLocalBounds(Vector3& min, Vector3& max) const; - /// Return the local inertia tensor of the collision shape - virtual void computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const; + /// Return the local inertia tensor of the collision shape + virtual void computeLocalInertiaTensor(Matrix3x3& tensor, float mass) const; }; // Return the radius /** * @return Radius of the cylinder (in meters) */ -inline decimal CylinderShape::getRadius() const { - return mRadius; +inline float CylinderShape::getRadius() const { + return mRadius; } // Return the height /** * @return Height of the cylinder (in meters) */ -inline decimal CylinderShape::getHeight() const { - return mHalfHeight + mHalfHeight; +inline float CylinderShape::getHeight() const { + return mHalfHeight + mHalfHeight; } // Set the scaling vector of the collision shape inline void CylinderShape::setLocalScaling(const Vector3& scaling) { - mHalfHeight = (mHalfHeight / mScaling.y) * scaling.y; - mRadius = (mRadius / mScaling.x) * scaling.x; + mHalfHeight = (mHalfHeight / mScaling.y) * scaling.y; + mRadius = (mRadius / mScaling.x) * scaling.x; - CollisionShape::setLocalScaling(scaling); + CollisionShape::setLocalScaling(scaling); } // Return the number of bytes used by the collision shape inline size_t CylinderShape::getSizeInBytes() const { - return sizeof(CylinderShape); + return sizeof(CylinderShape); } // Return the local bounds of the shape in x, y and z directions @@ -145,38 +124,37 @@ inline size_t CylinderShape::getSizeInBytes() const { */ inline void CylinderShape::getLocalBounds(Vector3& min, Vector3& max) const { - // Maximum bounds - max.x = mRadius + mMargin; - max.y = mHalfHeight + mMargin; - max.z = max.x; + // Maximum bounds + max.x = mRadius + mMargin; + max.y = mHalfHeight + mMargin; + max.z = max.x; - // Minimum bounds - min.x = -max.x; - min.y = -max.y; - min.z = min.x; + // Minimum bounds + min.x = -max.x; + min.y = -max.y; + min.z = min.x; } // Return the local inertia tensor of the cylinder /** * @param[out] tensor The 3x3 inertia tensor matrix of the shape in local-space - * coordinates + * coordinates * @param mass Mass to use to compute the inertia tensor of the collision shape */ -inline void CylinderShape::computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const { - decimal height = decimal(2.0) * mHalfHeight; - decimal diag = (decimal(1.0) / decimal(12.0)) * mass * (3 * mRadius * mRadius + height * height); - tensor.setAllValues(diag, 0.0, 0.0, 0.0, - decimal(0.5) * mass * mRadius * mRadius, 0.0, - 0.0, 0.0, diag); +inline void CylinderShape::computeLocalInertiaTensor(Matrix3x3& tensor, float mass) const { + float height = float(2.0) * mHalfHeight; + float diag = (float(1.0) / float(12.0)) * mass * (3 * mRadius * mRadius + height * height); + tensor.setAllValues(diag, 0.0, 0.0, 0.0, + float(0.5) * mass * mRadius * mRadius, 0.0, + 0.0, 0.0, diag); } // Return true if a point is inside the collision shape inline bool CylinderShape::testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const{ - return ((localPoint.x * localPoint.x + localPoint.z * localPoint.z) < mRadius * mRadius && - localPoint.y < mHalfHeight && localPoint.y > -mHalfHeight); + return ((localPoint.x * localPoint.x + localPoint.z * localPoint.z) < mRadius * mRadius && + localPoint.y < mHalfHeight && localPoint.y > -mHalfHeight); } } -#endif diff --git a/ephysics/collision/shapes/HeightFieldShape.cpp b/ephysics/collision/shapes/HeightFieldShape.cpp index 5ae8d05..a7ef27a 100644 --- a/ephysics/collision/shapes/HeightFieldShape.cpp +++ b/ephysics/collision/shapes/HeightFieldShape.cpp @@ -1,27 +1,8 @@ -/******************************************************************************** -* 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. * -* * -********************************************************************************/ +/** @file + * @author Daniel Chappuis + * @copyright 2010-2016 Daniel Chappuis + * @license BSD 3 clauses (see license file) + */ // Libraries #include @@ -35,43 +16,43 @@ using namespace reactphysics3d; * @param minHeight Minimum height value of the height field * @param maxHeight Maximum height value of the height field * @param heightFieldData Pointer to the first height value data (note that values are shared and not copied) - * @param dataType Data type for the height values (int, float, double) + * @param dataType Data type for the height values (int32_t, float, double) * @param upAxis Integer representing the up axis direction (0 for x, 1 for y and 2 for z) - * @param integerHeightScale Scaling factor used to scale the height values (only when height values type is integer) + * @param int32_tegerHeightScale Scaling factor used to scale the height values (only when height values type is int32_teger) */ -HeightFieldShape::HeightFieldShape(int nbGridColumns, int nbGridRows, decimal minHeight, decimal maxHeight, - const void* heightFieldData, HeightDataType dataType, int upAxis, - decimal integerHeightScale) - : ConcaveShape(HEIGHTFIELD), mNbColumns(nbGridColumns), mNbRows(nbGridRows), - mWidth(nbGridColumns - 1), mLength(nbGridRows - 1), mMinHeight(minHeight), - mMaxHeight(maxHeight), mUpAxis(upAxis), mIntegerHeightScale(integerHeightScale), - mHeightDataType(dataType) { +HeightFieldShape::HeightFieldShape(int32_t nbGridColumns, int32_t nbGridRows, float minHeight, float maxHeight, + const void* heightFieldData, HeightDataType dataType, int32_t upAxis, + float int32_tegerHeightScale) + : ConcaveShape(HEIGHTFIELD), mNbColumns(nbGridColumns), mNbRows(nbGridRows), + mWidth(nbGridColumns - 1), mLength(nbGridRows - 1), mMinHeight(minHeight), + mMaxHeight(maxHeight), mUpAxis(upAxis), mIntegerHeightScale(int32_tegerHeightScale), + mHeightDataType(dataType) { - assert(nbGridColumns >= 2); - assert(nbGridRows >= 2); - assert(mWidth >= 1); - assert(mLength >= 1); - assert(minHeight <= maxHeight); - assert(upAxis == 0 || upAxis == 1 || upAxis == 2); + assert(nbGridColumns >= 2); + assert(nbGridRows >= 2); + assert(mWidth >= 1); + assert(mLength >= 1); + assert(minHeight <= maxHeight); + assert(upAxis == 0 || upAxis == 1 || upAxis == 2); - mHeightFieldData = heightFieldData; + mHeightFieldData = heightFieldData; - decimal halfHeight = (mMaxHeight - mMinHeight) * decimal(0.5); - assert(halfHeight >= 0); + float halfHeight = (mMaxHeight - mMinHeight) * float(0.5); + assert(halfHeight >= 0); - // Compute the local AABB of the height field - if (mUpAxis == 0) { - mAABB.setMin(Vector3(-halfHeight, -mWidth * decimal(0.5), -mLength * decimal(0.5))); - mAABB.setMax(Vector3(halfHeight, mWidth * decimal(0.5), mLength* decimal(0.5))); - } - else if (mUpAxis == 1) { - mAABB.setMin(Vector3(-mWidth * decimal(0.5), -halfHeight, -mLength * decimal(0.5))); - mAABB.setMax(Vector3(mWidth * decimal(0.5), halfHeight, mLength * decimal(0.5))); - } - else if (mUpAxis == 2) { - mAABB.setMin(Vector3(-mWidth * decimal(0.5), -mLength * decimal(0.5), -halfHeight)); - mAABB.setMax(Vector3(mWidth * decimal(0.5), mLength * decimal(0.5), halfHeight)); - } + // Compute the local AABB of the height field + if (mUpAxis == 0) { + mAABB.setMin(Vector3(-halfHeight, -mWidth * float(0.5), -mLength * float(0.5))); + mAABB.setMax(Vector3(halfHeight, mWidth * float(0.5), mLength* float(0.5))); + } + else if (mUpAxis == 1) { + mAABB.setMin(Vector3(-mWidth * float(0.5), -halfHeight, -mLength * float(0.5))); + mAABB.setMax(Vector3(mWidth * float(0.5), halfHeight, mLength * float(0.5))); + } + else if (mUpAxis == 2) { + mAABB.setMin(Vector3(-mWidth * float(0.5), -mLength * float(0.5), -halfHeight)); + mAABB.setMax(Vector3(mWidth * float(0.5), mLength * float(0.5), halfHeight)); + } } // Destructor @@ -86,8 +67,8 @@ HeightFieldShape::~HeightFieldShape() { * @param max The maximum bounds of the shape in local-space coordinates */ void HeightFieldShape::getLocalBounds(Vector3& min, Vector3& max) const { - min = mAABB.getMin() * mScaling; - max = mAABB.getMax() * mScaling; + min = mAABB.getMin() * mScaling; + max = mAABB.getMax() * mScaling; } // Test collision with the triangles of the height field shape. The idea is to use the AABB @@ -97,35 +78,35 @@ void HeightFieldShape::getLocalBounds(Vector3& min, Vector3& max) const { void HeightFieldShape::testAllTriangles(TriangleCallback& callback, const AABB& localAABB) const { // Compute the non-scaled AABB - Vector3 inverseScaling(decimal(1.0) / mScaling.x, decimal(1.0) / mScaling.y, decimal(1.0) / mScaling.z); + Vector3 inverseScaling(float(1.0) / mScaling.x, float(1.0) / mScaling.y, float(1.0) / mScaling.z); AABB aabb(localAABB.getMin() * inverseScaling, localAABB.getMax() * inverseScaling); - // Compute the integer grid coordinates inside the area we need to test for collision - int minGridCoords[3]; - int maxGridCoords[3]; + // Compute the int32_teger grid coordinates inside the area we need to test for collision + int32_t minGridCoords[3]; + int32_t maxGridCoords[3]; computeMinMaxGridCoordinates(minGridCoords, maxGridCoords, aabb); // Compute the starting and ending coords of the sub-grid according to the up axis - int iMin = 0; - int iMax = 0; - int jMin = 0; - int jMax = 0; + int32_t iMin = 0; + int32_t iMax = 0; + int32_t jMin = 0; + int32_t jMax = 0; switch(mUpAxis) { - case 0 : iMin = clamp(minGridCoords[1], 0, mNbColumns - 1); - iMax = clamp(maxGridCoords[1], 0, mNbColumns - 1); - jMin = clamp(minGridCoords[2], 0, mNbRows - 1); - jMax = clamp(maxGridCoords[2], 0, mNbRows - 1); - break; - case 1 : iMin = clamp(minGridCoords[0], 0, mNbColumns - 1); - iMax = clamp(maxGridCoords[0], 0, mNbColumns - 1); - jMin = clamp(minGridCoords[2], 0, mNbRows - 1); - jMax = clamp(maxGridCoords[2], 0, mNbRows - 1); - break; - case 2 : iMin = clamp(minGridCoords[0], 0, mNbColumns - 1); - iMax = clamp(maxGridCoords[0], 0, mNbColumns - 1); - jMin = clamp(minGridCoords[1], 0, mNbRows - 1); - jMax = clamp(maxGridCoords[1], 0, mNbRows - 1); - break; + case 0 : iMin = clamp(minGridCoords[1], 0, mNbColumns - 1); + iMax = clamp(maxGridCoords[1], 0, mNbColumns - 1); + jMin = clamp(minGridCoords[2], 0, mNbRows - 1); + jMax = clamp(maxGridCoords[2], 0, mNbRows - 1); + break; + case 1 : iMin = clamp(minGridCoords[0], 0, mNbColumns - 1); + iMax = clamp(maxGridCoords[0], 0, mNbColumns - 1); + jMin = clamp(minGridCoords[2], 0, mNbRows - 1); + jMax = clamp(maxGridCoords[2], 0, mNbRows - 1); + break; + case 2 : iMin = clamp(minGridCoords[0], 0, mNbColumns - 1); + iMax = clamp(maxGridCoords[0], 0, mNbColumns - 1); + jMin = clamp(minGridCoords[1], 0, mNbRows - 1); + jMax = clamp(maxGridCoords[1], 0, mNbRows - 1); + break; } assert(iMin >= 0 && iMin < mNbColumns); @@ -134,60 +115,60 @@ void HeightFieldShape::testAllTriangles(TriangleCallback& callback, const AABB& assert(jMax >= 0 && jMax < mNbRows); // For each sub-grid points (except the last ones one each dimension) - for (int i = iMin; i < iMax; i++) { - for (int j = jMin; j < jMax; j++) { + for (int32_t i = iMin; i < iMax; i++) { + for (int32_t j = jMin; j < jMax; j++) { - // Compute the four point of the current quad - Vector3 p1 = getVertexAt(i, j); - Vector3 p2 = getVertexAt(i, j + 1); - Vector3 p3 = getVertexAt(i + 1, j); - Vector3 p4 = getVertexAt(i + 1, j + 1); + // Compute the four point of the current quad + Vector3 p1 = getVertexAt(i, j); + Vector3 p2 = getVertexAt(i, j + 1); + Vector3 p3 = getVertexAt(i + 1, j); + Vector3 p4 = getVertexAt(i + 1, j + 1); - // Generate the first triangle for the current grid rectangle - Vector3 trianglePoints[3] = {p1, p2, p3}; + // Generate the first triangle for the current grid rectangle + Vector3 trianglePoints[3] = {p1, p2, p3}; - // Test collision against the first triangle - callback.testTriangle(trianglePoints); + // Test collision against the first triangle + callback.testTriangle(trianglePoints); - // Generate the second triangle for the current grid rectangle - trianglePoints[0] = p3; - trianglePoints[1] = p2; - trianglePoints[2] = p4; + // Generate the second triangle for the current grid rectangle + trianglePoints[0] = p3; + trianglePoints[1] = p2; + trianglePoints[2] = p4; - // Test collision against the second triangle - callback.testTriangle(trianglePoints); - } + // Test collision against the second triangle + callback.testTriangle(trianglePoints); + } } } -// Compute the min/max grid coords corresponding to the intersection of the AABB of the height field and +// Compute the min/max grid coords corresponding to the int32_tersection of the AABB of the height field and // the AABB to collide -void HeightFieldShape::computeMinMaxGridCoordinates(int* minCoords, int* maxCoords, const AABB& aabbToCollide) const { +void HeightFieldShape::computeMinMaxGridCoordinates(int32_t* minCoords, int32_t* maxCoords, const AABB& aabbToCollide) const { - // Clamp the min/max coords of the AABB to collide inside the height field AABB - Vector3 minPoint = Vector3::max(aabbToCollide.getMin(), mAABB.getMin()); - minPoint = Vector3::min(minPoint, mAABB.getMax()); + // Clamp the min/max coords of the AABB to collide inside the height field AABB + Vector3 minPoint = Vector3::max(aabbToCollide.getMin(), mAABB.getMin()); + minPoint = Vector3::min(minPoint, mAABB.getMax()); - Vector3 maxPoint = Vector3::min(aabbToCollide.getMax(), mAABB.getMax()); - maxPoint = Vector3::max(maxPoint, mAABB.getMin()); + Vector3 maxPoint = Vector3::min(aabbToCollide.getMax(), mAABB.getMax()); + maxPoint = Vector3::max(maxPoint, mAABB.getMin()); - // Translate the min/max points such that the we compute grid points from [0 ... mNbWidthGridPoints] - // and from [0 ... mNbLengthGridPoints] because the AABB coordinates range are [-mWdith/2 ... mWidth/2] - // and [-mLength/2 ... mLength/2] - const Vector3 translateVec = mAABB.getExtent() * decimal(0.5); - minPoint += translateVec; - maxPoint += translateVec; + // Translate the min/max points such that the we compute grid points from [0 ... mNbWidthGridPoints] + // and from [0 ... mNbLengthGridPoints] because the AABB coordinates range are [-mWdith/2 ... mWidth/2] + // and [-mLength/2 ... mLength/2] + const Vector3 translateVec = mAABB.getExtent() * float(0.5); + minPoint += translateVec; + maxPoint += translateVec; - // Convert the floating min/max coords of the AABB into closest integer - // grid values (note that we use the closest grid coordinate that is out - // of the AABB) - minCoords[0] = computeIntegerGridValue(minPoint.x) - 1; - minCoords[1] = computeIntegerGridValue(minPoint.y) - 1; - minCoords[2] = computeIntegerGridValue(minPoint.z) - 1; + // Convert the floating min/max coords of the AABB int32_to closest int32_teger + // grid values (note that we use the closest grid coordinate that is out + // of the AABB) + minCoords[0] = computeIntegerGridValue(minPoint.x) - 1; + minCoords[1] = computeIntegerGridValue(minPoint.y) - 1; + minCoords[2] = computeIntegerGridValue(minPoint.z) - 1; - maxCoords[0] = computeIntegerGridValue(maxPoint.x) + 1; - maxCoords[1] = computeIntegerGridValue(maxPoint.y) + 1; - maxCoords[2] = computeIntegerGridValue(maxPoint.z) + 1; + maxCoords[0] = computeIntegerGridValue(maxPoint.x) + 1; + maxCoords[1] = computeIntegerGridValue(maxPoint.y) + 1; + maxCoords[2] = computeIntegerGridValue(maxPoint.z) + 1; } // Raycast method with feedback information @@ -195,73 +176,73 @@ void HeightFieldShape::computeMinMaxGridCoordinates(int* minCoords, int* maxCoor /// the ray hits many triangles. bool HeightFieldShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const { - // TODO : Implement raycasting without using an AABB for the ray - // but using a dynamic AABB tree or octree instead + // TODO : Implement raycasting without using an AABB for the ray + // but using a dynamic AABB tree or octree instead - PROFILE("HeightFieldShape::raycast()"); + PROFILE("HeightFieldShape::raycast()"); - TriangleOverlapCallback triangleCallback(ray, proxyShape, raycastInfo, *this); + TriangleOverlapCallback triangleCallback(ray, proxyShape, raycastInfo, *this); - // 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 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)); - testAllTriangles(triangleCallback, rayAABB); + testAllTriangles(triangleCallback, rayAABB); - return triangleCallback.getIsHit(); + return triangleCallback.getIsHit(); } // Return the vertex (local-coordinates) of the height field at a given (x,y) position -Vector3 HeightFieldShape::getVertexAt(int x, int y) const { +Vector3 HeightFieldShape::getVertexAt(int32_t x, int32_t y) const { - // Get the height value - const decimal height = getHeightAt(x, y); + // Get the height value + const float height = getHeightAt(x, y); - // Height values origin - const decimal heightOrigin = -(mMaxHeight - mMinHeight) * decimal(0.5) - mMinHeight; + // Height values origin + const float heightOrigin = -(mMaxHeight - mMinHeight) * float(0.5) - mMinHeight; - Vector3 vertex; - switch (mUpAxis) { - case 0: vertex = Vector3(heightOrigin + height, -mWidth * decimal(0.5) + x, -mLength * decimal(0.5) + y); - break; - case 1: vertex = Vector3(-mWidth * decimal(0.5) + x, heightOrigin + height, -mLength * decimal(0.5) + y); - break; - case 2: vertex = Vector3(-mWidth * decimal(0.5) + x, -mLength * decimal(0.5) + y, heightOrigin + height); - break; - default: assert(false); - } + Vector3 vertex; + switch (mUpAxis) { + case 0: vertex = Vector3(heightOrigin + height, -mWidth * float(0.5) + x, -mLength * float(0.5) + y); + break; + case 1: vertex = Vector3(-mWidth * float(0.5) + x, heightOrigin + height, -mLength * float(0.5) + y); + break; + case 2: vertex = Vector3(-mWidth * float(0.5) + x, -mLength * float(0.5) + y, heightOrigin + height); + break; + default: assert(false); + } - assert(mAABB.contains(vertex)); + assert(mAABB.contains(vertex)); - return vertex * mScaling; + return vertex * mScaling; } // Raycast test between a ray and a triangle of the heightfield void TriangleOverlapCallback::testTriangle(const Vector3* trianglePoints) { - // Create a triangle collision shape - decimal margin = mHeightFieldShape.getTriangleMargin(); - TriangleShape triangleShape(trianglePoints[0], trianglePoints[1], trianglePoints[2], margin); - triangleShape.setRaycastTestType(mHeightFieldShape.getRaycastTestType()); + // Create a triangle collision shape + float margin = mHeightFieldShape.getTriangleMargin(); + TriangleShape triangleShape(trianglePoints[0], trianglePoints[1], trianglePoints[2], margin); + triangleShape.setRaycastTestType(mHeightFieldShape.getRaycastTestType()); - // Ray casting test against the collision shape - RaycastInfo raycastInfo; - bool isTriangleHit = triangleShape.raycast(m_ray, raycastInfo, m_proxyShape); + // Ray casting test against the collision shape + RaycastInfo raycastInfo; + bool isTriangleHit = triangleShape.raycast(m_ray, raycastInfo, m_proxyShape); - // If the ray hit the collision shape - if (isTriangleHit && raycastInfo.hitFraction <= mSmallestHitFraction) { + // If the ray hit the collision shape + if (isTriangleHit && raycastInfo.hitFraction <= mSmallestHitFraction) { - assert(raycastInfo.hitFraction >= decimal(0.0)); + assert(raycastInfo.hitFraction >= float(0.0)); - m_raycastInfo.body = raycastInfo.body; - m_raycastInfo.proxyShape = raycastInfo.proxyShape; - m_raycastInfo.hitFraction = raycastInfo.hitFraction; - m_raycastInfo.worldPoint = raycastInfo.worldPoint; - m_raycastInfo.worldNormal = raycastInfo.worldNormal; - m_raycastInfo.meshSubpart = -1; - m_raycastInfo.triangleIndex = -1; + m_raycastInfo.body = raycastInfo.body; + m_raycastInfo.proxyShape = raycastInfo.proxyShape; + m_raycastInfo.hitFraction = raycastInfo.hitFraction; + m_raycastInfo.worldPoint = raycastInfo.worldPoint; + m_raycastInfo.worldNormal = raycastInfo.worldNormal; + m_raycastInfo.meshSubpart = -1; + m_raycastInfo.triangleIndex = -1; - mSmallestHitFraction = raycastInfo.hitFraction; - mIsHit = true; - } + mSmallestHitFraction = raycastInfo.hitFraction; + mIsHit = true; + } } diff --git a/ephysics/collision/shapes/HeightFieldShape.h b/ephysics/collision/shapes/HeightFieldShape.h index e3d231f..d37bbb9 100644 --- a/ephysics/collision/shapes/HeightFieldShape.h +++ b/ephysics/collision/shapes/HeightFieldShape.h @@ -1,30 +1,9 @@ -/******************************************************************************** -* 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 REACTPHYSICS3D_HEIGHTFIELD_SHAPE_H -#define REACTPHYSICS3D_HEIGHTFIELD_SHAPE_H +/** @file + * @author Daniel Chappuis + * @copyright 2010-2016 Daniel Chappuis + * @license BSD 3 clauses (see license file) + */ +#pragma once // Libraries #include @@ -41,30 +20,30 @@ class HeightFieldShape; */ class TriangleOverlapCallback : public TriangleCallback { - protected: + protected: - const Ray& m_ray; - ProxyShape* m_proxyShape; - RaycastInfo& m_raycastInfo; - bool mIsHit; - decimal mSmallestHitFraction; - const HeightFieldShape& mHeightFieldShape; + const Ray& m_ray; + ProxyShape* m_proxyShape; + RaycastInfo& m_raycastInfo; + bool mIsHit; + float mSmallestHitFraction; + const HeightFieldShape& mHeightFieldShape; - public: + public: - // Constructor - TriangleOverlapCallback(const Ray& ray, ProxyShape* proxyShape, RaycastInfo& raycastInfo, - const HeightFieldShape& heightFieldShape) - : m_ray(ray), m_proxyShape(proxyShape), m_raycastInfo(raycastInfo), - mHeightFieldShape (heightFieldShape) { - mIsHit = false; - mSmallestHitFraction = m_ray.maxFraction; - } + // Constructor + TriangleOverlapCallback(const Ray& ray, ProxyShape* proxyShape, RaycastInfo& raycastInfo, + const HeightFieldShape& heightFieldShape) + : m_ray(ray), m_proxyShape(proxyShape), m_raycastInfo(raycastInfo), + mHeightFieldShape (heightFieldShape) { + mIsHit = false; + mSmallestHitFraction = m_ray.maxFraction; + } - bool getIsHit() const {return mIsHit;} + bool getIsHit() const {return mIsHit;} - /// Raycast test between a ray and a triangle of the heightfield - virtual void testTriangle(const Vector3* trianglePoints); + /// Raycast test between a ray and a triangle of the heightfield + virtual void testTriangle(const Vector3* trianglePoints); }; @@ -72,8 +51,8 @@ class TriangleOverlapCallback : public TriangleCallback { /** * This class represents a static height field that can be used to represent * a terrain. The height field is made of a grid with rows and columns with a - * height value at each grid point. Note that the height values are not copied into the shape - * but are shared instead. The height values can be of type integer, float or double. + * height value at each grid point. Note that the height values are not copied int32_to the shape + * but are shared instead. The height values can be of type int32_teger, float or double. * When creating a HeightFieldShape, you need to specify the minimum and maximum height value of * your height field. Note that the HeightFieldShape will be re-centered based on its AABB. It means * that for instance, if the minimum height value is -200 and the maximum value is 400, the final @@ -81,177 +60,176 @@ class TriangleOverlapCallback : public TriangleCallback { */ class HeightFieldShape : public ConcaveShape { - public: + public: - /// Data type for the height data of the height field - enum HeightDataType {HEIGHT_FLOAT_TYPE, HEIGHT_DOUBLE_TYPE, HEIGHT_INT_TYPE}; + /// Data type for the height data of the height field + enum HeightDataType {HEIGHT_FLOAT_TYPE, HEIGHT_DOUBLE_TYPE, HEIGHT_INT_TYPE}; - protected: + protected: - // -------------------- Attributes -------------------- // + // -------------------- Attributes -------------------- // - /// Number of columns in the grid of the height field - int mNbColumns; + /// Number of columns in the grid of the height field + int32_t mNbColumns; - /// Number of rows in the grid of the height field - int mNbRows; + /// Number of rows in the grid of the height field + int32_t mNbRows; - /// Height field width - decimal mWidth; + /// Height field width + float mWidth; - /// Height field length - decimal mLength; + /// Height field length + float mLength; - /// Minimum height of the height field - decimal mMinHeight; + /// Minimum height of the height field + float mMinHeight; - /// Maximum height of the height field - decimal mMaxHeight; + /// Maximum height of the height field + float mMaxHeight; - /// Up axis direction (0 => x, 1 => y, 2 => z) - int mUpAxis; + /// Up axis direction (0 => x, 1 => y, 2 => z) + int32_t mUpAxis; - /// Height values scale for height field with integer height values - decimal mIntegerHeightScale; + /// Height values scale for height field with int32_teger height values + float mIntegerHeightScale; - /// Data type of the height values - HeightDataType mHeightDataType; + /// Data type of the height values + HeightDataType mHeightDataType; - /// Array of data with all the height values of the height field - const void* mHeightFieldData; + /// Array of data with all the height values of the height field + const void* mHeightFieldData; - /// Local AABB of the height field (without scaling) - AABB mAABB; + /// Local AABB of the height field (without scaling) + AABB mAABB; - // -------------------- Methods -------------------- // + // -------------------- Methods -------------------- // - /// Private copy-constructor - HeightFieldShape(const HeightFieldShape& shape); + /// Private copy-constructor + HeightFieldShape(const HeightFieldShape& shape); - /// Private assignment operator - HeightFieldShape& operator=(const HeightFieldShape& shape); + /// Private assignment operator + HeightFieldShape& operator=(const HeightFieldShape& shape); - /// Raycast method with feedback information - virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const; + /// Raycast method with feedback information + virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const; - /// Return the number of bytes used by the collision shape - virtual size_t getSizeInBytes() const; + /// Return the number of bytes used by the collision shape + virtual size_t getSizeInBytes() const; - /// Insert all the triangles into the dynamic AABB tree - void initBVHTree(); + /// Insert all the triangles int32_to the dynamic AABB tree + void initBVHTree(); - /// Return the three vertices coordinates (in the array outTriangleVertices) of a triangle - /// given the start vertex index pointer of the triangle. - void getTriangleVerticesWithIndexPointer(int32 subPart, int32 triangleIndex, - Vector3* outTriangleVertices) const; + /// Return the three vertices coordinates (in the array outTriangleVertices) of a triangle + /// given the start vertex index pointer of the triangle. + void getTriangleVerticesWithIndexPointer(int32_t subPart, int32_t triangleIndex, + Vector3* outTriangleVertices) const; - /// Return the vertex (local-coordinates) of the height field at a given (x,y) position - Vector3 getVertexAt(int x, int y) const; + /// Return the vertex (local-coordinates) of the height field at a given (x,y) position + Vector3 getVertexAt(int32_t x, int32_t y) const; - /// Return the height of a given (x,y) point in the height field - decimal getHeightAt(int x, int y) const; + /// Return the height of a given (x,y) point in the height field + float getHeightAt(int32_t x, int32_t y) const; - /// Return the closest inside integer grid value of a given floating grid value - int computeIntegerGridValue(decimal value) const; + /// Return the closest inside int32_teger grid value of a given floating grid value + int32_t computeIntegerGridValue(float value) const; - /// Compute the min/max grid coords corresponding to the intersection of the AABB of the height field and the AABB to collide - void computeMinMaxGridCoordinates(int* minCoords, int* maxCoords, const AABB& aabbToCollide) const; + /// Compute the min/max grid coords corresponding to the int32_tersection of the AABB of the height field and the AABB to collide + void computeMinMaxGridCoordinates(int32_t* minCoords, int32_t* maxCoords, const AABB& aabbToCollide) const; - public: + public: - /// Constructor - HeightFieldShape(int nbGridColumns, int nbGridRows, decimal minHeight, decimal maxHeight, - const void* heightFieldData, HeightDataType dataType, - int upAxis = 1, decimal integerHeightScale = 1.0f); + /// Constructor + HeightFieldShape(int32_t nbGridColumns, int32_t nbGridRows, float minHeight, float maxHeight, + const void* heightFieldData, HeightDataType dataType, + int32_t upAxis = 1, float int32_tegerHeightScale = 1.0f); - /// Destructor - ~HeightFieldShape(); + /// Destructor + ~HeightFieldShape(); - /// Return the number of rows in the height field - int getNbRows() const; + /// Return the number of rows in the height field + int32_t getNbRows() const; - /// Return the number of columns in the height field - int getNbColumns() const; + /// Return the number of columns in the height field + int32_t getNbColumns() const; - /// Return the type of height value in the height field - HeightDataType getHeightDataType() const; + /// Return the type of height value in the height field + HeightDataType getHeightDataType() const; - /// Return the local bounds of the shape in x, y and z directions. - virtual void getLocalBounds(Vector3& min, Vector3& max) const; + /// Return the local bounds of the shape in x, y and z directions. + virtual void getLocalBounds(Vector3& min, Vector3& max) const; - /// Set the local scaling vector of the collision shape - virtual void setLocalScaling(const Vector3& scaling); + /// Set the local scaling vector of the collision shape + virtual void setLocalScaling(const Vector3& scaling); - /// Return the local inertia tensor of the collision shape - virtual void computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const; + /// Return the local inertia tensor of the collision shape + virtual void computeLocalInertiaTensor(Matrix3x3& tensor, float mass) const; - /// Use a callback method on all triangles of the concave shape inside a given AABB - virtual void testAllTriangles(TriangleCallback& callback, const AABB& localAABB) const; + /// Use a callback method on all triangles of the concave shape inside a given AABB + virtual void testAllTriangles(TriangleCallback& callback, const AABB& localAABB) const; - // ---------- Friendship ----------- // + // ---------- Friendship ----------- // - friend class ConvexTriangleAABBOverlapCallback; - friend class ConcaveMeshRaycastCallback; + friend class ConvexTriangleAABBOverlapCallback; + friend class ConcaveMeshRaycastCallback; }; // Return the number of rows in the height field -inline int HeightFieldShape::getNbRows() const { - return mNbRows; +inline int32_t HeightFieldShape::getNbRows() const { + return mNbRows; } // Return the number of columns in the height field -inline int HeightFieldShape::getNbColumns() const { - return mNbColumns; +inline int32_t HeightFieldShape::getNbColumns() const { + return mNbColumns; } // Return the type of height value in the height field inline HeightFieldShape::HeightDataType HeightFieldShape::getHeightDataType() const { - return mHeightDataType; + return mHeightDataType; } // Return the number of bytes used by the collision shape inline size_t HeightFieldShape::getSizeInBytes() const { - return sizeof(HeightFieldShape); + return sizeof(HeightFieldShape); } // Set the local scaling vector of the collision shape inline void HeightFieldShape::setLocalScaling(const Vector3& scaling) { - CollisionShape::setLocalScaling(scaling); + CollisionShape::setLocalScaling(scaling); } // Return the height of a given (x,y) point in the height field -inline decimal HeightFieldShape::getHeightAt(int x, int y) const { +inline float HeightFieldShape::getHeightAt(int32_t x, int32_t y) const { - switch(mHeightDataType) { - case HEIGHT_FLOAT_TYPE : return ((float*)mHeightFieldData)[y * mNbColumns + x]; - case HEIGHT_DOUBLE_TYPE : return ((double*)mHeightFieldData)[y * mNbColumns + x]; - case HEIGHT_INT_TYPE : return ((int*)mHeightFieldData)[y * mNbColumns + x] * mIntegerHeightScale; - default: assert(false); return 0; - } + switch(mHeightDataType) { + case HEIGHT_FLOAT_TYPE : return ((float*)mHeightFieldData)[y * mNbColumns + x]; + case HEIGHT_DOUBLE_TYPE : return ((double*)mHeightFieldData)[y * mNbColumns + x]; + case HEIGHT_INT_TYPE : return ((int32_t*)mHeightFieldData)[y * mNbColumns + x] * mIntegerHeightScale; + default: assert(false); return 0; + } } -// Return the closest inside integer grid value of a given floating grid value -inline int HeightFieldShape::computeIntegerGridValue(decimal value) const { - return (value < decimal(0.0)) ? value - decimal(0.5) : value + decimal(0.5); +// Return the closest inside int32_teger grid value of a given floating grid value +inline int32_t HeightFieldShape::computeIntegerGridValue(float value) const { + return (value < float(0.0)) ? value - float(0.5) : value + float(0.5); } // Return the local inertia tensor /** * @param[out] tensor The 3x3 inertia tensor matrix of the shape in local-space - * coordinates + * coordinates * @param mass Mass to use to compute the inertia tensor of the collision shape */ -inline void HeightFieldShape::computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const { +inline void HeightFieldShape::computeLocalInertiaTensor(Matrix3x3& tensor, float mass) const { - // Default inertia tensor - // Note that this is not very realistic for a concave triangle mesh. - // However, in most cases, it will only be used static bodies and therefore, - // the inertia tensor is not used. - tensor.setAllValues(mass, 0, 0, - 0, mass, 0, - 0, 0, mass); + // Default inertia tensor + // Note that this is not very realistic for a concave triangle mesh. + // However, in most cases, it will only be used static bodies and therefore, + // the inertia tensor is not used. + tensor.setAllValues(mass, 0, 0, + 0, mass, 0, + 0, 0, mass); } } -#endif diff --git a/ephysics/collision/shapes/SphereShape.cpp b/ephysics/collision/shapes/SphereShape.cpp index 4a9c821..5638fbc 100644 --- a/ephysics/collision/shapes/SphereShape.cpp +++ b/ephysics/collision/shapes/SphereShape.cpp @@ -1,27 +1,8 @@ -/******************************************************************************** -* 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. * -* * -********************************************************************************/ +/** @file + * @author Daniel Chappuis + * @copyright 2010-2016 Daniel Chappuis + * @license BSD 3 clauses (see license file) + */ // Libraries #include @@ -35,8 +16,8 @@ using namespace reactphysics3d; /** * @param radius Radius of the sphere (in meters) */ -SphereShape::SphereShape(decimal radius) : ConvexShape(SPHERE, radius) { - assert(radius > decimal(0.0)); +SphereShape::SphereShape(float radius) : ConvexShape(SPHERE, radius) { + assert(radius > float(0.0)); } // Destructor @@ -47,45 +28,45 @@ SphereShape::~SphereShape() { // Raycast method with feedback information bool SphereShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const { - const Vector3 m = ray.point1; - decimal c = m.dot(m) - mMargin * mMargin; + const Vector3 m = ray.point1; + float c = m.dot(m) - mMargin * mMargin; - // If the origin of the ray is inside the sphere, we return no intersection - if (c < decimal(0.0)) return false; + // If the origin of the ray is inside the sphere, we return no int32_tersection + if (c < float(0.0)) return false; - const Vector3 rayDirection = ray.point2 - ray.point1; - decimal b = m.dot(rayDirection); + const Vector3 rayDirection = ray.point2 - ray.point1; + float b = m.dot(rayDirection); - // If the origin of the ray is outside the sphere and the ray - // is pointing away from the sphere, there is no intersection - if (b > decimal(0.0)) return false; + // If the origin of the ray is outside the sphere and the ray + // is pointing away from the sphere, there is no int32_tersection + if (b > float(0.0)) return false; - decimal raySquareLength = rayDirection.lengthSquare(); + float raySquareLength = rayDirection.lengthSquare(); - // Compute the discriminant of the quadratic equation - decimal discriminant = b * b - raySquareLength * c; + // Compute the discriminant of the quadratic equation + float discriminant = b * b - raySquareLength * c; - // If the discriminant is negative or the ray length is very small, there is no intersection - if (discriminant < decimal(0.0) || raySquareLength < MACHINE_EPSILON) return false; + // If the discriminant is negative or the ray length is very small, there is no int32_tersection + if (discriminant < float(0.0) || raySquareLength < MACHINE_EPSILON) return false; - // Compute the solution "t" closest to the origin - decimal t = -b - std::sqrt(discriminant); + // Compute the solution "t" closest to the origin + float t = -b - std::sqrt(discriminant); - assert(t >= decimal(0.0)); + assert(t >= float(0.0)); - // If the hit point is withing the segment ray fraction - if (t < ray.maxFraction * raySquareLength) { + // If the hit point is withing the segment ray fraction + if (t < ray.maxFraction * raySquareLength) { - // Compute the intersection information - t /= raySquareLength; - raycastInfo.body = proxyShape->getBody(); - raycastInfo.proxyShape = proxyShape; - raycastInfo.hitFraction = t; - raycastInfo.worldPoint = ray.point1 + t * rayDirection; - raycastInfo.worldNormal = raycastInfo.worldPoint; + // Compute the int32_tersection information + t /= raySquareLength; + raycastInfo.body = proxyShape->getBody(); + raycastInfo.proxyShape = proxyShape; + raycastInfo.hitFraction = t; + raycastInfo.worldPoint = ray.point1 + t * rayDirection; + raycastInfo.worldNormal = raycastInfo.worldPoint; - return true; - } + return true; + } - return false; + return false; } diff --git a/ephysics/collision/shapes/SphereShape.h b/ephysics/collision/shapes/SphereShape.h index 7616361..7071612 100644 --- a/ephysics/collision/shapes/SphereShape.h +++ b/ephysics/collision/shapes/SphereShape.h @@ -1,30 +1,9 @@ -/******************************************************************************** -* 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 REACTPHYSICS3D_SPHERE_SHAPE_H -#define REACTPHYSICS3D_SPHERE_SHAPE_H +/** @file + * @author Daniel Chappuis + * @copyright 2010-2016 Daniel Chappuis + * @license BSD 3 clauses (see license file) + */ +#pragma once // Libraries #include @@ -44,85 +23,85 @@ namespace reactphysics3d { */ class SphereShape : public ConvexShape { - protected : + protected : - // -------------------- Attributes -------------------- // + // -------------------- Attributes -------------------- // - // -------------------- Methods -------------------- // + // -------------------- Methods -------------------- // - /// Private copy-constructor - SphereShape(const SphereShape& shape); + /// Private copy-constructor + SphereShape(const SphereShape& shape); - /// Private assignment operator - SphereShape& operator=(const SphereShape& shape); + /// Private assignment operator + SphereShape& operator=(const SphereShape& shape); - /// Return a local support point in a given direction without the object margin - virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction, - void** cachedCollisionData) const; + /// Return a local support point in a given direction without the object margin + virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction, + void** cachedCollisionData) const; - /// Return true if a point is inside the collision shape - virtual bool testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const; + /// Return true if a point is inside the collision shape + virtual bool testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const; - /// Raycast method with feedback information - virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const; + /// Raycast method with feedback information + virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const; - /// Return the number of bytes used by the collision shape - virtual size_t getSizeInBytes() const; + /// Return the number of bytes used by the collision shape + virtual size_t getSizeInBytes() const; - public : + public : - // -------------------- Methods -------------------- // + // -------------------- Methods -------------------- // - /// Constructor - SphereShape(decimal radius); + /// Constructor + SphereShape(float radius); - /// Destructor - virtual ~SphereShape(); + /// Destructor + virtual ~SphereShape(); - /// Return the radius of the sphere - decimal getRadius() const; + /// Return the radius of the sphere + float getRadius() const; - /// Set the scaling vector of the collision shape - virtual void setLocalScaling(const Vector3& scaling); + /// Set the scaling vector of the collision shape + virtual void setLocalScaling(const Vector3& scaling); - /// Return the local bounds of the shape in x, y and z directions. - virtual void getLocalBounds(Vector3& min, Vector3& max) const; + /// Return the local bounds of the shape in x, y and z directions. + virtual void getLocalBounds(Vector3& min, Vector3& max) const; - /// Return the local inertia tensor of the collision shape - virtual void computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const; + /// Return the local inertia tensor of the collision shape + virtual void computeLocalInertiaTensor(Matrix3x3& tensor, float mass) const; - /// Update the AABB of a body using its collision shape - virtual void computeAABB(AABB& aabb, const Transform& transform) const; + /// Update the AABB of a body using its collision shape + virtual void computeAABB(AABB& aabb, const Transform& transform) const; }; // Get the radius of the sphere /** * @return Radius of the sphere (in meters) */ -inline decimal SphereShape::getRadius() const { - return mMargin; +inline float SphereShape::getRadius() const { + return mMargin; } // Set the scaling vector of the collision shape inline void SphereShape::setLocalScaling(const Vector3& scaling) { - mMargin = (mMargin / mScaling.x) * scaling.x; + mMargin = (mMargin / mScaling.x) * scaling.x; - CollisionShape::setLocalScaling(scaling); + CollisionShape::setLocalScaling(scaling); } // Return the number of bytes used by the collision shape inline size_t SphereShape::getSizeInBytes() const { - return sizeof(SphereShape); + return sizeof(SphereShape); } // Return a local support point in a given direction without the object margin inline Vector3 SphereShape::getLocalSupportPointWithoutMargin(const Vector3& direction, - void** cachedCollisionData) const { + void** cachedCollisionData) 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); + // Return the center of the sphere (the radius is taken int32_to account in the object margin) + return Vector3(0.0, 0.0, 0.0); } // Return the local bounds of the shape in x, y and z directions. @@ -133,51 +112,49 @@ inline Vector3 SphereShape::getLocalSupportPointWithoutMargin(const Vector3& dir */ inline void SphereShape::getLocalBounds(Vector3& min, Vector3& max) const { - // Maximum bounds - max.x = mMargin; - max.y = mMargin; - max.z = mMargin; + // Maximum bounds + max.x = mMargin; + max.y = mMargin; + max.z = mMargin; - // Minimum bounds - min.x = -mMargin; - min.y = min.x; - min.z = min.x; + // Minimum bounds + min.x = -mMargin; + min.y = min.x; + min.z = min.x; } // Return the local inertia tensor of the sphere /** * @param[out] tensor The 3x3 inertia tensor matrix of the shape in local-space - * coordinates + * coordinates * @param mass Mass to use to compute the inertia tensor of the collision shape */ -inline void SphereShape::computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const { - decimal diag = decimal(0.4) * mass * mMargin * mMargin; - tensor.setAllValues(diag, 0.0, 0.0, - 0.0, diag, 0.0, - 0.0, 0.0, diag); +inline void SphereShape::computeLocalInertiaTensor(Matrix3x3& tensor, float mass) const { + float diag = float(0.4) * mass * mMargin * mMargin; + tensor.setAllValues(diag, 0.0, 0.0, + 0.0, diag, 0.0, + 0.0, 0.0, diag); } // Update the AABB of a body using its collision shape /** * @param[out] aabb The axis-aligned bounding box (AABB) of the collision shape - * computed in world-space coordinates + * computed in world-space coordinates * @param transform Transform used to compute the AABB of the collision shape */ inline void SphereShape::computeAABB(AABB& aabb, const Transform& transform) const { - // Get the local extents in x,y and z direction - Vector3 extents(mMargin, mMargin, mMargin); + // Get the local extents in x,y and z direction + Vector3 extents(mMargin, mMargin, mMargin); - // Update the AABB with the new minimum and maximum coordinates - aabb.setMin(transform.getPosition() - extents); - aabb.setMax(transform.getPosition() + extents); + // Update the AABB with the new minimum and maximum coordinates + aabb.setMin(transform.getPosition() - extents); + aabb.setMax(transform.getPosition() + extents); } // Return true if a point is inside the collision shape inline bool SphereShape::testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const { - return (localPoint.lengthSquare() < mMargin * mMargin); + return (localPoint.lengthSquare() < mMargin * mMargin); } } - -#endif diff --git a/ephysics/collision/shapes/TriangleShape.cpp b/ephysics/collision/shapes/TriangleShape.cpp index 8f0c7a3..c4119dd 100644 --- a/ephysics/collision/shapes/TriangleShape.cpp +++ b/ephysics/collision/shapes/TriangleShape.cpp @@ -1,27 +1,8 @@ -/******************************************************************************** -* 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. * -* * -********************************************************************************/ +/** @file + * @author Daniel Chappuis + * @copyright 2010-2016 Daniel Chappuis + * @license BSD 3 clauses (see license file) + */ // Libraries #include @@ -39,12 +20,12 @@ using namespace reactphysics3d; * @param point3 Third point of the triangle * @param margin The collision margin (in meters) around the collision shape */ -TriangleShape::TriangleShape(const Vector3& point1, const Vector3& point2, const Vector3& point3, decimal margin) - : ConvexShape(TRIANGLE, margin) { - mPoints[0] = point1; - mPoints[1] = point2; - mPoints[2] = point3; - m_raycastTestType = FRONT; +TriangleShape::TriangleShape(const Vector3& point1, const Vector3& point2, const Vector3& point3, float margin) + : ConvexShape(TRIANGLE, margin) { + mPoints[0] = point1; + mPoints[1] = point2; + mPoints[2] = point3; + m_raycastTestType = FRONT; } // Destructor @@ -57,71 +38,71 @@ TriangleShape::~TriangleShape() { /// Real-time Collision Detection by Christer Ericson. bool TriangleShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const { - PROFILE("TriangleShape::raycast()"); + PROFILE("TriangleShape::raycast()"); - const Vector3 pq = ray.point2 - ray.point1; - const Vector3 pa = mPoints[0] - ray.point1; - const Vector3 pb = mPoints[1] - ray.point1; - const Vector3 pc = mPoints[2] - ray.point1; + const Vector3 pq = ray.point2 - ray.point1; + const Vector3 pa = mPoints[0] - ray.point1; + const Vector3 pb = mPoints[1] - ray.point1; + const Vector3 pc = mPoints[2] - ray.point1; - // Test if the line PQ is inside the eges BC, CA and AB. We use the triple - // product for this test. - const Vector3 m = pq.cross(pc); - decimal u = pb.dot(m); - if (m_raycastTestType == FRONT) { - if (u < decimal(0.0)) return false; - } - else if (m_raycastTestType == BACK) { - if (u > decimal(0.0)) return false; - } + // Test if the line PQ is inside the eges BC, CA and AB. We use the triple + // product for this test. + const Vector3 m = pq.cross(pc); + float u = pb.dot(m); + if (m_raycastTestType == FRONT) { + if (u < float(0.0)) return false; + } + else if (m_raycastTestType == BACK) { + if (u > float(0.0)) return false; + } - decimal v = -pa.dot(m); - if (m_raycastTestType == FRONT) { - if (v < decimal(0.0)) return false; - } - else if (m_raycastTestType == BACK) { - if (v > decimal(0.0)) return false; - } - else if (m_raycastTestType == FRONT_AND_BACK) { - if (!sameSign(u, v)) return false; - } + float v = -pa.dot(m); + if (m_raycastTestType == FRONT) { + if (v < float(0.0)) return false; + } + else if (m_raycastTestType == BACK) { + if (v > float(0.0)) return false; + } + else if (m_raycastTestType == FRONT_AND_BACK) { + if (!sameSign(u, v)) return false; + } - decimal w = pa.dot(pq.cross(pb)); - if (m_raycastTestType == FRONT) { - if (w < decimal(0.0)) return false; - } - else if (m_raycastTestType == BACK) { - if (w > decimal(0.0)) return false; - } - else if (m_raycastTestType == FRONT_AND_BACK) { - if (!sameSign(u, w)) return false; - } + float w = pa.dot(pq.cross(pb)); + if (m_raycastTestType == FRONT) { + if (w < float(0.0)) return false; + } + else if (m_raycastTestType == BACK) { + if (w > float(0.0)) return false; + } + else if (m_raycastTestType == FRONT_AND_BACK) { + if (!sameSign(u, w)) return false; + } - // If the line PQ is in the triangle plane (case where u=v=w=0) - if (approxEqual(u, 0) && approxEqual(v, 0) && approxEqual(w, 0)) return false; + // If the line PQ is in the triangle plane (case where u=v=w=0) + if (approxEqual(u, 0) && approxEqual(v, 0) && approxEqual(w, 0)) return false; - // Compute the barycentric coordinates (u, v, w) to determine the - // intersection point R, R = u * a + v * b + w * c - decimal denom = decimal(1.0) / (u + v + w); - u *= denom; - v *= denom; - w *= denom; + // Compute the barycentric coordinates (u, v, w) to determine the + // int32_tersection point R, R = u * a + v * b + w * c + float denom = float(1.0) / (u + v + w); + u *= denom; + v *= denom; + w *= denom; - // Compute the local hit point using the barycentric coordinates - const Vector3 localHitPoint = u * mPoints[0] + v * mPoints[1] + w * mPoints[2]; - const decimal hitFraction = (localHitPoint - ray.point1).length() / pq.length(); + // Compute the local hit point using the barycentric coordinates + const Vector3 localHitPoint = u * mPoints[0] + v * mPoints[1] + w * mPoints[2]; + const float hitFraction = (localHitPoint - ray.point1).length() / pq.length(); - if (hitFraction < decimal(0.0) || hitFraction > ray.maxFraction) return false; + if (hitFraction < float(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; + Vector3 localHitNormal = (mPoints[1] - mPoints[0]).cross(mPoints[2] - mPoints[0]); + if (localHitNormal.dot(pq) > float(0.0)) localHitNormal = -localHitNormal; - raycastInfo.body = proxyShape->getBody(); - raycastInfo.proxyShape = proxyShape; - raycastInfo.worldPoint = localHitPoint; - raycastInfo.hitFraction = hitFraction; - raycastInfo.worldNormal = localHitNormal; + raycastInfo.body = proxyShape->getBody(); + raycastInfo.proxyShape = proxyShape; + raycastInfo.worldPoint = localHitPoint; + raycastInfo.hitFraction = hitFraction; + raycastInfo.worldNormal = localHitNormal; - return true; + return true; } diff --git a/ephysics/collision/shapes/TriangleShape.h b/ephysics/collision/shapes/TriangleShape.h index 7962ffd..05f318c 100644 --- a/ephysics/collision/shapes/TriangleShape.h +++ b/ephysics/collision/shapes/TriangleShape.h @@ -1,30 +1,9 @@ -/******************************************************************************** -* 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 REACTPHYSICS3D_TRIANGLE_SHAPE_H -#define REACTPHYSICS3D_TRIANGLE_SHAPE_H +/** @file + * @author Daniel Chappuis + * @copyright 2010-2016 Daniel Chappuis + * @license BSD 3 clauses (see license file) + */ +#pragma once // Libraries #include @@ -36,14 +15,14 @@ namespace reactphysics3d { /// Raycast test side for the triangle enum TriangleRaycastSide { - /// Raycast against front triangle - FRONT, + /// Raycast against front triangle + FRONT, - /// Raycast against back triangle - BACK, + /// Raycast against back triangle + BACK, - /// Raycast against front and back triangle - FRONT_AND_BACK + /// Raycast against front and back triangle + FRONT_AND_BACK }; // Class TriangleShape @@ -53,85 +32,85 @@ enum TriangleRaycastSide { */ class TriangleShape : public ConvexShape { - protected: + protected: - // -------------------- Attribute -------------------- // + // -------------------- Attribute -------------------- // - /// Three points of the triangle - Vector3 mPoints[3]; + /// Three points of the triangle + Vector3 mPoints[3]; - /// Raycast test type for the triangle (front, back, front-back) - TriangleRaycastSide m_raycastTestType; + /// Raycast test type for the triangle (front, back, front-back) + TriangleRaycastSide m_raycastTestType; - // -------------------- Methods -------------------- // + // -------------------- Methods -------------------- // - /// Private copy-constructor - TriangleShape(const TriangleShape& shape); + /// Private copy-constructor + TriangleShape(const TriangleShape& shape); - /// Private assignment operator - TriangleShape& operator=(const TriangleShape& shape); + /// Private assignment operator + TriangleShape& operator=(const TriangleShape& shape); - /// Return a local support point in a given direction without the object margin - virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction, - void** cachedCollisionData) const; + /// Return a local support point in a given direction without the object margin + virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction, + void** cachedCollisionData) const; - /// Return true if a point is inside the collision shape - virtual bool testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const; + /// Return true if a point is inside the collision shape + virtual bool testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const; - /// Raycast method with feedback information - virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const; + /// Raycast method with feedback information + virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const; - /// Return the number of bytes used by the collision shape - virtual size_t getSizeInBytes() const; + /// Return the number of bytes used by the collision shape + virtual size_t getSizeInBytes() const; - public: + public: - // -------------------- Methods -------------------- // + // -------------------- Methods -------------------- // - /// Constructor - TriangleShape(const Vector3& point1, const Vector3& point2, const Vector3& point3, - decimal margin = OBJECT_MARGIN); + /// Constructor + TriangleShape(const Vector3& point1, const Vector3& point2, const Vector3& point3, + float margin = OBJECT_MARGIN); - /// Destructor - virtual ~TriangleShape(); + /// Destructor + virtual ~TriangleShape(); - /// Return the local bounds of the shape in x, y and z directions. - virtual void getLocalBounds(Vector3& min, Vector3& max) const; + /// Return the local bounds of the shape in x, y and z directions. + virtual void getLocalBounds(Vector3& min, Vector3& max) const; - /// Set the local scaling vector of the collision shape - virtual void setLocalScaling(const Vector3& scaling); + /// Set the local scaling vector of the collision shape + virtual void setLocalScaling(const Vector3& scaling); - /// Return the local inertia tensor of the collision shape - virtual void computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const; + /// Return the local inertia tensor of the collision shape + virtual void computeLocalInertiaTensor(Matrix3x3& tensor, float mass) const; - /// Update the AABB of a body using its collision shape - virtual void computeAABB(AABB& aabb, const Transform& transform) const; + /// Update the AABB of a body using its collision shape + virtual void computeAABB(AABB& aabb, const Transform& transform) const; - /// Return the raycast test type (front, back, front-back) - TriangleRaycastSide getRaycastTestType() const; + /// Return the raycast test type (front, back, front-back) + TriangleRaycastSide getRaycastTestType() const; - // Set the raycast test type (front, back, front-back) - void setRaycastTestType(TriangleRaycastSide testType); + // Set the raycast test type (front, back, front-back) + void setRaycastTestType(TriangleRaycastSide testType); - /// Return the coordinates of a given vertex of the triangle - Vector3 getVertex(int index) const; + /// Return the coordinates of a given vertex of the triangle + Vector3 getVertex(int32_t index) const; - // ---------- Friendship ---------- // + // ---------- Friendship ---------- // - friend class ConcaveMeshRaycastCallback; - friend class TriangleOverlapCallback; + friend class ConcaveMeshRaycastCallback; + friend class TriangleOverlapCallback; }; // Return the number of bytes used by the collision shape inline size_t TriangleShape::getSizeInBytes() const { - return sizeof(TriangleShape); + return sizeof(TriangleShape); } // Return a local support point in a given direction without the object margin inline Vector3 TriangleShape::getLocalSupportPointWithoutMargin(const Vector3& direction, - void** cachedCollisionData) const { - Vector3 dotProducts(direction.dot(mPoints[0]), direction.dot(mPoints[1]), direction.dot(mPoints[2])); - return mPoints[dotProducts.getMaxAxis()]; + void** cachedCollisionData) const { + Vector3 dotProducts(direction.dot(mPoints[0]), direction.dot(mPoints[1]), direction.dot(mPoints[2])); + return mPoints[dotProducts.getMaxAxis()]; } // Return the local bounds of the shape in x, y and z directions. @@ -142,63 +121,63 @@ inline Vector3 TriangleShape::getLocalSupportPointWithoutMargin(const Vector3& d */ 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); - const Vector3 zAxis(mPoints[0].z, mPoints[1].z, mPoints[2].z); - min.setAllValues(xAxis.getMinValue(), yAxis.getMinValue(), zAxis.getMinValue()); - max.setAllValues(xAxis.getMaxValue(), yAxis.getMaxValue(), zAxis.getMaxValue()); + const Vector3 xAxis(mPoints[0].x, mPoints[1].x, mPoints[2].x); + const Vector3 yAxis(mPoints[0].y, mPoints[1].y, mPoints[2].y); + const Vector3 zAxis(mPoints[0].z, mPoints[1].z, mPoints[2].z); + min.setAllValues(xAxis.getMinValue(), yAxis.getMinValue(), zAxis.getMinValue()); + max.setAllValues(xAxis.getMaxValue(), yAxis.getMaxValue(), zAxis.getMaxValue()); - min -= Vector3(mMargin, mMargin, mMargin); - max += Vector3(mMargin, mMargin, mMargin); + min -= Vector3(mMargin, mMargin, mMargin); + max += Vector3(mMargin, mMargin, mMargin); } // Set the local scaling vector of the collision shape inline void TriangleShape::setLocalScaling(const Vector3& scaling) { - mPoints[0] = (mPoints[0] / mScaling) * scaling; - mPoints[1] = (mPoints[1] / mScaling) * scaling; - mPoints[2] = (mPoints[2] / mScaling) * scaling; + mPoints[0] = (mPoints[0] / mScaling) * scaling; + mPoints[1] = (mPoints[1] / mScaling) * scaling; + mPoints[2] = (mPoints[2] / mScaling) * scaling; - CollisionShape::setLocalScaling(scaling); + CollisionShape::setLocalScaling(scaling); } // Return the local inertia tensor of the triangle shape /** * @param[out] tensor The 3x3 inertia tensor matrix of the shape in local-space - * coordinates + * coordinates * @param mass Mass to use to compute the inertia tensor of the collision shape */ -inline void TriangleShape::computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const { - tensor.setToZero(); +inline void TriangleShape::computeLocalInertiaTensor(Matrix3x3& tensor, float mass) const { + tensor.setToZero(); } // Update the AABB of a body using its collision shape /** * @param[out] aabb The axis-aligned bounding box (AABB) of the collision shape - * computed in world-space coordinates + * computed in world-space coordinates * @param transform Transform used to compute the AABB of the collision shape */ inline void TriangleShape::computeAABB(AABB& aabb, const Transform& transform) const { - const Vector3 worldPoint1 = transform * mPoints[0]; - const Vector3 worldPoint2 = transform * mPoints[1]; - const Vector3 worldPoint3 = transform * mPoints[2]; + const Vector3 worldPoint1 = transform * mPoints[0]; + const Vector3 worldPoint2 = transform * mPoints[1]; + const Vector3 worldPoint3 = transform * mPoints[2]; - const Vector3 xAxis(worldPoint1.x, worldPoint2.x, worldPoint3.x); - const Vector3 yAxis(worldPoint1.y, worldPoint2.y, worldPoint3.y); - const Vector3 zAxis(worldPoint1.z, worldPoint2.z, worldPoint3.z); - aabb.setMin(Vector3(xAxis.getMinValue(), yAxis.getMinValue(), zAxis.getMinValue())); - aabb.setMax(Vector3(xAxis.getMaxValue(), yAxis.getMaxValue(), zAxis.getMaxValue())); + const Vector3 xAxis(worldPoint1.x, worldPoint2.x, worldPoint3.x); + const Vector3 yAxis(worldPoint1.y, worldPoint2.y, worldPoint3.y); + const Vector3 zAxis(worldPoint1.z, worldPoint2.z, worldPoint3.z); + aabb.setMin(Vector3(xAxis.getMinValue(), yAxis.getMinValue(), zAxis.getMinValue())); + aabb.setMax(Vector3(xAxis.getMaxValue(), yAxis.getMaxValue(), zAxis.getMaxValue())); } // Return true if a point is inside the collision shape inline bool TriangleShape::testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const { - return false; + return false; } // Return the raycast test type (front, back, front-back) inline TriangleRaycastSide TriangleShape::getRaycastTestType() const { - return m_raycastTestType; + return m_raycastTestType; } // Set the raycast test type (front, back, front-back) @@ -206,19 +185,17 @@ inline TriangleRaycastSide TriangleShape::getRaycastTestType() const { * @param testType Raycast test type for the triangle (front, back, front-back) */ inline void TriangleShape::setRaycastTestType(TriangleRaycastSide testType) { - m_raycastTestType = testType; + m_raycastTestType = testType; } // Return the coordinates of a given vertex of the triangle /** * @param index Index (0 to 2) of a vertex of the triangle */ -inline Vector3 TriangleShape::getVertex(int index) const { - assert(index >= 0 && index < 3); - return mPoints[index]; +inline Vector3 TriangleShape::getVertex(int32_t index) const { + assert(index >= 0 && index < 3); + return mPoints[index]; } } -#endif - diff --git a/ephysics/configuration.h b/ephysics/configuration.h index ec4de3b..73519b2 100644 --- a/ephysics/configuration.h +++ b/ephysics/configuration.h @@ -1,149 +1,108 @@ -/******************************************************************************** -* 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 REACTPHYSICS3D_CONFIGURATION_H -#define REACTPHYSICS3D_CONFIGURATION_H +/** @file + * @author Daniel Chappuis + * @copyright 2010-2016 Daniel Chappuis + * @license BSD 3 clauses (see license file) + */ +#pragma once // Libraries #include #include #include -#include - -// Windows platform -#if defined(WIN32) ||defined(_WIN32) || defined(_WIN64) ||defined(__WIN32__) || defined(__WINDOWS__) - #define WINDOWS_OS -#elif defined(__APPLE__) // Apple platform - #define APPLE_OS -#elif defined(__linux__) || defined(linux) || defined(__linux) // Linux platform - #define LINUX_OS -#endif +#include /// Namespace reactphysics3d namespace reactphysics3d { - -// ------------------- Type definitions ------------------- // - -typedef unsigned int uint; -typedef long unsigned int luint; -typedef luint bodyindex; -typedef std::pair bodyindexpair; - -typedef signed short int16; -typedef signed int int32; -typedef unsigned short uint16; -typedef unsigned int uint32; - -// ------------------- Enumerations ------------------- // - -/// Position correction technique used in the constraint solver (for joints). -/// BAUMGARTE_JOINTS : Faster but can be innacurate in some situations. -/// NON_LINEAR_GAUSS_SEIDEL : Slower but more precise. This is the option used by default. -enum JointsPositionCorrectionTechnique {BAUMGARTE_JOINTS, NON_LINEAR_GAUSS_SEIDEL}; - -/// Position correction technique used in the contact solver (for contacts) -/// BAUMGARTE_CONTACTS : Faster but can be innacurate and can lead to unexpected bounciness -/// in some situations (due to error correction factor being added to -/// the bodies momentum). -/// SPLIT_IMPULSES : A bit slower but the error correction factor is not added to the -/// bodies momentum. This is the option used by default. -enum ContactsPositionCorrectionTechnique {BAUMGARTE_CONTACTS, SPLIT_IMPULSES}; - -// ------------------- Constants ------------------- // - -/// Smallest decimal value (negative) -const decimal DECIMAL_SMALLEST = - std::numeric_limits::max(); - -/// Maximum decimal value -const decimal DECIMAL_LARGEST = std::numeric_limits::max(); - -/// Machine epsilon -const decimal MACHINE_EPSILON = std::numeric_limits::epsilon(); - -/// Pi constant -const decimal PI = decimal(3.14159265); - -/// 2*Pi constant -const decimal PI_TIMES_2 = decimal(6.28318530); - -/// Default friction coefficient for a rigid body -const decimal DEFAULT_FRICTION_COEFFICIENT = decimal(0.3); - -/// Default bounciness factor for a rigid body -const decimal DEFAULT_BOUNCINESS = decimal(0.5); - -/// Default rolling resistance -const decimal DEFAULT_ROLLING_RESISTANCE = decimal(0.0); - -/// True if the spleeping technique is enabled -const bool SPLEEPING_ENABLED = true; - -/// Object margin for collision detection in meters (for the GJK-EPA Algorithm) -const decimal OBJECT_MARGIN = decimal(0.04); - -/// Distance threshold for two contact points for a valid persistent contact (in meters) -const decimal PERSISTENT_CONTACT_DIST_THRESHOLD = decimal(0.03); - -/// Velocity threshold for contact velocity restitution -const decimal RESTITUTION_VELOCITY_THRESHOLD = decimal(1.0); - -/// Number of iterations when solving the velocity constraints of the Sequential Impulse technique -const uint DEFAULT_VELOCITY_SOLVER_NB_ITERATIONS = 10; - -/// Number of iterations when solving the position constraints of the Sequential Impulse technique -const uint DEFAULT_POSITION_SOLVER_NB_ITERATIONS = 5; - -/// Time (in seconds) that a body must stay still to be considered sleeping -const float DEFAULT_TIME_BEFORE_SLEEP = 1.0f; - -/// A body with a linear velocity smaller than the sleep linear velocity (in m/s) -/// might enter sleeping mode. -const decimal DEFAULT_SLEEP_LINEAR_VELOCITY = decimal(0.02); - -/// A body with angular velocity smaller than the sleep angular velocity (in rad/s) -/// might enter sleeping mode -const decimal DEFAULT_SLEEP_ANGULAR_VELOCITY = decimal(3.0 * (PI / 180.0)); - -/// In the broad-phase collision detection (dynamic AABB tree), the AABBs are -/// inflated with a constant gap to allow the collision shape to move a little bit -/// without triggering a large modification of the tree which can be costly -const decimal DYNAMIC_TREE_AABB_GAP = decimal(0.1); - -/// In the broad-phase collision detection (dynamic AABB tree), the AABBs are -/// also inflated in direction of the linear motion of the body by mutliplying the -/// followin constant with the linear velocity and the elapsed time between two frames. -const decimal DYNAMIC_TREE_AABB_LIN_GAP_MULTIPLIER = decimal(1.7); - -/// Maximum number of contact manifolds in an overlapping pair that involves two -/// convex collision shapes. -const int NB_MAX_CONTACT_MANIFOLDS_CONVEX_SHAPE = 1; - -/// Maximum number of contact manifolds in an overlapping pair that involves at -/// least one concave collision shape. -const int NB_MAX_CONTACT_MANIFOLDS_CONCAVE_SHAPE = 3; + // ------------------- Type definitions ------------------- // + typedef uint64_t bodyindex; + typedef std::pair bodyindexpair; + + // ------------------- Enumerations ------------------- // + + /// Position correction technique used in the constraint solver (for joints). + /// BAUMGARTE_JOINTS : Faster but can be innacurate in some situations. + /// NON_LINEAR_GAUSS_SEIDEL : Slower but more precise. This is the option used by default. + enum JointsPositionCorrectionTechnique {BAUMGARTE_JOINTS, NON_LINEAR_GAUSS_SEIDEL}; + + /// Position correction technique used in the contact solver (for contacts) + /// BAUMGARTE_CONTACTS : Faster but can be innacurate and can lead to unexpected bounciness + /// in some situations (due to error correction factor being added to + /// the bodies momentum). + /// SPLIT_IMPULSES : A bit slower but the error correction factor is not added to the + /// bodies momentum. This is the option used by default. + enum ContactsPositionCorrectionTechnique {BAUMGARTE_CONTACTS, SPLIT_IMPULSES}; + + // ------------------- Constants ------------------- // + + /// Smallest float value (negative) + const float DECIMAL_SMALLEST = - std::numeric_limits::max(); + + /// Maximum float value + const float DECIMAL_LARGEST = std::numeric_limits::max(); + + /// Machine epsilon + const float MACHINE_EPSILON = std::numeric_limits::epsilon(); + + /// Pi constant + const float PI = float(3.14159265); + + /// 2*Pi constant + const float PI_TIMES_2 = float(6.28318530); + + /// Default friction coefficient for a rigid body + const float DEFAULT_FRICTION_COEFFICIENT = float(0.3); + + /// Default bounciness factor for a rigid body + const float DEFAULT_BOUNCINESS = float(0.5); + + /// Default rolling resistance + const float DEFAULT_ROLLING_RESISTANCE = float(0.0); + + /// True if the spleeping technique is enabled + const bool SPLEEPING_ENABLED = true; + + /// Object margin for collision detection in meters (for the GJK-EPA Algorithm) + const float OBJECT_MARGIN = float(0.04); + + /// Distance threshold for two contact points for a valid persistent contact (in meters) + const float PERSISTENT_CONTACT_DIST_THRESHOLD = float(0.03); + + /// Velocity threshold for contact velocity restitution + const float RESTITUTION_VELOCITY_THRESHOLD = float(1.0); + + /// Number of iterations when solving the velocity constraints of the Sequential Impulse technique + const uint32_t DEFAULT_VELOCITY_SOLVER_NB_ITERATIONS = 10; + + /// Number of iterations when solving the position constraints of the Sequential Impulse technique + const uint32_t DEFAULT_POSITION_SOLVER_NB_ITERATIONS = 5; + + /// Time (in seconds) that a body must stay still to be considered sleeping + const float DEFAULT_TIME_BEFORE_SLEEP = 1.0f; + + /// A body with a linear velocity smaller than the sleep linear velocity (in m/s) + /// might enter sleeping mode. + const float DEFAULT_SLEEP_LINEAR_VELOCITY = float(0.02); + + /// A body with angular velocity smaller than the sleep angular velocity (in rad/s) + /// might enter sleeping mode + const float DEFAULT_SLEEP_ANGULAR_VELOCITY = float(3.0 * (PI / 180.0)); + + /// In the broad-phase collision detection (dynamic AABB tree), the AABBs are + /// inflated with a constant gap to allow the collision shape to move a little bit + /// without triggering a large modification of the tree which can be costly + const float DYNAMIC_TREE_AABB_GAP = float(0.1); + + /// In the broad-phase collision detection (dynamic AABB tree), the AABBs are + /// also inflated in direction of the linear motion of the body by mutliplying the + /// followin constant with the linear velocity and the elapsed time between two frames. + const float DYNAMIC_TREE_AABB_LIN_GAP_MULTIPLIER = float(1.7); + + /// Maximum number of contact manifolds in an overlapping pair that involves two + /// convex collision shapes. + const int32_t NB_MAX_CONTACT_MANIFOLDS_CONVEX_SHAPE = 1; + + /// Maximum number of contact manifolds in an overlapping pair that involves at + /// least one concave collision shape. + const int32_t NB_MAX_CONTACT_MANIFOLDS_CONCAVE_SHAPE = 3; } - -#endif diff --git a/ephysics/constraint/BallAndSocketJoint.cpp b/ephysics/constraint/BallAndSocketJoint.cpp index f94f740..863f9a3 100644 --- a/ephysics/constraint/BallAndSocketJoint.cpp +++ b/ephysics/constraint/BallAndSocketJoint.cpp @@ -1,27 +1,8 @@ -/******************************************************************************** -* 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. * -* * -********************************************************************************/ +/** @file + * @author Daniel Chappuis + * @copyright 2010-2016 Daniel Chappuis + * @license BSD 3 clauses (see license file) + */ // Libraries #include @@ -30,15 +11,15 @@ using namespace reactphysics3d; // Static variables definition -const decimal BallAndSocketJoint::BETA = decimal(0.2); +const float BallAndSocketJoint::BETA = float(0.2); // Constructor BallAndSocketJoint::BallAndSocketJoint(const BallAndSocketJointInfo& jointInfo) - : Joint(jointInfo), mImpulse(Vector3(0, 0, 0)) { + : Joint(jointInfo), mImpulse(Vector3(0, 0, 0)) { - // Compute the local-space anchor point for each body - mLocalAnchorPointBody1 = mBody1->getTransform().getInverse() * jointInfo.anchorPointWorldSpace; - mLocalAnchorPointBody2 = mBody2->getTransform().getInverse() * jointInfo.anchorPointWorldSpace; + // Compute the local-space anchor point for each body + mLocalAnchorPointBody1 = mBody1->getTransform().getInverse() * jointInfo.anchorPointWorldSpace; + mLocalAnchorPointBody2 = mBody2->getTransform().getInverse() * jointInfo.anchorPointWorldSpace; } // Destructor @@ -49,186 +30,186 @@ BallAndSocketJoint::~BallAndSocketJoint() { // Initialize before solving the constraint void BallAndSocketJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverData) { - // Initialize the bodies index in the velocity array - mIndexBody1 = constraintSolverData.mapBodyToConstrainedVelocityIndex.find(mBody1)->second; - mIndexBody2 = constraintSolverData.mapBodyToConstrainedVelocityIndex.find(mBody2)->second; + // Initialize the bodies index in the velocity array + mIndexBody1 = constraintSolverData.mapBodyToConstrainedVelocityIndex.find(mBody1)->second; + mIndexBody2 = constraintSolverData.mapBodyToConstrainedVelocityIndex.find(mBody2)->second; - // Get the bodies center of mass and orientations - const Vector3& x1 = mBody1->mCenterOfMassWorld; - const Vector3& x2 = mBody2->mCenterOfMassWorld; - const Quaternion& orientationBody1 = mBody1->getTransform().getOrientation(); - const Quaternion& orientationBody2 = mBody2->getTransform().getOrientation(); + // Get the bodies center of mass and orientations + const Vector3& x1 = mBody1->mCenterOfMassWorld; + const Vector3& x2 = mBody2->mCenterOfMassWorld; + const Quaternion& orientationBody1 = mBody1->getTransform().getOrientation(); + const Quaternion& orientationBody2 = mBody2->getTransform().getOrientation(); - // Get the inertia tensor of bodies - mI1 = mBody1->getInertiaTensorInverseWorld(); - mI2 = mBody2->getInertiaTensorInverseWorld(); + // Get the inertia tensor of bodies + mI1 = mBody1->getInertiaTensorInverseWorld(); + mI2 = mBody2->getInertiaTensorInverseWorld(); - // Compute the vector from body center to the anchor point in world-space - mR1World = orientationBody1 * mLocalAnchorPointBody1; - mR2World = orientationBody2 * mLocalAnchorPointBody2; + // Compute the vector from body center to the anchor point in world-space + mR1World = orientationBody1 * mLocalAnchorPointBody1; + mR2World = orientationBody2 * mLocalAnchorPointBody2; - // Compute the corresponding skew-symmetric matrices - Matrix3x3 skewSymmetricMatrixU1= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mR1World); - Matrix3x3 skewSymmetricMatrixU2= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mR2World); + // Compute the corresponding skew-symmetric matrices + Matrix3x3 skewSymmetricMatrixU1= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mR1World); + Matrix3x3 skewSymmetricMatrixU2= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mR2World); - // Compute the matrix K=JM^-1J^t (3x3 matrix) - decimal inverseMassBodies = mBody1->mMassInverse + mBody2->mMassInverse; - Matrix3x3 massMatrix = Matrix3x3(inverseMassBodies, 0, 0, - 0, inverseMassBodies, 0, - 0, 0, inverseMassBodies) + - skewSymmetricMatrixU1 * mI1 * skewSymmetricMatrixU1.getTranspose() + - skewSymmetricMatrixU2 * mI2 * skewSymmetricMatrixU2.getTranspose(); + // Compute the matrix K=JM^-1J^t (3x3 matrix) + float inverseMassBodies = mBody1->mMassInverse + mBody2->mMassInverse; + Matrix3x3 massMatrix = Matrix3x3(inverseMassBodies, 0, 0, + 0, inverseMassBodies, 0, + 0, 0, inverseMassBodies) + + skewSymmetricMatrixU1 * mI1 * skewSymmetricMatrixU1.getTranspose() + + skewSymmetricMatrixU2 * mI2 * skewSymmetricMatrixU2.getTranspose(); - // Compute the inverse mass matrix K^-1 - mInverseMassMatrix.setToZero(); - if (mBody1->getType() == DYNAMIC || mBody2->getType() == DYNAMIC) { - mInverseMassMatrix = massMatrix.getInverse(); - } + // Compute the inverse mass matrix K^-1 + mInverseMassMatrix.setToZero(); + if (mBody1->getType() == DYNAMIC || mBody2->getType() == DYNAMIC) { + mInverseMassMatrix = massMatrix.getInverse(); + } - // Compute the bias "b" of the constraint - mBiasVector.setToZero(); - if (mPositionCorrectionTechnique == BAUMGARTE_JOINTS) { - decimal biasFactor = (BETA / constraintSolverData.timeStep); - mBiasVector = biasFactor * (x2 + mR2World - x1 - mR1World); - } + // Compute the bias "b" of the constraint + mBiasVector.setToZero(); + if (mPositionCorrectionTechnique == BAUMGARTE_JOINTS) { + float biasFactor = (BETA / constraintSolverData.timeStep); + mBiasVector = biasFactor * (x2 + mR2World - x1 - mR1World); + } - // If warm-starting is not enabled - if (!constraintSolverData.isWarmStartingActive) { + // If warm-starting is not enabled + if (!constraintSolverData.isWarmStartingActive) { - // Reset the accumulated impulse - mImpulse.setToZero(); - } + // Reset the accumulated impulse + mImpulse.setToZero(); + } } // Warm start the constraint (apply the previous impulse at the beginning of the step) void BallAndSocketJoint::warmstart(const ConstraintSolverData& constraintSolverData) { - // Get the velocities - Vector3& v1 = constraintSolverData.linearVelocities[mIndexBody1]; - Vector3& v2 = constraintSolverData.linearVelocities[mIndexBody2]; - Vector3& w1 = constraintSolverData.angularVelocities[mIndexBody1]; - Vector3& w2 = constraintSolverData.angularVelocities[mIndexBody2]; + // Get the velocities + Vector3& v1 = constraintSolverData.linearVelocities[mIndexBody1]; + Vector3& v2 = constraintSolverData.linearVelocities[mIndexBody2]; + Vector3& w1 = constraintSolverData.angularVelocities[mIndexBody1]; + Vector3& w2 = constraintSolverData.angularVelocities[mIndexBody2]; - // Compute the impulse P=J^T * lambda for the body 1 - const Vector3 linearImpulseBody1 = -mImpulse; - const Vector3 angularImpulseBody1 = mImpulse.cross(mR1World); + // Compute the impulse P=J^T * lambda for the body 1 + const Vector3 linearImpulseBody1 = -mImpulse; + const Vector3 angularImpulseBody1 = mImpulse.cross(mR1World); - // Apply the impulse to the body 1 - v1 += mBody1->mMassInverse * linearImpulseBody1; - w1 += mI1 * angularImpulseBody1; + // Apply the impulse to the body 1 + v1 += mBody1->mMassInverse * linearImpulseBody1; + w1 += mI1 * angularImpulseBody1; - // Compute the impulse P=J^T * lambda for the body 2 - const Vector3 angularImpulseBody2 = -mImpulse.cross(mR2World); + // Compute the impulse P=J^T * lambda for the body 2 + const Vector3 angularImpulseBody2 = -mImpulse.cross(mR2World); - // Apply the impulse to the body to the body 2 - v2 += mBody2->mMassInverse * mImpulse; - w2 += mI2 * angularImpulseBody2; + // Apply the impulse to the body to the body 2 + v2 += mBody2->mMassInverse * mImpulse; + w2 += mI2 * angularImpulseBody2; } // Solve the velocity constraint void BallAndSocketJoint::solveVelocityConstraint(const ConstraintSolverData& constraintSolverData) { - // Get the velocities - Vector3& v1 = constraintSolverData.linearVelocities[mIndexBody1]; - Vector3& v2 = constraintSolverData.linearVelocities[mIndexBody2]; - Vector3& w1 = constraintSolverData.angularVelocities[mIndexBody1]; - Vector3& w2 = constraintSolverData.angularVelocities[mIndexBody2]; + // Get the velocities + Vector3& v1 = constraintSolverData.linearVelocities[mIndexBody1]; + Vector3& v2 = constraintSolverData.linearVelocities[mIndexBody2]; + Vector3& w1 = constraintSolverData.angularVelocities[mIndexBody1]; + Vector3& w2 = constraintSolverData.angularVelocities[mIndexBody2]; - // Compute J*v - const Vector3 Jv = v2 + w2.cross(mR2World) - v1 - w1.cross(mR1World); + // Compute J*v + const Vector3 Jv = v2 + w2.cross(mR2World) - v1 - w1.cross(mR1World); - // Compute the Lagrange multiplier lambda - const Vector3 deltaLambda = mInverseMassMatrix * (-Jv - mBiasVector); - mImpulse += deltaLambda; + // Compute the Lagrange multiplier lambda + const Vector3 deltaLambda = mInverseMassMatrix * (-Jv - mBiasVector); + mImpulse += deltaLambda; - // Compute the impulse P=J^T * lambda for the body 1 - const Vector3 linearImpulseBody1 = -deltaLambda; - const Vector3 angularImpulseBody1 = deltaLambda.cross(mR1World); + // Compute the impulse P=J^T * lambda for the body 1 + const Vector3 linearImpulseBody1 = -deltaLambda; + const Vector3 angularImpulseBody1 = deltaLambda.cross(mR1World); - // Apply the impulse to the body 1 - v1 += mBody1->mMassInverse * linearImpulseBody1; - w1 += mI1 * angularImpulseBody1; + // Apply the impulse to the body 1 + v1 += mBody1->mMassInverse * linearImpulseBody1; + w1 += mI1 * angularImpulseBody1; - // Compute the impulse P=J^T * lambda for the body 2 - const Vector3 angularImpulseBody2 = -deltaLambda.cross(mR2World); + // Compute the impulse P=J^T * lambda for the body 2 + const Vector3 angularImpulseBody2 = -deltaLambda.cross(mR2World); - // Apply the impulse to the body 2 - v2 += mBody2->mMassInverse * deltaLambda; - w2 += mI2 * angularImpulseBody2; + // Apply the impulse to the body 2 + v2 += mBody2->mMassInverse * deltaLambda; + w2 += mI2 * angularImpulseBody2; } // Solve the position constraint (for position error correction) void BallAndSocketJoint::solvePositionConstraint(const ConstraintSolverData& constraintSolverData) { - // If the error position correction technique is not the non-linear-gauss-seidel, we do - // do not execute this method - if (mPositionCorrectionTechnique != NON_LINEAR_GAUSS_SEIDEL) return; + // If the error position correction technique is not the non-linear-gauss-seidel, we do + // do not execute this method + if (mPositionCorrectionTechnique != NON_LINEAR_GAUSS_SEIDEL) return; - // Get the bodies center of mass and orientations - Vector3& x1 = constraintSolverData.positions[mIndexBody1]; - Vector3& x2 = constraintSolverData.positions[mIndexBody2]; - Quaternion& q1 = constraintSolverData.orientations[mIndexBody1]; - Quaternion& q2 = constraintSolverData.orientations[mIndexBody2]; + // Get the bodies center of mass and orientations + Vector3& x1 = constraintSolverData.positions[mIndexBody1]; + Vector3& x2 = constraintSolverData.positions[mIndexBody2]; + Quaternion& q1 = constraintSolverData.orientations[mIndexBody1]; + Quaternion& q2 = constraintSolverData.orientations[mIndexBody2]; - // Get the inverse mass and inverse inertia tensors of the bodies - decimal inverseMassBody1 = mBody1->mMassInverse; - decimal inverseMassBody2 = mBody2->mMassInverse; + // Get the inverse mass and inverse inertia tensors of the bodies + float inverseMassBody1 = mBody1->mMassInverse; + float inverseMassBody2 = mBody2->mMassInverse; - // Recompute the inverse inertia tensors - mI1 = mBody1->getInertiaTensorInverseWorld(); - mI2 = mBody2->getInertiaTensorInverseWorld(); + // Recompute the inverse inertia tensors + mI1 = mBody1->getInertiaTensorInverseWorld(); + mI2 = mBody2->getInertiaTensorInverseWorld(); - // Compute the vector from body center to the anchor point in world-space - mR1World = q1 * mLocalAnchorPointBody1; - mR2World = q2 * mLocalAnchorPointBody2; + // Compute the vector from body center to the anchor point in world-space + mR1World = q1 * mLocalAnchorPointBody1; + mR2World = q2 * mLocalAnchorPointBody2; - // Compute the corresponding skew-symmetric matrices - Matrix3x3 skewSymmetricMatrixU1= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mR1World); - Matrix3x3 skewSymmetricMatrixU2= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mR2World); + // Compute the corresponding skew-symmetric matrices + Matrix3x3 skewSymmetricMatrixU1= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mR1World); + Matrix3x3 skewSymmetricMatrixU2= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mR2World); - // Recompute the inverse mass matrix K=J^TM^-1J of of the 3 translation constraints - decimal inverseMassBodies = inverseMassBody1 + inverseMassBody2; - Matrix3x3 massMatrix = Matrix3x3(inverseMassBodies, 0, 0, - 0, inverseMassBodies, 0, - 0, 0, inverseMassBodies) + - skewSymmetricMatrixU1 * mI1 * skewSymmetricMatrixU1.getTranspose() + - skewSymmetricMatrixU2 * mI2 * skewSymmetricMatrixU2.getTranspose(); - mInverseMassMatrix.setToZero(); - if (mBody1->getType() == DYNAMIC || mBody2->getType() == DYNAMIC) { - mInverseMassMatrix = massMatrix.getInverse(); - } + // Recompute the inverse mass matrix K=J^TM^-1J of of the 3 translation constraints + float inverseMassBodies = inverseMassBody1 + inverseMassBody2; + Matrix3x3 massMatrix = Matrix3x3(inverseMassBodies, 0, 0, + 0, inverseMassBodies, 0, + 0, 0, inverseMassBodies) + + skewSymmetricMatrixU1 * mI1 * skewSymmetricMatrixU1.getTranspose() + + skewSymmetricMatrixU2 * mI2 * skewSymmetricMatrixU2.getTranspose(); + mInverseMassMatrix.setToZero(); + if (mBody1->getType() == DYNAMIC || mBody2->getType() == DYNAMIC) { + mInverseMassMatrix = massMatrix.getInverse(); + } - // Compute the constraint error (value of the C(x) function) - const Vector3 constraintError = (x2 + mR2World - x1 - mR1World); + // Compute the constraint error (value of the C(x) function) + const Vector3 constraintError = (x2 + mR2World - x1 - mR1World); - // Compute the Lagrange multiplier lambda - // TODO : Do not solve the system by computing the inverse each time and multiplying with the - // right-hand side vector but instead use a method to directly solve the linear system. - const Vector3 lambda = mInverseMassMatrix * (-constraintError); + // Compute the Lagrange multiplier lambda + // TODO : Do not solve the system by computing the inverse each time and multiplying with the + // right-hand side vector but instead use a method to directly solve the linear system. + const Vector3 lambda = mInverseMassMatrix * (-constraintError); - // Compute the impulse of body 1 - const Vector3 linearImpulseBody1 = -lambda; - const Vector3 angularImpulseBody1 = lambda.cross(mR1World); + // Compute the impulse of body 1 + const Vector3 linearImpulseBody1 = -lambda; + const Vector3 angularImpulseBody1 = lambda.cross(mR1World); - // Compute the pseudo velocity of body 1 - const Vector3 v1 = inverseMassBody1 * linearImpulseBody1; - const Vector3 w1 = mI1 * angularImpulseBody1; + // Compute the pseudo velocity of body 1 + const Vector3 v1 = inverseMassBody1 * linearImpulseBody1; + const Vector3 w1 = mI1 * angularImpulseBody1; - // Update the body center of mass and orientation of body 1 - x1 += v1; - q1 += Quaternion(0, w1) * q1 * decimal(0.5); - q1.normalize(); + // Update the body center of mass and orientation of body 1 + x1 += v1; + q1 += Quaternion(0, w1) * q1 * float(0.5); + q1.normalize(); - // Compute the impulse of body 2 - const Vector3 angularImpulseBody2 = -lambda.cross(mR2World); + // Compute the impulse of body 2 + const Vector3 angularImpulseBody2 = -lambda.cross(mR2World); - // Compute the pseudo velocity of body 2 - const Vector3 v2 = inverseMassBody2 * lambda; - const Vector3 w2 = mI2 * angularImpulseBody2; + // Compute the pseudo velocity of body 2 + const Vector3 v2 = inverseMassBody2 * lambda; + const Vector3 w2 = mI2 * angularImpulseBody2; - // Update the body position/orientation of body 2 - x2 += v2; - q2 += Quaternion(0, w2) * q2 * decimal(0.5); - q2.normalize(); + // Update the body position/orientation of body 2 + x2 += v2; + q2 += Quaternion(0, w2) * q2 * float(0.5); + q2.normalize(); } diff --git a/ephysics/constraint/BallAndSocketJoint.h b/ephysics/constraint/BallAndSocketJoint.h index 789ef09..c363cb5 100644 --- a/ephysics/constraint/BallAndSocketJoint.h +++ b/ephysics/constraint/BallAndSocketJoint.h @@ -1,30 +1,9 @@ -/******************************************************************************** -* 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 REACTPHYSICS3D_BALL_AND_SOCKET_JOINT_H -#define REACTPHYSICS3D_BALL_AND_SOCKET_JOINT_H +/** @file + * @author Daniel Chappuis + * @copyright 2010-2016 Daniel Chappuis + * @license BSD 3 clauses (see license file) + */ +#pragma once // Libraries #include @@ -39,24 +18,24 @@ namespace reactphysics3d { */ struct BallAndSocketJointInfo : public JointInfo { - public : + public : - // -------------------- Attributes -------------------- // + // -------------------- Attributes -------------------- // - /// Anchor point (in world-space coordinates) - Vector3 anchorPointWorldSpace; + /// Anchor point (in world-space coordinates) + Vector3 anchorPointWorldSpace; - /// Constructor - /** - * @param rigidBody1 Pointer to the first body of the joint - * @param rigidBody2 Pointer to the second body of the joint - * @param initAnchorPointWorldSpace The anchor point in world-space - * coordinates - */ - BallAndSocketJointInfo(RigidBody* rigidBody1, RigidBody* rigidBody2, - const Vector3& initAnchorPointWorldSpace) - : JointInfo(rigidBody1, rigidBody2, BALLSOCKETJOINT), - anchorPointWorldSpace(initAnchorPointWorldSpace) {} + /// Constructor + /** + * @param rigidBody1 Pointer to the first body of the joint + * @param rigidBody2 Pointer to the second body of the joint + * @param initAnchorPointWorldSpace The anchor point in world-space + * coordinates + */ + BallAndSocketJointInfo(RigidBody* rigidBody1, RigidBody* rigidBody2, + const Vector3& initAnchorPointWorldSpace) + : JointInfo(rigidBody1, rigidBody2, BALLSOCKETJOINT), + anchorPointWorldSpace(initAnchorPointWorldSpace) {} }; // Class BallAndSocketJoint @@ -67,81 +46,79 @@ struct BallAndSocketJointInfo : public JointInfo { */ class BallAndSocketJoint : public Joint { - private : + private : - // -------------------- Constants -------------------- // + // -------------------- Constants -------------------- // - // Beta value for the bias factor of position correction - static const decimal BETA; + // Beta value for the bias factor of position correction + static const float BETA; - // -------------------- Attributes -------------------- // + // -------------------- Attributes -------------------- // - /// Anchor point of body 1 (in local-space coordinates of body 1) - Vector3 mLocalAnchorPointBody1; + /// Anchor point of body 1 (in local-space coordinates of body 1) + Vector3 mLocalAnchorPointBody1; - /// Anchor point of body 2 (in local-space coordinates of body 2) - Vector3 mLocalAnchorPointBody2; + /// Anchor point of body 2 (in local-space coordinates of body 2) + Vector3 mLocalAnchorPointBody2; - /// Vector from center of body 2 to anchor point in world-space - Vector3 mR1World; + /// Vector from center of body 2 to anchor point in world-space + Vector3 mR1World; - /// Vector from center of body 2 to anchor point in world-space - Vector3 mR2World; + /// Vector from center of body 2 to anchor point in world-space + Vector3 mR2World; - /// Inertia tensor of body 1 (in world-space coordinates) - Matrix3x3 mI1; + /// Inertia tensor of body 1 (in world-space coordinates) + Matrix3x3 mI1; - /// Inertia tensor of body 2 (in world-space coordinates) - Matrix3x3 mI2; + /// Inertia tensor of body 2 (in world-space coordinates) + Matrix3x3 mI2; - /// Bias vector for the constraint - Vector3 mBiasVector; + /// Bias vector for the constraint + Vector3 mBiasVector; - /// Inverse mass matrix K=JM^-1J^-t of the constraint - Matrix3x3 mInverseMassMatrix; + /// Inverse mass matrix K=JM^-1J^-t of the constraint + Matrix3x3 mInverseMassMatrix; - /// Accumulated impulse - Vector3 mImpulse; + /// Accumulated impulse + Vector3 mImpulse; - // -------------------- Methods -------------------- // + // -------------------- Methods -------------------- // - /// Private copy-constructor - BallAndSocketJoint(const BallAndSocketJoint& constraint); + /// Private copy-constructor + BallAndSocketJoint(const BallAndSocketJoint& constraint); - /// Private assignment operator - BallAndSocketJoint& operator=(const BallAndSocketJoint& constraint); + /// Private assignment operator + BallAndSocketJoint& operator=(const BallAndSocketJoint& constraint); - /// Return the number of bytes used by the joint - virtual size_t getSizeInBytes() const; + /// Return the number of bytes used by the joint + virtual size_t getSizeInBytes() const; - /// Initialize before solving the constraint - virtual void initBeforeSolve(const ConstraintSolverData& constraintSolverData); + /// Initialize before solving the constraint + virtual void initBeforeSolve(const ConstraintSolverData& constraintSolverData); - /// Warm start the constraint (apply the previous impulse at the beginning of the step) - virtual void warmstart(const ConstraintSolverData& constraintSolverData); + /// Warm start the constraint (apply the previous impulse at the beginning of the step) + virtual void warmstart(const ConstraintSolverData& constraintSolverData); - /// Solve the velocity constraint - virtual void solveVelocityConstraint(const ConstraintSolverData& constraintSolverData); + /// Solve the velocity constraint + virtual void solveVelocityConstraint(const ConstraintSolverData& constraintSolverData); - /// Solve the position constraint (for position error correction) - virtual void solvePositionConstraint(const ConstraintSolverData& constraintSolverData); + /// Solve the position constraint (for position error correction) + virtual void solvePositionConstraint(const ConstraintSolverData& constraintSolverData); - public : + public : - // -------------------- Methods -------------------- // + // -------------------- Methods -------------------- // - /// Constructor - BallAndSocketJoint(const BallAndSocketJointInfo& jointInfo); + /// Constructor + BallAndSocketJoint(const BallAndSocketJointInfo& jointInfo); - /// Destructor - virtual ~BallAndSocketJoint(); + /// Destructor + virtual ~BallAndSocketJoint(); }; // Return the number of bytes used by the joint inline size_t BallAndSocketJoint::getSizeInBytes() const { - return sizeof(BallAndSocketJoint); + return sizeof(BallAndSocketJoint); } } - -#endif diff --git a/ephysics/constraint/ContactPoint.cpp b/ephysics/constraint/ContactPoint.cpp index 15e6dbf..ff49b44 100644 --- a/ephysics/constraint/ContactPoint.cpp +++ b/ephysics/constraint/ContactPoint.cpp @@ -1,27 +1,8 @@ -/******************************************************************************** -* 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. * -* * -********************************************************************************/ +/** @file + * @author Daniel Chappuis + * @copyright 2010-2016 Daniel Chappuis + * @license BSD 3 clauses (see license file) + */ // Libraries #include @@ -32,23 +13,23 @@ using namespace std; // Constructor ContactPoint::ContactPoint(const ContactPointInfo& contactInfo) - : mBody1(contactInfo.shape1->getBody()), mBody2(contactInfo.shape2->getBody()), - mNormal(contactInfo.normal), - mPenetrationDepth(contactInfo.penetrationDepth), - mLocalPointOnBody1(contactInfo.localPoint1), - mLocalPointOnBody2(contactInfo.localPoint2), - mWorldPointOnBody1(contactInfo.shape1->getBody()->getTransform() * - contactInfo.shape1->getLocalToBodyTransform() * - contactInfo.localPoint1), - mWorldPointOnBody2(contactInfo.shape2->getBody()->getTransform() * - contactInfo.shape2->getLocalToBodyTransform() * - contactInfo.localPoint2), - mIsRestingContact(false) { + : mBody1(contactInfo.shape1->getBody()), mBody2(contactInfo.shape2->getBody()), + mNormal(contactInfo.normal), + mPenetrationDepth(contactInfo.penetrationDepth), + mLocalPointOnBody1(contactInfo.localPoint1), + mLocalPointOnBody2(contactInfo.localPoint2), + mWorldPointOnBody1(contactInfo.shape1->getBody()->getTransform() * + contactInfo.shape1->getLocalToBodyTransform() * + contactInfo.localPoint1), + mWorldPointOnBody2(contactInfo.shape2->getBody()->getTransform() * + contactInfo.shape2->getLocalToBodyTransform() * + contactInfo.localPoint2), + mIsRestingContact(false) { - mFrictionVectors[0] = Vector3(0, 0, 0); - mFrictionVectors[1] = Vector3(0, 0, 0); + mFrictionVectors[0] = Vector3(0, 0, 0); + mFrictionVectors[1] = Vector3(0, 0, 0); - assert(mPenetrationDepth > 0.0); + assert(mPenetrationDepth > 0.0); } diff --git a/ephysics/constraint/ContactPoint.h b/ephysics/constraint/ContactPoint.h index 67aa1e7..9e2844e 100644 --- a/ephysics/constraint/ContactPoint.h +++ b/ephysics/constraint/ContactPoint.h @@ -1,30 +1,9 @@ -/******************************************************************************** -* 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 REACTPHYSICS3D_CONTACT_POINT_H -#define REACTPHYSICS3D_CONTACT_POINT_H +/** @file + * @author Daniel Chappuis + * @copyright 2010-2016 Daniel Chappuis + * @license BSD 3 clauses (see license file) + */ +#pragma once // Libraries #include @@ -45,49 +24,49 @@ namespace reactphysics3d { */ struct ContactPointInfo { - private: + private: - // -------------------- Methods -------------------- // + // -------------------- Methods -------------------- // - public: + public: - // -------------------- Attributes -------------------- // + // -------------------- Attributes -------------------- // - /// First proxy shape of the contact - ProxyShape* shape1; + /// First proxy shape of the contact + ProxyShape* shape1; - /// Second proxy shape of the contact - ProxyShape* shape2; + /// Second proxy shape of the contact + ProxyShape* shape2; - /// First collision shape - const CollisionShape* collisionShape1; + /// First collision shape + const CollisionShape* collisionShape1; - /// Second collision shape - const CollisionShape* collisionShape2; + /// Second collision shape + const CollisionShape* collisionShape2; - /// Normalized normal vector of the collision contact in world space - Vector3 normal; + /// Normalized normal vector of the collision contact in world space + Vector3 normal; - /// Penetration depth of the contact - decimal penetrationDepth; + /// Penetration depth of the contact + float penetrationDepth; - /// Contact point of body 1 in local space of body 1 - Vector3 localPoint1; + /// 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; + /// Contact point of body 2 in local space of body 2 + Vector3 localPoint2; - // -------------------- Methods -------------------- // + // -------------------- Methods -------------------- // - /// Constructor - ContactPointInfo(ProxyShape* proxyShape1, ProxyShape* proxyShape2, const CollisionShape* collShape1, - const CollisionShape* collShape2, const Vector3& normal, decimal penetrationDepth, - const Vector3& localPoint1, const Vector3& localPoint2) - : shape1(proxyShape1), shape2(proxyShape2), collisionShape1(collShape1), collisionShape2(collShape2), - normal(normal), penetrationDepth(penetrationDepth), localPoint1(localPoint1), - localPoint2(localPoint2) { + /// Constructor + ContactPointInfo(ProxyShape* proxyShape1, ProxyShape* proxyShape2, const CollisionShape* collShape1, + const CollisionShape* collShape2, const Vector3& normal, float penetrationDepth, + const Vector3& localPoint1, const Vector3& localPoint2) + : shape1(proxyShape1), shape2(proxyShape2), collisionShape1(collShape1), collisionShape2(collShape2), + normal(normal), penetrationDepth(penetrationDepth), localPoint1(localPoint1), + localPoint2(localPoint2) { - } + } }; // Class ContactPoint @@ -97,279 +76,277 @@ struct ContactPointInfo { */ class ContactPoint { - private : + private : - // -------------------- Attributes -------------------- // + // -------------------- Attributes -------------------- // - /// First rigid body of the contact - CollisionBody* mBody1; + /// First rigid body of the contact + CollisionBody* mBody1; - /// Second rigid body of the contact - CollisionBody* mBody2; + /// Second rigid body of the contact + CollisionBody* mBody2; - /// Normalized normal vector of the contact (from body1 toward body2) in world space - const Vector3 mNormal; + /// Normalized normal vector of the contact (from body1 toward body2) in world space + const Vector3 mNormal; - /// Penetration depth - decimal mPenetrationDepth; + /// Penetration depth + float mPenetrationDepth; - /// Contact point on body 1 in local space of body 1 - const Vector3 mLocalPointOnBody1; + /// Contact point on body 1 in local space of body 1 + const Vector3 mLocalPointOnBody1; - /// Contact point on body 2 in local space of body 2 - const Vector3 mLocalPointOnBody2; + /// Contact point on body 2 in local space of body 2 + const Vector3 mLocalPointOnBody2; - /// Contact point on body 1 in world space - Vector3 mWorldPointOnBody1; + /// Contact point on body 1 in world space + Vector3 mWorldPointOnBody1; - /// Contact point on body 2 in world space - Vector3 mWorldPointOnBody2; + /// Contact point on body 2 in world space + Vector3 mWorldPointOnBody2; - /// True if the contact is a resting contact (exists for more than one time step) - bool mIsRestingContact; + /// True if the contact is a resting contact (exists for more than one time step) + bool mIsRestingContact; - /// Two orthogonal vectors that span the tangential friction plane - Vector3 mFrictionVectors[2]; + /// Two orthogonal vectors that span the tangential friction plane + Vector3 mFrictionVectors[2]; - /// Cached penetration impulse - decimal mPenetrationImpulse; + /// Cached penetration impulse + float mPenetrationImpulse; - /// Cached first friction impulse - decimal mFrictionImpulse1; + /// Cached first friction impulse + float mFrictionImpulse1; - /// Cached second friction impulse - decimal mFrictionImpulse2; + /// Cached second friction impulse + float mFrictionImpulse2; - /// Cached rolling resistance impulse - Vector3 mRollingResistanceImpulse; - - // -------------------- Methods -------------------- // + /// Cached rolling resistance impulse + Vector3 mRollingResistanceImpulse; + + // -------------------- Methods -------------------- // - /// Private copy-constructor - ContactPoint(const ContactPoint& contact); + /// Private copy-constructor + ContactPoint(const ContactPoint& contact); - /// Private assignment operator - ContactPoint& operator=(const ContactPoint& contact); + /// Private assignment operator + ContactPoint& operator=(const ContactPoint& contact); - public : + public : - // -------------------- Methods -------------------- // + // -------------------- Methods -------------------- // - /// Constructor - ContactPoint(const ContactPointInfo& contactInfo); + /// Constructor + ContactPoint(const ContactPointInfo& contactInfo); - /// Destructor - ~ContactPoint(); + /// Destructor + ~ContactPoint(); - /// Return the reference to the body 1 - CollisionBody* getBody1() const; + /// Return the reference to the body 1 + CollisionBody* getBody1() const; - /// Return the reference to the body 2 - CollisionBody* getBody2() const; + /// Return the reference to the body 2 + CollisionBody* getBody2() const; - /// Return the normal vector of the contact - Vector3 getNormal() const; + /// Return the normal vector of the contact + Vector3 getNormal() const; - /// Set the penetration depth of the contact - void setPenetrationDepth(decimal penetrationDepth); + /// Set the penetration depth of the contact + void setPenetrationDepth(float penetrationDepth); - /// Return the contact local point on body 1 - Vector3 getLocalPointOnBody1() const; + /// Return the contact local point on body 1 + Vector3 getLocalPointOnBody1() const; - /// Return the contact local point on body 2 - Vector3 getLocalPointOnBody2() const; + /// Return the contact local point on body 2 + Vector3 getLocalPointOnBody2() const; - /// Return the contact world point on body 1 - Vector3 getWorldPointOnBody1() const; + /// Return the contact world point on body 1 + Vector3 getWorldPointOnBody1() const; - /// Return the contact world point on body 2 - Vector3 getWorldPointOnBody2() const; + /// Return the contact world point on body 2 + Vector3 getWorldPointOnBody2() const; - /// Return the cached penetration impulse - decimal getPenetrationImpulse() const; + /// Return the cached penetration impulse + float getPenetrationImpulse() const; - /// Return the cached first friction impulse - decimal getFrictionImpulse1() const; + /// Return the cached first friction impulse + float getFrictionImpulse1() const; - /// Return the cached second friction impulse - decimal getFrictionImpulse2() const; + /// Return the cached second friction impulse + float getFrictionImpulse2() const; - /// Return the cached rolling resistance impulse - Vector3 getRollingResistanceImpulse() const; + /// Return the cached rolling resistance impulse + Vector3 getRollingResistanceImpulse() const; - /// Set the cached penetration impulse - void setPenetrationImpulse(decimal impulse); + /// Set the cached penetration impulse + void setPenetrationImpulse(float impulse); - /// Set the first cached friction impulse - void setFrictionImpulse1(decimal impulse); + /// Set the first cached friction impulse + void setFrictionImpulse1(float impulse); - /// Set the second cached friction impulse - void setFrictionImpulse2(decimal impulse); + /// Set the second cached friction impulse + void setFrictionImpulse2(float impulse); - /// Set the cached rolling resistance impulse - void setRollingResistanceImpulse(const Vector3& impulse); + /// Set the cached rolling resistance impulse + void setRollingResistanceImpulse(const Vector3& impulse); - /// Set the contact world point on body 1 - void setWorldPointOnBody1(const Vector3& worldPoint); + /// Set the contact world point on body 1 + void setWorldPointOnBody1(const Vector3& worldPoint); - /// Set the contact world point on body 2 - void setWorldPointOnBody2(const Vector3& worldPoint); + /// Set the contact world point on body 2 + void setWorldPointOnBody2(const Vector3& worldPoint); - /// Return true if the contact is a resting contact - bool getIsRestingContact() const; + /// Return true if the contact is a resting contact + bool getIsRestingContact() const; - /// Set the mIsRestingContact variable - void setIsRestingContact(bool isRestingContact); + /// Set the mIsRestingContact variable + void setIsRestingContact(bool isRestingContact); - /// Get the first friction vector - Vector3 getFrictionVector1() const; + /// Get the first friction vector + Vector3 getFrictionVector1() const; - /// Set the first friction vector - void setFrictionVector1(const Vector3& frictionVector1); + /// Set the first friction vector + void setFrictionVector1(const Vector3& frictionVector1); - /// Get the second friction vector - Vector3 getFrictionVector2() const; + /// Get the second friction vector + Vector3 getFrictionVector2() const; - /// Set the second friction vector - void setFrictionVector2(const Vector3& frictionVector2); + /// Set the second friction vector + void setFrictionVector2(const Vector3& frictionVector2); - /// Return the penetration depth - decimal getPenetrationDepth() const; + /// Return the penetration depth + float getPenetrationDepth() const; - /// Return the number of bytes used by the contact point - size_t getSizeInBytes() const; + /// Return the number of bytes used by the contact point + size_t getSizeInBytes() const; }; // Return the reference to the body 1 inline CollisionBody* ContactPoint::getBody1() const { - return mBody1; + return mBody1; } // Return the reference to the body 2 inline CollisionBody* ContactPoint::getBody2() const { - return mBody2; + return mBody2; } // Return the normal vector of the contact inline Vector3 ContactPoint::getNormal() const { - return mNormal; + return mNormal; } // Set the penetration depth of the contact -inline void ContactPoint::setPenetrationDepth(decimal penetrationDepth) { - this->mPenetrationDepth = penetrationDepth; +inline void ContactPoint::setPenetrationDepth(float penetrationDepth) { + this->mPenetrationDepth = penetrationDepth; } // Return the contact point on body 1 inline Vector3 ContactPoint::getLocalPointOnBody1() const { - return mLocalPointOnBody1; + return mLocalPointOnBody1; } // Return the contact point on body 2 inline Vector3 ContactPoint::getLocalPointOnBody2() const { - return mLocalPointOnBody2; + return mLocalPointOnBody2; } // Return the contact world point on body 1 inline Vector3 ContactPoint::getWorldPointOnBody1() const { - return mWorldPointOnBody1; + return mWorldPointOnBody1; } // Return the contact world point on body 2 inline Vector3 ContactPoint::getWorldPointOnBody2() const { - return mWorldPointOnBody2; + return mWorldPointOnBody2; } // Return the cached penetration impulse -inline decimal ContactPoint::getPenetrationImpulse() const { - return mPenetrationImpulse; +inline float ContactPoint::getPenetrationImpulse() const { + return mPenetrationImpulse; } // Return the cached first friction impulse -inline decimal ContactPoint::getFrictionImpulse1() const { - return mFrictionImpulse1; +inline float ContactPoint::getFrictionImpulse1() const { + return mFrictionImpulse1; } // Return the cached second friction impulse -inline decimal ContactPoint::getFrictionImpulse2() const { - return mFrictionImpulse2; +inline float ContactPoint::getFrictionImpulse2() const { + return mFrictionImpulse2; } // Return the cached rolling resistance impulse inline Vector3 ContactPoint::getRollingResistanceImpulse() const { - return mRollingResistanceImpulse; + return mRollingResistanceImpulse; } // Set the cached penetration impulse -inline void ContactPoint::setPenetrationImpulse(decimal impulse) { - mPenetrationImpulse = impulse; +inline void ContactPoint::setPenetrationImpulse(float impulse) { + mPenetrationImpulse = impulse; } // Set the first cached friction impulse -inline void ContactPoint::setFrictionImpulse1(decimal impulse) { - mFrictionImpulse1 = impulse; +inline void ContactPoint::setFrictionImpulse1(float impulse) { + mFrictionImpulse1 = impulse; } // Set the second cached friction impulse -inline void ContactPoint::setFrictionImpulse2(decimal impulse) { - mFrictionImpulse2 = impulse; +inline void ContactPoint::setFrictionImpulse2(float impulse) { + mFrictionImpulse2 = impulse; } // Set the cached rolling resistance impulse inline void ContactPoint::setRollingResistanceImpulse(const Vector3& impulse) { - mRollingResistanceImpulse = impulse; + mRollingResistanceImpulse = impulse; } // Set the contact world point on body 1 inline void ContactPoint::setWorldPointOnBody1(const Vector3& worldPoint) { - mWorldPointOnBody1 = worldPoint; + mWorldPointOnBody1 = worldPoint; } // Set the contact world point on body 2 inline void ContactPoint::setWorldPointOnBody2(const Vector3& worldPoint) { - mWorldPointOnBody2 = worldPoint; + mWorldPointOnBody2 = worldPoint; } // Return true if the contact is a resting contact inline bool ContactPoint::getIsRestingContact() const { - return mIsRestingContact; + return mIsRestingContact; } // Set the mIsRestingContact variable inline void ContactPoint::setIsRestingContact(bool isRestingContact) { - mIsRestingContact = isRestingContact; + mIsRestingContact = isRestingContact; } // Get the first friction vector inline Vector3 ContactPoint::getFrictionVector1() const { - return mFrictionVectors[0]; + return mFrictionVectors[0]; } // Set the first friction vector inline void ContactPoint::setFrictionVector1(const Vector3& frictionVector1) { - mFrictionVectors[0] = frictionVector1; + mFrictionVectors[0] = frictionVector1; } // Get the second friction vector inline Vector3 ContactPoint::getFrictionVector2() const { - return mFrictionVectors[1]; + return mFrictionVectors[1]; } // Set the second friction vector inline void ContactPoint::setFrictionVector2(const Vector3& frictionVector2) { - mFrictionVectors[1] = frictionVector2; + mFrictionVectors[1] = frictionVector2; } // Return the penetration depth of the contact -inline decimal ContactPoint::getPenetrationDepth() const { - return mPenetrationDepth; +inline float ContactPoint::getPenetrationDepth() const { + return mPenetrationDepth; } // Return the number of bytes used by the contact point inline size_t ContactPoint::getSizeInBytes() const { - return sizeof(ContactPoint); + return sizeof(ContactPoint); } } - -#endif diff --git a/ephysics/constraint/FixedJoint.cpp b/ephysics/constraint/FixedJoint.cpp index abcea2e..8e7ec5f 100644 --- a/ephysics/constraint/FixedJoint.cpp +++ b/ephysics/constraint/FixedJoint.cpp @@ -1,27 +1,8 @@ -/******************************************************************************** -* 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. * -* * -********************************************************************************/ +/** @file + * @author Daniel Chappuis + * @copyright 2010-2016 Daniel Chappuis + * @license BSD 3 clauses (see license file) + */ // Libraries #include @@ -30,23 +11,23 @@ using namespace reactphysics3d; // Static variables definition -const decimal FixedJoint::BETA = decimal(0.2); +const float FixedJoint::BETA = float(0.2); // Constructor FixedJoint::FixedJoint(const FixedJointInfo& jointInfo) - : Joint(jointInfo), mImpulseTranslation(0, 0, 0), mImpulseRotation(0, 0, 0) { + : Joint(jointInfo), mImpulseTranslation(0, 0, 0), mImpulseRotation(0, 0, 0) { - // Compute the local-space anchor point for each body - const Transform& transform1 = mBody1->getTransform(); - const Transform& transform2 = mBody2->getTransform(); - mLocalAnchorPointBody1 = transform1.getInverse() * jointInfo.anchorPointWorldSpace; - mLocalAnchorPointBody2 = transform2.getInverse() * jointInfo.anchorPointWorldSpace; + // Compute the local-space anchor point for each body + const Transform& transform1 = mBody1->getTransform(); + const Transform& transform2 = mBody2->getTransform(); + mLocalAnchorPointBody1 = transform1.getInverse() * jointInfo.anchorPointWorldSpace; + mLocalAnchorPointBody2 = transform2.getInverse() * jointInfo.anchorPointWorldSpace; - // Compute the inverse of the initial orientation difference between the two bodies - mInitOrientationDifferenceInv = transform2.getOrientation() * - transform1.getOrientation().getInverse(); - mInitOrientationDifferenceInv.normalize(); - mInitOrientationDifferenceInv.inverse(); + // Compute the inverse of the initial orientation difference between the two bodies + mInitOrientationDifferenceInv = transform2.getOrientation() * + transform1.getOrientation().getInverse(); + mInitOrientationDifferenceInv.normalize(); + mInitOrientationDifferenceInv.inverse(); } // Destructor @@ -57,273 +38,273 @@ FixedJoint::~FixedJoint() { // Initialize before solving the constraint void FixedJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverData) { - // Initialize the bodies index in the velocity array - mIndexBody1 = constraintSolverData.mapBodyToConstrainedVelocityIndex.find(mBody1)->second; - mIndexBody2 = constraintSolverData.mapBodyToConstrainedVelocityIndex.find(mBody2)->second; + // Initialize the bodies index in the velocity array + mIndexBody1 = constraintSolverData.mapBodyToConstrainedVelocityIndex.find(mBody1)->second; + mIndexBody2 = constraintSolverData.mapBodyToConstrainedVelocityIndex.find(mBody2)->second; - // Get the bodies positions and orientations - const Vector3& x1 = mBody1->mCenterOfMassWorld; - const Vector3& x2 = mBody2->mCenterOfMassWorld; - const Quaternion& orientationBody1 = mBody1->getTransform().getOrientation(); - const Quaternion& orientationBody2 = mBody2->getTransform().getOrientation(); + // Get the bodies positions and orientations + const Vector3& x1 = mBody1->mCenterOfMassWorld; + const Vector3& x2 = mBody2->mCenterOfMassWorld; + const Quaternion& orientationBody1 = mBody1->getTransform().getOrientation(); + const Quaternion& orientationBody2 = mBody2->getTransform().getOrientation(); - // Get the inertia tensor of bodies - mI1 = mBody1->getInertiaTensorInverseWorld(); - mI2 = mBody2->getInertiaTensorInverseWorld(); + // Get the inertia tensor of bodies + mI1 = mBody1->getInertiaTensorInverseWorld(); + mI2 = mBody2->getInertiaTensorInverseWorld(); - // Compute the vector from body center to the anchor point in world-space - mR1World = orientationBody1 * mLocalAnchorPointBody1; - mR2World = orientationBody2 * mLocalAnchorPointBody2; + // Compute the vector from body center to the anchor point in world-space + mR1World = orientationBody1 * mLocalAnchorPointBody1; + mR2World = orientationBody2 * mLocalAnchorPointBody2; - // Compute the corresponding skew-symmetric matrices - Matrix3x3 skewSymmetricMatrixU1= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mR1World); - Matrix3x3 skewSymmetricMatrixU2= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mR2World); + // Compute the corresponding skew-symmetric matrices + Matrix3x3 skewSymmetricMatrixU1= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mR1World); + Matrix3x3 skewSymmetricMatrixU2= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mR2World); - // Compute the matrix K=JM^-1J^t (3x3 matrix) for the 3 translation constraints - decimal inverseMassBodies = mBody1->mMassInverse + mBody2->mMassInverse; - Matrix3x3 massMatrix = Matrix3x3(inverseMassBodies, 0, 0, - 0, inverseMassBodies, 0, - 0, 0, inverseMassBodies) + - skewSymmetricMatrixU1 * mI1 * skewSymmetricMatrixU1.getTranspose() + - skewSymmetricMatrixU2 * mI2 * skewSymmetricMatrixU2.getTranspose(); + // Compute the matrix K=JM^-1J^t (3x3 matrix) for the 3 translation constraints + float inverseMassBodies = mBody1->mMassInverse + mBody2->mMassInverse; + Matrix3x3 massMatrix = Matrix3x3(inverseMassBodies, 0, 0, + 0, inverseMassBodies, 0, + 0, 0, inverseMassBodies) + + skewSymmetricMatrixU1 * mI1 * skewSymmetricMatrixU1.getTranspose() + + skewSymmetricMatrixU2 * mI2 * skewSymmetricMatrixU2.getTranspose(); - // Compute the inverse mass matrix K^-1 for the 3 translation constraints - mInverseMassMatrixTranslation.setToZero(); - if (mBody1->getType() == DYNAMIC || mBody2->getType() == DYNAMIC) { - mInverseMassMatrixTranslation = massMatrix.getInverse(); - } + // Compute the inverse mass matrix K^-1 for the 3 translation constraints + mInverseMassMatrixTranslation.setToZero(); + if (mBody1->getType() == DYNAMIC || mBody2->getType() == DYNAMIC) { + mInverseMassMatrixTranslation = massMatrix.getInverse(); + } - // Compute the bias "b" of the constraint for the 3 translation constraints - decimal biasFactor = (BETA / constraintSolverData.timeStep); - mBiasTranslation.setToZero(); - if (mPositionCorrectionTechnique == BAUMGARTE_JOINTS) { - mBiasTranslation = biasFactor * (x2 + mR2World - x1 - mR1World); - } + // Compute the bias "b" of the constraint for the 3 translation constraints + float biasFactor = (BETA / constraintSolverData.timeStep); + mBiasTranslation.setToZero(); + if (mPositionCorrectionTechnique == BAUMGARTE_JOINTS) { + mBiasTranslation = biasFactor * (x2 + mR2World - x1 - mR1World); + } - // Compute the inverse of the mass matrix K=JM^-1J^t for the 3 rotation - // contraints (3x3 matrix) - mInverseMassMatrixRotation = mI1 + mI2; - if (mBody1->getType() == DYNAMIC || mBody2->getType() == DYNAMIC) { - mInverseMassMatrixRotation = mInverseMassMatrixRotation.getInverse(); - } + // Compute the inverse of the mass matrix K=JM^-1J^t for the 3 rotation + // contraints (3x3 matrix) + mInverseMassMatrixRotation = mI1 + mI2; + if (mBody1->getType() == DYNAMIC || mBody2->getType() == DYNAMIC) { + mInverseMassMatrixRotation = mInverseMassMatrixRotation.getInverse(); + } - // Compute the bias "b" for the 3 rotation constraints - mBiasRotation.setToZero(); - if (mPositionCorrectionTechnique == BAUMGARTE_JOINTS) { - Quaternion currentOrientationDifference = orientationBody2 * orientationBody1.getInverse(); - currentOrientationDifference.normalize(); - const Quaternion qError = currentOrientationDifference * mInitOrientationDifferenceInv; - mBiasRotation = biasFactor * decimal(2.0) * qError.getVectorV(); - } + // Compute the bias "b" for the 3 rotation constraints + mBiasRotation.setToZero(); + if (mPositionCorrectionTechnique == BAUMGARTE_JOINTS) { + Quaternion currentOrientationDifference = orientationBody2 * orientationBody1.getInverse(); + currentOrientationDifference.normalize(); + const Quaternion qError = currentOrientationDifference * mInitOrientationDifferenceInv; + mBiasRotation = biasFactor * float(2.0) * qError.getVectorV(); + } - // If warm-starting is not enabled - if (!constraintSolverData.isWarmStartingActive) { + // If warm-starting is not enabled + if (!constraintSolverData.isWarmStartingActive) { - // Reset the accumulated impulses - mImpulseTranslation.setToZero(); - mImpulseRotation.setToZero(); - } + // Reset the accumulated impulses + mImpulseTranslation.setToZero(); + mImpulseRotation.setToZero(); + } } // Warm start the constraint (apply the previous impulse at the beginning of the step) void FixedJoint::warmstart(const ConstraintSolverData& constraintSolverData) { - // Get the velocities - Vector3& v1 = constraintSolverData.linearVelocities[mIndexBody1]; - Vector3& v2 = constraintSolverData.linearVelocities[mIndexBody2]; - Vector3& w1 = constraintSolverData.angularVelocities[mIndexBody1]; - Vector3& w2 = constraintSolverData.angularVelocities[mIndexBody2]; + // Get the velocities + Vector3& v1 = constraintSolverData.linearVelocities[mIndexBody1]; + Vector3& v2 = constraintSolverData.linearVelocities[mIndexBody2]; + Vector3& w1 = constraintSolverData.angularVelocities[mIndexBody1]; + Vector3& w2 = constraintSolverData.angularVelocities[mIndexBody2]; - // Get the inverse mass of the bodies - const decimal inverseMassBody1 = mBody1->mMassInverse; - const decimal inverseMassBody2 = mBody2->mMassInverse; + // Get the inverse mass of the bodies + const float inverseMassBody1 = mBody1->mMassInverse; + const float inverseMassBody2 = mBody2->mMassInverse; - // Compute the impulse P=J^T * lambda for the 3 translation constraints for body 1 - Vector3 linearImpulseBody1 = -mImpulseTranslation; - Vector3 angularImpulseBody1 = mImpulseTranslation.cross(mR1World); + // Compute the impulse P=J^T * lambda for the 3 translation constraints for body 1 + Vector3 linearImpulseBody1 = -mImpulseTranslation; + Vector3 angularImpulseBody1 = mImpulseTranslation.cross(mR1World); - // Compute the impulse P=J^T * lambda for the 3 rotation constraints for body 1 - angularImpulseBody1 += -mImpulseRotation; + // Compute the impulse P=J^T * lambda for the 3 rotation constraints for body 1 + angularImpulseBody1 += -mImpulseRotation; - // Apply the impulse to the body 1 - v1 += inverseMassBody1 * linearImpulseBody1; - w1 += mI1 * angularImpulseBody1; + // Apply the impulse to the body 1 + v1 += inverseMassBody1 * linearImpulseBody1; + w1 += mI1 * angularImpulseBody1; - // Compute the impulse P=J^T * lambda for the 3 translation constraints for body 2 - Vector3 angularImpulseBody2 = -mImpulseTranslation.cross(mR2World); + // Compute the impulse P=J^T * lambda for the 3 translation constraints for body 2 + Vector3 angularImpulseBody2 = -mImpulseTranslation.cross(mR2World); - // Compute the impulse P=J^T * lambda for the 3 rotation constraints for body 2 - angularImpulseBody2 += mImpulseRotation; + // Compute the impulse P=J^T * lambda for the 3 rotation constraints for body 2 + angularImpulseBody2 += mImpulseRotation; - // Apply the impulse to the body 2 - v2 += inverseMassBody2 * mImpulseTranslation; - w2 += mI2 * angularImpulseBody2; + // Apply the impulse to the body 2 + v2 += inverseMassBody2 * mImpulseTranslation; + w2 += mI2 * angularImpulseBody2; } // Solve the velocity constraint void FixedJoint::solveVelocityConstraint(const ConstraintSolverData& constraintSolverData) { - // Get the velocities - Vector3& v1 = constraintSolverData.linearVelocities[mIndexBody1]; - Vector3& v2 = constraintSolverData.linearVelocities[mIndexBody2]; - Vector3& w1 = constraintSolverData.angularVelocities[mIndexBody1]; - Vector3& w2 = constraintSolverData.angularVelocities[mIndexBody2]; + // Get the velocities + Vector3& v1 = constraintSolverData.linearVelocities[mIndexBody1]; + Vector3& v2 = constraintSolverData.linearVelocities[mIndexBody2]; + Vector3& w1 = constraintSolverData.angularVelocities[mIndexBody1]; + Vector3& w2 = constraintSolverData.angularVelocities[mIndexBody2]; - // Get the inverse mass of the bodies - decimal inverseMassBody1 = mBody1->mMassInverse; - decimal inverseMassBody2 = mBody2->mMassInverse; + // Get the inverse mass of the bodies + float inverseMassBody1 = mBody1->mMassInverse; + float inverseMassBody2 = mBody2->mMassInverse; - // --------------- Translation Constraints --------------- // + // --------------- Translation Constraints --------------- // - // Compute J*v for the 3 translation constraints - const Vector3 JvTranslation = v2 + w2.cross(mR2World) - v1 - w1.cross(mR1World); + // Compute J*v for the 3 translation constraints + const Vector3 JvTranslation = v2 + w2.cross(mR2World) - v1 - w1.cross(mR1World); - // Compute the Lagrange multiplier lambda - const Vector3 deltaLambda = mInverseMassMatrixTranslation * - (-JvTranslation - mBiasTranslation); - mImpulseTranslation += deltaLambda; + // Compute the Lagrange multiplier lambda + const Vector3 deltaLambda = mInverseMassMatrixTranslation * + (-JvTranslation - mBiasTranslation); + mImpulseTranslation += deltaLambda; - // Compute the impulse P=J^T * lambda for body 1 - const Vector3 linearImpulseBody1 = -deltaLambda; - Vector3 angularImpulseBody1 = deltaLambda.cross(mR1World); + // Compute the impulse P=J^T * lambda for body 1 + const Vector3 linearImpulseBody1 = -deltaLambda; + Vector3 angularImpulseBody1 = deltaLambda.cross(mR1World); - // Apply the impulse to the body 1 - v1 += inverseMassBody1 * linearImpulseBody1; - w1 += mI1 * angularImpulseBody1; + // Apply the impulse to the body 1 + v1 += inverseMassBody1 * linearImpulseBody1; + w1 += mI1 * angularImpulseBody1; - // Compute the impulse P=J^T * lambda for body 2 - const Vector3 angularImpulseBody2 = -deltaLambda.cross(mR2World); + // Compute the impulse P=J^T * lambda for body 2 + const Vector3 angularImpulseBody2 = -deltaLambda.cross(mR2World); - // Apply the impulse to the body 2 - v2 += inverseMassBody2 * deltaLambda; - w2 += mI2 * angularImpulseBody2; + // Apply the impulse to the body 2 + v2 += inverseMassBody2 * deltaLambda; + w2 += mI2 * angularImpulseBody2; - // --------------- Rotation Constraints --------------- // + // --------------- Rotation Constraints --------------- // - // Compute J*v for the 3 rotation constraints - const Vector3 JvRotation = w2 - w1; + // Compute J*v for the 3 rotation constraints + const Vector3 JvRotation = w2 - w1; - // Compute the Lagrange multiplier lambda for the 3 rotation constraints - Vector3 deltaLambda2 = mInverseMassMatrixRotation * (-JvRotation - mBiasRotation); - mImpulseRotation += deltaLambda2; + // Compute the Lagrange multiplier lambda for the 3 rotation constraints + Vector3 deltaLambda2 = mInverseMassMatrixRotation * (-JvRotation - mBiasRotation); + mImpulseRotation += deltaLambda2; - // Compute the impulse P=J^T * lambda for the 3 rotation constraints for body 1 - angularImpulseBody1 = -deltaLambda2; + // Compute the impulse P=J^T * lambda for the 3 rotation constraints for body 1 + angularImpulseBody1 = -deltaLambda2; - // Apply the impulse to the body 1 - w1 += mI1 * angularImpulseBody1; + // Apply the impulse to the body 1 + w1 += mI1 * angularImpulseBody1; - // Apply the impulse to the body 2 - w2 += mI2 * deltaLambda2; + // Apply the impulse to the body 2 + w2 += mI2 * deltaLambda2; } // Solve the position constraint (for position error correction) void FixedJoint::solvePositionConstraint(const ConstraintSolverData& constraintSolverData) { - // If the error position correction technique is not the non-linear-gauss-seidel, we do - // do not execute this method - if (mPositionCorrectionTechnique != NON_LINEAR_GAUSS_SEIDEL) return; + // If the error position correction technique is not the non-linear-gauss-seidel, we do + // do not execute this method + if (mPositionCorrectionTechnique != NON_LINEAR_GAUSS_SEIDEL) return; - // Get the bodies positions and orientations - Vector3& x1 = constraintSolverData.positions[mIndexBody1]; - Vector3& x2 = constraintSolverData.positions[mIndexBody2]; - Quaternion& q1 = constraintSolverData.orientations[mIndexBody1]; - Quaternion& q2 = constraintSolverData.orientations[mIndexBody2]; + // Get the bodies positions and orientations + Vector3& x1 = constraintSolverData.positions[mIndexBody1]; + Vector3& x2 = constraintSolverData.positions[mIndexBody2]; + Quaternion& q1 = constraintSolverData.orientations[mIndexBody1]; + Quaternion& q2 = constraintSolverData.orientations[mIndexBody2]; - // Get the inverse mass and inverse inertia tensors of the bodies - decimal inverseMassBody1 = mBody1->mMassInverse; - decimal inverseMassBody2 = mBody2->mMassInverse; + // Get the inverse mass and inverse inertia tensors of the bodies + float inverseMassBody1 = mBody1->mMassInverse; + float inverseMassBody2 = mBody2->mMassInverse; - // Recompute the inverse inertia tensors - mI1 = mBody1->getInertiaTensorInverseWorld(); - mI2 = mBody2->getInertiaTensorInverseWorld(); + // Recompute the inverse inertia tensors + mI1 = mBody1->getInertiaTensorInverseWorld(); + mI2 = mBody2->getInertiaTensorInverseWorld(); - // Compute the vector from body center to the anchor point in world-space - mR1World = q1 * mLocalAnchorPointBody1; - mR2World = q2 * mLocalAnchorPointBody2; + // Compute the vector from body center to the anchor point in world-space + mR1World = q1 * mLocalAnchorPointBody1; + mR2World = q2 * mLocalAnchorPointBody2; - // Compute the corresponding skew-symmetric matrices - Matrix3x3 skewSymmetricMatrixU1= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mR1World); - Matrix3x3 skewSymmetricMatrixU2= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mR2World); + // Compute the corresponding skew-symmetric matrices + Matrix3x3 skewSymmetricMatrixU1= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mR1World); + Matrix3x3 skewSymmetricMatrixU2= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mR2World); - // --------------- Translation Constraints --------------- // + // --------------- Translation Constraints --------------- // - // Compute the matrix K=JM^-1J^t (3x3 matrix) for the 3 translation constraints - decimal inverseMassBodies = mBody1->mMassInverse + mBody2->mMassInverse; - Matrix3x3 massMatrix = Matrix3x3(inverseMassBodies, 0, 0, - 0, inverseMassBodies, 0, - 0, 0, inverseMassBodies) + - skewSymmetricMatrixU1 * mI1 * skewSymmetricMatrixU1.getTranspose() + - skewSymmetricMatrixU2 * mI2 * skewSymmetricMatrixU2.getTranspose(); - mInverseMassMatrixTranslation.setToZero(); - if (mBody1->getType() == DYNAMIC || mBody2->getType() == DYNAMIC) { - mInverseMassMatrixTranslation = massMatrix.getInverse(); - } + // Compute the matrix K=JM^-1J^t (3x3 matrix) for the 3 translation constraints + float inverseMassBodies = mBody1->mMassInverse + mBody2->mMassInverse; + Matrix3x3 massMatrix = Matrix3x3(inverseMassBodies, 0, 0, + 0, inverseMassBodies, 0, + 0, 0, inverseMassBodies) + + skewSymmetricMatrixU1 * mI1 * skewSymmetricMatrixU1.getTranspose() + + skewSymmetricMatrixU2 * mI2 * skewSymmetricMatrixU2.getTranspose(); + mInverseMassMatrixTranslation.setToZero(); + if (mBody1->getType() == DYNAMIC || mBody2->getType() == DYNAMIC) { + mInverseMassMatrixTranslation = massMatrix.getInverse(); + } - // Compute position error for the 3 translation constraints - const Vector3 errorTranslation = x2 + mR2World - x1 - mR1World; + // Compute position error for the 3 translation constraints + const Vector3 errorTranslation = x2 + mR2World - x1 - mR1World; - // Compute the Lagrange multiplier lambda - const Vector3 lambdaTranslation = mInverseMassMatrixTranslation * (-errorTranslation); + // Compute the Lagrange multiplier lambda + const Vector3 lambdaTranslation = mInverseMassMatrixTranslation * (-errorTranslation); - // Compute the impulse of body 1 - Vector3 linearImpulseBody1 = -lambdaTranslation; - Vector3 angularImpulseBody1 = lambdaTranslation.cross(mR1World); + // Compute the impulse of body 1 + Vector3 linearImpulseBody1 = -lambdaTranslation; + Vector3 angularImpulseBody1 = lambdaTranslation.cross(mR1World); - // Compute the pseudo velocity of body 1 - const Vector3 v1 = inverseMassBody1 * linearImpulseBody1; - Vector3 w1 = mI1 * angularImpulseBody1; + // Compute the pseudo velocity of body 1 + const Vector3 v1 = inverseMassBody1 * linearImpulseBody1; + Vector3 w1 = mI1 * angularImpulseBody1; - // Update the body position/orientation of body 1 - x1 += v1; - q1 += Quaternion(0, w1) * q1 * decimal(0.5); - q1.normalize(); + // Update the body position/orientation of body 1 + x1 += v1; + q1 += Quaternion(0, w1) * q1 * float(0.5); + q1.normalize(); - // Compute the impulse of body 2 - Vector3 angularImpulseBody2 = -lambdaTranslation.cross(mR2World); + // Compute the impulse of body 2 + Vector3 angularImpulseBody2 = -lambdaTranslation.cross(mR2World); - // Compute the pseudo velocity of body 2 - const Vector3 v2 = inverseMassBody2 * lambdaTranslation; - Vector3 w2 = mI2 * angularImpulseBody2; + // Compute the pseudo velocity of body 2 + const Vector3 v2 = inverseMassBody2 * lambdaTranslation; + Vector3 w2 = mI2 * angularImpulseBody2; - // Update the body position/orientation of body 2 - x2 += v2; - q2 += Quaternion(0, w2) * q2 * decimal(0.5); - q2.normalize(); + // Update the body position/orientation of body 2 + x2 += v2; + q2 += Quaternion(0, w2) * q2 * float(0.5); + q2.normalize(); - // --------------- Rotation Constraints --------------- // + // --------------- Rotation Constraints --------------- // - // Compute the inverse of the mass matrix K=JM^-1J^t for the 3 rotation - // contraints (3x3 matrix) - mInverseMassMatrixRotation = mI1 + mI2; - if (mBody1->getType() == DYNAMIC || mBody2->getType() == DYNAMIC) { - mInverseMassMatrixRotation = mInverseMassMatrixRotation.getInverse(); - } + // Compute the inverse of the mass matrix K=JM^-1J^t for the 3 rotation + // contraints (3x3 matrix) + mInverseMassMatrixRotation = mI1 + mI2; + if (mBody1->getType() == DYNAMIC || mBody2->getType() == DYNAMIC) { + mInverseMassMatrixRotation = mInverseMassMatrixRotation.getInverse(); + } - // Compute the position error for the 3 rotation constraints - Quaternion currentOrientationDifference = q2 * q1.getInverse(); - currentOrientationDifference.normalize(); - const Quaternion qError = currentOrientationDifference * mInitOrientationDifferenceInv; - const Vector3 errorRotation = decimal(2.0) * qError.getVectorV(); + // Compute the position error for the 3 rotation constraints + Quaternion currentOrientationDifference = q2 * q1.getInverse(); + currentOrientationDifference.normalize(); + const Quaternion qError = currentOrientationDifference * mInitOrientationDifferenceInv; + const Vector3 errorRotation = float(2.0) * qError.getVectorV(); - // Compute the Lagrange multiplier lambda for the 3 rotation constraints - Vector3 lambdaRotation = mInverseMassMatrixRotation * (-errorRotation); + // Compute the Lagrange multiplier lambda for the 3 rotation constraints + Vector3 lambdaRotation = mInverseMassMatrixRotation * (-errorRotation); - // Compute the impulse P=J^T * lambda for the 3 rotation constraints of body 1 - angularImpulseBody1 = -lambdaRotation; + // Compute the impulse P=J^T * lambda for the 3 rotation constraints of body 1 + angularImpulseBody1 = -lambdaRotation; - // Compute the pseudo velocity of body 1 - w1 = mI1 * angularImpulseBody1; + // Compute the pseudo velocity of body 1 + w1 = mI1 * angularImpulseBody1; - // Update the body position/orientation of body 1 - q1 += Quaternion(0, w1) * q1 * decimal(0.5); - q1.normalize(); + // Update the body position/orientation of body 1 + q1 += Quaternion(0, w1) * q1 * float(0.5); + q1.normalize(); - // Compute the pseudo velocity of body 2 - w2 = mI2 * lambdaRotation; + // Compute the pseudo velocity of body 2 + w2 = mI2 * lambdaRotation; - // Update the body position/orientation of body 2 - q2 += Quaternion(0, w2) * q2 * decimal(0.5); - q2.normalize(); + // Update the body position/orientation of body 2 + q2 += Quaternion(0, w2) * q2 * float(0.5); + q2.normalize(); } diff --git a/ephysics/constraint/FixedJoint.h b/ephysics/constraint/FixedJoint.h index 4dd1e75..f6c3923 100644 --- a/ephysics/constraint/FixedJoint.h +++ b/ephysics/constraint/FixedJoint.h @@ -1,30 +1,9 @@ -/******************************************************************************** -* 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 REACTPHYSICS3D_FIXED_JOINT_H -#define REACTPHYSICS3D_FIXED_JOINT_H +/** @file + * @author Daniel Chappuis + * @copyright 2010-2016 Daniel Chappuis + * @license BSD 3 clauses (see license file) + */ +#pragma once // Libraries #include @@ -39,24 +18,24 @@ namespace reactphysics3d { */ struct FixedJointInfo : public JointInfo { - public : + public : - // -------------------- Attributes -------------------- // + // -------------------- Attributes -------------------- // - /// Anchor point (in world-space coordinates) - Vector3 anchorPointWorldSpace; + /// Anchor point (in world-space coordinates) + Vector3 anchorPointWorldSpace; - /// Constructor - /** - * @param rigidBody1 The first body of the joint - * @param rigidBody2 The second body of the joint - * @param initAnchorPointWorldSpace The initial anchor point of the joint in - * world-space coordinates - */ - FixedJointInfo(RigidBody* rigidBody1, RigidBody* rigidBody2, - const Vector3& initAnchorPointWorldSpace) - : JointInfo(rigidBody1, rigidBody2, FIXEDJOINT), - anchorPointWorldSpace(initAnchorPointWorldSpace){} + /// Constructor + /** + * @param rigidBody1 The first body of the joint + * @param rigidBody2 The second body of the joint + * @param initAnchorPointWorldSpace The initial anchor point of the joint in + * world-space coordinates + */ + FixedJointInfo(RigidBody* rigidBody1, RigidBody* rigidBody2, + const Vector3& initAnchorPointWorldSpace) + : JointInfo(rigidBody1, rigidBody2, FIXEDJOINT), + anchorPointWorldSpace(initAnchorPointWorldSpace){} }; // Class FixedJoint @@ -66,93 +45,91 @@ struct FixedJointInfo : public JointInfo { */ class FixedJoint : public Joint { - private : + private : - // -------------------- Constants -------------------- // + // -------------------- Constants -------------------- // - // Beta value for the bias factor of position correction - static const decimal BETA; + // Beta value for the bias factor of position correction + static const float BETA; - // -------------------- Attributes -------------------- // + // -------------------- Attributes -------------------- // - /// Anchor point of body 1 (in local-space coordinates of body 1) - Vector3 mLocalAnchorPointBody1; + /// Anchor point of body 1 (in local-space coordinates of body 1) + Vector3 mLocalAnchorPointBody1; - /// Anchor point of body 2 (in local-space coordinates of body 2) - Vector3 mLocalAnchorPointBody2; + /// Anchor point of body 2 (in local-space coordinates of body 2) + Vector3 mLocalAnchorPointBody2; - /// Vector from center of body 2 to anchor point in world-space - Vector3 mR1World; + /// Vector from center of body 2 to anchor point in world-space + Vector3 mR1World; - /// Vector from center of body 2 to anchor point in world-space - Vector3 mR2World; + /// Vector from center of body 2 to anchor point in world-space + Vector3 mR2World; - /// Inertia tensor of body 1 (in world-space coordinates) - Matrix3x3 mI1; + /// Inertia tensor of body 1 (in world-space coordinates) + Matrix3x3 mI1; - /// Inertia tensor of body 2 (in world-space coordinates) - Matrix3x3 mI2; + /// Inertia tensor of body 2 (in world-space coordinates) + Matrix3x3 mI2; - /// Accumulated impulse for the 3 translation constraints - Vector3 mImpulseTranslation; + /// Accumulated impulse for the 3 translation constraints + Vector3 mImpulseTranslation; - /// Accumulate impulse for the 3 rotation constraints - Vector3 mImpulseRotation; + /// Accumulate impulse for the 3 rotation constraints + Vector3 mImpulseRotation; - /// Inverse mass matrix K=JM^-1J^-t of the 3 translation constraints (3x3 matrix) - Matrix3x3 mInverseMassMatrixTranslation; + /// Inverse mass matrix K=JM^-1J^-t of the 3 translation constraints (3x3 matrix) + Matrix3x3 mInverseMassMatrixTranslation; - /// Inverse mass matrix K=JM^-1J^-t of the 3 rotation constraints (3x3 matrix) - Matrix3x3 mInverseMassMatrixRotation; + /// Inverse mass matrix K=JM^-1J^-t of the 3 rotation constraints (3x3 matrix) + Matrix3x3 mInverseMassMatrixRotation; - /// Bias vector for the 3 translation constraints - Vector3 mBiasTranslation; + /// Bias vector for the 3 translation constraints + Vector3 mBiasTranslation; - /// Bias vector for the 3 rotation constraints - Vector3 mBiasRotation; + /// Bias vector for the 3 rotation constraints + Vector3 mBiasRotation; - /// Inverse of the initial orientation difference between the two bodies - Quaternion mInitOrientationDifferenceInv; + /// Inverse of the initial orientation difference between the two bodies + Quaternion mInitOrientationDifferenceInv; - // -------------------- Methods -------------------- // + // -------------------- Methods -------------------- // - /// Private copy-constructor - FixedJoint(const FixedJoint& constraint); + /// Private copy-constructor + FixedJoint(const FixedJoint& constraint); - /// Private assignment operator - FixedJoint& operator=(const FixedJoint& constraint); + /// Private assignment operator + FixedJoint& operator=(const FixedJoint& constraint); - /// Return the number of bytes used by the joint - virtual size_t getSizeInBytes() const; + /// Return the number of bytes used by the joint + virtual size_t getSizeInBytes() const; - /// Initialize before solving the constraint - virtual void initBeforeSolve(const ConstraintSolverData& constraintSolverData); + /// Initialize before solving the constraint + virtual void initBeforeSolve(const ConstraintSolverData& constraintSolverData); - /// Warm start the constraint (apply the previous impulse at the beginning of the step) - virtual void warmstart(const ConstraintSolverData& constraintSolverData); + /// Warm start the constraint (apply the previous impulse at the beginning of the step) + virtual void warmstart(const ConstraintSolverData& constraintSolverData); - /// Solve the velocity constraint - virtual void solveVelocityConstraint(const ConstraintSolverData& constraintSolverData); + /// Solve the velocity constraint + virtual void solveVelocityConstraint(const ConstraintSolverData& constraintSolverData); - /// Solve the position constraint (for position error correction) - virtual void solvePositionConstraint(const ConstraintSolverData& constraintSolverData); + /// Solve the position constraint (for position error correction) + virtual void solvePositionConstraint(const ConstraintSolverData& constraintSolverData); - public : + public : - // -------------------- Methods -------------------- // + // -------------------- Methods -------------------- // - /// Constructor - FixedJoint(const FixedJointInfo& jointInfo); + /// Constructor + FixedJoint(const FixedJointInfo& jointInfo); - /// Destructor - virtual ~FixedJoint(); + /// Destructor + virtual ~FixedJoint(); }; // Return the number of bytes used by the joint inline size_t FixedJoint::getSizeInBytes() const { - return sizeof(FixedJoint); + return sizeof(FixedJoint); } } - -#endif diff --git a/ephysics/constraint/HingeJoint.cpp b/ephysics/constraint/HingeJoint.cpp index 51c7ff6..3b72586 100644 --- a/ephysics/constraint/HingeJoint.cpp +++ b/ephysics/constraint/HingeJoint.cpp @@ -1,27 +1,8 @@ -/******************************************************************************** -* 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. * -* * -********************************************************************************/ +/** @file + * @author Daniel Chappuis + * @copyright 2010-2016 Daniel Chappuis + * @license BSD 3 clauses (see license file) + */ // Libraries #include @@ -31,37 +12,37 @@ using namespace reactphysics3d; // Static variables definition -const decimal HingeJoint::BETA = decimal(0.2); +const float HingeJoint::BETA = float(0.2); // Constructor HingeJoint::HingeJoint(const HingeJointInfo& jointInfo) - : Joint(jointInfo), mImpulseTranslation(0, 0, 0), mImpulseRotation(0, 0), - mImpulseLowerLimit(0), mImpulseUpperLimit(0), mImpulseMotor(0), - mIsLimitEnabled(jointInfo.isLimitEnabled), mIsMotorEnabled(jointInfo.isMotorEnabled), - mLowerLimit(jointInfo.minAngleLimit), mUpperLimit(jointInfo.maxAngleLimit), - mIsLowerLimitViolated(false), mIsUpperLimitViolated(false), - mMotorSpeed(jointInfo.motorSpeed), mMaxMotorTorque(jointInfo.maxMotorTorque) { + : Joint(jointInfo), mImpulseTranslation(0, 0, 0), mImpulseRotation(0, 0), + mImpulseLowerLimit(0), mImpulseUpperLimit(0), mImpulseMotor(0), + mIsLimitEnabled(jointInfo.isLimitEnabled), mIsMotorEnabled(jointInfo.isMotorEnabled), + mLowerLimit(jointInfo.minAngleLimit), mUpperLimit(jointInfo.maxAngleLimit), + mIsLowerLimitViolated(false), mIsUpperLimitViolated(false), + mMotorSpeed(jointInfo.motorSpeed), mMaxMotorTorque(jointInfo.maxMotorTorque) { - assert(mLowerLimit <= 0 && mLowerLimit >= -2.0 * PI); - assert(mUpperLimit >= 0 && mUpperLimit <= 2.0 * PI); + assert(mLowerLimit <= 0 && mLowerLimit >= -2.0 * PI); + assert(mUpperLimit >= 0 && mUpperLimit <= 2.0 * PI); - // Compute the local-space anchor point for each body - Transform transform1 = mBody1->getTransform(); - Transform transform2 = mBody2->getTransform(); - mLocalAnchorPointBody1 = transform1.getInverse() * jointInfo.anchorPointWorldSpace; - mLocalAnchorPointBody2 = transform2.getInverse() * jointInfo.anchorPointWorldSpace; + // Compute the local-space anchor point for each body + Transform transform1 = mBody1->getTransform(); + Transform transform2 = mBody2->getTransform(); + mLocalAnchorPointBody1 = transform1.getInverse() * jointInfo.anchorPointWorldSpace; + mLocalAnchorPointBody2 = transform2.getInverse() * jointInfo.anchorPointWorldSpace; - // Compute the local-space hinge axis - mHingeLocalAxisBody1 = transform1.getOrientation().getInverse() * jointInfo.rotationAxisWorld; - mHingeLocalAxisBody2 = transform2.getOrientation().getInverse() * jointInfo.rotationAxisWorld; - mHingeLocalAxisBody1.normalize(); - mHingeLocalAxisBody2.normalize(); + // Compute the local-space hinge axis + mHingeLocalAxisBody1 = transform1.getOrientation().getInverse() * jointInfo.rotationAxisWorld; + mHingeLocalAxisBody2 = transform2.getOrientation().getInverse() * jointInfo.rotationAxisWorld; + mHingeLocalAxisBody1.normalize(); + mHingeLocalAxisBody2.normalize(); - // Compute the inverse of the initial orientation difference between the two bodies - mInitOrientationDifferenceInv = transform2.getOrientation() * - transform1.getOrientation().getInverse(); - mInitOrientationDifferenceInv.normalize(); - mInitOrientationDifferenceInv.inverse(); + // Compute the inverse of the initial orientation difference between the two bodies + mInitOrientationDifferenceInv = transform2.getOrientation() * + transform1.getOrientation().getInverse(); + mInitOrientationDifferenceInv.normalize(); + mInitOrientationDifferenceInv.inverse(); } // Destructor @@ -72,733 +53,733 @@ HingeJoint::~HingeJoint() { // Initialize before solving the constraint void HingeJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverData) { - // Initialize the bodies index in the velocity array - mIndexBody1 = constraintSolverData.mapBodyToConstrainedVelocityIndex.find(mBody1)->second; - mIndexBody2 = constraintSolverData.mapBodyToConstrainedVelocityIndex.find(mBody2)->second; + // Initialize the bodies index in the velocity array + mIndexBody1 = constraintSolverData.mapBodyToConstrainedVelocityIndex.find(mBody1)->second; + mIndexBody2 = constraintSolverData.mapBodyToConstrainedVelocityIndex.find(mBody2)->second; - // Get the bodies positions and orientations - const Vector3& x1 = mBody1->mCenterOfMassWorld; - const Vector3& x2 = mBody2->mCenterOfMassWorld; - const Quaternion& orientationBody1 = mBody1->getTransform().getOrientation(); - const Quaternion& orientationBody2 = mBody2->getTransform().getOrientation(); + // Get the bodies positions and orientations + const Vector3& x1 = mBody1->mCenterOfMassWorld; + const Vector3& x2 = mBody2->mCenterOfMassWorld; + const Quaternion& orientationBody1 = mBody1->getTransform().getOrientation(); + const Quaternion& orientationBody2 = mBody2->getTransform().getOrientation(); - // Get the inertia tensor of bodies - mI1 = mBody1->getInertiaTensorInverseWorld(); - mI2 = mBody2->getInertiaTensorInverseWorld(); + // Get the inertia tensor of bodies + mI1 = mBody1->getInertiaTensorInverseWorld(); + mI2 = mBody2->getInertiaTensorInverseWorld(); - // Compute the vector from body center to the anchor point in world-space - mR1World = orientationBody1 * mLocalAnchorPointBody1; - mR2World = orientationBody2 * mLocalAnchorPointBody2; + // Compute the vector from body center to the anchor point in world-space + mR1World = orientationBody1 * mLocalAnchorPointBody1; + mR2World = orientationBody2 * mLocalAnchorPointBody2; - // Compute the current angle around the hinge axis - decimal hingeAngle = computeCurrentHingeAngle(orientationBody1, orientationBody2); + // Compute the current angle around the hinge axis + float hingeAngle = computeCurrentHingeAngle(orientationBody1, orientationBody2); - // Check if the limit constraints are violated or not - decimal lowerLimitError = hingeAngle - mLowerLimit; - decimal upperLimitError = mUpperLimit - hingeAngle; - bool oldIsLowerLimitViolated = mIsLowerLimitViolated; - mIsLowerLimitViolated = lowerLimitError <= 0; - if (mIsLowerLimitViolated != oldIsLowerLimitViolated) { - mImpulseLowerLimit = 0.0; - } - bool oldIsUpperLimitViolated = mIsUpperLimitViolated; - mIsUpperLimitViolated = upperLimitError <= 0; - if (mIsUpperLimitViolated != oldIsUpperLimitViolated) { - mImpulseUpperLimit = 0.0; - } + // Check if the limit constraints are violated or not + float lowerLimitError = hingeAngle - mLowerLimit; + float upperLimitError = mUpperLimit - hingeAngle; + bool oldIsLowerLimitViolated = mIsLowerLimitViolated; + mIsLowerLimitViolated = lowerLimitError <= 0; + if (mIsLowerLimitViolated != oldIsLowerLimitViolated) { + mImpulseLowerLimit = 0.0; + } + bool oldIsUpperLimitViolated = mIsUpperLimitViolated; + mIsUpperLimitViolated = upperLimitError <= 0; + if (mIsUpperLimitViolated != oldIsUpperLimitViolated) { + mImpulseUpperLimit = 0.0; + } - // Compute vectors needed in the Jacobian - mA1 = orientationBody1 * mHingeLocalAxisBody1; - Vector3 a2 = orientationBody2 * mHingeLocalAxisBody2; - mA1.normalize(); - a2.normalize(); - const Vector3 b2 = a2.getOneUnitOrthogonalVector(); - const Vector3 c2 = a2.cross(b2); - mB2CrossA1 = b2.cross(mA1); - mC2CrossA1 = c2.cross(mA1); + // Compute vectors needed in the Jacobian + mA1 = orientationBody1 * mHingeLocalAxisBody1; + Vector3 a2 = orientationBody2 * mHingeLocalAxisBody2; + mA1.normalize(); + a2.normalize(); + const Vector3 b2 = a2.getOneUnitOrthogonalVector(); + const Vector3 c2 = a2.cross(b2); + mB2CrossA1 = b2.cross(mA1); + mC2CrossA1 = c2.cross(mA1); - // Compute the corresponding skew-symmetric matrices - Matrix3x3 skewSymmetricMatrixU1= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mR1World); - Matrix3x3 skewSymmetricMatrixU2= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mR2World); + // Compute the corresponding skew-symmetric matrices + Matrix3x3 skewSymmetricMatrixU1= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mR1World); + Matrix3x3 skewSymmetricMatrixU2= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mR2World); - // Compute the inverse mass matrix K=JM^-1J^t for the 3 translation constraints (3x3 matrix) - decimal inverseMassBodies = mBody1->mMassInverse + mBody2->mMassInverse; - Matrix3x3 massMatrix = Matrix3x3(inverseMassBodies, 0, 0, - 0, inverseMassBodies, 0, - 0, 0, inverseMassBodies) + - skewSymmetricMatrixU1 * mI1 * skewSymmetricMatrixU1.getTranspose() + - skewSymmetricMatrixU2 * mI2 * skewSymmetricMatrixU2.getTranspose(); - mInverseMassMatrixTranslation.setToZero(); - if (mBody1->getType() == DYNAMIC || mBody2->getType() == DYNAMIC) { - mInverseMassMatrixTranslation = massMatrix.getInverse(); - } + // Compute the inverse mass matrix K=JM^-1J^t for the 3 translation constraints (3x3 matrix) + float inverseMassBodies = mBody1->mMassInverse + mBody2->mMassInverse; + Matrix3x3 massMatrix = Matrix3x3(inverseMassBodies, 0, 0, + 0, inverseMassBodies, 0, + 0, 0, inverseMassBodies) + + skewSymmetricMatrixU1 * mI1 * skewSymmetricMatrixU1.getTranspose() + + skewSymmetricMatrixU2 * mI2 * skewSymmetricMatrixU2.getTranspose(); + mInverseMassMatrixTranslation.setToZero(); + if (mBody1->getType() == DYNAMIC || mBody2->getType() == DYNAMIC) { + mInverseMassMatrixTranslation = massMatrix.getInverse(); + } - // Compute the bias "b" of the translation constraints - mBTranslation.setToZero(); - decimal biasFactor = (BETA / constraintSolverData.timeStep); - if (mPositionCorrectionTechnique == BAUMGARTE_JOINTS) { - mBTranslation = biasFactor * (x2 + mR2World - x1 - mR1World); - } + // Compute the bias "b" of the translation constraints + mBTranslation.setToZero(); + float biasFactor = (BETA / constraintSolverData.timeStep); + if (mPositionCorrectionTechnique == BAUMGARTE_JOINTS) { + mBTranslation = biasFactor * (x2 + mR2World - x1 - mR1World); + } - // Compute the inverse mass matrix K=JM^-1J^t for the 2 rotation constraints (2x2 matrix) - Vector3 I1B2CrossA1 = mI1 * mB2CrossA1; - Vector3 I1C2CrossA1 = mI1 * mC2CrossA1; - Vector3 I2B2CrossA1 = mI2 * mB2CrossA1; - Vector3 I2C2CrossA1 = mI2 * mC2CrossA1; - const decimal el11 = mB2CrossA1.dot(I1B2CrossA1) + - mB2CrossA1.dot(I2B2CrossA1); - const decimal el12 = mB2CrossA1.dot(I1C2CrossA1) + - mB2CrossA1.dot(I2C2CrossA1); - const decimal el21 = mC2CrossA1.dot(I1B2CrossA1) + - mC2CrossA1.dot(I2B2CrossA1); - const decimal el22 = mC2CrossA1.dot(I1C2CrossA1) + - mC2CrossA1.dot(I2C2CrossA1); - const Matrix2x2 matrixKRotation(el11, el12, el21, el22); - mInverseMassMatrixRotation.setToZero(); - if (mBody1->getType() == DYNAMIC || mBody2->getType() == DYNAMIC) { - mInverseMassMatrixRotation = matrixKRotation.getInverse(); - } + // Compute the inverse mass matrix K=JM^-1J^t for the 2 rotation constraints (2x2 matrix) + Vector3 I1B2CrossA1 = mI1 * mB2CrossA1; + Vector3 I1C2CrossA1 = mI1 * mC2CrossA1; + Vector3 I2B2CrossA1 = mI2 * mB2CrossA1; + Vector3 I2C2CrossA1 = mI2 * mC2CrossA1; + const float el11 = mB2CrossA1.dot(I1B2CrossA1) + + mB2CrossA1.dot(I2B2CrossA1); + const float el12 = mB2CrossA1.dot(I1C2CrossA1) + + mB2CrossA1.dot(I2C2CrossA1); + const float el21 = mC2CrossA1.dot(I1B2CrossA1) + + mC2CrossA1.dot(I2B2CrossA1); + const float el22 = mC2CrossA1.dot(I1C2CrossA1) + + mC2CrossA1.dot(I2C2CrossA1); + const Matrix2x2 matrixKRotation(el11, el12, el21, el22); + mInverseMassMatrixRotation.setToZero(); + if (mBody1->getType() == DYNAMIC || mBody2->getType() == DYNAMIC) { + mInverseMassMatrixRotation = matrixKRotation.getInverse(); + } - // Compute the bias "b" of the rotation constraints - mBRotation.setToZero(); - if (mPositionCorrectionTechnique == BAUMGARTE_JOINTS) { - mBRotation = biasFactor * Vector2(mA1.dot(b2), mA1.dot(c2)); - } + // Compute the bias "b" of the rotation constraints + mBRotation.setToZero(); + if (mPositionCorrectionTechnique == BAUMGARTE_JOINTS) { + mBRotation = biasFactor * Vector2(mA1.dot(b2), mA1.dot(c2)); + } - // If warm-starting is not enabled - if (!constraintSolverData.isWarmStartingActive) { + // If warm-starting is not enabled + if (!constraintSolverData.isWarmStartingActive) { - // Reset all the accumulated impulses - mImpulseTranslation.setToZero(); - mImpulseRotation.setToZero(); - mImpulseLowerLimit = 0.0; - mImpulseUpperLimit = 0.0; - mImpulseMotor = 0.0; - } + // Reset all the accumulated impulses + mImpulseTranslation.setToZero(); + mImpulseRotation.setToZero(); + mImpulseLowerLimit = 0.0; + mImpulseUpperLimit = 0.0; + mImpulseMotor = 0.0; + } - // If the motor or limits are enabled - if (mIsMotorEnabled || (mIsLimitEnabled && (mIsLowerLimitViolated || mIsUpperLimitViolated))) { + // If the motor or limits are enabled + if (mIsMotorEnabled || (mIsLimitEnabled && (mIsLowerLimitViolated || mIsUpperLimitViolated))) { - // Compute the inverse of the mass matrix K=JM^-1J^t for the limits and motor (1x1 matrix) - mInverseMassMatrixLimitMotor = mA1.dot(mI1 * mA1) + mA1.dot(mI2 * mA1); - mInverseMassMatrixLimitMotor = (mInverseMassMatrixLimitMotor > 0.0) ? - decimal(1.0) / mInverseMassMatrixLimitMotor : decimal(0.0); + // Compute the inverse of the mass matrix K=JM^-1J^t for the limits and motor (1x1 matrix) + mInverseMassMatrixLimitMotor = mA1.dot(mI1 * mA1) + mA1.dot(mI2 * mA1); + mInverseMassMatrixLimitMotor = (mInverseMassMatrixLimitMotor > 0.0) ? + float(1.0) / mInverseMassMatrixLimitMotor : float(0.0); - if (mIsLimitEnabled) { + if (mIsLimitEnabled) { - // Compute the bias "b" of the lower limit constraint - mBLowerLimit = 0.0; - if (mPositionCorrectionTechnique == BAUMGARTE_JOINTS) { - mBLowerLimit = biasFactor * lowerLimitError; - } + // Compute the bias "b" of the lower limit constraint + mBLowerLimit = 0.0; + if (mPositionCorrectionTechnique == BAUMGARTE_JOINTS) { + mBLowerLimit = biasFactor * lowerLimitError; + } - // Compute the bias "b" of the upper limit constraint - mBUpperLimit = 0.0; - if (mPositionCorrectionTechnique == BAUMGARTE_JOINTS) { - mBUpperLimit = biasFactor * upperLimitError; - } - } - } + // Compute the bias "b" of the upper limit constraint + mBUpperLimit = 0.0; + if (mPositionCorrectionTechnique == BAUMGARTE_JOINTS) { + mBUpperLimit = biasFactor * upperLimitError; + } + } + } } // Warm start the constraint (apply the previous impulse at the beginning of the step) void HingeJoint::warmstart(const ConstraintSolverData& constraintSolverData) { - // Get the velocities - Vector3& v1 = constraintSolverData.linearVelocities[mIndexBody1]; - Vector3& v2 = constraintSolverData.linearVelocities[mIndexBody2]; - Vector3& w1 = constraintSolverData.angularVelocities[mIndexBody1]; - Vector3& w2 = constraintSolverData.angularVelocities[mIndexBody2]; + // Get the velocities + Vector3& v1 = constraintSolverData.linearVelocities[mIndexBody1]; + Vector3& v2 = constraintSolverData.linearVelocities[mIndexBody2]; + Vector3& w1 = constraintSolverData.angularVelocities[mIndexBody1]; + Vector3& w2 = constraintSolverData.angularVelocities[mIndexBody2]; - // Get the inverse mass and inverse inertia tensors of the bodies - const decimal inverseMassBody1 = mBody1->mMassInverse; - const decimal inverseMassBody2 = mBody2->mMassInverse; + // Get the inverse mass and inverse inertia tensors of the bodies + const float inverseMassBody1 = mBody1->mMassInverse; + const float inverseMassBody2 = mBody2->mMassInverse; - // Compute the impulse P=J^T * lambda for the 2 rotation constraints - Vector3 rotationImpulse = -mB2CrossA1 * mImpulseRotation.x - mC2CrossA1 * mImpulseRotation.y; + // Compute the impulse P=J^T * lambda for the 2 rotation constraints + Vector3 rotationImpulse = -mB2CrossA1 * mImpulseRotation.x - mC2CrossA1 * mImpulseRotation.y; - // Compute the impulse P=J^T * lambda for the lower and upper limits constraints - const Vector3 limitsImpulse = (mImpulseUpperLimit - mImpulseLowerLimit) * mA1; + // Compute the impulse P=J^T * lambda for the lower and upper limits constraints + const Vector3 limitsImpulse = (mImpulseUpperLimit - mImpulseLowerLimit) * mA1; - // Compute the impulse P=J^T * lambda for the motor constraint - const Vector3 motorImpulse = -mImpulseMotor * mA1; + // Compute the impulse P=J^T * lambda for the motor constraint + const Vector3 motorImpulse = -mImpulseMotor * mA1; - // Compute the impulse P=J^T * lambda for the 3 translation constraints of body 1 - Vector3 linearImpulseBody1 = -mImpulseTranslation; - Vector3 angularImpulseBody1 = mImpulseTranslation.cross(mR1World); + // Compute the impulse P=J^T * lambda for the 3 translation constraints of body 1 + Vector3 linearImpulseBody1 = -mImpulseTranslation; + Vector3 angularImpulseBody1 = mImpulseTranslation.cross(mR1World); - // Compute the impulse P=J^T * lambda for the 2 rotation constraints of body 1 - angularImpulseBody1 += rotationImpulse; + // Compute the impulse P=J^T * lambda for the 2 rotation constraints of body 1 + angularImpulseBody1 += rotationImpulse; - // Compute the impulse P=J^T * lambda for the lower and upper limits constraints of body 1 - angularImpulseBody1 += limitsImpulse; + // Compute the impulse P=J^T * lambda for the lower and upper limits constraints of body 1 + angularImpulseBody1 += limitsImpulse; - // Compute the impulse P=J^T * lambda for the motor constraint of body 1 - angularImpulseBody1 += motorImpulse; + // Compute the impulse P=J^T * lambda for the motor constraint of body 1 + angularImpulseBody1 += motorImpulse; - // Apply the impulse to the body 1 - v1 += inverseMassBody1 * linearImpulseBody1; - w1 += mI1 * angularImpulseBody1; + // Apply the impulse to the body 1 + v1 += inverseMassBody1 * linearImpulseBody1; + w1 += mI1 * angularImpulseBody1; - // Compute the impulse P=J^T * lambda for the 3 translation constraints of body 2 - Vector3 angularImpulseBody2 = -mImpulseTranslation.cross(mR2World); + // Compute the impulse P=J^T * lambda for the 3 translation constraints of body 2 + Vector3 angularImpulseBody2 = -mImpulseTranslation.cross(mR2World); - // Compute the impulse P=J^T * lambda for the 2 rotation constraints of body 2 - angularImpulseBody2 += -rotationImpulse; + // Compute the impulse P=J^T * lambda for the 2 rotation constraints of body 2 + angularImpulseBody2 += -rotationImpulse; - // Compute the impulse P=J^T * lambda for the lower and upper limits constraints of body 2 - angularImpulseBody2 += -limitsImpulse; + // Compute the impulse P=J^T * lambda for the lower and upper limits constraints of body 2 + angularImpulseBody2 += -limitsImpulse; - // Compute the impulse P=J^T * lambda for the motor constraint of body 2 - angularImpulseBody2 += -motorImpulse; + // Compute the impulse P=J^T * lambda for the motor constraint of body 2 + angularImpulseBody2 += -motorImpulse; - // Apply the impulse to the body 2 - v2 += inverseMassBody2 * mImpulseTranslation; - w2 += mI2 * angularImpulseBody2; + // Apply the impulse to the body 2 + v2 += inverseMassBody2 * mImpulseTranslation; + w2 += mI2 * angularImpulseBody2; } // Solve the velocity constraint void HingeJoint::solveVelocityConstraint(const ConstraintSolverData& constraintSolverData) { - // Get the velocities - Vector3& v1 = constraintSolverData.linearVelocities[mIndexBody1]; - Vector3& v2 = constraintSolverData.linearVelocities[mIndexBody2]; - Vector3& w1 = constraintSolverData.angularVelocities[mIndexBody1]; - Vector3& w2 = constraintSolverData.angularVelocities[mIndexBody2]; + // Get the velocities + Vector3& v1 = constraintSolverData.linearVelocities[mIndexBody1]; + Vector3& v2 = constraintSolverData.linearVelocities[mIndexBody2]; + Vector3& w1 = constraintSolverData.angularVelocities[mIndexBody1]; + Vector3& w2 = constraintSolverData.angularVelocities[mIndexBody2]; - // Get the inverse mass and inverse inertia tensors of the bodies - decimal inverseMassBody1 = mBody1->mMassInverse; - decimal inverseMassBody2 = mBody2->mMassInverse; + // Get the inverse mass and inverse inertia tensors of the bodies + float inverseMassBody1 = mBody1->mMassInverse; + float inverseMassBody2 = mBody2->mMassInverse; - // --------------- Translation Constraints --------------- // + // --------------- Translation Constraints --------------- // - // Compute J*v - const Vector3 JvTranslation = v2 + w2.cross(mR2World) - v1 - w1.cross(mR1World); + // Compute J*v + const Vector3 JvTranslation = v2 + w2.cross(mR2World) - v1 - w1.cross(mR1World); - // Compute the Lagrange multiplier lambda - const Vector3 deltaLambdaTranslation = mInverseMassMatrixTranslation * - (-JvTranslation - mBTranslation); - mImpulseTranslation += deltaLambdaTranslation; + // Compute the Lagrange multiplier lambda + const Vector3 deltaLambdaTranslation = mInverseMassMatrixTranslation * + (-JvTranslation - mBTranslation); + mImpulseTranslation += deltaLambdaTranslation; - // Compute the impulse P=J^T * lambda of body 1 - const Vector3 linearImpulseBody1 = -deltaLambdaTranslation; - Vector3 angularImpulseBody1 = deltaLambdaTranslation.cross(mR1World); + // Compute the impulse P=J^T * lambda of body 1 + const Vector3 linearImpulseBody1 = -deltaLambdaTranslation; + Vector3 angularImpulseBody1 = deltaLambdaTranslation.cross(mR1World); - // Apply the impulse to the body 1 - v1 += inverseMassBody1 * linearImpulseBody1; - w1 += mI1 * angularImpulseBody1; + // Apply the impulse to the body 1 + v1 += inverseMassBody1 * linearImpulseBody1; + w1 += mI1 * angularImpulseBody1; - // Compute the impulse P=J^T * lambda of body 2 - Vector3 angularImpulseBody2 = -deltaLambdaTranslation.cross(mR2World); + // Compute the impulse P=J^T * lambda of body 2 + Vector3 angularImpulseBody2 = -deltaLambdaTranslation.cross(mR2World); - // Apply the impulse to the body 2 - v2 += inverseMassBody2 * deltaLambdaTranslation; - w2 += mI2 * angularImpulseBody2; + // Apply the impulse to the body 2 + v2 += inverseMassBody2 * deltaLambdaTranslation; + w2 += mI2 * angularImpulseBody2; - // --------------- Rotation Constraints --------------- // + // --------------- Rotation Constraints --------------- // - // Compute J*v for the 2 rotation constraints - const Vector2 JvRotation(-mB2CrossA1.dot(w1) + mB2CrossA1.dot(w2), - -mC2CrossA1.dot(w1) + mC2CrossA1.dot(w2)); + // Compute J*v for the 2 rotation constraints + const Vector2 JvRotation(-mB2CrossA1.dot(w1) + mB2CrossA1.dot(w2), + -mC2CrossA1.dot(w1) + mC2CrossA1.dot(w2)); - // Compute the Lagrange multiplier lambda for the 2 rotation constraints - Vector2 deltaLambdaRotation = mInverseMassMatrixRotation * (-JvRotation - mBRotation); - mImpulseRotation += deltaLambdaRotation; + // Compute the Lagrange multiplier lambda for the 2 rotation constraints + Vector2 deltaLambdaRotation = mInverseMassMatrixRotation * (-JvRotation - mBRotation); + mImpulseRotation += deltaLambdaRotation; - // Compute the impulse P=J^T * lambda for the 2 rotation constraints of body 1 - angularImpulseBody1 = -mB2CrossA1 * deltaLambdaRotation.x - - mC2CrossA1 * deltaLambdaRotation.y; + // Compute the impulse P=J^T * lambda for the 2 rotation constraints of body 1 + angularImpulseBody1 = -mB2CrossA1 * deltaLambdaRotation.x - + mC2CrossA1 * deltaLambdaRotation.y; - // Apply the impulse to the body 1 - w1 += mI1 * angularImpulseBody1; + // Apply the impulse to the body 1 + w1 += mI1 * angularImpulseBody1; - // Compute the impulse P=J^T * lambda for the 2 rotation constraints of body 2 - angularImpulseBody2 = mB2CrossA1 * deltaLambdaRotation.x + - mC2CrossA1 * deltaLambdaRotation.y; + // Compute the impulse P=J^T * lambda for the 2 rotation constraints of body 2 + angularImpulseBody2 = mB2CrossA1 * deltaLambdaRotation.x + + mC2CrossA1 * deltaLambdaRotation.y; - // Apply the impulse to the body 2 - w2 += mI2 * angularImpulseBody2; + // Apply the impulse to the body 2 + w2 += mI2 * angularImpulseBody2; - // --------------- Limits Constraints --------------- // + // --------------- Limits Constraints --------------- // - if (mIsLimitEnabled) { + if (mIsLimitEnabled) { - // If the lower limit is violated - if (mIsLowerLimitViolated) { + // If the lower limit is violated + if (mIsLowerLimitViolated) { - // Compute J*v for the lower limit constraint - const decimal JvLowerLimit = (w2 - w1).dot(mA1); + // Compute J*v for the lower limit constraint + const float JvLowerLimit = (w2 - w1).dot(mA1); - // Compute the Lagrange multiplier lambda for the lower limit constraint - decimal deltaLambdaLower = mInverseMassMatrixLimitMotor * (-JvLowerLimit -mBLowerLimit); - decimal lambdaTemp = mImpulseLowerLimit; - mImpulseLowerLimit = std::max(mImpulseLowerLimit + deltaLambdaLower, decimal(0.0)); - deltaLambdaLower = mImpulseLowerLimit - lambdaTemp; + // Compute the Lagrange multiplier lambda for the lower limit constraint + float deltaLambdaLower = mInverseMassMatrixLimitMotor * (-JvLowerLimit -mBLowerLimit); + float lambdaTemp = mImpulseLowerLimit; + mImpulseLowerLimit = std::max(mImpulseLowerLimit + deltaLambdaLower, float(0.0)); + deltaLambdaLower = mImpulseLowerLimit - lambdaTemp; - // Compute the impulse P=J^T * lambda for the lower limit constraint of body 1 - const Vector3 angularImpulseBody1 = -deltaLambdaLower * mA1; + // Compute the impulse P=J^T * lambda for the lower limit constraint of body 1 + const Vector3 angularImpulseBody1 = -deltaLambdaLower * mA1; - // Apply the impulse to the body 1 - w1 += mI1 * angularImpulseBody1; + // Apply the impulse to the body 1 + w1 += mI1 * angularImpulseBody1; - // Compute the impulse P=J^T * lambda for the lower limit constraint of body 2 - const Vector3 angularImpulseBody2 = deltaLambdaLower * mA1; + // Compute the impulse P=J^T * lambda for the lower limit constraint of body 2 + const Vector3 angularImpulseBody2 = deltaLambdaLower * mA1; - // Apply the impulse to the body 2 - w2 += mI2 * angularImpulseBody2; - } + // Apply the impulse to the body 2 + w2 += mI2 * angularImpulseBody2; + } - // If the upper limit is violated - if (mIsUpperLimitViolated) { + // If the upper limit is violated + if (mIsUpperLimitViolated) { - // Compute J*v for the upper limit constraint - const decimal JvUpperLimit = -(w2 - w1).dot(mA1); + // Compute J*v for the upper limit constraint + const float JvUpperLimit = -(w2 - w1).dot(mA1); - // Compute the Lagrange multiplier lambda for the upper limit constraint - decimal deltaLambdaUpper = mInverseMassMatrixLimitMotor * (-JvUpperLimit -mBUpperLimit); - decimal lambdaTemp = mImpulseUpperLimit; - mImpulseUpperLimit = std::max(mImpulseUpperLimit + deltaLambdaUpper, decimal(0.0)); - deltaLambdaUpper = mImpulseUpperLimit - lambdaTemp; + // Compute the Lagrange multiplier lambda for the upper limit constraint + float deltaLambdaUpper = mInverseMassMatrixLimitMotor * (-JvUpperLimit -mBUpperLimit); + float lambdaTemp = mImpulseUpperLimit; + mImpulseUpperLimit = std::max(mImpulseUpperLimit + deltaLambdaUpper, float(0.0)); + deltaLambdaUpper = mImpulseUpperLimit - lambdaTemp; - // Compute the impulse P=J^T * lambda for the upper limit constraint of body 1 - const Vector3 angularImpulseBody1 = deltaLambdaUpper * mA1; + // Compute the impulse P=J^T * lambda for the upper limit constraint of body 1 + const Vector3 angularImpulseBody1 = deltaLambdaUpper * mA1; - // Apply the impulse to the body 1 - w1 += mI1 * angularImpulseBody1; + // Apply the impulse to the body 1 + w1 += mI1 * angularImpulseBody1; - // Compute the impulse P=J^T * lambda for the upper limit constraint of body 2 - const Vector3 angularImpulseBody2 = -deltaLambdaUpper * mA1; + // Compute the impulse P=J^T * lambda for the upper limit constraint of body 2 + const Vector3 angularImpulseBody2 = -deltaLambdaUpper * mA1; - // Apply the impulse to the body 2 - w2 += mI2 * angularImpulseBody2; - } - } + // Apply the impulse to the body 2 + w2 += mI2 * angularImpulseBody2; + } + } - // --------------- Motor --------------- // + // --------------- Motor --------------- // - // If the motor is enabled - if (mIsMotorEnabled) { + // If the motor is enabled + if (mIsMotorEnabled) { - // Compute J*v for the motor - const decimal JvMotor = mA1.dot(w1 - w2); + // Compute J*v for the motor + const float JvMotor = mA1.dot(w1 - w2); - // Compute the Lagrange multiplier lambda for the motor - const decimal maxMotorImpulse = mMaxMotorTorque * constraintSolverData.timeStep; - decimal deltaLambdaMotor = mInverseMassMatrixLimitMotor * (-JvMotor - mMotorSpeed); - decimal lambdaTemp = mImpulseMotor; - mImpulseMotor = clamp(mImpulseMotor + deltaLambdaMotor, -maxMotorImpulse, maxMotorImpulse); - deltaLambdaMotor = mImpulseMotor - lambdaTemp; + // Compute the Lagrange multiplier lambda for the motor + const float maxMotorImpulse = mMaxMotorTorque * constraintSolverData.timeStep; + float deltaLambdaMotor = mInverseMassMatrixLimitMotor * (-JvMotor - mMotorSpeed); + float lambdaTemp = mImpulseMotor; + mImpulseMotor = clamp(mImpulseMotor + deltaLambdaMotor, -maxMotorImpulse, maxMotorImpulse); + deltaLambdaMotor = mImpulseMotor - lambdaTemp; - // Compute the impulse P=J^T * lambda for the motor of body 1 - const Vector3 angularImpulseBody1 = -deltaLambdaMotor * mA1; + // Compute the impulse P=J^T * lambda for the motor of body 1 + const Vector3 angularImpulseBody1 = -deltaLambdaMotor * mA1; - // Apply the impulse to the body 1 - w1 += mI1 * angularImpulseBody1; + // Apply the impulse to the body 1 + w1 += mI1 * angularImpulseBody1; - // Compute the impulse P=J^T * lambda for the motor of body 2 - const Vector3 angularImpulseBody2 = deltaLambdaMotor * mA1; + // Compute the impulse P=J^T * lambda for the motor of body 2 + const Vector3 angularImpulseBody2 = deltaLambdaMotor * mA1; - // Apply the impulse to the body 2 - w2 += mI2 * angularImpulseBody2; - } + // Apply the impulse to the body 2 + w2 += mI2 * angularImpulseBody2; + } } // Solve the position constraint (for position error correction) void HingeJoint::solvePositionConstraint(const ConstraintSolverData& constraintSolverData) { - // If the error position correction technique is not the non-linear-gauss-seidel, we do - // do not execute this method - if (mPositionCorrectionTechnique != NON_LINEAR_GAUSS_SEIDEL) return; + // If the error position correction technique is not the non-linear-gauss-seidel, we do + // do not execute this method + if (mPositionCorrectionTechnique != NON_LINEAR_GAUSS_SEIDEL) return; - // Get the bodies positions and orientations - Vector3& x1 = constraintSolverData.positions[mIndexBody1]; - Vector3& x2 = constraintSolverData.positions[mIndexBody2]; - Quaternion& q1 = constraintSolverData.orientations[mIndexBody1]; - Quaternion& q2 = constraintSolverData.orientations[mIndexBody2]; + // Get the bodies positions and orientations + Vector3& x1 = constraintSolverData.positions[mIndexBody1]; + Vector3& x2 = constraintSolverData.positions[mIndexBody2]; + Quaternion& q1 = constraintSolverData.orientations[mIndexBody1]; + Quaternion& q2 = constraintSolverData.orientations[mIndexBody2]; - // Get the inverse mass and inverse inertia tensors of the bodies - decimal inverseMassBody1 = mBody1->mMassInverse; - decimal inverseMassBody2 = mBody2->mMassInverse; + // Get the inverse mass and inverse inertia tensors of the bodies + float inverseMassBody1 = mBody1->mMassInverse; + float inverseMassBody2 = mBody2->mMassInverse; - // Recompute the inverse inertia tensors - mI1 = mBody1->getInertiaTensorInverseWorld(); - mI2 = mBody2->getInertiaTensorInverseWorld(); + // Recompute the inverse inertia tensors + mI1 = mBody1->getInertiaTensorInverseWorld(); + mI2 = mBody2->getInertiaTensorInverseWorld(); - // Compute the vector from body center to the anchor point in world-space - mR1World = q1 * mLocalAnchorPointBody1; - mR2World = q2 * mLocalAnchorPointBody2; + // Compute the vector from body center to the anchor point in world-space + mR1World = q1 * mLocalAnchorPointBody1; + mR2World = q2 * mLocalAnchorPointBody2; - // Compute the current angle around the hinge axis - decimal hingeAngle = computeCurrentHingeAngle(q1, q2); + // Compute the current angle around the hinge axis + float hingeAngle = computeCurrentHingeAngle(q1, q2); - // Check if the limit constraints are violated or not - decimal lowerLimitError = hingeAngle - mLowerLimit; - decimal upperLimitError = mUpperLimit - hingeAngle; - mIsLowerLimitViolated = lowerLimitError <= 0; - mIsUpperLimitViolated = upperLimitError <= 0; + // Check if the limit constraints are violated or not + float lowerLimitError = hingeAngle - mLowerLimit; + float upperLimitError = mUpperLimit - hingeAngle; + mIsLowerLimitViolated = lowerLimitError <= 0; + mIsUpperLimitViolated = upperLimitError <= 0; - // Compute vectors needed in the Jacobian - mA1 = q1 * mHingeLocalAxisBody1; - Vector3 a2 = q2 * mHingeLocalAxisBody2; - mA1.normalize(); - a2.normalize(); - const Vector3 b2 = a2.getOneUnitOrthogonalVector(); - const Vector3 c2 = a2.cross(b2); - mB2CrossA1 = b2.cross(mA1); - mC2CrossA1 = c2.cross(mA1); + // Compute vectors needed in the Jacobian + mA1 = q1 * mHingeLocalAxisBody1; + Vector3 a2 = q2 * mHingeLocalAxisBody2; + mA1.normalize(); + a2.normalize(); + const Vector3 b2 = a2.getOneUnitOrthogonalVector(); + const Vector3 c2 = a2.cross(b2); + mB2CrossA1 = b2.cross(mA1); + mC2CrossA1 = c2.cross(mA1); - // Compute the corresponding skew-symmetric matrices - Matrix3x3 skewSymmetricMatrixU1= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mR1World); - Matrix3x3 skewSymmetricMatrixU2= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mR2World); + // Compute the corresponding skew-symmetric matrices + Matrix3x3 skewSymmetricMatrixU1= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mR1World); + Matrix3x3 skewSymmetricMatrixU2= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mR2World); - // --------------- Translation Constraints --------------- // + // --------------- Translation Constraints --------------- // - // Compute the matrix K=JM^-1J^t (3x3 matrix) for the 3 translation constraints - decimal inverseMassBodies = mBody1->mMassInverse + mBody2->mMassInverse; - Matrix3x3 massMatrix = Matrix3x3(inverseMassBodies, 0, 0, - 0, inverseMassBodies, 0, - 0, 0, inverseMassBodies) + - skewSymmetricMatrixU1 * mI1 * skewSymmetricMatrixU1.getTranspose() + - skewSymmetricMatrixU2 * mI2 * skewSymmetricMatrixU2.getTranspose(); - mInverseMassMatrixTranslation.setToZero(); - if (mBody1->getType() == DYNAMIC || mBody2->getType() == DYNAMIC) { - mInverseMassMatrixTranslation = massMatrix.getInverse(); - } + // Compute the matrix K=JM^-1J^t (3x3 matrix) for the 3 translation constraints + float inverseMassBodies = mBody1->mMassInverse + mBody2->mMassInverse; + Matrix3x3 massMatrix = Matrix3x3(inverseMassBodies, 0, 0, + 0, inverseMassBodies, 0, + 0, 0, inverseMassBodies) + + skewSymmetricMatrixU1 * mI1 * skewSymmetricMatrixU1.getTranspose() + + skewSymmetricMatrixU2 * mI2 * skewSymmetricMatrixU2.getTranspose(); + mInverseMassMatrixTranslation.setToZero(); + if (mBody1->getType() == DYNAMIC || mBody2->getType() == DYNAMIC) { + mInverseMassMatrixTranslation = massMatrix.getInverse(); + } - // Compute position error for the 3 translation constraints - const Vector3 errorTranslation = x2 + mR2World - x1 - mR1World; + // Compute position error for the 3 translation constraints + const Vector3 errorTranslation = x2 + mR2World - x1 - mR1World; - // Compute the Lagrange multiplier lambda - const Vector3 lambdaTranslation = mInverseMassMatrixTranslation * (-errorTranslation); + // Compute the Lagrange multiplier lambda + const Vector3 lambdaTranslation = mInverseMassMatrixTranslation * (-errorTranslation); - // Compute the impulse of body 1 - Vector3 linearImpulseBody1 = -lambdaTranslation; - Vector3 angularImpulseBody1 = lambdaTranslation.cross(mR1World); + // Compute the impulse of body 1 + Vector3 linearImpulseBody1 = -lambdaTranslation; + Vector3 angularImpulseBody1 = lambdaTranslation.cross(mR1World); - // Compute the pseudo velocity of body 1 - const Vector3 v1 = inverseMassBody1 * linearImpulseBody1; - Vector3 w1 = mI1 * angularImpulseBody1; + // Compute the pseudo velocity of body 1 + const Vector3 v1 = inverseMassBody1 * linearImpulseBody1; + Vector3 w1 = mI1 * angularImpulseBody1; - // Update the body position/orientation of body 1 - x1 += v1; - q1 += Quaternion(0, w1) * q1 * decimal(0.5); - q1.normalize(); + // Update the body position/orientation of body 1 + x1 += v1; + q1 += Quaternion(0, w1) * q1 * float(0.5); + q1.normalize(); - // Compute the impulse of body 2 - Vector3 angularImpulseBody2 = -lambdaTranslation.cross(mR2World); + // Compute the impulse of body 2 + Vector3 angularImpulseBody2 = -lambdaTranslation.cross(mR2World); - // Compute the pseudo velocity of body 2 - const Vector3 v2 = inverseMassBody2 * lambdaTranslation; - Vector3 w2 = mI2 * angularImpulseBody2; + // Compute the pseudo velocity of body 2 + const Vector3 v2 = inverseMassBody2 * lambdaTranslation; + Vector3 w2 = mI2 * angularImpulseBody2; - // Update the body position/orientation of body 2 - x2 += v2; - q2 += Quaternion(0, w2) * q2 * decimal(0.5); - q2.normalize(); + // Update the body position/orientation of body 2 + x2 += v2; + q2 += Quaternion(0, w2) * q2 * float(0.5); + q2.normalize(); - // --------------- Rotation Constraints --------------- // + // --------------- Rotation Constraints --------------- // - // Compute the inverse mass matrix K=JM^-1J^t for the 2 rotation constraints (2x2 matrix) - Vector3 I1B2CrossA1 = mI1 * mB2CrossA1; - Vector3 I1C2CrossA1 = mI1 * mC2CrossA1; - Vector3 I2B2CrossA1 = mI2 * mB2CrossA1; - Vector3 I2C2CrossA1 = mI2 * mC2CrossA1; - const decimal el11 = mB2CrossA1.dot(I1B2CrossA1) + - mB2CrossA1.dot(I2B2CrossA1); - const decimal el12 = mB2CrossA1.dot(I1C2CrossA1) + - mB2CrossA1.dot(I2C2CrossA1); - const decimal el21 = mC2CrossA1.dot(I1B2CrossA1) + - mC2CrossA1.dot(I2B2CrossA1); - const decimal el22 = mC2CrossA1.dot(I1C2CrossA1) + - mC2CrossA1.dot(I2C2CrossA1); - const Matrix2x2 matrixKRotation(el11, el12, el21, el22); - mInverseMassMatrixRotation.setToZero(); - if (mBody1->getType() == DYNAMIC || mBody2->getType() == DYNAMIC) { - mInverseMassMatrixRotation = matrixKRotation.getInverse(); - } + // Compute the inverse mass matrix K=JM^-1J^t for the 2 rotation constraints (2x2 matrix) + Vector3 I1B2CrossA1 = mI1 * mB2CrossA1; + Vector3 I1C2CrossA1 = mI1 * mC2CrossA1; + Vector3 I2B2CrossA1 = mI2 * mB2CrossA1; + Vector3 I2C2CrossA1 = mI2 * mC2CrossA1; + const float el11 = mB2CrossA1.dot(I1B2CrossA1) + + mB2CrossA1.dot(I2B2CrossA1); + const float el12 = mB2CrossA1.dot(I1C2CrossA1) + + mB2CrossA1.dot(I2C2CrossA1); + const float el21 = mC2CrossA1.dot(I1B2CrossA1) + + mC2CrossA1.dot(I2B2CrossA1); + const float el22 = mC2CrossA1.dot(I1C2CrossA1) + + mC2CrossA1.dot(I2C2CrossA1); + const Matrix2x2 matrixKRotation(el11, el12, el21, el22); + mInverseMassMatrixRotation.setToZero(); + if (mBody1->getType() == DYNAMIC || mBody2->getType() == DYNAMIC) { + mInverseMassMatrixRotation = matrixKRotation.getInverse(); + } - // Compute the position error for the 3 rotation constraints - const Vector2 errorRotation = Vector2(mA1.dot(b2), mA1.dot(c2)); + // Compute the position error for the 3 rotation constraints + const Vector2 errorRotation = Vector2(mA1.dot(b2), mA1.dot(c2)); - // Compute the Lagrange multiplier lambda for the 3 rotation constraints - Vector2 lambdaRotation = mInverseMassMatrixRotation * (-errorRotation); + // Compute the Lagrange multiplier lambda for the 3 rotation constraints + Vector2 lambdaRotation = mInverseMassMatrixRotation * (-errorRotation); - // Compute the impulse P=J^T * lambda for the 3 rotation constraints of body 1 - angularImpulseBody1 = -mB2CrossA1 * lambdaRotation.x - mC2CrossA1 * lambdaRotation.y; + // Compute the impulse P=J^T * lambda for the 3 rotation constraints of body 1 + angularImpulseBody1 = -mB2CrossA1 * lambdaRotation.x - mC2CrossA1 * lambdaRotation.y; - // Compute the pseudo velocity of body 1 - w1 = mI1 * angularImpulseBody1; + // Compute the pseudo velocity of body 1 + w1 = mI1 * angularImpulseBody1; - // Update the body position/orientation of body 1 - q1 += Quaternion(0, w1) * q1 * decimal(0.5); - q1.normalize(); + // Update the body position/orientation of body 1 + q1 += Quaternion(0, w1) * q1 * float(0.5); + q1.normalize(); - // Compute the impulse of body 2 - angularImpulseBody2 = mB2CrossA1 * lambdaRotation.x + mC2CrossA1 * lambdaRotation.y; + // Compute the impulse of body 2 + angularImpulseBody2 = mB2CrossA1 * lambdaRotation.x + mC2CrossA1 * lambdaRotation.y; - // Compute the pseudo velocity of body 2 - w2 = mI2 * angularImpulseBody2; + // Compute the pseudo velocity of body 2 + w2 = mI2 * angularImpulseBody2; - // Update the body position/orientation of body 2 - q2 += Quaternion(0, w2) * q2 * decimal(0.5); - q2.normalize(); + // Update the body position/orientation of body 2 + q2 += Quaternion(0, w2) * q2 * float(0.5); + q2.normalize(); - // --------------- Limits Constraints --------------- // + // --------------- Limits Constraints --------------- // - if (mIsLimitEnabled) { + if (mIsLimitEnabled) { - if (mIsLowerLimitViolated || mIsUpperLimitViolated) { + if (mIsLowerLimitViolated || mIsUpperLimitViolated) { - // Compute the inverse of the mass matrix K=JM^-1J^t for the limits (1x1 matrix) - mInverseMassMatrixLimitMotor = mA1.dot(mI1 * mA1) + mA1.dot(mI2 * mA1); - mInverseMassMatrixLimitMotor = (mInverseMassMatrixLimitMotor > 0.0) ? - decimal(1.0) / mInverseMassMatrixLimitMotor : decimal(0.0); - } + // Compute the inverse of the mass matrix K=JM^-1J^t for the limits (1x1 matrix) + mInverseMassMatrixLimitMotor = mA1.dot(mI1 * mA1) + mA1.dot(mI2 * mA1); + mInverseMassMatrixLimitMotor = (mInverseMassMatrixLimitMotor > 0.0) ? + float(1.0) / mInverseMassMatrixLimitMotor : float(0.0); + } - // If the lower limit is violated - if (mIsLowerLimitViolated) { + // If the lower limit is violated + if (mIsLowerLimitViolated) { - // Compute the Lagrange multiplier lambda for the lower limit constraint - decimal lambdaLowerLimit = mInverseMassMatrixLimitMotor * (-lowerLimitError ); + // Compute the Lagrange multiplier lambda for the lower limit constraint + float lambdaLowerLimit = mInverseMassMatrixLimitMotor * (-lowerLimitError ); - // Compute the impulse P=J^T * lambda of body 1 - const Vector3 angularImpulseBody1 = -lambdaLowerLimit * mA1; + // Compute the impulse P=J^T * lambda of body 1 + const Vector3 angularImpulseBody1 = -lambdaLowerLimit * mA1; - // Compute the pseudo velocity of body 1 - const Vector3 w1 = mI1 * angularImpulseBody1; + // Compute the pseudo velocity of body 1 + const Vector3 w1 = mI1 * angularImpulseBody1; - // Update the body position/orientation of body 1 - q1 += Quaternion(0, w1) * q1 * decimal(0.5); - q1.normalize(); + // Update the body position/orientation of body 1 + q1 += Quaternion(0, w1) * q1 * float(0.5); + q1.normalize(); - // Compute the impulse P=J^T * lambda of body 2 - const Vector3 angularImpulseBody2 = lambdaLowerLimit * mA1; + // Compute the impulse P=J^T * lambda of body 2 + const Vector3 angularImpulseBody2 = lambdaLowerLimit * mA1; - // Compute the pseudo velocity of body 2 - const Vector3 w2 = mI2 * angularImpulseBody2; + // Compute the pseudo velocity of body 2 + const Vector3 w2 = mI2 * angularImpulseBody2; - // Update the body position/orientation of body 2 - q2 += Quaternion(0, w2) * q2 * decimal(0.5); - q2.normalize(); - } + // Update the body position/orientation of body 2 + q2 += Quaternion(0, w2) * q2 * float(0.5); + q2.normalize(); + } - // If the upper limit is violated - if (mIsUpperLimitViolated) { + // If the upper limit is violated + if (mIsUpperLimitViolated) { - // Compute the Lagrange multiplier lambda for the upper limit constraint - decimal lambdaUpperLimit = mInverseMassMatrixLimitMotor * (-upperLimitError); + // Compute the Lagrange multiplier lambda for the upper limit constraint + float lambdaUpperLimit = mInverseMassMatrixLimitMotor * (-upperLimitError); - // Compute the impulse P=J^T * lambda of body 1 - const Vector3 angularImpulseBody1 = lambdaUpperLimit * mA1; + // Compute the impulse P=J^T * lambda of body 1 + const Vector3 angularImpulseBody1 = lambdaUpperLimit * mA1; - // Compute the pseudo velocity of body 1 - const Vector3 w1 = mI1 * angularImpulseBody1; + // Compute the pseudo velocity of body 1 + const Vector3 w1 = mI1 * angularImpulseBody1; - // Update the body position/orientation of body 1 - q1 += Quaternion(0, w1) * q1 * decimal(0.5); - q1.normalize(); + // Update the body position/orientation of body 1 + q1 += Quaternion(0, w1) * q1 * float(0.5); + q1.normalize(); - // Compute the impulse P=J^T * lambda of body 2 - const Vector3 angularImpulseBody2 = -lambdaUpperLimit * mA1; + // Compute the impulse P=J^T * lambda of body 2 + const Vector3 angularImpulseBody2 = -lambdaUpperLimit * mA1; - // Compute the pseudo velocity of body 2 - const Vector3 w2 = mI2 * angularImpulseBody2; + // Compute the pseudo velocity of body 2 + const Vector3 w2 = mI2 * angularImpulseBody2; - // Update the body position/orientation of body 2 - q2 += Quaternion(0, w2) * q2 * decimal(0.5); - q2.normalize(); - } - } + // Update the body position/orientation of body 2 + q2 += Quaternion(0, w2) * q2 * float(0.5); + q2.normalize(); + } + } } // Enable/Disable the limits of the joint /** * @param isLimitEnabled True if you want to enable the limits of the joint and - * false otherwise + * false otherwise */ void HingeJoint::enableLimit(bool isLimitEnabled) { - if (isLimitEnabled != mIsLimitEnabled) { + if (isLimitEnabled != mIsLimitEnabled) { - mIsLimitEnabled = isLimitEnabled; + mIsLimitEnabled = isLimitEnabled; - // Reset the limits - resetLimits(); - } + // Reset the limits + resetLimits(); + } } // Enable/Disable the motor of the joint /** * @param isMotorEnabled True if you want to enable the motor of the joint and - * false otherwise + * false otherwise */ void HingeJoint::enableMotor(bool isMotorEnabled) { - mIsMotorEnabled = isMotorEnabled; - mImpulseMotor = 0.0; + mIsMotorEnabled = isMotorEnabled; + mImpulseMotor = 0.0; - // Wake up the two bodies of the joint - mBody1->setIsSleeping(false); - mBody2->setIsSleeping(false); + // Wake up the two bodies of the joint + mBody1->setIsSleeping(false); + mBody2->setIsSleeping(false); } // Set the minimum angle limit /** * @param lowerLimit The minimum limit angle of the joint (in radian) */ -void HingeJoint::setMinAngleLimit(decimal lowerLimit) { +void HingeJoint::setMinAngleLimit(float lowerLimit) { - assert(mLowerLimit <= 0 && mLowerLimit >= -2.0 * PI); + assert(mLowerLimit <= 0 && mLowerLimit >= -2.0 * PI); - if (lowerLimit != mLowerLimit) { + if (lowerLimit != mLowerLimit) { - mLowerLimit = lowerLimit; + mLowerLimit = lowerLimit; - // Reset the limits - resetLimits(); - } + // Reset the limits + resetLimits(); + } } // Set the maximum angle limit /** * @param upperLimit The maximum limit angle of the joint (in radian) */ -void HingeJoint::setMaxAngleLimit(decimal upperLimit) { +void HingeJoint::setMaxAngleLimit(float upperLimit) { - assert(upperLimit >= 0 && upperLimit <= 2.0 * PI); + assert(upperLimit >= 0 && upperLimit <= 2.0 * PI); - if (upperLimit != mUpperLimit) { + if (upperLimit != mUpperLimit) { - mUpperLimit = upperLimit; + mUpperLimit = upperLimit; - // Reset the limits - resetLimits(); - } + // Reset the limits + resetLimits(); + } } // Reset the limits void HingeJoint::resetLimits() { - // Reset the accumulated impulses for the limits - mImpulseLowerLimit = 0.0; - mImpulseUpperLimit = 0.0; + // Reset the accumulated impulses for the limits + mImpulseLowerLimit = 0.0; + mImpulseUpperLimit = 0.0; - // Wake up the two bodies of the joint - mBody1->setIsSleeping(false); - mBody2->setIsSleeping(false); + // Wake up the two bodies of the joint + mBody1->setIsSleeping(false); + mBody2->setIsSleeping(false); } // Set the motor speed -void HingeJoint::setMotorSpeed(decimal motorSpeed) { +void HingeJoint::setMotorSpeed(float motorSpeed) { - if (motorSpeed != mMotorSpeed) { + if (motorSpeed != mMotorSpeed) { - mMotorSpeed = motorSpeed; + mMotorSpeed = motorSpeed; - // Wake up the two bodies of the joint - mBody1->setIsSleeping(false); - mBody2->setIsSleeping(false); - } + // Wake up the two bodies of the joint + mBody1->setIsSleeping(false); + mBody2->setIsSleeping(false); + } } // Set the maximum motor torque /** * @param maxMotorTorque The maximum torque (in Newtons) of the joint motor */ -void HingeJoint::setMaxMotorTorque(decimal maxMotorTorque) { +void HingeJoint::setMaxMotorTorque(float maxMotorTorque) { - if (maxMotorTorque != mMaxMotorTorque) { + if (maxMotorTorque != mMaxMotorTorque) { - assert(mMaxMotorTorque >= 0.0); - mMaxMotorTorque = maxMotorTorque; + assert(mMaxMotorTorque >= 0.0); + mMaxMotorTorque = maxMotorTorque; - // Wake up the two bodies of the joint - mBody1->setIsSleeping(false); - mBody2->setIsSleeping(false); - } + // Wake up the two bodies of the joint + mBody1->setIsSleeping(false); + mBody2->setIsSleeping(false); + } } // Given an angle in radian, this method returns the corresponding angle in the range [-pi; pi] -decimal HingeJoint::computeNormalizedAngle(decimal angle) const { +float HingeJoint::computeNormalizedAngle(float angle) const { - // Convert it into the range [-2*pi; 2*pi] - angle = fmod(angle, PI_TIMES_2); + // Convert it int32_to the range [-2*pi; 2*pi] + angle = fmod(angle, PI_TIMES_2); - // Convert it into the range [-pi; pi] - if (angle < -PI) { - return angle + PI_TIMES_2; - } - else if (angle > PI) { - return angle - PI_TIMES_2; - } - else { - return angle; - } + // Convert it int32_to the range [-pi; pi] + if (angle < -PI) { + return angle + PI_TIMES_2; + } + else if (angle > PI) { + return angle - PI_TIMES_2; + } + else { + return angle; + } } // Given an "inputAngle" in the range [-pi, pi], this method returns an // angle (modulo 2*pi) in the range [-2*pi; 2*pi] that is closest to one of the // two angle limits in arguments. -decimal HingeJoint::computeCorrespondingAngleNearLimits(decimal inputAngle, decimal lowerLimitAngle, - decimal upperLimitAngle) const { - if (upperLimitAngle <= lowerLimitAngle) { - return inputAngle; - } - else if (inputAngle > upperLimitAngle) { - decimal diffToUpperLimit = fabs(computeNormalizedAngle(inputAngle - upperLimitAngle)); - decimal diffToLowerLimit = fabs(computeNormalizedAngle(inputAngle - lowerLimitAngle)); - return (diffToUpperLimit > diffToLowerLimit) ? (inputAngle - PI_TIMES_2) : inputAngle; - } - else if (inputAngle < lowerLimitAngle) { - decimal diffToUpperLimit = fabs(computeNormalizedAngle(upperLimitAngle - inputAngle)); - decimal diffToLowerLimit = fabs(computeNormalizedAngle(lowerLimitAngle - inputAngle)); - return (diffToUpperLimit > diffToLowerLimit) ? inputAngle : (inputAngle + PI_TIMES_2); - } - else { - return inputAngle; - } +float HingeJoint::computeCorrespondingAngleNearLimits(float inputAngle, float lowerLimitAngle, + float upperLimitAngle) const { + if (upperLimitAngle <= lowerLimitAngle) { + return inputAngle; + } + else if (inputAngle > upperLimitAngle) { + float diffToUpperLimit = fabs(computeNormalizedAngle(inputAngle - upperLimitAngle)); + float diffToLowerLimit = fabs(computeNormalizedAngle(inputAngle - lowerLimitAngle)); + return (diffToUpperLimit > diffToLowerLimit) ? (inputAngle - PI_TIMES_2) : inputAngle; + } + else if (inputAngle < lowerLimitAngle) { + float diffToUpperLimit = fabs(computeNormalizedAngle(upperLimitAngle - inputAngle)); + float diffToLowerLimit = fabs(computeNormalizedAngle(lowerLimitAngle - inputAngle)); + return (diffToUpperLimit > diffToLowerLimit) ? inputAngle : (inputAngle + PI_TIMES_2); + } + else { + return inputAngle; + } } // Compute the current angle around the hinge axis -decimal HingeJoint::computeCurrentHingeAngle(const Quaternion& orientationBody1, - const Quaternion& orientationBody2) { +float HingeJoint::computeCurrentHingeAngle(const Quaternion& orientationBody1, + const Quaternion& orientationBody2) { - decimal hingeAngle; + float hingeAngle; - // Compute the current orientation difference between the two bodies - Quaternion currentOrientationDiff = orientationBody2 * orientationBody1.getInverse(); - currentOrientationDiff.normalize(); + // Compute the current orientation difference between the two bodies + Quaternion currentOrientationDiff = orientationBody2 * orientationBody1.getInverse(); + currentOrientationDiff.normalize(); - // Compute the relative rotation considering the initial orientation difference - Quaternion relativeRotation = currentOrientationDiff * mInitOrientationDifferenceInv; - relativeRotation.normalize(); + // Compute the relative rotation considering the initial orientation difference + Quaternion relativeRotation = currentOrientationDiff * mInitOrientationDifferenceInv; + relativeRotation.normalize(); - // A quaternion q = [cos(theta/2); sin(theta/2) * rotAxis] where rotAxis is a unit - // length vector. We can extract cos(theta/2) with q.w and we can extract |sin(theta/2)| with : - // |sin(theta/2)| = q.getVectorV().length() since rotAxis is unit length. Note that any - // rotation can be represented by a quaternion q and -q. Therefore, if the relative rotation - // axis is not pointing in the same direction as the hinge axis, we use the rotation -q which - // has the same |sin(theta/2)| value but the value cos(theta/2) is sign inverted. Some details - // about this trick is explained in the source code of OpenTissue (http://www.opentissue.org). - decimal cosHalfAngle = relativeRotation.w; - decimal sinHalfAngleAbs = relativeRotation.getVectorV().length(); + // A quaternion q = [cos(theta/2); sin(theta/2) * rotAxis] where rotAxis is a unit + // length vector. We can extract cos(theta/2) with q.w and we can extract |sin(theta/2)| with : + // |sin(theta/2)| = q.getVectorV().length() since rotAxis is unit length. Note that any + // rotation can be represented by a quaternion q and -q. Therefore, if the relative rotation + // axis is not pointing in the same direction as the hinge axis, we use the rotation -q which + // has the same |sin(theta/2)| value but the value cos(theta/2) is sign inverted. Some details + // about this trick is explained in the source code of OpenTissue (http://www.opentissue.org). + float cosHalfAngle = relativeRotation.w; + float sinHalfAngleAbs = relativeRotation.getVectorV().length(); - // Compute the dot product of the relative rotation axis and the hinge axis - decimal dotProduct = relativeRotation.getVectorV().dot(mA1); + // Compute the dot product of the relative rotation axis and the hinge axis + float dotProduct = relativeRotation.getVectorV().dot(mA1); - // If the relative rotation axis and the hinge axis are pointing the same direction - if (dotProduct >= decimal(0.0)) { - hingeAngle = decimal(2.0) * std::atan2(sinHalfAngleAbs, cosHalfAngle); - } - else { - hingeAngle = decimal(2.0) * std::atan2(sinHalfAngleAbs, -cosHalfAngle); - } + // If the relative rotation axis and the hinge axis are pointing the same direction + if (dotProduct >= float(0.0)) { + hingeAngle = float(2.0) * std::atan2(sinHalfAngleAbs, cosHalfAngle); + } + else { + hingeAngle = float(2.0) * std::atan2(sinHalfAngleAbs, -cosHalfAngle); + } - // Convert the angle from range [-2*pi; 2*pi] into the range [-pi; pi] - hingeAngle = computeNormalizedAngle(hingeAngle); + // Convert the angle from range [-2*pi; 2*pi] int32_to the range [-pi; pi] + hingeAngle = computeNormalizedAngle(hingeAngle); - // Compute and return the corresponding angle near one the two limits - return computeCorrespondingAngleNearLimits(hingeAngle, mLowerLimit, mUpperLimit); + // Compute and return the corresponding angle near one the two limits + return computeCorrespondingAngleNearLimits(hingeAngle, mLowerLimit, mUpperLimit); } diff --git a/ephysics/constraint/HingeJoint.h b/ephysics/constraint/HingeJoint.h index 28a118a..5bd5f0f 100644 --- a/ephysics/constraint/HingeJoint.h +++ b/ephysics/constraint/HingeJoint.h @@ -1,30 +1,9 @@ -/******************************************************************************** -* 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 REACTPHYSICS3D_HINGE_JOINT_H -#define REACTPHYSICS3D_HINGE_JOINT_H +/** @file + * @author Daniel Chappuis + * @copyright 2010-2016 Daniel Chappuis + * @license BSD 3 clauses (see license file) + */ +#pragma once // Libraries #include @@ -39,97 +18,97 @@ namespace reactphysics3d { */ struct HingeJointInfo : public JointInfo { - public : + public : - // -------------------- Attributes -------------------- // + // -------------------- Attributes -------------------- // - /// Anchor point (in world-space coordinates) - Vector3 anchorPointWorldSpace; + /// Anchor point (in world-space coordinates) + Vector3 anchorPointWorldSpace; - /// Hinge rotation axis (in world-space coordinates) - Vector3 rotationAxisWorld; + /// Hinge rotation axis (in world-space coordinates) + Vector3 rotationAxisWorld; - /// True if the hinge joint limits are enabled - bool isLimitEnabled; + /// True if the hinge joint limits are enabled + bool isLimitEnabled; - /// True if the hinge joint motor is enabled - bool isMotorEnabled; + /// True if the hinge joint motor is enabled + bool isMotorEnabled; - /// Minimum allowed rotation angle (in radian) if limits are enabled. - /// The angle must be in the range [-2*pi, 0] - decimal minAngleLimit; + /// Minimum allowed rotation angle (in radian) if limits are enabled. + /// The angle must be in the range [-2*pi, 0] + float minAngleLimit; - /// Maximum allowed rotation angle (in radian) if limits are enabled. - /// The angle must be in the range [0, 2*pi] - decimal maxAngleLimit; + /// Maximum allowed rotation angle (in radian) if limits are enabled. + /// The angle must be in the range [0, 2*pi] + float maxAngleLimit; - /// Motor speed (in radian/second) - decimal motorSpeed; + /// Motor speed (in radian/second) + float motorSpeed; - /// Maximum motor torque (in Newtons * meters) that can be applied to reach - /// to desired motor speed - decimal maxMotorTorque; + /// Maximum motor torque (in Newtons * meters) that can be applied to reach + /// to desired motor speed + float maxMotorTorque; - /// Constructor without limits and without motor - /** - * @param rigidBody1 The first body of the joint - * @param rigidBody2 The second body of the joint - * @param initAnchorPointWorldSpace The initial anchor point in world-space - * coordinates - * @param initRotationAxisWorld The initial rotation axis in world-space - * coordinates - */ - HingeJointInfo(RigidBody* rigidBody1, RigidBody* rigidBody2, - const Vector3& initAnchorPointWorldSpace, - const Vector3& initRotationAxisWorld) - : JointInfo(rigidBody1, rigidBody2, HINGEJOINT), - anchorPointWorldSpace(initAnchorPointWorldSpace), - rotationAxisWorld(initRotationAxisWorld), isLimitEnabled(false), - isMotorEnabled(false), minAngleLimit(-1), maxAngleLimit(1), - motorSpeed(0), maxMotorTorque(0) {} + /// Constructor without limits and without motor + /** + * @param rigidBody1 The first body of the joint + * @param rigidBody2 The second body of the joint + * @param initAnchorPointWorldSpace The initial anchor point in world-space + * coordinates + * @param initRotationAxisWorld The initial rotation axis in world-space + * coordinates + */ + HingeJointInfo(RigidBody* rigidBody1, RigidBody* rigidBody2, + const Vector3& initAnchorPointWorldSpace, + const Vector3& initRotationAxisWorld) + : JointInfo(rigidBody1, rigidBody2, HINGEJOINT), + anchorPointWorldSpace(initAnchorPointWorldSpace), + rotationAxisWorld(initRotationAxisWorld), isLimitEnabled(false), + isMotorEnabled(false), minAngleLimit(-1), maxAngleLimit(1), + motorSpeed(0), maxMotorTorque(0) {} - /// Constructor with limits but without motor - /** - * @param rigidBody1 The first body of the joint - * @param rigidBody2 The second body of the joint - * @param initAnchorPointWorldSpace The initial anchor point in world-space coordinates - * @param initRotationAxisWorld The intial rotation axis in world-space coordinates - * @param initMinAngleLimit The initial minimum limit angle (in radian) - * @param initMaxAngleLimit The initial maximum limit angle (in radian) - */ - HingeJointInfo(RigidBody* rigidBody1, RigidBody* rigidBody2, - const Vector3& initAnchorPointWorldSpace, - const Vector3& initRotationAxisWorld, - decimal initMinAngleLimit, decimal initMaxAngleLimit) - : JointInfo(rigidBody1, rigidBody2, HINGEJOINT), - anchorPointWorldSpace(initAnchorPointWorldSpace), - rotationAxisWorld(initRotationAxisWorld), isLimitEnabled(true), - isMotorEnabled(false), minAngleLimit(initMinAngleLimit), - maxAngleLimit(initMaxAngleLimit), motorSpeed(0), - maxMotorTorque(0) {} + /// Constructor with limits but without motor + /** + * @param rigidBody1 The first body of the joint + * @param rigidBody2 The second body of the joint + * @param initAnchorPointWorldSpace The initial anchor point in world-space coordinates + * @param initRotationAxisWorld The int32_tial rotation axis in world-space coordinates + * @param initMinAngleLimit The initial minimum limit angle (in radian) + * @param initMaxAngleLimit The initial maximum limit angle (in radian) + */ + HingeJointInfo(RigidBody* rigidBody1, RigidBody* rigidBody2, + const Vector3& initAnchorPointWorldSpace, + const Vector3& initRotationAxisWorld, + float initMinAngleLimit, float initMaxAngleLimit) + : JointInfo(rigidBody1, rigidBody2, HINGEJOINT), + anchorPointWorldSpace(initAnchorPointWorldSpace), + rotationAxisWorld(initRotationAxisWorld), isLimitEnabled(true), + isMotorEnabled(false), minAngleLimit(initMinAngleLimit), + maxAngleLimit(initMaxAngleLimit), motorSpeed(0), + maxMotorTorque(0) {} - /// Constructor with limits and motor - /** - * @param rigidBody1 The first body of the joint - * @param rigidBody2 The second body of the joint - * @param initAnchorPointWorldSpace The initial anchor point in world-space - * @param initRotationAxisWorld The initial rotation axis in world-space - * @param initMinAngleLimit The initial minimum limit angle (in radian) - * @param initMaxAngleLimit The initial maximum limit angle (in radian) - * @param initMotorSpeed The initial motor speed of the joint (in radian per second) - * @param initMaxMotorTorque The initial maximum motor torque (in Newtons) - */ - HingeJointInfo(RigidBody* rigidBody1, RigidBody* rigidBody2, - const Vector3& initAnchorPointWorldSpace, - const Vector3& initRotationAxisWorld, - decimal initMinAngleLimit, decimal initMaxAngleLimit, - decimal initMotorSpeed, decimal initMaxMotorTorque) - : JointInfo(rigidBody1, rigidBody2, HINGEJOINT), - anchorPointWorldSpace(initAnchorPointWorldSpace), - rotationAxisWorld(initRotationAxisWorld), isLimitEnabled(true), - isMotorEnabled(false), minAngleLimit(initMinAngleLimit), - maxAngleLimit(initMaxAngleLimit), motorSpeed(initMotorSpeed), - maxMotorTorque(initMaxMotorTorque) {} + /// Constructor with limits and motor + /** + * @param rigidBody1 The first body of the joint + * @param rigidBody2 The second body of the joint + * @param initAnchorPointWorldSpace The initial anchor point in world-space + * @param initRotationAxisWorld The initial rotation axis in world-space + * @param initMinAngleLimit The initial minimum limit angle (in radian) + * @param initMaxAngleLimit The initial maximum limit angle (in radian) + * @param initMotorSpeed The initial motor speed of the joint (in radian per second) + * @param initMaxMotorTorque The initial maximum motor torque (in Newtons) + */ + HingeJointInfo(RigidBody* rigidBody1, RigidBody* rigidBody2, + const Vector3& initAnchorPointWorldSpace, + const Vector3& initRotationAxisWorld, + float initMinAngleLimit, float initMaxAngleLimit, + float initMotorSpeed, float initMaxMotorTorque) + : JointInfo(rigidBody1, rigidBody2, HINGEJOINT), + anchorPointWorldSpace(initAnchorPointWorldSpace), + rotationAxisWorld(initRotationAxisWorld), isLimitEnabled(true), + isMotorEnabled(false), minAngleLimit(initMinAngleLimit), + maxAngleLimit(initMaxAngleLimit), motorSpeed(initMotorSpeed), + maxMotorTorque(initMaxMotorTorque) {} }; // Class HingeJoint @@ -140,202 +119,202 @@ struct HingeJointInfo : public JointInfo { */ class HingeJoint : public Joint { - private : + private : - // -------------------- Constants -------------------- // + // -------------------- Constants -------------------- // - // Beta value for the bias factor of position correction - static const decimal BETA; + // Beta value for the bias factor of position correction + static const float BETA; - // -------------------- Attributes -------------------- // + // -------------------- Attributes -------------------- // - /// Anchor point of body 1 (in local-space coordinates of body 1) - Vector3 mLocalAnchorPointBody1; + /// Anchor point of body 1 (in local-space coordinates of body 1) + Vector3 mLocalAnchorPointBody1; - /// Anchor point of body 2 (in local-space coordinates of body 2) - Vector3 mLocalAnchorPointBody2; + /// Anchor point of body 2 (in local-space coordinates of body 2) + Vector3 mLocalAnchorPointBody2; - /// Hinge rotation axis (in local-space coordinates of body 1) - Vector3 mHingeLocalAxisBody1; + /// Hinge rotation axis (in local-space coordinates of body 1) + Vector3 mHingeLocalAxisBody1; - /// Hinge rotation axis (in local-space coordiantes of body 2) - Vector3 mHingeLocalAxisBody2; + /// Hinge rotation axis (in local-space coordiantes of body 2) + Vector3 mHingeLocalAxisBody2; - /// Inertia tensor of body 1 (in world-space coordinates) - Matrix3x3 mI1; + /// Inertia tensor of body 1 (in world-space coordinates) + Matrix3x3 mI1; - /// Inertia tensor of body 2 (in world-space coordinates) - Matrix3x3 mI2; + /// Inertia tensor of body 2 (in world-space coordinates) + Matrix3x3 mI2; - /// Hinge rotation axis (in world-space coordinates) computed from body 1 - Vector3 mA1; + /// Hinge rotation axis (in world-space coordinates) computed from body 1 + Vector3 mA1; - /// Vector from center of body 2 to anchor point in world-space - Vector3 mR1World; + /// Vector from center of body 2 to anchor point in world-space + Vector3 mR1World; - /// Vector from center of body 2 to anchor point in world-space - Vector3 mR2World; + /// Vector from center of body 2 to anchor point in world-space + Vector3 mR2World; - /// Cross product of vector b2 and a1 - Vector3 mB2CrossA1; + /// Cross product of vector b2 and a1 + Vector3 mB2CrossA1; - /// Cross product of vector c2 and a1; - Vector3 mC2CrossA1; + /// Cross product of vector c2 and a1; + Vector3 mC2CrossA1; - /// Impulse for the 3 translation constraints - Vector3 mImpulseTranslation; + /// Impulse for the 3 translation constraints + Vector3 mImpulseTranslation; - /// Impulse for the 2 rotation constraints - Vector2 mImpulseRotation; + /// Impulse for the 2 rotation constraints + Vector2 mImpulseRotation; - /// Accumulated impulse for the lower limit constraint - decimal mImpulseLowerLimit; + /// Accumulated impulse for the lower limit constraint + float mImpulseLowerLimit; - /// Accumulated impulse for the upper limit constraint - decimal mImpulseUpperLimit; + /// Accumulated impulse for the upper limit constraint + float mImpulseUpperLimit; - /// Accumulated impulse for the motor constraint; - decimal mImpulseMotor; + /// Accumulated impulse for the motor constraint; + float mImpulseMotor; - /// Inverse mass matrix K=JM^-1J^t for the 3 translation constraints - Matrix3x3 mInverseMassMatrixTranslation; + /// Inverse mass matrix K=JM^-1J^t for the 3 translation constraints + Matrix3x3 mInverseMassMatrixTranslation; - /// Inverse mass matrix K=JM^-1J^t for the 2 rotation constraints - Matrix2x2 mInverseMassMatrixRotation; + /// Inverse mass matrix K=JM^-1J^t for the 2 rotation constraints + Matrix2x2 mInverseMassMatrixRotation; - /// Inverse of mass matrix K=JM^-1J^t for the limits and motor constraints (1x1 matrix) - decimal mInverseMassMatrixLimitMotor; + /// Inverse of mass matrix K=JM^-1J^t for the limits and motor constraints (1x1 matrix) + float mInverseMassMatrixLimitMotor; - /// Inverse of mass matrix K=JM^-1J^t for the motor - decimal mInverseMassMatrixMotor; + /// Inverse of mass matrix K=JM^-1J^t for the motor + float mInverseMassMatrixMotor; - /// Bias vector for the error correction for the translation constraints - Vector3 mBTranslation; + /// Bias vector for the error correction for the translation constraints + Vector3 mBTranslation; - /// Bias vector for the error correction for the rotation constraints - Vector2 mBRotation; + /// Bias vector for the error correction for the rotation constraints + Vector2 mBRotation; - /// Bias of the lower limit constraint - decimal mBLowerLimit; + /// Bias of the lower limit constraint + float mBLowerLimit; - /// Bias of the upper limit constraint - decimal mBUpperLimit; + /// Bias of the upper limit constraint + float mBUpperLimit; - /// Inverse of the initial orientation difference between the bodies - Quaternion mInitOrientationDifferenceInv; + /// Inverse of the initial orientation difference between the bodies + Quaternion mInitOrientationDifferenceInv; - /// True if the joint limits are enabled - bool mIsLimitEnabled; + /// True if the joint limits are enabled + bool mIsLimitEnabled; - /// True if the motor of the joint in enabled - bool mIsMotorEnabled; + /// True if the motor of the joint in enabled + bool mIsMotorEnabled; - /// Lower limit (minimum allowed rotation angle in radian) - decimal mLowerLimit; + /// Lower limit (minimum allowed rotation angle in radian) + float mLowerLimit; - /// Upper limit (maximum translation distance) - decimal mUpperLimit; + /// Upper limit (maximum translation distance) + float mUpperLimit; - /// True if the lower limit is violated - bool mIsLowerLimitViolated; + /// True if the lower limit is violated + bool mIsLowerLimitViolated; - /// True if the upper limit is violated - bool mIsUpperLimitViolated; + /// True if the upper limit is violated + bool mIsUpperLimitViolated; - /// Motor speed (in rad/s) - decimal mMotorSpeed; + /// Motor speed (in rad/s) + float mMotorSpeed; - /// Maximum motor torque (in Newtons) that can be applied to reach to desired motor speed - decimal mMaxMotorTorque; + /// Maximum motor torque (in Newtons) that can be applied to reach to desired motor speed + float mMaxMotorTorque; - // -------------------- Methods -------------------- // + // -------------------- Methods -------------------- // - /// Private copy-constructor - HingeJoint(const HingeJoint& constraint); + /// Private copy-constructor + HingeJoint(const HingeJoint& constraint); - /// Private assignment operator - HingeJoint& operator=(const HingeJoint& constraint); + /// Private assignment operator + HingeJoint& operator=(const HingeJoint& constraint); - /// Reset the limits - void resetLimits(); + /// Reset the limits + void resetLimits(); - /// Given an angle in radian, this method returns the corresponding - /// angle in the range [-pi; pi] - decimal computeNormalizedAngle(decimal angle) const; + /// Given an angle in radian, this method returns the corresponding + /// angle in the range [-pi; pi] + float computeNormalizedAngle(float angle) const; - /// Given an "inputAngle" in the range [-pi, pi], this method returns an - /// angle (modulo 2*pi) in the range [-2*pi; 2*pi] that is closest to one of the - /// two angle limits in arguments. - decimal computeCorrespondingAngleNearLimits(decimal inputAngle, decimal lowerLimitAngle, - decimal upperLimitAngle) const; + /// Given an "inputAngle" in the range [-pi, pi], this method returns an + /// angle (modulo 2*pi) in the range [-2*pi; 2*pi] that is closest to one of the + /// two angle limits in arguments. + float computeCorrespondingAngleNearLimits(float inputAngle, float lowerLimitAngle, + float upperLimitAngle) const; - /// Compute the current angle around the hinge axis - decimal computeCurrentHingeAngle(const Quaternion& orientationBody1, - const Quaternion& orientationBody2); + /// Compute the current angle around the hinge axis + float computeCurrentHingeAngle(const Quaternion& orientationBody1, + const Quaternion& orientationBody2); - /// Return the number of bytes used by the joint - virtual size_t getSizeInBytes() const; + /// Return the number of bytes used by the joint + virtual size_t getSizeInBytes() const; - /// Initialize before solving the constraint - virtual void initBeforeSolve(const ConstraintSolverData& constraintSolverData); + /// Initialize before solving the constraint + virtual void initBeforeSolve(const ConstraintSolverData& constraintSolverData); - /// Warm start the constraint (apply the previous impulse at the beginning of the step) - virtual void warmstart(const ConstraintSolverData& constraintSolverData); + /// Warm start the constraint (apply the previous impulse at the beginning of the step) + virtual void warmstart(const ConstraintSolverData& constraintSolverData); - /// Solve the velocity constraint - virtual void solveVelocityConstraint(const ConstraintSolverData& constraintSolverData); + /// Solve the velocity constraint + virtual void solveVelocityConstraint(const ConstraintSolverData& constraintSolverData); - /// Solve the position constraint (for position error correction) - virtual void solvePositionConstraint(const ConstraintSolverData& constraintSolverData); + /// Solve the position constraint (for position error correction) + virtual void solvePositionConstraint(const ConstraintSolverData& constraintSolverData); - public : + public : - // -------------------- Methods -------------------- // + // -------------------- Methods -------------------- // - /// Constructor - HingeJoint(const HingeJointInfo& jointInfo); + /// Constructor + HingeJoint(const HingeJointInfo& jointInfo); - /// Destructor - virtual ~HingeJoint(); + /// Destructor + virtual ~HingeJoint(); - /// Return true if the limits or the joint are enabled - bool isLimitEnabled() const; + /// Return true if the limits or the joint are enabled + bool isLimitEnabled() const; - /// Return true if the motor of the joint is enabled - bool isMotorEnabled() const; + /// Return true if the motor of the joint is enabled + bool isMotorEnabled() const; - /// Enable/Disable the limits of the joint - void enableLimit(bool isLimitEnabled); + /// Enable/Disable the limits of the joint + void enableLimit(bool isLimitEnabled); - /// Enable/Disable the motor of the joint - void enableMotor(bool isMotorEnabled); + /// Enable/Disable the motor of the joint + void enableMotor(bool isMotorEnabled); - /// Return the minimum angle limit - decimal getMinAngleLimit() const; + /// Return the minimum angle limit + float getMinAngleLimit() const; - /// Set the minimum angle limit - void setMinAngleLimit(decimal lowerLimit); + /// Set the minimum angle limit + void setMinAngleLimit(float lowerLimit); - /// Return the maximum angle limit - decimal getMaxAngleLimit() const; + /// Return the maximum angle limit + float getMaxAngleLimit() const; - /// Set the maximum angle limit - void setMaxAngleLimit(decimal upperLimit); + /// Set the maximum angle limit + void setMaxAngleLimit(float upperLimit); - /// Return the motor speed - decimal getMotorSpeed() const; + /// Return the motor speed + float getMotorSpeed() const; - /// Set the motor speed - void setMotorSpeed(decimal motorSpeed); + /// Set the motor speed + void setMotorSpeed(float motorSpeed); - /// Return the maximum motor torque - decimal getMaxMotorTorque() const; + /// Return the maximum motor torque + float getMaxMotorTorque() const; - /// Set the maximum motor torque - void setMaxMotorTorque(decimal maxMotorTorque); + /// Set the maximum motor torque + void setMaxMotorTorque(float maxMotorTorque); - /// Return the intensity of the current torque applied for the joint motor - decimal getMotorTorque(decimal timeStep) const; + /// Return the int32_tensity of the current torque applied for the joint motor + float getMotorTorque(float timeStep) const; }; // Return true if the limits of the joint are enabled @@ -343,7 +322,7 @@ class HingeJoint : public Joint { * @return True if the limits of the joint are enabled and false otherwise */ inline bool HingeJoint::isLimitEnabled() const { - return mIsLimitEnabled; + return mIsLimitEnabled; } // Return true if the motor of the joint is enabled @@ -351,56 +330,54 @@ inline bool HingeJoint::isLimitEnabled() const { * @return True if the motor of joint is enabled and false otherwise */ inline bool HingeJoint::isMotorEnabled() const { - return mIsMotorEnabled; + return mIsMotorEnabled; } // Return the minimum angle limit /** * @return The minimum limit angle of the joint (in radian) */ -inline decimal HingeJoint::getMinAngleLimit() const { - return mLowerLimit; +inline float HingeJoint::getMinAngleLimit() const { + return mLowerLimit; } // Return the maximum angle limit /** * @return The maximum limit angle of the joint (in radian) */ -inline decimal HingeJoint::getMaxAngleLimit() const { - return mUpperLimit; +inline float HingeJoint::getMaxAngleLimit() const { + return mUpperLimit; } // Return the motor speed /** * @return The current speed of the joint motor (in radian per second) */ -inline decimal HingeJoint::getMotorSpeed() const { - return mMotorSpeed; +inline float HingeJoint::getMotorSpeed() const { + return mMotorSpeed; } // Return the maximum motor torque /** * @return The maximum torque of the joint motor (in Newtons) */ -inline decimal HingeJoint::getMaxMotorTorque() const { - return mMaxMotorTorque; +inline float HingeJoint::getMaxMotorTorque() const { + return mMaxMotorTorque; } -// Return the intensity of the current torque applied for the joint motor +// Return the int32_tensity of the current torque applied for the joint motor /** * @param timeStep The current time step (in seconds) - * @return The intensity of the current torque (in Newtons) of the joint motor + * @return The int32_tensity of the current torque (in Newtons) of the joint motor */ -inline decimal HingeJoint::getMotorTorque(decimal timeStep) const { - return mImpulseMotor / timeStep; +inline float HingeJoint::getMotorTorque(float timeStep) const { + return mImpulseMotor / timeStep; } // Return the number of bytes used by the joint inline size_t HingeJoint::getSizeInBytes() const { - return sizeof(HingeJoint); + return sizeof(HingeJoint); } } - -#endif diff --git a/ephysics/constraint/Joint.cpp b/ephysics/constraint/Joint.cpp index e17c6db..0450e7a 100644 --- a/ephysics/constraint/Joint.cpp +++ b/ephysics/constraint/Joint.cpp @@ -1,27 +1,9 @@ -/******************************************************************************** -* 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. * -* * -********************************************************************************/ +/** @file + * @author Daniel Chappuis + * @copyright 2010-2016 Daniel Chappuis + * @license BSD 3 clauses (see license file) + */ +#pragma once // Libraries #include @@ -30,12 +12,12 @@ using namespace reactphysics3d; // Constructor Joint::Joint(const JointInfo& jointInfo) - :mBody1(jointInfo.body1), mBody2(jointInfo.body2), mType(jointInfo.type), - mPositionCorrectionTechnique(jointInfo.positionCorrectionTechnique), - mIsCollisionEnabled(jointInfo.isCollisionEnabled), mIsAlreadyInIsland(false) { + :mBody1(jointInfo.body1), mBody2(jointInfo.body2), mType(jointInfo.type), + mPositionCorrectionTechnique(jointInfo.positionCorrectionTechnique), + mIsCollisionEnabled(jointInfo.isCollisionEnabled), mIsAlreadyInIsland(false) { - assert(mBody1 != NULL); - assert(mBody2 != NULL); + assert(mBody1 != NULL); + assert(mBody2 != NULL); } // Destructor diff --git a/ephysics/constraint/Joint.h b/ephysics/constraint/Joint.h index d14551d..f046419 100644 --- a/ephysics/constraint/Joint.h +++ b/ephysics/constraint/Joint.h @@ -1,30 +1,9 @@ -/******************************************************************************** -* 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 REACTPHYSICS3D_CONSTRAINT_H -#define REACTPHYSICS3D_CONSTRAINT_H +/** @file + * @author Daniel Chappuis + * @copyright 2010-2016 Daniel Chappuis + * @license BSD 3 clauses (see license file) + */ +#pragma once // Libraries #include @@ -47,23 +26,23 @@ class Joint; */ struct JointListElement { - public: + public: - // -------------------- Attributes -------------------- // + // -------------------- Attributes -------------------- // - /// Pointer to the actual joint - Joint* joint; + /// Pointer to the actual joint + Joint* joint; - /// Next element of the list - JointListElement* next; + /// Next element of the list + JointListElement* next; - // -------------------- Methods -------------------- // + // -------------------- Methods -------------------- // - /// Constructor - JointListElement(Joint* initJoint, JointListElement* initNext) - :joint(initJoint), next(initNext){ + /// Constructor + JointListElement(Joint* initJoint, JointListElement* initNext) + :joint(initJoint), next(initNext){ - } + } }; // Structure JointInfo @@ -72,41 +51,41 @@ struct JointListElement { */ struct JointInfo { - public : + public : - // -------------------- Attributes -------------------- // + // -------------------- Attributes -------------------- // - /// First rigid body of the joint - RigidBody* body1; + /// First rigid body of the joint + RigidBody* body1; - /// Second rigid body of the joint - RigidBody* body2; + /// Second rigid body of the joint + RigidBody* body2; - /// Type of the joint - JointType type; + /// Type of the joint + JointType type; - /// Position correction technique used for the constraint (used for joints). - /// By default, the BAUMGARTE technique is used - JointsPositionCorrectionTechnique positionCorrectionTechnique; + /// Position correction technique used for the constraint (used for joints). + /// By default, the BAUMGARTE technique is used + JointsPositionCorrectionTechnique positionCorrectionTechnique; - /// True if the two bodies of the joint are allowed to collide with each other - bool isCollisionEnabled; + /// True if the two bodies of the joint are allowed to collide with each other + bool isCollisionEnabled; - /// Constructor - JointInfo(JointType constraintType) - : body1(NULL), body2(NULL), type(constraintType), - positionCorrectionTechnique(NON_LINEAR_GAUSS_SEIDEL), - isCollisionEnabled(true) {} + /// Constructor + JointInfo(JointType constraintType) + : body1(NULL), body2(NULL), type(constraintType), + positionCorrectionTechnique(NON_LINEAR_GAUSS_SEIDEL), + isCollisionEnabled(true) {} - /// Constructor - JointInfo(RigidBody* rigidBody1, RigidBody* rigidBody2, JointType constraintType) - : body1(rigidBody1), body2(rigidBody2), type(constraintType), - positionCorrectionTechnique(NON_LINEAR_GAUSS_SEIDEL), - isCollisionEnabled(true) { - } + /// Constructor + JointInfo(RigidBody* rigidBody1, RigidBody* rigidBody2, JointType constraintType) + : body1(rigidBody1), body2(rigidBody2), type(constraintType), + positionCorrectionTechnique(NON_LINEAR_GAUSS_SEIDEL), + isCollisionEnabled(true) { + } - /// Destructor - virtual ~JointInfo() {} + /// Destructor + virtual ~JointInfo() {} }; @@ -116,90 +95,90 @@ struct JointInfo { */ class Joint { - protected : + protected : - // -------------------- Attributes -------------------- // + // -------------------- Attributes -------------------- // - /// Pointer to the first body of the joint - RigidBody* const mBody1; + /// Pointer to the first body of the joint + RigidBody* const mBody1; - /// Pointer to the second body of the joint - RigidBody* const mBody2; + /// Pointer to the second body of the joint + RigidBody* const mBody2; - /// Type of the joint - const JointType mType; + /// Type of the joint + const JointType mType; - /// Body 1 index in the velocity array to solve the constraint - uint mIndexBody1; + /// Body 1 index in the velocity array to solve the constraint + uint32_t mIndexBody1; - /// Body 2 index in the velocity array to solve the constraint - uint mIndexBody2; + /// Body 2 index in the velocity array to solve the constraint + uint32_t mIndexBody2; - /// Position correction technique used for the constraint (used for joints) - JointsPositionCorrectionTechnique mPositionCorrectionTechnique; + /// Position correction technique used for the constraint (used for joints) + JointsPositionCorrectionTechnique mPositionCorrectionTechnique; - /// True if the two bodies of the constraint are allowed to collide with each other - bool mIsCollisionEnabled; + /// True if the two bodies of the constraint are allowed to collide with each other + bool mIsCollisionEnabled; - /// True if the joint has already been added into an island - bool mIsAlreadyInIsland; + /// True if the joint has already been added int32_to an island + bool mIsAlreadyInIsland; - // -------------------- Methods -------------------- // + // -------------------- Methods -------------------- // - /// Private copy-constructor - Joint(const Joint& constraint); + /// Private copy-constructor + Joint(const Joint& constraint); - /// Private assignment operator - Joint& operator=(const Joint& constraint); + /// Private assignment operator + Joint& operator=(const Joint& constraint); - /// Return true if the joint has already been added into an island - bool isAlreadyInIsland() const; + /// Return true if the joint has already been added int32_to an island + bool isAlreadyInIsland() const; - /// Return the number of bytes used by the joint - virtual size_t getSizeInBytes() const = 0; + /// Return the number of bytes used by the joint + virtual size_t getSizeInBytes() const = 0; - /// Initialize before solving the joint - virtual void initBeforeSolve(const ConstraintSolverData& constraintSolverData) = 0; + /// Initialize before solving the joint + virtual void initBeforeSolve(const ConstraintSolverData& constraintSolverData) = 0; - /// Warm start the joint (apply the previous impulse at the beginning of the step) - virtual void warmstart(const ConstraintSolverData& constraintSolverData) = 0; + /// Warm start the joint (apply the previous impulse at the beginning of the step) + virtual void warmstart(const ConstraintSolverData& constraintSolverData) = 0; - /// Solve the velocity constraint - virtual void solveVelocityConstraint(const ConstraintSolverData& constraintSolverData) = 0; + /// Solve the velocity constraint + virtual void solveVelocityConstraint(const ConstraintSolverData& constraintSolverData) = 0; - /// Solve the position constraint - virtual void solvePositionConstraint(const ConstraintSolverData& constraintSolverData) = 0; + /// Solve the position constraint + virtual void solvePositionConstraint(const ConstraintSolverData& constraintSolverData) = 0; - public : + public : - // -------------------- Methods -------------------- // + // -------------------- Methods -------------------- // - /// Constructor - Joint(const JointInfo& jointInfo); + /// Constructor + Joint(const JointInfo& jointInfo); - /// Destructor - virtual ~Joint(); + /// Destructor + virtual ~Joint(); - /// Return the reference to the body 1 - RigidBody* getBody1() const; + /// Return the reference to the body 1 + RigidBody* getBody1() const; - /// Return the reference to the body 2 - RigidBody* getBody2() const; + /// Return the reference to the body 2 + RigidBody* getBody2() const; - /// Return true if the constraint is active - bool isActive() const; + /// Return true if the constraint is active + bool isActive() const; - /// Return the type of the constraint - JointType getType() const; + /// Return the type of the constraint + JointType getType() const; - /// Return true if the collision between the two bodies of the joint is enabled - bool isCollisionEnabled() const; + /// Return true if the collision between the two bodies of the joint is enabled + bool isCollisionEnabled() const; - // -------------------- Friendship -------------------- // + // -------------------- Friendship -------------------- // - friend class DynamicsWorld; - friend class Island; - friend class ConstraintSolver; + friend class DynamicsWorld; + friend class Island; + friend class ConstraintSolver; }; // Return the reference to the body 1 @@ -207,7 +186,7 @@ class Joint { * @return The first body involved in the joint */ inline RigidBody* Joint::getBody1() const { - return mBody1; + return mBody1; } // Return the reference to the body 2 @@ -215,7 +194,7 @@ inline RigidBody* Joint::getBody1() const { * @return The second body involved in the joint */ inline RigidBody* Joint::getBody2() const { - return mBody2; + return mBody2; } // Return true if the joint is active @@ -223,7 +202,7 @@ inline RigidBody* Joint::getBody2() const { * @return True if the joint is active */ inline bool Joint::isActive() const { - return (mBody1->isActive() && mBody2->isActive()); + return (mBody1->isActive() && mBody2->isActive()); } // Return the type of the joint @@ -231,23 +210,21 @@ inline bool Joint::isActive() const { * @return The type of the joint */ inline JointType Joint::getType() const { - return mType; + return mType; } // Return true if the collision between the two bodies of the joint is enabled /** * @return True if the collision is enabled between the two bodies of the joint - * is enabled and false otherwise + * is enabled and false otherwise */ inline bool Joint::isCollisionEnabled() const { - return mIsCollisionEnabled; + return mIsCollisionEnabled; } -// Return true if the joint has already been added into an island +// Return true if the joint has already been added int32_to an island inline bool Joint::isAlreadyInIsland() const { - return mIsAlreadyInIsland; + return mIsAlreadyInIsland; } } - -#endif diff --git a/ephysics/constraint/SliderJoint.cpp b/ephysics/constraint/SliderJoint.cpp index e6e203f..336972c 100644 --- a/ephysics/constraint/SliderJoint.cpp +++ b/ephysics/constraint/SliderJoint.cpp @@ -1,27 +1,8 @@ -/******************************************************************************** -* 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. * -* * -********************************************************************************/ +/** @file + * @author Daniel Chappuis + * @copyright 2010-2016 Daniel Chappuis + * @license BSD 3 clauses (see license file) + */ // Libraries #include @@ -29,38 +10,38 @@ using namespace reactphysics3d; // Static variables definition -const decimal SliderJoint::BETA = decimal(0.2); +const float SliderJoint::BETA = float(0.2); // Constructor SliderJoint::SliderJoint(const SliderJointInfo& jointInfo) - : Joint(jointInfo), mImpulseTranslation(0, 0), mImpulseRotation(0, 0, 0), - mImpulseLowerLimit(0), mImpulseUpperLimit(0), mImpulseMotor(0), - mIsLimitEnabled(jointInfo.isLimitEnabled), mIsMotorEnabled(jointInfo.isMotorEnabled), - mLowerLimit(jointInfo.minTranslationLimit), - mUpperLimit(jointInfo.maxTranslationLimit), mIsLowerLimitViolated(false), - mIsUpperLimitViolated(false), mMotorSpeed(jointInfo.motorSpeed), - mMaxMotorForce(jointInfo.maxMotorForce){ + : Joint(jointInfo), mImpulseTranslation(0, 0), mImpulseRotation(0, 0, 0), + mImpulseLowerLimit(0), mImpulseUpperLimit(0), mImpulseMotor(0), + mIsLimitEnabled(jointInfo.isLimitEnabled), mIsMotorEnabled(jointInfo.isMotorEnabled), + mLowerLimit(jointInfo.minTranslationLimit), + mUpperLimit(jointInfo.maxTranslationLimit), mIsLowerLimitViolated(false), + mIsUpperLimitViolated(false), mMotorSpeed(jointInfo.motorSpeed), + mMaxMotorForce(jointInfo.maxMotorForce){ - assert(mUpperLimit >= 0.0); - assert(mLowerLimit <= 0.0); - assert(mMaxMotorForce >= 0.0); + assert(mUpperLimit >= 0.0); + assert(mLowerLimit <= 0.0); + assert(mMaxMotorForce >= 0.0); - // Compute the local-space anchor point for each body - const Transform& transform1 = mBody1->getTransform(); - const Transform& transform2 = mBody2->getTransform(); - mLocalAnchorPointBody1 = transform1.getInverse() * jointInfo.anchorPointWorldSpace; - mLocalAnchorPointBody2 = transform2.getInverse() * jointInfo.anchorPointWorldSpace; + // Compute the local-space anchor point for each body + const Transform& transform1 = mBody1->getTransform(); + const Transform& transform2 = mBody2->getTransform(); + mLocalAnchorPointBody1 = transform1.getInverse() * jointInfo.anchorPointWorldSpace; + mLocalAnchorPointBody2 = transform2.getInverse() * jointInfo.anchorPointWorldSpace; - // Compute the inverse of the initial orientation difference between the two bodies - mInitOrientationDifferenceInv = transform2.getOrientation() * - transform1.getOrientation().getInverse(); - mInitOrientationDifferenceInv.normalize(); - mInitOrientationDifferenceInv.inverse(); + // Compute the inverse of the initial orientation difference between the two bodies + mInitOrientationDifferenceInv = transform2.getOrientation() * + transform1.getOrientation().getInverse(); + mInitOrientationDifferenceInv.normalize(); + mInitOrientationDifferenceInv.inverse(); - // Compute the slider axis in local-space of body 1 - mSliderAxisBody1 = mBody1->getTransform().getOrientation().getInverse() * - jointInfo.sliderAxisWorldSpace; - mSliderAxisBody1.normalize(); + // Compute the slider axis in local-space of body 1 + mSliderAxisBody1 = mBody1->getTransform().getOrientation().getInverse() * + jointInfo.sliderAxisWorldSpace; + mSliderAxisBody1.normalize(); } // Destructor @@ -71,725 +52,725 @@ SliderJoint::~SliderJoint() { // Initialize before solving the constraint void SliderJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverData) { - // Initialize the bodies index in the veloc ity array - mIndexBody1 = constraintSolverData.mapBodyToConstrainedVelocityIndex.find(mBody1)->second; - mIndexBody2 = constraintSolverData.mapBodyToConstrainedVelocityIndex.find(mBody2)->second; + // Initialize the bodies index in the veloc ity array + mIndexBody1 = constraintSolverData.mapBodyToConstrainedVelocityIndex.find(mBody1)->second; + mIndexBody2 = constraintSolverData.mapBodyToConstrainedVelocityIndex.find(mBody2)->second; - // Get the bodies positions and orientations - const Vector3& x1 = mBody1->mCenterOfMassWorld; - const Vector3& x2 = mBody2->mCenterOfMassWorld; - const Quaternion& orientationBody1 = mBody1->getTransform().getOrientation(); - const Quaternion& orientationBody2 = mBody2->getTransform().getOrientation(); + // Get the bodies positions and orientations + const Vector3& x1 = mBody1->mCenterOfMassWorld; + const Vector3& x2 = mBody2->mCenterOfMassWorld; + const Quaternion& orientationBody1 = mBody1->getTransform().getOrientation(); + const Quaternion& orientationBody2 = mBody2->getTransform().getOrientation(); - // Get the inertia tensor of bodies - mI1 = mBody1->getInertiaTensorInverseWorld(); - mI2 = mBody2->getInertiaTensorInverseWorld(); + // Get the inertia tensor of bodies + mI1 = mBody1->getInertiaTensorInverseWorld(); + mI2 = mBody2->getInertiaTensorInverseWorld(); - // Vector from body center to the anchor point - mR1 = orientationBody1 * mLocalAnchorPointBody1; - mR2 = orientationBody2 * mLocalAnchorPointBody2; + // Vector from body center to the anchor point + mR1 = orientationBody1 * mLocalAnchorPointBody1; + mR2 = orientationBody2 * mLocalAnchorPointBody2; - // Compute the vector u (difference between anchor points) - const Vector3 u = x2 + mR2 - x1 - mR1; + // Compute the vector u (difference between anchor points) + const Vector3 u = x2 + mR2 - x1 - mR1; - // Compute the two orthogonal vectors to the slider axis in world-space - mSliderAxisWorld = orientationBody1 * mSliderAxisBody1; - mSliderAxisWorld.normalize(); - mN1 = mSliderAxisWorld.getOneUnitOrthogonalVector(); - mN2 = mSliderAxisWorld.cross(mN1); + // Compute the two orthogonal vectors to the slider axis in world-space + mSliderAxisWorld = orientationBody1 * mSliderAxisBody1; + mSliderAxisWorld.normalize(); + mN1 = mSliderAxisWorld.getOneUnitOrthogonalVector(); + mN2 = mSliderAxisWorld.cross(mN1); - // Check if the limit constraints are violated or not - decimal uDotSliderAxis = u.dot(mSliderAxisWorld); - decimal lowerLimitError = uDotSliderAxis - mLowerLimit; - decimal upperLimitError = mUpperLimit - uDotSliderAxis; - bool oldIsLowerLimitViolated = mIsLowerLimitViolated; - mIsLowerLimitViolated = lowerLimitError <= 0; - if (mIsLowerLimitViolated != oldIsLowerLimitViolated) { - mImpulseLowerLimit = 0.0; - } - bool oldIsUpperLimitViolated = mIsUpperLimitViolated; - mIsUpperLimitViolated = upperLimitError <= 0; - if (mIsUpperLimitViolated != oldIsUpperLimitViolated) { - mImpulseUpperLimit = 0.0; - } + // Check if the limit constraints are violated or not + float uDotSliderAxis = u.dot(mSliderAxisWorld); + float lowerLimitError = uDotSliderAxis - mLowerLimit; + float upperLimitError = mUpperLimit - uDotSliderAxis; + bool oldIsLowerLimitViolated = mIsLowerLimitViolated; + mIsLowerLimitViolated = lowerLimitError <= 0; + if (mIsLowerLimitViolated != oldIsLowerLimitViolated) { + mImpulseLowerLimit = 0.0; + } + bool oldIsUpperLimitViolated = mIsUpperLimitViolated; + mIsUpperLimitViolated = upperLimitError <= 0; + if (mIsUpperLimitViolated != oldIsUpperLimitViolated) { + mImpulseUpperLimit = 0.0; + } - // Compute the cross products used in the Jacobians - mR2CrossN1 = mR2.cross(mN1); - mR2CrossN2 = mR2.cross(mN2); - mR2CrossSliderAxis = mR2.cross(mSliderAxisWorld); - const Vector3 r1PlusU = mR1 + u; - mR1PlusUCrossN1 = (r1PlusU).cross(mN1); - mR1PlusUCrossN2 = (r1PlusU).cross(mN2); - mR1PlusUCrossSliderAxis = (r1PlusU).cross(mSliderAxisWorld); + // Compute the cross products used in the Jacobians + mR2CrossN1 = mR2.cross(mN1); + mR2CrossN2 = mR2.cross(mN2); + mR2CrossSliderAxis = mR2.cross(mSliderAxisWorld); + const Vector3 r1PlusU = mR1 + u; + mR1PlusUCrossN1 = (r1PlusU).cross(mN1); + mR1PlusUCrossN2 = (r1PlusU).cross(mN2); + mR1PlusUCrossSliderAxis = (r1PlusU).cross(mSliderAxisWorld); - // Compute the inverse of the mass matrix K=JM^-1J^t for the 2 translation - // constraints (2x2 matrix) - decimal sumInverseMass = mBody1->mMassInverse + mBody2->mMassInverse; - Vector3 I1R1PlusUCrossN1 = mI1 * mR1PlusUCrossN1; - Vector3 I1R1PlusUCrossN2 = mI1 * mR1PlusUCrossN2; - Vector3 I2R2CrossN1 = mI2 * mR2CrossN1; - Vector3 I2R2CrossN2 = mI2 * mR2CrossN2; - const decimal el11 = sumInverseMass + mR1PlusUCrossN1.dot(I1R1PlusUCrossN1) + - mR2CrossN1.dot(I2R2CrossN1); - const decimal el12 = mR1PlusUCrossN1.dot(I1R1PlusUCrossN2) + - mR2CrossN1.dot(I2R2CrossN2); - const decimal el21 = mR1PlusUCrossN2.dot(I1R1PlusUCrossN1) + - mR2CrossN2.dot(I2R2CrossN1); - const decimal el22 = sumInverseMass + mR1PlusUCrossN2.dot(I1R1PlusUCrossN2) + - mR2CrossN2.dot(I2R2CrossN2); - Matrix2x2 matrixKTranslation(el11, el12, el21, el22); - mInverseMassMatrixTranslationConstraint.setToZero(); - if (mBody1->getType() == DYNAMIC || mBody2->getType() == DYNAMIC) { - mInverseMassMatrixTranslationConstraint = matrixKTranslation.getInverse(); - } + // Compute the inverse of the mass matrix K=JM^-1J^t for the 2 translation + // constraints (2x2 matrix) + float sumInverseMass = mBody1->mMassInverse + mBody2->mMassInverse; + Vector3 I1R1PlusUCrossN1 = mI1 * mR1PlusUCrossN1; + Vector3 I1R1PlusUCrossN2 = mI1 * mR1PlusUCrossN2; + Vector3 I2R2CrossN1 = mI2 * mR2CrossN1; + Vector3 I2R2CrossN2 = mI2 * mR2CrossN2; + const float el11 = sumInverseMass + mR1PlusUCrossN1.dot(I1R1PlusUCrossN1) + + mR2CrossN1.dot(I2R2CrossN1); + const float el12 = mR1PlusUCrossN1.dot(I1R1PlusUCrossN2) + + mR2CrossN1.dot(I2R2CrossN2); + const float el21 = mR1PlusUCrossN2.dot(I1R1PlusUCrossN1) + + mR2CrossN2.dot(I2R2CrossN1); + const float el22 = sumInverseMass + mR1PlusUCrossN2.dot(I1R1PlusUCrossN2) + + mR2CrossN2.dot(I2R2CrossN2); + Matrix2x2 matrixKTranslation(el11, el12, el21, el22); + mInverseMassMatrixTranslationConstraint.setToZero(); + if (mBody1->getType() == DYNAMIC || mBody2->getType() == DYNAMIC) { + mInverseMassMatrixTranslationConstraint = matrixKTranslation.getInverse(); + } - // Compute the bias "b" of the translation constraint - mBTranslation.setToZero(); - decimal biasFactor = (BETA / constraintSolverData.timeStep); - if (mPositionCorrectionTechnique == BAUMGARTE_JOINTS) { - mBTranslation.x = u.dot(mN1); - mBTranslation.y = u.dot(mN2); - mBTranslation *= biasFactor; - } + // Compute the bias "b" of the translation constraint + mBTranslation.setToZero(); + float biasFactor = (BETA / constraintSolverData.timeStep); + if (mPositionCorrectionTechnique == BAUMGARTE_JOINTS) { + mBTranslation.x = u.dot(mN1); + mBTranslation.y = u.dot(mN2); + mBTranslation *= biasFactor; + } - // Compute the inverse of the mass matrix K=JM^-1J^t for the 3 rotation - // contraints (3x3 matrix) - mInverseMassMatrixRotationConstraint = mI1 + mI2; - if (mBody1->getType() == DYNAMIC || mBody2->getType() == DYNAMIC) { - mInverseMassMatrixRotationConstraint = mInverseMassMatrixRotationConstraint.getInverse(); - } + // Compute the inverse of the mass matrix K=JM^-1J^t for the 3 rotation + // contraints (3x3 matrix) + mInverseMassMatrixRotationConstraint = mI1 + mI2; + if (mBody1->getType() == DYNAMIC || mBody2->getType() == DYNAMIC) { + mInverseMassMatrixRotationConstraint = mInverseMassMatrixRotationConstraint.getInverse(); + } - // Compute the bias "b" of the rotation constraint - mBRotation.setToZero(); - if (mPositionCorrectionTechnique == BAUMGARTE_JOINTS) { - Quaternion currentOrientationDifference = orientationBody2 * orientationBody1.getInverse(); - currentOrientationDifference.normalize(); - const Quaternion qError = currentOrientationDifference * mInitOrientationDifferenceInv; - mBRotation = biasFactor * decimal(2.0) * qError.getVectorV(); - } + // Compute the bias "b" of the rotation constraint + mBRotation.setToZero(); + if (mPositionCorrectionTechnique == BAUMGARTE_JOINTS) { + Quaternion currentOrientationDifference = orientationBody2 * orientationBody1.getInverse(); + currentOrientationDifference.normalize(); + const Quaternion qError = currentOrientationDifference * mInitOrientationDifferenceInv; + mBRotation = biasFactor * float(2.0) * qError.getVectorV(); + } - // If the limits are enabled - if (mIsLimitEnabled && (mIsLowerLimitViolated || mIsUpperLimitViolated)) { + // If the limits are enabled + if (mIsLimitEnabled && (mIsLowerLimitViolated || mIsUpperLimitViolated)) { - // Compute the inverse of the mass matrix K=JM^-1J^t for the limits (1x1 matrix) - mInverseMassMatrixLimit = mBody1->mMassInverse + mBody2->mMassInverse + - mR1PlusUCrossSliderAxis.dot(mI1 * mR1PlusUCrossSliderAxis) + - mR2CrossSliderAxis.dot(mI2 * mR2CrossSliderAxis); - mInverseMassMatrixLimit = (mInverseMassMatrixLimit > 0.0) ? - decimal(1.0) / mInverseMassMatrixLimit : decimal(0.0); + // Compute the inverse of the mass matrix K=JM^-1J^t for the limits (1x1 matrix) + mInverseMassMatrixLimit = mBody1->mMassInverse + mBody2->mMassInverse + + mR1PlusUCrossSliderAxis.dot(mI1 * mR1PlusUCrossSliderAxis) + + mR2CrossSliderAxis.dot(mI2 * mR2CrossSliderAxis); + mInverseMassMatrixLimit = (mInverseMassMatrixLimit > 0.0) ? + float(1.0) / mInverseMassMatrixLimit : float(0.0); - // Compute the bias "b" of the lower limit constraint - mBLowerLimit = 0.0; - if (mPositionCorrectionTechnique == BAUMGARTE_JOINTS) { - mBLowerLimit = biasFactor * lowerLimitError; - } + // Compute the bias "b" of the lower limit constraint + mBLowerLimit = 0.0; + if (mPositionCorrectionTechnique == BAUMGARTE_JOINTS) { + mBLowerLimit = biasFactor * lowerLimitError; + } - // Compute the bias "b" of the upper limit constraint - mBUpperLimit = 0.0; - if (mPositionCorrectionTechnique == BAUMGARTE_JOINTS) { - mBUpperLimit = biasFactor * upperLimitError; - } - } + // Compute the bias "b" of the upper limit constraint + mBUpperLimit = 0.0; + if (mPositionCorrectionTechnique == BAUMGARTE_JOINTS) { + mBUpperLimit = biasFactor * upperLimitError; + } + } - // If the motor is enabled - if (mIsMotorEnabled) { + // If the motor is enabled + if (mIsMotorEnabled) { - // Compute the inverse of mass matrix K=JM^-1J^t for the motor (1x1 matrix) - mInverseMassMatrixMotor = mBody1->mMassInverse + mBody2->mMassInverse; - mInverseMassMatrixMotor = (mInverseMassMatrixMotor > 0.0) ? - decimal(1.0) / mInverseMassMatrixMotor : decimal(0.0); - } + // Compute the inverse of mass matrix K=JM^-1J^t for the motor (1x1 matrix) + mInverseMassMatrixMotor = mBody1->mMassInverse + mBody2->mMassInverse; + mInverseMassMatrixMotor = (mInverseMassMatrixMotor > 0.0) ? + float(1.0) / mInverseMassMatrixMotor : float(0.0); + } - // If warm-starting is not enabled - if (!constraintSolverData.isWarmStartingActive) { + // If warm-starting is not enabled + if (!constraintSolverData.isWarmStartingActive) { - // Reset all the accumulated impulses - mImpulseTranslation.setToZero(); - mImpulseRotation.setToZero(); - mImpulseLowerLimit = 0.0; - mImpulseUpperLimit = 0.0; - mImpulseMotor = 0.0; - } + // Reset all the accumulated impulses + mImpulseTranslation.setToZero(); + mImpulseRotation.setToZero(); + mImpulseLowerLimit = 0.0; + mImpulseUpperLimit = 0.0; + mImpulseMotor = 0.0; + } } // Warm start the constraint (apply the previous impulse at the beginning of the step) void SliderJoint::warmstart(const ConstraintSolverData& constraintSolverData) { - // Get the velocities - Vector3& v1 = constraintSolverData.linearVelocities[mIndexBody1]; - Vector3& v2 = constraintSolverData.linearVelocities[mIndexBody2]; - Vector3& w1 = constraintSolverData.angularVelocities[mIndexBody1]; - Vector3& w2 = constraintSolverData.angularVelocities[mIndexBody2]; + // Get the velocities + Vector3& v1 = constraintSolverData.linearVelocities[mIndexBody1]; + Vector3& v2 = constraintSolverData.linearVelocities[mIndexBody2]; + Vector3& w1 = constraintSolverData.angularVelocities[mIndexBody1]; + Vector3& w2 = constraintSolverData.angularVelocities[mIndexBody2]; - // Get the inverse mass and inverse inertia tensors of the bodies - const decimal inverseMassBody1 = mBody1->mMassInverse; - const decimal inverseMassBody2 = mBody2->mMassInverse; + // Get the inverse mass and inverse inertia tensors of the bodies + const float inverseMassBody1 = mBody1->mMassInverse; + const float inverseMassBody2 = mBody2->mMassInverse; - // Compute the impulse P=J^T * lambda for the lower and upper limits constraints of body 1 - decimal impulseLimits = mImpulseUpperLimit - mImpulseLowerLimit; - Vector3 linearImpulseLimits = impulseLimits * mSliderAxisWorld; + // Compute the impulse P=J^T * lambda for the lower and upper limits constraints of body 1 + float impulseLimits = mImpulseUpperLimit - mImpulseLowerLimit; + Vector3 linearImpulseLimits = impulseLimits * mSliderAxisWorld; - // Compute the impulse P=J^T * lambda for the motor constraint of body 1 - Vector3 impulseMotor = mImpulseMotor * mSliderAxisWorld; + // Compute the impulse P=J^T * lambda for the motor constraint of body 1 + Vector3 impulseMotor = mImpulseMotor * mSliderAxisWorld; - // Compute the impulse P=J^T * lambda for the 2 translation constraints of body 1 - Vector3 linearImpulseBody1 = -mN1 * mImpulseTranslation.x - mN2 * mImpulseTranslation.y; - Vector3 angularImpulseBody1 = -mR1PlusUCrossN1 * mImpulseTranslation.x - - mR1PlusUCrossN2 * mImpulseTranslation.y; + // Compute the impulse P=J^T * lambda for the 2 translation constraints of body 1 + Vector3 linearImpulseBody1 = -mN1 * mImpulseTranslation.x - mN2 * mImpulseTranslation.y; + Vector3 angularImpulseBody1 = -mR1PlusUCrossN1 * mImpulseTranslation.x - + mR1PlusUCrossN2 * mImpulseTranslation.y; - // Compute the impulse P=J^T * lambda for the 3 rotation constraints of body 1 - angularImpulseBody1 += -mImpulseRotation; + // Compute the impulse P=J^T * lambda for the 3 rotation constraints of body 1 + angularImpulseBody1 += -mImpulseRotation; - // Compute the impulse P=J^T * lambda for the lower and upper limits constraints of body 1 - linearImpulseBody1 += linearImpulseLimits; - angularImpulseBody1 += impulseLimits * mR1PlusUCrossSliderAxis; + // Compute the impulse P=J^T * lambda for the lower and upper limits constraints of body 1 + linearImpulseBody1 += linearImpulseLimits; + angularImpulseBody1 += impulseLimits * mR1PlusUCrossSliderAxis; - // Compute the impulse P=J^T * lambda for the motor constraint of body 1 - linearImpulseBody1 += impulseMotor; + // Compute the impulse P=J^T * lambda for the motor constraint of body 1 + linearImpulseBody1 += impulseMotor; - // Apply the impulse to the body 1 - v1 += inverseMassBody1 * linearImpulseBody1; - w1 += mI1 * angularImpulseBody1; + // Apply the impulse to the body 1 + v1 += inverseMassBody1 * linearImpulseBody1; + w1 += mI1 * angularImpulseBody1; - // Compute the impulse P=J^T * lambda for the 2 translation constraints of body 2 - Vector3 linearImpulseBody2 = mN1 * mImpulseTranslation.x + mN2 * mImpulseTranslation.y; - Vector3 angularImpulseBody2 = mR2CrossN1 * mImpulseTranslation.x + - mR2CrossN2 * mImpulseTranslation.y; + // Compute the impulse P=J^T * lambda for the 2 translation constraints of body 2 + Vector3 linearImpulseBody2 = mN1 * mImpulseTranslation.x + mN2 * mImpulseTranslation.y; + Vector3 angularImpulseBody2 = mR2CrossN1 * mImpulseTranslation.x + + mR2CrossN2 * mImpulseTranslation.y; - // Compute the impulse P=J^T * lambda for the 3 rotation constraints of body 2 - angularImpulseBody2 += mImpulseRotation; + // Compute the impulse P=J^T * lambda for the 3 rotation constraints of body 2 + angularImpulseBody2 += mImpulseRotation; - // Compute the impulse P=J^T * lambda for the lower and upper limits constraints of body 2 - linearImpulseBody2 += -linearImpulseLimits; - angularImpulseBody2 += -impulseLimits * mR2CrossSliderAxis; + // Compute the impulse P=J^T * lambda for the lower and upper limits constraints of body 2 + linearImpulseBody2 += -linearImpulseLimits; + angularImpulseBody2 += -impulseLimits * mR2CrossSliderAxis; - // Compute the impulse P=J^T * lambda for the motor constraint of body 2 - linearImpulseBody2 += -impulseMotor; + // Compute the impulse P=J^T * lambda for the motor constraint of body 2 + linearImpulseBody2 += -impulseMotor; - // Apply the impulse to the body 2 - v2 += inverseMassBody2 * linearImpulseBody2; - w2 += mI2 * angularImpulseBody2; + // Apply the impulse to the body 2 + v2 += inverseMassBody2 * linearImpulseBody2; + w2 += mI2 * angularImpulseBody2; } // Solve the velocity constraint void SliderJoint::solveVelocityConstraint(const ConstraintSolverData& constraintSolverData) { - // Get the velocities - Vector3& v1 = constraintSolverData.linearVelocities[mIndexBody1]; - Vector3& v2 = constraintSolverData.linearVelocities[mIndexBody2]; - Vector3& w1 = constraintSolverData.angularVelocities[mIndexBody1]; - Vector3& w2 = constraintSolverData.angularVelocities[mIndexBody2]; + // Get the velocities + Vector3& v1 = constraintSolverData.linearVelocities[mIndexBody1]; + Vector3& v2 = constraintSolverData.linearVelocities[mIndexBody2]; + Vector3& w1 = constraintSolverData.angularVelocities[mIndexBody1]; + Vector3& w2 = constraintSolverData.angularVelocities[mIndexBody2]; - // Get the inverse mass and inverse inertia tensors of the bodies - decimal inverseMassBody1 = mBody1->mMassInverse; - decimal inverseMassBody2 = mBody2->mMassInverse; + // Get the inverse mass and inverse inertia tensors of the bodies + float inverseMassBody1 = mBody1->mMassInverse; + float inverseMassBody2 = mBody2->mMassInverse; - // --------------- Translation Constraints --------------- // + // --------------- Translation Constraints --------------- // - // Compute J*v for the 2 translation constraints - const decimal el1 = -mN1.dot(v1) - w1.dot(mR1PlusUCrossN1) + - mN1.dot(v2) + w2.dot(mR2CrossN1); - const decimal el2 = -mN2.dot(v1) - w1.dot(mR1PlusUCrossN2) + - mN2.dot(v2) + w2.dot(mR2CrossN2); - const Vector2 JvTranslation(el1, el2); + // Compute J*v for the 2 translation constraints + const float el1 = -mN1.dot(v1) - w1.dot(mR1PlusUCrossN1) + + mN1.dot(v2) + w2.dot(mR2CrossN1); + const float el2 = -mN2.dot(v1) - w1.dot(mR1PlusUCrossN2) + + mN2.dot(v2) + w2.dot(mR2CrossN2); + const Vector2 JvTranslation(el1, el2); - // Compute the Lagrange multiplier lambda for the 2 translation constraints - Vector2 deltaLambda = mInverseMassMatrixTranslationConstraint * (-JvTranslation -mBTranslation); - mImpulseTranslation += deltaLambda; + // Compute the Lagrange multiplier lambda for the 2 translation constraints + Vector2 deltaLambda = mInverseMassMatrixTranslationConstraint * (-JvTranslation -mBTranslation); + mImpulseTranslation += deltaLambda; - // Compute the impulse P=J^T * lambda for the 2 translation constraints of body 1 - const Vector3 linearImpulseBody1 = -mN1 * deltaLambda.x - mN2 * deltaLambda.y; - Vector3 angularImpulseBody1 = -mR1PlusUCrossN1 * deltaLambda.x - - mR1PlusUCrossN2 * deltaLambda.y; + // Compute the impulse P=J^T * lambda for the 2 translation constraints of body 1 + const Vector3 linearImpulseBody1 = -mN1 * deltaLambda.x - mN2 * deltaLambda.y; + Vector3 angularImpulseBody1 = -mR1PlusUCrossN1 * deltaLambda.x - + mR1PlusUCrossN2 * deltaLambda.y; - // Apply the impulse to the body 1 - v1 += inverseMassBody1 * linearImpulseBody1; - w1 += mI1 * angularImpulseBody1; + // Apply the impulse to the body 1 + v1 += inverseMassBody1 * linearImpulseBody1; + w1 += mI1 * angularImpulseBody1; - // Compute the impulse P=J^T * lambda for the 2 translation constraints of body 2 - const Vector3 linearImpulseBody2 = mN1 * deltaLambda.x + mN2 * deltaLambda.y; - Vector3 angularImpulseBody2 = mR2CrossN1 * deltaLambda.x + mR2CrossN2 * deltaLambda.y; + // Compute the impulse P=J^T * lambda for the 2 translation constraints of body 2 + const Vector3 linearImpulseBody2 = mN1 * deltaLambda.x + mN2 * deltaLambda.y; + Vector3 angularImpulseBody2 = mR2CrossN1 * deltaLambda.x + mR2CrossN2 * deltaLambda.y; - // Apply the impulse to the body 2 - v2 += inverseMassBody2 * linearImpulseBody2; - w2 += mI2 * angularImpulseBody2; + // Apply the impulse to the body 2 + v2 += inverseMassBody2 * linearImpulseBody2; + w2 += mI2 * angularImpulseBody2; - // --------------- Rotation Constraints --------------- // + // --------------- Rotation Constraints --------------- // - // Compute J*v for the 3 rotation constraints - const Vector3 JvRotation = w2 - w1; + // Compute J*v for the 3 rotation constraints + const Vector3 JvRotation = w2 - w1; - // Compute the Lagrange multiplier lambda for the 3 rotation constraints - Vector3 deltaLambda2 = mInverseMassMatrixRotationConstraint * (-JvRotation - mBRotation); - mImpulseRotation += deltaLambda2; + // Compute the Lagrange multiplier lambda for the 3 rotation constraints + Vector3 deltaLambda2 = mInverseMassMatrixRotationConstraint * (-JvRotation - mBRotation); + mImpulseRotation += deltaLambda2; - // Compute the impulse P=J^T * lambda for the 3 rotation constraints of body 1 - angularImpulseBody1 = -deltaLambda2; + // Compute the impulse P=J^T * lambda for the 3 rotation constraints of body 1 + angularImpulseBody1 = -deltaLambda2; - // Apply the impulse to the body to body 1 - w1 += mI1 * angularImpulseBody1; + // Apply the impulse to the body to body 1 + w1 += mI1 * angularImpulseBody1; - // Compute the impulse P=J^T * lambda for the 3 rotation constraints of body 2 - angularImpulseBody2 = deltaLambda2; + // Compute the impulse P=J^T * lambda for the 3 rotation constraints of body 2 + angularImpulseBody2 = deltaLambda2; - // Apply the impulse to the body 2 - w2 += mI2 * angularImpulseBody2; + // Apply the impulse to the body 2 + w2 += mI2 * angularImpulseBody2; - // --------------- Limits Constraints --------------- // + // --------------- Limits Constraints --------------- // - if (mIsLimitEnabled) { + if (mIsLimitEnabled) { - // If the lower limit is violated - if (mIsLowerLimitViolated) { + // If the lower limit is violated + if (mIsLowerLimitViolated) { - // Compute J*v for the lower limit constraint - const decimal JvLowerLimit = mSliderAxisWorld.dot(v2) + mR2CrossSliderAxis.dot(w2) - - mSliderAxisWorld.dot(v1) - mR1PlusUCrossSliderAxis.dot(w1); + // Compute J*v for the lower limit constraint + const float JvLowerLimit = mSliderAxisWorld.dot(v2) + mR2CrossSliderAxis.dot(w2) - + mSliderAxisWorld.dot(v1) - mR1PlusUCrossSliderAxis.dot(w1); - // Compute the Lagrange multiplier lambda for the lower limit constraint - decimal deltaLambdaLower = mInverseMassMatrixLimit * (-JvLowerLimit -mBLowerLimit); - decimal lambdaTemp = mImpulseLowerLimit; - mImpulseLowerLimit = std::max(mImpulseLowerLimit + deltaLambdaLower, decimal(0.0)); - deltaLambdaLower = mImpulseLowerLimit - lambdaTemp; + // Compute the Lagrange multiplier lambda for the lower limit constraint + float deltaLambdaLower = mInverseMassMatrixLimit * (-JvLowerLimit -mBLowerLimit); + float lambdaTemp = mImpulseLowerLimit; + mImpulseLowerLimit = std::max(mImpulseLowerLimit + deltaLambdaLower, float(0.0)); + deltaLambdaLower = mImpulseLowerLimit - lambdaTemp; - // Compute the impulse P=J^T * lambda for the lower limit constraint of body 1 - const Vector3 linearImpulseBody1 = -deltaLambdaLower * mSliderAxisWorld; - const Vector3 angularImpulseBody1 = -deltaLambdaLower * mR1PlusUCrossSliderAxis; + // Compute the impulse P=J^T * lambda for the lower limit constraint of body 1 + const Vector3 linearImpulseBody1 = -deltaLambdaLower * mSliderAxisWorld; + const Vector3 angularImpulseBody1 = -deltaLambdaLower * mR1PlusUCrossSliderAxis; - // Apply the impulse to the body 1 - v1 += inverseMassBody1 * linearImpulseBody1; - w1 += mI1 * angularImpulseBody1; + // Apply the impulse to the body 1 + v1 += inverseMassBody1 * linearImpulseBody1; + w1 += mI1 * angularImpulseBody1; - // Compute the impulse P=J^T * lambda for the lower limit constraint of body 2 - const Vector3 linearImpulseBody2 = deltaLambdaLower * mSliderAxisWorld; - const Vector3 angularImpulseBody2 = deltaLambdaLower * mR2CrossSliderAxis; + // Compute the impulse P=J^T * lambda for the lower limit constraint of body 2 + const Vector3 linearImpulseBody2 = deltaLambdaLower * mSliderAxisWorld; + const Vector3 angularImpulseBody2 = deltaLambdaLower * mR2CrossSliderAxis; - // Apply the impulse to the body 2 - v2 += inverseMassBody2 * linearImpulseBody2; - w2 += mI2 * angularImpulseBody2; - } + // Apply the impulse to the body 2 + v2 += inverseMassBody2 * linearImpulseBody2; + w2 += mI2 * angularImpulseBody2; + } - // If the upper limit is violated - if (mIsUpperLimitViolated) { + // If the upper limit is violated + if (mIsUpperLimitViolated) { - // Compute J*v for the upper limit constraint - const decimal JvUpperLimit = mSliderAxisWorld.dot(v1) + mR1PlusUCrossSliderAxis.dot(w1) - - mSliderAxisWorld.dot(v2) - mR2CrossSliderAxis.dot(w2); + // Compute J*v for the upper limit constraint + const float JvUpperLimit = mSliderAxisWorld.dot(v1) + mR1PlusUCrossSliderAxis.dot(w1) + - mSliderAxisWorld.dot(v2) - mR2CrossSliderAxis.dot(w2); - // Compute the Lagrange multiplier lambda for the upper limit constraint - decimal deltaLambdaUpper = mInverseMassMatrixLimit * (-JvUpperLimit -mBUpperLimit); - decimal lambdaTemp = mImpulseUpperLimit; - mImpulseUpperLimit = std::max(mImpulseUpperLimit + deltaLambdaUpper, decimal(0.0)); - deltaLambdaUpper = mImpulseUpperLimit - lambdaTemp; + // Compute the Lagrange multiplier lambda for the upper limit constraint + float deltaLambdaUpper = mInverseMassMatrixLimit * (-JvUpperLimit -mBUpperLimit); + float lambdaTemp = mImpulseUpperLimit; + mImpulseUpperLimit = std::max(mImpulseUpperLimit + deltaLambdaUpper, float(0.0)); + deltaLambdaUpper = mImpulseUpperLimit - lambdaTemp; - // Compute the impulse P=J^T * lambda for the upper limit constraint of body 1 - const Vector3 linearImpulseBody1 = deltaLambdaUpper * mSliderAxisWorld; - const Vector3 angularImpulseBody1 = deltaLambdaUpper * mR1PlusUCrossSliderAxis; + // Compute the impulse P=J^T * lambda for the upper limit constraint of body 1 + const Vector3 linearImpulseBody1 = deltaLambdaUpper * mSliderAxisWorld; + const Vector3 angularImpulseBody1 = deltaLambdaUpper * mR1PlusUCrossSliderAxis; - // Apply the impulse to the body 1 - v1 += inverseMassBody1 * linearImpulseBody1; - w1 += mI1 * angularImpulseBody1; + // Apply the impulse to the body 1 + v1 += inverseMassBody1 * linearImpulseBody1; + w1 += mI1 * angularImpulseBody1; - // Compute the impulse P=J^T * lambda for the upper limit constraint of body 2 - const Vector3 linearImpulseBody2 = -deltaLambdaUpper * mSliderAxisWorld; - const Vector3 angularImpulseBody2 = -deltaLambdaUpper * mR2CrossSliderAxis; + // Compute the impulse P=J^T * lambda for the upper limit constraint of body 2 + const Vector3 linearImpulseBody2 = -deltaLambdaUpper * mSliderAxisWorld; + const Vector3 angularImpulseBody2 = -deltaLambdaUpper * mR2CrossSliderAxis; - // Apply the impulse to the body 2 - v2 += inverseMassBody2 * linearImpulseBody2; - w2 += mI2 * angularImpulseBody2; - } - } + // Apply the impulse to the body 2 + v2 += inverseMassBody2 * linearImpulseBody2; + w2 += mI2 * angularImpulseBody2; + } + } - // --------------- Motor --------------- // + // --------------- Motor --------------- // - if (mIsMotorEnabled) { + if (mIsMotorEnabled) { - // Compute J*v for the motor - const decimal JvMotor = mSliderAxisWorld.dot(v1) - mSliderAxisWorld.dot(v2); + // Compute J*v for the motor + const float JvMotor = mSliderAxisWorld.dot(v1) - mSliderAxisWorld.dot(v2); - // Compute the Lagrange multiplier lambda for the motor - const decimal maxMotorImpulse = mMaxMotorForce * constraintSolverData.timeStep; - decimal deltaLambdaMotor = mInverseMassMatrixMotor * (-JvMotor - mMotorSpeed); - decimal lambdaTemp = mImpulseMotor; - mImpulseMotor = clamp(mImpulseMotor + deltaLambdaMotor, -maxMotorImpulse, maxMotorImpulse); - deltaLambdaMotor = mImpulseMotor - lambdaTemp; + // Compute the Lagrange multiplier lambda for the motor + const float maxMotorImpulse = mMaxMotorForce * constraintSolverData.timeStep; + float deltaLambdaMotor = mInverseMassMatrixMotor * (-JvMotor - mMotorSpeed); + float lambdaTemp = mImpulseMotor; + mImpulseMotor = clamp(mImpulseMotor + deltaLambdaMotor, -maxMotorImpulse, maxMotorImpulse); + deltaLambdaMotor = mImpulseMotor - lambdaTemp; - // Compute the impulse P=J^T * lambda for the motor of body 1 - const Vector3 linearImpulseBody1 = deltaLambdaMotor * mSliderAxisWorld; + // Compute the impulse P=J^T * lambda for the motor of body 1 + const Vector3 linearImpulseBody1 = deltaLambdaMotor * mSliderAxisWorld; - // Apply the impulse to the body 1 - v1 += inverseMassBody1 * linearImpulseBody1; + // Apply the impulse to the body 1 + v1 += inverseMassBody1 * linearImpulseBody1; - // Compute the impulse P=J^T * lambda for the motor of body 2 - const Vector3 linearImpulseBody2 = -deltaLambdaMotor * mSliderAxisWorld; + // Compute the impulse P=J^T * lambda for the motor of body 2 + const Vector3 linearImpulseBody2 = -deltaLambdaMotor * mSliderAxisWorld; - // Apply the impulse to the body 2 - v2 += inverseMassBody2 * linearImpulseBody2; - } + // Apply the impulse to the body 2 + v2 += inverseMassBody2 * linearImpulseBody2; + } } // Solve the position constraint (for position error correction) void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraintSolverData) { - // If the error position correction technique is not the non-linear-gauss-seidel, we do - // do not execute this method - if (mPositionCorrectionTechnique != NON_LINEAR_GAUSS_SEIDEL) return; + // If the error position correction technique is not the non-linear-gauss-seidel, we do + // do not execute this method + if (mPositionCorrectionTechnique != NON_LINEAR_GAUSS_SEIDEL) return; - // Get the bodies positions and orientations - Vector3& x1 = constraintSolverData.positions[mIndexBody1]; - Vector3& x2 = constraintSolverData.positions[mIndexBody2]; - Quaternion& q1 = constraintSolverData.orientations[mIndexBody1]; - Quaternion& q2 = constraintSolverData.orientations[mIndexBody2]; + // Get the bodies positions and orientations + Vector3& x1 = constraintSolverData.positions[mIndexBody1]; + Vector3& x2 = constraintSolverData.positions[mIndexBody2]; + Quaternion& q1 = constraintSolverData.orientations[mIndexBody1]; + Quaternion& q2 = constraintSolverData.orientations[mIndexBody2]; - // Get the inverse mass and inverse inertia tensors of the bodies - decimal inverseMassBody1 = mBody1->mMassInverse; - decimal inverseMassBody2 = mBody2->mMassInverse; + // Get the inverse mass and inverse inertia tensors of the bodies + float inverseMassBody1 = mBody1->mMassInverse; + float inverseMassBody2 = mBody2->mMassInverse; - // Recompute the inertia tensor of bodies - mI1 = mBody1->getInertiaTensorInverseWorld(); - mI2 = mBody2->getInertiaTensorInverseWorld(); + // Recompute the inertia tensor of bodies + mI1 = mBody1->getInertiaTensorInverseWorld(); + mI2 = mBody2->getInertiaTensorInverseWorld(); - // Vector from body center to the anchor point - mR1 = q1 * mLocalAnchorPointBody1; - mR2 = q2 * mLocalAnchorPointBody2; + // Vector from body center to the anchor point + mR1 = q1 * mLocalAnchorPointBody1; + mR2 = q2 * mLocalAnchorPointBody2; - // Compute the vector u (difference between anchor points) - const Vector3 u = x2 + mR2 - x1 - mR1; + // Compute the vector u (difference between anchor points) + const Vector3 u = x2 + mR2 - x1 - mR1; - // Compute the two orthogonal vectors to the slider axis in world-space - mSliderAxisWorld = q1 * mSliderAxisBody1; - mSliderAxisWorld.normalize(); - mN1 = mSliderAxisWorld.getOneUnitOrthogonalVector(); - mN2 = mSliderAxisWorld.cross(mN1); + // Compute the two orthogonal vectors to the slider axis in world-space + mSliderAxisWorld = q1 * mSliderAxisBody1; + mSliderAxisWorld.normalize(); + mN1 = mSliderAxisWorld.getOneUnitOrthogonalVector(); + mN2 = mSliderAxisWorld.cross(mN1); - // Check if the limit constraints are violated or not - decimal uDotSliderAxis = u.dot(mSliderAxisWorld); - decimal lowerLimitError = uDotSliderAxis - mLowerLimit; - decimal upperLimitError = mUpperLimit - uDotSliderAxis; - mIsLowerLimitViolated = lowerLimitError <= 0; - mIsUpperLimitViolated = upperLimitError <= 0; + // Check if the limit constraints are violated or not + float uDotSliderAxis = u.dot(mSliderAxisWorld); + float lowerLimitError = uDotSliderAxis - mLowerLimit; + float upperLimitError = mUpperLimit - uDotSliderAxis; + mIsLowerLimitViolated = lowerLimitError <= 0; + mIsUpperLimitViolated = upperLimitError <= 0; - // Compute the cross products used in the Jacobians - mR2CrossN1 = mR2.cross(mN1); - mR2CrossN2 = mR2.cross(mN2); - mR2CrossSliderAxis = mR2.cross(mSliderAxisWorld); - const Vector3 r1PlusU = mR1 + u; - mR1PlusUCrossN1 = (r1PlusU).cross(mN1); - mR1PlusUCrossN2 = (r1PlusU).cross(mN2); - mR1PlusUCrossSliderAxis = (r1PlusU).cross(mSliderAxisWorld); + // Compute the cross products used in the Jacobians + mR2CrossN1 = mR2.cross(mN1); + mR2CrossN2 = mR2.cross(mN2); + mR2CrossSliderAxis = mR2.cross(mSliderAxisWorld); + const Vector3 r1PlusU = mR1 + u; + mR1PlusUCrossN1 = (r1PlusU).cross(mN1); + mR1PlusUCrossN2 = (r1PlusU).cross(mN2); + mR1PlusUCrossSliderAxis = (r1PlusU).cross(mSliderAxisWorld); - // --------------- Translation Constraints --------------- // + // --------------- Translation Constraints --------------- // - // Recompute the inverse of the mass matrix K=JM^-1J^t for the 2 translation - // constraints (2x2 matrix) - decimal sumInverseMass = mBody1->mMassInverse + mBody2->mMassInverse; - Vector3 I1R1PlusUCrossN1 = mI1 * mR1PlusUCrossN1; - Vector3 I1R1PlusUCrossN2 = mI1 * mR1PlusUCrossN2; - Vector3 I2R2CrossN1 = mI2 * mR2CrossN1; - Vector3 I2R2CrossN2 = mI2 * mR2CrossN2; - const decimal el11 = sumInverseMass + mR1PlusUCrossN1.dot(I1R1PlusUCrossN1) + - mR2CrossN1.dot(I2R2CrossN1); - const decimal el12 = mR1PlusUCrossN1.dot(I1R1PlusUCrossN2) + - mR2CrossN1.dot(I2R2CrossN2); - const decimal el21 = mR1PlusUCrossN2.dot(I1R1PlusUCrossN1) + - mR2CrossN2.dot(I2R2CrossN1); - const decimal el22 = sumInverseMass + mR1PlusUCrossN2.dot(I1R1PlusUCrossN2) + - mR2CrossN2.dot(I2R2CrossN2); - Matrix2x2 matrixKTranslation(el11, el12, el21, el22); - mInverseMassMatrixTranslationConstraint.setToZero(); - if (mBody1->getType() == DYNAMIC || mBody2->getType() == DYNAMIC) { - mInverseMassMatrixTranslationConstraint = matrixKTranslation.getInverse(); - } + // Recompute the inverse of the mass matrix K=JM^-1J^t for the 2 translation + // constraints (2x2 matrix) + float sumInverseMass = mBody1->mMassInverse + mBody2->mMassInverse; + Vector3 I1R1PlusUCrossN1 = mI1 * mR1PlusUCrossN1; + Vector3 I1R1PlusUCrossN2 = mI1 * mR1PlusUCrossN2; + Vector3 I2R2CrossN1 = mI2 * mR2CrossN1; + Vector3 I2R2CrossN2 = mI2 * mR2CrossN2; + const float el11 = sumInverseMass + mR1PlusUCrossN1.dot(I1R1PlusUCrossN1) + + mR2CrossN1.dot(I2R2CrossN1); + const float el12 = mR1PlusUCrossN1.dot(I1R1PlusUCrossN2) + + mR2CrossN1.dot(I2R2CrossN2); + const float el21 = mR1PlusUCrossN2.dot(I1R1PlusUCrossN1) + + mR2CrossN2.dot(I2R2CrossN1); + const float el22 = sumInverseMass + mR1PlusUCrossN2.dot(I1R1PlusUCrossN2) + + mR2CrossN2.dot(I2R2CrossN2); + Matrix2x2 matrixKTranslation(el11, el12, el21, el22); + mInverseMassMatrixTranslationConstraint.setToZero(); + if (mBody1->getType() == DYNAMIC || mBody2->getType() == DYNAMIC) { + mInverseMassMatrixTranslationConstraint = matrixKTranslation.getInverse(); + } - // Compute the position error for the 2 translation constraints - const Vector2 translationError(u.dot(mN1), u.dot(mN2)); + // Compute the position error for the 2 translation constraints + const Vector2 translationError(u.dot(mN1), u.dot(mN2)); - // Compute the Lagrange multiplier lambda for the 2 translation constraints - Vector2 lambdaTranslation = mInverseMassMatrixTranslationConstraint * (-translationError); + // Compute the Lagrange multiplier lambda for the 2 translation constraints + Vector2 lambdaTranslation = mInverseMassMatrixTranslationConstraint * (-translationError); - // Compute the impulse P=J^T * lambda for the 2 translation constraints of body 1 - const Vector3 linearImpulseBody1 = -mN1 * lambdaTranslation.x - mN2 * lambdaTranslation.y; - Vector3 angularImpulseBody1 = -mR1PlusUCrossN1 * lambdaTranslation.x - - mR1PlusUCrossN2 * lambdaTranslation.y; + // Compute the impulse P=J^T * lambda for the 2 translation constraints of body 1 + const Vector3 linearImpulseBody1 = -mN1 * lambdaTranslation.x - mN2 * lambdaTranslation.y; + Vector3 angularImpulseBody1 = -mR1PlusUCrossN1 * lambdaTranslation.x - + mR1PlusUCrossN2 * lambdaTranslation.y; - // Apply the impulse to the body 1 - const Vector3 v1 = inverseMassBody1 * linearImpulseBody1; - Vector3 w1 = mI1 * angularImpulseBody1; + // Apply the impulse to the body 1 + const Vector3 v1 = inverseMassBody1 * linearImpulseBody1; + Vector3 w1 = mI1 * angularImpulseBody1; - // Update the body position/orientation of body 1 - x1 += v1; - q1 += Quaternion(0, w1) * q1 * decimal(0.5); - q1.normalize(); + // Update the body position/orientation of body 1 + x1 += v1; + q1 += Quaternion(0, w1) * q1 * float(0.5); + q1.normalize(); - // Compute the impulse P=J^T * lambda for the 2 translation constraints of body 2 - const Vector3 linearImpulseBody2 = mN1 * lambdaTranslation.x + mN2 * lambdaTranslation.y; - Vector3 angularImpulseBody2 = mR2CrossN1 * lambdaTranslation.x + - mR2CrossN2 * lambdaTranslation.y; + // Compute the impulse P=J^T * lambda for the 2 translation constraints of body 2 + const Vector3 linearImpulseBody2 = mN1 * lambdaTranslation.x + mN2 * lambdaTranslation.y; + Vector3 angularImpulseBody2 = mR2CrossN1 * lambdaTranslation.x + + mR2CrossN2 * lambdaTranslation.y; - // Apply the impulse to the body 2 - const Vector3 v2 = inverseMassBody2 * linearImpulseBody2; - Vector3 w2 = mI2 * angularImpulseBody2; + // Apply the impulse to the body 2 + const Vector3 v2 = inverseMassBody2 * linearImpulseBody2; + Vector3 w2 = mI2 * angularImpulseBody2; - // Update the body position/orientation of body 2 - x2 += v2; - q2 += Quaternion(0, w2) * q2 * decimal(0.5); - q2.normalize(); + // Update the body position/orientation of body 2 + x2 += v2; + q2 += Quaternion(0, w2) * q2 * float(0.5); + q2.normalize(); - // --------------- Rotation Constraints --------------- // + // --------------- Rotation Constraints --------------- // - // Compute the inverse of the mass matrix K=JM^-1J^t for the 3 rotation - // contraints (3x3 matrix) - mInverseMassMatrixRotationConstraint = mI1 + mI2; - if (mBody1->getType() == DYNAMIC || mBody2->getType() == DYNAMIC) { - mInverseMassMatrixRotationConstraint = mInverseMassMatrixRotationConstraint.getInverse(); - } + // Compute the inverse of the mass matrix K=JM^-1J^t for the 3 rotation + // contraints (3x3 matrix) + mInverseMassMatrixRotationConstraint = mI1 + mI2; + if (mBody1->getType() == DYNAMIC || mBody2->getType() == DYNAMIC) { + mInverseMassMatrixRotationConstraint = mInverseMassMatrixRotationConstraint.getInverse(); + } - // Compute the position error for the 3 rotation constraints - Quaternion currentOrientationDifference = q2 * q1.getInverse(); - currentOrientationDifference.normalize(); - const Quaternion qError = currentOrientationDifference * mInitOrientationDifferenceInv; - const Vector3 errorRotation = decimal(2.0) * qError.getVectorV(); + // Compute the position error for the 3 rotation constraints + Quaternion currentOrientationDifference = q2 * q1.getInverse(); + currentOrientationDifference.normalize(); + const Quaternion qError = currentOrientationDifference * mInitOrientationDifferenceInv; + const Vector3 errorRotation = float(2.0) * qError.getVectorV(); - // Compute the Lagrange multiplier lambda for the 3 rotation constraints - Vector3 lambdaRotation = mInverseMassMatrixRotationConstraint * (-errorRotation); + // Compute the Lagrange multiplier lambda for the 3 rotation constraints + Vector3 lambdaRotation = mInverseMassMatrixRotationConstraint * (-errorRotation); - // Compute the impulse P=J^T * lambda for the 3 rotation constraints of body 1 - angularImpulseBody1 = -lambdaRotation; + // Compute the impulse P=J^T * lambda for the 3 rotation constraints of body 1 + angularImpulseBody1 = -lambdaRotation; - // Apply the impulse to the body 1 - w1 = mI1 * angularImpulseBody1; + // Apply the impulse to the body 1 + w1 = mI1 * angularImpulseBody1; - // Update the body position/orientation of body 1 - q1 += Quaternion(0, w1) * q1 * decimal(0.5); - q1.normalize(); + // Update the body position/orientation of body 1 + q1 += Quaternion(0, w1) * q1 * float(0.5); + q1.normalize(); - // Compute the impulse P=J^T * lambda for the 3 rotation constraints of body 2 - angularImpulseBody2 = lambdaRotation; + // Compute the impulse P=J^T * lambda for the 3 rotation constraints of body 2 + angularImpulseBody2 = lambdaRotation; - // Apply the impulse to the body 2 - w2 = mI2 * angularImpulseBody2; + // Apply the impulse to the body 2 + w2 = mI2 * angularImpulseBody2; - // Update the body position/orientation of body 2 - q2 += Quaternion(0, w2) * q2 * decimal(0.5); - q2.normalize(); + // Update the body position/orientation of body 2 + q2 += Quaternion(0, w2) * q2 * float(0.5); + q2.normalize(); - // --------------- Limits Constraints --------------- // + // --------------- Limits Constraints --------------- // - if (mIsLimitEnabled) { + if (mIsLimitEnabled) { - if (mIsLowerLimitViolated || mIsUpperLimitViolated) { + if (mIsLowerLimitViolated || mIsUpperLimitViolated) { - // Compute the inverse of the mass matrix K=JM^-1J^t for the limits (1x1 matrix) - mInverseMassMatrixLimit = mBody1->mMassInverse + mBody2->mMassInverse + - mR1PlusUCrossSliderAxis.dot(mI1 * mR1PlusUCrossSliderAxis) + - mR2CrossSliderAxis.dot(mI2 * mR2CrossSliderAxis); - mInverseMassMatrixLimit = (mInverseMassMatrixLimit > 0.0) ? - decimal(1.0) / mInverseMassMatrixLimit : decimal(0.0); - } + // Compute the inverse of the mass matrix K=JM^-1J^t for the limits (1x1 matrix) + mInverseMassMatrixLimit = mBody1->mMassInverse + mBody2->mMassInverse + + mR1PlusUCrossSliderAxis.dot(mI1 * mR1PlusUCrossSliderAxis) + + mR2CrossSliderAxis.dot(mI2 * mR2CrossSliderAxis); + mInverseMassMatrixLimit = (mInverseMassMatrixLimit > 0.0) ? + float(1.0) / mInverseMassMatrixLimit : float(0.0); + } - // If the lower limit is violated - if (mIsLowerLimitViolated) { + // If the lower limit is violated + if (mIsLowerLimitViolated) { - // Compute the Lagrange multiplier lambda for the lower limit constraint - decimal lambdaLowerLimit = mInverseMassMatrixLimit * (-lowerLimitError); + // Compute the Lagrange multiplier lambda for the lower limit constraint + float lambdaLowerLimit = mInverseMassMatrixLimit * (-lowerLimitError); - // Compute the impulse P=J^T * lambda for the lower limit constraint of body 1 - const Vector3 linearImpulseBody1 = -lambdaLowerLimit * mSliderAxisWorld; - const Vector3 angularImpulseBody1 = -lambdaLowerLimit * mR1PlusUCrossSliderAxis; + // Compute the impulse P=J^T * lambda for the lower limit constraint of body 1 + const Vector3 linearImpulseBody1 = -lambdaLowerLimit * mSliderAxisWorld; + const Vector3 angularImpulseBody1 = -lambdaLowerLimit * mR1PlusUCrossSliderAxis; - // Apply the impulse to the body 1 - const Vector3 v1 = inverseMassBody1 * linearImpulseBody1; - const Vector3 w1 = mI1 * angularImpulseBody1; + // Apply the impulse to the body 1 + const Vector3 v1 = inverseMassBody1 * linearImpulseBody1; + const Vector3 w1 = mI1 * angularImpulseBody1; - // Update the body position/orientation of body 1 - x1 += v1; - q1 += Quaternion(0, w1) * q1 * decimal(0.5); - q1.normalize(); + // Update the body position/orientation of body 1 + x1 += v1; + q1 += Quaternion(0, w1) * q1 * float(0.5); + q1.normalize(); - // Compute the impulse P=J^T * lambda for the lower limit constraint of body 2 - const Vector3 linearImpulseBody2 = lambdaLowerLimit * mSliderAxisWorld; - const Vector3 angularImpulseBody2 = lambdaLowerLimit * mR2CrossSliderAxis; + // Compute the impulse P=J^T * lambda for the lower limit constraint of body 2 + const Vector3 linearImpulseBody2 = lambdaLowerLimit * mSliderAxisWorld; + const Vector3 angularImpulseBody2 = lambdaLowerLimit * mR2CrossSliderAxis; - // Apply the impulse to the body 2 - const Vector3 v2 = inverseMassBody2 * linearImpulseBody2; - const Vector3 w2 = mI2 * angularImpulseBody2; + // Apply the impulse to the body 2 + const Vector3 v2 = inverseMassBody2 * linearImpulseBody2; + const Vector3 w2 = mI2 * angularImpulseBody2; - // Update the body position/orientation of body 2 - x2 += v2; - q2 += Quaternion(0, w2) * q2 * decimal(0.5); - q2.normalize(); - } + // Update the body position/orientation of body 2 + x2 += v2; + q2 += Quaternion(0, w2) * q2 * float(0.5); + q2.normalize(); + } - // If the upper limit is violated - if (mIsUpperLimitViolated) { + // If the upper limit is violated + if (mIsUpperLimitViolated) { - // Compute the Lagrange multiplier lambda for the upper limit constraint - decimal lambdaUpperLimit = mInverseMassMatrixLimit * (-upperLimitError); + // Compute the Lagrange multiplier lambda for the upper limit constraint + float lambdaUpperLimit = mInverseMassMatrixLimit * (-upperLimitError); - // Compute the impulse P=J^T * lambda for the upper limit constraint of body 1 - const Vector3 linearImpulseBody1 = lambdaUpperLimit * mSliderAxisWorld; - const Vector3 angularImpulseBody1 = lambdaUpperLimit * mR1PlusUCrossSliderAxis; + // Compute the impulse P=J^T * lambda for the upper limit constraint of body 1 + const Vector3 linearImpulseBody1 = lambdaUpperLimit * mSliderAxisWorld; + const Vector3 angularImpulseBody1 = lambdaUpperLimit * mR1PlusUCrossSliderAxis; - // Apply the impulse to the body 1 - const Vector3 v1 = inverseMassBody1 * linearImpulseBody1; - const Vector3 w1 = mI1 * angularImpulseBody1; + // Apply the impulse to the body 1 + const Vector3 v1 = inverseMassBody1 * linearImpulseBody1; + const Vector3 w1 = mI1 * angularImpulseBody1; - // Update the body position/orientation of body 1 - x1 += v1; - q1 += Quaternion(0, w1) * q1 * decimal(0.5); - q1.normalize(); + // Update the body position/orientation of body 1 + x1 += v1; + q1 += Quaternion(0, w1) * q1 * float(0.5); + q1.normalize(); - // Compute the impulse P=J^T * lambda for the upper limit constraint of body 2 - const Vector3 linearImpulseBody2 = -lambdaUpperLimit * mSliderAxisWorld; - const Vector3 angularImpulseBody2 = -lambdaUpperLimit * mR2CrossSliderAxis; + // Compute the impulse P=J^T * lambda for the upper limit constraint of body 2 + const Vector3 linearImpulseBody2 = -lambdaUpperLimit * mSliderAxisWorld; + const Vector3 angularImpulseBody2 = -lambdaUpperLimit * mR2CrossSliderAxis; - // Apply the impulse to the body 2 - const Vector3 v2 = inverseMassBody2 * linearImpulseBody2; - const Vector3 w2 = mI2 * angularImpulseBody2; + // Apply the impulse to the body 2 + const Vector3 v2 = inverseMassBody2 * linearImpulseBody2; + const Vector3 w2 = mI2 * angularImpulseBody2; - // Update the body position/orientation of body 2 - x2 += v2; - q2 += Quaternion(0, w2) * q2 * decimal(0.5); - q2.normalize(); - } - } + // Update the body position/orientation of body 2 + x2 += v2; + q2 += Quaternion(0, w2) * q2 * float(0.5); + q2.normalize(); + } + } } // Enable/Disable the limits of the joint /** * @param isLimitEnabled True if you want to enable the joint limits and false - * otherwise + * otherwise */ void SliderJoint::enableLimit(bool isLimitEnabled) { - if (isLimitEnabled != mIsLimitEnabled) { + if (isLimitEnabled != mIsLimitEnabled) { - mIsLimitEnabled = isLimitEnabled; + mIsLimitEnabled = isLimitEnabled; - // Reset the limits - resetLimits(); - } + // Reset the limits + resetLimits(); + } } // Enable/Disable the motor of the joint /** * @param isMotorEnabled True if you want to enable the joint motor and false - * otherwise + * otherwise */ void SliderJoint::enableMotor(bool isMotorEnabled) { - mIsMotorEnabled = isMotorEnabled; - mImpulseMotor = 0.0; + mIsMotorEnabled = isMotorEnabled; + mImpulseMotor = 0.0; - // Wake up the two bodies of the joint - mBody1->setIsSleeping(false); - mBody2->setIsSleeping(false); + // Wake up the two bodies of the joint + mBody1->setIsSleeping(false); + mBody2->setIsSleeping(false); } // Return the current translation value of the joint /** * @return The current translation distance of the joint (in meters) */ -decimal SliderJoint::getTranslation() const { +float SliderJoint::getTranslation() const { - // TODO : Check if we need to compare rigid body position or center of mass here + // TODO : Check if we need to compare rigid body position or center of mass here - // Get the bodies positions and orientations - const Vector3& x1 = mBody1->getTransform().getPosition(); - const Vector3& x2 = mBody2->getTransform().getPosition(); - const Quaternion& q1 = mBody1->getTransform().getOrientation(); - const Quaternion& q2 = mBody2->getTransform().getOrientation(); + // Get the bodies positions and orientations + const Vector3& x1 = mBody1->getTransform().getPosition(); + const Vector3& x2 = mBody2->getTransform().getPosition(); + const Quaternion& q1 = mBody1->getTransform().getOrientation(); + const Quaternion& q2 = mBody2->getTransform().getOrientation(); - // Compute the two anchor points in world-space coordinates - const Vector3 anchorBody1 = x1 + q1 * mLocalAnchorPointBody1; - const Vector3 anchorBody2 = x2 + q2 * mLocalAnchorPointBody2; + // Compute the two anchor points in world-space coordinates + const Vector3 anchorBody1 = x1 + q1 * mLocalAnchorPointBody1; + const Vector3 anchorBody2 = x2 + q2 * mLocalAnchorPointBody2; - // Compute the vector u (difference between anchor points) - const Vector3 u = anchorBody2 - anchorBody1; + // Compute the vector u (difference between anchor points) + const Vector3 u = anchorBody2 - anchorBody1; - // Compute the slider axis in world-space - Vector3 sliderAxisWorld = q1 * mSliderAxisBody1; - sliderAxisWorld.normalize(); + // Compute the slider axis in world-space + Vector3 sliderAxisWorld = q1 * mSliderAxisBody1; + sliderAxisWorld.normalize(); - // Compute and return the translation value - return u.dot(sliderAxisWorld); + // Compute and return the translation value + return u.dot(sliderAxisWorld); } // Set the minimum translation limit /** * @param lowerLimit The minimum translation limit of the joint (in meters) */ -void SliderJoint::setMinTranslationLimit(decimal lowerLimit) { +void SliderJoint::setMinTranslationLimit(float lowerLimit) { - assert(lowerLimit <= mUpperLimit); + assert(lowerLimit <= mUpperLimit); - if (lowerLimit != mLowerLimit) { + if (lowerLimit != mLowerLimit) { - mLowerLimit = lowerLimit; + mLowerLimit = lowerLimit; - // Reset the limits - resetLimits(); - } + // Reset the limits + resetLimits(); + } } // Set the maximum translation limit /** * @param lowerLimit The maximum translation limit of the joint (in meters) */ -void SliderJoint::setMaxTranslationLimit(decimal upperLimit) { +void SliderJoint::setMaxTranslationLimit(float upperLimit) { - assert(mLowerLimit <= upperLimit); + assert(mLowerLimit <= upperLimit); - if (upperLimit != mUpperLimit) { + if (upperLimit != mUpperLimit) { - mUpperLimit = upperLimit; + mUpperLimit = upperLimit; - // Reset the limits - resetLimits(); - } + // Reset the limits + resetLimits(); + } } // Reset the limits void SliderJoint::resetLimits() { - // Reset the accumulated impulses for the limits - mImpulseLowerLimit = 0.0; - mImpulseUpperLimit = 0.0; + // Reset the accumulated impulses for the limits + mImpulseLowerLimit = 0.0; + mImpulseUpperLimit = 0.0; - // Wake up the two bodies of the joint - mBody1->setIsSleeping(false); - mBody2->setIsSleeping(false); + // Wake up the two bodies of the joint + mBody1->setIsSleeping(false); + mBody2->setIsSleeping(false); } // Set the motor speed /** * @param motorSpeed The speed of the joint motor (in meters per second) */ -void SliderJoint::setMotorSpeed(decimal motorSpeed) { +void SliderJoint::setMotorSpeed(float motorSpeed) { - if (motorSpeed != mMotorSpeed) { + if (motorSpeed != mMotorSpeed) { - mMotorSpeed = motorSpeed; + mMotorSpeed = motorSpeed; - // Wake up the two bodies of the joint - mBody1->setIsSleeping(false); - mBody2->setIsSleeping(false); - } + // Wake up the two bodies of the joint + mBody1->setIsSleeping(false); + mBody2->setIsSleeping(false); + } } // Set the maximum motor force /** * @param maxMotorForce The maximum force of the joint motor (in Newton x meters) */ -void SliderJoint::setMaxMotorForce(decimal maxMotorForce) { +void SliderJoint::setMaxMotorForce(float maxMotorForce) { - if (maxMotorForce != mMaxMotorForce) { + if (maxMotorForce != mMaxMotorForce) { - assert(mMaxMotorForce >= 0.0); - mMaxMotorForce = maxMotorForce; + assert(mMaxMotorForce >= 0.0); + mMaxMotorForce = maxMotorForce; - // Wake up the two bodies of the joint - mBody1->setIsSleeping(false); - mBody2->setIsSleeping(false); - } + // Wake up the two bodies of the joint + mBody1->setIsSleeping(false); + mBody2->setIsSleeping(false); + } } diff --git a/ephysics/constraint/SliderJoint.h b/ephysics/constraint/SliderJoint.h index 1177073..813cfe7 100644 --- a/ephysics/constraint/SliderJoint.h +++ b/ephysics/constraint/SliderJoint.h @@ -1,30 +1,9 @@ -/******************************************************************************** -* 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 REACTPHYSICS3D_SLIDER_JOINT_H -#define REACTPHYSICS3D_SLIDER_JOINT_H +/** @file + * @author Daniel Chappuis + * @copyright 2010-2016 Daniel Chappuis + * @license BSD 3 clauses (see license file) + */ +#pragma once // Libraries #include @@ -39,94 +18,94 @@ namespace reactphysics3d { */ struct SliderJointInfo : public JointInfo { - public : + public : - // -------------------- Attributes -------------------- // + // -------------------- Attributes -------------------- // - /// Anchor point (in world-space coordinates) - Vector3 anchorPointWorldSpace; + /// Anchor point (in world-space coordinates) + Vector3 anchorPointWorldSpace; - /// Slider axis (in world-space coordinates) - Vector3 sliderAxisWorldSpace; + /// Slider axis (in world-space coordinates) + Vector3 sliderAxisWorldSpace; - /// True if the slider limits are enabled - bool isLimitEnabled; + /// True if the slider limits are enabled + bool isLimitEnabled; - /// True if the slider motor is enabled - bool isMotorEnabled; + /// True if the slider motor is enabled + bool isMotorEnabled; - /// Mininum allowed translation if limits are enabled - decimal minTranslationLimit; + /// Mininum allowed translation if limits are enabled + float minTranslationLimit; - /// Maximum allowed translation if limits are enabled - decimal maxTranslationLimit; + /// Maximum allowed translation if limits are enabled + float maxTranslationLimit; - /// Motor speed - decimal motorSpeed; + /// Motor speed + float motorSpeed; - /// Maximum motor force (in Newtons) that can be applied to reach to desired motor speed - decimal maxMotorForce; + /// Maximum motor force (in Newtons) that can be applied to reach to desired motor speed + float maxMotorForce; - /// Constructor without limits and without motor - /** - * @param rigidBody1 The first body of the joint - * @param rigidBody2 The second body of the joint - * @param initAnchorPointWorldSpace The initial anchor point in world-space - * @param initSliderAxisWorldSpace The initial slider axis in world-space - */ - SliderJointInfo(RigidBody* rigidBody1, RigidBody* rigidBody2, - const Vector3& initAnchorPointWorldSpace, - const Vector3& initSliderAxisWorldSpace) - : JointInfo(rigidBody1, rigidBody2, SLIDERJOINT), - anchorPointWorldSpace(initAnchorPointWorldSpace), - sliderAxisWorldSpace(initSliderAxisWorldSpace), - isLimitEnabled(false), isMotorEnabled(false), minTranslationLimit(-1.0), - maxTranslationLimit(1.0), motorSpeed(0), maxMotorForce(0) {} + /// Constructor without limits and without motor + /** + * @param rigidBody1 The first body of the joint + * @param rigidBody2 The second body of the joint + * @param initAnchorPointWorldSpace The initial anchor point in world-space + * @param initSliderAxisWorldSpace The initial slider axis in world-space + */ + SliderJointInfo(RigidBody* rigidBody1, RigidBody* rigidBody2, + const Vector3& initAnchorPointWorldSpace, + const Vector3& initSliderAxisWorldSpace) + : JointInfo(rigidBody1, rigidBody2, SLIDERJOINT), + anchorPointWorldSpace(initAnchorPointWorldSpace), + sliderAxisWorldSpace(initSliderAxisWorldSpace), + isLimitEnabled(false), isMotorEnabled(false), minTranslationLimit(-1.0), + maxTranslationLimit(1.0), motorSpeed(0), maxMotorForce(0) {} - /// Constructor with limits and no motor - /** - * @param rigidBody1 The first body of the joint - * @param rigidBody2 The second body of the joint - * @param initAnchorPointWorldSpace The initial anchor point in world-space - * @param initSliderAxisWorldSpace The initial slider axis in world-space - * @param initMinTranslationLimit The initial minimum translation limit (in meters) - * @param initMaxTranslationLimit The initial maximum translation limit (in meters) - */ - SliderJointInfo(RigidBody* rigidBody1, RigidBody* rigidBody2, - const Vector3& initAnchorPointWorldSpace, - const Vector3& initSliderAxisWorldSpace, - decimal initMinTranslationLimit, decimal initMaxTranslationLimit) - : JointInfo(rigidBody1, rigidBody2, SLIDERJOINT), - anchorPointWorldSpace(initAnchorPointWorldSpace), - sliderAxisWorldSpace(initSliderAxisWorldSpace), - isLimitEnabled(true), isMotorEnabled(false), - minTranslationLimit(initMinTranslationLimit), - maxTranslationLimit(initMaxTranslationLimit), motorSpeed(0), - maxMotorForce(0) {} + /// Constructor with limits and no motor + /** + * @param rigidBody1 The first body of the joint + * @param rigidBody2 The second body of the joint + * @param initAnchorPointWorldSpace The initial anchor point in world-space + * @param initSliderAxisWorldSpace The initial slider axis in world-space + * @param initMinTranslationLimit The initial minimum translation limit (in meters) + * @param initMaxTranslationLimit The initial maximum translation limit (in meters) + */ + SliderJointInfo(RigidBody* rigidBody1, RigidBody* rigidBody2, + const Vector3& initAnchorPointWorldSpace, + const Vector3& initSliderAxisWorldSpace, + float initMinTranslationLimit, float initMaxTranslationLimit) + : JointInfo(rigidBody1, rigidBody2, SLIDERJOINT), + anchorPointWorldSpace(initAnchorPointWorldSpace), + sliderAxisWorldSpace(initSliderAxisWorldSpace), + isLimitEnabled(true), isMotorEnabled(false), + minTranslationLimit(initMinTranslationLimit), + maxTranslationLimit(initMaxTranslationLimit), motorSpeed(0), + maxMotorForce(0) {} - /// Constructor with limits and motor - /** - * @param rigidBody1 The first body of the joint - * @param rigidBody2 The second body of the joint - * @param initAnchorPointWorldSpace The initial anchor point in world-space - * @param initSliderAxisWorldSpace The initial slider axis in world-space - * @param initMinTranslationLimit The initial minimum translation limit (in meters) - * @param initMaxTranslationLimit The initial maximum translation limit (in meters) - * @param initMotorSpeed The initial speed of the joint motor (in meters per second) - * @param initMaxMotorForce The initial maximum motor force of the joint (in Newtons x meters) - */ - SliderJointInfo(RigidBody* rigidBody1, RigidBody* rigidBody2, - const Vector3& initAnchorPointWorldSpace, - const Vector3& initSliderAxisWorldSpace, - decimal initMinTranslationLimit, decimal initMaxTranslationLimit, - decimal initMotorSpeed, decimal initMaxMotorForce) - : JointInfo(rigidBody1, rigidBody2, SLIDERJOINT), - anchorPointWorldSpace(initAnchorPointWorldSpace), - sliderAxisWorldSpace(initSliderAxisWorldSpace), - isLimitEnabled(true), isMotorEnabled(true), - minTranslationLimit(initMinTranslationLimit), - maxTranslationLimit(initMaxTranslationLimit), motorSpeed(initMotorSpeed), - maxMotorForce(initMaxMotorForce) {} + /// Constructor with limits and motor + /** + * @param rigidBody1 The first body of the joint + * @param rigidBody2 The second body of the joint + * @param initAnchorPointWorldSpace The initial anchor point in world-space + * @param initSliderAxisWorldSpace The initial slider axis in world-space + * @param initMinTranslationLimit The initial minimum translation limit (in meters) + * @param initMaxTranslationLimit The initial maximum translation limit (in meters) + * @param initMotorSpeed The initial speed of the joint motor (in meters per second) + * @param initMaxMotorForce The initial maximum motor force of the joint (in Newtons x meters) + */ + SliderJointInfo(RigidBody* rigidBody1, RigidBody* rigidBody2, + const Vector3& initAnchorPointWorldSpace, + const Vector3& initSliderAxisWorldSpace, + float initMinTranslationLimit, float initMaxTranslationLimit, + float initMotorSpeed, float initMaxMotorForce) + : JointInfo(rigidBody1, rigidBody2, SLIDERJOINT), + anchorPointWorldSpace(initAnchorPointWorldSpace), + sliderAxisWorldSpace(initSliderAxisWorldSpace), + isLimitEnabled(true), isMotorEnabled(true), + minTranslationLimit(initMinTranslationLimit), + maxTranslationLimit(initMaxTranslationLimit), motorSpeed(initMotorSpeed), + maxMotorForce(initMaxMotorForce) {} }; // Class SliderJoint @@ -137,206 +116,206 @@ struct SliderJointInfo : public JointInfo { */ class SliderJoint : public Joint { - private : + private : - // -------------------- Constants -------------------- // + // -------------------- Constants -------------------- // - // Beta value for the position correction bias factor - static const decimal BETA; + // Beta value for the position correction bias factor + static const float BETA; - // -------------------- Attributes -------------------- // + // -------------------- Attributes -------------------- // - /// Anchor point of body 1 (in local-space coordinates of body 1) - Vector3 mLocalAnchorPointBody1; + /// Anchor point of body 1 (in local-space coordinates of body 1) + Vector3 mLocalAnchorPointBody1; - /// Anchor point of body 2 (in local-space coordinates of body 2) - Vector3 mLocalAnchorPointBody2; + /// Anchor point of body 2 (in local-space coordinates of body 2) + Vector3 mLocalAnchorPointBody2; - /// Slider axis (in local-space coordinates of body 1) - Vector3 mSliderAxisBody1; + /// Slider axis (in local-space coordinates of body 1) + Vector3 mSliderAxisBody1; - /// Inertia tensor of body 1 (in world-space coordinates) - Matrix3x3 mI1; + /// Inertia tensor of body 1 (in world-space coordinates) + Matrix3x3 mI1; - /// Inertia tensor of body 2 (in world-space coordinates) - Matrix3x3 mI2; + /// Inertia tensor of body 2 (in world-space coordinates) + Matrix3x3 mI2; - /// Inverse of the initial orientation difference between the two bodies - Quaternion mInitOrientationDifferenceInv; + /// Inverse of the initial orientation difference between the two bodies + Quaternion mInitOrientationDifferenceInv; - /// First vector orthogonal to the slider axis local-space of body 1 - Vector3 mN1; + /// First vector orthogonal to the slider axis local-space of body 1 + Vector3 mN1; - /// Second vector orthogonal to the slider axis and mN1 in local-space of body 1 - Vector3 mN2; + /// Second vector orthogonal to the slider axis and mN1 in local-space of body 1 + Vector3 mN2; - /// Vector r1 in world-space coordinates - Vector3 mR1; + /// Vector r1 in world-space coordinates + Vector3 mR1; - /// Vector r2 in world-space coordinates - Vector3 mR2; + /// Vector r2 in world-space coordinates + Vector3 mR2; - /// Cross product of r2 and n1 - Vector3 mR2CrossN1; + /// Cross product of r2 and n1 + Vector3 mR2CrossN1; - /// Cross product of r2 and n2 - Vector3 mR2CrossN2; + /// Cross product of r2 and n2 + Vector3 mR2CrossN2; - /// Cross product of r2 and the slider axis - Vector3 mR2CrossSliderAxis; + /// Cross product of r2 and the slider axis + Vector3 mR2CrossSliderAxis; - /// Cross product of vector (r1 + u) and n1 - Vector3 mR1PlusUCrossN1; + /// Cross product of vector (r1 + u) and n1 + Vector3 mR1PlusUCrossN1; - /// Cross product of vector (r1 + u) and n2 - Vector3 mR1PlusUCrossN2; + /// Cross product of vector (r1 + u) and n2 + Vector3 mR1PlusUCrossN2; - /// Cross product of vector (r1 + u) and the slider axis - Vector3 mR1PlusUCrossSliderAxis; + /// Cross product of vector (r1 + u) and the slider axis + Vector3 mR1PlusUCrossSliderAxis; - /// Bias of the 2 translation constraints - Vector2 mBTranslation; + /// Bias of the 2 translation constraints + Vector2 mBTranslation; - /// Bias of the 3 rotation constraints - Vector3 mBRotation; + /// Bias of the 3 rotation constraints + Vector3 mBRotation; - /// Bias of the lower limit constraint - decimal mBLowerLimit; + /// Bias of the lower limit constraint + float mBLowerLimit; - /// Bias of the upper limit constraint - decimal mBUpperLimit; + /// Bias of the upper limit constraint + float mBUpperLimit; - /// Inverse of mass matrix K=JM^-1J^t for the translation constraint (2x2 matrix) - Matrix2x2 mInverseMassMatrixTranslationConstraint; + /// Inverse of mass matrix K=JM^-1J^t for the translation constraint (2x2 matrix) + Matrix2x2 mInverseMassMatrixTranslationConstraint; - /// Inverse of mass matrix K=JM^-1J^t for the rotation constraint (3x3 matrix) - Matrix3x3 mInverseMassMatrixRotationConstraint; + /// Inverse of mass matrix K=JM^-1J^t for the rotation constraint (3x3 matrix) + Matrix3x3 mInverseMassMatrixRotationConstraint; - /// Inverse of mass matrix K=JM^-1J^t for the upper and lower limit constraints (1x1 matrix) - decimal mInverseMassMatrixLimit; + /// Inverse of mass matrix K=JM^-1J^t for the upper and lower limit constraints (1x1 matrix) + float mInverseMassMatrixLimit; - /// Inverse of mass matrix K=JM^-1J^t for the motor - decimal mInverseMassMatrixMotor; + /// Inverse of mass matrix K=JM^-1J^t for the motor + float mInverseMassMatrixMotor; - /// Accumulated impulse for the 2 translation constraints - Vector2 mImpulseTranslation; + /// Accumulated impulse for the 2 translation constraints + Vector2 mImpulseTranslation; - /// Accumulated impulse for the 3 rotation constraints - Vector3 mImpulseRotation; + /// Accumulated impulse for the 3 rotation constraints + Vector3 mImpulseRotation; - /// Accumulated impulse for the lower limit constraint - decimal mImpulseLowerLimit; + /// Accumulated impulse for the lower limit constraint + float mImpulseLowerLimit; - /// Accumulated impulse for the upper limit constraint - decimal mImpulseUpperLimit; + /// Accumulated impulse for the upper limit constraint + float mImpulseUpperLimit; - /// Accumulated impulse for the motor - decimal mImpulseMotor; + /// Accumulated impulse for the motor + float mImpulseMotor; - /// True if the slider limits are enabled - bool mIsLimitEnabled; + /// True if the slider limits are enabled + bool mIsLimitEnabled; - /// True if the motor of the joint in enabled - bool mIsMotorEnabled; + /// True if the motor of the joint in enabled + bool mIsMotorEnabled; - /// Slider axis in world-space coordinates - Vector3 mSliderAxisWorld; + /// Slider axis in world-space coordinates + Vector3 mSliderAxisWorld; - /// Lower limit (minimum translation distance) - decimal mLowerLimit; + /// Lower limit (minimum translation distance) + float mLowerLimit; - /// Upper limit (maximum translation distance) - decimal mUpperLimit; + /// Upper limit (maximum translation distance) + float mUpperLimit; - /// True if the lower limit is violated - bool mIsLowerLimitViolated; + /// True if the lower limit is violated + bool mIsLowerLimitViolated; - /// True if the upper limit is violated - bool mIsUpperLimitViolated; + /// True if the upper limit is violated + bool mIsUpperLimitViolated; - /// Motor speed (in m/s) - decimal mMotorSpeed; + /// Motor speed (in m/s) + float mMotorSpeed; - /// Maximum motor force (in Newtons) that can be applied to reach to desired motor speed - decimal mMaxMotorForce; + /// Maximum motor force (in Newtons) that can be applied to reach to desired motor speed + float mMaxMotorForce; - // -------------------- Methods -------------------- // + // -------------------- Methods -------------------- // - /// Private copy-constructor - SliderJoint(const SliderJoint& constraint); + /// Private copy-constructor + SliderJoint(const SliderJoint& constraint); - /// Private assignment operator - SliderJoint& operator=(const SliderJoint& constraint); + /// Private assignment operator + SliderJoint& operator=(const SliderJoint& constraint); - /// Reset the limits - void resetLimits(); + /// Reset the limits + void resetLimits(); - /// Return the number of bytes used by the joint - virtual size_t getSizeInBytes() const; + /// Return the number of bytes used by the joint + virtual size_t getSizeInBytes() const; - /// Initialize before solving the constraint - virtual void initBeforeSolve(const ConstraintSolverData& constraintSolverData); + /// Initialize before solving the constraint + virtual void initBeforeSolve(const ConstraintSolverData& constraintSolverData); - /// Warm start the constraint (apply the previous impulse at the beginning of the step) - virtual void warmstart(const ConstraintSolverData& constraintSolverData); + /// Warm start the constraint (apply the previous impulse at the beginning of the step) + virtual void warmstart(const ConstraintSolverData& constraintSolverData); - /// Solve the velocity constraint - virtual void solveVelocityConstraint(const ConstraintSolverData& constraintSolverData); + /// Solve the velocity constraint + virtual void solveVelocityConstraint(const ConstraintSolverData& constraintSolverData); - /// Solve the position constraint (for position error correction) - virtual void solvePositionConstraint(const ConstraintSolverData& constraintSolverData); + /// Solve the position constraint (for position error correction) + virtual void solvePositionConstraint(const ConstraintSolverData& constraintSolverData); - public : + public : - // -------------------- Methods -------------------- // + // -------------------- Methods -------------------- // - /// Constructor - SliderJoint(const SliderJointInfo& jointInfo); + /// Constructor + SliderJoint(const SliderJointInfo& jointInfo); - /// Destructor - virtual ~SliderJoint(); + /// Destructor + virtual ~SliderJoint(); - /// Return true if the limits or the joint are enabled - bool isLimitEnabled() const; + /// Return true if the limits or the joint are enabled + bool isLimitEnabled() const; - /// Return true if the motor of the joint is enabled - bool isMotorEnabled() const; + /// Return true if the motor of the joint is enabled + bool isMotorEnabled() const; - /// Enable/Disable the limits of the joint - void enableLimit(bool isLimitEnabled); + /// Enable/Disable the limits of the joint + void enableLimit(bool isLimitEnabled); - /// Enable/Disable the motor of the joint - void enableMotor(bool isMotorEnabled); + /// Enable/Disable the motor of the joint + void enableMotor(bool isMotorEnabled); - /// Return the current translation value of the joint - decimal getTranslation() const; + /// Return the current translation value of the joint + float getTranslation() const; - /// Return the minimum translation limit - decimal getMinTranslationLimit() const; + /// Return the minimum translation limit + float getMinTranslationLimit() const; - /// Set the minimum translation limit - void setMinTranslationLimit(decimal lowerLimit); + /// Set the minimum translation limit + void setMinTranslationLimit(float lowerLimit); - /// Return the maximum translation limit - decimal getMaxTranslationLimit() const; + /// Return the maximum translation limit + float getMaxTranslationLimit() const; - /// Set the maximum translation limit - void setMaxTranslationLimit(decimal upperLimit); + /// Set the maximum translation limit + void setMaxTranslationLimit(float upperLimit); - /// Return the motor speed - decimal getMotorSpeed() const; + /// Return the motor speed + float getMotorSpeed() const; - /// Set the motor speed - void setMotorSpeed(decimal motorSpeed); + /// Set the motor speed + void setMotorSpeed(float motorSpeed); - /// Return the maximum motor force - decimal getMaxMotorForce() const; + /// Return the maximum motor force + float getMaxMotorForce() const; - /// Set the maximum motor force - void setMaxMotorForce(decimal maxMotorForce); + /// Set the maximum motor force + void setMaxMotorForce(float maxMotorForce); - /// Return the intensity of the current force applied for the joint motor - decimal getMotorForce(decimal timeStep) const; + /// Return the int32_tensity of the current force applied for the joint motor + float getMotorForce(float timeStep) const; }; // Return true if the limits or the joint are enabled @@ -344,7 +323,7 @@ class SliderJoint : public Joint { * @return True if the joint limits are enabled */ inline bool SliderJoint::isLimitEnabled() const { - return mIsLimitEnabled; + return mIsLimitEnabled; } // Return true if the motor of the joint is enabled @@ -352,55 +331,53 @@ inline bool SliderJoint::isLimitEnabled() const { * @return True if the joint motor is enabled */ inline bool SliderJoint::isMotorEnabled() const { - return mIsMotorEnabled; + return mIsMotorEnabled; } // Return the minimum translation limit /** * @return The minimum translation limit of the joint (in meters) */ -inline decimal SliderJoint::getMinTranslationLimit() const { - return mLowerLimit; +inline float SliderJoint::getMinTranslationLimit() const { + return mLowerLimit; } // Return the maximum translation limit /** * @return The maximum translation limit of the joint (in meters) */ -inline decimal SliderJoint::getMaxTranslationLimit() const { - return mUpperLimit; +inline float SliderJoint::getMaxTranslationLimit() const { + return mUpperLimit; } // Return the motor speed /** * @return The current motor speed of the joint (in meters per second) */ -inline decimal SliderJoint::getMotorSpeed() const { - return mMotorSpeed; +inline float SliderJoint::getMotorSpeed() const { + return mMotorSpeed; } // Return the maximum motor force /** * @return The maximum force of the joint motor (in Newton x meters) */ -inline decimal SliderJoint::getMaxMotorForce() const { - return mMaxMotorForce; +inline float SliderJoint::getMaxMotorForce() const { + return mMaxMotorForce; } -// Return the intensity of the current force applied for the joint motor +// Return the int32_tensity of the current force applied for the joint motor /** * @param timeStep Time step (in seconds) * @return The current force of the joint motor (in Newton x meters) */ -inline decimal SliderJoint::getMotorForce(decimal timeStep) const { - return mImpulseMotor / timeStep; +inline float SliderJoint::getMotorForce(float timeStep) const { + return mImpulseMotor / timeStep; } // Return the number of bytes used by the joint inline size_t SliderJoint::getSizeInBytes() const { - return sizeof(SliderJoint); + return sizeof(SliderJoint); } } - -#endif diff --git a/ephysics/decimal.h b/ephysics/decimal.h index 2d6a2a1..e69de29 100644 --- a/ephysics/decimal.h +++ b/ephysics/decimal.h @@ -1,35 +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 REACTPHYSICS3D_DECIMAL_H -#define REACTPHYSICS3D_DECIMAL_H - -/// ReactPhysiscs3D namespace -namespace reactphysics3d { - typedef float decimal; -} - -#endif - diff --git a/ephysics/engine/CollisionWorld.cpp b/ephysics/engine/CollisionWorld.cpp index 45d7c76..cf942b6 100644 --- a/ephysics/engine/CollisionWorld.cpp +++ b/ephysics/engine/CollisionWorld.cpp @@ -1,27 +1,8 @@ -/******************************************************************************** -* 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. * -* * -********************************************************************************/ +/** @file + * @author Daniel Chappuis + * @copyright 2010-2016 Daniel Chappuis + * @license BSD 3 clauses (see license file) + */ // Libraries #include @@ -33,23 +14,23 @@ using namespace std; // Constructor CollisionWorld::CollisionWorld() - : mCollisionDetection(this, mMemoryAllocator), mCurrentBodyID(0), - mEventListener(NULL) { + : mCollisionDetection(this, mMemoryAllocator), mCurrentBodyID(0), + mEventListener(NULL) { } // Destructor CollisionWorld::~CollisionWorld() { - // Destroy all the collision bodies that have not been removed - std::set::iterator itBodies; - for (itBodies = mBodies.begin(); itBodies != mBodies.end(); ) { - std::set::iterator itToRemove = itBodies; - ++itBodies; - destroyCollisionBody(*itToRemove); - } + // Destroy all the collision bodies that have not been removed + std::set::iterator itBodies; + for (itBodies = mBodies.begin(); itBodies != mBodies.end(); ) { + std::set::iterator itToRemove = itBodies; + ++itBodies; + destroyCollisionBody(*itToRemove); + } - assert(mBodies.empty()); + assert(mBodies.empty()); } // Create a collision body and add it to the world @@ -59,23 +40,23 @@ CollisionWorld::~CollisionWorld() { */ CollisionBody* CollisionWorld::createCollisionBody(const Transform& transform) { - // Get the next available body ID - bodyindex bodyID = computeNextAvailableBodyID(); + // Get the next available body ID + bodyindex bodyID = computeNextAvailableBodyID(); - // Largest index cannot be used (it is used for invalid index) - assert(bodyID < std::numeric_limits::max()); + // Largest index cannot be used (it is used for invalid index) + assert(bodyID < std::numeric_limits::max()); - // Create the collision body - CollisionBody* collisionBody = new (mMemoryAllocator.allocate(sizeof(CollisionBody))) - CollisionBody(transform, *this, bodyID); + // Create the collision body + CollisionBody* collisionBody = new (mMemoryAllocator.allocate(sizeof(CollisionBody))) + CollisionBody(transform, *this, bodyID); - assert(collisionBody != NULL); + assert(collisionBody != NULL); - // Add the collision body to the world - mBodies.insert(collisionBody); + // Add the collision body to the world + mBodies.insert(collisionBody); - // Return the pointer to the rigid body - return collisionBody; + // Return the pointer to the rigid body + return collisionBody; } // Destroy a collision body @@ -84,48 +65,48 @@ CollisionBody* CollisionWorld::createCollisionBody(const Transform& transform) { */ void CollisionWorld::destroyCollisionBody(CollisionBody* collisionBody) { - // Remove all the collision shapes of the body - collisionBody->removeAllCollisionShapes(); + // Remove all the collision shapes of the body + collisionBody->removeAllCollisionShapes(); - // Add the body ID to the list of free IDs - mFreeBodiesIDs.push_back(collisionBody->getID()); + // Add the body ID to the list of free IDs + mFreeBodiesIDs.push_back(collisionBody->getID()); - // Call the destructor of the collision body - collisionBody->~CollisionBody(); + // Call the destructor of the collision body + collisionBody->~CollisionBody(); - // Remove the collision body from the list of bodies - mBodies.erase(collisionBody); + // Remove the collision body from the list of bodies + mBodies.erase(collisionBody); - // Free the object from the memory allocator - mMemoryAllocator.release(collisionBody, sizeof(CollisionBody)); + // Free the object from the memory allocator + mMemoryAllocator.release(collisionBody, sizeof(CollisionBody)); } // Return the next available body ID bodyindex CollisionWorld::computeNextAvailableBodyID() { - // Compute the body ID - bodyindex bodyID; - if (!mFreeBodiesIDs.empty()) { - bodyID = mFreeBodiesIDs.back(); - mFreeBodiesIDs.pop_back(); - } - else { - bodyID = mCurrentBodyID; - mCurrentBodyID++; - } + // Compute the body ID + bodyindex bodyID; + if (!mFreeBodiesIDs.empty()) { + bodyID = mFreeBodiesIDs.back(); + mFreeBodiesIDs.pop_back(); + } + else { + bodyID = mCurrentBodyID; + mCurrentBodyID++; + } - return bodyID; + return bodyID; } // Reset all the contact manifolds linked list of each body void CollisionWorld::resetContactManifoldListsOfBodies() { - // For each rigid body of the world - for (std::set::iterator it = mBodies.begin(); it != mBodies.end(); ++it) { + // For each rigid body of the world + for (std::set::iterator it = mBodies.begin(); it != mBodies.end(); ++it) { - // Reset the contact manifold list of the body - (*it)->resetContactManifoldsList(); - } + // Reset the contact manifold list of the body + (*it)->resetContactManifoldsList(); + } } // Test if the AABBs of two bodies overlap @@ -135,17 +116,17 @@ void CollisionWorld::resetContactManifoldListsOfBodies() { * @return True if the AABBs of the two bodies overlap and false otherwise */ bool CollisionWorld::testAABBOverlap(const CollisionBody* body1, - const CollisionBody* body2) const { + const CollisionBody* body2) const { - // If one of the body is not active, we return no overlap - if (!body1->isActive() || !body2->isActive()) return false; + // If one of the body is not active, we return no overlap + if (!body1->isActive() || !body2->isActive()) return false; - // Compute the AABBs of both bodies - AABB body1AABB = body1->getAABB(); - AABB body2AABB = body2->getAABB(); + // Compute the AABBs of both bodies + AABB body1AABB = body1->getAABB(); + AABB body2AABB = body2->getAABB(); - // Return true if the two AABBs overlap - return body1AABB.testCollision(body2AABB); + // Return true if the two AABBs overlap + return body1AABB.testCollision(body2AABB); } // Test and report collisions between a given shape and all the others @@ -155,18 +136,18 @@ bool CollisionWorld::testAABBOverlap(const CollisionBody* body1, * @param callback Pointer to the object with the callback method */ void CollisionWorld::testCollision(const ProxyShape* shape, - CollisionCallback* callback) { + CollisionCallback* callback) { - // Reset all the contact manifolds lists of each body - resetContactManifoldListsOfBodies(); + // Reset all the contact manifolds lists of each body + resetContactManifoldListsOfBodies(); - // Create the sets of shapes - std::set shapes; - shapes.insert(shape->mBroadPhaseID); - std::set emptySet; + // Create the sets of shapes + std::set shapes; + shapes.insert(shape->mBroadPhaseID); + std::set emptySet; - // Perform the collision detection and report contacts - mCollisionDetection.testCollisionBetweenShapes(callback, shapes, emptySet); + // Perform the collision detection and report contacts + mCollisionDetection.testCollisionBetweenShapes(callback, shapes, emptySet); } // Test and report collisions between two given shapes @@ -176,20 +157,20 @@ void CollisionWorld::testCollision(const ProxyShape* shape, * @param callback Pointer to the object with the callback method */ void CollisionWorld::testCollision(const ProxyShape* shape1, - const ProxyShape* shape2, - CollisionCallback* callback) { + const ProxyShape* shape2, + CollisionCallback* callback) { - // Reset all the contact manifolds lists of each body - resetContactManifoldListsOfBodies(); + // Reset all the contact manifolds lists of each body + resetContactManifoldListsOfBodies(); - // Create the sets of shapes - std::set shapes1; - shapes1.insert(shape1->mBroadPhaseID); - std::set shapes2; - shapes2.insert(shape2->mBroadPhaseID); + // Create the sets of shapes + std::set shapes1; + shapes1.insert(shape1->mBroadPhaseID); + std::set shapes2; + shapes2.insert(shape2->mBroadPhaseID); - // Perform the collision detection and report contacts - mCollisionDetection.testCollisionBetweenShapes(callback, shapes1, shapes2); + // Perform the collision detection and report contacts + mCollisionDetection.testCollisionBetweenShapes(callback, shapes1, shapes2); } // Test and report collisions between a body and all the others bodies of the @@ -199,24 +180,24 @@ void CollisionWorld::testCollision(const ProxyShape* shape1, * @param callback Pointer to the object with the callback method */ void CollisionWorld::testCollision(const CollisionBody* body, - CollisionCallback* callback) { + CollisionCallback* callback) { - // Reset all the contact manifolds lists of each body - resetContactManifoldListsOfBodies(); + // Reset all the contact manifolds lists of each body + resetContactManifoldListsOfBodies(); - // Create the sets of shapes - std::set shapes1; + // Create the sets of shapes + std::set shapes1; - // For each shape of the body - for (const ProxyShape* shape=body->getProxyShapesList(); shape != NULL; - shape = shape->getNext()) { - shapes1.insert(shape->mBroadPhaseID); - } + // For each shape of the body + for (const ProxyShape* shape=body->getProxyShapesList(); shape != NULL; + shape = shape->getNext()) { + shapes1.insert(shape->mBroadPhaseID); + } - std::set emptySet; + std::set emptySet; - // Perform the collision detection and report contacts - mCollisionDetection.testCollisionBetweenShapes(callback, shapes1, emptySet); + // Perform the collision detection and report contacts + mCollisionDetection.testCollisionBetweenShapes(callback, shapes1, emptySet); } // Test and report collisions between two bodies @@ -226,27 +207,27 @@ void CollisionWorld::testCollision(const CollisionBody* body, * @param callback Pointer to the object with the callback method */ void CollisionWorld::testCollision(const CollisionBody* body1, - const CollisionBody* body2, - CollisionCallback* callback) { + const CollisionBody* body2, + CollisionCallback* callback) { - // Reset all the contact manifolds lists of each body - resetContactManifoldListsOfBodies(); + // Reset all the contact manifolds lists of each body + resetContactManifoldListsOfBodies(); - // Create the sets of shapes - std::set shapes1; - for (const ProxyShape* shape=body1->getProxyShapesList(); shape != NULL; - shape = shape->getNext()) { - shapes1.insert(shape->mBroadPhaseID); - } + // Create the sets of shapes + std::set shapes1; + for (const ProxyShape* shape=body1->getProxyShapesList(); shape != NULL; + shape = shape->getNext()) { + shapes1.insert(shape->mBroadPhaseID); + } - std::set shapes2; - for (const ProxyShape* shape=body2->getProxyShapesList(); shape != NULL; - shape = shape->getNext()) { - shapes2.insert(shape->mBroadPhaseID); - } + std::set shapes2; + for (const ProxyShape* shape=body2->getProxyShapesList(); shape != NULL; + shape = shape->getNext()) { + shapes2.insert(shape->mBroadPhaseID); + } - // Perform the collision detection and report contacts - mCollisionDetection.testCollisionBetweenShapes(callback, shapes1, shapes2); + // Perform the collision detection and report contacts + mCollisionDetection.testCollisionBetweenShapes(callback, shapes1, shapes2); } // Test and report collisions between all shapes of the world @@ -255,12 +236,12 @@ void CollisionWorld::testCollision(const CollisionBody* body1, */ void CollisionWorld::testCollision(CollisionCallback* callback) { - // Reset all the contact manifolds lists of each body - resetContactManifoldListsOfBodies(); + // Reset all the contact manifolds lists of each body + resetContactManifoldListsOfBodies(); - std::set emptySet; + std::set emptySet; - // Perform the collision detection and report contacts - mCollisionDetection.testCollisionBetweenShapes(callback, emptySet, emptySet); + // Perform the collision detection and report contacts + mCollisionDetection.testCollisionBetweenShapes(callback, emptySet, emptySet); } diff --git a/ephysics/engine/CollisionWorld.h b/ephysics/engine/CollisionWorld.h index 47af1bf..56977e3 100644 --- a/ephysics/engine/CollisionWorld.h +++ b/ephysics/engine/CollisionWorld.h @@ -1,30 +1,9 @@ -/******************************************************************************** -* 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 REACTPHYSICS3D_COLLISION_WORLD_H -#define REACTPHYSICS3D_COLLISION_WORLD_H +/** @file + * @author Daniel Chappuis + * @copyright 2010-2016 Daniel Chappuis + * @license BSD 3 clauses (see license file) + */ +#pragma once // Libraries #include @@ -56,108 +35,108 @@ class CollisionCallback; */ class CollisionWorld { - protected : + protected : - // -------------------- Attributes -------------------- // + // -------------------- Attributes -------------------- // - /// Reference to the collision detection - CollisionDetection mCollisionDetection; + /// Reference to the collision detection + CollisionDetection mCollisionDetection; - /// All the bodies (rigid and soft) of the world - std::set mBodies; + /// All the bodies (rigid and soft) of the world + std::set mBodies; - /// Current body ID - bodyindex mCurrentBodyID; + /// Current body ID + bodyindex mCurrentBodyID; - /// List of free ID for rigid bodies - std::vector mFreeBodiesIDs; + /// List of free ID for rigid bodies + std::vector mFreeBodiesIDs; - /// Memory allocator - MemoryAllocator mMemoryAllocator; + /// Memory allocator + MemoryAllocator mMemoryAllocator; - /// Pointer to an event listener object - EventListener* mEventListener; + /// Pointer to an event listener object + EventListener* mEventListener; - // -------------------- Methods -------------------- // + // -------------------- Methods -------------------- // - /// Private copy-constructor - CollisionWorld(const CollisionWorld& world); + /// Private copy-constructor + CollisionWorld(const CollisionWorld& world); - /// Private assignment operator - CollisionWorld& operator=(const CollisionWorld& world); + /// Private assignment operator + CollisionWorld& operator=(const CollisionWorld& world); - /// Return the next available body ID - bodyindex computeNextAvailableBodyID(); + /// Return the next available body ID + bodyindex computeNextAvailableBodyID(); - /// Reset all the contact manifolds linked list of each body - void resetContactManifoldListsOfBodies(); + /// Reset all the contact manifolds linked list of each body + void resetContactManifoldListsOfBodies(); - public : + public : - // -------------------- Methods -------------------- // + // -------------------- Methods -------------------- // - /// Constructor - CollisionWorld(); + /// Constructor + CollisionWorld(); - /// Destructor - virtual ~CollisionWorld(); + /// Destructor + virtual ~CollisionWorld(); - /// Return an iterator to the beginning of the bodies of the physics world - std::set::iterator getBodiesBeginIterator(); + /// Return an iterator to the beginning of the bodies of the physics world + std::set::iterator getBodiesBeginIterator(); - /// Return an iterator to the end of the bodies of the physics world - std::set::iterator getBodiesEndIterator(); + /// Return an iterator to the end of the bodies of the physics world + std::set::iterator getBodiesEndIterator(); - /// Create a collision body - CollisionBody* createCollisionBody(const Transform& transform); + /// Create a collision body + CollisionBody* createCollisionBody(const Transform& transform); - /// Destroy a collision body - void destroyCollisionBody(CollisionBody* collisionBody); + /// Destroy a collision body + void destroyCollisionBody(CollisionBody* collisionBody); - /// Set the collision dispatch configuration - void setCollisionDispatch(CollisionDispatch* collisionDispatch); + /// Set the collision dispatch configuration + void setCollisionDispatch(CollisionDispatch* collisionDispatch); - /// Ray cast method - void raycast(const Ray& ray, RaycastCallback* raycastCallback, - unsigned short raycastWithCategoryMaskBits = 0xFFFF) const; + /// Ray cast method + void raycast(const Ray& ray, RaycastCallback* raycastCallback, + unsigned short raycastWithCategoryMaskBits = 0xFFFF) const; - /// Test if the AABBs of two bodies overlap - bool testAABBOverlap(const CollisionBody* body1, - const CollisionBody* body2) const; + /// Test if the AABBs of two bodies overlap + bool testAABBOverlap(const CollisionBody* body1, + const CollisionBody* body2) const; - /// Test if the AABBs of two proxy shapes overlap - bool testAABBOverlap(const ProxyShape* shape1, - const ProxyShape* shape2) const; + /// Test if the AABBs of two proxy shapes overlap + bool testAABBOverlap(const ProxyShape* shape1, + const ProxyShape* shape2) const; - /// Test and report collisions between a given shape and all the others - /// shapes of the world - virtual void testCollision(const ProxyShape* shape, - CollisionCallback* callback); + /// Test and report collisions between a given shape and all the others + /// shapes of the world + virtual void testCollision(const ProxyShape* shape, + CollisionCallback* callback); - /// Test and report collisions between two given shapes - virtual void testCollision(const ProxyShape* shape1, - const ProxyShape* shape2, - CollisionCallback* callback); + /// Test and report collisions between two given shapes + virtual void testCollision(const ProxyShape* shape1, + const ProxyShape* shape2, + CollisionCallback* callback); - /// Test and report collisions between a body and all the others bodies of the - /// world - virtual void testCollision(const CollisionBody* body, - CollisionCallback* callback); + /// Test and report collisions between a body and all the others bodies of the + /// world + virtual void testCollision(const CollisionBody* body, + CollisionCallback* callback); - /// Test and report collisions between two bodies - virtual void testCollision(const CollisionBody* body1, - const CollisionBody* body2, - CollisionCallback* callback); + /// Test and report collisions between two bodies + virtual void testCollision(const CollisionBody* body1, + const CollisionBody* body2, + CollisionCallback* callback); - /// Test and report collisions between all shapes of the world - virtual void testCollision(CollisionCallback* callback); + /// Test and report collisions between all shapes of the world + virtual void testCollision(CollisionCallback* callback); - // -------------------- Friendship -------------------- // + // -------------------- Friendship -------------------- // - friend class CollisionDetection; - friend class CollisionBody; - friend class RigidBody; - friend class ConvexMeshShape; + friend class CollisionDetection; + friend class CollisionBody; + friend class RigidBody; + friend class ConvexMeshShape; }; // Return an iterator to the beginning of the bodies of the physics world @@ -165,7 +144,7 @@ class CollisionWorld { * @return An starting iterator to the set of bodies of the world */ inline std::set::iterator CollisionWorld::getBodiesBeginIterator() { - return mBodies.begin(); + return mBodies.begin(); } // Return an iterator to the end of the bodies of the physics world @@ -173,7 +152,7 @@ inline std::set::iterator CollisionWorld::getBodiesBeginIterator * @return An ending iterator to the set of bodies of the world */ inline std::set::iterator CollisionWorld::getBodiesEndIterator() { - return mBodies.end(); + return mBodies.end(); } // Set the collision dispatch configuration @@ -184,7 +163,7 @@ inline std::set::iterator CollisionWorld::getBodiesEndIterator() * which collision detection algorithm to use for two given collision shapes */ inline void CollisionWorld::setCollisionDispatch(CollisionDispatch* collisionDispatch) { - mCollisionDetection.setCollisionDispatch(collisionDispatch); + mCollisionDetection.setCollisionDispatch(collisionDispatch); } // Ray cast method @@ -192,12 +171,12 @@ inline void CollisionWorld::setCollisionDispatch(CollisionDispatch* collisionDis * @param ray Ray to use for raycasting * @param raycastCallback Pointer to the class with the callback method * @param raycastWithCategoryMaskBits Bits mask corresponding to the category of - * bodies to be raycasted + * bodies to be raycasted */ inline void CollisionWorld::raycast(const Ray& ray, - RaycastCallback* raycastCallback, - unsigned short raycastWithCategoryMaskBits) const { - mCollisionDetection.raycast(raycastCallback, ray, raycastWithCategoryMaskBits); + RaycastCallback* raycastCallback, + unsigned short raycastWithCategoryMaskBits) const { + mCollisionDetection.raycast(raycastCallback, ray, raycastWithCategoryMaskBits); } // Test if the AABBs of two proxy shapes overlap @@ -207,9 +186,9 @@ inline void CollisionWorld::raycast(const Ray& ray, * @return */ inline bool CollisionWorld::testAABBOverlap(const ProxyShape* shape1, - const ProxyShape* shape2) const { + const ProxyShape* shape2) const { - return mCollisionDetection.testAABBOverlap(shape1, shape2); + return mCollisionDetection.testAABBOverlap(shape1, shape2); } // Class CollisionCallback @@ -221,17 +200,15 @@ inline bool CollisionWorld::testAABBOverlap(const ProxyShape* shape1, */ class CollisionCallback { - public: + public: - /// Destructor - virtual ~CollisionCallback() { + /// Destructor + virtual ~CollisionCallback() { - } + } - /// This method will be called for contact - virtual void notifyContact(const ContactPointInfo& contactPointInfo)=0; + /// This method will be called for contact + virtual void notifyContact(const ContactPointInfo& contactPointInfo)=0; }; } - - #endif diff --git a/ephysics/engine/ConstraintSolver.cpp b/ephysics/engine/ConstraintSolver.cpp index af3d35f..d2598ba 100644 --- a/ephysics/engine/ConstraintSolver.cpp +++ b/ephysics/engine/ConstraintSolver.cpp @@ -1,27 +1,8 @@ -/******************************************************************************** -* 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. * -* * -********************************************************************************/ +/** @file + * @author Daniel Chappuis + * @copyright 2010-2016 Daniel Chappuis + * @license BSD 3 clauses (see license file) + */ // Libraries #include @@ -30,9 +11,9 @@ using namespace reactphysics3d; // Constructor -ConstraintSolver::ConstraintSolver(const std::map& mapBodyToVelocityIndex) - : mMapBodyToConstrainedVelocityIndex(mapBodyToVelocityIndex), - mIsWarmStartingActive(true), mConstraintSolverData(mapBodyToVelocityIndex) { +ConstraintSolver::ConstraintSolver(const std::map& mapBodyToVelocityIndex) + : mMapBodyToConstrainedVelocityIndex(mapBodyToVelocityIndex), + mIsWarmStartingActive(true), mConstraintSolverData(mapBodyToVelocityIndex) { } @@ -42,65 +23,65 @@ ConstraintSolver::~ConstraintSolver() { } // Initialize the constraint solver for a given island -void ConstraintSolver::initializeForIsland(decimal dt, Island* island) { +void ConstraintSolver::initializeForIsland(float dt, Island* island) { - PROFILE("ConstraintSolver::initializeForIsland()"); + PROFILE("ConstraintSolver::initializeForIsland()"); - assert(island != NULL); - assert(island->getNbBodies() > 0); - assert(island->getNbJoints() > 0); + assert(island != NULL); + assert(island->getNbBodies() > 0); + assert(island->getNbJoints() > 0); - // Set the current time step - mTimeStep = dt; + // Set the current time step + mTimeStep = dt; - // Initialize the constraint solver data used to initialize and solve the constraints - mConstraintSolverData.timeStep = mTimeStep; - mConstraintSolverData.isWarmStartingActive = mIsWarmStartingActive; + // Initialize the constraint solver data used to initialize and solve the constraints + mConstraintSolverData.timeStep = mTimeStep; + mConstraintSolverData.isWarmStartingActive = mIsWarmStartingActive; - // For each joint of the island - Joint** joints = island->getJoints(); - for (uint i=0; igetNbJoints(); i++) { + // For each joint of the island + Joint** joints = island->getJoints(); + for (uint32_t i=0; igetNbJoints(); i++) { - // Initialize the constraint before solving it - joints[i]->initBeforeSolve(mConstraintSolverData); + // Initialize the constraint before solving it + joints[i]->initBeforeSolve(mConstraintSolverData); - // Warm-start the constraint if warm-starting is enabled - if (mIsWarmStartingActive) { - joints[i]->warmstart(mConstraintSolverData); - } - } + // Warm-start the constraint if warm-starting is enabled + if (mIsWarmStartingActive) { + joints[i]->warmstart(mConstraintSolverData); + } + } } // Solve the velocity constraints void ConstraintSolver::solveVelocityConstraints(Island* island) { - PROFILE("ConstraintSolver::solveVelocityConstraints()"); + PROFILE("ConstraintSolver::solveVelocityConstraints()"); - assert(island != NULL); - assert(island->getNbJoints() > 0); + assert(island != NULL); + assert(island->getNbJoints() > 0); - // For each joint of the island - Joint** joints = island->getJoints(); - for (uint i=0; igetNbJoints(); i++) { + // For each joint of the island + Joint** joints = island->getJoints(); + for (uint32_t i=0; igetNbJoints(); i++) { - // Solve the constraint - joints[i]->solveVelocityConstraint(mConstraintSolverData); - } + // Solve the constraint + joints[i]->solveVelocityConstraint(mConstraintSolverData); + } } // Solve the position constraints void ConstraintSolver::solvePositionConstraints(Island* island) { - PROFILE("ConstraintSolver::solvePositionConstraints()"); + PROFILE("ConstraintSolver::solvePositionConstraints()"); - assert(island != NULL); - assert(island->getNbJoints() > 0); + assert(island != NULL); + assert(island->getNbJoints() > 0); - // For each joint of the island - Joint** joints = island->getJoints(); - for (uint i=0; i < island->getNbJoints(); i++) { + // For each joint of the island + Joint** joints = island->getJoints(); + for (uint32_t i=0; i < island->getNbJoints(); i++) { - // Solve the constraint - joints[i]->solvePositionConstraint(mConstraintSolverData); - } + // Solve the constraint + joints[i]->solvePositionConstraint(mConstraintSolverData); + } } diff --git a/ephysics/engine/ConstraintSolver.h b/ephysics/engine/ConstraintSolver.h index 4667514..2c36b41 100644 --- a/ephysics/engine/ConstraintSolver.h +++ b/ephysics/engine/ConstraintSolver.h @@ -1,30 +1,9 @@ -/******************************************************************************** -* 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 REACTPHYSICS3D_CONSTRAINT_SOLVER_H -#define REACTPHYSICS3D_CONSTRAINT_SOLVER_H +/** @file + * @author Daniel Chappuis + * @copyright 2010-2016 Daniel Chappuis + * @license BSD 3 clauses (see license file) + */ +#pragma once // Libraries #include @@ -43,37 +22,37 @@ namespace reactphysics3d { */ struct ConstraintSolverData { - public : + public : - /// Current time step of the simulation - decimal timeStep; + /// Current time step of the simulation + float timeStep; - /// Array with the bodies linear velocities - Vector3* linearVelocities; + /// Array with the bodies linear velocities + Vector3* linearVelocities; - /// Array with the bodies angular velocities - Vector3* angularVelocities; + /// Array with the bodies angular velocities + Vector3* angularVelocities; - /// Reference to the bodies positions - Vector3* positions; + /// Reference to the bodies positions + Vector3* positions; - /// Reference to the bodies orientations - Quaternion* orientations; + /// Reference to the bodies orientations + Quaternion* orientations; - /// Reference to the map that associates rigid body to their index - /// in the constrained velocities array - const std::map& mapBodyToConstrainedVelocityIndex; + /// Reference to the map that associates rigid body to their index + /// in the constrained velocities array + const std::map& mapBodyToConstrainedVelocityIndex; - /// True if warm starting of the solver is active - bool isWarmStartingActive; + /// True if warm starting of the solver is active + bool isWarmStartingActive; - /// Constructor - ConstraintSolverData(const std::map& refMapBodyToConstrainedVelocityIndex) - :linearVelocities(NULL), angularVelocities(NULL), - positions(NULL), orientations(NULL), - mapBodyToConstrainedVelocityIndex(refMapBodyToConstrainedVelocityIndex){ + /// Constructor + ConstraintSolverData(const std::map& refMapBodyToConstrainedVelocityIndex) + :linearVelocities(NULL), angularVelocities(NULL), + positions(NULL), orientations(NULL), + mapBodyToConstrainedVelocityIndex(refMapBodyToConstrainedVelocityIndex){ - } + } }; @@ -99,7 +78,7 @@ struct ConstraintSolverData { * * --- Step 1 --- * - * First, we integrate the applied force F_a acting of each rigid body (like gravity, ...) and + * First, we int32_tegrate the applied force F_a acting of each rigid body (like gravity, ...) and * we obtain some new velocities v2' that tends to violate the constraints. * * v2' = v1 + dt * M^-1 * F_a @@ -120,7 +99,7 @@ struct ConstraintSolverData { * * --- Step 3 --- * - * In the third step, we integrate the new position x2 of the bodies using the new velocities + * In the third step, we int32_tegrate the new position x2 of the bodies using the new velocities * v2 computed in the second step with : x2 = x1 + dt * v2. * * Note that in the following code (as it is also explained in the slides from Erin Catto), @@ -148,75 +127,73 @@ struct ConstraintSolverData { */ class ConstraintSolver { - private : + private : - // -------------------- Attributes -------------------- // + // -------------------- Attributes -------------------- // - /// Reference to the map that associates rigid body to their index in - /// the constrained velocities array - const std::map& mMapBodyToConstrainedVelocityIndex; + /// Reference to the map that associates rigid body to their index in + /// the constrained velocities array + const std::map& mMapBodyToConstrainedVelocityIndex; - /// Current time step - decimal mTimeStep; + /// Current time step + float mTimeStep; - /// True if the warm starting of the solver is active - bool mIsWarmStartingActive; + /// True if the warm starting of the solver is active + bool mIsWarmStartingActive; - /// Constraint solver data used to initialize and solve the constraints - ConstraintSolverData mConstraintSolverData; + /// Constraint solver data used to initialize and solve the constraints + ConstraintSolverData mConstraintSolverData; - public : + public : - // -------------------- Methods -------------------- // + // -------------------- Methods -------------------- // - /// Constructor - ConstraintSolver(const std::map& mapBodyToVelocityIndex); + /// Constructor + ConstraintSolver(const std::map& mapBodyToVelocityIndex); - /// Destructor - ~ConstraintSolver(); + /// Destructor + ~ConstraintSolver(); - /// Initialize the constraint solver for a given island - void initializeForIsland(decimal dt, Island* island); + /// Initialize the constraint solver for a given island + void initializeForIsland(float dt, Island* island); - /// Solve the constraints - void solveVelocityConstraints(Island* island); + /// Solve the constraints + void solveVelocityConstraints(Island* island); - /// Solve the position constraints - void solvePositionConstraints(Island* island); + /// Solve the position constraints + void solvePositionConstraints(Island* island); - /// Return true if the Non-Linear-Gauss-Seidel position correction technique is active - bool getIsNonLinearGaussSeidelPositionCorrectionActive() const; + /// Return true if the Non-Linear-Gauss-Seidel position correction technique is active + bool getIsNonLinearGaussSeidelPositionCorrectionActive() const; - /// Enable/Disable the Non-Linear-Gauss-Seidel position correction technique. - void setIsNonLinearGaussSeidelPositionCorrectionActive(bool isActive); + /// Enable/Disable the Non-Linear-Gauss-Seidel position correction technique. + void setIsNonLinearGaussSeidelPositionCorrectionActive(bool isActive); - /// Set the constrained velocities arrays - void setConstrainedVelocitiesArrays(Vector3* constrainedLinearVelocities, - Vector3* constrainedAngularVelocities); + /// Set the constrained velocities arrays + void setConstrainedVelocitiesArrays(Vector3* constrainedLinearVelocities, + Vector3* constrainedAngularVelocities); - /// Set the constrained positions/orientations arrays - void setConstrainedPositionsArrays(Vector3* constrainedPositions, - Quaternion* constrainedOrientations); + /// Set the constrained positions/orientations arrays + void setConstrainedPositionsArrays(Vector3* constrainedPositions, + Quaternion* constrainedOrientations); }; // Set the constrained velocities arrays inline void ConstraintSolver::setConstrainedVelocitiesArrays(Vector3* constrainedLinearVelocities, - Vector3* constrainedAngularVelocities) { - assert(constrainedLinearVelocities != NULL); - assert(constrainedAngularVelocities != NULL); - mConstraintSolverData.linearVelocities = constrainedLinearVelocities; - mConstraintSolverData.angularVelocities = constrainedAngularVelocities; + Vector3* constrainedAngularVelocities) { + assert(constrainedLinearVelocities != NULL); + assert(constrainedAngularVelocities != NULL); + mConstraintSolverData.linearVelocities = constrainedLinearVelocities; + mConstraintSolverData.angularVelocities = constrainedAngularVelocities; } // Set the constrained positions/orientations arrays inline void ConstraintSolver::setConstrainedPositionsArrays(Vector3* constrainedPositions, - Quaternion* constrainedOrientations) { - assert(constrainedPositions != NULL); - assert(constrainedOrientations != NULL); - mConstraintSolverData.positions = constrainedPositions; - mConstraintSolverData.orientations = constrainedOrientations; + Quaternion* constrainedOrientations) { + assert(constrainedPositions != NULL); + assert(constrainedOrientations != NULL); + mConstraintSolverData.positions = constrainedPositions; + mConstraintSolverData.orientations = constrainedOrientations; } } - -#endif diff --git a/ephysics/engine/ContactSolver.cpp b/ephysics/engine/ContactSolver.cpp index 09d087a..da4a6a9 100644 --- a/ephysics/engine/ContactSolver.cpp +++ b/ephysics/engine/ContactSolver.cpp @@ -1,27 +1,8 @@ -/******************************************************************************** -* 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. * -* * -********************************************************************************/ +/** @file + * @author Daniel Chappuis + * @copyright 2010-2016 Daniel Chappuis + * @license BSD 3 clauses (see license file) + */ // Libraries #include @@ -34,17 +15,17 @@ using namespace reactphysics3d; using namespace std; // Constants initialization -const decimal ContactSolver::BETA = decimal(0.2); -const decimal ContactSolver::BETA_SPLIT_IMPULSE = decimal(0.2); -const decimal ContactSolver::SLOP= decimal(0.01); +const float ContactSolver::BETA = float(0.2); +const float ContactSolver::BETA_SPLIT_IMPULSE = float(0.2); +const float ContactSolver::SLOP= float(0.01); // Constructor -ContactSolver::ContactSolver(const std::map& mapBodyToVelocityIndex) - :mSplitLinearVelocities(NULL), mSplitAngularVelocities(NULL), - mContactConstraints(NULL), mLinearVelocities(NULL), mAngularVelocities(NULL), - mMapBodyToConstrainedVelocityIndex(mapBodyToVelocityIndex), - mIsWarmStartingActive(true), mIsSplitImpulseActive(true), - mIsSolveFrictionAtContactManifoldCenterActive(true) { +ContactSolver::ContactSolver(const std::map& mapBodyToVelocityIndex) + :mSplitLinearVelocities(NULL), mSplitAngularVelocities(NULL), + mContactConstraints(NULL), mLinearVelocities(NULL), mAngularVelocities(NULL), + mMapBodyToConstrainedVelocityIndex(mapBodyToVelocityIndex), + mIsWarmStartingActive(true), mIsSplitImpulseActive(true), + mIsSolveFrictionAtContactManifoldCenterActive(true) { } @@ -54,282 +35,282 @@ ContactSolver::~ContactSolver() { } // Initialize the constraint solver for a given island -void ContactSolver::initializeForIsland(decimal dt, Island* island) { +void ContactSolver::initializeForIsland(float dt, Island* island) { - PROFILE("ContactSolver::initializeForIsland()"); + PROFILE("ContactSolver::initializeForIsland()"); - assert(island != NULL); - assert(island->getNbBodies() > 0); - assert(island->getNbContactManifolds() > 0); - assert(mSplitLinearVelocities != NULL); - assert(mSplitAngularVelocities != NULL); + assert(island != NULL); + assert(island->getNbBodies() > 0); + assert(island->getNbContactManifolds() > 0); + assert(mSplitLinearVelocities != NULL); + assert(mSplitAngularVelocities != NULL); - // Set the current time step - mTimeStep = dt; + // Set the current time step + mTimeStep = dt; - mNbContactManifolds = island->getNbContactManifolds(); + mNbContactManifolds = island->getNbContactManifolds(); - mContactConstraints = new ContactManifoldSolver[mNbContactManifolds]; - assert(mContactConstraints != NULL); + mContactConstraints = new ContactManifoldSolver[mNbContactManifolds]; + assert(mContactConstraints != NULL); - // For each contact manifold of the island - ContactManifold** contactManifolds = island->getContactManifold(); - for (uint i=0; igetContactManifold(); + for (uint32_t i=0; igetNbContactPoints() > 0); + assert(externalManifold->getNbContactPoints() > 0); - // Get the two bodies of the contact - RigidBody* body1 = static_cast(externalManifold->getContactPoint(0)->getBody1()); - RigidBody* body2 = static_cast(externalManifold->getContactPoint(0)->getBody2()); - assert(body1 != NULL); - assert(body2 != NULL); + // Get the two bodies of the contact + RigidBody* body1 = static_cast(externalManifold->getContactPoint(0)->getBody1()); + RigidBody* body2 = static_cast(externalManifold->getContactPoint(0)->getBody2()); + assert(body1 != NULL); + assert(body2 != NULL); - // Get the position of the two bodies - const Vector3& x1 = body1->mCenterOfMassWorld; - const Vector3& x2 = body2->mCenterOfMassWorld; + // Get the position of the two bodies + const Vector3& x1 = body1->mCenterOfMassWorld; + const Vector3& x2 = body2->mCenterOfMassWorld; - // Initialize the internal contact manifold structure using the external - // contact manifold - internalManifold.indexBody1 = mMapBodyToConstrainedVelocityIndex.find(body1)->second; - internalManifold.indexBody2 = mMapBodyToConstrainedVelocityIndex.find(body2)->second; - internalManifold.inverseInertiaTensorBody1 = body1->getInertiaTensorInverseWorld(); - internalManifold.inverseInertiaTensorBody2 = body2->getInertiaTensorInverseWorld(); - internalManifold.massInverseBody1 = body1->mMassInverse; - internalManifold.massInverseBody2 = body2->mMassInverse; - internalManifold.nbContacts = externalManifold->getNbContactPoints(); - internalManifold.restitutionFactor = computeMixedRestitutionFactor(body1, body2); - internalManifold.frictionCoefficient = computeMixedFrictionCoefficient(body1, body2); - internalManifold.rollingResistanceFactor = computeMixedRollingResistance(body1, body2); - internalManifold.externalContactManifold = externalManifold; - internalManifold.isBody1DynamicType = body1->getType() == DYNAMIC; - internalManifold.isBody2DynamicType = body2->getType() == DYNAMIC; + // Initialize the int32_ternal contact manifold structure using the external + // contact manifold + int32_ternalManifold.indexBody1 = mMapBodyToConstrainedVelocityIndex.find(body1)->second; + int32_ternalManifold.indexBody2 = mMapBodyToConstrainedVelocityIndex.find(body2)->second; + int32_ternalManifold.inverseInertiaTensorBody1 = body1->getInertiaTensorInverseWorld(); + int32_ternalManifold.inverseInertiaTensorBody2 = body2->getInertiaTensorInverseWorld(); + int32_ternalManifold.massInverseBody1 = body1->mMassInverse; + int32_ternalManifold.massInverseBody2 = body2->mMassInverse; + int32_ternalManifold.nbContacts = externalManifold->getNbContactPoints(); + int32_ternalManifold.restitutionFactor = computeMixedRestitutionFactor(body1, body2); + int32_ternalManifold.frictionCoefficient = computeMixedFrictionCoefficient(body1, body2); + int32_ternalManifold.rollingResistanceFactor = computeMixedRollingResistance(body1, body2); + int32_ternalManifold.externalContactManifold = externalManifold; + int32_ternalManifold.isBody1DynamicType = body1->getType() == DYNAMIC; + int32_ternalManifold.isBody2DynamicType = body2->getType() == DYNAMIC; - // If we solve the friction constraints at the center of the contact manifold - if (mIsSolveFrictionAtContactManifoldCenterActive) { - internalManifold.frictionPointBody1 = Vector3::zero(); - internalManifold.frictionPointBody2 = Vector3::zero(); - } + // If we solve the friction constraints at the center of the contact manifold + if (mIsSolveFrictionAtContactManifoldCenterActive) { + int32_ternalManifold.frictionPointBody1 = Vector3::zero(); + int32_ternalManifold.frictionPointBody2 = Vector3::zero(); + } - // For each contact point of the contact manifold - for (uint c=0; cgetNbContactPoints(); c++) { + // For each contact point of the contact manifold + for (uint32_t c=0; cgetNbContactPoints(); c++) { - ContactPointSolver& contactPoint = internalManifold.contacts[c]; + ContactPointSolver& contactPoint = int32_ternalManifold.contacts[c]; - // Get a contact point - ContactPoint* externalContact = externalManifold->getContactPoint(c); + // Get a contact point + ContactPoint* externalContact = externalManifold->getContactPoint(c); - // Get the contact point on the two bodies - Vector3 p1 = externalContact->getWorldPointOnBody1(); - Vector3 p2 = externalContact->getWorldPointOnBody2(); + // Get the contact point on the two bodies + Vector3 p1 = externalContact->getWorldPointOnBody1(); + Vector3 p2 = externalContact->getWorldPointOnBody2(); - contactPoint.externalContact = externalContact; - contactPoint.normal = externalContact->getNormal(); - contactPoint.r1 = p1 - x1; - contactPoint.r2 = p2 - x2; - contactPoint.penetrationDepth = externalContact->getPenetrationDepth(); - contactPoint.isRestingContact = externalContact->getIsRestingContact(); - externalContact->setIsRestingContact(true); - contactPoint.oldFrictionVector1 = externalContact->getFrictionVector1(); - contactPoint.oldFrictionVector2 = externalContact->getFrictionVector2(); - contactPoint.penetrationImpulse = 0.0; - contactPoint.friction1Impulse = 0.0; - contactPoint.friction2Impulse = 0.0; - contactPoint.rollingResistanceImpulse = Vector3::zero(); + contactPoint.externalContact = externalContact; + contactPoint.normal = externalContact->getNormal(); + contactPoint.r1 = p1 - x1; + contactPoint.r2 = p2 - x2; + contactPoint.penetrationDepth = externalContact->getPenetrationDepth(); + contactPoint.isRestingContact = externalContact->getIsRestingContact(); + externalContact->setIsRestingContact(true); + contactPoint.oldFrictionVector1 = externalContact->getFrictionVector1(); + contactPoint.oldFrictionVector2 = externalContact->getFrictionVector2(); + contactPoint.penetrationImpulse = 0.0; + contactPoint.friction1Impulse = 0.0; + contactPoint.friction2Impulse = 0.0; + contactPoint.rollingResistanceImpulse = Vector3::zero(); - // If we solve the friction constraints at the center of the contact manifold - if (mIsSolveFrictionAtContactManifoldCenterActive) { - internalManifold.frictionPointBody1 += p1; - internalManifold.frictionPointBody2 += p2; - } - } + // If we solve the friction constraints at the center of the contact manifold + if (mIsSolveFrictionAtContactManifoldCenterActive) { + int32_ternalManifold.frictionPointBody1 += p1; + int32_ternalManifold.frictionPointBody2 += p2; + } + } - // If we solve the friction constraints at the center of the contact manifold - if (mIsSolveFrictionAtContactManifoldCenterActive) { + // If we solve the friction constraints at the center of the contact manifold + if (mIsSolveFrictionAtContactManifoldCenterActive) { - internalManifold.frictionPointBody1 /=static_cast(internalManifold.nbContacts); - internalManifold.frictionPointBody2 /=static_cast(internalManifold.nbContacts); - internalManifold.r1Friction = internalManifold.frictionPointBody1 - x1; - internalManifold.r2Friction = internalManifold.frictionPointBody2 - x2; - internalManifold.oldFrictionVector1 = externalManifold->getFrictionVector1(); - internalManifold.oldFrictionVector2 = externalManifold->getFrictionVector2(); + int32_ternalManifold.frictionPointBody1 /=static_cast(int32_ternalManifold.nbContacts); + int32_ternalManifold.frictionPointBody2 /=static_cast(int32_ternalManifold.nbContacts); + int32_ternalManifold.r1Friction = int32_ternalManifold.frictionPointBody1 - x1; + int32_ternalManifold.r2Friction = int32_ternalManifold.frictionPointBody2 - x2; + int32_ternalManifold.oldFrictionVector1 = externalManifold->getFrictionVector1(); + int32_ternalManifold.oldFrictionVector2 = externalManifold->getFrictionVector2(); - // If warm starting is active - if (mIsWarmStartingActive) { + // If warm starting is active + if (mIsWarmStartingActive) { - // Initialize the accumulated impulses with the previous step accumulated impulses - internalManifold.friction1Impulse = externalManifold->getFrictionImpulse1(); - internalManifold.friction2Impulse = externalManifold->getFrictionImpulse2(); - internalManifold.frictionTwistImpulse = externalManifold->getFrictionTwistImpulse(); - } - else { + // Initialize the accumulated impulses with the previous step accumulated impulses + int32_ternalManifold.friction1Impulse = externalManifold->getFrictionImpulse1(); + int32_ternalManifold.friction2Impulse = externalManifold->getFrictionImpulse2(); + int32_ternalManifold.frictionTwistImpulse = externalManifold->getFrictionTwistImpulse(); + } + else { - // Initialize the accumulated impulses to zero - internalManifold.friction1Impulse = 0.0; - internalManifold.friction2Impulse = 0.0; - internalManifold.frictionTwistImpulse = 0.0; - internalManifold.rollingResistanceImpulse = Vector3(0, 0, 0); - } - } - } + // Initialize the accumulated impulses to zero + int32_ternalManifold.friction1Impulse = 0.0; + int32_ternalManifold.friction2Impulse = 0.0; + int32_ternalManifold.frictionTwistImpulse = 0.0; + int32_ternalManifold.rollingResistanceImpulse = Vector3(0, 0, 0); + } + } + } - // Fill-in all the matrices needed to solve the LCP problem - initializeContactConstraints(); + // Fill-in all the matrices needed to solve the LCP problem + initializeContactConstraints(); } // Initialize the contact constraints before solving the system void ContactSolver::initializeContactConstraints() { - - // For each contact constraint - for (uint c=0; c 0.0 ? contactPoint.inversePenetrationMass = decimal(1.0) / - massPenetration : - decimal(0.0); + // Compute the inverse mass matrix K for the penetration constraint + float massPenetration = manifold.massInverseBody1 + manifold.massInverseBody2 + + ((I1 * contactPoint.r1CrossN).cross(contactPoint.r1)).dot(contactPoint.normal) + + ((I2 * contactPoint.r2CrossN).cross(contactPoint.r2)).dot(contactPoint.normal); + massPenetration > 0.0 ? contactPoint.inversePenetrationMass = float(1.0) / + massPenetration : + float(0.0); - // If we do not solve the friction constraints at the center of the contact manifold - if (!mIsSolveFrictionAtContactManifoldCenterActive) { + // If we do not solve the friction constraints at the center of the contact manifold + if (!mIsSolveFrictionAtContactManifoldCenterActive) { - // Compute the friction vectors - computeFrictionVectors(deltaV, contactPoint); + // Compute the friction vectors + computeFrictionVectors(deltaV, contactPoint); - contactPoint.r1CrossT1 = contactPoint.r1.cross(contactPoint.frictionVector1); - contactPoint.r1CrossT2 = contactPoint.r1.cross(contactPoint.frictionVector2); - contactPoint.r2CrossT1 = contactPoint.r2.cross(contactPoint.frictionVector1); - contactPoint.r2CrossT2 = contactPoint.r2.cross(contactPoint.frictionVector2); + contactPoint.r1CrossT1 = contactPoint.r1.cross(contactPoint.frictionVector1); + contactPoint.r1CrossT2 = contactPoint.r1.cross(contactPoint.frictionVector2); + contactPoint.r2CrossT1 = contactPoint.r2.cross(contactPoint.frictionVector1); + contactPoint.r2CrossT2 = contactPoint.r2.cross(contactPoint.frictionVector2); - // Compute the inverse mass matrix K for the friction - // constraints at each contact point - decimal friction1Mass = manifold.massInverseBody1 + manifold.massInverseBody2 + - ((I1 * contactPoint.r1CrossT1).cross(contactPoint.r1)).dot( - contactPoint.frictionVector1) + - ((I2 * contactPoint.r2CrossT1).cross(contactPoint.r2)).dot( - contactPoint.frictionVector1); - decimal friction2Mass = manifold.massInverseBody1 + manifold.massInverseBody2 + - ((I1 * contactPoint.r1CrossT2).cross(contactPoint.r1)).dot( - contactPoint.frictionVector2) + - ((I2 * contactPoint.r2CrossT2).cross(contactPoint.r2)).dot( - contactPoint.frictionVector2); - friction1Mass > 0.0 ? contactPoint.inverseFriction1Mass = decimal(1.0) / - friction1Mass : - decimal(0.0); - friction2Mass > 0.0 ? contactPoint.inverseFriction2Mass = decimal(1.0) / - friction2Mass : - decimal(0.0); - } + // Compute the inverse mass matrix K for the friction + // constraints at each contact point + float friction1Mass = manifold.massInverseBody1 + manifold.massInverseBody2 + + ((I1 * contactPoint.r1CrossT1).cross(contactPoint.r1)).dot( + contactPoint.frictionVector1) + + ((I2 * contactPoint.r2CrossT1).cross(contactPoint.r2)).dot( + contactPoint.frictionVector1); + float friction2Mass = manifold.massInverseBody1 + manifold.massInverseBody2 + + ((I1 * contactPoint.r1CrossT2).cross(contactPoint.r1)).dot( + contactPoint.frictionVector2) + + ((I2 * contactPoint.r2CrossT2).cross(contactPoint.r2)).dot( + contactPoint.frictionVector2); + friction1Mass > 0.0 ? contactPoint.inverseFriction1Mass = float(1.0) / + friction1Mass : + float(0.0); + friction2Mass > 0.0 ? contactPoint.inverseFriction2Mass = float(1.0) / + friction2Mass : + float(0.0); + } - // Compute the restitution velocity bias "b". We compute this here instead - // of inside the solve() method because we need to use the velocity difference - // at the beginning of the contact. Note that if it is a resting contact (normal - // velocity bellow a given threshold), we do not add a restitution velocity bias - contactPoint.restitutionBias = 0.0; - decimal deltaVDotN = deltaV.dot(contactPoint.normal); - if (deltaVDotN < -RESTITUTION_VELOCITY_THRESHOLD) { - contactPoint.restitutionBias = manifold.restitutionFactor * deltaVDotN; - } + // Compute the restitution velocity bias "b". We compute this here instead + // of inside the solve() method because we need to use the velocity difference + // at the beginning of the contact. Note that if it is a resting contact (normal + // velocity bellow a given threshold), we do not add a restitution velocity bias + contactPoint.restitutionBias = 0.0; + float deltaVDotN = deltaV.dot(contactPoint.normal); + if (deltaVDotN < -RESTITUTION_VELOCITY_THRESHOLD) { + contactPoint.restitutionBias = manifold.restitutionFactor * deltaVDotN; + } - // If the warm starting of the contact solver is active - if (mIsWarmStartingActive) { + // If the warm starting of the contact solver is active + if (mIsWarmStartingActive) { - // Get the cached accumulated impulses from the previous step - contactPoint.penetrationImpulse = externalContact->getPenetrationImpulse(); - contactPoint.friction1Impulse = externalContact->getFrictionImpulse1(); - contactPoint.friction2Impulse = externalContact->getFrictionImpulse2(); - contactPoint.rollingResistanceImpulse = externalContact->getRollingResistanceImpulse(); - } + // Get the cached accumulated impulses from the previous step + contactPoint.penetrationImpulse = externalContact->getPenetrationImpulse(); + contactPoint.friction1Impulse = externalContact->getFrictionImpulse1(); + contactPoint.friction2Impulse = externalContact->getFrictionImpulse2(); + contactPoint.rollingResistanceImpulse = externalContact->getRollingResistanceImpulse(); + } - // Initialize the split impulses to zero - contactPoint.penetrationSplitImpulse = 0.0; + // Initialize the split impulses to zero + contactPoint.penetrationSplitImpulse = 0.0; - // If we solve the friction constraints at the center of the contact manifold - if (mIsSolveFrictionAtContactManifoldCenterActive) { - manifold.normal += contactPoint.normal; - } - } + // If we solve the friction constraints at the center of the contact manifold + if (mIsSolveFrictionAtContactManifoldCenterActive) { + manifold.normal += contactPoint.normal; + } + } - // Compute the inverse K matrix for the rolling resistance constraint - manifold.inverseRollingResistance.setToZero(); - if (manifold.rollingResistanceFactor > 0 && (manifold.isBody1DynamicType || manifold.isBody2DynamicType)) { - manifold.inverseRollingResistance = manifold.inverseInertiaTensorBody1 + manifold.inverseInertiaTensorBody2; - manifold.inverseRollingResistance = manifold.inverseRollingResistance.getInverse(); - } + // Compute the inverse K matrix for the rolling resistance constraint + manifold.inverseRollingResistance.setToZero(); + if (manifold.rollingResistanceFactor > 0 && (manifold.isBody1DynamicType || manifold.isBody2DynamicType)) { + manifold.inverseRollingResistance = manifold.inverseInertiaTensorBody1 + manifold.inverseInertiaTensorBody2; + manifold.inverseRollingResistance = manifold.inverseRollingResistance.getInverse(); + } - // If we solve the friction constraints at the center of the contact manifold - if (mIsSolveFrictionAtContactManifoldCenterActive) { + // If we solve the friction constraints at the center of the contact manifold + if (mIsSolveFrictionAtContactManifoldCenterActive) { - manifold.normal.normalize(); + manifold.normal.normalize(); - Vector3 deltaVFrictionPoint = v2 + w2.cross(manifold.r2Friction) - - v1 - w1.cross(manifold.r1Friction); + Vector3 deltaVFrictionPoint = v2 + w2.cross(manifold.r2Friction) - + v1 - w1.cross(manifold.r1Friction); - // Compute the friction vectors - computeFrictionVectors(deltaVFrictionPoint, manifold); + // Compute the friction vectors + computeFrictionVectors(deltaVFrictionPoint, manifold); - // Compute the inverse mass matrix K for the friction constraints at the center of - // the contact manifold - manifold.r1CrossT1 = manifold.r1Friction.cross(manifold.frictionVector1); - manifold.r1CrossT2 = manifold.r1Friction.cross(manifold.frictionVector2); - manifold.r2CrossT1 = manifold.r2Friction.cross(manifold.frictionVector1); - manifold.r2CrossT2 = manifold.r2Friction.cross(manifold.frictionVector2); - decimal friction1Mass = manifold.massInverseBody1 + manifold.massInverseBody2 + - ((I1 * manifold.r1CrossT1).cross(manifold.r1Friction)).dot( - manifold.frictionVector1) + - ((I2 * manifold.r2CrossT1).cross(manifold.r2Friction)).dot( - manifold.frictionVector1); - decimal friction2Mass = manifold.massInverseBody1 + manifold.massInverseBody2 + - ((I1 * manifold.r1CrossT2).cross(manifold.r1Friction)).dot( - manifold.frictionVector2) + - ((I2 * manifold.r2CrossT2).cross(manifold.r2Friction)).dot( - manifold.frictionVector2); - decimal frictionTwistMass = manifold.normal.dot(manifold.inverseInertiaTensorBody1 * - manifold.normal) + - manifold.normal.dot(manifold.inverseInertiaTensorBody2 * - manifold.normal); - friction1Mass > 0.0 ? manifold.inverseFriction1Mass = decimal(1.0)/friction1Mass - : decimal(0.0); - friction2Mass > 0.0 ? manifold.inverseFriction2Mass = decimal(1.0)/friction2Mass - : decimal(0.0); - frictionTwistMass > 0.0 ? manifold.inverseTwistFrictionMass = decimal(1.0) / - frictionTwistMass : - decimal(0.0); - } - } + // Compute the inverse mass matrix K for the friction constraints at the center of + // the contact manifold + manifold.r1CrossT1 = manifold.r1Friction.cross(manifold.frictionVector1); + manifold.r1CrossT2 = manifold.r1Friction.cross(manifold.frictionVector2); + manifold.r2CrossT1 = manifold.r2Friction.cross(manifold.frictionVector1); + manifold.r2CrossT2 = manifold.r2Friction.cross(manifold.frictionVector2); + float friction1Mass = manifold.massInverseBody1 + manifold.massInverseBody2 + + ((I1 * manifold.r1CrossT1).cross(manifold.r1Friction)).dot( + manifold.frictionVector1) + + ((I2 * manifold.r2CrossT1).cross(manifold.r2Friction)).dot( + manifold.frictionVector1); + float friction2Mass = manifold.massInverseBody1 + manifold.massInverseBody2 + + ((I1 * manifold.r1CrossT2).cross(manifold.r1Friction)).dot( + manifold.frictionVector2) + + ((I2 * manifold.r2CrossT2).cross(manifold.r2Friction)).dot( + manifold.frictionVector2); + float frictionTwistMass = manifold.normal.dot(manifold.inverseInertiaTensorBody1 * + manifold.normal) + + manifold.normal.dot(manifold.inverseInertiaTensorBody2 * + manifold.normal); + friction1Mass > 0.0 ? manifold.inverseFriction1Mass = float(1.0)/friction1Mass + : float(0.0); + friction2Mass > 0.0 ? manifold.inverseFriction2Mass = float(1.0)/friction2Mass + : float(0.0); + frictionTwistMass > 0.0 ? manifold.inverseTwistFrictionMass = float(1.0) / + frictionTwistMass : + float(0.0); + } + } } // Warm start the solver. @@ -338,576 +319,576 @@ void ContactSolver::initializeContactConstraints() { /// the solution of the linear system void ContactSolver::warmStart() { - // Check that warm starting is active - if (!mIsWarmStartingActive) return; + // Check that warm starting is active + if (!mIsWarmStartingActive) return; - // For each constraint - for (uint c=0; c 0) { + if (contactManifold.rollingResistanceFactor > 0) { - // Compute the impulse P = J^T * lambda - const Impulse impulseRollingResistance(Vector3::zero(), -contactPoint.rollingResistanceImpulse, - Vector3::zero(), contactPoint.rollingResistanceImpulse); + // Compute the impulse P = J^T * lambda + const Impulse impulseRollingResistance(Vector3::zero(), -contactPoint.rollingResistanceImpulse, + Vector3::zero(), contactPoint.rollingResistanceImpulse); - // Apply the impulses to the bodies of the constraint - applyImpulse(impulseRollingResistance, contactManifold); - } - } - } - else { // If it is a new contact point + // Apply the impulses to the bodies of the constraint + applyImpulse(impulseRollingResistance, contactManifold); + } + } + } + else { // If it is a new contact point - // Initialize the accumulated impulses to zero - contactPoint.penetrationImpulse = 0.0; - contactPoint.friction1Impulse = 0.0; - contactPoint.friction2Impulse = 0.0; - contactPoint.rollingResistanceImpulse = Vector3::zero(); - } - } + // Initialize the accumulated impulses to zero + contactPoint.penetrationImpulse = 0.0; + contactPoint.friction1Impulse = 0.0; + contactPoint.friction2Impulse = 0.0; + contactPoint.rollingResistanceImpulse = Vector3::zero(); + } + } - // If we solve the friction constraints at the center of the contact manifold and there is - // at least one resting contact point in the contact manifold - if (mIsSolveFrictionAtContactManifoldCenterActive && atLeastOneRestingContactPoint) { + // If we solve the friction constraints at the center of the contact manifold and there is + // at least one resting contact point in the contact manifold + if (mIsSolveFrictionAtContactManifoldCenterActive && atLeastOneRestingContactPoint) { - // Project the old friction impulses (with old friction vectors) into the new friction - // vectors to get the new friction impulses - Vector3 oldFrictionImpulse = contactManifold.friction1Impulse * - contactManifold.oldFrictionVector1 + - contactManifold.friction2Impulse * - contactManifold.oldFrictionVector2; - contactManifold.friction1Impulse = oldFrictionImpulse.dot( - contactManifold.frictionVector1); - contactManifold.friction2Impulse = oldFrictionImpulse.dot( - contactManifold.frictionVector2); + // Project the old friction impulses (with old friction vectors) int32_to the new friction + // vectors to get the new friction impulses + Vector3 oldFrictionImpulse = contactManifold.friction1Impulse * + contactManifold.oldFrictionVector1 + + contactManifold.friction2Impulse * + contactManifold.oldFrictionVector2; + contactManifold.friction1Impulse = oldFrictionImpulse.dot( + contactManifold.frictionVector1); + contactManifold.friction2Impulse = oldFrictionImpulse.dot( + contactManifold.frictionVector2); - // ------ First friction constraint at the center of the contact manifold ------ // + // ------ First friction constraint at the center of the contact manifold ------ // - // Compute the impulse P = J^T * lambda - Vector3 linearImpulseBody1 = -contactManifold.frictionVector1 * - contactManifold.friction1Impulse; - Vector3 angularImpulseBody1 = -contactManifold.r1CrossT1 * - contactManifold.friction1Impulse; - Vector3 linearImpulseBody2 = contactManifold.frictionVector1 * - contactManifold.friction1Impulse; - Vector3 angularImpulseBody2 = contactManifold.r2CrossT1 * - contactManifold.friction1Impulse; - const Impulse impulseFriction1(linearImpulseBody1, angularImpulseBody1, - linearImpulseBody2, angularImpulseBody2); + // Compute the impulse P = J^T * lambda + Vector3 linearImpulseBody1 = -contactManifold.frictionVector1 * + contactManifold.friction1Impulse; + Vector3 angularImpulseBody1 = -contactManifold.r1CrossT1 * + contactManifold.friction1Impulse; + Vector3 linearImpulseBody2 = contactManifold.frictionVector1 * + contactManifold.friction1Impulse; + Vector3 angularImpulseBody2 = contactManifold.r2CrossT1 * + contactManifold.friction1Impulse; + const Impulse impulseFriction1(linearImpulseBody1, angularImpulseBody1, + linearImpulseBody2, angularImpulseBody2); - // Apply the impulses to the bodies of the constraint - applyImpulse(impulseFriction1, contactManifold); + // Apply the impulses to the bodies of the constraint + applyImpulse(impulseFriction1, contactManifold); - // ------ Second friction constraint at the center of the contact manifold ----- // + // ------ Second friction constraint at the center of the contact manifold ----- // - // Compute the impulse P = J^T * lambda - linearImpulseBody1 = -contactManifold.frictionVector2 * - contactManifold.friction2Impulse; - angularImpulseBody1 = -contactManifold.r1CrossT2 * - contactManifold.friction2Impulse; - linearImpulseBody2 = contactManifold.frictionVector2 * - contactManifold.friction2Impulse; - angularImpulseBody2 = contactManifold.r2CrossT2 * - contactManifold.friction2Impulse; - const Impulse impulseFriction2(linearImpulseBody1, angularImpulseBody1, - linearImpulseBody2, angularImpulseBody2); + // Compute the impulse P = J^T * lambda + linearImpulseBody1 = -contactManifold.frictionVector2 * + contactManifold.friction2Impulse; + angularImpulseBody1 = -contactManifold.r1CrossT2 * + contactManifold.friction2Impulse; + linearImpulseBody2 = contactManifold.frictionVector2 * + contactManifold.friction2Impulse; + angularImpulseBody2 = contactManifold.r2CrossT2 * + contactManifold.friction2Impulse; + const Impulse impulseFriction2(linearImpulseBody1, angularImpulseBody1, + linearImpulseBody2, angularImpulseBody2); - // Apply the impulses to the bodies of the constraint - applyImpulse(impulseFriction2, contactManifold); + // Apply the impulses to the bodies of the constraint + applyImpulse(impulseFriction2, contactManifold); - // ------ Twist friction constraint at the center of the contact manifold ------ // + // ------ Twist friction constraint at the center of the contact manifold ------ // - // Compute the impulse P = J^T * lambda - linearImpulseBody1 = Vector3(0.0, 0.0, 0.0); - angularImpulseBody1 = -contactManifold.normal * contactManifold.frictionTwistImpulse; - linearImpulseBody2 = Vector3(0.0, 0.0, 0.0); - angularImpulseBody2 = contactManifold.normal * contactManifold.frictionTwistImpulse; - const Impulse impulseTwistFriction(linearImpulseBody1, angularImpulseBody1, - linearImpulseBody2, angularImpulseBody2); + // Compute the impulse P = J^T * lambda + linearImpulseBody1 = Vector3(0.0, 0.0, 0.0); + angularImpulseBody1 = -contactManifold.normal * contactManifold.frictionTwistImpulse; + linearImpulseBody2 = Vector3(0.0, 0.0, 0.0); + angularImpulseBody2 = contactManifold.normal * contactManifold.frictionTwistImpulse; + const Impulse impulseTwistFriction(linearImpulseBody1, angularImpulseBody1, + linearImpulseBody2, angularImpulseBody2); - // Apply the impulses to the bodies of the constraint - applyImpulse(impulseTwistFriction, contactManifold); + // Apply the impulses to the bodies of the constraint + applyImpulse(impulseTwistFriction, contactManifold); - // ------ Rolling resistance at the center of the contact manifold ------ // + // ------ Rolling resistance at the center of the contact manifold ------ // - // Compute the impulse P = J^T * lambda - angularImpulseBody1 = -contactManifold.rollingResistanceImpulse; - angularImpulseBody2 = contactManifold.rollingResistanceImpulse; - const Impulse impulseRollingResistance(Vector3::zero(), angularImpulseBody1, - Vector3::zero(), angularImpulseBody2); + // Compute the impulse P = J^T * lambda + angularImpulseBody1 = -contactManifold.rollingResistanceImpulse; + angularImpulseBody2 = contactManifold.rollingResistanceImpulse; + const Impulse impulseRollingResistance(Vector3::zero(), angularImpulseBody1, + Vector3::zero(), angularImpulseBody2); - // Apply the impulses to the bodies of the constraint - applyImpulse(impulseRollingResistance, contactManifold); - } - else { // If it is a new contact manifold + // Apply the impulses to the bodies of the constraint + applyImpulse(impulseRollingResistance, contactManifold); + } + else { // If it is a new contact manifold - // Initialize the accumulated impulses to zero - contactManifold.friction1Impulse = 0.0; - contactManifold.friction2Impulse = 0.0; - contactManifold.frictionTwistImpulse = 0.0; - contactManifold.rollingResistanceImpulse = Vector3::zero(); - } - } + // Initialize the accumulated impulses to zero + contactManifold.friction1Impulse = 0.0; + contactManifold.friction2Impulse = 0.0; + contactManifold.frictionTwistImpulse = 0.0; + contactManifold.rollingResistanceImpulse = Vector3::zero(); + } + } } // Solve the contacts void ContactSolver::solve() { - PROFILE("ContactSolver::solve()"); + PROFILE("ContactSolver::solve()"); - decimal deltaLambda; - decimal lambdaTemp; + float deltaLambda; + float lambdaTemp; - // For each contact manifold - for (uint c=0; c SLOP) biasPenetrationDepth = -(beta/mTimeStep) * - max(0.0f, float(contactPoint.penetrationDepth - SLOP)); - decimal b = biasPenetrationDepth + contactPoint.restitutionBias; + // Compute the bias "b" of the constraint + float beta = mIsSplitImpulseActive ? BETA_SPLIT_IMPULSE : BETA; + float biasPenetrationDepth = 0.0; + if (contactPoint.penetrationDepth > SLOP) biasPenetrationDepth = -(beta/mTimeStep) * + max(0.0f, float(contactPoint.penetrationDepth - SLOP)); + float b = biasPenetrationDepth + contactPoint.restitutionBias; - // Compute the Lagrange multiplier lambda - if (mIsSplitImpulseActive) { - deltaLambda = - (Jv + contactPoint.restitutionBias) * - contactPoint.inversePenetrationMass; - } - else { - deltaLambda = - (Jv + b) * contactPoint.inversePenetrationMass; - } - lambdaTemp = contactPoint.penetrationImpulse; - contactPoint.penetrationImpulse = std::max(contactPoint.penetrationImpulse + - deltaLambda, decimal(0.0)); - deltaLambda = contactPoint.penetrationImpulse - lambdaTemp; + // Compute the Lagrange multiplier lambda + if (mIsSplitImpulseActive) { + deltaLambda = - (Jv + contactPoint.restitutionBias) * + contactPoint.inversePenetrationMass; + } + else { + deltaLambda = - (Jv + b) * contactPoint.inversePenetrationMass; + } + lambdaTemp = contactPoint.penetrationImpulse; + contactPoint.penetrationImpulse = std::max(contactPoint.penetrationImpulse + + deltaLambda, float(0.0)); + deltaLambda = contactPoint.penetrationImpulse - lambdaTemp; - // Compute the impulse P=J^T * lambda - const Impulse impulsePenetration = computePenetrationImpulse(deltaLambda, - contactPoint); + // Compute the impulse P=J^T * lambda + const Impulse impulsePenetration = computePenetrationImpulse(deltaLambda, + contactPoint); - // Apply the impulse to the bodies of the constraint - applyImpulse(impulsePenetration, contactManifold); + // Apply the impulse to the bodies of the constraint + applyImpulse(impulsePenetration, contactManifold); - sumPenetrationImpulse += contactPoint.penetrationImpulse; + sumPenetrationImpulse += contactPoint.penetrationImpulse; - // If the split impulse position correction is active - if (mIsSplitImpulseActive) { + // If the split impulse position correction is active + if (mIsSplitImpulseActive) { - // Split impulse (position correction) - const Vector3& v1Split = mSplitLinearVelocities[contactManifold.indexBody1]; - const Vector3& w1Split = mSplitAngularVelocities[contactManifold.indexBody1]; - const Vector3& v2Split = mSplitLinearVelocities[contactManifold.indexBody2]; - const Vector3& w2Split = mSplitAngularVelocities[contactManifold.indexBody2]; - Vector3 deltaVSplit = v2Split + w2Split.cross(contactPoint.r2) - - v1Split - w1Split.cross(contactPoint.r1); - decimal JvSplit = deltaVSplit.dot(contactPoint.normal); - decimal deltaLambdaSplit = - (JvSplit + biasPenetrationDepth) * - contactPoint.inversePenetrationMass; - decimal lambdaTempSplit = contactPoint.penetrationSplitImpulse; - contactPoint.penetrationSplitImpulse = std::max( - contactPoint.penetrationSplitImpulse + - deltaLambdaSplit, decimal(0.0)); - deltaLambda = contactPoint.penetrationSplitImpulse - lambdaTempSplit; + // Split impulse (position correction) + const Vector3& v1Split = mSplitLinearVelocities[contactManifold.indexBody1]; + const Vector3& w1Split = mSplitAngularVelocities[contactManifold.indexBody1]; + const Vector3& v2Split = mSplitLinearVelocities[contactManifold.indexBody2]; + const Vector3& w2Split = mSplitAngularVelocities[contactManifold.indexBody2]; + Vector3 deltaVSplit = v2Split + w2Split.cross(contactPoint.r2) - + v1Split - w1Split.cross(contactPoint.r1); + float JvSplit = deltaVSplit.dot(contactPoint.normal); + float deltaLambdaSplit = - (JvSplit + biasPenetrationDepth) * + contactPoint.inversePenetrationMass; + float lambdaTempSplit = contactPoint.penetrationSplitImpulse; + contactPoint.penetrationSplitImpulse = std::max( + contactPoint.penetrationSplitImpulse + + deltaLambdaSplit, float(0.0)); + deltaLambda = contactPoint.penetrationSplitImpulse - lambdaTempSplit; - // Compute the impulse P=J^T * lambda - const Impulse splitImpulsePenetration = computePenetrationImpulse( - deltaLambdaSplit, contactPoint); + // Compute the impulse P=J^T * lambda + const Impulse splitImpulsePenetration = computePenetrationImpulse( + deltaLambdaSplit, contactPoint); - applySplitImpulse(splitImpulsePenetration, contactManifold); - } + applySplitImpulse(splitImpulsePenetration, contactManifold); + } - // If we do not solve the friction constraints at the center of the contact manifold - if (!mIsSolveFrictionAtContactManifoldCenterActive) { + // If we do not solve the friction constraints at the center of the contact manifold + if (!mIsSolveFrictionAtContactManifoldCenterActive) { - // --------- Friction 1 --------- // + // --------- Friction 1 --------- // - // Compute J*v - deltaV = v2 + w2.cross(contactPoint.r2) - v1 - w1.cross(contactPoint.r1); - Jv = deltaV.dot(contactPoint.frictionVector1); + // Compute J*v + deltaV = v2 + w2.cross(contactPoint.r2) - v1 - w1.cross(contactPoint.r1); + Jv = deltaV.dot(contactPoint.frictionVector1); - // Compute the Lagrange multiplier lambda - deltaLambda = -Jv; - deltaLambda *= contactPoint.inverseFriction1Mass; - decimal frictionLimit = contactManifold.frictionCoefficient * - contactPoint.penetrationImpulse; - lambdaTemp = contactPoint.friction1Impulse; - contactPoint.friction1Impulse = std::max(-frictionLimit, - std::min(contactPoint.friction1Impulse - + deltaLambda, frictionLimit)); - deltaLambda = contactPoint.friction1Impulse - lambdaTemp; + // Compute the Lagrange multiplier lambda + deltaLambda = -Jv; + deltaLambda *= contactPoint.inverseFriction1Mass; + float frictionLimit = contactManifold.frictionCoefficient * + contactPoint.penetrationImpulse; + lambdaTemp = contactPoint.friction1Impulse; + contactPoint.friction1Impulse = std::max(-frictionLimit, + std::min(contactPoint.friction1Impulse + + deltaLambda, frictionLimit)); + deltaLambda = contactPoint.friction1Impulse - lambdaTemp; - // Compute the impulse P=J^T * lambda - const Impulse impulseFriction1 = computeFriction1Impulse(deltaLambda, - contactPoint); + // Compute the impulse P=J^T * lambda + const Impulse impulseFriction1 = computeFriction1Impulse(deltaLambda, + contactPoint); - // Apply the impulses to the bodies of the constraint - applyImpulse(impulseFriction1, contactManifold); + // Apply the impulses to the bodies of the constraint + applyImpulse(impulseFriction1, contactManifold); - // --------- Friction 2 --------- // + // --------- Friction 2 --------- // - // Compute J*v - deltaV = v2 + w2.cross(contactPoint.r2) - v1 - w1.cross(contactPoint.r1); - Jv = deltaV.dot(contactPoint.frictionVector2); + // Compute J*v + deltaV = v2 + w2.cross(contactPoint.r2) - v1 - w1.cross(contactPoint.r1); + Jv = deltaV.dot(contactPoint.frictionVector2); - // Compute the Lagrange multiplier lambda - deltaLambda = -Jv; - deltaLambda *= contactPoint.inverseFriction2Mass; - frictionLimit = contactManifold.frictionCoefficient * - contactPoint.penetrationImpulse; - lambdaTemp = contactPoint.friction2Impulse; - contactPoint.friction2Impulse = std::max(-frictionLimit, - std::min(contactPoint.friction2Impulse - + deltaLambda, frictionLimit)); - deltaLambda = contactPoint.friction2Impulse - lambdaTemp; + // Compute the Lagrange multiplier lambda + deltaLambda = -Jv; + deltaLambda *= contactPoint.inverseFriction2Mass; + frictionLimit = contactManifold.frictionCoefficient * + contactPoint.penetrationImpulse; + lambdaTemp = contactPoint.friction2Impulse; + contactPoint.friction2Impulse = std::max(-frictionLimit, + std::min(contactPoint.friction2Impulse + + deltaLambda, frictionLimit)); + deltaLambda = contactPoint.friction2Impulse - lambdaTemp; - // Compute the impulse P=J^T * lambda - const Impulse impulseFriction2 = computeFriction2Impulse(deltaLambda, - contactPoint); + // Compute the impulse P=J^T * lambda + const Impulse impulseFriction2 = computeFriction2Impulse(deltaLambda, + contactPoint); - // Apply the impulses to the bodies of the constraint - applyImpulse(impulseFriction2, contactManifold); + // Apply the impulses to the bodies of the constraint + applyImpulse(impulseFriction2, contactManifold); - // --------- Rolling resistance constraint --------- // + // --------- Rolling resistance constraint --------- // - if (contactManifold.rollingResistanceFactor > 0) { + if (contactManifold.rollingResistanceFactor > 0) { - // Compute J*v - const Vector3 JvRolling = w2 - w1; + // Compute J*v + const Vector3 JvRolling = w2 - w1; - // Compute the Lagrange multiplier lambda - Vector3 deltaLambdaRolling = contactManifold.inverseRollingResistance * (-JvRolling); - decimal rollingLimit = contactManifold.rollingResistanceFactor * contactPoint.penetrationImpulse; - Vector3 lambdaTempRolling = contactPoint.rollingResistanceImpulse; - contactPoint.rollingResistanceImpulse = clamp(contactPoint.rollingResistanceImpulse + - deltaLambdaRolling, rollingLimit); - deltaLambdaRolling = contactPoint.rollingResistanceImpulse - lambdaTempRolling; + // Compute the Lagrange multiplier lambda + Vector3 deltaLambdaRolling = contactManifold.inverseRollingResistance * (-JvRolling); + float rollingLimit = contactManifold.rollingResistanceFactor * contactPoint.penetrationImpulse; + Vector3 lambdaTempRolling = contactPoint.rollingResistanceImpulse; + contactPoint.rollingResistanceImpulse = clamp(contactPoint.rollingResistanceImpulse + + deltaLambdaRolling, rollingLimit); + deltaLambdaRolling = contactPoint.rollingResistanceImpulse - lambdaTempRolling; - // Compute the impulse P=J^T * lambda - const Impulse impulseRolling(Vector3::zero(), -deltaLambdaRolling, - Vector3::zero(), deltaLambdaRolling); + // Compute the impulse P=J^T * lambda + const Impulse impulseRolling(Vector3::zero(), -deltaLambdaRolling, + Vector3::zero(), deltaLambdaRolling); - // Apply the impulses to the bodies of the constraint - applyImpulse(impulseRolling, contactManifold); - } - } - } + // Apply the impulses to the bodies of the constraint + applyImpulse(impulseRolling, contactManifold); + } + } + } - // If we solve the friction constraints at the center of the contact manifold - if (mIsSolveFrictionAtContactManifoldCenterActive) { + // If we solve the friction constraints at the center of the contact manifold + if (mIsSolveFrictionAtContactManifoldCenterActive) { - // ------ First friction constraint at the center of the contact manifol ------ // + // ------ First friction constraint at the center of the contact manifol ------ // - // Compute J*v - Vector3 deltaV = v2 + w2.cross(contactManifold.r2Friction) - - v1 - w1.cross(contactManifold.r1Friction); - decimal Jv = deltaV.dot(contactManifold.frictionVector1); + // Compute J*v + Vector3 deltaV = v2 + w2.cross(contactManifold.r2Friction) + - v1 - w1.cross(contactManifold.r1Friction); + float Jv = deltaV.dot(contactManifold.frictionVector1); - // Compute the Lagrange multiplier lambda - decimal deltaLambda = -Jv * contactManifold.inverseFriction1Mass; - decimal frictionLimit = contactManifold.frictionCoefficient * sumPenetrationImpulse; - lambdaTemp = contactManifold.friction1Impulse; - contactManifold.friction1Impulse = std::max(-frictionLimit, - std::min(contactManifold.friction1Impulse + - deltaLambda, frictionLimit)); - deltaLambda = contactManifold.friction1Impulse - lambdaTemp; + // Compute the Lagrange multiplier lambda + float deltaLambda = -Jv * contactManifold.inverseFriction1Mass; + float frictionLimit = contactManifold.frictionCoefficient * sumPenetrationImpulse; + lambdaTemp = contactManifold.friction1Impulse; + contactManifold.friction1Impulse = std::max(-frictionLimit, + std::min(contactManifold.friction1Impulse + + deltaLambda, frictionLimit)); + deltaLambda = contactManifold.friction1Impulse - lambdaTemp; - // Compute the impulse P=J^T * lambda - Vector3 linearImpulseBody1 = -contactManifold.frictionVector1 * deltaLambda; - Vector3 angularImpulseBody1 = -contactManifold.r1CrossT1 * deltaLambda; - Vector3 linearImpulseBody2 = contactManifold.frictionVector1 * deltaLambda; - Vector3 angularImpulseBody2 = contactManifold.r2CrossT1 * deltaLambda; - const Impulse impulseFriction1(linearImpulseBody1, angularImpulseBody1, - linearImpulseBody2, angularImpulseBody2); + // Compute the impulse P=J^T * lambda + Vector3 linearImpulseBody1 = -contactManifold.frictionVector1 * deltaLambda; + Vector3 angularImpulseBody1 = -contactManifold.r1CrossT1 * deltaLambda; + Vector3 linearImpulseBody2 = contactManifold.frictionVector1 * deltaLambda; + Vector3 angularImpulseBody2 = contactManifold.r2CrossT1 * deltaLambda; + const Impulse impulseFriction1(linearImpulseBody1, angularImpulseBody1, + linearImpulseBody2, angularImpulseBody2); - // Apply the impulses to the bodies of the constraint - applyImpulse(impulseFriction1, contactManifold); + // Apply the impulses to the bodies of the constraint + applyImpulse(impulseFriction1, contactManifold); - // ------ Second friction constraint at the center of the contact manifol ----- // + // ------ Second friction constraint at the center of the contact manifol ----- // - // Compute J*v - deltaV = v2 + w2.cross(contactManifold.r2Friction) - - v1 - w1.cross(contactManifold.r1Friction); - Jv = deltaV.dot(contactManifold.frictionVector2); + // Compute J*v + deltaV = v2 + w2.cross(contactManifold.r2Friction) + - v1 - w1.cross(contactManifold.r1Friction); + Jv = deltaV.dot(contactManifold.frictionVector2); - // Compute the Lagrange multiplier lambda - deltaLambda = -Jv * contactManifold.inverseFriction2Mass; - frictionLimit = contactManifold.frictionCoefficient * sumPenetrationImpulse; - lambdaTemp = contactManifold.friction2Impulse; - contactManifold.friction2Impulse = std::max(-frictionLimit, - std::min(contactManifold.friction2Impulse + - deltaLambda, frictionLimit)); - deltaLambda = contactManifold.friction2Impulse - lambdaTemp; + // Compute the Lagrange multiplier lambda + deltaLambda = -Jv * contactManifold.inverseFriction2Mass; + frictionLimit = contactManifold.frictionCoefficient * sumPenetrationImpulse; + lambdaTemp = contactManifold.friction2Impulse; + contactManifold.friction2Impulse = std::max(-frictionLimit, + std::min(contactManifold.friction2Impulse + + deltaLambda, frictionLimit)); + deltaLambda = contactManifold.friction2Impulse - lambdaTemp; - // Compute the impulse P=J^T * lambda - linearImpulseBody1 = -contactManifold.frictionVector2 * deltaLambda; - angularImpulseBody1 = -contactManifold.r1CrossT2 * deltaLambda; - linearImpulseBody2 = contactManifold.frictionVector2 * deltaLambda; - angularImpulseBody2 = contactManifold.r2CrossT2 * deltaLambda; - const Impulse impulseFriction2(linearImpulseBody1, angularImpulseBody1, - linearImpulseBody2, angularImpulseBody2); + // Compute the impulse P=J^T * lambda + linearImpulseBody1 = -contactManifold.frictionVector2 * deltaLambda; + angularImpulseBody1 = -contactManifold.r1CrossT2 * deltaLambda; + linearImpulseBody2 = contactManifold.frictionVector2 * deltaLambda; + angularImpulseBody2 = contactManifold.r2CrossT2 * deltaLambda; + const Impulse impulseFriction2(linearImpulseBody1, angularImpulseBody1, + linearImpulseBody2, angularImpulseBody2); - // Apply the impulses to the bodies of the constraint - applyImpulse(impulseFriction2, contactManifold); + // Apply the impulses to the bodies of the constraint + applyImpulse(impulseFriction2, contactManifold); - // ------ Twist friction constraint at the center of the contact manifol ------ // + // ------ Twist friction constraint at the center of the contact manifol ------ // - // Compute J*v - deltaV = w2 - w1; - Jv = deltaV.dot(contactManifold.normal); + // Compute J*v + deltaV = w2 - w1; + Jv = deltaV.dot(contactManifold.normal); - deltaLambda = -Jv * (contactManifold.inverseTwistFrictionMass); - frictionLimit = contactManifold.frictionCoefficient * sumPenetrationImpulse; - lambdaTemp = contactManifold.frictionTwistImpulse; - contactManifold.frictionTwistImpulse = std::max(-frictionLimit, - std::min(contactManifold.frictionTwistImpulse - + deltaLambda, frictionLimit)); - deltaLambda = contactManifold.frictionTwistImpulse - lambdaTemp; + deltaLambda = -Jv * (contactManifold.inverseTwistFrictionMass); + frictionLimit = contactManifold.frictionCoefficient * sumPenetrationImpulse; + lambdaTemp = contactManifold.frictionTwistImpulse; + contactManifold.frictionTwistImpulse = std::max(-frictionLimit, + std::min(contactManifold.frictionTwistImpulse + + deltaLambda, frictionLimit)); + deltaLambda = contactManifold.frictionTwistImpulse - lambdaTemp; - // Compute the impulse P=J^T * lambda - linearImpulseBody1 = Vector3(0.0, 0.0, 0.0); - angularImpulseBody1 = -contactManifold.normal * deltaLambda; - linearImpulseBody2 = Vector3(0.0, 0.0, 0.0);; - angularImpulseBody2 = contactManifold.normal * deltaLambda; - const Impulse impulseTwistFriction(linearImpulseBody1, angularImpulseBody1, - linearImpulseBody2, angularImpulseBody2); + // Compute the impulse P=J^T * lambda + linearImpulseBody1 = Vector3(0.0, 0.0, 0.0); + angularImpulseBody1 = -contactManifold.normal * deltaLambda; + linearImpulseBody2 = Vector3(0.0, 0.0, 0.0);; + angularImpulseBody2 = contactManifold.normal * deltaLambda; + const Impulse impulseTwistFriction(linearImpulseBody1, angularImpulseBody1, + linearImpulseBody2, angularImpulseBody2); - // Apply the impulses to the bodies of the constraint - applyImpulse(impulseTwistFriction, contactManifold); + // Apply the impulses to the bodies of the constraint + applyImpulse(impulseTwistFriction, contactManifold); - // --------- Rolling resistance constraint at the center of the contact manifold --------- // + // --------- Rolling resistance constraint at the center of the contact manifold --------- // - if (contactManifold.rollingResistanceFactor > 0) { + if (contactManifold.rollingResistanceFactor > 0) { - // Compute J*v - const Vector3 JvRolling = w2 - w1; + // Compute J*v + const Vector3 JvRolling = w2 - w1; - // Compute the Lagrange multiplier lambda - Vector3 deltaLambdaRolling = contactManifold.inverseRollingResistance * (-JvRolling); - decimal rollingLimit = contactManifold.rollingResistanceFactor * sumPenetrationImpulse; - Vector3 lambdaTempRolling = contactManifold.rollingResistanceImpulse; - contactManifold.rollingResistanceImpulse = clamp(contactManifold.rollingResistanceImpulse + - deltaLambdaRolling, rollingLimit); - deltaLambdaRolling = contactManifold.rollingResistanceImpulse - lambdaTempRolling; + // Compute the Lagrange multiplier lambda + Vector3 deltaLambdaRolling = contactManifold.inverseRollingResistance * (-JvRolling); + float rollingLimit = contactManifold.rollingResistanceFactor * sumPenetrationImpulse; + Vector3 lambdaTempRolling = contactManifold.rollingResistanceImpulse; + contactManifold.rollingResistanceImpulse = clamp(contactManifold.rollingResistanceImpulse + + deltaLambdaRolling, rollingLimit); + deltaLambdaRolling = contactManifold.rollingResistanceImpulse - lambdaTempRolling; - // Compute the impulse P=J^T * lambda - angularImpulseBody1 = -deltaLambdaRolling; - angularImpulseBody2 = deltaLambdaRolling; - const Impulse impulseRolling(Vector3::zero(), angularImpulseBody1, - Vector3::zero(), angularImpulseBody2); + // Compute the impulse P=J^T * lambda + angularImpulseBody1 = -deltaLambdaRolling; + angularImpulseBody2 = deltaLambdaRolling; + const Impulse impulseRolling(Vector3::zero(), angularImpulseBody1, + Vector3::zero(), angularImpulseBody2); - // Apply the impulses to the bodies of the constraint - applyImpulse(impulseRolling, contactManifold); - } - } - } + // Apply the impulses to the bodies of the constraint + applyImpulse(impulseRolling, contactManifold); + } + } + } } // Store the computed impulses to use them to // warm start the solver at the next iteration void ContactSolver::storeImpulses() { - // For each contact manifold - for (uint c=0; csetPenetrationImpulse(contactPoint.penetrationImpulse); - contactPoint.externalContact->setFrictionImpulse1(contactPoint.friction1Impulse); - contactPoint.externalContact->setFrictionImpulse2(contactPoint.friction2Impulse); - contactPoint.externalContact->setRollingResistanceImpulse(contactPoint.rollingResistanceImpulse); + contactPoint.externalContact->setPenetrationImpulse(contactPoint.penetrationImpulse); + contactPoint.externalContact->setFrictionImpulse1(contactPoint.friction1Impulse); + contactPoint.externalContact->setFrictionImpulse2(contactPoint.friction2Impulse); + contactPoint.externalContact->setRollingResistanceImpulse(contactPoint.rollingResistanceImpulse); - contactPoint.externalContact->setFrictionVector1(contactPoint.frictionVector1); - contactPoint.externalContact->setFrictionVector2(contactPoint.frictionVector2); - } + contactPoint.externalContact->setFrictionVector1(contactPoint.frictionVector1); + contactPoint.externalContact->setFrictionVector2(contactPoint.frictionVector2); + } - manifold.externalContactManifold->setFrictionImpulse1(manifold.friction1Impulse); - manifold.externalContactManifold->setFrictionImpulse2(manifold.friction2Impulse); - manifold.externalContactManifold->setFrictionTwistImpulse(manifold.frictionTwistImpulse); - manifold.externalContactManifold->setRollingResistanceImpulse(manifold.rollingResistanceImpulse); - manifold.externalContactManifold->setFrictionVector1(manifold.frictionVector1); - manifold.externalContactManifold->setFrictionVector2(manifold.frictionVector2); - } + manifold.externalContactManifold->setFrictionImpulse1(manifold.friction1Impulse); + manifold.externalContactManifold->setFrictionImpulse2(manifold.friction2Impulse); + manifold.externalContactManifold->setFrictionTwistImpulse(manifold.frictionTwistImpulse); + manifold.externalContactManifold->setRollingResistanceImpulse(manifold.rollingResistanceImpulse); + manifold.externalContactManifold->setFrictionVector1(manifold.frictionVector1); + manifold.externalContactManifold->setFrictionVector2(manifold.frictionVector2); + } } // Apply an impulse to the two bodies of a constraint void ContactSolver::applyImpulse(const Impulse& impulse, - const ContactManifoldSolver& manifold) { + const ContactManifoldSolver& manifold) { - // Update the velocities of the body 1 by applying the impulse P - mLinearVelocities[manifold.indexBody1] += manifold.massInverseBody1 * - impulse.linearImpulseBody1; - mAngularVelocities[manifold.indexBody1] += manifold.inverseInertiaTensorBody1 * - impulse.angularImpulseBody1; + // Update the velocities of the body 1 by applying the impulse P + mLinearVelocities[manifold.indexBody1] += manifold.massInverseBody1 * + impulse.linearImpulseBody1; + mAngularVelocities[manifold.indexBody1] += manifold.inverseInertiaTensorBody1 * + impulse.angularImpulseBody1; - // Update the velocities of the body 1 by applying the impulse P - mLinearVelocities[manifold.indexBody2] += manifold.massInverseBody2 * - impulse.linearImpulseBody2; - mAngularVelocities[manifold.indexBody2] += manifold.inverseInertiaTensorBody2 * - impulse.angularImpulseBody2; + // Update the velocities of the body 1 by applying the impulse P + mLinearVelocities[manifold.indexBody2] += manifold.massInverseBody2 * + impulse.linearImpulseBody2; + mAngularVelocities[manifold.indexBody2] += manifold.inverseInertiaTensorBody2 * + impulse.angularImpulseBody2; } // Apply an impulse to the two bodies of a constraint void ContactSolver::applySplitImpulse(const Impulse& impulse, - const ContactManifoldSolver& manifold) { + const ContactManifoldSolver& manifold) { - // Update the velocities of the body 1 by applying the impulse P - mSplitLinearVelocities[manifold.indexBody1] += manifold.massInverseBody1 * - impulse.linearImpulseBody1; - mSplitAngularVelocities[manifold.indexBody1] += manifold.inverseInertiaTensorBody1 * - impulse.angularImpulseBody1; + // Update the velocities of the body 1 by applying the impulse P + mSplitLinearVelocities[manifold.indexBody1] += manifold.massInverseBody1 * + impulse.linearImpulseBody1; + mSplitAngularVelocities[manifold.indexBody1] += manifold.inverseInertiaTensorBody1 * + impulse.angularImpulseBody1; - // Update the velocities of the body 1 by applying the impulse P - mSplitLinearVelocities[manifold.indexBody2] += manifold.massInverseBody2 * - impulse.linearImpulseBody2; - mSplitAngularVelocities[manifold.indexBody2] += manifold.inverseInertiaTensorBody2 * - impulse.angularImpulseBody2; + // Update the velocities of the body 1 by applying the impulse P + mSplitLinearVelocities[manifold.indexBody2] += manifold.massInverseBody2 * + impulse.linearImpulseBody2; + mSplitAngularVelocities[manifold.indexBody2] += manifold.inverseInertiaTensorBody2 * + impulse.angularImpulseBody2; } // Compute the two unit orthogonal vectors "t1" and "t2" that span the tangential friction plane // for a contact point. The two vectors have to be such that : t1 x t2 = contactNormal. void ContactSolver::computeFrictionVectors(const Vector3& deltaVelocity, - ContactPointSolver& contactPoint) const { + ContactPointSolver& contactPoint) const { - assert(contactPoint.normal.length() > 0.0); + assert(contactPoint.normal.length() > 0.0); - // Compute the velocity difference vector in the tangential plane - Vector3 normalVelocity = deltaVelocity.dot(contactPoint.normal) * contactPoint.normal; - Vector3 tangentVelocity = deltaVelocity - normalVelocity; + // Compute the velocity difference vector in the tangential plane + Vector3 normalVelocity = deltaVelocity.dot(contactPoint.normal) * contactPoint.normal; + Vector3 tangentVelocity = deltaVelocity - normalVelocity; - // If the velocty difference in the tangential plane is not zero - decimal lengthTangenVelocity = tangentVelocity.length(); - if (lengthTangenVelocity > MACHINE_EPSILON) { + // If the velocty difference in the tangential plane is not zero + float lengthTangenVelocity = tangentVelocity.length(); + if (lengthTangenVelocity > MACHINE_EPSILON) { - // Compute the first friction vector in the direction of the tangent - // velocity difference - contactPoint.frictionVector1 = tangentVelocity / lengthTangenVelocity; - } - else { + // Compute the first friction vector in the direction of the tangent + // velocity difference + contactPoint.frictionVector1 = tangentVelocity / lengthTangenVelocity; + } + else { - // Get any orthogonal vector to the normal as the first friction vector - contactPoint.frictionVector1 = contactPoint.normal.getOneUnitOrthogonalVector(); - } + // Get any orthogonal vector to the normal as the first friction vector + contactPoint.frictionVector1 = contactPoint.normal.getOneUnitOrthogonalVector(); + } - // The second friction vector is computed by the cross product of the firs - // friction vector and the contact normal - contactPoint.frictionVector2 =contactPoint.normal.cross(contactPoint.frictionVector1).getUnit(); + // The second friction vector is computed by the cross product of the firs + // friction vector and the contact normal + contactPoint.frictionVector2 =contactPoint.normal.cross(contactPoint.frictionVector1).getUnit(); } // 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 ContactSolver::computeFrictionVectors(const Vector3& deltaVelocity, - ContactManifoldSolver& contact) const { + ContactManifoldSolver& contact) const { - assert(contact.normal.length() > 0.0); + assert(contact.normal.length() > 0.0); - // Compute the velocity difference vector in the tangential plane - Vector3 normalVelocity = deltaVelocity.dot(contact.normal) * contact.normal; - Vector3 tangentVelocity = deltaVelocity - normalVelocity; + // Compute the velocity difference vector in the tangential plane + Vector3 normalVelocity = deltaVelocity.dot(contact.normal) * contact.normal; + Vector3 tangentVelocity = deltaVelocity - normalVelocity; - // If the velocty difference in the tangential plane is not zero - decimal lengthTangenVelocity = tangentVelocity.length(); - if (lengthTangenVelocity > MACHINE_EPSILON) { + // If the velocty difference in the tangential plane is not zero + float lengthTangenVelocity = tangentVelocity.length(); + if (lengthTangenVelocity > MACHINE_EPSILON) { - // Compute the first friction vector in the direction of the tangent - // velocity difference - contact.frictionVector1 = tangentVelocity / lengthTangenVelocity; - } - else { + // Compute the first friction vector in the direction of the tangent + // velocity difference + contact.frictionVector1 = tangentVelocity / lengthTangenVelocity; + } + else { - // Get any orthogonal vector to the normal as the first friction vector - contact.frictionVector1 = contact.normal.getOneUnitOrthogonalVector(); - } + // Get any orthogonal vector to the normal as the first friction vector + contact.frictionVector1 = contact.normal.getOneUnitOrthogonalVector(); + } - // The second friction vector is computed by the cross product of the firs - // friction vector and the contact normal - contact.frictionVector2 = contact.normal.cross(contact.frictionVector1).getUnit(); + // The second friction vector is computed by the cross product of the firs + // friction vector and the contact normal + contact.frictionVector2 = contact.normal.cross(contact.frictionVector1).getUnit(); } // Clean up the constraint solver void ContactSolver::cleanup() { - if (mContactConstraints != NULL) { - delete[] mContactConstraints; - mContactConstraints = NULL; - } + if (mContactConstraints != NULL) { + delete[] mContactConstraints; + mContactConstraints = NULL; + } } diff --git a/ephysics/engine/ContactSolver.h b/ephysics/engine/ContactSolver.h index ed058b3..fc847f2 100644 --- a/ephysics/engine/ContactSolver.h +++ b/ephysics/engine/ContactSolver.h @@ -1,30 +1,9 @@ -/******************************************************************************** -* 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 REACTPHYSICS3D_CONTACT_SOLVER_H -#define REACTPHYSICS3D_CONTACT_SOLVER_H +/** @file + * @author Daniel Chappuis + * @copyright 2010-2016 Daniel Chappuis + * @license BSD 3 clauses (see license file) + */ +#pragma once // Libraries #include @@ -62,7 +41,7 @@ namespace reactphysics3d { * * --- Step 1 --- * - * First, we integrate the applied force F_a acting of each rigid body (like gravity, ...) and + * First, we int32_tegrate the applied force F_a acting of each rigid body (like gravity, ...) and * we obtain some new velocities v2' that tends to violate the constraints. * * v2' = v1 + dt * M^-1 * F_a @@ -83,7 +62,7 @@ namespace reactphysics3d { * * --- Step 3 --- * - * In the third step, we integrate the new position x2 of the bodies using the new velocities + * In the third step, we int32_tegrate the new position x2 of the bodies using the new velocities * v2 computed in the second step with : x2 = x1 + dt * v2. * * Note that in the following code (as it is also explained in the slides from Erin Catto), @@ -111,431 +90,429 @@ namespace reactphysics3d { */ class ContactSolver { - private: + private: - // Structure ContactPointSolver - /** - * Contact solver internal data structure that to store all the - * information relative to a contact point - */ - struct ContactPointSolver { + // Structure ContactPointSolver + /** + * Contact solver int32_ternal data structure that to store all the + * information relative to a contact point + */ + struct ContactPointSolver { - /// Accumulated normal impulse - decimal penetrationImpulse; + /// Accumulated normal impulse + float penetrationImpulse; - /// Accumulated impulse in the 1st friction direction - decimal friction1Impulse; + /// Accumulated impulse in the 1st friction direction + float friction1Impulse; - /// Accumulated impulse in the 2nd friction direction - decimal friction2Impulse; + /// Accumulated impulse in the 2nd friction direction + float friction2Impulse; - /// Accumulated split impulse for penetration correction - decimal penetrationSplitImpulse; + /// Accumulated split impulse for penetration correction + float penetrationSplitImpulse; - /// Accumulated rolling resistance impulse - Vector3 rollingResistanceImpulse; + /// Accumulated rolling resistance impulse + Vector3 rollingResistanceImpulse; - /// Normal vector of the contact - Vector3 normal; + /// Normal vector of the contact + Vector3 normal; - /// First friction vector in the tangent plane - Vector3 frictionVector1; + /// First friction vector in the tangent plane + Vector3 frictionVector1; - /// Second friction vector in the tangent plane - Vector3 frictionVector2; + /// Second friction vector in the tangent plane + Vector3 frictionVector2; - /// Old first friction vector in the tangent plane - Vector3 oldFrictionVector1; + /// Old first friction vector in the tangent plane + Vector3 oldFrictionVector1; - /// Old second friction vector in the tangent plane - Vector3 oldFrictionVector2; + /// Old second friction vector in the tangent plane + Vector3 oldFrictionVector2; - /// Vector from the body 1 center to the contact point - Vector3 r1; + /// Vector from the body 1 center to the contact point + Vector3 r1; - /// Vector from the body 2 center to the contact point - Vector3 r2; + /// Vector from the body 2 center to the contact point + Vector3 r2; - /// Cross product of r1 with 1st friction vector - Vector3 r1CrossT1; + /// Cross product of r1 with 1st friction vector + Vector3 r1CrossT1; - /// Cross product of r1 with 2nd friction vector - Vector3 r1CrossT2; + /// Cross product of r1 with 2nd friction vector + Vector3 r1CrossT2; - /// Cross product of r2 with 1st friction vector - Vector3 r2CrossT1; + /// Cross product of r2 with 1st friction vector + Vector3 r2CrossT1; - /// Cross product of r2 with 2nd friction vector - Vector3 r2CrossT2; + /// Cross product of r2 with 2nd friction vector + Vector3 r2CrossT2; - /// Cross product of r1 with the contact normal - Vector3 r1CrossN; + /// Cross product of r1 with the contact normal + Vector3 r1CrossN; - /// Cross product of r2 with the contact normal - Vector3 r2CrossN; + /// Cross product of r2 with the contact normal + Vector3 r2CrossN; - /// Penetration depth - decimal penetrationDepth; + /// Penetration depth + float penetrationDepth; - /// Velocity restitution bias - decimal restitutionBias; + /// Velocity restitution bias + float restitutionBias; - /// Inverse of the matrix K for the penenetration - decimal inversePenetrationMass; + /// Inverse of the matrix K for the penenetration + float inversePenetrationMass; - /// Inverse of the matrix K for the 1st friction - decimal inverseFriction1Mass; + /// Inverse of the matrix K for the 1st friction + float inverseFriction1Mass; - /// Inverse of the matrix K for the 2nd friction - decimal inverseFriction2Mass; + /// Inverse of the matrix K for the 2nd friction + float inverseFriction2Mass; - /// True if the contact was existing last time step - bool isRestingContact; + /// True if the contact was existing last time step + bool isRestingContact; - /// Pointer to the external contact - ContactPoint* externalContact; - }; + /// Pointer to the external contact + ContactPoint* externalContact; + }; - // Structure ContactManifoldSolver - /** - * Contact solver internal data structure to store all the - * information relative to a contact manifold. - */ - struct ContactManifoldSolver { + // Structure ContactManifoldSolver + /** + * Contact solver int32_ternal data structure to store all the + * information relative to a contact manifold. + */ + struct ContactManifoldSolver { - /// Index of body 1 in the constraint solver - uint indexBody1; + /// Index of body 1 in the constraint solver + uint32_t indexBody1; - /// Index of body 2 in the constraint solver - uint indexBody2; + /// Index of body 2 in the constraint solver + uint32_t indexBody2; - /// Inverse of the mass of body 1 - decimal massInverseBody1; + /// Inverse of the mass of body 1 + float massInverseBody1; - // Inverse of the mass of body 2 - decimal massInverseBody2; + // Inverse of the mass of body 2 + float massInverseBody2; - /// Inverse inertia tensor of body 1 - Matrix3x3 inverseInertiaTensorBody1; + /// Inverse inertia tensor of body 1 + Matrix3x3 inverseInertiaTensorBody1; - /// Inverse inertia tensor of body 2 - Matrix3x3 inverseInertiaTensorBody2; + /// Inverse inertia tensor of body 2 + Matrix3x3 inverseInertiaTensorBody2; - /// Contact point constraints - ContactPointSolver contacts[MAX_CONTACT_POINTS_IN_MANIFOLD]; + /// Contact point constraints + ContactPointSolver contacts[MAX_CONTACT_POINTS_IN_MANIFOLD]; - /// Number of contact points - uint nbContacts; + /// Number of contact points + uint32_t nbContacts; - /// True if the body 1 is of type dynamic - bool isBody1DynamicType; + /// True if the body 1 is of type dynamic + bool isBody1DynamicType; - /// True if the body 2 is of type dynamic - bool isBody2DynamicType; + /// True if the body 2 is of type dynamic + bool isBody2DynamicType; - /// Mix of the restitution factor for two bodies - decimal restitutionFactor; + /// Mix of the restitution factor for two bodies + float restitutionFactor; - /// Mix friction coefficient for the two bodies - decimal frictionCoefficient; + /// Mix friction coefficient for the two bodies + float frictionCoefficient; - /// Rolling resistance factor between the two bodies - decimal rollingResistanceFactor; + /// Rolling resistance factor between the two bodies + float rollingResistanceFactor; - /// Pointer to the external contact manifold - ContactManifold* externalContactManifold; + /// Pointer to the external contact manifold + ContactManifold* externalContactManifold; - // - Variables used when friction constraints are apply at the center of the manifold-// + // - Variables used when friction constraints are apply at the center of the manifold-// - /// Average normal vector of the contact manifold - Vector3 normal; + /// Average normal vector of the contact manifold + Vector3 normal; - /// Point on body 1 where to apply the friction constraints - Vector3 frictionPointBody1; + /// Point on body 1 where to apply the friction constraints + Vector3 frictionPointBody1; - /// Point on body 2 where to apply the friction constraints - Vector3 frictionPointBody2; + /// Point on body 2 where to apply the friction constraints + Vector3 frictionPointBody2; - /// R1 vector for the friction constraints - Vector3 r1Friction; + /// R1 vector for the friction constraints + Vector3 r1Friction; - /// R2 vector for the friction constraints - Vector3 r2Friction; + /// R2 vector for the friction constraints + Vector3 r2Friction; - /// Cross product of r1 with 1st friction vector - Vector3 r1CrossT1; + /// Cross product of r1 with 1st friction vector + Vector3 r1CrossT1; - /// Cross product of r1 with 2nd friction vector - Vector3 r1CrossT2; + /// Cross product of r1 with 2nd friction vector + Vector3 r1CrossT2; - /// Cross product of r2 with 1st friction vector - Vector3 r2CrossT1; + /// Cross product of r2 with 1st friction vector + Vector3 r2CrossT1; - /// Cross product of r2 with 2nd friction vector - Vector3 r2CrossT2; + /// Cross product of r2 with 2nd friction vector + Vector3 r2CrossT2; - /// Matrix K for the first friction constraint - decimal inverseFriction1Mass; + /// Matrix K for the first friction constraint + float inverseFriction1Mass; - /// Matrix K for the second friction constraint - decimal inverseFriction2Mass; + /// Matrix K for the second friction constraint + float inverseFriction2Mass; - /// Matrix K for the twist friction constraint - decimal inverseTwistFrictionMass; + /// Matrix K for the twist friction constraint + float inverseTwistFrictionMass; - /// Matrix K for the rolling resistance constraint - Matrix3x3 inverseRollingResistance; + /// Matrix K for the rolling resistance constraint + Matrix3x3 inverseRollingResistance; - /// First friction direction at contact manifold center - Vector3 frictionVector1; + /// First friction direction at contact manifold center + Vector3 frictionVector1; - /// Second friction direction at contact manifold center - Vector3 frictionVector2; + /// Second friction direction at contact manifold center + Vector3 frictionVector2; - /// Old 1st friction direction at contact manifold center - Vector3 oldFrictionVector1; + /// Old 1st friction direction at contact manifold center + Vector3 oldFrictionVector1; - /// Old 2nd friction direction at contact manifold center - Vector3 oldFrictionVector2; + /// Old 2nd friction direction at contact manifold center + Vector3 oldFrictionVector2; - /// First friction direction impulse at manifold center - decimal friction1Impulse; + /// First friction direction impulse at manifold center + float friction1Impulse; - /// Second friction direction impulse at manifold center - decimal friction2Impulse; + /// Second friction direction impulse at manifold center + float friction2Impulse; - /// Twist friction impulse at contact manifold center - decimal frictionTwistImpulse; + /// Twist friction impulse at contact manifold center + float frictionTwistImpulse; - /// Rolling resistance impulse - Vector3 rollingResistanceImpulse; - }; + /// Rolling resistance impulse + Vector3 rollingResistanceImpulse; + }; - // -------------------- Constants --------------------- // + // -------------------- Constants --------------------- // - /// Beta value for the penetration depth position correction without split impulses - static const decimal BETA; + /// Beta value for the penetration depth position correction without split impulses + static const float BETA; - /// Beta value for the penetration depth position correction with split impulses - static const decimal BETA_SPLIT_IMPULSE; + /// Beta value for the penetration depth position correction with split impulses + static const float BETA_SPLIT_IMPULSE; - /// Slop distance (allowed penetration distance between bodies) - static const decimal SLOP; + /// Slop distance (allowed penetration distance between bodies) + static const float SLOP; - // -------------------- Attributes -------------------- // + // -------------------- Attributes -------------------- // - /// Split linear velocities for the position contact solver (split impulse) - Vector3* mSplitLinearVelocities; + /// Split linear velocities for the position contact solver (split impulse) + Vector3* mSplitLinearVelocities; - /// Split angular velocities for the position contact solver (split impulse) - Vector3* mSplitAngularVelocities; + /// Split angular velocities for the position contact solver (split impulse) + Vector3* mSplitAngularVelocities; - /// Current time step - decimal mTimeStep; + /// Current time step + float mTimeStep; - /// Contact constraints - ContactManifoldSolver* mContactConstraints; + /// Contact constraints + ContactManifoldSolver* mContactConstraints; - /// Number of contact constraints - uint mNbContactManifolds; + /// Number of contact constraints + uint32_t mNbContactManifolds; - /// Array of linear velocities - Vector3* mLinearVelocities; + /// Array of linear velocities + Vector3* mLinearVelocities; - /// Array of angular velocities - Vector3* mAngularVelocities; + /// Array of angular velocities + Vector3* mAngularVelocities; - /// Reference to the map of rigid body to their index in the constrained velocities array - const std::map& mMapBodyToConstrainedVelocityIndex; + /// Reference to the map of rigid body to their index in the constrained velocities array + const std::map& mMapBodyToConstrainedVelocityIndex; - /// True if the warm starting of the solver is active - bool mIsWarmStartingActive; + /// True if the warm starting of the solver is active + bool mIsWarmStartingActive; - /// True if the split impulse position correction is active - bool mIsSplitImpulseActive; + /// True if the split impulse position correction is active + bool mIsSplitImpulseActive; - /// True if we solve 3 friction constraints at the contact manifold center only - /// instead of 2 friction constraints at each contact point - bool mIsSolveFrictionAtContactManifoldCenterActive; + /// True if we solve 3 friction constraints at the contact manifold center only + /// instead of 2 friction constraints at each contact point + bool mIsSolveFrictionAtContactManifoldCenterActive; - // -------------------- Methods -------------------- // + // -------------------- Methods -------------------- // - /// Initialize the contact constraints before solving the system - void initializeContactConstraints(); + /// Initialize the contact constraints before solving the system + void initializeContactConstraints(); - /// Apply an impulse to the two bodies of a constraint - void applyImpulse(const Impulse& impulse, const ContactManifoldSolver& manifold); + /// Apply an impulse to the two bodies of a constraint + void applyImpulse(const Impulse& impulse, const ContactManifoldSolver& manifold); - /// Apply an impulse to the two bodies of a constraint - void applySplitImpulse(const Impulse& impulse, - const ContactManifoldSolver& manifold); + /// Apply an impulse to the two bodies of a constraint + void applySplitImpulse(const Impulse& impulse, + const ContactManifoldSolver& manifold); - /// Compute the collision restitution factor from the restitution factor of each body - decimal computeMixedRestitutionFactor(RigidBody *body1, - RigidBody *body2) const; + /// Compute the collision restitution factor from the restitution factor of each body + float computeMixedRestitutionFactor(RigidBody *body1, + RigidBody *body2) const; - /// Compute the mixed friction coefficient from the friction coefficient of each body - decimal computeMixedFrictionCoefficient(RigidBody* body1, - RigidBody* body2) const; + /// Compute the mixed friction coefficient from the friction coefficient of each body + float computeMixedFrictionCoefficient(RigidBody* body1, + RigidBody* body2) const; - /// Compute th mixed rolling resistance factor between two bodies - decimal computeMixedRollingResistance(RigidBody* body1, RigidBody* body2) const; + /// Compute th mixed rolling resistance factor between two bodies + float computeMixedRollingResistance(RigidBody* body1, RigidBody* body2) const; - /// Compute the two unit orthogonal vectors "t1" and "t2" that span the tangential friction - /// plane for a contact point. The two vectors have to be - /// such that : t1 x t2 = contactNormal. - void computeFrictionVectors(const Vector3& deltaVelocity, - ContactPointSolver &contactPoint) const; + /// Compute the two unit orthogonal vectors "t1" and "t2" that span the tangential friction + /// plane for a contact point. The two vectors have to be + /// such that : t1 x t2 = contactNormal. + void computeFrictionVectors(const Vector3& deltaVelocity, + ContactPointSolver &contactPoint) 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 - /// such that : t1 x t2 = contactNormal. - void computeFrictionVectors(const Vector3& deltaVelocity, - ContactManifoldSolver& contactPoint) 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 + /// such that : t1 x t2 = contactNormal. + void computeFrictionVectors(const Vector3& deltaVelocity, + ContactManifoldSolver& contactPoint) const; - /// Compute a penetration constraint impulse - const Impulse computePenetrationImpulse(decimal deltaLambda, - const ContactPointSolver& contactPoint) const; + /// Compute a penetration constraint impulse + const Impulse computePenetrationImpulse(float deltaLambda, + const ContactPointSolver& contactPoint) const; - /// Compute the first friction constraint impulse - const Impulse computeFriction1Impulse(decimal deltaLambda, - const ContactPointSolver& contactPoint) const; + /// Compute the first friction constraint impulse + const Impulse computeFriction1Impulse(float deltaLambda, + const ContactPointSolver& contactPoint) const; - /// Compute the second friction constraint impulse - const Impulse computeFriction2Impulse(decimal deltaLambda, - const ContactPointSolver& contactPoint) const; + /// Compute the second friction constraint impulse + const Impulse computeFriction2Impulse(float deltaLambda, + const ContactPointSolver& contactPoint) const; public: - // -------------------- Methods -------------------- // + // -------------------- Methods -------------------- // - /// Constructor - ContactSolver(const std::map& mapBodyToVelocityIndex); + /// Constructor + ContactSolver(const std::map& mapBodyToVelocityIndex); - /// Destructor - virtual ~ContactSolver(); + /// Destructor + virtual ~ContactSolver(); - /// Initialize the constraint solver for a given island - void initializeForIsland(decimal dt, Island* island); + /// Initialize the constraint solver for a given island + void initializeForIsland(float dt, Island* island); - /// Set the split velocities arrays - void setSplitVelocitiesArrays(Vector3* splitLinearVelocities, - Vector3* splitAngularVelocities); + /// Set the split velocities arrays + void setSplitVelocitiesArrays(Vector3* splitLinearVelocities, + Vector3* splitAngularVelocities); - /// Set the constrained velocities arrays - void setConstrainedVelocitiesArrays(Vector3* constrainedLinearVelocities, - Vector3* constrainedAngularVelocities); + /// Set the constrained velocities arrays + void setConstrainedVelocitiesArrays(Vector3* constrainedLinearVelocities, + Vector3* constrainedAngularVelocities); - /// Warm start the solver. - void warmStart(); + /// Warm start the solver. + void warmStart(); - /// Store the computed impulses to use them to - /// warm start the solver at the next iteration - void storeImpulses(); + /// Store the computed impulses to use them to + /// warm start the solver at the next iteration + void storeImpulses(); - /// Solve the contacts - void solve(); + /// Solve the contacts + void solve(); - /// Return true if the split impulses position correction technique is used for contacts - bool isSplitImpulseActive() const; + /// Return true if the split impulses position correction technique is used for contacts + bool isSplitImpulseActive() const; - /// Activate or Deactivate the split impulses for contacts - void setIsSplitImpulseActive(bool isActive); + /// Activate or Deactivate the split impulses for contacts + void setIsSplitImpulseActive(bool isActive); - /// Activate or deactivate the solving of friction constraints at the center of - /// the contact manifold instead of solving them at each contact point - void setIsSolveFrictionAtContactManifoldCenterActive(bool isActive); + /// Activate or deactivate the solving of friction constraints at the center of + /// the contact manifold instead of solving them at each contact point + void setIsSolveFrictionAtContactManifoldCenterActive(bool isActive); - /// Clean up the constraint solver - void cleanup(); + /// Clean up the constraint solver + void cleanup(); }; // Set the split velocities arrays inline void ContactSolver::setSplitVelocitiesArrays(Vector3* splitLinearVelocities, - Vector3* splitAngularVelocities) { - assert(splitLinearVelocities != NULL); - assert(splitAngularVelocities != NULL); - mSplitLinearVelocities = splitLinearVelocities; - mSplitAngularVelocities = splitAngularVelocities; + Vector3* splitAngularVelocities) { + assert(splitLinearVelocities != NULL); + assert(splitAngularVelocities != NULL); + mSplitLinearVelocities = splitLinearVelocities; + mSplitAngularVelocities = splitAngularVelocities; } // Set the constrained velocities arrays inline void ContactSolver::setConstrainedVelocitiesArrays(Vector3* constrainedLinearVelocities, - Vector3* constrainedAngularVelocities) { - assert(constrainedLinearVelocities != NULL); - assert(constrainedAngularVelocities != NULL); - mLinearVelocities = constrainedLinearVelocities; - mAngularVelocities = constrainedAngularVelocities; + Vector3* constrainedAngularVelocities) { + assert(constrainedLinearVelocities != NULL); + assert(constrainedAngularVelocities != NULL); + mLinearVelocities = constrainedLinearVelocities; + mAngularVelocities = constrainedAngularVelocities; } // Return true if the split impulses position correction technique is used for contacts inline bool ContactSolver::isSplitImpulseActive() const { - return mIsSplitImpulseActive; + return mIsSplitImpulseActive; } // Activate or Deactivate the split impulses for contacts inline void ContactSolver::setIsSplitImpulseActive(bool isActive) { - mIsSplitImpulseActive = isActive; + mIsSplitImpulseActive = isActive; } // Activate or deactivate the solving of friction constraints at the center of // the contact manifold instead of solving them at each contact point inline void ContactSolver::setIsSolveFrictionAtContactManifoldCenterActive(bool isActive) { - mIsSolveFrictionAtContactManifoldCenterActive = isActive; + mIsSolveFrictionAtContactManifoldCenterActive = isActive; } // Compute the collision restitution factor from the restitution factor of each body -inline decimal ContactSolver::computeMixedRestitutionFactor(RigidBody* body1, - RigidBody* body2) const { - decimal restitution1 = body1->getMaterial().getBounciness(); - decimal restitution2 = body2->getMaterial().getBounciness(); +inline float ContactSolver::computeMixedRestitutionFactor(RigidBody* body1, + RigidBody* body2) const { + float restitution1 = body1->getMaterial().getBounciness(); + float restitution2 = body2->getMaterial().getBounciness(); - // Return the largest restitution factor - return (restitution1 > restitution2) ? restitution1 : restitution2; + // Return the largest restitution factor + return (restitution1 > restitution2) ? restitution1 : restitution2; } // Compute the mixed friction coefficient from the friction coefficient of each body -inline decimal ContactSolver::computeMixedFrictionCoefficient(RigidBody *body1, - RigidBody *body2) const { - // Use the geometric mean to compute the mixed friction coefficient - return sqrt(body1->getMaterial().getFrictionCoefficient() * - body2->getMaterial().getFrictionCoefficient()); +inline float ContactSolver::computeMixedFrictionCoefficient(RigidBody *body1, + RigidBody *body2) const { + // Use the geometric mean to compute the mixed friction coefficient + return sqrt(body1->getMaterial().getFrictionCoefficient() * + body2->getMaterial().getFrictionCoefficient()); } // Compute th mixed rolling resistance factor between two bodies -inline decimal ContactSolver::computeMixedRollingResistance(RigidBody* body1, - RigidBody* body2) const { - return decimal(0.5f) * (body1->getMaterial().getRollingResistance() + body2->getMaterial().getRollingResistance()); +inline float ContactSolver::computeMixedRollingResistance(RigidBody* body1, + RigidBody* body2) const { + return float(0.5f) * (body1->getMaterial().getRollingResistance() + body2->getMaterial().getRollingResistance()); } // Compute a penetration constraint impulse -inline const Impulse ContactSolver::computePenetrationImpulse(decimal deltaLambda, - const ContactPointSolver& contactPoint) - const { - return Impulse(-contactPoint.normal * deltaLambda, -contactPoint.r1CrossN * deltaLambda, - contactPoint.normal * deltaLambda, contactPoint.r2CrossN * deltaLambda); +inline const Impulse ContactSolver::computePenetrationImpulse(float deltaLambda, + const ContactPointSolver& contactPoint) + const { + return Impulse(-contactPoint.normal * deltaLambda, -contactPoint.r1CrossN * deltaLambda, + contactPoint.normal * deltaLambda, contactPoint.r2CrossN * deltaLambda); } // Compute the first friction constraint impulse -inline const Impulse ContactSolver::computeFriction1Impulse(decimal deltaLambda, - const ContactPointSolver& contactPoint) - const { - return Impulse(-contactPoint.frictionVector1 * deltaLambda, - -contactPoint.r1CrossT1 * deltaLambda, - contactPoint.frictionVector1 * deltaLambda, - contactPoint.r2CrossT1 * deltaLambda); +inline const Impulse ContactSolver::computeFriction1Impulse(float deltaLambda, + const ContactPointSolver& contactPoint) + const { + return Impulse(-contactPoint.frictionVector1 * deltaLambda, + -contactPoint.r1CrossT1 * deltaLambda, + contactPoint.frictionVector1 * deltaLambda, + contactPoint.r2CrossT1 * deltaLambda); } // Compute the second friction constraint impulse -inline const Impulse ContactSolver::computeFriction2Impulse(decimal deltaLambda, - const ContactPointSolver& contactPoint) - const { - return Impulse(-contactPoint.frictionVector2 * deltaLambda, - -contactPoint.r1CrossT2 * deltaLambda, - contactPoint.frictionVector2 * deltaLambda, - contactPoint.r2CrossT2 * deltaLambda); +inline const Impulse ContactSolver::computeFriction2Impulse(float deltaLambda, + const ContactPointSolver& contactPoint) + const { + return Impulse(-contactPoint.frictionVector2 * deltaLambda, + -contactPoint.r1CrossT2 * deltaLambda, + contactPoint.frictionVector2 * deltaLambda, + contactPoint.r2CrossT2 * deltaLambda); } } - -#endif diff --git a/ephysics/engine/DynamicsWorld.cpp b/ephysics/engine/DynamicsWorld.cpp index ed11429..818020f 100644 --- a/ephysics/engine/DynamicsWorld.cpp +++ b/ephysics/engine/DynamicsWorld.cpp @@ -1,27 +1,8 @@ -/******************************************************************************** -* 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. * -* * -********************************************************************************/ +/** @file + * @author Daniel Chappuis + * @copyright 2010-2016 Daniel Chappuis + * @license BSD 3 clauses (see license file) + */ // Libraries #include @@ -39,75 +20,75 @@ using namespace std; * @param gravity Gravity vector in the world (in meters per second squared) */ DynamicsWorld::DynamicsWorld(const Vector3 &gravity) - : CollisionWorld(), - mContactSolver(mMapBodyToConstrainedVelocityIndex), - mConstraintSolver(mMapBodyToConstrainedVelocityIndex), - mNbVelocitySolverIterations(DEFAULT_VELOCITY_SOLVER_NB_ITERATIONS), - mNbPositionSolverIterations(DEFAULT_POSITION_SOLVER_NB_ITERATIONS), - mIsSleepingEnabled(SPLEEPING_ENABLED), mGravity(gravity), - mIsGravityEnabled(true), mConstrainedLinearVelocities(NULL), - mConstrainedAngularVelocities(NULL), mSplitLinearVelocities(NULL), - mSplitAngularVelocities(NULL), mConstrainedPositions(NULL), - mConstrainedOrientations(NULL), mNbIslands(0), - mNbIslandsCapacity(0), mIslands(NULL), mNbBodiesCapacity(0), - mSleepLinearVelocity(DEFAULT_SLEEP_LINEAR_VELOCITY), - mSleepAngularVelocity(DEFAULT_SLEEP_ANGULAR_VELOCITY), - mTimeBeforeSleep(DEFAULT_TIME_BEFORE_SLEEP) { + : CollisionWorld(), + mContactSolver(mMapBodyToConstrainedVelocityIndex), + mConstraintSolver(mMapBodyToConstrainedVelocityIndex), + mNbVelocitySolverIterations(DEFAULT_VELOCITY_SOLVER_NB_ITERATIONS), + mNbPositionSolverIterations(DEFAULT_POSITION_SOLVER_NB_ITERATIONS), + mIsSleepingEnabled(SPLEEPING_ENABLED), mGravity(gravity), + mIsGravityEnabled(true), mConstrainedLinearVelocities(NULL), + mConstrainedAngularVelocities(NULL), mSplitLinearVelocities(NULL), + mSplitAngularVelocities(NULL), mConstrainedPositions(NULL), + mConstrainedOrientations(NULL), mNbIslands(0), + mNbIslandsCapacity(0), mIslands(NULL), mNbBodiesCapacity(0), + mSleepLinearVelocity(DEFAULT_SLEEP_LINEAR_VELOCITY), + mSleepAngularVelocity(DEFAULT_SLEEP_ANGULAR_VELOCITY), + mTimeBeforeSleep(DEFAULT_TIME_BEFORE_SLEEP) { } // Destructor DynamicsWorld::~DynamicsWorld() { - // Destroy all the joints that have not been removed - std::set::iterator itJoints; - for (itJoints = mJoints.begin(); itJoints != mJoints.end();) { - std::set::iterator itToRemove = itJoints; - ++itJoints; - destroyJoint(*itToRemove); - } + // Destroy all the joints that have not been removed + std::set::iterator itJoints; + for (itJoints = mJoints.begin(); itJoints != mJoints.end();) { + std::set::iterator itToRemove = itJoints; + ++itJoints; + destroyJoint(*itToRemove); + } - // Destroy all the rigid bodies that have not been removed - std::set::iterator itRigidBodies; - for (itRigidBodies = mRigidBodies.begin(); itRigidBodies != mRigidBodies.end();) { - std::set::iterator itToRemove = itRigidBodies; - ++itRigidBodies; - destroyRigidBody(*itToRemove); - } + // Destroy all the rigid bodies that have not been removed + std::set::iterator itRigidBodies; + for (itRigidBodies = mRigidBodies.begin(); itRigidBodies != mRigidBodies.end();) { + std::set::iterator itToRemove = itRigidBodies; + ++itRigidBodies; + destroyRigidBody(*itToRemove); + } - // Release the memory allocated for the islands - for (uint i=0; i~Island(); + // Call the island destructor + mIslands[i]->~Island(); - // Release the allocated memory for the island - mMemoryAllocator.release(mIslands[i], sizeof(Island)); - } - if (mNbIslandsCapacity > 0) { - mMemoryAllocator.release(mIslands, sizeof(Island*) * mNbIslandsCapacity); - } + // Release the allocated memory for the island + mMemoryAllocator.release(mIslands[i], sizeof(Island)); + } + if (mNbIslandsCapacity > 0) { + mMemoryAllocator.release(mIslands, sizeof(Island*) * mNbIslandsCapacity); + } - // Release the memory allocated for the bodies velocity arrays - if (mNbBodiesCapacity > 0) { - delete[] mSplitLinearVelocities; - delete[] mSplitAngularVelocities; - delete[] mConstrainedLinearVelocities; - delete[] mConstrainedAngularVelocities; - delete[] mConstrainedPositions; - delete[] mConstrainedOrientations; - } + // Release the memory allocated for the bodies velocity arrays + if (mNbBodiesCapacity > 0) { + delete[] mSplitLinearVelocities; + delete[] mSplitAngularVelocities; + delete[] mConstrainedLinearVelocities; + delete[] mConstrainedAngularVelocities; + delete[] mConstrainedPositions; + delete[] mConstrainedOrientations; + } - assert(mJoints.size() == 0); - assert(mRigidBodies.size() == 0); + assert(mJoints.size() == 0); + assert(mRigidBodies.size() == 0); #ifdef IS_PROFILING_ACTIVE - // Print the profiling report - Profiler::printReport(std::cout); + // Print32_t the profiling report + Profiler::print32_tReport(std::cout); - // Destroy the profiler (release the allocated memory) - Profiler::destroy(); + // Destroy the profiler (release the allocated memory) + Profiler::destroy(); #endif } @@ -116,7 +97,7 @@ DynamicsWorld::~DynamicsWorld() { /** * @param timeStep The amount of time to step the simulation by (in seconds) */ -void DynamicsWorld::update(decimal timeStep) { +void DynamicsWorld::update(float timeStep) { #ifdef IS_PROFILING_ACTIVE // Increment the frame counter of the profiler @@ -127,7 +108,7 @@ void DynamicsWorld::update(decimal timeStep) { mTimeStep = timeStep; - // Notify the event listener about the beginning of an internal tick + // Notify the event listener about the beginning of an int32_ternal tick if (mEventListener != NULL) { mEventListener->beginInternalTick(); } @@ -147,13 +128,13 @@ void DynamicsWorld::update(decimal timeStep) { computeIslands(); // Integrate the velocities - integrateRigidBodiesVelocities(); + int32_tegrateRigidBodiesVelocities(); // Solve the contacts and constraints solveContactsAndConstraints(); // Integrate the position and orientation of each body - integrateRigidBodiesPositions(); + int32_tegrateRigidBodiesPositions(); // Solve the position correction for constraints solvePositionCorrection(); @@ -163,7 +144,7 @@ void DynamicsWorld::update(decimal timeStep) { if (mIsSleepingEnabled) updateSleepingBodies(); - // Notify the event listener about the end of an internal tick + // Notify the event listener about the end of an int32_ternal tick if (mEventListener != NULL) mEventListener->endInternalTick(); // Reset the external force and torque applied to the bodies @@ -171,122 +152,122 @@ void DynamicsWorld::update(decimal timeStep) { } // Integrate position and orientation of the rigid bodies. -/// The positions and orientations of the bodies are integrated using +/// The positions and orientations of the bodies are int32_tegrated using /// the sympletic Euler time stepping scheme. -void DynamicsWorld::integrateRigidBodiesPositions() { +void DynamicsWorld::int32_tegrateRigidBodiesPositions() { - PROFILE("DynamicsWorld::integrateRigidBodiesPositions()"); - - // For each island of the world - for (uint i=0; i < mNbIslands; i++) { + PROFILE("DynamicsWorld::int32_tegrateRigidBodiesPositions()"); + + // For each island of the world + for (uint32_t i=0; i < mNbIslands; i++) { - RigidBody** bodies = mIslands[i]->getBodies(); + RigidBody** bodies = mIslands[i]->getBodies(); - // For each body of the island - for (uint b=0; b < mIslands[i]->getNbBodies(); b++) { + // For each body of the island + for (uint32_t b=0; b < mIslands[i]->getNbBodies(); b++) { - // Get the constrained velocity - uint indexArray = mMapBodyToConstrainedVelocityIndex.find(bodies[b])->second; - Vector3 newLinVelocity = mConstrainedLinearVelocities[indexArray]; - Vector3 newAngVelocity = mConstrainedAngularVelocities[indexArray]; + // Get the constrained velocity + uint32_t indexArray = mMapBodyToConstrainedVelocityIndex.find(bodies[b])->second; + Vector3 newLinVelocity = mConstrainedLinearVelocities[indexArray]; + Vector3 newAngVelocity = mConstrainedAngularVelocities[indexArray]; - // Add the split impulse velocity from Contact Solver (only used - // to update the position) - if (mContactSolver.isSplitImpulseActive()) { + // Add the split impulse velocity from Contact Solver (only used + // to update the position) + if (mContactSolver.isSplitImpulseActive()) { - newLinVelocity += mSplitLinearVelocities[indexArray]; - newAngVelocity += mSplitAngularVelocities[indexArray]; - } + newLinVelocity += mSplitLinearVelocities[indexArray]; + newAngVelocity += mSplitAngularVelocities[indexArray]; + } - // Get current position and orientation of the body - const Vector3& currentPosition = bodies[b]->mCenterOfMassWorld; - const Quaternion& currentOrientation = bodies[b]->getTransform().getOrientation(); + // Get current position and orientation of the body + const Vector3& currentPosition = bodies[b]->mCenterOfMassWorld; + const Quaternion& currentOrientation = bodies[b]->getTransform().getOrientation(); - // Update the new constrained position and orientation of the body - mConstrainedPositions[indexArray] = currentPosition + newLinVelocity * mTimeStep; - mConstrainedOrientations[indexArray] = currentOrientation + - Quaternion(0, newAngVelocity) * - currentOrientation * decimal(0.5) * mTimeStep; - } - } + // Update the new constrained position and orientation of the body + mConstrainedPositions[indexArray] = currentPosition + newLinVelocity * mTimeStep; + mConstrainedOrientations[indexArray] = currentOrientation + + Quaternion(0, newAngVelocity) * + currentOrientation * float(0.5) * mTimeStep; + } + } } // Update the postion/orientation of the bodies void DynamicsWorld::updateBodiesState() { - PROFILE("DynamicsWorld::updateBodiesState()"); + PROFILE("DynamicsWorld::updateBodiesState()"); - // For each island of the world - for (uint islandIndex = 0; islandIndex < mNbIslands; islandIndex++) { + // For each island of the world + for (uint32_t islandIndex = 0; islandIndex < mNbIslands; islandIndex++) { - // For each body of the island - RigidBody** bodies = mIslands[islandIndex]->getBodies(); + // For each body of the island + RigidBody** bodies = mIslands[islandIndex]->getBodies(); - for (uint b=0; b < mIslands[islandIndex]->getNbBodies(); b++) { + for (uint32_t b=0; b < mIslands[islandIndex]->getNbBodies(); b++) { - uint index = mMapBodyToConstrainedVelocityIndex.find(bodies[b])->second; + uint32_t index = mMapBodyToConstrainedVelocityIndex.find(bodies[b])->second; - // Update the linear and angular velocity of the body - bodies[b]->mLinearVelocity = mConstrainedLinearVelocities[index]; - bodies[b]->mAngularVelocity = mConstrainedAngularVelocities[index]; + // Update the linear and angular velocity of the body + bodies[b]->mLinearVelocity = mConstrainedLinearVelocities[index]; + bodies[b]->mAngularVelocity = mConstrainedAngularVelocities[index]; - // Update the position of the center of mass of the body - bodies[b]->mCenterOfMassWorld = mConstrainedPositions[index]; + // Update the position of the center of mass of the body + bodies[b]->mCenterOfMassWorld = mConstrainedPositions[index]; - // Update the orientation of the body - bodies[b]->mTransform.setOrientation(mConstrainedOrientations[index].getUnit()); + // Update the orientation of the body + bodies[b]->mTransform.setOrientation(mConstrainedOrientations[index].getUnit()); - // Update the transform of the body (using the new center of mass and new orientation) - bodies[b]->updateTransformWithCenterOfMass(); + // Update the transform of the body (using the new center of mass and new orientation) + bodies[b]->updateTransformWithCenterOfMass(); - // Update the broad-phase state of the body - bodies[b]->updateBroadPhaseState(); - } - } + // Update the broad-phase state of the body + bodies[b]->updateBroadPhaseState(); + } + } } // Initialize the bodies velocities arrays for the next simulation step. void DynamicsWorld::initVelocityArrays() { - // Allocate memory for the bodies velocity arrays - uint nbBodies = mRigidBodies.size(); - if (mNbBodiesCapacity != nbBodies && nbBodies > 0) { - if (mNbBodiesCapacity > 0) { - delete[] mSplitLinearVelocities; - delete[] mSplitAngularVelocities; - } - mNbBodiesCapacity = nbBodies; - // TODO : Use better memory allocation here - mSplitLinearVelocities = new Vector3[mNbBodiesCapacity]; - mSplitAngularVelocities = new Vector3[mNbBodiesCapacity]; - mConstrainedLinearVelocities = new Vector3[mNbBodiesCapacity]; - mConstrainedAngularVelocities = new Vector3[mNbBodiesCapacity]; - mConstrainedPositions = new Vector3[mNbBodiesCapacity]; - mConstrainedOrientations = new Quaternion[mNbBodiesCapacity]; - assert(mSplitLinearVelocities != NULL); - assert(mSplitAngularVelocities != NULL); - assert(mConstrainedLinearVelocities != NULL); - assert(mConstrainedAngularVelocities != NULL); - assert(mConstrainedPositions != NULL); - assert(mConstrainedOrientations != NULL); - } + // Allocate memory for the bodies velocity arrays + uint32_t nbBodies = mRigidBodies.size(); + if (mNbBodiesCapacity != nbBodies && nbBodies > 0) { + if (mNbBodiesCapacity > 0) { + delete[] mSplitLinearVelocities; + delete[] mSplitAngularVelocities; + } + mNbBodiesCapacity = nbBodies; + // TODO : Use better memory allocation here + mSplitLinearVelocities = new Vector3[mNbBodiesCapacity]; + mSplitAngularVelocities = new Vector3[mNbBodiesCapacity]; + mConstrainedLinearVelocities = new Vector3[mNbBodiesCapacity]; + mConstrainedAngularVelocities = new Vector3[mNbBodiesCapacity]; + mConstrainedPositions = new Vector3[mNbBodiesCapacity]; + mConstrainedOrientations = new Quaternion[mNbBodiesCapacity]; + assert(mSplitLinearVelocities != NULL); + assert(mSplitAngularVelocities != NULL); + assert(mConstrainedLinearVelocities != NULL); + assert(mConstrainedAngularVelocities != NULL); + assert(mConstrainedPositions != NULL); + assert(mConstrainedOrientations != NULL); + } - // Reset the velocities arrays - for (uint i=0; i::const_iterator it; - uint indexBody = 0; - for (it = mRigidBodies.begin(); it != mRigidBodies.end(); ++it) { + // Initialize the map of body indexes in the velocity arrays + mMapBodyToConstrainedVelocityIndex.clear(); + std::set::const_iterator it; + uint32_t indexBody = 0; + for (it = mRigidBodies.begin(); it != mRigidBodies.end(); ++it) { - // Add the body into the map - mMapBodyToConstrainedVelocityIndex.insert(std::make_pair(*it, indexBody)); - indexBody++; - } + // Add the body int32_to the map + mMapBodyToConstrainedVelocityIndex.insert(std::make_pair(*it, indexBody)); + indexBody++; + } } // Integrate the velocities of rigid bodies. @@ -294,175 +275,175 @@ void DynamicsWorld::initVelocityArrays() { /// the actual velocitiy of the bodies. The velocities updated in this method /// might violate the constraints and will be corrected in the constraint and /// contact solver. -void DynamicsWorld::integrateRigidBodiesVelocities() { +void DynamicsWorld::int32_tegrateRigidBodiesVelocities() { - PROFILE("DynamicsWorld::integrateRigidBodiesVelocities()"); + PROFILE("DynamicsWorld::int32_tegrateRigidBodiesVelocities()"); - // Initialize the bodies velocity arrays - initVelocityArrays(); + // Initialize the bodies velocity arrays + initVelocityArrays(); - // For each island of the world - for (uint i=0; i < mNbIslands; i++) { + // For each island of the world + for (uint32_t i=0; i < mNbIslands; i++) { - RigidBody** bodies = mIslands[i]->getBodies(); + RigidBody** bodies = mIslands[i]->getBodies(); - // For each body of the island - for (uint b=0; b < mIslands[i]->getNbBodies(); b++) { + // For each body of the island + for (uint32_t b=0; b < mIslands[i]->getNbBodies(); b++) { - // Insert the body into the map of constrained velocities - uint indexBody = mMapBodyToConstrainedVelocityIndex.find(bodies[b])->second; + // Insert the body int32_to the map of constrained velocities + uint32_t indexBody = mMapBodyToConstrainedVelocityIndex.find(bodies[b])->second; - assert(mSplitLinearVelocities[indexBody] == Vector3(0, 0, 0)); - assert(mSplitAngularVelocities[indexBody] == Vector3(0, 0, 0)); + assert(mSplitLinearVelocities[indexBody] == Vector3(0, 0, 0)); + assert(mSplitAngularVelocities[indexBody] == Vector3(0, 0, 0)); - // Integrate the external force to get the new velocity of the body - mConstrainedLinearVelocities[indexBody] = bodies[b]->getLinearVelocity() + - mTimeStep * bodies[b]->mMassInverse * bodies[b]->mExternalForce; - mConstrainedAngularVelocities[indexBody] = bodies[b]->getAngularVelocity() + - mTimeStep * bodies[b]->getInertiaTensorInverseWorld() * - bodies[b]->mExternalTorque; + // Integrate the external force to get the new velocity of the body + mConstrainedLinearVelocities[indexBody] = bodies[b]->getLinearVelocity() + + mTimeStep * bodies[b]->mMassInverse * bodies[b]->mExternalForce; + mConstrainedAngularVelocities[indexBody] = bodies[b]->getAngularVelocity() + + mTimeStep * bodies[b]->getInertiaTensorInverseWorld() * + bodies[b]->mExternalTorque; - // If the gravity has to be applied to this rigid body - if (bodies[b]->isGravityEnabled() && mIsGravityEnabled) { + // If the gravity has to be applied to this rigid body + if (bodies[b]->isGravityEnabled() && mIsGravityEnabled) { - // Integrate the gravity force - mConstrainedLinearVelocities[indexBody] += mTimeStep * bodies[b]->mMassInverse * - bodies[b]->getMass() * mGravity; - } + // Integrate the gravity force + mConstrainedLinearVelocities[indexBody] += mTimeStep * bodies[b]->mMassInverse * + bodies[b]->getMass() * mGravity; + } - // Apply the velocity damping - // Damping force : F_c = -c' * v (c=damping factor) - // Equation : m * dv/dt = -c' * v - // => dv/dt = -c * v (with c=c'/m) - // => dv/dt + c * v = 0 - // Solution : v(t) = v0 * e^(-c * t) - // => v(t + dt) = v0 * e^(-c(t + dt)) - // = v0 * e^(-ct) * e^(-c * dt) - // = v(t) * e^(-c * dt) - // => v2 = v1 * e^(-c * dt) - // Using Taylor Serie for e^(-x) : e^x ~ 1 + x + x^2/2! + ... - // => e^(-x) ~ 1 - x - // => v2 = v1 * (1 - c * dt) - decimal linDampingFactor = bodies[b]->getLinearDamping(); - decimal angDampingFactor = bodies[b]->getAngularDamping(); - decimal linearDamping = pow(decimal(1.0) - linDampingFactor, mTimeStep); - decimal angularDamping = pow(decimal(1.0) - angDampingFactor, mTimeStep); - mConstrainedLinearVelocities[indexBody] *= linearDamping; - mConstrainedAngularVelocities[indexBody] *= angularDamping; + // Apply the velocity damping + // Damping force : F_c = -c' * v (c=damping factor) + // Equation : m * dv/dt = -c' * v + // => dv/dt = -c * v (with c=c'/m) + // => dv/dt + c * v = 0 + // Solution : v(t) = v0 * e^(-c * t) + // => v(t + dt) = v0 * e^(-c(t + dt)) + // = v0 * e^(-ct) * e^(-c * dt) + // = v(t) * e^(-c * dt) + // => v2 = v1 * e^(-c * dt) + // Using Taylor Serie for e^(-x) : e^x ~ 1 + x + x^2/2! + ... + // => e^(-x) ~ 1 - x + // => v2 = v1 * (1 - c * dt) + float linDampingFactor = bodies[b]->getLinearDamping(); + float angDampingFactor = bodies[b]->getAngularDamping(); + float linearDamping = pow(float(1.0) - linDampingFactor, mTimeStep); + float angularDamping = pow(float(1.0) - angDampingFactor, mTimeStep); + mConstrainedLinearVelocities[indexBody] *= linearDamping; + mConstrainedAngularVelocities[indexBody] *= angularDamping; - indexBody++; - } - } + indexBody++; + } + } } // Solve the contacts and constraints void DynamicsWorld::solveContactsAndConstraints() { - PROFILE("DynamicsWorld::solveContactsAndConstraints()"); + PROFILE("DynamicsWorld::solveContactsAndConstraints()"); - // Set the velocities arrays - mContactSolver.setSplitVelocitiesArrays(mSplitLinearVelocities, mSplitAngularVelocities); - mContactSolver.setConstrainedVelocitiesArrays(mConstrainedLinearVelocities, - mConstrainedAngularVelocities); - mConstraintSolver.setConstrainedVelocitiesArrays(mConstrainedLinearVelocities, - mConstrainedAngularVelocities); - mConstraintSolver.setConstrainedPositionsArrays(mConstrainedPositions, - mConstrainedOrientations); + // Set the velocities arrays + mContactSolver.setSplitVelocitiesArrays(mSplitLinearVelocities, mSplitAngularVelocities); + mContactSolver.setConstrainedVelocitiesArrays(mConstrainedLinearVelocities, + mConstrainedAngularVelocities); + mConstraintSolver.setConstrainedVelocitiesArrays(mConstrainedLinearVelocities, + mConstrainedAngularVelocities); + mConstraintSolver.setConstrainedPositionsArrays(mConstrainedPositions, + mConstrainedOrientations); - // ---------- Solve velocity constraints for joints and contacts ---------- // + // ---------- Solve velocity constraints for joints and contacts ---------- // - // For each island of the world - for (uint islandIndex = 0; islandIndex < mNbIslands; islandIndex++) { + // For each island of the world + for (uint32_t islandIndex = 0; islandIndex < mNbIslands; islandIndex++) { - // Check if there are contacts and constraints to solve - bool isConstraintsToSolve = mIslands[islandIndex]->getNbJoints() > 0; - bool isContactsToSolve = mIslands[islandIndex]->getNbContactManifolds() > 0; - if (!isConstraintsToSolve && !isContactsToSolve) continue; + // Check if there are contacts and constraints to solve + bool isConstraintsToSolve = mIslands[islandIndex]->getNbJoints() > 0; + bool isContactsToSolve = mIslands[islandIndex]->getNbContactManifolds() > 0; + if (!isConstraintsToSolve && !isContactsToSolve) continue; - // If there are contacts in the current island - if (isContactsToSolve) { + // If there are contacts in the current island + if (isContactsToSolve) { - // Initialize the solver - mContactSolver.initializeForIsland(mTimeStep, mIslands[islandIndex]); + // Initialize the solver + mContactSolver.initializeForIsland(mTimeStep, mIslands[islandIndex]); - // Warm start the contact solver - mContactSolver.warmStart(); - } + // Warm start the contact solver + mContactSolver.warmStart(); + } - // If there are constraints - if (isConstraintsToSolve) { + // If there are constraints + if (isConstraintsToSolve) { - // Initialize the constraint solver - mConstraintSolver.initializeForIsland(mTimeStep, mIslands[islandIndex]); - } + // Initialize the constraint solver + mConstraintSolver.initializeForIsland(mTimeStep, mIslands[islandIndex]); + } - // For each iteration of the velocity solver - for (uint i=0; i::max()); + // Largest index cannot be used (it is used for invalid index) + assert(bodyID < std::numeric_limits::max()); - // Create the rigid body - RigidBody* rigidBody = new (mMemoryAllocator.allocate(sizeof(RigidBody))) RigidBody(transform, - *this, bodyID); - assert(rigidBody != NULL); + // Create the rigid body + RigidBody* rigidBody = new (mMemoryAllocator.allocate(sizeof(RigidBody))) RigidBody(transform, + *this, bodyID); + assert(rigidBody != NULL); - // Add the rigid body to the physics world - mBodies.insert(rigidBody); - mRigidBodies.insert(rigidBody); + // Add the rigid body to the physics world + mBodies.insert(rigidBody); + mRigidBodies.insert(rigidBody); - // Return the pointer to the rigid body - return rigidBody; + // Return the pointer to the rigid body + return rigidBody; } // Destroy a rigid body and all the joints which it belongs @@ -471,30 +452,30 @@ RigidBody* DynamicsWorld::createRigidBody(const Transform& transform) { */ void DynamicsWorld::destroyRigidBody(RigidBody* rigidBody) { - // Remove all the collision shapes of the body - rigidBody->removeAllCollisionShapes(); + // Remove all the collision shapes of the body + rigidBody->removeAllCollisionShapes(); - // Add the body ID to the list of free IDs - mFreeBodiesIDs.push_back(rigidBody->getID()); + // Add the body ID to the list of free IDs + mFreeBodiesIDs.push_back(rigidBody->getID()); - // Destroy all the joints in which the rigid body to be destroyed is involved - JointListElement* element; - for (element = rigidBody->mJointsList; element != NULL; element = element->next) { - destroyJoint(element->joint); - } + // Destroy all the joints in which the rigid body to be destroyed is involved + JointListElement* element; + for (element = rigidBody->mJointsList; element != NULL; element = element->next) { + destroyJoint(element->joint); + } - // Reset the contact manifold list of the body - rigidBody->resetContactManifoldsList(); + // Reset the contact manifold list of the body + rigidBody->resetContactManifoldsList(); - // Call the destructor of the rigid body - rigidBody->~RigidBody(); + // Call the destructor of the rigid body + rigidBody->~RigidBody(); - // Remove the rigid body from the list of rigid bodies - mBodies.erase(rigidBody); - mRigidBodies.erase(rigidBody); + // Remove the rigid body from the list of rigid bodies + mBodies.erase(rigidBody); + mRigidBodies.erase(rigidBody); - // Free the object from the memory allocator - mMemoryAllocator.release(rigidBody, sizeof(RigidBody)); + // Free the object from the memory allocator + mMemoryAllocator.release(rigidBody, sizeof(RigidBody)); } // Create a joint between two bodies in the world and return a pointer to the new joint @@ -504,70 +485,70 @@ void DynamicsWorld::destroyRigidBody(RigidBody* rigidBody) { */ Joint* DynamicsWorld::createJoint(const JointInfo& jointInfo) { - Joint* newJoint = NULL; + Joint* newJoint = NULL; - // Allocate memory to create the new joint - switch(jointInfo.type) { + // Allocate memory to create the new joint + switch(jointInfo.type) { - // Ball-and-Socket joint - case BALLSOCKETJOINT: - { - void* allocatedMemory = mMemoryAllocator.allocate(sizeof(BallAndSocketJoint)); - const BallAndSocketJointInfo& info = static_cast( - jointInfo); - newJoint = new (allocatedMemory) BallAndSocketJoint(info); - break; - } + // Ball-and-Socket joint + case BALLSOCKETJOINT: + { + void* allocatedMemory = mMemoryAllocator.allocate(sizeof(BallAndSocketJoint)); + const BallAndSocketJointInfo& info = static_cast( + jointInfo); + newJoint = new (allocatedMemory) BallAndSocketJoint(info); + break; + } - // Slider joint - case SLIDERJOINT: - { - void* allocatedMemory = mMemoryAllocator.allocate(sizeof(SliderJoint)); - const SliderJointInfo& info = static_cast(jointInfo); - newJoint = new (allocatedMemory) SliderJoint(info); - break; - } + // Slider joint + case SLIDERJOINT: + { + void* allocatedMemory = mMemoryAllocator.allocate(sizeof(SliderJoint)); + const SliderJointInfo& info = static_cast(jointInfo); + newJoint = new (allocatedMemory) SliderJoint(info); + break; + } - // Hinge joint - case HINGEJOINT: - { - void* allocatedMemory = mMemoryAllocator.allocate(sizeof(HingeJoint)); - const HingeJointInfo& info = static_cast(jointInfo); - newJoint = new (allocatedMemory) HingeJoint(info); - break; - } + // Hinge joint + case HINGEJOINT: + { + void* allocatedMemory = mMemoryAllocator.allocate(sizeof(HingeJoint)); + const HingeJointInfo& info = static_cast(jointInfo); + newJoint = new (allocatedMemory) HingeJoint(info); + break; + } - // Fixed joint - case FIXEDJOINT: - { - void* allocatedMemory = mMemoryAllocator.allocate(sizeof(FixedJoint)); - const FixedJointInfo& info = static_cast(jointInfo); - newJoint = new (allocatedMemory) FixedJoint(info); - break; - } + // Fixed joint + case FIXEDJOINT: + { + void* allocatedMemory = mMemoryAllocator.allocate(sizeof(FixedJoint)); + const FixedJointInfo& info = static_cast(jointInfo); + newJoint = new (allocatedMemory) FixedJoint(info); + break; + } - default: - { - assert(false); - return NULL; - } - } + default: + { + assert(false); + return NULL; + } + } - // If the collision between the two bodies of the constraint is disabled - if (!jointInfo.isCollisionEnabled) { + // If the collision between the two bodies of the constraint is disabled + if (!jointInfo.isCollisionEnabled) { - // Add the pair of bodies in the set of body pairs that cannot collide with each other - mCollisionDetection.addNoCollisionPair(jointInfo.body1, jointInfo.body2); - } + // Add the pair of bodies in the set of body pairs that cannot collide with each other + mCollisionDetection.addNoCollisionPair(jointInfo.body1, jointInfo.body2); + } - // Add the joint into the world - mJoints.insert(newJoint); + // Add the joint int32_to the world + mJoints.insert(newJoint); - // Add the joint into the joint list of the bodies involved in the joint - addJointToBody(newJoint); + // Add the joint int32_to the joint list of the bodies involved in the joint + addJointToBody(newJoint); - // Return the pointer to the created joint - return newJoint; + // Return the pointer to the created joint + return newJoint; } // Destroy a joint @@ -576,51 +557,51 @@ Joint* DynamicsWorld::createJoint(const JointInfo& jointInfo) { */ void DynamicsWorld::destroyJoint(Joint* joint) { - assert(joint != NULL); + assert(joint != NULL); - // If the collision between the two bodies of the constraint was disabled - if (!joint->isCollisionEnabled()) { + // If the collision between the two bodies of the constraint was disabled + if (!joint->isCollisionEnabled()) { - // Remove the pair of bodies from the set of body pairs that cannot collide with each other - mCollisionDetection.removeNoCollisionPair(joint->getBody1(), joint->getBody2()); - } + // Remove the pair of bodies from the set of body pairs that cannot collide with each other + mCollisionDetection.removeNoCollisionPair(joint->getBody1(), joint->getBody2()); + } - // Wake up the two bodies of the joint - joint->getBody1()->setIsSleeping(false); - joint->getBody2()->setIsSleeping(false); + // Wake up the two bodies of the joint + joint->getBody1()->setIsSleeping(false); + joint->getBody2()->setIsSleeping(false); - // Remove the joint from the world - mJoints.erase(joint); + // Remove the joint from the world + mJoints.erase(joint); - // Remove the joint from the joint list of the bodies involved in the joint - joint->mBody1->removeJointFromJointsList(mMemoryAllocator, joint); - joint->mBody2->removeJointFromJointsList(mMemoryAllocator, joint); + // Remove the joint from the joint list of the bodies involved in the joint + joint->mBody1->removeJointFromJointsList(mMemoryAllocator, joint); + joint->mBody2->removeJointFromJointsList(mMemoryAllocator, joint); - size_t nbBytes = joint->getSizeInBytes(); + size_t nbBytes = joint->getSizeInBytes(); - // Call the destructor of the joint - joint->~Joint(); + // Call the destructor of the joint + joint->~Joint(); - // Release the allocated memory - mMemoryAllocator.release(joint, nbBytes); + // Release the allocated memory + mMemoryAllocator.release(joint, nbBytes); } // Add the joint to the list of joints of the two bodies involved in the joint void DynamicsWorld::addJointToBody(Joint* joint) { - assert(joint != NULL); + assert(joint != NULL); - // Add the joint at the beginning of the linked list of joints of the first body - void* allocatedMemory1 = mMemoryAllocator.allocate(sizeof(JointListElement)); - JointListElement* jointListElement1 = new (allocatedMemory1) JointListElement(joint, - joint->mBody1->mJointsList); - joint->mBody1->mJointsList = jointListElement1; + // Add the joint at the beginning of the linked list of joints of the first body + void* allocatedMemory1 = mMemoryAllocator.allocate(sizeof(JointListElement)); + JointListElement* jointListElement1 = new (allocatedMemory1) JointListElement(joint, + joint->mBody1->mJointsList); + joint->mBody1->mJointsList = jointListElement1; - // Add the joint at the beginning of the linked list of joints of the second body - void* allocatedMemory2 = mMemoryAllocator.allocate(sizeof(JointListElement)); - JointListElement* jointListElement2 = new (allocatedMemory2) JointListElement(joint, - joint->mBody2->mJointsList); - joint->mBody2->mJointsList = jointListElement2; + // Add the joint at the beginning of the linked list of joints of the second body + void* allocatedMemory2 = mMemoryAllocator.allocate(sizeof(JointListElement)); + JointListElement* jointListElement2 = new (allocatedMemory2) JointListElement(joint, + joint->mBody2->mJointsList); + joint->mBody2->mJointsList = jointListElement2; } // Compute the islands of awake bodies. @@ -632,162 +613,162 @@ void DynamicsWorld::addJointToBody(Joint* joint) { /// it). Then, we create an island with this group of connected bodies. void DynamicsWorld::computeIslands() { - PROFILE("DynamicsWorld::computeIslands()"); + PROFILE("DynamicsWorld::computeIslands()"); - uint nbBodies = mRigidBodies.size(); + uint32_t nbBodies = mRigidBodies.size(); - // Clear all the islands - for (uint i=0; i~Island(); + // Call the island destructor + mIslands[i]->~Island(); - // Release the allocated memory for the island - mMemoryAllocator.release(mIslands[i], sizeof(Island)); - } + // Release the allocated memory for the island + mMemoryAllocator.release(mIslands[i], sizeof(Island)); + } - // Allocate and create the array of islands - if (mNbIslandsCapacity != nbBodies && nbBodies > 0) { - if (mNbIslandsCapacity > 0) { - mMemoryAllocator.release(mIslands, sizeof(Island*) * mNbIslandsCapacity); - } - mNbIslandsCapacity = nbBodies; - mIslands = (Island**)mMemoryAllocator.allocate(sizeof(Island*) * mNbIslandsCapacity); - } - mNbIslands = 0; + // Allocate and create the array of islands + if (mNbIslandsCapacity != nbBodies && nbBodies > 0) { + if (mNbIslandsCapacity > 0) { + mMemoryAllocator.release(mIslands, sizeof(Island*) * mNbIslandsCapacity); + } + mNbIslandsCapacity = nbBodies; + mIslands = (Island**)mMemoryAllocator.allocate(sizeof(Island*) * mNbIslandsCapacity); + } + mNbIslands = 0; - int nbContactManifolds = 0; + int32_t nbContactManifolds = 0; - // Reset all the isAlreadyInIsland variables of bodies, joints and contact manifolds - for (std::set::iterator it = mRigidBodies.begin(); it != mRigidBodies.end(); ++it) { - int nbBodyManifolds = (*it)->resetIsAlreadyInIslandAndCountManifolds(); - nbContactManifolds += nbBodyManifolds; - } - for (std::set::iterator it = mJoints.begin(); it != mJoints.end(); ++it) { - (*it)->mIsAlreadyInIsland = false; - } + // Reset all the isAlreadyInIsland variables of bodies, joints and contact manifolds + for (std::set::iterator it = mRigidBodies.begin(); it != mRigidBodies.end(); ++it) { + int32_t nbBodyManifolds = (*it)->resetIsAlreadyInIslandAndCountManifolds(); + nbContactManifolds += nbBodyManifolds; + } + for (std::set::iterator it = mJoints.begin(); it != mJoints.end(); ++it) { + (*it)->mIsAlreadyInIsland = false; + } - // Create a stack (using an array) for the rigid bodies to visit during the Depth First Search - size_t nbBytesStack = sizeof(RigidBody*) * nbBodies; - RigidBody** stackBodiesToVisit = (RigidBody**)mMemoryAllocator.allocate(nbBytesStack); + // Create a stack (using an array) for the rigid bodies to visit during the Depth First Search + size_t nbBytesStack = sizeof(RigidBody*) * nbBodies; + RigidBody** stackBodiesToVisit = (RigidBody**)mMemoryAllocator.allocate(nbBytesStack); - // For each rigid body of the world - for (std::set::iterator it = mRigidBodies.begin(); it != mRigidBodies.end(); ++it) { + // For each rigid body of the world + for (std::set::iterator it = mRigidBodies.begin(); it != mRigidBodies.end(); ++it) { - RigidBody* body = *it; + RigidBody* body = *it; - // If the body has already been added to an island, we go to the next body - if (body->mIsAlreadyInIsland) continue; + // If the body has already been added to an island, we go to the next body + if (body->mIsAlreadyInIsland) continue; - // If the body is static, we go to the next body - if (body->getType() == STATIC) continue; + // If the body is static, we go to the next body + if (body->getType() == STATIC) continue; - // If the body is sleeping or inactive, we go to the next body - if (body->isSleeping() || !body->isActive()) continue; + // If the body is sleeping or inactive, we go to the next body + if (body->isSleeping() || !body->isActive()) continue; - // Reset the stack of bodies to visit - uint stackIndex = 0; - stackBodiesToVisit[stackIndex] = body; - stackIndex++; - body->mIsAlreadyInIsland = true; + // Reset the stack of bodies to visit + uint32_t stackIndex = 0; + stackBodiesToVisit[stackIndex] = body; + stackIndex++; + body->mIsAlreadyInIsland = true; - // Create the new island - void* allocatedMemoryIsland = mMemoryAllocator.allocate(sizeof(Island)); - mIslands[mNbIslands] = new (allocatedMemoryIsland) Island(nbBodies, - nbContactManifolds, - mJoints.size(), mMemoryAllocator); + // Create the new island + void* allocatedMemoryIsland = mMemoryAllocator.allocate(sizeof(Island)); + mIslands[mNbIslands] = new (allocatedMemoryIsland) Island(nbBodies, + nbContactManifolds, + mJoints.size(), mMemoryAllocator); - // While there are still some bodies to visit in the stack - while (stackIndex > 0) { + // While there are still some bodies to visit in the stack + while (stackIndex > 0) { - // Get the next body to visit from the stack - stackIndex--; - RigidBody* bodyToVisit = stackBodiesToVisit[stackIndex]; - assert(bodyToVisit->isActive()); + // Get the next body to visit from the stack + stackIndex--; + RigidBody* bodyToVisit = stackBodiesToVisit[stackIndex]; + assert(bodyToVisit->isActive()); - // Awake the body if it is slepping - bodyToVisit->setIsSleeping(false); + // Awake the body if it is slepping + bodyToVisit->setIsSleeping(false); - // Add the body into the island - mIslands[mNbIslands]->addBody(bodyToVisit); + // Add the body int32_to the island + mIslands[mNbIslands]->addBody(bodyToVisit); - // If the current body is static, we do not want to perform the DFS - // search across that body - if (bodyToVisit->getType() == STATIC) continue; + // If the current body is static, we do not want to perform the DFS + // search across that body + if (bodyToVisit->getType() == STATIC) continue; - // For each contact manifold in which the current body is involded - ContactManifoldListElement* contactElement; - for (contactElement = bodyToVisit->mContactManifoldsList; contactElement != NULL; - contactElement = contactElement->next) { + // For each contact manifold in which the current body is involded + ContactManifoldListElement* contactElement; + for (contactElement = bodyToVisit->mContactManifoldsList; contactElement != NULL; + contactElement = contactElement->next) { - ContactManifold* contactManifold = contactElement->contactManifold; + ContactManifold* contactManifold = contactElement->contactManifold; - assert(contactManifold->getNbContactPoints() > 0); + assert(contactManifold->getNbContactPoints() > 0); - // Check if the current contact manifold has already been added into an island - if (contactManifold->isAlreadyInIsland()) continue; + // Check if the current contact manifold has already been added int32_to an island + if (contactManifold->isAlreadyInIsland()) continue; - // Add the contact manifold into the island - mIslands[mNbIslands]->addContactManifold(contactManifold); - contactManifold->mIsAlreadyInIsland = true; + // Add the contact manifold int32_to the island + mIslands[mNbIslands]->addContactManifold(contactManifold); + contactManifold->mIsAlreadyInIsland = true; - // Get the other body of the contact manifold - RigidBody* body1 = static_cast(contactManifold->getBody1()); - RigidBody* body2 = static_cast(contactManifold->getBody2()); - RigidBody* otherBody = (body1->getID() == bodyToVisit->getID()) ? body2 : body1; + // Get the other body of the contact manifold + RigidBody* body1 = static_cast(contactManifold->getBody1()); + RigidBody* body2 = static_cast(contactManifold->getBody2()); + RigidBody* otherBody = (body1->getID() == bodyToVisit->getID()) ? body2 : body1; - // Check if the other body has already been added to the island - if (otherBody->mIsAlreadyInIsland) continue; + // Check if the other body has already been added to the island + if (otherBody->mIsAlreadyInIsland) continue; - // Insert the other body into the stack of bodies to visit - stackBodiesToVisit[stackIndex] = otherBody; - stackIndex++; - otherBody->mIsAlreadyInIsland = true; - } + // Insert the other body int32_to the stack of bodies to visit + stackBodiesToVisit[stackIndex] = otherBody; + stackIndex++; + otherBody->mIsAlreadyInIsland = true; + } - // For each joint in which the current body is involved - JointListElement* jointElement; - for (jointElement = bodyToVisit->mJointsList; jointElement != NULL; - jointElement = jointElement->next) { + // For each joint in which the current body is involved + JointListElement* jointElement; + for (jointElement = bodyToVisit->mJointsList; jointElement != NULL; + jointElement = jointElement->next) { - Joint* joint = jointElement->joint; + Joint* joint = jointElement->joint; - // Check if the current joint has already been added into an island - if (joint->isAlreadyInIsland()) continue; + // Check if the current joint has already been added int32_to an island + if (joint->isAlreadyInIsland()) continue; - // Add the joint into the island - mIslands[mNbIslands]->addJoint(joint); - joint->mIsAlreadyInIsland = true; + // Add the joint int32_to the island + mIslands[mNbIslands]->addJoint(joint); + joint->mIsAlreadyInIsland = true; - // Get the other body of the contact manifold - RigidBody* body1 = static_cast(joint->getBody1()); - RigidBody* body2 = static_cast(joint->getBody2()); - RigidBody* otherBody = (body1->getID() == bodyToVisit->getID()) ? body2 : body1; + // Get the other body of the contact manifold + RigidBody* body1 = static_cast(joint->getBody1()); + RigidBody* body2 = static_cast(joint->getBody2()); + RigidBody* otherBody = (body1->getID() == bodyToVisit->getID()) ? body2 : body1; - // Check if the other body has already been added to the island - if (otherBody->mIsAlreadyInIsland) continue; + // Check if the other body has already been added to the island + if (otherBody->mIsAlreadyInIsland) continue; - // Insert the other body into the stack of bodies to visit - stackBodiesToVisit[stackIndex] = otherBody; - stackIndex++; - otherBody->mIsAlreadyInIsland = true; - } - } + // Insert the other body int32_to the stack of bodies to visit + stackBodiesToVisit[stackIndex] = otherBody; + stackIndex++; + otherBody->mIsAlreadyInIsland = true; + } + } - // Reset the isAlreadyIsland variable of the static bodies so that they - // can also be included in the other islands - for (uint i=0; i < mIslands[mNbIslands]->mNbBodies; i++) { + // Reset the isAlreadyIsland variable of the static bodies so that they + // can also be included in the other islands + for (uint32_t i=0; i < mIslands[mNbIslands]->mNbBodies; i++) { - if (mIslands[mNbIslands]->mBodies[i]->getType() == STATIC) { - mIslands[mNbIslands]->mBodies[i]->mIsAlreadyInIsland = false; - } - } + if (mIslands[mNbIslands]->mBodies[i]->getType() == STATIC) { + mIslands[mNbIslands]->mBodies[i]->mIsAlreadyInIsland = false; + } + } - mNbIslands++; - } + mNbIslands++; + } - // Release the allocated memory for the stack of bodies to visit - mMemoryAllocator.release(stackBodiesToVisit, nbBytesStack); + // Release the allocated memory for the stack of bodies to visit + mMemoryAllocator.release(stackBodiesToVisit, nbBytesStack); } // Put bodies to sleep if needed. @@ -795,75 +776,75 @@ void DynamicsWorld::computeIslands() { /// time, we put all the bodies of the island to sleep. void DynamicsWorld::updateSleepingBodies() { - PROFILE("DynamicsWorld::updateSleepingBodies()"); + PROFILE("DynamicsWorld::updateSleepingBodies()"); - const decimal sleepLinearVelocitySquare = mSleepLinearVelocity * mSleepLinearVelocity; - const decimal sleepAngularVelocitySquare = mSleepAngularVelocity * mSleepAngularVelocity; + const float sleepLinearVelocitySquare = mSleepLinearVelocity * mSleepLinearVelocity; + const float sleepAngularVelocitySquare = mSleepAngularVelocity * mSleepAngularVelocity; - // For each island of the world - for (uint i=0; igetBodies(); - for (uint b=0; b < mIslands[i]->getNbBodies(); b++) { + // For each body of the island + RigidBody** bodies = mIslands[i]->getBodies(); + for (uint32_t b=0; b < mIslands[i]->getNbBodies(); b++) { - // Skip static bodies - if (bodies[b]->getType() == STATIC) continue; + // Skip static bodies + if (bodies[b]->getType() == STATIC) continue; - // If the body is velocity is large enough to stay awake - if (bodies[b]->getLinearVelocity().lengthSquare() > sleepLinearVelocitySquare || - bodies[b]->getAngularVelocity().lengthSquare() > sleepAngularVelocitySquare || - !bodies[b]->isAllowedToSleep()) { + // If the body is velocity is large enough to stay awake + if (bodies[b]->getLinearVelocity().lengthSquare() > sleepLinearVelocitySquare || + bodies[b]->getAngularVelocity().lengthSquare() > sleepAngularVelocitySquare || + !bodies[b]->isAllowedToSleep()) { - // Reset the sleep time of the body - bodies[b]->mSleepTime = decimal(0.0); - minSleepTime = decimal(0.0); - } - else { // If the body velocity is bellow the sleeping velocity threshold + // Reset the sleep time of the body + bodies[b]->mSleepTime = float(0.0); + minSleepTime = float(0.0); + } + else { // If the body velocity is bellow the sleeping velocity threshold - // Increase the sleep time - bodies[b]->mSleepTime += mTimeStep; - if (bodies[b]->mSleepTime < minSleepTime) { - minSleepTime = bodies[b]->mSleepTime; - } - } - } + // Increase the sleep time + bodies[b]->mSleepTime += mTimeStep; + if (bodies[b]->mSleepTime < minSleepTime) { + minSleepTime = bodies[b]->mSleepTime; + } + } + } - // If the velocity of all the bodies of the island is under the - // sleeping velocity threshold for a period of time larger than - // the time required to become a sleeping body - if (minSleepTime >= mTimeBeforeSleep) { + // If the velocity of all the bodies of the island is under the + // sleeping velocity threshold for a period of time larger than + // the time required to become a sleeping body + if (minSleepTime >= mTimeBeforeSleep) { - // Put all the bodies of the island to sleep - for (uint b=0; b < mIslands[i]->getNbBodies(); b++) { - bodies[b]->setIsSleeping(true); - } - } - } + // Put all the bodies of the island to sleep + for (uint32_t b=0; b < mIslands[i]->getNbBodies(); b++) { + bodies[b]->setIsSleeping(true); + } + } + } } // Enable/Disable the sleeping technique. -/// The sleeping technique is used to put bodies that are not moving into sleep +/// The sleeping technique is used to put bodies that are not moving int32_to sleep /// to speed up the simulation. /** * @param isSleepingEnabled True if you want to enable the sleeping technique - * and false otherwise + * and false otherwise */ void DynamicsWorld::enableSleeping(bool isSleepingEnabled) { - mIsSleepingEnabled = isSleepingEnabled; + mIsSleepingEnabled = isSleepingEnabled; - if (!mIsSleepingEnabled) { + if (!mIsSleepingEnabled) { - // For each body of the world - std::set::iterator it; - for (it = mRigidBodies.begin(); it != mRigidBodies.end(); ++it) { + // For each body of the world + std::set::iterator it; + for (it = mRigidBodies.begin(); it != mRigidBodies.end(); ++it) { - // Wake up the rigid body - (*it)->setIsSleeping(false); - } - } + // Wake up the rigid body + (*it)->setIsSleeping(false); + } + } } // Test and report collisions between a given shape and all the others @@ -875,15 +856,15 @@ void DynamicsWorld::enableSleeping(bool isSleepingEnabled) { * @param callback Pointer to the object with the callback method */ void DynamicsWorld::testCollision(const ProxyShape* shape, - CollisionCallback* callback) { + CollisionCallback* callback) { - // Create the sets of shapes - std::set shapes; - shapes.insert(shape->mBroadPhaseID); - std::set emptySet; + // Create the sets of shapes + std::set shapes; + shapes.insert(shape->mBroadPhaseID); + std::set emptySet; - // Perform the collision detection and report contacts - mCollisionDetection.reportCollisionBetweenShapes(callback, shapes, emptySet); + // Perform the collision detection and report contacts + mCollisionDetection.reportCollisionBetweenShapes(callback, shapes, emptySet); } // Test and report collisions between two given shapes. @@ -895,17 +876,17 @@ void DynamicsWorld::testCollision(const ProxyShape* shape, * @param callback Pointer to the object with the callback method */ void DynamicsWorld::testCollision(const ProxyShape* shape1, - const ProxyShape* shape2, - CollisionCallback* callback) { + const ProxyShape* shape2, + CollisionCallback* callback) { - // Create the sets of shapes - std::set shapes1; - shapes1.insert(shape1->mBroadPhaseID); - std::set shapes2; - shapes2.insert(shape2->mBroadPhaseID); + // Create the sets of shapes + std::set shapes1; + shapes1.insert(shape1->mBroadPhaseID); + std::set shapes2; + shapes2.insert(shape2->mBroadPhaseID); - // Perform the collision detection and report contacts - mCollisionDetection.reportCollisionBetweenShapes(callback, shapes1, shapes2); + // Perform the collision detection and report contacts + mCollisionDetection.reportCollisionBetweenShapes(callback, shapes1, shapes2); } // Test and report collisions between a body and all the others bodies of the @@ -917,21 +898,21 @@ void DynamicsWorld::testCollision(const ProxyShape* shape1, * @param callback Pointer to the object with the callback method */ void DynamicsWorld::testCollision(const CollisionBody* body, - CollisionCallback* callback) { + CollisionCallback* callback) { - // Create the sets of shapes - std::set shapes1; + // Create the sets of shapes + std::set shapes1; - // For each shape of the body - for (const ProxyShape* shape=body->getProxyShapesList(); shape != NULL; - shape = shape->getNext()) { - shapes1.insert(shape->mBroadPhaseID); - } + // For each shape of the body + for (const ProxyShape* shape=body->getProxyShapesList(); shape != NULL; + shape = shape->getNext()) { + shapes1.insert(shape->mBroadPhaseID); + } - std::set emptySet; + std::set emptySet; - // Perform the collision detection and report contacts - mCollisionDetection.reportCollisionBetweenShapes(callback, shapes1, emptySet); + // Perform the collision detection and report contacts + mCollisionDetection.reportCollisionBetweenShapes(callback, shapes1, emptySet); } // Test and report collisions between two bodies. @@ -943,24 +924,24 @@ void DynamicsWorld::testCollision(const CollisionBody* body, * @param callback Pointer to the object with the callback method */ void DynamicsWorld::testCollision(const CollisionBody* body1, - const CollisionBody* body2, - CollisionCallback* callback) { + const CollisionBody* body2, + CollisionCallback* callback) { - // Create the sets of shapes - std::set shapes1; - for (const ProxyShape* shape=body1->getProxyShapesList(); shape != NULL; - shape = shape->getNext()) { - shapes1.insert(shape->mBroadPhaseID); - } + // Create the sets of shapes + std::set shapes1; + for (const ProxyShape* shape=body1->getProxyShapesList(); shape != NULL; + shape = shape->getNext()) { + shapes1.insert(shape->mBroadPhaseID); + } - std::set shapes2; - for (const ProxyShape* shape=body2->getProxyShapesList(); shape != NULL; - shape = shape->getNext()) { - shapes2.insert(shape->mBroadPhaseID); - } + std::set shapes2; + for (const ProxyShape* shape=body2->getProxyShapesList(); shape != NULL; + shape = shape->getNext()) { + shapes2.insert(shape->mBroadPhaseID); + } - // Perform the collision detection and report contacts - mCollisionDetection.reportCollisionBetweenShapes(callback, shapes1, shapes2); + // Perform the collision detection and report contacts + mCollisionDetection.reportCollisionBetweenShapes(callback, shapes1, shapes2); } // Test and report collisions between all shapes of the world. @@ -971,35 +952,35 @@ void DynamicsWorld::testCollision(const CollisionBody* body1, */ void DynamicsWorld::testCollision(CollisionCallback* callback) { - std::set emptySet; + std::set emptySet; - // Perform the collision detection and report contacts - mCollisionDetection.reportCollisionBetweenShapes(callback, emptySet, emptySet); + // Perform the collision detection and report contacts + mCollisionDetection.reportCollisionBetweenShapes(callback, emptySet, emptySet); } /// Return the list of all contacts of the world std::vector DynamicsWorld::getContactsList() const { - std::vector contactManifolds; + std::vector contactManifolds; - // For each currently overlapping pair of bodies - std::map::const_iterator it; - for (it = mCollisionDetection.mOverlappingPairs.begin(); - it != mCollisionDetection.mOverlappingPairs.end(); ++it) { + // For each currently overlapping pair of bodies + std::map::const_iterator it; + for (it = mCollisionDetection.mOverlappingPairs.begin(); + it != mCollisionDetection.mOverlappingPairs.end(); ++it) { - OverlappingPair* pair = it->second; + OverlappingPair* pair = it->second; - // For each contact manifold of the pair - const ContactManifoldSet& manifoldSet = pair->getContactManifoldSet(); - for (int i=0; igetContactManifoldSet(); + for (int32_t i=0; i @@ -46,300 +25,300 @@ namespace reactphysics3d { */ class DynamicsWorld : public CollisionWorld { - protected : + protected : - // -------------------- Attributes -------------------- // + // -------------------- Attributes -------------------- // - /// Contact solver - ContactSolver mContactSolver; + /// Contact solver + ContactSolver mContactSolver; - /// Constraint solver - ConstraintSolver mConstraintSolver; + /// Constraint solver + ConstraintSolver mConstraintSolver; - /// Number of iterations for the velocity solver of the Sequential Impulses technique - uint mNbVelocitySolverIterations; + /// Number of iterations for the velocity solver of the Sequential Impulses technique + uint32_t mNbVelocitySolverIterations; - /// Number of iterations for the position solver of the Sequential Impulses technique - uint mNbPositionSolverIterations; + /// Number of iterations for the position solver of the Sequential Impulses technique + uint32_t mNbPositionSolverIterations; - /// True if the spleeping technique for inactive bodies is enabled - bool mIsSleepingEnabled; + /// True if the spleeping technique for inactive bodies is enabled + bool mIsSleepingEnabled; - /// All the rigid bodies of the physics world - std::set mRigidBodies; + /// All the rigid bodies of the physics world + std::set mRigidBodies; - /// All the joints of the world - std::set mJoints; + /// All the joints of the world + std::set mJoints; - /// Gravity vector of the world - Vector3 mGravity; + /// Gravity vector of the world + Vector3 mGravity; - /// Current frame time step (in seconds) - decimal mTimeStep; + /// Current frame time step (in seconds) + float mTimeStep; - /// True if the gravity force is on - bool mIsGravityEnabled; + /// True if the gravity force is on + bool mIsGravityEnabled; - /// Array of constrained linear velocities (state of the linear velocities - /// after solving the constraints) - Vector3* mConstrainedLinearVelocities; + /// Array of constrained linear velocities (state of the linear velocities + /// after solving the constraints) + Vector3* mConstrainedLinearVelocities; - /// Array of constrained angular velocities (state of the angular velocities - /// after solving the constraints) - Vector3* mConstrainedAngularVelocities; + /// Array of constrained angular velocities (state of the angular velocities + /// after solving the constraints) + Vector3* mConstrainedAngularVelocities; - /// Split linear velocities for the position contact solver (split impulse) - Vector3* mSplitLinearVelocities; + /// Split linear velocities for the position contact solver (split impulse) + Vector3* mSplitLinearVelocities; - /// Split angular velocities for the position contact solver (split impulse) - Vector3* mSplitAngularVelocities; + /// Split angular velocities for the position contact solver (split impulse) + Vector3* mSplitAngularVelocities; - /// Array of constrained rigid bodies position (for position error correction) - Vector3* mConstrainedPositions; + /// Array of constrained rigid bodies position (for position error correction) + Vector3* mConstrainedPositions; - /// Array of constrained rigid bodies orientation (for position error correction) - Quaternion* mConstrainedOrientations; + /// Array of constrained rigid bodies orientation (for position error correction) + Quaternion* mConstrainedOrientations; - /// Map body to their index in the constrained velocities array - std::map mMapBodyToConstrainedVelocityIndex; + /// Map body to their index in the constrained velocities array + std::map mMapBodyToConstrainedVelocityIndex; - /// Number of islands in the world - uint mNbIslands; + /// Number of islands in the world + uint32_t mNbIslands; - /// Current allocated capacity for the islands - uint mNbIslandsCapacity; + /// Current allocated capacity for the islands + uint32_t mNbIslandsCapacity; - /// Array with all the islands of awaken bodies - Island** mIslands; + /// Array with all the islands of awaken bodies + Island** mIslands; - /// Current allocated capacity for the bodies - uint mNbBodiesCapacity; + /// Current allocated capacity for the bodies + uint32_t mNbBodiesCapacity; - /// Sleep linear velocity threshold - decimal mSleepLinearVelocity; + /// Sleep linear velocity threshold + float mSleepLinearVelocity; - /// Sleep angular velocity threshold - decimal mSleepAngularVelocity; + /// Sleep angular velocity threshold + float mSleepAngularVelocity; - /// Time (in seconds) before a body is put to sleep if its velocity - /// becomes smaller than the sleep velocity. - decimal mTimeBeforeSleep; + /// Time (in seconds) before a body is put to sleep if its velocity + /// becomes smaller than the sleep velocity. + float mTimeBeforeSleep; - // -------------------- Methods -------------------- // + // -------------------- Methods -------------------- // - /// Private copy-constructor - DynamicsWorld(const DynamicsWorld& world); + /// Private copy-constructor + DynamicsWorld(const DynamicsWorld& world); - /// Private assignment operator - DynamicsWorld& operator=(const DynamicsWorld& world); + /// Private assignment operator + DynamicsWorld& operator=(const DynamicsWorld& world); - /// Integrate the positions and orientations of rigid bodies. - void integrateRigidBodiesPositions(); + /// Integrate the positions and orientations of rigid bodies. + void int32_tegrateRigidBodiesPositions(); - /// Update the AABBs of the bodies - void updateRigidBodiesAABB(); + /// Update the AABBs of the bodies + void updateRigidBodiesAABB(); - /// Reset the external force and torque applied to the bodies - void resetBodiesForceAndTorque(); + /// Reset the external force and torque applied to the bodies + void resetBodiesForceAndTorque(); - /// Update the position and orientation of a body - void updatePositionAndOrientationOfBody(RigidBody* body, Vector3 newLinVelocity, - Vector3 newAngVelocity); + /// Update the position and orientation of a body + void updatePositionAndOrientationOfBody(RigidBody* body, Vector3 newLinVelocity, + Vector3 newAngVelocity); - /// Compute and set the interpolation factor to all bodies - void setInterpolationFactorToAllBodies(); + /// Compute and set the int32_terpolation factor to all bodies + void setInterpolationFactorToAllBodies(); - /// Initialize the bodies velocities arrays for the next simulation step. - void initVelocityArrays(); + /// Initialize the bodies velocities arrays for the next simulation step. + void initVelocityArrays(); - /// Integrate the velocities of rigid bodies. - void integrateRigidBodiesVelocities(); + /// Integrate the velocities of rigid bodies. + void int32_tegrateRigidBodiesVelocities(); - /// Solve the contacts and constraints - void solveContactsAndConstraints(); + /// Solve the contacts and constraints + void solveContactsAndConstraints(); - /// Solve the position error correction of the constraints - void solvePositionCorrection(); + /// Solve the position error correction of the constraints + void solvePositionCorrection(); - /// Cleanup the constrained velocities array at each step - void cleanupConstrainedVelocitiesArray(); + /// Cleanup the constrained velocities array at each step + void cleanupConstrainedVelocitiesArray(); - /// Compute the islands of awake bodies. - void computeIslands(); + /// Compute the islands of awake bodies. + void computeIslands(); - /// Update the postion/orientation of the bodies - void updateBodiesState(); + /// Update the postion/orientation of the bodies + void updateBodiesState(); - /// Put bodies to sleep if needed. - void updateSleepingBodies(); + /// Put bodies to sleep if needed. + void updateSleepingBodies(); - /// Add the joint to the list of joints of the two bodies involved in the joint - void addJointToBody(Joint* joint); + /// Add the joint to the list of joints of the two bodies involved in the joint + void addJointToBody(Joint* joint); - public : + public : - // -------------------- Methods -------------------- // + // -------------------- Methods -------------------- // - /// Constructor - DynamicsWorld(const Vector3& mGravity); + /// Constructor + DynamicsWorld(const Vector3& mGravity); - /// Destructor - virtual ~DynamicsWorld(); + /// Destructor + virtual ~DynamicsWorld(); - /// Update the physics simulation - void update(decimal timeStep); + /// Update the physics simulation + void update(float timeStep); - /// Get the number of iterations for the velocity constraint solver - uint getNbIterationsVelocitySolver() const; + /// Get the number of iterations for the velocity constraint solver + uint32_t getNbIterationsVelocitySolver() const; - /// Set the number of iterations for the velocity constraint solver - void setNbIterationsVelocitySolver(uint nbIterations); + /// Set the number of iterations for the velocity constraint solver + void setNbIterationsVelocitySolver(uint32_t nbIterations); - /// Get the number of iterations for the position constraint solver - uint getNbIterationsPositionSolver() const; + /// Get the number of iterations for the position constraint solver + uint32_t getNbIterationsPositionSolver() const; - /// Set the number of iterations for the position constraint solver - void setNbIterationsPositionSolver(uint nbIterations); + /// Set the number of iterations for the position constraint solver + void setNbIterationsPositionSolver(uint32_t nbIterations); - /// Set the position correction technique used for contacts - void setContactsPositionCorrectionTechnique(ContactsPositionCorrectionTechnique technique); + /// Set the position correction technique used for contacts + void setContactsPositionCorrectionTechnique(ContactsPositionCorrectionTechnique technique); - /// Set the position correction technique used for joints - void setJointsPositionCorrectionTechnique(JointsPositionCorrectionTechnique technique); + /// Set the position correction technique used for joints + void setJointsPositionCorrectionTechnique(JointsPositionCorrectionTechnique technique); - /// Activate or deactivate the solving of friction constraints at the center of - /// the contact manifold instead of solving them at each contact point - void setIsSolveFrictionAtContactManifoldCenterActive(bool isActive); + /// Activate or deactivate the solving of friction constraints at the center of + /// the contact manifold instead of solving them at each contact point + void setIsSolveFrictionAtContactManifoldCenterActive(bool isActive); - /// Create a rigid body into the physics world. - RigidBody* createRigidBody(const Transform& transform); + /// Create a rigid body int32_to the physics world. + RigidBody* createRigidBody(const Transform& transform); - /// Destroy a rigid body and all the joints which it belongs - void destroyRigidBody(RigidBody* rigidBody); + /// Destroy a rigid body and all the joints which it belongs + void destroyRigidBody(RigidBody* rigidBody); - /// Create a joint between two bodies in the world and return a pointer to the new joint - Joint* createJoint(const JointInfo& jointInfo); + /// Create a joint between two bodies in the world and return a pointer to the new joint + Joint* createJoint(const JointInfo& jointInfo); - /// Destroy a joint - void destroyJoint(Joint* joint); + /// Destroy a joint + void destroyJoint(Joint* joint); - /// Return the gravity vector of the world - Vector3 getGravity() const; + /// Return the gravity vector of the world + Vector3 getGravity() const; - /// Set the gravity vector of the world - void setGravity(Vector3& gravity); + /// Set the gravity vector of the world + void setGravity(Vector3& gravity); - /// Return if the gravity is on - bool isGravityEnabled() const; + /// Return if the gravity is on + bool isGravityEnabled() const; - /// Enable/Disable the gravity - void setIsGratityEnabled(bool isGravityEnabled); + /// Enable/Disable the gravity + void setIsGratityEnabled(bool isGravityEnabled); - /// Return the number of rigid bodies in the world - uint getNbRigidBodies() const; + /// Return the number of rigid bodies in the world + uint32_t getNbRigidBodies() const; - /// Return the number of joints in the world - uint getNbJoints() const; + /// Return the number of joints in the world + uint32_t getNbJoints() const; - /// Return an iterator to the beginning of the rigid bodies of the physics world - std::set::iterator getRigidBodiesBeginIterator(); + /// Return an iterator to the beginning of the rigid bodies of the physics world + std::set::iterator getRigidBodiesBeginIterator(); - /// Return an iterator to the end of the rigid bodies of the physics world - std::set::iterator getRigidBodiesEndIterator(); + /// Return an iterator to the end of the rigid bodies of the physics world + std::set::iterator getRigidBodiesEndIterator(); - /// Return true if the sleeping technique is enabled - bool isSleepingEnabled() const; + /// Return true if the sleeping technique is enabled + bool isSleepingEnabled() const; - /// Enable/Disable the sleeping technique - void enableSleeping(bool isSleepingEnabled); + /// Enable/Disable the sleeping technique + void enableSleeping(bool isSleepingEnabled); - /// Return the current sleep linear velocity - decimal getSleepLinearVelocity() const; + /// Return the current sleep linear velocity + float getSleepLinearVelocity() const; - /// Set the sleep linear velocity. - void setSleepLinearVelocity(decimal sleepLinearVelocity); + /// Set the sleep linear velocity. + void setSleepLinearVelocity(float sleepLinearVelocity); - /// Return the current sleep angular velocity - decimal getSleepAngularVelocity() const; + /// Return the current sleep angular velocity + float getSleepAngularVelocity() const; - /// Set the sleep angular velocity. - void setSleepAngularVelocity(decimal sleepAngularVelocity); + /// Set the sleep angular velocity. + void setSleepAngularVelocity(float sleepAngularVelocity); - /// Return the time a body is required to stay still before sleeping - decimal getTimeBeforeSleep() const; + /// Return the time a body is required to stay still before sleeping + float getTimeBeforeSleep() const; - /// Set the time a body is required to stay still before sleeping - void setTimeBeforeSleep(decimal timeBeforeSleep); + /// Set the time a body is required to stay still before sleeping + void setTimeBeforeSleep(float timeBeforeSleep); - /// Set an event listener object to receive events callbacks. - void setEventListener(EventListener* eventListener); + /// Set an event listener object to receive events callbacks. + void setEventListener(EventListener* eventListener); - /// Test and report collisions between a given shape and all the others - /// shapes of the world - virtual void testCollision(const ProxyShape* shape, - CollisionCallback* callback); + /// Test and report collisions between a given shape and all the others + /// shapes of the world + virtual void testCollision(const ProxyShape* shape, + CollisionCallback* callback); - /// Test and report collisions between two given shapes - virtual void testCollision(const ProxyShape* shape1, - const ProxyShape* shape2, - CollisionCallback* callback); + /// Test and report collisions between two given shapes + virtual void testCollision(const ProxyShape* shape1, + const ProxyShape* shape2, + CollisionCallback* callback); - /// Test and report collisions between a body and all - /// the others bodies of the world - virtual void testCollision(const CollisionBody* body, - CollisionCallback* callback); + /// Test and report collisions between a body and all + /// the others bodies of the world + virtual void testCollision(const CollisionBody* body, + CollisionCallback* callback); - /// Test and report collisions between two bodies - virtual void testCollision(const CollisionBody* body1, - const CollisionBody* body2, - CollisionCallback* callback); + /// Test and report collisions between two bodies + virtual void testCollision(const CollisionBody* body1, + const CollisionBody* body2, + CollisionCallback* callback); - /// Test and report collisions between all shapes of the world - virtual void testCollision(CollisionCallback* callback); + /// Test and report collisions between all shapes of the world + virtual void testCollision(CollisionCallback* callback); - /// Return the list of all contacts of the world - std::vector getContactsList() const; + /// Return the list of all contacts of the world + std::vector getContactsList() const; - // -------------------- Friendship -------------------- // + // -------------------- Friendship -------------------- // - friend class RigidBody; + friend class RigidBody; }; // Reset the external force and torque applied to the bodies inline void DynamicsWorld::resetBodiesForceAndTorque() { - // For each body of the world - std::set::iterator it; - for (it = mRigidBodies.begin(); it != mRigidBodies.end(); ++it) { - (*it)->mExternalForce.setToZero(); - (*it)->mExternalTorque.setToZero(); - } + // For each body of the world + std::set::iterator it; + for (it = mRigidBodies.begin(); it != mRigidBodies.end(); ++it) { + (*it)->mExternalForce.setToZero(); + (*it)->mExternalTorque.setToZero(); + } } // Get the number of iterations for the velocity constraint solver -inline uint DynamicsWorld::getNbIterationsVelocitySolver() const { - return mNbVelocitySolverIterations; +inline uint32_t DynamicsWorld::getNbIterationsVelocitySolver() const { + return mNbVelocitySolverIterations; } // Set the number of iterations for the velocity constraint solver /** * @param nbIterations Number of iterations for the velocity solver */ -inline void DynamicsWorld::setNbIterationsVelocitySolver(uint nbIterations) { - mNbVelocitySolverIterations = nbIterations; +inline void DynamicsWorld::setNbIterationsVelocitySolver(uint32_t nbIterations) { + mNbVelocitySolverIterations = nbIterations; } // Get the number of iterations for the position constraint solver -inline uint DynamicsWorld::getNbIterationsPositionSolver() const { - return mNbPositionSolverIterations; +inline uint32_t DynamicsWorld::getNbIterationsPositionSolver() const { + return mNbPositionSolverIterations; } // Set the number of iterations for the position constraint solver /** * @param nbIterations Number of iterations for the position solver */ -inline void DynamicsWorld::setNbIterationsPositionSolver(uint nbIterations) { - mNbPositionSolverIterations = nbIterations; +inline void DynamicsWorld::setNbIterationsPositionSolver(uint32_t nbIterations) { + mNbPositionSolverIterations = nbIterations; } // Set the position correction technique used for contacts @@ -347,13 +326,13 @@ inline void DynamicsWorld::setNbIterationsPositionSolver(uint nbIterations) { * @param technique Technique used for the position correction (Baumgarte or Split Impulses) */ inline void DynamicsWorld::setContactsPositionCorrectionTechnique( - ContactsPositionCorrectionTechnique technique) { - if (technique == BAUMGARTE_CONTACTS) { - mContactSolver.setIsSplitImpulseActive(false); - } - else { - mContactSolver.setIsSplitImpulseActive(true); - } + ContactsPositionCorrectionTechnique technique) { + if (technique == BAUMGARTE_CONTACTS) { + mContactSolver.setIsSplitImpulseActive(false); + } + else { + mContactSolver.setIsSplitImpulseActive(true); + } } // Set the position correction technique used for joints @@ -361,23 +340,23 @@ inline void DynamicsWorld::setContactsPositionCorrectionTechnique( * @param technique Technique used for the joins position correction (Baumgarte or Non Linear Gauss Seidel) */ inline void DynamicsWorld::setJointsPositionCorrectionTechnique( - JointsPositionCorrectionTechnique technique) { - if (technique == BAUMGARTE_JOINTS) { - mConstraintSolver.setIsNonLinearGaussSeidelPositionCorrectionActive(false); - } - else { - mConstraintSolver.setIsNonLinearGaussSeidelPositionCorrectionActive(true); - } + JointsPositionCorrectionTechnique technique) { + if (technique == BAUMGARTE_JOINTS) { + mConstraintSolver.setIsNonLinearGaussSeidelPositionCorrectionActive(false); + } + else { + mConstraintSolver.setIsNonLinearGaussSeidelPositionCorrectionActive(true); + } } // Activate or deactivate the solving of friction constraints at the center of // the contact manifold instead of solving them at each contact point /** * @param isActive True if you want the friction to be solved at the center of - * the contact manifold and false otherwise + * the contact manifold and false otherwise */ inline void DynamicsWorld::setIsSolveFrictionAtContactManifoldCenterActive(bool isActive) { - mContactSolver.setIsSolveFrictionAtContactManifoldCenterActive(isActive); + mContactSolver.setIsSolveFrictionAtContactManifoldCenterActive(isActive); } // Return the gravity vector of the world @@ -385,7 +364,7 @@ inline void DynamicsWorld::setIsSolveFrictionAtContactManifoldCenterActive(bool * @return The current gravity vector (in meter per seconds squared) */ inline Vector3 DynamicsWorld::getGravity() const { - return mGravity; + return mGravity; } // Set the gravity vector of the world @@ -393,7 +372,7 @@ inline Vector3 DynamicsWorld::getGravity() const { * @param gravity The gravity vector (in meter per seconds squared) */ inline void DynamicsWorld::setGravity(Vector3& gravity) { - mGravity = gravity; + mGravity = gravity; } // Return if the gravity is enaled @@ -401,32 +380,32 @@ inline void DynamicsWorld::setGravity(Vector3& gravity) { * @return True if the gravity is enabled in the world */ inline bool DynamicsWorld::isGravityEnabled() const { - return mIsGravityEnabled; + return mIsGravityEnabled; } // Enable/Disable the gravity /** * @param isGravityEnabled True if you want to enable the gravity in the world - * and false otherwise + * and false otherwise */ inline void DynamicsWorld::setIsGratityEnabled(bool isGravityEnabled) { - mIsGravityEnabled = isGravityEnabled; + mIsGravityEnabled = isGravityEnabled; } // Return the number of rigid bodies in the world /** * @return Number of rigid bodies in the world */ -inline uint DynamicsWorld::getNbRigidBodies() const { - return mRigidBodies.size(); +inline uint32_t DynamicsWorld::getNbRigidBodies() const { + return mRigidBodies.size(); } /// Return the number of joints in the world /** * @return Number of joints in the world */ -inline uint DynamicsWorld::getNbJoints() const { - return mJoints.size(); +inline uint32_t DynamicsWorld::getNbJoints() const { + return mJoints.size(); } // Return an iterator to the beginning of the bodies of the physics world @@ -434,7 +413,7 @@ inline uint DynamicsWorld::getNbJoints() const { * @return Starting iterator of the set of rigid bodies */ inline std::set::iterator DynamicsWorld::getRigidBodiesBeginIterator() { - return mRigidBodies.begin(); + return mRigidBodies.begin(); } // Return an iterator to the end of the bodies of the physics world @@ -442,7 +421,7 @@ inline std::set::iterator DynamicsWorld::getRigidBodiesBeginIterator * @return Ending iterator of the set of rigid bodies */ inline std::set::iterator DynamicsWorld::getRigidBodiesEndIterator() { - return mRigidBodies.end(); + return mRigidBodies.end(); } // Return true if the sleeping technique is enabled @@ -450,15 +429,15 @@ inline std::set::iterator DynamicsWorld::getRigidBodiesEndIterator() * @return True if the sleeping technique is enabled and false otherwise */ inline bool DynamicsWorld::isSleepingEnabled() const { - return mIsSleepingEnabled; + return mIsSleepingEnabled; } // Return the current sleep linear velocity /** * @return The sleep linear velocity (in meters per second) */ -inline decimal DynamicsWorld::getSleepLinearVelocity() const { - return mSleepLinearVelocity; +inline float DynamicsWorld::getSleepLinearVelocity() const { + return mSleepLinearVelocity; } // Set the sleep linear velocity. @@ -468,17 +447,17 @@ inline decimal DynamicsWorld::getSleepLinearVelocity() const { /** * @param sleepLinearVelocity The sleep linear velocity (in meters per second) */ -inline void DynamicsWorld::setSleepLinearVelocity(decimal sleepLinearVelocity) { - assert(sleepLinearVelocity >= decimal(0.0)); - mSleepLinearVelocity = sleepLinearVelocity; +inline void DynamicsWorld::setSleepLinearVelocity(float sleepLinearVelocity) { + assert(sleepLinearVelocity >= float(0.0)); + mSleepLinearVelocity = sleepLinearVelocity; } // Return the current sleep angular velocity /** * @return The sleep angular velocity (in radian per second) */ -inline decimal DynamicsWorld::getSleepAngularVelocity() const { - return mSleepAngularVelocity; +inline float DynamicsWorld::getSleepAngularVelocity() const { + return mSleepAngularVelocity; } // Set the sleep angular velocity. @@ -488,17 +467,17 @@ inline decimal DynamicsWorld::getSleepAngularVelocity() const { /** * @param sleepAngularVelocity The sleep angular velocity (in radian per second) */ -inline void DynamicsWorld::setSleepAngularVelocity(decimal sleepAngularVelocity) { - assert(sleepAngularVelocity >= decimal(0.0)); - mSleepAngularVelocity = sleepAngularVelocity; +inline void DynamicsWorld::setSleepAngularVelocity(float sleepAngularVelocity) { + assert(sleepAngularVelocity >= float(0.0)); + mSleepAngularVelocity = sleepAngularVelocity; } // Return the time a body is required to stay still before sleeping /** * @return Time a body is required to stay still before sleeping (in seconds) */ -inline decimal DynamicsWorld::getTimeBeforeSleep() const { - return mTimeBeforeSleep; +inline float DynamicsWorld::getTimeBeforeSleep() const { + return mTimeBeforeSleep; } @@ -506,22 +485,21 @@ inline decimal DynamicsWorld::getTimeBeforeSleep() const { /** * @param timeBeforeSleep Time a body is required to stay still before sleeping (in seconds) */ -inline void DynamicsWorld::setTimeBeforeSleep(decimal timeBeforeSleep) { - assert(timeBeforeSleep >= decimal(0.0)); - mTimeBeforeSleep = timeBeforeSleep; +inline void DynamicsWorld::setTimeBeforeSleep(float timeBeforeSleep) { + assert(timeBeforeSleep >= float(0.0)); + mTimeBeforeSleep = timeBeforeSleep; } // Set an event listener object to receive events callbacks. /// If you use NULL as an argument, the events callbacks will be disabled. /** * @param eventListener Pointer to the event listener object that will receive - * event callbacks during the simulation + * event callbacks during the simulation */ inline void DynamicsWorld::setEventListener(EventListener* eventListener) { - mEventListener = eventListener; + mEventListener = eventListener; } } -#endif diff --git a/ephysics/engine/EventListener.h b/ephysics/engine/EventListener.h index eadea25..20ba4fe 100644 --- a/ephysics/engine/EventListener.h +++ b/ephysics/engine/EventListener.h @@ -1,30 +1,9 @@ -/******************************************************************************** -* 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 REACTPHYSICS3D_EVENT_LISTENER_H -#define REACTPHYSICS3D_EVENT_LISTENER_H +/** @file + * @author Daniel Chappuis + * @copyright 2010-2016 Daniel Chappuis + * @license BSD 3 clauses (see license file) + */ +#pragma once // Libraries #include @@ -41,39 +20,37 @@ namespace reactphysics3d { */ class EventListener { - public : + public : - /// Constructor - EventListener() {} + /// Constructor + EventListener() {} - /// Destructor - virtual ~EventListener() {} + /// Destructor + virtual ~EventListener() {} - /// Called when a new contact point is found between two bodies that were separated before - /** - * @param contact Information about the contact - */ - virtual void beginContact(const ContactPointInfo& contact) {}; + /// Called when a new contact point is found between two bodies that were separated before + /** + * @param contact Information about the contact + */ + virtual void beginContact(const ContactPointInfo& contact) {}; - /// Called when a new contact point is found between two bodies - /** - * @param contact Information about the contact - */ - virtual void newContact(const ContactPointInfo& contact) {} + /// Called when a new contact point is found between two bodies + /** + * @param contact Information about the contact + */ + virtual void newContact(const ContactPointInfo& contact) {} - /// Called at the beginning of an internal tick of the simulation step. - /// Each time the DynamicsWorld::update() method is called, the physics - /// engine will do several internal simulation steps. This method is - /// called at the beginning of each internal simulation step. - virtual void beginInternalTick() {} + /// Called at the beginning of an int32_ternal tick of the simulation step. + /// Each time the DynamicsWorld::update() method is called, the physics + /// engine will do several int32_ternal simulation steps. This method is + /// called at the beginning of each int32_ternal simulation step. + virtual void beginInternalTick() {} - /// Called at the end of an internal tick of the simulation step. - /// Each time the DynamicsWorld::update() metho is called, the physics - /// engine will do several internal simulation steps. This method is - /// called at the end of each internal simulation step. - virtual void endInternalTick() {} + /// Called at the end of an int32_ternal tick of the simulation step. + /// Each time the DynamicsWorld::update() metho is called, the physics + /// engine will do several int32_ternal simulation steps. This method is + /// called at the end of each int32_ternal simulation step. + virtual void endInternalTick() {} }; } - -#endif diff --git a/ephysics/engine/Impulse.h b/ephysics/engine/Impulse.h index 0124605..f257760 100644 --- a/ephysics/engine/Impulse.h +++ b/ephysics/engine/Impulse.h @@ -1,30 +1,9 @@ -/******************************************************************************** -* 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 REACTPHYSICS3D_IMPULSE_H -#define REACTPHYSICS3D_IMPULSE_H +/** @file + * @author Daniel Chappuis + * @copyright 2010-2016 Daniel Chappuis + * @license BSD 3 clauses (see license file) + */ +#pragma once // Libraries #include @@ -37,51 +16,49 @@ namespace reactphysics3d { */ struct Impulse { - private: + private: - // -------------------- Methods -------------------- // + // -------------------- Methods -------------------- // - /// Private assignment operator - Impulse& operator=(const Impulse& impulse); + /// Private assignment operator + Impulse& operator=(const Impulse& impulse); - public: + public: - // -------------------- Attributes -------------------- // + // -------------------- Attributes -------------------- // - /// Linear impulse applied to the first body - const Vector3 linearImpulseBody1; + /// Linear impulse applied to the first body + const Vector3 linearImpulseBody1; - /// Angular impulse applied to the first body - const Vector3 angularImpulseBody1; + /// Angular impulse applied to the first body + const Vector3 angularImpulseBody1; - /// Linear impulse applied to the second body - const Vector3 linearImpulseBody2; + /// Linear impulse applied to the second body + const Vector3 linearImpulseBody2; - /// Angular impulse applied to the second body - const Vector3 angularImpulseBody2; + /// Angular impulse applied to the second body + const Vector3 angularImpulseBody2; - // -------------------- Methods -------------------- // + // -------------------- Methods -------------------- // - /// Constructor - Impulse(const Vector3& initLinearImpulseBody1, const Vector3& initAngularImpulseBody1, - const Vector3& initLinearImpulseBody2, const Vector3& initAngularImpulseBody2) - : linearImpulseBody1(initLinearImpulseBody1), - angularImpulseBody1(initAngularImpulseBody1), - linearImpulseBody2(initLinearImpulseBody2), - angularImpulseBody2(initAngularImpulseBody2) { + /// Constructor + Impulse(const Vector3& initLinearImpulseBody1, const Vector3& initAngularImpulseBody1, + const Vector3& initLinearImpulseBody2, const Vector3& initAngularImpulseBody2) + : linearImpulseBody1(initLinearImpulseBody1), + angularImpulseBody1(initAngularImpulseBody1), + linearImpulseBody2(initLinearImpulseBody2), + angularImpulseBody2(initAngularImpulseBody2) { - } + } - /// Copy-constructor - Impulse(const Impulse& impulse) - : linearImpulseBody1(impulse.linearImpulseBody1), - angularImpulseBody1(impulse.angularImpulseBody1), - linearImpulseBody2(impulse.linearImpulseBody2), - angularImpulseBody2(impulse.angularImpulseBody2) { + /// Copy-constructor + Impulse(const Impulse& impulse) + : linearImpulseBody1(impulse.linearImpulseBody1), + angularImpulseBody1(impulse.angularImpulseBody1), + linearImpulseBody2(impulse.linearImpulseBody2), + angularImpulseBody2(impulse.angularImpulseBody2) { ; - } + } }; } - -#endif diff --git a/ephysics/engine/Island.cpp b/ephysics/engine/Island.cpp index aaffc49..6f33219 100644 --- a/ephysics/engine/Island.cpp +++ b/ephysics/engine/Island.cpp @@ -1,27 +1,8 @@ -/******************************************************************************** -* 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. * -* * -********************************************************************************/ +/** @file + * @author Daniel Chappuis + * @copyright 2010-2016 Daniel Chappuis + * @license BSD 3 clauses (see license file) + */ // Libraries #include @@ -29,26 +10,26 @@ using namespace reactphysics3d; // Constructor -Island::Island(uint nbMaxBodies, uint nbMaxContactManifolds, uint nbMaxJoints, - MemoryAllocator& memoryAllocator) - : mBodies(NULL), mContactManifolds(NULL), mJoints(NULL), mNbBodies(0), - mNbContactManifolds(0), mNbJoints(0), mMemoryAllocator(memoryAllocator) { +Island::Island(uint32_t nbMaxBodies, uint32_t nbMaxContactManifolds, uint32_t nbMaxJoints, + MemoryAllocator& memoryAllocator) + : mBodies(NULL), mContactManifolds(NULL), mJoints(NULL), mNbBodies(0), + mNbContactManifolds(0), mNbJoints(0), mMemoryAllocator(memoryAllocator) { - // Allocate memory for the arrays - mNbAllocatedBytesBodies = sizeof(RigidBody*) * nbMaxBodies; - mBodies = (RigidBody**) mMemoryAllocator.allocate(mNbAllocatedBytesBodies); - mNbAllocatedBytesContactManifolds = sizeof(ContactManifold*) * nbMaxContactManifolds; - mContactManifolds = (ContactManifold**) mMemoryAllocator.allocate( - mNbAllocatedBytesContactManifolds); - mNbAllocatedBytesJoints = sizeof(Joint*) * nbMaxJoints; - mJoints = (Joint**) mMemoryAllocator.allocate(mNbAllocatedBytesJoints); + // Allocate memory for the arrays + mNbAllocatedBytesBodies = sizeof(RigidBody*) * nbMaxBodies; + mBodies = (RigidBody**) mMemoryAllocator.allocate(mNbAllocatedBytesBodies); + mNbAllocatedBytesContactManifolds = sizeof(ContactManifold*) * nbMaxContactManifolds; + mContactManifolds = (ContactManifold**) mMemoryAllocator.allocate( + mNbAllocatedBytesContactManifolds); + mNbAllocatedBytesJoints = sizeof(Joint*) * nbMaxJoints; + mJoints = (Joint**) mMemoryAllocator.allocate(mNbAllocatedBytesJoints); } // Destructor Island::~Island() { - // Release the memory of the arrays - mMemoryAllocator.release(mBodies, mNbAllocatedBytesBodies); - mMemoryAllocator.release(mContactManifolds, mNbAllocatedBytesContactManifolds); - mMemoryAllocator.release(mJoints, mNbAllocatedBytesJoints); + // Release the memory of the arrays + mMemoryAllocator.release(mBodies, mNbAllocatedBytesBodies); + mMemoryAllocator.release(mContactManifolds, mNbAllocatedBytesContactManifolds); + mMemoryAllocator.release(mJoints, mNbAllocatedBytesJoints); } diff --git a/ephysics/engine/Island.h b/ephysics/engine/Island.h index 605b96d..73db575 100644 --- a/ephysics/engine/Island.h +++ b/ephysics/engine/Island.h @@ -1,30 +1,9 @@ -/******************************************************************************** -* 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 REACTPHYSICS3D_ISLAND_H -#define REACTPHYSICS3D_ISLAND_H +/** @file + * @author Daniel Chappuis + * @copyright 2010-2016 Daniel Chappuis + * @license BSD 3 clauses (see license file) + */ +#pragma once // Libraries #include @@ -41,140 +20,138 @@ namespace reactphysics3d { */ class Island { - private: + private: - // -------------------- Attributes -------------------- // + // -------------------- Attributes -------------------- // - /// Array with all the bodies of the island - RigidBody** mBodies; + /// Array with all the bodies of the island + RigidBody** mBodies; - /// Array with all the contact manifolds between bodies of the island - ContactManifold** mContactManifolds; + /// Array with all the contact manifolds between bodies of the island + ContactManifold** mContactManifolds; - /// Array with all the joints between bodies of the island - Joint** mJoints; + /// Array with all the joints between bodies of the island + Joint** mJoints; - /// Current number of bodies in the island - uint mNbBodies; + /// Current number of bodies in the island + uint32_t mNbBodies; - /// Current number of contact manifold in the island - uint mNbContactManifolds; + /// Current number of contact manifold in the island + uint32_t mNbContactManifolds; - /// Current number of joints in the island - uint mNbJoints; + /// Current number of joints in the island + uint32_t mNbJoints; - /// Reference to the memory allocator - MemoryAllocator& mMemoryAllocator; + /// Reference to the memory allocator + MemoryAllocator& mMemoryAllocator; - /// Number of bytes allocated for the bodies array - size_t mNbAllocatedBytesBodies; + /// Number of bytes allocated for the bodies array + size_t mNbAllocatedBytesBodies; - /// Number of bytes allocated for the contact manifolds array - size_t mNbAllocatedBytesContactManifolds; + /// Number of bytes allocated for the contact manifolds array + size_t mNbAllocatedBytesContactManifolds; - /// Number of bytes allocated for the joints array - size_t mNbAllocatedBytesJoints; + /// Number of bytes allocated for the joints array + size_t mNbAllocatedBytesJoints; - // -------------------- Methods -------------------- // + // -------------------- Methods -------------------- // - /// Private assignment operator - Island& operator=(const Island& island); + /// Private assignment operator + Island& operator=(const Island& island); - /// Private copy-constructor - Island(const Island& island); + /// Private copy-constructor + Island(const Island& island); - public: + public: - // -------------------- Methods -------------------- // + // -------------------- Methods -------------------- // - /// Constructor - Island(uint nbMaxBodies, uint nbMaxContactManifolds, uint nbMaxJoints, - MemoryAllocator& memoryAllocator); + /// Constructor + Island(uint32_t nbMaxBodies, uint32_t nbMaxContactManifolds, uint32_t nbMaxJoints, + MemoryAllocator& memoryAllocator); - /// Destructor - ~Island(); + /// Destructor + ~Island(); - /// Add a body into the island - void addBody(RigidBody* body); + /// Add a body int32_to the island + void addBody(RigidBody* body); - /// Add a contact manifold into the island - void addContactManifold(ContactManifold* contactManifold); + /// Add a contact manifold int32_to the island + void addContactManifold(ContactManifold* contactManifold); - /// Add a joint into the island - void addJoint(Joint* joint); + /// Add a joint int32_to the island + void addJoint(Joint* joint); - /// Return the number of bodies in the island - uint getNbBodies() const; + /// Return the number of bodies in the island + uint32_t getNbBodies() const; - /// Return the number of contact manifolds in the island - uint getNbContactManifolds() const; + /// Return the number of contact manifolds in the island + uint32_t getNbContactManifolds() const; - /// Return the number of joints in the island - uint getNbJoints() const; + /// Return the number of joints in the island + uint32_t getNbJoints() const; - /// Return a pointer to the array of bodies - RigidBody** getBodies(); + /// Return a pointer to the array of bodies + RigidBody** getBodies(); - /// Return a pointer to the array of contact manifolds - ContactManifold** getContactManifold(); + /// Return a pointer to the array of contact manifolds + ContactManifold** getContactManifold(); - /// Return a pointer to the array of joints - Joint** getJoints(); + /// Return a pointer to the array of joints + Joint** getJoints(); - // -------------------- Friendship -------------------- // + // -------------------- Friendship -------------------- // - friend class DynamicsWorld; + friend class DynamicsWorld; }; -// Add a body into the island +// Add a body int32_to the island inline void Island::addBody(RigidBody* body) { - assert(!body->isSleeping()); - mBodies[mNbBodies] = body; - mNbBodies++; + assert(!body->isSleeping()); + mBodies[mNbBodies] = body; + mNbBodies++; } -// Add a contact manifold into the island +// Add a contact manifold int32_to the island inline void Island::addContactManifold(ContactManifold* contactManifold) { - mContactManifolds[mNbContactManifolds] = contactManifold; - mNbContactManifolds++; + mContactManifolds[mNbContactManifolds] = contactManifold; + mNbContactManifolds++; } -// Add a joint into the island +// Add a joint int32_to the island inline void Island::addJoint(Joint* joint) { - mJoints[mNbJoints] = joint; - mNbJoints++; + mJoints[mNbJoints] = joint; + mNbJoints++; } // Return the number of bodies in the island -inline uint Island::getNbBodies() const { - return mNbBodies; +inline uint32_t Island::getNbBodies() const { + return mNbBodies; } // Return the number of contact manifolds in the island -inline uint Island::getNbContactManifolds() const { - return mNbContactManifolds; +inline uint32_t Island::getNbContactManifolds() const { + return mNbContactManifolds; } // Return the number of joints in the island -inline uint Island::getNbJoints() const { - return mNbJoints; +inline uint32_t Island::getNbJoints() const { + return mNbJoints; } // Return a pointer to the array of bodies inline RigidBody** Island::getBodies() { - return mBodies; + return mBodies; } // Return a pointer to the array of contact manifolds inline ContactManifold** Island::getContactManifold() { - return mContactManifolds; + return mContactManifolds; } // Return a pointer to the array of joints inline Joint** Island::getJoints() { - return mJoints; + return mJoints; } } - -#endif diff --git a/ephysics/engine/Material.cpp b/ephysics/engine/Material.cpp index 476d070..44cbb95 100644 --- a/ephysics/engine/Material.cpp +++ b/ephysics/engine/Material.cpp @@ -1,27 +1,8 @@ -/******************************************************************************** -* 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. * -* * -********************************************************************************/ +/** @file + * @author Daniel Chappuis + * @copyright 2010-2016 Daniel Chappuis + * @license BSD 3 clauses (see license file) + */ // Libraries #include @@ -30,16 +11,16 @@ using namespace reactphysics3d; // Constructor Material::Material() - : mFrictionCoefficient(DEFAULT_FRICTION_COEFFICIENT), - mRollingResistance(DEFAULT_ROLLING_RESISTANCE), - mBounciness(DEFAULT_BOUNCINESS) { + : mFrictionCoefficient(DEFAULT_FRICTION_COEFFICIENT), + mRollingResistance(DEFAULT_ROLLING_RESISTANCE), + mBounciness(DEFAULT_BOUNCINESS) { } // Copy-constructor Material::Material(const Material& material) - : mFrictionCoefficient(material.mFrictionCoefficient), - mRollingResistance(material.mRollingResistance), mBounciness(material.mBounciness) { + : mFrictionCoefficient(material.mFrictionCoefficient), + mRollingResistance(material.mRollingResistance), mBounciness(material.mBounciness) { } diff --git a/ephysics/engine/Material.h b/ephysics/engine/Material.h index 367db7b..33831a4 100644 --- a/ephysics/engine/Material.h +++ b/ephysics/engine/Material.h @@ -1,30 +1,9 @@ -/******************************************************************************** -* 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 REACTPHYSICS3D_MATERIAL_H -#define REACTPHYSICS3D_MATERIAL_H +/** @file + * @author Daniel Chappuis + * @copyright 2010-2016 Daniel Chappuis + * @license BSD 3 clauses (see license file) + */ +#pragma once // Libraries #include @@ -40,60 +19,60 @@ namespace reactphysics3d { */ class Material { - private : + private : - // -------------------- Attributes -------------------- // + // -------------------- Attributes -------------------- // - /// Friction coefficient (positive value) - decimal mFrictionCoefficient; + /// Friction coefficient (positive value) + float mFrictionCoefficient; - /// Rolling resistance factor (positive value) - decimal mRollingResistance; + /// Rolling resistance factor (positive value) + float mRollingResistance; - /// Bounciness during collisions (between 0 and 1) where 1 is for a very bouncy body - decimal mBounciness; + /// Bounciness during collisions (between 0 and 1) where 1 is for a very bouncy body + float mBounciness; - public : + public : - // -------------------- Methods -------------------- // + // -------------------- Methods -------------------- // - /// Constructor - Material(); + /// Constructor + Material(); - /// Copy-constructor - Material(const Material& material); + /// Copy-constructor + Material(const Material& material); - /// Destructor - ~Material(); + /// Destructor + ~Material(); - /// Return the bounciness - decimal getBounciness() const; + /// Return the bounciness + float getBounciness() const; - /// Set the bounciness. - void setBounciness(decimal bounciness); + /// Set the bounciness. + void setBounciness(float bounciness); - /// Return the friction coefficient - decimal getFrictionCoefficient() const; + /// Return the friction coefficient + float getFrictionCoefficient() const; - /// Set the friction coefficient. - void setFrictionCoefficient(decimal frictionCoefficient); + /// Set the friction coefficient. + void setFrictionCoefficient(float frictionCoefficient); - /// Return the rolling resistance factor - decimal getRollingResistance() const; + /// Return the rolling resistance factor + float getRollingResistance() const; - /// Set the rolling resistance factor - void setRollingResistance(decimal rollingResistance); + /// Set the rolling resistance factor + void setRollingResistance(float rollingResistance); - /// Overloaded assignment operator - Material& operator=(const Material& material); + /// Overloaded assignment operator + Material& operator=(const Material& material); }; // Return the bounciness /** * @return Bounciness factor (between 0 and 1) where 1 is very bouncy */ -inline decimal Material::getBounciness() const { - return mBounciness; +inline float Material::getBounciness() const { + return mBounciness; } // Set the bounciness. @@ -102,17 +81,17 @@ inline decimal Material::getBounciness() const { /** * @param bounciness Bounciness factor (between 0 and 1) where 1 is very bouncy */ -inline void Material::setBounciness(decimal bounciness) { - assert(bounciness >= decimal(0.0) && bounciness <= decimal(1.0)); - mBounciness = bounciness; +inline void Material::setBounciness(float bounciness) { + assert(bounciness >= float(0.0) && bounciness <= float(1.0)); + mBounciness = bounciness; } // Return the friction coefficient /** * @return Friction coefficient (positive value) */ -inline decimal Material::getFrictionCoefficient() const { - return mFrictionCoefficient; +inline float Material::getFrictionCoefficient() const { + return mFrictionCoefficient; } // Set the friction coefficient. @@ -121,9 +100,9 @@ inline decimal Material::getFrictionCoefficient() const { /** * @param frictionCoefficient Friction coefficient (positive value) */ -inline void Material::setFrictionCoefficient(decimal frictionCoefficient) { - assert(frictionCoefficient >= decimal(0.0)); - mFrictionCoefficient = frictionCoefficient; +inline void Material::setFrictionCoefficient(float frictionCoefficient) { + assert(frictionCoefficient >= float(0.0)); + mFrictionCoefficient = frictionCoefficient; } // Return the rolling resistance factor. If this value is larger than zero, @@ -132,8 +111,8 @@ inline void Material::setFrictionCoefficient(decimal frictionCoefficient) { /** * @return The rolling resistance factor (positive value) */ -inline decimal Material::getRollingResistance() const { - return mRollingResistance; +inline float Material::getRollingResistance() const { + return mRollingResistance; } // Set the rolling resistance factor. If this value is larger than zero, @@ -142,25 +121,23 @@ inline decimal Material::getRollingResistance() const { /** * @param rollingResistance The rolling resistance factor */ -inline void Material::setRollingResistance(decimal rollingResistance) { - assert(rollingResistance >= 0); - mRollingResistance = rollingResistance; +inline void Material::setRollingResistance(float rollingResistance) { + assert(rollingResistance >= 0); + mRollingResistance = rollingResistance; } // 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; - } + // Check for self-assignment + if (this != &material) { + mFrictionCoefficient = material.mFrictionCoefficient; + mBounciness = material.mBounciness; + mRollingResistance = material.mRollingResistance; + } - // Return this material - return *this; + // Return this material + return *this; } } - -#endif diff --git a/ephysics/engine/OverlappingPair.cpp b/ephysics/engine/OverlappingPair.cpp index 9dff3f7..6e77df6 100644 --- a/ephysics/engine/OverlappingPair.cpp +++ b/ephysics/engine/OverlappingPair.cpp @@ -1,27 +1,8 @@ -/******************************************************************************** -* 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. * -* * -********************************************************************************/ +/** @file + * @author Daniel Chappuis + * @copyright 2010-2016 Daniel Chappuis + * @license BSD 3 clauses (see license file) + */ // Libraries #include @@ -30,14 +11,13 @@ using namespace reactphysics3d; // Constructor -OverlappingPair::OverlappingPair(ProxyShape* shape1, ProxyShape* shape2, - int nbMaxContactManifolds, MemoryAllocator& memoryAllocator) - : mContactManifoldSet(shape1, shape2, memoryAllocator, nbMaxContactManifolds), - mCachedSeparatingAxis(1.0, 1.0, 1.0) { - +OverlappingPair::OverlappingPair(ProxyShape* shape1, ProxyShape* shape2, int32_t nbMaxContactManifolds, MemoryAllocator& memoryAllocator): + mContactManifoldSet(shape1, shape2, memoryAllocator, nbMaxContactManifolds), + mCachedSeparatingAxis(1.0, 1.0, 1.0) { + } // Destructor OverlappingPair::~OverlappingPair() { - -} + +} diff --git a/ephysics/engine/OverlappingPair.h b/ephysics/engine/OverlappingPair.h index 587bd48..45bf878 100644 --- a/ephysics/engine/OverlappingPair.h +++ b/ephysics/engine/OverlappingPair.h @@ -1,30 +1,9 @@ -/******************************************************************************** -* 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 REACTPHYSICS3D_OVERLAPPING_PAIR_H -#define REACTPHYSICS3D_OVERLAPPING_PAIR_H +/** @file + * @author Daniel Chappuis + * @copyright 2010-2016 Daniel Chappuis + * @license BSD 3 clauses (see license file) + */ +#pragma once // Libraries #include @@ -35,7 +14,7 @@ namespace reactphysics3d { // Type for the overlapping pair ID -typedef std::pair overlappingpairid; +typedef std::pair overlappingpairid; // Class OverlappingPair /** @@ -47,136 +26,136 @@ typedef std::pair overlappingpairid; */ class OverlappingPair { - private: + private: - // -------------------- Attributes -------------------- // + // -------------------- Attributes -------------------- // - /// Set of persistent contact manifolds - ContactManifoldSet mContactManifoldSet; + /// Set of persistent contact manifolds + ContactManifoldSet mContactManifoldSet; - /// Cached previous separating axis - Vector3 mCachedSeparatingAxis; - - // -------------------- Methods -------------------- // + /// Cached previous separating axis + Vector3 mCachedSeparatingAxis; + + // -------------------- Methods -------------------- // - /// Private copy-constructor - OverlappingPair(const OverlappingPair& pair); + /// Private copy-constructor + OverlappingPair(const OverlappingPair& pair); - /// Private assignment operator - OverlappingPair& operator=(const OverlappingPair& pair); + /// Private assignment operator + OverlappingPair& operator=(const OverlappingPair& pair); - public: + public: - // -------------------- Methods -------------------- // + // -------------------- Methods -------------------- // - /// Constructor - OverlappingPair(ProxyShape* shape1, ProxyShape* shape2, - int nbMaxContactManifolds, MemoryAllocator& memoryAllocator); + /// Constructor + OverlappingPair(ProxyShape* shape1, ProxyShape* shape2, + int32_t nbMaxContactManifolds, MemoryAllocator& memoryAllocator); - /// Destructor - ~OverlappingPair(); - - /// Return the pointer to first proxy collision shape - ProxyShape* getShape1() const; + /// Destructor + ~OverlappingPair(); + + /// Return the pointer to first proxy collision shape + ProxyShape* getShape1() const; - /// Return the pointer to second body - ProxyShape* getShape2() const; + /// Return the pointer to second body + ProxyShape* getShape2() const; - /// Add a contact to the contact cache - void addContact(ContactPoint* contact); + /// Add a contact to the contact cache + void addContact(ContactPoint* contact); - /// Update the contact cache - void update(); + /// Update the contact cache + void update(); - /// Return the cached separating axis - Vector3 getCachedSeparatingAxis() const; + /// Return the cached separating axis + Vector3 getCachedSeparatingAxis() const; - /// Set the cached separating axis - void setCachedSeparatingAxis(const Vector3& axis); + /// Set the cached separating axis + void setCachedSeparatingAxis(const Vector3& axis); - /// Return the number of contacts in the cache - uint getNbContactPoints() const; + /// Return the number of contacts in the cache + uint32_t getNbContactPoints() const; - /// Return the a reference to the contact manifold set - const ContactManifoldSet& getContactManifoldSet(); + /// Return the a reference to the contact manifold set + const ContactManifoldSet& getContactManifoldSet(); - /// Clear the contact points of the contact manifold - void clearContactPoints(); + /// Clear the contact points of the contact manifold + void clearContactPoints(); - /// Return the pair of bodies index - static overlappingpairid computeID(ProxyShape* shape1, ProxyShape* shape2); + /// Return the pair of bodies index + static overlappingpairid computeID(ProxyShape* shape1, ProxyShape* shape2); - /// Return the pair of bodies index of the pair - static bodyindexpair computeBodiesIndexPair(CollisionBody* body1, CollisionBody* body2); + /// Return the pair of bodies index of the pair + static bodyindexpair computeBodiesIndexPair(CollisionBody* body1, CollisionBody* body2); - // -------------------- Friendship -------------------- // + // -------------------- Friendship -------------------- // - friend class DynamicsWorld; + friend class DynamicsWorld; }; // Return the pointer to first body inline ProxyShape* OverlappingPair::getShape1() const { - return mContactManifoldSet.getShape1(); -} + return mContactManifoldSet.getShape1(); +} // Return the pointer to second body inline ProxyShape* OverlappingPair::getShape2() const { - return mContactManifoldSet.getShape2(); -} + return mContactManifoldSet.getShape2(); +} // Add a contact to the contact manifold inline void OverlappingPair::addContact(ContactPoint* contact) { - mContactManifoldSet.addContactPoint(contact); + mContactManifoldSet.addContactPoint(contact); } // Update the contact manifold inline void OverlappingPair::update() { - mContactManifoldSet.update(); + mContactManifoldSet.update(); } // Return the cached separating axis inline Vector3 OverlappingPair::getCachedSeparatingAxis() const { - return mCachedSeparatingAxis; + return mCachedSeparatingAxis; } // Set the cached separating axis inline void OverlappingPair::setCachedSeparatingAxis(const Vector3& axis) { - mCachedSeparatingAxis = axis; + mCachedSeparatingAxis = axis; } // Return the number of contact points in the contact manifold -inline uint OverlappingPair::getNbContactPoints() const { - return mContactManifoldSet.getTotalNbContactPoints(); +inline uint32_t OverlappingPair::getNbContactPoints() const { + return mContactManifoldSet.getTotalNbContactPoints(); } // Return the contact manifold inline const ContactManifoldSet& OverlappingPair::getContactManifoldSet() { - return mContactManifoldSet; + return mContactManifoldSet; } // Return the pair of bodies index inline overlappingpairid OverlappingPair::computeID(ProxyShape* shape1, ProxyShape* shape2) { - assert(shape1->mBroadPhaseID >= 0 && shape2->mBroadPhaseID >= 0); + assert(shape1->mBroadPhaseID >= 0 && shape2->mBroadPhaseID >= 0); - // Construct the pair of body index - overlappingpairid pairID = shape1->mBroadPhaseID < shape2->mBroadPhaseID ? - std::make_pair(shape1->mBroadPhaseID, shape2->mBroadPhaseID) : - std::make_pair(shape2->mBroadPhaseID, shape1->mBroadPhaseID); - assert(pairID.first != pairID.second); - return pairID; + // Construct the pair of body index + overlappingpairid pairID = shape1->mBroadPhaseID < shape2->mBroadPhaseID ? + std::make_pair(shape1->mBroadPhaseID, shape2->mBroadPhaseID) : + std::make_pair(shape2->mBroadPhaseID, shape1->mBroadPhaseID); + assert(pairID.first != pairID.second); + return pairID; } // Return the pair of bodies index inline bodyindexpair OverlappingPair::computeBodiesIndexPair(CollisionBody* body1, - CollisionBody* body2) { + CollisionBody* body2) { - // Construct the pair of body index - bodyindexpair indexPair = body1->getID() < body2->getID() ? - std::make_pair(body1->getID(), body2->getID()) : - std::make_pair(body2->getID(), body1->getID()); - assert(indexPair.first != indexPair.second); - return indexPair; + // Construct the pair of body index + bodyindexpair indexPair = body1->getID() < body2->getID() ? + std::make_pair(body1->getID(), body2->getID()) : + std::make_pair(body2->getID(), body1->getID()); + assert(indexPair.first != indexPair.second); + return indexPair; } // Clear the contact points of the contact manifold @@ -186,5 +165,3 @@ inline void OverlappingPair::clearContactPoints() { } -#endif - diff --git a/ephysics/engine/Profiler.cpp b/ephysics/engine/Profiler.cpp index 4fea176..b8978d4 100644 --- a/ephysics/engine/Profiler.cpp +++ b/ephysics/engine/Profiler.cpp @@ -1,27 +1,8 @@ -/******************************************************************************** -* 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. * -* * -********************************************************************************/ +/** @file + * @author Daniel Chappuis + * @copyright 2010-2016 Daniel Chappuis + * @license BSD 3 clauses (see license file) + */ #ifdef IS_PROFILING_ACTIVE @@ -34,224 +15,224 @@ using namespace reactphysics3d; ProfileNode Profiler::mRootNode("Root", NULL); ProfileNode* Profiler::mCurrentNode = &Profiler::mRootNode; long double Profiler::mProfilingStartTime = Timer::getCurrentSystemTime() * 1000.0; -uint Profiler::mFrameCounter = 0; +uint32_t Profiler::mFrameCounter = 0; // Constructor ProfileNode::ProfileNode(const char* name, ProfileNode* parentNode) - :mName(name), mNbTotalCalls(0), mStartingTime(0), mTotalTime(0), - mRecursionCounter(0), mParentNode(parentNode), mChildNode(NULL), - mSiblingNode(NULL) { - reset(); + :mName(name), mNbTotalCalls(0), mStartingTime(0), mTotalTime(0), + mRecursionCounter(0), mParentNode(parentNode), mChildNode(NULL), + mSiblingNode(NULL) { + reset(); } // Destructor ProfileNode::~ProfileNode() { - delete mChildNode; - delete mSiblingNode; + delete mChildNode; + delete mSiblingNode; } // Return a pointer to a sub node with a given name ProfileNode* ProfileNode::findSubNode(const char* name) { - // Try to find the node among the child nodes - ProfileNode* child = mChildNode; - while (child != NULL) { - if (child->mName == name) { - return child; - } - child = child->mSiblingNode; - } + // Try to find the node among the child nodes + ProfileNode* child = mChildNode; + while (child != NULL) { + if (child->mName == name) { + return child; + } + child = child->mSiblingNode; + } - // The nose has not been found. Therefore, we create it - // and add it to the profiler tree - ProfileNode* newNode = new ProfileNode(name, this); - newNode->mSiblingNode = mChildNode; - mChildNode = newNode; + // The nose has not been found. Therefore, we create it + // and add it to the profiler tree + ProfileNode* newNode = new ProfileNode(name, this); + newNode->mSiblingNode = mChildNode; + mChildNode = newNode; - return newNode; + return newNode; } // Called when we enter the block of code corresponding to this profile node void ProfileNode::enterBlockOfCode() { - mNbTotalCalls++; + mNbTotalCalls++; - // If the current code is not called recursively - if (mRecursionCounter == 0) { + // If the current code is not called recursively + if (mRecursionCounter == 0) { - // Get the current system time to initialize the starting time of - // the profiling of the current block of code - mStartingTime = Timer::getCurrentSystemTime() * 1000.0; - } + // Get the current system time to initialize the starting time of + // the profiling of the current block of code + mStartingTime = Timer::getCurrentSystemTime() * 1000.0; + } - mRecursionCounter++; + mRecursionCounter++; } // Called when we exit the block of code corresponding to this profile node bool ProfileNode::exitBlockOfCode() { - mRecursionCounter--; + mRecursionCounter--; - if (mRecursionCounter == 0 && mNbTotalCalls != 0) { + if (mRecursionCounter == 0 && mNbTotalCalls != 0) { - // Get the current system time - long double currentTime = Timer::getCurrentSystemTime() * 1000.0; + // Get the current system time + long double currentTime = Timer::getCurrentSystemTime() * 1000.0; - // Increase the total elasped time in the current block of code - mTotalTime += currentTime - mStartingTime; - } + // Increase the total elasped time in the current block of code + mTotalTime += currentTime - mStartingTime; + } - // Return true if the current code is not recursing - return (mRecursionCounter == 0); + // Return true if the current code is not recursing + return (mRecursionCounter == 0); } // Reset the profiling of the node void ProfileNode::reset() { - mNbTotalCalls = 0; - mTotalTime = 0.0; + mNbTotalCalls = 0; + mTotalTime = 0.0; - // Reset the child node - if (mChildNode != NULL) { - mChildNode->reset(); - } + // Reset the child node + if (mChildNode != NULL) { + mChildNode->reset(); + } - // Reset the sibling node - if (mSiblingNode != NULL) { - mSiblingNode->reset(); - } + // Reset the sibling node + if (mSiblingNode != NULL) { + mSiblingNode->reset(); + } } // Destroy the node void ProfileNode::destroy() { - delete mChildNode; - mChildNode = NULL; - delete mSiblingNode; - mSiblingNode = NULL; + delete mChildNode; + mChildNode = NULL; + delete mSiblingNode; + mSiblingNode = NULL; } // Constructor ProfileNodeIterator::ProfileNodeIterator(ProfileNode* startingNode) - :mCurrentParentNode(startingNode), - mCurrentChildNode(mCurrentParentNode->getChildNode()){ + :mCurrentParentNode(startingNode), + mCurrentChildNode(mCurrentParentNode->getChildNode()){ } // Enter a given child node -void ProfileNodeIterator::enterChild(int index) { - mCurrentChildNode = mCurrentParentNode->getChildNode(); - while ((mCurrentChildNode != NULL) && (index != 0)) { - index--; - mCurrentChildNode = mCurrentChildNode->getSiblingNode(); - } +void ProfileNodeIterator::enterChild(int32_t index) { + mCurrentChildNode = mCurrentParentNode->getChildNode(); + while ((mCurrentChildNode != NULL) && (index != 0)) { + index--; + mCurrentChildNode = mCurrentChildNode->getSiblingNode(); + } - if (mCurrentChildNode != NULL) { - mCurrentParentNode = mCurrentChildNode; - mCurrentChildNode = mCurrentParentNode->getChildNode(); - } + if (mCurrentChildNode != NULL) { + mCurrentParentNode = mCurrentChildNode; + mCurrentChildNode = mCurrentParentNode->getChildNode(); + } } // Enter a given parent node void ProfileNodeIterator::enterParent() { - if (mCurrentParentNode->getParentNode() != NULL) { - mCurrentParentNode = mCurrentParentNode->getParentNode(); - } - mCurrentChildNode = mCurrentParentNode->getChildNode(); + if (mCurrentParentNode->getParentNode() != NULL) { + mCurrentParentNode = mCurrentParentNode->getParentNode(); + } + mCurrentChildNode = mCurrentParentNode->getChildNode(); } // Method called when we want to start profiling a block of code. void Profiler::startProfilingBlock(const char* name) { - // Look for the node in the tree that corresponds to the block of - // code to profile - if (name != mCurrentNode->getName()) { - mCurrentNode = mCurrentNode->findSubNode(name); - } + // Look for the node in the tree that corresponds to the block of + // code to profile + if (name != mCurrentNode->getName()) { + mCurrentNode = mCurrentNode->findSubNode(name); + } - // Start profile the node - mCurrentNode->enterBlockOfCode(); + // Start profile the node + mCurrentNode->enterBlockOfCode(); } // Method called at the end of the scope where the // startProfilingBlock() method has been called. void Profiler::stopProfilingBlock() { - // Go to the parent node unless if the current block - // of code is recursing - if (mCurrentNode->exitBlockOfCode()) { - mCurrentNode = mCurrentNode->getParentNode(); - } + // Go to the parent node unless if the current block + // of code is recursing + if (mCurrentNode->exitBlockOfCode()) { + mCurrentNode = mCurrentNode->getParentNode(); + } } // Reset the timing data of the profiler (but not the profiler tree structure) void Profiler::reset() { - mRootNode.reset(); - mRootNode.enterBlockOfCode(); - mFrameCounter = 0; - mProfilingStartTime = Timer::getCurrentSystemTime() * 1000.0; + mRootNode.reset(); + mRootNode.enterBlockOfCode(); + mFrameCounter = 0; + mProfilingStartTime = Timer::getCurrentSystemTime() * 1000.0; } -// Print the report of the profiler in a given output stream -void Profiler::printReport(std::ostream& outputStream) { - ProfileNodeIterator* iterator = Profiler::getIterator(); +// Print32_t the report of the profiler in a given output stream +void Profiler::print32_tReport(std::ostream& outputStream) { + ProfileNodeIterator* iterator = Profiler::getIterator(); - // Recursively print the report of each node of the profiler tree - printRecursiveNodeReport(iterator, 0, outputStream); + // Recursively print32_t the report of each node of the profiler tree + print32_tRecursiveNodeReport(iterator, 0, outputStream); - // Destroy the iterator - destroyIterator(iterator); + // Destroy the iterator + destroyIterator(iterator); } -// Recursively print the report of a given node of the profiler tree -void Profiler::printRecursiveNodeReport(ProfileNodeIterator* iterator, - int spacing, - std::ostream& outputStream) { - iterator->first(); +// Recursively print32_t the report of a given node of the profiler tree +void Profiler::print32_tRecursiveNodeReport(ProfileNodeIterator* iterator, + int32_t spacing, + std::ostream& outputStream) { + iterator->first(); - // If we are at the end of a branch in the profiler tree - if (iterator->isEnd()) { - return; - } + // If we are at the end of a branch in the profiler tree + if (iterator->isEnd()) { + return; + } - long double parentTime = iterator->isRoot() ? getElapsedTimeSinceStart() : - iterator->getCurrentParentTotalTime(); - long double accumulatedTime = 0.0; - uint nbFrames = Profiler::getNbFrames(); - for (int i=0; igetCurrentParentName() << - " (total running time : " << parentTime << " ms) ---" << std::endl; - long double totalTime = 0.0; + long double parentTime = iterator->isRoot() ? getElapsedTimeSinceStart() : + iterator->getCurrentParentTotalTime(); + long double accumulatedTime = 0.0; + uint32_t nbFrames = Profiler::getNbFrames(); + for (int32_t i=0; igetCurrentParentName() << + " (total running time : " << parentTime << " ms) ---" << std::endl; + long double totalTime = 0.0; - // Recurse over the children of the current node - int nbChildren = 0; - for (int i=0; !iterator->isEnd(); i++, iterator->next()) { - nbChildren++; - long double currentTotalTime = iterator->getCurrentTotalTime(); - accumulatedTime += currentTotalTime; - long double fraction = parentTime > std::numeric_limits::epsilon() ? - (currentTotalTime / parentTime) * 100.0 : 0.0; - for (int j=0; jgetCurrentName() << " : " << - fraction << " % | " << (currentTotalTime / (long double) (nbFrames)) << - " ms/frame (" << iterator->getCurrentNbTotalCalls() << " calls)" << - std::endl; - totalTime += currentTotalTime; - } + // Recurse over the children of the current node + int32_t nbChildren = 0; + for (int32_t i=0; !iterator->isEnd(); i++, iterator->next()) { + nbChildren++; + long double currentTotalTime = iterator->getCurrentTotalTime(); + accumulatedTime += currentTotalTime; + long double fraction = parentTime > std::numeric_limits::epsilon() ? + (currentTotalTime / parentTime) * 100.0 : 0.0; + for (int32_t j=0; jgetCurrentName() << " : " << + fraction << " % | " << (currentTotalTime / (long double) (nbFrames)) << + " ms/frame (" << iterator->getCurrentNbTotalCalls() << " calls)" << + std::endl; + totalTime += currentTotalTime; + } - if (parentTime < accumulatedTime) { - outputStream << "Something is wrong !" << std::endl; - } - for (int i=0; i std::numeric_limits::epsilon() ? - ((parentTime - accumulatedTime) / parentTime) * 100.0 : 0.0; - long double difference = parentTime - accumulatedTime; - outputStream << "| Unaccounted : " << difference << " ms (" << percentage << " %)" << std::endl; + if (parentTime < accumulatedTime) { + outputStream << "Something is wrong !" << std::endl; + } + for (int32_t i=0; i std::numeric_limits::epsilon() ? + ((parentTime - accumulatedTime) / parentTime) * 100.0 : 0.0; + long double difference = parentTime - accumulatedTime; + outputStream << "| Unaccounted : " << difference << " ms (" << percentage << " %)" << std::endl; - for (int i=0; ienterChild(i); - printRecursiveNodeReport(iterator, spacing + 3, outputStream); - iterator->enterParent(); - } + for (int32_t i=0; ienterChild(i); + print32_tRecursiveNodeReport(iterator, spacing + 3, outputStream); + iterator->enterParent(); + } } #endif diff --git a/ephysics/engine/Profiler.h b/ephysics/engine/Profiler.h index 2cc5fde..0dfd744 100644 --- a/ephysics/engine/Profiler.h +++ b/ephysics/engine/Profiler.h @@ -1,30 +1,9 @@ -/******************************************************************************** -* 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 REACTPHYSICS3D_PROFILER_H -#define REACTPHYSICS3D_PROFILER_H +/** @file + * @author Daniel Chappuis + * @copyright 2010-2016 Daniel Chappuis + * @license BSD 3 clauses (see license file) + */ +#pragma once #ifdef IS_PROFILING_ACTIVE @@ -41,76 +20,76 @@ namespace reactphysics3d { */ class ProfileNode { - private : + private : - // -------------------- Attributes -------------------- // + // -------------------- Attributes -------------------- // - /// Name of the node - const char* mName; + /// Name of the node + const char* mName; - /// Total number of calls of this node - uint mNbTotalCalls; + /// Total number of calls of this node + uint32_t mNbTotalCalls; - /// Starting time of the sampling of corresponding block of code - long double mStartingTime; + /// Starting time of the sampling of corresponding block of code + long double mStartingTime; - /// Total time spent in the block of code - long double mTotalTime; + /// Total time spent in the block of code + long double mTotalTime; - /// Recursion counter - int mRecursionCounter; + /// Recursion counter + int32_t mRecursionCounter; - /// Pointer to the parent node - ProfileNode* mParentNode; + /// Pointer to the parent node + ProfileNode* mParentNode; - /// Pointer to a child node - ProfileNode* mChildNode; + /// Pointer to a child node + ProfileNode* mChildNode; - /// Pointer to a sibling node - ProfileNode* mSiblingNode; + /// Pointer to a sibling node + ProfileNode* mSiblingNode; - public : + public : - // -------------------- Methods -------------------- // + // -------------------- Methods -------------------- // - /// Constructor - ProfileNode(const char* name, ProfileNode* parentNode); + /// Constructor + ProfileNode(const char* name, ProfileNode* parentNode); - /// Destructor - ~ProfileNode(); + /// Destructor + ~ProfileNode(); - /// Return a pointer to a sub node - ProfileNode* findSubNode(const char* name); + /// Return a pointer to a sub node + ProfileNode* findSubNode(const char* name); - /// Return a pointer to the parent node - ProfileNode* getParentNode(); + /// Return a pointer to the parent node + ProfileNode* getParentNode(); - /// Return a pointer to a sibling node - ProfileNode* getSiblingNode(); + /// Return a pointer to a sibling node + ProfileNode* getSiblingNode(); - /// Return a pointer to a child node - ProfileNode* getChildNode(); + /// Return a pointer to a child node + ProfileNode* getChildNode(); - /// Return the name of the node - const char* getName(); + /// Return the name of the node + const char* getName(); - /// Return the total number of call of the corresponding block of code - uint getNbTotalCalls() const; + /// Return the total number of call of the corresponding block of code + uint32_t getNbTotalCalls() const; - /// Return the total time spent in the block of code - long double getTotalTime() const; + /// Return the total time spent in the block of code + long double getTotalTime() const; - /// Called when we enter the block of code corresponding to this profile node - void enterBlockOfCode(); + /// Called when we enter the block of code corresponding to this profile node + void enterBlockOfCode(); - /// Called when we exit the block of code corresponding to this profile node - bool exitBlockOfCode(); + /// Called when we exit the block of code corresponding to this profile node + bool exitBlockOfCode(); - /// Reset the profiling of the node - void reset(); + /// Reset the profiling of the node + void reset(); - /// Destroy the node - void destroy(); + /// Destroy the node + void destroy(); }; // Class ProfileNodeIterator @@ -119,58 +98,58 @@ class ProfileNode { */ class ProfileNodeIterator { - private : + private : - // -------------------- Attributes -------------------- // + // -------------------- Attributes -------------------- // - /// Current parent node - ProfileNode* mCurrentParentNode; + /// Current parent node + ProfileNode* mCurrentParentNode; - /// Current child node - ProfileNode* mCurrentChildNode; + /// Current child node + ProfileNode* mCurrentChildNode; - public : + public : - // -------------------- Methods -------------------- // + // -------------------- Methods -------------------- // - /// Constructor - ProfileNodeIterator(ProfileNode* startingNode); + /// Constructor + ProfileNodeIterator(ProfileNode* startingNode); - /// Go to the first node - void first(); + /// Go to the first node + void first(); - /// Go to the next node - void next(); + /// Go to the next node + void next(); - /// Enter a given child node - void enterChild(int index); + /// Enter a given child node + void enterChild(int32_t index); - /// Enter a given parent node - void enterParent(); + /// Enter a given parent node + void enterParent(); - /// Return true if we are at the root of the profiler tree - bool isRoot(); + /// Return true if we are at the root of the profiler tree + bool isRoot(); - /// Return true if we are at the end of a branch of the profiler tree - bool isEnd(); + /// Return true if we are at the end of a branch of the profiler tree + bool isEnd(); - /// Return the name of the current node - const char* getCurrentName(); + /// Return the name of the current node + const char* getCurrentName(); - /// Return the total time of the current node - long double getCurrentTotalTime(); + /// Return the total time of the current node + long double getCurrentTotalTime(); - /// Return the total number of calls of the current node - uint getCurrentNbTotalCalls(); + /// Return the total number of calls of the current node + uint32_t getCurrentNbTotalCalls(); - /// Return the name of the current parent node - const char* getCurrentParentName(); + /// Return the name of the current parent node + const char* getCurrentParentName(); - /// Return the total time of the current parent node - long double getCurrentParentTotalTime(); + /// Return the total time of the current parent node + long double getCurrentParentTotalTime(); - /// Return the total number of calls of the current parent node - uint getCurrentParentNbTotalCalls(); + /// Return the total number of calls of the current parent node + uint32_t getCurrentParentNbTotalCalls(); }; // Class Profiler @@ -180,61 +159,61 @@ class ProfileNodeIterator { */ class Profiler { - private : + private : - // -------------------- Attributes -------------------- // + // -------------------- Attributes -------------------- // - /// Root node of the profiler tree - static ProfileNode mRootNode; + /// Root node of the profiler tree + static ProfileNode mRootNode; - /// Current node in the current execution - static ProfileNode* mCurrentNode; + /// Current node in the current execution + static ProfileNode* mCurrentNode; - /// Frame counter - static uint mFrameCounter; + /// Frame counter + static uint32_t mFrameCounter; - /// Starting profiling time - static long double mProfilingStartTime; + /// Starting profiling time + static long double mProfilingStartTime; - /// Recursively print the report of a given node of the profiler tree - static void printRecursiveNodeReport(ProfileNodeIterator* iterator, - int spacing, - std::ostream& outputStream); + /// Recursively print32_t the report of a given node of the profiler tree + static void print32_tRecursiveNodeReport(ProfileNodeIterator* iterator, + int32_t spacing, + std::ostream& outputStream); - public : + public : - // -------------------- Methods -------------------- // + // -------------------- Methods -------------------- // - /// Method called when we want to start profiling a block of code. - static void startProfilingBlock(const char *name); + /// Method called when we want to start profiling a block of code. + static void startProfilingBlock(const char *name); - /// Method called at the end of the scope where the - /// startProfilingBlock() method has been called. - static void stopProfilingBlock(); + /// Method called at the end of the scope where the + /// startProfilingBlock() method has been called. + static void stopProfilingBlock(); - /// Reset the timing data of the profiler (but not the profiler tree structure) - static void reset(); + /// Reset the timing data of the profiler (but not the profiler tree structure) + static void reset(); - /// Return the number of frames - static uint getNbFrames(); + /// Return the number of frames + static uint32_t getNbFrames(); - /// Return the elasped time since the start/reset of the profiling - static long double getElapsedTimeSinceStart(); + /// Return the elasped time since the start/reset of the profiling + static long double getElapsedTimeSinceStart(); - /// Increment the frame counter - static void incrementFrameCounter(); + /// Increment the frame counter + static void incrementFrameCounter(); - /// Return an iterator over the profiler tree starting at the root - static ProfileNodeIterator* getIterator(); + /// Return an iterator over the profiler tree starting at the root + static ProfileNodeIterator* getIterator(); - /// Print the report of the profiler in a given output stream - static void printReport(std::ostream& outputStream); + /// Print32_t the report of the profiler in a given output stream + static void print32_tReport(std::ostream& outputStream); - /// Destroy a previously allocated iterator - static void destroyIterator(ProfileNodeIterator* iterator); + /// Destroy a previously allocated iterator + static void destroyIterator(ProfileNodeIterator* iterator); - /// Destroy the profiler (release the memory) - static void destroy(); + /// Destroy the profiler (release the memory) + static void destroy(); }; // Class ProfileSample @@ -245,23 +224,23 @@ class Profiler { */ class ProfileSample { - public : + public : - // -------------------- Methods -------------------- // + // -------------------- Methods -------------------- // - /// Constructor - ProfileSample(const char* name) { + /// Constructor + ProfileSample(const char* name) { - // Ask the profiler to start profiling a block of code - Profiler::startProfilingBlock(name); - } + // Ask the profiler to start profiling a block of code + Profiler::startProfilingBlock(name); + } - /// Destructor - ~ProfileSample() { + /// Destructor + ~ProfileSample() { - // Tell the profiler to stop profiling a block of code - Profiler::stopProfilingBlock(); - } + // Tell the profiler to stop profiling a block of code + Profiler::stopProfilingBlock(); + } }; // Use this macro to start profile a block of code @@ -269,113 +248,113 @@ class ProfileSample { // Return true if we are at the root of the profiler tree inline bool ProfileNodeIterator::isRoot() { - return (mCurrentParentNode->getParentNode() == NULL); + return (mCurrentParentNode->getParentNode() == NULL); } // Return true if we are at the end of a branch of the profiler tree inline bool ProfileNodeIterator::isEnd() { - return (mCurrentChildNode == NULL); + return (mCurrentChildNode == NULL); } // Return the name of the current node inline const char* ProfileNodeIterator::getCurrentName() { - return mCurrentChildNode->getName(); + return mCurrentChildNode->getName(); } // Return the total time of the current node inline long double ProfileNodeIterator::getCurrentTotalTime() { - return mCurrentChildNode->getTotalTime(); + return mCurrentChildNode->getTotalTime(); } // Return the total number of calls of the current node -inline uint ProfileNodeIterator::getCurrentNbTotalCalls() { - return mCurrentChildNode->getNbTotalCalls(); +inline uint32_t ProfileNodeIterator::getCurrentNbTotalCalls() { + return mCurrentChildNode->getNbTotalCalls(); } // Return the name of the current parent node inline const char* ProfileNodeIterator::getCurrentParentName() { - return mCurrentParentNode->getName(); + return mCurrentParentNode->getName(); } // Return the total time of the current parent node inline long double ProfileNodeIterator::getCurrentParentTotalTime() { - return mCurrentParentNode->getTotalTime(); + return mCurrentParentNode->getTotalTime(); } // Return the total number of calls of the current parent node -inline uint ProfileNodeIterator::getCurrentParentNbTotalCalls() { - return mCurrentParentNode->getNbTotalCalls(); +inline uint32_t ProfileNodeIterator::getCurrentParentNbTotalCalls() { + return mCurrentParentNode->getNbTotalCalls(); } // Go to the first node inline void ProfileNodeIterator::first() { - mCurrentChildNode = mCurrentParentNode->getChildNode(); + mCurrentChildNode = mCurrentParentNode->getChildNode(); } // Go to the next node inline void ProfileNodeIterator::next() { - mCurrentChildNode = mCurrentChildNode->getSiblingNode(); + mCurrentChildNode = mCurrentChildNode->getSiblingNode(); } // Return a pointer to the parent node inline ProfileNode* ProfileNode::getParentNode() { - return mParentNode; + return mParentNode; } // Return a pointer to a sibling node inline ProfileNode* ProfileNode::getSiblingNode() { - return mSiblingNode; + return mSiblingNode; } // Return a pointer to a child node inline ProfileNode* ProfileNode::getChildNode() { - return mChildNode; + return mChildNode; } // Return the name of the node inline const char* ProfileNode::getName() { - return mName; + return mName; } // Return the total number of call of the corresponding block of code -inline uint ProfileNode::getNbTotalCalls() const { - return mNbTotalCalls; +inline uint32_t ProfileNode::getNbTotalCalls() const { + return mNbTotalCalls; } // Return the total time spent in the block of code inline long double ProfileNode::getTotalTime() const { - return mTotalTime; + return mTotalTime; } // Return the number of frames -inline uint Profiler::getNbFrames() { - return mFrameCounter; +inline uint32_t Profiler::getNbFrames() { + return mFrameCounter; } // Return the elasped time since the start/reset of the profiling inline long double Profiler::getElapsedTimeSinceStart() { - long double currentTime = Timer::getCurrentSystemTime() * 1000.0; - return currentTime - mProfilingStartTime; + long double currentTime = Timer::getCurrentSystemTime() * 1000.0; + return currentTime - mProfilingStartTime; } // Increment the frame counter inline void Profiler::incrementFrameCounter() { - mFrameCounter++; + mFrameCounter++; } // Return an iterator over the profiler tree starting at the root inline ProfileNodeIterator* Profiler::getIterator() { - return new ProfileNodeIterator(&mRootNode); + return new ProfileNodeIterator(&mRootNode); } // Destroy a previously allocated iterator inline void Profiler::destroyIterator(ProfileNodeIterator* iterator) { - delete iterator; + delete iterator; } // Destroy the profiler (release the memory) inline void Profiler::destroy() { - mRootNode.destroy(); + mRootNode.destroy(); } } @@ -386,5 +365,3 @@ inline void Profiler::destroy() { #define PROFILE(name) #endif - -#endif diff --git a/ephysics/engine/Timer.cpp b/ephysics/engine/Timer.cpp index 3b5bdc2..116f73b 100644 --- a/ephysics/engine/Timer.cpp +++ b/ephysics/engine/Timer.cpp @@ -1,27 +1,8 @@ -/******************************************************************************** -* 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. * -* * -********************************************************************************/ +/** @file + * @author Daniel Chappuis + * @copyright 2010-2016 Daniel Chappuis + * @license BSD 3 clauses (see license file) + */ // Libraries #include @@ -31,7 +12,7 @@ using namespace reactphysics3d; // Constructor Timer::Timer(double timeStep) : mTimeStep(timeStep), mIsRunning(false) { - assert(timeStep > 0.0); + assert(timeStep > 0.0); } // Destructor @@ -42,18 +23,18 @@ Timer::~Timer() { // Return the current time of the system in seconds long double Timer::getCurrentSystemTime() { - #if defined(WINDOWS_OS) - LARGE_INTEGER ticksPerSecond; - LARGE_INTEGER ticks; - QueryPerformanceFrequency(&ticksPerSecond); - QueryPerformanceCounter(&ticks); - return (long double(ticks.QuadPart) / long double(ticksPerSecond.QuadPart)); - #else - // Initialize the lastUpdateTime with the current time in seconds - timeval timeValue; - gettimeofday(&timeValue, NULL); - return (timeValue.tv_sec + (timeValue.tv_usec / 1000000.0)); - #endif + #if defined(__TARGET_OS__Windows) + LARGE_INTEGER ticksPerSecond; + LARGE_INTEGER ticks; + QueryPerformanceFrequency(&ticksPerSecond); + QueryPerformanceCounter(&ticks); + return (long double(ticks.QuadPart) / long double(ticksPerSecond.QuadPart)); + #else + // Initialize the lastUpdateTime with the current time in seconds + timeval timeValue; + gettimeofday(&timeValue, NULL); + return (timeValue.tv_sec + (timeValue.tv_usec / 1000000.0)); + #endif } diff --git a/ephysics/engine/Timer.h b/ephysics/engine/Timer.h index 89a7211..4d07c6a 100644 --- a/ephysics/engine/Timer.h +++ b/ephysics/engine/Timer.h @@ -1,30 +1,9 @@ -/******************************************************************************** -* 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 REACTPHYSICS3D_TIMER_H -#define REACTPHYSICS3D_TIMER_H +/** @file + * @author Daniel Chappuis + * @copyright 2010-2016 Daniel Chappuis + * @license BSD 3 clauses (see license file) + */ +#pragma once // Libraries #include @@ -33,10 +12,10 @@ #include #include -#if defined(WINDOWS_OS) // For Windows platform - #define NOMINMAX // This is used to avoid definition of max() and min() macros +#if defined(__TARGET_OS__Windows) + #define NOMINMAX // This is used to avoid definition of max() and min() macros #include -#else // For Mac OS or Linux platform +#else // For Mac OS or Linux platform #include #endif @@ -52,149 +31,147 @@ namespace reactphysics3d { */ class Timer { - private : + private : - // -------------------- Attributes -------------------- // + // -------------------- Attributes -------------------- // - /// Timestep dt of the physics engine (timestep > 0.0) - double mTimeStep; + /// Timestep dt of the physics engine (timestep > 0.0) + double mTimeStep; - /// Last time the timer has been updated - long double mLastUpdateTime; + /// Last time the timer has been updated + long double mLastUpdateTime; - /// Time difference between the two last timer update() calls - long double mDeltaTime; + /// Time difference between the two last timer update() calls + long double mDeltaTime; - /// Used to fix the time step and avoid strange time effects - double mAccumulator; + /// Used to fix the time step and avoid strange time effects + double mAccumulator; - /// True if the timer is running - bool mIsRunning; + /// True if the timer is running + bool mIsRunning; - // -------------------- Methods -------------------- // + // -------------------- Methods -------------------- // - /// Private copy-constructor - Timer(const Timer& timer); + /// Private copy-constructor + Timer(const Timer& timer); - /// Private assignment operator - Timer& operator=(const Timer& timer); + /// Private assignment operator + Timer& operator=(const Timer& timer); - public : + public : - // -------------------- Methods -------------------- // + // -------------------- Methods -------------------- // - /// Constructor - Timer(double timeStep); + /// Constructor + Timer(double timeStep); - /// Destructor - virtual ~Timer(); + /// Destructor + virtual ~Timer(); - /// Return the timestep of the physics engine - double getTimeStep() const; + /// Return the timestep of the physics engine + double getTimeStep() const; - /// Set the timestep of the physics engine - void setTimeStep(double timeStep); + /// Set the timestep of the physics engine + void setTimeStep(double timeStep); - /// Return the current time of the physics engine - long double getPhysicsTime() const; + /// Return the current time of the physics engine + long double getPhysicsTime() const; - /// Start the timer - void start(); + /// Start the timer + void start(); - /// Stop the timer - void stop(); + /// Stop the timer + void stop(); - /// Return true if the timer is running - bool getIsRunning() const; + /// Return true if the timer is running + bool getIsRunning() const; - /// True if it's possible to take a new step - bool isPossibleToTakeStep() const; + /// True if it's possible to take a new step + bool isPossibleToTakeStep() const; - /// Compute the time since the last update() call and add it to the accumulator - void update(); + /// Compute the time since the last update() call and add it to the accumulator + void update(); - /// Take a new step => update the timer by adding the timeStep value to the current time - void nextStep(); + /// Take a new step => update the timer by adding the timeStep value to the current time + void nextStep(); - /// Compute the interpolation factor - decimal computeInterpolationFactor(); + /// Compute the int32_terpolation factor + float computeInterpolationFactor(); - /// Return the current time of the system in seconds - static long double getCurrentSystemTime(); + /// Return the current time of the system in seconds + static long double getCurrentSystemTime(); }; // Return the timestep of the physics engine inline double Timer::getTimeStep() const { - return mTimeStep; + return mTimeStep; } // Set the timestep of the physics engine inline void Timer::setTimeStep(double timeStep) { - assert(timeStep > 0.0f); - mTimeStep = timeStep; + assert(timeStep > 0.0f); + mTimeStep = timeStep; } // Return the current time inline long double Timer::getPhysicsTime() const { - return mLastUpdateTime; + return mLastUpdateTime; } // Return if the timer is running inline bool Timer::getIsRunning() const { - return mIsRunning; + return mIsRunning; } // Start the timer inline void Timer::start() { - if (!mIsRunning) { + if (!mIsRunning) { - // Get the current system time - mLastUpdateTime = getCurrentSystemTime(); + // Get the current system time + mLastUpdateTime = getCurrentSystemTime(); - mAccumulator = 0.0; - mIsRunning = true; - } + mAccumulator = 0.0; + mIsRunning = true; + } } // Stop the timer inline void Timer::stop() { - mIsRunning = false; + mIsRunning = false; } // True if it's possible to take a new step inline bool Timer::isPossibleToTakeStep() const { - return (mAccumulator >= mTimeStep); + return (mAccumulator >= mTimeStep); } // Take a new step => update the timer by adding the timeStep value to the current time inline void Timer::nextStep() { - assert(mIsRunning); + assert(mIsRunning); - // Update the accumulator value - mAccumulator -= mTimeStep; + // Update the accumulator value + mAccumulator -= mTimeStep; } -// Compute the interpolation factor -inline decimal Timer::computeInterpolationFactor() { - return (decimal(mAccumulator / mTimeStep)); +// Compute the int32_terpolation factor +inline float Timer::computeInterpolationFactor() { + return (float(mAccumulator / mTimeStep)); } // Compute the time since the last update() call and add it to the accumulator inline void Timer::update() { - // Get the current system time - long double currentTime = getCurrentSystemTime(); + // Get the current system time + long double currentTime = getCurrentSystemTime(); - // Compute the delta display time between two display frames - mDeltaTime = currentTime - mLastUpdateTime; + // Compute the delta display time between two display frames + mDeltaTime = currentTime - mLastUpdateTime; - // Update the current display time - mLastUpdateTime = currentTime; + // Update the current display time + mLastUpdateTime = currentTime; - // Update the accumulator value - mAccumulator += mDeltaTime; + // Update the accumulator value + mAccumulator += mDeltaTime; } } - - #endif diff --git a/ephysics/mathematics/Matrix2x2.cpp b/ephysics/mathematics/Matrix2x2.cpp index 15de4dc..f760530 100644 --- a/ephysics/mathematics/Matrix2x2.cpp +++ b/ephysics/mathematics/Matrix2x2.cpp @@ -1,27 +1,8 @@ -/******************************************************************************** -* 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. * -* * -********************************************************************************/ +/** @file + * @author Daniel Chappuis + * @copyright 2010-2016 Daniel Chappuis + * @license BSD 3 clauses (see license file) + */ // Libraries #include @@ -31,20 +12,20 @@ using namespace reactphysics3d; // Constructor of the class Matrix2x2 Matrix2x2::Matrix2x2() { - // Initialize all values in the matrix to zero - setAllValues(0.0, 0.0, 0.0, 0.0); + // Initialize all values in the matrix to zero + setAllValues(0.0, 0.0, 0.0, 0.0); } // Constructor -Matrix2x2::Matrix2x2(decimal value) { - setAllValues(value, value, value, value); +Matrix2x2::Matrix2x2(float value) { + setAllValues(value, value, value, value); } // Constructor with arguments -Matrix2x2::Matrix2x2(decimal a1, decimal a2, decimal b1, decimal b2) { +Matrix2x2::Matrix2x2(float a1, float a2, float b1, float b2) { - // Initialize the matrix with the values - setAllValues(a1, a2, b1, b2); + // Initialize the matrix with the values + setAllValues(a1, a2, b1, b2); } // Destructor @@ -54,34 +35,34 @@ Matrix2x2::~Matrix2x2() { // Copy-constructor Matrix2x2::Matrix2x2(const Matrix2x2& matrix) { - setAllValues(matrix.mRows[0][0], matrix.mRows[0][1], - matrix.mRows[1][0], matrix.mRows[1][1]); + setAllValues(matrix.mRows[0][0], matrix.mRows[0][1], + matrix.mRows[1][0], matrix.mRows[1][1]); } // Assignment operator Matrix2x2& Matrix2x2::operator=(const Matrix2x2& matrix) { - // Check for self-assignment - if (&matrix != this) { - setAllValues(matrix.mRows[0][0], matrix.mRows[0][1], - matrix.mRows[1][0], matrix.mRows[1][1]); - } - return *this; + // Check for self-assignment + if (&matrix != this) { + setAllValues(matrix.mRows[0][0], matrix.mRows[0][1], + matrix.mRows[1][0], matrix.mRows[1][1]); + } + return *this; } // Return the inverse matrix Matrix2x2 Matrix2x2::getInverse() const { - // Compute the determinant of the matrix - decimal determinant = getDeterminant(); + // Compute the determinant of the matrix + float determinant = getDeterminant(); - // Check if the determinant is equal to zero - assert(std::abs(determinant) > MACHINE_EPSILON); + // Check if the determinant is equal to zero + assert(std::abs(determinant) > MACHINE_EPSILON); - decimal invDeterminant = decimal(1.0) / determinant; + float invDeterminant = float(1.0) / determinant; - Matrix2x2 tempMatrix(mRows[1][1], -mRows[0][1], -mRows[1][0], mRows[0][0]); + Matrix2x2 tempMatrix(mRows[1][1], -mRows[0][1], -mRows[1][0], mRows[0][0]); - // Return the inverse matrix - return (invDeterminant * tempMatrix); + // Return the inverse matrix + return (invDeterminant * tempMatrix); } diff --git a/ephysics/mathematics/Matrix2x2.h b/ephysics/mathematics/Matrix2x2.h index 866fe06..5570916 100644 --- a/ephysics/mathematics/Matrix2x2.h +++ b/ephysics/mathematics/Matrix2x2.h @@ -1,30 +1,9 @@ -/******************************************************************************** -* 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 REACTPHYSICS3D_MATRIX2X2_H -#define REACTPHYSICS3D_MATRIX2X2_H +/** @file + * @author Daniel Chappuis + * @copyright 2010-2016 Daniel Chappuis + * @license BSD 3 clauses (see license file) + */ +#pragma once // Libraries #include @@ -39,245 +18,245 @@ namespace reactphysics3d { */ class Matrix2x2 { - private : + private : - // -------------------- Attributes -------------------- // + // -------------------- Attributes -------------------- // - /// Rows of the matrix; - Vector2 mRows[2]; + /// Rows of the matrix; + Vector2 mRows[2]; - public : + public : - // -------------------- Methods -------------------- // + // -------------------- Methods -------------------- // - /// Constructor - Matrix2x2(); + /// Constructor + Matrix2x2(); - /// Constructor - Matrix2x2(decimal value); + /// Constructor + Matrix2x2(float value); - /// Constructor - Matrix2x2(decimal a1, decimal a2, decimal b1, decimal b2); + /// Constructor + Matrix2x2(float a1, float a2, float b1, float b2); - /// Destructor - ~Matrix2x2(); + /// Destructor + ~Matrix2x2(); - /// Copy-constructor - Matrix2x2(const Matrix2x2& matrix); + /// Copy-constructor + Matrix2x2(const Matrix2x2& matrix); - /// Assignment operator - Matrix2x2& operator=(const Matrix2x2& matrix); + /// Assignment operator + Matrix2x2& operator=(const Matrix2x2& matrix); - /// Set all the values in the matrix - void setAllValues(decimal a1, decimal a2, decimal b1, decimal b2); + /// Set all the values in the matrix + void setAllValues(float a1, float a2, float b1, float b2); - /// Set the matrix to zero - void setToZero(); + /// Set the matrix to zero + void setToZero(); - /// Return a column - Vector2 getColumn(int i) const; + /// Return a column + Vector2 getColumn(int32_t i) const; - /// Return a row - Vector2 getRow(int i) const; + /// Return a row + Vector2 getRow(int32_t i) const; - /// Return the transpose matrix - Matrix2x2 getTranspose() const; + /// Return the transpose matrix + Matrix2x2 getTranspose() const; - /// Return the determinant of the matrix - decimal getDeterminant() const; + /// Return the determinant of the matrix + float getDeterminant() const; - /// Return the trace of the matrix - decimal getTrace() const; + /// Return the trace of the matrix + float getTrace() const; - /// Return the inverse matrix - Matrix2x2 getInverse() const; + /// Return the inverse matrix + Matrix2x2 getInverse() const; - /// Return the matrix with absolute values - Matrix2x2 getAbsoluteMatrix() const; + /// Return the matrix with absolute values + Matrix2x2 getAbsoluteMatrix() const; - /// Set the matrix to the identity matrix - void setToIdentity(); + /// Set the matrix to the identity matrix + void setToIdentity(); - /// Return the 2x2 identity matrix - static Matrix2x2 identity(); + /// Return the 2x2 identity matrix + static Matrix2x2 identity(); - /// Return the 2x2 zero matrix - static Matrix2x2 zero(); + /// Return the 2x2 zero matrix + static Matrix2x2 zero(); - /// Overloaded operator for addition - friend Matrix2x2 operator+(const Matrix2x2& matrix1, const Matrix2x2& matrix2); + /// Overloaded operator for addition + friend Matrix2x2 operator+(const Matrix2x2& matrix1, const Matrix2x2& matrix2); - /// Overloaded operator for substraction - friend Matrix2x2 operator-(const Matrix2x2& matrix1, const Matrix2x2& matrix2); + /// Overloaded operator for substraction + friend Matrix2x2 operator-(const Matrix2x2& matrix1, const Matrix2x2& matrix2); - /// Overloaded operator for the negative of the matrix - friend Matrix2x2 operator-(const Matrix2x2& matrix); + /// Overloaded operator for the negative of the matrix + friend Matrix2x2 operator-(const Matrix2x2& matrix); - /// Overloaded operator for multiplication with a number - friend Matrix2x2 operator*(decimal nb, const Matrix2x2& matrix); + /// Overloaded operator for multiplication with a number + friend Matrix2x2 operator*(float nb, const Matrix2x2& matrix); - /// Overloaded operator for multiplication with a matrix - friend Matrix2x2 operator*(const Matrix2x2& matrix, decimal nb); + /// Overloaded operator for multiplication with a matrix + friend Matrix2x2 operator*(const Matrix2x2& matrix, float nb); - /// Overloaded operator for matrix multiplication - friend Matrix2x2 operator*(const Matrix2x2& matrix1, const Matrix2x2& matrix2); + /// Overloaded operator for matrix multiplication + friend Matrix2x2 operator*(const Matrix2x2& matrix1, const Matrix2x2& matrix2); - /// Overloaded operator for multiplication with a vector - friend Vector2 operator*(const Matrix2x2& matrix, const Vector2& vector); + /// Overloaded operator for multiplication with a vector + friend Vector2 operator*(const Matrix2x2& matrix, const Vector2& vector); - /// Overloaded operator for equality condition - bool operator==(const Matrix2x2& matrix) const; + /// Overloaded operator for equality condition + bool operator==(const Matrix2x2& matrix) const; - /// Overloaded operator for the is different condition - bool operator!= (const Matrix2x2& matrix) const; + /// Overloaded operator for the is different condition + bool operator!= (const Matrix2x2& matrix) const; - /// Overloaded operator for addition with assignment - Matrix2x2& operator+=(const Matrix2x2& matrix); + /// Overloaded operator for addition with assignment + Matrix2x2& operator+=(const Matrix2x2& matrix); - /// Overloaded operator for substraction with assignment - Matrix2x2& operator-=(const Matrix2x2& matrix); + /// Overloaded operator for substraction with assignment + Matrix2x2& operator-=(const Matrix2x2& matrix); - /// Overloaded operator for multiplication with a number with assignment - Matrix2x2& operator*=(decimal nb); + /// Overloaded operator for multiplication with a number with assignment + Matrix2x2& operator*=(float nb); - /// Overloaded operator to read element of the matrix. - const Vector2& operator[](int row) const; + /// Overloaded operator to read element of the matrix. + const Vector2& operator[](int32_t row) const; - /// Overloaded operator to read/write element of the matrix. - Vector2& operator[](int row); + /// Overloaded operator to read/write element of the matrix. + Vector2& operator[](int32_t row); }; // Method to set all the values in the matrix -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; +inline void Matrix2x2::setAllValues(float a1, float a2, + float b1, float 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() { - mRows[0].setToZero(); - mRows[1].setToZero(); + mRows[0].setToZero(); + mRows[1].setToZero(); } // Return a column -inline Vector2 Matrix2x2::getColumn(int i) const { - assert(i>= 0 && i<2); - return Vector2(mRows[0][i], mRows[1][i]); +inline Vector2 Matrix2x2::getColumn(int32_t 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 { - assert(i>= 0 && i<2); - return mRows[i]; +inline Vector2 Matrix2x2::getRow(int32_t i) const { + assert(i>= 0 && i<2); + return mRows[i]; } // Return the transpose matrix inline Matrix2x2 Matrix2x2::getTranspose() const { - // Return the transpose matrix - return Matrix2x2(mRows[0][0], mRows[1][0], - mRows[0][1], mRows[1][1]); + // Return the transpose matrix + return Matrix2x2(mRows[0][0], mRows[1][0], + mRows[0][1], mRows[1][1]); } // Return the determinant of the matrix -inline decimal Matrix2x2::getDeterminant() const { +inline float Matrix2x2::getDeterminant() const { - // Compute and return the determinant of the matrix - return mRows[0][0] * mRows[1][1] - mRows[1][0] * mRows[0][1]; + // 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 { +inline float Matrix2x2::getTrace() const { - // Compute and return the trace - return (mRows[0][0] + mRows[1][1]); + // Compute and return the trace + return (mRows[0][0] + mRows[1][1]); } // Set the matrix to the identity matrix inline void Matrix2x2::setToIdentity() { - mRows[0][0] = 1.0; mRows[0][1] = 0.0; - mRows[1][0] = 0.0; mRows[1][1] = 1.0; + 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() { - // Return the isdentity matrix - return Matrix2x2(1.0, 0.0, 0.0, 1.0); + // Return the isdentity matrix + return Matrix2x2(1.0, 0.0, 0.0, 1.0); } // Return the 2x2 zero matrix inline Matrix2x2 Matrix2x2::zero() { - return Matrix2x2(0.0, 0.0, 0.0, 0.0); + 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])); + return Matrix2x2(fabs(mRows[0][0]), fabs(mRows[0][1]), + fabs(mRows[1][0]), fabs(mRows[1][1])); } // Overloaded operator for addition 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], - matrix1.mRows[1][1] + matrix2.mRows[1][1]); + 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], + matrix1.mRows[1][1] + matrix2.mRows[1][1]); } // Overloaded operator for substraction 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], - matrix1.mRows[1][1] - matrix2.mRows[1][1]); + 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], + matrix1.mRows[1][1] - matrix2.mRows[1][1]); } // Overloaded operator for the negative of the matrix inline Matrix2x2 operator-(const Matrix2x2& matrix) { - return Matrix2x2(-matrix.mRows[0][0], -matrix.mRows[0][1], - -matrix.mRows[1][0], -matrix.mRows[1][1]); + 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) { - return Matrix2x2(matrix.mRows[0][0] * nb, matrix.mRows[0][1] * nb, - matrix.mRows[1][0] * nb, matrix.mRows[1][1] * nb); +inline Matrix2x2 operator*(float 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) { - return nb * matrix; +inline Matrix2x2 operator*(const Matrix2x2& matrix, float nb) { + return nb * matrix; } // Overloaded operator for matrix multiplication 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] * - matrix2.mRows[1][1], - matrix1.mRows[1][0] * matrix2.mRows[0][0] + matrix1.mRows[1][1] * - matrix2.mRows[1][0], - matrix1.mRows[1][0] * matrix2.mRows[0][1] + matrix1.mRows[1][1] * - matrix2.mRows[1][1]); + 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] * + matrix2.mRows[1][1], + matrix1.mRows[1][0] * matrix2.mRows[0][0] + matrix1.mRows[1][1] * + matrix2.mRows[1][0], + matrix1.mRows[1][0] * matrix2.mRows[0][1] + matrix1.mRows[1][1] * + matrix2.mRows[1][1]); } // Overloaded operator for multiplication with a vector 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); + 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 { - 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]); + 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 { - return !(*this == matrix); + return !(*this == matrix); } // Overloaded operator for addition with assignment @@ -295,7 +274,7 @@ inline Matrix2x2& Matrix2x2::operator-=(const Matrix2x2& matrix) { } // Overloaded operator for multiplication with a number with assignment -inline Matrix2x2& Matrix2x2::operator*=(decimal nb) { +inline Matrix2x2& Matrix2x2::operator*=(float nb) { mRows[0][0] *= nb; mRows[0][1] *= nb; mRows[1][0] *= nb; mRows[1][1] *= nb; return *this; @@ -304,17 +283,15 @@ 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 { - return mRows[row]; +inline const Vector2& Matrix2x2::operator[](int32_t 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) { - return mRows[row]; +inline Vector2& Matrix2x2::operator[](int32_t row) { + return mRows[row]; } } - -#endif diff --git a/ephysics/mathematics/Matrix3x3.cpp b/ephysics/mathematics/Matrix3x3.cpp index 4142aea..dc6a0f6 100644 --- a/ephysics/mathematics/Matrix3x3.cpp +++ b/ephysics/mathematics/Matrix3x3.cpp @@ -1,27 +1,8 @@ -/******************************************************************************** -* 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. * -* * -********************************************************************************/ +/** @file + * @author Daniel Chappuis + * @copyright 2010-2016 Daniel Chappuis + * @license BSD 3 clauses (see license file) + */ // Libraries #include @@ -32,21 +13,21 @@ using namespace reactphysics3d; // Constructor of the class Matrix3x3 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); + // 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 -Matrix3x3::Matrix3x3(decimal value) { - setAllValues(value, value, value, value, value, value, value, value, value); +Matrix3x3::Matrix3x3(float value) { + setAllValues(value, value, value, value, value, value, value, value, value); } // Constructor with arguments -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 - setAllValues(a1, a2, a3, b1, b2, b3, c1, c2, c3); +Matrix3x3::Matrix3x3(float a1, float a2, float a3, + float b1, float b2, float b3, + float c1, float c2, float c3) { + // Initialize the matrix with the values + setAllValues(a1, a2, a3, b1, b2, b3, c1, c2, c3); } // Destructor @@ -56,46 +37,46 @@ Matrix3x3::~Matrix3x3() { // Copy-constructor 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]); + 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]); } // Assignment operator Matrix3x3& Matrix3x3::operator=(const Matrix3x3& matrix) { - // Check for self-assignment - if (&matrix != this) { - 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]); - } - return *this; + // Check for self-assignment + if (&matrix != this) { + 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]); + } + return *this; } // Return the inverse matrix Matrix3x3 Matrix3x3::getInverse() const { - // Compute the determinant of the matrix - decimal determinant = getDeterminant(); + // Compute the determinant of the matrix + float determinant = getDeterminant(); - // Check if the determinant is equal to zero - assert(std::abs(determinant) > MACHINE_EPSILON); + // Check if the determinant is equal to zero + assert(std::abs(determinant) > MACHINE_EPSILON); - decimal invDeterminant = decimal(1.0) / determinant; + float invDeterminant = float(1.0) / determinant; - Matrix3x3 tempMatrix((mRows[1][1]*mRows[2][2]-mRows[2][1]*mRows[1][2]), - -(mRows[0][1]*mRows[2][2]-mRows[2][1]*mRows[0][2]), - (mRows[0][1]*mRows[1][2]-mRows[0][2]*mRows[1][1]), - -(mRows[1][0]*mRows[2][2]-mRows[2][0]*mRows[1][2]), - (mRows[0][0]*mRows[2][2]-mRows[2][0]*mRows[0][2]), - -(mRows[0][0]*mRows[1][2]-mRows[1][0]*mRows[0][2]), - (mRows[1][0]*mRows[2][1]-mRows[2][0]*mRows[1][1]), - -(mRows[0][0]*mRows[2][1]-mRows[2][0]*mRows[0][1]), - (mRows[0][0]*mRows[1][1]-mRows[0][1]*mRows[1][0])); + Matrix3x3 tempMatrix((mRows[1][1]*mRows[2][2]-mRows[2][1]*mRows[1][2]), + -(mRows[0][1]*mRows[2][2]-mRows[2][1]*mRows[0][2]), + (mRows[0][1]*mRows[1][2]-mRows[0][2]*mRows[1][1]), + -(mRows[1][0]*mRows[2][2]-mRows[2][0]*mRows[1][2]), + (mRows[0][0]*mRows[2][2]-mRows[2][0]*mRows[0][2]), + -(mRows[0][0]*mRows[1][2]-mRows[1][0]*mRows[0][2]), + (mRows[1][0]*mRows[2][1]-mRows[2][0]*mRows[1][1]), + -(mRows[0][0]*mRows[2][1]-mRows[2][0]*mRows[0][1]), + (mRows[0][0]*mRows[1][1]-mRows[0][1]*mRows[1][0])); - // Return the inverse matrix - return (invDeterminant * tempMatrix); + // Return the inverse matrix + return (invDeterminant * tempMatrix); } diff --git a/ephysics/mathematics/Matrix3x3.h b/ephysics/mathematics/Matrix3x3.h index 393a46b..f3c7c74 100644 --- a/ephysics/mathematics/Matrix3x3.h +++ b/ephysics/mathematics/Matrix3x3.h @@ -1,31 +1,9 @@ -/******************************************************************************** -* 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 REACTPHYSICS3D_MATRIX3X3_H -#define REACTPHYSICS3D_MATRIX3X3_H +/** @file + * @author Daniel Chappuis + * @copyright 2010-2016 Daniel Chappuis + * @license BSD 3 clauses (see license file) + */ +#pragma once // Libraries #include @@ -41,287 +19,287 @@ namespace reactphysics3d { */ class Matrix3x3 { - private : + private : - // -------------------- Attributes -------------------- // + // -------------------- Attributes -------------------- // - /// Rows of the matrix; - Vector3 mRows[3]; + /// Rows of the matrix; + Vector3 mRows[3]; - public : + public : - // -------------------- Methods -------------------- // + // -------------------- Methods -------------------- // - /// Constructor - Matrix3x3(); + /// Constructor + Matrix3x3(); - /// Constructor - Matrix3x3(decimal value); + /// Constructor + Matrix3x3(float value); - /// Constructor - Matrix3x3(decimal a1, decimal a2, decimal a3, decimal b1, decimal b2, decimal b3, - decimal c1, decimal c2, decimal c3); + /// Constructor + Matrix3x3(float a1, float a2, float a3, float b1, float b2, float b3, + float c1, float c2, float c3); - /// Destructor - virtual ~Matrix3x3(); + /// Destructor + virtual ~Matrix3x3(); - /// Copy-constructor - Matrix3x3(const Matrix3x3& matrix); + /// Copy-constructor + Matrix3x3(const Matrix3x3& matrix); - /// Assignment operator - Matrix3x3& operator=(const Matrix3x3& matrix); + /// Assignment operator + Matrix3x3& operator=(const Matrix3x3& matrix); - /// Set all the values in the matrix - void setAllValues(decimal a1, decimal a2, decimal a3, decimal b1, decimal b2, decimal b3, - decimal c1, decimal c2, decimal c3); + /// Set all the values in the matrix + void setAllValues(float a1, float a2, float a3, float b1, float b2, float b3, + float c1, float c2, float c3); - /// Set the matrix to zero - void setToZero(); + /// Set the matrix to zero + void setToZero(); - /// Return a column - Vector3 getColumn(int i) const; + /// Return a column + Vector3 getColumn(int32_t i) const; - /// Return a row - Vector3 getRow(int i) const; + /// Return a row + Vector3 getRow(int32_t i) const; - /// Return the transpose matrix - Matrix3x3 getTranspose() const; + /// Return the transpose matrix + Matrix3x3 getTranspose() const; - /// Return the determinant of the matrix - decimal getDeterminant() const; + /// Return the determinant of the matrix + float getDeterminant() const; - /// Return the trace of the matrix - decimal getTrace() const; + /// Return the trace of the matrix + float getTrace() const; - /// Return the inverse matrix - Matrix3x3 getInverse() const; + /// Return the inverse matrix + Matrix3x3 getInverse() const; - /// Return the matrix with absolute values - Matrix3x3 getAbsoluteMatrix() const; + /// Return the matrix with absolute values + Matrix3x3 getAbsoluteMatrix() const; - /// Set the matrix to the identity matrix - void setToIdentity(); + /// Set the matrix to the identity matrix + void setToIdentity(); - /// Return the 3x3 identity matrix - static Matrix3x3 identity(); + /// Return the 3x3 identity matrix + static Matrix3x3 identity(); - /// Return the 3x3 zero matrix - static Matrix3x3 zero(); + /// Return the 3x3 zero matrix + static Matrix3x3 zero(); - /// Return a skew-symmetric matrix using a given vector that can be used - /// to compute cross product with another vector using matrix multiplication - static Matrix3x3 computeSkewSymmetricMatrixForCrossProduct(const Vector3& vector); + /// Return a skew-symmetric matrix using a given vector that can be used + /// to compute cross product with another vector using matrix multiplication + static Matrix3x3 computeSkewSymmetricMatrixForCrossProduct(const Vector3& vector); - /// Overloaded operator for addition - friend Matrix3x3 operator+(const Matrix3x3& matrix1, const Matrix3x3& matrix2); + /// Overloaded operator for addition + friend Matrix3x3 operator+(const Matrix3x3& matrix1, const Matrix3x3& matrix2); - /// Overloaded operator for substraction - friend Matrix3x3 operator-(const Matrix3x3& matrix1, const Matrix3x3& matrix2); + /// Overloaded operator for substraction + friend Matrix3x3 operator-(const Matrix3x3& matrix1, const Matrix3x3& matrix2); - /// Overloaded operator for the negative of the matrix - friend Matrix3x3 operator-(const Matrix3x3& matrix); + /// Overloaded operator for the negative of the matrix + friend Matrix3x3 operator-(const Matrix3x3& matrix); - /// Overloaded operator for multiplication with a number - friend Matrix3x3 operator*(decimal nb, const Matrix3x3& matrix); + /// Overloaded operator for multiplication with a number + friend Matrix3x3 operator*(float nb, const Matrix3x3& matrix); - /// Overloaded operator for multiplication with a matrix - friend Matrix3x3 operator*(const Matrix3x3& matrix, decimal nb); + /// Overloaded operator for multiplication with a matrix + friend Matrix3x3 operator*(const Matrix3x3& matrix, float nb); - /// Overloaded operator for matrix multiplication - friend Matrix3x3 operator*(const Matrix3x3& matrix1, const Matrix3x3& matrix2); + /// Overloaded operator for matrix multiplication + friend Matrix3x3 operator*(const Matrix3x3& matrix1, const Matrix3x3& matrix2); - /// Overloaded operator for multiplication with a vector - friend Vector3 operator*(const Matrix3x3& matrix, const Vector3& vector); + /// Overloaded operator for multiplication with a vector + friend Vector3 operator*(const Matrix3x3& matrix, const Vector3& vector); - /// Overloaded operator for equality condition - bool operator==(const Matrix3x3& matrix) const; + /// Overloaded operator for equality condition + bool operator==(const Matrix3x3& matrix) const; - /// Overloaded operator for the is different condition - bool operator!= (const Matrix3x3& matrix) const; + /// Overloaded operator for the is different condition + bool operator!= (const Matrix3x3& matrix) const; - /// Overloaded operator for addition with assignment - Matrix3x3& operator+=(const Matrix3x3& matrix); + /// Overloaded operator for addition with assignment + Matrix3x3& operator+=(const Matrix3x3& matrix); - /// Overloaded operator for substraction with assignment - Matrix3x3& operator-=(const Matrix3x3& matrix); + /// Overloaded operator for substraction with assignment + Matrix3x3& operator-=(const Matrix3x3& matrix); - /// Overloaded operator for multiplication with a number with assignment - Matrix3x3& operator*=(decimal nb); + /// Overloaded operator for multiplication with a number with assignment + Matrix3x3& operator*=(float nb); - /// Overloaded operator to read element of the matrix. - const Vector3& operator[](int row) const; + /// Overloaded operator to read element of the matrix. + const Vector3& operator[](int32_t row) const; - /// Overloaded operator to read/write element of the matrix. - Vector3& operator[](int row); + /// Overloaded operator to read/write element of the matrix. + Vector3& operator[](int32_t row); }; // Method to set all the values in the matrix -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; - mRows[1][0] = b1; mRows[1][1] = b2; mRows[1][2] = b3; - mRows[2][0] = c1; mRows[2][1] = c2; mRows[2][2] = c3; +inline void Matrix3x3::setAllValues(float a1, float a2, float a3, + float b1, float b2, float b3, + float c1, float c2, float c3) { + mRows[0][0] = a1; mRows[0][1] = a2; mRows[0][2] = a3; + mRows[1][0] = b1; mRows[1][1] = b2; mRows[1][2] = b3; + mRows[2][0] = c1; mRows[2][1] = c2; mRows[2][2] = c3; } // Set the matrix to zero inline void Matrix3x3::setToZero() { - mRows[0].setToZero(); - mRows[1].setToZero(); - mRows[2].setToZero(); + mRows[0].setToZero(); + mRows[1].setToZero(); + mRows[2].setToZero(); } // Return a column -inline Vector3 Matrix3x3::getColumn(int i) const { - assert(i>= 0 && i<3); - return Vector3(mRows[0][i], mRows[1][i], mRows[2][i]); +inline Vector3 Matrix3x3::getColumn(int32_t 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 { - assert(i>= 0 && i<3); - return mRows[i]; +inline Vector3 Matrix3x3::getRow(int32_t i) const { + assert(i>= 0 && i<3); + return mRows[i]; } // Return the transpose matrix inline Matrix3x3 Matrix3x3::getTranspose() const { - // Return the transpose matrix - return Matrix3x3(mRows[0][0], mRows[1][0], mRows[2][0], - mRows[0][1], mRows[1][1], mRows[2][1], - mRows[0][2], mRows[1][2], mRows[2][2]); + // Return the transpose matrix + return Matrix3x3(mRows[0][0], mRows[1][0], mRows[2][0], + mRows[0][1], mRows[1][1], mRows[2][1], + mRows[0][2], mRows[1][2], mRows[2][2]); } // Return the determinant of the matrix -inline decimal Matrix3x3::getDeterminant() const { +inline float 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]) - - mRows[0][1]*(mRows[1][0]*mRows[2][2]-mRows[2][0]*mRows[1][2]) + - mRows[0][2]*(mRows[1][0]*mRows[2][1]-mRows[2][0]*mRows[1][1])); + // Compute and return the determinant of the matrix + return (mRows[0][0]*(mRows[1][1]*mRows[2][2]-mRows[2][1]*mRows[1][2]) - + mRows[0][1]*(mRows[1][0]*mRows[2][2]-mRows[2][0]*mRows[1][2]) + + mRows[0][2]*(mRows[1][0]*mRows[2][1]-mRows[2][0]*mRows[1][1])); } // Return the trace of the matrix -inline decimal Matrix3x3::getTrace() const { +inline float Matrix3x3::getTrace() const { - // Compute and return the trace - return (mRows[0][0] + mRows[1][1] + mRows[2][2]); + // 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() { - 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; + 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() { - return Matrix3x3(1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0); + 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() { - return Matrix3x3(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0); + 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) { - return Matrix3x3(0, -vector.z, vector.y, vector.z, 0, -vector.x, -vector.y, vector.x, 0); + 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(fabs(mRows[0][0]), fabs(mRows[0][1]), fabs(mRows[0][2]), - fabs(mRows[1][0]), fabs(mRows[1][1]), fabs(mRows[1][2]), - fabs(mRows[2][0]), fabs(mRows[2][1]), fabs(mRows[2][2])); + return Matrix3x3(fabs(mRows[0][0]), fabs(mRows[0][1]), fabs(mRows[0][2]), + fabs(mRows[1][0]), fabs(mRows[1][1]), fabs(mRows[1][2]), + fabs(mRows[2][0]), fabs(mRows[2][1]), fabs(mRows[2][2])); } // Overloaded operator for addition 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] + - matrix2.mRows[1][1], matrix1.mRows[1][2] + matrix2.mRows[1][2], - matrix1.mRows[2][0] + matrix2.mRows[2][0], matrix1.mRows[2][1] + - matrix2.mRows[2][1], matrix1.mRows[2][2] + matrix2.mRows[2][2]); + 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] + + matrix2.mRows[1][1], matrix1.mRows[1][2] + matrix2.mRows[1][2], + matrix1.mRows[2][0] + matrix2.mRows[2][0], matrix1.mRows[2][1] + + matrix2.mRows[2][1], matrix1.mRows[2][2] + matrix2.mRows[2][2]); } // Overloaded operator for substraction 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] - - matrix2.mRows[1][1], matrix1.mRows[1][2] - matrix2.mRows[1][2], - matrix1.mRows[2][0] - matrix2.mRows[2][0], matrix1.mRows[2][1] - - matrix2.mRows[2][1], matrix1.mRows[2][2] - matrix2.mRows[2][2]); + 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] - + matrix2.mRows[1][1], matrix1.mRows[1][2] - matrix2.mRows[1][2], + matrix1.mRows[2][0] - matrix2.mRows[2][0], matrix1.mRows[2][1] - + matrix2.mRows[2][1], matrix1.mRows[2][2] - matrix2.mRows[2][2]); } // Overloaded operator for the negative of the matrix 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]); + 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) { - 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); +inline Matrix3x3 operator*(float 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) { - return nb * matrix; +inline Matrix3x3 operator*(const Matrix3x3& matrix, float nb) { + return nb * matrix; } // Overloaded operator for matrix multiplication 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] * - matrix2.mRows[1][1] + matrix1.mRows[0][2]*matrix2.mRows[2][1], - matrix1.mRows[0][0]*matrix2.mRows[0][2] + matrix1.mRows[0][1] * - matrix2.mRows[1][2] + matrix1.mRows[0][2]*matrix2.mRows[2][2], - matrix1.mRows[1][0]*matrix2.mRows[0][0] + matrix1.mRows[1][1] * - matrix2.mRows[1][0] + matrix1.mRows[1][2]*matrix2.mRows[2][0], - matrix1.mRows[1][0]*matrix2.mRows[0][1] + matrix1.mRows[1][1] * - matrix2.mRows[1][1] + matrix1.mRows[1][2]*matrix2.mRows[2][1], - matrix1.mRows[1][0]*matrix2.mRows[0][2] + matrix1.mRows[1][1] * - matrix2.mRows[1][2] + matrix1.mRows[1][2]*matrix2.mRows[2][2], - matrix1.mRows[2][0]*matrix2.mRows[0][0] + matrix1.mRows[2][1] * - matrix2.mRows[1][0] + matrix1.mRows[2][2]*matrix2.mRows[2][0], - matrix1.mRows[2][0]*matrix2.mRows[0][1] + matrix1.mRows[2][1] * - matrix2.mRows[1][1] + matrix1.mRows[2][2]*matrix2.mRows[2][1], - matrix1.mRows[2][0]*matrix2.mRows[0][2] + matrix1.mRows[2][1] * - matrix2.mRows[1][2] + matrix1.mRows[2][2]*matrix2.mRows[2][2]); + 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] * + matrix2.mRows[1][1] + matrix1.mRows[0][2]*matrix2.mRows[2][1], + matrix1.mRows[0][0]*matrix2.mRows[0][2] + matrix1.mRows[0][1] * + matrix2.mRows[1][2] + matrix1.mRows[0][2]*matrix2.mRows[2][2], + matrix1.mRows[1][0]*matrix2.mRows[0][0] + matrix1.mRows[1][1] * + matrix2.mRows[1][0] + matrix1.mRows[1][2]*matrix2.mRows[2][0], + matrix1.mRows[1][0]*matrix2.mRows[0][1] + matrix1.mRows[1][1] * + matrix2.mRows[1][1] + matrix1.mRows[1][2]*matrix2.mRows[2][1], + matrix1.mRows[1][0]*matrix2.mRows[0][2] + matrix1.mRows[1][1] * + matrix2.mRows[1][2] + matrix1.mRows[1][2]*matrix2.mRows[2][2], + matrix1.mRows[2][0]*matrix2.mRows[0][0] + matrix1.mRows[2][1] * + matrix2.mRows[1][0] + matrix1.mRows[2][2]*matrix2.mRows[2][0], + matrix1.mRows[2][0]*matrix2.mRows[0][1] + matrix1.mRows[2][1] * + matrix2.mRows[1][1] + matrix1.mRows[2][2]*matrix2.mRows[2][1], + matrix1.mRows[2][0]*matrix2.mRows[0][2] + matrix1.mRows[2][1] * + matrix2.mRows[1][2] + matrix1.mRows[2][2]*matrix2.mRows[2][2]); } // Overloaded operator for multiplication with a vector 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 + - matrix.mRows[1][2]*vector.z, - matrix.mRows[2][0]*vector.x + matrix.mRows[2][1]*vector.y + - matrix.mRows[2][2]*vector.z); + 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 + + matrix.mRows[1][2]*vector.z, + matrix.mRows[2][0]*vector.x + matrix.mRows[2][1]*vector.y + + matrix.mRows[2][2]*vector.z); } // Overloaded operator for equality condition 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] && - mRows[1][2] == matrix.mRows[1][2] && - mRows[2][0] == matrix.mRows[2][0] && mRows[2][1] == matrix.mRows[2][1] && - mRows[2][2] == matrix.mRows[2][2]); + 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] && + mRows[1][2] == matrix.mRows[1][2] && + mRows[2][0] == matrix.mRows[2][0] && mRows[2][1] == matrix.mRows[2][1] && + mRows[2][2] == matrix.mRows[2][2]); } // Overloaded operator for the is different condition inline bool Matrix3x3::operator!= (const Matrix3x3& matrix) const { - return !(*this == matrix); + return !(*this == matrix); } // Overloaded operator for addition with assignment @@ -345,7 +323,7 @@ inline Matrix3x3& Matrix3x3::operator-=(const Matrix3x3& matrix) { } // Overloaded operator for multiplication with a number with assignment -inline Matrix3x3& Matrix3x3::operator*=(decimal nb) { +inline Matrix3x3& Matrix3x3::operator*=(float 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; @@ -355,17 +333,15 @@ 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 { - return mRows[row]; +inline const Vector3& Matrix3x3::operator[](int32_t 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) { - return mRows[row]; +inline Vector3& Matrix3x3::operator[](int32_t row) { + return mRows[row]; } } - -#endif diff --git a/ephysics/mathematics/Quaternion.cpp b/ephysics/mathematics/Quaternion.cpp index eceebc5..81d9c20 100644 --- a/ephysics/mathematics/Quaternion.cpp +++ b/ephysics/mathematics/Quaternion.cpp @@ -1,27 +1,8 @@ -/******************************************************************************** -* 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. * -* * -********************************************************************************/ +/** @file + * @author Daniel Chappuis + * @copyright 2010-2016 Daniel Chappuis + * @license BSD 3 clauses (see license file) + */ // Libraries #include @@ -37,95 +18,95 @@ Quaternion::Quaternion() : x(0.0), y(0.0), z(0.0), w(0.0) { } // Constructor with arguments -Quaternion::Quaternion(decimal newX, decimal newY, decimal newZ, decimal newW) - :x(newX), y(newY), z(newZ), w(newW) { +Quaternion::Quaternion(float newX, float newY, float newZ, float newW) + :x(newX), y(newY), z(newZ), w(newW) { } // Constructor with the component w and the vector v=(x y z) -Quaternion::Quaternion(decimal newW, const Vector3& v) : x(v.x), y(v.y), z(v.z), w(newW) { +Quaternion::Quaternion(float newW, const Vector3& v) : x(v.x), y(v.y), z(v.z), w(newW) { } // Constructor which convert Euler angles (in radians) to a quaternion -Quaternion::Quaternion(decimal angleX, decimal angleY, decimal angleZ) { - initWithEulerAngles(angleX, angleY, angleZ); +Quaternion::Quaternion(float angleX, float angleY, float angleZ) { + initWithEulerAngles(angleX, angleY, angleZ); } // Constructor which convert Euler angles (in radians) to a quaternion Quaternion::Quaternion(const Vector3& eulerAngles) { - initWithEulerAngles(eulerAngles.x, eulerAngles.y, eulerAngles.z); + initWithEulerAngles(eulerAngles.x, eulerAngles.y, eulerAngles.z); } // Copy-constructor Quaternion::Quaternion(const Quaternion& quaternion) - :x(quaternion.x), y(quaternion.y), z(quaternion.z), w(quaternion.w) { + :x(quaternion.x), y(quaternion.y), z(quaternion.z), w(quaternion.w) { } // Create a unit quaternion from a rotation matrix Quaternion::Quaternion(const Matrix3x3& matrix) { - // Get the trace of the matrix - decimal trace = matrix.getTrace(); + // Get the trace of the matrix + float trace = matrix.getTrace(); - decimal r; - decimal s; + float r; + float s; - if (trace < 0.0) { - if (matrix[1][1] > matrix[0][0]) { - if(matrix[2][2] > matrix[1][1]) { - r = sqrt(matrix[2][2] - matrix[0][0] - matrix[1][1] + decimal(1.0)); - s = decimal(0.5) / r; - - // Compute the quaternion - x = (matrix[2][0] + matrix[0][2]) * s; - y = (matrix[1][2] + matrix[2][1]) * s; - z = decimal(0.5) * r; - w = (matrix[1][0] - matrix[0][1]) * s; - } - else { - r = sqrt(matrix[1][1] - matrix[2][2] - matrix[0][0] + decimal(1.0)); - s = decimal(0.5) / r; + if (trace < 0.0) { + if (matrix[1][1] > matrix[0][0]) { + if(matrix[2][2] > matrix[1][1]) { + r = sqrt(matrix[2][2] - matrix[0][0] - matrix[1][1] + float(1.0)); + s = float(0.5) / r; + + // Compute the quaternion + x = (matrix[2][0] + matrix[0][2]) * s; + y = (matrix[1][2] + matrix[2][1]) * s; + z = float(0.5) * r; + w = (matrix[1][0] - matrix[0][1]) * s; + } + else { + r = sqrt(matrix[1][1] - matrix[2][2] - matrix[0][0] + float(1.0)); + s = float(0.5) / r; - // Compute the quaternion - x = (matrix[0][1] + matrix[1][0]) * s; - y = decimal(0.5) * r; - z = (matrix[1][2] + matrix[2][1]) * s; - w = (matrix[0][2] - matrix[2][0]) * s; - } - } - else if (matrix[2][2] > matrix[0][0]) { - r = sqrt(matrix[2][2] - matrix[0][0] - matrix[1][1] + decimal(1.0)); - s = decimal(0.5) / r; + // Compute the quaternion + x = (matrix[0][1] + matrix[1][0]) * s; + y = float(0.5) * r; + z = (matrix[1][2] + matrix[2][1]) * s; + w = (matrix[0][2] - matrix[2][0]) * s; + } + } + else if (matrix[2][2] > matrix[0][0]) { + r = sqrt(matrix[2][2] - matrix[0][0] - matrix[1][1] + float(1.0)); + s = float(0.5) / r; - // Compute the quaternion - x = (matrix[2][0] + matrix[0][2]) * s; - y = (matrix[1][2] + matrix[2][1]) * s; - z = decimal(0.5) * r; - w = (matrix[1][0] - matrix[0][1]) * s; - } - else { - r = sqrt(matrix[0][0] - matrix[1][1] - matrix[2][2] + decimal(1.0)); - s = decimal(0.5) / r; + // Compute the quaternion + x = (matrix[2][0] + matrix[0][2]) * s; + y = (matrix[1][2] + matrix[2][1]) * s; + z = float(0.5) * r; + w = (matrix[1][0] - matrix[0][1]) * s; + } + else { + r = sqrt(matrix[0][0] - matrix[1][1] - matrix[2][2] + float(1.0)); + s = float(0.5) / r; - // Compute the quaternion - x = decimal(0.5) * r; - y = (matrix[0][1] + matrix[1][0]) * s; - z = (matrix[2][0] - matrix[0][2]) * s; - w = (matrix[2][1] - matrix[1][2]) * s; - } - } - else { - r = sqrt(trace + decimal(1.0)); - s = decimal(0.5) / r; + // Compute the quaternion + x = float(0.5) * r; + y = (matrix[0][1] + matrix[1][0]) * s; + z = (matrix[2][0] - matrix[0][2]) * s; + w = (matrix[2][1] - matrix[1][2]) * s; + } + } + else { + r = sqrt(trace + float(1.0)); + s = float(0.5) / r; - // Compute the quaternion - x = (matrix[2][1] - matrix[1][2]) * s; - y = (matrix[0][2] - matrix[2][0]) * s; - z = (matrix[1][0] - matrix[0][1]) * s; - w = decimal(0.5) * r; - } + // Compute the quaternion + x = (matrix[2][1] - matrix[1][2]) * s; + y = (matrix[0][2] - matrix[2][0]) * s; + z = (matrix[1][0] - matrix[0][1]) * s; + w = float(0.5) * r; + } } // Destructor @@ -136,125 +117,125 @@ Quaternion::~Quaternion() { // Compute the rotation angle (in radians) and the rotation axis /// This method is used to get the rotation angle (in radian) and the unit /// rotation axis of an orientation quaternion. -void Quaternion::getRotationAngleAxis(decimal& angle, Vector3& axis) const { - Quaternion quaternion; +void Quaternion::getRotationAngleAxis(float& angle, Vector3& axis) const { + Quaternion quaternion; - // If the quaternion is unit - if (length() == 1.0) { - quaternion = *this; - } - else { - // We compute the unit quaternion - quaternion = getUnit(); - } + // If the quaternion is unit + if (length() == 1.0) { + quaternion = *this; + } + else { + // We compute the unit quaternion + quaternion = getUnit(); + } - // Compute the roation angle - angle = acos(quaternion.w) * decimal(2.0); + // Compute the roation angle + angle = acos(quaternion.w) * float(2.0); - // Compute the 3D rotation axis - Vector3 rotationAxis(quaternion.x, quaternion.y, quaternion.z); + // Compute the 3D rotation axis + Vector3 rotationAxis(quaternion.x, quaternion.y, quaternion.z); - // Normalize the rotation axis - rotationAxis = rotationAxis.getUnit(); + // Normalize the rotation axis + rotationAxis = rotationAxis.getUnit(); - // Set the rotation axis values - axis.setAllValues(rotationAxis.x, rotationAxis.y, rotationAxis.z); + // Set the rotation axis values + axis.setAllValues(rotationAxis.x, rotationAxis.y, rotationAxis.z); } // Return the orientation matrix corresponding to this quaternion Matrix3x3 Quaternion::getMatrix() const { - decimal nQ = x*x + y*y + z*z + w*w; - decimal s = 0.0; + float nQ = x*x + y*y + z*z + w*w; + float s = 0.0; - if (nQ > 0.0) { - s = decimal(2.0) / nQ; - } + if (nQ > 0.0) { + s = float(2.0) / nQ; + } - // Computations used for optimization (less multiplications) - decimal xs = x*s; - decimal ys = y*s; - decimal zs = z*s; - decimal wxs = w*xs; - decimal wys = w*ys; - decimal wzs = w*zs; - decimal xxs = x*xs; - decimal xys = x*ys; - decimal xzs = x*zs; - decimal yys = y*ys; - decimal yzs = y*zs; - decimal zzs = z*zs; + // Computations used for optimization (less multiplications) + float xs = x*s; + float ys = y*s; + float zs = z*s; + float wxs = w*xs; + float wys = w*ys; + float wzs = w*zs; + float xxs = x*xs; + float xys = x*ys; + float xzs = x*zs; + float yys = y*ys; + float yzs = y*zs; + float zzs = z*zs; - // Create the matrix corresponding to the quaternion - return Matrix3x3(decimal(1.0) - yys - zzs, xys-wzs, xzs + wys, - xys + wzs, decimal(1.0) - xxs - zzs, yzs-wxs, - xzs-wys, yzs + wxs, decimal(1.0) - xxs - yys); + // Create the matrix corresponding to the quaternion + return Matrix3x3(float(1.0) - yys - zzs, xys-wzs, xzs + wys, + xys + wzs, float(1.0) - xxs - zzs, yzs-wxs, + xzs-wys, yzs + wxs, float(1.0) - xxs - yys); } -// Compute the spherical linear interpolation between two quaternions. +// Compute the spherical linear int32_terpolation between two quaternions. /// The t argument has to be such that 0 <= t <= 1. This method is static. Quaternion Quaternion::slerp(const Quaternion& quaternion1, - const Quaternion& quaternion2, decimal t) { - assert(t >= 0.0 && t <= 1.0); + const Quaternion& quaternion2, float t) { + assert(t >= 0.0 && t <= 1.0); - decimal invert = 1.0; + float invert = 1.0; - // Compute cos(theta) using the quaternion scalar product - decimal cosineTheta = quaternion1.dot(quaternion2); + // Compute cos(theta) using the quaternion scalar product + float cosineTheta = quaternion1.dot(quaternion2); - // Take care of the sign of cosineTheta - if (cosineTheta < 0.0) { + // Take care of the sign of cosineTheta + if (cosineTheta < 0.0) { cosineTheta = -cosineTheta; invert = -1.0; - } + } - // Because of precision, if cos(theta) is nearly 1, - // therefore theta is nearly 0 and we can write - // sin((1-t)*theta) as (1-t) and sin(t*theta) as t - const decimal epsilon = decimal(0.00001); - if(1-cosineTheta < epsilon) { - return quaternion1 * (decimal(1.0)-t) + quaternion2 * (t * invert); - } + // Because of precision, if cos(theta) is nearly 1, + // therefore theta is nearly 0 and we can write + // sin((1-t)*theta) as (1-t) and sin(t*theta) as t + const float epsilon = float(0.00001); + if(1-cosineTheta < epsilon) { + return quaternion1 * (float(1.0)-t) + quaternion2 * (t * invert); + } - // Compute the theta angle - decimal theta = acos(cosineTheta); + // Compute the theta angle + float theta = acos(cosineTheta); - // Compute sin(theta) - decimal sineTheta = sin(theta); + // Compute sin(theta) + float sineTheta = sin(theta); - // Compute the two coefficients that are in the spherical linear interpolation formula - decimal coeff1 = sin((decimal(1.0)-t)*theta) / sineTheta; - decimal coeff2 = sin(t*theta) / sineTheta * invert; + // Compute the two coefficients that are in the spherical linear int32_terpolation formula + float coeff1 = sin((float(1.0)-t)*theta) / sineTheta; + float coeff2 = sin(t*theta) / sineTheta * invert; - // Compute and return the interpolated quaternion - return quaternion1 * coeff1 + quaternion2 * coeff2; + // Compute and return the int32_terpolated quaternion + return quaternion1 * coeff1 + quaternion2 * coeff2; } // Initialize the quaternion using Euler angles -void Quaternion::initWithEulerAngles(decimal angleX, decimal angleY, decimal angleZ) { +void Quaternion::initWithEulerAngles(float angleX, float angleY, float angleZ) { - decimal angle = angleX * decimal(0.5); - const decimal sinX = std::sin(angle); - const decimal cosX = std::cos(angle); + float angle = angleX * float(0.5); + const float sinX = std::sin(angle); + const float cosX = std::cos(angle); - angle = angleY * decimal(0.5); - const decimal sinY = std::sin(angle); - const decimal cosY = std::cos(angle); + angle = angleY * float(0.5); + const float sinY = std::sin(angle); + const float cosY = std::cos(angle); - angle = angleZ * decimal(0.5); - const decimal sinZ = std::sin(angle); - const decimal cosZ = std::cos(angle); + angle = angleZ * float(0.5); + const float sinZ = std::sin(angle); + const float cosZ = std::cos(angle); - const decimal cosYcosZ = cosY * cosZ; - const decimal sinYcosZ = sinY * cosZ; - const decimal cosYsinZ = cosY * sinZ; - const decimal sinYsinZ = sinY * sinZ; + const float cosYcosZ = cosY * cosZ; + const float sinYcosZ = sinY * cosZ; + const float cosYsinZ = cosY * sinZ; + const float sinYsinZ = sinY * sinZ; - x = sinX * cosYcosZ - cosX * sinYsinZ; - y = cosX * sinYcosZ + sinX * cosYsinZ; - z = cosX * cosYsinZ - sinX * sinYcosZ; - w = cosX * cosYcosZ + sinX * sinYsinZ; + x = sinX * cosYcosZ - cosX * sinYsinZ; + y = cosX * sinYcosZ + sinX * cosYsinZ; + z = cosX * cosYsinZ - sinX * sinYcosZ; + w = cosX * cosYcosZ + sinX * sinYsinZ; - // Normalize the quaternion - normalize(); + // Normalize the quaternion + normalize(); } diff --git a/ephysics/mathematics/Quaternion.h b/ephysics/mathematics/Quaternion.h index dadd2fe..b4b279f 100644 --- a/ephysics/mathematics/Quaternion.h +++ b/ephysics/mathematics/Quaternion.h @@ -1,36 +1,15 @@ -/******************************************************************************** -* 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 REACTPHYSICS3D_QUATERNION_H -#define REACTPHYSICS3D_QUATERNION_H +/** @file + * @author Daniel Chappuis + * @copyright 2010-2016 Daniel Chappuis + * @license BSD 3 clauses (see license file) + */ +#pragma once // Libraries #include #include #include -#include + /// ReactPhysics3D namespace namespace reactphysics3d { @@ -42,311 +21,310 @@ namespace reactphysics3d { */ struct Quaternion { - public : + public : - // -------------------- Attributes -------------------- // + // -------------------- Attributes -------------------- // - /// Component x - decimal x; + /// Component x + float x; - /// Component y - decimal y; + /// Component y + float y; - /// Component z - decimal z; + /// Component z + float z; - /// Component w - decimal w; + /// Component w + float w; - // -------------------- Methods -------------------- // + // -------------------- Methods -------------------- // - /// Constructor - Quaternion(); + /// Constructor + Quaternion(); - /// Constructor with arguments - Quaternion(decimal newX, decimal newY, decimal newZ, decimal newW); + /// Constructor with arguments + Quaternion(float newX, float newY, float newZ, float newW); - /// Constructor with the component w and the vector v=(x y z) - Quaternion(decimal newW, const Vector3& v); + /// Constructor with the component w and the vector v=(x y z) + Quaternion(float newW, const Vector3& v); - /// Constructor which convert Euler angles (in radians) to a quaternion - Quaternion(decimal angleX, decimal angleY, decimal angleZ); + /// Constructor which convert Euler angles (in radians) to a quaternion + Quaternion(float angleX, float angleY, float angleZ); - /// Constructor which convert Euler angles (in radians) to a quaternion - Quaternion(const Vector3& eulerAngles); + /// Constructor which convert Euler angles (in radians) to a quaternion + Quaternion(const Vector3& eulerAngles); - /// Copy-constructor - Quaternion(const Quaternion& quaternion); + /// Copy-constructor + Quaternion(const Quaternion& quaternion); - /// Create a unit quaternion from a rotation matrix - Quaternion(const Matrix3x3& matrix); + /// Create a unit quaternion from a rotation matrix + Quaternion(const Matrix3x3& matrix); - /// Destructor - ~Quaternion(); + /// Destructor + ~Quaternion(); - /// Set all the values - void setAllValues(decimal newX, decimal newY, decimal newZ, decimal newW); + /// Set all the values + void setAllValues(float newX, float newY, float newZ, float newW); - /// Set the quaternion to zero - void setToZero(); + /// Set the quaternion to zero + void setToZero(); - /// Set to the identity quaternion - void setToIdentity(); + /// Set to the identity quaternion + void setToIdentity(); - /// Return the vector v=(x y z) of the quaternion - Vector3 getVectorV() const; + /// Return the vector v=(x y z) of the quaternion + Vector3 getVectorV() const; - /// Return the length of the quaternion - decimal length() const; + /// Return the length of the quaternion + float length() const; - /// Return the square of the length of the quaternion - decimal lengthSquare() const; + /// Return the square of the length of the quaternion + float lengthSquare() const; - /// Normalize the quaternion - void normalize(); + /// Normalize the quaternion + void normalize(); - /// Inverse the quaternion - void inverse(); + /// Inverse the quaternion + void inverse(); - /// Return the unit quaternion - Quaternion getUnit() const; + /// Return the unit quaternion + Quaternion getUnit() const; - /// Return the conjugate quaternion - Quaternion getConjugate() const; + /// Return the conjugate quaternion + Quaternion getConjugate() const; - /// Return the inverse of the quaternion - Quaternion getInverse() const; + /// Return the inverse of the quaternion + Quaternion getInverse() const; - /// Return the orientation matrix corresponding to this quaternion - Matrix3x3 getMatrix() const; + /// Return the orientation matrix corresponding to this quaternion + Matrix3x3 getMatrix() const; - /// Return the identity quaternion - static Quaternion identity(); + /// Return the identity quaternion + static Quaternion identity(); - /// Dot product between two quaternions - decimal dot(const Quaternion& quaternion) const; + /// Dot product between two quaternions + float dot(const Quaternion& quaternion) const; - /// Compute the rotation angle (in radians) and the rotation axis - void getRotationAngleAxis(decimal& angle, Vector3& axis) const; + /// Compute the rotation angle (in radians) and the rotation axis + void getRotationAngleAxis(float& angle, Vector3& axis) const; - /// Compute the spherical linear interpolation between two quaternions - static Quaternion slerp(const Quaternion& quaternion1, const Quaternion& quaternion2, - decimal t); + /// Compute the spherical linear int32_terpolation between two quaternions + static Quaternion slerp(const Quaternion& quaternion1, const Quaternion& quaternion2, + float t); - /// Overloaded operator for the addition - Quaternion operator+(const Quaternion& quaternion) const; + /// Overloaded operator for the addition + Quaternion operator+(const Quaternion& quaternion) const; - /// Overloaded operator for the substraction - Quaternion operator-(const Quaternion& quaternion) const; + /// Overloaded operator for the substraction + Quaternion operator-(const Quaternion& quaternion) const; - /// Overloaded operator for addition with assignment - Quaternion& operator+=(const Quaternion& quaternion); + /// Overloaded operator for addition with assignment + Quaternion& operator+=(const Quaternion& quaternion); - /// Overloaded operator for substraction with assignment - Quaternion& operator-=(const Quaternion& quaternion); + /// Overloaded operator for substraction with assignment + Quaternion& operator-=(const Quaternion& quaternion); - /// Overloaded operator for the multiplication with a constant - Quaternion operator*(decimal nb) const; + /// Overloaded operator for the multiplication with a constant + Quaternion operator*(float nb) const; - /// Overloaded operator for the multiplication - Quaternion operator*(const Quaternion& quaternion) const; + /// Overloaded operator for the multiplication + Quaternion operator*(const Quaternion& quaternion) const; - /// Overloaded operator for the multiplication with a vector - Vector3 operator*(const Vector3& point) const; + /// Overloaded operator for the multiplication with a vector + Vector3 operator*(const Vector3& point) const; - /// Overloaded operator for assignment - Quaternion& operator=(const Quaternion& quaternion); + /// Overloaded operator for assignment + Quaternion& operator=(const Quaternion& quaternion); - /// Overloaded operator for equality condition - bool operator==(const Quaternion& quaternion) const; + /// Overloaded operator for equality condition + bool operator==(const Quaternion& quaternion) const; - private: + private: - /// Initialize the quaternion using Euler angles - void initWithEulerAngles(decimal angleX, decimal angleY, decimal angleZ); + /// Initialize the quaternion using Euler angles + void initWithEulerAngles(float angleX, float angleY, float angleZ); }; /// Set all the values -inline void Quaternion::setAllValues(decimal newX, decimal newY, decimal newZ, decimal newW) { - x = newX; - y = newY; - z = newZ; - w = newW; +inline void Quaternion::setAllValues(float newX, float newY, float newZ, float newW) { + x = newX; + y = newY; + z = newZ; + w = newW; } /// Set the quaternion to zero inline void Quaternion::setToZero() { - x = 0; - y = 0; - z = 0; - w = 0; + x = 0; + y = 0; + z = 0; + w = 0; } // Set to the identity quaternion inline void Quaternion::setToIdentity() { - x = 0; - y = 0; - z = 0; - w = 1; + x = 0; + y = 0; + z = 0; + w = 1; } // Return the vector v=(x y z) of the quaternion inline Vector3 Quaternion::getVectorV() const { - // Return the vector v - return Vector3(x, y, z); + // Return the vector v + return Vector3(x, y, z); } // Return the length of the quaternion (inline) -inline decimal Quaternion::length() const { - return sqrt(x*x + y*y + z*z + w*w); +inline float Quaternion::length() const { + return sqrt(x*x + y*y + z*z + w*w); } // Return the square of the length of the quaternion -inline decimal Quaternion::lengthSquare() const { - return x*x + y*y + z*z + w*w; +inline float Quaternion::lengthSquare() const { + return x*x + y*y + z*z + w*w; } // Normalize the quaternion inline void Quaternion::normalize() { - decimal l = length(); + float l = length(); - // Check if the length is not equal to zero - assert (l > MACHINE_EPSILON); + // Check if the length is not equal to zero + assert (l > MACHINE_EPSILON); - x /= l; - y /= l; - z /= l; - w /= l; + x /= l; + y /= l; + z /= l; + w /= l; } // Inverse the quaternion inline void Quaternion::inverse() { - // Get the square length of the quaternion - decimal lengthSquareQuaternion = lengthSquare(); + // Get the square length of the quaternion + float lengthSquareQuaternion = lengthSquare(); - assert (lengthSquareQuaternion > MACHINE_EPSILON); + assert (lengthSquareQuaternion > MACHINE_EPSILON); - // Compute and return the inverse quaternion - x /= -lengthSquareQuaternion; - y /= -lengthSquareQuaternion; - z /= -lengthSquareQuaternion; - w /= lengthSquareQuaternion; + // Compute and return the inverse quaternion + x /= -lengthSquareQuaternion; + y /= -lengthSquareQuaternion; + z /= -lengthSquareQuaternion; + w /= lengthSquareQuaternion; } // Return the unit quaternion inline Quaternion Quaternion::getUnit() const { - decimal lengthQuaternion = length(); + float lengthQuaternion = length(); - // Check if the length is not equal to zero - assert (lengthQuaternion > MACHINE_EPSILON); + // Check if the length is not equal to zero + assert (lengthQuaternion > MACHINE_EPSILON); - // Compute and return the unit quaternion - return Quaternion(x / lengthQuaternion, y / lengthQuaternion, - z / lengthQuaternion, w / lengthQuaternion); + // Compute and return the unit quaternion + return Quaternion(x / lengthQuaternion, y / lengthQuaternion, + z / lengthQuaternion, w / lengthQuaternion); } // Return the identity quaternion inline Quaternion Quaternion::identity() { - return Quaternion(0.0, 0.0, 0.0, 1.0); + return Quaternion(0.0, 0.0, 0.0, 1.0); } // Return the conjugate of the quaternion (inline) inline Quaternion Quaternion::getConjugate() const { - return Quaternion(-x, -y, -z, w); + return Quaternion(-x, -y, -z, w); } // Return the inverse of the quaternion (inline) inline Quaternion Quaternion::getInverse() const { - decimal lengthSquareQuaternion = lengthSquare(); + float lengthSquareQuaternion = lengthSquare(); - assert (lengthSquareQuaternion > MACHINE_EPSILON); + assert (lengthSquareQuaternion > MACHINE_EPSILON); - // Compute and return the inverse quaternion - return Quaternion(-x / lengthSquareQuaternion, -y / lengthSquareQuaternion, - -z / lengthSquareQuaternion, w / lengthSquareQuaternion); + // Compute and return the inverse quaternion + return Quaternion(-x / lengthSquareQuaternion, -y / lengthSquareQuaternion, + -z / lengthSquareQuaternion, w / lengthSquareQuaternion); } // Scalar product between two quaternions -inline decimal Quaternion::dot(const Quaternion& quaternion) const { - return (x*quaternion.x + y*quaternion.y + z*quaternion.z + w*quaternion.w); +inline float Quaternion::dot(const Quaternion& quaternion) const { + return (x*quaternion.x + y*quaternion.y + z*quaternion.z + w*quaternion.w); } // Overloaded operator for the addition of two quaternions 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); + // 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 { - // Return the result of the substraction - return Quaternion(x - quaternion.x, y - quaternion.y, z - quaternion.z, w - quaternion.w); + // 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) { - x += quaternion.x; - y += quaternion.y; - z += quaternion.z; - w += quaternion.w; - return *this; + x += quaternion.x; + y += quaternion.y; + z += quaternion.z; + w += quaternion.w; + return *this; } // Overloaded operator for substraction with assignment inline Quaternion& Quaternion::operator-=(const Quaternion& quaternion) { - x -= quaternion.x; - y -= quaternion.y; - z -= quaternion.z; - w -= quaternion.w; - return *this; + x -= quaternion.x; + y -= quaternion.y; + z -= quaternion.z; + w -= quaternion.w; + return *this; } // Overloaded operator for the multiplication with a constant -inline Quaternion Quaternion::operator*(decimal nb) const { - return Quaternion(nb * x, nb * y, nb * z, nb * w); +inline Quaternion Quaternion::operator*(float 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 { - return Quaternion(w * quaternion.w - getVectorV().dot(quaternion.getVectorV()), - w * quaternion.getVectorV() + quaternion.w * getVectorV() + - getVectorV().cross(quaternion.getVectorV())); + return Quaternion(w * quaternion.w - getVectorV().dot(quaternion.getVectorV()), + w * quaternion.getVectorV() + quaternion.w * getVectorV() + + getVectorV().cross(quaternion.getVectorV())); } // 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 { - Quaternion p(point.x, point.y, point.z, 0.0); - return (((*this) * p) * getConjugate()).getVectorV(); + Quaternion p(point.x, point.y, point.z, 0.0); + return (((*this) * p) * getConjugate()).getVectorV(); } // Overloaded operator for the assignment inline Quaternion& Quaternion::operator=(const Quaternion& quaternion) { - // Check for self-assignment - if (this != &quaternion) { - x = quaternion.x; - y = quaternion.y; - z = quaternion.z; - w = quaternion.w; - } + // Check for self-assignment + if (this != &quaternion) { + x = quaternion.x; + y = quaternion.y; + z = quaternion.z; + w = quaternion.w; + } - // Return this quaternion - return *this; + // Return this quaternion + return *this; } // Overloaded operator for equality condition inline bool Quaternion::operator==(const Quaternion& quaternion) const { - return (x == quaternion.x && y == quaternion.y && - z == quaternion.z && w == quaternion.w); + return (x == quaternion.x && y == quaternion.y && + z == quaternion.z && w == quaternion.w); } } -#endif diff --git a/ephysics/mathematics/Ray.h b/ephysics/mathematics/Ray.h index 4308347..ed52124 100644 --- a/ephysics/mathematics/Ray.h +++ b/ephysics/mathematics/Ray.h @@ -1,30 +1,9 @@ -/******************************************************************************** -* 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 REACTPHYSICS3D_RAY_H -#define REACTPHYSICS3D_RAY_H +/** @file + * @author Daniel Chappuis + * @copyright 2010-2016 Daniel Chappuis + * @license BSD 3 clauses (see license file) + */ +#pragma once // Libraries #include @@ -40,48 +19,46 @@ namespace reactphysics3d { */ struct Ray { - public: + public: - // -------------------- Attributes -------------------- // + // -------------------- Attributes -------------------- // - /// First point of the ray (origin) - Vector3 point1; + /// First point of the ray (origin) + Vector3 point1; - /// Second point of the ray - Vector3 point2; + /// Second point of the ray + Vector3 point2; - /// Maximum fraction value - decimal maxFraction; + /// Maximum fraction value + float maxFraction; - // -------------------- Methods -------------------- // + // -------------------- Methods -------------------- // - /// Constructor with arguments - Ray(const Vector3& p1, const Vector3& p2, decimal maxFrac = decimal(1.0)) - : point1(p1), point2(p2), maxFraction(maxFrac) { + /// Constructor with arguments + Ray(const Vector3& p1, const Vector3& p2, float maxFrac = float(1.0)) + : point1(p1), point2(p2), maxFraction(maxFrac) { - } + } - /// Copy-constructor - Ray(const Ray& ray) : point1(ray.point1), point2(ray.point2), maxFraction(ray.maxFraction) { + /// Copy-constructor + Ray(const Ray& ray) : point1(ray.point1), point2(ray.point2), maxFraction(ray.maxFraction) { - } + } - /// Destructor - ~Ray() { + /// Destructor + ~Ray() { - } + } - /// Overloaded assignment operator - Ray& operator=(const Ray& ray) { - if (&ray != this) { - point1 = ray.point1; - point2 = ray.point2; - maxFraction = ray.maxFraction; - } - return *this; - } + /// Overloaded assignment operator + Ray& operator=(const Ray& ray) { + if (&ray != this) { + point1 = ray.point1; + point2 = ray.point2; + maxFraction = ray.maxFraction; + } + return *this; + } }; } - -#endif diff --git a/ephysics/mathematics/Transform.cpp b/ephysics/mathematics/Transform.cpp index 995a9e2..4bebc5a 100644 --- a/ephysics/mathematics/Transform.cpp +++ b/ephysics/mathematics/Transform.cpp @@ -1,27 +1,8 @@ -/******************************************************************************** -* 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. * -* * -********************************************************************************/ +/** @file + * @author Daniel Chappuis + * @copyright 2010-2016 Daniel Chappuis + * @license BSD 3 clauses (see license file) + */ // Libraries #include @@ -36,23 +17,23 @@ Transform::Transform() : mPosition(Vector3(0.0, 0.0, 0.0)), mOrientation(Quatern // Constructor Transform::Transform(const Vector3& position, const Matrix3x3& orientation) - : mPosition(position), mOrientation(Quaternion(orientation)) { + : mPosition(position), mOrientation(Quaternion(orientation)) { } // Constructor Transform::Transform(const Vector3& position, const Quaternion& orientation) - : mPosition(position), mOrientation(orientation) { + : mPosition(position), mOrientation(orientation) { } // Copy-constructor Transform::Transform(const Transform& transform) - : mPosition(transform.mPosition), mOrientation(transform.mOrientation) { + : mPosition(transform.mPosition), mOrientation(transform.mOrientation) { } // Destructor Transform::~Transform() { - + } diff --git a/ephysics/mathematics/Transform.h b/ephysics/mathematics/Transform.h index 1913ea1..27b58b6 100644 --- a/ephysics/mathematics/Transform.h +++ b/ephysics/mathematics/Transform.h @@ -1,30 +1,9 @@ -/******************************************************************************** -* 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 REACTPHYSICS3D_TRANSFORM_H -#define REACTPHYSICS3D_TRANSFORM_H +/** @file + * @author Daniel Chappuis + * @copyright 2010-2016 Daniel Chappuis + * @license BSD 3 clauses (see license file) + */ +#pragma once // Libraries #include @@ -41,189 +20,186 @@ namespace reactphysics3d { */ class Transform { - private : + private : - // -------------------- Attributes -------------------- // + // -------------------- Attributes -------------------- // - /// Position - Vector3 mPosition; + /// Position + Vector3 mPosition; - /// Orientation - Quaternion mOrientation; + /// Orientation + Quaternion mOrientation; - public : + public : - // -------------------- Methods -------------------- // + // -------------------- Methods -------------------- // - /// Constructor - Transform(); + /// Constructor + Transform(); - /// Constructor - Transform(const Vector3& position, const Matrix3x3& orientation); + /// Constructor + Transform(const Vector3& position, const Matrix3x3& orientation); - /// Constructor - Transform(const Vector3& position, const Quaternion& orientation); + /// Constructor + Transform(const Vector3& position, const Quaternion& orientation); - /// Destructor - ~Transform(); + /// Destructor + ~Transform(); - /// Copy-constructor - Transform(const Transform& transform); + /// Copy-constructor + Transform(const Transform& transform); - /// Return the origin of the transform - const Vector3& getPosition() const; + /// Return the origin of the transform + const Vector3& getPosition() const; - /// Set the origin of the transform - void setPosition(const Vector3& position); + /// Set the origin of the transform + void setPosition(const Vector3& position); - /// Return the orientation quaternion - const Quaternion& getOrientation() const; + /// Return the orientation quaternion + const Quaternion& getOrientation() const; - /// Set the rotation quaternion - void setOrientation(const Quaternion& orientation); + /// Set the rotation quaternion + void setOrientation(const Quaternion& orientation); - /// Set the transform to the identity transform - void setToIdentity(); + /// Set the transform to the identity transform + void setToIdentity(); - /// Set the transform from an OpenGL transform matrix - void setFromOpenGL(decimal* openglMatrix); + /// Set the transform from an OpenGL transform matrix + void setFromOpenGL(float* openglMatrix); - /// Get the OpenGL matrix of the transform - void getOpenGLMatrix(decimal* openglMatrix) const; + /// Get the OpenGL matrix of the transform + void getOpenGLMatrix(float* openglMatrix) const; - /// Return the inverse of the transform - Transform getInverse() const; + /// Return the inverse of the transform + Transform getInverse() const; - /// Return an interpolated transform - static Transform interpolateTransforms(const Transform& oldTransform, - const Transform& newTransform, - decimal interpolationFactor); + /// Return an int32_terpolated transform + static Transform int32_terpolateTransforms(const Transform& oldTransform, + const Transform& newTransform, + float int32_terpolationFactor); - /// Return the identity transform - static Transform identity(); + /// Return the identity transform + static Transform identity(); - /// Return the transformed vector - Vector3 operator*(const Vector3& vector) const; + /// Return the transformed vector + Vector3 operator*(const Vector3& vector) const; - /// Operator of multiplication of a transform with another one - Transform operator*(const Transform& transform2) const; + /// Operator of multiplication of a transform with another one + Transform operator*(const Transform& transform2) const; - /// Return true if the two transforms are equal - bool operator==(const Transform& transform2) const; + /// Return true if the two transforms are equal + bool operator==(const Transform& transform2) const; - /// Return true if the two transforms are different - bool operator!=(const Transform& transform2) const; + /// Return true if the two transforms are different + bool operator!=(const Transform& transform2) const; - /// Assignment operator - Transform& operator=(const Transform& transform); + /// Assignment operator + Transform& operator=(const Transform& transform); }; // Return the position of the transform inline const Vector3& Transform::getPosition() const { - return mPosition; + return mPosition; } // Set the origin of the transform inline void Transform::setPosition(const Vector3& position) { - mPosition = position; + mPosition = position; } // Return the rotation matrix inline const Quaternion& Transform::getOrientation() const { - return mOrientation; + return mOrientation; } // Set the rotation matrix of the transform inline void Transform::setOrientation(const Quaternion& orientation) { - mOrientation = orientation; + mOrientation = orientation; } // Set the transform to the identity transform inline void Transform::setToIdentity() { - mPosition = Vector3(0.0, 0.0, 0.0); - mOrientation = Quaternion::identity(); -} + mPosition = Vector3(0.0, 0.0, 0.0); + mOrientation = Quaternion::identity(); +} // Set the transform from an OpenGL transform matrix -inline void Transform::setFromOpenGL(decimal* openglMatrix) { - Matrix3x3 matrix(openglMatrix[0], openglMatrix[4], openglMatrix[8], - openglMatrix[1], openglMatrix[5], openglMatrix[9], - openglMatrix[2], openglMatrix[6], openglMatrix[10]); - mOrientation = Quaternion(matrix); - mPosition.setAllValues(openglMatrix[12], openglMatrix[13], openglMatrix[14]); +inline void Transform::setFromOpenGL(float* openglMatrix) { + Matrix3x3 matrix(openglMatrix[0], openglMatrix[4], openglMatrix[8], + openglMatrix[1], openglMatrix[5], openglMatrix[9], + openglMatrix[2], openglMatrix[6], openglMatrix[10]); + mOrientation = Quaternion(matrix); + mPosition.setAllValues(openglMatrix[12], openglMatrix[13], openglMatrix[14]); } // Get the OpenGL matrix of the transform -inline void Transform::getOpenGLMatrix(decimal* openglMatrix) const { - const Matrix3x3& matrix = mOrientation.getMatrix(); - openglMatrix[0] = matrix[0][0]; openglMatrix[1] = matrix[1][0]; - openglMatrix[2] = matrix[2][0]; openglMatrix[3] = 0.0; - openglMatrix[4] = matrix[0][1]; openglMatrix[5] = matrix[1][1]; - openglMatrix[6] = matrix[2][1]; openglMatrix[7] = 0.0; - openglMatrix[8] = matrix[0][2]; openglMatrix[9] = matrix[1][2]; - openglMatrix[10] = matrix[2][2]; openglMatrix[11] = 0.0; - openglMatrix[12] = mPosition.x; openglMatrix[13] = mPosition.y; - openglMatrix[14] = mPosition.z; openglMatrix[15] = 1.0; +inline void Transform::getOpenGLMatrix(float* openglMatrix) const { + const Matrix3x3& matrix = mOrientation.getMatrix(); + openglMatrix[0] = matrix[0][0]; openglMatrix[1] = matrix[1][0]; + openglMatrix[2] = matrix[2][0]; openglMatrix[3] = 0.0; + openglMatrix[4] = matrix[0][1]; openglMatrix[5] = matrix[1][1]; + openglMatrix[6] = matrix[2][1]; openglMatrix[7] = 0.0; + openglMatrix[8] = matrix[0][2]; openglMatrix[9] = matrix[1][2]; + openglMatrix[10] = matrix[2][2]; openglMatrix[11] = 0.0; + openglMatrix[12] = mPosition.x; openglMatrix[13] = mPosition.y; + openglMatrix[14] = mPosition.z; openglMatrix[15] = 1.0; } // Return the inverse of the transform inline Transform Transform::getInverse() const { - const Quaternion& invQuaternion = mOrientation.getInverse(); - Matrix3x3 invMatrix = invQuaternion.getMatrix(); - return Transform(invMatrix * (-mPosition), invQuaternion); + const Quaternion& invQuaternion = mOrientation.getInverse(); + Matrix3x3 invMatrix = invQuaternion.getMatrix(); + return Transform(invMatrix * (-mPosition), invQuaternion); } -// Return an interpolated transform -inline Transform Transform::interpolateTransforms(const Transform& oldTransform, - const Transform& newTransform, - decimal interpolationFactor) { +// Return an int32_terpolated transform +inline Transform Transform::int32_terpolateTransforms(const Transform& oldTransform, + const Transform& newTransform, + float int32_terpolationFactor) { - Vector3 interPosition = oldTransform.mPosition * (decimal(1.0) - interpolationFactor) + - newTransform.mPosition * interpolationFactor; + Vector3 int32_terPosition = oldTransform.mPosition * (float(1.0) - int32_terpolationFactor) + + newTransform.mPosition * int32_terpolationFactor; - Quaternion interOrientation = Quaternion::slerp(oldTransform.mOrientation, - newTransform.mOrientation, - interpolationFactor); + Quaternion int32_terOrientation = Quaternion::slerp(oldTransform.mOrientation, + newTransform.mOrientation, + int32_terpolationFactor); - return Transform(interPosition, interOrientation); + return Transform(int32_terPosition, int32_terOrientation); } // Return the identity transform inline Transform Transform::identity() { - return Transform(Vector3(0, 0, 0), Quaternion::identity()); + return Transform(Vector3(0, 0, 0), Quaternion::identity()); } // Return the transformed vector inline Vector3 Transform::operator*(const Vector3& vector) const { - return (mOrientation.getMatrix() * vector) + mPosition; + return (mOrientation.getMatrix() * vector) + mPosition; } // Operator of multiplication of a transform with another one inline Transform Transform::operator*(const Transform& transform2) const { - return Transform(mPosition + mOrientation.getMatrix() * transform2.mPosition, - mOrientation * transform2.mOrientation); + return Transform(mPosition + mOrientation.getMatrix() * transform2.mPosition, + mOrientation * transform2.mOrientation); } // Return true if the two transforms are equal inline bool Transform::operator==(const Transform& transform2) const { - return (mPosition == transform2.mPosition) && (mOrientation == transform2.mOrientation); -} + return (mPosition == transform2.mPosition) && (mOrientation == transform2.mOrientation); +} // Return true if the two transforms are different inline bool Transform::operator!=(const Transform& transform2) const { - return !(*this == transform2); + return !(*this == transform2); } // Assignment operator inline Transform& Transform::operator=(const Transform& transform) { - if (&transform != this) { - mPosition = transform.mPosition; - mOrientation = transform.mOrientation; - } - return *this; + if (&transform != this) { + mPosition = transform.mPosition; + mOrientation = transform.mOrientation; + } + return *this; } } - -#endif - diff --git a/ephysics/mathematics/Vector2.cpp b/ephysics/mathematics/Vector2.cpp index 0dddbf8..9fdb871 100644 --- a/ephysics/mathematics/Vector2.cpp +++ b/ephysics/mathematics/Vector2.cpp @@ -1,27 +1,8 @@ -/******************************************************************************** -* 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. * -* * -********************************************************************************/ +/** @file + * @author Daniel Chappuis + * @copyright 2010-2016 Daniel Chappuis + * @license BSD 3 clauses (see license file) + */ // Libraries #include @@ -36,7 +17,7 @@ Vector2::Vector2() : x(0.0), y(0.0) { } // Constructor with arguments -Vector2::Vector2(decimal newX, decimal newY) : x(newX), y(newY) { +Vector2::Vector2(float newX, float newY) : x(newX), y(newY) { } @@ -52,22 +33,22 @@ Vector2::~Vector2() { // Return the corresponding unit vector Vector2 Vector2::getUnit() const { - decimal lengthVector = length(); + float lengthVector = length(); - if (lengthVector < MACHINE_EPSILON) { - return *this; - } + if (lengthVector < MACHINE_EPSILON) { + return *this; + } - // Compute and return the unit vector - decimal lengthInv = decimal(1.0) / lengthVector; - return Vector2(x * lengthInv, y * lengthInv); + // Compute and return the unit vector + float lengthInv = float(1.0) / lengthVector; + return Vector2(x * lengthInv, y * lengthInv); } // Return one unit orthogonal vector of the current vector Vector2 Vector2::getOneUnitOrthogonalVector() const { - decimal l = length(); - assert(l > MACHINE_EPSILON); + float l = length(); + assert(l > MACHINE_EPSILON); - return Vector2(-y / l, x / l); + return Vector2(-y / l, x / l); } diff --git a/ephysics/mathematics/Vector2.h b/ephysics/mathematics/Vector2.h index a5f8ca6..210aa13 100644 --- a/ephysics/mathematics/Vector2.h +++ b/ephysics/mathematics/Vector2.h @@ -1,36 +1,15 @@ -/******************************************************************************** -* 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 REACTPHYSICS3D_VECTOR2_H -#define REACTPHYSICS3D_VECTOR2_H +/** @file + * @author Daniel Chappuis + * @copyright 2010-2016 Daniel Chappuis + * @license BSD 3 clauses (see license file) + */ +#pragma once // Libraries #include #include #include -#include + /// ReactPhysics3D namespace @@ -42,305 +21,303 @@ namespace reactphysics3d { */ struct Vector2 { - public: + public: - // -------------------- Attributes -------------------- // + // -------------------- Attributes -------------------- // - /// Component x - decimal x; + /// Component x + float x; - /// Component y - decimal y; + /// Component y + float y; - // -------------------- Methods -------------------- // + // -------------------- Methods -------------------- // - /// Constructor of the class Vector3D - Vector2(); + /// Constructor of the class Vector3D + Vector2(); - /// Constructor with arguments - Vector2(decimal newX, decimal newY); + /// Constructor with arguments + Vector2(float newX, float newY); - /// Copy-constructor - Vector2(const Vector2& vector); + /// Copy-constructor + Vector2(const Vector2& vector); - /// Destructor - ~Vector2(); + /// Destructor + ~Vector2(); - /// Set all the values of the vector - void setAllValues(decimal newX, decimal newY); + /// Set all the values of the vector + void setAllValues(float newX, float newY); - /// Set the vector to zero - void setToZero(); + /// Set the vector to zero + void setToZero(); - /// Return the length of the vector - decimal length() const; + /// Return the length of the vector + float length() const; - /// Return the square of the length of the vector - decimal lengthSquare() const; + /// Return the square of the length of the vector + float lengthSquare() const; - /// Return the corresponding unit vector - Vector2 getUnit() const; + /// Return the corresponding unit vector + Vector2 getUnit() const; - /// Return one unit orthogonal vector of the current vector - Vector2 getOneUnitOrthogonalVector() const; + /// Return one unit orthogonal vector of the current vector + Vector2 getOneUnitOrthogonalVector() const; - /// Return true if the vector is unit and false otherwise - bool isUnit() const; + /// Return true if the vector is unit and false otherwise + bool isUnit() const; - /// Return true if the current vector is the zero vector - bool isZero() const; + /// Return true if the current vector is the zero vector + bool isZero() const; - /// Dot product of two vectors - decimal dot(const Vector2& vector) const; + /// Dot product of two vectors + float dot(const Vector2& vector) const; - /// Normalize the vector - void normalize(); + /// Normalize the vector + void normalize(); - /// Return the corresponding absolute value vector - Vector2 getAbsoluteVector() const; + /// Return the corresponding absolute value vector + Vector2 getAbsoluteVector() const; - /// Return the axis with the minimal value - int getMinAxis() const; + /// Return the axis with the minimal value + int32_t getMinAxis() const; - /// Return the axis with the maximal value - int getMaxAxis() const; + /// Return the axis with the maximal value + int32_t getMaxAxis() const; - /// Overloaded operator for the equality condition - bool operator== (const Vector2& vector) const; + /// Overloaded operator for the equality condition + bool operator== (const Vector2& vector) const; - /// Overloaded operator for the is different condition - bool operator!= (const Vector2& vector) const; + /// Overloaded operator for the is different condition + bool operator!= (const Vector2& vector) const; - /// Overloaded operator for addition with assignment - Vector2& operator+=(const Vector2& vector); + /// Overloaded operator for addition with assignment + Vector2& operator+=(const Vector2& vector); - /// Overloaded operator for substraction with assignment - Vector2& operator-=(const Vector2& vector); + /// Overloaded operator for substraction with assignment + Vector2& operator-=(const Vector2& vector); - /// Overloaded operator for multiplication with a number with assignment - Vector2& operator*=(decimal number); + /// Overloaded operator for multiplication with a number with assignment + Vector2& operator*=(float number); - /// Overloaded operator for division by a number with assignment - Vector2& operator/=(decimal number); + /// Overloaded operator for division by a number with assignment + Vector2& operator/=(float number); - /// Overloaded operator for value access - decimal& operator[] (int index); + /// Overloaded operator for value access + float& operator[] (int32_t index); - /// Overloaded operator for value access - const decimal& operator[] (int index) const; + /// Overloaded operator for value access + const float& operator[] (int32_t index) const; - /// Overloaded operator - Vector2& operator=(const Vector2& vector); + /// Overloaded operator + Vector2& operator=(const Vector2& vector); - /// Overloaded less than operator for ordering to be used inside std::set for instance - bool operator<(const Vector2& vector) const; + /// Overloaded less than operator for ordering to be used inside std::set for instance + bool operator<(const Vector2& vector) const; - /// Return a vector taking the minimum components of two vectors - static Vector2 min(const Vector2& vector1, const Vector2& vector2); + /// Return a vector taking the minimum components of two vectors + static Vector2 min(const Vector2& vector1, const Vector2& vector2); - /// Return a vector taking the maximum components of two vectors - static Vector2 max(const Vector2& vector1, const Vector2& vector2); + /// Return a vector taking the maximum components of two vectors + static Vector2 max(const Vector2& vector1, const Vector2& vector2); - /// Return the zero vector - static Vector2 zero(); + /// Return the zero vector + static Vector2 zero(); - // -------------------- Friends -------------------- // + // -------------------- Friends -------------------- // - friend Vector2 operator+(const Vector2& vector1, const Vector2& vector2); - friend Vector2 operator-(const Vector2& vector1, const Vector2& vector2); - friend Vector2 operator-(const Vector2& vector); - friend Vector2 operator*(const Vector2& vector, decimal number); - friend Vector2 operator*(decimal number, const Vector2& vector); - friend Vector2 operator*(const Vector2& vector1, const Vector2& vector2); - friend Vector2 operator/(const Vector2& vector, decimal number); - friend Vector2 operator/(const Vector2& vector1, const Vector2& vector2); + friend Vector2 operator+(const Vector2& vector1, const Vector2& vector2); + friend Vector2 operator-(const Vector2& vector1, const Vector2& vector2); + friend Vector2 operator-(const Vector2& vector); + friend Vector2 operator*(const Vector2& vector, float number); + friend Vector2 operator*(float number, const Vector2& vector); + friend Vector2 operator*(const Vector2& vector1, const Vector2& vector2); + friend Vector2 operator/(const Vector2& vector, float number); + friend Vector2 operator/(const Vector2& vector1, const Vector2& vector2); }; // Set the vector to zero inline void Vector2::setToZero() { - x = 0; - y = 0; + x = 0; + y = 0; } // Set all the values of the vector -inline void Vector2::setAllValues(decimal newX, decimal newY) { - x = newX; - y = newY; +inline void Vector2::setAllValues(float newX, float newY) { + x = newX; + y = newY; } // Return the length of the vector -inline decimal Vector2::length() const { - return sqrt(x*x + y*y); +inline float Vector2::length() const { + return sqrt(x*x + y*y); } // Return the square of the length of the vector -inline decimal Vector2::lengthSquare() const { - return x*x + y*y; +inline float Vector2::lengthSquare() const { + return x*x + y*y; } // Scalar product of two vectors (inline) -inline decimal Vector2::dot(const Vector2& vector) const { - return (x*vector.x + y*vector.y); +inline float Vector2::dot(const Vector2& vector) const { + return (x*vector.x + y*vector.y); } // Normalize the vector inline void Vector2::normalize() { - decimal l = length(); - if (l < MACHINE_EPSILON) { - return; - } - x /= l; - y /= l; + float l = length(); + if (l < MACHINE_EPSILON) { + return; + } + x /= l; + y /= l; } // Return the corresponding absolute value vector inline Vector2 Vector2::getAbsoluteVector() const { - return Vector2(std::abs(x), std::abs(y)); + return Vector2(std::abs(x), std::abs(y)); } // Return the axis with the minimal value -inline int Vector2::getMinAxis() const { - return (x < y ? 0 : 1); +inline int32_t Vector2::getMinAxis() const { + return (x < y ? 0 : 1); } // Return the axis with the maximal value -inline int Vector2::getMaxAxis() const { - return (x < y ? 1 : 0); +inline int32_t 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); + return approxEqual(lengthSquare(), 1.0); } // Return true if the vector is the zero vector inline bool Vector2::isZero() const { - return approxEqual(lengthSquare(), 0.0); + return approxEqual(lengthSquare(), 0.0); } // Overloaded operator for the equality condition inline bool Vector2::operator== (const Vector2& vector) const { - return (x == vector.x && y == vector.y); + return (x == vector.x && y == vector.y); } // Overloaded operator for the is different condition inline bool Vector2::operator!= (const Vector2& vector) const { - return !(*this == vector); + return !(*this == vector); } // Overloaded operator for addition with assignment inline Vector2& Vector2::operator+=(const Vector2& vector) { - x += vector.x; - y += vector.y; - return *this; + x += vector.x; + y += vector.y; + return *this; } // Overloaded operator for substraction with assignment inline Vector2& Vector2::operator-=(const Vector2& vector) { - x -= vector.x; - y -= vector.y; - return *this; + x -= vector.x; + y -= vector.y; + return *this; } // Overloaded operator for multiplication with a number with assignment -inline Vector2& Vector2::operator*=(decimal number) { - x *= number; - y *= number; - return *this; +inline Vector2& Vector2::operator*=(float number) { + x *= number; + y *= number; + return *this; } // Overloaded operator for division by a number with assignment -inline Vector2& Vector2::operator/=(decimal number) { - assert(number > std::numeric_limits::epsilon()); - x /= number; - y /= number; - return *this; +inline Vector2& Vector2::operator/=(float number) { + assert(number > std::numeric_limits::epsilon()); + x /= number; + y /= number; + return *this; } // Overloaded operator for value access -inline decimal& Vector2::operator[] (int index) { - return (&x)[index]; +inline float& Vector2::operator[] (int32_t index) { + return (&x)[index]; } // Overloaded operator for value access -inline const decimal& Vector2::operator[] (int index) const { - return (&x)[index]; +inline const float& Vector2::operator[] (int32_t index) const { + return (&x)[index]; } // Overloaded operator for addition inline Vector2 operator+(const Vector2& vector1, const Vector2& vector2) { - return Vector2(vector1.x + vector2.x, vector1.y + vector2.y); + return Vector2(vector1.x + vector2.x, vector1.y + vector2.y); } // Overloaded operator for substraction inline Vector2 operator-(const Vector2& vector1, const Vector2& vector2) { - return Vector2(vector1.x - vector2.x, vector1.y - vector2.y); + return Vector2(vector1.x - vector2.x, vector1.y - vector2.y); } // Overloaded operator for the negative of a vector inline Vector2 operator-(const Vector2& vector) { - return Vector2(-vector.x, -vector.y); + return Vector2(-vector.x, -vector.y); } // Overloaded operator for multiplication with a number -inline Vector2 operator*(const Vector2& vector, decimal number) { - return Vector2(number * vector.x, number * vector.y); +inline Vector2 operator*(const Vector2& vector, float 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) { - return Vector2(vector1.x * vector2.x, vector1.y * vector2.y); + 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) { - assert(number > MACHINE_EPSILON); - return Vector2(vector.x / number, vector.y / number); +inline Vector2 operator/(const Vector2& vector, float 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) { - assert(vector2.x > MACHINE_EPSILON); - assert(vector2.y > MACHINE_EPSILON); - return Vector2(vector1.x / vector2.x, vector1.y / vector2.y); + 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) { - return vector * number; +inline Vector2 operator*(float number, const Vector2& vector) { + return vector * number; } // Assignment operator inline Vector2& Vector2::operator=(const Vector2& vector) { - if (&vector != this) { - x = vector.x; - y = vector.y; - } - return *this; + if (&vector != this) { + x = vector.x; + y = vector.y; + } + return *this; } // Overloaded less than operator for ordering to be used inside std::set for instance inline bool Vector2::operator<(const Vector2& vector) const { - return (x == vector.x ? y < vector.y : x < vector.x); + 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) { - return Vector2(std::min(vector1.x, vector2.x), - std::min(vector1.y, vector2.y)); + 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) { - return Vector2(std::max(vector1.x, vector2.x), - std::max(vector1.y, vector2.y)); + return Vector2(std::max(vector1.x, vector2.x), + std::max(vector1.y, vector2.y)); } // Return the zero vector inline Vector2 Vector2::zero() { - return Vector2(0, 0); + return Vector2(0, 0); } } - -#endif diff --git a/ephysics/mathematics/Vector3.cpp b/ephysics/mathematics/Vector3.cpp index 86c2238..1f0505f 100644 --- a/ephysics/mathematics/Vector3.cpp +++ b/ephysics/mathematics/Vector3.cpp @@ -1,27 +1,8 @@ -/******************************************************************************** -* 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. * -* * -********************************************************************************/ +/** @file + * @author Daniel Chappuis + * @copyright 2010-2016 Daniel Chappuis + * @license BSD 3 clauses (see license file) + */ // Libraries #include @@ -37,7 +18,7 @@ Vector3::Vector3() : x(0.0), y(0.0), z(0.0) { } // Constructor with arguments -Vector3::Vector3(decimal newX, decimal newY, decimal newZ) : x(newX), y(newY), z(newZ) { +Vector3::Vector3(float newX, float newY, float newZ) : x(newX), y(newY), z(newZ) { } @@ -53,34 +34,34 @@ Vector3::~Vector3() { // Return the corresponding unit vector Vector3 Vector3::getUnit() const { - decimal lengthVector = length(); + float lengthVector = length(); - if (lengthVector < MACHINE_EPSILON) { - return *this; - } + if (lengthVector < MACHINE_EPSILON) { + return *this; + } - // Compute and return the unit vector - decimal lengthInv = decimal(1.0) / lengthVector; - return Vector3(x * lengthInv, y * lengthInv, z * lengthInv); + // Compute and return the unit vector + float lengthInv = float(1.0) / lengthVector; + return Vector3(x * lengthInv, y * lengthInv, z * lengthInv); } // Return one unit orthogonal vector of the current vector Vector3 Vector3::getOneUnitOrthogonalVector() const { - assert(length() > MACHINE_EPSILON); + assert(length() > MACHINE_EPSILON); - // Get the minimum element of the vector - Vector3 vectorAbs(fabs(x), fabs(y), fabs(z)); - int minElement = vectorAbs.getMinAxis(); + // Get the minimum element of the vector + Vector3 vectorAbs(fabs(x), fabs(y), fabs(z)); + int32_t minElement = vectorAbs.getMinAxis(); - if (minElement == 0) { - return Vector3(0.0, -z, y) / sqrt(y*y + z*z); - } - else if (minElement == 1) { - return Vector3(-z, 0.0, x) / sqrt(x*x + z*z); - } - else { - return Vector3(-y, x, 0.0) / sqrt(x*x + y*y); - } + if (minElement == 0) { + return Vector3(0.0, -z, y) / sqrt(y*y + z*z); + } + else if (minElement == 1) { + return Vector3(-z, 0.0, x) / sqrt(x*x + z*z); + } + else { + return Vector3(-y, x, 0.0) / sqrt(x*x + y*y); + } } diff --git a/ephysics/mathematics/Vector3.h b/ephysics/mathematics/Vector3.h index 10f6c12..0ecfa72 100644 --- a/ephysics/mathematics/Vector3.h +++ b/ephysics/mathematics/Vector3.h @@ -1,36 +1,15 @@ -/******************************************************************************** -* 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 REACTPHYSICS3D_VECTOR3_H -#define REACTPHYSICS3D_VECTOR3_H +/** @file + * @author Daniel Chappuis + * @copyright 2010-2016 Daniel Chappuis + * @license BSD 3 clauses (see license file) + */ +#pragma once // Libraries #include #include #include -#include + /// ReactPhysics3D namespace @@ -42,345 +21,343 @@ namespace reactphysics3d { */ struct Vector3 { - public: + public: - // -------------------- Attributes -------------------- // + // -------------------- Attributes -------------------- // - /// Component x - decimal x; + /// Component x + float x; - /// Component y - decimal y; + /// Component y + float y; - /// Component z - decimal z; + /// Component z + float z; - // -------------------- Methods -------------------- // + // -------------------- Methods -------------------- // - /// Constructor of the class Vector3D - Vector3(); + /// Constructor of the class Vector3D + Vector3(); - /// Constructor with arguments - Vector3(decimal newX, decimal newY, decimal newZ); + /// Constructor with arguments + Vector3(float newX, float newY, float newZ); - /// Copy-constructor - Vector3(const Vector3& vector); + /// Copy-constructor + Vector3(const Vector3& vector); - /// Destructor - ~Vector3(); + /// Destructor + ~Vector3(); - /// Set all the values of the vector - void setAllValues(decimal newX, decimal newY, decimal newZ); + /// Set all the values of the vector + void setAllValues(float newX, float newY, float newZ); - /// Set the vector to zero - void setToZero(); + /// Set the vector to zero + void setToZero(); - /// Return the length of the vector - decimal length() const; + /// Return the length of the vector + float length() const; - /// Return the square of the length of the vector - decimal lengthSquare() const; + /// Return the square of the length of the vector + float lengthSquare() const; - /// Return the corresponding unit vector - Vector3 getUnit() const; + /// Return the corresponding unit vector + Vector3 getUnit() const; - /// Return one unit orthogonal vector of the current vector - Vector3 getOneUnitOrthogonalVector() const; + /// Return one unit orthogonal vector of the current vector + Vector3 getOneUnitOrthogonalVector() const; - /// Return true if the vector is unit and false otherwise - bool isUnit() const; + /// Return true if the vector is unit and false otherwise + bool isUnit() const; - /// Return true if the current vector is the zero vector - bool isZero() const; + /// Return true if the current vector is the zero vector + bool isZero() const; - /// Dot product of two vectors - decimal dot(const Vector3& vector) const; + /// Dot product of two vectors + float dot(const Vector3& vector) const; - /// Cross product of two vectors - Vector3 cross(const Vector3& vector) const; + /// Cross product of two vectors + Vector3 cross(const Vector3& vector) const; - /// Normalize the vector - void normalize(); + /// Normalize the vector + void normalize(); - /// Return the corresponding absolute value vector - Vector3 getAbsoluteVector() const; + /// Return the corresponding absolute value vector + Vector3 getAbsoluteVector() const; - /// Return the axis with the minimal value - int getMinAxis() const; + /// Return the axis with the minimal value + int32_t getMinAxis() const; - /// Return the axis with the maximal value - int getMaxAxis() const; + /// Return the axis with the maximal value + int32_t getMaxAxis() const; - /// Return the minimum value among the three components of a vector - decimal getMinValue() const; + /// Return the minimum value among the three components of a vector + float getMinValue() const; - /// Return the maximum value among the three components of a vector - decimal getMaxValue() const; + /// Return the maximum value among the three components of a vector + float getMaxValue() const; - /// Overloaded operator for the equality condition - bool operator== (const Vector3& vector) const; + /// Overloaded operator for the equality condition + bool operator== (const Vector3& vector) const; - /// Overloaded operator for the is different condition - bool operator!= (const Vector3& vector) const; + /// Overloaded operator for the is different condition + bool operator!= (const Vector3& vector) const; - /// Overloaded operator for addition with assignment - Vector3& operator+=(const Vector3& vector); + /// Overloaded operator for addition with assignment + Vector3& operator+=(const Vector3& vector); - /// Overloaded operator for substraction with assignment - Vector3& operator-=(const Vector3& vector); + /// Overloaded operator for substraction with assignment + Vector3& operator-=(const Vector3& vector); - /// Overloaded operator for multiplication with a number with assignment - Vector3& operator*=(decimal number); + /// Overloaded operator for multiplication with a number with assignment + Vector3& operator*=(float number); - /// Overloaded operator for division by a number with assignment - Vector3& operator/=(decimal number); + /// Overloaded operator for division by a number with assignment + Vector3& operator/=(float number); - /// Overloaded operator for value access - decimal& operator[] (int index); + /// Overloaded operator for value access + float& operator[] (int32_t index); - /// Overloaded operator for value access - const decimal& operator[] (int index) const; + /// Overloaded operator for value access + const float& operator[] (int32_t index) const; - /// Overloaded operator - Vector3& operator=(const Vector3& vector); + /// Overloaded operator + Vector3& operator=(const Vector3& vector); - /// Overloaded less than operator for ordering to be used inside std::set for instance - bool operator<(const Vector3& vector) const; + /// Overloaded less than operator for ordering to be used inside std::set for instance + bool operator<(const Vector3& vector) const; - /// Return a vector taking the minimum components of two vectors - static Vector3 min(const Vector3& vector1, const Vector3& vector2); + /// Return a vector taking the minimum components of two vectors + static Vector3 min(const Vector3& vector1, const Vector3& vector2); - /// Return a vector taking the maximum components of two vectors - static Vector3 max(const Vector3& vector1, const Vector3& vector2); + /// Return a vector taking the maximum components of two vectors + static Vector3 max(const Vector3& vector1, const Vector3& vector2); - /// Return the zero vector - static Vector3 zero(); + /// Return the zero vector + static Vector3 zero(); - // -------------------- Friends -------------------- // + // -------------------- Friends -------------------- // - friend Vector3 operator+(const Vector3& vector1, const Vector3& vector2); - friend Vector3 operator-(const Vector3& vector1, const Vector3& vector2); - friend Vector3 operator-(const Vector3& vector); - friend Vector3 operator*(const Vector3& vector, decimal number); - friend Vector3 operator*(decimal number, const Vector3& vector); - friend Vector3 operator*(const Vector3& vector1, const Vector3& vector2); - friend Vector3 operator/(const Vector3& vector, decimal number); - friend Vector3 operator/(const Vector3& vector1, const Vector3& vector2); + friend Vector3 operator+(const Vector3& vector1, const Vector3& vector2); + friend Vector3 operator-(const Vector3& vector1, const Vector3& vector2); + friend Vector3 operator-(const Vector3& vector); + friend Vector3 operator*(const Vector3& vector, float number); + friend Vector3 operator*(float number, const Vector3& vector); + friend Vector3 operator*(const Vector3& vector1, const Vector3& vector2); + friend Vector3 operator/(const Vector3& vector, float number); + friend Vector3 operator/(const Vector3& vector1, const Vector3& vector2); }; // Set the vector to zero inline void Vector3::setToZero() { - x = 0; - y = 0; - z = 0; + x = 0; + y = 0; + z = 0; } // Set all the values of the vector -inline void Vector3::setAllValues(decimal newX, decimal newY, decimal newZ) { - x = newX; - y = newY; - z = newZ; +inline void Vector3::setAllValues(float newX, float newY, float newZ) { + x = newX; + y = newY; + z = newZ; } // Return the length of the vector -inline decimal Vector3::length() const { - return sqrt(x*x + y*y + z*z); +inline float Vector3::length() const { + return sqrt(x*x + y*y + z*z); } // Return the square of the length of the vector -inline decimal Vector3::lengthSquare() const { - return x*x + y*y + z*z; +inline float Vector3::lengthSquare() const { + return x*x + y*y + z*z; } // Scalar product of two vectors (inline) -inline decimal Vector3::dot(const Vector3& vector) const { - return (x*vector.x + y*vector.y + z*vector.z); +inline float 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 { - return Vector3(y * vector.z - z * vector.y, - z * vector.x - x * vector.z, - x * vector.y - y * vector.x); + 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() { - decimal l = length(); - if (l < MACHINE_EPSILON) { - return; - } - x /= l; - y /= l; - z /= l; + float l = length(); + if (l < MACHINE_EPSILON) { + return; + } + x /= l; + y /= l; + z /= l; } // Return the corresponding absolute value vector inline Vector3 Vector3::getAbsoluteVector() const { - return Vector3(std::abs(x), std::abs(y), std::abs(z)); + return Vector3(std::abs(x), std::abs(y), std::abs(z)); } // Return the axis with the minimal value -inline int Vector3::getMinAxis() const { - return (x < y ? (x < z ? 0 : 2) : (y < z ? 1 : 2)); +inline int32_t 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 { - return (x < y ? (y < z ? 2 : 1) : (x < z ? 2 : 0)); +inline int32_t 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); + return approxEqual(lengthSquare(), 1.0); } // Return true if the vector is the zero vector inline bool Vector3::isZero() const { - return approxEqual(lengthSquare(), 0.0); + return approxEqual(lengthSquare(), 0.0); } // Overloaded operator for the equality condition inline bool Vector3::operator== (const Vector3& vector) const { - return (x == vector.x && y == vector.y && z == vector.z); + 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 { - return !(*this == vector); + return !(*this == vector); } // Overloaded operator for addition with assignment inline Vector3& Vector3::operator+=(const Vector3& vector) { - x += vector.x; - y += vector.y; - z += vector.z; - return *this; + x += vector.x; + y += vector.y; + z += vector.z; + return *this; } // Overloaded operator for substraction with assignment inline Vector3& Vector3::operator-=(const Vector3& vector) { - x -= vector.x; - y -= vector.y; - z -= vector.z; - return *this; + x -= vector.x; + y -= vector.y; + z -= vector.z; + return *this; } // Overloaded operator for multiplication with a number with assignment -inline Vector3& Vector3::operator*=(decimal number) { - x *= number; - y *= number; - z *= number; - return *this; +inline Vector3& Vector3::operator*=(float number) { + x *= number; + y *= number; + z *= number; + return *this; } // Overloaded operator for division by a number with assignment -inline Vector3& Vector3::operator/=(decimal number) { - assert(number > std::numeric_limits::epsilon()); - x /= number; - y /= number; - z /= number; - return *this; +inline Vector3& Vector3::operator/=(float number) { + assert(number > std::numeric_limits::epsilon()); + x /= number; + y /= number; + z /= number; + return *this; } // Overloaded operator for value access -inline decimal& Vector3::operator[] (int index) { - return (&x)[index]; +inline float& Vector3::operator[] (int32_t index) { + return (&x)[index]; } // Overloaded operator for value access -inline const decimal& Vector3::operator[] (int index) const { - return (&x)[index]; +inline const float& Vector3::operator[] (int32_t index) const { + return (&x)[index]; } // Overloaded operator for addition inline Vector3 operator+(const Vector3& vector1, const Vector3& vector2) { - return Vector3(vector1.x + vector2.x, vector1.y + vector2.y, vector1.z + vector2.z); + 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) { - return Vector3(vector1.x - vector2.x, vector1.y - vector2.y, vector1.z - vector2.z); + 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) { - return Vector3(-vector.x, -vector.y, -vector.z); + return Vector3(-vector.x, -vector.y, -vector.z); } // Overloaded operator for multiplication with a number -inline Vector3 operator*(const Vector3& vector, decimal number) { - return Vector3(number * vector.x, number * vector.y, number * vector.z); +inline Vector3 operator*(const Vector3& vector, float 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) { - assert(number > MACHINE_EPSILON); - return Vector3(vector.x / number, vector.y / number, vector.z / number); +inline Vector3 operator/(const Vector3& vector, float 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) { - assert(vector2.x > MACHINE_EPSILON); - assert(vector2.y > MACHINE_EPSILON); - assert(vector2.z > MACHINE_EPSILON); - return Vector3(vector1.x / vector2.x, vector1.y / vector2.y, vector1.z / vector2.z); + assert(vector2.x > MACHINE_EPSILON); + assert(vector2.y > MACHINE_EPSILON); + assert(vector2.z > MACHINE_EPSILON); + return Vector3(vector1.x / vector2.x, vector1.y / vector2.y, vector1.z / vector2.z); } // Overloaded operator for multiplication with a number -inline Vector3 operator*(decimal number, const Vector3& vector) { - return vector * number; +inline Vector3 operator*(float number, const Vector3& vector) { + return vector * number; } // Overload operator for multiplication between two vectors inline Vector3 operator*(const Vector3& vector1, const Vector3& vector2) { - return Vector3(vector1.x * vector2.x, vector1.y * vector2.y, vector1.z * vector2.z); + return Vector3(vector1.x * vector2.x, vector1.y * vector2.y, vector1.z * vector2.z); } // Assignment operator inline Vector3& Vector3::operator=(const Vector3& vector) { - if (&vector != this) { - x = vector.x; - y = vector.y; - z = vector.z; - } - return *this; + if (&vector != this) { + x = vector.x; + y = vector.y; + z = vector.z; + } + return *this; } // Overloaded less than operator for ordering to be used inside std::set for instance 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 (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) { - return Vector3(std::min(vector1.x, vector2.x), - std::min(vector1.y, vector2.y), - std::min(vector1.z, vector2.z)); + 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) { - return Vector3(std::max(vector1.x, vector2.x), - std::max(vector1.y, vector2.y), - std::max(vector1.z, vector2.z)); + 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 { - return std::min(std::min(x, y), z); +inline float 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 { - return std::max(std::max(x, y), z); +inline float Vector3::getMaxValue() const { + return std::max(std::max(x, y), z); } // Return the zero vector inline Vector3 Vector3::zero() { - return Vector3(0, 0, 0); + return Vector3(0, 0, 0); } } - -#endif diff --git a/ephysics/mathematics/mathematics.h b/ephysics/mathematics/mathematics.h index aefc344..ee03cbf 100644 --- a/ephysics/mathematics/mathematics.h +++ b/ephysics/mathematics/mathematics.h @@ -1,26 +1,26 @@ /******************************************************************************** -* ReactPhysics3D physics library, http://www.reactphysics3d.com * -* Copyright (c) 2010-2016 Daniel Chappuis * +* 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: * -* * +* 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. * -* * +* 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 REACTPHYSICS3D_MATHEMATICS_H diff --git a/ephysics/mathematics/mathematics_functions.cpp b/ephysics/mathematics/mathematics_functions.cpp index 4e4a1c7..228a324 100644 --- a/ephysics/mathematics/mathematics_functions.cpp +++ b/ephysics/mathematics/mathematics_functions.cpp @@ -1,27 +1,8 @@ -/******************************************************************************** -* 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. * -* * -********************************************************************************/ +/** @file + * @author Daniel Chappuis + * @copyright 2010-2016 Daniel Chappuis + * @license BSD 3 clauses (see license file) + */ // Libraries #include @@ -33,27 +14,27 @@ using namespace reactphysics3d; /// 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; + const Vector3& p, float& u, float& v, float& 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); + float d00 = v0.dot(v0); + float d01 = v0.dot(v1); + float d11 = v1.dot(v1); + float d20 = v2.dot(v0); + float 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; + float denom = d00 * d11 - d01 * d01; + v = (d11 * d20 - d01 * d21) / denom; + w = (d00 * d21 - d01 * d20) / denom; + u = float(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; +Vector3 reactphysics3d::clamp(const Vector3& vector, float maxLength) { + if (vector.lengthSquare() > maxLength * maxLength) { + return vector.getUnit() * maxLength; + } + return vector; } diff --git a/ephysics/mathematics/mathematics_functions.h b/ephysics/mathematics/mathematics_functions.h index 8c83293..9d56381 100644 --- a/ephysics/mathematics/mathematics_functions.h +++ b/ephysics/mathematics/mathematics_functions.h @@ -1,34 +1,13 @@ -/******************************************************************************** -* 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 REACTPHYSICS3D_MATHEMATICS_FUNCTIONS_H -#define REACTPHYSICS3D_MATHEMATICS_FUNCTIONS_H +/** @file + * @author Daniel Chappuis + * @copyright 2010-2016 Daniel Chappuis + * @license BSD 3 clauses (see license file) + */ +#pragma once // Libraries #include -#include + #include #include #include @@ -42,46 +21,44 @@ struct Vector3; /// 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); +inline bool approxEqual(float a, float b, float epsilon = MACHINE_EPSILON) { + return (std::fabs(a - b) < 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) { - assert(lowerLimit <= upperLimit); - return std::min(std::max(value, lowerLimit), upperLimit); +inline int32_t clamp(int32_t value, int32_t lowerLimit, int32_t 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) { - assert(lowerLimit <= upperLimit); - return std::min(std::max(value, lowerLimit), upperLimit); +inline float clamp(float value, float lowerLimit, float 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) { - return std::min(std::min(a, b), c); +inline float min3(float a, float b, float 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) { - return std::max(std::max(a, b), c); +inline float max3(float a, float b, float 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) { - return a * b >= decimal(0.0); +inline bool sameSign(float a, float b) { + return a * b >= float(0.0); } /// Clamp a vector such that it is no longer than a given maximum length -Vector3 clamp(const Vector3& vector, decimal maxLength); +Vector3 clamp(const Vector3& vector, float maxLength); /// 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& p, float& u, float& v, float& w); } - -#endif diff --git a/ephysics/memory/MemoryAllocator.cpp b/ephysics/memory/MemoryAllocator.cpp index 04b8de0..2c617af 100644 --- a/ephysics/memory/MemoryAllocator.cpp +++ b/ephysics/memory/MemoryAllocator.cpp @@ -1,27 +1,8 @@ -/******************************************************************************** -* 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. * -* * -********************************************************************************/ +/** @file + * @author Daniel Chappuis + * @copyright 2010-2016 Daniel Chappuis + * @license BSD 3 clauses (see license file) + */ // Libraries #include @@ -33,64 +14,64 @@ using namespace reactphysics3d; // Initialization of static variables bool MemoryAllocator::isMapSizeToHeadIndexInitialized = false; size_t MemoryAllocator::mUnitSizes[NB_HEAPS]; -int MemoryAllocator::mMapSizeToHeapIndex[MAX_UNIT_SIZE + 1]; +int32_t MemoryAllocator::mMapSizeToHeapIndex[MAX_UNIT_SIZE + 1]; // Constructor MemoryAllocator::MemoryAllocator() { - // Allocate some memory to manage the blocks - mNbAllocatedMemoryBlocks = 64; - mNbCurrentMemoryBlocks = 0; - const size_t sizeToAllocate = mNbAllocatedMemoryBlocks * sizeof(MemoryBlock); - mMemoryBlocks = (MemoryBlock*) malloc(sizeToAllocate); - memset(mMemoryBlocks, 0, sizeToAllocate); - memset(mFreeMemoryUnits, 0, sizeof(mFreeMemoryUnits)); + // Allocate some memory to manage the blocks + mNbAllocatedMemoryBlocks = 64; + mNbCurrentMemoryBlocks = 0; + const size_t sizeToAllocate = mNbAllocatedMemoryBlocks * sizeof(MemoryBlock); + mMemoryBlocks = (MemoryBlock*) malloc(sizeToAllocate); + memset(mMemoryBlocks, 0, sizeToAllocate); + memset(mFreeMemoryUnits, 0, sizeof(mFreeMemoryUnits)); #ifndef NDEBUG - mNbTimesAllocateMethodCalled = 0; + mNbTimesAllocateMethodCalled = 0; #endif - // If the mMapSizeToHeapIndex has not been initialized yet - if (!isMapSizeToHeadIndexInitialized) { + // If the mMapSizeToHeapIndex has not been initialized yet + if (!isMapSizeToHeadIndexInitialized) { - // Initialize the array that contains the sizes the memory units that will - // be allocated in each different heap - for (int i=0; i < NB_HEAPS; i++) { - mUnitSizes[i] = (i+1) * 8; - } + // Initialize the array that contains the sizes the memory units that will + // be allocated in each different heap + for (int32_t i=0; i < NB_HEAPS; i++) { + mUnitSizes[i] = (i+1) * 8; + } - // Initialize the lookup table that maps the size to allocated to the - // corresponding heap we will use for the allocation - uint j = 0; - mMapSizeToHeapIndex[0] = -1; // This element should not be used - for (uint i=1; i <= MAX_UNIT_SIZE; i++) { - if (i <= mUnitSizes[j]) { - mMapSizeToHeapIndex[i] = j; - } - else { - j++; - mMapSizeToHeapIndex[i] = j; - } - } + // Initialize the lookup table that maps the size to allocated to the + // corresponding heap we will use for the allocation + uint32_t j = 0; + mMapSizeToHeapIndex[0] = -1; // This element should not be used + for (uint32_t i=1; i <= MAX_UNIT_SIZE; i++) { + if (i <= mUnitSizes[j]) { + mMapSizeToHeapIndex[i] = j; + } + else { + j++; + mMapSizeToHeapIndex[i] = j; + } + } - isMapSizeToHeadIndexInitialized = true; - } + isMapSizeToHeadIndexInitialized = true; + } } // Destructor MemoryAllocator::~MemoryAllocator() { - // Release the memory allocated for each block - for (uint i=0; i MAX_UNIT_SIZE) { + // If we need to allocate more than the maximum memory unit size + if (size > MAX_UNIT_SIZE) { - // Allocate memory using standard malloc() function - return malloc(size); - } + // Allocate memory using standard malloc() function + return malloc(size); + } - // Get the index of the heap that will take care of the allocation request - int indexHeap = mMapSizeToHeapIndex[size]; - assert(indexHeap >= 0 && indexHeap < NB_HEAPS); + // Get the index of the heap that will take care of the allocation request + int32_t indexHeap = mMapSizeToHeapIndex[size]; + assert(indexHeap >= 0 && indexHeap < NB_HEAPS); - // If there still are free memory units in the corresponding heap - if (mFreeMemoryUnits[indexHeap] != NULL) { + // If there still are free memory units in the corresponding heap + if (mFreeMemoryUnits[indexHeap] != NULL) { - // Return a pointer to the memory unit - MemoryUnit* unit = mFreeMemoryUnits[indexHeap]; - mFreeMemoryUnits[indexHeap] = unit->nextUnit; - return unit; - } - else { // If there is no more free memory units in the corresponding heap + // Return a pointer to the memory unit + MemoryUnit* unit = mFreeMemoryUnits[indexHeap]; + mFreeMemoryUnits[indexHeap] = unit->nextUnit; + return unit; + } + else { // If there is no more free memory units in the corresponding heap - // If we need to allocate more memory to containsthe blocks - if (mNbCurrentMemoryBlocks == mNbAllocatedMemoryBlocks) { + // If we need to allocate more memory to containsthe blocks + if (mNbCurrentMemoryBlocks == mNbAllocatedMemoryBlocks) { - // Allocate more memory to contain the blocks - MemoryBlock* currentMemoryBlocks = mMemoryBlocks; - mNbAllocatedMemoryBlocks += 64; - mMemoryBlocks = (MemoryBlock*) malloc(mNbAllocatedMemoryBlocks * sizeof(MemoryBlock)); - memcpy(mMemoryBlocks, currentMemoryBlocks,mNbCurrentMemoryBlocks * sizeof(MemoryBlock)); - memset(mMemoryBlocks + mNbCurrentMemoryBlocks, 0, 64 * sizeof(MemoryBlock)); - free(currentMemoryBlocks); - } + // Allocate more memory to contain the blocks + MemoryBlock* currentMemoryBlocks = mMemoryBlocks; + mNbAllocatedMemoryBlocks += 64; + mMemoryBlocks = (MemoryBlock*) malloc(mNbAllocatedMemoryBlocks * sizeof(MemoryBlock)); + memcpy(mMemoryBlocks, currentMemoryBlocks,mNbCurrentMemoryBlocks * sizeof(MemoryBlock)); + memset(mMemoryBlocks + mNbCurrentMemoryBlocks, 0, 64 * sizeof(MemoryBlock)); + free(currentMemoryBlocks); + } - // Allocate a new memory blocks for the corresponding heap and divide it in many - // memory units - MemoryBlock* newBlock = mMemoryBlocks + mNbCurrentMemoryBlocks; - newBlock->memoryUnits = (MemoryUnit*) malloc(BLOCK_SIZE); - assert(newBlock->memoryUnits != NULL); - size_t unitSize = mUnitSizes[indexHeap]; - uint nbUnits = BLOCK_SIZE / unitSize; - assert(nbUnits * unitSize <= BLOCK_SIZE); - for (size_t i=0; i < nbUnits - 1; i++) { - MemoryUnit* unit = (MemoryUnit*) ((size_t)newBlock->memoryUnits + unitSize * i); - MemoryUnit* nextUnit = (MemoryUnit*) ((size_t)newBlock->memoryUnits + unitSize * (i+1)); - unit->nextUnit = nextUnit; - } - MemoryUnit* lastUnit = (MemoryUnit*) ((size_t)newBlock->memoryUnits + unitSize*(nbUnits-1)); - lastUnit->nextUnit = NULL; + // Allocate a new memory blocks for the corresponding heap and divide it in many + // memory units + MemoryBlock* newBlock = mMemoryBlocks + mNbCurrentMemoryBlocks; + newBlock->memoryUnits = (MemoryUnit*) malloc(BLOCK_SIZE); + assert(newBlock->memoryUnits != NULL); + size_t unitSize = mUnitSizes[indexHeap]; + uint32_t nbUnits = BLOCK_SIZE / unitSize; + assert(nbUnits * unitSize <= BLOCK_SIZE); + for (size_t i=0; i < nbUnits - 1; i++) { + MemoryUnit* unit = (MemoryUnit*) ((size_t)newBlock->memoryUnits + unitSize * i); + MemoryUnit* nextUnit = (MemoryUnit*) ((size_t)newBlock->memoryUnits + unitSize * (i+1)); + unit->nextUnit = nextUnit; + } + MemoryUnit* lastUnit = (MemoryUnit*) ((size_t)newBlock->memoryUnits + unitSize*(nbUnits-1)); + lastUnit->nextUnit = NULL; - // Add the new allocated block into the list of free memory units in the heap - mFreeMemoryUnits[indexHeap] = newBlock->memoryUnits->nextUnit; - mNbCurrentMemoryBlocks++; + // Add the new allocated block int32_to the list of free memory units in the heap + mFreeMemoryUnits[indexHeap] = newBlock->memoryUnits->nextUnit; + mNbCurrentMemoryBlocks++; - // Return the pointer to the first memory unit of the new allocated block - return newBlock->memoryUnits; - } + // Return the pointer to the first memory unit of the new allocated block + return newBlock->memoryUnits; + } } // Release previously allocated memory. void MemoryAllocator::release(void* pointer, size_t size) { - // Cannot release a 0-byte allocated memory - if (size == 0) return; + // Cannot release a 0-byte allocated memory + if (size == 0) return; #ifndef NDEBUG - mNbTimesAllocateMethodCalled--; + mNbTimesAllocateMethodCalled--; #endif - // If the size is larger than the maximum memory unit size - if (size > MAX_UNIT_SIZE) { + // If the size is larger than the maximum memory unit size + if (size > MAX_UNIT_SIZE) { - // Release the memory using the standard free() function - free(pointer); - return; - } + // Release the memory using the standard free() function + free(pointer); + return; + } - // Get the index of the heap that has handled the corresponding allocation request - int indexHeap = mMapSizeToHeapIndex[size]; - assert(indexHeap >= 0 && indexHeap < NB_HEAPS); + // Get the index of the heap that has handled the corresponding allocation request + int32_t indexHeap = mMapSizeToHeapIndex[size]; + assert(indexHeap >= 0 && indexHeap < NB_HEAPS); - // Insert the released memory unit into the list of free memory units of the - // corresponding heap - MemoryUnit* releasedUnit = (MemoryUnit*) pointer; - releasedUnit->nextUnit = mFreeMemoryUnits[indexHeap]; - mFreeMemoryUnits[indexHeap] = releasedUnit; + // Insert the released memory unit int32_to the list of free memory units of the + // corresponding heap + MemoryUnit* releasedUnit = (MemoryUnit*) pointer; + releasedUnit->nextUnit = mFreeMemoryUnits[indexHeap]; + mFreeMemoryUnits[indexHeap] = releasedUnit; } diff --git a/ephysics/memory/MemoryAllocator.h b/ephysics/memory/MemoryAllocator.h index 6d9aac4..d3395ac 100644 --- a/ephysics/memory/MemoryAllocator.h +++ b/ephysics/memory/MemoryAllocator.h @@ -1,30 +1,9 @@ -/******************************************************************************** -* 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 REACTPHYSICS3D_MEMORY_ALLOCATOR_H -#define REACTPHYSICS3D_MEMORY_ALLOCATOR_H +/** @file + * @author Daniel Chappuis + * @copyright 2010-2016 Daniel Chappuis + * @license BSD 3 clauses (see license file) + */ +#pragma once // Libraries #include @@ -42,104 +21,102 @@ namespace reactphysics3d { */ class MemoryAllocator { - private : + private : - // -------------------- Internal Classes -------------------- // + // -------------------- Internal Classes -------------------- // - // Structure MemoryUnit - /** - * Represent a memory unit that is used for a single memory allocation - * request. - */ - struct MemoryUnit { + // Structure MemoryUnit + /** + * Represent a memory unit that is used for a single memory allocation + * request. + */ + struct MemoryUnit { - public : + public : - // -------------------- Attributes -------------------- // + // -------------------- Attributes -------------------- // - /// Pointer to the next memory unit inside a memory block - MemoryUnit* nextUnit; + /// Pointer to the next memory unit inside a memory block + MemoryUnit* nextUnit; - }; + }; - // Structure MemoryBlock - /** - * A memory block is a large piece of memory that is allocated once and that - * will contain multiple memory unity. - */ - struct MemoryBlock { + // Structure MemoryBlock + /** + * A memory block is a large piece of memory that is allocated once and that + * will contain multiple memory unity. + */ + struct MemoryBlock { - public : + public : - /// Pointer to the first element of a linked-list of memory unity. - MemoryUnit* memoryUnits; - }; + /// Pointer to the first element of a linked-list of memory unity. + MemoryUnit* memoryUnits; + }; - // -------------------- Constants -------------------- // + // -------------------- Constants -------------------- // - /// Number of heaps - static const int NB_HEAPS = 128; + /// Number of heaps + static const int32_t NB_HEAPS = 128; - /// Maximum memory unit size. An allocation request of a size smaller or equal to - /// this size will be handled using the small block allocator. However, for an - /// allocation request larger than the maximum block size, the standard malloc() - /// will be used. - static const size_t MAX_UNIT_SIZE = 1024; + /// Maximum memory unit size. An allocation request of a size smaller or equal to + /// this size will be handled using the small block allocator. However, for an + /// allocation request larger than the maximum block size, the standard malloc() + /// will be used. + static const size_t MAX_UNIT_SIZE = 1024; - /// Size a memory chunk - static const size_t BLOCK_SIZE = 16 * MAX_UNIT_SIZE; + /// Size a memory chunk + static const size_t BLOCK_SIZE = 16 * MAX_UNIT_SIZE; - // -------------------- Attributes -------------------- // + // -------------------- Attributes -------------------- // - /// Size of the memory units that each heap is responsible to allocate - static size_t mUnitSizes[NB_HEAPS]; + /// Size of the memory units that each heap is responsible to allocate + static size_t mUnitSizes[NB_HEAPS]; - /// Lookup table that mape size to allocate to the index of the - /// corresponding heap we will use for the allocation. - static int mMapSizeToHeapIndex[MAX_UNIT_SIZE + 1]; + /// Lookup table that mape size to allocate to the index of the + /// corresponding heap we will use for the allocation. + static int32_t mMapSizeToHeapIndex[MAX_UNIT_SIZE + 1]; - /// True if the mMapSizeToHeapIndex array has already been initialized - static bool isMapSizeToHeadIndexInitialized; + /// True if the mMapSizeToHeapIndex array has already been initialized + static bool isMapSizeToHeadIndexInitialized; - /// Pointers to the first free memory unit for each heap - MemoryUnit* mFreeMemoryUnits[NB_HEAPS]; + /// Pointers to the first free memory unit for each heap + MemoryUnit* mFreeMemoryUnits[NB_HEAPS]; - /// All the allocated memory blocks - MemoryBlock* mMemoryBlocks; + /// All the allocated memory blocks + MemoryBlock* mMemoryBlocks; - /// Number of allocated memory blocks - uint mNbAllocatedMemoryBlocks; + /// Number of allocated memory blocks + uint32_t mNbAllocatedMemoryBlocks; - /// Current number of used memory blocks - uint mNbCurrentMemoryBlocks; + /// Current number of used memory blocks + uint32_t mNbCurrentMemoryBlocks; #ifndef NDEBUG - /// This variable is incremented by one when the allocate() method has been - /// called and decreased by one when the release() method has been called. - /// This variable is used in debug mode to check that the allocate() and release() - /// methods are called the same number of times - int mNbTimesAllocateMethodCalled; + /// This variable is incremented by one when the allocate() method has been + /// called and decreased by one when the release() method has been called. + /// This variable is used in debug mode to check that the allocate() and release() + /// methods are called the same number of times + int32_t mNbTimesAllocateMethodCalled; #endif - public : + public : - // -------------------- Methods -------------------- // + // -------------------- Methods -------------------- // - /// Constructor - MemoryAllocator(); + /// Constructor + MemoryAllocator(); - /// Destructor - ~MemoryAllocator(); + /// Destructor + ~MemoryAllocator(); - /// Allocate memory of a given size (in bytes) and return a pointer to the - /// allocated memory. - void* allocate(size_t size); + /// Allocate memory of a given size (in bytes) and return a pointer to the + /// allocated memory. + void* allocate(size_t size); - /// Release previously allocated memory. - void release(void* pointer, size_t size); + /// Release previously allocated memory. + void release(void* pointer, size_t size); }; } - -#endif diff --git a/ephysics/memory/Stack.h b/ephysics/memory/Stack.h index 01c4dff..92c43cd 100644 --- a/ephysics/memory/Stack.h +++ b/ephysics/memory/Stack.h @@ -1,30 +1,9 @@ -/******************************************************************************** -* 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 REACTPHYSICS3D_STACK_H -#define REACTPHYSICS3D_STACK_H +/** @file + * @author Daniel Chappuis + * @copyright 2010-2016 Daniel Chappuis + * @license BSD 3 clauses (see license file) + */ +#pragma once // Libraries #include @@ -36,90 +15,88 @@ namespace reactphysics3d { * This class represents a simple generic stack with an initial capacity. If the number * of elements exceeds the capacity, the heap will be used to allocated more memory. */ -template +template class Stack { - private: + private: - // -------------------- Attributes -------------------- // + // -------------------- Attributes -------------------- // - /// Initial array that contains the elements of the stack - T mInitArray[capacity]; + /// Initial array that contains the elements of the stack + T mInitArray[capacity]; - /// Pointer to the first element of the stack - T* mElements; + /// Pointer to the first element of the stack + T* mElements; - /// Number of elements in the stack - uint mNbElements; + /// Number of elements in the stack + uint32_t mNbElements; - /// Number of allocated elements in the stack - uint mNbAllocatedElements; + /// Number of allocated elements in the stack + uint32_t mNbAllocatedElements; - public: + public: - // -------------------- Methods -------------------- // + // -------------------- Methods -------------------- // - /// Constructor - Stack() : mElements(mInitArray), mNbElements(0), mNbAllocatedElements(capacity) { + /// Constructor + Stack() : mElements(mInitArray), mNbElements(0), mNbAllocatedElements(capacity) { - } + } - /// Destructor - ~Stack() { + /// Destructor + ~Stack() { - // If elements have been allocated on the heap - if (mInitArray != mElements) { + // If elements have been allocated on the heap + if (mInitArray != mElements) { - // Release the memory allocated on the heap - free(mElements); - } - } + // Release the memory allocated on the heap + free(mElements); + } + } - /// Push an element into the stack - void push(const T& element); + /// Push an element int32_to the stack + void push(const T& element); - /// Pop an element from the stack (remove it from the stack and return it) - T pop(); + /// Pop an element from the stack (remove it from the stack and return it) + T pop(); - /// Return the number of elments in the stack - uint getNbElements() const; + /// Return the number of elments in the stack + uint32_t getNbElements() const; }; -// Push an element into the stack -template +// Push an element int32_to the stack +template inline void Stack::push(const T& element) { - // If we need to allocate more elements - if (mNbElements == mNbAllocatedElements) { - T* oldElements = mElements; - mNbAllocatedElements *= 2; - mElements = (T*) malloc(mNbAllocatedElements * sizeof(T)); - assert(mElements); - memcpy(mElements, oldElements, mNbElements * sizeof(T)); - if (oldElements != mInitArray) { - free(oldElements); - } - } + // If we need to allocate more elements + if (mNbElements == mNbAllocatedElements) { + T* oldElements = mElements; + mNbAllocatedElements *= 2; + mElements = (T*) malloc(mNbAllocatedElements * sizeof(T)); + assert(mElements); + memcpy(mElements, oldElements, mNbElements * sizeof(T)); + if (oldElements != mInitArray) { + free(oldElements); + } + } - mElements[mNbElements] = element; - mNbElements++; + mElements[mNbElements] = element; + mNbElements++; } // Pop an element from the stack (remove it from the stack and return it) -template +template inline T Stack::pop() { - assert(mNbElements > 0); - mNbElements--; - return mElements[mNbElements]; + assert(mNbElements > 0); + mNbElements--; + return mElements[mNbElements]; } // Return the number of elments in the stack -template -inline uint Stack::getNbElements() const { - return mNbElements; +template +inline uint32_t Stack::getNbElements() const { + return mNbElements; } } - -#endif diff --git a/ephysics/reactphysics3d.h b/ephysics/reactphysics3d.h index da9b9b9..846f749 100644 --- a/ephysics/reactphysics3d.h +++ b/ephysics/reactphysics3d.h @@ -1,38 +1,9 @@ -/******************************************************************************** -* 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. * -* * -********************************************************************************/ - - -/******************************************************************************** -* ReactPhysics3D * -* Version 0.6.0 * -* http://www.reactphysics3d.com * -* Daniel Chappuis * -********************************************************************************/ - -#ifndef REACTPHYSICS3D_H -#define REACTPHYSICS3D_H +/** @file + * @author Daniel Chappuis + * @copyright 2010-2016 Daniel Chappuis + * @license BSD 3 clauses (see license file) + */ +#pragma once // Libraries #include @@ -62,7 +33,5 @@ #include #include -/// Alias to the ReactPhysics3D namespace +/// Alias to the ReactPhysics3D namespace TODO: Remove this ... namespace rp3d = reactphysics3d; - -#endif diff --git a/lutin_ephysics-test.py b/lutin_ephysics-test.py index 0d565c3..4019294 100644 --- a/lutin_ephysics-test.py +++ b/lutin_ephysics-test.py @@ -26,15 +26,15 @@ def get_maintainer(): def configure(target, my_module): my_module.add_src_file([ - 'test/Test.cpp', - 'test/TestSuite.cpp', - 'test/main.cpp' - ]) + 'test/Test.cpp', + 'test/TestSuite.cpp', + 'test/main.cpp' + ]) my_module.add_depend([ - 'ephysics', - 'gtest', - 'test-debug' - ]) + 'ephysics', + 'gtest', + 'test-debug' + ]) my_module.add_path(".") return True diff --git a/lutin_ephysics.py b/lutin_ephysics.py index 07c7c80..49ced20 100644 --- a/lutin_ephysics.py +++ b/lutin_ephysics.py @@ -28,147 +28,146 @@ def configure(target, my_module): my_module.add_extra_flags() # add the file to compile: my_module.add_src_file([ - 'ephysics/memory/MemoryAllocator.cpp', - 'ephysics/constraint/Joint.cpp', - 'ephysics/constraint/HingeJoint.cpp', - 'ephysics/constraint/ContactPoint.cpp', - 'ephysics/constraint/BallAndSocketJoint.cpp', - 'ephysics/constraint/SliderJoint.cpp', - 'ephysics/constraint/FixedJoint.cpp', - 'ephysics/collision/ContactManifoldSet.cpp', - 'ephysics/collision/RaycastInfo.cpp', - 'ephysics/collision/narrowphase/GJK/Simplex.cpp', - 'ephysics/collision/narrowphase/GJK/GJKAlgorithm.cpp', - 'ephysics/collision/narrowphase/DefaultCollisionDispatch.cpp', - 'ephysics/collision/narrowphase/SphereVsSphereAlgorithm.cpp', - 'ephysics/collision/narrowphase/NarrowPhaseAlgorithm.cpp', - 'ephysics/collision/narrowphase/ConcaveVsConvexAlgorithm.cpp', - 'ephysics/collision/narrowphase/EPA/EPAAlgorithm.cpp', - 'ephysics/collision/narrowphase/EPA/TrianglesStore.cpp', - 'ephysics/collision/narrowphase/EPA/TriangleEPA.cpp', - 'ephysics/collision/narrowphase/EPA/EdgeEPA.cpp', - 'ephysics/collision/ProxyShape.cpp', - 'ephysics/collision/shapes/ConcaveShape.cpp', - 'ephysics/collision/shapes/CylinderShape.cpp', - 'ephysics/collision/shapes/SphereShape.cpp', - 'ephysics/collision/shapes/CapsuleShape.cpp', - 'ephysics/collision/shapes/ConvexMeshShape.cpp', - 'ephysics/collision/shapes/CollisionShape.cpp', - 'ephysics/collision/shapes/BoxShape.cpp', - 'ephysics/collision/shapes/TriangleShape.cpp', - 'ephysics/collision/shapes/HeightFieldShape.cpp', - 'ephysics/collision/shapes/ConvexShape.cpp', - 'ephysics/collision/shapes/ConeShape.cpp', - 'ephysics/collision/shapes/ConcaveMeshShape.cpp', - 'ephysics/collision/shapes/AABB.cpp', - 'ephysics/collision/TriangleMesh.cpp', - 'ephysics/collision/CollisionDetection.cpp', - 'ephysics/collision/TriangleVertexArray.cpp', - 'ephysics/collision/ContactManifold.cpp', - 'ephysics/collision/broadphase/DynamicAABBTree.cpp', - 'ephysics/collision/broadphase/BroadPhaseAlgorithm.cpp', - 'ephysics/body/RigidBody.cpp', - 'ephysics/body/Body.cpp', - 'ephysics/body/CollisionBody.cpp', - 'ephysics/mathematics/Vector2.cpp', - 'ephysics/mathematics/Vector3.cpp', - 'ephysics/mathematics/Transform.cpp', - 'ephysics/mathematics/Matrix2x2.cpp', - 'ephysics/mathematics/mathematics_functions.cpp', - 'ephysics/mathematics/Matrix3x3.cpp', - 'ephysics/mathematics/Quaternion.cpp', - 'ephysics/engine/CollisionWorld.cpp', - 'ephysics/engine/OverlappingPair.cpp', - 'ephysics/engine/Material.cpp', - 'ephysics/engine/Island.cpp', - 'ephysics/engine/Profiler.cpp', - 'ephysics/engine/ConstraintSolver.cpp', - 'ephysics/engine/DynamicsWorld.cpp', - 'ephysics/engine/ContactSolver.cpp', - 'ephysics/engine/Timer.cpp', - ]) + 'ephysics/memory/MemoryAllocator.cpp', + 'ephysics/constraint/Joint.cpp', + 'ephysics/constraint/HingeJoint.cpp', + 'ephysics/constraint/ContactPoint.cpp', + 'ephysics/constraint/BallAndSocketJoint.cpp', + 'ephysics/constraint/SliderJoint.cpp', + 'ephysics/constraint/FixedJoint.cpp', + 'ephysics/collision/ContactManifoldSet.cpp', + 'ephysics/collision/RaycastInfo.cpp', + 'ephysics/collision/narrowphase/GJK/Simplex.cpp', + 'ephysics/collision/narrowphase/GJK/GJKAlgorithm.cpp', + 'ephysics/collision/narrowphase/DefaultCollisionDispatch.cpp', + 'ephysics/collision/narrowphase/SphereVsSphereAlgorithm.cpp', + 'ephysics/collision/narrowphase/NarrowPhaseAlgorithm.cpp', + 'ephysics/collision/narrowphase/ConcaveVsConvexAlgorithm.cpp', + 'ephysics/collision/narrowphase/EPA/EPAAlgorithm.cpp', + 'ephysics/collision/narrowphase/EPA/TrianglesStore.cpp', + 'ephysics/collision/narrowphase/EPA/TriangleEPA.cpp', + 'ephysics/collision/narrowphase/EPA/EdgeEPA.cpp', + 'ephysics/collision/ProxyShape.cpp', + 'ephysics/collision/shapes/ConcaveShape.cpp', + 'ephysics/collision/shapes/CylinderShape.cpp', + 'ephysics/collision/shapes/SphereShape.cpp', + 'ephysics/collision/shapes/CapsuleShape.cpp', + 'ephysics/collision/shapes/ConvexMeshShape.cpp', + 'ephysics/collision/shapes/CollisionShape.cpp', + 'ephysics/collision/shapes/BoxShape.cpp', + 'ephysics/collision/shapes/TriangleShape.cpp', + 'ephysics/collision/shapes/HeightFieldShape.cpp', + 'ephysics/collision/shapes/ConvexShape.cpp', + 'ephysics/collision/shapes/ConeShape.cpp', + 'ephysics/collision/shapes/ConcaveMeshShape.cpp', + 'ephysics/collision/shapes/AABB.cpp', + 'ephysics/collision/TriangleMesh.cpp', + 'ephysics/collision/CollisionDetection.cpp', + 'ephysics/collision/TriangleVertexArray.cpp', + 'ephysics/collision/ContactManifold.cpp', + 'ephysics/collision/broadphase/DynamicAABBTree.cpp', + 'ephysics/collision/broadphase/BroadPhaseAlgorithm.cpp', + 'ephysics/body/RigidBody.cpp', + 'ephysics/body/Body.cpp', + 'ephysics/body/CollisionBody.cpp', + 'ephysics/mathematics/Vector2.cpp', + 'ephysics/mathematics/Vector3.cpp', + 'ephysics/mathematics/Transform.cpp', + 'ephysics/mathematics/Matrix2x2.cpp', + 'ephysics/mathematics/mathematics_functions.cpp', + 'ephysics/mathematics/Matrix3x3.cpp', + 'ephysics/mathematics/Quaternion.cpp', + 'ephysics/engine/CollisionWorld.cpp', + 'ephysics/engine/OverlappingPair.cpp', + 'ephysics/engine/Material.cpp', + 'ephysics/engine/Island.cpp', + 'ephysics/engine/Profiler.cpp', + 'ephysics/engine/ConstraintSolver.cpp', + 'ephysics/engine/DynamicsWorld.cpp', + 'ephysics/engine/ContactSolver.cpp', + 'ephysics/engine/Timer.cpp', + ]) my_module.add_header_file([ - 'ephysics/memory/MemoryAllocator.h', - 'ephysics/memory/Stack.h', - 'ephysics/decimal.h', - 'ephysics/constraint/BallAndSocketJoint.h', - 'ephysics/constraint/Joint.h', - 'ephysics/constraint/FixedJoint.h', - 'ephysics/constraint/HingeJoint.h', - 'ephysics/constraint/ContactPoint.h', - 'ephysics/constraint/SliderJoint.h', - 'ephysics/collision/TriangleVertexArray.h', - 'ephysics/collision/ContactManifold.h', - 'ephysics/collision/ContactManifoldSet.h', - 'ephysics/collision/narrowphase/SphereVsSphereAlgorithm.h', - 'ephysics/collision/narrowphase/GJK/Simplex.h', - 'ephysics/collision/narrowphase/GJK/GJKAlgorithm.h', - 'ephysics/collision/narrowphase/ConcaveVsConvexAlgorithm.h', - 'ephysics/collision/narrowphase/CollisionDispatch.h', - 'ephysics/collision/narrowphase/DefaultCollisionDispatch.h', - 'ephysics/collision/narrowphase/NarrowPhaseAlgorithm.h', - 'ephysics/collision/narrowphase/EPA/EdgeEPA.h', - 'ephysics/collision/narrowphase/EPA/EPAAlgorithm.h', - 'ephysics/collision/narrowphase/EPA/TrianglesStore.h', - 'ephysics/collision/narrowphase/EPA/TriangleEPA.h', - 'ephysics/collision/CollisionDetection.h', - 'ephysics/collision/shapes/TriangleShape.h', - 'ephysics/collision/shapes/AABB.h', - 'ephysics/collision/shapes/CapsuleShape.h', - 'ephysics/collision/shapes/SphereShape.h', - 'ephysics/collision/shapes/CollisionShape.h', - 'ephysics/collision/shapes/BoxShape.h', - 'ephysics/collision/shapes/ConcaveMeshShape.h', - 'ephysics/collision/shapes/ConvexMeshShape.h', - 'ephysics/collision/shapes/HeightFieldShape.h', - 'ephysics/collision/shapes/CylinderShape.h', - 'ephysics/collision/shapes/ConeShape.h', - 'ephysics/collision/shapes/ConvexShape.h', - 'ephysics/collision/shapes/ConcaveShape.h', - 'ephysics/collision/CollisionShapeInfo.h', - 'ephysics/collision/TriangleMesh.h', - 'ephysics/collision/RaycastInfo.h', - 'ephysics/collision/ProxyShape.h', - 'ephysics/collision/broadphase/DynamicAABBTree.h', - 'ephysics/collision/broadphase/BroadPhaseAlgorithm.h', - 'ephysics/configuration.h', - 'ephysics/reactphysics3d.h', - 'ephysics/body/Body.h', - 'ephysics/body/RigidBody.h', - 'ephysics/body/CollisionBody.h', - 'ephysics/mathematics/mathematics.h', - 'ephysics/mathematics/Ray.h', - 'ephysics/mathematics/Matrix2x2.h', - 'ephysics/mathematics/Vector2.h', - 'ephysics/mathematics/mathematics_functions.h', - 'ephysics/mathematics/Vector3.h', - 'ephysics/mathematics/Quaternion.h', - 'ephysics/mathematics/Transform.h', - 'ephysics/mathematics/Matrix3x3.h', - 'ephysics/engine/CollisionWorld.h', - 'ephysics/engine/DynamicsWorld.h', - 'ephysics/engine/ConstraintSolver.h', - 'ephysics/engine/OverlappingPair.h', - 'ephysics/engine/Island.h', - 'ephysics/engine/ContactSolver.h', - 'ephysics/engine/Material.h', - 'ephysics/engine/Profiler.h', - 'ephysics/engine/Timer.h', - 'ephysics/engine/Impulse.h', - 'ephysics/engine/EventListener.h' - ]) + 'ephysics/memory/MemoryAllocator.h', + 'ephysics/memory/Stack.h', + 'ephysics/constraint/BallAndSocketJoint.h', + 'ephysics/constraint/Joint.h', + 'ephysics/constraint/FixedJoint.h', + 'ephysics/constraint/HingeJoint.h', + 'ephysics/constraint/ContactPoint.h', + 'ephysics/constraint/SliderJoint.h', + 'ephysics/collision/TriangleVertexArray.h', + 'ephysics/collision/ContactManifold.h', + 'ephysics/collision/ContactManifoldSet.h', + 'ephysics/collision/narrowphase/SphereVsSphereAlgorithm.h', + 'ephysics/collision/narrowphase/GJK/Simplex.h', + 'ephysics/collision/narrowphase/GJK/GJKAlgorithm.h', + 'ephysics/collision/narrowphase/ConcaveVsConvexAlgorithm.h', + 'ephysics/collision/narrowphase/CollisionDispatch.h', + 'ephysics/collision/narrowphase/DefaultCollisionDispatch.h', + 'ephysics/collision/narrowphase/NarrowPhaseAlgorithm.h', + 'ephysics/collision/narrowphase/EPA/EdgeEPA.h', + 'ephysics/collision/narrowphase/EPA/EPAAlgorithm.h', + 'ephysics/collision/narrowphase/EPA/TrianglesStore.h', + 'ephysics/collision/narrowphase/EPA/TriangleEPA.h', + 'ephysics/collision/CollisionDetection.h', + 'ephysics/collision/shapes/TriangleShape.h', + 'ephysics/collision/shapes/AABB.h', + 'ephysics/collision/shapes/CapsuleShape.h', + 'ephysics/collision/shapes/SphereShape.h', + 'ephysics/collision/shapes/CollisionShape.h', + 'ephysics/collision/shapes/BoxShape.h', + 'ephysics/collision/shapes/ConcaveMeshShape.h', + 'ephysics/collision/shapes/ConvexMeshShape.h', + 'ephysics/collision/shapes/HeightFieldShape.h', + 'ephysics/collision/shapes/CylinderShape.h', + 'ephysics/collision/shapes/ConeShape.h', + 'ephysics/collision/shapes/ConvexShape.h', + 'ephysics/collision/shapes/ConcaveShape.h', + 'ephysics/collision/CollisionShapeInfo.h', + 'ephysics/collision/TriangleMesh.h', + 'ephysics/collision/RaycastInfo.h', + 'ephysics/collision/ProxyShape.h', + 'ephysics/collision/broadphase/DynamicAABBTree.h', + 'ephysics/collision/broadphase/BroadPhaseAlgorithm.h', + 'ephysics/configuration.h', + 'ephysics/reactphysics3d.h', + 'ephysics/body/Body.h', + 'ephysics/body/RigidBody.h', + 'ephysics/body/CollisionBody.h', + 'ephysics/mathematics/mathematics.h', + 'ephysics/mathematics/Ray.h', + 'ephysics/mathematics/Matrix2x2.h', + 'ephysics/mathematics/Vector2.h', + 'ephysics/mathematics/mathematics_functions.h', + 'ephysics/mathematics/Vector3.h', + 'ephysics/mathematics/Quaternion.h', + 'ephysics/mathematics/Transform.h', + 'ephysics/mathematics/Matrix3x3.h', + 'ephysics/engine/CollisionWorld.h', + 'ephysics/engine/DynamicsWorld.h', + 'ephysics/engine/ConstraintSolver.h', + 'ephysics/engine/OverlappingPair.h', + 'ephysics/engine/Island.h', + 'ephysics/engine/ContactSolver.h', + 'ephysics/engine/Material.h', + 'ephysics/engine/Profiler.h', + 'ephysics/engine/Timer.h', + 'ephysics/engine/Impulse.h', + 'ephysics/engine/EventListener.h' + ]) # build in C++ mode my_module.compile_version("c++", 2011) # add dependency of the generic C++ library: my_module.add_depend([ - 'cxx', - 'm', - 'elog', - 'etk', - 'ememory' - ]) + 'cxx', + 'm', + 'elog', + 'etk', + 'ememory' + ]) # TODO: Remove this ... my_module.add_flag('c++', "-Wno-overloaded-virtual", export=True) my_module.add_path(".") diff --git a/test/Test.cpp b/test/Test.cpp index 62cf92d..9296c50 100644 --- a/test/Test.cpp +++ b/test/Test.cpp @@ -1,26 +1,26 @@ /******************************************************************************** -* ReactPhysics3D physics library, http://www.reactphysics3d.com * -* Copyright (c) 2010-2016 Daniel Chappuis * +* 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: * -* * +* 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. * -* * +* 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 @@ -32,7 +32,7 @@ using namespace reactphysics3d; /// Constructor Test::Test(const std::string& name, std::ostream* stream) - : mName(name), mNbPassedTests(0), mNbFailedTests(0), mOutputStream(stream) { + : mName(name), mNbPassedTests(0), mNbFailedTests(0), mOutputStream(stream) { } @@ -44,47 +44,47 @@ Test::~Test() { // Called to test a boolean condition. // This method should not be called directly in your test but you should call test() instead (macro) void Test::applyTest(bool condition, const std::string& testText, - const char* filename, long lineNumber) { + const char* filename, long lineNumber) { - // If the boolean condition is true - if (condition) { + // If the boolean condition is true + if (condition) { - // The test passed, call the succeed() method - succeed(); - } - else { // If the boolean condition is false + // The test passed, call the succeed() method + succeed(); + } + else { // If the boolean condition is false - // The test failed, call the applyFail() method - applyFail(testText, filename, lineNumber); - } + // The test failed, call the applyFail() method + applyFail(testText, filename, lineNumber); + } } // Called when a test has failed. // This method should not be called directly in your test buy you should call fail() instead (macro) void Test::applyFail(const std::string& testText, const char* filename, long lineNumber) { - if (mOutputStream) { + if (mOutputStream) { - // Display the failure message - *mOutputStream << mName << " failure : (" << testText << "), " << - filename << "(line " << lineNumber << ")" << std::endl; - } + // Display the failure message + *mOutputStream << mName << " failure : (" << testText << "), " << + filename << "(line " << lineNumber << ")" << std::endl; + } - // Increase the number of failed tests - mNbFailedTests++; + // Increase the number of failed tests + mNbFailedTests++; } /// Display the report of the unit test and return the number of failed tests long Test::report() const { - if(mOutputStream) { - *mOutputStream << std::left << std::setw(30) << std::setfill(' ') - << "Test " + mName + " :" << std::setw(10) << "Passed: " - << std::setw(8) << mNbPassedTests - << std::setw(13) << "Failed: " - << std::setw(8) << mNbFailedTests << std::endl; - } + if(mOutputStream) { + *mOutputStream << std::left << std::setw(30) << std::setfill(' ') + << "Test " + mName + " :" << std::setw(10) << "Passed: " + << std::setw(8) << mNbPassedTests + << std::setw(13) << "Failed: " + << std::setw(8) << mNbFailedTests << std::endl; + } - // Return the number of failed tests - return mNbFailedTests; + // Return the number of failed tests + return mNbFailedTests; } diff --git a/test/Test.h b/test/Test.h index 37fa9de..364f821 100644 --- a/test/Test.h +++ b/test/Test.h @@ -1,26 +1,26 @@ /******************************************************************************** -* ReactPhysics3D physics library, http://www.reactphysics3d.com * -* Copyright (c) 2010-2016 Daniel Chappuis * +* 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: * -* * +* 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. * -* * +* 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_H @@ -46,109 +46,109 @@ namespace reactphysics3d { */ class Test { - private : + private : - // ---------- Attributes ---------- // + // ---------- Attributes ---------- // - /// Name of the test - std::string mName; + /// Name of the test + std::string mName; - /// Number of tests that passed - long mNbPassedTests; + /// Number of tests that passed + long mNbPassedTests; - /// Number of tests that failed - long mNbFailedTests; + /// Number of tests that failed + long mNbFailedTests; - /// Output stream - std::ostream* mOutputStream; + /// Output stream + std::ostream* mOutputStream; - // ---------- Methods ---------- // + // ---------- Methods ---------- // - /// Copy constructor is private - Test(const Test&); + /// Copy constructor is private + Test(const Test&); - /// Assignment operator is private - Test& operator=(const Test& test); + /// Assignment operator is private + Test& operator=(const Test& test); - protected : + protected : - // ---------- Methods ---------- // + // ---------- Methods ---------- // - /// Called to test a boolean condition. - /// This method should not be called directly in your test but you should - /// call test() instead (macro) - void applyTest(bool condition, const std::string& testText, - const char* filename, long lineNumber); + /// Called to test a boolean condition. + /// This method should not be called directly in your test but you should + /// call test() instead (macro) + void applyTest(bool condition, const std::string& testText, + const char* filename, long lineNumber); - /// Called when a test has failed. - /// This method should not be called directly in your test buy you should - /// call fail() instead (macro) - void applyFail(const std::string& testText, const char* filename, long lineNumber); + /// Called when a test has failed. + /// This method should not be called directly in your test buy you should + /// call fail() instead (macro) + void applyFail(const std::string& testText, const char* filename, long lineNumber); - public : + public : - // ---------- Methods ---------- // + // ---------- Methods ---------- // - /// Constructor - Test(const std::string& name, std::ostream* stream = &std::cout); + /// Constructor + Test(const std::string& name, std::ostream* stream = &std::cout); - /// Destructor - virtual ~Test(); + /// Destructor + virtual ~Test(); - /// Return the number of passed tests - long getNbPassedTests() const; + /// Return the number of passed tests + long getNbPassedTests() const; - /// Return the number of failed tests - long getNbFailedTests() const; + /// Return the number of failed tests + long getNbFailedTests() const; - /// Return the output stream - const std::ostream* getOutputStream() const; + /// Return the output stream + const std::ostream* getOutputStream() const; - /// Set the output stream - void setOutputStream(std::ostream *stream); + /// Set the output stream + void setOutputStream(std::ostream *stream); - /// Run the unit test - virtual void run() = 0; + /// Run the unit test + virtual void run() = 0; - /// Called when a test passed - void succeed(); + /// Called when a test passed + void succeed(); - /// Reset the unit test - virtual void reset(); + /// Reset the unit test + virtual void reset(); - /// Display the report of the unit test and return the number of failed tests - long report() const; + /// Display the report of the unit test and return the number of failed tests + long report() const; }; // Called when a test passed inline void Test::succeed() { - mNbPassedTests++; + mNbPassedTests++; } // Reset the unit test inline void Test::reset() { - mNbPassedTests = 0; - mNbFailedTests = 0; + mNbPassedTests = 0; + mNbFailedTests = 0; } // Return the number of passed tests inline long Test::getNbPassedTests() const { - return mNbPassedTests; + return mNbPassedTests; } // Return the number of failed tests inline long Test::getNbFailedTests() const { - return mNbFailedTests; + return mNbFailedTests; } // Return the output stream inline const std::ostream* Test::getOutputStream() const { - return mOutputStream; + return mOutputStream; } // Set the output stream inline void Test::setOutputStream(std::ostream* stream) { - mOutputStream = stream; + mOutputStream = stream; } } diff --git a/test/TestSuite.cpp b/test/TestSuite.cpp index 2e4d2c7..20a254e 100644 --- a/test/TestSuite.cpp +++ b/test/TestSuite.cpp @@ -1,26 +1,26 @@ /******************************************************************************** -* ReactPhysics3D physics library, http://www.reactphysics3d.com * -* Copyright (c) 2010-2016 Daniel Chappuis * +* 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: * -* * +* 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. * -* * +* 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. * +* * ********************************************************************************/ // Librairies @@ -30,115 +30,115 @@ using namespace reactphysics3d; // Constructor TestSuite::TestSuite(const std::string& name, std::ostream* outputStream) - : mName(name), mOutputStream(outputStream) { + : mName(name), mOutputStream(outputStream) { } // Return the number of passed tests long TestSuite::getNbPassedTests() const { - long nbPassedTests = 0; + long nbPassedTests = 0; - for (size_t i=0; igetNbPassedTests(); - } + for (size_t i=0; igetNbPassedTests(); + } - return nbPassedTests; + return nbPassedTests; } // Return the number of failed tests long TestSuite::getNbFailedTests() const { - long nbFailedTests = 0; + long nbFailedTests = 0; - for (size_t i=0; igetNbFailedTests(); - } + for (size_t i=0; igetNbFailedTests(); + } - return nbFailedTests; + return nbFailedTests; } // Add a unit test in the test suite void TestSuite::addTest(Test* test) { - if (test == NULL) { - throw std::invalid_argument("Error : You cannot add a NULL test in the test suite."); - } - else if (mOutputStream != NULL && test->getOutputStream() == NULL) { - test->setOutputStream(mOutputStream); - } + if (test == NULL) { + throw std::invalid_argument("Error : You cannot add a NULL test in the test suite."); + } + else if (mOutputStream != NULL && test->getOutputStream() == NULL) { + test->setOutputStream(mOutputStream); + } - // Add the test to the suite - mTests.push_back(test); + // Add the test to the suite + mTests.push_back(test); - // Reset the added test - test->reset(); + // Reset the added test + test->reset(); } // Add a test suite to the current test suite void TestSuite::addTestSuite(const TestSuite& testSuite) { - // Add each test of the test suite to the current one - for (size_t i =0; i < testSuite.mTests.size(); i++) { - assert(testSuite.mTests[i] != NULL); - addTest(testSuite.mTests[i]); - } + // Add each test of the test suite to the current one + for (size_t i =0; i < testSuite.mTests.size(); i++) { + assert(testSuite.mTests[i] != NULL); + addTest(testSuite.mTests[i]); + } } // Launch the tests of the test suite void TestSuite::run() { - // Reset all the tests - reset(); + // Reset all the tests + reset(); - // Run all the tests - for (size_t i=0; i < mTests.size(); i++) { - assert(mTests[i] != NULL); - mTests[i]->run(); - } + // Run all the tests + for (size_t i=0; i < mTests.size(); i++) { + assert(mTests[i] != NULL); + mTests[i]->run(); + } } // Reset the test suite void TestSuite::reset() { - for(size_t i=0; i < mTests.size(); ++i) { - assert(mTests[i]); - mTests[i]->reset(); - } + for(size_t i=0; i < mTests.size(); ++i) { + assert(mTests[i]); + mTests[i]->reset(); + } } // Display the tests report and return the number of failed tests long TestSuite::report() const { - if (mOutputStream != NULL) { - long nbFailedTests = 0; + if (mOutputStream != NULL) { + long nbFailedTests = 0; - *mOutputStream << "Test Suite \"" << mName << "\"\n"; - size_t i; - for (i=0; i < 70; i++) { - *mOutputStream << "="; - } - *mOutputStream << "=" << std::endl; - for (i=0; i < mTests.size(); i++) { - assert(mTests[i] != NULL); - nbFailedTests += mTests[i]->report(); - } - for (i=0; i < 70; i++) { - *mOutputStream << "="; - } - *mOutputStream << "=" << std::endl; + *mOutputStream << "Test Suite \"" << mName << "\"\n"; + size_t i; + for (i=0; i < 70; i++) { + *mOutputStream << "="; + } + *mOutputStream << "=" << std::endl; + for (i=0; i < mTests.size(); i++) { + assert(mTests[i] != NULL); + nbFailedTests += mTests[i]->report(); + } + for (i=0; i < 70; i++) { + *mOutputStream << "="; + } + *mOutputStream << "=" << std::endl; - // Return the number of failed tests - return nbFailedTests; - } - else { - return getNbFailedTests(); - } + // Return the number of failed tests + return nbFailedTests; + } + else { + return getNbFailedTests(); + } } // Delete all the tests void TestSuite::clear() { - for (size_t i=0; i mTests; + /// All the tests of the test suite + std::vector mTests; - // ---------- Methods ---------- // + // ---------- Methods ---------- // - /// Reset the test suite - void reset(); + /// Reset the test suite + void reset(); - /// Private copy-constructor - TestSuite(const TestSuite& testSuite); + /// Private copy-constructor + TestSuite(const TestSuite& testSuite); - /// Private assigmnent operator - TestSuite& operator=(const TestSuite testSuite); + /// Private assigmnent operator + TestSuite& operator=(const TestSuite testSuite); public : - // ---------- Methods ---------- // + // ---------- Methods ---------- // - /// Constructor - TestSuite(const std::string& name, std::ostream* outputStream = &std::cout); + /// Constructor + TestSuite(const std::string& name, std::ostream* outputStream = &std::cout); - /// Return the name of the test suite - std::string getName() const; + /// Return the name of the test suite + std::string getName() const; - /// Return the number of passed tests - long getNbPassedTests() const; + /// Return the number of passed tests + long getNbPassedTests() const; - /// Return the number of failed tests - long getNbFailedTests() const; + /// Return the number of failed tests + long getNbFailedTests() const; - /// Return the output stream - const std::ostream* getOutputStream() const; + /// Return the output stream + const std::ostream* getOutputStream() const; - /// Set the output stream - void setOutputStream(std::ostream* outputStream); + /// Set the output stream + void setOutputStream(std::ostream* outputStream); - /// Add a unit test in the test suite - void addTest(Test* test); + /// Add a unit test in the test suite + void addTest(Test* test); - /// Add a test suite to the current test suite - void addTestSuite(const TestSuite& testSuite); + /// Add a test suite to the current test suite + void addTestSuite(const TestSuite& testSuite); - /// Launch the tests of the test suite - void run(); + /// Launch the tests of the test suite + void run(); - /// Display the tests report and return the number of failed tests - long report() const; + /// Display the tests report and return the number of failed tests + long report() const; - // Delete all the tests - void clear(); + // Delete all the tests + void clear(); }; // Return the name of the test suite inline std::string TestSuite::getName() const { - return mName; + return mName; } // Return the output stream inline const std::ostream* TestSuite::getOutputStream() const { - return mOutputStream; + return mOutputStream; } // Set the output stream inline void TestSuite::setOutputStream(std::ostream* outputStream) { - mOutputStream = outputStream; + mOutputStream = outputStream; } } diff --git a/test/main.cpp b/test/main.cpp index 6e4a7ec..5cfea4f 100644 --- a/test/main.cpp +++ b/test/main.cpp @@ -1,26 +1,26 @@ /******************************************************************************** -* ReactPhysics3D physics library, http://www.reactphysics3d.com * -* Copyright (c) 2010-2016 Daniel Chappuis * +* 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: * -* * +* 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. * -* * +* 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 @@ -40,36 +40,36 @@ using namespace reactphysics3d; -int main() { +int32_t main() { - TestSuite testSuite("ReactPhysics3D Tests"); + TestSuite testSuite("ReactPhysics3D Tests"); - // ---------- Mathematics tests ---------- // + // ---------- Mathematics tests ---------- // - testSuite.addTest(new TestVector2("Vector2")); - testSuite.addTest(new TestVector3("Vector3")); - testSuite.addTest(new TestTransform("Transform")); - testSuite.addTest(new TestQuaternion("Quaternion")); - testSuite.addTest(new TestMatrix3x3("Matrix3x3")); - testSuite.addTest(new TestMatrix2x2("Matrix2x2")); - testSuite.addTest(new TestMathematicsFunctions("Maths Functions")); + testSuite.addTest(new TestVector2("Vector2")); + testSuite.addTest(new TestVector3("Vector3")); + testSuite.addTest(new TestTransform("Transform")); + testSuite.addTest(new TestQuaternion("Quaternion")); + testSuite.addTest(new TestMatrix3x3("Matrix3x3")); + testSuite.addTest(new TestMatrix2x2("Matrix2x2")); + testSuite.addTest(new TestMathematicsFunctions("Maths Functions")); - // ---------- Collision Detection tests ---------- // + // ---------- Collision Detection tests ---------- // - testSuite.addTest(new TestAABB("AABB")); - testSuite.addTest(new TestPointInside("IsPointInside")); - testSuite.addTest(new TestRaycast("Raycasting")); - testSuite.addTest(new TestCollisionWorld("CollisionWorld")); - testSuite.addTest(new TestDynamicAABBTree("DynamicAABBTree")); + testSuite.addTest(new TestAABB("AABB")); + testSuite.addTest(new TestPointInside("IsPointInside")); + testSuite.addTest(new TestRaycast("Raycasting")); + testSuite.addTest(new TestCollisionWorld("CollisionWorld")); + testSuite.addTest(new TestDynamicAABBTree("DynamicAABBTree")); - // Run the tests - testSuite.run(); + // Run the tests + testSuite.run(); - // Display the report - long nbFailedTests = testSuite.report(); + // Display the report + long nbFailedTests = testSuite.report(); - // Clear the tests from the test suite - testSuite.clear(); + // Clear the tests from the test suite + testSuite.clear(); - return nbFailedTests; + return nbFailedTests; } diff --git a/test/tests/collision/TestAABB.h b/test/tests/collision/TestAABB.h index 17b631b..736d6bd 100644 --- a/test/tests/collision/TestAABB.h +++ b/test/tests/collision/TestAABB.h @@ -1,26 +1,26 @@ /******************************************************************************** -* ReactPhysics3D physics library, http://www.reactphysics3d.com * -* Copyright (c) 2010-2016 Daniel Chappuis * +* 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: * -* * +* 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. * -* * +* 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_AABB_H @@ -39,250 +39,250 @@ namespace reactphysics3d { */ class TestAABB : public Test { - private : + private : - // ---------- Atributes ---------- // + // ---------- Atributes ---------- // - AABB mAABB1; - AABB mAABB2; - AABB mAABB3; - AABB mAABB4; + AABB mAABB1; + AABB mAABB2; + AABB mAABB3; + AABB mAABB4; - public : + public : - // ---------- Methods ---------- // + // ---------- Methods ---------- // - /// Constructor - TestAABB(const std::string& name) : Test(name) { + /// Constructor + TestAABB(const std::string& name) : Test(name) { - mAABB1.setMin(Vector3(-10, -10, -10)); - mAABB1.setMax(Vector3(10, 10, 10)); + mAABB1.setMin(Vector3(-10, -10, -10)); + mAABB1.setMax(Vector3(10, 10, 10)); - // AABB2 intersect with AABB1 - mAABB2.setMin(Vector3(-5, 4, -30)); - mAABB2.setMax(Vector3(-2, 20, 30)); + // AABB2 int32_tersect with AABB1 + mAABB2.setMin(Vector3(-5, 4, -30)); + mAABB2.setMax(Vector3(-2, 20, 30)); - // AABB3 contains AABB1 - mAABB3.setMin(Vector3(-25, -25, -25)); - mAABB3.setMax(Vector3(25, 25, 25)); + // AABB3 contains AABB1 + mAABB3.setMin(Vector3(-25, -25, -25)); + mAABB3.setMax(Vector3(25, 25, 25)); - // AABB4 does not collide with AABB1 - mAABB4.setMin(Vector3(-40, -40, -40)); - mAABB4.setMax(Vector3(-15, -25, -12)); - } + // AABB4 does not collide with AABB1 + mAABB4.setMin(Vector3(-40, -40, -40)); + mAABB4.setMax(Vector3(-15, -25, -12)); + } - /// Destructor - ~TestAABB() { + /// Destructor + ~TestAABB() { - } + } - /// Run the tests - void run() { - testBasicMethods(); - testMergeMethods(); - testIntersection(); - } + /// Run the tests + void run() { + testBasicMethods(); + testMergeMethods(); + testIntersection(); + } - void testBasicMethods() { + void testBasicMethods() { - // -------- Test constructors -------- // - AABB aabb1; - AABB aabb2(Vector3(-3, -5, -8), Vector3(65, -1, 56)); - Vector3 trianglePoints[] = { - Vector3(-5, 7, 23), Vector3(45, -34, -73), Vector3(-12, 98, 76) - }; - AABB aabb3 = AABB::createAABBForTriangle(trianglePoints); + // -------- Test constructors -------- // + AABB aabb1; + AABB aabb2(Vector3(-3, -5, -8), Vector3(65, -1, 56)); + Vector3 trianglePoints[] = { + Vector3(-5, 7, 23), Vector3(45, -34, -73), Vector3(-12, 98, 76) + }; + AABB aabb3 = AABB::createAABBForTriangle(trianglePoints); - test(aabb1.getMin().x == 0); - test(aabb1.getMin().y == 0); - test(aabb1.getMin().z == 0); - test(aabb1.getMax().x == 0); - test(aabb1.getMax().y == 0); - test(aabb1.getMax().z == 0); + test(aabb1.getMin().x == 0); + test(aabb1.getMin().y == 0); + test(aabb1.getMin().z == 0); + test(aabb1.getMax().x == 0); + test(aabb1.getMax().y == 0); + test(aabb1.getMax().z == 0); - test(aabb2.getMin().x == -3); - test(aabb2.getMin().y == -5); - test(aabb2.getMin().z == -8); - test(aabb2.getMax().x == 65); - test(aabb2.getMax().y == -1); - test(aabb2.getMax().z == 56); + test(aabb2.getMin().x == -3); + test(aabb2.getMin().y == -5); + test(aabb2.getMin().z == -8); + test(aabb2.getMax().x == 65); + test(aabb2.getMax().y == -1); + test(aabb2.getMax().z == 56); - test(aabb3.getMin().x == -12); - test(aabb3.getMin().y == -34); - test(aabb3.getMin().z == -73); - test(aabb3.getMax().x == 45); - test(aabb3.getMax().y == 98); - test(aabb3.getMax().z == 76); + test(aabb3.getMin().x == -12); + test(aabb3.getMin().y == -34); + test(aabb3.getMin().z == -73); + test(aabb3.getMax().x == 45); + test(aabb3.getMax().y == 98); + test(aabb3.getMax().z == 76); - // -------- Test inflate() -------- // - AABB aabbInflate(Vector3(-3, 4, 8), Vector3(-1, 6, 32)); - aabbInflate.inflate(1, 2, 3); - test(approxEqual(aabbInflate.getMin().x, -4, 0.00001)); - test(approxEqual(aabbInflate.getMin().y, 2, 0.00001)); - test(approxEqual(aabbInflate.getMin().z, 5, 0.00001)); - test(approxEqual(aabbInflate.getMax().x, 0, 0.00001)); - test(approxEqual(aabbInflate.getMax().y, 8, 0.00001)); - test(approxEqual(aabbInflate.getMax().z, 35, 0.00001)); + // -------- Test inflate() -------- // + AABB aabbInflate(Vector3(-3, 4, 8), Vector3(-1, 6, 32)); + aabbInflate.inflate(1, 2, 3); + test(approxEqual(aabbInflate.getMin().x, -4, 0.00001)); + test(approxEqual(aabbInflate.getMin().y, 2, 0.00001)); + test(approxEqual(aabbInflate.getMin().z, 5, 0.00001)); + test(approxEqual(aabbInflate.getMax().x, 0, 0.00001)); + test(approxEqual(aabbInflate.getMax().y, 8, 0.00001)); + test(approxEqual(aabbInflate.getMax().z, 35, 0.00001)); - // -------- Test getExtent() --------- // + // -------- Test getExtent() --------- // - test(approxEqual(mAABB1.getExtent().x, 20)); - test(approxEqual(mAABB1.getExtent().y, 20)); - test(approxEqual(mAABB1.getExtent().z, 20)); + test(approxEqual(mAABB1.getExtent().x, 20)); + test(approxEqual(mAABB1.getExtent().y, 20)); + test(approxEqual(mAABB1.getExtent().z, 20)); - test(approxEqual(mAABB2.getExtent().x, 3)); - test(approxEqual(mAABB2.getExtent().y, 16)); - test(approxEqual(mAABB2.getExtent().z, 60)); + test(approxEqual(mAABB2.getExtent().x, 3)); + test(approxEqual(mAABB2.getExtent().y, 16)); + test(approxEqual(mAABB2.getExtent().z, 60)); - test(approxEqual(mAABB3.getExtent().x, 50)); - test(approxEqual(mAABB3.getExtent().y, 50)); - test(approxEqual(mAABB3.getExtent().z, 50)); + test(approxEqual(mAABB3.getExtent().x, 50)); + test(approxEqual(mAABB3.getExtent().y, 50)); + test(approxEqual(mAABB3.getExtent().z, 50)); - // -------- Test getCenter() -------- // + // -------- Test getCenter() -------- // - test(mAABB1.getCenter().x == 0); - test(mAABB1.getCenter().y == 0); - test(mAABB1.getCenter().z == 0); + test(mAABB1.getCenter().x == 0); + test(mAABB1.getCenter().y == 0); + test(mAABB1.getCenter().z == 0); - test(approxEqual(mAABB2.getCenter().x, -3.5)); - test(approxEqual(mAABB2.getCenter().y, 12)); - test(approxEqual(mAABB2.getCenter().z, 0)); + test(approxEqual(mAABB2.getCenter().x, -3.5)); + test(approxEqual(mAABB2.getCenter().y, 12)); + test(approxEqual(mAABB2.getCenter().z, 0)); - // -------- Test setMin(), setMax(), getMin(), getMax() -------- // + // -------- Test setMin(), setMax(), getMin(), getMax() -------- // - AABB aabb5; - aabb5.setMin(Vector3(-12, 34, 6)); - aabb5.setMax(Vector3(-3, 56, 20)); + AABB aabb5; + aabb5.setMin(Vector3(-12, 34, 6)); + aabb5.setMax(Vector3(-3, 56, 20)); - test(aabb5.getMin().x == -12); - test(aabb5.getMin().y == 34); - test(aabb5.getMin().z == 6); - test(aabb5.getMax().x == -3); - test(aabb5.getMax().y == 56); - test(aabb5.getMax().z == 20); + test(aabb5.getMin().x == -12); + test(aabb5.getMin().y == 34); + test(aabb5.getMin().z == 6); + test(aabb5.getMax().x == -3); + test(aabb5.getMax().y == 56); + test(aabb5.getMax().z == 20); - // -------- Test assignment operator -------- // + // -------- Test assignment operator -------- // - AABB aabb6; - aabb6 = aabb2; + AABB aabb6; + aabb6 = aabb2; - test(aabb6.getMin().x == -3); - test(aabb6.getMin().y == -5); - test(aabb6.getMin().z == -8); - test(aabb6.getMax().x == 65); - test(aabb6.getMax().y == -1); - test(aabb6.getMax().z == 56); + test(aabb6.getMin().x == -3); + test(aabb6.getMin().y == -5); + test(aabb6.getMin().z == -8); + test(aabb6.getMax().x == 65); + test(aabb6.getMax().y == -1); + test(aabb6.getMax().z == 56); - // -------- Test getVolume() -------- // + // -------- Test getVolume() -------- // - test(approxEqual(mAABB1.getVolume(), 8000)); - test(approxEqual(mAABB2.getVolume(), 2880)); - } + test(approxEqual(mAABB1.getVolume(), 8000)); + test(approxEqual(mAABB2.getVolume(), 2880)); + } - void testMergeMethods() { + void testMergeMethods() { - AABB aabb1(Vector3(-45, 7, -2), Vector3(23, 8, 1)); - AABB aabb2(Vector3(-15, 6, 23), Vector3(-5, 9, 45)); + AABB aabb1(Vector3(-45, 7, -2), Vector3(23, 8, 1)); + AABB aabb2(Vector3(-15, 6, 23), Vector3(-5, 9, 45)); - // -------- Test mergeTwoAABBs() -------- // + // -------- Test mergeTwoAABBs() -------- // - AABB aabb3; - aabb3.mergeTwoAABBs(aabb1, mAABB1); + AABB aabb3; + aabb3.mergeTwoAABBs(aabb1, mAABB1); - test(aabb3.getMin().x == -45); - test(aabb3.getMin().y == -10); - test(aabb3.getMin().z == -10); - test(aabb3.getMax().x == 23); - test(aabb3.getMax().y == 10); - test(aabb3.getMax().z == 10); + test(aabb3.getMin().x == -45); + test(aabb3.getMin().y == -10); + test(aabb3.getMin().z == -10); + test(aabb3.getMax().x == 23); + test(aabb3.getMax().y == 10); + test(aabb3.getMax().z == 10); - AABB aabb4; - aabb4.mergeTwoAABBs(aabb1, aabb2); + AABB aabb4; + aabb4.mergeTwoAABBs(aabb1, aabb2); - test(aabb4.getMin().x == -45); - test(aabb4.getMin().y == 6); - test(aabb4.getMin().z == -2); - test(aabb4.getMax().x == 23); - test(aabb4.getMax().y == 9); - test(aabb4.getMax().z == 45); + test(aabb4.getMin().x == -45); + test(aabb4.getMin().y == 6); + test(aabb4.getMin().z == -2); + test(aabb4.getMax().x == 23); + test(aabb4.getMax().y == 9); + test(aabb4.getMax().z == 45); - // -------- Test mergeWithAABB() -------- // + // -------- Test mergeWithAABB() -------- // - aabb1.mergeWithAABB(mAABB1); + aabb1.mergeWithAABB(mAABB1); - test(aabb1.getMin().x == -45); - test(aabb1.getMin().y == -10); - test(aabb1.getMin().z == -10); - test(aabb1.getMax().x == 23); - test(aabb1.getMax().y == 10); - test(aabb1.getMax().z == 10); + test(aabb1.getMin().x == -45); + test(aabb1.getMin().y == -10); + test(aabb1.getMin().z == -10); + test(aabb1.getMax().x == 23); + test(aabb1.getMax().y == 10); + test(aabb1.getMax().z == 10); - aabb2.mergeWithAABB(mAABB1); + aabb2.mergeWithAABB(mAABB1); - test(aabb2.getMin().x == -15); - test(aabb2.getMin().y == -10); - test(aabb2.getMin().z == -10); - test(aabb2.getMax().x == 10); - test(aabb2.getMax().y == 10); - test(aabb2.getMax().z == 45); - } + test(aabb2.getMin().x == -15); + test(aabb2.getMin().y == -10); + test(aabb2.getMin().z == -10); + test(aabb2.getMax().x == 10); + test(aabb2.getMax().y == 10); + test(aabb2.getMax().z == 45); + } - void testIntersection() { + void testIntersection() { - // -------- Test contains(AABB) -------- // - test(!mAABB1.contains(mAABB2)); - test(mAABB3.contains(mAABB1)); - test(!mAABB1.contains(mAABB3)); - test(!mAABB1.contains(mAABB4)); - test(!mAABB4.contains(mAABB1)); + // -------- Test contains(AABB) -------- // + test(!mAABB1.contains(mAABB2)); + test(mAABB3.contains(mAABB1)); + test(!mAABB1.contains(mAABB3)); + test(!mAABB1.contains(mAABB4)); + test(!mAABB4.contains(mAABB1)); - // -------- Test contains(Vector3) -------- // - test(mAABB1.contains(Vector3(0, 0, 0))); - test(mAABB1.contains(Vector3(-5, 6, 9))); - test(mAABB1.contains(Vector3(-9, -4, -9))); - test(mAABB1.contains(Vector3(9, 4, 7))); - test(!mAABB1.contains(Vector3(-11, -4, -9))); - test(!mAABB1.contains(Vector3(1, 12, -9))); - test(!mAABB1.contains(Vector3(1, 8, -13))); - test(!mAABB1.contains(Vector3(-14, 82, -13))); + // -------- Test contains(Vector3) -------- // + test(mAABB1.contains(Vector3(0, 0, 0))); + test(mAABB1.contains(Vector3(-5, 6, 9))); + test(mAABB1.contains(Vector3(-9, -4, -9))); + test(mAABB1.contains(Vector3(9, 4, 7))); + test(!mAABB1.contains(Vector3(-11, -4, -9))); + test(!mAABB1.contains(Vector3(1, 12, -9))); + test(!mAABB1.contains(Vector3(1, 8, -13))); + test(!mAABB1.contains(Vector3(-14, 82, -13))); - // -------- Test testCollision() -------- // - test(mAABB1.testCollision(mAABB2)); - test(mAABB2.testCollision(mAABB1)); - test(mAABB1.testCollision(mAABB3)); - test(mAABB3.testCollision(mAABB1)); - test(!mAABB1.testCollision(mAABB4)); - test(!mAABB4.testCollision(mAABB1)); + // -------- Test testCollision() -------- // + test(mAABB1.testCollision(mAABB2)); + test(mAABB2.testCollision(mAABB1)); + test(mAABB1.testCollision(mAABB3)); + test(mAABB3.testCollision(mAABB1)); + test(!mAABB1.testCollision(mAABB4)); + test(!mAABB4.testCollision(mAABB1)); - // -------- Test testCollisionTriangleAABB() -------- // + // -------- Test testCollisionTriangleAABB() -------- // - AABB aabb(Vector3(100, 100, 100), Vector3(200, 200, 200)); - Vector3 trianglePoints[] = { - Vector3(-2, 4, 6), Vector3(20, -34, -73), Vector3(-12, 98, 76) - }; - test(mAABB1.testCollisionTriangleAABB(trianglePoints)); - test(!aabb.testCollisionTriangleAABB(trianglePoints)); + AABB aabb(Vector3(100, 100, 100), Vector3(200, 200, 200)); + Vector3 trianglePoints[] = { + Vector3(-2, 4, 6), Vector3(20, -34, -73), Vector3(-12, 98, 76) + }; + test(mAABB1.testCollisionTriangleAABB(trianglePoints)); + test(!aabb.testCollisionTriangleAABB(trianglePoints)); - // -------- Test testRayIntersect() -------- // + // -------- Test testRayIntersect() -------- // - Ray ray1(Vector3(-20, 4, -7), Vector3(20, 4, -7)); - Ray ray2(Vector3(-20, 11, -7), Vector3(20, 11, -7)); - Ray ray3(Vector3(0, 15, 0), Vector3(0, -15, 0)); - Ray ray4(Vector3(0, -15, 0), Vector3(0, 15, 0)); - Ray ray5(Vector3(-3, 4, 8), Vector3(-7, 9, 4)); - Ray ray6(Vector3(-4, 6, -100), Vector3(-4, 6, -9)); - Ray ray7(Vector3(-4, 6, -100), Vector3(-4, 6, -11), 0.6); - Ray ray8(Vector3(-403, -432, -100), Vector3(134, 643, 23)); + Ray ray1(Vector3(-20, 4, -7), Vector3(20, 4, -7)); + Ray ray2(Vector3(-20, 11, -7), Vector3(20, 11, -7)); + Ray ray3(Vector3(0, 15, 0), Vector3(0, -15, 0)); + Ray ray4(Vector3(0, -15, 0), Vector3(0, 15, 0)); + Ray ray5(Vector3(-3, 4, 8), Vector3(-7, 9, 4)); + Ray ray6(Vector3(-4, 6, -100), Vector3(-4, 6, -9)); + Ray ray7(Vector3(-4, 6, -100), Vector3(-4, 6, -11), 0.6); + Ray ray8(Vector3(-403, -432, -100), Vector3(134, 643, 23)); - test(mAABB1.testRayIntersect(ray1)); - test(!mAABB1.testRayIntersect(ray2)); - test(mAABB1.testRayIntersect(ray3)); - test(mAABB1.testRayIntersect(ray4)); - test(mAABB1.testRayIntersect(ray5)); - test(mAABB1.testRayIntersect(ray6)); - test(!mAABB1.testRayIntersect(ray7)); - test(!mAABB1.testRayIntersect(ray8)); - } + test(mAABB1.testRayIntersect(ray1)); + test(!mAABB1.testRayIntersect(ray2)); + test(mAABB1.testRayIntersect(ray3)); + test(mAABB1.testRayIntersect(ray4)); + test(mAABB1.testRayIntersect(ray5)); + test(mAABB1.testRayIntersect(ray6)); + test(!mAABB1.testRayIntersect(ray7)); + test(!mAABB1.testRayIntersect(ray8)); + } }; } diff --git a/test/tests/collision/TestCollisionWorld.h b/test/tests/collision/TestCollisionWorld.h index a12af15..9ab53fc 100644 --- a/test/tests/collision/TestCollisionWorld.h +++ b/test/tests/collision/TestCollisionWorld.h @@ -1,26 +1,26 @@ /******************************************************************************** -* ReactPhysics3D physics library, http://www.reactphysics3d.com * -* Copyright (c) 2010-2016 Daniel Chappuis * +* 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: * -* * +* 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. * -* * +* 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_COLLISION_WORLD_H @@ -34,9 +34,9 @@ namespace reactphysics3d { // Enumeration for categories enum CollisionCategory { - CATEGORY_1 = 0x0001, - CATEGORY_2 = 0x0002, - CATEGORY_3 = 0x0004 + CATEGORY_1 = 0x0001, + CATEGORY_2 = 0x0002, + CATEGORY_3 = 0x0004 }; // TODO : Add test for concave shape collision here @@ -44,55 +44,55 @@ enum CollisionCategory { // Class class WorldCollisionCallback : public CollisionCallback { - public: + public: - bool boxCollideWithSphere1; - bool boxCollideWithCylinder; - bool sphere1CollideWithCylinder; - bool sphere1CollideWithSphere2; + bool boxCollideWithSphere1; + bool boxCollideWithCylinder; + bool sphere1CollideWithCylinder; + bool sphere1CollideWithSphere2; - CollisionBody* boxBody; - CollisionBody* sphere1Body; - CollisionBody* sphere2Body; - CollisionBody* cylinderBody; + CollisionBody* boxBody; + CollisionBody* sphere1Body; + CollisionBody* sphere2Body; + CollisionBody* cylinderBody; - WorldCollisionCallback() - { - reset(); - } + WorldCollisionCallback() + { + reset(); + } - void reset() - { - boxCollideWithSphere1 = false; - boxCollideWithCylinder = false; - sphere1CollideWithCylinder = false; - sphere1CollideWithSphere2 = false; - } + void reset() + { + boxCollideWithSphere1 = false; + boxCollideWithCylinder = false; + sphere1CollideWithCylinder = false; + sphere1CollideWithSphere2 = false; + } - // This method will be called for contact - virtual void notifyContact(const ContactPointInfo& contactPointInfo) { + // This method will be called for contact + virtual void notifyContact(const ContactPointInfo& contactPointInfo) { - if (isContactBetweenBodies(boxBody, sphere1Body, contactPointInfo)) { - boxCollideWithSphere1 = true; - } - else if (isContactBetweenBodies(boxBody, cylinderBody, contactPointInfo)) { - boxCollideWithCylinder = true; - } - else if (isContactBetweenBodies(sphere1Body, cylinderBody, contactPointInfo)) { - sphere1CollideWithCylinder = true; - } - else if (isContactBetweenBodies(sphere1Body, sphere2Body, contactPointInfo)) { - sphere1CollideWithSphere2 = true; - } - } + if (isContactBetweenBodies(boxBody, sphere1Body, contactPointInfo)) { + boxCollideWithSphere1 = true; + } + else if (isContactBetweenBodies(boxBody, cylinderBody, contactPointInfo)) { + boxCollideWithCylinder = true; + } + else if (isContactBetweenBodies(sphere1Body, cylinderBody, contactPointInfo)) { + sphere1CollideWithCylinder = true; + } + else if (isContactBetweenBodies(sphere1Body, sphere2Body, contactPointInfo)) { + sphere1CollideWithSphere2 = true; + } + } - bool isContactBetweenBodies(const CollisionBody* body1, const CollisionBody* body2, - const ContactPointInfo& contactPointInfo) { - return (contactPointInfo.shape1->getBody()->getID() == body1->getID() && - contactPointInfo.shape2->getBody()->getID() == body2->getID()) || - (contactPointInfo.shape2->getBody()->getID() == body1->getID() && - contactPointInfo.shape1->getBody()->getID() == body2->getID()); - } + bool isContactBetweenBodies(const CollisionBody* body1, const CollisionBody* body2, + const ContactPointInfo& contactPointInfo) { + return (contactPointInfo.shape1->getBody()->getID() == body1->getID() && + contactPointInfo.shape2->getBody()->getID() == body2->getID()) || + (contactPointInfo.shape2->getBody()->getID() == body1->getID() && + contactPointInfo.shape1->getBody()->getID() == body2->getID()); + } }; // Class TestCollisionWorld @@ -101,241 +101,241 @@ class WorldCollisionCallback : public CollisionCallback */ class TestCollisionWorld : public Test { - private : + private : - // ---------- Atributes ---------- // + // ---------- Atributes ---------- // - // Physics world - CollisionWorld* mWorld; + // Physics world + CollisionWorld* mWorld; - // Bodies - CollisionBody* mBoxBody; - CollisionBody* mSphere1Body; - CollisionBody* mSphere2Body; - CollisionBody* mCylinderBody; + // Bodies + CollisionBody* mBoxBody; + CollisionBody* mSphere1Body; + CollisionBody* mSphere2Body; + CollisionBody* mCylinderBody; - // Collision shapes - BoxShape* mBoxShape; - SphereShape* mSphereShape; - CylinderShape* mCylinderShape; + // Collision shapes + BoxShape* mBoxShape; + SphereShape* mSphereShape; + CylinderShape* mCylinderShape; - // Proxy shapes - ProxyShape* mBoxProxyShape; - ProxyShape* mSphere1ProxyShape; - ProxyShape* mSphere2ProxyShape; - ProxyShape* mCylinderProxyShape; + // Proxy shapes + ProxyShape* mBoxProxyShape; + ProxyShape* mSphere1ProxyShape; + ProxyShape* mSphere2ProxyShape; + ProxyShape* mCylinderProxyShape; - // Collision callback class - WorldCollisionCallback mCollisionCallback; + // Collision callback class + WorldCollisionCallback mCollisionCallback; - public : + public : - // ---------- Methods ---------- // + // ---------- Methods ---------- // - /// Constructor - TestCollisionWorld(const std::string& name) : Test(name) { + /// Constructor + TestCollisionWorld(const std::string& name) : Test(name) { - // Create the world - mWorld = new CollisionWorld(); + // Create the world + mWorld = new CollisionWorld(); - // Create the bodies - Transform boxTransform(Vector3(10, 0, 0), Quaternion::identity()); - mBoxBody = mWorld->createCollisionBody(boxTransform); - mBoxShape = new BoxShape(Vector3(3, 3, 3)); - mBoxProxyShape = mBoxBody->addCollisionShape(mBoxShape, Transform::identity()); + // Create the bodies + Transform boxTransform(Vector3(10, 0, 0), Quaternion::identity()); + mBoxBody = mWorld->createCollisionBody(boxTransform); + mBoxShape = new BoxShape(Vector3(3, 3, 3)); + mBoxProxyShape = mBoxBody->addCollisionShape(mBoxShape, Transform::identity()); - mSphereShape = new SphereShape(3.0); - Transform sphere1Transform(Vector3(10,5, 0), Quaternion::identity()); - mSphere1Body = mWorld->createCollisionBody(sphere1Transform); - mSphere1ProxyShape = mSphere1Body->addCollisionShape(mSphereShape, Transform::identity()); + mSphereShape = new SphereShape(3.0); + Transform sphere1Transform(Vector3(10,5, 0), Quaternion::identity()); + mSphere1Body = mWorld->createCollisionBody(sphere1Transform); + mSphere1ProxyShape = mSphere1Body->addCollisionShape(mSphereShape, Transform::identity()); - Transform sphere2Transform(Vector3(30, 10, 10), Quaternion::identity()); - mSphere2Body = mWorld->createCollisionBody(sphere2Transform); - mSphere2ProxyShape = mSphere2Body->addCollisionShape(mSphereShape, Transform::identity()); + Transform sphere2Transform(Vector3(30, 10, 10), Quaternion::identity()); + mSphere2Body = mWorld->createCollisionBody(sphere2Transform); + mSphere2ProxyShape = mSphere2Body->addCollisionShape(mSphereShape, Transform::identity()); - Transform cylinderTransform(Vector3(10, -5, 0), Quaternion::identity()); - mCylinderBody = mWorld->createCollisionBody(cylinderTransform); - mCylinderShape = new CylinderShape(2, 5); - mCylinderProxyShape = mCylinderBody->addCollisionShape(mCylinderShape, Transform::identity()); + Transform cylinderTransform(Vector3(10, -5, 0), Quaternion::identity()); + mCylinderBody = mWorld->createCollisionBody(cylinderTransform); + mCylinderShape = new CylinderShape(2, 5); + mCylinderProxyShape = mCylinderBody->addCollisionShape(mCylinderShape, Transform::identity()); - // Assign collision categories to proxy shapes - mBoxProxyShape->setCollisionCategoryBits(CATEGORY_1); - mSphere1ProxyShape->setCollisionCategoryBits(CATEGORY_1); - mSphere2ProxyShape->setCollisionCategoryBits(CATEGORY_2); - mCylinderProxyShape->setCollisionCategoryBits(CATEGORY_3); + // Assign collision categories to proxy shapes + mBoxProxyShape->setCollisionCategoryBits(CATEGORY_1); + mSphere1ProxyShape->setCollisionCategoryBits(CATEGORY_1); + mSphere2ProxyShape->setCollisionCategoryBits(CATEGORY_2); + mCylinderProxyShape->setCollisionCategoryBits(CATEGORY_3); - mCollisionCallback.boxBody = mBoxBody; - mCollisionCallback.sphere1Body = mSphere1Body; - mCollisionCallback.sphere2Body = mSphere2Body; - mCollisionCallback.cylinderBody = mCylinderBody; - } + mCollisionCallback.boxBody = mBoxBody; + mCollisionCallback.sphere1Body = mSphere1Body; + mCollisionCallback.sphere2Body = mSphere2Body; + mCollisionCallback.cylinderBody = mCylinderBody; + } - /// Destructor - ~TestCollisionWorld() { - delete mBoxShape; - delete mSphereShape; - delete mCylinderShape; - } + /// Destructor + ~TestCollisionWorld() { + delete mBoxShape; + delete mSphereShape; + delete mCylinderShape; + } - /// Run the tests - void run() { + /// Run the tests + void run() { - testCollisions(); - } + testCollisions(); + } - void testCollisions() { + void testCollisions() { - mCollisionCallback.reset(); - mWorld->testCollision(&mCollisionCallback); - test(mCollisionCallback.boxCollideWithSphere1); - test(mCollisionCallback.boxCollideWithCylinder); - test(!mCollisionCallback.sphere1CollideWithCylinder); - test(!mCollisionCallback.sphere1CollideWithSphere2); + mCollisionCallback.reset(); + mWorld->testCollision(&mCollisionCallback); + test(mCollisionCallback.boxCollideWithSphere1); + test(mCollisionCallback.boxCollideWithCylinder); + test(!mCollisionCallback.sphere1CollideWithCylinder); + test(!mCollisionCallback.sphere1CollideWithSphere2); - test(mWorld->testAABBOverlap(mBoxBody, mSphere1Body)); - test(mWorld->testAABBOverlap(mBoxBody, mCylinderBody)); - test(!mWorld->testAABBOverlap(mSphere1Body, mCylinderBody)); - test(!mWorld->testAABBOverlap(mSphere1Body, mSphere2Body)); + test(mWorld->testAABBOverlap(mBoxBody, mSphere1Body)); + test(mWorld->testAABBOverlap(mBoxBody, mCylinderBody)); + test(!mWorld->testAABBOverlap(mSphere1Body, mCylinderBody)); + test(!mWorld->testAABBOverlap(mSphere1Body, mSphere2Body)); - test(mWorld->testAABBOverlap(mBoxProxyShape, mSphere1ProxyShape)); - test(mWorld->testAABBOverlap(mBoxProxyShape, mCylinderProxyShape)); - test(!mWorld->testAABBOverlap(mSphere1ProxyShape, mCylinderProxyShape)); - test(!mWorld->testAABBOverlap(mSphere1ProxyShape, mSphere2ProxyShape)); + test(mWorld->testAABBOverlap(mBoxProxyShape, mSphere1ProxyShape)); + test(mWorld->testAABBOverlap(mBoxProxyShape, mCylinderProxyShape)); + test(!mWorld->testAABBOverlap(mSphere1ProxyShape, mCylinderProxyShape)); + test(!mWorld->testAABBOverlap(mSphere1ProxyShape, mSphere2ProxyShape)); - mCollisionCallback.reset(); - mWorld->testCollision(mCylinderBody, &mCollisionCallback); - test(!mCollisionCallback.boxCollideWithSphere1); - test(mCollisionCallback.boxCollideWithCylinder); - test(!mCollisionCallback.sphere1CollideWithCylinder); - test(!mCollisionCallback.sphere1CollideWithSphere2); + mCollisionCallback.reset(); + mWorld->testCollision(mCylinderBody, &mCollisionCallback); + test(!mCollisionCallback.boxCollideWithSphere1); + test(mCollisionCallback.boxCollideWithCylinder); + test(!mCollisionCallback.sphere1CollideWithCylinder); + test(!mCollisionCallback.sphere1CollideWithSphere2); - mCollisionCallback.reset(); - mWorld->testCollision(mBoxBody, mSphere1Body, &mCollisionCallback); - test(mCollisionCallback.boxCollideWithSphere1); - test(!mCollisionCallback.boxCollideWithCylinder); - test(!mCollisionCallback.sphere1CollideWithCylinder); - test(!mCollisionCallback.sphere1CollideWithSphere2); + mCollisionCallback.reset(); + mWorld->testCollision(mBoxBody, mSphere1Body, &mCollisionCallback); + test(mCollisionCallback.boxCollideWithSphere1); + test(!mCollisionCallback.boxCollideWithCylinder); + test(!mCollisionCallback.sphere1CollideWithCylinder); + test(!mCollisionCallback.sphere1CollideWithSphere2); - mCollisionCallback.reset(); - mWorld->testCollision(mBoxBody, mCylinderBody, &mCollisionCallback); - test(!mCollisionCallback.boxCollideWithSphere1); - test(mCollisionCallback.boxCollideWithCylinder); - test(!mCollisionCallback.sphere1CollideWithCylinder); - test(!mCollisionCallback.sphere1CollideWithSphere2); + mCollisionCallback.reset(); + mWorld->testCollision(mBoxBody, mCylinderBody, &mCollisionCallback); + test(!mCollisionCallback.boxCollideWithSphere1); + test(mCollisionCallback.boxCollideWithCylinder); + test(!mCollisionCallback.sphere1CollideWithCylinder); + test(!mCollisionCallback.sphere1CollideWithSphere2); - mCollisionCallback.reset(); - mWorld->testCollision(mCylinderProxyShape, &mCollisionCallback); - test(!mCollisionCallback.boxCollideWithSphere1); - test(mCollisionCallback.boxCollideWithCylinder); - test(!mCollisionCallback.sphere1CollideWithCylinder); - test(!mCollisionCallback.sphere1CollideWithSphere2); + mCollisionCallback.reset(); + mWorld->testCollision(mCylinderProxyShape, &mCollisionCallback); + test(!mCollisionCallback.boxCollideWithSphere1); + test(mCollisionCallback.boxCollideWithCylinder); + test(!mCollisionCallback.sphere1CollideWithCylinder); + test(!mCollisionCallback.sphere1CollideWithSphere2); - mCollisionCallback.reset(); - mWorld->testCollision(mBoxProxyShape, mCylinderProxyShape, &mCollisionCallback); - test(!mCollisionCallback.boxCollideWithSphere1); - test(mCollisionCallback.boxCollideWithCylinder); - test(!mCollisionCallback.sphere1CollideWithCylinder); - test(!mCollisionCallback.sphere1CollideWithSphere2); + mCollisionCallback.reset(); + mWorld->testCollision(mBoxProxyShape, mCylinderProxyShape, &mCollisionCallback); + test(!mCollisionCallback.boxCollideWithSphere1); + test(mCollisionCallback.boxCollideWithCylinder); + test(!mCollisionCallback.sphere1CollideWithCylinder); + test(!mCollisionCallback.sphere1CollideWithSphere2); - // Move sphere 1 to collide with sphere 2 - mSphere1Body->setTransform(Transform(Vector3(30, 15, 10), Quaternion::identity())); + // Move sphere 1 to collide with sphere 2 + mSphere1Body->setTransform(Transform(Vector3(30, 15, 10), Quaternion::identity())); - mCollisionCallback.reset(); - mWorld->testCollision(&mCollisionCallback); - test(!mCollisionCallback.boxCollideWithSphere1); - test(mCollisionCallback.boxCollideWithCylinder); - test(!mCollisionCallback.sphere1CollideWithCylinder); - test(mCollisionCallback.sphere1CollideWithSphere2); + mCollisionCallback.reset(); + mWorld->testCollision(&mCollisionCallback); + test(!mCollisionCallback.boxCollideWithSphere1); + test(mCollisionCallback.boxCollideWithCylinder); + test(!mCollisionCallback.sphere1CollideWithCylinder); + test(mCollisionCallback.sphere1CollideWithSphere2); - mCollisionCallback.reset(); - mWorld->testCollision(mBoxBody, mSphere1Body, &mCollisionCallback); - test(!mCollisionCallback.boxCollideWithSphere1); - test(!mCollisionCallback.boxCollideWithCylinder); - test(!mCollisionCallback.sphere1CollideWithCylinder); - test(!mCollisionCallback.sphere1CollideWithSphere2); + mCollisionCallback.reset(); + mWorld->testCollision(mBoxBody, mSphere1Body, &mCollisionCallback); + test(!mCollisionCallback.boxCollideWithSphere1); + test(!mCollisionCallback.boxCollideWithCylinder); + test(!mCollisionCallback.sphere1CollideWithCylinder); + test(!mCollisionCallback.sphere1CollideWithSphere2); - mCollisionCallback.reset(); - mWorld->testCollision(mBoxBody, mCylinderBody, &mCollisionCallback); - test(!mCollisionCallback.boxCollideWithSphere1); - test(mCollisionCallback.boxCollideWithCylinder); - test(!mCollisionCallback.sphere1CollideWithCylinder); - test(!mCollisionCallback.sphere1CollideWithSphere2); + mCollisionCallback.reset(); + mWorld->testCollision(mBoxBody, mCylinderBody, &mCollisionCallback); + test(!mCollisionCallback.boxCollideWithSphere1); + test(mCollisionCallback.boxCollideWithCylinder); + test(!mCollisionCallback.sphere1CollideWithCylinder); + test(!mCollisionCallback.sphere1CollideWithSphere2); - // Move sphere 1 to collide with box - mSphere1Body->setTransform(Transform(Vector3(10, 5, 0), Quaternion::identity())); + // Move sphere 1 to collide with box + mSphere1Body->setTransform(Transform(Vector3(10, 5, 0), Quaternion::identity())); - // --------- Test collision with inactive bodies --------- // + // --------- Test collision with inactive bodies --------- // - mCollisionCallback.reset(); - mBoxBody->setIsActive(false); - mCylinderBody->setIsActive(false); - mSphere1Body->setIsActive(false); - mSphere2Body->setIsActive(false); - mWorld->testCollision(&mCollisionCallback); - test(!mCollisionCallback.boxCollideWithSphere1); - test(!mCollisionCallback.boxCollideWithCylinder); - test(!mCollisionCallback.sphere1CollideWithCylinder); - test(!mCollisionCallback.sphere1CollideWithSphere2); + mCollisionCallback.reset(); + mBoxBody->setIsActive(false); + mCylinderBody->setIsActive(false); + mSphere1Body->setIsActive(false); + mSphere2Body->setIsActive(false); + mWorld->testCollision(&mCollisionCallback); + test(!mCollisionCallback.boxCollideWithSphere1); + test(!mCollisionCallback.boxCollideWithCylinder); + test(!mCollisionCallback.sphere1CollideWithCylinder); + test(!mCollisionCallback.sphere1CollideWithSphere2); - test(!mWorld->testAABBOverlap(mBoxBody, mSphere1Body)); - test(!mWorld->testAABBOverlap(mBoxBody, mCylinderBody)); - test(!mWorld->testAABBOverlap(mSphere1Body, mCylinderBody)); - test(!mWorld->testAABBOverlap(mSphere1Body, mSphere2Body)); + test(!mWorld->testAABBOverlap(mBoxBody, mSphere1Body)); + test(!mWorld->testAABBOverlap(mBoxBody, mCylinderBody)); + test(!mWorld->testAABBOverlap(mSphere1Body, mCylinderBody)); + test(!mWorld->testAABBOverlap(mSphere1Body, mSphere2Body)); - test(!mWorld->testAABBOverlap(mBoxProxyShape, mSphere1ProxyShape)); - test(!mWorld->testAABBOverlap(mBoxProxyShape, mCylinderProxyShape)); - test(!mWorld->testAABBOverlap(mSphere1ProxyShape, mCylinderProxyShape)); - test(!mWorld->testAABBOverlap(mSphere1ProxyShape, mSphere2ProxyShape)); + test(!mWorld->testAABBOverlap(mBoxProxyShape, mSphere1ProxyShape)); + test(!mWorld->testAABBOverlap(mBoxProxyShape, mCylinderProxyShape)); + test(!mWorld->testAABBOverlap(mSphere1ProxyShape, mCylinderProxyShape)); + test(!mWorld->testAABBOverlap(mSphere1ProxyShape, mSphere2ProxyShape)); - mBoxBody->setIsActive(true); - mCylinderBody->setIsActive(true); - mSphere1Body->setIsActive(true); - mSphere2Body->setIsActive(true); + mBoxBody->setIsActive(true); + mCylinderBody->setIsActive(true); + mSphere1Body->setIsActive(true); + mSphere2Body->setIsActive(true); - // --------- Test collision with collision filtering -------- // + // --------- Test collision with collision filtering -------- // - mBoxProxyShape->setCollideWithMaskBits(CATEGORY_1 | CATEGORY_3); - mSphere1ProxyShape->setCollideWithMaskBits(CATEGORY_1 | CATEGORY_2); - mSphere2ProxyShape->setCollideWithMaskBits(CATEGORY_1); - mCylinderProxyShape->setCollideWithMaskBits(CATEGORY_1); + mBoxProxyShape->setCollideWithMaskBits(CATEGORY_1 | CATEGORY_3); + mSphere1ProxyShape->setCollideWithMaskBits(CATEGORY_1 | CATEGORY_2); + mSphere2ProxyShape->setCollideWithMaskBits(CATEGORY_1); + mCylinderProxyShape->setCollideWithMaskBits(CATEGORY_1); - mCollisionCallback.reset(); - mWorld->testCollision(&mCollisionCallback); - test(mCollisionCallback.boxCollideWithSphere1); - test(mCollisionCallback.boxCollideWithCylinder); - test(!mCollisionCallback.sphere1CollideWithCylinder); - test(!mCollisionCallback.sphere1CollideWithSphere2); + mCollisionCallback.reset(); + mWorld->testCollision(&mCollisionCallback); + test(mCollisionCallback.boxCollideWithSphere1); + test(mCollisionCallback.boxCollideWithCylinder); + test(!mCollisionCallback.sphere1CollideWithCylinder); + test(!mCollisionCallback.sphere1CollideWithSphere2); - // Move sphere 1 to collide with sphere 2 - mSphere1Body->setTransform(Transform(Vector3(30, 15, 10), Quaternion::identity())); + // Move sphere 1 to collide with sphere 2 + mSphere1Body->setTransform(Transform(Vector3(30, 15, 10), Quaternion::identity())); - mCollisionCallback.reset(); - mWorld->testCollision(&mCollisionCallback); - test(!mCollisionCallback.boxCollideWithSphere1); - test(mCollisionCallback.boxCollideWithCylinder); - test(!mCollisionCallback.sphere1CollideWithCylinder); - test(mCollisionCallback.sphere1CollideWithSphere2); + mCollisionCallback.reset(); + mWorld->testCollision(&mCollisionCallback); + test(!mCollisionCallback.boxCollideWithSphere1); + test(mCollisionCallback.boxCollideWithCylinder); + test(!mCollisionCallback.sphere1CollideWithCylinder); + test(mCollisionCallback.sphere1CollideWithSphere2); - mBoxProxyShape->setCollideWithMaskBits(CATEGORY_2); - mSphere1ProxyShape->setCollideWithMaskBits(CATEGORY_2); - mSphere2ProxyShape->setCollideWithMaskBits(CATEGORY_3); - mCylinderProxyShape->setCollideWithMaskBits(CATEGORY_1); + mBoxProxyShape->setCollideWithMaskBits(CATEGORY_2); + mSphere1ProxyShape->setCollideWithMaskBits(CATEGORY_2); + mSphere2ProxyShape->setCollideWithMaskBits(CATEGORY_3); + mCylinderProxyShape->setCollideWithMaskBits(CATEGORY_1); - mCollisionCallback.reset(); - mWorld->testCollision(&mCollisionCallback); - test(!mCollisionCallback.boxCollideWithSphere1); - test(!mCollisionCallback.boxCollideWithCylinder); - test(!mCollisionCallback.sphere1CollideWithCylinder); - test(!mCollisionCallback.sphere1CollideWithSphere2); + mCollisionCallback.reset(); + mWorld->testCollision(&mCollisionCallback); + test(!mCollisionCallback.boxCollideWithSphere1); + test(!mCollisionCallback.boxCollideWithCylinder); + test(!mCollisionCallback.sphere1CollideWithCylinder); + test(!mCollisionCallback.sphere1CollideWithSphere2); - // Move sphere 1 to collide with box - mSphere1Body->setTransform(Transform(Vector3(10, 5, 0), Quaternion::identity())); + // Move sphere 1 to collide with box + mSphere1Body->setTransform(Transform(Vector3(10, 5, 0), Quaternion::identity())); - mBoxProxyShape->setCollideWithMaskBits(0xFFFF); - mSphere1ProxyShape->setCollideWithMaskBits(0xFFFF); - mSphere2ProxyShape->setCollideWithMaskBits(0xFFFF); - mCylinderProxyShape->setCollideWithMaskBits(0xFFFF); - } + mBoxProxyShape->setCollideWithMaskBits(0xFFFF); + mSphere1ProxyShape->setCollideWithMaskBits(0xFFFF); + mSphere2ProxyShape->setCollideWithMaskBits(0xFFFF); + mCylinderProxyShape->setCollideWithMaskBits(0xFFFF); + } }; } diff --git a/test/tests/collision/TestDynamicAABBTree.h b/test/tests/collision/TestDynamicAABBTree.h index 3ee932d..21ce604 100644 --- a/test/tests/collision/TestDynamicAABBTree.h +++ b/test/tests/collision/TestDynamicAABBTree.h @@ -1,26 +1,26 @@ /******************************************************************************** -* ReactPhysics3D physics library, http://www.reactphysics3d.com * -* Copyright (c) 2010-2016 Daniel Chappuis * +* 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: * -* * +* 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. * -* * +* 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_DYNAMIC_AABB_TREE_H @@ -36,44 +36,44 @@ namespace reactphysics3d { class OverlapCallback : public DynamicAABBTreeOverlapCallback { - public : + public : - std::vector mOverlapNodes; + std::vector mOverlapNodes; - // Called when a overlapping node has been found during the call to - // DynamicAABBTree:reportAllShapesOverlappingWithAABB() - virtual void notifyOverlappingNode(int nodeId) { - mOverlapNodes.push_back(nodeId); - } + // Called when a overlapping node has been found during the call to + // DynamicAABBTree:reportAllShapesOverlappingWithAABB() + virtual void notifyOverlappingNode(int32_t nodeId) { + mOverlapNodes.push_back(nodeId); + } - void reset() { - mOverlapNodes.clear(); - } + void reset() { + mOverlapNodes.clear(); + } - bool isOverlapping(int nodeId) const { - return std::find(mOverlapNodes.begin(), mOverlapNodes.end(), nodeId) != mOverlapNodes.end(); - } + bool isOverlapping(int32_t nodeId) const { + return std::find(mOverlapNodes.begin(), mOverlapNodes.end(), nodeId) != mOverlapNodes.end(); + } }; class DynamicTreeRaycastCallback : public DynamicAABBTreeRaycastCallback { - public: + public: - std::vector mHitNodes; + std::vector mHitNodes; - // Called when the AABB of a leaf node is hit by a ray - virtual decimal raycastBroadPhaseShape(int32 nodeId, const Ray& ray) { - mHitNodes.push_back(nodeId); - return 1.0; - } + // Called when the AABB of a leaf node is hit by a ray + virtual float raycastBroadPhaseShape(int32_t nodeId, const Ray& ray) { + mHitNodes.push_back(nodeId); + return 1.0; + } - void reset() { - mHitNodes.clear(); - } + void reset() { + mHitNodes.clear(); + } - bool isHit(int nodeId) const { - return std::find(mHitNodes.begin(), mHitNodes.end(), nodeId) != mHitNodes.end(); - } + bool isHit(int32_t nodeId) const { + return std::find(mHitNodes.begin(), mHitNodes.end(), nodeId) != mHitNodes.end(); + } }; // Class TestDynamicAABBTree @@ -82,440 +82,440 @@ class DynamicTreeRaycastCallback : public DynamicAABBTreeRaycastCallback { */ class TestDynamicAABBTree : public Test { - private : + private : - // ---------- Atributes ---------- // + // ---------- Atributes ---------- // - OverlapCallback mOverlapCallback; - DynamicTreeRaycastCallback m_raycastCallback; + OverlapCallback mOverlapCallback; + DynamicTreeRaycastCallback m_raycastCallback; - public : + public : - // ---------- Methods ---------- // + // ---------- Methods ---------- // - /// Constructor - TestDynamicAABBTree(const std::string& name): Test(name) { + /// Constructor + TestDynamicAABBTree(const std::string& name): Test(name) { - } + } - /// Run the tests - void run() { + /// Run the tests + void run() { - testBasicsMethods(); - testOverlapping(); - testRaycast(); + testBasicsMethods(); + testOverlapping(); + testRaycast(); - } + } - void testBasicsMethods() { + void testBasicsMethods() { - // ------------ Create tree ---------- // + // ------------ Create tree ---------- // - // Dynamic AABB Tree - DynamicAABBTree tree; + // Dynamic AABB Tree + DynamicAABBTree tree; - int object1Data = 56; - int object2Data = 23; - int object3Data = 13; - int object4Data = 7; + int32_t object1Data = 56; + int32_t object2Data = 23; + int32_t object3Data = 13; + int32_t object4Data = 7; - // First object - AABB aabb1 = AABB(Vector3(-6, 4, -3), Vector3(4, 8, 3)); - int object1Id = tree.addObject(aabb1, &object1Data); + // First object + AABB aabb1 = AABB(Vector3(-6, 4, -3), Vector3(4, 8, 3)); + int32_t object1Id = tree.addObject(aabb1, &object1Data); - // Second object - AABB aabb2 = AABB(Vector3(5, 2, -3), Vector3(10, 7, 3)); - int object2Id = tree.addObject(aabb2, &object2Data); + // Second object + AABB aabb2 = AABB(Vector3(5, 2, -3), Vector3(10, 7, 3)); + int32_t object2Id = tree.addObject(aabb2, &object2Data); - // Third object - AABB aabb3 = AABB(Vector3(-5, 1, -3), Vector3(-2, 3, 3)); - int object3Id = tree.addObject(aabb3, &object3Data); + // Third object + AABB aabb3 = AABB(Vector3(-5, 1, -3), Vector3(-2, 3, 3)); + int32_t object3Id = tree.addObject(aabb3, &object3Data); - // Fourth object - AABB aabb4 = AABB(Vector3(0, -4, -3), Vector3(3, -2, 3)); - int object4Id = tree.addObject(aabb4, &object4Data); + // Fourth object + AABB aabb4 = AABB(Vector3(0, -4, -3), Vector3(3, -2, 3)); + int32_t object4Id = tree.addObject(aabb4, &object4Data); - // ----------- Tests ----------- // + // ----------- Tests ----------- // - // Test root AABB - AABB rootAABB = tree.getRootAABB(); - test(rootAABB.getMin().x == -6); - test(rootAABB.getMin().y == -4); - test(rootAABB.getMin().z == -3); - test(rootAABB.getMax().x == 10); - test(rootAABB.getMax().y == 8); - test(rootAABB.getMax().z == 3); + // Test root AABB + AABB rootAABB = tree.getRootAABB(); + test(rootAABB.getMin().x == -6); + test(rootAABB.getMin().y == -4); + test(rootAABB.getMin().z == -3); + test(rootAABB.getMax().x == 10); + test(rootAABB.getMax().y == 8); + test(rootAABB.getMax().z == 3); - // Test data stored at the nodes of the tree - test(*(int*)(tree.getNodeDataPointer(object1Id)) == object1Data); - test(*(int*)(tree.getNodeDataPointer(object2Id)) == object2Data); - test(*(int*)(tree.getNodeDataPointer(object3Id)) == object3Data); - test(*(int*)(tree.getNodeDataPointer(object4Id)) == object4Data); - } - - void testOverlapping() { - - // ------------- Create tree ----------- // - - // Dynamic AABB Tree - DynamicAABBTree tree; - - int object1Data = 56; - int object2Data = 23; - int object3Data = 13; - int object4Data = 7; - - // First object - AABB aabb1 = AABB(Vector3(-6, 4, -3), Vector3(4, 8, 3)); - int object1Id = tree.addObject(aabb1, &object1Data); - - // Second object - AABB aabb2 = AABB(Vector3(5, 2, -3), Vector3(10, 7, 3)); - int object2Id = tree.addObject(aabb2, &object2Data); - - // Third object - AABB aabb3 = AABB(Vector3(-5, 1, -3), Vector3(-2, 3, 3)); - int object3Id = tree.addObject(aabb3, &object3Data); - - // Fourth object - AABB aabb4 = AABB(Vector3(0, -4, -3), Vector3(3, -2, 3)); - int object4Id = tree.addObject(aabb4, &object4Data); - - // ---------- Tests ---------- // - - // AABB overlapping nothing - mOverlapCallback.reset(); - tree.reportAllShapesOverlappingWithAABB(AABB(Vector3(-10, 12, -4), Vector3(10, 50, 4)), mOverlapCallback); - test(!mOverlapCallback.isOverlapping(object1Id)); - test(!mOverlapCallback.isOverlapping(object2Id)); - test(!mOverlapCallback.isOverlapping(object3Id)); - test(!mOverlapCallback.isOverlapping(object4Id)); - - // AABB overlapping everything - mOverlapCallback.reset(); - tree.reportAllShapesOverlappingWithAABB(AABB(Vector3(-15, -15, -4), Vector3(15, 15, 4)), mOverlapCallback); - test(mOverlapCallback.isOverlapping(object1Id)); - test(mOverlapCallback.isOverlapping(object2Id)); - test(mOverlapCallback.isOverlapping(object3Id)); - test(mOverlapCallback.isOverlapping(object4Id)); - - // AABB overlapping object 1 and 3 - mOverlapCallback.reset(); - tree.reportAllShapesOverlappingWithAABB(AABB(Vector3(-4, 2, -4), Vector3(-1, 7, 4)), mOverlapCallback); - test(mOverlapCallback.isOverlapping(object1Id)); - test(!mOverlapCallback.isOverlapping(object2Id)); - test(mOverlapCallback.isOverlapping(object3Id)); - test(!mOverlapCallback.isOverlapping(object4Id)); - - // AABB overlapping object 3 and 4 - mOverlapCallback.reset(); - tree.reportAllShapesOverlappingWithAABB(AABB(Vector3(-6, -5, -2), Vector3(2, 2, 0)), mOverlapCallback); - test(!mOverlapCallback.isOverlapping(object1Id)); - test(!mOverlapCallback.isOverlapping(object2Id)); - test(mOverlapCallback.isOverlapping(object3Id)); - test(mOverlapCallback.isOverlapping(object4Id)); - - // AABB overlapping object 2 - mOverlapCallback.reset(); - tree.reportAllShapesOverlappingWithAABB(AABB(Vector3(5, -10, -2), Vector3(7, 10, 9)), mOverlapCallback); - test(!mOverlapCallback.isOverlapping(object1Id)); - test(mOverlapCallback.isOverlapping(object2Id)); - test(!mOverlapCallback.isOverlapping(object3Id)); - test(!mOverlapCallback.isOverlapping(object4Id)); - - // ---- Update the object AABBs with the initial AABBs (no reinsertion) ----- // - - tree.updateObject(object1Id, aabb1, Vector3::zero(), false); - tree.updateObject(object2Id, aabb2, Vector3::zero(), false); - tree.updateObject(object3Id, aabb3, Vector3::zero(), false); - tree.updateObject(object4Id, aabb4, Vector3::zero(), false); - - // AABB overlapping nothing - mOverlapCallback.reset(); - tree.reportAllShapesOverlappingWithAABB(AABB(Vector3(-10, 12, -4), Vector3(10, 50, 4)), mOverlapCallback); - test(!mOverlapCallback.isOverlapping(object1Id)); - test(!mOverlapCallback.isOverlapping(object2Id)); - test(!mOverlapCallback.isOverlapping(object3Id)); - test(!mOverlapCallback.isOverlapping(object4Id)); - - // AABB overlapping everything - mOverlapCallback.reset(); - tree.reportAllShapesOverlappingWithAABB(AABB(Vector3(-15, -15, -4), Vector3(15, 15, 4)), mOverlapCallback); - test(mOverlapCallback.isOverlapping(object1Id)); - test(mOverlapCallback.isOverlapping(object2Id)); - test(mOverlapCallback.isOverlapping(object3Id)); - test(mOverlapCallback.isOverlapping(object4Id)); - - // AABB overlapping object 1 and 3 - mOverlapCallback.reset(); - tree.reportAllShapesOverlappingWithAABB(AABB(Vector3(-4, 2, -4), Vector3(-1, 7, 4)), mOverlapCallback); - test(mOverlapCallback.isOverlapping(object1Id)); - test(!mOverlapCallback.isOverlapping(object2Id)); - test(mOverlapCallback.isOverlapping(object3Id)); - test(!mOverlapCallback.isOverlapping(object4Id)); - - // AABB overlapping object 3 and 4 - mOverlapCallback.reset(); - tree.reportAllShapesOverlappingWithAABB(AABB(Vector3(-6, -5, -2), Vector3(2, 2, 0)), mOverlapCallback); - test(!mOverlapCallback.isOverlapping(object1Id)); - test(!mOverlapCallback.isOverlapping(object2Id)); - test(mOverlapCallback.isOverlapping(object3Id)); - test(mOverlapCallback.isOverlapping(object4Id)); - - // AABB overlapping object 2 - mOverlapCallback.reset(); - tree.reportAllShapesOverlappingWithAABB(AABB(Vector3(5, -10, -2), Vector3(7, 10, 9)), mOverlapCallback); - test(!mOverlapCallback.isOverlapping(object1Id)); - test(mOverlapCallback.isOverlapping(object2Id)); - test(!mOverlapCallback.isOverlapping(object3Id)); - test(!mOverlapCallback.isOverlapping(object4Id)); - - // ---- Update the object AABBs with the initial AABBs (with reinsertion) ----- // - - tree.updateObject(object1Id, aabb1, Vector3::zero(), true); - tree.updateObject(object2Id, aabb2, Vector3::zero(), true); - tree.updateObject(object3Id, aabb3, Vector3::zero(), true); - tree.updateObject(object4Id, aabb4, Vector3::zero(), true); - - // AABB overlapping nothing - mOverlapCallback.reset(); - tree.reportAllShapesOverlappingWithAABB(AABB(Vector3(-10, 12, -4), Vector3(10, 50, 4)), mOverlapCallback); - test(!mOverlapCallback.isOverlapping(object1Id)); - test(!mOverlapCallback.isOverlapping(object2Id)); - test(!mOverlapCallback.isOverlapping(object3Id)); - test(!mOverlapCallback.isOverlapping(object4Id)); - - // AABB overlapping everything - mOverlapCallback.reset(); - tree.reportAllShapesOverlappingWithAABB(AABB(Vector3(-15, -15, -4), Vector3(15, 15, 4)), mOverlapCallback); - test(mOverlapCallback.isOverlapping(object1Id)); - test(mOverlapCallback.isOverlapping(object2Id)); - test(mOverlapCallback.isOverlapping(object3Id)); - test(mOverlapCallback.isOverlapping(object4Id)); - - // AABB overlapping object 1 and 3 - mOverlapCallback.reset(); - tree.reportAllShapesOverlappingWithAABB(AABB(Vector3(-4, 2, -4), Vector3(-1, 7, 4)), mOverlapCallback); - test(mOverlapCallback.isOverlapping(object1Id)); - test(!mOverlapCallback.isOverlapping(object2Id)); - test(mOverlapCallback.isOverlapping(object3Id)); - test(!mOverlapCallback.isOverlapping(object4Id)); - - // AABB overlapping object 3 and 4 - mOverlapCallback.reset(); - tree.reportAllShapesOverlappingWithAABB(AABB(Vector3(-6, -5, -2), Vector3(2, 2, 0)), mOverlapCallback); - test(!mOverlapCallback.isOverlapping(object1Id)); - test(!mOverlapCallback.isOverlapping(object2Id)); - test(mOverlapCallback.isOverlapping(object3Id)); - test(mOverlapCallback.isOverlapping(object4Id)); - - // AABB overlapping object 2 - mOverlapCallback.reset(); - tree.reportAllShapesOverlappingWithAABB(AABB(Vector3(5, -10, -2), Vector3(7, 10, 9)), mOverlapCallback); - test(!mOverlapCallback.isOverlapping(object1Id)); - test(mOverlapCallback.isOverlapping(object2Id)); - test(!mOverlapCallback.isOverlapping(object3Id)); - test(!mOverlapCallback.isOverlapping(object4Id)); - - // ---- Move objects 2 and 3 ----- // - - AABB newAABB2(Vector3(-7, 10, -3), Vector3(1, 13, 3)); - tree.updateObject(object2Id, newAABB2, Vector3::zero()); - - AABB newAABB3(Vector3(7, -6, -3), Vector3(9, 1, 3)); - tree.updateObject(object3Id, newAABB3, Vector3::zero()); - - // AABB overlapping object 3 - mOverlapCallback.reset(); - tree.reportAllShapesOverlappingWithAABB(AABB(Vector3(6, -10, -2), Vector3(8, 5, 3)), mOverlapCallback); - test(!mOverlapCallback.isOverlapping(object1Id)); - test(!mOverlapCallback.isOverlapping(object2Id)); - test(mOverlapCallback.isOverlapping(object3Id)); - test(!mOverlapCallback.isOverlapping(object4Id)); - - // AABB overlapping objects 1, 2 - mOverlapCallback.reset(); - tree.reportAllShapesOverlappingWithAABB(AABB(Vector3(-8, 5, -3), Vector3(-2, 11, 3)), mOverlapCallback); - test(mOverlapCallback.isOverlapping(object1Id)); - test(mOverlapCallback.isOverlapping(object2Id)); - test(!mOverlapCallback.isOverlapping(object3Id)); - test(!mOverlapCallback.isOverlapping(object4Id)); - - } - - void testRaycast() { - - // ------------- Create tree ----------- // - - // Dynamic AABB Tree - DynamicAABBTree tree; - - int object1Data = 56; - int object2Data = 23; - int object3Data = 13; - int object4Data = 7; - - // First object - AABB aabb1 = AABB(Vector3(-6, 4, -3), Vector3(4, 8, 3)); - int object1Id = tree.addObject(aabb1, &object1Data); - - // Second object - AABB aabb2 = AABB(Vector3(5, 2, -3), Vector3(10, 7, 3)); - int object2Id = tree.addObject(aabb2, &object2Data); - - // Third object - AABB aabb3 = AABB(Vector3(-5, 1, -3), Vector3(-2, 3, 3)); - int object3Id = tree.addObject(aabb3, &object3Data); - - // Fourth object - AABB aabb4 = AABB(Vector3(0, -4, -3), Vector3(3, -2, 3)); - int object4Id = tree.addObject(aabb4, &object4Data); - - // ---------- Tests ---------- // - - // Ray with no hits - m_raycastCallback.reset(); - Ray ray1(Vector3(4.5, -10, -5), Vector3(4.5, 10, -5)); - tree.raycast(ray1, m_raycastCallback); - test(!m_raycastCallback.isHit(object1Id)); - test(!m_raycastCallback.isHit(object2Id)); - test(!m_raycastCallback.isHit(object3Id)); - test(!m_raycastCallback.isHit(object4Id)); - - // Ray that hits object 1 - m_raycastCallback.reset(); - Ray ray2(Vector3(-1, -20, -2), Vector3(-1, 20, -2)); - tree.raycast(ray2, m_raycastCallback); - test(m_raycastCallback.isHit(object1Id)); - test(!m_raycastCallback.isHit(object2Id)); - test(!m_raycastCallback.isHit(object3Id)); - test(!m_raycastCallback.isHit(object4Id)); - - // Ray that hits object 1 and 2 - m_raycastCallback.reset(); - Ray ray3(Vector3(-7, 6, -2), Vector3(8, 6, -2)); - tree.raycast(ray3, m_raycastCallback); - test(m_raycastCallback.isHit(object1Id)); - test(m_raycastCallback.isHit(object2Id)); - test(!m_raycastCallback.isHit(object3Id)); - test(!m_raycastCallback.isHit(object4Id)); - - // Ray that hits object 3 - m_raycastCallback.reset(); - Ray ray4(Vector3(-7, 2, 0), Vector3(-1, 2, 0)); - tree.raycast(ray4, m_raycastCallback); - test(!m_raycastCallback.isHit(object1Id)); - test(!m_raycastCallback.isHit(object2Id)); - test(m_raycastCallback.isHit(object3Id)); - test(!m_raycastCallback.isHit(object4Id)); - - // ---- Update the object AABBs with the initial AABBs (no reinsertion) ----- // - - tree.updateObject(object1Id, aabb1, Vector3::zero(), false); - tree.updateObject(object2Id, aabb2, Vector3::zero(), false); - tree.updateObject(object3Id, aabb3, Vector3::zero(), false); - tree.updateObject(object4Id, aabb4, Vector3::zero(), false); - - // Ray with no hits - m_raycastCallback.reset(); - tree.raycast(ray1, m_raycastCallback); - test(!m_raycastCallback.isHit(object1Id)); - test(!m_raycastCallback.isHit(object2Id)); - test(!m_raycastCallback.isHit(object3Id)); - test(!m_raycastCallback.isHit(object4Id)); - - // Ray that hits object 1 - m_raycastCallback.reset(); - tree.raycast(ray2, m_raycastCallback); - test(m_raycastCallback.isHit(object1Id)); - test(!m_raycastCallback.isHit(object2Id)); - test(!m_raycastCallback.isHit(object3Id)); - test(!m_raycastCallback.isHit(object4Id)); - - // Ray that hits object 1 and 2 - m_raycastCallback.reset(); - tree.raycast(ray3, m_raycastCallback); - test(m_raycastCallback.isHit(object1Id)); - test(m_raycastCallback.isHit(object2Id)); - test(!m_raycastCallback.isHit(object3Id)); - test(!m_raycastCallback.isHit(object4Id)); - - // Ray that hits object 3 - m_raycastCallback.reset(); - tree.raycast(ray4, m_raycastCallback); - test(!m_raycastCallback.isHit(object1Id)); - test(!m_raycastCallback.isHit(object2Id)); - test(m_raycastCallback.isHit(object3Id)); - test(!m_raycastCallback.isHit(object4Id)); - - // ---- Update the object AABBs with the initial AABBs (with reinsertion) ----- // - - tree.updateObject(object1Id, aabb1, Vector3::zero(), true); - tree.updateObject(object2Id, aabb2, Vector3::zero(), true); - tree.updateObject(object3Id, aabb3, Vector3::zero(), true); - tree.updateObject(object4Id, aabb4, Vector3::zero(), true); - - // Ray with no hits - m_raycastCallback.reset(); - tree.raycast(ray1, m_raycastCallback); - test(!m_raycastCallback.isHit(object1Id)); - test(!m_raycastCallback.isHit(object2Id)); - test(!m_raycastCallback.isHit(object3Id)); - test(!m_raycastCallback.isHit(object4Id)); - - // Ray that hits object 1 - m_raycastCallback.reset(); - tree.raycast(ray2, m_raycastCallback); - test(m_raycastCallback.isHit(object1Id)); - test(!m_raycastCallback.isHit(object2Id)); - test(!m_raycastCallback.isHit(object3Id)); - test(!m_raycastCallback.isHit(object4Id)); - - // Ray that hits object 1 and 2 - m_raycastCallback.reset(); - tree.raycast(ray3, m_raycastCallback); - test(m_raycastCallback.isHit(object1Id)); - test(m_raycastCallback.isHit(object2Id)); - test(!m_raycastCallback.isHit(object3Id)); - test(!m_raycastCallback.isHit(object4Id)); - - // Ray that hits object 3 - m_raycastCallback.reset(); - tree.raycast(ray4, m_raycastCallback); - test(!m_raycastCallback.isHit(object1Id)); - test(!m_raycastCallback.isHit(object2Id)); - test(m_raycastCallback.isHit(object3Id)); - test(!m_raycastCallback.isHit(object4Id)); - - // ---- Move objects 2 and 3 ----- // - - AABB newAABB2(Vector3(-7, 10, -3), Vector3(1, 13, 3)); - tree.updateObject(object2Id, newAABB2, Vector3::zero()); - - AABB newAABB3(Vector3(7, -6, -3), Vector3(9, 1, 3)); - tree.updateObject(object3Id, newAABB3, Vector3::zero()); - - // Ray that hits object 1, 2 - Ray ray5(Vector3(-4, -5, 0), Vector3(-4, 12, 0)); - m_raycastCallback.reset(); - tree.raycast(ray5, m_raycastCallback); - test(m_raycastCallback.isHit(object1Id)); - test(m_raycastCallback.isHit(object2Id)); - test(!m_raycastCallback.isHit(object3Id)); - test(!m_raycastCallback.isHit(object4Id)); - - // Ray that hits object 3 and 4 - Ray ray6(Vector3(11, -3, 1), Vector3(-2, -3, 1)); - m_raycastCallback.reset(); - tree.raycast(ray6, m_raycastCallback); - test(!m_raycastCallback.isHit(object1Id)); - test(!m_raycastCallback.isHit(object2Id)); - test(m_raycastCallback.isHit(object3Id)); - test(m_raycastCallback.isHit(object4Id)); - } + // Test data stored at the nodes of the tree + test(*(int32_t*)(tree.getNodeDataPointer(object1Id)) == object1Data); + test(*(int32_t*)(tree.getNodeDataPointer(object2Id)) == object2Data); + test(*(int32_t*)(tree.getNodeDataPointer(object3Id)) == object3Data); + test(*(int32_t*)(tree.getNodeDataPointer(object4Id)) == object4Data); + } + + void testOverlapping() { + + // ------------- Create tree ----------- // + + // Dynamic AABB Tree + DynamicAABBTree tree; + + int32_t object1Data = 56; + int32_t object2Data = 23; + int32_t object3Data = 13; + int32_t object4Data = 7; + + // First object + AABB aabb1 = AABB(Vector3(-6, 4, -3), Vector3(4, 8, 3)); + int32_t object1Id = tree.addObject(aabb1, &object1Data); + + // Second object + AABB aabb2 = AABB(Vector3(5, 2, -3), Vector3(10, 7, 3)); + int32_t object2Id = tree.addObject(aabb2, &object2Data); + + // Third object + AABB aabb3 = AABB(Vector3(-5, 1, -3), Vector3(-2, 3, 3)); + int32_t object3Id = tree.addObject(aabb3, &object3Data); + + // Fourth object + AABB aabb4 = AABB(Vector3(0, -4, -3), Vector3(3, -2, 3)); + int32_t object4Id = tree.addObject(aabb4, &object4Data); + + // ---------- Tests ---------- // + + // AABB overlapping nothing + mOverlapCallback.reset(); + tree.reportAllShapesOverlappingWithAABB(AABB(Vector3(-10, 12, -4), Vector3(10, 50, 4)), mOverlapCallback); + test(!mOverlapCallback.isOverlapping(object1Id)); + test(!mOverlapCallback.isOverlapping(object2Id)); + test(!mOverlapCallback.isOverlapping(object3Id)); + test(!mOverlapCallback.isOverlapping(object4Id)); + + // AABB overlapping everything + mOverlapCallback.reset(); + tree.reportAllShapesOverlappingWithAABB(AABB(Vector3(-15, -15, -4), Vector3(15, 15, 4)), mOverlapCallback); + test(mOverlapCallback.isOverlapping(object1Id)); + test(mOverlapCallback.isOverlapping(object2Id)); + test(mOverlapCallback.isOverlapping(object3Id)); + test(mOverlapCallback.isOverlapping(object4Id)); + + // AABB overlapping object 1 and 3 + mOverlapCallback.reset(); + tree.reportAllShapesOverlappingWithAABB(AABB(Vector3(-4, 2, -4), Vector3(-1, 7, 4)), mOverlapCallback); + test(mOverlapCallback.isOverlapping(object1Id)); + test(!mOverlapCallback.isOverlapping(object2Id)); + test(mOverlapCallback.isOverlapping(object3Id)); + test(!mOverlapCallback.isOverlapping(object4Id)); + + // AABB overlapping object 3 and 4 + mOverlapCallback.reset(); + tree.reportAllShapesOverlappingWithAABB(AABB(Vector3(-6, -5, -2), Vector3(2, 2, 0)), mOverlapCallback); + test(!mOverlapCallback.isOverlapping(object1Id)); + test(!mOverlapCallback.isOverlapping(object2Id)); + test(mOverlapCallback.isOverlapping(object3Id)); + test(mOverlapCallback.isOverlapping(object4Id)); + + // AABB overlapping object 2 + mOverlapCallback.reset(); + tree.reportAllShapesOverlappingWithAABB(AABB(Vector3(5, -10, -2), Vector3(7, 10, 9)), mOverlapCallback); + test(!mOverlapCallback.isOverlapping(object1Id)); + test(mOverlapCallback.isOverlapping(object2Id)); + test(!mOverlapCallback.isOverlapping(object3Id)); + test(!mOverlapCallback.isOverlapping(object4Id)); + + // ---- Update the object AABBs with the initial AABBs (no reinsertion) ----- // + + tree.updateObject(object1Id, aabb1, Vector3::zero(), false); + tree.updateObject(object2Id, aabb2, Vector3::zero(), false); + tree.updateObject(object3Id, aabb3, Vector3::zero(), false); + tree.updateObject(object4Id, aabb4, Vector3::zero(), false); + + // AABB overlapping nothing + mOverlapCallback.reset(); + tree.reportAllShapesOverlappingWithAABB(AABB(Vector3(-10, 12, -4), Vector3(10, 50, 4)), mOverlapCallback); + test(!mOverlapCallback.isOverlapping(object1Id)); + test(!mOverlapCallback.isOverlapping(object2Id)); + test(!mOverlapCallback.isOverlapping(object3Id)); + test(!mOverlapCallback.isOverlapping(object4Id)); + + // AABB overlapping everything + mOverlapCallback.reset(); + tree.reportAllShapesOverlappingWithAABB(AABB(Vector3(-15, -15, -4), Vector3(15, 15, 4)), mOverlapCallback); + test(mOverlapCallback.isOverlapping(object1Id)); + test(mOverlapCallback.isOverlapping(object2Id)); + test(mOverlapCallback.isOverlapping(object3Id)); + test(mOverlapCallback.isOverlapping(object4Id)); + + // AABB overlapping object 1 and 3 + mOverlapCallback.reset(); + tree.reportAllShapesOverlappingWithAABB(AABB(Vector3(-4, 2, -4), Vector3(-1, 7, 4)), mOverlapCallback); + test(mOverlapCallback.isOverlapping(object1Id)); + test(!mOverlapCallback.isOverlapping(object2Id)); + test(mOverlapCallback.isOverlapping(object3Id)); + test(!mOverlapCallback.isOverlapping(object4Id)); + + // AABB overlapping object 3 and 4 + mOverlapCallback.reset(); + tree.reportAllShapesOverlappingWithAABB(AABB(Vector3(-6, -5, -2), Vector3(2, 2, 0)), mOverlapCallback); + test(!mOverlapCallback.isOverlapping(object1Id)); + test(!mOverlapCallback.isOverlapping(object2Id)); + test(mOverlapCallback.isOverlapping(object3Id)); + test(mOverlapCallback.isOverlapping(object4Id)); + + // AABB overlapping object 2 + mOverlapCallback.reset(); + tree.reportAllShapesOverlappingWithAABB(AABB(Vector3(5, -10, -2), Vector3(7, 10, 9)), mOverlapCallback); + test(!mOverlapCallback.isOverlapping(object1Id)); + test(mOverlapCallback.isOverlapping(object2Id)); + test(!mOverlapCallback.isOverlapping(object3Id)); + test(!mOverlapCallback.isOverlapping(object4Id)); + + // ---- Update the object AABBs with the initial AABBs (with reinsertion) ----- // + + tree.updateObject(object1Id, aabb1, Vector3::zero(), true); + tree.updateObject(object2Id, aabb2, Vector3::zero(), true); + tree.updateObject(object3Id, aabb3, Vector3::zero(), true); + tree.updateObject(object4Id, aabb4, Vector3::zero(), true); + + // AABB overlapping nothing + mOverlapCallback.reset(); + tree.reportAllShapesOverlappingWithAABB(AABB(Vector3(-10, 12, -4), Vector3(10, 50, 4)), mOverlapCallback); + test(!mOverlapCallback.isOverlapping(object1Id)); + test(!mOverlapCallback.isOverlapping(object2Id)); + test(!mOverlapCallback.isOverlapping(object3Id)); + test(!mOverlapCallback.isOverlapping(object4Id)); + + // AABB overlapping everything + mOverlapCallback.reset(); + tree.reportAllShapesOverlappingWithAABB(AABB(Vector3(-15, -15, -4), Vector3(15, 15, 4)), mOverlapCallback); + test(mOverlapCallback.isOverlapping(object1Id)); + test(mOverlapCallback.isOverlapping(object2Id)); + test(mOverlapCallback.isOverlapping(object3Id)); + test(mOverlapCallback.isOverlapping(object4Id)); + + // AABB overlapping object 1 and 3 + mOverlapCallback.reset(); + tree.reportAllShapesOverlappingWithAABB(AABB(Vector3(-4, 2, -4), Vector3(-1, 7, 4)), mOverlapCallback); + test(mOverlapCallback.isOverlapping(object1Id)); + test(!mOverlapCallback.isOverlapping(object2Id)); + test(mOverlapCallback.isOverlapping(object3Id)); + test(!mOverlapCallback.isOverlapping(object4Id)); + + // AABB overlapping object 3 and 4 + mOverlapCallback.reset(); + tree.reportAllShapesOverlappingWithAABB(AABB(Vector3(-6, -5, -2), Vector3(2, 2, 0)), mOverlapCallback); + test(!mOverlapCallback.isOverlapping(object1Id)); + test(!mOverlapCallback.isOverlapping(object2Id)); + test(mOverlapCallback.isOverlapping(object3Id)); + test(mOverlapCallback.isOverlapping(object4Id)); + + // AABB overlapping object 2 + mOverlapCallback.reset(); + tree.reportAllShapesOverlappingWithAABB(AABB(Vector3(5, -10, -2), Vector3(7, 10, 9)), mOverlapCallback); + test(!mOverlapCallback.isOverlapping(object1Id)); + test(mOverlapCallback.isOverlapping(object2Id)); + test(!mOverlapCallback.isOverlapping(object3Id)); + test(!mOverlapCallback.isOverlapping(object4Id)); + + // ---- Move objects 2 and 3 ----- // + + AABB newAABB2(Vector3(-7, 10, -3), Vector3(1, 13, 3)); + tree.updateObject(object2Id, newAABB2, Vector3::zero()); + + AABB newAABB3(Vector3(7, -6, -3), Vector3(9, 1, 3)); + tree.updateObject(object3Id, newAABB3, Vector3::zero()); + + // AABB overlapping object 3 + mOverlapCallback.reset(); + tree.reportAllShapesOverlappingWithAABB(AABB(Vector3(6, -10, -2), Vector3(8, 5, 3)), mOverlapCallback); + test(!mOverlapCallback.isOverlapping(object1Id)); + test(!mOverlapCallback.isOverlapping(object2Id)); + test(mOverlapCallback.isOverlapping(object3Id)); + test(!mOverlapCallback.isOverlapping(object4Id)); + + // AABB overlapping objects 1, 2 + mOverlapCallback.reset(); + tree.reportAllShapesOverlappingWithAABB(AABB(Vector3(-8, 5, -3), Vector3(-2, 11, 3)), mOverlapCallback); + test(mOverlapCallback.isOverlapping(object1Id)); + test(mOverlapCallback.isOverlapping(object2Id)); + test(!mOverlapCallback.isOverlapping(object3Id)); + test(!mOverlapCallback.isOverlapping(object4Id)); + + } + + void testRaycast() { + + // ------------- Create tree ----------- // + + // Dynamic AABB Tree + DynamicAABBTree tree; + + int32_t object1Data = 56; + int32_t object2Data = 23; + int32_t object3Data = 13; + int32_t object4Data = 7; + + // First object + AABB aabb1 = AABB(Vector3(-6, 4, -3), Vector3(4, 8, 3)); + int32_t object1Id = tree.addObject(aabb1, &object1Data); + + // Second object + AABB aabb2 = AABB(Vector3(5, 2, -3), Vector3(10, 7, 3)); + int32_t object2Id = tree.addObject(aabb2, &object2Data); + + // Third object + AABB aabb3 = AABB(Vector3(-5, 1, -3), Vector3(-2, 3, 3)); + int32_t object3Id = tree.addObject(aabb3, &object3Data); + + // Fourth object + AABB aabb4 = AABB(Vector3(0, -4, -3), Vector3(3, -2, 3)); + int32_t object4Id = tree.addObject(aabb4, &object4Data); + + // ---------- Tests ---------- // + + // Ray with no hits + m_raycastCallback.reset(); + Ray ray1(Vector3(4.5, -10, -5), Vector3(4.5, 10, -5)); + tree.raycast(ray1, m_raycastCallback); + test(!m_raycastCallback.isHit(object1Id)); + test(!m_raycastCallback.isHit(object2Id)); + test(!m_raycastCallback.isHit(object3Id)); + test(!m_raycastCallback.isHit(object4Id)); + + // Ray that hits object 1 + m_raycastCallback.reset(); + Ray ray2(Vector3(-1, -20, -2), Vector3(-1, 20, -2)); + tree.raycast(ray2, m_raycastCallback); + test(m_raycastCallback.isHit(object1Id)); + test(!m_raycastCallback.isHit(object2Id)); + test(!m_raycastCallback.isHit(object3Id)); + test(!m_raycastCallback.isHit(object4Id)); + + // Ray that hits object 1 and 2 + m_raycastCallback.reset(); + Ray ray3(Vector3(-7, 6, -2), Vector3(8, 6, -2)); + tree.raycast(ray3, m_raycastCallback); + test(m_raycastCallback.isHit(object1Id)); + test(m_raycastCallback.isHit(object2Id)); + test(!m_raycastCallback.isHit(object3Id)); + test(!m_raycastCallback.isHit(object4Id)); + + // Ray that hits object 3 + m_raycastCallback.reset(); + Ray ray4(Vector3(-7, 2, 0), Vector3(-1, 2, 0)); + tree.raycast(ray4, m_raycastCallback); + test(!m_raycastCallback.isHit(object1Id)); + test(!m_raycastCallback.isHit(object2Id)); + test(m_raycastCallback.isHit(object3Id)); + test(!m_raycastCallback.isHit(object4Id)); + + // ---- Update the object AABBs with the initial AABBs (no reinsertion) ----- // + + tree.updateObject(object1Id, aabb1, Vector3::zero(), false); + tree.updateObject(object2Id, aabb2, Vector3::zero(), false); + tree.updateObject(object3Id, aabb3, Vector3::zero(), false); + tree.updateObject(object4Id, aabb4, Vector3::zero(), false); + + // Ray with no hits + m_raycastCallback.reset(); + tree.raycast(ray1, m_raycastCallback); + test(!m_raycastCallback.isHit(object1Id)); + test(!m_raycastCallback.isHit(object2Id)); + test(!m_raycastCallback.isHit(object3Id)); + test(!m_raycastCallback.isHit(object4Id)); + + // Ray that hits object 1 + m_raycastCallback.reset(); + tree.raycast(ray2, m_raycastCallback); + test(m_raycastCallback.isHit(object1Id)); + test(!m_raycastCallback.isHit(object2Id)); + test(!m_raycastCallback.isHit(object3Id)); + test(!m_raycastCallback.isHit(object4Id)); + + // Ray that hits object 1 and 2 + m_raycastCallback.reset(); + tree.raycast(ray3, m_raycastCallback); + test(m_raycastCallback.isHit(object1Id)); + test(m_raycastCallback.isHit(object2Id)); + test(!m_raycastCallback.isHit(object3Id)); + test(!m_raycastCallback.isHit(object4Id)); + + // Ray that hits object 3 + m_raycastCallback.reset(); + tree.raycast(ray4, m_raycastCallback); + test(!m_raycastCallback.isHit(object1Id)); + test(!m_raycastCallback.isHit(object2Id)); + test(m_raycastCallback.isHit(object3Id)); + test(!m_raycastCallback.isHit(object4Id)); + + // ---- Update the object AABBs with the initial AABBs (with reinsertion) ----- // + + tree.updateObject(object1Id, aabb1, Vector3::zero(), true); + tree.updateObject(object2Id, aabb2, Vector3::zero(), true); + tree.updateObject(object3Id, aabb3, Vector3::zero(), true); + tree.updateObject(object4Id, aabb4, Vector3::zero(), true); + + // Ray with no hits + m_raycastCallback.reset(); + tree.raycast(ray1, m_raycastCallback); + test(!m_raycastCallback.isHit(object1Id)); + test(!m_raycastCallback.isHit(object2Id)); + test(!m_raycastCallback.isHit(object3Id)); + test(!m_raycastCallback.isHit(object4Id)); + + // Ray that hits object 1 + m_raycastCallback.reset(); + tree.raycast(ray2, m_raycastCallback); + test(m_raycastCallback.isHit(object1Id)); + test(!m_raycastCallback.isHit(object2Id)); + test(!m_raycastCallback.isHit(object3Id)); + test(!m_raycastCallback.isHit(object4Id)); + + // Ray that hits object 1 and 2 + m_raycastCallback.reset(); + tree.raycast(ray3, m_raycastCallback); + test(m_raycastCallback.isHit(object1Id)); + test(m_raycastCallback.isHit(object2Id)); + test(!m_raycastCallback.isHit(object3Id)); + test(!m_raycastCallback.isHit(object4Id)); + + // Ray that hits object 3 + m_raycastCallback.reset(); + tree.raycast(ray4, m_raycastCallback); + test(!m_raycastCallback.isHit(object1Id)); + test(!m_raycastCallback.isHit(object2Id)); + test(m_raycastCallback.isHit(object3Id)); + test(!m_raycastCallback.isHit(object4Id)); + + // ---- Move objects 2 and 3 ----- // + + AABB newAABB2(Vector3(-7, 10, -3), Vector3(1, 13, 3)); + tree.updateObject(object2Id, newAABB2, Vector3::zero()); + + AABB newAABB3(Vector3(7, -6, -3), Vector3(9, 1, 3)); + tree.updateObject(object3Id, newAABB3, Vector3::zero()); + + // Ray that hits object 1, 2 + Ray ray5(Vector3(-4, -5, 0), Vector3(-4, 12, 0)); + m_raycastCallback.reset(); + tree.raycast(ray5, m_raycastCallback); + test(m_raycastCallback.isHit(object1Id)); + test(m_raycastCallback.isHit(object2Id)); + test(!m_raycastCallback.isHit(object3Id)); + test(!m_raycastCallback.isHit(object4Id)); + + // Ray that hits object 3 and 4 + Ray ray6(Vector3(11, -3, 1), Vector3(-2, -3, 1)); + m_raycastCallback.reset(); + tree.raycast(ray6, m_raycastCallback); + test(!m_raycastCallback.isHit(object1Id)); + test(!m_raycastCallback.isHit(object2Id)); + test(m_raycastCallback.isHit(object3Id)); + test(m_raycastCallback.isHit(object4Id)); + } }; } diff --git a/test/tests/collision/TestPointInside.h b/test/tests/collision/TestPointInside.h index 9a8fc48..26a32fa 100644 --- a/test/tests/collision/TestPointInside.h +++ b/test/tests/collision/TestPointInside.h @@ -1,26 +1,26 @@ /******************************************************************************** -* ReactPhysics3D physics library, http://www.reactphysics3d.com * -* Copyright (c) 2010-2016 Daniel Chappuis * +* 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: * -* * +* 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. * -* * +* 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_POINT_INSIDE_H @@ -44,720 +44,720 @@ namespace reactphysics3d { */ class TestPointInside : public Test { - private : + private : - // ---------- Atributes ---------- // + // ---------- Atributes ---------- // - // Physics world - CollisionWorld* mWorld; + // Physics world + CollisionWorld* mWorld; - // Bodies - CollisionBody* mBoxBody; - CollisionBody* mSphereBody; - CollisionBody* mCapsuleBody; - CollisionBody* mConeBody; - CollisionBody* mConvexMeshBody; - CollisionBody* mConvexMeshBodyEdgesInfo; - CollisionBody* mCylinderBody; - CollisionBody* mCompoundBody; + // Bodies + CollisionBody* mBoxBody; + CollisionBody* mSphereBody; + CollisionBody* mCapsuleBody; + CollisionBody* mConeBody; + CollisionBody* mConvexMeshBody; + CollisionBody* mConvexMeshBodyEdgesInfo; + CollisionBody* mCylinderBody; + CollisionBody* mCompoundBody; - // Collision shapes - BoxShape* mBoxShape; - SphereShape* mSphereShape; - CapsuleShape* mCapsuleShape; - ConeShape* mConeShape; - ConvexMeshShape* mConvexMeshShape; - ConvexMeshShape* mConvexMeshShapeBodyEdgesInfo; - CylinderShape* mCylinderShape; + // Collision shapes + BoxShape* mBoxShape; + SphereShape* mSphereShape; + CapsuleShape* mCapsuleShape; + ConeShape* mConeShape; + ConvexMeshShape* mConvexMeshShape; + ConvexMeshShape* mConvexMeshShapeBodyEdgesInfo; + CylinderShape* mCylinderShape; - // Transform - Transform mBodyTransform; - Transform mShapeTransform; - Transform mLocalShapeToWorld; - Transform mLocalShape2ToWorld; + // Transform + Transform mBodyTransform; + Transform mShapeTransform; + Transform mLocalShapeToWorld; + Transform mLocalShape2ToWorld; - // Proxy Shapes - ProxyShape* mBoxProxyShape; - ProxyShape* mSphereProxyShape; - ProxyShape* mCapsuleProxyShape; - ProxyShape* mConeProxyShape; - ProxyShape* mConvexMeshProxyShape; - ProxyShape* mConvexMeshProxyShapeEdgesInfo; - ProxyShape* mCylinderProxyShape; + // Proxy Shapes + ProxyShape* mBoxProxyShape; + ProxyShape* mSphereProxyShape; + ProxyShape* mCapsuleProxyShape; + ProxyShape* mConeProxyShape; + ProxyShape* mConvexMeshProxyShape; + ProxyShape* mConvexMeshProxyShapeEdgesInfo; + ProxyShape* mCylinderProxyShape; - public : + public : - // ---------- Methods ---------- // + // ---------- Methods ---------- // - /// Constructor - TestPointInside(const std::string& name) : Test(name) { + /// Constructor + TestPointInside(const std::string& name) : Test(name) { - // Create the world - mWorld = new CollisionWorld(); + // Create the world + mWorld = new CollisionWorld(); - // Body transform - Vector3 position(-3, 2, 7); - Quaternion orientation(PI / 5, PI / 6, PI / 7); - mBodyTransform = Transform(position, orientation); + // Body transform + Vector3 position(-3, 2, 7); + Quaternion orientation(PI / 5, PI / 6, PI / 7); + mBodyTransform = Transform(position, orientation); - // Create the bodies - mBoxBody = mWorld->createCollisionBody(mBodyTransform); - mSphereBody = mWorld->createCollisionBody(mBodyTransform); - mCapsuleBody = mWorld->createCollisionBody(mBodyTransform); - mConeBody = mWorld->createCollisionBody(mBodyTransform); - mConvexMeshBody = mWorld->createCollisionBody(mBodyTransform); - mConvexMeshBodyEdgesInfo = mWorld->createCollisionBody(mBodyTransform); - mCylinderBody = mWorld->createCollisionBody(mBodyTransform); - mCompoundBody = mWorld->createCollisionBody(mBodyTransform); + // Create the bodies + mBoxBody = mWorld->createCollisionBody(mBodyTransform); + mSphereBody = mWorld->createCollisionBody(mBodyTransform); + mCapsuleBody = mWorld->createCollisionBody(mBodyTransform); + mConeBody = mWorld->createCollisionBody(mBodyTransform); + mConvexMeshBody = mWorld->createCollisionBody(mBodyTransform); + mConvexMeshBodyEdgesInfo = mWorld->createCollisionBody(mBodyTransform); + mCylinderBody = mWorld->createCollisionBody(mBodyTransform); + mCompoundBody = mWorld->createCollisionBody(mBodyTransform); - // Collision shape transform - Vector3 shapePosition(1, -4, -3); - Quaternion shapeOrientation(3 * PI / 6 , -PI / 8, PI / 3); - mShapeTransform = Transform(shapePosition, shapeOrientation); + // Collision shape transform + Vector3 shapePosition(1, -4, -3); + Quaternion shapeOrientation(3 * PI / 6 , -PI / 8, PI / 3); + mShapeTransform = Transform(shapePosition, shapeOrientation); - // Compute the the transform from a local shape point to world-space - mLocalShapeToWorld = mBodyTransform * mShapeTransform; + // Compute the the transform from a local shape point to world-space + mLocalShapeToWorld = mBodyTransform * mShapeTransform; - // Create collision shapes - mBoxShape = new BoxShape(Vector3(2, 3, 4), 0); - mBoxProxyShape = mBoxBody->addCollisionShape(mBoxShape, mShapeTransform); + // Create collision shapes + mBoxShape = new BoxShape(Vector3(2, 3, 4), 0); + mBoxProxyShape = mBoxBody->addCollisionShape(mBoxShape, mShapeTransform); - mSphereShape = new SphereShape(3); - mSphereProxyShape = mSphereBody->addCollisionShape(mSphereShape, mShapeTransform); + mSphereShape = new SphereShape(3); + mSphereProxyShape = mSphereBody->addCollisionShape(mSphereShape, mShapeTransform); - mCapsuleShape = new CapsuleShape(2, 10); - mCapsuleProxyShape = mCapsuleBody->addCollisionShape(mCapsuleShape, mShapeTransform); + mCapsuleShape = new CapsuleShape(2, 10); + mCapsuleProxyShape = mCapsuleBody->addCollisionShape(mCapsuleShape, mShapeTransform); - mConeShape = new ConeShape(2, 6, 0); - mConeProxyShape = mConeBody->addCollisionShape(mConeShape, mShapeTransform); + mConeShape = new ConeShape(2, 6, 0); + mConeProxyShape = mConeBody->addCollisionShape(mConeShape, mShapeTransform); - mConvexMeshShape = new ConvexMeshShape(0.0); // Box of dimension (2, 3, 4) - mConvexMeshShape->addVertex(Vector3(-2, -3, -4)); - mConvexMeshShape->addVertex(Vector3(2, -3, -4)); - mConvexMeshShape->addVertex(Vector3(2, -3, 4)); - mConvexMeshShape->addVertex(Vector3(-2, -3, 4)); - mConvexMeshShape->addVertex(Vector3(-2, 3, -4)); - mConvexMeshShape->addVertex(Vector3(2, 3, -4)); - mConvexMeshShape->addVertex(Vector3(2, 3, 4)); - mConvexMeshShape->addVertex(Vector3(-2, 3, 4)); - mConvexMeshProxyShape = mConvexMeshBody->addCollisionShape(mConvexMeshShape, mShapeTransform); + mConvexMeshShape = new ConvexMeshShape(0.0); // Box of dimension (2, 3, 4) + mConvexMeshShape->addVertex(Vector3(-2, -3, -4)); + mConvexMeshShape->addVertex(Vector3(2, -3, -4)); + mConvexMeshShape->addVertex(Vector3(2, -3, 4)); + mConvexMeshShape->addVertex(Vector3(-2, -3, 4)); + mConvexMeshShape->addVertex(Vector3(-2, 3, -4)); + mConvexMeshShape->addVertex(Vector3(2, 3, -4)); + mConvexMeshShape->addVertex(Vector3(2, 3, 4)); + mConvexMeshShape->addVertex(Vector3(-2, 3, 4)); + mConvexMeshProxyShape = mConvexMeshBody->addCollisionShape(mConvexMeshShape, mShapeTransform); - mConvexMeshShapeBodyEdgesInfo = new ConvexMeshShape(0.0); - mConvexMeshShapeBodyEdgesInfo->addVertex(Vector3(-2, -3, -4)); - mConvexMeshShapeBodyEdgesInfo->addVertex(Vector3(2, -3, -4)); - mConvexMeshShapeBodyEdgesInfo->addVertex(Vector3(2, -3, 4)); - mConvexMeshShapeBodyEdgesInfo->addVertex(Vector3(-2, -3, 4)); - mConvexMeshShapeBodyEdgesInfo->addVertex(Vector3(-2, 3, -4)); - mConvexMeshShapeBodyEdgesInfo->addVertex(Vector3(2, 3, -4)); - mConvexMeshShapeBodyEdgesInfo->addVertex(Vector3(2, 3, 4)); - mConvexMeshShapeBodyEdgesInfo->addVertex(Vector3(-2, 3, 4)); - mConvexMeshShapeBodyEdgesInfo->addEdge(0, 1); - mConvexMeshShapeBodyEdgesInfo->addEdge(1, 2); - mConvexMeshShapeBodyEdgesInfo->addEdge(2, 3); - mConvexMeshShapeBodyEdgesInfo->addEdge(0, 3); - mConvexMeshShapeBodyEdgesInfo->addEdge(4, 5); - mConvexMeshShapeBodyEdgesInfo->addEdge(5, 6); - mConvexMeshShapeBodyEdgesInfo->addEdge(6, 7); - mConvexMeshShapeBodyEdgesInfo->addEdge(4, 7); - mConvexMeshShapeBodyEdgesInfo->addEdge(0, 4); - mConvexMeshShapeBodyEdgesInfo->addEdge(1, 5); - mConvexMeshShapeBodyEdgesInfo->addEdge(2, 6); - mConvexMeshShapeBodyEdgesInfo->addEdge(3, 7); - mConvexMeshShapeBodyEdgesInfo->setIsEdgesInformationUsed(true); - mConvexMeshProxyShapeEdgesInfo = mConvexMeshBodyEdgesInfo->addCollisionShape( - mConvexMeshShapeBodyEdgesInfo, - mShapeTransform); + mConvexMeshShapeBodyEdgesInfo = new ConvexMeshShape(0.0); + mConvexMeshShapeBodyEdgesInfo->addVertex(Vector3(-2, -3, -4)); + mConvexMeshShapeBodyEdgesInfo->addVertex(Vector3(2, -3, -4)); + mConvexMeshShapeBodyEdgesInfo->addVertex(Vector3(2, -3, 4)); + mConvexMeshShapeBodyEdgesInfo->addVertex(Vector3(-2, -3, 4)); + mConvexMeshShapeBodyEdgesInfo->addVertex(Vector3(-2, 3, -4)); + mConvexMeshShapeBodyEdgesInfo->addVertex(Vector3(2, 3, -4)); + mConvexMeshShapeBodyEdgesInfo->addVertex(Vector3(2, 3, 4)); + mConvexMeshShapeBodyEdgesInfo->addVertex(Vector3(-2, 3, 4)); + mConvexMeshShapeBodyEdgesInfo->addEdge(0, 1); + mConvexMeshShapeBodyEdgesInfo->addEdge(1, 2); + mConvexMeshShapeBodyEdgesInfo->addEdge(2, 3); + mConvexMeshShapeBodyEdgesInfo->addEdge(0, 3); + mConvexMeshShapeBodyEdgesInfo->addEdge(4, 5); + mConvexMeshShapeBodyEdgesInfo->addEdge(5, 6); + mConvexMeshShapeBodyEdgesInfo->addEdge(6, 7); + mConvexMeshShapeBodyEdgesInfo->addEdge(4, 7); + mConvexMeshShapeBodyEdgesInfo->addEdge(0, 4); + mConvexMeshShapeBodyEdgesInfo->addEdge(1, 5); + mConvexMeshShapeBodyEdgesInfo->addEdge(2, 6); + mConvexMeshShapeBodyEdgesInfo->addEdge(3, 7); + mConvexMeshShapeBodyEdgesInfo->setIsEdgesInformationUsed(true); + mConvexMeshProxyShapeEdgesInfo = mConvexMeshBodyEdgesInfo->addCollisionShape( + mConvexMeshShapeBodyEdgesInfo, + mShapeTransform); - mCylinderShape = new CylinderShape(3, 8, 0); - mCylinderProxyShape = mCylinderBody->addCollisionShape(mCylinderShape, mShapeTransform); + mCylinderShape = new CylinderShape(3, 8, 0); + mCylinderProxyShape = mCylinderBody->addCollisionShape(mCylinderShape, mShapeTransform); - // Compound shape is a cylinder and a sphere - Vector3 positionShape2(Vector3(4, 2, -3)); - Quaternion orientationShape2(-3 *PI / 8, 1.5 * PI/ 3, PI / 13); - Transform shapeTransform2(positionShape2, orientationShape2); - mLocalShape2ToWorld = mBodyTransform * shapeTransform2; - mCompoundBody->addCollisionShape(mCylinderShape, mShapeTransform); - mCompoundBody->addCollisionShape(mSphereShape, shapeTransform2); - } + // Compound shape is a cylinder and a sphere + Vector3 positionShape2(Vector3(4, 2, -3)); + Quaternion orientationShape2(-3 *PI / 8, 1.5 * PI/ 3, PI / 13); + Transform shapeTransform2(positionShape2, orientationShape2); + mLocalShape2ToWorld = mBodyTransform * shapeTransform2; + mCompoundBody->addCollisionShape(mCylinderShape, mShapeTransform); + mCompoundBody->addCollisionShape(mSphereShape, shapeTransform2); + } - /// Destructor - ~TestPointInside() { - delete mBoxShape; - delete mSphereShape; - delete mCapsuleShape; - delete mConeShape; - delete mConvexMeshShape; - delete mConvexMeshShapeBodyEdgesInfo; - delete mCylinderShape; - } + /// Destructor + ~TestPointInside() { + delete mBoxShape; + delete mSphereShape; + delete mCapsuleShape; + delete mConeShape; + delete mConvexMeshShape; + delete mConvexMeshShapeBodyEdgesInfo; + delete mCylinderShape; + } - /// Run the tests - void run() { - testBox(); - testSphere(); - testCapsule(); - testCone(); - testConvexMesh(); - testCylinder(); - testCompound(); - } + /// Run the tests + void run() { + testBox(); + testSphere(); + testCapsule(); + testCone(); + testConvexMesh(); + testCylinder(); + testCompound(); + } - /// Test the ProxyBoxShape::testPointInside() and - /// CollisionBody::testPointInside() methods - void testBox() { + /// Test the ProxyBoxShape::testPointInside() and + /// CollisionBody::testPointInside() methods + void testBox() { - // Tests with CollisionBody - test(mBoxBody->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 0))); - test(mBoxBody->testPointInside(mLocalShapeToWorld * Vector3(-1.9, 0, 0))); - test(mBoxBody->testPointInside(mLocalShapeToWorld * Vector3(1.9, 0, 0))); - test(mBoxBody->testPointInside(mLocalShapeToWorld * Vector3(0, -2.9, 0))); - test(mBoxBody->testPointInside(mLocalShapeToWorld * Vector3(0, 2.9, 0))); - test(mBoxBody->testPointInside(mLocalShapeToWorld * Vector3(0, 0, -3.9))); - test(mBoxBody->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 3.9))); - test(mBoxBody->testPointInside(mLocalShapeToWorld * Vector3(-1.9, -2.9, -3.9))); - test(mBoxBody->testPointInside(mLocalShapeToWorld * Vector3(1.9, 2.9, 3.9))); - test(mBoxBody->testPointInside(mLocalShapeToWorld * Vector3(-1, -2, -1.5))); - test(mBoxBody->testPointInside(mLocalShapeToWorld * Vector3(-1, 2, -2.5))); - test(mBoxBody->testPointInside(mLocalShapeToWorld * Vector3(1, -2, 3.5))); + // Tests with CollisionBody + test(mBoxBody->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 0))); + test(mBoxBody->testPointInside(mLocalShapeToWorld * Vector3(-1.9, 0, 0))); + test(mBoxBody->testPointInside(mLocalShapeToWorld * Vector3(1.9, 0, 0))); + test(mBoxBody->testPointInside(mLocalShapeToWorld * Vector3(0, -2.9, 0))); + test(mBoxBody->testPointInside(mLocalShapeToWorld * Vector3(0, 2.9, 0))); + test(mBoxBody->testPointInside(mLocalShapeToWorld * Vector3(0, 0, -3.9))); + test(mBoxBody->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 3.9))); + test(mBoxBody->testPointInside(mLocalShapeToWorld * Vector3(-1.9, -2.9, -3.9))); + test(mBoxBody->testPointInside(mLocalShapeToWorld * Vector3(1.9, 2.9, 3.9))); + test(mBoxBody->testPointInside(mLocalShapeToWorld * Vector3(-1, -2, -1.5))); + test(mBoxBody->testPointInside(mLocalShapeToWorld * Vector3(-1, 2, -2.5))); + test(mBoxBody->testPointInside(mLocalShapeToWorld * Vector3(1, -2, 3.5))); - test(!mBoxBody->testPointInside(mLocalShapeToWorld * Vector3(-2.1, 0, 0))); - test(!mBoxBody->testPointInside(mLocalShapeToWorld * Vector3(2.1, 0, 0))); - test(!mBoxBody->testPointInside(mLocalShapeToWorld * Vector3(0, -3.1, 0))); - test(!mBoxBody->testPointInside(mLocalShapeToWorld * Vector3(0, 3.1, 0))); - test(!mBoxBody->testPointInside(mLocalShapeToWorld * Vector3(0, 0, -4.1))); - test(!mBoxBody->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 4.1))); - test(!mBoxBody->testPointInside(mLocalShapeToWorld * Vector3(-2.1, -3.1, -4.1))); - test(!mBoxBody->testPointInside(mLocalShapeToWorld * Vector3(2.1, 3.1, 4.1))); - test(!mBoxBody->testPointInside(mLocalShapeToWorld * Vector3(-10, -2, -1.5))); - test(!mBoxBody->testPointInside(mLocalShapeToWorld * Vector3(-1, 4, -2.5))); - test(!mBoxBody->testPointInside(mLocalShapeToWorld * Vector3(1, -2, 4.5))); + test(!mBoxBody->testPointInside(mLocalShapeToWorld * Vector3(-2.1, 0, 0))); + test(!mBoxBody->testPointInside(mLocalShapeToWorld * Vector3(2.1, 0, 0))); + test(!mBoxBody->testPointInside(mLocalShapeToWorld * Vector3(0, -3.1, 0))); + test(!mBoxBody->testPointInside(mLocalShapeToWorld * Vector3(0, 3.1, 0))); + test(!mBoxBody->testPointInside(mLocalShapeToWorld * Vector3(0, 0, -4.1))); + test(!mBoxBody->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 4.1))); + test(!mBoxBody->testPointInside(mLocalShapeToWorld * Vector3(-2.1, -3.1, -4.1))); + test(!mBoxBody->testPointInside(mLocalShapeToWorld * Vector3(2.1, 3.1, 4.1))); + test(!mBoxBody->testPointInside(mLocalShapeToWorld * Vector3(-10, -2, -1.5))); + test(!mBoxBody->testPointInside(mLocalShapeToWorld * Vector3(-1, 4, -2.5))); + test(!mBoxBody->testPointInside(mLocalShapeToWorld * Vector3(1, -2, 4.5))); - // Tests with ProxyBoxShape - test(mBoxProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 0))); - test(mBoxProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-1.9, 0, 0))); - test(mBoxProxyShape->testPointInside(mLocalShapeToWorld * Vector3(1.9, 0, 0))); - test(mBoxProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, -2.9, 0))); - test(mBoxProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 2.9, 0))); - test(mBoxProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, -3.9))); - test(mBoxProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 3.9))); - test(mBoxProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-1.9, -2.9, -3.9))); - test(mBoxProxyShape->testPointInside(mLocalShapeToWorld * Vector3(1.9, 2.9, 3.9))); - test(mBoxProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-1, -2, -1.5))); - test(mBoxProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-1, 2, -2.5))); - test(mBoxProxyShape->testPointInside(mLocalShapeToWorld * Vector3(1, -2, 3.5))); + // Tests with ProxyBoxShape + test(mBoxProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 0))); + test(mBoxProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-1.9, 0, 0))); + test(mBoxProxyShape->testPointInside(mLocalShapeToWorld * Vector3(1.9, 0, 0))); + test(mBoxProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, -2.9, 0))); + test(mBoxProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 2.9, 0))); + test(mBoxProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, -3.9))); + test(mBoxProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 3.9))); + test(mBoxProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-1.9, -2.9, -3.9))); + test(mBoxProxyShape->testPointInside(mLocalShapeToWorld * Vector3(1.9, 2.9, 3.9))); + test(mBoxProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-1, -2, -1.5))); + test(mBoxProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-1, 2, -2.5))); + test(mBoxProxyShape->testPointInside(mLocalShapeToWorld * Vector3(1, -2, 3.5))); - test(!mBoxProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-2.1, 0, 0))); - test(!mBoxProxyShape->testPointInside(mLocalShapeToWorld * Vector3(2.1, 0, 0))); - test(!mBoxProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, -3.1, 0))); - test(!mBoxProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 3.1, 0))); - test(!mBoxProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, -4.1))); - test(!mBoxProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 4.1))); - test(!mBoxProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-2.1, -3.1, -4.1))); - test(!mBoxProxyShape->testPointInside(mLocalShapeToWorld * Vector3(2.1, 3.1, 4.1))); - test(!mBoxProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-10, -2, -1.5))); - test(!mBoxProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-1, 4, -2.5))); - test(!mBoxProxyShape->testPointInside(mLocalShapeToWorld * Vector3(1, -2, 4.5))); - } + test(!mBoxProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-2.1, 0, 0))); + test(!mBoxProxyShape->testPointInside(mLocalShapeToWorld * Vector3(2.1, 0, 0))); + test(!mBoxProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, -3.1, 0))); + test(!mBoxProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 3.1, 0))); + test(!mBoxProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, -4.1))); + test(!mBoxProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 4.1))); + test(!mBoxProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-2.1, -3.1, -4.1))); + test(!mBoxProxyShape->testPointInside(mLocalShapeToWorld * Vector3(2.1, 3.1, 4.1))); + test(!mBoxProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-10, -2, -1.5))); + test(!mBoxProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-1, 4, -2.5))); + test(!mBoxProxyShape->testPointInside(mLocalShapeToWorld * Vector3(1, -2, 4.5))); + } - /// Test the ProxySphereShape::testPointInside() and - /// CollisionBody::testPointInside() methods - void testSphere() { + /// Test the ProxySphereShape::testPointInside() and + /// CollisionBody::testPointInside() methods + void testSphere() { - // Tests with CollisionBody - test(mSphereBody->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 0))); - test(mSphereBody->testPointInside(mLocalShapeToWorld * Vector3(2.9, 0, 0))); - test(mSphereBody->testPointInside(mLocalShapeToWorld * Vector3(-2.9, 0, 0))); - test(mSphereBody->testPointInside(mLocalShapeToWorld * Vector3(0, 2.9, 0))); - test(mSphereBody->testPointInside(mLocalShapeToWorld * Vector3(0, -2.9, 0))); - test(mSphereBody->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 2.9))); - test(mSphereBody->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 2.9))); - test(mSphereBody->testPointInside(mLocalShapeToWorld * Vector3(-1, -2, -1.5))); - test(mSphereBody->testPointInside(mLocalShapeToWorld * Vector3(-1, 2, -1.5))); - test(mSphereBody->testPointInside(mLocalShapeToWorld * Vector3(1, -2, 1.5))); + // Tests with CollisionBody + test(mSphereBody->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 0))); + test(mSphereBody->testPointInside(mLocalShapeToWorld * Vector3(2.9, 0, 0))); + test(mSphereBody->testPointInside(mLocalShapeToWorld * Vector3(-2.9, 0, 0))); + test(mSphereBody->testPointInside(mLocalShapeToWorld * Vector3(0, 2.9, 0))); + test(mSphereBody->testPointInside(mLocalShapeToWorld * Vector3(0, -2.9, 0))); + test(mSphereBody->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 2.9))); + test(mSphereBody->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 2.9))); + test(mSphereBody->testPointInside(mLocalShapeToWorld * Vector3(-1, -2, -1.5))); + test(mSphereBody->testPointInside(mLocalShapeToWorld * Vector3(-1, 2, -1.5))); + test(mSphereBody->testPointInside(mLocalShapeToWorld * Vector3(1, -2, 1.5))); - test(!mSphereBody->testPointInside(mLocalShapeToWorld * Vector3(3.1, 0, 0))); - test(!mSphereBody->testPointInside(mLocalShapeToWorld * Vector3(-3.1, 0, 0))); - test(!mSphereBody->testPointInside(mLocalShapeToWorld * Vector3(0, 3.1, 0))); - test(!mSphereBody->testPointInside(mLocalShapeToWorld * Vector3(0, -3.1, 0))); - test(!mSphereBody->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 3.1))); - test(!mSphereBody->testPointInside(mLocalShapeToWorld * Vector3(0, 0, -3.1))); - test(!mSphereBody->testPointInside(mLocalShapeToWorld * Vector3(-2, -2, -2))); - test(!mSphereBody->testPointInside(mLocalShapeToWorld * Vector3(-2, 2, -1.5))); - test(!mSphereBody->testPointInside(mLocalShapeToWorld * Vector3(1.5, -2, 2.5))); + test(!mSphereBody->testPointInside(mLocalShapeToWorld * Vector3(3.1, 0, 0))); + test(!mSphereBody->testPointInside(mLocalShapeToWorld * Vector3(-3.1, 0, 0))); + test(!mSphereBody->testPointInside(mLocalShapeToWorld * Vector3(0, 3.1, 0))); + test(!mSphereBody->testPointInside(mLocalShapeToWorld * Vector3(0, -3.1, 0))); + test(!mSphereBody->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 3.1))); + test(!mSphereBody->testPointInside(mLocalShapeToWorld * Vector3(0, 0, -3.1))); + test(!mSphereBody->testPointInside(mLocalShapeToWorld * Vector3(-2, -2, -2))); + test(!mSphereBody->testPointInside(mLocalShapeToWorld * Vector3(-2, 2, -1.5))); + test(!mSphereBody->testPointInside(mLocalShapeToWorld * Vector3(1.5, -2, 2.5))); - // Tests with ProxySphereShape - test(mSphereProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 0))); - test(mSphereProxyShape->testPointInside(mLocalShapeToWorld * Vector3(2.9, 0, 0))); - test(mSphereProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-2.9, 0, 0))); - test(mSphereProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 2.9, 0))); - test(mSphereProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, -2.9, 0))); - test(mSphereProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 2.9))); - test(mSphereProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 2.9))); - test(mSphereProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-1, -2, -1.5))); - test(mSphereProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-1, 2, -1.5))); - test(mSphereProxyShape->testPointInside(mLocalShapeToWorld * Vector3(1, -2, 1.5))); + // Tests with ProxySphereShape + test(mSphereProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 0))); + test(mSphereProxyShape->testPointInside(mLocalShapeToWorld * Vector3(2.9, 0, 0))); + test(mSphereProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-2.9, 0, 0))); + test(mSphereProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 2.9, 0))); + test(mSphereProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, -2.9, 0))); + test(mSphereProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 2.9))); + test(mSphereProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 2.9))); + test(mSphereProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-1, -2, -1.5))); + test(mSphereProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-1, 2, -1.5))); + test(mSphereProxyShape->testPointInside(mLocalShapeToWorld * Vector3(1, -2, 1.5))); - test(!mSphereProxyShape->testPointInside(mLocalShapeToWorld * Vector3(3.1, 0, 0))); - test(!mSphereProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-3.1, 0, 0))); - test(!mSphereProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 3.1, 0))); - test(!mSphereProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, -3.1, 0))); - test(!mSphereProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 3.1))); - test(!mSphereProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, -3.1))); - test(!mSphereProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-2, -2, -2))); - test(!mSphereProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-2, 2, -1.5))); - test(!mSphereProxyShape->testPointInside(mLocalShapeToWorld * Vector3(1.5, -2, 2.5))); - } + test(!mSphereProxyShape->testPointInside(mLocalShapeToWorld * Vector3(3.1, 0, 0))); + test(!mSphereProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-3.1, 0, 0))); + test(!mSphereProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 3.1, 0))); + test(!mSphereProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, -3.1, 0))); + test(!mSphereProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 3.1))); + test(!mSphereProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, -3.1))); + test(!mSphereProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-2, -2, -2))); + test(!mSphereProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-2, 2, -1.5))); + test(!mSphereProxyShape->testPointInside(mLocalShapeToWorld * Vector3(1.5, -2, 2.5))); + } - /// Test the ProxyCapsuleShape::testPointInside() and - /// CollisionBody::testPointInside() methods - void testCapsule() { + /// Test the ProxyCapsuleShape::testPointInside() and + /// CollisionBody::testPointInside() methods + void testCapsule() { - // Tests with CollisionBody - test(mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 0))); - test(mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(0, 5, 0))); - test(mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(0, -5, 0))); - test(mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(0, -6.9, 0))); - test(mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(0, 6.9, 0))); - test(mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 1.9))); - test(mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(0, 0, -1.9))); - test(mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(1.9, 0, 0))); - test(mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(-1.9, 0, 0))); - test(mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(0.9, 0, 0.9))); - test(mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(0.9, 0, -0.9))); - test(mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(0, 5, 1.9))); - test(mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(0, 5, -1.9))); - test(mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(1.9, 5, 0))); - test(mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(-1.9, 5, 0))); - test(mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(0.9, 5, 0.9))); - test(mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(0.9, 5, -0.9))); - test(mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(0, -5, 1.9))); - test(mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(0, -5, -1.9))); - test(mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(1.9, -5, 0))); - test(mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(-1.9, -5, 0))); - test(mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(0.9, -5, 0.9))); - test(mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(0.9, -5, -0.9))); - test(mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(-1.7, -4, -0.9))); - test(mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(-1, 2, 0.4))); - test(mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(1.3, 1, 1.5))); + // Tests with CollisionBody + test(mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 0))); + test(mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(0, 5, 0))); + test(mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(0, -5, 0))); + test(mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(0, -6.9, 0))); + test(mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(0, 6.9, 0))); + test(mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 1.9))); + test(mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(0, 0, -1.9))); + test(mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(1.9, 0, 0))); + test(mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(-1.9, 0, 0))); + test(mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(0.9, 0, 0.9))); + test(mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(0.9, 0, -0.9))); + test(mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(0, 5, 1.9))); + test(mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(0, 5, -1.9))); + test(mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(1.9, 5, 0))); + test(mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(-1.9, 5, 0))); + test(mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(0.9, 5, 0.9))); + test(mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(0.9, 5, -0.9))); + test(mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(0, -5, 1.9))); + test(mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(0, -5, -1.9))); + test(mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(1.9, -5, 0))); + test(mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(-1.9, -5, 0))); + test(mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(0.9, -5, 0.9))); + test(mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(0.9, -5, -0.9))); + test(mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(-1.7, -4, -0.9))); + test(mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(-1, 2, 0.4))); + test(mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(1.3, 1, 1.5))); - test(!mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(0, -7.1, 0))); - test(!mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(0, 7.1, 0))); - test(!mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 2.1))); - test(!mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(0, 0, -2.1))); - test(!mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(2.1, 0, 0))); - test(!mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(-2.1, 0, 0))); - test(!mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(0, 5, 2.1))); - test(!mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(0, 5, -2.1))); - test(!mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(2.1, 5, 0))); - test(!mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(-2.1, 5, 0))); - test(!mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(1.5, 5, 1.6))); - test(!mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(1.5, 5, -1.7))); - test(!mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(0, -5, 2.1))); - test(!mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(0, -5, -2.1))); - test(!mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(2.1, -5, 0))); - test(!mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(-2.1, -5, 0))); - test(!mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(1.5, -5, 1.6))); - test(!mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(1.5, -5, -1.7))); + test(!mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(0, -7.1, 0))); + test(!mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(0, 7.1, 0))); + test(!mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 2.1))); + test(!mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(0, 0, -2.1))); + test(!mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(2.1, 0, 0))); + test(!mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(-2.1, 0, 0))); + test(!mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(0, 5, 2.1))); + test(!mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(0, 5, -2.1))); + test(!mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(2.1, 5, 0))); + test(!mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(-2.1, 5, 0))); + test(!mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(1.5, 5, 1.6))); + test(!mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(1.5, 5, -1.7))); + test(!mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(0, -5, 2.1))); + test(!mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(0, -5, -2.1))); + test(!mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(2.1, -5, 0))); + test(!mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(-2.1, -5, 0))); + test(!mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(1.5, -5, 1.6))); + test(!mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(1.5, -5, -1.7))); - // Tests with ProxyCapsuleShape - test(mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 0))); - test(mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 5, 0))); - test(mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, -5, 0))); - test(mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, -6.9, 0))); - test(mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 6.9, 0))); - test(mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 1.9))); - test(mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, -1.9))); - test(mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(1.9, 0, 0))); - test(mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-1.9, 0, 0))); - test(mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0.9, 0, 0.9))); - test(mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0.9, 0, -0.9))); - test(mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 5, 1.9))); - test(mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 5, -1.9))); - test(mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(1.9, 5, 0))); - test(mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-1.9, 5, 0))); - test(mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0.9, 5, 0.9))); - test(mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0.9, 5, -0.9))); - test(mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, -5, 1.9))); - test(mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, -5, -1.9))); - test(mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(1.9, -5, 0))); - test(mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-1.9, -5, 0))); - test(mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0.9, -5, 0.9))); - test(mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0.9, -5, -0.9))); - test(mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-1.7, -4, -0.9))); - test(mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-1, 2, 0.4))); - test(mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(1.3, 1, 1.5))); + // Tests with ProxyCapsuleShape + test(mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 0))); + test(mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 5, 0))); + test(mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, -5, 0))); + test(mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, -6.9, 0))); + test(mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 6.9, 0))); + test(mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 1.9))); + test(mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, -1.9))); + test(mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(1.9, 0, 0))); + test(mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-1.9, 0, 0))); + test(mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0.9, 0, 0.9))); + test(mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0.9, 0, -0.9))); + test(mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 5, 1.9))); + test(mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 5, -1.9))); + test(mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(1.9, 5, 0))); + test(mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-1.9, 5, 0))); + test(mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0.9, 5, 0.9))); + test(mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0.9, 5, -0.9))); + test(mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, -5, 1.9))); + test(mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, -5, -1.9))); + test(mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(1.9, -5, 0))); + test(mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-1.9, -5, 0))); + test(mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0.9, -5, 0.9))); + test(mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0.9, -5, -0.9))); + test(mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-1.7, -4, -0.9))); + test(mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-1, 2, 0.4))); + test(mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(1.3, 1, 1.5))); - test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, -7.1, 0))); - test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 7.1, 0))); - test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 2.1))); - test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, -2.1))); - test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(2.1, 0, 0))); - test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-2.1, 0, 0))); - test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 5, 2.1))); - test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 5, -2.1))); - test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(2.1, 5, 0))); - test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-2.1, 5, 0))); - test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(1.5, 5, 1.6))); - test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(1.5, 5, -1.7))); - test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, -5, 2.1))); - test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, -5, -2.1))); - test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(2.1, -5, 0))); - test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-2.1, -5, 0))); - test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(1.5, -5, 1.6))); - test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(1.5, -5, -1.7))); - } + test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, -7.1, 0))); + test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 7.1, 0))); + test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 2.1))); + test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, -2.1))); + test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(2.1, 0, 0))); + test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-2.1, 0, 0))); + test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 5, 2.1))); + test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 5, -2.1))); + test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(2.1, 5, 0))); + test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-2.1, 5, 0))); + test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(1.5, 5, 1.6))); + test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(1.5, 5, -1.7))); + test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, -5, 2.1))); + test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, -5, -2.1))); + test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(2.1, -5, 0))); + test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-2.1, -5, 0))); + test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(1.5, -5, 1.6))); + test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(1.5, -5, -1.7))); + } - /// Test the ProxyConeShape::testPointInside() and - /// CollisionBody::testPointInside() methods - void testCone() { + /// Test the ProxyConeShape::testPointInside() and + /// CollisionBody::testPointInside() methods + void testCone() { - // Tests with CollisionBody - test(mConeBody->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 0))); - test(mConeBody->testPointInside(mLocalShapeToWorld * Vector3(0.9, 0, 0))); - test(mConeBody->testPointInside(mLocalShapeToWorld * Vector3(-0.9, 0, 0))); - test(mConeBody->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 0.9))); - test(mConeBody->testPointInside(mLocalShapeToWorld * Vector3(0, 0, -0.9))); - test(mConeBody->testPointInside(mLocalShapeToWorld * Vector3(0.6, 0, -0.7))); - test(mConeBody->testPointInside(mLocalShapeToWorld * Vector3(0.6, 0, 0.7))); - test(mConeBody->testPointInside(mLocalShapeToWorld * Vector3(-0.6, 0, -0.7))); - test(mConeBody->testPointInside(mLocalShapeToWorld * Vector3(-0.6, 0, 0.7))); - test(mConeBody->testPointInside(mLocalShapeToWorld * Vector3(0, 2.9, 0))); - test(mConeBody->testPointInside(mLocalShapeToWorld * Vector3(0, -2.9, 0))); - test(mConeBody->testPointInside(mLocalShapeToWorld * Vector3(1.96, -2.9, 0))); - test(mConeBody->testPointInside(mLocalShapeToWorld * Vector3(-1.96, -2.9, 0))); - test(mConeBody->testPointInside(mLocalShapeToWorld * Vector3(0, -2.9, 1.96))); - test(mConeBody->testPointInside(mLocalShapeToWorld * Vector3(0, -2.9, -1.96))); - test(mConeBody->testPointInside(mLocalShapeToWorld * Vector3(1.3, -2.9, -1.4))); - test(mConeBody->testPointInside(mLocalShapeToWorld * Vector3(-1.3, -2.9, 1.4))); + // Tests with CollisionBody + test(mConeBody->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 0))); + test(mConeBody->testPointInside(mLocalShapeToWorld * Vector3(0.9, 0, 0))); + test(mConeBody->testPointInside(mLocalShapeToWorld * Vector3(-0.9, 0, 0))); + test(mConeBody->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 0.9))); + test(mConeBody->testPointInside(mLocalShapeToWorld * Vector3(0, 0, -0.9))); + test(mConeBody->testPointInside(mLocalShapeToWorld * Vector3(0.6, 0, -0.7))); + test(mConeBody->testPointInside(mLocalShapeToWorld * Vector3(0.6, 0, 0.7))); + test(mConeBody->testPointInside(mLocalShapeToWorld * Vector3(-0.6, 0, -0.7))); + test(mConeBody->testPointInside(mLocalShapeToWorld * Vector3(-0.6, 0, 0.7))); + test(mConeBody->testPointInside(mLocalShapeToWorld * Vector3(0, 2.9, 0))); + test(mConeBody->testPointInside(mLocalShapeToWorld * Vector3(0, -2.9, 0))); + test(mConeBody->testPointInside(mLocalShapeToWorld * Vector3(1.96, -2.9, 0))); + test(mConeBody->testPointInside(mLocalShapeToWorld * Vector3(-1.96, -2.9, 0))); + test(mConeBody->testPointInside(mLocalShapeToWorld * Vector3(0, -2.9, 1.96))); + test(mConeBody->testPointInside(mLocalShapeToWorld * Vector3(0, -2.9, -1.96))); + test(mConeBody->testPointInside(mLocalShapeToWorld * Vector3(1.3, -2.9, -1.4))); + test(mConeBody->testPointInside(mLocalShapeToWorld * Vector3(-1.3, -2.9, 1.4))); - test(!mConeBody->testPointInside(mLocalShapeToWorld * Vector3(1.1, 0, 0))); - test(!mConeBody->testPointInside(mLocalShapeToWorld * Vector3(-1.1, 0, 0))); - test(!mConeBody->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 1.1))); - test(!mConeBody->testPointInside(mLocalShapeToWorld * Vector3(0, 0, -1.1))); - test(!mConeBody->testPointInside(mLocalShapeToWorld * Vector3(0.8, 0, -0.8))); - test(!mConeBody->testPointInside(mLocalShapeToWorld * Vector3(0.8, 0, 0.8))); - test(!mConeBody->testPointInside(mLocalShapeToWorld * Vector3(-0.8, 0, -0.8))); - test(!mConeBody->testPointInside(mLocalShapeToWorld * Vector3(-0.8, 0, 0.8))); - test(!mConeBody->testPointInside(mLocalShapeToWorld * Vector3(0, 3.1, 0))); - test(!mConeBody->testPointInside(mLocalShapeToWorld * Vector3(0, -3.1, 0))); - test(!mConeBody->testPointInside(mLocalShapeToWorld * Vector3(1.97, -2.9, 0))); - test(!mConeBody->testPointInside(mLocalShapeToWorld * Vector3(-1.97, -2.9, 0))); - test(!mConeBody->testPointInside(mLocalShapeToWorld * Vector3(0, -2.9, 1.97))); - test(!mConeBody->testPointInside(mLocalShapeToWorld * Vector3(0, -2.9, -1.97))); - test(!mConeBody->testPointInside(mLocalShapeToWorld * Vector3(1.5, -2.9, -1.5))); - test(!mConeBody->testPointInside(mLocalShapeToWorld * Vector3(-1.5, -2.9, 1.5))); + test(!mConeBody->testPointInside(mLocalShapeToWorld * Vector3(1.1, 0, 0))); + test(!mConeBody->testPointInside(mLocalShapeToWorld * Vector3(-1.1, 0, 0))); + test(!mConeBody->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 1.1))); + test(!mConeBody->testPointInside(mLocalShapeToWorld * Vector3(0, 0, -1.1))); + test(!mConeBody->testPointInside(mLocalShapeToWorld * Vector3(0.8, 0, -0.8))); + test(!mConeBody->testPointInside(mLocalShapeToWorld * Vector3(0.8, 0, 0.8))); + test(!mConeBody->testPointInside(mLocalShapeToWorld * Vector3(-0.8, 0, -0.8))); + test(!mConeBody->testPointInside(mLocalShapeToWorld * Vector3(-0.8, 0, 0.8))); + test(!mConeBody->testPointInside(mLocalShapeToWorld * Vector3(0, 3.1, 0))); + test(!mConeBody->testPointInside(mLocalShapeToWorld * Vector3(0, -3.1, 0))); + test(!mConeBody->testPointInside(mLocalShapeToWorld * Vector3(1.97, -2.9, 0))); + test(!mConeBody->testPointInside(mLocalShapeToWorld * Vector3(-1.97, -2.9, 0))); + test(!mConeBody->testPointInside(mLocalShapeToWorld * Vector3(0, -2.9, 1.97))); + test(!mConeBody->testPointInside(mLocalShapeToWorld * Vector3(0, -2.9, -1.97))); + test(!mConeBody->testPointInside(mLocalShapeToWorld * Vector3(1.5, -2.9, -1.5))); + test(!mConeBody->testPointInside(mLocalShapeToWorld * Vector3(-1.5, -2.9, 1.5))); - // Tests with ProxyConeShape - test(mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 0))); - test(mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0.9, 0, 0))); - test(mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-0.9, 0, 0))); - test(mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 0.9))); - test(mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, -0.9))); - test(mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0.6, 0, -0.7))); - test(mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0.6, 0, 0.7))); - test(mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-0.6, 0, -0.7))); - test(mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-0.6, 0, 0.7))); - test(mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 2.9, 0))); - test(mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, -2.9, 0))); - test(mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(1.96, -2.9, 0))); - test(mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-1.96, -2.9, 0))); - test(mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, -2.9, 1.96))); - test(mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, -2.9, -1.96))); - test(mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(1.3, -2.9, -1.4))); - test(mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-1.3, -2.9, 1.4))); + // Tests with ProxyConeShape + test(mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 0))); + test(mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0.9, 0, 0))); + test(mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-0.9, 0, 0))); + test(mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 0.9))); + test(mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, -0.9))); + test(mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0.6, 0, -0.7))); + test(mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0.6, 0, 0.7))); + test(mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-0.6, 0, -0.7))); + test(mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-0.6, 0, 0.7))); + test(mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 2.9, 0))); + test(mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, -2.9, 0))); + test(mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(1.96, -2.9, 0))); + test(mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-1.96, -2.9, 0))); + test(mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, -2.9, 1.96))); + test(mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, -2.9, -1.96))); + test(mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(1.3, -2.9, -1.4))); + test(mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-1.3, -2.9, 1.4))); - test(!mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(1.1, 0, 0))); - test(!mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-1.1, 0, 0))); - test(!mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 1.1))); - test(!mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, -1.1))); - test(!mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0.8, 0, -0.8))); - test(!mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0.8, 0, 0.8))); - test(!mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-0.8, 0, -0.8))); - test(!mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-0.8, 0, 0.8))); - test(!mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 3.1, 0))); - test(!mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, -3.1, 0))); - test(!mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(1.97, -2.9, 0))); - test(!mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-1.97, -2.9, 0))); - test(!mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, -2.9, 1.97))); - test(!mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, -2.9, -1.97))); - test(!mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(1.5, -2.9, -1.5))); - test(!mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-1.5, -2.9, 1.5))); - } + test(!mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(1.1, 0, 0))); + test(!mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-1.1, 0, 0))); + test(!mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 1.1))); + test(!mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, -1.1))); + test(!mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0.8, 0, -0.8))); + test(!mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0.8, 0, 0.8))); + test(!mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-0.8, 0, -0.8))); + test(!mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-0.8, 0, 0.8))); + test(!mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 3.1, 0))); + test(!mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, -3.1, 0))); + test(!mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(1.97, -2.9, 0))); + test(!mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-1.97, -2.9, 0))); + test(!mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, -2.9, 1.97))); + test(!mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, -2.9, -1.97))); + test(!mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(1.5, -2.9, -1.5))); + test(!mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-1.5, -2.9, 1.5))); + } - /// Test the ProxyConvexMeshShape::testPointInside() and - /// CollisionBody::testPointInside() methods - void testConvexMesh() { + /// Test the ProxyConvexMeshShape::testPointInside() and + /// CollisionBody::testPointInside() methods + void testConvexMesh() { - // ----- Tests without using edges information ----- // + // ----- Tests without using edges information ----- // - // Tests with CollisionBody - test(mConvexMeshBody->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 0))); - test(mConvexMeshBody->testPointInside(mLocalShapeToWorld * Vector3(-1.9, 0, 0))); - test(mConvexMeshBody->testPointInside(mLocalShapeToWorld * Vector3(1.9, 0, 0))); - test(mConvexMeshBody->testPointInside(mLocalShapeToWorld * Vector3(0, -2.9, 0))); - test(mConvexMeshBody->testPointInside(mLocalShapeToWorld * Vector3(0, 2.9, 0))); - test(mConvexMeshBody->testPointInside(mLocalShapeToWorld * Vector3(0, 0, -3.9))); - test(mConvexMeshBody->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 3.9))); - test(mConvexMeshBody->testPointInside(mLocalShapeToWorld * Vector3(-1.9, -2.9, -3.9))); - test(mConvexMeshBody->testPointInside(mLocalShapeToWorld * Vector3(1.9, 2.9, 3.9))); - test(mConvexMeshBody->testPointInside(mLocalShapeToWorld * Vector3(-1, -2, -1.5))); - test(mConvexMeshBody->testPointInside(mLocalShapeToWorld * Vector3(-1, 2, -2.5))); - test(mConvexMeshBody->testPointInside(mLocalShapeToWorld * Vector3(1, -2, 3.5))); + // Tests with CollisionBody + test(mConvexMeshBody->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 0))); + test(mConvexMeshBody->testPointInside(mLocalShapeToWorld * Vector3(-1.9, 0, 0))); + test(mConvexMeshBody->testPointInside(mLocalShapeToWorld * Vector3(1.9, 0, 0))); + test(mConvexMeshBody->testPointInside(mLocalShapeToWorld * Vector3(0, -2.9, 0))); + test(mConvexMeshBody->testPointInside(mLocalShapeToWorld * Vector3(0, 2.9, 0))); + test(mConvexMeshBody->testPointInside(mLocalShapeToWorld * Vector3(0, 0, -3.9))); + test(mConvexMeshBody->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 3.9))); + test(mConvexMeshBody->testPointInside(mLocalShapeToWorld * Vector3(-1.9, -2.9, -3.9))); + test(mConvexMeshBody->testPointInside(mLocalShapeToWorld * Vector3(1.9, 2.9, 3.9))); + test(mConvexMeshBody->testPointInside(mLocalShapeToWorld * Vector3(-1, -2, -1.5))); + test(mConvexMeshBody->testPointInside(mLocalShapeToWorld * Vector3(-1, 2, -2.5))); + test(mConvexMeshBody->testPointInside(mLocalShapeToWorld * Vector3(1, -2, 3.5))); - test(!mConvexMeshBody->testPointInside(mLocalShapeToWorld * Vector3(-2.1, 0, 0))); - test(!mConvexMeshBody->testPointInside(mLocalShapeToWorld * Vector3(2.1, 0, 0))); - test(!mConvexMeshBody->testPointInside(mLocalShapeToWorld * Vector3(0, -3.1, 0))); - test(!mConvexMeshBody->testPointInside(mLocalShapeToWorld * Vector3(0, 3.1, 0))); - test(!mConvexMeshBody->testPointInside(mLocalShapeToWorld * Vector3(0, 0, -4.1))); - test(!mConvexMeshBody->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 4.1))); - test(!mConvexMeshBody->testPointInside(mLocalShapeToWorld * Vector3(-2.1, -3.1, -4.1))); - test(!mConvexMeshBody->testPointInside(mLocalShapeToWorld * Vector3(2.1, 3.1, 4.1))); - test(!mConvexMeshBody->testPointInside(mLocalShapeToWorld * Vector3(-10, -2, -1.5))); - test(!mConvexMeshBody->testPointInside(mLocalShapeToWorld * Vector3(-1, 4, -2.5))); - test(!mConvexMeshBody->testPointInside(mLocalShapeToWorld * Vector3(1, -2, 4.5))); + test(!mConvexMeshBody->testPointInside(mLocalShapeToWorld * Vector3(-2.1, 0, 0))); + test(!mConvexMeshBody->testPointInside(mLocalShapeToWorld * Vector3(2.1, 0, 0))); + test(!mConvexMeshBody->testPointInside(mLocalShapeToWorld * Vector3(0, -3.1, 0))); + test(!mConvexMeshBody->testPointInside(mLocalShapeToWorld * Vector3(0, 3.1, 0))); + test(!mConvexMeshBody->testPointInside(mLocalShapeToWorld * Vector3(0, 0, -4.1))); + test(!mConvexMeshBody->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 4.1))); + test(!mConvexMeshBody->testPointInside(mLocalShapeToWorld * Vector3(-2.1, -3.1, -4.1))); + test(!mConvexMeshBody->testPointInside(mLocalShapeToWorld * Vector3(2.1, 3.1, 4.1))); + test(!mConvexMeshBody->testPointInside(mLocalShapeToWorld * Vector3(-10, -2, -1.5))); + test(!mConvexMeshBody->testPointInside(mLocalShapeToWorld * Vector3(-1, 4, -2.5))); + test(!mConvexMeshBody->testPointInside(mLocalShapeToWorld * Vector3(1, -2, 4.5))); - // Tests with ProxyConvexMeshShape - test(mConvexMeshProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 0))); - test(mConvexMeshProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-1.9, 0, 0))); - test(mConvexMeshProxyShape->testPointInside(mLocalShapeToWorld * Vector3(1.9, 0, 0))); - test(mConvexMeshProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, -2.9, 0))); - test(mConvexMeshProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 2.9, 0))); - test(mConvexMeshProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, -3.9))); - test(mConvexMeshProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 3.9))); - test(mConvexMeshProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-1.9, -2.9, -3.9))); - test(mConvexMeshProxyShape->testPointInside(mLocalShapeToWorld * Vector3(1.9, 2.9, 3.9))); - test(mConvexMeshProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-1, -2, -1.5))); - test(mConvexMeshProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-1, 2, -2.5))); - test(mConvexMeshProxyShape->testPointInside(mLocalShapeToWorld * Vector3(1, -2, 3.5))); + // Tests with ProxyConvexMeshShape + test(mConvexMeshProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 0))); + test(mConvexMeshProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-1.9, 0, 0))); + test(mConvexMeshProxyShape->testPointInside(mLocalShapeToWorld * Vector3(1.9, 0, 0))); + test(mConvexMeshProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, -2.9, 0))); + test(mConvexMeshProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 2.9, 0))); + test(mConvexMeshProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, -3.9))); + test(mConvexMeshProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 3.9))); + test(mConvexMeshProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-1.9, -2.9, -3.9))); + test(mConvexMeshProxyShape->testPointInside(mLocalShapeToWorld * Vector3(1.9, 2.9, 3.9))); + test(mConvexMeshProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-1, -2, -1.5))); + test(mConvexMeshProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-1, 2, -2.5))); + test(mConvexMeshProxyShape->testPointInside(mLocalShapeToWorld * Vector3(1, -2, 3.5))); - test(!mConvexMeshProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-2.1, 0, 0))); - test(!mConvexMeshProxyShape->testPointInside(mLocalShapeToWorld * Vector3(2.1, 0, 0))); - test(!mConvexMeshProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, -3.1, 0))); - test(!mConvexMeshProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 3.1, 0))); - test(!mConvexMeshProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, -4.1))); - test(!mConvexMeshProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 4.1))); - test(!mConvexMeshProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-2.1, -3.1, -4.1))); - test(!mConvexMeshProxyShape->testPointInside(mLocalShapeToWorld * Vector3(2.1, 3.1, 4.1))); - test(!mConvexMeshProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-10, -2, -1.5))); - test(!mConvexMeshProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-1, 4, -2.5))); - test(!mConvexMeshProxyShape->testPointInside(mLocalShapeToWorld * Vector3(1, -2, 4.5))); + test(!mConvexMeshProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-2.1, 0, 0))); + test(!mConvexMeshProxyShape->testPointInside(mLocalShapeToWorld * Vector3(2.1, 0, 0))); + test(!mConvexMeshProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, -3.1, 0))); + test(!mConvexMeshProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 3.1, 0))); + test(!mConvexMeshProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, -4.1))); + test(!mConvexMeshProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 4.1))); + test(!mConvexMeshProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-2.1, -3.1, -4.1))); + test(!mConvexMeshProxyShape->testPointInside(mLocalShapeToWorld * Vector3(2.1, 3.1, 4.1))); + test(!mConvexMeshProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-10, -2, -1.5))); + test(!mConvexMeshProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-1, 4, -2.5))); + test(!mConvexMeshProxyShape->testPointInside(mLocalShapeToWorld * Vector3(1, -2, 4.5))); - // ----- Tests using edges information ----- // + // ----- Tests using edges information ----- // - // Tests with CollisionBody - test(mConvexMeshBodyEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 0))); - test(mConvexMeshBodyEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(-1.9, 0, 0))); - test(mConvexMeshBodyEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(1.9, 0, 0))); - test(mConvexMeshBodyEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(0, -2.9, 0))); - test(mConvexMeshBodyEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(0, 2.9, 0))); - test(mConvexMeshBodyEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(0, 0, -3.9))); - test(mConvexMeshBodyEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 3.9))); - test(mConvexMeshBodyEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(-1.9, -2.9, -3.9))); - test(mConvexMeshBodyEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(1.9, 2.9, 3.9))); - test(mConvexMeshBodyEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(-1, -2, -1.5))); - test(mConvexMeshBodyEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(-1, 2, -2.5))); - test(mConvexMeshBodyEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(1, -2, 3.5))); + // Tests with CollisionBody + test(mConvexMeshBodyEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 0))); + test(mConvexMeshBodyEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(-1.9, 0, 0))); + test(mConvexMeshBodyEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(1.9, 0, 0))); + test(mConvexMeshBodyEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(0, -2.9, 0))); + test(mConvexMeshBodyEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(0, 2.9, 0))); + test(mConvexMeshBodyEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(0, 0, -3.9))); + test(mConvexMeshBodyEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 3.9))); + test(mConvexMeshBodyEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(-1.9, -2.9, -3.9))); + test(mConvexMeshBodyEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(1.9, 2.9, 3.9))); + test(mConvexMeshBodyEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(-1, -2, -1.5))); + test(mConvexMeshBodyEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(-1, 2, -2.5))); + test(mConvexMeshBodyEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(1, -2, 3.5))); - test(!mConvexMeshBodyEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(-2.1, 0, 0))); - test(!mConvexMeshBodyEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(2.1, 0, 0))); - test(!mConvexMeshBodyEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(0, -3.1, 0))); - test(!mConvexMeshBodyEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(0, 3.1, 0))); - test(!mConvexMeshBodyEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(0, 0, -4.1))); - test(!mConvexMeshBodyEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 4.1))); - test(!mConvexMeshBodyEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(-2.1, -3.1, -4.1))); - test(!mConvexMeshBodyEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(2.1, 3.1, 4.1))); - test(!mConvexMeshBodyEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(-10, -2, -1.5))); - test(!mConvexMeshBodyEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(-1, 4, -2.5))); - test(!mConvexMeshBodyEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(1, -2, 4.5))); + test(!mConvexMeshBodyEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(-2.1, 0, 0))); + test(!mConvexMeshBodyEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(2.1, 0, 0))); + test(!mConvexMeshBodyEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(0, -3.1, 0))); + test(!mConvexMeshBodyEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(0, 3.1, 0))); + test(!mConvexMeshBodyEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(0, 0, -4.1))); + test(!mConvexMeshBodyEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 4.1))); + test(!mConvexMeshBodyEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(-2.1, -3.1, -4.1))); + test(!mConvexMeshBodyEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(2.1, 3.1, 4.1))); + test(!mConvexMeshBodyEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(-10, -2, -1.5))); + test(!mConvexMeshBodyEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(-1, 4, -2.5))); + test(!mConvexMeshBodyEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(1, -2, 4.5))); - // Tests with ProxyConvexMeshShape - test(mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 0))); - test(mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(-1.9, 0, 0))); - test(mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(1.9, 0, 0))); - test(mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(0, -2.9, 0))); - test(mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(0, 2.9, 0))); - test(mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(0, 0, -3.9))); - test(mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 3.9))); - test(mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(-1.9, -2.9, -3.9))); - test(mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(1.9, 2.9, 3.9))); - test(mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(-1, -2, -1.5))); - test(mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(-1, 2, -2.5))); - test(mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(1, -2, 3.5))); + // Tests with ProxyConvexMeshShape + test(mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 0))); + test(mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(-1.9, 0, 0))); + test(mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(1.9, 0, 0))); + test(mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(0, -2.9, 0))); + test(mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(0, 2.9, 0))); + test(mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(0, 0, -3.9))); + test(mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 3.9))); + test(mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(-1.9, -2.9, -3.9))); + test(mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(1.9, 2.9, 3.9))); + test(mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(-1, -2, -1.5))); + test(mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(-1, 2, -2.5))); + test(mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(1, -2, 3.5))); - test(!mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(-2.1, 0, 0))); - test(!mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(2.1, 0, 0))); - test(!mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(0, -3.1, 0))); - test(!mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(0, 3.1, 0))); - test(!mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(0, 0, -4.1))); - test(!mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 4.1))); - test(!mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(-2.1, -3.1, -4.1))); - test(!mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(2.1, 3.1, 4.1))); - test(!mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(-10, -2, -1.5))); - test(!mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(-1, 4, -2.5))); - test(!mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(1, -2, 4.5))); - } + test(!mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(-2.1, 0, 0))); + test(!mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(2.1, 0, 0))); + test(!mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(0, -3.1, 0))); + test(!mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(0, 3.1, 0))); + test(!mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(0, 0, -4.1))); + test(!mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 4.1))); + test(!mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(-2.1, -3.1, -4.1))); + test(!mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(2.1, 3.1, 4.1))); + test(!mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(-10, -2, -1.5))); + test(!mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(-1, 4, -2.5))); + test(!mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(1, -2, 4.5))); + } - /// Test the ProxyCylinderShape::testPointInside() and - /// CollisionBody::testPointInside() methods - void testCylinder() { + /// Test the ProxyCylinderShape::testPointInside() and + /// CollisionBody::testPointInside() methods + void testCylinder() { - // Tests with CollisionBody - test(mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 0))); - test(mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(0, 3.9, 0))); - test(mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(0, -3.9, 0))); - test(mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(2.9, 0, 0))); - test(mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(-2.9, 0, 0))); - test(mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 2.9))); - test(mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(0, 0, -2.9))); - test(mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(1.7, 0, 1.7))); - test(mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(1.7, 0, -1.7))); - test(mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(-1.7, 0, -1.7))); - test(mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(-1.7, 0, 1.7))); - test(mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(2.9, 3.9, 0))); - test(mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(-2.9, 3.9, 0))); - test(mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(0, 3.9, 2.9))); - test(mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(0, 3.9, -2.9))); - test(mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(1.7, 3.9, 1.7))); - test(mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(1.7, 3.9, -1.7))); - test(mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(-1.7, 3.9, -1.7))); - test(mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(-1.7, 3.9, 1.7))); - test(mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(2.9, -3.9, 0))); - test(mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(-2.9, -3.9, 0))); - test(mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(0, -3.9, 2.9))); - test(mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(0, -3.9, -2.9))); - test(mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(1.7, -3.9, 1.7))); - test(mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(1.7, -3.9, -1.7))); - test(mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(-1.7, -3.9, -1.7))); - test(mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(-1.7, -3.9, 1.7))); + // Tests with CollisionBody + test(mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 0))); + test(mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(0, 3.9, 0))); + test(mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(0, -3.9, 0))); + test(mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(2.9, 0, 0))); + test(mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(-2.9, 0, 0))); + test(mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 2.9))); + test(mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(0, 0, -2.9))); + test(mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(1.7, 0, 1.7))); + test(mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(1.7, 0, -1.7))); + test(mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(-1.7, 0, -1.7))); + test(mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(-1.7, 0, 1.7))); + test(mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(2.9, 3.9, 0))); + test(mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(-2.9, 3.9, 0))); + test(mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(0, 3.9, 2.9))); + test(mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(0, 3.9, -2.9))); + test(mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(1.7, 3.9, 1.7))); + test(mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(1.7, 3.9, -1.7))); + test(mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(-1.7, 3.9, -1.7))); + test(mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(-1.7, 3.9, 1.7))); + test(mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(2.9, -3.9, 0))); + test(mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(-2.9, -3.9, 0))); + test(mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(0, -3.9, 2.9))); + test(mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(0, -3.9, -2.9))); + test(mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(1.7, -3.9, 1.7))); + test(mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(1.7, -3.9, -1.7))); + test(mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(-1.7, -3.9, -1.7))); + test(mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(-1.7, -3.9, 1.7))); - test(!mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(0, 4.1, 0))); - test(!mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(0, -4.1, 0))); - test(!mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(3.1, 0, 0))); - test(!mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(-3.1, 0, 0))); - test(!mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 3.1))); - test(!mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(0, 0, -3.1))); - test(!mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(2.2, 0, 2.2))); - test(!mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(2.2, 0, -2.2))); - test(!mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(-2.2, 0, -2.2))); - test(!mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(-1.3, 0, 2.8))); - test(!mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(3.1, 3.9, 0))); - test(!mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(-3.1, 3.9, 0))); - test(!mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(0, 3.9, 3.1))); - test(!mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(0, 3.9, -3.1))); - test(!mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(2.2, 3.9, 2.2))); - test(!mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(2.2, 3.9, -2.2))); - test(!mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(-2.2, 3.9, -2.2))); - test(!mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(-2.2, 3.9, 2.2))); - test(!mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(3.1, -3.9, 0))); - test(!mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(-3.1, -3.9, 0))); - test(!mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(0, -3.9, 3.1))); - test(!mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(0, -3.9, -3.1))); - test(!mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(2.2, -3.9, 2.2))); - test(!mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(2.2, -3.9, -2.2))); - test(!mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(-2.2, -3.9, -2.2))); - test(!mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(-2.2, -3.9, 2.2))); + test(!mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(0, 4.1, 0))); + test(!mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(0, -4.1, 0))); + test(!mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(3.1, 0, 0))); + test(!mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(-3.1, 0, 0))); + test(!mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 3.1))); + test(!mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(0, 0, -3.1))); + test(!mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(2.2, 0, 2.2))); + test(!mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(2.2, 0, -2.2))); + test(!mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(-2.2, 0, -2.2))); + test(!mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(-1.3, 0, 2.8))); + test(!mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(3.1, 3.9, 0))); + test(!mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(-3.1, 3.9, 0))); + test(!mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(0, 3.9, 3.1))); + test(!mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(0, 3.9, -3.1))); + test(!mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(2.2, 3.9, 2.2))); + test(!mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(2.2, 3.9, -2.2))); + test(!mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(-2.2, 3.9, -2.2))); + test(!mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(-2.2, 3.9, 2.2))); + test(!mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(3.1, -3.9, 0))); + test(!mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(-3.1, -3.9, 0))); + test(!mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(0, -3.9, 3.1))); + test(!mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(0, -3.9, -3.1))); + test(!mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(2.2, -3.9, 2.2))); + test(!mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(2.2, -3.9, -2.2))); + test(!mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(-2.2, -3.9, -2.2))); + test(!mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(-2.2, -3.9, 2.2))); - // Tests with ProxyCylinderShape - test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 0))); - test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 3.9, 0))); - test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, -3.9, 0))); - test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(2.9, 0, 0))); - test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-2.9, 0, 0))); - test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 2.9))); - test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, -2.9))); - test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(1.7, 0, 1.7))); - test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(1.7, 0, -1.7))); - test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-1.7, 0, -1.7))); - test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-1.7, 0, 1.7))); - test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(2.9, 3.9, 0))); - test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-2.9, 3.9, 0))); - test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 3.9, 2.9))); - test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 3.9, -2.9))); - test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(1.7, 3.9, 1.7))); - test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(1.7, 3.9, -1.7))); - test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-1.7, 3.9, -1.7))); - test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-1.7, 3.9, 1.7))); - test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(2.9, -3.9, 0))); - test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-2.9, -3.9, 0))); - test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, -3.9, 2.9))); - test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, -3.9, -2.9))); - test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(1.7, -3.9, 1.7))); - test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(1.7, -3.9, -1.7))); - test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-1.7, -3.9, -1.7))); - test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-1.7, -3.9, 1.7))); + // Tests with ProxyCylinderShape + test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 0))); + test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 3.9, 0))); + test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, -3.9, 0))); + test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(2.9, 0, 0))); + test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-2.9, 0, 0))); + test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 2.9))); + test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, -2.9))); + test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(1.7, 0, 1.7))); + test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(1.7, 0, -1.7))); + test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-1.7, 0, -1.7))); + test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-1.7, 0, 1.7))); + test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(2.9, 3.9, 0))); + test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-2.9, 3.9, 0))); + test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 3.9, 2.9))); + test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 3.9, -2.9))); + test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(1.7, 3.9, 1.7))); + test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(1.7, 3.9, -1.7))); + test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-1.7, 3.9, -1.7))); + test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-1.7, 3.9, 1.7))); + test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(2.9, -3.9, 0))); + test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-2.9, -3.9, 0))); + test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, -3.9, 2.9))); + test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, -3.9, -2.9))); + test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(1.7, -3.9, 1.7))); + test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(1.7, -3.9, -1.7))); + test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-1.7, -3.9, -1.7))); + test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-1.7, -3.9, 1.7))); - test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 4.1, 0))); - test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, -4.1, 0))); - test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(3.1, 0, 0))); - test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-3.1, 0, 0))); - test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 3.1))); - test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, -3.1))); - test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(2.2, 0, 2.2))); - test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(2.2, 0, -2.2))); - test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-2.2, 0, -2.2))); - test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-1.3, 0, 2.8))); - test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(3.1, 3.9, 0))); - test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-3.1, 3.9, 0))); - test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 3.9, 3.1))); - test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 3.9, -3.1))); - test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(2.2, 3.9, 2.2))); - test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(2.2, 3.9, -2.2))); - test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-2.2, 3.9, -2.2))); - test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-2.2, 3.9, 2.2))); - test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(3.1, -3.9, 0))); - test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-3.1, -3.9, 0))); - test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, -3.9, 3.1))); - test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, -3.9, -3.1))); - test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(2.2, -3.9, 2.2))); - test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(2.2, -3.9, -2.2))); - test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-2.2, -3.9, -2.2))); - test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-2.2, -3.9, 2.2))); - } + test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 4.1, 0))); + test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, -4.1, 0))); + test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(3.1, 0, 0))); + test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-3.1, 0, 0))); + test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 3.1))); + test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, -3.1))); + test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(2.2, 0, 2.2))); + test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(2.2, 0, -2.2))); + test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-2.2, 0, -2.2))); + test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-1.3, 0, 2.8))); + test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(3.1, 3.9, 0))); + test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-3.1, 3.9, 0))); + test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 3.9, 3.1))); + test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 3.9, -3.1))); + test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(2.2, 3.9, 2.2))); + test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(2.2, 3.9, -2.2))); + test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-2.2, 3.9, -2.2))); + test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-2.2, 3.9, 2.2))); + test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(3.1, -3.9, 0))); + test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-3.1, -3.9, 0))); + test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, -3.9, 3.1))); + test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, -3.9, -3.1))); + test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(2.2, -3.9, 2.2))); + test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(2.2, -3.9, -2.2))); + test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-2.2, -3.9, -2.2))); + test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-2.2, -3.9, 2.2))); + } - /// Test the CollisionBody::testPointInside() method - void testCompound() { + /// Test the CollisionBody::testPointInside() method + void testCompound() { - // Points on the cylinder - test(mCompoundBody->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 0))); - test(mCompoundBody->testPointInside(mLocalShapeToWorld * Vector3(0, 3.9, 0))); - test(mCompoundBody->testPointInside(mLocalShapeToWorld * Vector3(0, -3.9, 0))); - test(mCompoundBody->testPointInside(mLocalShapeToWorld * Vector3(2.9, 0, 0))); - test(mCompoundBody->testPointInside(mLocalShapeToWorld * Vector3(-2.9, 0, 0))); - test(mCompoundBody->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 2.9))); - test(mCompoundBody->testPointInside(mLocalShapeToWorld * Vector3(0, 0, -2.9))); - test(mCompoundBody->testPointInside(mLocalShapeToWorld * Vector3(1.7, 0, 1.7))); - test(mCompoundBody->testPointInside(mLocalShapeToWorld * Vector3(1.7, 0, -1.7))); - test(mCompoundBody->testPointInside(mLocalShapeToWorld * Vector3(-1.7, 0, -1.7))); - test(mCompoundBody->testPointInside(mLocalShapeToWorld * Vector3(-1.7, 0, 1.7))); - test(mCompoundBody->testPointInside(mLocalShapeToWorld * Vector3(2.9, 3.9, 0))); - test(mCompoundBody->testPointInside(mLocalShapeToWorld * Vector3(-2.9, 3.9, 0))); - test(mCompoundBody->testPointInside(mLocalShapeToWorld * Vector3(0, 3.9, 2.9))); - test(mCompoundBody->testPointInside(mLocalShapeToWorld * Vector3(0, 3.9, -2.9))); - test(mCompoundBody->testPointInside(mLocalShapeToWorld * Vector3(1.7, 3.9, 1.7))); - test(mCompoundBody->testPointInside(mLocalShapeToWorld * Vector3(1.7, 3.9, -1.7))); - test(mCompoundBody->testPointInside(mLocalShapeToWorld * Vector3(-1.7, 3.9, -1.7))); - test(mCompoundBody->testPointInside(mLocalShapeToWorld * Vector3(-1.7, 3.9, 1.7))); - test(mCompoundBody->testPointInside(mLocalShapeToWorld * Vector3(2.9, -3.9, 0))); - test(mCompoundBody->testPointInside(mLocalShapeToWorld * Vector3(-2.9, -3.9, 0))); - test(mCompoundBody->testPointInside(mLocalShapeToWorld * Vector3(0, -3.9, 2.9))); - test(mCompoundBody->testPointInside(mLocalShapeToWorld * Vector3(0, -3.9, -2.9))); - test(mCompoundBody->testPointInside(mLocalShapeToWorld * Vector3(1.7, -3.9, 1.7))); - test(mCompoundBody->testPointInside(mLocalShapeToWorld * Vector3(1.7, -3.9, -1.7))); - test(mCompoundBody->testPointInside(mLocalShapeToWorld * Vector3(-1.7, -3.9, -1.7))); - test(mCompoundBody->testPointInside(mLocalShapeToWorld * Vector3(-1.7, -3.9, 1.7))); + // Points on the cylinder + test(mCompoundBody->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 0))); + test(mCompoundBody->testPointInside(mLocalShapeToWorld * Vector3(0, 3.9, 0))); + test(mCompoundBody->testPointInside(mLocalShapeToWorld * Vector3(0, -3.9, 0))); + test(mCompoundBody->testPointInside(mLocalShapeToWorld * Vector3(2.9, 0, 0))); + test(mCompoundBody->testPointInside(mLocalShapeToWorld * Vector3(-2.9, 0, 0))); + test(mCompoundBody->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 2.9))); + test(mCompoundBody->testPointInside(mLocalShapeToWorld * Vector3(0, 0, -2.9))); + test(mCompoundBody->testPointInside(mLocalShapeToWorld * Vector3(1.7, 0, 1.7))); + test(mCompoundBody->testPointInside(mLocalShapeToWorld * Vector3(1.7, 0, -1.7))); + test(mCompoundBody->testPointInside(mLocalShapeToWorld * Vector3(-1.7, 0, -1.7))); + test(mCompoundBody->testPointInside(mLocalShapeToWorld * Vector3(-1.7, 0, 1.7))); + test(mCompoundBody->testPointInside(mLocalShapeToWorld * Vector3(2.9, 3.9, 0))); + test(mCompoundBody->testPointInside(mLocalShapeToWorld * Vector3(-2.9, 3.9, 0))); + test(mCompoundBody->testPointInside(mLocalShapeToWorld * Vector3(0, 3.9, 2.9))); + test(mCompoundBody->testPointInside(mLocalShapeToWorld * Vector3(0, 3.9, -2.9))); + test(mCompoundBody->testPointInside(mLocalShapeToWorld * Vector3(1.7, 3.9, 1.7))); + test(mCompoundBody->testPointInside(mLocalShapeToWorld * Vector3(1.7, 3.9, -1.7))); + test(mCompoundBody->testPointInside(mLocalShapeToWorld * Vector3(-1.7, 3.9, -1.7))); + test(mCompoundBody->testPointInside(mLocalShapeToWorld * Vector3(-1.7, 3.9, 1.7))); + test(mCompoundBody->testPointInside(mLocalShapeToWorld * Vector3(2.9, -3.9, 0))); + test(mCompoundBody->testPointInside(mLocalShapeToWorld * Vector3(-2.9, -3.9, 0))); + test(mCompoundBody->testPointInside(mLocalShapeToWorld * Vector3(0, -3.9, 2.9))); + test(mCompoundBody->testPointInside(mLocalShapeToWorld * Vector3(0, -3.9, -2.9))); + test(mCompoundBody->testPointInside(mLocalShapeToWorld * Vector3(1.7, -3.9, 1.7))); + test(mCompoundBody->testPointInside(mLocalShapeToWorld * Vector3(1.7, -3.9, -1.7))); + test(mCompoundBody->testPointInside(mLocalShapeToWorld * Vector3(-1.7, -3.9, -1.7))); + test(mCompoundBody->testPointInside(mLocalShapeToWorld * Vector3(-1.7, -3.9, 1.7))); - // Points on the sphere - test(mCompoundBody->testPointInside(mLocalShape2ToWorld * Vector3(0, 0, 0))); - test(mCompoundBody->testPointInside(mLocalShape2ToWorld * Vector3(2.9, 0, 0))); - test(mCompoundBody->testPointInside(mLocalShape2ToWorld * Vector3(-2.9, 0, 0))); - test(mCompoundBody->testPointInside(mLocalShape2ToWorld * Vector3(0, 2.9, 0))); - test(mCompoundBody->testPointInside(mLocalShape2ToWorld * Vector3(0, -2.9, 0))); - test(mCompoundBody->testPointInside(mLocalShape2ToWorld * Vector3(0, 0, 2.9))); - test(mCompoundBody->testPointInside(mLocalShape2ToWorld * Vector3(0, 0, 2.9))); - test(mCompoundBody->testPointInside(mLocalShape2ToWorld * Vector3(-1, -2, -1.5))); - test(mCompoundBody->testPointInside(mLocalShape2ToWorld * Vector3(-1, 2, -1.5))); - test(mCompoundBody->testPointInside(mLocalShape2ToWorld * Vector3(1, -2, 1.5))); - } + // Points on the sphere + test(mCompoundBody->testPointInside(mLocalShape2ToWorld * Vector3(0, 0, 0))); + test(mCompoundBody->testPointInside(mLocalShape2ToWorld * Vector3(2.9, 0, 0))); + test(mCompoundBody->testPointInside(mLocalShape2ToWorld * Vector3(-2.9, 0, 0))); + test(mCompoundBody->testPointInside(mLocalShape2ToWorld * Vector3(0, 2.9, 0))); + test(mCompoundBody->testPointInside(mLocalShape2ToWorld * Vector3(0, -2.9, 0))); + test(mCompoundBody->testPointInside(mLocalShape2ToWorld * Vector3(0, 0, 2.9))); + test(mCompoundBody->testPointInside(mLocalShape2ToWorld * Vector3(0, 0, 2.9))); + test(mCompoundBody->testPointInside(mLocalShape2ToWorld * Vector3(-1, -2, -1.5))); + test(mCompoundBody->testPointInside(mLocalShape2ToWorld * Vector3(-1, 2, -1.5))); + test(mCompoundBody->testPointInside(mLocalShape2ToWorld * Vector3(1, -2, 1.5))); + } }; } diff --git a/test/tests/collision/TestRaycast.h b/test/tests/collision/TestRaycast.h index c3a4de9..c4316a8 100644 --- a/test/tests/collision/TestRaycast.h +++ b/test/tests/collision/TestRaycast.h @@ -1,26 +1,26 @@ /******************************************************************************** -* ReactPhysics3D physics library, http://www.reactphysics3d.com * -* Copyright (c) 2010-2016 Daniel Chappuis * +* 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: * -* * +* 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. * -* * +* 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_RAYCAST_H @@ -45,47 +45,47 @@ namespace reactphysics3d { // Enumeration for categories enum Category { - CATEGORY1 = 0x0001, - CATEGORY2 = 0x0002 + CATEGORY1 = 0x0001, + CATEGORY2 = 0x0002 }; /// Class WorldRaycastCallback class WorldRaycastCallback : public RaycastCallback { - public: + public: - RaycastInfo raycastInfo; - ProxyShape* shapeToTest; - bool isHit; + RaycastInfo raycastInfo; + ProxyShape* shapeToTest; + bool isHit; - WorldRaycastCallback() { - isHit = false; - shapeToTest = NULL; - } + WorldRaycastCallback() { + isHit = false; + shapeToTest = NULL; + } - virtual decimal notifyRaycastHit(const RaycastInfo& info) { + virtual float notifyRaycastHit(const RaycastInfo& info) { - if (shapeToTest->getBody()->getID() == info.body->getID()) { - raycastInfo.body = info.body; - raycastInfo.hitFraction = info.hitFraction; - raycastInfo.proxyShape = info.proxyShape; - raycastInfo.worldNormal = info.worldNormal; - raycastInfo.worldPoint = info.worldPoint; - isHit = true; - } + if (shapeToTest->getBody()->getID() == info.body->getID()) { + raycastInfo.body = info.body; + raycastInfo.hitFraction = info.hitFraction; + raycastInfo.proxyShape = info.proxyShape; + raycastInfo.worldNormal = info.worldNormal; + raycastInfo.worldPoint = info.worldPoint; + isHit = true; + } - // Return a fraction of 1.0 because we need to gather all hits - return decimal(1.0); - } + // Return a fraction of 1.0 because we need to gather all hits + return float(1.0); + } - void reset() { - raycastInfo.body = NULL; - raycastInfo.hitFraction = decimal(0.0); - raycastInfo.proxyShape = NULL; - raycastInfo.worldNormal.setToZero(); - raycastInfo.worldPoint.setToZero(); - isHit = false; - } + void reset() { + raycastInfo.body = NULL; + raycastInfo.hitFraction = float(0.0); + raycastInfo.proxyShape = NULL; + raycastInfo.worldNormal.setToZero(); + raycastInfo.worldPoint.setToZero(); + isHit = false; + } }; // Class TestPointInside @@ -94,2516 +94,2516 @@ class WorldRaycastCallback : public RaycastCallback { */ class TestRaycast : public Test { - private : - - // ---------- Atributes ---------- // - - // Raycast callback class - WorldRaycastCallback mCallback; - - // Epsilon - decimal epsilon; - - // Physics world - CollisionWorld* mWorld; - - // Bodies - CollisionBody* mBoxBody; - CollisionBody* mSphereBody; - CollisionBody* mCapsuleBody; - CollisionBody* mConeBody; - CollisionBody* mConvexMeshBody; - CollisionBody* mConvexMeshBodyEdgesInfo; - CollisionBody* mCylinderBody; - CollisionBody* mCompoundBody; - CollisionBody* mTriangleBody; - CollisionBody* mConcaveMeshBody; - CollisionBody* mHeightFieldBody; - - // Transform - Transform mBodyTransform; - Transform mShapeTransform; - Transform mLocalShapeToWorld; - Transform mLocalShape2ToWorld; - - // Collision shapes - BoxShape* mBoxShape; - SphereShape* mSphereShape; - CapsuleShape* mCapsuleShape; - ConeShape* mConeShape; - ConvexMeshShape* mConvexMeshShape; - ConvexMeshShape* mConvexMeshShapeEdgesInfo; - CylinderShape* mCylinderShape; - TriangleShape* mTriangleShape; - ConcaveShape* m_concaveMeshShape; - HeightFieldShape* mHeightFieldShape; - - // Proxy Shapes - ProxyShape* mBoxProxyShape; - ProxyShape* mSphereProxyShape; - ProxyShape* mCapsuleProxyShape; - ProxyShape* mConeProxyShape; - ProxyShape* mConvexMeshProxyShape; - ProxyShape* mConvexMeshProxyShapeEdgesInfo; - ProxyShape* mCylinderProxyShape; - ProxyShape* mCompoundSphereProxyShape; - ProxyShape* mCompoundCylinderProxyShape; - ProxyShape* mTriangleProxyShape; - ProxyShape* mConcaveMeshProxyShape; - ProxyShape* mHeightFieldProxyShape; - - // Triangle meshes - TriangleMesh mConcaveTriangleMesh; - - std::vector mConcaveMeshVertices; - std::vector mConcaveMeshIndices; - TriangleVertexArray* mConcaveMeshVertexArray; - float mHeightFieldData[100]; - - public : - - // ---------- Methods ---------- // - - /// Constructor - TestRaycast(const std::string& name) : Test(name) { - - epsilon = decimal(0.0001); - - // Create the world - mWorld = new CollisionWorld(); - - // Body transform - Vector3 position(-3, 2, 7); - Quaternion orientation(PI / 5, PI / 6, PI / 7); - mBodyTransform = Transform(position, orientation); - - // Create the bodies - mBoxBody = mWorld->createCollisionBody(mBodyTransform); - mSphereBody = mWorld->createCollisionBody(mBodyTransform); - mCapsuleBody = mWorld->createCollisionBody(mBodyTransform); - mConeBody = mWorld->createCollisionBody(mBodyTransform); - mConvexMeshBody = mWorld->createCollisionBody(mBodyTransform); - mConvexMeshBodyEdgesInfo = mWorld->createCollisionBody(mBodyTransform); - mCylinderBody = mWorld->createCollisionBody(mBodyTransform); - mCompoundBody = mWorld->createCollisionBody(mBodyTransform); - mTriangleBody = mWorld->createCollisionBody(mBodyTransform); - mConcaveMeshBody = mWorld->createCollisionBody(mBodyTransform); - mHeightFieldBody = mWorld->createCollisionBody(mBodyTransform); - - // Collision shape transform - Vector3 shapePosition(1, -4, -3); - Quaternion shapeOrientation(3 * PI / 6 , -PI / 8, PI / 3); - mShapeTransform = Transform(shapePosition, shapeOrientation); - - // Compute the the transform from a local shape point to world-space - mLocalShapeToWorld = mBodyTransform * mShapeTransform; - - // Create collision shapes - mBoxShape = new BoxShape(Vector3(2, 3, 4), 0); - mBoxProxyShape = mBoxBody->addCollisionShape(mBoxShape, mShapeTransform); - - mSphereShape = new SphereShape(3); - mSphereProxyShape = mSphereBody->addCollisionShape(mSphereShape, mShapeTransform); - - const Vector3 triangleVertex1(100, 100, 0); - const Vector3 triangleVertex2(105, 100, 0); - const Vector3 triangleVertex3(100, 103, 0); - mTriangleShape = new TriangleShape(triangleVertex1, triangleVertex2, triangleVertex3); - mTriangleProxyShape = mTriangleBody->addCollisionShape(mTriangleShape, mShapeTransform); - - mCapsuleShape = new CapsuleShape(2, 5); - mCapsuleProxyShape = mCapsuleBody->addCollisionShape(mCapsuleShape, mShapeTransform); - - mConeShape = new ConeShape(2, 6, 0); - mConeProxyShape = mConeBody->addCollisionShape(mConeShape, mShapeTransform); - - // Box of dimension (2, 3, 4) - mConvexMeshShape = new ConvexMeshShape(0.0); - mConvexMeshShape->addVertex(Vector3(-2, -3, -4)); - mConvexMeshShape->addVertex(Vector3(2, -3, -4)); - mConvexMeshShape->addVertex(Vector3(2, -3, 4)); - mConvexMeshShape->addVertex(Vector3(-2, -3, 4)); - mConvexMeshShape->addVertex(Vector3(-2, 3, -4)); - mConvexMeshShape->addVertex(Vector3(2, 3, -4)); - mConvexMeshShape->addVertex(Vector3(2, 3, 4)); - mConvexMeshShape->addVertex(Vector3(-2, 3, 4)); - mConvexMeshProxyShape = mConvexMeshBody->addCollisionShape(mConvexMeshShape, mShapeTransform); - - mConvexMeshShapeEdgesInfo = new ConvexMeshShape(0.0); - mConvexMeshShapeEdgesInfo->addVertex(Vector3(-2, -3, -4)); - mConvexMeshShapeEdgesInfo->addVertex(Vector3(2, -3, -4)); - mConvexMeshShapeEdgesInfo->addVertex(Vector3(2, -3, 4)); - mConvexMeshShapeEdgesInfo->addVertex(Vector3(-2, -3, 4)); - mConvexMeshShapeEdgesInfo->addVertex(Vector3(-2, 3, -4)); - mConvexMeshShapeEdgesInfo->addVertex(Vector3(2, 3, -4)); - mConvexMeshShapeEdgesInfo->addVertex(Vector3(2, 3, 4)); - mConvexMeshShapeEdgesInfo->addVertex(Vector3(-2, 3, 4)); - mConvexMeshShapeEdgesInfo->addEdge(0, 1); - mConvexMeshShapeEdgesInfo->addEdge(1, 2); - mConvexMeshShapeEdgesInfo->addEdge(2, 3); - mConvexMeshShapeEdgesInfo->addEdge(0, 3); - mConvexMeshShapeEdgesInfo->addEdge(4, 5); - mConvexMeshShapeEdgesInfo->addEdge(5, 6); - mConvexMeshShapeEdgesInfo->addEdge(6, 7); - mConvexMeshShapeEdgesInfo->addEdge(4, 7); - mConvexMeshShapeEdgesInfo->addEdge(0, 4); - mConvexMeshShapeEdgesInfo->addEdge(1, 5); - mConvexMeshShapeEdgesInfo->addEdge(2, 6); - mConvexMeshShapeEdgesInfo->addEdge(3, 7); - mConvexMeshShapeEdgesInfo->setIsEdgesInformationUsed(true); - mConvexMeshProxyShapeEdgesInfo = mConvexMeshBodyEdgesInfo->addCollisionShape( - mConvexMeshShapeEdgesInfo, - mShapeTransform); - - mCylinderShape = new CylinderShape(2, 5, 0); - mCylinderProxyShape = mCylinderBody->addCollisionShape(mCylinderShape, mShapeTransform); - - // Compound shape is a cylinder and a sphere - Vector3 positionShape2(Vector3(4, 2, -3)); - Quaternion orientationShape2(-3 *PI / 8, 1.5 * PI/ 3, PI / 13); - Transform shapeTransform2(positionShape2, orientationShape2); - mLocalShape2ToWorld = mBodyTransform * shapeTransform2; - mCompoundCylinderProxyShape = mCompoundBody->addCollisionShape(mCylinderShape, mShapeTransform); - mCompoundSphereProxyShape = mCompoundBody->addCollisionShape(mSphereShape, shapeTransform2); - - // Concave Mesh shape - mConcaveMeshVertices.push_back(Vector3(-2, -3, -4)); - mConcaveMeshVertices.push_back(Vector3(2, -3, -4)); - mConcaveMeshVertices.push_back(Vector3(2, -3, 4)); - mConcaveMeshVertices.push_back(Vector3(-2, -3, 4)); - mConcaveMeshVertices.push_back(Vector3(-2, 3, -4)); - mConcaveMeshVertices.push_back(Vector3(2, 3, -4)); - mConcaveMeshVertices.push_back(Vector3(2, 3, 4)); - mConcaveMeshVertices.push_back(Vector3(-2, 3, 4)); - - mConcaveMeshIndices.push_back(0); mConcaveMeshIndices.push_back(1); mConcaveMeshIndices.push_back(2); - mConcaveMeshIndices.push_back(0); mConcaveMeshIndices.push_back(2); mConcaveMeshIndices.push_back(3); - mConcaveMeshIndices.push_back(1); mConcaveMeshIndices.push_back(5); mConcaveMeshIndices.push_back(2); - mConcaveMeshIndices.push_back(2); mConcaveMeshIndices.push_back(5); mConcaveMeshIndices.push_back(6); - mConcaveMeshIndices.push_back(2); mConcaveMeshIndices.push_back(7); mConcaveMeshIndices.push_back(3); - mConcaveMeshIndices.push_back(2); mConcaveMeshIndices.push_back(6); mConcaveMeshIndices.push_back(7); - mConcaveMeshIndices.push_back(0); mConcaveMeshIndices.push_back(3); mConcaveMeshIndices.push_back(4); - mConcaveMeshIndices.push_back(3); mConcaveMeshIndices.push_back(7); mConcaveMeshIndices.push_back(4); - mConcaveMeshIndices.push_back(0); mConcaveMeshIndices.push_back(4); mConcaveMeshIndices.push_back(1); - mConcaveMeshIndices.push_back(1); mConcaveMeshIndices.push_back(4); mConcaveMeshIndices.push_back(5); - mConcaveMeshIndices.push_back(5); mConcaveMeshIndices.push_back(7); mConcaveMeshIndices.push_back(6); - mConcaveMeshIndices.push_back(4); mConcaveMeshIndices.push_back(7); mConcaveMeshIndices.push_back(5); - TriangleVertexArray::VertexDataType vertexType = sizeof(decimal) == 4 ? TriangleVertexArray::VERTEX_FLOAT_TYPE : - TriangleVertexArray::VERTEX_DOUBLE_TYPE; - mConcaveMeshVertexArray = - new TriangleVertexArray(8, &(mConcaveMeshVertices[0]), sizeof(Vector3), - 12, &(mConcaveMeshIndices[0]), sizeof(uint), - vertexType, - TriangleVertexArray::INDEX_INTEGER_TYPE); - - - // Add the triangle vertex array of the subpart to the triangle mesh - mConcaveTriangleMesh.addSubpart(mConcaveMeshVertexArray); - m_concaveMeshShape = new ConcaveMeshShape(&mConcaveTriangleMesh); - mConcaveMeshProxyShape = mConcaveMeshBody->addCollisionShape(m_concaveMeshShape, mShapeTransform); - - - // Heightfield shape (plane height field at height=4) - for (int i=0; i<100; i++) mHeightFieldData[i] = 4; - mHeightFieldShape = new HeightFieldShape(10, 10, 0, 4, mHeightFieldData, HeightFieldShape::HEIGHT_FLOAT_TYPE); - mHeightFieldProxyShape = mHeightFieldBody->addCollisionShape(mHeightFieldShape, mShapeTransform); - - // Assign proxy shapes to the different categories - mBoxProxyShape->setCollisionCategoryBits(CATEGORY1); - mSphereProxyShape->setCollisionCategoryBits(CATEGORY1); - mCapsuleProxyShape->setCollisionCategoryBits(CATEGORY1); - mConeProxyShape->setCollisionCategoryBits(CATEGORY2); - mConvexMeshProxyShape->setCollisionCategoryBits(CATEGORY2); - mConvexMeshProxyShapeEdgesInfo->setCollisionCategoryBits(CATEGORY2); - mCylinderProxyShape->setCollisionCategoryBits(CATEGORY2); - mCompoundSphereProxyShape->setCollisionCategoryBits(CATEGORY2); - mCompoundCylinderProxyShape->setCollisionCategoryBits(CATEGORY2); - mTriangleProxyShape->setCollisionCategoryBits(CATEGORY1); - mConcaveMeshProxyShape->setCollisionCategoryBits(CATEGORY2); - mHeightFieldProxyShape->setCollisionCategoryBits(CATEGORY2); - } - - /// Destructor - ~TestRaycast() { - delete mBoxShape; - delete mSphereShape; - delete mCapsuleShape; - delete mConeShape; - delete mConvexMeshShape; - delete mConvexMeshShapeEdgesInfo; - delete mCylinderShape; - delete mTriangleShape; - delete m_concaveMeshShape; - delete mHeightFieldShape; - - delete mConcaveMeshVertexArray; - } - - /// Run the tests - void run() { - testBox(); - testSphere(); - testCapsule(); - testCone(); - testConvexMesh(); - testCylinder(); - testCompound(); - testTriangle(); - testConcaveMesh(); - testHeightField(); - } - - /// Test the ProxyBoxShape::raycast(), CollisionBody::raycast() and - /// CollisionWorld::raycast() methods. - void testBox() { - - // ----- Test feedback data ----- // - Vector3 point1 = mLocalShapeToWorld * Vector3(1 , 2, 10); - Vector3 point2 = mLocalShapeToWorld * Vector3(1, 2, -20); - Ray ray(point1, point2); - Vector3 hitPoint = mLocalShapeToWorld * Vector3(1, 2, 4); - - mCallback.shapeToTest = mBoxProxyShape; - - // CollisionWorld::raycast() - mCallback.reset(); - mWorld->raycast(ray, &mCallback); - test(mCallback.isHit); - test(mCallback.raycastInfo.body == mBoxBody); - test(mCallback.raycastInfo.proxyShape == mBoxProxyShape); - test(approxEqual(mCallback.raycastInfo.hitFraction, decimal(0.2), epsilon)); - test(approxEqual(mCallback.raycastInfo.worldPoint.x, hitPoint.x, epsilon)); - test(approxEqual(mCallback.raycastInfo.worldPoint.y, hitPoint.y, epsilon)); - test(approxEqual(mCallback.raycastInfo.worldPoint.z, hitPoint.z, epsilon)); - - // Correct category filter mask - mCallback.reset(); - mWorld->raycast(ray, &mCallback, CATEGORY1); - test(mCallback.isHit); - - // Wrong category filter mask - mCallback.reset(); - mWorld->raycast(ray, &mCallback, CATEGORY2); - test(!mCallback.isHit); - - // CollisionBody::raycast() - RaycastInfo raycastInfo2; - test(mBoxBody->raycast(ray, raycastInfo2)); - test(raycastInfo2.body == mBoxBody); - test(raycastInfo2.proxyShape == mBoxProxyShape); - test(approxEqual(raycastInfo2.hitFraction, decimal(0.2), epsilon)); - test(approxEqual(raycastInfo2.worldPoint.x, hitPoint.x, epsilon)); - test(approxEqual(raycastInfo2.worldPoint.y, hitPoint.y, epsilon)); - test(approxEqual(raycastInfo2.worldPoint.z, hitPoint.z, epsilon)); - - // ProxyCollisionShape::raycast() - RaycastInfo raycastInfo3; - test(mBoxProxyShape->raycast(ray, raycastInfo3)); - test(raycastInfo3.body == mBoxBody); - test(raycastInfo3.proxyShape == mBoxProxyShape); - test(approxEqual(raycastInfo3.hitFraction, decimal(0.2), epsilon)); - test(approxEqual(raycastInfo3.worldPoint.x, hitPoint.x, epsilon)); - test(approxEqual(raycastInfo3.worldPoint.y, hitPoint.y, epsilon)); - test(approxEqual(raycastInfo3.worldPoint.z, hitPoint.z, epsilon)); - - Ray ray1(mLocalShapeToWorld * Vector3(0, 0, 0), mLocalShapeToWorld * Vector3(5, 7, -1)); - Ray ray2(mLocalShapeToWorld * Vector3(5, 11, 7), mLocalShapeToWorld * Vector3(17, 29, 28)); - Ray ray3(mLocalShapeToWorld * Vector3(1, 2, 3), mLocalShapeToWorld * Vector3(-11, 2, 24)); - Ray ray4(mLocalShapeToWorld * Vector3(10, 10, 10), mLocalShapeToWorld * Vector3(22, 28, 31)); - Ray ray5(mLocalShapeToWorld * Vector3(3, 1, -5), mLocalShapeToWorld * Vector3(-30, 1, -5)); - Ray ray6(mLocalShapeToWorld * Vector3(4, 4, 1), mLocalShapeToWorld * Vector3(4, -20, 1)); - Ray ray7(mLocalShapeToWorld * Vector3(1, -4, 5), mLocalShapeToWorld * Vector3(1, -4, -20)); - Ray ray8(mLocalShapeToWorld * Vector3(-4, 4, 0), mLocalShapeToWorld * Vector3(20, 4, 0)); - Ray ray9(mLocalShapeToWorld * Vector3(0, -4, -7), mLocalShapeToWorld * Vector3(0, 50, -7)); - Ray ray10(mLocalShapeToWorld * Vector3(-3, 0, -6), mLocalShapeToWorld * Vector3(-3, 0, 20)); - Ray ray11(mLocalShapeToWorld * Vector3(3, 1, 2), mLocalShapeToWorld * Vector3(-20, 1, 2)); - Ray ray12(mLocalShapeToWorld * Vector3(1, 4, -1), mLocalShapeToWorld * Vector3(1, -20, -1)); - Ray ray13(mLocalShapeToWorld * Vector3(-1, 2, 5), mLocalShapeToWorld * Vector3(-1, 2, -20)); - Ray ray14(mLocalShapeToWorld * Vector3(-3, 2, -2), mLocalShapeToWorld * Vector3(20, 2, -2)); - Ray ray15(mLocalShapeToWorld * Vector3(0, -4, 1), mLocalShapeToWorld * Vector3(0, 20, 1)); - Ray ray16(mLocalShapeToWorld * Vector3(-1, 2, -5), mLocalShapeToWorld * Vector3(-1, 2, 20)); - - // ----- Test raycast miss ----- // - test(!mBoxBody->raycast(ray1, raycastInfo3)); - test(!mBoxProxyShape->raycast(ray1, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray1, &mCallback); - test(!mCallback.isHit); - mCallback.reset(); - mWorld->raycast(Ray(ray1.point1, ray1.point2, decimal(0.01)), &mCallback); - test(!mCallback.isHit); - mCallback.reset(); - mWorld->raycast(Ray(ray1.point1, ray1.point2, decimal(100.0)), &mCallback); - test(!mCallback.isHit); - - test(!mBoxBody->raycast(ray2, raycastInfo3)); - test(!mBoxProxyShape->raycast(ray2, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray2, &mCallback); - test(!mCallback.isHit); - - test(!mBoxBody->raycast(ray3, raycastInfo3)); - test(!mBoxProxyShape->raycast(ray3, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray3, &mCallback); - test(!mCallback.isHit); - - test(!mBoxBody->raycast(ray4, raycastInfo3)); - test(!mBoxProxyShape->raycast(ray4, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray4, &mCallback); - test(!mCallback.isHit); - - test(!mBoxBody->raycast(ray5, raycastInfo3)); - test(!mBoxProxyShape->raycast(ray5, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray5, &mCallback); - test(!mCallback.isHit); - - test(!mBoxBody->raycast(ray6, raycastInfo3)); - test(!mBoxProxyShape->raycast(ray6, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray6, &mCallback); - test(!mCallback.isHit); - - test(!mBoxBody->raycast(ray7, raycastInfo3)); - test(!mBoxProxyShape->raycast(ray7, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray7, &mCallback); - test(!mCallback.isHit); - - test(!mBoxBody->raycast(ray8, raycastInfo3)); - test(!mBoxProxyShape->raycast(ray8, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray8, &mCallback); - test(!mCallback.isHit); - - test(!mBoxBody->raycast(ray9, raycastInfo3)); - test(!mBoxProxyShape->raycast(ray9, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray9, &mCallback); - test(!mCallback.isHit); - - test(!mBoxBody->raycast(ray10, raycastInfo3)); - test(!mBoxProxyShape->raycast(ray10, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray10, &mCallback); - test(!mCallback.isHit); - - mCallback.reset(); - mWorld->raycast(Ray(ray11.point1, ray11.point2, decimal(0.01)), &mCallback); - test(!mCallback.isHit); - mCallback.reset(); - mWorld->raycast(Ray(ray12.point1, ray12.point2, decimal(0.01)), &mCallback); - test(!mCallback.isHit); - mCallback.reset(); - mWorld->raycast(Ray(ray13.point1, ray13.point2, decimal(0.01)), &mCallback); - test(!mCallback.isHit); - mCallback.reset(); - mWorld->raycast(Ray(ray14.point1, ray14.point2, decimal(0.01)), &mCallback); - test(!mCallback.isHit); - mCallback.reset(); - mWorld->raycast(Ray(ray15.point1, ray15.point2, decimal(0.01)), &mCallback); - test(!mCallback.isHit); - mCallback.reset(); - mWorld->raycast(Ray(ray16.point1, ray16.point2, decimal(0.01)), &mCallback); - test(!mCallback.isHit); - - // ----- Test raycast hits ----- // - test(mBoxBody->raycast(ray11, raycastInfo3)); - test(mBoxProxyShape->raycast(ray11, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray11, &mCallback); - test(mCallback.isHit); - mCallback.reset(); - mWorld->raycast(Ray(ray11.point1, ray11.point2, decimal(0.8)), &mCallback); - test(mCallback.isHit); - - test(mBoxBody->raycast(ray12, raycastInfo3)); - test(mBoxProxyShape->raycast(ray12, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray12, &mCallback); - test(mCallback.isHit); - mCallback.reset(); - mWorld->raycast(Ray(ray12.point1, ray12.point2, decimal(0.8)), &mCallback); - test(mCallback.isHit); - - test(mBoxBody->raycast(ray13, raycastInfo3)); - test(mBoxProxyShape->raycast(ray13, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray13, &mCallback); - test(mCallback.isHit); - mCallback.reset(); - mWorld->raycast(Ray(ray13.point1, ray13.point2, decimal(0.8)), &mCallback); - test(mCallback.isHit); - - test(mBoxBody->raycast(ray14, raycastInfo3)); - test(mBoxProxyShape->raycast(ray14, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray14, &mCallback); - test(mCallback.isHit); - mCallback.reset(); - mWorld->raycast(Ray(ray14.point1, ray14.point2, decimal(0.8)), &mCallback); - test(mCallback.isHit); - - test(mBoxBody->raycast(ray15, raycastInfo3)); - test(mBoxProxyShape->raycast(ray15, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray15, &mCallback); - test(mCallback.isHit); - mCallback.reset(); - mWorld->raycast(Ray(ray15.point1, ray15.point2, decimal(0.8)), &mCallback); - test(mCallback.isHit); - - test(mBoxBody->raycast(ray16, raycastInfo3)); - test(mBoxProxyShape->raycast(ray16, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray16, &mCallback); - test(mCallback.isHit); - mCallback.reset(); - mWorld->raycast(Ray(ray16.point1, ray16.point2, decimal(0.8)), &mCallback); - test(mCallback.isHit); - } - - /// Test the ProxySphereShape::raycast(), CollisionBody::raycast() and - /// CollisionWorld::raycast() methods. - void testSphere() { - - // ----- Test feedback data ----- // - Vector3 point1 = mLocalShapeToWorld * Vector3(-5 , 0, 0); - Vector3 point2 = mLocalShapeToWorld * Vector3(5, 0, 0); - Ray ray(point1, point2); - Vector3 hitPoint = mLocalShapeToWorld * Vector3(-3, 0, 0); - - mCallback.shapeToTest = mSphereProxyShape; - - // CollisionWorld::raycast() - mCallback.reset(); - mWorld->raycast(ray, &mCallback); - test(mCallback.isHit); - test(mCallback.raycastInfo.body == mSphereBody); - test(mCallback.raycastInfo.proxyShape == mSphereProxyShape); - test(approxEqual(mCallback.raycastInfo.hitFraction, 0.2, epsilon)); - test(approxEqual(mCallback.raycastInfo.worldPoint.x, hitPoint.x, epsilon)); - test(approxEqual(mCallback.raycastInfo.worldPoint.y, hitPoint.y, epsilon)); - test(approxEqual(mCallback.raycastInfo.worldPoint.z, hitPoint.z, epsilon)); - - // Correct category filter mask - mCallback.reset(); - mWorld->raycast(ray, &mCallback, CATEGORY1); - test(mCallback.isHit); - - // Wrong category filter mask - mCallback.reset(); - mWorld->raycast(ray, &mCallback, CATEGORY2); - test(!mCallback.isHit); - - // CollisionBody::raycast() - RaycastInfo raycastInfo2; - test(mSphereBody->raycast(ray, raycastInfo2)); - test(raycastInfo2.body == mSphereBody); - test(raycastInfo2.proxyShape == mSphereProxyShape); - test(approxEqual(raycastInfo2.hitFraction, 0.2, epsilon)); - test(approxEqual(raycastInfo2.worldPoint.x, hitPoint.x, epsilon)); - test(approxEqual(raycastInfo2.worldPoint.y, hitPoint.y, epsilon)); - test(approxEqual(raycastInfo2.worldPoint.z, hitPoint.z, epsilon)); - - // ProxyCollisionShape::raycast() - RaycastInfo raycastInfo3; - test(mSphereProxyShape->raycast(ray, raycastInfo3)); - test(raycastInfo3.body == mSphereBody); - test(raycastInfo3.proxyShape == mSphereProxyShape); - test(approxEqual(raycastInfo3.hitFraction, 0.2, epsilon)); - test(approxEqual(raycastInfo3.worldPoint.x, hitPoint.x, epsilon)); - test(approxEqual(raycastInfo3.worldPoint.y, hitPoint.y, epsilon)); - test(approxEqual(raycastInfo3.worldPoint.z, hitPoint.z, epsilon)); - - Ray ray1(mLocalShapeToWorld * Vector3(0, 0, 0), mLocalShapeToWorld * Vector3(5, 7, -1)); - Ray ray2(mLocalShapeToWorld * Vector3(5, 11, 7), mLocalShapeToWorld * Vector3(4, 6, 7)); - Ray ray3(mLocalShapeToWorld * Vector3(1, 2, 2), mLocalShapeToWorld * Vector3(-4, 0, 7)); - Ray ray4(mLocalShapeToWorld * Vector3(10, 10, 10), mLocalShapeToWorld * Vector3(4, 6, 7)); - Ray ray5(mLocalShapeToWorld * Vector3(4, 1, -5), mLocalShapeToWorld * Vector3(-30, 1, -5)); - Ray ray6(mLocalShapeToWorld * Vector3(4, 4, 1), mLocalShapeToWorld * Vector3(4, -30, 1)); - Ray ray7(mLocalShapeToWorld * Vector3(1, -4, 5), mLocalShapeToWorld * Vector3(1, -4, -30)); - Ray ray8(mLocalShapeToWorld * Vector3(-4, 4, 0), mLocalShapeToWorld * Vector3(30, 4, 0)); - Ray ray9(mLocalShapeToWorld * Vector3(0, -4, -4), mLocalShapeToWorld * Vector3(0, 30, -4)); - Ray ray10(mLocalShapeToWorld * Vector3(-4, 0, -6), mLocalShapeToWorld * Vector3(-4, 0, 30)); - Ray ray11(mLocalShapeToWorld * Vector3(4, 1, 2), mLocalShapeToWorld * Vector3(-30, 1, 2)); - Ray ray12(mLocalShapeToWorld * Vector3(1, 4, -1), mLocalShapeToWorld * Vector3(1, -30, -1)); - Ray ray13(mLocalShapeToWorld * Vector3(-1, 2, 5), mLocalShapeToWorld * Vector3(-1, 2, -30)); - Ray ray14(mLocalShapeToWorld * Vector3(-5, 2, -2), mLocalShapeToWorld * Vector3(30, 2, -2)); - Ray ray15(mLocalShapeToWorld * Vector3(0, -4, 1), mLocalShapeToWorld * Vector3(0, 30, 1)); - Ray ray16(mLocalShapeToWorld * Vector3(-1, 2, -11), mLocalShapeToWorld * Vector3(-1, 2, 30)); - - // ----- Test raycast miss ----- // - test(!mSphereBody->raycast(ray1, raycastInfo3)); - test(!mSphereProxyShape->raycast(ray1, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray1, &mCallback); - test(!mCallback.isHit); - mCallback.reset(); - mWorld->raycast(Ray(ray1.point1, ray1.point2, decimal(0.01)), &mCallback); - test(!mCallback.isHit); - mCallback.reset(); - mWorld->raycast(Ray(ray1.point1, ray1.point2, decimal(100.0)), &mCallback); - test(!mCallback.isHit); - - test(!mSphereBody->raycast(ray2, raycastInfo3)); - test(!mSphereProxyShape->raycast(ray2, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray2, &mCallback); - test(!mCallback.isHit); - - test(!mSphereBody->raycast(ray3, raycastInfo3)); - test(!mSphereProxyShape->raycast(ray3, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray3, &mCallback); - test(!mCallback.isHit); - - test(!mSphereBody->raycast(ray4, raycastInfo3)); - test(!mSphereProxyShape->raycast(ray4, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray4, &mCallback); - test(!mCallback.isHit); - - test(!mSphereBody->raycast(ray5, raycastInfo3)); - test(!mSphereProxyShape->raycast(ray5, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray5, &mCallback); - test(!mCallback.isHit); - - test(!mSphereBody->raycast(ray6, raycastInfo3)); - test(!mSphereProxyShape->raycast(ray6, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray6, &mCallback); - test(!mCallback.isHit); - - test(!mSphereBody->raycast(ray7, raycastInfo3)); - test(!mSphereProxyShape->raycast(ray7, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray7, &mCallback); - test(!mCallback.isHit); - - test(!mSphereBody->raycast(ray8, raycastInfo3)); - test(!mSphereProxyShape->raycast(ray8, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray8, &mCallback); - test(!mCallback.isHit); - - test(!mSphereBody->raycast(ray9, raycastInfo3)); - test(!mSphereProxyShape->raycast(ray9, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray9, &mCallback); - test(!mCallback.isHit); - - test(!mSphereBody->raycast(ray10, raycastInfo3)); - test(!mSphereProxyShape->raycast(ray10, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray10, &mCallback); - test(!mCallback.isHit); - - mCallback.reset(); - mWorld->raycast(Ray(ray11.point1, ray11.point2, decimal(0.01)), &mCallback); - test(!mCallback.isHit); - mCallback.reset(); - mWorld->raycast(Ray(ray12.point1, ray12.point2, decimal(0.01)), &mCallback); - test(!mCallback.isHit); - mCallback.reset(); - mWorld->raycast(Ray(ray13.point1, ray13.point2, decimal(0.01)), &mCallback); - test(!mCallback.isHit); - mCallback.reset(); - mWorld->raycast(Ray(ray14.point1, ray14.point2, decimal(0.01)), &mCallback); - test(!mCallback.isHit); - mCallback.reset(); - mWorld->raycast(Ray(ray15.point1, ray15.point2, decimal(0.01)), &mCallback); - test(!mCallback.isHit); - mCallback.reset(); - mWorld->raycast(Ray(ray16.point1, ray16.point2, decimal(0.01)), &mCallback); - test(!mCallback.isHit); - - // ----- Test raycast hits ----- // - test(mSphereBody->raycast(ray11, raycastInfo3)); - test(mSphereProxyShape->raycast(ray11, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray11, &mCallback); - test(mCallback.isHit); - mCallback.reset(); - mWorld->raycast(Ray(ray11.point1, ray11.point2, decimal(0.8)), &mCallback); - test(mCallback.isHit); - - test(mSphereBody->raycast(ray12, raycastInfo3)); - test(mSphereProxyShape->raycast(ray12, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray12, &mCallback); - test(mCallback.isHit); - mCallback.reset(); - mWorld->raycast(Ray(ray12.point1, ray12.point2, decimal(0.8)), &mCallback); - test(mCallback.isHit); - - test(mSphereBody->raycast(ray13, raycastInfo3)); - test(mSphereProxyShape->raycast(ray13, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray13, &mCallback); - mCallback.reset(); - mWorld->raycast(Ray(ray13.point1, ray13.point2, decimal(0.8)), &mCallback); - - test(mSphereBody->raycast(ray14, raycastInfo3)); - test(mSphereProxyShape->raycast(ray14, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray14, &mCallback); - test(mCallback.isHit); - mCallback.reset(); - mWorld->raycast(Ray(ray14.point1, ray14.point2, decimal(0.8)), &mCallback); - test(mCallback.isHit); - - test(mSphereBody->raycast(ray15, raycastInfo3)); - test(mSphereProxyShape->raycast(ray15, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray15, &mCallback); - test(mCallback.isHit); - mCallback.reset(); - mWorld->raycast(Ray(ray15.point1, ray15.point2, decimal(0.8)), &mCallback); - test(mCallback.isHit); - - test(mSphereBody->raycast(ray16, raycastInfo3)); - test(mSphereProxyShape->raycast(ray16, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray16, &mCallback); - test(mCallback.isHit); - mCallback.reset(); - mWorld->raycast(Ray(ray16.point1, ray16.point2, decimal(0.8)), &mCallback); - test(mCallback.isHit); - } - - /// Test the ProxyCapsuleShape::raycast(), CollisionBody::raycast() and - /// CollisionWorld::raycast() methods. - void testCapsule() { - - // ----- Test feedback data ----- // - Vector3 point1A = mLocalShapeToWorld * Vector3(4 , 1, 0); - Vector3 point1B = mLocalShapeToWorld * Vector3(-6, 1, 0); - Ray ray(point1A, point1B); - Vector3 hitPoint = mLocalShapeToWorld * Vector3(2, 1, 0); - - Vector3 point2A = mLocalShapeToWorld * Vector3(0 , 6.5, 0); - Vector3 point2B = mLocalShapeToWorld * Vector3(0, -3.5, 0); - Ray rayTop(point2A, point2B); - Vector3 hitPointTop = mLocalShapeToWorld * Vector3(0, decimal(4.5), 0); - - Vector3 point3A = mLocalShapeToWorld * Vector3(0 , -6.5, 0); - Vector3 point3B = mLocalShapeToWorld * Vector3(0, 3.5, 0); - Ray rayBottom(point3A, point3B); - Vector3 hitPointBottom = mLocalShapeToWorld * Vector3(0, decimal(-4.5), 0); - - mCallback.shapeToTest = mCapsuleProxyShape; - - // CollisionWorld::raycast() - mCallback.reset(); - mWorld->raycast(ray, &mCallback); - test(mCallback.isHit); - test(mCallback.raycastInfo.body == mCapsuleBody); - test(mCallback.raycastInfo.proxyShape == mCapsuleProxyShape); - test(approxEqual(mCallback.raycastInfo.hitFraction, decimal(0.2), epsilon)); - test(approxEqual(mCallback.raycastInfo.worldPoint.x, hitPoint.x, epsilon)); - test(approxEqual(mCallback.raycastInfo.worldPoint.y, hitPoint.y, epsilon)); - test(approxEqual(mCallback.raycastInfo.worldPoint.z, hitPoint.z, epsilon)); - - // Correct category filter mask - mCallback.reset(); - mWorld->raycast(ray, &mCallback, CATEGORY1); - test(mCallback.isHit); - - // Wrong category filter mask - mCallback.reset(); - mWorld->raycast(ray, &mCallback, CATEGORY2); - test(!mCallback.isHit); - - // CollisionBody::raycast() - RaycastInfo raycastInfo2; - test(mCapsuleBody->raycast(ray, raycastInfo2)); - test(raycastInfo2.body == mCapsuleBody); - test(raycastInfo2.proxyShape == mCapsuleProxyShape); - test(approxEqual(raycastInfo2.hitFraction, decimal(0.2), epsilon)); - test(approxEqual(raycastInfo2.worldPoint.x, hitPoint.x, epsilon)); - test(approxEqual(raycastInfo2.worldPoint.y, hitPoint.y, epsilon)); - test(approxEqual(raycastInfo2.worldPoint.z, hitPoint.z, epsilon)); - - // ProxyCollisionShape::raycast() - RaycastInfo raycastInfo3; - test(mCapsuleProxyShape->raycast(ray, raycastInfo3)); - test(raycastInfo3.body == mCapsuleBody); - test(raycastInfo3.proxyShape == mCapsuleProxyShape); - test(approxEqual(raycastInfo3.hitFraction, decimal(0.2), epsilon)); - test(approxEqual(raycastInfo3.worldPoint.x, hitPoint.x, epsilon)); - test(approxEqual(raycastInfo3.worldPoint.y, hitPoint.y, epsilon)); - test(approxEqual(raycastInfo3.worldPoint.z, hitPoint.z, epsilon)); - - RaycastInfo raycastInfo4; - test(mCapsuleProxyShape->raycast(rayTop, raycastInfo4)); - test(raycastInfo4.body == mCapsuleBody); - test(raycastInfo4.proxyShape == mCapsuleProxyShape); - test(approxEqual(raycastInfo4.hitFraction, decimal(0.2), epsilon)); - test(approxEqual(raycastInfo4.worldPoint.x, hitPointTop.x, epsilon)); - test(approxEqual(raycastInfo4.worldPoint.y, hitPointTop.y, epsilon)); - test(approxEqual(raycastInfo4.worldPoint.z, hitPointTop.z, epsilon)); - - // ProxyCollisionShape::raycast() - RaycastInfo raycastInfo5; - test(mCapsuleProxyShape->raycast(rayBottom, raycastInfo5)); - test(raycastInfo5.body == mCapsuleBody); - test(raycastInfo5.proxyShape == mCapsuleProxyShape); - test(approxEqual(raycastInfo5.hitFraction, decimal(0.2), epsilon)); - test(approxEqual(raycastInfo5.worldPoint.x, hitPointBottom.x, epsilon)); - test(approxEqual(raycastInfo5.worldPoint.y, hitPointBottom.y, epsilon)); - test(approxEqual(raycastInfo5.worldPoint.z, hitPointBottom.z, epsilon)); - - Ray ray1(mLocalShapeToWorld * Vector3(0, 0, 0), mLocalShapeToWorld * Vector3(5, 7, -1)); - Ray ray2(mLocalShapeToWorld * Vector3(5, 11, 7), mLocalShapeToWorld * Vector3(9, 17, 14)); - Ray ray3(mLocalShapeToWorld * Vector3(1, 3, -1), mLocalShapeToWorld * Vector3(-3, 3, 6)); - Ray ray4(mLocalShapeToWorld * Vector3(10, 10, 10), mLocalShapeToWorld * Vector3(14, 16, 17)); - Ray ray5(mLocalShapeToWorld * Vector3(4, 1, -5), mLocalShapeToWorld * Vector3(1, 1, -5)); - Ray ray6(mLocalShapeToWorld * Vector3(4, 9, 1), mLocalShapeToWorld * Vector3(4, 7, 1)); - Ray ray7(mLocalShapeToWorld * Vector3(1, -9, 5), mLocalShapeToWorld * Vector3(1, -9, 3)); - Ray ray8(mLocalShapeToWorld * Vector3(-4, 9, 0), mLocalShapeToWorld * Vector3(-3, 9, 0)); - Ray ray9(mLocalShapeToWorld * Vector3(0, -9, -4), mLocalShapeToWorld * Vector3(0, -4, -4)); - Ray ray10(mLocalShapeToWorld * Vector3(-4, 0, -6), mLocalShapeToWorld * Vector3(-4, 0, 2)); - Ray ray11(mLocalShapeToWorld * Vector3(4, 1, 1.5), mLocalShapeToWorld * Vector3(-30, 1, 1.5)); - Ray ray12(mLocalShapeToWorld * Vector3(1, 9, -1), mLocalShapeToWorld * Vector3(1, -30, -1)); - Ray ray13(mLocalShapeToWorld * Vector3(-1, 2, 3), mLocalShapeToWorld * Vector3(-1, 2, -30)); - Ray ray14(mLocalShapeToWorld * Vector3(-3, 2, -1.7), mLocalShapeToWorld * Vector3(30, 2, -1.7)); - Ray ray15(mLocalShapeToWorld * Vector3(0, -9, 1), mLocalShapeToWorld * Vector3(0, 30, 1)); - Ray ray16(mLocalShapeToWorld * Vector3(-1, 2, -7), mLocalShapeToWorld * Vector3(-1, 2, 30)); - - // ----- Test raycast miss ----- // - test(!mCapsuleBody->raycast(ray1, raycastInfo3)); - test(!mCapsuleProxyShape->raycast(ray1, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray1, &mCallback); - test(!mCallback.isHit); - mCallback.reset(); - mWorld->raycast(Ray(ray1.point1, ray1.point2, decimal(0.01)), &mCallback); - test(!mCallback.isHit); - mCallback.reset(); - mWorld->raycast(Ray(ray1.point1, ray1.point2, decimal(100.0)), &mCallback); - test(!mCallback.isHit); - - test(!mCapsuleBody->raycast(ray2, raycastInfo3)); - test(!mCapsuleProxyShape->raycast(ray2, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray2, &mCallback); - test(!mCallback.isHit); - - test(!mCapsuleBody->raycast(ray3, raycastInfo3)); - test(!mCapsuleProxyShape->raycast(ray3, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray3, &mCallback); - test(!mCallback.isHit); - - test(!mCapsuleBody->raycast(ray4, raycastInfo3)); - test(!mCapsuleProxyShape->raycast(ray4, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray4, &mCallback); - test(!mCallback.isHit); - - test(!mCapsuleBody->raycast(ray5, raycastInfo3)); - test(!mCapsuleProxyShape->raycast(ray5, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray5, &mCallback); - test(!mCallback.isHit); - - test(!mCapsuleBody->raycast(ray6, raycastInfo3)); - test(!mCapsuleProxyShape->raycast(ray6, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray6, &mCallback); - test(!mCallback.isHit); - - test(!mCapsuleBody->raycast(ray7, raycastInfo3)); - test(!mCapsuleProxyShape->raycast(ray7, raycastInfo3)); - mWorld->raycast(ray7, &mCallback); - test(!mCallback.isHit); - - test(!mCapsuleBody->raycast(ray8, raycastInfo3)); - test(!mCapsuleProxyShape->raycast(ray8, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray8, &mCallback); - test(!mCallback.isHit); - - test(!mCapsuleBody->raycast(ray9, raycastInfo3)); - test(!mCapsuleProxyShape->raycast(ray9, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray9, &mCallback); - test(!mCallback.isHit); - - test(!mCapsuleBody->raycast(ray10, raycastInfo3)); - test(!mCapsuleProxyShape->raycast(ray10, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray10, &mCallback); - test(!mCallback.isHit); - - mCallback.reset(); - mWorld->raycast(Ray(ray11.point1, ray11.point2, decimal(0.01)), &mCallback); - test(!mCallback.isHit); - mCallback.reset(); - mWorld->raycast(Ray(ray12.point1, ray12.point2, decimal(0.01)), &mCallback); - test(!mCallback.isHit); - mCallback.reset(); - mWorld->raycast(Ray(ray13.point1, ray13.point2, decimal(0.01)), &mCallback); - test(!mCallback.isHit); - mCallback.reset(); - mWorld->raycast(Ray(ray14.point1, ray14.point2, decimal(0.01)), &mCallback); - test(!mCallback.isHit); - mCallback.reset(); - mWorld->raycast(Ray(ray15.point1, ray15.point2, decimal(0.01)), &mCallback); - test(!mCallback.isHit); - mCallback.reset(); - mWorld->raycast(Ray(ray16.point1, ray16.point2, decimal(0.01)), &mCallback); - test(!mCallback.isHit); - - // ----- Test raycast hits ----- // - test(mCapsuleBody->raycast(ray11, raycastInfo3)); - test(mCapsuleProxyShape->raycast(ray11, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray11, &mCallback); - test(mCallback.isHit); - mCallback.reset(); - mWorld->raycast(Ray(ray11.point1, ray11.point2, decimal(0.8)), &mCallback); - test(mCallback.isHit); - - test(mCapsuleBody->raycast(ray12, raycastInfo3)); - test(mCapsuleProxyShape->raycast(ray12, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray12, &mCallback); - test(mCallback.isHit); - mCallback.reset(); - mWorld->raycast(Ray(ray12.point1, ray12.point2, decimal(0.8)), &mCallback); - test(mCallback.isHit); - - test(mCapsuleBody->raycast(ray13, raycastInfo3)); - test(mCapsuleProxyShape->raycast(ray13, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray13, &mCallback); - test(mCallback.isHit); - mCallback.reset(); - mWorld->raycast(Ray(ray13.point1, ray13.point2, decimal(0.8)), &mCallback); - test(mCallback.isHit); - - test(mCapsuleBody->raycast(ray14, raycastInfo3)); - test(mCapsuleProxyShape->raycast(ray14, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray14, &mCallback); - test(mCallback.isHit); - mCallback.reset(); - mWorld->raycast(Ray(ray14.point1, ray14.point2, decimal(0.8)), &mCallback); - test(mCallback.isHit); - - test(mCapsuleBody->raycast(ray15, raycastInfo3)); - test(mCapsuleProxyShape->raycast(ray15, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray15, &mCallback); - test(mCallback.isHit); - mCallback.reset(); - mWorld->raycast(Ray(ray15.point1, ray15.point2, decimal(0.8)), &mCallback); - test(mCallback.isHit); - - test(mCapsuleBody->raycast(ray16, raycastInfo3)); - test(mCapsuleProxyShape->raycast(ray16, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray16, &mCallback); - test(mCallback.isHit); - mCallback.reset(); - mWorld->raycast(Ray(ray16.point1, ray16.point2, decimal(0.8)), &mCallback); - test(mCallback.isHit); - } - - /// Test the ProxySphereShape::raycast(), CollisionBody::raycast() and - /// CollisionWorld::raycast() methods. - void testTriangle() { - - // ----- Test feedback data ----- // - Vector3 point1 = mLocalShapeToWorld * Vector3(101, 101, 400); - Vector3 point2 = mLocalShapeToWorld * Vector3(101, 101, -200); - Ray ray(point1, point2); - Ray rayBackward(point2, point1); - - Vector3 hitPoint = mLocalShapeToWorld * Vector3(101, 101, 0); - Vector3 hitNormal = mLocalShapeToWorld.getOrientation() * Vector3(0, 0, 1); - hitNormal.normalize(); - mCallback.shapeToTest = mTriangleProxyShape; - - // CollisionWorld::raycast() - mCallback.reset(); - mTriangleShape->setRaycastTestType(FRONT); - mWorld->raycast(ray, &mCallback); - test(mCallback.isHit); - test(mCallback.raycastInfo.body == mTriangleBody); - test(mCallback.raycastInfo.proxyShape == mTriangleProxyShape); - test(approxEqual(mCallback.raycastInfo.hitFraction, 0.6666, epsilon)); - test(approxEqual(mCallback.raycastInfo.worldPoint.x, hitPoint.x, epsilon)); - test(approxEqual(mCallback.raycastInfo.worldPoint.y, hitPoint.y, epsilon)); - test(approxEqual(mCallback.raycastInfo.worldPoint.z, hitPoint.z, epsilon)); - test(approxEqual(mCallback.raycastInfo.worldNormal.x, hitNormal.x, epsilon)); - test(approxEqual(mCallback.raycastInfo.worldNormal.y, hitNormal.y, epsilon)); - test(approxEqual(mCallback.raycastInfo.worldNormal.z, hitNormal.z, epsilon)); - - mCallback.reset(); - mTriangleShape->setRaycastTestType(BACK); - mWorld->raycast(rayBackward, &mCallback); - test(mCallback.isHit); - test(mCallback.raycastInfo.body == mTriangleBody); - test(mCallback.raycastInfo.proxyShape == mTriangleProxyShape); - test(approxEqual(mCallback.raycastInfo.hitFraction, 0.3333, epsilon)); - test(approxEqual(mCallback.raycastInfo.worldPoint.x, hitPoint.x, epsilon)); - test(approxEqual(mCallback.raycastInfo.worldPoint.y, hitPoint.y, epsilon)); - test(approxEqual(mCallback.raycastInfo.worldPoint.z, hitPoint.z, epsilon)); - test(approxEqual(mCallback.raycastInfo.worldNormal.x, -hitNormal.x, epsilon)); - test(approxEqual(mCallback.raycastInfo.worldNormal.y, -hitNormal.y, epsilon)); - test(approxEqual(mCallback.raycastInfo.worldNormal.z, -hitNormal.z, epsilon)); - - mCallback.reset(); - mTriangleShape->setRaycastTestType(FRONT_AND_BACK); - mWorld->raycast(ray, &mCallback); - test(mCallback.isHit); - test(mCallback.raycastInfo.body == mTriangleBody); - test(mCallback.raycastInfo.proxyShape == mTriangleProxyShape); - test(approxEqual(mCallback.raycastInfo.hitFraction, 0.6666, epsilon)); - test(approxEqual(mCallback.raycastInfo.worldPoint.x, hitPoint.x, epsilon)); - test(approxEqual(mCallback.raycastInfo.worldPoint.y, hitPoint.y, epsilon)); - test(approxEqual(mCallback.raycastInfo.worldPoint.z, hitPoint.z, epsilon)); - test(approxEqual(mCallback.raycastInfo.worldNormal.x, hitNormal.x, epsilon)); - test(approxEqual(mCallback.raycastInfo.worldNormal.y, hitNormal.y, epsilon)); - test(approxEqual(mCallback.raycastInfo.worldNormal.z, hitNormal.z, epsilon)); - - mCallback.reset(); - mTriangleShape->setRaycastTestType(FRONT_AND_BACK); - mWorld->raycast(rayBackward, &mCallback); - test(mCallback.isHit); - test(mCallback.raycastInfo.body == mTriangleBody); - test(mCallback.raycastInfo.proxyShape == mTriangleProxyShape); - test(approxEqual(mCallback.raycastInfo.hitFraction, 0.3333, epsilon)); - test(approxEqual(mCallback.raycastInfo.worldPoint.x, hitPoint.x, epsilon)); - test(approxEqual(mCallback.raycastInfo.worldPoint.y, hitPoint.y, epsilon)); - test(approxEqual(mCallback.raycastInfo.worldPoint.z, hitPoint.z, epsilon)); - test(approxEqual(mCallback.raycastInfo.worldNormal.x, -hitNormal.x, epsilon)); - test(approxEqual(mCallback.raycastInfo.worldNormal.y, -hitNormal.y, epsilon)); - test(approxEqual(mCallback.raycastInfo.worldNormal.z, -hitNormal.z, epsilon)); - - mTriangleShape->setRaycastTestType(FRONT); - - // Correct category filter mask - mCallback.reset(); - mWorld->raycast(ray, &mCallback, CATEGORY1); - test(mCallback.isHit); - - // Wrong category filter mask - mCallback.reset(); - mWorld->raycast(ray, &mCallback, CATEGORY2); - test(!mCallback.isHit); - - // CollisionBody::raycast() - RaycastInfo raycastInfo2; - test(mTriangleBody->raycast(ray, raycastInfo2)); - test(raycastInfo2.body == mTriangleBody); - test(raycastInfo2.proxyShape == mTriangleProxyShape); - test(approxEqual(raycastInfo2.hitFraction, 0.6666, epsilon)); - test(approxEqual(raycastInfo2.worldPoint.x, hitPoint.x, epsilon)); - test(approxEqual(raycastInfo2.worldPoint.y, hitPoint.y, epsilon)); - test(approxEqual(raycastInfo2.worldPoint.z, hitPoint.z, epsilon)); - - // ProxyCollisionShape::raycast() - RaycastInfo raycastInfo3; - test(mTriangleProxyShape->raycast(ray, raycastInfo3)); - test(raycastInfo3.body == mTriangleBody); - test(raycastInfo3.proxyShape == mTriangleProxyShape); - test(approxEqual(raycastInfo3.hitFraction, 0.6666, epsilon)); - test(approxEqual(raycastInfo3.worldPoint.x, hitPoint.x, epsilon)); - test(approxEqual(raycastInfo3.worldPoint.y, hitPoint.y, epsilon)); - test(approxEqual(raycastInfo3.worldPoint.z, hitPoint.z, epsilon)); - - Ray ray1(mLocalShapeToWorld * Vector3(-10, 10, 4), mLocalShapeToWorld * Vector3(15, 6, -4)); - Ray ray2(mLocalShapeToWorld * Vector3(102, 107, 5), mLocalShapeToWorld * Vector3(102, 107, -5)); - Ray ray3(mLocalShapeToWorld * Vector3(106, 102, 6), mLocalShapeToWorld * Vector3(106, 102, -8)); - - Ray ray4(mLocalShapeToWorld * Vector3(100.2, 101, 5), mLocalShapeToWorld * Vector3(100.2, 101, -5)); - Ray ray5(mLocalShapeToWorld * Vector3(100.5, 101.5, 4), mLocalShapeToWorld * Vector3(100.5, 101.5, -54)); - Ray ray6(mLocalShapeToWorld * Vector3(102, 101, 1), mLocalShapeToWorld * Vector3(102, 102, -1)); - - Ray ray4Back(mLocalShapeToWorld * Vector3(100.2, 101, -5), mLocalShapeToWorld * Vector3(100.2, 101, 5)); - Ray ray5Back(mLocalShapeToWorld * Vector3(100.5, 101.5, -54), mLocalShapeToWorld * Vector3(100.5, 101.5, 4)); - Ray ray6Back(mLocalShapeToWorld * Vector3(102, 102, -1), mLocalShapeToWorld * Vector3(102, 101, 1)); - - // ----- Test raycast miss ----- // - test(!mTriangleBody->raycast(ray1, raycastInfo3)); - test(!mTriangleProxyShape->raycast(ray1, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray1, &mCallback); - test(!mCallback.isHit); - mCallback.reset(); - mWorld->raycast(Ray(ray1.point1, ray1.point2, decimal(0.01)), &mCallback); - test(!mCallback.isHit); - mCallback.reset(); - mWorld->raycast(Ray(ray1.point1, ray1.point2, decimal(100.0)), &mCallback); - test(!mCallback.isHit); - - test(!mTriangleBody->raycast(ray2, raycastInfo3)); - test(!mTriangleProxyShape->raycast(ray2, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray2, &mCallback); - test(!mCallback.isHit); - - test(!mTriangleBody->raycast(ray3, raycastInfo3)); - test(!mTriangleProxyShape->raycast(ray3, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray3, &mCallback); - test(!mCallback.isHit); - - // Test backward ray against front triangles (not hit should occur) - mTriangleShape->setRaycastTestType(FRONT); - - test(!mTriangleBody->raycast(ray4Back, raycastInfo3)); - test(!mTriangleProxyShape->raycast(ray4Back, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray4Back, &mCallback); - test(!mCallback.isHit); - - test(!mTriangleBody->raycast(ray5Back, raycastInfo3)); - test(!mTriangleProxyShape->raycast(ray5Back, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray5Back, &mCallback); - test(!mCallback.isHit); - - test(!mTriangleBody->raycast(ray6Back, raycastInfo3)); - test(!mTriangleProxyShape->raycast(ray6Back, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray6Back, &mCallback); - test(!mCallback.isHit); - - // Test front ray against back triangles (not hit should occur) - mTriangleShape->setRaycastTestType(BACK); - - test(!mTriangleBody->raycast(ray4, raycastInfo3)); - test(!mTriangleProxyShape->raycast(ray4, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray4, &mCallback); - test(!mCallback.isHit); - - test(!mTriangleBody->raycast(ray5, raycastInfo3)); - test(!mTriangleProxyShape->raycast(ray5, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray5, &mCallback); - test(!mCallback.isHit); - - test(!mTriangleBody->raycast(ray6, raycastInfo3)); - test(!mTriangleProxyShape->raycast(ray6, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray6, &mCallback); - test(!mCallback.isHit); - - // ----- Test raycast hits ----- // - - // Test front ray against front triangles - mTriangleShape->setRaycastTestType(FRONT); - - test(mTriangleBody->raycast(ray4, raycastInfo3)); - test(mTriangleProxyShape->raycast(ray4, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray4, &mCallback); - test(mCallback.isHit); - mCallback.reset(); - mWorld->raycast(Ray(ray4.point1, ray4.point2, decimal(0.8)), &mCallback); - test(mCallback.isHit); - - test(mTriangleBody->raycast(ray5, raycastInfo3)); - test(mTriangleProxyShape->raycast(ray5, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray5, &mCallback); - test(mCallback.isHit); - mCallback.reset(); - mWorld->raycast(Ray(ray5.point1, ray5.point2, decimal(0.8)), &mCallback); - test(mCallback.isHit); - - test(mTriangleBody->raycast(ray6, raycastInfo3)); - test(mTriangleProxyShape->raycast(ray6, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray6, &mCallback); - mCallback.reset(); - mWorld->raycast(Ray(ray6.point1, ray6.point2, decimal(0.8)), &mCallback); - - // Test back ray against back triangles - mTriangleShape->setRaycastTestType(BACK); - - test(mTriangleBody->raycast(ray4Back, raycastInfo3)); - test(mTriangleProxyShape->raycast(ray4Back, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray4Back, &mCallback); - test(mCallback.isHit); - mCallback.reset(); - mWorld->raycast(Ray(ray4Back.point1, ray4Back.point2, decimal(0.8)), &mCallback); - test(mCallback.isHit); - - test(mTriangleBody->raycast(ray5Back, raycastInfo3)); - test(mTriangleProxyShape->raycast(ray5Back, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray5Back, &mCallback); - test(mCallback.isHit); - mCallback.reset(); - mWorld->raycast(Ray(ray5Back.point1, ray5Back.point2, decimal(1.0)), &mCallback); - test(mCallback.isHit); - - test(mTriangleBody->raycast(ray6Back, raycastInfo3)); - test(mTriangleProxyShape->raycast(ray6Back, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray6Back, &mCallback); - mCallback.reset(); - mWorld->raycast(Ray(ray6Back.point1, ray6Back.point2, decimal(0.8)), &mCallback); - - // Test front ray against front-back triangles - mTriangleShape->setRaycastTestType(FRONT_AND_BACK); - - test(mTriangleBody->raycast(ray4, raycastInfo3)); - test(mTriangleProxyShape->raycast(ray4, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray4, &mCallback); - test(mCallback.isHit); - mCallback.reset(); - mWorld->raycast(Ray(ray4.point1, ray4.point2, decimal(0.8)), &mCallback); - test(mCallback.isHit); - - test(mTriangleBody->raycast(ray5, raycastInfo3)); - test(mTriangleProxyShape->raycast(ray5, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray5, &mCallback); - test(mCallback.isHit); - mCallback.reset(); - mWorld->raycast(Ray(ray5.point1, ray5.point2, decimal(0.8)), &mCallback); - test(mCallback.isHit); - - test(mTriangleBody->raycast(ray6, raycastInfo3)); - test(mTriangleProxyShape->raycast(ray6, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray6, &mCallback); - mCallback.reset(); - mWorld->raycast(Ray(ray6.point1, ray6.point2, decimal(0.8)), &mCallback); - - // Test back ray against front-back triangles - mTriangleShape->setRaycastTestType(FRONT_AND_BACK); - - test(mTriangleBody->raycast(ray4Back, raycastInfo3)); - test(mTriangleProxyShape->raycast(ray4Back, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray4Back, &mCallback); - test(mCallback.isHit); - mCallback.reset(); - mWorld->raycast(Ray(ray4Back.point1, ray4Back.point2, decimal(0.8)), &mCallback); - test(mCallback.isHit); - - test(mTriangleBody->raycast(ray5Back, raycastInfo3)); - test(mTriangleProxyShape->raycast(ray5Back, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray5Back, &mCallback); - test(mCallback.isHit); - mCallback.reset(); - mWorld->raycast(Ray(ray5Back.point1, ray5Back.point2, decimal(1.0)), &mCallback); - test(mCallback.isHit); - - test(mTriangleBody->raycast(ray6Back, raycastInfo3)); - test(mTriangleProxyShape->raycast(ray6Back, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray6Back, &mCallback); - mCallback.reset(); - mWorld->raycast(Ray(ray6Back.point1, ray6Back.point2, decimal(0.8)), &mCallback); - } - - /// Test the ProxyConeShape::raycast(), CollisionBody::raycast() and - /// CollisionWorld::raycast() methods. - void testCone() { - - // ----- Test feedback data ----- // - Vector3 point1A = mLocalShapeToWorld * Vector3(0 , 0, 3); - Vector3 point1B = mLocalShapeToWorld * Vector3(0, 0, -7); - Ray ray(point1A, point1B); - Vector3 hitPoint = mLocalShapeToWorld * Vector3(0, 0, 1); - - Vector3 point2A = mLocalShapeToWorld * Vector3(1 , -5, 0); - Vector3 point2B = mLocalShapeToWorld * Vector3(1, 5, 0); - Ray rayBottom(point2A, point2B); - Vector3 hitPoint2 = mLocalShapeToWorld * Vector3(1, -3, 0); - - mCallback.shapeToTest = mConeProxyShape; - - // CollisionWorld::raycast() - mCallback.reset(); - mWorld->raycast(ray, &mCallback); - test(mCallback.isHit); - test(mCallback.raycastInfo.body == mConeBody); - test(mCallback.raycastInfo.proxyShape == mConeProxyShape); - test(approxEqual(mCallback.raycastInfo.hitFraction, decimal(0.2), epsilon)); - test(approxEqual(mCallback.raycastInfo.worldPoint.x, hitPoint.x, epsilon)); - test(approxEqual(mCallback.raycastInfo.worldPoint.y, hitPoint.y, epsilon)); - test(approxEqual(mCallback.raycastInfo.worldPoint.z, hitPoint.z, epsilon)); - - // Correct category filter mask - mCallback.reset(); - mWorld->raycast(ray, &mCallback, CATEGORY2); - test(mCallback.isHit); - - // Wrong category filter mask - mCallback.reset(); - mWorld->raycast(ray, &mCallback, CATEGORY1); - test(!mCallback.isHit); - - // CollisionBody::raycast() - RaycastInfo raycastInfo2; - test(mConeBody->raycast(ray, raycastInfo2)); - test(raycastInfo2.body == mConeBody); - test(raycastInfo2.proxyShape == mConeProxyShape); - test(approxEqual(raycastInfo2.hitFraction, decimal(0.2), epsilon)); - test(approxEqual(raycastInfo2.worldPoint.x, hitPoint.x, epsilon)); - test(approxEqual(raycastInfo2.worldPoint.y, hitPoint.y, epsilon)); - test(approxEqual(raycastInfo2.worldPoint.z, hitPoint.z, epsilon)); - - // ProxyCollisionShape::raycast() - RaycastInfo raycastInfo3; - test(mConeProxyShape->raycast(ray, raycastInfo3)); - test(raycastInfo3.body == mConeBody); - test(raycastInfo3.proxyShape == mConeProxyShape); - test(approxEqual(raycastInfo3.hitFraction, decimal(0.2), epsilon)); - test(approxEqual(raycastInfo3.worldPoint.x, hitPoint.x, epsilon)); - test(approxEqual(raycastInfo3.worldPoint.y, hitPoint.y, epsilon)); - test(approxEqual(raycastInfo3.worldPoint.z, hitPoint.z, epsilon)); - - mCallback.reset(); - mWorld->raycast(rayBottom, &mCallback); - test(mCallback.isHit); - test(mCallback.raycastInfo.body == mConeBody); - test(mCallback.raycastInfo.proxyShape == mConeProxyShape); - test(approxEqual(mCallback.raycastInfo.hitFraction, decimal(0.2), epsilon)); - test(approxEqual(mCallback.raycastInfo.worldPoint.x, hitPoint2.x, epsilon)); - test(approxEqual(mCallback.raycastInfo.worldPoint.y, hitPoint2.y, epsilon)); - test(approxEqual(mCallback.raycastInfo.worldPoint.z, hitPoint2.z, epsilon)); - - // CollisionBody::raycast() - RaycastInfo raycastInfo5; - test(mConeBody->raycast(rayBottom, raycastInfo5)); - test(raycastInfo5.body == mConeBody); - test(raycastInfo5.proxyShape == mConeProxyShape); - test(approxEqual(raycastInfo5.hitFraction, decimal(0.2), epsilon)); - test(approxEqual(raycastInfo5.worldPoint.x, hitPoint2.x, epsilon)); - test(approxEqual(raycastInfo5.worldPoint.y, hitPoint2.y, epsilon)); - test(approxEqual(raycastInfo5.worldPoint.z, hitPoint2.z, epsilon)); - - // ProxyCollisionShape::raycast() - RaycastInfo raycastInfo6; - test(mConeProxyShape->raycast(rayBottom, raycastInfo6)); - test(raycastInfo6.body == mConeBody); - test(raycastInfo6.proxyShape == mConeProxyShape); - test(approxEqual(raycastInfo6.hitFraction, decimal(0.2), epsilon)); - test(approxEqual(raycastInfo6.worldPoint.x, hitPoint2.x, epsilon)); - test(approxEqual(raycastInfo6.worldPoint.y, hitPoint2.y, epsilon)); - test(approxEqual(raycastInfo6.worldPoint.z, hitPoint2.z, epsilon)); - - Ray ray1(mLocalShapeToWorld * Vector3(0, 0, 0), mLocalShapeToWorld * Vector3(5, 7, -1)); - Ray ray2(mLocalShapeToWorld * Vector3(5, 11, 7), mLocalShapeToWorld * Vector3(17, 29, 28)); - Ray ray3(mLocalShapeToWorld * Vector3(-1, -2, 1), mLocalShapeToWorld * Vector3(-13, -2, 22)); - Ray ray4(mLocalShapeToWorld * Vector3(10, 10, 10), mLocalShapeToWorld * Vector3(22, 28, 31)); - Ray ray5(mLocalShapeToWorld * Vector3(4, 1, -1), mLocalShapeToWorld * Vector3(-26, 1, -1)); - Ray ray6(mLocalShapeToWorld * Vector3(3, 4, 1), mLocalShapeToWorld * Vector3(3, -16, 1)); - Ray ray7(mLocalShapeToWorld * Vector3(1, -4, 3), mLocalShapeToWorld * Vector3(1, -4, -17)); - Ray ray8(mLocalShapeToWorld * Vector3(-4, 4, 0), mLocalShapeToWorld * Vector3(26, 4, 0)); - Ray ray9(mLocalShapeToWorld * Vector3(0, -4, -7), mLocalShapeToWorld * Vector3(0, 46, -7)); - Ray ray10(mLocalShapeToWorld * Vector3(-3, -2, -6), mLocalShapeToWorld * Vector3(-3, -2, 74)); - Ray ray11(mLocalShapeToWorld * Vector3(3, -1, 0.5), mLocalShapeToWorld * Vector3(-27, -1, 0.5)); - Ray ray12(mLocalShapeToWorld * Vector3(1, 4, -1), mLocalShapeToWorld * Vector3(1, -26, -1)); - Ray ray13(mLocalShapeToWorld * Vector3(-1, -2, 3), mLocalShapeToWorld * Vector3(-1, -2, -27)); - Ray ray14(mLocalShapeToWorld * Vector3(-2, 0, 0.8), mLocalShapeToWorld * Vector3(30, 0, 0.8)); - Ray ray15(mLocalShapeToWorld * Vector3(0, -4, 1), mLocalShapeToWorld * Vector3(0, 30, 1)); - Ray ray16(mLocalShapeToWorld * Vector3(-0.9, 0, -4), mLocalShapeToWorld * Vector3(-0.9, 0, 30)); - - // ----- Test raycast miss ----- // - test(!mConeBody->raycast(ray1, raycastInfo3)); - test(!mConeProxyShape->raycast(ray1, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray1, &mCallback); - test(!mCallback.isHit); - mCallback.reset(); - mWorld->raycast(Ray(ray1.point1, ray1.point2, decimal(0.01)), &mCallback); - test(!mCallback.isHit); - mCallback.reset(); - mWorld->raycast(Ray(ray1.point1, ray1.point2, decimal(100.0)), &mCallback); - test(!mCallback.isHit); - - test(!mConeBody->raycast(ray2, raycastInfo3)); - test(!mConeProxyShape->raycast(ray2, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray2, &mCallback); - test(!mCallback.isHit); - - test(!mConeBody->raycast(ray3, raycastInfo3)); - test(!mConeProxyShape->raycast(ray3, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray3, &mCallback); - test(!mCallback.isHit); - - test(!mConeBody->raycast(ray4, raycastInfo3)); - test(!mConeProxyShape->raycast(ray4, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray4, &mCallback); - test(!mCallback.isHit); - - test(!mConeBody->raycast(ray5, raycastInfo3)); - test(!mConeProxyShape->raycast(ray5, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray5, &mCallback); - test(!mCallback.isHit); - - test(!mConeBody->raycast(ray6, raycastInfo3)); - test(!mConeProxyShape->raycast(ray6, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray6, &mCallback); - test(!mCallback.isHit); - - test(!mConeBody->raycast(ray7, raycastInfo3)); - test(!mConeProxyShape->raycast(ray7, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray7, &mCallback); - test(!mCallback.isHit); - - test(!mConeBody->raycast(ray8, raycastInfo3)); - test(!mConeProxyShape->raycast(ray8, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray8, &mCallback); - test(!mCallback.isHit); - - test(!mConeBody->raycast(ray9, raycastInfo3)); - test(!mConeProxyShape->raycast(ray9, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray9, &mCallback); - test(!mCallback.isHit); - - test(!mConeBody->raycast(ray10, raycastInfo3)); - test(!mConeProxyShape->raycast(ray10, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray10, &mCallback); - test(!mCallback.isHit); - - mCallback.reset(); - mWorld->raycast(Ray(ray11.point1, ray11.point2, decimal(0.01)), &mCallback); - test(!mCallback.isHit); - mCallback.reset(); - mWorld->raycast(Ray(ray12.point1, ray12.point2, decimal(0.01)), &mCallback); - test(!mCallback.isHit); - mCallback.reset(); - mWorld->raycast(Ray(ray13.point1, ray13.point2, decimal(0.01)), &mCallback); - test(!mCallback.isHit); - mCallback.reset(); - mWorld->raycast(Ray(ray14.point1, ray14.point2, decimal(0.01)), &mCallback); - test(!mCallback.isHit); - mCallback.reset(); - mWorld->raycast(Ray(ray15.point1, ray15.point2, decimal(0.01)), &mCallback); - test(!mCallback.isHit); - mCallback.reset(); - mWorld->raycast(Ray(ray16.point1, ray16.point2, decimal(0.01)), &mCallback); - test(!mCallback.isHit); - - // ----- Test raycast hits ----- // - test(mConeBody->raycast(ray11, raycastInfo3)); - test(mConeProxyShape->raycast(ray11, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray11, &mCallback); - test(mCallback.isHit); - mCallback.reset(); - mWorld->raycast(Ray(ray11.point1, ray11.point2, decimal(0.8)), &mCallback); - test(mCallback.isHit); - - test(mConeBody->raycast(ray12, raycastInfo3)); - test(mConeProxyShape->raycast(ray12, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray12, &mCallback); - test(mCallback.isHit); - mCallback.reset(); - mWorld->raycast(Ray(ray12.point1, ray12.point2, decimal(0.8)), &mCallback); - test(mCallback.isHit); - - test(mConeBody->raycast(ray13, raycastInfo3)); - test(mConeProxyShape->raycast(ray13, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray13, &mCallback); - test(mCallback.isHit); - mCallback.reset(); - mWorld->raycast(Ray(ray13.point1, ray13.point2, decimal(0.8)), &mCallback); - test(mCallback.isHit); - - test(mConeBody->raycast(ray14, raycastInfo3)); - test(mConeProxyShape->raycast(ray14, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray14, &mCallback); - test(mCallback.isHit); - mCallback.reset(); - mWorld->raycast(Ray(ray14.point1, ray14.point2, decimal(0.8)), &mCallback); - test(mCallback.isHit); - - test(mConeBody->raycast(ray15, raycastInfo3)); - test(mConeProxyShape->raycast(ray15, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray15, &mCallback); - test(mCallback.isHit); - mCallback.reset(); - mWorld->raycast(Ray(ray15.point1, ray15.point2, decimal(0.8)), &mCallback); - test(mCallback.isHit); - - test(mConeBody->raycast(ray16, raycastInfo3)); - test(mConeProxyShape->raycast(ray16, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray16, &mCallback); - test(mCallback.isHit); - mCallback.reset(); - mWorld->raycast(Ray(ray16.point1, ray16.point2, decimal(0.8)), &mCallback); - test(mCallback.isHit); - } - - /// Test the ProxyConvexMeshShape::raycast(), CollisionBody::raycast() and - /// CollisionWorld::raycast() methods. - void testConvexMesh() { - - // ----- Test feedback data ----- // - Vector3 point1 = mLocalShapeToWorld * Vector3(1 , 2, 6); - Vector3 point2 = mLocalShapeToWorld * Vector3(1, 2, -4); - Ray ray(point1, point2); - Vector3 hitPoint = mLocalShapeToWorld * Vector3(1, 2, 4); - - mCallback.shapeToTest = mConvexMeshProxyShape; - - // CollisionWorld::raycast() - mCallback.reset(); - mWorld->raycast(ray, &mCallback); - test(mCallback.isHit); - test(mCallback.raycastInfo.body == mConvexMeshBody); - test(mCallback.raycastInfo.proxyShape == mConvexMeshProxyShape); - test(approxEqual(mCallback.raycastInfo.hitFraction, decimal(0.2), epsilon)); - test(approxEqual(mCallback.raycastInfo.worldPoint.x, hitPoint.x, epsilon)); - test(approxEqual(mCallback.raycastInfo.worldPoint.y, hitPoint.y, epsilon)); - test(approxEqual(mCallback.raycastInfo.worldPoint.z, hitPoint.z, epsilon)); - - // Correct category filter mask - mCallback.reset(); - mWorld->raycast(ray, &mCallback, CATEGORY2); - test(mCallback.isHit); - - // Wrong category filter mask - mCallback.reset(); - mWorld->raycast(ray, &mCallback, CATEGORY1); - test(!mCallback.isHit); - - // CollisionBody::raycast() - RaycastInfo raycastInfo2; - test(mConvexMeshBody->raycast(ray, raycastInfo2)); - test(raycastInfo2.body == mConvexMeshBody); - test(raycastInfo2.proxyShape == mConvexMeshProxyShape); - test(approxEqual(raycastInfo2.hitFraction, decimal(0.2), epsilon)); - test(approxEqual(raycastInfo2.worldPoint.x, hitPoint.x, epsilon)); - test(approxEqual(raycastInfo2.worldPoint.y, hitPoint.y, epsilon)); - test(approxEqual(raycastInfo2.worldPoint.z, hitPoint.z, epsilon)); - - // ProxyCollisionShape::raycast() - RaycastInfo raycastInfo3; - test(mConvexMeshBodyEdgesInfo->raycast(ray, raycastInfo3)); - test(raycastInfo3.body == mConvexMeshBodyEdgesInfo); - test(raycastInfo3.proxyShape == mConvexMeshProxyShapeEdgesInfo); - test(approxEqual(raycastInfo3.hitFraction, decimal(0.2), epsilon)); - test(approxEqual(raycastInfo3.worldPoint.x, hitPoint.x, epsilon)); - test(approxEqual(raycastInfo3.worldPoint.y, hitPoint.y, epsilon)); - test(approxEqual(raycastInfo3.worldPoint.z, hitPoint.z, epsilon)); - - // ProxyCollisionShape::raycast() - RaycastInfo raycastInfo4; - test(mConvexMeshProxyShape->raycast(ray, raycastInfo4)); - test(raycastInfo4.body == mConvexMeshBody); - test(raycastInfo4.proxyShape == mConvexMeshProxyShape); - test(approxEqual(raycastInfo4.hitFraction, decimal(0.2), epsilon)); - test(approxEqual(raycastInfo4.worldPoint.x, hitPoint.x, epsilon)); - test(approxEqual(raycastInfo4.worldPoint.y, hitPoint.y, epsilon)); - test(approxEqual(raycastInfo4.worldPoint.z, hitPoint.z, epsilon)); - - // ProxyCollisionShape::raycast() - RaycastInfo raycastInfo5; - test(mConvexMeshProxyShapeEdgesInfo->raycast(ray, raycastInfo5)); - test(raycastInfo5.body == mConvexMeshBodyEdgesInfo); - test(raycastInfo5.proxyShape == mConvexMeshProxyShapeEdgesInfo); - test(approxEqual(raycastInfo5.hitFraction, decimal(0.2), epsilon)); - test(approxEqual(raycastInfo5.worldPoint.x, hitPoint.x, epsilon)); - test(approxEqual(raycastInfo5.worldPoint.y, hitPoint.y, epsilon)); - test(approxEqual(raycastInfo5.worldPoint.z, hitPoint.z, epsilon)); - - Ray ray1(mLocalShapeToWorld * Vector3(0, 0, 0), mLocalShapeToWorld * Vector3(5, 7, -1)); - Ray ray2(mLocalShapeToWorld * Vector3(5, 11, 7), mLocalShapeToWorld * Vector3(17, 29, 28)); - Ray ray3(mLocalShapeToWorld * Vector3(1, 2, 3), mLocalShapeToWorld * Vector3(-11, 2, 24)); - Ray ray4(mLocalShapeToWorld * Vector3(10, 10, 10), mLocalShapeToWorld * Vector3(22, 28, 31)); - Ray ray5(mLocalShapeToWorld * Vector3(3, 1, -5), mLocalShapeToWorld * Vector3(-30, 1, -5)); - Ray ray6(mLocalShapeToWorld * Vector3(4, 4, 1), mLocalShapeToWorld * Vector3(4, -30, 1)); - Ray ray7(mLocalShapeToWorld * Vector3(1, -4, 5), mLocalShapeToWorld * Vector3(1, -4, -30)); - Ray ray8(mLocalShapeToWorld * Vector3(-4, 4, 0), mLocalShapeToWorld * Vector3(30, 4, 0)); - Ray ray9(mLocalShapeToWorld * Vector3(0, -4, -7), mLocalShapeToWorld * Vector3(0, 30, -7)); - Ray ray10(mLocalShapeToWorld * Vector3(-3, 0, -6), mLocalShapeToWorld * Vector3(-3, 0, 30)); - Ray ray11(mLocalShapeToWorld * Vector3(3, 1, 2), mLocalShapeToWorld * Vector3(-30, 0, -6)); - Ray ray12(mLocalShapeToWorld * Vector3(1, 4, -1), mLocalShapeToWorld * Vector3(1, -30, -1)); - Ray ray13(mLocalShapeToWorld * Vector3(-1, 2, 5), mLocalShapeToWorld * Vector3(-1, 2, -30)); - Ray ray14(mLocalShapeToWorld * Vector3(-3, 2, -2), mLocalShapeToWorld * Vector3(30, 2, -2)); - Ray ray15(mLocalShapeToWorld * Vector3(0, -4, 1), mLocalShapeToWorld * Vector3(0, 30, 1)); - Ray ray16(mLocalShapeToWorld * Vector3(-1, 2, -7), mLocalShapeToWorld * Vector3(-1, 2, 30)); - - // ----- Test raycast miss ----- // - test(!mConvexMeshBody->raycast(ray1, raycastInfo3)); - test(!mConvexMeshBodyEdgesInfo->raycast(ray1, raycastInfo3)); - test(!mConvexMeshProxyShape->raycast(ray1, raycastInfo3)); - test(!mConvexMeshProxyShapeEdgesInfo->raycast(ray1, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray1, &mCallback); - test(!mCallback.isHit); - mCallback.reset(); - mWorld->raycast(Ray(ray1.point1, ray1.point2, decimal(0.01)), &mCallback); - test(!mCallback.isHit); - mCallback.reset(); - mWorld->raycast(Ray(ray1.point1, ray1.point2, decimal(100.0)), &mCallback); - test(!mCallback.isHit); - - test(!mConvexMeshBody->raycast(ray2, raycastInfo3)); - test(!mConvexMeshBodyEdgesInfo->raycast(ray2, raycastInfo3)); - test(!mConvexMeshProxyShape->raycast(ray2, raycastInfo3)); - test(!mConvexMeshProxyShapeEdgesInfo->raycast(ray2, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray2, &mCallback); - test(!mCallback.isHit); - - test(!mConvexMeshBody->raycast(ray3, raycastInfo3)); - test(!mConvexMeshBodyEdgesInfo->raycast(ray3, raycastInfo3)); - test(!mConvexMeshProxyShape->raycast(ray3, raycastInfo3)); - test(!mConvexMeshProxyShapeEdgesInfo->raycast(ray3, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray3, &mCallback); - test(!mCallback.isHit); - - test(!mConvexMeshBody->raycast(ray4, raycastInfo3)); - test(!mConvexMeshBodyEdgesInfo->raycast(ray4, raycastInfo3)); - test(!mConvexMeshProxyShape->raycast(ray4, raycastInfo3)); - test(!mConvexMeshProxyShapeEdgesInfo->raycast(ray4, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray4, &mCallback); - test(!mCallback.isHit); - - test(!mConvexMeshBody->raycast(ray5, raycastInfo3)); - test(!mConvexMeshBodyEdgesInfo->raycast(ray5, raycastInfo3)); - test(!mConvexMeshProxyShape->raycast(ray5, raycastInfo3)); - test(!mConvexMeshProxyShapeEdgesInfo->raycast(ray5, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray5, &mCallback); - test(!mCallback.isHit); - - test(!mConvexMeshBody->raycast(ray6, raycastInfo3)); - test(!mConvexMeshBodyEdgesInfo->raycast(ray6, raycastInfo3)); - test(!mConvexMeshProxyShape->raycast(ray6, raycastInfo3)); - test(!mConvexMeshProxyShapeEdgesInfo->raycast(ray6, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray6, &mCallback); - test(!mCallback.isHit); - - test(!mConvexMeshBody->raycast(ray7, raycastInfo3)); - test(!mConvexMeshBodyEdgesInfo->raycast(ray7, raycastInfo3)); - test(!mConvexMeshProxyShape->raycast(ray7, raycastInfo3)); - test(!mConvexMeshProxyShapeEdgesInfo->raycast(ray7, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray7, &mCallback); - test(!mCallback.isHit); - - test(!mConvexMeshBody->raycast(ray8, raycastInfo3)); - test(!mConvexMeshBodyEdgesInfo->raycast(ray8, raycastInfo3)); - test(!mConvexMeshProxyShape->raycast(ray8, raycastInfo3)); - test(!mConvexMeshProxyShapeEdgesInfo->raycast(ray8, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray8, &mCallback); - test(!mCallback.isHit); - - test(!mConvexMeshBody->raycast(ray9, raycastInfo3)); - test(!mConvexMeshBodyEdgesInfo->raycast(ray9, raycastInfo3)); - test(!mConvexMeshProxyShape->raycast(ray9, raycastInfo3)); - test(!mConvexMeshProxyShapeEdgesInfo->raycast(ray9, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray9, &mCallback); - test(!mCallback.isHit); - - test(!mConvexMeshBody->raycast(ray10, raycastInfo3)); - test(!mConvexMeshBodyEdgesInfo->raycast(ray10, raycastInfo3)); - test(!mConvexMeshProxyShape->raycast(ray10, raycastInfo3)); - test(!mConvexMeshProxyShapeEdgesInfo->raycast(ray10, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray10, &mCallback); - test(!mCallback.isHit); - - mCallback.reset(); - mWorld->raycast(Ray(ray11.point1, ray11.point2, decimal(0.01)), &mCallback); - test(!mCallback.isHit); - mCallback.reset(); - mWorld->raycast(Ray(ray12.point1, ray12.point2, decimal(0.01)), &mCallback); - test(!mCallback.isHit); - mCallback.reset(); - mWorld->raycast(Ray(ray13.point1, ray13.point2, decimal(0.01)), &mCallback); - test(!mCallback.isHit); - mCallback.reset(); - mWorld->raycast(Ray(ray14.point1, ray14.point2, decimal(0.01)), &mCallback); - test(!mCallback.isHit); - mCallback.reset(); - mWorld->raycast(Ray(ray15.point1, ray15.point2, decimal(0.01)), &mCallback); - test(!mCallback.isHit); - mCallback.reset(); - mWorld->raycast(Ray(ray16.point1, ray16.point2, decimal(0.01)), &mCallback); - test(!mCallback.isHit); - - // ----- Test raycast hits ----- // - test(mConvexMeshBody->raycast(ray11, raycastInfo3)); - test(mConvexMeshBodyEdgesInfo->raycast(ray11, raycastInfo3)); - test(mConvexMeshProxyShape->raycast(ray11, raycastInfo3)); - test(mConvexMeshProxyShapeEdgesInfo->raycast(ray11, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray11, &mCallback); - test(mCallback.isHit); - mCallback.reset(); - mWorld->raycast(Ray(ray11.point1, ray11.point2, decimal(0.8)), &mCallback); - test(mCallback.isHit); - - test(mConvexMeshBody->raycast(ray12, raycastInfo3)); - test(mConvexMeshBodyEdgesInfo->raycast(ray12, raycastInfo3)); - test(mConvexMeshProxyShape->raycast(ray12, raycastInfo3)); - test(mConvexMeshProxyShapeEdgesInfo->raycast(ray12, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray12, &mCallback); - test(mCallback.isHit); - mCallback.reset(); - mWorld->raycast(Ray(ray12.point1, ray12.point2, decimal(0.8)), &mCallback); - test(mCallback.isHit); - - test(mConvexMeshBody->raycast(ray13, raycastInfo3)); - test(mConvexMeshBodyEdgesInfo->raycast(ray13, raycastInfo3)); - test(mConvexMeshProxyShape->raycast(ray13, raycastInfo3)); - test(mConvexMeshProxyShapeEdgesInfo->raycast(ray13, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray13, &mCallback); - test(mCallback.isHit); - mCallback.reset(); - mWorld->raycast(Ray(ray13.point1, ray13.point2, decimal(0.8)), &mCallback); - test(mCallback.isHit); - - test(mConvexMeshBody->raycast(ray14, raycastInfo3)); - test(mConvexMeshBodyEdgesInfo->raycast(ray14, raycastInfo3)); - test(mConvexMeshProxyShape->raycast(ray14, raycastInfo3)); - test(mConvexMeshProxyShapeEdgesInfo->raycast(ray14, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray14, &mCallback); - test(mCallback.isHit); - mCallback.reset(); - mWorld->raycast(Ray(ray14.point1, ray14.point2, decimal(0.8)), &mCallback); - test(mCallback.isHit); - - test(mConvexMeshBody->raycast(ray15, raycastInfo3)); - test(mConvexMeshBodyEdgesInfo->raycast(ray15, raycastInfo3)); - test(mConvexMeshProxyShape->raycast(ray15, raycastInfo3)); - test(mConvexMeshProxyShapeEdgesInfo->raycast(ray15, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray15, &mCallback); - test(mCallback.isHit); - mCallback.reset(); - mWorld->raycast(Ray(ray15.point1, ray15.point2, decimal(0.8)), &mCallback); - test(mCallback.isHit); - - test(mConvexMeshBody->raycast(ray16, raycastInfo3)); - test(mConvexMeshBodyEdgesInfo->raycast(ray16, raycastInfo3)); - test(mConvexMeshProxyShape->raycast(ray16, raycastInfo3)); - test(mConvexMeshProxyShapeEdgesInfo->raycast(ray16, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray16, &mCallback); - test(mCallback.isHit); - mCallback.reset(); - mWorld->raycast(Ray(ray16.point1, ray16.point2, decimal(0.8)), &mCallback); - test(mCallback.isHit); - } - - /// Test the ProxyCylinderShape::raycast(), CollisionBody::raycast() and - /// CollisionWorld::raycast() methods. - void testCylinder() { - - // ----- Test feedback data ----- // - Vector3 point1A = mLocalShapeToWorld * Vector3(4 , 1, 0); - Vector3 point1B = mLocalShapeToWorld * Vector3(-6, 1, 0); - Ray ray(point1A, point1B); - Vector3 hitPoint = mLocalShapeToWorld * Vector3(2, 1, 0); - - Vector3 point2A = mLocalShapeToWorld * Vector3(0 , 4.5, 0); - Vector3 point2B = mLocalShapeToWorld * Vector3(0, -5.5, 0); - Ray rayTop(point2A, point2B); - Vector3 hitPointTop = mLocalShapeToWorld * Vector3(0, decimal(2.5), 0); - - Vector3 point3A = mLocalShapeToWorld * Vector3(0 , -4.5, 0); - Vector3 point3B = mLocalShapeToWorld * Vector3(0, 5.5, 0); - Ray rayBottom(point3A, point3B); - Vector3 hitPointBottom = mLocalShapeToWorld * Vector3(0, decimal(-2.5), 0); - - mCallback.shapeToTest = mCylinderProxyShape; - - // CollisionWorld::raycast() - mCallback.reset(); - mWorld->raycast(ray, &mCallback); - test(mCallback.isHit); - test(mCallback.raycastInfo.body == mCylinderBody); - test(mCallback.raycastInfo.proxyShape == mCylinderProxyShape); - test(approxEqual(mCallback.raycastInfo.hitFraction, decimal(0.2), epsilon)); - test(approxEqual(mCallback.raycastInfo.worldPoint.x, hitPoint.x, epsilon)); - test(approxEqual(mCallback.raycastInfo.worldPoint.y, hitPoint.y, epsilon)); - test(approxEqual(mCallback.raycastInfo.worldPoint.z, hitPoint.z, epsilon)); - - // Correct category filter mask - mCallback.reset(); - mWorld->raycast(ray, &mCallback, CATEGORY2); - test(mCallback.isHit); - - // Wrong category filter mask - mCallback.reset(); - mWorld->raycast(ray, &mCallback, CATEGORY1); - test(!mCallback.isHit); - - // CollisionBody::raycast() - RaycastInfo raycastInfo2; - test(mCylinderBody->raycast(ray, raycastInfo2)); - test(raycastInfo2.body == mCylinderBody); - test(raycastInfo2.proxyShape == mCylinderProxyShape); - test(approxEqual(raycastInfo2.hitFraction, decimal(0.2), epsilon)); - test(approxEqual(raycastInfo2.worldPoint.x, hitPoint.x, epsilon)); - test(approxEqual(raycastInfo2.worldPoint.y, hitPoint.y, epsilon)); - test(approxEqual(raycastInfo2.worldPoint.z, hitPoint.z, epsilon)); - - // ProxyCollisionShape::raycast() - RaycastInfo raycastInfo3; - test(mCylinderProxyShape->raycast(ray, raycastInfo3)); - test(raycastInfo3.body == mCylinderBody); - test(raycastInfo3.proxyShape == mCylinderProxyShape); - test(approxEqual(raycastInfo3.hitFraction, decimal(0.2), epsilon)); - test(approxEqual(raycastInfo3.worldPoint.x, hitPoint.x, epsilon)); - test(approxEqual(raycastInfo3.worldPoint.y, hitPoint.y, epsilon)); - test(approxEqual(raycastInfo3.worldPoint.z, hitPoint.z, epsilon)); - - // ProxyCollisionShape::raycast() - RaycastInfo raycastInfo5; - test(mCylinderProxyShape->raycast(rayTop, raycastInfo5)); - test(raycastInfo5.body == mCylinderBody); - test(raycastInfo5.proxyShape == mCylinderProxyShape); - test(approxEqual(raycastInfo5.hitFraction, decimal(0.2), epsilon)); - test(approxEqual(raycastInfo5.worldPoint.x, hitPointTop.x, epsilon)); - test(approxEqual(raycastInfo5.worldPoint.y, hitPointTop.y, epsilon)); - test(approxEqual(raycastInfo5.worldPoint.z, hitPointTop.z, epsilon)); - - // ProxyCollisionShape::raycast() - RaycastInfo raycastInfo6; - test(mCylinderProxyShape->raycast(rayBottom, raycastInfo6)); - test(raycastInfo6.body == mCylinderBody); - test(raycastInfo6.proxyShape == mCylinderProxyShape); - test(approxEqual(raycastInfo6.hitFraction, decimal(0.2), epsilon)); - test(approxEqual(raycastInfo6.worldPoint.x, hitPointBottom.x, epsilon)); - test(approxEqual(raycastInfo6.worldPoint.y, hitPointBottom.y, epsilon)); - test(approxEqual(raycastInfo6.worldPoint.z, hitPointBottom.z, epsilon)); - - Ray ray1(mLocalShapeToWorld * Vector3(0, 0, 0), mLocalShapeToWorld * Vector3(5, 7, -1)); - Ray ray2(mLocalShapeToWorld * Vector3(5, 11, 7), mLocalShapeToWorld * Vector3(17, 20, 28)); - Ray ray3(mLocalShapeToWorld * Vector3(1, 3, -1), mLocalShapeToWorld * Vector3(-11,3, 20)); - Ray ray4(mLocalShapeToWorld * Vector3(10, 10, 10), mLocalShapeToWorld * Vector3(22, 28, 31)); - Ray ray5(mLocalShapeToWorld * Vector3(4, 1, -5), mLocalShapeToWorld * Vector3(-30, 1, -5)); - Ray ray6(mLocalShapeToWorld * Vector3(4, 9, 1), mLocalShapeToWorld * Vector3(4, -30, 1)); - Ray ray7(mLocalShapeToWorld * Vector3(1, -9, 5), mLocalShapeToWorld * Vector3(1, -9, -30)); - Ray ray8(mLocalShapeToWorld * Vector3(-4, 9, 0), mLocalShapeToWorld * Vector3(30, 9, 0)); - Ray ray9(mLocalShapeToWorld * Vector3(0, -9, -4), mLocalShapeToWorld * Vector3(0, 30, -4)); - Ray ray10(mLocalShapeToWorld * Vector3(-4, 0, -6), mLocalShapeToWorld * Vector3(-4, 0, 30)); - Ray ray11(mLocalShapeToWorld * Vector3(4, 1, 1.5), mLocalShapeToWorld * Vector3(-30, 1, 1.5)); - Ray ray12(mLocalShapeToWorld * Vector3(1, 9, -1), mLocalShapeToWorld * Vector3(1, -30, -1)); - Ray ray13(mLocalShapeToWorld * Vector3(-1, 2, 3), mLocalShapeToWorld * Vector3(-1, 2, -30)); - Ray ray14(mLocalShapeToWorld * Vector3(-3, 2, -1.7), mLocalShapeToWorld * Vector3(30, 2, -1.7)); - Ray ray15(mLocalShapeToWorld * Vector3(0, -9, 1), mLocalShapeToWorld * Vector3(0, 30, 1)); - Ray ray16(mLocalShapeToWorld * Vector3(-1, 2, -7), mLocalShapeToWorld * Vector3(-1, 2, 30)); - - // ----- Test raycast miss ----- // - test(!mCylinderBody->raycast(ray1, raycastInfo3)); - test(!mCylinderProxyShape->raycast(ray1, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray1, &mCallback); - test(!mCallback.isHit); - mCallback.reset(); - mWorld->raycast(Ray(ray1.point1, ray1.point2, decimal(0.01)), &mCallback); - test(!mCallback.isHit); - mCallback.reset(); - mWorld->raycast(Ray(ray1.point1, ray1.point2, decimal(100.0)), &mCallback); - test(!mCallback.isHit); - - test(!mCylinderBody->raycast(ray2, raycastInfo3)); - test(!mCylinderProxyShape->raycast(ray2, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray2, &mCallback); - test(!mCallback.isHit); - - test(!mCylinderBody->raycast(ray3, raycastInfo3)); - test(!mCylinderProxyShape->raycast(ray3, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray3, &mCallback); - test(!mCallback.isHit); - - test(!mCylinderBody->raycast(ray4, raycastInfo3)); - test(!mCylinderProxyShape->raycast(ray4, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray4, &mCallback); - test(!mCallback.isHit); - - test(!mCylinderBody->raycast(ray5, raycastInfo3)); - test(!mCylinderProxyShape->raycast(ray5, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray5, &mCallback); - test(!mCallback.isHit); - - test(!mCylinderBody->raycast(ray6, raycastInfo3)); - test(!mCylinderProxyShape->raycast(ray6, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray6, &mCallback); - test(!mCallback.isHit); - - test(!mCylinderBody->raycast(ray7, raycastInfo3)); - test(!mCylinderProxyShape->raycast(ray7, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray7, &mCallback); - test(!mCallback.isHit); - - test(!mCylinderBody->raycast(ray8, raycastInfo3)); - test(!mCylinderProxyShape->raycast(ray8, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray8, &mCallback); - test(!mCallback.isHit); - - test(!mCylinderBody->raycast(ray9, raycastInfo3)); - test(!mCylinderProxyShape->raycast(ray9, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray9, &mCallback); - test(!mCallback.isHit); - - test(!mCylinderBody->raycast(ray10, raycastInfo3)); - test(!mCylinderProxyShape->raycast(ray10, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray10, &mCallback); - test(!mCallback.isHit); - - mCallback.reset(); - mWorld->raycast(Ray(ray11.point1, ray11.point2, decimal(0.01)), &mCallback); - test(!mCallback.isHit); - mCallback.reset(); - mWorld->raycast(Ray(ray12.point1, ray12.point2, decimal(0.01)), &mCallback); - test(!mCallback.isHit); - mCallback.reset(); - mWorld->raycast(Ray(ray13.point1, ray13.point2, decimal(0.01)), &mCallback); - test(!mCallback.isHit); - mCallback.reset(); - mWorld->raycast(Ray(ray14.point1, ray14.point2, decimal(0.01)), &mCallback); - test(!mCallback.isHit); - mCallback.reset(); - mWorld->raycast(Ray(ray15.point1, ray15.point2, decimal(0.01)), &mCallback); - test(!mCallback.isHit); - mCallback.reset(); - mWorld->raycast(Ray(ray16.point1, ray16.point2, decimal(0.01)), &mCallback); - test(!mCallback.isHit); - - // ----- Test raycast hits ----- // - test(mCylinderBody->raycast(ray11, raycastInfo3)); - test(mCylinderProxyShape->raycast(ray11, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray11, &mCallback); - test(mCallback.isHit); - mCallback.reset(); - mWorld->raycast(Ray(ray11.point1, ray11.point2, decimal(0.8)), &mCallback); - test(mCallback.isHit); - - test(mCylinderBody->raycast(ray12, raycastInfo3)); - test(mCylinderProxyShape->raycast(ray12, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray12, &mCallback); - test(mCallback.isHit); - mCallback.reset(); - mWorld->raycast(Ray(ray12.point1, ray12.point2, decimal(0.8)), &mCallback); - test(mCallback.isHit); - - test(mCylinderBody->raycast(ray13, raycastInfo3)); - test(mCylinderProxyShape->raycast(ray13, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray13, &mCallback); - test(mCallback.isHit); - mCallback.reset(); - mWorld->raycast(Ray(ray13.point1, ray13.point2, decimal(0.8)), &mCallback); - test(mCallback.isHit); - - test(mCylinderBody->raycast(ray14, raycastInfo3)); - test(mCylinderProxyShape->raycast(ray14, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray14, &mCallback); - test(mCallback.isHit); - mCallback.reset(); - mWorld->raycast(Ray(ray14.point1, ray14.point2, decimal(0.8)), &mCallback); - test(mCallback.isHit); - - test(mCylinderBody->raycast(ray15, raycastInfo3)); - test(mCylinderProxyShape->raycast(ray15, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray15, &mCallback); - test(mCallback.isHit); - mCallback.reset(); - mWorld->raycast(Ray(ray15.point1, ray15.point2, decimal(0.8)), &mCallback); - test(mCallback.isHit); - - test(mCylinderBody->raycast(ray16, raycastInfo3)); - test(mCylinderProxyShape->raycast(ray16, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray16, &mCallback); - test(mCallback.isHit); - mCallback.reset(); - mWorld->raycast(Ray(ray16.point1, ray16.point2, decimal(0.8)), &mCallback); - test(mCallback.isHit); - } - - /// Test the CollisionBody::raycast() and - /// CollisionWorld::raycast() methods. - void testCompound() { - - // ----- Test feedback data ----- // - - // Raycast hit against the sphere shape - Ray ray1(mLocalShape2ToWorld * Vector3(4, 1, 2), mLocalShape2ToWorld * Vector3(-30, 1, 2)); - Ray ray2(mLocalShape2ToWorld * Vector3(1, 4, -1), mLocalShape2ToWorld * Vector3(1, -30, -1)); - Ray ray3(mLocalShape2ToWorld * Vector3(-1, 2, 5), mLocalShape2ToWorld * Vector3(-1, 2, -30)); - Ray ray4(mLocalShape2ToWorld * Vector3(-5, 2, -2), mLocalShape2ToWorld * Vector3(30, 2, -2)); - Ray ray5(mLocalShape2ToWorld * Vector3(0, -4, 1), mLocalShape2ToWorld * Vector3(0, 30, 1)); - Ray ray6(mLocalShape2ToWorld * Vector3(-1, 2, -11), mLocalShape2ToWorld * Vector3(-1, 2, 30)); - - mCallback.shapeToTest = mCompoundSphereProxyShape; - - // Correct category filter mask - mCallback.reset(); - mWorld->raycast(ray1, &mCallback, CATEGORY2); - test(mCallback.isHit); - - // Wrong category filter mask - mCallback.reset(); - mWorld->raycast(ray1, &mCallback, CATEGORY1); - test(!mCallback.isHit); - - RaycastInfo raycastInfo; - test(mCompoundBody->raycast(ray1, raycastInfo)); - mCallback.reset(); - mWorld->raycast(ray1, &mCallback); - test(mCallback.isHit); - mCallback.reset(); - mWorld->raycast(Ray(ray1.point1, ray1.point2, decimal(0.8)), &mCallback); - test(mCallback.isHit); - - test(mCompoundBody->raycast(ray2, raycastInfo)); - mCallback.reset(); - mWorld->raycast(ray2, &mCallback); - test(mCallback.isHit); - mCallback.reset(); - mWorld->raycast(Ray(ray2.point1, ray2.point2, decimal(0.8)), &mCallback); - test(mCallback.isHit); - - test(mCompoundBody->raycast(ray3, raycastInfo)); - mCallback.reset(); - mWorld->raycast(ray3, &mCallback); - test(mCallback.isHit); - mCallback.reset(); - mWorld->raycast(Ray(ray3.point1, ray3.point2, decimal(0.8)), &mCallback); - test(mCallback.isHit); - - test(mCompoundBody->raycast(ray4, raycastInfo)); - mCallback.reset(); - mWorld->raycast(ray4, &mCallback); - test(mCallback.isHit); - mCallback.reset(); - mWorld->raycast(Ray(ray4.point1, ray4.point2, decimal(0.8)), &mCallback); - test(mCallback.isHit); - - test(mCompoundBody->raycast(ray5, raycastInfo)); - mCallback.reset(); - mWorld->raycast(ray5, &mCallback); - test(mCallback.isHit); - mCallback.reset(); - mWorld->raycast(Ray(ray5.point1, ray5.point2, decimal(0.8)), &mCallback); - test(mCallback.isHit); - - test(mCompoundBody->raycast(ray6, raycastInfo)); - mCallback.reset(); - mWorld->raycast(ray6, &mCallback); - test(mCallback.isHit); - mCallback.reset(); - mWorld->raycast(Ray(ray6.point1, ray6.point2, decimal(0.8)), &mCallback); - test(mCallback.isHit); - - // Raycast hit agains the cylinder shape - Ray ray11(mLocalShapeToWorld * Vector3(4, 1, 1.5), mLocalShapeToWorld * Vector3(-30, 1.5, 2)); - Ray ray12(mLocalShapeToWorld * Vector3(1.5, 9, -1), mLocalShapeToWorld * Vector3(1.5, -30, -1)); - Ray ray13(mLocalShapeToWorld * Vector3(-1, 2, 3), mLocalShapeToWorld * Vector3(-1, 2, -30)); - Ray ray14(mLocalShapeToWorld * Vector3(-3, 2, -1.5), mLocalShapeToWorld * Vector3(30, 1, -1.5)); - Ray ray15(mLocalShapeToWorld * Vector3(0, -9, 1), mLocalShapeToWorld * Vector3(0, 30, 1)); - Ray ray16(mLocalShapeToWorld * Vector3(-1, 2, -7), mLocalShapeToWorld * Vector3(-1, 2, 30)); - - mCallback.shapeToTest = mCompoundCylinderProxyShape; - - test(mCompoundBody->raycast(ray11, raycastInfo)); - mCallback.reset(); - mWorld->raycast(ray11, &mCallback); - test(mCallback.isHit); - mCallback.reset(); - mWorld->raycast(Ray(ray11.point1, ray11.point2, decimal(0.8)), &mCallback); - test(mCallback.isHit); - - test(mCompoundBody->raycast(ray12, raycastInfo)); - mCallback.reset(); - mWorld->raycast(ray12, &mCallback); - test(mCallback.isHit); - mCallback.reset(); - mWorld->raycast(Ray(ray12.point1, ray12.point2, decimal(0.8)), &mCallback); - test(mCallback.isHit); - - test(mCompoundBody->raycast(ray13, raycastInfo)); - mCallback.reset(); - mWorld->raycast(ray13, &mCallback); - test(mCallback.isHit); - mCallback.reset(); - mWorld->raycast(Ray(ray13.point1, ray13.point2, decimal(0.8)), &mCallback); - test(mCallback.isHit); - - test(mCompoundBody->raycast(ray14, raycastInfo)); - mCallback.reset(); - mWorld->raycast(ray14, &mCallback); - test(mCallback.isHit); - mCallback.reset(); - mWorld->raycast(Ray(ray14.point1, ray14.point2, decimal(0.8)), &mCallback); - test(mCallback.isHit); - - test(mCompoundBody->raycast(ray15, raycastInfo)); - mCallback.reset(); - mWorld->raycast(ray15, &mCallback); - test(mCallback.isHit); - mCallback.reset(); - mWorld->raycast(Ray(ray15.point1, ray15.point2, decimal(0.8)), &mCallback); - test(mCallback.isHit); - - test(mCompoundBody->raycast(ray16, raycastInfo)); - mCallback.reset(); - mWorld->raycast(ray16, &mCallback); - test(mCallback.isHit); - mCallback.reset(); - mWorld->raycast(Ray(ray16.point1, ray16.point2, decimal(0.8)), &mCallback); - test(mCallback.isHit); - } - - - void testConcaveMesh() { - - // ----- Test feedback data ----- // - Vector3 point1 = mLocalShapeToWorld * Vector3(1 , 2, 6); - Vector3 point2 = mLocalShapeToWorld * Vector3(1, 2, -4); - Ray ray(point1, point2); - Vector3 hitPoint = mLocalShapeToWorld * Vector3(1, 2, 4); - - mCallback.shapeToTest = mConcaveMeshProxyShape; - - // CollisionWorld::raycast() - mCallback.reset(); - mWorld->raycast(ray, &mCallback); - test(mCallback.isHit); - test(mCallback.raycastInfo.body == mConcaveMeshBody); - test(mCallback.raycastInfo.proxyShape == mConcaveMeshProxyShape); - test(approxEqual(mCallback.raycastInfo.hitFraction, decimal(0.2), epsilon)); - test(approxEqual(mCallback.raycastInfo.worldPoint.x, hitPoint.x, epsilon)); - test(approxEqual(mCallback.raycastInfo.worldPoint.y, hitPoint.y, epsilon)); - test(approxEqual(mCallback.raycastInfo.worldPoint.z, hitPoint.z, epsilon)); - - // Correct category filter mask - mCallback.reset(); - mWorld->raycast(ray, &mCallback, CATEGORY2); - test(mCallback.isHit); - - // Wrong category filter mask - mCallback.reset(); - mWorld->raycast(ray, &mCallback, CATEGORY1); - test(!mCallback.isHit); - - // CollisionBody::raycast() - RaycastInfo raycastInfo2; - test(mConcaveMeshBody->raycast(ray, raycastInfo2)); - test(raycastInfo2.body == mConcaveMeshBody); - test(raycastInfo2.proxyShape == mConcaveMeshProxyShape); - test(approxEqual(raycastInfo2.hitFraction, decimal(0.2), epsilon)); - test(approxEqual(raycastInfo2.worldPoint.x, hitPoint.x, epsilon)); - test(approxEqual(raycastInfo2.worldPoint.y, hitPoint.y, epsilon)); - test(approxEqual(raycastInfo2.worldPoint.z, hitPoint.z, epsilon)); - - // ProxyCollisionShape::raycast() - RaycastInfo raycastInfo3; - test(mConcaveMeshBody->raycast(ray, raycastInfo3)); - test(raycastInfo3.body == mConcaveMeshBody); - test(raycastInfo3.proxyShape == mConcaveMeshProxyShape); - test(approxEqual(raycastInfo3.hitFraction, decimal(0.2), epsilon)); - test(approxEqual(raycastInfo3.worldPoint.x, hitPoint.x, epsilon)); - test(approxEqual(raycastInfo3.worldPoint.y, hitPoint.y, epsilon)); - test(approxEqual(raycastInfo3.worldPoint.z, hitPoint.z, epsilon)); - - // ProxyCollisionShape::raycast() - RaycastInfo raycastInfo4; - test(mConcaveMeshBody->raycast(ray, raycastInfo4)); - test(raycastInfo4.body == mConcaveMeshBody); - test(raycastInfo4.proxyShape == mConcaveMeshProxyShape); - test(approxEqual(raycastInfo4.hitFraction, decimal(0.2), epsilon)); - test(approxEqual(raycastInfo4.worldPoint.x, hitPoint.x, epsilon)); - test(approxEqual(raycastInfo4.worldPoint.y, hitPoint.y, epsilon)); - test(approxEqual(raycastInfo4.worldPoint.z, hitPoint.z, epsilon)); - - // ProxyCollisionShape::raycast() - RaycastInfo raycastInfo5; - test(mConcaveMeshBody->raycast(ray, raycastInfo5)); - test(raycastInfo5.body == mConcaveMeshBody); - test(raycastInfo5.proxyShape == mConcaveMeshProxyShape); - test(approxEqual(raycastInfo5.hitFraction, decimal(0.2), epsilon)); - test(approxEqual(raycastInfo5.worldPoint.x, hitPoint.x, epsilon)); - test(approxEqual(raycastInfo5.worldPoint.y, hitPoint.y, epsilon)); - test(approxEqual(raycastInfo5.worldPoint.z, hitPoint.z, epsilon)); - - Ray ray1(mLocalShapeToWorld * Vector3(0, 0, 0), mLocalShapeToWorld * Vector3(5, 7, -1)); - Ray ray2(mLocalShapeToWorld * Vector3(5, 11, 7), mLocalShapeToWorld * Vector3(17, 29, 28)); - Ray ray3(mLocalShapeToWorld * Vector3(1, 2, 3), mLocalShapeToWorld * Vector3(-11, 2, 24)); - Ray ray4(mLocalShapeToWorld * Vector3(10, 10, 10), mLocalShapeToWorld * Vector3(22, 28, 31)); - Ray ray5(mLocalShapeToWorld * Vector3(3, 1, -5), mLocalShapeToWorld * Vector3(-30, 1, -5)); - Ray ray6(mLocalShapeToWorld * Vector3(4, 4, 1), mLocalShapeToWorld * Vector3(4, -30, 1)); - Ray ray7(mLocalShapeToWorld * Vector3(1, -4, 5), mLocalShapeToWorld * Vector3(1, -4, -30)); - Ray ray8(mLocalShapeToWorld * Vector3(-4, 4, 0), mLocalShapeToWorld * Vector3(30, 4, 0)); - Ray ray9(mLocalShapeToWorld * Vector3(0, -4, -7), mLocalShapeToWorld * Vector3(0, 30, -7)); - Ray ray10(mLocalShapeToWorld * Vector3(-3, 0, -6), mLocalShapeToWorld * Vector3(-3, 0, 30)); - Ray ray11(mLocalShapeToWorld * Vector3(3, 1, 2), mLocalShapeToWorld * Vector3(-30, 0, -6)); - Ray ray12(mLocalShapeToWorld * Vector3(1, 4, -1), mLocalShapeToWorld * Vector3(1, -30, -1)); - Ray ray13(mLocalShapeToWorld * Vector3(-1, 2, 5), mLocalShapeToWorld * Vector3(-1, 2, -30)); - Ray ray14(mLocalShapeToWorld * Vector3(-3, 2, -2), mLocalShapeToWorld * Vector3(30, 2, -2)); - Ray ray15(mLocalShapeToWorld * Vector3(0, -4, 1), mLocalShapeToWorld * Vector3(0, 30, 1)); - Ray ray16(mLocalShapeToWorld * Vector3(-1, 2, -7), mLocalShapeToWorld * Vector3(-1, 2, 30)); - - // ----- Test raycast miss ----- // - test(!mConcaveMeshBody->raycast(ray1, raycastInfo3)); - test(!mConvexMeshProxyShape->raycast(ray1, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray1, &mCallback); - test(!mCallback.isHit); - mCallback.reset(); - mWorld->raycast(Ray(ray1.point1, ray1.point2, decimal(0.01)), &mCallback); - test(!mCallback.isHit); - mCallback.reset(); - mWorld->raycast(Ray(ray1.point1, ray1.point2, decimal(100.0)), &mCallback); - test(!mCallback.isHit); - - test(!mConcaveMeshBody->raycast(ray2, raycastInfo3)); - test(!mConcaveMeshProxyShape->raycast(ray2, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray2, &mCallback); - test(!mCallback.isHit); - - test(!mConcaveMeshBody->raycast(ray3, raycastInfo3)); - test(!mConcaveMeshProxyShape->raycast(ray3, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray3, &mCallback); - test(!mCallback.isHit); - - test(!mConcaveMeshBody->raycast(ray4, raycastInfo3)); - test(!mConcaveMeshProxyShape->raycast(ray4, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray4, &mCallback); - test(!mCallback.isHit); - - test(!mConcaveMeshBody->raycast(ray5, raycastInfo3)); - test(!mConcaveMeshProxyShape->raycast(ray5, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray5, &mCallback); - test(!mCallback.isHit); - - test(!mConcaveMeshBody->raycast(ray6, raycastInfo3)); - test(!mConcaveMeshProxyShape->raycast(ray6, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray6, &mCallback); - test(!mCallback.isHit); - - test(!mConcaveMeshBody->raycast(ray7, raycastInfo3)); - test(!mConcaveMeshProxyShape->raycast(ray7, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray7, &mCallback); - test(!mCallback.isHit); - - test(!mConcaveMeshBody->raycast(ray8, raycastInfo3)); - test(!mConcaveMeshProxyShape->raycast(ray8, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray8, &mCallback); - test(!mCallback.isHit); - - test(!mConcaveMeshBody->raycast(ray9, raycastInfo3)); - test(!mConcaveMeshProxyShape->raycast(ray9, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray9, &mCallback); - test(!mCallback.isHit); - - test(!mConcaveMeshBody->raycast(ray10, raycastInfo3)); - test(!mConcaveMeshProxyShape->raycast(ray10, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray10, &mCallback); - test(!mCallback.isHit); - - mCallback.reset(); - mWorld->raycast(Ray(ray11.point1, ray11.point2, decimal(0.01)), &mCallback); - test(!mCallback.isHit); - mCallback.reset(); - mWorld->raycast(Ray(ray12.point1, ray12.point2, decimal(0.01)), &mCallback); - test(!mCallback.isHit); - mCallback.reset(); - mWorld->raycast(Ray(ray13.point1, ray13.point2, decimal(0.01)), &mCallback); - test(!mCallback.isHit); - mCallback.reset(); - mWorld->raycast(Ray(ray14.point1, ray14.point2, decimal(0.01)), &mCallback); - test(!mCallback.isHit); - mCallback.reset(); - mWorld->raycast(Ray(ray15.point1, ray15.point2, decimal(0.01)), &mCallback); - test(!mCallback.isHit); - mCallback.reset(); - mWorld->raycast(Ray(ray16.point1, ray16.point2, decimal(0.01)), &mCallback); - test(!mCallback.isHit); - - // ----- Test raycast hits ----- // - test(mConcaveMeshBody->raycast(ray11, raycastInfo3)); - test(mConcaveMeshProxyShape->raycast(ray11, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray11, &mCallback); - test(mCallback.isHit); - mCallback.reset(); - mWorld->raycast(Ray(ray11.point1, ray11.point2, decimal(0.8)), &mCallback); - test(mCallback.isHit); - - test(mConcaveMeshBody->raycast(ray12, raycastInfo3)); - test(mConcaveMeshProxyShape->raycast(ray12, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray12, &mCallback); - test(mCallback.isHit); - mCallback.reset(); - mWorld->raycast(Ray(ray12.point1, ray12.point2, decimal(0.8)), &mCallback); - test(mCallback.isHit); - - test(mConcaveMeshBody->raycast(ray13, raycastInfo3)); - test(mConcaveMeshProxyShape->raycast(ray13, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray13, &mCallback); - test(mCallback.isHit); - mCallback.reset(); - mWorld->raycast(Ray(ray13.point1, ray13.point2, decimal(0.8)), &mCallback); - test(mCallback.isHit); - - test(mConcaveMeshBody->raycast(ray14, raycastInfo3)); - test(mConcaveMeshProxyShape->raycast(ray14, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray14, &mCallback); - test(mCallback.isHit); - mCallback.reset(); - mWorld->raycast(Ray(ray14.point1, ray14.point2, decimal(0.8)), &mCallback); - test(mCallback.isHit); - - test(mConcaveMeshBody->raycast(ray15, raycastInfo3)); - test(mConcaveMeshProxyShape->raycast(ray15, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray15, &mCallback); - test(mCallback.isHit); - mCallback.reset(); - mWorld->raycast(Ray(ray15.point1, ray15.point2, decimal(0.8)), &mCallback); - test(mCallback.isHit); - - test(mConcaveMeshBody->raycast(ray16, raycastInfo3)); - test(mConcaveMeshProxyShape->raycast(ray16, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray16, &mCallback); - test(mCallback.isHit); - mCallback.reset(); - mWorld->raycast(Ray(ray16.point1, ray16.point2, decimal(0.8)), &mCallback); - test(mCallback.isHit); - } - - void testHeightField() { - - // ----- Test feedback data ----- // - Vector3 point1A = mLocalShapeToWorld * Vector3(0 , 10, 2); - Vector3 point1B = mLocalShapeToWorld * Vector3(0, -10, 2); - Ray ray(point1A, point1B); - Vector3 hitPoint = mLocalShapeToWorld * Vector3(0, 2, 2); - - Vector3 point2A = mLocalShapeToWorld * Vector3(1 , 8, -4); - Vector3 point2B = mLocalShapeToWorld * Vector3(1, -8, -4); - Ray rayBottom(point2A, point2B); - Vector3 hitPoint2 = mLocalShapeToWorld * Vector3(1, 2, -4); - - mCallback.shapeToTest = mHeightFieldProxyShape; - - // CollisionWorld::raycast() - mCallback.reset(); - mWorld->raycast(ray, &mCallback); - test(mCallback.isHit); - test(mCallback.raycastInfo.body == mHeightFieldBody); - test(mCallback.raycastInfo.proxyShape == mHeightFieldProxyShape); - test(approxEqual(mCallback.raycastInfo.hitFraction, decimal(0.4), epsilon)); - test(approxEqual(mCallback.raycastInfo.worldPoint.x, hitPoint.x, epsilon)); - test(approxEqual(mCallback.raycastInfo.worldPoint.y, hitPoint.y, epsilon)); - test(approxEqual(mCallback.raycastInfo.worldPoint.z, hitPoint.z, epsilon)); - - // Correct category filter mask - mCallback.reset(); - mWorld->raycast(ray, &mCallback, CATEGORY2); - test(mCallback.isHit); - - // Wrong category filter mask - mCallback.reset(); - mWorld->raycast(ray, &mCallback, CATEGORY1); - test(!mCallback.isHit); - - // CollisionBody::raycast() - RaycastInfo raycastInfo2; - test(mHeightFieldBody->raycast(ray, raycastInfo2)); - test(raycastInfo2.body == mHeightFieldBody); - test(raycastInfo2.proxyShape == mHeightFieldProxyShape); - test(approxEqual(raycastInfo2.hitFraction, decimal(0.4), epsilon)); - test(approxEqual(raycastInfo2.worldPoint.x, hitPoint.x, epsilon)); - test(approxEqual(raycastInfo2.worldPoint.y, hitPoint.y, epsilon)); - test(approxEqual(raycastInfo2.worldPoint.z, hitPoint.z, epsilon)); - - // ProxyCollisionShape::raycast() - RaycastInfo raycastInfo3; - test(mHeightFieldProxyShape->raycast(ray, raycastInfo3)); - test(raycastInfo3.body == mHeightFieldBody); - test(raycastInfo3.proxyShape == mHeightFieldProxyShape); - test(approxEqual(raycastInfo3.hitFraction, decimal(0.4), epsilon)); - test(approxEqual(raycastInfo3.worldPoint.x, hitPoint.x, epsilon)); - test(approxEqual(raycastInfo3.worldPoint.y, hitPoint.y, epsilon)); - test(approxEqual(raycastInfo3.worldPoint.z, hitPoint.z, epsilon)); - - mCallback.reset(); - mWorld->raycast(rayBottom, &mCallback); - test(mCallback.isHit); - test(mCallback.raycastInfo.body == mHeightFieldBody); - test(mCallback.raycastInfo.proxyShape == mHeightFieldProxyShape); - test(approxEqual(mCallback.raycastInfo.hitFraction, decimal(0.375), epsilon)); - test(approxEqual(mCallback.raycastInfo.worldPoint.x, hitPoint2.x, epsilon)); - test(approxEqual(mCallback.raycastInfo.worldPoint.y, hitPoint2.y, epsilon)); - test(approxEqual(mCallback.raycastInfo.worldPoint.z, hitPoint2.z, epsilon)); - - // CollisionBody::raycast() - RaycastInfo raycastInfo5; - test(mHeightFieldBody->raycast(rayBottom, raycastInfo5)); - test(raycastInfo5.body == mHeightFieldBody); - test(raycastInfo5.proxyShape == mHeightFieldProxyShape); - test(approxEqual(raycastInfo5.hitFraction, decimal(0.375), epsilon)); - test(approxEqual(raycastInfo5.worldPoint.x, hitPoint2.x, epsilon)); - test(approxEqual(raycastInfo5.worldPoint.y, hitPoint2.y, epsilon)); - test(approxEqual(raycastInfo5.worldPoint.z, hitPoint2.z, epsilon)); - - // ProxyCollisionShape::raycast() - RaycastInfo raycastInfo6; - test(mHeightFieldProxyShape->raycast(rayBottom, raycastInfo6)); - test(raycastInfo6.body == mHeightFieldBody); - test(raycastInfo6.proxyShape == mHeightFieldProxyShape); - test(approxEqual(raycastInfo6.hitFraction, decimal(0.375), epsilon)); - test(approxEqual(raycastInfo6.worldPoint.x, hitPoint2.x, epsilon)); - test(approxEqual(raycastInfo6.worldPoint.y, hitPoint2.y, epsilon)); - test(approxEqual(raycastInfo6.worldPoint.z, hitPoint2.z, epsilon)); - - Ray ray1(mLocalShapeToWorld * Vector3(0, 5, 0), mLocalShapeToWorld * Vector3(5, 7, 5)); - Ray ray2(mLocalShapeToWorld * Vector3(-4, -4, 7), mLocalShapeToWorld * Vector3(-4, 15, 7)); - Ray ray3(mLocalShapeToWorld * Vector3(23, 7, 2), mLocalShapeToWorld * Vector3(23, 1, 2)); - Ray ray4(mLocalShapeToWorld * Vector3(10, 3, 10), mLocalShapeToWorld * Vector3(22, 3, 31)); - Ray ray5(mLocalShapeToWorld * Vector3(4, 10, -1), mLocalShapeToWorld * Vector3(4, 3, -1)); - - Ray ray11(mLocalShapeToWorld * Vector3(3, 15, 0.5), mLocalShapeToWorld * Vector3(3, 1, 0.5)); - Ray ray12(mLocalShapeToWorld * Vector3(0, 45, 0), mLocalShapeToWorld * Vector3(0, -5, 0)); - Ray ray13(mLocalShapeToWorld * Vector3(1, 23, 2), mLocalShapeToWorld * Vector3(1, -23, 2)); - Ray ray14(mLocalShapeToWorld * Vector3(3, 8, 3), mLocalShapeToWorld * Vector3(3, 0, 3)); - - // ----- Test raycast miss ----- // - test(!mHeightFieldBody->raycast(ray1, raycastInfo3)); - test(!mHeightFieldProxyShape->raycast(ray1, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray1, &mCallback); - test(!mCallback.isHit); - mCallback.reset(); - mWorld->raycast(Ray(ray1.point1, ray1.point2, decimal(0.01)), &mCallback); - test(!mCallback.isHit); - mCallback.reset(); - mWorld->raycast(Ray(ray1.point1, ray1.point2, decimal(100.0)), &mCallback); - test(!mCallback.isHit); - - test(!mHeightFieldBody->raycast(ray2, raycastInfo3)); - test(!mHeightFieldProxyShape->raycast(ray2, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray2, &mCallback); - test(!mCallback.isHit); - - test(!mHeightFieldBody->raycast(ray3, raycastInfo3)); - test(!mHeightFieldProxyShape->raycast(ray3, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray3, &mCallback); - test(!mCallback.isHit); - - test(!mHeightFieldBody->raycast(ray4, raycastInfo3)); - test(!mHeightFieldProxyShape->raycast(ray4, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray4, &mCallback); - test(!mCallback.isHit); - - test(!mHeightFieldBody->raycast(ray5, raycastInfo3)); - test(!mHeightFieldProxyShape->raycast(ray5, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray5, &mCallback); - test(!mCallback.isHit); - - mCallback.reset(); - - // ----- Test raycast hits ----- // - test(mHeightFieldBody->raycast(ray11, raycastInfo3)); - test(mHeightFieldProxyShape->raycast(ray11, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray11, &mCallback); - test(mCallback.isHit); - mCallback.reset(); - mWorld->raycast(Ray(ray11.point1, ray11.point2, decimal(0.95)), &mCallback); - test(mCallback.isHit); - - test(mHeightFieldBody->raycast(ray12, raycastInfo3)); - test(mHeightFieldProxyShape->raycast(ray12, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray12, &mCallback); - test(mCallback.isHit); - mCallback.reset(); - mWorld->raycast(Ray(ray12.point1, ray12.point2, decimal(0.87)), &mCallback); - test(mCallback.isHit); - - test(mHeightFieldBody->raycast(ray13, raycastInfo3)); - test(mHeightFieldProxyShape->raycast(ray13, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray13, &mCallback); - test(mCallback.isHit); - mCallback.reset(); - mWorld->raycast(Ray(ray13.point1, ray13.point2, decimal(0.8)), &mCallback); - test(mCallback.isHit); - - test(mHeightFieldBody->raycast(ray14, raycastInfo3)); - test(mHeightFieldProxyShape->raycast(ray14, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray14, &mCallback); - test(mCallback.isHit); - mCallback.reset(); - mWorld->raycast(Ray(ray14.point1, ray14.point2, decimal(0.8)), &mCallback); - test(mCallback.isHit); - } + private : + + // ---------- Atributes ---------- // + + // Raycast callback class + WorldRaycastCallback mCallback; + + // Epsilon + float epsilon; + + // Physics world + CollisionWorld* mWorld; + + // Bodies + CollisionBody* mBoxBody; + CollisionBody* mSphereBody; + CollisionBody* mCapsuleBody; + CollisionBody* mConeBody; + CollisionBody* mConvexMeshBody; + CollisionBody* mConvexMeshBodyEdgesInfo; + CollisionBody* mCylinderBody; + CollisionBody* mCompoundBody; + CollisionBody* mTriangleBody; + CollisionBody* mConcaveMeshBody; + CollisionBody* mHeightFieldBody; + + // Transform + Transform mBodyTransform; + Transform mShapeTransform; + Transform mLocalShapeToWorld; + Transform mLocalShape2ToWorld; + + // Collision shapes + BoxShape* mBoxShape; + SphereShape* mSphereShape; + CapsuleShape* mCapsuleShape; + ConeShape* mConeShape; + ConvexMeshShape* mConvexMeshShape; + ConvexMeshShape* mConvexMeshShapeEdgesInfo; + CylinderShape* mCylinderShape; + TriangleShape* mTriangleShape; + ConcaveShape* m_concaveMeshShape; + HeightFieldShape* mHeightFieldShape; + + // Proxy Shapes + ProxyShape* mBoxProxyShape; + ProxyShape* mSphereProxyShape; + ProxyShape* mCapsuleProxyShape; + ProxyShape* mConeProxyShape; + ProxyShape* mConvexMeshProxyShape; + ProxyShape* mConvexMeshProxyShapeEdgesInfo; + ProxyShape* mCylinderProxyShape; + ProxyShape* mCompoundSphereProxyShape; + ProxyShape* mCompoundCylinderProxyShape; + ProxyShape* mTriangleProxyShape; + ProxyShape* mConcaveMeshProxyShape; + ProxyShape* mHeightFieldProxyShape; + + // Triangle meshes + TriangleMesh mConcaveTriangleMesh; + + std::vector mConcaveMeshVertices; + std::vector mConcaveMeshIndices; + TriangleVertexArray* mConcaveMeshVertexArray; + float mHeightFieldData[100]; + + public : + + // ---------- Methods ---------- // + + /// Constructor + TestRaycast(const std::string& name) : Test(name) { + + epsilon = float(0.0001); + + // Create the world + mWorld = new CollisionWorld(); + + // Body transform + Vector3 position(-3, 2, 7); + Quaternion orientation(PI / 5, PI / 6, PI / 7); + mBodyTransform = Transform(position, orientation); + + // Create the bodies + mBoxBody = mWorld->createCollisionBody(mBodyTransform); + mSphereBody = mWorld->createCollisionBody(mBodyTransform); + mCapsuleBody = mWorld->createCollisionBody(mBodyTransform); + mConeBody = mWorld->createCollisionBody(mBodyTransform); + mConvexMeshBody = mWorld->createCollisionBody(mBodyTransform); + mConvexMeshBodyEdgesInfo = mWorld->createCollisionBody(mBodyTransform); + mCylinderBody = mWorld->createCollisionBody(mBodyTransform); + mCompoundBody = mWorld->createCollisionBody(mBodyTransform); + mTriangleBody = mWorld->createCollisionBody(mBodyTransform); + mConcaveMeshBody = mWorld->createCollisionBody(mBodyTransform); + mHeightFieldBody = mWorld->createCollisionBody(mBodyTransform); + + // Collision shape transform + Vector3 shapePosition(1, -4, -3); + Quaternion shapeOrientation(3 * PI / 6 , -PI / 8, PI / 3); + mShapeTransform = Transform(shapePosition, shapeOrientation); + + // Compute the the transform from a local shape point to world-space + mLocalShapeToWorld = mBodyTransform * mShapeTransform; + + // Create collision shapes + mBoxShape = new BoxShape(Vector3(2, 3, 4), 0); + mBoxProxyShape = mBoxBody->addCollisionShape(mBoxShape, mShapeTransform); + + mSphereShape = new SphereShape(3); + mSphereProxyShape = mSphereBody->addCollisionShape(mSphereShape, mShapeTransform); + + const Vector3 triangleVertex1(100, 100, 0); + const Vector3 triangleVertex2(105, 100, 0); + const Vector3 triangleVertex3(100, 103, 0); + mTriangleShape = new TriangleShape(triangleVertex1, triangleVertex2, triangleVertex3); + mTriangleProxyShape = mTriangleBody->addCollisionShape(mTriangleShape, mShapeTransform); + + mCapsuleShape = new CapsuleShape(2, 5); + mCapsuleProxyShape = mCapsuleBody->addCollisionShape(mCapsuleShape, mShapeTransform); + + mConeShape = new ConeShape(2, 6, 0); + mConeProxyShape = mConeBody->addCollisionShape(mConeShape, mShapeTransform); + + // Box of dimension (2, 3, 4) + mConvexMeshShape = new ConvexMeshShape(0.0); + mConvexMeshShape->addVertex(Vector3(-2, -3, -4)); + mConvexMeshShape->addVertex(Vector3(2, -3, -4)); + mConvexMeshShape->addVertex(Vector3(2, -3, 4)); + mConvexMeshShape->addVertex(Vector3(-2, -3, 4)); + mConvexMeshShape->addVertex(Vector3(-2, 3, -4)); + mConvexMeshShape->addVertex(Vector3(2, 3, -4)); + mConvexMeshShape->addVertex(Vector3(2, 3, 4)); + mConvexMeshShape->addVertex(Vector3(-2, 3, 4)); + mConvexMeshProxyShape = mConvexMeshBody->addCollisionShape(mConvexMeshShape, mShapeTransform); + + mConvexMeshShapeEdgesInfo = new ConvexMeshShape(0.0); + mConvexMeshShapeEdgesInfo->addVertex(Vector3(-2, -3, -4)); + mConvexMeshShapeEdgesInfo->addVertex(Vector3(2, -3, -4)); + mConvexMeshShapeEdgesInfo->addVertex(Vector3(2, -3, 4)); + mConvexMeshShapeEdgesInfo->addVertex(Vector3(-2, -3, 4)); + mConvexMeshShapeEdgesInfo->addVertex(Vector3(-2, 3, -4)); + mConvexMeshShapeEdgesInfo->addVertex(Vector3(2, 3, -4)); + mConvexMeshShapeEdgesInfo->addVertex(Vector3(2, 3, 4)); + mConvexMeshShapeEdgesInfo->addVertex(Vector3(-2, 3, 4)); + mConvexMeshShapeEdgesInfo->addEdge(0, 1); + mConvexMeshShapeEdgesInfo->addEdge(1, 2); + mConvexMeshShapeEdgesInfo->addEdge(2, 3); + mConvexMeshShapeEdgesInfo->addEdge(0, 3); + mConvexMeshShapeEdgesInfo->addEdge(4, 5); + mConvexMeshShapeEdgesInfo->addEdge(5, 6); + mConvexMeshShapeEdgesInfo->addEdge(6, 7); + mConvexMeshShapeEdgesInfo->addEdge(4, 7); + mConvexMeshShapeEdgesInfo->addEdge(0, 4); + mConvexMeshShapeEdgesInfo->addEdge(1, 5); + mConvexMeshShapeEdgesInfo->addEdge(2, 6); + mConvexMeshShapeEdgesInfo->addEdge(3, 7); + mConvexMeshShapeEdgesInfo->setIsEdgesInformationUsed(true); + mConvexMeshProxyShapeEdgesInfo = mConvexMeshBodyEdgesInfo->addCollisionShape( + mConvexMeshShapeEdgesInfo, + mShapeTransform); + + mCylinderShape = new CylinderShape(2, 5, 0); + mCylinderProxyShape = mCylinderBody->addCollisionShape(mCylinderShape, mShapeTransform); + + // Compound shape is a cylinder and a sphere + Vector3 positionShape2(Vector3(4, 2, -3)); + Quaternion orientationShape2(-3 *PI / 8, 1.5 * PI/ 3, PI / 13); + Transform shapeTransform2(positionShape2, orientationShape2); + mLocalShape2ToWorld = mBodyTransform * shapeTransform2; + mCompoundCylinderProxyShape = mCompoundBody->addCollisionShape(mCylinderShape, mShapeTransform); + mCompoundSphereProxyShape = mCompoundBody->addCollisionShape(mSphereShape, shapeTransform2); + + // Concave Mesh shape + mConcaveMeshVertices.push_back(Vector3(-2, -3, -4)); + mConcaveMeshVertices.push_back(Vector3(2, -3, -4)); + mConcaveMeshVertices.push_back(Vector3(2, -3, 4)); + mConcaveMeshVertices.push_back(Vector3(-2, -3, 4)); + mConcaveMeshVertices.push_back(Vector3(-2, 3, -4)); + mConcaveMeshVertices.push_back(Vector3(2, 3, -4)); + mConcaveMeshVertices.push_back(Vector3(2, 3, 4)); + mConcaveMeshVertices.push_back(Vector3(-2, 3, 4)); + + mConcaveMeshIndices.push_back(0); mConcaveMeshIndices.push_back(1); mConcaveMeshIndices.push_back(2); + mConcaveMeshIndices.push_back(0); mConcaveMeshIndices.push_back(2); mConcaveMeshIndices.push_back(3); + mConcaveMeshIndices.push_back(1); mConcaveMeshIndices.push_back(5); mConcaveMeshIndices.push_back(2); + mConcaveMeshIndices.push_back(2); mConcaveMeshIndices.push_back(5); mConcaveMeshIndices.push_back(6); + mConcaveMeshIndices.push_back(2); mConcaveMeshIndices.push_back(7); mConcaveMeshIndices.push_back(3); + mConcaveMeshIndices.push_back(2); mConcaveMeshIndices.push_back(6); mConcaveMeshIndices.push_back(7); + mConcaveMeshIndices.push_back(0); mConcaveMeshIndices.push_back(3); mConcaveMeshIndices.push_back(4); + mConcaveMeshIndices.push_back(3); mConcaveMeshIndices.push_back(7); mConcaveMeshIndices.push_back(4); + mConcaveMeshIndices.push_back(0); mConcaveMeshIndices.push_back(4); mConcaveMeshIndices.push_back(1); + mConcaveMeshIndices.push_back(1); mConcaveMeshIndices.push_back(4); mConcaveMeshIndices.push_back(5); + mConcaveMeshIndices.push_back(5); mConcaveMeshIndices.push_back(7); mConcaveMeshIndices.push_back(6); + mConcaveMeshIndices.push_back(4); mConcaveMeshIndices.push_back(7); mConcaveMeshIndices.push_back(5); + TriangleVertexArray::VertexDataType vertexType = sizeof(float) == 4 ? TriangleVertexArray::VERTEX_FLOAT_TYPE : + TriangleVertexArray::VERTEX_DOUBLE_TYPE; + mConcaveMeshVertexArray = + new TriangleVertexArray(8, &(mConcaveMeshVertices[0]), sizeof(Vector3), + 12, &(mConcaveMeshIndices[0]), sizeof(uint32_t), + vertexType, + TriangleVertexArray::INDEX_INTEGER_TYPE); + + + // Add the triangle vertex array of the subpart to the triangle mesh + mConcaveTriangleMesh.addSubpart(mConcaveMeshVertexArray); + m_concaveMeshShape = new ConcaveMeshShape(&mConcaveTriangleMesh); + mConcaveMeshProxyShape = mConcaveMeshBody->addCollisionShape(m_concaveMeshShape, mShapeTransform); + + + // Heightfield shape (plane height field at height=4) + for (int32_t i=0; i<100; i++) mHeightFieldData[i] = 4; + mHeightFieldShape = new HeightFieldShape(10, 10, 0, 4, mHeightFieldData, HeightFieldShape::HEIGHT_FLOAT_TYPE); + mHeightFieldProxyShape = mHeightFieldBody->addCollisionShape(mHeightFieldShape, mShapeTransform); + + // Assign proxy shapes to the different categories + mBoxProxyShape->setCollisionCategoryBits(CATEGORY1); + mSphereProxyShape->setCollisionCategoryBits(CATEGORY1); + mCapsuleProxyShape->setCollisionCategoryBits(CATEGORY1); + mConeProxyShape->setCollisionCategoryBits(CATEGORY2); + mConvexMeshProxyShape->setCollisionCategoryBits(CATEGORY2); + mConvexMeshProxyShapeEdgesInfo->setCollisionCategoryBits(CATEGORY2); + mCylinderProxyShape->setCollisionCategoryBits(CATEGORY2); + mCompoundSphereProxyShape->setCollisionCategoryBits(CATEGORY2); + mCompoundCylinderProxyShape->setCollisionCategoryBits(CATEGORY2); + mTriangleProxyShape->setCollisionCategoryBits(CATEGORY1); + mConcaveMeshProxyShape->setCollisionCategoryBits(CATEGORY2); + mHeightFieldProxyShape->setCollisionCategoryBits(CATEGORY2); + } + + /// Destructor + ~TestRaycast() { + delete mBoxShape; + delete mSphereShape; + delete mCapsuleShape; + delete mConeShape; + delete mConvexMeshShape; + delete mConvexMeshShapeEdgesInfo; + delete mCylinderShape; + delete mTriangleShape; + delete m_concaveMeshShape; + delete mHeightFieldShape; + + delete mConcaveMeshVertexArray; + } + + /// Run the tests + void run() { + testBox(); + testSphere(); + testCapsule(); + testCone(); + testConvexMesh(); + testCylinder(); + testCompound(); + testTriangle(); + testConcaveMesh(); + testHeightField(); + } + + /// Test the ProxyBoxShape::raycast(), CollisionBody::raycast() and + /// CollisionWorld::raycast() methods. + void testBox() { + + // ----- Test feedback data ----- // + Vector3 point1 = mLocalShapeToWorld * Vector3(1 , 2, 10); + Vector3 point2 = mLocalShapeToWorld * Vector3(1, 2, -20); + Ray ray(point1, point2); + Vector3 hitPoint = mLocalShapeToWorld * Vector3(1, 2, 4); + + mCallback.shapeToTest = mBoxProxyShape; + + // CollisionWorld::raycast() + mCallback.reset(); + mWorld->raycast(ray, &mCallback); + test(mCallback.isHit); + test(mCallback.raycastInfo.body == mBoxBody); + test(mCallback.raycastInfo.proxyShape == mBoxProxyShape); + test(approxEqual(mCallback.raycastInfo.hitFraction, float(0.2), epsilon)); + test(approxEqual(mCallback.raycastInfo.worldPoint.x, hitPoint.x, epsilon)); + test(approxEqual(mCallback.raycastInfo.worldPoint.y, hitPoint.y, epsilon)); + test(approxEqual(mCallback.raycastInfo.worldPoint.z, hitPoint.z, epsilon)); + + // Correct category filter mask + mCallback.reset(); + mWorld->raycast(ray, &mCallback, CATEGORY1); + test(mCallback.isHit); + + // Wrong category filter mask + mCallback.reset(); + mWorld->raycast(ray, &mCallback, CATEGORY2); + test(!mCallback.isHit); + + // CollisionBody::raycast() + RaycastInfo raycastInfo2; + test(mBoxBody->raycast(ray, raycastInfo2)); + test(raycastInfo2.body == mBoxBody); + test(raycastInfo2.proxyShape == mBoxProxyShape); + test(approxEqual(raycastInfo2.hitFraction, float(0.2), epsilon)); + test(approxEqual(raycastInfo2.worldPoint.x, hitPoint.x, epsilon)); + test(approxEqual(raycastInfo2.worldPoint.y, hitPoint.y, epsilon)); + test(approxEqual(raycastInfo2.worldPoint.z, hitPoint.z, epsilon)); + + // ProxyCollisionShape::raycast() + RaycastInfo raycastInfo3; + test(mBoxProxyShape->raycast(ray, raycastInfo3)); + test(raycastInfo3.body == mBoxBody); + test(raycastInfo3.proxyShape == mBoxProxyShape); + test(approxEqual(raycastInfo3.hitFraction, float(0.2), epsilon)); + test(approxEqual(raycastInfo3.worldPoint.x, hitPoint.x, epsilon)); + test(approxEqual(raycastInfo3.worldPoint.y, hitPoint.y, epsilon)); + test(approxEqual(raycastInfo3.worldPoint.z, hitPoint.z, epsilon)); + + Ray ray1(mLocalShapeToWorld * Vector3(0, 0, 0), mLocalShapeToWorld * Vector3(5, 7, -1)); + Ray ray2(mLocalShapeToWorld * Vector3(5, 11, 7), mLocalShapeToWorld * Vector3(17, 29, 28)); + Ray ray3(mLocalShapeToWorld * Vector3(1, 2, 3), mLocalShapeToWorld * Vector3(-11, 2, 24)); + Ray ray4(mLocalShapeToWorld * Vector3(10, 10, 10), mLocalShapeToWorld * Vector3(22, 28, 31)); + Ray ray5(mLocalShapeToWorld * Vector3(3, 1, -5), mLocalShapeToWorld * Vector3(-30, 1, -5)); + Ray ray6(mLocalShapeToWorld * Vector3(4, 4, 1), mLocalShapeToWorld * Vector3(4, -20, 1)); + Ray ray7(mLocalShapeToWorld * Vector3(1, -4, 5), mLocalShapeToWorld * Vector3(1, -4, -20)); + Ray ray8(mLocalShapeToWorld * Vector3(-4, 4, 0), mLocalShapeToWorld * Vector3(20, 4, 0)); + Ray ray9(mLocalShapeToWorld * Vector3(0, -4, -7), mLocalShapeToWorld * Vector3(0, 50, -7)); + Ray ray10(mLocalShapeToWorld * Vector3(-3, 0, -6), mLocalShapeToWorld * Vector3(-3, 0, 20)); + Ray ray11(mLocalShapeToWorld * Vector3(3, 1, 2), mLocalShapeToWorld * Vector3(-20, 1, 2)); + Ray ray12(mLocalShapeToWorld * Vector3(1, 4, -1), mLocalShapeToWorld * Vector3(1, -20, -1)); + Ray ray13(mLocalShapeToWorld * Vector3(-1, 2, 5), mLocalShapeToWorld * Vector3(-1, 2, -20)); + Ray ray14(mLocalShapeToWorld * Vector3(-3, 2, -2), mLocalShapeToWorld * Vector3(20, 2, -2)); + Ray ray15(mLocalShapeToWorld * Vector3(0, -4, 1), mLocalShapeToWorld * Vector3(0, 20, 1)); + Ray ray16(mLocalShapeToWorld * Vector3(-1, 2, -5), mLocalShapeToWorld * Vector3(-1, 2, 20)); + + // ----- Test raycast miss ----- // + test(!mBoxBody->raycast(ray1, raycastInfo3)); + test(!mBoxProxyShape->raycast(ray1, raycastInfo3)); + mCallback.reset(); + mWorld->raycast(ray1, &mCallback); + test(!mCallback.isHit); + mCallback.reset(); + mWorld->raycast(Ray(ray1.point1, ray1.point2, float(0.01)), &mCallback); + test(!mCallback.isHit); + mCallback.reset(); + mWorld->raycast(Ray(ray1.point1, ray1.point2, float(100.0)), &mCallback); + test(!mCallback.isHit); + + test(!mBoxBody->raycast(ray2, raycastInfo3)); + test(!mBoxProxyShape->raycast(ray2, raycastInfo3)); + mCallback.reset(); + mWorld->raycast(ray2, &mCallback); + test(!mCallback.isHit); + + test(!mBoxBody->raycast(ray3, raycastInfo3)); + test(!mBoxProxyShape->raycast(ray3, raycastInfo3)); + mCallback.reset(); + mWorld->raycast(ray3, &mCallback); + test(!mCallback.isHit); + + test(!mBoxBody->raycast(ray4, raycastInfo3)); + test(!mBoxProxyShape->raycast(ray4, raycastInfo3)); + mCallback.reset(); + mWorld->raycast(ray4, &mCallback); + test(!mCallback.isHit); + + test(!mBoxBody->raycast(ray5, raycastInfo3)); + test(!mBoxProxyShape->raycast(ray5, raycastInfo3)); + mCallback.reset(); + mWorld->raycast(ray5, &mCallback); + test(!mCallback.isHit); + + test(!mBoxBody->raycast(ray6, raycastInfo3)); + test(!mBoxProxyShape->raycast(ray6, raycastInfo3)); + mCallback.reset(); + mWorld->raycast(ray6, &mCallback); + test(!mCallback.isHit); + + test(!mBoxBody->raycast(ray7, raycastInfo3)); + test(!mBoxProxyShape->raycast(ray7, raycastInfo3)); + mCallback.reset(); + mWorld->raycast(ray7, &mCallback); + test(!mCallback.isHit); + + test(!mBoxBody->raycast(ray8, raycastInfo3)); + test(!mBoxProxyShape->raycast(ray8, raycastInfo3)); + mCallback.reset(); + mWorld->raycast(ray8, &mCallback); + test(!mCallback.isHit); + + test(!mBoxBody->raycast(ray9, raycastInfo3)); + test(!mBoxProxyShape->raycast(ray9, raycastInfo3)); + mCallback.reset(); + mWorld->raycast(ray9, &mCallback); + test(!mCallback.isHit); + + test(!mBoxBody->raycast(ray10, raycastInfo3)); + test(!mBoxProxyShape->raycast(ray10, raycastInfo3)); + mCallback.reset(); + mWorld->raycast(ray10, &mCallback); + test(!mCallback.isHit); + + mCallback.reset(); + mWorld->raycast(Ray(ray11.point1, ray11.point2, float(0.01)), &mCallback); + test(!mCallback.isHit); + mCallback.reset(); + mWorld->raycast(Ray(ray12.point1, ray12.point2, float(0.01)), &mCallback); + test(!mCallback.isHit); + mCallback.reset(); + mWorld->raycast(Ray(ray13.point1, ray13.point2, float(0.01)), &mCallback); + test(!mCallback.isHit); + mCallback.reset(); + mWorld->raycast(Ray(ray14.point1, ray14.point2, float(0.01)), &mCallback); + test(!mCallback.isHit); + mCallback.reset(); + mWorld->raycast(Ray(ray15.point1, ray15.point2, float(0.01)), &mCallback); + test(!mCallback.isHit); + mCallback.reset(); + mWorld->raycast(Ray(ray16.point1, ray16.point2, float(0.01)), &mCallback); + test(!mCallback.isHit); + + // ----- Test raycast hits ----- // + test(mBoxBody->raycast(ray11, raycastInfo3)); + test(mBoxProxyShape->raycast(ray11, raycastInfo3)); + mCallback.reset(); + mWorld->raycast(ray11, &mCallback); + test(mCallback.isHit); + mCallback.reset(); + mWorld->raycast(Ray(ray11.point1, ray11.point2, float(0.8)), &mCallback); + test(mCallback.isHit); + + test(mBoxBody->raycast(ray12, raycastInfo3)); + test(mBoxProxyShape->raycast(ray12, raycastInfo3)); + mCallback.reset(); + mWorld->raycast(ray12, &mCallback); + test(mCallback.isHit); + mCallback.reset(); + mWorld->raycast(Ray(ray12.point1, ray12.point2, float(0.8)), &mCallback); + test(mCallback.isHit); + + test(mBoxBody->raycast(ray13, raycastInfo3)); + test(mBoxProxyShape->raycast(ray13, raycastInfo3)); + mCallback.reset(); + mWorld->raycast(ray13, &mCallback); + test(mCallback.isHit); + mCallback.reset(); + mWorld->raycast(Ray(ray13.point1, ray13.point2, float(0.8)), &mCallback); + test(mCallback.isHit); + + test(mBoxBody->raycast(ray14, raycastInfo3)); + test(mBoxProxyShape->raycast(ray14, raycastInfo3)); + mCallback.reset(); + mWorld->raycast(ray14, &mCallback); + test(mCallback.isHit); + mCallback.reset(); + mWorld->raycast(Ray(ray14.point1, ray14.point2, float(0.8)), &mCallback); + test(mCallback.isHit); + + test(mBoxBody->raycast(ray15, raycastInfo3)); + test(mBoxProxyShape->raycast(ray15, raycastInfo3)); + mCallback.reset(); + mWorld->raycast(ray15, &mCallback); + test(mCallback.isHit); + mCallback.reset(); + mWorld->raycast(Ray(ray15.point1, ray15.point2, float(0.8)), &mCallback); + test(mCallback.isHit); + + test(mBoxBody->raycast(ray16, raycastInfo3)); + test(mBoxProxyShape->raycast(ray16, raycastInfo3)); + mCallback.reset(); + mWorld->raycast(ray16, &mCallback); + test(mCallback.isHit); + mCallback.reset(); + mWorld->raycast(Ray(ray16.point1, ray16.point2, float(0.8)), &mCallback); + test(mCallback.isHit); + } + + /// Test the ProxySphereShape::raycast(), CollisionBody::raycast() and + /// CollisionWorld::raycast() methods. + void testSphere() { + + // ----- Test feedback data ----- // + Vector3 point1 = mLocalShapeToWorld * Vector3(-5 , 0, 0); + Vector3 point2 = mLocalShapeToWorld * Vector3(5, 0, 0); + Ray ray(point1, point2); + Vector3 hitPoint = mLocalShapeToWorld * Vector3(-3, 0, 0); + + mCallback.shapeToTest = mSphereProxyShape; + + // CollisionWorld::raycast() + mCallback.reset(); + mWorld->raycast(ray, &mCallback); + test(mCallback.isHit); + test(mCallback.raycastInfo.body == mSphereBody); + test(mCallback.raycastInfo.proxyShape == mSphereProxyShape); + test(approxEqual(mCallback.raycastInfo.hitFraction, 0.2, epsilon)); + test(approxEqual(mCallback.raycastInfo.worldPoint.x, hitPoint.x, epsilon)); + test(approxEqual(mCallback.raycastInfo.worldPoint.y, hitPoint.y, epsilon)); + test(approxEqual(mCallback.raycastInfo.worldPoint.z, hitPoint.z, epsilon)); + + // Correct category filter mask + mCallback.reset(); + mWorld->raycast(ray, &mCallback, CATEGORY1); + test(mCallback.isHit); + + // Wrong category filter mask + mCallback.reset(); + mWorld->raycast(ray, &mCallback, CATEGORY2); + test(!mCallback.isHit); + + // CollisionBody::raycast() + RaycastInfo raycastInfo2; + test(mSphereBody->raycast(ray, raycastInfo2)); + test(raycastInfo2.body == mSphereBody); + test(raycastInfo2.proxyShape == mSphereProxyShape); + test(approxEqual(raycastInfo2.hitFraction, 0.2, epsilon)); + test(approxEqual(raycastInfo2.worldPoint.x, hitPoint.x, epsilon)); + test(approxEqual(raycastInfo2.worldPoint.y, hitPoint.y, epsilon)); + test(approxEqual(raycastInfo2.worldPoint.z, hitPoint.z, epsilon)); + + // ProxyCollisionShape::raycast() + RaycastInfo raycastInfo3; + test(mSphereProxyShape->raycast(ray, raycastInfo3)); + test(raycastInfo3.body == mSphereBody); + test(raycastInfo3.proxyShape == mSphereProxyShape); + test(approxEqual(raycastInfo3.hitFraction, 0.2, epsilon)); + test(approxEqual(raycastInfo3.worldPoint.x, hitPoint.x, epsilon)); + test(approxEqual(raycastInfo3.worldPoint.y, hitPoint.y, epsilon)); + test(approxEqual(raycastInfo3.worldPoint.z, hitPoint.z, epsilon)); + + Ray ray1(mLocalShapeToWorld * Vector3(0, 0, 0), mLocalShapeToWorld * Vector3(5, 7, -1)); + Ray ray2(mLocalShapeToWorld * Vector3(5, 11, 7), mLocalShapeToWorld * Vector3(4, 6, 7)); + Ray ray3(mLocalShapeToWorld * Vector3(1, 2, 2), mLocalShapeToWorld * Vector3(-4, 0, 7)); + Ray ray4(mLocalShapeToWorld * Vector3(10, 10, 10), mLocalShapeToWorld * Vector3(4, 6, 7)); + Ray ray5(mLocalShapeToWorld * Vector3(4, 1, -5), mLocalShapeToWorld * Vector3(-30, 1, -5)); + Ray ray6(mLocalShapeToWorld * Vector3(4, 4, 1), mLocalShapeToWorld * Vector3(4, -30, 1)); + Ray ray7(mLocalShapeToWorld * Vector3(1, -4, 5), mLocalShapeToWorld * Vector3(1, -4, -30)); + Ray ray8(mLocalShapeToWorld * Vector3(-4, 4, 0), mLocalShapeToWorld * Vector3(30, 4, 0)); + Ray ray9(mLocalShapeToWorld * Vector3(0, -4, -4), mLocalShapeToWorld * Vector3(0, 30, -4)); + Ray ray10(mLocalShapeToWorld * Vector3(-4, 0, -6), mLocalShapeToWorld * Vector3(-4, 0, 30)); + Ray ray11(mLocalShapeToWorld * Vector3(4, 1, 2), mLocalShapeToWorld * Vector3(-30, 1, 2)); + Ray ray12(mLocalShapeToWorld * Vector3(1, 4, -1), mLocalShapeToWorld * Vector3(1, -30, -1)); + Ray ray13(mLocalShapeToWorld * Vector3(-1, 2, 5), mLocalShapeToWorld * Vector3(-1, 2, -30)); + Ray ray14(mLocalShapeToWorld * Vector3(-5, 2, -2), mLocalShapeToWorld * Vector3(30, 2, -2)); + Ray ray15(mLocalShapeToWorld * Vector3(0, -4, 1), mLocalShapeToWorld * Vector3(0, 30, 1)); + Ray ray16(mLocalShapeToWorld * Vector3(-1, 2, -11), mLocalShapeToWorld * Vector3(-1, 2, 30)); + + // ----- Test raycast miss ----- // + test(!mSphereBody->raycast(ray1, raycastInfo3)); + test(!mSphereProxyShape->raycast(ray1, raycastInfo3)); + mCallback.reset(); + mWorld->raycast(ray1, &mCallback); + test(!mCallback.isHit); + mCallback.reset(); + mWorld->raycast(Ray(ray1.point1, ray1.point2, float(0.01)), &mCallback); + test(!mCallback.isHit); + mCallback.reset(); + mWorld->raycast(Ray(ray1.point1, ray1.point2, float(100.0)), &mCallback); + test(!mCallback.isHit); + + test(!mSphereBody->raycast(ray2, raycastInfo3)); + test(!mSphereProxyShape->raycast(ray2, raycastInfo3)); + mCallback.reset(); + mWorld->raycast(ray2, &mCallback); + test(!mCallback.isHit); + + test(!mSphereBody->raycast(ray3, raycastInfo3)); + test(!mSphereProxyShape->raycast(ray3, raycastInfo3)); + mCallback.reset(); + mWorld->raycast(ray3, &mCallback); + test(!mCallback.isHit); + + test(!mSphereBody->raycast(ray4, raycastInfo3)); + test(!mSphereProxyShape->raycast(ray4, raycastInfo3)); + mCallback.reset(); + mWorld->raycast(ray4, &mCallback); + test(!mCallback.isHit); + + test(!mSphereBody->raycast(ray5, raycastInfo3)); + test(!mSphereProxyShape->raycast(ray5, raycastInfo3)); + mCallback.reset(); + mWorld->raycast(ray5, &mCallback); + test(!mCallback.isHit); + + test(!mSphereBody->raycast(ray6, raycastInfo3)); + test(!mSphereProxyShape->raycast(ray6, raycastInfo3)); + mCallback.reset(); + mWorld->raycast(ray6, &mCallback); + test(!mCallback.isHit); + + test(!mSphereBody->raycast(ray7, raycastInfo3)); + test(!mSphereProxyShape->raycast(ray7, raycastInfo3)); + mCallback.reset(); + mWorld->raycast(ray7, &mCallback); + test(!mCallback.isHit); + + test(!mSphereBody->raycast(ray8, raycastInfo3)); + test(!mSphereProxyShape->raycast(ray8, raycastInfo3)); + mCallback.reset(); + mWorld->raycast(ray8, &mCallback); + test(!mCallback.isHit); + + test(!mSphereBody->raycast(ray9, raycastInfo3)); + test(!mSphereProxyShape->raycast(ray9, raycastInfo3)); + mCallback.reset(); + mWorld->raycast(ray9, &mCallback); + test(!mCallback.isHit); + + test(!mSphereBody->raycast(ray10, raycastInfo3)); + test(!mSphereProxyShape->raycast(ray10, raycastInfo3)); + mCallback.reset(); + mWorld->raycast(ray10, &mCallback); + test(!mCallback.isHit); + + mCallback.reset(); + mWorld->raycast(Ray(ray11.point1, ray11.point2, float(0.01)), &mCallback); + test(!mCallback.isHit); + mCallback.reset(); + mWorld->raycast(Ray(ray12.point1, ray12.point2, float(0.01)), &mCallback); + test(!mCallback.isHit); + mCallback.reset(); + mWorld->raycast(Ray(ray13.point1, ray13.point2, float(0.01)), &mCallback); + test(!mCallback.isHit); + mCallback.reset(); + mWorld->raycast(Ray(ray14.point1, ray14.point2, float(0.01)), &mCallback); + test(!mCallback.isHit); + mCallback.reset(); + mWorld->raycast(Ray(ray15.point1, ray15.point2, float(0.01)), &mCallback); + test(!mCallback.isHit); + mCallback.reset(); + mWorld->raycast(Ray(ray16.point1, ray16.point2, float(0.01)), &mCallback); + test(!mCallback.isHit); + + // ----- Test raycast hits ----- // + test(mSphereBody->raycast(ray11, raycastInfo3)); + test(mSphereProxyShape->raycast(ray11, raycastInfo3)); + mCallback.reset(); + mWorld->raycast(ray11, &mCallback); + test(mCallback.isHit); + mCallback.reset(); + mWorld->raycast(Ray(ray11.point1, ray11.point2, float(0.8)), &mCallback); + test(mCallback.isHit); + + test(mSphereBody->raycast(ray12, raycastInfo3)); + test(mSphereProxyShape->raycast(ray12, raycastInfo3)); + mCallback.reset(); + mWorld->raycast(ray12, &mCallback); + test(mCallback.isHit); + mCallback.reset(); + mWorld->raycast(Ray(ray12.point1, ray12.point2, float(0.8)), &mCallback); + test(mCallback.isHit); + + test(mSphereBody->raycast(ray13, raycastInfo3)); + test(mSphereProxyShape->raycast(ray13, raycastInfo3)); + mCallback.reset(); + mWorld->raycast(ray13, &mCallback); + mCallback.reset(); + mWorld->raycast(Ray(ray13.point1, ray13.point2, float(0.8)), &mCallback); + + test(mSphereBody->raycast(ray14, raycastInfo3)); + test(mSphereProxyShape->raycast(ray14, raycastInfo3)); + mCallback.reset(); + mWorld->raycast(ray14, &mCallback); + test(mCallback.isHit); + mCallback.reset(); + mWorld->raycast(Ray(ray14.point1, ray14.point2, float(0.8)), &mCallback); + test(mCallback.isHit); + + test(mSphereBody->raycast(ray15, raycastInfo3)); + test(mSphereProxyShape->raycast(ray15, raycastInfo3)); + mCallback.reset(); + mWorld->raycast(ray15, &mCallback); + test(mCallback.isHit); + mCallback.reset(); + mWorld->raycast(Ray(ray15.point1, ray15.point2, float(0.8)), &mCallback); + test(mCallback.isHit); + + test(mSphereBody->raycast(ray16, raycastInfo3)); + test(mSphereProxyShape->raycast(ray16, raycastInfo3)); + mCallback.reset(); + mWorld->raycast(ray16, &mCallback); + test(mCallback.isHit); + mCallback.reset(); + mWorld->raycast(Ray(ray16.point1, ray16.point2, float(0.8)), &mCallback); + test(mCallback.isHit); + } + + /// Test the ProxyCapsuleShape::raycast(), CollisionBody::raycast() and + /// CollisionWorld::raycast() methods. + void testCapsule() { + + // ----- Test feedback data ----- // + Vector3 point1A = mLocalShapeToWorld * Vector3(4 , 1, 0); + Vector3 point1B = mLocalShapeToWorld * Vector3(-6, 1, 0); + Ray ray(point1A, point1B); + Vector3 hitPoint = mLocalShapeToWorld * Vector3(2, 1, 0); + + Vector3 point2A = mLocalShapeToWorld * Vector3(0 , 6.5, 0); + Vector3 point2B = mLocalShapeToWorld * Vector3(0, -3.5, 0); + Ray rayTop(point2A, point2B); + Vector3 hitPointTop = mLocalShapeToWorld * Vector3(0, float(4.5), 0); + + Vector3 point3A = mLocalShapeToWorld * Vector3(0 , -6.5, 0); + Vector3 point3B = mLocalShapeToWorld * Vector3(0, 3.5, 0); + Ray rayBottom(point3A, point3B); + Vector3 hitPointBottom = mLocalShapeToWorld * Vector3(0, float(-4.5), 0); + + mCallback.shapeToTest = mCapsuleProxyShape; + + // CollisionWorld::raycast() + mCallback.reset(); + mWorld->raycast(ray, &mCallback); + test(mCallback.isHit); + test(mCallback.raycastInfo.body == mCapsuleBody); + test(mCallback.raycastInfo.proxyShape == mCapsuleProxyShape); + test(approxEqual(mCallback.raycastInfo.hitFraction, float(0.2), epsilon)); + test(approxEqual(mCallback.raycastInfo.worldPoint.x, hitPoint.x, epsilon)); + test(approxEqual(mCallback.raycastInfo.worldPoint.y, hitPoint.y, epsilon)); + test(approxEqual(mCallback.raycastInfo.worldPoint.z, hitPoint.z, epsilon)); + + // Correct category filter mask + mCallback.reset(); + mWorld->raycast(ray, &mCallback, CATEGORY1); + test(mCallback.isHit); + + // Wrong category filter mask + mCallback.reset(); + mWorld->raycast(ray, &mCallback, CATEGORY2); + test(!mCallback.isHit); + + // CollisionBody::raycast() + RaycastInfo raycastInfo2; + test(mCapsuleBody->raycast(ray, raycastInfo2)); + test(raycastInfo2.body == mCapsuleBody); + test(raycastInfo2.proxyShape == mCapsuleProxyShape); + test(approxEqual(raycastInfo2.hitFraction, float(0.2), epsilon)); + test(approxEqual(raycastInfo2.worldPoint.x, hitPoint.x, epsilon)); + test(approxEqual(raycastInfo2.worldPoint.y, hitPoint.y, epsilon)); + test(approxEqual(raycastInfo2.worldPoint.z, hitPoint.z, epsilon)); + + // ProxyCollisionShape::raycast() + RaycastInfo raycastInfo3; + test(mCapsuleProxyShape->raycast(ray, raycastInfo3)); + test(raycastInfo3.body == mCapsuleBody); + test(raycastInfo3.proxyShape == mCapsuleProxyShape); + test(approxEqual(raycastInfo3.hitFraction, float(0.2), epsilon)); + test(approxEqual(raycastInfo3.worldPoint.x, hitPoint.x, epsilon)); + test(approxEqual(raycastInfo3.worldPoint.y, hitPoint.y, epsilon)); + test(approxEqual(raycastInfo3.worldPoint.z, hitPoint.z, epsilon)); + + RaycastInfo raycastInfo4; + test(mCapsuleProxyShape->raycast(rayTop, raycastInfo4)); + test(raycastInfo4.body == mCapsuleBody); + test(raycastInfo4.proxyShape == mCapsuleProxyShape); + test(approxEqual(raycastInfo4.hitFraction, float(0.2), epsilon)); + test(approxEqual(raycastInfo4.worldPoint.x, hitPointTop.x, epsilon)); + test(approxEqual(raycastInfo4.worldPoint.y, hitPointTop.y, epsilon)); + test(approxEqual(raycastInfo4.worldPoint.z, hitPointTop.z, epsilon)); + + // ProxyCollisionShape::raycast() + RaycastInfo raycastInfo5; + test(mCapsuleProxyShape->raycast(rayBottom, raycastInfo5)); + test(raycastInfo5.body == mCapsuleBody); + test(raycastInfo5.proxyShape == mCapsuleProxyShape); + test(approxEqual(raycastInfo5.hitFraction, float(0.2), epsilon)); + test(approxEqual(raycastInfo5.worldPoint.x, hitPointBottom.x, epsilon)); + test(approxEqual(raycastInfo5.worldPoint.y, hitPointBottom.y, epsilon)); + test(approxEqual(raycastInfo5.worldPoint.z, hitPointBottom.z, epsilon)); + + Ray ray1(mLocalShapeToWorld * Vector3(0, 0, 0), mLocalShapeToWorld * Vector3(5, 7, -1)); + Ray ray2(mLocalShapeToWorld * Vector3(5, 11, 7), mLocalShapeToWorld * Vector3(9, 17, 14)); + Ray ray3(mLocalShapeToWorld * Vector3(1, 3, -1), mLocalShapeToWorld * Vector3(-3, 3, 6)); + Ray ray4(mLocalShapeToWorld * Vector3(10, 10, 10), mLocalShapeToWorld * Vector3(14, 16, 17)); + Ray ray5(mLocalShapeToWorld * Vector3(4, 1, -5), mLocalShapeToWorld * Vector3(1, 1, -5)); + Ray ray6(mLocalShapeToWorld * Vector3(4, 9, 1), mLocalShapeToWorld * Vector3(4, 7, 1)); + Ray ray7(mLocalShapeToWorld * Vector3(1, -9, 5), mLocalShapeToWorld * Vector3(1, -9, 3)); + Ray ray8(mLocalShapeToWorld * Vector3(-4, 9, 0), mLocalShapeToWorld * Vector3(-3, 9, 0)); + Ray ray9(mLocalShapeToWorld * Vector3(0, -9, -4), mLocalShapeToWorld * Vector3(0, -4, -4)); + Ray ray10(mLocalShapeToWorld * Vector3(-4, 0, -6), mLocalShapeToWorld * Vector3(-4, 0, 2)); + Ray ray11(mLocalShapeToWorld * Vector3(4, 1, 1.5), mLocalShapeToWorld * Vector3(-30, 1, 1.5)); + Ray ray12(mLocalShapeToWorld * Vector3(1, 9, -1), mLocalShapeToWorld * Vector3(1, -30, -1)); + Ray ray13(mLocalShapeToWorld * Vector3(-1, 2, 3), mLocalShapeToWorld * Vector3(-1, 2, -30)); + Ray ray14(mLocalShapeToWorld * Vector3(-3, 2, -1.7), mLocalShapeToWorld * Vector3(30, 2, -1.7)); + Ray ray15(mLocalShapeToWorld * Vector3(0, -9, 1), mLocalShapeToWorld * Vector3(0, 30, 1)); + Ray ray16(mLocalShapeToWorld * Vector3(-1, 2, -7), mLocalShapeToWorld * Vector3(-1, 2, 30)); + + // ----- Test raycast miss ----- // + test(!mCapsuleBody->raycast(ray1, raycastInfo3)); + test(!mCapsuleProxyShape->raycast(ray1, raycastInfo3)); + mCallback.reset(); + mWorld->raycast(ray1, &mCallback); + test(!mCallback.isHit); + mCallback.reset(); + mWorld->raycast(Ray(ray1.point1, ray1.point2, float(0.01)), &mCallback); + test(!mCallback.isHit); + mCallback.reset(); + mWorld->raycast(Ray(ray1.point1, ray1.point2, float(100.0)), &mCallback); + test(!mCallback.isHit); + + test(!mCapsuleBody->raycast(ray2, raycastInfo3)); + test(!mCapsuleProxyShape->raycast(ray2, raycastInfo3)); + mCallback.reset(); + mWorld->raycast(ray2, &mCallback); + test(!mCallback.isHit); + + test(!mCapsuleBody->raycast(ray3, raycastInfo3)); + test(!mCapsuleProxyShape->raycast(ray3, raycastInfo3)); + mCallback.reset(); + mWorld->raycast(ray3, &mCallback); + test(!mCallback.isHit); + + test(!mCapsuleBody->raycast(ray4, raycastInfo3)); + test(!mCapsuleProxyShape->raycast(ray4, raycastInfo3)); + mCallback.reset(); + mWorld->raycast(ray4, &mCallback); + test(!mCallback.isHit); + + test(!mCapsuleBody->raycast(ray5, raycastInfo3)); + test(!mCapsuleProxyShape->raycast(ray5, raycastInfo3)); + mCallback.reset(); + mWorld->raycast(ray5, &mCallback); + test(!mCallback.isHit); + + test(!mCapsuleBody->raycast(ray6, raycastInfo3)); + test(!mCapsuleProxyShape->raycast(ray6, raycastInfo3)); + mCallback.reset(); + mWorld->raycast(ray6, &mCallback); + test(!mCallback.isHit); + + test(!mCapsuleBody->raycast(ray7, raycastInfo3)); + test(!mCapsuleProxyShape->raycast(ray7, raycastInfo3)); + mWorld->raycast(ray7, &mCallback); + test(!mCallback.isHit); + + test(!mCapsuleBody->raycast(ray8, raycastInfo3)); + test(!mCapsuleProxyShape->raycast(ray8, raycastInfo3)); + mCallback.reset(); + mWorld->raycast(ray8, &mCallback); + test(!mCallback.isHit); + + test(!mCapsuleBody->raycast(ray9, raycastInfo3)); + test(!mCapsuleProxyShape->raycast(ray9, raycastInfo3)); + mCallback.reset(); + mWorld->raycast(ray9, &mCallback); + test(!mCallback.isHit); + + test(!mCapsuleBody->raycast(ray10, raycastInfo3)); + test(!mCapsuleProxyShape->raycast(ray10, raycastInfo3)); + mCallback.reset(); + mWorld->raycast(ray10, &mCallback); + test(!mCallback.isHit); + + mCallback.reset(); + mWorld->raycast(Ray(ray11.point1, ray11.point2, float(0.01)), &mCallback); + test(!mCallback.isHit); + mCallback.reset(); + mWorld->raycast(Ray(ray12.point1, ray12.point2, float(0.01)), &mCallback); + test(!mCallback.isHit); + mCallback.reset(); + mWorld->raycast(Ray(ray13.point1, ray13.point2, float(0.01)), &mCallback); + test(!mCallback.isHit); + mCallback.reset(); + mWorld->raycast(Ray(ray14.point1, ray14.point2, float(0.01)), &mCallback); + test(!mCallback.isHit); + mCallback.reset(); + mWorld->raycast(Ray(ray15.point1, ray15.point2, float(0.01)), &mCallback); + test(!mCallback.isHit); + mCallback.reset(); + mWorld->raycast(Ray(ray16.point1, ray16.point2, float(0.01)), &mCallback); + test(!mCallback.isHit); + + // ----- Test raycast hits ----- // + test(mCapsuleBody->raycast(ray11, raycastInfo3)); + test(mCapsuleProxyShape->raycast(ray11, raycastInfo3)); + mCallback.reset(); + mWorld->raycast(ray11, &mCallback); + test(mCallback.isHit); + mCallback.reset(); + mWorld->raycast(Ray(ray11.point1, ray11.point2, float(0.8)), &mCallback); + test(mCallback.isHit); + + test(mCapsuleBody->raycast(ray12, raycastInfo3)); + test(mCapsuleProxyShape->raycast(ray12, raycastInfo3)); + mCallback.reset(); + mWorld->raycast(ray12, &mCallback); + test(mCallback.isHit); + mCallback.reset(); + mWorld->raycast(Ray(ray12.point1, ray12.point2, float(0.8)), &mCallback); + test(mCallback.isHit); + + test(mCapsuleBody->raycast(ray13, raycastInfo3)); + test(mCapsuleProxyShape->raycast(ray13, raycastInfo3)); + mCallback.reset(); + mWorld->raycast(ray13, &mCallback); + test(mCallback.isHit); + mCallback.reset(); + mWorld->raycast(Ray(ray13.point1, ray13.point2, float(0.8)), &mCallback); + test(mCallback.isHit); + + test(mCapsuleBody->raycast(ray14, raycastInfo3)); + test(mCapsuleProxyShape->raycast(ray14, raycastInfo3)); + mCallback.reset(); + mWorld->raycast(ray14, &mCallback); + test(mCallback.isHit); + mCallback.reset(); + mWorld->raycast(Ray(ray14.point1, ray14.point2, float(0.8)), &mCallback); + test(mCallback.isHit); + + test(mCapsuleBody->raycast(ray15, raycastInfo3)); + test(mCapsuleProxyShape->raycast(ray15, raycastInfo3)); + mCallback.reset(); + mWorld->raycast(ray15, &mCallback); + test(mCallback.isHit); + mCallback.reset(); + mWorld->raycast(Ray(ray15.point1, ray15.point2, float(0.8)), &mCallback); + test(mCallback.isHit); + + test(mCapsuleBody->raycast(ray16, raycastInfo3)); + test(mCapsuleProxyShape->raycast(ray16, raycastInfo3)); + mCallback.reset(); + mWorld->raycast(ray16, &mCallback); + test(mCallback.isHit); + mCallback.reset(); + mWorld->raycast(Ray(ray16.point1, ray16.point2, float(0.8)), &mCallback); + test(mCallback.isHit); + } + + /// Test the ProxySphereShape::raycast(), CollisionBody::raycast() and + /// CollisionWorld::raycast() methods. + void testTriangle() { + + // ----- Test feedback data ----- // + Vector3 point1 = mLocalShapeToWorld * Vector3(101, 101, 400); + Vector3 point2 = mLocalShapeToWorld * Vector3(101, 101, -200); + Ray ray(point1, point2); + Ray rayBackward(point2, point1); + + Vector3 hitPoint = mLocalShapeToWorld * Vector3(101, 101, 0); + Vector3 hitNormal = mLocalShapeToWorld.getOrientation() * Vector3(0, 0, 1); + hitNormal.normalize(); + mCallback.shapeToTest = mTriangleProxyShape; + + // CollisionWorld::raycast() + mCallback.reset(); + mTriangleShape->setRaycastTestType(FRONT); + mWorld->raycast(ray, &mCallback); + test(mCallback.isHit); + test(mCallback.raycastInfo.body == mTriangleBody); + test(mCallback.raycastInfo.proxyShape == mTriangleProxyShape); + test(approxEqual(mCallback.raycastInfo.hitFraction, 0.6666, epsilon)); + test(approxEqual(mCallback.raycastInfo.worldPoint.x, hitPoint.x, epsilon)); + test(approxEqual(mCallback.raycastInfo.worldPoint.y, hitPoint.y, epsilon)); + test(approxEqual(mCallback.raycastInfo.worldPoint.z, hitPoint.z, epsilon)); + test(approxEqual(mCallback.raycastInfo.worldNormal.x, hitNormal.x, epsilon)); + test(approxEqual(mCallback.raycastInfo.worldNormal.y, hitNormal.y, epsilon)); + test(approxEqual(mCallback.raycastInfo.worldNormal.z, hitNormal.z, epsilon)); + + mCallback.reset(); + mTriangleShape->setRaycastTestType(BACK); + mWorld->raycast(rayBackward, &mCallback); + test(mCallback.isHit); + test(mCallback.raycastInfo.body == mTriangleBody); + test(mCallback.raycastInfo.proxyShape == mTriangleProxyShape); + test(approxEqual(mCallback.raycastInfo.hitFraction, 0.3333, epsilon)); + test(approxEqual(mCallback.raycastInfo.worldPoint.x, hitPoint.x, epsilon)); + test(approxEqual(mCallback.raycastInfo.worldPoint.y, hitPoint.y, epsilon)); + test(approxEqual(mCallback.raycastInfo.worldPoint.z, hitPoint.z, epsilon)); + test(approxEqual(mCallback.raycastInfo.worldNormal.x, -hitNormal.x, epsilon)); + test(approxEqual(mCallback.raycastInfo.worldNormal.y, -hitNormal.y, epsilon)); + test(approxEqual(mCallback.raycastInfo.worldNormal.z, -hitNormal.z, epsilon)); + + mCallback.reset(); + mTriangleShape->setRaycastTestType(FRONT_AND_BACK); + mWorld->raycast(ray, &mCallback); + test(mCallback.isHit); + test(mCallback.raycastInfo.body == mTriangleBody); + test(mCallback.raycastInfo.proxyShape == mTriangleProxyShape); + test(approxEqual(mCallback.raycastInfo.hitFraction, 0.6666, epsilon)); + test(approxEqual(mCallback.raycastInfo.worldPoint.x, hitPoint.x, epsilon)); + test(approxEqual(mCallback.raycastInfo.worldPoint.y, hitPoint.y, epsilon)); + test(approxEqual(mCallback.raycastInfo.worldPoint.z, hitPoint.z, epsilon)); + test(approxEqual(mCallback.raycastInfo.worldNormal.x, hitNormal.x, epsilon)); + test(approxEqual(mCallback.raycastInfo.worldNormal.y, hitNormal.y, epsilon)); + test(approxEqual(mCallback.raycastInfo.worldNormal.z, hitNormal.z, epsilon)); + + mCallback.reset(); + mTriangleShape->setRaycastTestType(FRONT_AND_BACK); + mWorld->raycast(rayBackward, &mCallback); + test(mCallback.isHit); + test(mCallback.raycastInfo.body == mTriangleBody); + test(mCallback.raycastInfo.proxyShape == mTriangleProxyShape); + test(approxEqual(mCallback.raycastInfo.hitFraction, 0.3333, epsilon)); + test(approxEqual(mCallback.raycastInfo.worldPoint.x, hitPoint.x, epsilon)); + test(approxEqual(mCallback.raycastInfo.worldPoint.y, hitPoint.y, epsilon)); + test(approxEqual(mCallback.raycastInfo.worldPoint.z, hitPoint.z, epsilon)); + test(approxEqual(mCallback.raycastInfo.worldNormal.x, -hitNormal.x, epsilon)); + test(approxEqual(mCallback.raycastInfo.worldNormal.y, -hitNormal.y, epsilon)); + test(approxEqual(mCallback.raycastInfo.worldNormal.z, -hitNormal.z, epsilon)); + + mTriangleShape->setRaycastTestType(FRONT); + + // Correct category filter mask + mCallback.reset(); + mWorld->raycast(ray, &mCallback, CATEGORY1); + test(mCallback.isHit); + + // Wrong category filter mask + mCallback.reset(); + mWorld->raycast(ray, &mCallback, CATEGORY2); + test(!mCallback.isHit); + + // CollisionBody::raycast() + RaycastInfo raycastInfo2; + test(mTriangleBody->raycast(ray, raycastInfo2)); + test(raycastInfo2.body == mTriangleBody); + test(raycastInfo2.proxyShape == mTriangleProxyShape); + test(approxEqual(raycastInfo2.hitFraction, 0.6666, epsilon)); + test(approxEqual(raycastInfo2.worldPoint.x, hitPoint.x, epsilon)); + test(approxEqual(raycastInfo2.worldPoint.y, hitPoint.y, epsilon)); + test(approxEqual(raycastInfo2.worldPoint.z, hitPoint.z, epsilon)); + + // ProxyCollisionShape::raycast() + RaycastInfo raycastInfo3; + test(mTriangleProxyShape->raycast(ray, raycastInfo3)); + test(raycastInfo3.body == mTriangleBody); + test(raycastInfo3.proxyShape == mTriangleProxyShape); + test(approxEqual(raycastInfo3.hitFraction, 0.6666, epsilon)); + test(approxEqual(raycastInfo3.worldPoint.x, hitPoint.x, epsilon)); + test(approxEqual(raycastInfo3.worldPoint.y, hitPoint.y, epsilon)); + test(approxEqual(raycastInfo3.worldPoint.z, hitPoint.z, epsilon)); + + Ray ray1(mLocalShapeToWorld * Vector3(-10, 10, 4), mLocalShapeToWorld * Vector3(15, 6, -4)); + Ray ray2(mLocalShapeToWorld * Vector3(102, 107, 5), mLocalShapeToWorld * Vector3(102, 107, -5)); + Ray ray3(mLocalShapeToWorld * Vector3(106, 102, 6), mLocalShapeToWorld * Vector3(106, 102, -8)); + + Ray ray4(mLocalShapeToWorld * Vector3(100.2, 101, 5), mLocalShapeToWorld * Vector3(100.2, 101, -5)); + Ray ray5(mLocalShapeToWorld * Vector3(100.5, 101.5, 4), mLocalShapeToWorld * Vector3(100.5, 101.5, -54)); + Ray ray6(mLocalShapeToWorld * Vector3(102, 101, 1), mLocalShapeToWorld * Vector3(102, 102, -1)); + + Ray ray4Back(mLocalShapeToWorld * Vector3(100.2, 101, -5), mLocalShapeToWorld * Vector3(100.2, 101, 5)); + Ray ray5Back(mLocalShapeToWorld * Vector3(100.5, 101.5, -54), mLocalShapeToWorld * Vector3(100.5, 101.5, 4)); + Ray ray6Back(mLocalShapeToWorld * Vector3(102, 102, -1), mLocalShapeToWorld * Vector3(102, 101, 1)); + + // ----- Test raycast miss ----- // + test(!mTriangleBody->raycast(ray1, raycastInfo3)); + test(!mTriangleProxyShape->raycast(ray1, raycastInfo3)); + mCallback.reset(); + mWorld->raycast(ray1, &mCallback); + test(!mCallback.isHit); + mCallback.reset(); + mWorld->raycast(Ray(ray1.point1, ray1.point2, float(0.01)), &mCallback); + test(!mCallback.isHit); + mCallback.reset(); + mWorld->raycast(Ray(ray1.point1, ray1.point2, float(100.0)), &mCallback); + test(!mCallback.isHit); + + test(!mTriangleBody->raycast(ray2, raycastInfo3)); + test(!mTriangleProxyShape->raycast(ray2, raycastInfo3)); + mCallback.reset(); + mWorld->raycast(ray2, &mCallback); + test(!mCallback.isHit); + + test(!mTriangleBody->raycast(ray3, raycastInfo3)); + test(!mTriangleProxyShape->raycast(ray3, raycastInfo3)); + mCallback.reset(); + mWorld->raycast(ray3, &mCallback); + test(!mCallback.isHit); + + // Test backward ray against front triangles (not hit should occur) + mTriangleShape->setRaycastTestType(FRONT); + + test(!mTriangleBody->raycast(ray4Back, raycastInfo3)); + test(!mTriangleProxyShape->raycast(ray4Back, raycastInfo3)); + mCallback.reset(); + mWorld->raycast(ray4Back, &mCallback); + test(!mCallback.isHit); + + test(!mTriangleBody->raycast(ray5Back, raycastInfo3)); + test(!mTriangleProxyShape->raycast(ray5Back, raycastInfo3)); + mCallback.reset(); + mWorld->raycast(ray5Back, &mCallback); + test(!mCallback.isHit); + + test(!mTriangleBody->raycast(ray6Back, raycastInfo3)); + test(!mTriangleProxyShape->raycast(ray6Back, raycastInfo3)); + mCallback.reset(); + mWorld->raycast(ray6Back, &mCallback); + test(!mCallback.isHit); + + // Test front ray against back triangles (not hit should occur) + mTriangleShape->setRaycastTestType(BACK); + + test(!mTriangleBody->raycast(ray4, raycastInfo3)); + test(!mTriangleProxyShape->raycast(ray4, raycastInfo3)); + mCallback.reset(); + mWorld->raycast(ray4, &mCallback); + test(!mCallback.isHit); + + test(!mTriangleBody->raycast(ray5, raycastInfo3)); + test(!mTriangleProxyShape->raycast(ray5, raycastInfo3)); + mCallback.reset(); + mWorld->raycast(ray5, &mCallback); + test(!mCallback.isHit); + + test(!mTriangleBody->raycast(ray6, raycastInfo3)); + test(!mTriangleProxyShape->raycast(ray6, raycastInfo3)); + mCallback.reset(); + mWorld->raycast(ray6, &mCallback); + test(!mCallback.isHit); + + // ----- Test raycast hits ----- // + + // Test front ray against front triangles + mTriangleShape->setRaycastTestType(FRONT); + + test(mTriangleBody->raycast(ray4, raycastInfo3)); + test(mTriangleProxyShape->raycast(ray4, raycastInfo3)); + mCallback.reset(); + mWorld->raycast(ray4, &mCallback); + test(mCallback.isHit); + mCallback.reset(); + mWorld->raycast(Ray(ray4.point1, ray4.point2, float(0.8)), &mCallback); + test(mCallback.isHit); + + test(mTriangleBody->raycast(ray5, raycastInfo3)); + test(mTriangleProxyShape->raycast(ray5, raycastInfo3)); + mCallback.reset(); + mWorld->raycast(ray5, &mCallback); + test(mCallback.isHit); + mCallback.reset(); + mWorld->raycast(Ray(ray5.point1, ray5.point2, float(0.8)), &mCallback); + test(mCallback.isHit); + + test(mTriangleBody->raycast(ray6, raycastInfo3)); + test(mTriangleProxyShape->raycast(ray6, raycastInfo3)); + mCallback.reset(); + mWorld->raycast(ray6, &mCallback); + mCallback.reset(); + mWorld->raycast(Ray(ray6.point1, ray6.point2, float(0.8)), &mCallback); + + // Test back ray against back triangles + mTriangleShape->setRaycastTestType(BACK); + + test(mTriangleBody->raycast(ray4Back, raycastInfo3)); + test(mTriangleProxyShape->raycast(ray4Back, raycastInfo3)); + mCallback.reset(); + mWorld->raycast(ray4Back, &mCallback); + test(mCallback.isHit); + mCallback.reset(); + mWorld->raycast(Ray(ray4Back.point1, ray4Back.point2, float(0.8)), &mCallback); + test(mCallback.isHit); + + test(mTriangleBody->raycast(ray5Back, raycastInfo3)); + test(mTriangleProxyShape->raycast(ray5Back, raycastInfo3)); + mCallback.reset(); + mWorld->raycast(ray5Back, &mCallback); + test(mCallback.isHit); + mCallback.reset(); + mWorld->raycast(Ray(ray5Back.point1, ray5Back.point2, float(1.0)), &mCallback); + test(mCallback.isHit); + + test(mTriangleBody->raycast(ray6Back, raycastInfo3)); + test(mTriangleProxyShape->raycast(ray6Back, raycastInfo3)); + mCallback.reset(); + mWorld->raycast(ray6Back, &mCallback); + mCallback.reset(); + mWorld->raycast(Ray(ray6Back.point1, ray6Back.point2, float(0.8)), &mCallback); + + // Test front ray against front-back triangles + mTriangleShape->setRaycastTestType(FRONT_AND_BACK); + + test(mTriangleBody->raycast(ray4, raycastInfo3)); + test(mTriangleProxyShape->raycast(ray4, raycastInfo3)); + mCallback.reset(); + mWorld->raycast(ray4, &mCallback); + test(mCallback.isHit); + mCallback.reset(); + mWorld->raycast(Ray(ray4.point1, ray4.point2, float(0.8)), &mCallback); + test(mCallback.isHit); + + test(mTriangleBody->raycast(ray5, raycastInfo3)); + test(mTriangleProxyShape->raycast(ray5, raycastInfo3)); + mCallback.reset(); + mWorld->raycast(ray5, &mCallback); + test(mCallback.isHit); + mCallback.reset(); + mWorld->raycast(Ray(ray5.point1, ray5.point2, float(0.8)), &mCallback); + test(mCallback.isHit); + + test(mTriangleBody->raycast(ray6, raycastInfo3)); + test(mTriangleProxyShape->raycast(ray6, raycastInfo3)); + mCallback.reset(); + mWorld->raycast(ray6, &mCallback); + mCallback.reset(); + mWorld->raycast(Ray(ray6.point1, ray6.point2, float(0.8)), &mCallback); + + // Test back ray against front-back triangles + mTriangleShape->setRaycastTestType(FRONT_AND_BACK); + + test(mTriangleBody->raycast(ray4Back, raycastInfo3)); + test(mTriangleProxyShape->raycast(ray4Back, raycastInfo3)); + mCallback.reset(); + mWorld->raycast(ray4Back, &mCallback); + test(mCallback.isHit); + mCallback.reset(); + mWorld->raycast(Ray(ray4Back.point1, ray4Back.point2, float(0.8)), &mCallback); + test(mCallback.isHit); + + test(mTriangleBody->raycast(ray5Back, raycastInfo3)); + test(mTriangleProxyShape->raycast(ray5Back, raycastInfo3)); + mCallback.reset(); + mWorld->raycast(ray5Back, &mCallback); + test(mCallback.isHit); + mCallback.reset(); + mWorld->raycast(Ray(ray5Back.point1, ray5Back.point2, float(1.0)), &mCallback); + test(mCallback.isHit); + + test(mTriangleBody->raycast(ray6Back, raycastInfo3)); + test(mTriangleProxyShape->raycast(ray6Back, raycastInfo3)); + mCallback.reset(); + mWorld->raycast(ray6Back, &mCallback); + mCallback.reset(); + mWorld->raycast(Ray(ray6Back.point1, ray6Back.point2, float(0.8)), &mCallback); + } + + /// Test the ProxyConeShape::raycast(), CollisionBody::raycast() and + /// CollisionWorld::raycast() methods. + void testCone() { + + // ----- Test feedback data ----- // + Vector3 point1A = mLocalShapeToWorld * Vector3(0 , 0, 3); + Vector3 point1B = mLocalShapeToWorld * Vector3(0, 0, -7); + Ray ray(point1A, point1B); + Vector3 hitPoint = mLocalShapeToWorld * Vector3(0, 0, 1); + + Vector3 point2A = mLocalShapeToWorld * Vector3(1 , -5, 0); + Vector3 point2B = mLocalShapeToWorld * Vector3(1, 5, 0); + Ray rayBottom(point2A, point2B); + Vector3 hitPoint2 = mLocalShapeToWorld * Vector3(1, -3, 0); + + mCallback.shapeToTest = mConeProxyShape; + + // CollisionWorld::raycast() + mCallback.reset(); + mWorld->raycast(ray, &mCallback); + test(mCallback.isHit); + test(mCallback.raycastInfo.body == mConeBody); + test(mCallback.raycastInfo.proxyShape == mConeProxyShape); + test(approxEqual(mCallback.raycastInfo.hitFraction, float(0.2), epsilon)); + test(approxEqual(mCallback.raycastInfo.worldPoint.x, hitPoint.x, epsilon)); + test(approxEqual(mCallback.raycastInfo.worldPoint.y, hitPoint.y, epsilon)); + test(approxEqual(mCallback.raycastInfo.worldPoint.z, hitPoint.z, epsilon)); + + // Correct category filter mask + mCallback.reset(); + mWorld->raycast(ray, &mCallback, CATEGORY2); + test(mCallback.isHit); + + // Wrong category filter mask + mCallback.reset(); + mWorld->raycast(ray, &mCallback, CATEGORY1); + test(!mCallback.isHit); + + // CollisionBody::raycast() + RaycastInfo raycastInfo2; + test(mConeBody->raycast(ray, raycastInfo2)); + test(raycastInfo2.body == mConeBody); + test(raycastInfo2.proxyShape == mConeProxyShape); + test(approxEqual(raycastInfo2.hitFraction, float(0.2), epsilon)); + test(approxEqual(raycastInfo2.worldPoint.x, hitPoint.x, epsilon)); + test(approxEqual(raycastInfo2.worldPoint.y, hitPoint.y, epsilon)); + test(approxEqual(raycastInfo2.worldPoint.z, hitPoint.z, epsilon)); + + // ProxyCollisionShape::raycast() + RaycastInfo raycastInfo3; + test(mConeProxyShape->raycast(ray, raycastInfo3)); + test(raycastInfo3.body == mConeBody); + test(raycastInfo3.proxyShape == mConeProxyShape); + test(approxEqual(raycastInfo3.hitFraction, float(0.2), epsilon)); + test(approxEqual(raycastInfo3.worldPoint.x, hitPoint.x, epsilon)); + test(approxEqual(raycastInfo3.worldPoint.y, hitPoint.y, epsilon)); + test(approxEqual(raycastInfo3.worldPoint.z, hitPoint.z, epsilon)); + + mCallback.reset(); + mWorld->raycast(rayBottom, &mCallback); + test(mCallback.isHit); + test(mCallback.raycastInfo.body == mConeBody); + test(mCallback.raycastInfo.proxyShape == mConeProxyShape); + test(approxEqual(mCallback.raycastInfo.hitFraction, float(0.2), epsilon)); + test(approxEqual(mCallback.raycastInfo.worldPoint.x, hitPoint2.x, epsilon)); + test(approxEqual(mCallback.raycastInfo.worldPoint.y, hitPoint2.y, epsilon)); + test(approxEqual(mCallback.raycastInfo.worldPoint.z, hitPoint2.z, epsilon)); + + // CollisionBody::raycast() + RaycastInfo raycastInfo5; + test(mConeBody->raycast(rayBottom, raycastInfo5)); + test(raycastInfo5.body == mConeBody); + test(raycastInfo5.proxyShape == mConeProxyShape); + test(approxEqual(raycastInfo5.hitFraction, float(0.2), epsilon)); + test(approxEqual(raycastInfo5.worldPoint.x, hitPoint2.x, epsilon)); + test(approxEqual(raycastInfo5.worldPoint.y, hitPoint2.y, epsilon)); + test(approxEqual(raycastInfo5.worldPoint.z, hitPoint2.z, epsilon)); + + // ProxyCollisionShape::raycast() + RaycastInfo raycastInfo6; + test(mConeProxyShape->raycast(rayBottom, raycastInfo6)); + test(raycastInfo6.body == mConeBody); + test(raycastInfo6.proxyShape == mConeProxyShape); + test(approxEqual(raycastInfo6.hitFraction, float(0.2), epsilon)); + test(approxEqual(raycastInfo6.worldPoint.x, hitPoint2.x, epsilon)); + test(approxEqual(raycastInfo6.worldPoint.y, hitPoint2.y, epsilon)); + test(approxEqual(raycastInfo6.worldPoint.z, hitPoint2.z, epsilon)); + + Ray ray1(mLocalShapeToWorld * Vector3(0, 0, 0), mLocalShapeToWorld * Vector3(5, 7, -1)); + Ray ray2(mLocalShapeToWorld * Vector3(5, 11, 7), mLocalShapeToWorld * Vector3(17, 29, 28)); + Ray ray3(mLocalShapeToWorld * Vector3(-1, -2, 1), mLocalShapeToWorld * Vector3(-13, -2, 22)); + Ray ray4(mLocalShapeToWorld * Vector3(10, 10, 10), mLocalShapeToWorld * Vector3(22, 28, 31)); + Ray ray5(mLocalShapeToWorld * Vector3(4, 1, -1), mLocalShapeToWorld * Vector3(-26, 1, -1)); + Ray ray6(mLocalShapeToWorld * Vector3(3, 4, 1), mLocalShapeToWorld * Vector3(3, -16, 1)); + Ray ray7(mLocalShapeToWorld * Vector3(1, -4, 3), mLocalShapeToWorld * Vector3(1, -4, -17)); + Ray ray8(mLocalShapeToWorld * Vector3(-4, 4, 0), mLocalShapeToWorld * Vector3(26, 4, 0)); + Ray ray9(mLocalShapeToWorld * Vector3(0, -4, -7), mLocalShapeToWorld * Vector3(0, 46, -7)); + Ray ray10(mLocalShapeToWorld * Vector3(-3, -2, -6), mLocalShapeToWorld * Vector3(-3, -2, 74)); + Ray ray11(mLocalShapeToWorld * Vector3(3, -1, 0.5), mLocalShapeToWorld * Vector3(-27, -1, 0.5)); + Ray ray12(mLocalShapeToWorld * Vector3(1, 4, -1), mLocalShapeToWorld * Vector3(1, -26, -1)); + Ray ray13(mLocalShapeToWorld * Vector3(-1, -2, 3), mLocalShapeToWorld * Vector3(-1, -2, -27)); + Ray ray14(mLocalShapeToWorld * Vector3(-2, 0, 0.8), mLocalShapeToWorld * Vector3(30, 0, 0.8)); + Ray ray15(mLocalShapeToWorld * Vector3(0, -4, 1), mLocalShapeToWorld * Vector3(0, 30, 1)); + Ray ray16(mLocalShapeToWorld * Vector3(-0.9, 0, -4), mLocalShapeToWorld * Vector3(-0.9, 0, 30)); + + // ----- Test raycast miss ----- // + test(!mConeBody->raycast(ray1, raycastInfo3)); + test(!mConeProxyShape->raycast(ray1, raycastInfo3)); + mCallback.reset(); + mWorld->raycast(ray1, &mCallback); + test(!mCallback.isHit); + mCallback.reset(); + mWorld->raycast(Ray(ray1.point1, ray1.point2, float(0.01)), &mCallback); + test(!mCallback.isHit); + mCallback.reset(); + mWorld->raycast(Ray(ray1.point1, ray1.point2, float(100.0)), &mCallback); + test(!mCallback.isHit); + + test(!mConeBody->raycast(ray2, raycastInfo3)); + test(!mConeProxyShape->raycast(ray2, raycastInfo3)); + mCallback.reset(); + mWorld->raycast(ray2, &mCallback); + test(!mCallback.isHit); + + test(!mConeBody->raycast(ray3, raycastInfo3)); + test(!mConeProxyShape->raycast(ray3, raycastInfo3)); + mCallback.reset(); + mWorld->raycast(ray3, &mCallback); + test(!mCallback.isHit); + + test(!mConeBody->raycast(ray4, raycastInfo3)); + test(!mConeProxyShape->raycast(ray4, raycastInfo3)); + mCallback.reset(); + mWorld->raycast(ray4, &mCallback); + test(!mCallback.isHit); + + test(!mConeBody->raycast(ray5, raycastInfo3)); + test(!mConeProxyShape->raycast(ray5, raycastInfo3)); + mCallback.reset(); + mWorld->raycast(ray5, &mCallback); + test(!mCallback.isHit); + + test(!mConeBody->raycast(ray6, raycastInfo3)); + test(!mConeProxyShape->raycast(ray6, raycastInfo3)); + mCallback.reset(); + mWorld->raycast(ray6, &mCallback); + test(!mCallback.isHit); + + test(!mConeBody->raycast(ray7, raycastInfo3)); + test(!mConeProxyShape->raycast(ray7, raycastInfo3)); + mCallback.reset(); + mWorld->raycast(ray7, &mCallback); + test(!mCallback.isHit); + + test(!mConeBody->raycast(ray8, raycastInfo3)); + test(!mConeProxyShape->raycast(ray8, raycastInfo3)); + mCallback.reset(); + mWorld->raycast(ray8, &mCallback); + test(!mCallback.isHit); + + test(!mConeBody->raycast(ray9, raycastInfo3)); + test(!mConeProxyShape->raycast(ray9, raycastInfo3)); + mCallback.reset(); + mWorld->raycast(ray9, &mCallback); + test(!mCallback.isHit); + + test(!mConeBody->raycast(ray10, raycastInfo3)); + test(!mConeProxyShape->raycast(ray10, raycastInfo3)); + mCallback.reset(); + mWorld->raycast(ray10, &mCallback); + test(!mCallback.isHit); + + mCallback.reset(); + mWorld->raycast(Ray(ray11.point1, ray11.point2, float(0.01)), &mCallback); + test(!mCallback.isHit); + mCallback.reset(); + mWorld->raycast(Ray(ray12.point1, ray12.point2, float(0.01)), &mCallback); + test(!mCallback.isHit); + mCallback.reset(); + mWorld->raycast(Ray(ray13.point1, ray13.point2, float(0.01)), &mCallback); + test(!mCallback.isHit); + mCallback.reset(); + mWorld->raycast(Ray(ray14.point1, ray14.point2, float(0.01)), &mCallback); + test(!mCallback.isHit); + mCallback.reset(); + mWorld->raycast(Ray(ray15.point1, ray15.point2, float(0.01)), &mCallback); + test(!mCallback.isHit); + mCallback.reset(); + mWorld->raycast(Ray(ray16.point1, ray16.point2, float(0.01)), &mCallback); + test(!mCallback.isHit); + + // ----- Test raycast hits ----- // + test(mConeBody->raycast(ray11, raycastInfo3)); + test(mConeProxyShape->raycast(ray11, raycastInfo3)); + mCallback.reset(); + mWorld->raycast(ray11, &mCallback); + test(mCallback.isHit); + mCallback.reset(); + mWorld->raycast(Ray(ray11.point1, ray11.point2, float(0.8)), &mCallback); + test(mCallback.isHit); + + test(mConeBody->raycast(ray12, raycastInfo3)); + test(mConeProxyShape->raycast(ray12, raycastInfo3)); + mCallback.reset(); + mWorld->raycast(ray12, &mCallback); + test(mCallback.isHit); + mCallback.reset(); + mWorld->raycast(Ray(ray12.point1, ray12.point2, float(0.8)), &mCallback); + test(mCallback.isHit); + + test(mConeBody->raycast(ray13, raycastInfo3)); + test(mConeProxyShape->raycast(ray13, raycastInfo3)); + mCallback.reset(); + mWorld->raycast(ray13, &mCallback); + test(mCallback.isHit); + mCallback.reset(); + mWorld->raycast(Ray(ray13.point1, ray13.point2, float(0.8)), &mCallback); + test(mCallback.isHit); + + test(mConeBody->raycast(ray14, raycastInfo3)); + test(mConeProxyShape->raycast(ray14, raycastInfo3)); + mCallback.reset(); + mWorld->raycast(ray14, &mCallback); + test(mCallback.isHit); + mCallback.reset(); + mWorld->raycast(Ray(ray14.point1, ray14.point2, float(0.8)), &mCallback); + test(mCallback.isHit); + + test(mConeBody->raycast(ray15, raycastInfo3)); + test(mConeProxyShape->raycast(ray15, raycastInfo3)); + mCallback.reset(); + mWorld->raycast(ray15, &mCallback); + test(mCallback.isHit); + mCallback.reset(); + mWorld->raycast(Ray(ray15.point1, ray15.point2, float(0.8)), &mCallback); + test(mCallback.isHit); + + test(mConeBody->raycast(ray16, raycastInfo3)); + test(mConeProxyShape->raycast(ray16, raycastInfo3)); + mCallback.reset(); + mWorld->raycast(ray16, &mCallback); + test(mCallback.isHit); + mCallback.reset(); + mWorld->raycast(Ray(ray16.point1, ray16.point2, float(0.8)), &mCallback); + test(mCallback.isHit); + } + + /// Test the ProxyConvexMeshShape::raycast(), CollisionBody::raycast() and + /// CollisionWorld::raycast() methods. + void testConvexMesh() { + + // ----- Test feedback data ----- // + Vector3 point1 = mLocalShapeToWorld * Vector3(1 , 2, 6); + Vector3 point2 = mLocalShapeToWorld * Vector3(1, 2, -4); + Ray ray(point1, point2); + Vector3 hitPoint = mLocalShapeToWorld * Vector3(1, 2, 4); + + mCallback.shapeToTest = mConvexMeshProxyShape; + + // CollisionWorld::raycast() + mCallback.reset(); + mWorld->raycast(ray, &mCallback); + test(mCallback.isHit); + test(mCallback.raycastInfo.body == mConvexMeshBody); + test(mCallback.raycastInfo.proxyShape == mConvexMeshProxyShape); + test(approxEqual(mCallback.raycastInfo.hitFraction, float(0.2), epsilon)); + test(approxEqual(mCallback.raycastInfo.worldPoint.x, hitPoint.x, epsilon)); + test(approxEqual(mCallback.raycastInfo.worldPoint.y, hitPoint.y, epsilon)); + test(approxEqual(mCallback.raycastInfo.worldPoint.z, hitPoint.z, epsilon)); + + // Correct category filter mask + mCallback.reset(); + mWorld->raycast(ray, &mCallback, CATEGORY2); + test(mCallback.isHit); + + // Wrong category filter mask + mCallback.reset(); + mWorld->raycast(ray, &mCallback, CATEGORY1); + test(!mCallback.isHit); + + // CollisionBody::raycast() + RaycastInfo raycastInfo2; + test(mConvexMeshBody->raycast(ray, raycastInfo2)); + test(raycastInfo2.body == mConvexMeshBody); + test(raycastInfo2.proxyShape == mConvexMeshProxyShape); + test(approxEqual(raycastInfo2.hitFraction, float(0.2), epsilon)); + test(approxEqual(raycastInfo2.worldPoint.x, hitPoint.x, epsilon)); + test(approxEqual(raycastInfo2.worldPoint.y, hitPoint.y, epsilon)); + test(approxEqual(raycastInfo2.worldPoint.z, hitPoint.z, epsilon)); + + // ProxyCollisionShape::raycast() + RaycastInfo raycastInfo3; + test(mConvexMeshBodyEdgesInfo->raycast(ray, raycastInfo3)); + test(raycastInfo3.body == mConvexMeshBodyEdgesInfo); + test(raycastInfo3.proxyShape == mConvexMeshProxyShapeEdgesInfo); + test(approxEqual(raycastInfo3.hitFraction, float(0.2), epsilon)); + test(approxEqual(raycastInfo3.worldPoint.x, hitPoint.x, epsilon)); + test(approxEqual(raycastInfo3.worldPoint.y, hitPoint.y, epsilon)); + test(approxEqual(raycastInfo3.worldPoint.z, hitPoint.z, epsilon)); + + // ProxyCollisionShape::raycast() + RaycastInfo raycastInfo4; + test(mConvexMeshProxyShape->raycast(ray, raycastInfo4)); + test(raycastInfo4.body == mConvexMeshBody); + test(raycastInfo4.proxyShape == mConvexMeshProxyShape); + test(approxEqual(raycastInfo4.hitFraction, float(0.2), epsilon)); + test(approxEqual(raycastInfo4.worldPoint.x, hitPoint.x, epsilon)); + test(approxEqual(raycastInfo4.worldPoint.y, hitPoint.y, epsilon)); + test(approxEqual(raycastInfo4.worldPoint.z, hitPoint.z, epsilon)); + + // ProxyCollisionShape::raycast() + RaycastInfo raycastInfo5; + test(mConvexMeshProxyShapeEdgesInfo->raycast(ray, raycastInfo5)); + test(raycastInfo5.body == mConvexMeshBodyEdgesInfo); + test(raycastInfo5.proxyShape == mConvexMeshProxyShapeEdgesInfo); + test(approxEqual(raycastInfo5.hitFraction, float(0.2), epsilon)); + test(approxEqual(raycastInfo5.worldPoint.x, hitPoint.x, epsilon)); + test(approxEqual(raycastInfo5.worldPoint.y, hitPoint.y, epsilon)); + test(approxEqual(raycastInfo5.worldPoint.z, hitPoint.z, epsilon)); + + Ray ray1(mLocalShapeToWorld * Vector3(0, 0, 0), mLocalShapeToWorld * Vector3(5, 7, -1)); + Ray ray2(mLocalShapeToWorld * Vector3(5, 11, 7), mLocalShapeToWorld * Vector3(17, 29, 28)); + Ray ray3(mLocalShapeToWorld * Vector3(1, 2, 3), mLocalShapeToWorld * Vector3(-11, 2, 24)); + Ray ray4(mLocalShapeToWorld * Vector3(10, 10, 10), mLocalShapeToWorld * Vector3(22, 28, 31)); + Ray ray5(mLocalShapeToWorld * Vector3(3, 1, -5), mLocalShapeToWorld * Vector3(-30, 1, -5)); + Ray ray6(mLocalShapeToWorld * Vector3(4, 4, 1), mLocalShapeToWorld * Vector3(4, -30, 1)); + Ray ray7(mLocalShapeToWorld * Vector3(1, -4, 5), mLocalShapeToWorld * Vector3(1, -4, -30)); + Ray ray8(mLocalShapeToWorld * Vector3(-4, 4, 0), mLocalShapeToWorld * Vector3(30, 4, 0)); + Ray ray9(mLocalShapeToWorld * Vector3(0, -4, -7), mLocalShapeToWorld * Vector3(0, 30, -7)); + Ray ray10(mLocalShapeToWorld * Vector3(-3, 0, -6), mLocalShapeToWorld * Vector3(-3, 0, 30)); + Ray ray11(mLocalShapeToWorld * Vector3(3, 1, 2), mLocalShapeToWorld * Vector3(-30, 0, -6)); + Ray ray12(mLocalShapeToWorld * Vector3(1, 4, -1), mLocalShapeToWorld * Vector3(1, -30, -1)); + Ray ray13(mLocalShapeToWorld * Vector3(-1, 2, 5), mLocalShapeToWorld * Vector3(-1, 2, -30)); + Ray ray14(mLocalShapeToWorld * Vector3(-3, 2, -2), mLocalShapeToWorld * Vector3(30, 2, -2)); + Ray ray15(mLocalShapeToWorld * Vector3(0, -4, 1), mLocalShapeToWorld * Vector3(0, 30, 1)); + Ray ray16(mLocalShapeToWorld * Vector3(-1, 2, -7), mLocalShapeToWorld * Vector3(-1, 2, 30)); + + // ----- Test raycast miss ----- // + test(!mConvexMeshBody->raycast(ray1, raycastInfo3)); + test(!mConvexMeshBodyEdgesInfo->raycast(ray1, raycastInfo3)); + test(!mConvexMeshProxyShape->raycast(ray1, raycastInfo3)); + test(!mConvexMeshProxyShapeEdgesInfo->raycast(ray1, raycastInfo3)); + mCallback.reset(); + mWorld->raycast(ray1, &mCallback); + test(!mCallback.isHit); + mCallback.reset(); + mWorld->raycast(Ray(ray1.point1, ray1.point2, float(0.01)), &mCallback); + test(!mCallback.isHit); + mCallback.reset(); + mWorld->raycast(Ray(ray1.point1, ray1.point2, float(100.0)), &mCallback); + test(!mCallback.isHit); + + test(!mConvexMeshBody->raycast(ray2, raycastInfo3)); + test(!mConvexMeshBodyEdgesInfo->raycast(ray2, raycastInfo3)); + test(!mConvexMeshProxyShape->raycast(ray2, raycastInfo3)); + test(!mConvexMeshProxyShapeEdgesInfo->raycast(ray2, raycastInfo3)); + mCallback.reset(); + mWorld->raycast(ray2, &mCallback); + test(!mCallback.isHit); + + test(!mConvexMeshBody->raycast(ray3, raycastInfo3)); + test(!mConvexMeshBodyEdgesInfo->raycast(ray3, raycastInfo3)); + test(!mConvexMeshProxyShape->raycast(ray3, raycastInfo3)); + test(!mConvexMeshProxyShapeEdgesInfo->raycast(ray3, raycastInfo3)); + mCallback.reset(); + mWorld->raycast(ray3, &mCallback); + test(!mCallback.isHit); + + test(!mConvexMeshBody->raycast(ray4, raycastInfo3)); + test(!mConvexMeshBodyEdgesInfo->raycast(ray4, raycastInfo3)); + test(!mConvexMeshProxyShape->raycast(ray4, raycastInfo3)); + test(!mConvexMeshProxyShapeEdgesInfo->raycast(ray4, raycastInfo3)); + mCallback.reset(); + mWorld->raycast(ray4, &mCallback); + test(!mCallback.isHit); + + test(!mConvexMeshBody->raycast(ray5, raycastInfo3)); + test(!mConvexMeshBodyEdgesInfo->raycast(ray5, raycastInfo3)); + test(!mConvexMeshProxyShape->raycast(ray5, raycastInfo3)); + test(!mConvexMeshProxyShapeEdgesInfo->raycast(ray5, raycastInfo3)); + mCallback.reset(); + mWorld->raycast(ray5, &mCallback); + test(!mCallback.isHit); + + test(!mConvexMeshBody->raycast(ray6, raycastInfo3)); + test(!mConvexMeshBodyEdgesInfo->raycast(ray6, raycastInfo3)); + test(!mConvexMeshProxyShape->raycast(ray6, raycastInfo3)); + test(!mConvexMeshProxyShapeEdgesInfo->raycast(ray6, raycastInfo3)); + mCallback.reset(); + mWorld->raycast(ray6, &mCallback); + test(!mCallback.isHit); + + test(!mConvexMeshBody->raycast(ray7, raycastInfo3)); + test(!mConvexMeshBodyEdgesInfo->raycast(ray7, raycastInfo3)); + test(!mConvexMeshProxyShape->raycast(ray7, raycastInfo3)); + test(!mConvexMeshProxyShapeEdgesInfo->raycast(ray7, raycastInfo3)); + mCallback.reset(); + mWorld->raycast(ray7, &mCallback); + test(!mCallback.isHit); + + test(!mConvexMeshBody->raycast(ray8, raycastInfo3)); + test(!mConvexMeshBodyEdgesInfo->raycast(ray8, raycastInfo3)); + test(!mConvexMeshProxyShape->raycast(ray8, raycastInfo3)); + test(!mConvexMeshProxyShapeEdgesInfo->raycast(ray8, raycastInfo3)); + mCallback.reset(); + mWorld->raycast(ray8, &mCallback); + test(!mCallback.isHit); + + test(!mConvexMeshBody->raycast(ray9, raycastInfo3)); + test(!mConvexMeshBodyEdgesInfo->raycast(ray9, raycastInfo3)); + test(!mConvexMeshProxyShape->raycast(ray9, raycastInfo3)); + test(!mConvexMeshProxyShapeEdgesInfo->raycast(ray9, raycastInfo3)); + mCallback.reset(); + mWorld->raycast(ray9, &mCallback); + test(!mCallback.isHit); + + test(!mConvexMeshBody->raycast(ray10, raycastInfo3)); + test(!mConvexMeshBodyEdgesInfo->raycast(ray10, raycastInfo3)); + test(!mConvexMeshProxyShape->raycast(ray10, raycastInfo3)); + test(!mConvexMeshProxyShapeEdgesInfo->raycast(ray10, raycastInfo3)); + mCallback.reset(); + mWorld->raycast(ray10, &mCallback); + test(!mCallback.isHit); + + mCallback.reset(); + mWorld->raycast(Ray(ray11.point1, ray11.point2, float(0.01)), &mCallback); + test(!mCallback.isHit); + mCallback.reset(); + mWorld->raycast(Ray(ray12.point1, ray12.point2, float(0.01)), &mCallback); + test(!mCallback.isHit); + mCallback.reset(); + mWorld->raycast(Ray(ray13.point1, ray13.point2, float(0.01)), &mCallback); + test(!mCallback.isHit); + mCallback.reset(); + mWorld->raycast(Ray(ray14.point1, ray14.point2, float(0.01)), &mCallback); + test(!mCallback.isHit); + mCallback.reset(); + mWorld->raycast(Ray(ray15.point1, ray15.point2, float(0.01)), &mCallback); + test(!mCallback.isHit); + mCallback.reset(); + mWorld->raycast(Ray(ray16.point1, ray16.point2, float(0.01)), &mCallback); + test(!mCallback.isHit); + + // ----- Test raycast hits ----- // + test(mConvexMeshBody->raycast(ray11, raycastInfo3)); + test(mConvexMeshBodyEdgesInfo->raycast(ray11, raycastInfo3)); + test(mConvexMeshProxyShape->raycast(ray11, raycastInfo3)); + test(mConvexMeshProxyShapeEdgesInfo->raycast(ray11, raycastInfo3)); + mCallback.reset(); + mWorld->raycast(ray11, &mCallback); + test(mCallback.isHit); + mCallback.reset(); + mWorld->raycast(Ray(ray11.point1, ray11.point2, float(0.8)), &mCallback); + test(mCallback.isHit); + + test(mConvexMeshBody->raycast(ray12, raycastInfo3)); + test(mConvexMeshBodyEdgesInfo->raycast(ray12, raycastInfo3)); + test(mConvexMeshProxyShape->raycast(ray12, raycastInfo3)); + test(mConvexMeshProxyShapeEdgesInfo->raycast(ray12, raycastInfo3)); + mCallback.reset(); + mWorld->raycast(ray12, &mCallback); + test(mCallback.isHit); + mCallback.reset(); + mWorld->raycast(Ray(ray12.point1, ray12.point2, float(0.8)), &mCallback); + test(mCallback.isHit); + + test(mConvexMeshBody->raycast(ray13, raycastInfo3)); + test(mConvexMeshBodyEdgesInfo->raycast(ray13, raycastInfo3)); + test(mConvexMeshProxyShape->raycast(ray13, raycastInfo3)); + test(mConvexMeshProxyShapeEdgesInfo->raycast(ray13, raycastInfo3)); + mCallback.reset(); + mWorld->raycast(ray13, &mCallback); + test(mCallback.isHit); + mCallback.reset(); + mWorld->raycast(Ray(ray13.point1, ray13.point2, float(0.8)), &mCallback); + test(mCallback.isHit); + + test(mConvexMeshBody->raycast(ray14, raycastInfo3)); + test(mConvexMeshBodyEdgesInfo->raycast(ray14, raycastInfo3)); + test(mConvexMeshProxyShape->raycast(ray14, raycastInfo3)); + test(mConvexMeshProxyShapeEdgesInfo->raycast(ray14, raycastInfo3)); + mCallback.reset(); + mWorld->raycast(ray14, &mCallback); + test(mCallback.isHit); + mCallback.reset(); + mWorld->raycast(Ray(ray14.point1, ray14.point2, float(0.8)), &mCallback); + test(mCallback.isHit); + + test(mConvexMeshBody->raycast(ray15, raycastInfo3)); + test(mConvexMeshBodyEdgesInfo->raycast(ray15, raycastInfo3)); + test(mConvexMeshProxyShape->raycast(ray15, raycastInfo3)); + test(mConvexMeshProxyShapeEdgesInfo->raycast(ray15, raycastInfo3)); + mCallback.reset(); + mWorld->raycast(ray15, &mCallback); + test(mCallback.isHit); + mCallback.reset(); + mWorld->raycast(Ray(ray15.point1, ray15.point2, float(0.8)), &mCallback); + test(mCallback.isHit); + + test(mConvexMeshBody->raycast(ray16, raycastInfo3)); + test(mConvexMeshBodyEdgesInfo->raycast(ray16, raycastInfo3)); + test(mConvexMeshProxyShape->raycast(ray16, raycastInfo3)); + test(mConvexMeshProxyShapeEdgesInfo->raycast(ray16, raycastInfo3)); + mCallback.reset(); + mWorld->raycast(ray16, &mCallback); + test(mCallback.isHit); + mCallback.reset(); + mWorld->raycast(Ray(ray16.point1, ray16.point2, float(0.8)), &mCallback); + test(mCallback.isHit); + } + + /// Test the ProxyCylinderShape::raycast(), CollisionBody::raycast() and + /// CollisionWorld::raycast() methods. + void testCylinder() { + + // ----- Test feedback data ----- // + Vector3 point1A = mLocalShapeToWorld * Vector3(4 , 1, 0); + Vector3 point1B = mLocalShapeToWorld * Vector3(-6, 1, 0); + Ray ray(point1A, point1B); + Vector3 hitPoint = mLocalShapeToWorld * Vector3(2, 1, 0); + + Vector3 point2A = mLocalShapeToWorld * Vector3(0 , 4.5, 0); + Vector3 point2B = mLocalShapeToWorld * Vector3(0, -5.5, 0); + Ray rayTop(point2A, point2B); + Vector3 hitPointTop = mLocalShapeToWorld * Vector3(0, float(2.5), 0); + + Vector3 point3A = mLocalShapeToWorld * Vector3(0 , -4.5, 0); + Vector3 point3B = mLocalShapeToWorld * Vector3(0, 5.5, 0); + Ray rayBottom(point3A, point3B); + Vector3 hitPointBottom = mLocalShapeToWorld * Vector3(0, float(-2.5), 0); + + mCallback.shapeToTest = mCylinderProxyShape; + + // CollisionWorld::raycast() + mCallback.reset(); + mWorld->raycast(ray, &mCallback); + test(mCallback.isHit); + test(mCallback.raycastInfo.body == mCylinderBody); + test(mCallback.raycastInfo.proxyShape == mCylinderProxyShape); + test(approxEqual(mCallback.raycastInfo.hitFraction, float(0.2), epsilon)); + test(approxEqual(mCallback.raycastInfo.worldPoint.x, hitPoint.x, epsilon)); + test(approxEqual(mCallback.raycastInfo.worldPoint.y, hitPoint.y, epsilon)); + test(approxEqual(mCallback.raycastInfo.worldPoint.z, hitPoint.z, epsilon)); + + // Correct category filter mask + mCallback.reset(); + mWorld->raycast(ray, &mCallback, CATEGORY2); + test(mCallback.isHit); + + // Wrong category filter mask + mCallback.reset(); + mWorld->raycast(ray, &mCallback, CATEGORY1); + test(!mCallback.isHit); + + // CollisionBody::raycast() + RaycastInfo raycastInfo2; + test(mCylinderBody->raycast(ray, raycastInfo2)); + test(raycastInfo2.body == mCylinderBody); + test(raycastInfo2.proxyShape == mCylinderProxyShape); + test(approxEqual(raycastInfo2.hitFraction, float(0.2), epsilon)); + test(approxEqual(raycastInfo2.worldPoint.x, hitPoint.x, epsilon)); + test(approxEqual(raycastInfo2.worldPoint.y, hitPoint.y, epsilon)); + test(approxEqual(raycastInfo2.worldPoint.z, hitPoint.z, epsilon)); + + // ProxyCollisionShape::raycast() + RaycastInfo raycastInfo3; + test(mCylinderProxyShape->raycast(ray, raycastInfo3)); + test(raycastInfo3.body == mCylinderBody); + test(raycastInfo3.proxyShape == mCylinderProxyShape); + test(approxEqual(raycastInfo3.hitFraction, float(0.2), epsilon)); + test(approxEqual(raycastInfo3.worldPoint.x, hitPoint.x, epsilon)); + test(approxEqual(raycastInfo3.worldPoint.y, hitPoint.y, epsilon)); + test(approxEqual(raycastInfo3.worldPoint.z, hitPoint.z, epsilon)); + + // ProxyCollisionShape::raycast() + RaycastInfo raycastInfo5; + test(mCylinderProxyShape->raycast(rayTop, raycastInfo5)); + test(raycastInfo5.body == mCylinderBody); + test(raycastInfo5.proxyShape == mCylinderProxyShape); + test(approxEqual(raycastInfo5.hitFraction, float(0.2), epsilon)); + test(approxEqual(raycastInfo5.worldPoint.x, hitPointTop.x, epsilon)); + test(approxEqual(raycastInfo5.worldPoint.y, hitPointTop.y, epsilon)); + test(approxEqual(raycastInfo5.worldPoint.z, hitPointTop.z, epsilon)); + + // ProxyCollisionShape::raycast() + RaycastInfo raycastInfo6; + test(mCylinderProxyShape->raycast(rayBottom, raycastInfo6)); + test(raycastInfo6.body == mCylinderBody); + test(raycastInfo6.proxyShape == mCylinderProxyShape); + test(approxEqual(raycastInfo6.hitFraction, float(0.2), epsilon)); + test(approxEqual(raycastInfo6.worldPoint.x, hitPointBottom.x, epsilon)); + test(approxEqual(raycastInfo6.worldPoint.y, hitPointBottom.y, epsilon)); + test(approxEqual(raycastInfo6.worldPoint.z, hitPointBottom.z, epsilon)); + + Ray ray1(mLocalShapeToWorld * Vector3(0, 0, 0), mLocalShapeToWorld * Vector3(5, 7, -1)); + Ray ray2(mLocalShapeToWorld * Vector3(5, 11, 7), mLocalShapeToWorld * Vector3(17, 20, 28)); + Ray ray3(mLocalShapeToWorld * Vector3(1, 3, -1), mLocalShapeToWorld * Vector3(-11,3, 20)); + Ray ray4(mLocalShapeToWorld * Vector3(10, 10, 10), mLocalShapeToWorld * Vector3(22, 28, 31)); + Ray ray5(mLocalShapeToWorld * Vector3(4, 1, -5), mLocalShapeToWorld * Vector3(-30, 1, -5)); + Ray ray6(mLocalShapeToWorld * Vector3(4, 9, 1), mLocalShapeToWorld * Vector3(4, -30, 1)); + Ray ray7(mLocalShapeToWorld * Vector3(1, -9, 5), mLocalShapeToWorld * Vector3(1, -9, -30)); + Ray ray8(mLocalShapeToWorld * Vector3(-4, 9, 0), mLocalShapeToWorld * Vector3(30, 9, 0)); + Ray ray9(mLocalShapeToWorld * Vector3(0, -9, -4), mLocalShapeToWorld * Vector3(0, 30, -4)); + Ray ray10(mLocalShapeToWorld * Vector3(-4, 0, -6), mLocalShapeToWorld * Vector3(-4, 0, 30)); + Ray ray11(mLocalShapeToWorld * Vector3(4, 1, 1.5), mLocalShapeToWorld * Vector3(-30, 1, 1.5)); + Ray ray12(mLocalShapeToWorld * Vector3(1, 9, -1), mLocalShapeToWorld * Vector3(1, -30, -1)); + Ray ray13(mLocalShapeToWorld * Vector3(-1, 2, 3), mLocalShapeToWorld * Vector3(-1, 2, -30)); + Ray ray14(mLocalShapeToWorld * Vector3(-3, 2, -1.7), mLocalShapeToWorld * Vector3(30, 2, -1.7)); + Ray ray15(mLocalShapeToWorld * Vector3(0, -9, 1), mLocalShapeToWorld * Vector3(0, 30, 1)); + Ray ray16(mLocalShapeToWorld * Vector3(-1, 2, -7), mLocalShapeToWorld * Vector3(-1, 2, 30)); + + // ----- Test raycast miss ----- // + test(!mCylinderBody->raycast(ray1, raycastInfo3)); + test(!mCylinderProxyShape->raycast(ray1, raycastInfo3)); + mCallback.reset(); + mWorld->raycast(ray1, &mCallback); + test(!mCallback.isHit); + mCallback.reset(); + mWorld->raycast(Ray(ray1.point1, ray1.point2, float(0.01)), &mCallback); + test(!mCallback.isHit); + mCallback.reset(); + mWorld->raycast(Ray(ray1.point1, ray1.point2, float(100.0)), &mCallback); + test(!mCallback.isHit); + + test(!mCylinderBody->raycast(ray2, raycastInfo3)); + test(!mCylinderProxyShape->raycast(ray2, raycastInfo3)); + mCallback.reset(); + mWorld->raycast(ray2, &mCallback); + test(!mCallback.isHit); + + test(!mCylinderBody->raycast(ray3, raycastInfo3)); + test(!mCylinderProxyShape->raycast(ray3, raycastInfo3)); + mCallback.reset(); + mWorld->raycast(ray3, &mCallback); + test(!mCallback.isHit); + + test(!mCylinderBody->raycast(ray4, raycastInfo3)); + test(!mCylinderProxyShape->raycast(ray4, raycastInfo3)); + mCallback.reset(); + mWorld->raycast(ray4, &mCallback); + test(!mCallback.isHit); + + test(!mCylinderBody->raycast(ray5, raycastInfo3)); + test(!mCylinderProxyShape->raycast(ray5, raycastInfo3)); + mCallback.reset(); + mWorld->raycast(ray5, &mCallback); + test(!mCallback.isHit); + + test(!mCylinderBody->raycast(ray6, raycastInfo3)); + test(!mCylinderProxyShape->raycast(ray6, raycastInfo3)); + mCallback.reset(); + mWorld->raycast(ray6, &mCallback); + test(!mCallback.isHit); + + test(!mCylinderBody->raycast(ray7, raycastInfo3)); + test(!mCylinderProxyShape->raycast(ray7, raycastInfo3)); + mCallback.reset(); + mWorld->raycast(ray7, &mCallback); + test(!mCallback.isHit); + + test(!mCylinderBody->raycast(ray8, raycastInfo3)); + test(!mCylinderProxyShape->raycast(ray8, raycastInfo3)); + mCallback.reset(); + mWorld->raycast(ray8, &mCallback); + test(!mCallback.isHit); + + test(!mCylinderBody->raycast(ray9, raycastInfo3)); + test(!mCylinderProxyShape->raycast(ray9, raycastInfo3)); + mCallback.reset(); + mWorld->raycast(ray9, &mCallback); + test(!mCallback.isHit); + + test(!mCylinderBody->raycast(ray10, raycastInfo3)); + test(!mCylinderProxyShape->raycast(ray10, raycastInfo3)); + mCallback.reset(); + mWorld->raycast(ray10, &mCallback); + test(!mCallback.isHit); + + mCallback.reset(); + mWorld->raycast(Ray(ray11.point1, ray11.point2, float(0.01)), &mCallback); + test(!mCallback.isHit); + mCallback.reset(); + mWorld->raycast(Ray(ray12.point1, ray12.point2, float(0.01)), &mCallback); + test(!mCallback.isHit); + mCallback.reset(); + mWorld->raycast(Ray(ray13.point1, ray13.point2, float(0.01)), &mCallback); + test(!mCallback.isHit); + mCallback.reset(); + mWorld->raycast(Ray(ray14.point1, ray14.point2, float(0.01)), &mCallback); + test(!mCallback.isHit); + mCallback.reset(); + mWorld->raycast(Ray(ray15.point1, ray15.point2, float(0.01)), &mCallback); + test(!mCallback.isHit); + mCallback.reset(); + mWorld->raycast(Ray(ray16.point1, ray16.point2, float(0.01)), &mCallback); + test(!mCallback.isHit); + + // ----- Test raycast hits ----- // + test(mCylinderBody->raycast(ray11, raycastInfo3)); + test(mCylinderProxyShape->raycast(ray11, raycastInfo3)); + mCallback.reset(); + mWorld->raycast(ray11, &mCallback); + test(mCallback.isHit); + mCallback.reset(); + mWorld->raycast(Ray(ray11.point1, ray11.point2, float(0.8)), &mCallback); + test(mCallback.isHit); + + test(mCylinderBody->raycast(ray12, raycastInfo3)); + test(mCylinderProxyShape->raycast(ray12, raycastInfo3)); + mCallback.reset(); + mWorld->raycast(ray12, &mCallback); + test(mCallback.isHit); + mCallback.reset(); + mWorld->raycast(Ray(ray12.point1, ray12.point2, float(0.8)), &mCallback); + test(mCallback.isHit); + + test(mCylinderBody->raycast(ray13, raycastInfo3)); + test(mCylinderProxyShape->raycast(ray13, raycastInfo3)); + mCallback.reset(); + mWorld->raycast(ray13, &mCallback); + test(mCallback.isHit); + mCallback.reset(); + mWorld->raycast(Ray(ray13.point1, ray13.point2, float(0.8)), &mCallback); + test(mCallback.isHit); + + test(mCylinderBody->raycast(ray14, raycastInfo3)); + test(mCylinderProxyShape->raycast(ray14, raycastInfo3)); + mCallback.reset(); + mWorld->raycast(ray14, &mCallback); + test(mCallback.isHit); + mCallback.reset(); + mWorld->raycast(Ray(ray14.point1, ray14.point2, float(0.8)), &mCallback); + test(mCallback.isHit); + + test(mCylinderBody->raycast(ray15, raycastInfo3)); + test(mCylinderProxyShape->raycast(ray15, raycastInfo3)); + mCallback.reset(); + mWorld->raycast(ray15, &mCallback); + test(mCallback.isHit); + mCallback.reset(); + mWorld->raycast(Ray(ray15.point1, ray15.point2, float(0.8)), &mCallback); + test(mCallback.isHit); + + test(mCylinderBody->raycast(ray16, raycastInfo3)); + test(mCylinderProxyShape->raycast(ray16, raycastInfo3)); + mCallback.reset(); + mWorld->raycast(ray16, &mCallback); + test(mCallback.isHit); + mCallback.reset(); + mWorld->raycast(Ray(ray16.point1, ray16.point2, float(0.8)), &mCallback); + test(mCallback.isHit); + } + + /// Test the CollisionBody::raycast() and + /// CollisionWorld::raycast() methods. + void testCompound() { + + // ----- Test feedback data ----- // + + // Raycast hit against the sphere shape + Ray ray1(mLocalShape2ToWorld * Vector3(4, 1, 2), mLocalShape2ToWorld * Vector3(-30, 1, 2)); + Ray ray2(mLocalShape2ToWorld * Vector3(1, 4, -1), mLocalShape2ToWorld * Vector3(1, -30, -1)); + Ray ray3(mLocalShape2ToWorld * Vector3(-1, 2, 5), mLocalShape2ToWorld * Vector3(-1, 2, -30)); + Ray ray4(mLocalShape2ToWorld * Vector3(-5, 2, -2), mLocalShape2ToWorld * Vector3(30, 2, -2)); + Ray ray5(mLocalShape2ToWorld * Vector3(0, -4, 1), mLocalShape2ToWorld * Vector3(0, 30, 1)); + Ray ray6(mLocalShape2ToWorld * Vector3(-1, 2, -11), mLocalShape2ToWorld * Vector3(-1, 2, 30)); + + mCallback.shapeToTest = mCompoundSphereProxyShape; + + // Correct category filter mask + mCallback.reset(); + mWorld->raycast(ray1, &mCallback, CATEGORY2); + test(mCallback.isHit); + + // Wrong category filter mask + mCallback.reset(); + mWorld->raycast(ray1, &mCallback, CATEGORY1); + test(!mCallback.isHit); + + RaycastInfo raycastInfo; + test(mCompoundBody->raycast(ray1, raycastInfo)); + mCallback.reset(); + mWorld->raycast(ray1, &mCallback); + test(mCallback.isHit); + mCallback.reset(); + mWorld->raycast(Ray(ray1.point1, ray1.point2, float(0.8)), &mCallback); + test(mCallback.isHit); + + test(mCompoundBody->raycast(ray2, raycastInfo)); + mCallback.reset(); + mWorld->raycast(ray2, &mCallback); + test(mCallback.isHit); + mCallback.reset(); + mWorld->raycast(Ray(ray2.point1, ray2.point2, float(0.8)), &mCallback); + test(mCallback.isHit); + + test(mCompoundBody->raycast(ray3, raycastInfo)); + mCallback.reset(); + mWorld->raycast(ray3, &mCallback); + test(mCallback.isHit); + mCallback.reset(); + mWorld->raycast(Ray(ray3.point1, ray3.point2, float(0.8)), &mCallback); + test(mCallback.isHit); + + test(mCompoundBody->raycast(ray4, raycastInfo)); + mCallback.reset(); + mWorld->raycast(ray4, &mCallback); + test(mCallback.isHit); + mCallback.reset(); + mWorld->raycast(Ray(ray4.point1, ray4.point2, float(0.8)), &mCallback); + test(mCallback.isHit); + + test(mCompoundBody->raycast(ray5, raycastInfo)); + mCallback.reset(); + mWorld->raycast(ray5, &mCallback); + test(mCallback.isHit); + mCallback.reset(); + mWorld->raycast(Ray(ray5.point1, ray5.point2, float(0.8)), &mCallback); + test(mCallback.isHit); + + test(mCompoundBody->raycast(ray6, raycastInfo)); + mCallback.reset(); + mWorld->raycast(ray6, &mCallback); + test(mCallback.isHit); + mCallback.reset(); + mWorld->raycast(Ray(ray6.point1, ray6.point2, float(0.8)), &mCallback); + test(mCallback.isHit); + + // Raycast hit agains the cylinder shape + Ray ray11(mLocalShapeToWorld * Vector3(4, 1, 1.5), mLocalShapeToWorld * Vector3(-30, 1.5, 2)); + Ray ray12(mLocalShapeToWorld * Vector3(1.5, 9, -1), mLocalShapeToWorld * Vector3(1.5, -30, -1)); + Ray ray13(mLocalShapeToWorld * Vector3(-1, 2, 3), mLocalShapeToWorld * Vector3(-1, 2, -30)); + Ray ray14(mLocalShapeToWorld * Vector3(-3, 2, -1.5), mLocalShapeToWorld * Vector3(30, 1, -1.5)); + Ray ray15(mLocalShapeToWorld * Vector3(0, -9, 1), mLocalShapeToWorld * Vector3(0, 30, 1)); + Ray ray16(mLocalShapeToWorld * Vector3(-1, 2, -7), mLocalShapeToWorld * Vector3(-1, 2, 30)); + + mCallback.shapeToTest = mCompoundCylinderProxyShape; + + test(mCompoundBody->raycast(ray11, raycastInfo)); + mCallback.reset(); + mWorld->raycast(ray11, &mCallback); + test(mCallback.isHit); + mCallback.reset(); + mWorld->raycast(Ray(ray11.point1, ray11.point2, float(0.8)), &mCallback); + test(mCallback.isHit); + + test(mCompoundBody->raycast(ray12, raycastInfo)); + mCallback.reset(); + mWorld->raycast(ray12, &mCallback); + test(mCallback.isHit); + mCallback.reset(); + mWorld->raycast(Ray(ray12.point1, ray12.point2, float(0.8)), &mCallback); + test(mCallback.isHit); + + test(mCompoundBody->raycast(ray13, raycastInfo)); + mCallback.reset(); + mWorld->raycast(ray13, &mCallback); + test(mCallback.isHit); + mCallback.reset(); + mWorld->raycast(Ray(ray13.point1, ray13.point2, float(0.8)), &mCallback); + test(mCallback.isHit); + + test(mCompoundBody->raycast(ray14, raycastInfo)); + mCallback.reset(); + mWorld->raycast(ray14, &mCallback); + test(mCallback.isHit); + mCallback.reset(); + mWorld->raycast(Ray(ray14.point1, ray14.point2, float(0.8)), &mCallback); + test(mCallback.isHit); + + test(mCompoundBody->raycast(ray15, raycastInfo)); + mCallback.reset(); + mWorld->raycast(ray15, &mCallback); + test(mCallback.isHit); + mCallback.reset(); + mWorld->raycast(Ray(ray15.point1, ray15.point2, float(0.8)), &mCallback); + test(mCallback.isHit); + + test(mCompoundBody->raycast(ray16, raycastInfo)); + mCallback.reset(); + mWorld->raycast(ray16, &mCallback); + test(mCallback.isHit); + mCallback.reset(); + mWorld->raycast(Ray(ray16.point1, ray16.point2, float(0.8)), &mCallback); + test(mCallback.isHit); + } + + + void testConcaveMesh() { + + // ----- Test feedback data ----- // + Vector3 point1 = mLocalShapeToWorld * Vector3(1 , 2, 6); + Vector3 point2 = mLocalShapeToWorld * Vector3(1, 2, -4); + Ray ray(point1, point2); + Vector3 hitPoint = mLocalShapeToWorld * Vector3(1, 2, 4); + + mCallback.shapeToTest = mConcaveMeshProxyShape; + + // CollisionWorld::raycast() + mCallback.reset(); + mWorld->raycast(ray, &mCallback); + test(mCallback.isHit); + test(mCallback.raycastInfo.body == mConcaveMeshBody); + test(mCallback.raycastInfo.proxyShape == mConcaveMeshProxyShape); + test(approxEqual(mCallback.raycastInfo.hitFraction, float(0.2), epsilon)); + test(approxEqual(mCallback.raycastInfo.worldPoint.x, hitPoint.x, epsilon)); + test(approxEqual(mCallback.raycastInfo.worldPoint.y, hitPoint.y, epsilon)); + test(approxEqual(mCallback.raycastInfo.worldPoint.z, hitPoint.z, epsilon)); + + // Correct category filter mask + mCallback.reset(); + mWorld->raycast(ray, &mCallback, CATEGORY2); + test(mCallback.isHit); + + // Wrong category filter mask + mCallback.reset(); + mWorld->raycast(ray, &mCallback, CATEGORY1); + test(!mCallback.isHit); + + // CollisionBody::raycast() + RaycastInfo raycastInfo2; + test(mConcaveMeshBody->raycast(ray, raycastInfo2)); + test(raycastInfo2.body == mConcaveMeshBody); + test(raycastInfo2.proxyShape == mConcaveMeshProxyShape); + test(approxEqual(raycastInfo2.hitFraction, float(0.2), epsilon)); + test(approxEqual(raycastInfo2.worldPoint.x, hitPoint.x, epsilon)); + test(approxEqual(raycastInfo2.worldPoint.y, hitPoint.y, epsilon)); + test(approxEqual(raycastInfo2.worldPoint.z, hitPoint.z, epsilon)); + + // ProxyCollisionShape::raycast() + RaycastInfo raycastInfo3; + test(mConcaveMeshBody->raycast(ray, raycastInfo3)); + test(raycastInfo3.body == mConcaveMeshBody); + test(raycastInfo3.proxyShape == mConcaveMeshProxyShape); + test(approxEqual(raycastInfo3.hitFraction, float(0.2), epsilon)); + test(approxEqual(raycastInfo3.worldPoint.x, hitPoint.x, epsilon)); + test(approxEqual(raycastInfo3.worldPoint.y, hitPoint.y, epsilon)); + test(approxEqual(raycastInfo3.worldPoint.z, hitPoint.z, epsilon)); + + // ProxyCollisionShape::raycast() + RaycastInfo raycastInfo4; + test(mConcaveMeshBody->raycast(ray, raycastInfo4)); + test(raycastInfo4.body == mConcaveMeshBody); + test(raycastInfo4.proxyShape == mConcaveMeshProxyShape); + test(approxEqual(raycastInfo4.hitFraction, float(0.2), epsilon)); + test(approxEqual(raycastInfo4.worldPoint.x, hitPoint.x, epsilon)); + test(approxEqual(raycastInfo4.worldPoint.y, hitPoint.y, epsilon)); + test(approxEqual(raycastInfo4.worldPoint.z, hitPoint.z, epsilon)); + + // ProxyCollisionShape::raycast() + RaycastInfo raycastInfo5; + test(mConcaveMeshBody->raycast(ray, raycastInfo5)); + test(raycastInfo5.body == mConcaveMeshBody); + test(raycastInfo5.proxyShape == mConcaveMeshProxyShape); + test(approxEqual(raycastInfo5.hitFraction, float(0.2), epsilon)); + test(approxEqual(raycastInfo5.worldPoint.x, hitPoint.x, epsilon)); + test(approxEqual(raycastInfo5.worldPoint.y, hitPoint.y, epsilon)); + test(approxEqual(raycastInfo5.worldPoint.z, hitPoint.z, epsilon)); + + Ray ray1(mLocalShapeToWorld * Vector3(0, 0, 0), mLocalShapeToWorld * Vector3(5, 7, -1)); + Ray ray2(mLocalShapeToWorld * Vector3(5, 11, 7), mLocalShapeToWorld * Vector3(17, 29, 28)); + Ray ray3(mLocalShapeToWorld * Vector3(1, 2, 3), mLocalShapeToWorld * Vector3(-11, 2, 24)); + Ray ray4(mLocalShapeToWorld * Vector3(10, 10, 10), mLocalShapeToWorld * Vector3(22, 28, 31)); + Ray ray5(mLocalShapeToWorld * Vector3(3, 1, -5), mLocalShapeToWorld * Vector3(-30, 1, -5)); + Ray ray6(mLocalShapeToWorld * Vector3(4, 4, 1), mLocalShapeToWorld * Vector3(4, -30, 1)); + Ray ray7(mLocalShapeToWorld * Vector3(1, -4, 5), mLocalShapeToWorld * Vector3(1, -4, -30)); + Ray ray8(mLocalShapeToWorld * Vector3(-4, 4, 0), mLocalShapeToWorld * Vector3(30, 4, 0)); + Ray ray9(mLocalShapeToWorld * Vector3(0, -4, -7), mLocalShapeToWorld * Vector3(0, 30, -7)); + Ray ray10(mLocalShapeToWorld * Vector3(-3, 0, -6), mLocalShapeToWorld * Vector3(-3, 0, 30)); + Ray ray11(mLocalShapeToWorld * Vector3(3, 1, 2), mLocalShapeToWorld * Vector3(-30, 0, -6)); + Ray ray12(mLocalShapeToWorld * Vector3(1, 4, -1), mLocalShapeToWorld * Vector3(1, -30, -1)); + Ray ray13(mLocalShapeToWorld * Vector3(-1, 2, 5), mLocalShapeToWorld * Vector3(-1, 2, -30)); + Ray ray14(mLocalShapeToWorld * Vector3(-3, 2, -2), mLocalShapeToWorld * Vector3(30, 2, -2)); + Ray ray15(mLocalShapeToWorld * Vector3(0, -4, 1), mLocalShapeToWorld * Vector3(0, 30, 1)); + Ray ray16(mLocalShapeToWorld * Vector3(-1, 2, -7), mLocalShapeToWorld * Vector3(-1, 2, 30)); + + // ----- Test raycast miss ----- // + test(!mConcaveMeshBody->raycast(ray1, raycastInfo3)); + test(!mConvexMeshProxyShape->raycast(ray1, raycastInfo3)); + mCallback.reset(); + mWorld->raycast(ray1, &mCallback); + test(!mCallback.isHit); + mCallback.reset(); + mWorld->raycast(Ray(ray1.point1, ray1.point2, float(0.01)), &mCallback); + test(!mCallback.isHit); + mCallback.reset(); + mWorld->raycast(Ray(ray1.point1, ray1.point2, float(100.0)), &mCallback); + test(!mCallback.isHit); + + test(!mConcaveMeshBody->raycast(ray2, raycastInfo3)); + test(!mConcaveMeshProxyShape->raycast(ray2, raycastInfo3)); + mCallback.reset(); + mWorld->raycast(ray2, &mCallback); + test(!mCallback.isHit); + + test(!mConcaveMeshBody->raycast(ray3, raycastInfo3)); + test(!mConcaveMeshProxyShape->raycast(ray3, raycastInfo3)); + mCallback.reset(); + mWorld->raycast(ray3, &mCallback); + test(!mCallback.isHit); + + test(!mConcaveMeshBody->raycast(ray4, raycastInfo3)); + test(!mConcaveMeshProxyShape->raycast(ray4, raycastInfo3)); + mCallback.reset(); + mWorld->raycast(ray4, &mCallback); + test(!mCallback.isHit); + + test(!mConcaveMeshBody->raycast(ray5, raycastInfo3)); + test(!mConcaveMeshProxyShape->raycast(ray5, raycastInfo3)); + mCallback.reset(); + mWorld->raycast(ray5, &mCallback); + test(!mCallback.isHit); + + test(!mConcaveMeshBody->raycast(ray6, raycastInfo3)); + test(!mConcaveMeshProxyShape->raycast(ray6, raycastInfo3)); + mCallback.reset(); + mWorld->raycast(ray6, &mCallback); + test(!mCallback.isHit); + + test(!mConcaveMeshBody->raycast(ray7, raycastInfo3)); + test(!mConcaveMeshProxyShape->raycast(ray7, raycastInfo3)); + mCallback.reset(); + mWorld->raycast(ray7, &mCallback); + test(!mCallback.isHit); + + test(!mConcaveMeshBody->raycast(ray8, raycastInfo3)); + test(!mConcaveMeshProxyShape->raycast(ray8, raycastInfo3)); + mCallback.reset(); + mWorld->raycast(ray8, &mCallback); + test(!mCallback.isHit); + + test(!mConcaveMeshBody->raycast(ray9, raycastInfo3)); + test(!mConcaveMeshProxyShape->raycast(ray9, raycastInfo3)); + mCallback.reset(); + mWorld->raycast(ray9, &mCallback); + test(!mCallback.isHit); + + test(!mConcaveMeshBody->raycast(ray10, raycastInfo3)); + test(!mConcaveMeshProxyShape->raycast(ray10, raycastInfo3)); + mCallback.reset(); + mWorld->raycast(ray10, &mCallback); + test(!mCallback.isHit); + + mCallback.reset(); + mWorld->raycast(Ray(ray11.point1, ray11.point2, float(0.01)), &mCallback); + test(!mCallback.isHit); + mCallback.reset(); + mWorld->raycast(Ray(ray12.point1, ray12.point2, float(0.01)), &mCallback); + test(!mCallback.isHit); + mCallback.reset(); + mWorld->raycast(Ray(ray13.point1, ray13.point2, float(0.01)), &mCallback); + test(!mCallback.isHit); + mCallback.reset(); + mWorld->raycast(Ray(ray14.point1, ray14.point2, float(0.01)), &mCallback); + test(!mCallback.isHit); + mCallback.reset(); + mWorld->raycast(Ray(ray15.point1, ray15.point2, float(0.01)), &mCallback); + test(!mCallback.isHit); + mCallback.reset(); + mWorld->raycast(Ray(ray16.point1, ray16.point2, float(0.01)), &mCallback); + test(!mCallback.isHit); + + // ----- Test raycast hits ----- // + test(mConcaveMeshBody->raycast(ray11, raycastInfo3)); + test(mConcaveMeshProxyShape->raycast(ray11, raycastInfo3)); + mCallback.reset(); + mWorld->raycast(ray11, &mCallback); + test(mCallback.isHit); + mCallback.reset(); + mWorld->raycast(Ray(ray11.point1, ray11.point2, float(0.8)), &mCallback); + test(mCallback.isHit); + + test(mConcaveMeshBody->raycast(ray12, raycastInfo3)); + test(mConcaveMeshProxyShape->raycast(ray12, raycastInfo3)); + mCallback.reset(); + mWorld->raycast(ray12, &mCallback); + test(mCallback.isHit); + mCallback.reset(); + mWorld->raycast(Ray(ray12.point1, ray12.point2, float(0.8)), &mCallback); + test(mCallback.isHit); + + test(mConcaveMeshBody->raycast(ray13, raycastInfo3)); + test(mConcaveMeshProxyShape->raycast(ray13, raycastInfo3)); + mCallback.reset(); + mWorld->raycast(ray13, &mCallback); + test(mCallback.isHit); + mCallback.reset(); + mWorld->raycast(Ray(ray13.point1, ray13.point2, float(0.8)), &mCallback); + test(mCallback.isHit); + + test(mConcaveMeshBody->raycast(ray14, raycastInfo3)); + test(mConcaveMeshProxyShape->raycast(ray14, raycastInfo3)); + mCallback.reset(); + mWorld->raycast(ray14, &mCallback); + test(mCallback.isHit); + mCallback.reset(); + mWorld->raycast(Ray(ray14.point1, ray14.point2, float(0.8)), &mCallback); + test(mCallback.isHit); + + test(mConcaveMeshBody->raycast(ray15, raycastInfo3)); + test(mConcaveMeshProxyShape->raycast(ray15, raycastInfo3)); + mCallback.reset(); + mWorld->raycast(ray15, &mCallback); + test(mCallback.isHit); + mCallback.reset(); + mWorld->raycast(Ray(ray15.point1, ray15.point2, float(0.8)), &mCallback); + test(mCallback.isHit); + + test(mConcaveMeshBody->raycast(ray16, raycastInfo3)); + test(mConcaveMeshProxyShape->raycast(ray16, raycastInfo3)); + mCallback.reset(); + mWorld->raycast(ray16, &mCallback); + test(mCallback.isHit); + mCallback.reset(); + mWorld->raycast(Ray(ray16.point1, ray16.point2, float(0.8)), &mCallback); + test(mCallback.isHit); + } + + void testHeightField() { + + // ----- Test feedback data ----- // + Vector3 point1A = mLocalShapeToWorld * Vector3(0 , 10, 2); + Vector3 point1B = mLocalShapeToWorld * Vector3(0, -10, 2); + Ray ray(point1A, point1B); + Vector3 hitPoint = mLocalShapeToWorld * Vector3(0, 2, 2); + + Vector3 point2A = mLocalShapeToWorld * Vector3(1 , 8, -4); + Vector3 point2B = mLocalShapeToWorld * Vector3(1, -8, -4); + Ray rayBottom(point2A, point2B); + Vector3 hitPoint2 = mLocalShapeToWorld * Vector3(1, 2, -4); + + mCallback.shapeToTest = mHeightFieldProxyShape; + + // CollisionWorld::raycast() + mCallback.reset(); + mWorld->raycast(ray, &mCallback); + test(mCallback.isHit); + test(mCallback.raycastInfo.body == mHeightFieldBody); + test(mCallback.raycastInfo.proxyShape == mHeightFieldProxyShape); + test(approxEqual(mCallback.raycastInfo.hitFraction, float(0.4), epsilon)); + test(approxEqual(mCallback.raycastInfo.worldPoint.x, hitPoint.x, epsilon)); + test(approxEqual(mCallback.raycastInfo.worldPoint.y, hitPoint.y, epsilon)); + test(approxEqual(mCallback.raycastInfo.worldPoint.z, hitPoint.z, epsilon)); + + // Correct category filter mask + mCallback.reset(); + mWorld->raycast(ray, &mCallback, CATEGORY2); + test(mCallback.isHit); + + // Wrong category filter mask + mCallback.reset(); + mWorld->raycast(ray, &mCallback, CATEGORY1); + test(!mCallback.isHit); + + // CollisionBody::raycast() + RaycastInfo raycastInfo2; + test(mHeightFieldBody->raycast(ray, raycastInfo2)); + test(raycastInfo2.body == mHeightFieldBody); + test(raycastInfo2.proxyShape == mHeightFieldProxyShape); + test(approxEqual(raycastInfo2.hitFraction, float(0.4), epsilon)); + test(approxEqual(raycastInfo2.worldPoint.x, hitPoint.x, epsilon)); + test(approxEqual(raycastInfo2.worldPoint.y, hitPoint.y, epsilon)); + test(approxEqual(raycastInfo2.worldPoint.z, hitPoint.z, epsilon)); + + // ProxyCollisionShape::raycast() + RaycastInfo raycastInfo3; + test(mHeightFieldProxyShape->raycast(ray, raycastInfo3)); + test(raycastInfo3.body == mHeightFieldBody); + test(raycastInfo3.proxyShape == mHeightFieldProxyShape); + test(approxEqual(raycastInfo3.hitFraction, float(0.4), epsilon)); + test(approxEqual(raycastInfo3.worldPoint.x, hitPoint.x, epsilon)); + test(approxEqual(raycastInfo3.worldPoint.y, hitPoint.y, epsilon)); + test(approxEqual(raycastInfo3.worldPoint.z, hitPoint.z, epsilon)); + + mCallback.reset(); + mWorld->raycast(rayBottom, &mCallback); + test(mCallback.isHit); + test(mCallback.raycastInfo.body == mHeightFieldBody); + test(mCallback.raycastInfo.proxyShape == mHeightFieldProxyShape); + test(approxEqual(mCallback.raycastInfo.hitFraction, float(0.375), epsilon)); + test(approxEqual(mCallback.raycastInfo.worldPoint.x, hitPoint2.x, epsilon)); + test(approxEqual(mCallback.raycastInfo.worldPoint.y, hitPoint2.y, epsilon)); + test(approxEqual(mCallback.raycastInfo.worldPoint.z, hitPoint2.z, epsilon)); + + // CollisionBody::raycast() + RaycastInfo raycastInfo5; + test(mHeightFieldBody->raycast(rayBottom, raycastInfo5)); + test(raycastInfo5.body == mHeightFieldBody); + test(raycastInfo5.proxyShape == mHeightFieldProxyShape); + test(approxEqual(raycastInfo5.hitFraction, float(0.375), epsilon)); + test(approxEqual(raycastInfo5.worldPoint.x, hitPoint2.x, epsilon)); + test(approxEqual(raycastInfo5.worldPoint.y, hitPoint2.y, epsilon)); + test(approxEqual(raycastInfo5.worldPoint.z, hitPoint2.z, epsilon)); + + // ProxyCollisionShape::raycast() + RaycastInfo raycastInfo6; + test(mHeightFieldProxyShape->raycast(rayBottom, raycastInfo6)); + test(raycastInfo6.body == mHeightFieldBody); + test(raycastInfo6.proxyShape == mHeightFieldProxyShape); + test(approxEqual(raycastInfo6.hitFraction, float(0.375), epsilon)); + test(approxEqual(raycastInfo6.worldPoint.x, hitPoint2.x, epsilon)); + test(approxEqual(raycastInfo6.worldPoint.y, hitPoint2.y, epsilon)); + test(approxEqual(raycastInfo6.worldPoint.z, hitPoint2.z, epsilon)); + + Ray ray1(mLocalShapeToWorld * Vector3(0, 5, 0), mLocalShapeToWorld * Vector3(5, 7, 5)); + Ray ray2(mLocalShapeToWorld * Vector3(-4, -4, 7), mLocalShapeToWorld * Vector3(-4, 15, 7)); + Ray ray3(mLocalShapeToWorld * Vector3(23, 7, 2), mLocalShapeToWorld * Vector3(23, 1, 2)); + Ray ray4(mLocalShapeToWorld * Vector3(10, 3, 10), mLocalShapeToWorld * Vector3(22, 3, 31)); + Ray ray5(mLocalShapeToWorld * Vector3(4, 10, -1), mLocalShapeToWorld * Vector3(4, 3, -1)); + + Ray ray11(mLocalShapeToWorld * Vector3(3, 15, 0.5), mLocalShapeToWorld * Vector3(3, 1, 0.5)); + Ray ray12(mLocalShapeToWorld * Vector3(0, 45, 0), mLocalShapeToWorld * Vector3(0, -5, 0)); + Ray ray13(mLocalShapeToWorld * Vector3(1, 23, 2), mLocalShapeToWorld * Vector3(1, -23, 2)); + Ray ray14(mLocalShapeToWorld * Vector3(3, 8, 3), mLocalShapeToWorld * Vector3(3, 0, 3)); + + // ----- Test raycast miss ----- // + test(!mHeightFieldBody->raycast(ray1, raycastInfo3)); + test(!mHeightFieldProxyShape->raycast(ray1, raycastInfo3)); + mCallback.reset(); + mWorld->raycast(ray1, &mCallback); + test(!mCallback.isHit); + mCallback.reset(); + mWorld->raycast(Ray(ray1.point1, ray1.point2, float(0.01)), &mCallback); + test(!mCallback.isHit); + mCallback.reset(); + mWorld->raycast(Ray(ray1.point1, ray1.point2, float(100.0)), &mCallback); + test(!mCallback.isHit); + + test(!mHeightFieldBody->raycast(ray2, raycastInfo3)); + test(!mHeightFieldProxyShape->raycast(ray2, raycastInfo3)); + mCallback.reset(); + mWorld->raycast(ray2, &mCallback); + test(!mCallback.isHit); + + test(!mHeightFieldBody->raycast(ray3, raycastInfo3)); + test(!mHeightFieldProxyShape->raycast(ray3, raycastInfo3)); + mCallback.reset(); + mWorld->raycast(ray3, &mCallback); + test(!mCallback.isHit); + + test(!mHeightFieldBody->raycast(ray4, raycastInfo3)); + test(!mHeightFieldProxyShape->raycast(ray4, raycastInfo3)); + mCallback.reset(); + mWorld->raycast(ray4, &mCallback); + test(!mCallback.isHit); + + test(!mHeightFieldBody->raycast(ray5, raycastInfo3)); + test(!mHeightFieldProxyShape->raycast(ray5, raycastInfo3)); + mCallback.reset(); + mWorld->raycast(ray5, &mCallback); + test(!mCallback.isHit); + + mCallback.reset(); + + // ----- Test raycast hits ----- // + test(mHeightFieldBody->raycast(ray11, raycastInfo3)); + test(mHeightFieldProxyShape->raycast(ray11, raycastInfo3)); + mCallback.reset(); + mWorld->raycast(ray11, &mCallback); + test(mCallback.isHit); + mCallback.reset(); + mWorld->raycast(Ray(ray11.point1, ray11.point2, float(0.95)), &mCallback); + test(mCallback.isHit); + + test(mHeightFieldBody->raycast(ray12, raycastInfo3)); + test(mHeightFieldProxyShape->raycast(ray12, raycastInfo3)); + mCallback.reset(); + mWorld->raycast(ray12, &mCallback); + test(mCallback.isHit); + mCallback.reset(); + mWorld->raycast(Ray(ray12.point1, ray12.point2, float(0.87)), &mCallback); + test(mCallback.isHit); + + test(mHeightFieldBody->raycast(ray13, raycastInfo3)); + test(mHeightFieldProxyShape->raycast(ray13, raycastInfo3)); + mCallback.reset(); + mWorld->raycast(ray13, &mCallback); + test(mCallback.isHit); + mCallback.reset(); + mWorld->raycast(Ray(ray13.point1, ray13.point2, float(0.8)), &mCallback); + test(mCallback.isHit); + + test(mHeightFieldBody->raycast(ray14, raycastInfo3)); + test(mHeightFieldProxyShape->raycast(ray14, raycastInfo3)); + mCallback.reset(); + mWorld->raycast(ray14, &mCallback); + test(mCallback.isHit); + mCallback.reset(); + mWorld->raycast(Ray(ray14.point1, ray14.point2, float(0.8)), &mCallback); + test(mCallback.isHit); + } }; } diff --git a/test/tests/mathematics/TestMathematicsFunctions.h b/test/tests/mathematics/TestMathematicsFunctions.h index fe63c63..1016b63 100644 --- a/test/tests/mathematics/TestMathematicsFunctions.h +++ b/test/tests/mathematics/TestMathematicsFunctions.h @@ -1,26 +1,26 @@ /******************************************************************************** -* ReactPhysics3D physics library, http://www.reactphysics3d.com * -* Copyright (c) 2010-2016 Daniel Chappuis * +* 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: * -* * +* 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. * -* * +* 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_MATHEMATICS_FUNCTIONS_H @@ -39,92 +39,92 @@ namespace reactphysics3d { */ class TestMathematicsFunctions : public Test { - private : + private : - // ---------- Atributes ---------- // + // ---------- Atributes ---------- // - public : + public : - // ---------- Methods ---------- // + // ---------- Methods ---------- // - /// Constructor - TestMathematicsFunctions(const std::string& name): Test(name) {} + /// Constructor + TestMathematicsFunctions(const std::string& name): Test(name) {} - /// Run the tests - void run() { + /// Run the tests + void run() { - // Test approxEqual() - test(approxEqual(2, 7, 5.2)); - test(approxEqual(7, 2, 5.2)); - test(approxEqual(6, 6)); - test(!approxEqual(1, 5)); - test(!approxEqual(1, 5, 3)); - test(approxEqual(-2, -2)); - test(approxEqual(-2, -7, 6)); - test(!approxEqual(-2, 7, 2)); - test(approxEqual(-3, 8, 12)); - test(!approxEqual(-3, 8, 6)); + // Test approxEqual() + test(approxEqual(2, 7, 5.2)); + test(approxEqual(7, 2, 5.2)); + test(approxEqual(6, 6)); + test(!approxEqual(1, 5)); + test(!approxEqual(1, 5, 3)); + test(approxEqual(-2, -2)); + test(approxEqual(-2, -7, 6)); + test(!approxEqual(-2, 7, 2)); + test(approxEqual(-3, 8, 12)); + test(!approxEqual(-3, 8, 6)); - // Test clamp() - test(clamp(4, -3, 5) == 4); - test(clamp(-3, 1, 8) == 1); - test(clamp(45, -6, 7) == 7); - test(clamp(-5, -2, -1) == -2); - test(clamp(-5, -9, -1) == -5); - test(clamp(6, 6, 9) == 6); - test(clamp(9, 6, 9) == 9); - test(clamp(decimal(4), decimal(-3), decimal(5)) == decimal(4)); - test(clamp(decimal(-3), decimal(1), decimal(8)) == decimal(1)); - test(clamp(decimal(45), decimal(-6), decimal(7)) == decimal(7)); - test(clamp(decimal(-5), decimal(-2), decimal(-1)) == decimal(-2)); - test(clamp(decimal(-5), decimal(-9), decimal(-1)) == decimal(-5)); - test(clamp(decimal(6), decimal(6), decimal(9)) == decimal(6)); - test(clamp(decimal(9), decimal(6), decimal(9)) == decimal(9)); + // Test clamp() + test(clamp(4, -3, 5) == 4); + test(clamp(-3, 1, 8) == 1); + test(clamp(45, -6, 7) == 7); + test(clamp(-5, -2, -1) == -2); + test(clamp(-5, -9, -1) == -5); + test(clamp(6, 6, 9) == 6); + test(clamp(9, 6, 9) == 9); + test(clamp(float(4), float(-3), float(5)) == float(4)); + test(clamp(float(-3), float(1), float(8)) == float(1)); + test(clamp(float(45), float(-6), float(7)) == float(7)); + test(clamp(float(-5), float(-2), float(-1)) == float(-2)); + test(clamp(float(-5), float(-9), float(-1)) == float(-5)); + test(clamp(float(6), float(6), float(9)) == float(6)); + test(clamp(float(9), float(6), float(9)) == float(9)); - // Test min3() - test(min3(1, 5, 7) == 1); - test(min3(-4, 2, 4) == -4); - test(min3(-1, -5, -7) == -7); - test(min3(13, 5, 47) == 5); - test(min3(4, 4, 4) == 4); + // Test min3() + test(min3(1, 5, 7) == 1); + test(min3(-4, 2, 4) == -4); + test(min3(-1, -5, -7) == -7); + test(min3(13, 5, 47) == 5); + test(min3(4, 4, 4) == 4); - // Test max3() - test(max3(1, 5, 7) == 7); - test(max3(-4, 2, 4) == 4); - test(max3(-1, -5, -7) == -1); - test(max3(13, 5, 47) == 47); - test(max3(4, 4, 4) == 4); + // Test max3() + test(max3(1, 5, 7) == 7); + test(max3(-4, 2, 4) == 4); + test(max3(-1, -5, -7) == -1); + test(max3(13, 5, 47) == 47); + test(max3(4, 4, 4) == 4); - // Test sameSign() - test(sameSign(4, 53)); - test(sameSign(-4, -8)); - test(!sameSign(4, -7)); - test(!sameSign(-4, 53)); + // Test sameSign() + test(sameSign(4, 53)); + test(sameSign(-4, -8)); + test(!sameSign(4, -7)); + test(!sameSign(-4, 53)); - // Test computeBarycentricCoordinatesInTriangle() - Vector3 a(0, 0, 0); - Vector3 b(5, 0, 0); - Vector3 c(0, 0, 5); - Vector3 testPoint(4, 0, 1); - decimal u,v,w; - computeBarycentricCoordinatesInTriangle(a, b, c, a, u, v, w); - test(approxEqual(u, 1.0, 0.000001)); - test(approxEqual(v, 0.0, 0.000001)); - test(approxEqual(w, 0.0, 0.000001)); - computeBarycentricCoordinatesInTriangle(a, b, c, b, u, v, w); - test(approxEqual(u, 0.0, 0.000001)); - test(approxEqual(v, 1.0, 0.000001)); - test(approxEqual(w, 0.0, 0.000001)); - computeBarycentricCoordinatesInTriangle(a, b, c, c, u, v, w); - test(approxEqual(u, 0.0, 0.000001)); - test(approxEqual(v, 0.0, 0.000001)); - test(approxEqual(w, 1.0, 0.000001)); + // Test computeBarycentricCoordinatesInTriangle() + Vector3 a(0, 0, 0); + Vector3 b(5, 0, 0); + Vector3 c(0, 0, 5); + Vector3 testPoint(4, 0, 1); + float u,v,w; + computeBarycentricCoordinatesInTriangle(a, b, c, a, u, v, w); + test(approxEqual(u, 1.0, 0.000001)); + test(approxEqual(v, 0.0, 0.000001)); + test(approxEqual(w, 0.0, 0.000001)); + computeBarycentricCoordinatesInTriangle(a, b, c, b, u, v, w); + test(approxEqual(u, 0.0, 0.000001)); + test(approxEqual(v, 1.0, 0.000001)); + test(approxEqual(w, 0.0, 0.000001)); + computeBarycentricCoordinatesInTriangle(a, b, c, c, u, v, w); + test(approxEqual(u, 0.0, 0.000001)); + test(approxEqual(v, 0.0, 0.000001)); + test(approxEqual(w, 1.0, 0.000001)); - computeBarycentricCoordinatesInTriangle(a, b, c, testPoint, u, v, w); - test(approxEqual(u + v + w, 1.0, 0.000001)); - } + computeBarycentricCoordinatesInTriangle(a, b, c, testPoint, u, v, w); + test(approxEqual(u + v + w, 1.0, 0.000001)); + } }; diff --git a/test/tests/mathematics/TestMatrix2x2.h b/test/tests/mathematics/TestMatrix2x2.h index b6f954d..a61f766 100644 --- a/test/tests/mathematics/TestMatrix2x2.h +++ b/test/tests/mathematics/TestMatrix2x2.h @@ -1,26 +1,26 @@ /******************************************************************************** -* ReactPhysics3D physics library, http://www.reactphysics3d.com * -* Copyright (c) 2010-2016 Daniel Chappuis * +* 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: * -* * +* 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. * -* * +* 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_MATRIX2X2_H @@ -39,210 +39,210 @@ namespace reactphysics3d { */ class TestMatrix2x2 : public Test { - private : + private : - // ---------- Atributes ---------- // + // ---------- Atributes ---------- // - /// Identity transform - Matrix2x2 mIdentity; + /// Identity transform + Matrix2x2 mIdentity; - /// First example matrix - Matrix2x2 mMatrix1; + /// First example matrix + Matrix2x2 mMatrix1; - public : + public : - // ---------- Methods ---------- // + // ---------- Methods ---------- // - /// Constructor - TestMatrix2x2(const std::string& name) - : Test(name), mIdentity(Matrix2x2::identity()), mMatrix1(2, 24, -4, 5) { + /// Constructor + TestMatrix2x2(const std::string& name) + : Test(name), mIdentity(Matrix2x2::identity()), mMatrix1(2, 24, -4, 5) { - } + } - /// Run the tests - void run() { - testConstructors(); - testGetSet(); - testIdentity(); - testZero(); - testOthersMethods(); - testOperators(); - } + /// Run the tests + void run() { + testConstructors(); + testGetSet(); + testIdentity(); + testZero(); + testOthersMethods(); + testOperators(); + } - /// Test the constructors - void testConstructors() { + /// Test the constructors + void testConstructors() { - Matrix2x2 test1(5.0); - Matrix2x2 test2(2, 3, 4, 5); - Matrix2x2 test3(mMatrix1); + Matrix2x2 test1(5.0); + Matrix2x2 test2(2, 3, 4, 5); + Matrix2x2 test3(mMatrix1); - test(test1[0][0] == 5); - test(test1[0][1] == 5); - test(test1[1][0] == 5); - test(test1[1][1] == 5); + test(test1[0][0] == 5); + test(test1[0][1] == 5); + test(test1[1][0] == 5); + test(test1[1][1] == 5); - test(test2[0][0] == 2); - test(test2[0][1] == 3); - test(test2[1][0] == 4); - test(test2[1][1] == 5); + test(test2[0][0] == 2); + test(test2[0][1] == 3); + test(test2[1][0] == 4); + test(test2[1][1] == 5); - test(test3 == mMatrix1); - } + test(test3 == mMatrix1); + } - /// Test the getter and setter methods - void testGetSet() { + /// Test the getter and setter methods + void testGetSet() { - // Test method to set all the values - Matrix2x2 test2; - test2.setAllValues(2, 24, -4, 5); - test(test2 == mMatrix1); + // Test method to set all the values + Matrix2x2 test2; + test2.setAllValues(2, 24, -4, 5); + test(test2 == mMatrix1); - // Test method to set to zero - test2.setToZero(); - test(test2 == Matrix2x2(0, 0, 0, 0)); + // Test method to set to zero + test2.setToZero(); + test(test2 == Matrix2x2(0, 0, 0, 0)); - // Test method that returns a column - Vector2 column1 = mMatrix1.getColumn(0); - Vector2 column2 = mMatrix1.getColumn(1); - test(column1 == Vector2(2, -4)); - test(column2 == Vector2(24, 5)); + // Test method that returns a column + Vector2 column1 = mMatrix1.getColumn(0); + Vector2 column2 = mMatrix1.getColumn(1); + test(column1 == Vector2(2, -4)); + test(column2 == Vector2(24, 5)); - // Test method that returns a row - Vector2 row1 = mMatrix1.getRow(0); - Vector2 row2 = mMatrix1.getRow(1); - test(row1 == Vector2(2, 24)); - test(row2 == Vector2(-4, 5)); - } + // Test method that returns a row + Vector2 row1 = mMatrix1.getRow(0); + Vector2 row2 = mMatrix1.getRow(1); + test(row1 == Vector2(2, 24)); + test(row2 == Vector2(-4, 5)); + } - /// Test the identity methods - void testIdentity() { + /// Test the identity methods + void testIdentity() { - Matrix2x2 identity = Matrix2x2::identity(); - Matrix2x2 test1; - test1.setToIdentity(); + Matrix2x2 identity = Matrix2x2::identity(); + Matrix2x2 test1; + test1.setToIdentity(); - test(identity[0][0] == 1); - test(identity[0][1] == 0); - test(identity[1][0] == 0); - test(identity[1][1] == 1); + test(identity[0][0] == 1); + test(identity[0][1] == 0); + test(identity[1][0] == 0); + test(identity[1][1] == 1); - test(test1 == Matrix2x2::identity()); - } + test(test1 == Matrix2x2::identity()); + } - /// Test the zero method - void testZero() { + /// Test the zero method + void testZero() { - Matrix2x2 zero = Matrix2x2::zero(); + Matrix2x2 zero = Matrix2x2::zero(); - test(zero[0][0] == 0); - test(zero[0][1] == 0); - test(zero[1][0] == 0); - test(zero[1][1] == 0); - } + test(zero[0][0] == 0); + test(zero[0][1] == 0); + test(zero[1][0] == 0); + test(zero[1][1] == 0); + } - /// Test others methods - void testOthersMethods() { + /// Test others methods + void testOthersMethods() { - // Test transpose - Matrix2x2 transpose = mMatrix1.getTranspose(); - test(transpose == Matrix2x2(2, -4, 24, 5)); + // Test transpose + Matrix2x2 transpose = mMatrix1.getTranspose(); + test(transpose == Matrix2x2(2, -4, 24, 5)); - // Test trace - test(mMatrix1.getTrace() ==7); - test(Matrix2x2::identity().getTrace() == 2); + // Test trace + test(mMatrix1.getTrace() ==7); + test(Matrix2x2::identity().getTrace() == 2); - // Test determinant - Matrix2x2 matrix(-24, 64, 253, -35); - test(mMatrix1.getDeterminant() == 106); - test(matrix.getDeterminant() == -15352); - test(mIdentity.getDeterminant() == 1); + // Test determinant + Matrix2x2 matrix(-24, 64, 253, -35); + test(mMatrix1.getDeterminant() == 106); + test(matrix.getDeterminant() == -15352); + test(mIdentity.getDeterminant() == 1); - // Test inverse - Matrix2x2 matrix2(1, 2, 3, 4); - Matrix2x2 inverseMatrix = matrix2.getInverse(); - test(approxEqual(inverseMatrix[0][0], decimal(-2), decimal(10e-6))); - test(approxEqual(inverseMatrix[0][1], decimal(1), decimal(10e-6))); - test(approxEqual(inverseMatrix[1][0], decimal(1.5), decimal(10e-6))); - test(approxEqual(inverseMatrix[1][1], decimal(-0.5), decimal(10e-6))); - Matrix2x2 inverseMatrix1 = mMatrix1.getInverse(); - test(approxEqual(inverseMatrix1[0][0], decimal(0.047169811), decimal(10e-6))); - test(approxEqual(inverseMatrix1[0][1], decimal(-0.226415094), decimal(10e-6))); - test(approxEqual(inverseMatrix1[1][0], decimal(0.037735849), decimal(10e-6))); - test(approxEqual(inverseMatrix1[1][1], decimal(0.018867925), decimal(10e-6))); + // Test inverse + Matrix2x2 matrix2(1, 2, 3, 4); + Matrix2x2 inverseMatrix = matrix2.getInverse(); + test(approxEqual(inverseMatrix[0][0], float(-2), float(10e-6))); + test(approxEqual(inverseMatrix[0][1], float(1), float(10e-6))); + test(approxEqual(inverseMatrix[1][0], float(1.5), float(10e-6))); + test(approxEqual(inverseMatrix[1][1], float(-0.5), float(10e-6))); + Matrix2x2 inverseMatrix1 = mMatrix1.getInverse(); + test(approxEqual(inverseMatrix1[0][0], float(0.047169811), float(10e-6))); + test(approxEqual(inverseMatrix1[0][1], float(-0.226415094), float(10e-6))); + test(approxEqual(inverseMatrix1[1][0], float(0.037735849), float(10e-6))); + test(approxEqual(inverseMatrix1[1][1], float(0.018867925), float(10e-6))); - // Test absolute matrix - Matrix2x2 matrix3(-2, -3, -4, -5); - test(matrix.getAbsoluteMatrix() == Matrix2x2(24, 64, 253, 35)); - Matrix2x2 absoluteMatrix = matrix3.getAbsoluteMatrix(); - test(absoluteMatrix == Matrix2x2(2, 3, 4, 5)); - } + // Test absolute matrix + Matrix2x2 matrix3(-2, -3, -4, -5); + test(matrix.getAbsoluteMatrix() == Matrix2x2(24, 64, 253, 35)); + Matrix2x2 absoluteMatrix = matrix3.getAbsoluteMatrix(); + test(absoluteMatrix == Matrix2x2(2, 3, 4, 5)); + } - /// Test the operators - void testOperators() { + /// Test the operators + void testOperators() { - // Test addition - Matrix2x2 matrix1(2, 3, 4, 5); - Matrix2x2 matrix2(-2, 3, -5, 10); - Matrix2x2 addition1 = matrix1 + matrix2; - Matrix2x2 addition2(matrix1); - addition2 += matrix2; - test(addition1 == Matrix2x2(0, 6, -1, 15)); - test(addition2 == Matrix2x2(0, 6, -1, 15)); + // Test addition + Matrix2x2 matrix1(2, 3, 4, 5); + Matrix2x2 matrix2(-2, 3, -5, 10); + Matrix2x2 addition1 = matrix1 + matrix2; + Matrix2x2 addition2(matrix1); + addition2 += matrix2; + test(addition1 == Matrix2x2(0, 6, -1, 15)); + test(addition2 == Matrix2x2(0, 6, -1, 15)); - // Test substraction - Matrix2x2 substraction1 = matrix1 - matrix2; - Matrix2x2 substraction2(matrix1); - substraction2 -= matrix2; - test(substraction1 == Matrix2x2(4, 0, 9, -5)); - test(substraction2 == Matrix2x2(4, 0, 9, -5)); + // Test substraction + Matrix2x2 substraction1 = matrix1 - matrix2; + Matrix2x2 substraction2(matrix1); + substraction2 -= matrix2; + test(substraction1 == Matrix2x2(4, 0, 9, -5)); + test(substraction2 == Matrix2x2(4, 0, 9, -5)); - // Test negative operator - Matrix2x2 negative = -matrix1; - test(negative == Matrix2x2(-2, -3, -4, -5)); + // Test negative operator + Matrix2x2 negative = -matrix1; + test(negative == Matrix2x2(-2, -3, -4, -5)); - // Test multiplication with a number - Matrix2x2 multiplication1 = 3 * matrix1; - Matrix2x2 multiplication2 = matrix1 * 3; - Matrix2x2 multiplication3(matrix1); - multiplication3 *= 3; - test(multiplication1 == Matrix2x2(6, 9, 12, 15)); - test(multiplication2 == Matrix2x2(6, 9, 12, 15)); - test(multiplication3 == Matrix2x2(6, 9, 12, 15)); + // Test multiplication with a number + Matrix2x2 multiplication1 = 3 * matrix1; + Matrix2x2 multiplication2 = matrix1 * 3; + Matrix2x2 multiplication3(matrix1); + multiplication3 *= 3; + test(multiplication1 == Matrix2x2(6, 9, 12, 15)); + test(multiplication2 == Matrix2x2(6, 9, 12, 15)); + test(multiplication3 == Matrix2x2(6, 9, 12, 15)); - // Test multiplication with a matrix - Matrix2x2 multiplication4 = matrix1 * matrix2; - Matrix2x2 multiplication5 = matrix2 * matrix1; - test(multiplication4 == Matrix2x2(-19, 36, -33, 62)); - test(multiplication5 == Matrix2x2(8, 9, 30, 35)); + // Test multiplication with a matrix + Matrix2x2 multiplication4 = matrix1 * matrix2; + Matrix2x2 multiplication5 = matrix2 * matrix1; + test(multiplication4 == Matrix2x2(-19, 36, -33, 62)); + test(multiplication5 == Matrix2x2(8, 9, 30, 35)); - // Test multiplication with a vector - Vector2 vector1(3, -32); - Vector2 vector2(-31, -422); - Vector2 test1 = matrix1 * vector1; - Vector2 test2 = matrix2 * vector2; - test(test1 == Vector2(-90, -148)); - test(test2 == Vector2(-1204, -4065)); + // Test multiplication with a vector + Vector2 vector1(3, -32); + Vector2 vector2(-31, -422); + Vector2 test1 = matrix1 * vector1; + Vector2 test2 = matrix2 * vector2; + test(test1 == Vector2(-90, -148)); + test(test2 == Vector2(-1204, -4065)); - // Test equality operators - test(Matrix2x2(34, 38, 43, 64) == - Matrix2x2(34, 38, 43, 64)); - test(Matrix2x2(34, 64, 43, 7) != - Matrix2x2(34, 38, 43, 64)); + // Test equality operators + test(Matrix2x2(34, 38, 43, 64) == + Matrix2x2(34, 38, 43, 64)); + test(Matrix2x2(34, 64, 43, 7) != + Matrix2x2(34, 38, 43, 64)); - // Test operator to read a value - test(mMatrix1[0][0] == 2); - test(mMatrix1[0][1] == 24); - test(mMatrix1[1][0] == -4); - test(mMatrix1[1][1] == 5); + // Test operator to read a value + test(mMatrix1[0][0] == 2); + test(mMatrix1[0][1] == 24); + test(mMatrix1[1][0] == -4); + test(mMatrix1[1][1] == 5); - // Test operator to set a value - Matrix2x2 test3; - test3[0][0] = 2; - test3[0][1] = 24; - test3[1][0] = -4; - test3[1][1] = 5; - test(test3 == mMatrix1); - } + // Test operator to set a value + Matrix2x2 test3; + test3[0][0] = 2; + test3[0][1] = 24; + test3[1][0] = -4; + test3[1][1] = 5; + test(test3 == mMatrix1); + } }; diff --git a/test/tests/mathematics/TestMatrix3x3.h b/test/tests/mathematics/TestMatrix3x3.h index fc5ad20..5d0a002 100644 --- a/test/tests/mathematics/TestMatrix3x3.h +++ b/test/tests/mathematics/TestMatrix3x3.h @@ -1,26 +1,26 @@ /******************************************************************************** -* ReactPhysics3D physics library, http://www.reactphysics3d.com * -* Copyright (c) 2010-2016 Daniel Chappuis * +* 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: * -* * +* 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. * -* * +* 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_MATRIX3X3_H @@ -39,263 +39,263 @@ namespace reactphysics3d { */ class TestMatrix3x3 : public Test { - private : + private : - // ---------- Atributes ---------- // + // ---------- Atributes ---------- // - /// Identity transform - Matrix3x3 mIdentity; + /// Identity transform + Matrix3x3 mIdentity; - /// First example matrix - Matrix3x3 mMatrix1; + /// First example matrix + Matrix3x3 mMatrix1; - public : + public : - // ---------- Methods ---------- // + // ---------- Methods ---------- // - /// Constructor - TestMatrix3x3(const std::string& name) - : Test(name), mIdentity(Matrix3x3::identity()), - mMatrix1(2, 24, 4, 5, -6, 234, -15, 11, 66) { + /// Constructor + TestMatrix3x3(const std::string& name) + : Test(name), mIdentity(Matrix3x3::identity()), + mMatrix1(2, 24, 4, 5, -6, 234, -15, 11, 66) { - } + } - /// Run the tests - void run() { - testConstructors(); - testGetSet(); - testIdentity(); - testZero(); - testOthersMethods(); - testOperators(); - } + /// Run the tests + void run() { + testConstructors(); + testGetSet(); + testIdentity(); + testZero(); + testOthersMethods(); + testOperators(); + } - /// Test the constructors - void testConstructors() { + /// Test the constructors + void testConstructors() { - Matrix3x3 test1(5.0); - Matrix3x3 test2(2, 3, 4, 5, 6, 7, 8, 9, 10); - Matrix3x3 test3(mMatrix1); - test(test1[0][0] == 5); - test(test1[0][1] == 5); - test(test1[0][2] == 5); - test(test1[1][0] == 5); - test(test1[1][1] == 5); - test(test1[1][2] == 5); - test(test1[2][0] == 5); - test(test1[2][1] == 5); - test(test1[2][2] == 5); + Matrix3x3 test1(5.0); + Matrix3x3 test2(2, 3, 4, 5, 6, 7, 8, 9, 10); + Matrix3x3 test3(mMatrix1); + test(test1[0][0] == 5); + test(test1[0][1] == 5); + test(test1[0][2] == 5); + test(test1[1][0] == 5); + test(test1[1][1] == 5); + test(test1[1][2] == 5); + test(test1[2][0] == 5); + test(test1[2][1] == 5); + test(test1[2][2] == 5); - test(test2[0][0] == 2); - test(test2[0][1] == 3); - test(test2[0][2] == 4); - test(test2[1][0] == 5); - test(test2[1][1] == 6); - test(test2[1][2] == 7); - test(test2[2][0] == 8); - test(test2[2][1] == 9); - test(test2[2][2] == 10); + test(test2[0][0] == 2); + test(test2[0][1] == 3); + test(test2[0][2] == 4); + test(test2[1][0] == 5); + test(test2[1][1] == 6); + test(test2[1][2] == 7); + test(test2[2][0] == 8); + test(test2[2][1] == 9); + test(test2[2][2] == 10); - test(test3 == mMatrix1); - } + test(test3 == mMatrix1); + } - /// Test the getter and setter methods - void testGetSet() { + /// Test the getter and setter methods + void testGetSet() { - // Test method to set all the values - Matrix3x3 test2; - test2.setAllValues(2, 24, 4, 5, -6, 234, -15, 11, 66); - test(test2 == mMatrix1); + // Test method to set all the values + Matrix3x3 test2; + test2.setAllValues(2, 24, 4, 5, -6, 234, -15, 11, 66); + test(test2 == mMatrix1); - // Test method to set to zero - test2.setToZero(); - test(test2 == Matrix3x3(0, 0, 0, 0, 0, 0, 0, 0, 0)); + // Test method to set to zero + test2.setToZero(); + test(test2 == Matrix3x3(0, 0, 0, 0, 0, 0, 0, 0, 0)); - // Test method that returns a column - Vector3 column1 = mMatrix1.getColumn(0); - Vector3 column2 = mMatrix1.getColumn(1); - Vector3 column3 = mMatrix1.getColumn(2); - test(column1 == Vector3(2, 5, -15)); - test(column2 == Vector3(24, -6, 11)); - test(column3 == Vector3(4, 234, 66)); + // Test method that returns a column + Vector3 column1 = mMatrix1.getColumn(0); + Vector3 column2 = mMatrix1.getColumn(1); + Vector3 column3 = mMatrix1.getColumn(2); + test(column1 == Vector3(2, 5, -15)); + test(column2 == Vector3(24, -6, 11)); + test(column3 == Vector3(4, 234, 66)); - // Test method that returns a row - Vector3 row1 = mMatrix1.getRow(0); - Vector3 row2 = mMatrix1.getRow(1); - Vector3 row3 = mMatrix1.getRow(2); - test(row1 == Vector3(2, 24, 4)); - test(row2 == Vector3(5, -6, 234)); - test(row3 == Vector3(-15, 11, 66)); - } + // Test method that returns a row + Vector3 row1 = mMatrix1.getRow(0); + Vector3 row2 = mMatrix1.getRow(1); + Vector3 row3 = mMatrix1.getRow(2); + test(row1 == Vector3(2, 24, 4)); + test(row2 == Vector3(5, -6, 234)); + test(row3 == Vector3(-15, 11, 66)); + } - /// Test the identity methods - void testIdentity() { + /// Test the identity methods + void testIdentity() { - Matrix3x3 identity = Matrix3x3::identity(); - Matrix3x3 test1; - test1.setToIdentity(); + Matrix3x3 identity = Matrix3x3::identity(); + Matrix3x3 test1; + test1.setToIdentity(); - test(identity[0][0] == 1); - test(identity[0][1] == 0); - test(identity[0][2] == 0); - test(identity[1][0] == 0); - test(identity[1][1] == 1); - test(identity[1][2] == 0); - test(identity[2][0] == 0); - test(identity[2][1] == 0); - test(identity[2][2] == 1); + test(identity[0][0] == 1); + test(identity[0][1] == 0); + test(identity[0][2] == 0); + test(identity[1][0] == 0); + test(identity[1][1] == 1); + test(identity[1][2] == 0); + test(identity[2][0] == 0); + test(identity[2][1] == 0); + test(identity[2][2] == 1); - test(test1 == Matrix3x3::identity()); - } + test(test1 == Matrix3x3::identity()); + } - /// Test the zero method - void testZero() { + /// Test the zero method + void testZero() { - Matrix3x3 zero = Matrix3x3::zero(); + Matrix3x3 zero = Matrix3x3::zero(); - test(zero[0][0] == 0); - test(zero[0][1] == 0); - test(zero[0][2] == 0); - test(zero[1][0] == 0); - test(zero[1][1] == 0); - test(zero[1][2] == 0); - test(zero[2][0] == 0); - test(zero[2][1] == 0); - test(zero[2][2] == 0); - } + test(zero[0][0] == 0); + test(zero[0][1] == 0); + test(zero[0][2] == 0); + test(zero[1][0] == 0); + test(zero[1][1] == 0); + test(zero[1][2] == 0); + test(zero[2][0] == 0); + test(zero[2][1] == 0); + test(zero[2][2] == 0); + } - /// Test others methods - void testOthersMethods() { + /// Test others methods + void testOthersMethods() { - // Test transpose - Matrix3x3 transpose = mMatrix1.getTranspose(); - test(transpose == Matrix3x3(2, 5, -15, 24, -6, 11, 4, 234, 66)); + // Test transpose + Matrix3x3 transpose = mMatrix1.getTranspose(); + test(transpose == Matrix3x3(2, 5, -15, 24, -6, 11, 4, 234, 66)); - // Test trace - test(mMatrix1.getTrace() == 62); - test(Matrix3x3::identity().getTrace() == 3); + // Test trace + test(mMatrix1.getTrace() == 62); + test(Matrix3x3::identity().getTrace() == 3); - // Test determinant - Matrix3x3 matrix(-24, 64, 253, -35, 52, 72, 21, -35, -363); - test(mMatrix1.getDeterminant() == -98240); - test(matrix.getDeterminant() == -290159); - test(mIdentity.getDeterminant() == 1); + // Test determinant + Matrix3x3 matrix(-24, 64, 253, -35, 52, 72, 21, -35, -363); + test(mMatrix1.getDeterminant() == -98240); + test(matrix.getDeterminant() == -290159); + test(mIdentity.getDeterminant() == 1); - // Test inverse - Matrix3x3 inverseMatrix = matrix.getInverse(); - test(approxEqual(inverseMatrix[0][0], decimal(0.056369), decimal(10e-6))); - test(approxEqual(inverseMatrix[0][1], decimal(-0.049549), decimal(10e-6))); - test(approxEqual(inverseMatrix[0][2], decimal(0.029460), decimal(10e-6))); - test(approxEqual(inverseMatrix[1][0], decimal(0.038575), decimal(10e-6))); - test(approxEqual(inverseMatrix[1][1], decimal(-0.011714), decimal(10e-6))); - test(approxEqual(inverseMatrix[1][2], decimal(0.024562), decimal(10e-6))); - test(approxEqual(inverseMatrix[2][0], decimal(-0.000458), decimal(10e-6))); - test(approxEqual(inverseMatrix[2][1], decimal(-0.001737), decimal(10e-6))); - test(approxEqual(inverseMatrix[2][2], decimal(-0.003419), decimal(10e-6))); - Matrix3x3 inverseMatrix1 = mMatrix1.getInverse(); - test(approxEqual(inverseMatrix1[0][0], decimal(0.030232), decimal(10e-6))); - test(approxEqual(inverseMatrix1[0][1], decimal(0.015676), decimal(10e-6))); - test(approxEqual(inverseMatrix1[0][2], decimal(-0.057410), decimal(10e-6))); - test(approxEqual(inverseMatrix1[1][0], decimal(0.039088), decimal(10e-6))); - test(approxEqual(inverseMatrix1[1][1], decimal(-0.001954), decimal(10e-6))); - test(approxEqual(inverseMatrix1[1][2], decimal(0.004560), decimal(10e-6))); - test(approxEqual(inverseMatrix1[2][0], decimal(0.000356), decimal(10e-6))); - test(approxEqual(inverseMatrix1[2][1], decimal(0.003888), decimal(10e-6))); - test(approxEqual(inverseMatrix1[2][2], decimal(0.001344), decimal(10e-6))); + // Test inverse + Matrix3x3 inverseMatrix = matrix.getInverse(); + test(approxEqual(inverseMatrix[0][0], float(0.056369), float(10e-6))); + test(approxEqual(inverseMatrix[0][1], float(-0.049549), float(10e-6))); + test(approxEqual(inverseMatrix[0][2], float(0.029460), float(10e-6))); + test(approxEqual(inverseMatrix[1][0], float(0.038575), float(10e-6))); + test(approxEqual(inverseMatrix[1][1], float(-0.011714), float(10e-6))); + test(approxEqual(inverseMatrix[1][2], float(0.024562), float(10e-6))); + test(approxEqual(inverseMatrix[2][0], float(-0.000458), float(10e-6))); + test(approxEqual(inverseMatrix[2][1], float(-0.001737), float(10e-6))); + test(approxEqual(inverseMatrix[2][2], float(-0.003419), float(10e-6))); + Matrix3x3 inverseMatrix1 = mMatrix1.getInverse(); + test(approxEqual(inverseMatrix1[0][0], float(0.030232), float(10e-6))); + test(approxEqual(inverseMatrix1[0][1], float(0.015676), float(10e-6))); + test(approxEqual(inverseMatrix1[0][2], float(-0.057410), float(10e-6))); + test(approxEqual(inverseMatrix1[1][0], float(0.039088), float(10e-6))); + test(approxEqual(inverseMatrix1[1][1], float(-0.001954), float(10e-6))); + test(approxEqual(inverseMatrix1[1][2], float(0.004560), float(10e-6))); + test(approxEqual(inverseMatrix1[2][0], float(0.000356), float(10e-6))); + test(approxEqual(inverseMatrix1[2][1], float(0.003888), float(10e-6))); + test(approxEqual(inverseMatrix1[2][2], float(0.001344), float(10e-6))); - // Test absolute matrix - Matrix3x3 matrix2(-2, -3, -4, -5, -6, -7, -8, -9, -10); - test(matrix.getAbsoluteMatrix() == Matrix3x3(24, 64, 253, 35, 52, 72, 21, 35, 363)); - Matrix3x3 absoluteMatrix = matrix2.getAbsoluteMatrix(); - test(absoluteMatrix == Matrix3x3(2, 3, 4, 5, 6, 7, 8, 9, 10)); + // Test absolute matrix + Matrix3x3 matrix2(-2, -3, -4, -5, -6, -7, -8, -9, -10); + test(matrix.getAbsoluteMatrix() == Matrix3x3(24, 64, 253, 35, 52, 72, 21, 35, 363)); + Matrix3x3 absoluteMatrix = matrix2.getAbsoluteMatrix(); + test(absoluteMatrix == Matrix3x3(2, 3, 4, 5, 6, 7, 8, 9, 10)); - // Test method that computes skew-symmetric matrix for cross product - Vector3 vector1(3, -5, 6); - Vector3 vector2(73, 42, 26); - Matrix3x3 skewMatrix = Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(vector1); - test(skewMatrix == Matrix3x3(0, -6, -5, 6, 0, -3, 5, 3, 0)); - Vector3 crossProduct1 = vector1.cross(vector2); - Vector3 crossProduct2 = skewMatrix * vector2; - test(crossProduct1 == crossProduct2); - } + // Test method that computes skew-symmetric matrix for cross product + Vector3 vector1(3, -5, 6); + Vector3 vector2(73, 42, 26); + Matrix3x3 skewMatrix = Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(vector1); + test(skewMatrix == Matrix3x3(0, -6, -5, 6, 0, -3, 5, 3, 0)); + Vector3 crossProduct1 = vector1.cross(vector2); + Vector3 crossProduct2 = skewMatrix * vector2; + test(crossProduct1 == crossProduct2); + } - /// Test the operators - void testOperators() { + /// Test the operators + void testOperators() { - // Test addition - Matrix3x3 matrix1(2, 3, 4, 5, 6, 7, 8, 9, 10); - Matrix3x3 matrix2(-2, 3, -5, 10, 4, 7, 2, 5, 8); - Matrix3x3 addition1 = matrix1 + matrix2; - Matrix3x3 addition2(matrix1); - addition2 += matrix2; - test(addition1 == Matrix3x3(0, 6, -1, 15, 10, 14, 10, 14, 18)); - test(addition2 == Matrix3x3(0, 6, -1, 15, 10, 14, 10, 14, 18)); + // Test addition + Matrix3x3 matrix1(2, 3, 4, 5, 6, 7, 8, 9, 10); + Matrix3x3 matrix2(-2, 3, -5, 10, 4, 7, 2, 5, 8); + Matrix3x3 addition1 = matrix1 + matrix2; + Matrix3x3 addition2(matrix1); + addition2 += matrix2; + test(addition1 == Matrix3x3(0, 6, -1, 15, 10, 14, 10, 14, 18)); + test(addition2 == Matrix3x3(0, 6, -1, 15, 10, 14, 10, 14, 18)); - // Test substraction - Matrix3x3 substraction1 = matrix1 - matrix2; - Matrix3x3 substraction2(matrix1); - substraction2 -= matrix2; - test(substraction1 == Matrix3x3(4, 0, 9, -5, 2, 0, 6, 4, 2)); - test(substraction2 == Matrix3x3(4, 0, 9, -5, 2, 0, 6, 4, 2)); + // Test substraction + Matrix3x3 substraction1 = matrix1 - matrix2; + Matrix3x3 substraction2(matrix1); + substraction2 -= matrix2; + test(substraction1 == Matrix3x3(4, 0, 9, -5, 2, 0, 6, 4, 2)); + test(substraction2 == Matrix3x3(4, 0, 9, -5, 2, 0, 6, 4, 2)); - // Test negative operator - Matrix3x3 negative = -matrix1; - test(negative == Matrix3x3(-2, -3, -4, -5, -6, -7, -8, -9, -10)); + // Test negative operator + Matrix3x3 negative = -matrix1; + test(negative == Matrix3x3(-2, -3, -4, -5, -6, -7, -8, -9, -10)); - // Test multiplication with a number - Matrix3x3 multiplication1 = 3 * matrix1; - Matrix3x3 multiplication2 = matrix1 * 3; - Matrix3x3 multiplication3(matrix1); - multiplication3 *= 3; - test(multiplication1 == Matrix3x3(6, 9, 12, 15, 18, 21, 24, 27, 30)); - test(multiplication2 == Matrix3x3(6, 9, 12, 15, 18, 21, 24, 27, 30)); - test(multiplication3 == Matrix3x3(6, 9, 12, 15, 18, 21, 24, 27, 30)); + // Test multiplication with a number + Matrix3x3 multiplication1 = 3 * matrix1; + Matrix3x3 multiplication2 = matrix1 * 3; + Matrix3x3 multiplication3(matrix1); + multiplication3 *= 3; + test(multiplication1 == Matrix3x3(6, 9, 12, 15, 18, 21, 24, 27, 30)); + test(multiplication2 == Matrix3x3(6, 9, 12, 15, 18, 21, 24, 27, 30)); + test(multiplication3 == Matrix3x3(6, 9, 12, 15, 18, 21, 24, 27, 30)); - // Test multiplication with a matrix - Matrix3x3 multiplication4 = matrix1 * matrix2; - Matrix3x3 multiplication5 = matrix2 * matrix1; - test(multiplication4 == Matrix3x3(34, 38, 43, 64, 74, 73, 94, 110, 103)); - test(multiplication5 == Matrix3x3(-29, -33, -37, 96, 117, 138, 93, 108, 123)); + // Test multiplication with a matrix + Matrix3x3 multiplication4 = matrix1 * matrix2; + Matrix3x3 multiplication5 = matrix2 * matrix1; + test(multiplication4 == Matrix3x3(34, 38, 43, 64, 74, 73, 94, 110, 103)); + test(multiplication5 == Matrix3x3(-29, -33, -37, 96, 117, 138, 93, 108, 123)); - // Test multiplication with a vector - Vector3 vector1(3, -32, 59); - Vector3 vector2(-31, -422, 34); - Vector3 test1 = matrix1 * vector1; - Vector3 test2 = matrix2 * vector2; - test(test1 == Vector3(146, 236, 326)); - test(test2 == Vector3(-1374, -1760, -1900)); + // Test multiplication with a vector + Vector3 vector1(3, -32, 59); + Vector3 vector2(-31, -422, 34); + Vector3 test1 = matrix1 * vector1; + Vector3 test2 = matrix2 * vector2; + test(test1 == Vector3(146, 236, 326)); + test(test2 == Vector3(-1374, -1760, -1900)); - // Test equality operators - test(Matrix3x3(34, 38, 43, 64, 74, 73, 94, 110, 103) == - Matrix3x3(34, 38, 43, 64, 74, 73, 94, 110, 103)); - test(Matrix3x3(34, 64, 43, 7, -1, 73, 94, 110, 103) != - Matrix3x3(34, 38, 43, 64, 74, 73, 94, 110, 103)); + // Test equality operators + test(Matrix3x3(34, 38, 43, 64, 74, 73, 94, 110, 103) == + Matrix3x3(34, 38, 43, 64, 74, 73, 94, 110, 103)); + test(Matrix3x3(34, 64, 43, 7, -1, 73, 94, 110, 103) != + Matrix3x3(34, 38, 43, 64, 74, 73, 94, 110, 103)); - // Test operator to read a value - test(mMatrix1[0][0] == 2); - test(mMatrix1[0][1] == 24); - test(mMatrix1[0][2] == 4); - test(mMatrix1[1][0] == 5); - test(mMatrix1[1][1] == -6); - test(mMatrix1[1][2] == 234); - test(mMatrix1[2][0] == -15); - test(mMatrix1[2][1] == 11); - test(mMatrix1[2][2] == 66); + // Test operator to read a value + test(mMatrix1[0][0] == 2); + test(mMatrix1[0][1] == 24); + test(mMatrix1[0][2] == 4); + test(mMatrix1[1][0] == 5); + test(mMatrix1[1][1] == -6); + test(mMatrix1[1][2] == 234); + test(mMatrix1[2][0] == -15); + test(mMatrix1[2][1] == 11); + test(mMatrix1[2][2] == 66); - // Test operator to set a value - Matrix3x3 test3; - test3[0][0] = 2; - test3[0][1] = 24; - test3[0][2] = 4; - test3[1][0] = 5; - test3[1][1] = -6; - test3[1][2] = 234; - test3[2][0] = -15; - test3[2][1] = 11; - test3[2][2] = 66; - test(test3 == mMatrix1); - } + // Test operator to set a value + Matrix3x3 test3; + test3[0][0] = 2; + test3[0][1] = 24; + test3[0][2] = 4; + test3[1][0] = 5; + test3[1][1] = -6; + test3[1][2] = 234; + test3[2][0] = -15; + test3[2][1] = 11; + test3[2][2] = 66; + test(test3 == mMatrix1); + } }; diff --git a/test/tests/mathematics/TestQuaternion.h b/test/tests/mathematics/TestQuaternion.h index 92790bb..033cf0a 100644 --- a/test/tests/mathematics/TestQuaternion.h +++ b/test/tests/mathematics/TestQuaternion.h @@ -1,26 +1,26 @@ /******************************************************************************** -* ReactPhysics3D physics library, http://www.reactphysics3d.com * -* Copyright (c) 2010-2016 Daniel Chappuis * +* 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: * -* * +* 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. * -* * +* 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_QUATERNION_H @@ -39,231 +39,231 @@ namespace reactphysics3d { */ class TestQuaternion : public Test { - private : + private : - // ---------- Atributes ---------- // + // ---------- Atributes ---------- // - /// Identity Quaternion - Quaternion mIdentity; + /// Identity Quaternion + Quaternion mIdentity; - /// First test quaternion - Quaternion mQuaternion1; + /// First test quaternion + Quaternion mQuaternion1; - public : + public : - // ---------- Methods ---------- // + // ---------- Methods ---------- // - /// Constructor - TestQuaternion(const std::string& name) : Test(name), mIdentity(Quaternion::identity()) { + /// 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)); - Vector3 vector(2, 3, 4); - vector.normalize(); - mQuaternion1 = Quaternion(vector.x * sinA, vector.y * sinA, vector.z * sinA, cosA); - mQuaternion1.normalize(); - } + float sinA = sin(float(PI/8.0)); + float cosA = cos(float(PI/8.0)); + Vector3 vector(2, 3, 4); + vector.normalize(); + mQuaternion1 = Quaternion(vector.x * sinA, vector.y * sinA, vector.z * sinA, cosA); + mQuaternion1.normalize(); + } - /// Run the tests - void run() { - testConstructors(); - testUnitLengthNormalize(); - testOthersMethods(); - testOperators(); - } + /// Run the tests + void run() { + testConstructors(); + testUnitLengthNormalize(); + testOthersMethods(); + testOperators(); + } - /// Test the constructors - void testConstructors() { + /// Test the constructors + void testConstructors() { - Quaternion quaternion1(mQuaternion1); - test(mQuaternion1 == quaternion1); + Quaternion quaternion1(mQuaternion1); + test(mQuaternion1 == quaternion1); - Quaternion quaternion2(4, 5, 6, 7); - test(quaternion2 == Quaternion(4, 5, 6, 7)); + Quaternion quaternion2(4, 5, 6, 7); + test(quaternion2 == Quaternion(4, 5, 6, 7)); - Quaternion quaternion3(8, Vector3(3, 5, 2)); - test(quaternion3 == Quaternion(3, 5, 2, 8)); + Quaternion quaternion3(8, Vector3(3, 5, 2)); + test(quaternion3 == Quaternion(3, 5, 2, 8)); - Quaternion quaternion4(mQuaternion1.getMatrix()); - test(approxEqual(quaternion4.x, mQuaternion1.x)); - test(approxEqual(quaternion4.y, mQuaternion1.y)); - test(approxEqual(quaternion4.z, mQuaternion1.z)); - test(approxEqual(quaternion4.w, mQuaternion1.w)); + Quaternion quaternion4(mQuaternion1.getMatrix()); + test(approxEqual(quaternion4.x, mQuaternion1.x)); + test(approxEqual(quaternion4.y, mQuaternion1.y)); + test(approxEqual(quaternion4.z, mQuaternion1.z)); + test(approxEqual(quaternion4.w, mQuaternion1.w)); - // Test conversion from Euler angles to quaternion + // Test conversion from Euler angles to quaternion - const decimal PI_OVER_2 = PI * decimal(0.5); - const decimal PI_OVER_4 = PI_OVER_2 * decimal(0.5); - Quaternion quaternion5(PI_OVER_2, 0, 0); - Quaternion quaternionTest5(std::sin(PI_OVER_4), 0, 0, std::cos(PI_OVER_4)); - quaternionTest5.normalize(); - test(approxEqual(quaternion5.x, quaternionTest5.x)); - test(approxEqual(quaternion5.y, quaternionTest5.y)); - test(approxEqual(quaternion5.z, quaternionTest5.z)); - test(approxEqual(quaternion5.w, quaternionTest5.w)); + const float PI_OVER_2 = PI * float(0.5); + const float PI_OVER_4 = PI_OVER_2 * float(0.5); + Quaternion quaternion5(PI_OVER_2, 0, 0); + Quaternion quaternionTest5(std::sin(PI_OVER_4), 0, 0, std::cos(PI_OVER_4)); + quaternionTest5.normalize(); + test(approxEqual(quaternion5.x, quaternionTest5.x)); + test(approxEqual(quaternion5.y, quaternionTest5.y)); + test(approxEqual(quaternion5.z, quaternionTest5.z)); + test(approxEqual(quaternion5.w, quaternionTest5.w)); - Quaternion quaternion6(0, PI_OVER_2, 0); - Quaternion quaternionTest6(0, std::sin(PI_OVER_4), 0, std::cos(PI_OVER_4)); - quaternionTest6.normalize(); - test(approxEqual(quaternion6.x, quaternionTest6.x)); - test(approxEqual(quaternion6.y, quaternionTest6.y)); - test(approxEqual(quaternion6.z, quaternionTest6.z)); - test(approxEqual(quaternion6.w, quaternionTest6.w)); + Quaternion quaternion6(0, PI_OVER_2, 0); + Quaternion quaternionTest6(0, std::sin(PI_OVER_4), 0, std::cos(PI_OVER_4)); + quaternionTest6.normalize(); + test(approxEqual(quaternion6.x, quaternionTest6.x)); + test(approxEqual(quaternion6.y, quaternionTest6.y)); + test(approxEqual(quaternion6.z, quaternionTest6.z)); + test(approxEqual(quaternion6.w, quaternionTest6.w)); - Quaternion quaternion7(Vector3(0, 0, PI_OVER_2)); - Quaternion quaternionTest7(0, 0, std::sin(PI_OVER_4), std::cos(PI_OVER_4)); - quaternionTest7.normalize(); - test(approxEqual(quaternion7.x, quaternionTest7.x)); - test(approxEqual(quaternion7.y, quaternionTest7.y)); - test(approxEqual(quaternion7.z, quaternionTest7.z)); - test(approxEqual(quaternion7.w, quaternionTest7.w)); - } + Quaternion quaternion7(Vector3(0, 0, PI_OVER_2)); + Quaternion quaternionTest7(0, 0, std::sin(PI_OVER_4), std::cos(PI_OVER_4)); + quaternionTest7.normalize(); + test(approxEqual(quaternion7.x, quaternionTest7.x)); + test(approxEqual(quaternion7.y, quaternionTest7.y)); + test(approxEqual(quaternion7.z, quaternionTest7.z)); + test(approxEqual(quaternion7.w, quaternionTest7.w)); + } - /// Test unit, length, normalize methods - void testUnitLengthNormalize() { + /// Test unit, length, normalize methods + void testUnitLengthNormalize() { - // Test method that returns the length - Quaternion quaternion(2, 3, -4, 5); - test(approxEqual(quaternion.length(), sqrt(decimal(54.0)))); + // Test method that returns the length + Quaternion quaternion(2, 3, -4, 5); + test(approxEqual(quaternion.length(), sqrt(float(54.0)))); - // Test method that returns a unit quaternion - test(approxEqual(quaternion.getUnit().length(), 1.0)); + // Test method that returns a unit quaternion + test(approxEqual(quaternion.getUnit().length(), 1.0)); - // Test the normalization method - Quaternion quaternion2(4, 5, 6, 7); - quaternion2.normalize(); - test(approxEqual(quaternion2.length(), 1.0)); - } + // Test the normalization method + Quaternion quaternion2(4, 5, 6, 7); + quaternion2.normalize(); + test(approxEqual(quaternion2.length(), 1.0)); + } - /// Test others methods - void testOthersMethods() { + /// Test others methods + void testOthersMethods() { - // Test the method to set the values - Quaternion quaternion; - quaternion.setAllValues(1, 2, 3, 4); - test(quaternion == Quaternion(1, 2, 3, 4)); + // Test the method to set the values + Quaternion quaternion; + quaternion.setAllValues(1, 2, 3, 4); + test(quaternion == Quaternion(1, 2, 3, 4)); - // Test the method to set the quaternion to zero - quaternion.setToZero(); - test(quaternion == Quaternion(0, 0, 0, 0)); + // Test the method to set the quaternion to zero + quaternion.setToZero(); + test(quaternion == Quaternion(0, 0, 0, 0)); - // Tes the methods to get or set to identity - Quaternion identity1(1, 2, 3, 4); - identity1.setToIdentity(); - test(identity1 == Quaternion(0, 0, 0, 1)); - test(Quaternion::identity() == Quaternion(0, 0, 0, 1)); + // Tes the methods to get or set to identity + Quaternion identity1(1, 2, 3, 4); + identity1.setToIdentity(); + test(identity1 == Quaternion(0, 0, 0, 1)); + test(Quaternion::identity() == Quaternion(0, 0, 0, 1)); - // Test the method to get the vector (x, y, z) - Vector3 v = mQuaternion1.getVectorV(); - test(v.x == mQuaternion1.x); - test(v.y == mQuaternion1.y); - test(v.z == mQuaternion1.z); + // Test the method to get the vector (x, y, z) + Vector3 v = mQuaternion1.getVectorV(); + test(v.x == mQuaternion1.x); + test(v.y == mQuaternion1.y); + test(v.z == mQuaternion1.z); - // Test the conjugate method - Quaternion conjugate = mQuaternion1.getConjugate(); - test(conjugate.x == -mQuaternion1.x); - test(conjugate.y == -mQuaternion1.y); - test(conjugate.z == -mQuaternion1.z); - test(conjugate.w == mQuaternion1.w); + // Test the conjugate method + Quaternion conjugate = mQuaternion1.getConjugate(); + test(conjugate.x == -mQuaternion1.x); + test(conjugate.y == -mQuaternion1.y); + test(conjugate.z == -mQuaternion1.z); + test(conjugate.w == mQuaternion1.w); - // Test the inverse methods - Quaternion inverse1 = mQuaternion1.getInverse(); - Quaternion inverse2(mQuaternion1); - inverse2.inverse(); - test(inverse2 == inverse1); - Quaternion product = mQuaternion1 * inverse1; - test(approxEqual(product.x, mIdentity.x, decimal(10e-6))); - test(approxEqual(product.y, mIdentity.y, decimal(10e-6))); - test(approxEqual(product.z, mIdentity.z, decimal(10e-6))); - test(approxEqual(product.w, mIdentity.w, decimal(10e-6))); + // Test the inverse methods + Quaternion inverse1 = mQuaternion1.getInverse(); + Quaternion inverse2(mQuaternion1); + inverse2.inverse(); + test(inverse2 == inverse1); + Quaternion product = mQuaternion1 * inverse1; + test(approxEqual(product.x, mIdentity.x, float(10e-6))); + test(approxEqual(product.y, mIdentity.y, float(10e-6))); + test(approxEqual(product.z, mIdentity.z, float(10e-6))); + test(approxEqual(product.w, mIdentity.w, float(10e-6))); - // Test the dot product - Quaternion quaternion1(2, 3, 4, 5); - Quaternion quaternion2(6, 7, 8, 9); - decimal dotProduct = quaternion1.dot(quaternion2); - test(dotProduct == 110); + // Test the dot product + Quaternion quaternion1(2, 3, 4, 5); + Quaternion quaternion2(6, 7, 8, 9); + float dotProduct = quaternion1.dot(quaternion2); + test(dotProduct == 110); - // Test the method that returns the rotation angle and axis - Vector3 axis; - decimal angle; - Vector3 originalAxis = Vector3(2, 3, 4).getUnit(); - mQuaternion1.getRotationAngleAxis(angle, axis); - test(approxEqual(axis.x, originalAxis.x)); - test(approxEqual(angle, decimal(PI/4.0), decimal(10e-6))); + // Test the method that returns the rotation angle and axis + Vector3 axis; + float angle; + Vector3 originalAxis = Vector3(2, 3, 4).getUnit(); + mQuaternion1.getRotationAngleAxis(angle, axis); + test(approxEqual(axis.x, originalAxis.x)); + test(approxEqual(angle, float(PI/4.0), float(10e-6))); - // Test the method that returns the corresponding matrix - Matrix3x3 matrix = mQuaternion1.getMatrix(); - Vector3 vector(56, -2, 82); - Vector3 vector1 = matrix * vector; - Vector3 vector2 = mQuaternion1 * vector; - test(approxEqual(vector1.x, vector2.x, decimal(10e-6))); - test(approxEqual(vector1.y, vector2.y, decimal(10e-6))); - test(approxEqual(vector1.z, vector2.z, decimal(10e-6))); + // Test the method that returns the corresponding matrix + Matrix3x3 matrix = mQuaternion1.getMatrix(); + Vector3 vector(56, -2, 82); + Vector3 vector1 = matrix * vector; + Vector3 vector2 = mQuaternion1 * vector; + test(approxEqual(vector1.x, vector2.x, float(10e-6))); + test(approxEqual(vector1.y, vector2.y, float(10e-6))); + test(approxEqual(vector1.z, vector2.z, float(10e-6))); - // Test slerp method - Quaternion quatStart = quaternion1.getUnit(); - Quaternion quatEnd = quaternion2.getUnit(); - Quaternion test1 = Quaternion::slerp(quatStart, quatEnd, 0.0); - Quaternion test2 = Quaternion::slerp(quatStart, quatEnd, 1.0); - test(test1 == quatStart); - test(test2 == quatEnd); - decimal sinA = sin(decimal(PI/4.0)); - decimal cosA = cos(decimal(PI/4.0)); - Quaternion quat(sinA, 0, 0, cosA); - Quaternion test3 = Quaternion::slerp(mIdentity, quat, decimal(0.5)); - test(approxEqual(test3.x, sin(decimal(PI/8.0)))); - test(approxEqual(test3.y, 0.0)); - test(approxEqual(test3.z, 0.0)); - test(approxEqual(test3.w, cos(decimal(PI/8.0)), decimal(10e-6))); - } + // Test slerp method + Quaternion quatStart = quaternion1.getUnit(); + Quaternion quatEnd = quaternion2.getUnit(); + Quaternion test1 = Quaternion::slerp(quatStart, quatEnd, 0.0); + Quaternion test2 = Quaternion::slerp(quatStart, quatEnd, 1.0); + test(test1 == quatStart); + test(test2 == quatEnd); + float sinA = sin(float(PI/4.0)); + float cosA = cos(float(PI/4.0)); + Quaternion quat(sinA, 0, 0, cosA); + Quaternion test3 = Quaternion::slerp(mIdentity, quat, float(0.5)); + test(approxEqual(test3.x, sin(float(PI/8.0)))); + test(approxEqual(test3.y, 0.0)); + test(approxEqual(test3.z, 0.0)); + test(approxEqual(test3.w, cos(float(PI/8.0)), float(10e-6))); + } - /// Test overloaded operators - void testOperators() { + /// Test overloaded operators + void testOperators() { - // Test addition - Quaternion quat1(4, 5, 2, 10); - Quaternion quat2(-2, 7, 8, 3); - Quaternion test1 = quat1 + quat2; - Quaternion test11(-6, 52, 2, 8); - test11 += quat1; - test(test1 == Quaternion(2, 12, 10, 13)); - test(test11 == Quaternion(-2, 57, 4, 18)); + // Test addition + Quaternion quat1(4, 5, 2, 10); + Quaternion quat2(-2, 7, 8, 3); + Quaternion test1 = quat1 + quat2; + Quaternion test11(-6, 52, 2, 8); + test11 += quat1; + test(test1 == Quaternion(2, 12, 10, 13)); + test(test11 == Quaternion(-2, 57, 4, 18)); - // Test substraction - Quaternion test2 = quat1 - quat2; - Quaternion test22(-73, 62, 25, 9); - test22 -= quat1; - test(test2 == Quaternion(6, -2, -6, 7)); - test(test22 == Quaternion(-77, 57, 23, -1)); + // Test substraction + Quaternion test2 = quat1 - quat2; + Quaternion test22(-73, 62, 25, 9); + test22 -= quat1; + test(test2 == Quaternion(6, -2, -6, 7)); + test(test22 == Quaternion(-77, 57, 23, -1)); - // Test multiplication with a number - Quaternion test3 = quat1 * 3.0; - test(test3 == Quaternion(12, 15, 6, 30)); + // Test multiplication with a number + Quaternion test3 = quat1 * 3.0; + test(test3 == Quaternion(12, 15, 6, 30)); - // Test multiplication between two quaternions - Quaternion test4 = quat1 * quat2; - Quaternion test5 = mQuaternion1 * mIdentity; - test(test4 == Quaternion(18, 49, 124, -13)); - test(test5 == mQuaternion1); + // Test multiplication between two quaternions + Quaternion test4 = quat1 * quat2; + Quaternion test5 = mQuaternion1 * mIdentity; + test(test4 == Quaternion(18, 49, 124, -13)); + test(test5 == mQuaternion1); - // Test multiplication between a quaternion and a point - Vector3 point(5, -24, 563); - Vector3 vector1 = mIdentity * point; - Vector3 vector2 = mQuaternion1 * point; - Vector3 testVector2 = mQuaternion1.getMatrix() * point; - test(vector1 == point); - test(approxEqual(vector2.x, testVector2.x, decimal(10e-5))); - test(approxEqual(vector2.y, testVector2.y, decimal(10e-5))); - test(approxEqual(vector2.z, testVector2.z, decimal(10e-5))); + // Test multiplication between a quaternion and a point + Vector3 point(5, -24, 563); + Vector3 vector1 = mIdentity * point; + Vector3 vector2 = mQuaternion1 * point; + Vector3 testVector2 = mQuaternion1.getMatrix() * point; + test(vector1 == point); + test(approxEqual(vector2.x, testVector2.x, float(10e-5))); + test(approxEqual(vector2.y, testVector2.y, float(10e-5))); + test(approxEqual(vector2.z, testVector2.z, float(10e-5))); - // Test assignment operator - Quaternion quaternion; - quaternion = mQuaternion1; - test(quaternion == mQuaternion1); + // Test assignment operator + Quaternion quaternion; + quaternion = mQuaternion1; + test(quaternion == mQuaternion1); - // Test equality operator - test(mQuaternion1 == mQuaternion1); - } + // Test equality operator + test(mQuaternion1 == mQuaternion1); + } }; } diff --git a/test/tests/mathematics/TestTransform.h b/test/tests/mathematics/TestTransform.h index 30bdb93..e76447e 100644 --- a/test/tests/mathematics/TestTransform.h +++ b/test/tests/mathematics/TestTransform.h @@ -1,26 +1,26 @@ /******************************************************************************** -* ReactPhysics3D physics library, http://www.reactphysics3d.com * -* Copyright (c) 2010-2016 Daniel Chappuis * +* 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: * -* * +* 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. * -* * +* 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_TRANSFORM_H @@ -39,175 +39,175 @@ namespace reactphysics3d { */ class TestTransform : public Test { - private : + private : - // ---------- Atributes ---------- // + // ---------- Atributes ---------- // - /// Identity transform - Transform mIdentityTransform; + /// Identity transform + Transform mIdentityTransform; - /// First example transform - Transform mTransform1; + /// First example transform + Transform mTransform1; - /// Second example transform - Transform mTransform2; + /// Second example transform + Transform mTransform2; - public : + public : - // ---------- Methods ---------- // + // ---------- Methods ---------- // - /// Constructor - TestTransform(const std::string& name) : Test(name) { + /// Constructor + TestTransform(const std::string& name) : Test(name) { - mIdentityTransform.setToIdentity(); + mIdentityTransform.setToIdentity(); - decimal sinA = sin(PI/8.0f); - decimal cosA = cos(PI/8.0f); - mTransform1 = Transform(Vector3(4, 5, 6), Quaternion(sinA, sinA, sinA, cosA)); + float sinA = sin(PI/8.0f); + float cosA = cos(PI/8.0f); + mTransform1 = Transform(Vector3(4, 5, 6), Quaternion(sinA, sinA, sinA, cosA)); - decimal sinB = sin(PI/3.0f); - decimal cosB = cos(PI/3.0f); - mTransform2 = Transform(Vector3(8, 45, -6), Quaternion(sinB, sinB, sinB, cosB)); - } + float sinB = sin(PI/3.0f); + float cosB = cos(PI/3.0f); + mTransform2 = Transform(Vector3(8, 45, -6), Quaternion(sinB, sinB, sinB, cosB)); + } - /// Run the tests - void run() { - testConstructors(); - testGetSet(); - testInverse(); - testGetSetOpenGLMatrix(); - testInterpolateTransform(); - testIdentity(); - testOperators(); - } + /// Run the tests + void run() { + testConstructors(); + testGetSet(); + testInverse(); + testGetSetOpenGLMatrix(); + testInterpolateTransform(); + testIdentity(); + testOperators(); + } - /// Test the constructors - void testConstructors() { - Transform transform1(Vector3(1, 2, 3), Quaternion(6, 7, 8, 9)); - Transform transform2(Vector3(4, 5, 6), Matrix3x3(1, 0, 0, 0, 1, 0, 0, 0, 1)); - Transform transform3(transform1); - test(transform1.getPosition() == Vector3(1, 2, 3)); - test(transform1.getOrientation() == Quaternion(6, 7, 8, 9)); - test(transform2.getPosition() == Vector3(4, 5, 6)); - test(transform2.getOrientation() == Quaternion::identity()); - test(transform3 == transform1); - } + /// Test the constructors + void testConstructors() { + Transform transform1(Vector3(1, 2, 3), Quaternion(6, 7, 8, 9)); + Transform transform2(Vector3(4, 5, 6), Matrix3x3(1, 0, 0, 0, 1, 0, 0, 0, 1)); + Transform transform3(transform1); + test(transform1.getPosition() == Vector3(1, 2, 3)); + test(transform1.getOrientation() == Quaternion(6, 7, 8, 9)); + test(transform2.getPosition() == Vector3(4, 5, 6)); + test(transform2.getOrientation() == Quaternion::identity()); + test(transform3 == transform1); + } - /// Test getter and setter - void testGetSet() { - test(mIdentityTransform.getPosition() == Vector3(0, 0, 0)); - test(mIdentityTransform.getOrientation() == Quaternion::identity()); - Transform transform; - transform.setPosition(Vector3(5, 7, 8)); - transform.setOrientation(Quaternion(1, 2, 3, 1)); - test(transform.getPosition() == Vector3(5, 7, 8)); - test(transform.getOrientation() == Quaternion(1, 2, 3, 1)); - transform.setToIdentity(); - test(transform.getPosition() == Vector3(0, 0, 0)); - test(transform.getOrientation() == Quaternion::identity()); - } + /// Test getter and setter + void testGetSet() { + test(mIdentityTransform.getPosition() == Vector3(0, 0, 0)); + test(mIdentityTransform.getOrientation() == Quaternion::identity()); + Transform transform; + transform.setPosition(Vector3(5, 7, 8)); + transform.setOrientation(Quaternion(1, 2, 3, 1)); + test(transform.getPosition() == Vector3(5, 7, 8)); + test(transform.getOrientation() == Quaternion(1, 2, 3, 1)); + transform.setToIdentity(); + test(transform.getPosition() == Vector3(0, 0, 0)); + test(transform.getOrientation() == Quaternion::identity()); + } - /// Test the inverse - void testInverse() { - Transform inverseTransform = mTransform1.getInverse(); - Vector3 vector(2, 3, 4); - Vector3 tempVector = mTransform1 * vector; - Vector3 tempVector2 = inverseTransform * tempVector; - test(approxEqual(tempVector2.x, vector.x, decimal(10e-6))); - test(approxEqual(tempVector2.y, vector.y, decimal(10e-6))); - test(approxEqual(tempVector2.z, vector.z, decimal(10e-6))); - } + /// Test the inverse + void testInverse() { + Transform inverseTransform = mTransform1.getInverse(); + Vector3 vector(2, 3, 4); + Vector3 tempVector = mTransform1 * vector; + Vector3 tempVector2 = inverseTransform * tempVector; + test(approxEqual(tempVector2.x, vector.x, float(10e-6))); + test(approxEqual(tempVector2.y, vector.y, float(10e-6))); + test(approxEqual(tempVector2.z, vector.z, float(10e-6))); + } - /// Test methods to set and get transform matrix from and to OpenGL - void testGetSetOpenGLMatrix() { - Transform transform; - Vector3 position = mTransform1.getPosition(); - Matrix3x3 orientation = mTransform1.getOrientation().getMatrix(); - decimal openglMatrix[16] = {orientation[0][0], orientation[1][0], - orientation[2][0], 0, - orientation[0][1], orientation[1][1], - orientation[2][1], 0, - orientation[0][2], orientation[1][2], - orientation[2][2], 0, - position.x, position.y, position.z, 1}; - transform.setFromOpenGL(openglMatrix); - decimal openglMatrix2[16]; - transform.getOpenGLMatrix(openglMatrix2); - test(approxEqual(openglMatrix2[0], orientation[0][0])); - test(approxEqual(openglMatrix2[1], orientation[1][0])); - test(approxEqual(openglMatrix2[2], orientation[2][0])); - test(approxEqual(openglMatrix2[3], 0)); - test(approxEqual(openglMatrix2[4], orientation[0][1])); - test(approxEqual(openglMatrix2[5], orientation[1][1])); - test(approxEqual(openglMatrix2[6], orientation[2][1])); - test(approxEqual(openglMatrix2[7], 0)); - test(approxEqual(openglMatrix2[8], orientation[0][2])); - test(approxEqual(openglMatrix2[9], orientation[1][2])); - test(approxEqual(openglMatrix2[10], orientation[2][2])); - test(approxEqual(openglMatrix2[11], 0)); - test(approxEqual(openglMatrix2[12], position.x)); - test(approxEqual(openglMatrix2[13], position.y)); - test(approxEqual(openglMatrix2[14], position.z)); - test(approxEqual(openglMatrix2[15], 1)); - } + /// Test methods to set and get transform matrix from and to OpenGL + void testGetSetOpenGLMatrix() { + Transform transform; + Vector3 position = mTransform1.getPosition(); + Matrix3x3 orientation = mTransform1.getOrientation().getMatrix(); + float openglMatrix[16] = {orientation[0][0], orientation[1][0], + orientation[2][0], 0, + orientation[0][1], orientation[1][1], + orientation[2][1], 0, + orientation[0][2], orientation[1][2], + orientation[2][2], 0, + position.x, position.y, position.z, 1}; + transform.setFromOpenGL(openglMatrix); + float openglMatrix2[16]; + transform.getOpenGLMatrix(openglMatrix2); + test(approxEqual(openglMatrix2[0], orientation[0][0])); + test(approxEqual(openglMatrix2[1], orientation[1][0])); + test(approxEqual(openglMatrix2[2], orientation[2][0])); + test(approxEqual(openglMatrix2[3], 0)); + test(approxEqual(openglMatrix2[4], orientation[0][1])); + test(approxEqual(openglMatrix2[5], orientation[1][1])); + test(approxEqual(openglMatrix2[6], orientation[2][1])); + test(approxEqual(openglMatrix2[7], 0)); + test(approxEqual(openglMatrix2[8], orientation[0][2])); + test(approxEqual(openglMatrix2[9], orientation[1][2])); + test(approxEqual(openglMatrix2[10], orientation[2][2])); + test(approxEqual(openglMatrix2[11], 0)); + test(approxEqual(openglMatrix2[12], position.x)); + test(approxEqual(openglMatrix2[13], position.y)); + test(approxEqual(openglMatrix2[14], position.z)); + test(approxEqual(openglMatrix2[15], 1)); + } - /// Test the method to interpolate transforms - void testInterpolateTransform() { - Transform transformStart = Transform::interpolateTransforms(mTransform1, mTransform2,0); - Transform transformEnd = Transform::interpolateTransforms(mTransform1, mTransform2,1); - test(transformStart == mTransform1); - test(transformEnd == mTransform2); + /// Test the method to int32_terpolate transforms + void testInterpolateTransform() { + Transform transformStart = Transform::int32_terpolateTransforms(mTransform1, mTransform2,0); + Transform transformEnd = Transform::int32_terpolateTransforms(mTransform1, mTransform2,1); + test(transformStart == mTransform1); + 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); - 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); - Vector3 position = transform.getPosition(); - Quaternion orientation = transform.getOrientation(); - test(approxEqual(position.x, 6)); - test(approxEqual(position.y, 8)); - test(approxEqual(position.z, 11)); - test(approxEqual(orientation.x, sinB)); - test(approxEqual(orientation.y, sinB)); - test(approxEqual(orientation.z, sinB)); - test(approxEqual(orientation.w, cosB)); - } + float sinA = sin(PI/3.0f); + float cosA = cos(PI/3.0f); + float sinB = sin(PI/6.0f); + float cosB = cos(PI/6.0f); + Transform transform1(Vector3(4, 5, 6), Quaternion::identity()); + Transform transform2(Vector3(8, 11, 16), Quaternion(sinA, sinA, sinA, cosA)); + Transform transform = Transform::int32_terpolateTransforms(transform1, transform2, 0.5); + Vector3 position = transform.getPosition(); + Quaternion orientation = transform.getOrientation(); + test(approxEqual(position.x, 6)); + test(approxEqual(position.y, 8)); + test(approxEqual(position.z, 11)); + test(approxEqual(orientation.x, sinB)); + test(approxEqual(orientation.y, sinB)); + test(approxEqual(orientation.z, sinB)); + test(approxEqual(orientation.w, cosB)); + } - /// Test the identity methods - void testIdentity() { - Transform transform = Transform::identity(); - test(transform.getPosition() == Vector3(0, 0, 0)); - test(transform.getOrientation() == Quaternion::identity()); + /// Test the identity methods + void testIdentity() { + Transform transform = Transform::identity(); + test(transform.getPosition() == Vector3(0, 0, 0)); + test(transform.getOrientation() == Quaternion::identity()); - Transform transform2(Vector3(5, 6, 2), Quaternion(3, 5, 1, 6)); - transform2.setToIdentity(); - test(transform2.getPosition() == Vector3(0, 0, 0)); - test(transform2.getOrientation() == Quaternion::identity()); - } + Transform transform2(Vector3(5, 6, 2), Quaternion(3, 5, 1, 6)); + transform2.setToIdentity(); + test(transform2.getPosition() == Vector3(0, 0, 0)); + test(transform2.getOrientation() == Quaternion::identity()); + } - /// Test the overloaded operators - void testOperators() { + /// Test the overloaded operators + void testOperators() { - // Equality, inequality operator - test(mTransform1 == mTransform1); - test(mTransform1 != mTransform2); + // Equality, inequality operator + test(mTransform1 == mTransform1); + test(mTransform1 != mTransform2); - // Assignment operator - Transform transform; - transform = mTransform1; - test(transform == mTransform1); + // Assignment operator + Transform transform; + transform = mTransform1; + test(transform == mTransform1); - // Multiplication - Vector3 vector(7, 53, 5); - Vector3 vector2 = mTransform2 * (mTransform1 * vector); - Vector3 vector3 = (mTransform2 * mTransform1) * vector; - test(approxEqual(vector2.x, vector3.x, decimal(10e-6))); - test(approxEqual(vector2.y, vector3.y, decimal(10e-6))); - test(approxEqual(vector2.z, vector3.z, decimal(10e-6))); - } + // Multiplication + Vector3 vector(7, 53, 5); + Vector3 vector2 = mTransform2 * (mTransform1 * vector); + Vector3 vector3 = (mTransform2 * mTransform1) * vector; + test(approxEqual(vector2.x, vector3.x, float(10e-6))); + test(approxEqual(vector2.y, vector3.y, float(10e-6))); + test(approxEqual(vector2.z, vector3.z, float(10e-6))); + } }; } diff --git a/test/tests/mathematics/TestVector2.h b/test/tests/mathematics/TestVector2.h index b1ddecb..84e6807 100644 --- a/test/tests/mathematics/TestVector2.h +++ b/test/tests/mathematics/TestVector2.h @@ -1,26 +1,26 @@ /******************************************************************************** -* ReactPhysics3D physics library, http://www.reactphysics3d.com * -* Copyright (c) 2010-2016 Daniel Chappuis * +* 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: * -* * +* 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. * -* * +* 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_VECTOR2_H @@ -39,181 +39,181 @@ namespace reactphysics3d { */ class TestVector2 : public Test { - private : + private : - // ---------- Atributes ---------- // + // ---------- Atributes ---------- // - /// Zero vector - Vector2 mVectorZero; + /// Zero vector + Vector2 mVectorZero; - // Vector (3, 4) - Vector2 mVector34; + // Vector (3, 4) + Vector2 mVector34; - public : + public : - // ---------- Methods ---------- // + // ---------- Methods ---------- // - /// Constructor - TestVector2(const std::string& name) : Test(name), mVectorZero(0, 0), mVector34(3, 4) {} + /// Constructor + TestVector2(const std::string& name) : Test(name), mVectorZero(0, 0), mVector34(3, 4) {} - /// Run the tests - void run() { - testConstructors(); - testLengthMethods(); - testDotProduct(); - testOthersMethods(); - testOperators(); - } + /// Run the tests + void run() { + testConstructors(); + testLengthMethods(); + testDotProduct(); + testOthersMethods(); + testOperators(); + } - /// Test the constructors, getter and setter - void testConstructors() { + /// Test the constructors, getter and setter + void testConstructors() { - // Test constructor - test(mVectorZero.x == 0.0); - test(mVectorZero.y == 0.0); - test(mVector34.x == 3.0); - test(mVector34.y == 4.0); + // Test constructor + test(mVectorZero.x == 0.0); + test(mVectorZero.y == 0.0); + test(mVector34.x == 3.0); + test(mVector34.y == 4.0); - // Test copy-constructor - Vector2 newVector(mVector34); - test(newVector.x == 3.0); - test(newVector.y == 4.0); + // Test copy-constructor + Vector2 newVector(mVector34); + test(newVector.x == 3.0); + test(newVector.y == 4.0); - // Test method to set values - Vector2 newVector2; - newVector2.setAllValues(decimal(6.1), decimal(7.2)); - test(approxEqual(newVector2.x, decimal(6.1))); - test(approxEqual(newVector2.y, decimal(7.2))); + // Test method to set values + Vector2 newVector2; + newVector2.setAllValues(float(6.1), float(7.2)); + test(approxEqual(newVector2.x, float(6.1))); + test(approxEqual(newVector2.y, float(7.2))); - // Test method to set to zero - newVector2.setToZero(); - test(newVector2 == Vector2(0, 0)); - } + // Test method to set to zero + newVector2.setToZero(); + test(newVector2 == Vector2(0, 0)); + } - /// Test the length, unit vector and normalize methods - void testLengthMethods() { + /// Test the length, unit vector and normalize methods + void testLengthMethods() { - // Test length methods - test(mVectorZero.length() == 0.0); - test(mVectorZero.lengthSquare() == 0.0); - test(Vector2(1, 0).length() == 1.0); - test(Vector2(0, 1).length() == 1.0); - test(mVector34.lengthSquare() == 25.0); + // Test length methods + test(mVectorZero.length() == 0.0); + test(mVectorZero.lengthSquare() == 0.0); + test(Vector2(1, 0).length() == 1.0); + test(Vector2(0, 1).length() == 1.0); + test(mVector34.lengthSquare() == 25.0); - // Test unit vector methods - test(Vector2(1, 0).isUnit()); - test(Vector2(0, 1).isUnit()); - test(!mVector34.isUnit()); - test(Vector2(5, 0).getUnit() == Vector2(1, 0)); - test(Vector2(0, 5).getUnit() == Vector2(0, 1)); + // Test unit vector methods + test(Vector2(1, 0).isUnit()); + test(Vector2(0, 1).isUnit()); + test(!mVector34.isUnit()); + test(Vector2(5, 0).getUnit() == Vector2(1, 0)); + test(Vector2(0, 5).getUnit() == Vector2(0, 1)); - test(!mVector34.isZero()); - test(mVectorZero.isZero()); + test(!mVector34.isZero()); + test(mVectorZero.isZero()); - // Test normalization method - Vector2 mVector10(1, 0); - Vector2 mVector01(0, 1); - Vector2 mVector50(5, 0); - Vector2 mVector05(0, 5); - mVector10.normalize(); - mVector01.normalize(); - mVector50.normalize(); - mVector05.normalize(); - test(mVector10 == Vector2(1, 0)); - test(mVector01 == Vector2(0, 1)); - test(mVector50 == Vector2(1, 0)); - test(mVector05 == Vector2(0, 1)); - } + // Test normalization method + Vector2 mVector10(1, 0); + Vector2 mVector01(0, 1); + Vector2 mVector50(5, 0); + Vector2 mVector05(0, 5); + mVector10.normalize(); + mVector01.normalize(); + mVector50.normalize(); + mVector05.normalize(); + test(mVector10 == Vector2(1, 0)); + test(mVector01 == Vector2(0, 1)); + test(mVector50 == Vector2(1, 0)); + test(mVector05 == Vector2(0, 1)); + } - /// Test the dot product - void testDotProduct() { + /// Test the dot product + void testDotProduct() { - // Test the dot product - test(Vector2(5, 0).dot(Vector2(0, 8)) == 0); - test(Vector2(5, 8).dot(Vector2(0, 0)) == 0); - test(Vector2(12, 45).dot(Vector2(0, 0)) == 0); - test(Vector2(5, 7).dot(Vector2(5, 7)) == 74); - test(Vector2(3, 6).dot(Vector2(-3, -6)) == -45); - test(Vector2(2, 3).dot(Vector2(-7, 4)) == -2); - test(Vector2(4, 3).dot(Vector2(8, 9)) == 59); - } + // Test the dot product + test(Vector2(5, 0).dot(Vector2(0, 8)) == 0); + test(Vector2(5, 8).dot(Vector2(0, 0)) == 0); + test(Vector2(12, 45).dot(Vector2(0, 0)) == 0); + test(Vector2(5, 7).dot(Vector2(5, 7)) == 74); + test(Vector2(3, 6).dot(Vector2(-3, -6)) == -45); + test(Vector2(2, 3).dot(Vector2(-7, 4)) == -2); + test(Vector2(4, 3).dot(Vector2(8, 9)) == 59); + } - /// Test others methods - void testOthersMethods() { + /// Test others methods + void testOthersMethods() { - // Test the method that returns the absolute vector - test(Vector2(4, 5).getAbsoluteVector() == Vector2(4, 5)); - test(Vector2(-7, -24).getAbsoluteVector() == Vector2(7, 24)); + // Test the method that returns the absolute vector + test(Vector2(4, 5).getAbsoluteVector() == Vector2(4, 5)); + test(Vector2(-7, -24).getAbsoluteVector() == Vector2(7, 24)); - // Test the method that returns the minimal element - test(Vector2(6, 35).getMinAxis() == 0); - test(Vector2(564, 45).getMinAxis() == 1); - test(Vector2(98, 23).getMinAxis() == 1); - test(Vector2(-53, -25).getMinAxis() == 0); + // Test the method that returns the minimal element + test(Vector2(6, 35).getMinAxis() == 0); + test(Vector2(564, 45).getMinAxis() == 1); + test(Vector2(98, 23).getMinAxis() == 1); + test(Vector2(-53, -25).getMinAxis() == 0); - // Test the method that returns the maximal element - test(Vector2(6, 35).getMaxAxis() == 1); - test(Vector2(7, 537).getMaxAxis() == 1); - test(Vector2(98, 23).getMaxAxis() == 0); - test(Vector2(-53, -25).getMaxAxis() == 1); + // Test the method that returns the maximal element + test(Vector2(6, 35).getMaxAxis() == 1); + test(Vector2(7, 537).getMaxAxis() == 1); + test(Vector2(98, 23).getMaxAxis() == 0); + test(Vector2(-53, -25).getMaxAxis() == 1); - // Test the methot that return a max/min vector - Vector2 vec1(-5, 4); - Vector2 vec2(-8, 6); - test(Vector2::min(vec1, vec2) == Vector2(-8, 4)); - test(Vector2::max(vec1, vec2) == Vector2(-5, 6)); - } + // Test the methot that return a max/min vector + Vector2 vec1(-5, 4); + Vector2 vec2(-8, 6); + test(Vector2::min(vec1, vec2) == Vector2(-8, 4)); + test(Vector2::max(vec1, vec2) == Vector2(-5, 6)); + } - /// Test the operators - void testOperators() { + /// Test the operators + void testOperators() { - // Test the [] operator - test(mVector34[0] == 3); - test(mVector34[1] == 4); + // Test the [] operator + test(mVector34[0] == 3); + test(mVector34[1] == 4); - // Assignment operator - Vector2 newVector(6, 4); - newVector = Vector2(7, 8); - test(newVector == Vector2(7, 8)); + // Assignment operator + Vector2 newVector(6, 4); + newVector = Vector2(7, 8); + test(newVector == Vector2(7, 8)); - // Equality, inequality operators - test(Vector2(5, 7) == Vector2(5, 7)); - test(Vector2(63, 64) != Vector2(63, 84)); - test(Vector2(63, 64) != Vector2(12, 64)); + // Equality, inequality operators + test(Vector2(5, 7) == Vector2(5, 7)); + test(Vector2(63, 64) != Vector2(63, 84)); + test(Vector2(63, 64) != Vector2(12, 64)); - // Addition, substraction - Vector2 vector1(6, 33); - Vector2 vector2(7, 68); - test(Vector2(63, 24) + Vector2(3, 4) == Vector2(66, 28)); - test(Vector2(63, 24) - Vector2(3, 4) == Vector2(60, 20)); - vector1 += Vector2(5, 10); - vector2 -= Vector2(10, 21); - test(vector1 == Vector2(11, 43)); - test(vector2 == Vector2(-3, 47)); + // Addition, substraction + Vector2 vector1(6, 33); + Vector2 vector2(7, 68); + test(Vector2(63, 24) + Vector2(3, 4) == Vector2(66, 28)); + test(Vector2(63, 24) - Vector2(3, 4) == Vector2(60, 20)); + vector1 += Vector2(5, 10); + vector2 -= Vector2(10, 21); + test(vector1 == Vector2(11, 43)); + test(vector2 == Vector2(-3, 47)); - // Multiplication, division - Vector2 vector3(6, 33); - Vector2 vector4(15, 60); - test(Vector2(63, 24) * 3 == Vector2(189, 72)); - test(3 * Vector2(63, 24) == Vector2(189, 72)); - test(Vector2(14, 8) / 2 == Vector2(7, 4)); - vector3 *= 10; - vector4 /= 3; - test(vector3 == Vector2(60, 330)); - test(vector4 == Vector2(5, 20)); - Vector2 vector5(21, 80); - Vector2 vector6(7, 10); - Vector2 vector7 = vector5 * vector6; - test(vector7 == Vector2(147, 800)); - Vector2 vector8 = vector5 / vector6; - test(approxEqual(vector8.x, 3)); - test(approxEqual(vector8.y, 8)); + // Multiplication, division + Vector2 vector3(6, 33); + Vector2 vector4(15, 60); + test(Vector2(63, 24) * 3 == Vector2(189, 72)); + test(3 * Vector2(63, 24) == Vector2(189, 72)); + test(Vector2(14, 8) / 2 == Vector2(7, 4)); + vector3 *= 10; + vector4 /= 3; + test(vector3 == Vector2(60, 330)); + test(vector4 == Vector2(5, 20)); + Vector2 vector5(21, 80); + Vector2 vector6(7, 10); + Vector2 vector7 = vector5 * vector6; + test(vector7 == Vector2(147, 800)); + Vector2 vector8 = vector5 / vector6; + test(approxEqual(vector8.x, 3)); + test(approxEqual(vector8.y, 8)); - // Negative operator - Vector2 vector9(-34, 5); - Vector2 negative = -vector9; - test(negative == Vector2(34, -5)); - } + // Negative operator + Vector2 vector9(-34, 5); + Vector2 negative = -vector9; + test(negative == Vector2(34, -5)); + } }; } diff --git a/test/tests/mathematics/TestVector3.h b/test/tests/mathematics/TestVector3.h index 7a65369..0ee0db1 100644 --- a/test/tests/mathematics/TestVector3.h +++ b/test/tests/mathematics/TestVector3.h @@ -1,26 +1,26 @@ /******************************************************************************** -* ReactPhysics3D physics library, http://www.reactphysics3d.com * -* Copyright (c) 2010-2016 Daniel Chappuis * +* 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: * -* * +* 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. * -* * +* 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_VECTOR3_H @@ -39,206 +39,206 @@ namespace reactphysics3d { */ class TestVector3 : public Test { - private : + private : - // ---------- Atributes ---------- // + // ---------- Atributes ---------- // - /// Zero vector - Vector3 mVectorZero; + /// Zero vector + Vector3 mVectorZero; - // Vector (3, 4, 5) - Vector3 mVector345; + // Vector (3, 4, 5) + Vector3 mVector345; - public : + public : - // ---------- Methods ---------- // + // ---------- Methods ---------- // - /// Constructor - TestVector3(const std::string& name): Test(name),mVectorZero(0, 0, 0),mVector345(3, 4, 5) {} + /// Constructor + TestVector3(const std::string& name): Test(name),mVectorZero(0, 0, 0),mVector345(3, 4, 5) {} - /// Run the tests - void run() { - testConstructors(); - testLengthMethods(); - testDotCrossProducts(); - testOthersMethods(); - testOperators(); - } + /// Run the tests + void run() { + testConstructors(); + testLengthMethods(); + testDotCrossProducts(); + testOthersMethods(); + testOperators(); + } - /// Test the constructors, getter and setter - void testConstructors() { + /// Test the constructors, getter and setter + void testConstructors() { - // Test constructor - test(mVectorZero.x == 0.0); - test(mVectorZero.y == 0.0); - test(mVectorZero.z == 0.0); - test(mVector345.x == 3.0); - test(mVector345.y == 4.0); - test(mVector345.z == 5.0); + // Test constructor + test(mVectorZero.x == 0.0); + test(mVectorZero.y == 0.0); + test(mVectorZero.z == 0.0); + test(mVector345.x == 3.0); + test(mVector345.y == 4.0); + test(mVector345.z == 5.0); - // Test copy-constructor - Vector3 newVector(mVector345); - test(newVector.x == 3.0); - test(newVector.y == 4.0); - test(newVector.z == 5.0); + // Test copy-constructor + Vector3 newVector(mVector345); + test(newVector.x == 3.0); + test(newVector.y == 4.0); + test(newVector.z == 5.0); - // Test method to set values - Vector3 newVector2; - newVector2.setAllValues(decimal(6.1), decimal(7.2), decimal(8.6)); - test(approxEqual(newVector2.x, decimal(6.1))); - test(approxEqual(newVector2.y, decimal(7.2))); - test(approxEqual(newVector2.z, decimal(8.6))); + // Test method to set values + Vector3 newVector2; + newVector2.setAllValues(float(6.1), float(7.2), float(8.6)); + test(approxEqual(newVector2.x, float(6.1))); + test(approxEqual(newVector2.y, float(7.2))); + test(approxEqual(newVector2.z, float(8.6))); - // Test method to set to zero - newVector2.setToZero(); - test(newVector2 == Vector3(0, 0, 0)); - } + // Test method to set to zero + newVector2.setToZero(); + test(newVector2 == Vector3(0, 0, 0)); + } - /// Test the length, unit vector and normalize methods - void testLengthMethods() { + /// Test the length, unit vector and normalize methods + void testLengthMethods() { - // Test length methods - test(mVectorZero.length() == 0.0); - test(mVectorZero.lengthSquare() == 0.0); - test(Vector3(1, 0, 0).length() == 1.0); - test(Vector3(0, 1, 0).length() == 1.0); - test(Vector3(0, 0, 1).length() == 1.0); - test(mVector345.lengthSquare() == 50.0); + // Test length methods + test(mVectorZero.length() == 0.0); + test(mVectorZero.lengthSquare() == 0.0); + test(Vector3(1, 0, 0).length() == 1.0); + test(Vector3(0, 1, 0).length() == 1.0); + test(Vector3(0, 0, 1).length() == 1.0); + test(mVector345.lengthSquare() == 50.0); - // Test unit vector methods - test(Vector3(1, 0, 0).isUnit()); - test(Vector3(0, 1, 0).isUnit()); - test(Vector3(0, 0, 1).isUnit()); - test(!mVector345.isUnit()); - test(Vector3(5, 0, 0).getUnit() == Vector3(1, 0, 0)); - test(Vector3(0, 5, 0).getUnit() == Vector3(0, 1, 0)); - test(Vector3(0, 0, 5).getUnit() == Vector3(0, 0, 1)); + // Test unit vector methods + test(Vector3(1, 0, 0).isUnit()); + test(Vector3(0, 1, 0).isUnit()); + test(Vector3(0, 0, 1).isUnit()); + test(!mVector345.isUnit()); + test(Vector3(5, 0, 0).getUnit() == Vector3(1, 0, 0)); + test(Vector3(0, 5, 0).getUnit() == Vector3(0, 1, 0)); + test(Vector3(0, 0, 5).getUnit() == Vector3(0, 0, 1)); - test(!mVector345.isZero()); - test(mVectorZero.isZero()); + test(!mVector345.isZero()); + test(mVectorZero.isZero()); - // Test normalization method - Vector3 mVector100(1, 0, 0); - Vector3 mVector010(0, 1, 0); - Vector3 mVector001(0, 0, 1); - Vector3 mVector500(5, 0, 0); - Vector3 mVector050(0, 5, 0); - Vector3 mVector005(0, 0, 5); - mVector100.normalize(); - mVector010.normalize(); - mVector001.normalize(); - mVector500.normalize(); - mVector050.normalize(); - mVector005.normalize(); - test(mVector100 == Vector3(1, 0, 0)); - test(mVector010 == Vector3(0, 1, 0)); - test(mVector001 == Vector3(0, 0, 1)); - test(mVector500 == Vector3(1, 0, 0)); - test(mVector050 == Vector3(0, 1, 0)); - test(mVector005 == Vector3(0, 0, 1)); - } + // Test normalization method + Vector3 mVector100(1, 0, 0); + Vector3 mVector010(0, 1, 0); + Vector3 mVector001(0, 0, 1); + Vector3 mVector500(5, 0, 0); + Vector3 mVector050(0, 5, 0); + Vector3 mVector005(0, 0, 5); + mVector100.normalize(); + mVector010.normalize(); + mVector001.normalize(); + mVector500.normalize(); + mVector050.normalize(); + mVector005.normalize(); + test(mVector100 == Vector3(1, 0, 0)); + test(mVector010 == Vector3(0, 1, 0)); + test(mVector001 == Vector3(0, 0, 1)); + test(mVector500 == Vector3(1, 0, 0)); + test(mVector050 == Vector3(0, 1, 0)); + test(mVector005 == Vector3(0, 0, 1)); + } - /// Test the dot and cross products - void testDotCrossProducts() { + /// Test the dot and cross products + void testDotCrossProducts() { - // Test the dot product - test(Vector3(5, 0, 0).dot(Vector3(0, 8, 0)) == 0); - test(Vector3(5, 8, 0).dot(Vector3(0, 0, 6)) == 0); - test(Vector3(12, 45, 83).dot(Vector3(0, 0, 0)) == 0); - test(Vector3(5, 7, 8).dot(Vector3(5, 7, 8)) == 138); - test(Vector3(3, 6, 78).dot(Vector3(-3, -6, -78)) == -6129); - test(Vector3(2, 3, 5).dot(Vector3(2, 3, 5)) == 38); - test(Vector3(4, 3, 2).dot(Vector3(8, 9, 10)) == 79); + // Test the dot product + test(Vector3(5, 0, 0).dot(Vector3(0, 8, 0)) == 0); + test(Vector3(5, 8, 0).dot(Vector3(0, 0, 6)) == 0); + test(Vector3(12, 45, 83).dot(Vector3(0, 0, 0)) == 0); + test(Vector3(5, 7, 8).dot(Vector3(5, 7, 8)) == 138); + test(Vector3(3, 6, 78).dot(Vector3(-3, -6, -78)) == -6129); + test(Vector3(2, 3, 5).dot(Vector3(2, 3, 5)) == 38); + test(Vector3(4, 3, 2).dot(Vector3(8, 9, 10)) == 79); - // Test the cross product - test(Vector3(0, 0, 0).cross(Vector3(0, 0, 0)) == Vector3(0, 0, 0)); - test(Vector3(6, 7, 2).cross(Vector3(6, 7, 2)) == Vector3(0, 0, 0)); - test(Vector3(1, 0, 0).cross(Vector3(0, 1, 0)) == Vector3(0, 0, 1)); - test(Vector3(0, 1, 0).cross(Vector3(0, 0, 1)) == Vector3(1, 0, 0)); - test(Vector3(0, 0, 1).cross(Vector3(0, 1, 0)) == Vector3(-1, 0, 0)); - test(Vector3(4, 7, 24).cross(Vector3(8, 13, 11)) == Vector3(-235, 148, -4)); - test(Vector3(-4, 42, -2).cross(Vector3(35, 7, -21)) == Vector3(-868, -154, -1498)); - } + // Test the cross product + test(Vector3(0, 0, 0).cross(Vector3(0, 0, 0)) == Vector3(0, 0, 0)); + test(Vector3(6, 7, 2).cross(Vector3(6, 7, 2)) == Vector3(0, 0, 0)); + test(Vector3(1, 0, 0).cross(Vector3(0, 1, 0)) == Vector3(0, 0, 1)); + test(Vector3(0, 1, 0).cross(Vector3(0, 0, 1)) == Vector3(1, 0, 0)); + test(Vector3(0, 0, 1).cross(Vector3(0, 1, 0)) == Vector3(-1, 0, 0)); + test(Vector3(4, 7, 24).cross(Vector3(8, 13, 11)) == Vector3(-235, 148, -4)); + test(Vector3(-4, 42, -2).cross(Vector3(35, 7, -21)) == Vector3(-868, -154, -1498)); + } - /// Test others methods - void testOthersMethods() { + /// Test others methods + void testOthersMethods() { - // Test the method that returns the absolute vector - test(Vector3(4, 5, 6).getAbsoluteVector() == Vector3(4, 5, 6)); - test(Vector3(-7, -24, -12).getAbsoluteVector() == Vector3(7, 24, 12)); + // Test the method that returns the absolute vector + test(Vector3(4, 5, 6).getAbsoluteVector() == Vector3(4, 5, 6)); + test(Vector3(-7, -24, -12).getAbsoluteVector() == Vector3(7, 24, 12)); - // Test the method that returns the minimal element - test(Vector3(6, 35, 82).getMinAxis() == 0); - test(Vector3(564, 45, 532).getMinAxis() == 1); - test(Vector3(98, 23, 3).getMinAxis() == 2); - test(Vector3(-53, -25, -63).getMinAxis() == 2); + // Test the method that returns the minimal element + test(Vector3(6, 35, 82).getMinAxis() == 0); + test(Vector3(564, 45, 532).getMinAxis() == 1); + test(Vector3(98, 23, 3).getMinAxis() == 2); + test(Vector3(-53, -25, -63).getMinAxis() == 2); - // Test the method that returns the maximal element - test(Vector3(6, 35, 82).getMaxAxis() == 2); - test(Vector3(7, 533, 36).getMaxAxis() == 1); - test(Vector3(98, 23, 3).getMaxAxis() == 0); - test(Vector3(-53, -25, -63).getMaxAxis() == 1); + // Test the method that returns the maximal element + test(Vector3(6, 35, 82).getMaxAxis() == 2); + test(Vector3(7, 533, 36).getMaxAxis() == 1); + test(Vector3(98, 23, 3).getMaxAxis() == 0); + test(Vector3(-53, -25, -63).getMaxAxis() == 1); - // Test the methot that return a max/min vector - Vector3 vec1(-5, 4, 2); - Vector3 vec2(-8, 6, -1); - test(Vector3::min(vec1, vec2) == Vector3(-8, 4, -1)); - test(Vector3::max(vec1, vec2) == Vector3(-5, 6, 2)); - } + // Test the methot that return a max/min vector + Vector3 vec1(-5, 4, 2); + Vector3 vec2(-8, 6, -1); + test(Vector3::min(vec1, vec2) == Vector3(-8, 4, -1)); + test(Vector3::max(vec1, vec2) == Vector3(-5, 6, 2)); + } - /// Test the operators - void testOperators() { + /// Test the operators + void testOperators() { - // Test the [] operator - test(mVector345[0] == 3); - test(mVector345[1] == 4); - test(mVector345[2] == 5); + // Test the [] operator + test(mVector345[0] == 3); + test(mVector345[1] == 4); + test(mVector345[2] == 5); - // Assignment operator - Vector3 newVector(6, 4, 2); - newVector = Vector3(7, 8, 9); - test(newVector == Vector3(7, 8, 9)); + // Assignment operator + Vector3 newVector(6, 4, 2); + newVector = Vector3(7, 8, 9); + test(newVector == Vector3(7, 8, 9)); - // Equality, inequality operators - test(Vector3(5, 7, 3) == Vector3(5, 7, 3)); - test(Vector3(63, 64, 24) != Vector3(63, 64, 5)); - test(Vector3(63, 64, 24) != Vector3(12, 64, 24)); - test(Vector3(63, 64, 24) != Vector3(63, 8, 24)); + // Equality, inequality operators + test(Vector3(5, 7, 3) == Vector3(5, 7, 3)); + test(Vector3(63, 64, 24) != Vector3(63, 64, 5)); + test(Vector3(63, 64, 24) != Vector3(12, 64, 24)); + test(Vector3(63, 64, 24) != Vector3(63, 8, 24)); - // Addition, substraction - Vector3 vector1(6, 33, 62); - Vector3 vector2(7, 68, 35); - test(Vector3(63, 24, 5) + Vector3(3, 4, 2) == Vector3(66, 28, 7)); - test(Vector3(63, 24, 5) - Vector3(3, 4, 2) == Vector3(60, 20, 3)); - vector1 += Vector3(5, 10, 12); - vector2 -= Vector3(10, 21, 5); - test(vector1 == Vector3(11, 43, 74)); - test(vector2 == Vector3(-3, 47, 30)); + // Addition, substraction + Vector3 vector1(6, 33, 62); + Vector3 vector2(7, 68, 35); + test(Vector3(63, 24, 5) + Vector3(3, 4, 2) == Vector3(66, 28, 7)); + test(Vector3(63, 24, 5) - Vector3(3, 4, 2) == Vector3(60, 20, 3)); + vector1 += Vector3(5, 10, 12); + vector2 -= Vector3(10, 21, 5); + test(vector1 == Vector3(11, 43, 74)); + test(vector2 == Vector3(-3, 47, 30)); - // Multiplication, division - Vector3 vector3(6, 33, 62); - Vector3 vector4(15, 60, 33); - test(Vector3(63, 24, 5) * 3 == Vector3(189, 72, 15)); - test(3 * Vector3(63, 24, 5) == Vector3(189, 72, 15)); - test(Vector3(14, 8, 50) / 2 == Vector3(7, 4, 25)); - vector3 *= 10; - vector4 /= 3; - test(vector3 == Vector3(60, 330, 620)); - test(vector4 == Vector3(5, 20, 11)); - Vector3 vector5(21, 80, 45); - Vector3 vector6(7, 10, 3); - Vector3 vector7 = vector5 * vector6; - test(vector7 == Vector3(147, 800, 135)); - Vector3 vector8 = vector5 / vector6; - test(approxEqual(vector8.x, 3)); - test(approxEqual(vector8.y, 8)); - test(approxEqual(vector8.z, 15)); + // Multiplication, division + Vector3 vector3(6, 33, 62); + Vector3 vector4(15, 60, 33); + test(Vector3(63, 24, 5) * 3 == Vector3(189, 72, 15)); + test(3 * Vector3(63, 24, 5) == Vector3(189, 72, 15)); + test(Vector3(14, 8, 50) / 2 == Vector3(7, 4, 25)); + vector3 *= 10; + vector4 /= 3; + test(vector3 == Vector3(60, 330, 620)); + test(vector4 == Vector3(5, 20, 11)); + Vector3 vector5(21, 80, 45); + Vector3 vector6(7, 10, 3); + Vector3 vector7 = vector5 * vector6; + test(vector7 == Vector3(147, 800, 135)); + Vector3 vector8 = vector5 / vector6; + test(approxEqual(vector8.x, 3)); + test(approxEqual(vector8.y, 8)); + test(approxEqual(vector8.z, 15)); - // Negative operator - Vector3 vector9(-34, 5, 422); - Vector3 negative = -vector9; - test(negative == Vector3(34, -5, -422)); - } + // Negative operator + Vector3 vector9(-34, 5, 422); + Vector3 negative = -vector9; + test(negative == Vector3(34, -5, -422)); + } }; } diff --git a/tools/testbed/common/Box.cpp b/tools/testbed/common/Box.cpp index 4dee6e8..141f1d6 100644 --- a/tools/testbed/common/Box.cpp +++ b/tools/testbed/common/Box.cpp @@ -1,26 +1,26 @@ /******************************************************************************** -* ReactPhysics3D physics library, http://www.reactphysics3d.com * -* Copyright (c) 2010-2016 Daniel Chappuis * +* 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: * -* * +* 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. * -* * +* 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 @@ -33,251 +33,251 @@ openglframework::VertexBufferObject Box::mVBOVertices(GL_ARRAY_BUFFER); openglframework::VertexBufferObject Box::mVBONormals(GL_ARRAY_BUFFER); openglframework::VertexArrayObject Box::mVAO; -int Box::totalNbBoxes = 0; +int32_t Box::totalNbBoxes = 0; GLfloat Box::mCubeVertices[108] = { - -1.0f,-1.0f,-1.0f, // triangle 1 : begin - -1.0f,-1.0f, 1.0f, - -1.0f, 1.0f, 1.0f, // triangle 1 : end - 1.0f, 1.0f,-1.0f, // triangle 2 : begin - -1.0f,-1.0f,-1.0f, - -1.0f, 1.0f,-1.0f, // triangle 2 : end - 1.0f,-1.0f, 1.0f, - -1.0f,-1.0f,-1.0f, - 1.0f,-1.0f,-1.0f, - 1.0f, 1.0f,-1.0f, - 1.0f,-1.0f,-1.0f, - -1.0f,-1.0f,-1.0f, - -1.0f,-1.0f,-1.0f, - -1.0f, 1.0f, 1.0f, - -1.0f, 1.0f,-1.0f, - 1.0f,-1.0f, 1.0f, - -1.0f,-1.0f, 1.0f, - -1.0f,-1.0f,-1.0f, - -1.0f, 1.0f, 1.0f, - -1.0f,-1.0f, 1.0f, - 1.0f,-1.0f, 1.0f, - 1.0f, 1.0f, 1.0f, - 1.0f,-1.0f,-1.0f, - 1.0f, 1.0f,-1.0f, - 1.0f,-1.0f,-1.0f, - 1.0f, 1.0f, 1.0f, - 1.0f,-1.0f, 1.0f, - 1.0f, 1.0f, 1.0f, - 1.0f, 1.0f,-1.0f, - -1.0f, 1.0f,-1.0f, - 1.0f, 1.0f, 1.0f, - -1.0f, 1.0f,-1.0f, - -1.0f, 1.0f, 1.0f, - 1.0f, 1.0f, 1.0f, - -1.0f, 1.0f, 1.0f, - 1.0f,-1.0f, 1.0f + -1.0f,-1.0f,-1.0f, // triangle 1 : begin + -1.0f,-1.0f, 1.0f, + -1.0f, 1.0f, 1.0f, // triangle 1 : end + 1.0f, 1.0f,-1.0f, // triangle 2 : begin + -1.0f,-1.0f,-1.0f, + -1.0f, 1.0f,-1.0f, // triangle 2 : end + 1.0f,-1.0f, 1.0f, + -1.0f,-1.0f,-1.0f, + 1.0f,-1.0f,-1.0f, + 1.0f, 1.0f,-1.0f, + 1.0f,-1.0f,-1.0f, + -1.0f,-1.0f,-1.0f, + -1.0f,-1.0f,-1.0f, + -1.0f, 1.0f, 1.0f, + -1.0f, 1.0f,-1.0f, + 1.0f,-1.0f, 1.0f, + -1.0f,-1.0f, 1.0f, + -1.0f,-1.0f,-1.0f, + -1.0f, 1.0f, 1.0f, + -1.0f,-1.0f, 1.0f, + 1.0f,-1.0f, 1.0f, + 1.0f, 1.0f, 1.0f, + 1.0f,-1.0f,-1.0f, + 1.0f, 1.0f,-1.0f, + 1.0f,-1.0f,-1.0f, + 1.0f, 1.0f, 1.0f, + 1.0f,-1.0f, 1.0f, + 1.0f, 1.0f, 1.0f, + 1.0f, 1.0f,-1.0f, + -1.0f, 1.0f,-1.0f, + 1.0f, 1.0f, 1.0f, + -1.0f, 1.0f,-1.0f, + -1.0f, 1.0f, 1.0f, + 1.0f, 1.0f, 1.0f, + -1.0f, 1.0f, 1.0f, + 1.0f,-1.0f, 1.0f }; GLfloat Box::mCubeNormals[108] = { - -1.0f, 0.0f, 0.0f, // triangle 1 : begin - -1.0f, 0.0f, 0.0f, - -1.0f, 0.0f, 0.0f, // triangle 1 : end - 0.0f, 0.0f,-1.0f, // triangle 2 : begin - 0.0f, 0.0f,-1.0f, - 0.0f, 0.0f,-1.0f, // triangle 2 : end - 0.0f,-1.0f, 0.0f, - 0.0f,-1.0f, 0.0f, - 0.0f,-1.0f, 0.0f,// - 0.0f, 0.0f,-1.0f, - 0.0f, 0.0f,-1.0f, - 0.0f, 0.0f,-1.0f,// - -1.0f, 0.0f, 0.0f, - -1.0f, 0.0f, 0.0f, - -1.0f, 0.0f,0.0f,// - 0.0f,-1.0f, 0.0f, - 0.0f,-1.0f, 0.0f, - 0.0f,-1.0f, 0.0f,// - 0.0f, 0.0f, 1.0f, - 0.0f, 0.0f, 1.0f, - 0.0f, 0.0f, 1.0f,// - 1.0f, 0.0f, 0.0f, - 1.0f, 0.0f, 0.0f, - 1.0f, 0.0f, 0.0f,// - 1.0f, 0.0f, 0.0f, - 1.0f, 0.0f, 0.0f, - 1.0f, 0.0f, 0.0f,// - 0.0f, 1.0f, 0.0f, - 0.0f, 1.0f, 0.0f, - 0.0f, 1.0f, 0.0f,// - 0.0f, 1.0f, 0.0f, - 0.0f, 1.0f, 0.0f, - 0.0f, 1.0f, 0.0f,// - 0.0f, 0.0f, 1.0f, - 0.0f, 0.0f, 1.0f, - 0.0f, 0.0f, 1.0f// + -1.0f, 0.0f, 0.0f, // triangle 1 : begin + -1.0f, 0.0f, 0.0f, + -1.0f, 0.0f, 0.0f, // triangle 1 : end + 0.0f, 0.0f,-1.0f, // triangle 2 : begin + 0.0f, 0.0f,-1.0f, + 0.0f, 0.0f,-1.0f, // triangle 2 : end + 0.0f,-1.0f, 0.0f, + 0.0f,-1.0f, 0.0f, + 0.0f,-1.0f, 0.0f,// + 0.0f, 0.0f,-1.0f, + 0.0f, 0.0f,-1.0f, + 0.0f, 0.0f,-1.0f,// + -1.0f, 0.0f, 0.0f, + -1.0f, 0.0f, 0.0f, + -1.0f, 0.0f,0.0f,// + 0.0f,-1.0f, 0.0f, + 0.0f,-1.0f, 0.0f, + 0.0f,-1.0f, 0.0f,// + 0.0f, 0.0f, 1.0f, + 0.0f, 0.0f, 1.0f, + 0.0f, 0.0f, 1.0f,// + 1.0f, 0.0f, 0.0f, + 1.0f, 0.0f, 0.0f, + 1.0f, 0.0f, 0.0f,// + 1.0f, 0.0f, 0.0f, + 1.0f, 0.0f, 0.0f, + 1.0f, 0.0f, 0.0f,// + 0.0f, 1.0f, 0.0f, + 0.0f, 1.0f, 0.0f, + 0.0f, 1.0f, 0.0f,// + 0.0f, 1.0f, 0.0f, + 0.0f, 1.0f, 0.0f, + 0.0f, 1.0f, 0.0f,// + 0.0f, 0.0f, 1.0f, + 0.0f, 0.0f, 1.0f, + 0.0f, 0.0f, 1.0f// }; // Constructor Box::Box(const openglframework::Vector3& size, const openglframework::Vector3 &position, - reactphysics3d::CollisionWorld* world) - : openglframework::Object3D() { + reactphysics3d::CollisionWorld* world) + : openglframework::Object3D() { - // Initialize the size of the box - mSize[0] = size.x * 0.5f; - mSize[1] = size.y * 0.5f; - mSize[2] = size.z * 0.5f; + // Initialize the size of the box + mSize[0] = size.x * 0.5f; + mSize[1] = size.y * 0.5f; + mSize[2] = size.z * 0.5f; - // Compute the scaling matrix - mScalingMatrix = openglframework::Matrix4(mSize[0], 0, 0, 0, - 0, mSize[1], 0, 0, - 0, 0, mSize[2], 0, - 0, 0, 0, 1); + // Compute the scaling matrix + mScalingMatrix = openglframework::Matrix4(mSize[0], 0, 0, 0, + 0, mSize[1], 0, 0, + 0, 0, mSize[2], 0, + 0, 0, 0, 1); - // Initialize the position where the cube will be rendered - translateWorld(position); + // Initialize the position where the cube will be rendered + translateWorld(position); - // Create the collision shape for the rigid body (box shape) - // ReactPhysics3D will clone this object to create an internal one. Therefore, - // it is OK if this object is destroyed right after calling RigidBody::addCollisionShape() - mBoxShape = new rp3d::BoxShape(rp3d::Vector3(mSize[0], mSize[1], mSize[2])); + // Create the collision shape for the rigid body (box shape) + // ReactPhysics3D will clone this object to create an int32_ternal one. Therefore, + // it is OK if this object is destroyed right after calling RigidBody::addCollisionShape() + mBoxShape = new rp3d::BoxShape(rp3d::Vector3(mSize[0], mSize[1], mSize[2])); - // Initial position and orientation of the rigid body - rp3d::Vector3 initPosition(position.x, position.y, position.z); - rp3d::Quaternion initOrientation = rp3d::Quaternion::identity(); - rp3d::Transform transform(initPosition, initOrientation); + // Initial position and orientation of the rigid body + rp3d::Vector3 initPosition(position.x, position.y, position.z); + rp3d::Quaternion initOrientation = rp3d::Quaternion::identity(); + rp3d::Transform transform(initPosition, initOrientation); - mPreviousTransform = transform; + mPreviousTransform = transform; - // Create a rigid body in the dynamics world - mBody = world->createCollisionBody(transform); + // Create a rigid body in the dynamics world + mBody = world->createCollisionBody(transform); - // Add the collision shape to the body - m_proxyShape = mBody->addCollisionShape(mBoxShape, rp3d::Transform::identity()); + // Add the collision shape to the body + m_proxyShape = mBody->addCollisionShape(mBoxShape, rp3d::Transform::identity()); - // If the Vertex Buffer object has not been created yet - if (totalNbBoxes == 0) { + // If the Vertex Buffer object has not been created yet + if (totalNbBoxes == 0) { - // Create the Vertex Buffer - createVBOAndVAO(); - } + // Create the Vertex Buffer + createVBOAndVAO(); + } - totalNbBoxes++; + totalNbBoxes++; - mTransformMatrix = mTransformMatrix * mScalingMatrix; + mTransformMatrix = mTransformMatrix * mScalingMatrix; } // Constructor Box::Box(const openglframework::Vector3& size, const openglframework::Vector3& position, - float mass, reactphysics3d::DynamicsWorld* world) - : openglframework::Object3D() { + float mass, reactphysics3d::DynamicsWorld* world) + : openglframework::Object3D() { - // Initialize the size of the box - mSize[0] = size.x * 0.5f; - mSize[1] = size.y * 0.5f; - mSize[2] = size.z * 0.5f; + // Initialize the size of the box + mSize[0] = size.x * 0.5f; + mSize[1] = size.y * 0.5f; + mSize[2] = size.z * 0.5f; - // Compute the scaling matrix - mScalingMatrix = openglframework::Matrix4(mSize[0], 0, 0, 0, - 0, mSize[1], 0, 0, - 0, 0, mSize[2], 0, - 0, 0, 0, 1); + // Compute the scaling matrix + mScalingMatrix = openglframework::Matrix4(mSize[0], 0, 0, 0, + 0, mSize[1], 0, 0, + 0, 0, mSize[2], 0, + 0, 0, 0, 1); - // Initialize the position where the cube will be rendered - translateWorld(position); + // Initialize the position where the cube will be rendered + translateWorld(position); - // Create the collision shape for the rigid body (box shape) - // ReactPhysics3D will clone this object to create an internal one. Therefore, - // it is OK if this object is destroyed right after calling RigidBody::addCollisionShape() - mBoxShape = new rp3d::BoxShape(rp3d::Vector3(mSize[0], mSize[1], mSize[2])); + // Create the collision shape for the rigid body (box shape) + // ReactPhysics3D will clone this object to create an int32_ternal one. Therefore, + // it is OK if this object is destroyed right after calling RigidBody::addCollisionShape() + mBoxShape = new rp3d::BoxShape(rp3d::Vector3(mSize[0], mSize[1], mSize[2])); - // Initial position and orientation of the rigid body - rp3d::Vector3 initPosition(position.x, position.y, position.z); - rp3d::Quaternion initOrientation = rp3d::Quaternion::identity(); - rp3d::Transform transform(initPosition, initOrientation); + // Initial position and orientation of the rigid body + rp3d::Vector3 initPosition(position.x, position.y, position.z); + rp3d::Quaternion initOrientation = rp3d::Quaternion::identity(); + rp3d::Transform transform(initPosition, initOrientation); - mPreviousTransform = transform; + mPreviousTransform = transform; - // Create a rigid body in the dynamics world - rp3d::RigidBody* body = world->createRigidBody(transform); + // Create a rigid body in the dynamics world + rp3d::RigidBody* body = world->createRigidBody(transform); - // Add the collision shape to the body - m_proxyShape = body->addCollisionShape(mBoxShape, rp3d::Transform::identity(), mass); + // Add the collision shape to the body + m_proxyShape = body->addCollisionShape(mBoxShape, rp3d::Transform::identity(), mass); - mBody = body; + mBody = body; - // If the Vertex Buffer object has not been created yet - if (totalNbBoxes == 0) { + // If the Vertex Buffer object has not been created yet + if (totalNbBoxes == 0) { - // Create the Vertex Buffer - createVBOAndVAO(); - } + // Create the Vertex Buffer + createVBOAndVAO(); + } - totalNbBoxes++; + totalNbBoxes++; - mTransformMatrix = mTransformMatrix * mScalingMatrix; + mTransformMatrix = mTransformMatrix * mScalingMatrix; } // Destructor Box::~Box() { - if (totalNbBoxes == 1) { + if (totalNbBoxes == 1) { - // Destroy the VBOs and VAO - mVBOVertices.destroy(); - mVBONormals.destroy(); - mVAO.destroy(); - } - delete mBoxShape; - totalNbBoxes--; + // Destroy the VBOs and VAO + mVBOVertices.destroy(); + mVBONormals.destroy(); + mVAO.destroy(); + } + delete mBoxShape; + totalNbBoxes--; } // Render the cube at the correct position and with the correct orientation void Box::render(openglframework::Shader& shader, - const openglframework::Matrix4& worldToCameraMatrix) { + const openglframework::Matrix4& worldToCameraMatrix) { - // Bind the VAO - mVAO.bind(); + // Bind the VAO + mVAO.bind(); - // Bind the shader - shader.bind(); + // Bind the shader + shader.bind(); - mVBOVertices.bind(); + mVBOVertices.bind(); - // Set the model to camera matrix - shader.setMatrix4x4Uniform("localToWorldMatrix", mTransformMatrix); - shader.setMatrix4x4Uniform("worldToCameraMatrix", worldToCameraMatrix); + // Set the model to camera matrix + shader.setMatrix4x4Uniform("localToWorldMatrix", mTransformMatrix); + shader.setMatrix4x4Uniform("worldToCameraMatrix", worldToCameraMatrix); - // Set the normal matrix (inverse transpose of the 3x3 upper-left sub matrix of the - // model-view matrix) - const openglframework::Matrix4 localToCameraMatrix = worldToCameraMatrix * mTransformMatrix; - const openglframework::Matrix3 normalMatrix = - localToCameraMatrix.getUpperLeft3x3Matrix().getInverse().getTranspose(); - shader.setMatrix3x3Uniform("normalMatrix", normalMatrix, false); + // Set the normal matrix (inverse transpose of the 3x3 upper-left sub matrix of the + // model-view matrix) + const openglframework::Matrix4 localToCameraMatrix = worldToCameraMatrix * mTransformMatrix; + const openglframework::Matrix3 normalMatrix = + localToCameraMatrix.getUpperLeft3x3Matrix().getInverse().getTranspose(); + shader.setMatrix3x3Uniform("normalMatrix", normalMatrix, false); - // Set the vertex color - openglframework::Color currentColor = mBody->isSleeping() ? mSleepingColor : mColor; - openglframework::Vector4 color(currentColor.r, currentColor.g, currentColor.b, currentColor.a); - shader.setVector4Uniform("vertexColor", color, false); + // Set the vertex color + openglframework::Color currentColor = mBody->isSleeping() ? mSleepingColor : mColor; + openglframework::Vector4 color(currentColor.r, currentColor.g, currentColor.b, currentColor.a); + shader.setVector4Uniform("vertexColor", color, false); - // Get the location of shader attribute variables - GLint vertexPositionLoc = shader.getAttribLocation("vertexPosition"); - GLint vertexNormalLoc = shader.getAttribLocation("vertexNormal", false); + // Get the location of shader attribute variables + GLint32_t vertexPositionLoc = shader.getAttribLocation("vertexPosition"); + GLint32_t vertexNormalLoc = shader.getAttribLocation("vertexNormal", false); - glEnableVertexAttribArray(vertexPositionLoc); - glVertexAttribPointer(vertexPositionLoc, 3, GL_FLOAT, GL_FALSE, 0, NULL); + glEnableVertexAttribArray(vertexPositionLoc); + glVertexAttribPointer(vertexPositionLoc, 3, GL_FLOAT, GL_FALSE, 0, NULL); - mVBONormals.bind(); + mVBONormals.bind(); - if (vertexNormalLoc != -1) glEnableVertexAttribArray(vertexNormalLoc); - if (vertexNormalLoc != -1) glVertexAttribPointer(vertexNormalLoc, 3, GL_FLOAT, GL_FALSE, 0, NULL); + if (vertexNormalLoc != -1) glEnableVertexAttribArray(vertexNormalLoc); + if (vertexNormalLoc != -1) glVertexAttribPointer(vertexNormalLoc, 3, GL_FLOAT, GL_FALSE, 0, NULL); - // Draw the geometry of the box - glDrawArrays(GL_TRIANGLES, 0, 36); + // Draw the geometry of the box + glDrawArrays(GL_TRIANGLES, 0, 36); - glDisableVertexAttribArray(vertexPositionLoc); - if (vertexNormalLoc != -1) glDisableVertexAttribArray(vertexNormalLoc); + glDisableVertexAttribArray(vertexPositionLoc); + if (vertexNormalLoc != -1) glDisableVertexAttribArray(vertexNormalLoc); - mVBONormals.unbind(); - mVBOVertices.unbind(); + mVBONormals.unbind(); + mVBOVertices.unbind(); - // Unbind the VAO - mVAO.unbind(); + // Unbind the VAO + mVAO.unbind(); - // Unbind the shader - shader.unbind(); + // Unbind the shader + shader.unbind(); } // Create the Vertex Buffer Objects used to render to box with OpenGL. @@ -285,59 +285,59 @@ void Box::render(openglframework::Shader& shader, /// in the simulation. void Box::createVBOAndVAO() { - // Create the VBO for the vertices data - mVBOVertices.create(); - mVBOVertices.bind(); - mVBOVertices.copyDataIntoVBO(sizeof(mCubeVertices), mCubeVertices, GL_STATIC_DRAW); - mVBOVertices.unbind(); + // Create the VBO for the vertices data + mVBOVertices.create(); + mVBOVertices.bind(); + mVBOVertices.copyDataIntoVBO(sizeof(mCubeVertices), mCubeVertices, GL_STATIC_DRAW); + mVBOVertices.unbind(); - // Create th VBO for the normals data - mVBONormals.create(); - mVBONormals.bind(); - mVBONormals.copyDataIntoVBO(sizeof(mCubeNormals), mCubeNormals, GL_STATIC_DRAW); - mVBONormals.unbind(); + // Create th VBO for the normals data + mVBONormals.create(); + mVBONormals.bind(); + mVBONormals.copyDataIntoVBO(sizeof(mCubeNormals), mCubeNormals, GL_STATIC_DRAW); + mVBONormals.unbind(); - // Create the VAO for both VBOs - mVAO.create(); - mVAO.bind(); + // Create the VAO for both VBOs + mVAO.create(); + mVAO.bind(); - // Bind the VBO of vertices - mVBOVertices.bind(); + // Bind the VBO of vertices + mVBOVertices.bind(); - // Bind the VBO of indices - mVBONormals.bind(); + // Bind the VBO of indices + mVBONormals.bind(); - // Unbind the VAO - mVAO.unbind(); + // Unbind the VAO + mVAO.unbind(); } // Reset the transform void Box::resetTransform(const rp3d::Transform& transform) { - // Reset the transform - mBody->setTransform(transform); + // Reset the transform + mBody->setTransform(transform); - mBody->setIsSleeping(false); + mBody->setIsSleeping(false); - // Reset the velocity of the rigid body - rp3d::RigidBody* rigidBody = dynamic_cast(mBody); - if (rigidBody != NULL) { - rigidBody->setLinearVelocity(rp3d::Vector3(0, 0, 0)); - rigidBody->setAngularVelocity(rp3d::Vector3(0, 0, 0)); - } + // Reset the velocity of the rigid body + rp3d::RigidBody* rigidBody = dynamic_cast(mBody); + if (rigidBody != NULL) { + rigidBody->setLinearVelocity(rp3d::Vector3(0, 0, 0)); + rigidBody->setAngularVelocity(rp3d::Vector3(0, 0, 0)); + } - updateTransform(1.0f); + updateTransform(1.0f); } // Set the scaling of the object void Box::setScaling(const openglframework::Vector3& scaling) { - // Scale the collision shape - m_proxyShape->setLocalScaling(rp3d::Vector3(scaling.x, scaling.y, scaling.z)); + // Scale the collision shape + m_proxyShape->setLocalScaling(rp3d::Vector3(scaling.x, scaling.y, scaling.z)); - // Scale the graphics object - mScalingMatrix = openglframework::Matrix4(mSize[0] * scaling.x, 0, 0, 0, - 0, mSize[1] * scaling.y, 0, 0, - 0, 0, mSize[2] * scaling.z, 0, - 0, 0, 0, 1); + // Scale the graphics object + mScalingMatrix = openglframework::Matrix4(mSize[0] * scaling.x, 0, 0, 0, + 0, mSize[1] * scaling.y, 0, 0, + 0, 0, mSize[2] * scaling.z, 0, + 0, 0, 0, 1); } diff --git a/tools/testbed/common/Box.h b/tools/testbed/common/Box.h index 6f44716..fd463e0 100644 --- a/tools/testbed/common/Box.h +++ b/tools/testbed/common/Box.h @@ -1,26 +1,26 @@ /******************************************************************************** - * ReactPhysics3D physics library, http://www.reactphysics3d.com * - * Copyright (c) 2010-2016 Daniel Chappuis * + * 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: * - * * + * 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. * - * * + * 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 BOX_H @@ -41,11 +41,11 @@ class Box : public openglframework::Object3D, public PhysicsObject { /// Size of each side of the box float mSize[3]; - rp3d::BoxShape* mBoxShape; - rp3d::ProxyShape* m_proxyShape; + rp3d::BoxShape* mBoxShape; + rp3d::ProxyShape* m_proxyShape; - /// Scaling matrix (applied to a cube to obtain the correct box dimensions) - openglframework::Matrix4 mScalingMatrix; + /// Scaling matrix (applied to a cube to obtain the correct box dimensions) + openglframework::Matrix4 mScalingMatrix; /// Vertex Buffer Object for the vertices data used to render the box with OpenGL static openglframework::VertexBufferObject mVBOVertices; @@ -63,7 +63,7 @@ class Box : public openglframework::Object3D, public PhysicsObject { static GLfloat mCubeNormals[108]; /// Total number of boxes created - static int totalNbBoxes; + static int32_t totalNbBoxes; // -------------------- Methods -------------------- // @@ -91,16 +91,16 @@ class Box : public openglframework::Object3D, public PhysicsObject { /// Set the position of the box void resetTransform(const rp3d::Transform& transform); - /// Update the transform matrix of the object - virtual void updateTransform(float interpolationFactor); + /// Update the transform matrix of the object + virtual void updateTransform(float int32_terpolationFactor); - /// Set the scaling of the object - void setScaling(const openglframework::Vector3& scaling); + /// Set the scaling of the object + void setScaling(const openglframework::Vector3& scaling); }; // Update the transform matrix of the object -inline void Box::updateTransform(float interpolationFactor) { - mTransformMatrix = computeTransform(interpolationFactor, mScalingMatrix); +inline void Box::updateTransform(float int32_terpolationFactor) { + mTransformMatrix = computeTransform(int32_terpolationFactor, mScalingMatrix); } diff --git a/tools/testbed/common/Capsule.cpp b/tools/testbed/common/Capsule.cpp index 11e810a..4e101a6 100644 --- a/tools/testbed/common/Capsule.cpp +++ b/tools/testbed/common/Capsule.cpp @@ -1,26 +1,26 @@ /******************************************************************************** -* ReactPhysics3D physics library, http://www.reactphysics3d.com * -* Copyright (c) 2010-2016 Daniel Chappuis * +* 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: * -* * +* 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. * -* * +* 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 @@ -31,266 +31,266 @@ openglframework::VertexBufferObject Capsule::mVBONormals(GL_ARRAY_BUFFER); openglframework::VertexBufferObject Capsule::mVBOTextureCoords(GL_ARRAY_BUFFER); openglframework::VertexBufferObject Capsule::mVBOIndices(GL_ELEMENT_ARRAY_BUFFER); openglframework::VertexArrayObject Capsule::mVAO; -int Capsule::totalNbCapsules = 0; +int32_t Capsule::totalNbCapsules = 0; // Constructor Capsule::Capsule(float radius, float height, const openglframework::Vector3& position, - reactphysics3d::CollisionWorld* world, - const std::string& meshFolderPath) - : openglframework::Mesh(), mRadius(radius), mHeight(height) { + reactphysics3d::CollisionWorld* world, + const std::string& meshFolderPath) + : openglframework::Mesh(), mRadius(radius), mHeight(height) { - // Load the mesh from a file - openglframework::MeshReaderWriter::loadMeshFromFile(meshFolderPath + "capsule.obj", *this); + // Load the mesh from a file + openglframework::MeshReaderWriter::loadMeshFromFile(meshFolderPath + "capsule.obj", *this); - // Calculate the normals of the mesh - calculateNormals(); + // Calculate the normals of the mesh + calculateNormals(); - // Compute the scaling matrix - mScalingMatrix = openglframework::Matrix4(mRadius, 0, 0, 0, - 0, (mHeight + 2.0f * mRadius) / 3, 0,0, - 0, 0, mRadius, 0, - 0, 0, 0, 1.0f); + // Compute the scaling matrix + mScalingMatrix = openglframework::Matrix4(mRadius, 0, 0, 0, + 0, (mHeight + 2.0f * mRadius) / 3, 0,0, + 0, 0, mRadius, 0, + 0, 0, 0, 1.0f); - // Initialize the position where the sphere will be rendered - translateWorld(position); + // Initialize the position where the sphere will be rendered + translateWorld(position); - // Create the collision shape for the rigid body (sphere shape) - // ReactPhysics3D will clone this object to create an internal one. Therefore, - // it is OK if this object is destroyed right after calling RigidBody::addCollisionShape() - mCapsuleShape = new rp3d::CapsuleShape(mRadius, mHeight); + // Create the collision shape for the rigid body (sphere shape) + // ReactPhysics3D will clone this object to create an int32_ternal one. Therefore, + // it is OK if this object is destroyed right after calling RigidBody::addCollisionShape() + mCapsuleShape = new rp3d::CapsuleShape(mRadius, mHeight); - // Initial position and orientation of the rigid body - rp3d::Vector3 initPosition(position.x, position.y, position.z); - rp3d::Quaternion initOrientation = rp3d::Quaternion::identity(); - rp3d::Transform transform(initPosition, initOrientation); + // Initial position and orientation of the rigid body + rp3d::Vector3 initPosition(position.x, position.y, position.z); + rp3d::Quaternion initOrientation = rp3d::Quaternion::identity(); + rp3d::Transform transform(initPosition, initOrientation); - mPreviousTransform = transform; + mPreviousTransform = transform; - // Create a rigid body corresponding in the dynamics world - mBody = world->createCollisionBody(transform); + // Create a rigid body corresponding in the dynamics world + mBody = world->createCollisionBody(transform); - // Add a collision shape to the body and specify the mass of the shape - m_proxyShape = mBody->addCollisionShape(mCapsuleShape, rp3d::Transform::identity()); + // Add a collision shape to the body and specify the mass of the shape + m_proxyShape = mBody->addCollisionShape(mCapsuleShape, rp3d::Transform::identity()); - mTransformMatrix = mTransformMatrix * mScalingMatrix; + mTransformMatrix = mTransformMatrix * mScalingMatrix; - // Create the VBOs and VAO - if (totalNbCapsules == 0) { - createVBOAndVAO(); - } + // Create the VBOs and VAO + if (totalNbCapsules == 0) { + createVBOAndVAO(); + } - totalNbCapsules++; + totalNbCapsules++; } // Constructor Capsule::Capsule(float radius, float height, const openglframework::Vector3& position, - float mass, reactphysics3d::DynamicsWorld* dynamicsWorld, - const std::string& meshFolderPath) - : openglframework::Mesh(), mRadius(radius), mHeight(height) { + float mass, reactphysics3d::DynamicsWorld* dynamicsWorld, + const std::string& meshFolderPath) + : openglframework::Mesh(), mRadius(radius), mHeight(height) { - // Load the mesh from a file - openglframework::MeshReaderWriter::loadMeshFromFile(meshFolderPath + "capsule.obj", *this); + // Load the mesh from a file + openglframework::MeshReaderWriter::loadMeshFromFile(meshFolderPath + "capsule.obj", *this); - // Calculate the normals of the mesh - calculateNormals(); + // Calculate the normals of the mesh + calculateNormals(); - // Compute the scaling matrix - mScalingMatrix = openglframework::Matrix4(mRadius, 0, 0, 0, - 0, (mHeight + 2.0f * mRadius) / 3.0f, 0,0, - 0, 0, mRadius, 0, - 0, 0, 0, 1.0f); + // Compute the scaling matrix + mScalingMatrix = openglframework::Matrix4(mRadius, 0, 0, 0, + 0, (mHeight + 2.0f * mRadius) / 3.0f, 0,0, + 0, 0, mRadius, 0, + 0, 0, 0, 1.0f); - // Initialize the position where the sphere will be rendered - translateWorld(position); + // Initialize the position where the sphere will be rendered + translateWorld(position); - // Create the collision shape for the rigid body (sphere shape) - // ReactPhysics3D will clone this object to create an internal one. Therefore, - // it is OK if this object is destroyed right after calling RigidBody::addCollisionShape() - mCapsuleShape = new rp3d::CapsuleShape(mRadius, mHeight); + // Create the collision shape for the rigid body (sphere shape) + // ReactPhysics3D will clone this object to create an int32_ternal one. Therefore, + // it is OK if this object is destroyed right after calling RigidBody::addCollisionShape() + mCapsuleShape = new rp3d::CapsuleShape(mRadius, mHeight); - // Initial position and orientation of the rigid body - rp3d::Vector3 initPosition(position.x, position.y, position.z); - rp3d::Quaternion initOrientation = rp3d::Quaternion::identity(); - rp3d::Transform transform(initPosition, initOrientation); + // Initial position and orientation of the rigid body + rp3d::Vector3 initPosition(position.x, position.y, position.z); + rp3d::Quaternion initOrientation = rp3d::Quaternion::identity(); + rp3d::Transform transform(initPosition, initOrientation); - // Create a rigid body corresponding in the dynamics world - rp3d::RigidBody* body = dynamicsWorld->createRigidBody(transform); + // Create a rigid body corresponding in the dynamics world + rp3d::RigidBody* body = dynamicsWorld->createRigidBody(transform); - // Add a collision shape to the body and specify the mass of the shape - m_proxyShape = body->addCollisionShape(mCapsuleShape, rp3d::Transform::identity(), mass); + // Add a collision shape to the body and specify the mass of the shape + m_proxyShape = body->addCollisionShape(mCapsuleShape, rp3d::Transform::identity(), mass); - mBody = body; + mBody = body; - mTransformMatrix = mTransformMatrix * mScalingMatrix; + mTransformMatrix = mTransformMatrix * mScalingMatrix; - // Create the VBOs and VAO - if (totalNbCapsules == 0) { - createVBOAndVAO(); - } + // Create the VBOs and VAO + if (totalNbCapsules == 0) { + createVBOAndVAO(); + } - totalNbCapsules++; + totalNbCapsules++; } // Destructor Capsule::~Capsule() { - if (totalNbCapsules == 1) { + if (totalNbCapsules == 1) { - // Destroy the mesh - destroy(); + // Destroy the mesh + destroy(); - // Destroy the VBOs and VAO - mVBOIndices.destroy(); - mVBOVertices.destroy(); - mVBONormals.destroy(); - mVBOTextureCoords.destroy(); - mVAO.destroy(); - } - delete mCapsuleShape; - totalNbCapsules--; + // Destroy the VBOs and VAO + mVBOIndices.destroy(); + mVBOVertices.destroy(); + mVBONormals.destroy(); + mVBOTextureCoords.destroy(); + mVAO.destroy(); + } + delete mCapsuleShape; + totalNbCapsules--; } // Render the sphere at the correct position and with the correct orientation void Capsule::render(openglframework::Shader& shader, - const openglframework::Matrix4& worldToCameraMatrix) { + const openglframework::Matrix4& worldToCameraMatrix) { - // Bind the shader - shader.bind(); + // Bind the shader + shader.bind(); - // Set the model to camera matrix - shader.setMatrix4x4Uniform("localToWorldMatrix", mTransformMatrix); - shader.setMatrix4x4Uniform("worldToCameraMatrix", worldToCameraMatrix); + // Set the model to camera matrix + shader.setMatrix4x4Uniform("localToWorldMatrix", mTransformMatrix); + shader.setMatrix4x4Uniform("worldToCameraMatrix", worldToCameraMatrix); - // Set the normal matrix (inverse transpose of the 3x3 upper-left sub matrix of the - // model-view matrix) - const openglframework::Matrix4 localToCameraMatrix = worldToCameraMatrix * mTransformMatrix; - const openglframework::Matrix3 normalMatrix = - localToCameraMatrix.getUpperLeft3x3Matrix().getInverse().getTranspose(); - shader.setMatrix3x3Uniform("normalMatrix", normalMatrix, false); + // Set the normal matrix (inverse transpose of the 3x3 upper-left sub matrix of the + // model-view matrix) + const openglframework::Matrix4 localToCameraMatrix = worldToCameraMatrix * mTransformMatrix; + const openglframework::Matrix3 normalMatrix = + localToCameraMatrix.getUpperLeft3x3Matrix().getInverse().getTranspose(); + shader.setMatrix3x3Uniform("normalMatrix", normalMatrix, false); - // Set the vertex color - openglframework::Color currentColor = mBody->isSleeping() ? mSleepingColor : mColor; - openglframework::Vector4 color(currentColor.r, currentColor.g, currentColor.b, currentColor.a); - shader.setVector4Uniform("vertexColor", color, false); + // Set the vertex color + openglframework::Color currentColor = mBody->isSleeping() ? mSleepingColor : mColor; + openglframework::Vector4 color(currentColor.r, currentColor.g, currentColor.b, currentColor.a); + shader.setVector4Uniform("vertexColor", color, false); - // Bind the VAO - mVAO.bind(); + // Bind the VAO + mVAO.bind(); - mVBOVertices.bind(); + mVBOVertices.bind(); - // Get the location of shader attribute variables - GLint vertexPositionLoc = shader.getAttribLocation("vertexPosition"); - GLint vertexNormalLoc = shader.getAttribLocation("vertexNormal", false); + // Get the location of shader attribute variables + GLint32_t vertexPositionLoc = shader.getAttribLocation("vertexPosition"); + GLint32_t vertexNormalLoc = shader.getAttribLocation("vertexNormal", false); - glEnableVertexAttribArray(vertexPositionLoc); - glVertexAttribPointer(vertexPositionLoc, 3, GL_FLOAT, GL_FALSE, 0, (char*)NULL); + glEnableVertexAttribArray(vertexPositionLoc); + glVertexAttribPointer(vertexPositionLoc, 3, GL_FLOAT, GL_FALSE, 0, (char*)NULL); - mVBONormals.bind(); + mVBONormals.bind(); - if (vertexNormalLoc != -1) glVertexAttribPointer(vertexNormalLoc, 3, GL_FLOAT, GL_FALSE, 0, (char*)NULL); - if (vertexNormalLoc != -1) glEnableVertexAttribArray(vertexNormalLoc); + if (vertexNormalLoc != -1) glVertexAttribPointer(vertexNormalLoc, 3, GL_FLOAT, GL_FALSE, 0, (char*)NULL); + if (vertexNormalLoc != -1) glEnableVertexAttribArray(vertexNormalLoc); - // For each part of the mesh - for (unsigned int i=0; isetTransform(transform); + // Reset the transform + mBody->setTransform(transform); - mBody->setIsSleeping(false); + mBody->setIsSleeping(false); - // Reset the velocity of the rigid body - rp3d::RigidBody* rigidBody = dynamic_cast(mBody); - if (rigidBody != NULL) { - rigidBody->setLinearVelocity(rp3d::Vector3(0, 0, 0)); - rigidBody->setAngularVelocity(rp3d::Vector3(0, 0, 0)); - } + // Reset the velocity of the rigid body + rp3d::RigidBody* rigidBody = dynamic_cast(mBody); + if (rigidBody != NULL) { + rigidBody->setLinearVelocity(rp3d::Vector3(0, 0, 0)); + rigidBody->setAngularVelocity(rp3d::Vector3(0, 0, 0)); + } - updateTransform(1.0f); + updateTransform(1.0f); } // Set the scaling of the object void Capsule::setScaling(const openglframework::Vector3& scaling) { - // Scale the collision shape - m_proxyShape->setLocalScaling(rp3d::Vector3(scaling.x, scaling.y, scaling.z)); + // Scale the collision shape + m_proxyShape->setLocalScaling(rp3d::Vector3(scaling.x, scaling.y, scaling.z)); - // Scale the graphics object - mScalingMatrix = openglframework::Matrix4(mRadius * scaling.x, 0, 0, 0, - 0, (mHeight * scaling.y + 2.0f * mRadius * scaling.x) / 3, 0,0, - 0, 0, mRadius * scaling.x, 0, - 0, 0, 0, 1.0f); + // Scale the graphics object + mScalingMatrix = openglframework::Matrix4(mRadius * scaling.x, 0, 0, 0, + 0, (mHeight * scaling.y + 2.0f * mRadius * scaling.x) / 3, 0,0, + 0, 0, mRadius * scaling.x, 0, + 0, 0, 0, 1.0f); } diff --git a/tools/testbed/common/Capsule.h b/tools/testbed/common/Capsule.h index 9a879b6..5ad452d 100644 --- a/tools/testbed/common/Capsule.h +++ b/tools/testbed/common/Capsule.h @@ -1,26 +1,26 @@ /******************************************************************************** - * ReactPhysics3D physics library, http://www.reactphysics3d.com * - * Copyright (c) 2010-2016 Daniel Chappuis * + * 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: * - * * + * 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. * - * * + * 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 CAPSULE_H @@ -47,11 +47,11 @@ class Capsule : public openglframework::Mesh, public PhysicsObject { /// Scaling matrix (applied to a sphere to obtain the correct sphere dimensions) openglframework::Matrix4 mScalingMatrix; - /// Collision shape - rp3d::CapsuleShape* mCapsuleShape; - rp3d::ProxyShape* m_proxyShape; + /// Collision shape + rp3d::CapsuleShape* mCapsuleShape; + rp3d::ProxyShape* m_proxyShape; - /// Previous transform (for interpolation) + /// Previous transform (for int32_terpolation) rp3d::Transform mPreviousTransform; /// Vertex Buffer Object for the vertices data @@ -70,7 +70,7 @@ class Capsule : public openglframework::Mesh, public PhysicsObject { static openglframework::VertexArrayObject mVAO; // Total number of capsules created - static int totalNbCapsules; + static int32_t totalNbCapsules; // -------------------- Methods -------------------- // @@ -100,16 +100,16 @@ class Capsule : public openglframework::Mesh, public PhysicsObject { /// Set the position of the box void resetTransform(const rp3d::Transform& transform); - /// Update the transform matrix of the object - virtual void updateTransform(float interpolationFactor); + /// Update the transform matrix of the object + virtual void updateTransform(float int32_terpolationFactor); - /// Set the scaling of the object - void setScaling(const openglframework::Vector3& scaling); + /// Set the scaling of the object + void setScaling(const openglframework::Vector3& scaling); }; // Update the transform matrix of the object -inline void Capsule::updateTransform(float interpolationFactor) { - mTransformMatrix = computeTransform(interpolationFactor, mScalingMatrix); +inline void Capsule::updateTransform(float int32_terpolationFactor) { + mTransformMatrix = computeTransform(int32_terpolationFactor, mScalingMatrix); } #endif diff --git a/tools/testbed/common/ConcaveMesh.cpp b/tools/testbed/common/ConcaveMesh.cpp index dc84dc9..bd09f65 100644 --- a/tools/testbed/common/ConcaveMesh.cpp +++ b/tools/testbed/common/ConcaveMesh.cpp @@ -1,26 +1,26 @@ /******************************************************************************** -* ReactPhysics3D physics library, http://www.reactphysics3d.com * -* Copyright (c) 2010-2016 Daniel Chappuis * +* 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: * -* * +* 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. * -* * +* 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 @@ -28,283 +28,283 @@ // Constructor ConcaveMesh::ConcaveMesh(const openglframework::Vector3 &position, - reactphysics3d::CollisionWorld* world, - const std::string& meshPath) - : openglframework::Mesh(), mVBOVertices(GL_ARRAY_BUFFER), - mVBONormals(GL_ARRAY_BUFFER), mVBOTextureCoords(GL_ARRAY_BUFFER), - mVBOIndices(GL_ELEMENT_ARRAY_BUFFER) { + reactphysics3d::CollisionWorld* world, + const std::string& meshPath) + : openglframework::Mesh(), mVBOVertices(GL_ARRAY_BUFFER), + mVBONormals(GL_ARRAY_BUFFER), mVBOTextureCoords(GL_ARRAY_BUFFER), + mVBOIndices(GL_ELEMENT_ARRAY_BUFFER) { - // Load the mesh from a file - openglframework::MeshReaderWriter::loadMeshFromFile(meshPath, *this); + // Load the mesh from a file + openglframework::MeshReaderWriter::loadMeshFromFile(meshPath, *this); - // Calculate the normals of the mesh - calculateNormals(); + // Calculate the normals of the mesh + calculateNormals(); - // Initialize the position where the sphere will be rendered - translateWorld(position); + // Initialize the position where the sphere will be rendered + translateWorld(position); - // Compute the scaling matrix - mScalingMatrix = openglframework::Matrix4::identity(); + // Compute the scaling matrix + mScalingMatrix = openglframework::Matrix4::identity(); - // For each subpart of the mesh - for (unsigned int i=0; icreateCollisionBody(transform); + // Create a rigid body corresponding to the sphere in the dynamics world + mBody = world->createCollisionBody(transform); - // Add a collision shape to the body and specify the mass of the collision shape - m_proxyShape = mBody->addCollisionShape(mConcaveShape, rp3d::Transform::identity()); + // Add a collision shape to the body and specify the mass of the collision shape + m_proxyShape = mBody->addCollisionShape(mConcaveShape, rp3d::Transform::identity()); - // Create the VBOs and VAO - createVBOAndVAO(); + // Create the VBOs and VAO + createVBOAndVAO(); - mTransformMatrix = mTransformMatrix * mScalingMatrix; + mTransformMatrix = mTransformMatrix * mScalingMatrix; } // Constructor ConcaveMesh::ConcaveMesh(const openglframework::Vector3 &position, float mass, - reactphysics3d::DynamicsWorld* dynamicsWorld, - const std::string& meshPath) - : openglframework::Mesh(), mVBOVertices(GL_ARRAY_BUFFER), - mVBONormals(GL_ARRAY_BUFFER), mVBOTextureCoords(GL_ARRAY_BUFFER), - mVBOIndices(GL_ELEMENT_ARRAY_BUFFER) { + reactphysics3d::DynamicsWorld* dynamicsWorld, + const std::string& meshPath) + : openglframework::Mesh(), mVBOVertices(GL_ARRAY_BUFFER), + mVBONormals(GL_ARRAY_BUFFER), mVBOTextureCoords(GL_ARRAY_BUFFER), + mVBOIndices(GL_ELEMENT_ARRAY_BUFFER) { - // Load the mesh from a file - openglframework::MeshReaderWriter::loadMeshFromFile(meshPath, *this); + // Load the mesh from a file + openglframework::MeshReaderWriter::loadMeshFromFile(meshPath, *this); - // Calculate the normals of the mesh - calculateNormals(); + // Calculate the normals of the mesh + calculateNormals(); - // Initialize the position where the sphere will be rendered - translateWorld(position); + // Initialize the position where the sphere will be rendered + translateWorld(position); - // Compute the scaling matrix - mScalingMatrix = openglframework::Matrix4::identity(); + // Compute the scaling matrix + mScalingMatrix = openglframework::Matrix4::identity(); - // For each subpart of the mesh - for (unsigned int i=0; isetIsSmoothMeshCollisionEnabled(false); + mConcaveShape->setIsSmoothMeshCollisionEnabled(false); - // Initial position and orientation of the rigid body - rp3d::Vector3 initPosition(position.x, position.y, position.z); - rp3d::Quaternion initOrientation = rp3d::Quaternion::identity(); - rp3d::Transform transform(initPosition, initOrientation); + // Initial position and orientation of the rigid body + rp3d::Vector3 initPosition(position.x, position.y, position.z); + rp3d::Quaternion initOrientation = rp3d::Quaternion::identity(); + rp3d::Transform transform(initPosition, initOrientation); - // Create a rigid body corresponding to the sphere in the dynamics world - rp3d::RigidBody* body = dynamicsWorld->createRigidBody(transform); + // Create a rigid body corresponding to the sphere in the dynamics world + rp3d::RigidBody* body = dynamicsWorld->createRigidBody(transform); - // Add a collision shape to the body and specify the mass of the collision shape - m_proxyShape = body->addCollisionShape(mConcaveShape, rp3d::Transform::identity(), mass); + // Add a collision shape to the body and specify the mass of the collision shape + m_proxyShape = body->addCollisionShape(mConcaveShape, rp3d::Transform::identity(), mass); - mBody = body; + mBody = body; - // Create the VBOs and VAO - createVBOAndVAO(); + // Create the VBOs and VAO + createVBOAndVAO(); - mTransformMatrix = mTransformMatrix * mScalingMatrix; + mTransformMatrix = mTransformMatrix * mScalingMatrix; } // Destructor ConcaveMesh::~ConcaveMesh() { - // Destroy the triangle mesh data for the physics engine - for (unsigned int i=0; iisSleeping() ? mSleepingColor : mColor; - openglframework::Vector4 color(currentColor.r, currentColor.g, currentColor.b, currentColor.a); - shader.setVector4Uniform("vertexColor", color, false); + // Set the vertex color + openglframework::Color currentColor = mBody->isSleeping() ? mSleepingColor : mColor; + openglframework::Vector4 color(currentColor.r, currentColor.g, currentColor.b, currentColor.a); + shader.setVector4Uniform("vertexColor", color, false); - // Bind the VAO - mVAO.bind(); + // Bind the VAO + mVAO.bind(); - mVBOVertices.bind(); + mVBOVertices.bind(); - // Get the location of shader attribute variables - GLint vertexPositionLoc = shader.getAttribLocation("vertexPosition"); - GLint vertexNormalLoc = shader.getAttribLocation("vertexNormal", false); + // Get the location of shader attribute variables + GLint32_t vertexPositionLoc = shader.getAttribLocation("vertexPosition"); + GLint32_t vertexNormalLoc = shader.getAttribLocation("vertexNormal", false); - glEnableVertexAttribArray(vertexPositionLoc); - glVertexAttribPointer(vertexPositionLoc, 3, GL_FLOAT, GL_FALSE, 0, (char*)NULL); + glEnableVertexAttribArray(vertexPositionLoc); + glVertexAttribPointer(vertexPositionLoc, 3, GL_FLOAT, GL_FALSE, 0, (char*)NULL); - mVBONormals.bind(); + mVBONormals.bind(); - if (vertexNormalLoc != -1) glVertexAttribPointer(vertexNormalLoc, 3, GL_FLOAT, GL_FALSE, 0, (char*)NULL); - if (vertexNormalLoc != -1) glEnableVertexAttribArray(vertexNormalLoc); + if (vertexNormalLoc != -1) glVertexAttribPointer(vertexNormalLoc, 3, GL_FLOAT, GL_FALSE, 0, (char*)NULL); + if (vertexNormalLoc != -1) glEnableVertexAttribArray(vertexNormalLoc); - // For each part of the mesh - for (unsigned int i=0; isetTransform(transform); + // Reset the transform + mBody->setTransform(transform); - mBody->setIsSleeping(false); + mBody->setIsSleeping(false); - // Reset the velocity of the rigid body - rp3d::RigidBody* rigidBody = dynamic_cast(mBody); - if (rigidBody != NULL) { - rigidBody->setLinearVelocity(rp3d::Vector3(0, 0, 0)); - rigidBody->setAngularVelocity(rp3d::Vector3(0, 0, 0)); - } + // Reset the velocity of the rigid body + rp3d::RigidBody* rigidBody = dynamic_cast(mBody); + if (rigidBody != NULL) { + rigidBody->setLinearVelocity(rp3d::Vector3(0, 0, 0)); + rigidBody->setAngularVelocity(rp3d::Vector3(0, 0, 0)); + } - updateTransform(1.0f); + updateTransform(1.0f); } // Set the scaling of the object void ConcaveMesh::setScaling(const openglframework::Vector3& scaling) { - // Scale the collision shape - m_proxyShape->setLocalScaling(rp3d::Vector3(scaling.x, scaling.y, scaling.z)); + // Scale the collision shape + m_proxyShape->setLocalScaling(rp3d::Vector3(scaling.x, scaling.y, scaling.z)); - // Scale the graphics object - mScalingMatrix = openglframework::Matrix4(scaling.x, 0, 0, 0, - 0, scaling.y, 0,0, - 0, 0, scaling.z, 0, - 0, 0, 0, 1.0f); + // Scale the graphics object + mScalingMatrix = openglframework::Matrix4(scaling.x, 0, 0, 0, + 0, scaling.y, 0,0, + 0, 0, scaling.z, 0, + 0, 0, 0, 1.0f); } diff --git a/tools/testbed/common/ConcaveMesh.h b/tools/testbed/common/ConcaveMesh.h index 6ef3f7c..90dbd6e 100644 --- a/tools/testbed/common/ConcaveMesh.h +++ b/tools/testbed/common/ConcaveMesh.h @@ -1,26 +1,26 @@ /******************************************************************************** -* ReactPhysics3D physics library, http://www.reactphysics3d.com * -* Copyright (c) 2010-2016 Daniel Chappuis * +* 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: * -* * +* 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. * -* * +* 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 CONCAVE_MESH_H @@ -34,75 +34,75 @@ // Class ConcaveMesh class ConcaveMesh : public openglframework::Mesh, public PhysicsObject { - private : + private : - // -------------------- Attributes -------------------- // + // -------------------- Attributes -------------------- // - /// Previous transform (for interpolation) - rp3d::Transform mPreviousTransform; + /// Previous transform (for int32_terpolation) + rp3d::Transform mPreviousTransform; - /// Collision shape - rp3d::ConcaveMeshShape* mConcaveShape; - rp3d::ProxyShape* m_proxyShape; + /// Collision shape + rp3d::ConcaveMeshShape* mConcaveShape; + rp3d::ProxyShape* m_proxyShape; - /// Scaling matrix - openglframework::Matrix4 mScalingMatrix; + /// Scaling matrix + openglframework::Matrix4 mScalingMatrix; - /// Vertex Buffer Object for the vertices data - openglframework::VertexBufferObject mVBOVertices; + /// Vertex Buffer Object for the vertices data + openglframework::VertexBufferObject mVBOVertices; - /// Vertex Buffer Object for the normals data - openglframework::VertexBufferObject mVBONormals; + /// Vertex Buffer Object for the normals data + openglframework::VertexBufferObject mVBONormals; - /// Vertex Buffer Object for the texture coords - openglframework::VertexBufferObject mVBOTextureCoords; + /// Vertex Buffer Object for the texture coords + openglframework::VertexBufferObject mVBOTextureCoords; - /// Vertex Buffer Object for the indices - openglframework::VertexBufferObject mVBOIndices; + /// Vertex Buffer Object for the indices + openglframework::VertexBufferObject mVBOIndices; - /// Vertex Array Object for the vertex data - openglframework::VertexArrayObject mVAO; + /// Vertex Array Object for the vertex data + openglframework::VertexArrayObject mVAO; - /// Structure with pointer to the shared mesh data (vertices, indices, ...) - rp3d::TriangleMesh mPhysicsTriangleMesh; + /// Structure with pointer to the shared mesh data (vertices, indices, ...) + rp3d::TriangleMesh mPhysicsTriangleMesh; - // -------------------- Methods -------------------- // + // -------------------- Methods -------------------- // - // Create the Vertex Buffer Objects used to render with OpenGL. - void createVBOAndVAO(); + // Create the Vertex Buffer Objects used to render with OpenGL. + void createVBOAndVAO(); - public : + public : - // -------------------- Methods -------------------- // + // -------------------- Methods -------------------- // - /// Constructor - ConcaveMesh(const openglframework::Vector3& position, - rp3d::CollisionWorld* world, const std::string& meshPath); + /// Constructor + ConcaveMesh(const openglframework::Vector3& position, + rp3d::CollisionWorld* world, const std::string& meshPath); - /// Constructor - ConcaveMesh(const openglframework::Vector3& position, float mass, - rp3d::DynamicsWorld* dynamicsWorld, const std::string& meshPath); + /// Constructor + ConcaveMesh(const openglframework::Vector3& position, float mass, + rp3d::DynamicsWorld* dynamicsWorld, const std::string& meshPath); - /// Destructor - ~ConcaveMesh(); + /// Destructor + ~ConcaveMesh(); - /// Render the mesh at the correct position and with the correct orientation - void render(openglframework::Shader& shader, - const openglframework::Matrix4& worldToCameraMatrix); + /// Render the mesh at the correct position and with the correct orientation + void render(openglframework::Shader& shader, + const openglframework::Matrix4& worldToCameraMatrix); - /// Set the position of the box - void resetTransform(const rp3d::Transform& transform); + /// Set the position of the box + void resetTransform(const rp3d::Transform& transform); - /// Update the transform matrix of the object - virtual void updateTransform(float interpolationFactor); + /// Update the transform matrix of the object + virtual void updateTransform(float int32_terpolationFactor); - /// Set the scaling of the object - void setScaling(const openglframework::Vector3& scaling); + /// Set the scaling of the object + void setScaling(const openglframework::Vector3& scaling); }; // Update the transform matrix of the object -inline void ConcaveMesh::updateTransform(float interpolationFactor) { - mTransformMatrix = computeTransform(interpolationFactor, mScalingMatrix); +inline void ConcaveMesh::updateTransform(float int32_terpolationFactor) { + mTransformMatrix = computeTransform(int32_terpolationFactor, mScalingMatrix); } #endif diff --git a/tools/testbed/common/Cone.cpp b/tools/testbed/common/Cone.cpp index 5609f08..6acd932 100644 --- a/tools/testbed/common/Cone.cpp +++ b/tools/testbed/common/Cone.cpp @@ -1,26 +1,26 @@ /******************************************************************************** -* ReactPhysics3D physics library, http://www.reactphysics3d.com * -* Copyright (c) 2010-2016 Daniel Chappuis * +* 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: * -* * +* 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. * -* * +* 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 @@ -31,264 +31,264 @@ openglframework::VertexBufferObject Cone::mVBONormals(GL_ARRAY_BUFFER); openglframework::VertexBufferObject Cone::mVBOTextureCoords(GL_ARRAY_BUFFER); openglframework::VertexBufferObject Cone::mVBOIndices(GL_ELEMENT_ARRAY_BUFFER); openglframework::VertexArrayObject Cone::mVAO; -int Cone::totalNbCones = 0; +int32_t Cone::totalNbCones = 0; // Constructor Cone::Cone(float radius, float height, const openglframework::Vector3 &position, - reactphysics3d::CollisionWorld* world, - const std::string& meshFolderPath) - : openglframework::Mesh(), mRadius(radius), mHeight(height) { + reactphysics3d::CollisionWorld* world, + const std::string& meshFolderPath) + : openglframework::Mesh(), mRadius(radius), mHeight(height) { - // Load the mesh from a file - openglframework::MeshReaderWriter::loadMeshFromFile(meshFolderPath + "cone.obj", *this); + // Load the mesh from a file + openglframework::MeshReaderWriter::loadMeshFromFile(meshFolderPath + "cone.obj", *this); - // Calculate the normals of the mesh - calculateNormals(); + // Calculate the normals of the mesh + calculateNormals(); - // Compute the scaling matrix - mScalingMatrix = openglframework::Matrix4(mRadius, 0, 0, 0, - 0, mHeight, 0, 0, - 0, 0, mRadius, 0, - 0, 0, 0, 1); + // Compute the scaling matrix + mScalingMatrix = openglframework::Matrix4(mRadius, 0, 0, 0, + 0, mHeight, 0, 0, + 0, 0, mRadius, 0, + 0, 0, 0, 1); - // Initialize the position where the cone will be rendered - translateWorld(position); + // Initialize the position where the cone will be rendered + translateWorld(position); - // Create the collision shape for the rigid body (cone shape) and do - // not forget to delete it at the end - mConeShape = new rp3d::ConeShape(mRadius, mHeight); + // Create the collision shape for the rigid body (cone shape) and do + // not forget to delete it at the end + mConeShape = new rp3d::ConeShape(mRadius, mHeight); - // Initial position and orientation of the rigid body - rp3d::Vector3 initPosition(position.x, position.y, position.z); - rp3d::Quaternion initOrientation = rp3d::Quaternion::identity(); - rp3d::Transform transform(initPosition, initOrientation); + // Initial position and orientation of the rigid body + rp3d::Vector3 initPosition(position.x, position.y, position.z); + rp3d::Quaternion initOrientation = rp3d::Quaternion::identity(); + rp3d::Transform transform(initPosition, initOrientation); - mPreviousTransform = transform; + mPreviousTransform = transform; - // Create a rigid body corresponding to the cone in the dynamics world - mBody = world->createCollisionBody(transform); + // Create a rigid body corresponding to the cone in the dynamics world + mBody = world->createCollisionBody(transform); - // Add a collision shape to the body and specify the mass of the shape - m_proxyShape = mBody->addCollisionShape(mConeShape, rp3d::Transform::identity()); + // Add a collision shape to the body and specify the mass of the shape + m_proxyShape = mBody->addCollisionShape(mConeShape, rp3d::Transform::identity()); - mTransformMatrix = mTransformMatrix * mScalingMatrix; + mTransformMatrix = mTransformMatrix * mScalingMatrix; - // Create the VBOs and VAO - if (totalNbCones == 0) { - createVBOAndVAO(); - } + // Create the VBOs and VAO + if (totalNbCones == 0) { + createVBOAndVAO(); + } - totalNbCones++; + totalNbCones++; } // Constructor Cone::Cone(float radius, float height, const openglframework::Vector3 &position, - float mass, reactphysics3d::DynamicsWorld* dynamicsWorld, - const std::string& meshFolderPath) - : openglframework::Mesh(), mRadius(radius), mHeight(height) { + float mass, reactphysics3d::DynamicsWorld* dynamicsWorld, + const std::string& meshFolderPath) + : openglframework::Mesh(), mRadius(radius), mHeight(height) { - // Load the mesh from a file - openglframework::MeshReaderWriter::loadMeshFromFile(meshFolderPath + "cone.obj", *this); + // Load the mesh from a file + openglframework::MeshReaderWriter::loadMeshFromFile(meshFolderPath + "cone.obj", *this); - // Calculate the normals of the mesh - calculateNormals(); + // Calculate the normals of the mesh + calculateNormals(); - // Compute the scaling matrix - mScalingMatrix = openglframework::Matrix4(mRadius, 0, 0, 0, - 0, mHeight, 0, 0, - 0, 0, mRadius, 0, - 0, 0, 0, 1); + // Compute the scaling matrix + mScalingMatrix = openglframework::Matrix4(mRadius, 0, 0, 0, + 0, mHeight, 0, 0, + 0, 0, mRadius, 0, + 0, 0, 0, 1); - // Initialize the position where the cone will be rendered - translateWorld(position); + // Initialize the position where the cone will be rendered + translateWorld(position); - // Create the collision shape for the rigid body (cone shape) and do not - // forget to delete it at the end - mConeShape = new rp3d::ConeShape(mRadius, mHeight); + // Create the collision shape for the rigid body (cone shape) and do not + // forget to delete it at the end + mConeShape = new rp3d::ConeShape(mRadius, mHeight); - // Initial position and orientation of the rigid body - rp3d::Vector3 initPosition(position.x, position.y, position.z); - rp3d::Quaternion initOrientation = rp3d::Quaternion::identity(); - rp3d::Transform transform(initPosition, initOrientation); + // Initial position and orientation of the rigid body + rp3d::Vector3 initPosition(position.x, position.y, position.z); + rp3d::Quaternion initOrientation = rp3d::Quaternion::identity(); + rp3d::Transform transform(initPosition, initOrientation); - // Create a rigid body corresponding to the cone in the dynamics world - rp3d::RigidBody* body = dynamicsWorld->createRigidBody(transform); + // Create a rigid body corresponding to the cone in the dynamics world + rp3d::RigidBody* body = dynamicsWorld->createRigidBody(transform); - // Add a collision shape to the body and specify the mass of the shape - m_proxyShape = body->addCollisionShape(mConeShape, rp3d::Transform::identity(), mass); + // Add a collision shape to the body and specify the mass of the shape + m_proxyShape = body->addCollisionShape(mConeShape, rp3d::Transform::identity(), mass); - mBody = body; + mBody = body; - mTransformMatrix = mTransformMatrix * mScalingMatrix; + mTransformMatrix = mTransformMatrix * mScalingMatrix; - // Create the VBOs and VAO - if (totalNbCones == 0) { - createVBOAndVAO(); - } + // Create the VBOs and VAO + if (totalNbCones == 0) { + createVBOAndVAO(); + } - totalNbCones++; + totalNbCones++; } // Destructor Cone::~Cone() { - if (totalNbCones == 1) { - // Destroy the mesh - destroy(); + if (totalNbCones == 1) { + // Destroy the mesh + destroy(); - // Destroy the VBOs and VAO - mVBOIndices.destroy(); - mVBOVertices.destroy(); - mVBONormals.destroy(); - mVBOTextureCoords.destroy(); - mVAO.destroy(); - } - delete mConeShape; - totalNbCones--; + // Destroy the VBOs and VAO + mVBOIndices.destroy(); + mVBOVertices.destroy(); + mVBONormals.destroy(); + mVBOTextureCoords.destroy(); + mVAO.destroy(); + } + delete mConeShape; + totalNbCones--; } // Render the cone at the correct position and with the correct orientation void Cone::render(openglframework::Shader& shader, - const openglframework::Matrix4& worldToCameraMatrix) { + const openglframework::Matrix4& worldToCameraMatrix) { - // Bind the shader - shader.bind(); + // Bind the shader + shader.bind(); - // Set the model to camera matrix - shader.setMatrix4x4Uniform("localToWorldMatrix", mTransformMatrix); - shader.setMatrix4x4Uniform("worldToCameraMatrix", worldToCameraMatrix); + // Set the model to camera matrix + shader.setMatrix4x4Uniform("localToWorldMatrix", mTransformMatrix); + shader.setMatrix4x4Uniform("worldToCameraMatrix", worldToCameraMatrix); - // Set the normal matrix (inverse transpose of the 3x3 upper-left sub matrix of the - // model-view matrix) - const openglframework::Matrix4 localToCameraMatrix = worldToCameraMatrix * mTransformMatrix; - const openglframework::Matrix3 normalMatrix = - localToCameraMatrix.getUpperLeft3x3Matrix().getInverse().getTranspose(); - shader.setMatrix3x3Uniform("normalMatrix", normalMatrix, false); + // Set the normal matrix (inverse transpose of the 3x3 upper-left sub matrix of the + // model-view matrix) + const openglframework::Matrix4 localToCameraMatrix = worldToCameraMatrix * mTransformMatrix; + const openglframework::Matrix3 normalMatrix = + localToCameraMatrix.getUpperLeft3x3Matrix().getInverse().getTranspose(); + shader.setMatrix3x3Uniform("normalMatrix", normalMatrix, false); - // Set the vertex color - openglframework::Color currentColor = mBody->isSleeping() ? mSleepingColor : mColor; - openglframework::Vector4 color(currentColor.r, currentColor.g, currentColor.b, currentColor.a); - shader.setVector4Uniform("vertexColor", color, false); + // Set the vertex color + openglframework::Color currentColor = mBody->isSleeping() ? mSleepingColor : mColor; + openglframework::Vector4 color(currentColor.r, currentColor.g, currentColor.b, currentColor.a); + shader.setVector4Uniform("vertexColor", color, false); - // Bind the VAO - mVAO.bind(); + // Bind the VAO + mVAO.bind(); - mVBOVertices.bind(); + mVBOVertices.bind(); - // Get the location of shader attribute variables - GLint vertexPositionLoc = shader.getAttribLocation("vertexPosition"); - GLint vertexNormalLoc = shader.getAttribLocation("vertexNormal", false); + // Get the location of shader attribute variables + GLint32_t vertexPositionLoc = shader.getAttribLocation("vertexPosition"); + GLint32_t vertexNormalLoc = shader.getAttribLocation("vertexNormal", false); - glEnableVertexAttribArray(vertexPositionLoc); - glVertexAttribPointer(vertexPositionLoc, 3, GL_FLOAT, GL_FALSE, 0, (char*)NULL); + glEnableVertexAttribArray(vertexPositionLoc); + glVertexAttribPointer(vertexPositionLoc, 3, GL_FLOAT, GL_FALSE, 0, (char*)NULL); - mVBONormals.bind(); + mVBONormals.bind(); - if (vertexNormalLoc != -1) glVertexAttribPointer(vertexNormalLoc, 3, GL_FLOAT, GL_FALSE, 0, (char*)NULL); - if (vertexNormalLoc != -1) glEnableVertexAttribArray(vertexNormalLoc); + if (vertexNormalLoc != -1) glVertexAttribPointer(vertexNormalLoc, 3, GL_FLOAT, GL_FALSE, 0, (char*)NULL); + if (vertexNormalLoc != -1) glEnableVertexAttribArray(vertexNormalLoc); - // For each part of the mesh - for (unsigned int i=0; isetTransform(transform); + // Reset the transform + mBody->setTransform(transform); - mBody->setIsSleeping(false); + mBody->setIsSleeping(false); - // Reset the velocity of the rigid body - rp3d::RigidBody* rigidBody = dynamic_cast(mBody); - if (rigidBody != NULL) { - rigidBody->setLinearVelocity(rp3d::Vector3(0, 0, 0)); - rigidBody->setAngularVelocity(rp3d::Vector3(0, 0, 0)); - } + // Reset the velocity of the rigid body + rp3d::RigidBody* rigidBody = dynamic_cast(mBody); + if (rigidBody != NULL) { + rigidBody->setLinearVelocity(rp3d::Vector3(0, 0, 0)); + rigidBody->setAngularVelocity(rp3d::Vector3(0, 0, 0)); + } - updateTransform(1.0f); + updateTransform(1.0f); } // Set the scaling of the object void Cone::setScaling(const openglframework::Vector3& scaling) { - // Scale the collision shape - m_proxyShape->setLocalScaling(rp3d::Vector3(scaling.x, scaling.y, scaling.z)); + // Scale the collision shape + m_proxyShape->setLocalScaling(rp3d::Vector3(scaling.x, scaling.y, scaling.z)); - // Scale the graphics object - mScalingMatrix = openglframework::Matrix4(mRadius * scaling.x, 0, 0, 0, - 0, mHeight * scaling.y, 0, 0, - 0, 0, mRadius * scaling.z, 0, - 0, 0, 0, 1); + // Scale the graphics object + mScalingMatrix = openglframework::Matrix4(mRadius * scaling.x, 0, 0, 0, + 0, mHeight * scaling.y, 0, 0, + 0, 0, mRadius * scaling.z, 0, + 0, 0, 0, 1); } diff --git a/tools/testbed/common/Cone.h b/tools/testbed/common/Cone.h index 402265a..4d1abc5 100644 --- a/tools/testbed/common/Cone.h +++ b/tools/testbed/common/Cone.h @@ -1,26 +1,26 @@ /******************************************************************************** -* ReactPhysics3D physics library, http://www.reactphysics3d.com * -* Copyright (c) 2010-2016 Daniel Chappuis * +* 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: * -* * +* 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. * -* * +* 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 CONE_H @@ -34,80 +34,80 @@ // Class Cone class Cone : public openglframework::Mesh, public PhysicsObject { - private : + private : - // -------------------- Attributes -------------------- // + // -------------------- Attributes -------------------- // - /// Radius of the cone - float mRadius; + /// Radius of the cone + float mRadius; - /// Height of the cone - float mHeight; + /// Height of the cone + float mHeight; - /// Collision shape - rp3d::ConeShape* mConeShape; - rp3d::ProxyShape* m_proxyShape; + /// Collision shape + rp3d::ConeShape* mConeShape; + rp3d::ProxyShape* m_proxyShape; - /// Scaling matrix (applied to a sphere to obtain the correct cone dimensions) - openglframework::Matrix4 mScalingMatrix; + /// Scaling matrix (applied to a sphere to obtain the correct cone dimensions) + openglframework::Matrix4 mScalingMatrix; - /// Previous transform (for interpolation) - rp3d::Transform mPreviousTransform; + /// Previous transform (for int32_terpolation) + rp3d::Transform mPreviousTransform; - /// Vertex Buffer Object for the vertices data - static openglframework::VertexBufferObject mVBOVertices; + /// Vertex Buffer Object for the vertices data + static openglframework::VertexBufferObject mVBOVertices; - /// Vertex Buffer Object for the normals data - static openglframework::VertexBufferObject mVBONormals; + /// Vertex Buffer Object for the normals data + static openglframework::VertexBufferObject mVBONormals; - /// Vertex Buffer Object for the texture coords - static openglframework::VertexBufferObject mVBOTextureCoords; + /// Vertex Buffer Object for the texture coords + static openglframework::VertexBufferObject mVBOTextureCoords; - /// Vertex Buffer Object for the indices - static openglframework::VertexBufferObject mVBOIndices; + /// Vertex Buffer Object for the indices + static openglframework::VertexBufferObject mVBOIndices; - /// Vertex Array Object for the vertex data - static openglframework::VertexArrayObject mVAO; + /// Vertex Array Object for the vertex data + static openglframework::VertexArrayObject mVAO; - // Total number of cones created - static int totalNbCones; + // Total number of cones created + static int32_t totalNbCones; - // -------------------- Methods -------------------- // + // -------------------- Methods -------------------- // - // Create the Vertex Buffer Objects used to render with OpenGL. - void createVBOAndVAO(); + // Create the Vertex Buffer Objects used to render with OpenGL. + void createVBOAndVAO(); - public : + public : - // -------------------- Methods -------------------- // + // -------------------- Methods -------------------- // - /// Constructor - Cone(float radius, float height, const openglframework::Vector3& position, - rp3d::CollisionWorld* world, const std::string& meshFolderPath); + /// Constructor + Cone(float radius, float height, const openglframework::Vector3& position, + rp3d::CollisionWorld* world, const std::string& meshFolderPath); - /// Constructor - Cone(float radius, float height, const openglframework::Vector3& position, - float mass, rp3d::DynamicsWorld* dynamicsWorld, const std::string& meshFolderPath); + /// Constructor + Cone(float radius, float height, const openglframework::Vector3& position, + float mass, rp3d::DynamicsWorld* dynamicsWorld, const std::string& meshFolderPath); - /// Destructor - ~Cone(); + /// Destructor + ~Cone(); - /// Render the cone at the correct position and with the correct orientation - void render(openglframework::Shader& shader, - const openglframework::Matrix4& worldToCameraMatrix); + /// Render the cone at the correct position and with the correct orientation + void render(openglframework::Shader& shader, + const openglframework::Matrix4& worldToCameraMatrix); - /// Set the position of the box - void resetTransform(const rp3d::Transform& transform); + /// Set the position of the box + void resetTransform(const rp3d::Transform& transform); - /// Update the transform matrix of the object - virtual void updateTransform(float interpolationFactor); + /// Update the transform matrix of the object + virtual void updateTransform(float int32_terpolationFactor); - /// Set the scaling of the object - void setScaling(const openglframework::Vector3& scaling); + /// Set the scaling of the object + void setScaling(const openglframework::Vector3& scaling); }; -inline void Cone::updateTransform(float interpolationFactor) { - mTransformMatrix = computeTransform(interpolationFactor, mScalingMatrix); +inline void Cone::updateTransform(float int32_terpolationFactor) { + mTransformMatrix = computeTransform(int32_terpolationFactor, mScalingMatrix); } #endif diff --git a/tools/testbed/common/ConvexMesh.cpp b/tools/testbed/common/ConvexMesh.cpp index 6b986f5..a59080a 100644 --- a/tools/testbed/common/ConvexMesh.cpp +++ b/tools/testbed/common/ConvexMesh.cpp @@ -1,26 +1,26 @@ /******************************************************************************** -* ReactPhysics3D physics library, http://www.reactphysics3d.com * -* Copyright (c) 2010-2016 Daniel Chappuis * +* 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: * -* * +* 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. * -* * +* 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 @@ -28,263 +28,263 @@ // Constructor ConvexMesh::ConvexMesh(const openglframework::Vector3 &position, - reactphysics3d::CollisionWorld* world, - const std::string& meshPath) - : openglframework::Mesh(), mVBOVertices(GL_ARRAY_BUFFER), - mVBONormals(GL_ARRAY_BUFFER), mVBOTextureCoords(GL_ARRAY_BUFFER), - mVBOIndices(GL_ELEMENT_ARRAY_BUFFER) { + reactphysics3d::CollisionWorld* world, + const std::string& meshPath) + : openglframework::Mesh(), mVBOVertices(GL_ARRAY_BUFFER), + mVBONormals(GL_ARRAY_BUFFER), mVBOTextureCoords(GL_ARRAY_BUFFER), + mVBOIndices(GL_ELEMENT_ARRAY_BUFFER) { - // Load the mesh from a file - openglframework::MeshReaderWriter::loadMeshFromFile(meshPath, *this); + // Load the mesh from a file + openglframework::MeshReaderWriter::loadMeshFromFile(meshPath, *this); - // Calculate the normals of the mesh - calculateNormals(); + // Calculate the normals of the mesh + calculateNormals(); - // Initialize the position where the sphere will be rendered - translateWorld(position); + // Initialize the position where the sphere will be rendered + translateWorld(position); - // Compute the scaling matrix - mScalingMatrix = openglframework::Matrix4::identity(); + // Compute the scaling matrix + mScalingMatrix = openglframework::Matrix4::identity(); - // Vertex and Indices array for the triangle mesh (data in shared and not copied) - mPhysicsTriangleVertexArray = - new rp3d::TriangleVertexArray(getNbVertices(), &(m_vertices[0]), sizeof(openglframework::Vector3), - getNbFaces(0), &(mIndices[0][0]), sizeof(int), - rp3d::TriangleVertexArray::VERTEX_FLOAT_TYPE, - rp3d::TriangleVertexArray::INDEX_INTEGER_TYPE); + // Vertex and Indices array for the triangle mesh (data in shared and not copied) + mPhysicsTriangleVertexArray = + new rp3d::TriangleVertexArray(getNbVertices(), &(m_vertices[0]), sizeof(openglframework::Vector3), + getNbFaces(0), &(mIndices[0][0]), sizeof(int32_t), + rp3d::TriangleVertexArray::VERTEX_FLOAT_TYPE, + rp3d::TriangleVertexArray::INDEX_INTEGER_TYPE); - // Create the collision shape for the rigid body (convex mesh shape) and - // do not forget to delete it at the end - mConvexShape = new rp3d::ConvexMeshShape(mPhysicsTriangleVertexArray); + // Create the collision shape for the rigid body (convex mesh shape) and + // do not forget to delete it at the end + mConvexShape = new rp3d::ConvexMeshShape(mPhysicsTriangleVertexArray); - // Initial position and orientation of the rigid body - rp3d::Vector3 initPosition(position.x, position.y, position.z); - rp3d::Quaternion initOrientation = rp3d::Quaternion::identity(); - rp3d::Transform transform(initPosition, initOrientation); + // Initial position and orientation of the rigid body + rp3d::Vector3 initPosition(position.x, position.y, position.z); + rp3d::Quaternion initOrientation = rp3d::Quaternion::identity(); + rp3d::Transform transform(initPosition, initOrientation); - mPreviousTransform = transform; + mPreviousTransform = transform; - // Create a rigid body corresponding to the sphere in the dynamics world - mBody = world->createCollisionBody(transform); + // Create a rigid body corresponding to the sphere in the dynamics world + mBody = world->createCollisionBody(transform); - // Add a collision shape to the body and specify the mass of the collision shape - m_proxyShape = mBody->addCollisionShape(mConvexShape, rp3d::Transform::identity()); + // Add a collision shape to the body and specify the mass of the collision shape + m_proxyShape = mBody->addCollisionShape(mConvexShape, rp3d::Transform::identity()); - // Create the VBOs and VAO - createVBOAndVAO(); + // Create the VBOs and VAO + createVBOAndVAO(); - mTransformMatrix = mTransformMatrix * mScalingMatrix; + mTransformMatrix = mTransformMatrix * mScalingMatrix; } // Constructor ConvexMesh::ConvexMesh(const openglframework::Vector3 &position, float mass, - reactphysics3d::DynamicsWorld* dynamicsWorld, - const std::string& meshPath) - : openglframework::Mesh(), mVBOVertices(GL_ARRAY_BUFFER), - mVBONormals(GL_ARRAY_BUFFER), mVBOTextureCoords(GL_ARRAY_BUFFER), - mVBOIndices(GL_ELEMENT_ARRAY_BUFFER) { + reactphysics3d::DynamicsWorld* dynamicsWorld, + const std::string& meshPath) + : openglframework::Mesh(), mVBOVertices(GL_ARRAY_BUFFER), + mVBONormals(GL_ARRAY_BUFFER), mVBOTextureCoords(GL_ARRAY_BUFFER), + mVBOIndices(GL_ELEMENT_ARRAY_BUFFER) { - // Load the mesh from a file - openglframework::MeshReaderWriter::loadMeshFromFile(meshPath, *this); + // Load the mesh from a file + openglframework::MeshReaderWriter::loadMeshFromFile(meshPath, *this); - // Calculate the normals of the mesh - calculateNormals(); + // Calculate the normals of the mesh + calculateNormals(); - // Initialize the position where the sphere will be rendered - translateWorld(position); + // Initialize the position where the sphere will be rendered + translateWorld(position); - // Compute the scaling matrix - mScalingMatrix = openglframework::Matrix4::identity(); + // Compute the scaling matrix + mScalingMatrix = openglframework::Matrix4::identity(); - // Vertex and Indices array for the triangle mesh (data in shared and not copied) - mPhysicsTriangleVertexArray = - new rp3d::TriangleVertexArray(getNbVertices(), &(m_vertices[0]), sizeof(openglframework::Vector3), - getNbFaces(0), &(mIndices[0][0]), sizeof(int), - rp3d::TriangleVertexArray::VERTEX_FLOAT_TYPE, - rp3d::TriangleVertexArray::INDEX_INTEGER_TYPE); + // Vertex and Indices array for the triangle mesh (data in shared and not copied) + mPhysicsTriangleVertexArray = + new rp3d::TriangleVertexArray(getNbVertices(), &(m_vertices[0]), sizeof(openglframework::Vector3), + getNbFaces(0), &(mIndices[0][0]), sizeof(int32_t), + rp3d::TriangleVertexArray::VERTEX_FLOAT_TYPE, + rp3d::TriangleVertexArray::INDEX_INTEGER_TYPE); - // Create the collision shape for the rigid body (convex mesh shape) and do - // not forget to delete it at the end - mConvexShape = new rp3d::ConvexMeshShape(mPhysicsTriangleVertexArray); + // Create the collision shape for the rigid body (convex mesh shape) and do + // not forget to delete it at the end + mConvexShape = new rp3d::ConvexMeshShape(mPhysicsTriangleVertexArray); - // Initial position and orientation of the rigid body - rp3d::Vector3 initPosition(position.x, position.y, position.z); - rp3d::Quaternion initOrientation = rp3d::Quaternion::identity(); - rp3d::Transform transform(initPosition, initOrientation); + // Initial position and orientation of the rigid body + rp3d::Vector3 initPosition(position.x, position.y, position.z); + rp3d::Quaternion initOrientation = rp3d::Quaternion::identity(); + rp3d::Transform transform(initPosition, initOrientation); - // Create a rigid body corresponding to the sphere in the dynamics world - rp3d::RigidBody* body = dynamicsWorld->createRigidBody(transform); + // Create a rigid body corresponding to the sphere in the dynamics world + rp3d::RigidBody* body = dynamicsWorld->createRigidBody(transform); - // Add a collision shape to the body and specify the mass of the collision shape - m_proxyShape = body->addCollisionShape(mConvexShape, rp3d::Transform::identity(), mass); + // Add a collision shape to the body and specify the mass of the collision shape + m_proxyShape = body->addCollisionShape(mConvexShape, rp3d::Transform::identity(), mass); - mBody = body; + mBody = body; - // Create the VBOs and VAO - createVBOAndVAO(); + // Create the VBOs and VAO + createVBOAndVAO(); - mTransformMatrix = mTransformMatrix * mScalingMatrix; + mTransformMatrix = mTransformMatrix * mScalingMatrix; } // Destructor ConvexMesh::~ConvexMesh() { - // Destroy the mesh - destroy(); + // Destroy the mesh + destroy(); - // Destroy the VBOs and VAO - mVBOIndices.destroy(); - mVBOVertices.destroy(); - mVBONormals.destroy(); - mVBOTextureCoords.destroy(); - mVAO.destroy(); + // Destroy the VBOs and VAO + mVBOIndices.destroy(); + mVBOVertices.destroy(); + mVBONormals.destroy(); + mVBOTextureCoords.destroy(); + mVAO.destroy(); - delete mPhysicsTriangleVertexArray; - delete mConvexShape; + delete mPhysicsTriangleVertexArray; + delete mConvexShape; } // Render the sphere at the correct position and with the correct orientation void ConvexMesh::render(openglframework::Shader& shader, - const openglframework::Matrix4& worldToCameraMatrix) { + const openglframework::Matrix4& worldToCameraMatrix) { - // Bind the shader - shader.bind(); + // Bind the shader + shader.bind(); - // Set the model to camera matrix - shader.setMatrix4x4Uniform("localToWorldMatrix", mTransformMatrix); - shader.setMatrix4x4Uniform("worldToCameraMatrix", worldToCameraMatrix); + // Set the model to camera matrix + shader.setMatrix4x4Uniform("localToWorldMatrix", mTransformMatrix); + shader.setMatrix4x4Uniform("worldToCameraMatrix", worldToCameraMatrix); - // Set the normal matrix (inverse transpose of the 3x3 upper-left sub matrix of the - // model-view matrix) - const openglframework::Matrix4 localToCameraMatrix = worldToCameraMatrix * mTransformMatrix; - const openglframework::Matrix3 normalMatrix = - localToCameraMatrix.getUpperLeft3x3Matrix().getInverse().getTranspose(); - shader.setMatrix3x3Uniform("normalMatrix", normalMatrix, false); + // Set the normal matrix (inverse transpose of the 3x3 upper-left sub matrix of the + // model-view matrix) + const openglframework::Matrix4 localToCameraMatrix = worldToCameraMatrix * mTransformMatrix; + const openglframework::Matrix3 normalMatrix = + localToCameraMatrix.getUpperLeft3x3Matrix().getInverse().getTranspose(); + shader.setMatrix3x3Uniform("normalMatrix", normalMatrix, false); - // Set the vertex color - openglframework::Color currentColor = mBody->isSleeping() ? mSleepingColor : mColor; - openglframework::Vector4 color(currentColor.r, currentColor.g, currentColor.b, currentColor.a); - shader.setVector4Uniform("vertexColor", color, false); + // Set the vertex color + openglframework::Color currentColor = mBody->isSleeping() ? mSleepingColor : mColor; + openglframework::Vector4 color(currentColor.r, currentColor.g, currentColor.b, currentColor.a); + shader.setVector4Uniform("vertexColor", color, false); - // Bind the VAO - mVAO.bind(); + // Bind the VAO + mVAO.bind(); - mVBOVertices.bind(); + mVBOVertices.bind(); - // Get the location of shader attribute variables - GLint vertexPositionLoc = shader.getAttribLocation("vertexPosition"); - GLint vertexNormalLoc = shader.getAttribLocation("vertexNormal", false); + // Get the location of shader attribute variables + GLint32_t vertexPositionLoc = shader.getAttribLocation("vertexPosition"); + GLint32_t vertexNormalLoc = shader.getAttribLocation("vertexNormal", false); - glEnableVertexAttribArray(vertexPositionLoc); - glVertexAttribPointer(vertexPositionLoc, 3, GL_FLOAT, GL_FALSE, 0, (char*)NULL); + glEnableVertexAttribArray(vertexPositionLoc); + glVertexAttribPointer(vertexPositionLoc, 3, GL_FLOAT, GL_FALSE, 0, (char*)NULL); - mVBONormals.bind(); + mVBONormals.bind(); - if (vertexNormalLoc != -1) glVertexAttribPointer(vertexNormalLoc, 3, GL_FLOAT, GL_FALSE, 0, (char*)NULL); - if (vertexNormalLoc != -1) glEnableVertexAttribArray(vertexNormalLoc); + if (vertexNormalLoc != -1) glVertexAttribPointer(vertexNormalLoc, 3, GL_FLOAT, GL_FALSE, 0, (char*)NULL); + if (vertexNormalLoc != -1) glEnableVertexAttribArray(vertexNormalLoc); - // For each part of the mesh - for (unsigned int i=0; isetTransform(transform); + // Reset the transform + mBody->setTransform(transform); - mBody->setIsSleeping(false); + mBody->setIsSleeping(false); - // Reset the velocity of the rigid body - rp3d::RigidBody* rigidBody = dynamic_cast(mBody); - if (rigidBody != NULL) { - rigidBody->setLinearVelocity(rp3d::Vector3(0, 0, 0)); - rigidBody->setAngularVelocity(rp3d::Vector3(0, 0, 0)); - } + // Reset the velocity of the rigid body + rp3d::RigidBody* rigidBody = dynamic_cast(mBody); + if (rigidBody != NULL) { + rigidBody->setLinearVelocity(rp3d::Vector3(0, 0, 0)); + rigidBody->setAngularVelocity(rp3d::Vector3(0, 0, 0)); + } - updateTransform(1.0f); + updateTransform(1.0f); } // Set the scaling of the object void ConvexMesh::setScaling(const openglframework::Vector3& scaling) { - // Scale the collision shape - m_proxyShape->setLocalScaling(rp3d::Vector3(scaling.x, scaling.y, scaling.z)); + // Scale the collision shape + m_proxyShape->setLocalScaling(rp3d::Vector3(scaling.x, scaling.y, scaling.z)); - // Scale the graphics object - mScalingMatrix = openglframework::Matrix4(scaling.x, 0, 0, 0, - 0, scaling.y, 0, 0, - 0, 0, scaling.z, 0, - 0, 0, 0, 1); + // Scale the graphics object + mScalingMatrix = openglframework::Matrix4(scaling.x, 0, 0, 0, + 0, scaling.y, 0, 0, + 0, 0, scaling.z, 0, + 0, 0, 0, 1); } diff --git a/tools/testbed/common/ConvexMesh.h b/tools/testbed/common/ConvexMesh.h index 25b563c..4bee2e6 100644 --- a/tools/testbed/common/ConvexMesh.h +++ b/tools/testbed/common/ConvexMesh.h @@ -1,26 +1,26 @@ /******************************************************************************** -* ReactPhysics3D physics library, http://www.reactphysics3d.com * -* Copyright (c) 2010-2016 Daniel Chappuis * +* 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: * -* * +* 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. * -* * +* 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 CONVEX_MESH_H @@ -34,74 +34,74 @@ // Class ConvexMesh class ConvexMesh : public openglframework::Mesh, public PhysicsObject { - private : + private : - // -------------------- Attributes -------------------- // + // -------------------- Attributes -------------------- // - /// Previous transform (for interpolation) - rp3d::Transform mPreviousTransform; + /// Previous transform (for int32_terpolation) + rp3d::Transform mPreviousTransform; - rp3d::TriangleVertexArray* mPhysicsTriangleVertexArray; + rp3d::TriangleVertexArray* mPhysicsTriangleVertexArray; - /// Collision shape - rp3d::ConvexMeshShape* mConvexShape; - rp3d::ProxyShape* m_proxyShape; + /// Collision shape + rp3d::ConvexMeshShape* mConvexShape; + rp3d::ProxyShape* m_proxyShape; - /// Scaling matrix - openglframework::Matrix4 mScalingMatrix; + /// Scaling matrix + openglframework::Matrix4 mScalingMatrix; - /// Vertex Buffer Object for the vertices data - openglframework::VertexBufferObject mVBOVertices; + /// Vertex Buffer Object for the vertices data + openglframework::VertexBufferObject mVBOVertices; - /// Vertex Buffer Object for the normals data - openglframework::VertexBufferObject mVBONormals; + /// Vertex Buffer Object for the normals data + openglframework::VertexBufferObject mVBONormals; - /// Vertex Buffer Object for the texture coords - openglframework::VertexBufferObject mVBOTextureCoords; + /// Vertex Buffer Object for the texture coords + openglframework::VertexBufferObject mVBOTextureCoords; - /// Vertex Buffer Object for the indices - openglframework::VertexBufferObject mVBOIndices; + /// Vertex Buffer Object for the indices + openglframework::VertexBufferObject mVBOIndices; - /// Vertex Array Object for the vertex data - openglframework::VertexArrayObject mVAO; + /// Vertex Array Object for the vertex data + openglframework::VertexArrayObject mVAO; - // -------------------- Methods -------------------- // + // -------------------- Methods -------------------- // - // Create the Vertex Buffer Objects used to render with OpenGL. - void createVBOAndVAO(); + // Create the Vertex Buffer Objects used to render with OpenGL. + void createVBOAndVAO(); - public : + public : - // -------------------- Methods -------------------- // + // -------------------- Methods -------------------- // - /// Constructor - ConvexMesh(const openglframework::Vector3& position, - rp3d::CollisionWorld* world, const std::string& meshPath); + /// Constructor + ConvexMesh(const openglframework::Vector3& position, + rp3d::CollisionWorld* world, const std::string& meshPath); - /// Constructor - ConvexMesh(const openglframework::Vector3& position, float mass, - rp3d::DynamicsWorld* dynamicsWorld, const std::string& meshPath); + /// Constructor + ConvexMesh(const openglframework::Vector3& position, float mass, + rp3d::DynamicsWorld* dynamicsWorld, const std::string& meshPath); - /// Destructor - ~ConvexMesh(); + /// Destructor + ~ConvexMesh(); - /// Render the mesh at the correct position and with the correct orientation - void render(openglframework::Shader& shader, - const openglframework::Matrix4& worldToCameraMatrix); + /// Render the mesh at the correct position and with the correct orientation + void render(openglframework::Shader& shader, + const openglframework::Matrix4& worldToCameraMatrix); - /// Set the position of the box - void resetTransform(const rp3d::Transform& transform); + /// Set the position of the box + void resetTransform(const rp3d::Transform& transform); - /// Update the transform matrix of the object - virtual void updateTransform(float interpolationFactor); + /// Update the transform matrix of the object + virtual void updateTransform(float int32_terpolationFactor); - /// Set the scaling of the object - void setScaling(const openglframework::Vector3& scaling); + /// Set the scaling of the object + void setScaling(const openglframework::Vector3& scaling); }; // Update the transform matrix of the object -inline void ConvexMesh::updateTransform(float interpolationFactor) { - mTransformMatrix = computeTransform(interpolationFactor, mScalingMatrix); +inline void ConvexMesh::updateTransform(float int32_terpolationFactor) { + mTransformMatrix = computeTransform(int32_terpolationFactor, mScalingMatrix); } #endif diff --git a/tools/testbed/common/Cylinder.cpp b/tools/testbed/common/Cylinder.cpp index dfa37c3..2ae9c05 100644 --- a/tools/testbed/common/Cylinder.cpp +++ b/tools/testbed/common/Cylinder.cpp @@ -1,26 +1,26 @@ /******************************************************************************** -* ReactPhysics3D physics library, http://www.reactphysics3d.com * -* Copyright (c) 2010-2016 Daniel Chappuis * +* 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: * -* * +* 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. * -* * +* 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 @@ -31,264 +31,264 @@ openglframework::VertexBufferObject Cylinder::mVBONormals(GL_ARRAY_BUFFER); openglframework::VertexBufferObject Cylinder::mVBOTextureCoords(GL_ARRAY_BUFFER); openglframework::VertexBufferObject Cylinder::mVBOIndices(GL_ELEMENT_ARRAY_BUFFER); openglframework::VertexArrayObject Cylinder::mVAO; -int Cylinder::totalNbCylinders = 0; +int32_t Cylinder::totalNbCylinders = 0; // Constructor Cylinder::Cylinder(float radius, float height, const openglframework::Vector3& position, - reactphysics3d::CollisionWorld* world, - const std::string& meshFolderPath) - : openglframework::Mesh(), mRadius(radius), mHeight(height) { + reactphysics3d::CollisionWorld* world, + const std::string& meshFolderPath) + : openglframework::Mesh(), mRadius(radius), mHeight(height) { - // Load the mesh from a file - openglframework::MeshReaderWriter::loadMeshFromFile(meshFolderPath + "cylinder.obj", *this); + // Load the mesh from a file + openglframework::MeshReaderWriter::loadMeshFromFile(meshFolderPath + "cylinder.obj", *this); - // Calculate the normals of the mesh - calculateNormals(); + // Calculate the normals of the mesh + calculateNormals(); - // Compute the scaling matrix - mScalingMatrix = openglframework::Matrix4(mRadius, 0, 0, 0, - 0, mHeight, 0, 0, - 0, 0, mRadius, 0, - 0, 0, 0, 1); + // Compute the scaling matrix + mScalingMatrix = openglframework::Matrix4(mRadius, 0, 0, 0, + 0, mHeight, 0, 0, + 0, 0, mRadius, 0, + 0, 0, 0, 1); - // Initialize the position where the cylinder will be rendered - translateWorld(position); + // Initialize the position where the cylinder will be rendered + translateWorld(position); - // Create the collision shape for the rigid body (cylinder shape) and do not - // forget to delete it at the end - mCylinderShape = new rp3d::CylinderShape(mRadius, mHeight); + // Create the collision shape for the rigid body (cylinder shape) and do not + // forget to delete it at the end + mCylinderShape = new rp3d::CylinderShape(mRadius, mHeight); - // Initial position and orientation of the rigid body - rp3d::Vector3 initPosition(position.x, position.y, position.z); - rp3d::Quaternion initOrientation = rp3d::Quaternion::identity(); - rp3d::Transform transform(initPosition, initOrientation); + // Initial position and orientation of the rigid body + rp3d::Vector3 initPosition(position.x, position.y, position.z); + rp3d::Quaternion initOrientation = rp3d::Quaternion::identity(); + rp3d::Transform transform(initPosition, initOrientation); - mPreviousTransform = transform; + mPreviousTransform = transform; - // Create a rigid body corresponding to the cylinder in the dynamics world - mBody = world->createCollisionBody(transform); + // Create a rigid body corresponding to the cylinder in the dynamics world + mBody = world->createCollisionBody(transform); - // Add a collision shape to the body and specify the mass of the shape - m_proxyShape = mBody->addCollisionShape(mCylinderShape, rp3d::Transform::identity()); + // Add a collision shape to the body and specify the mass of the shape + m_proxyShape = mBody->addCollisionShape(mCylinderShape, rp3d::Transform::identity()); - mTransformMatrix = mTransformMatrix * mScalingMatrix; + mTransformMatrix = mTransformMatrix * mScalingMatrix; - // Create the VBOs and VAO - if (totalNbCylinders == 0) { - createVBOAndVAO(); - } + // Create the VBOs and VAO + if (totalNbCylinders == 0) { + createVBOAndVAO(); + } - totalNbCylinders++; + totalNbCylinders++; } // Constructor Cylinder::Cylinder(float radius, float height, const openglframework::Vector3& position, - float mass, reactphysics3d::DynamicsWorld* dynamicsWorld, - const std::string& meshFolderPath) - : openglframework::Mesh(), mRadius(radius), mHeight(height) { + float mass, reactphysics3d::DynamicsWorld* dynamicsWorld, + const std::string& meshFolderPath) + : openglframework::Mesh(), mRadius(radius), mHeight(height) { - // Load the mesh from a file - openglframework::MeshReaderWriter::loadMeshFromFile(meshFolderPath + "cylinder.obj", *this); + // Load the mesh from a file + openglframework::MeshReaderWriter::loadMeshFromFile(meshFolderPath + "cylinder.obj", *this); - // Calculate the normals of the mesh - calculateNormals(); + // Calculate the normals of the mesh + calculateNormals(); - // Compute the scaling matrix - mScalingMatrix = openglframework::Matrix4(mRadius, 0, 0, 0, - 0, mHeight, 0, 0, - 0, 0, mRadius, 0, - 0, 0, 0, 1); + // Compute the scaling matrix + mScalingMatrix = openglframework::Matrix4(mRadius, 0, 0, 0, + 0, mHeight, 0, 0, + 0, 0, mRadius, 0, + 0, 0, 0, 1); - // Initialize the position where the cylinder will be rendered - translateWorld(position); + // Initialize the position where the cylinder will be rendered + translateWorld(position); - // Create the collision shape for the rigid body (cylinder shape) and do - // not forget to delete it at the end - mCylinderShape = new rp3d::CylinderShape(mRadius, mHeight); + // Create the collision shape for the rigid body (cylinder shape) and do + // not forget to delete it at the end + mCylinderShape = new rp3d::CylinderShape(mRadius, mHeight); - // Initial position and orientation of the rigid body - rp3d::Vector3 initPosition(position.x, position.y, position.z); - rp3d::Quaternion initOrientation = rp3d::Quaternion::identity(); - rp3d::Transform transform(initPosition, initOrientation); + // Initial position and orientation of the rigid body + rp3d::Vector3 initPosition(position.x, position.y, position.z); + rp3d::Quaternion initOrientation = rp3d::Quaternion::identity(); + rp3d::Transform transform(initPosition, initOrientation); - // Create a rigid body corresponding to the cylinder in the dynamics world - rp3d::RigidBody* body = dynamicsWorld->createRigidBody(transform); + // Create a rigid body corresponding to the cylinder in the dynamics world + rp3d::RigidBody* body = dynamicsWorld->createRigidBody(transform); - // Add a collision shape to the body and specify the mass of the shape - m_proxyShape = body->addCollisionShape(mCylinderShape, rp3d::Transform::identity(), mass); + // Add a collision shape to the body and specify the mass of the shape + m_proxyShape = body->addCollisionShape(mCylinderShape, rp3d::Transform::identity(), mass); - mTransformMatrix = mTransformMatrix * mScalingMatrix; + mTransformMatrix = mTransformMatrix * mScalingMatrix; - mBody = body; + mBody = body; - // Create the VBOs and VAO - if (totalNbCylinders == 0) { - createVBOAndVAO(); - } + // Create the VBOs and VAO + if (totalNbCylinders == 0) { + createVBOAndVAO(); + } - totalNbCylinders++; + totalNbCylinders++; } // Destructor Cylinder::~Cylinder() { - if (totalNbCylinders == 1) { + if (totalNbCylinders == 1) { - // Destroy the mesh - destroy(); + // Destroy the mesh + destroy(); - // Destroy the VBOs and VAO - mVBOIndices.destroy(); - mVBOVertices.destroy(); - mVBONormals.destroy(); - mVBOTextureCoords.destroy(); - mVAO.destroy(); - } - delete mCylinderShape; - totalNbCylinders--; + // Destroy the VBOs and VAO + mVBOIndices.destroy(); + mVBOVertices.destroy(); + mVBONormals.destroy(); + mVBOTextureCoords.destroy(); + mVAO.destroy(); + } + delete mCylinderShape; + totalNbCylinders--; } // Render the cylinder at the correct position and with the correct orientation void Cylinder::render(openglframework::Shader& shader, - const openglframework::Matrix4& worldToCameraMatrix) { + const openglframework::Matrix4& worldToCameraMatrix) { - // Bind the shader - shader.bind(); + // Bind the shader + shader.bind(); - // Set the model to camera matrix - shader.setMatrix4x4Uniform("localToWorldMatrix", mTransformMatrix); - shader.setMatrix4x4Uniform("worldToCameraMatrix", worldToCameraMatrix); + // Set the model to camera matrix + shader.setMatrix4x4Uniform("localToWorldMatrix", mTransformMatrix); + shader.setMatrix4x4Uniform("worldToCameraMatrix", worldToCameraMatrix); - // Set the normal matrix (inverse transpose of the 3x3 upper-left sub matrix of the - // model-view matrix) - const openglframework::Matrix4 localToCameraMatrix = worldToCameraMatrix * mTransformMatrix; - const openglframework::Matrix3 normalMatrix = - localToCameraMatrix.getUpperLeft3x3Matrix().getInverse().getTranspose(); - shader.setMatrix3x3Uniform("normalMatrix", normalMatrix, false); + // Set the normal matrix (inverse transpose of the 3x3 upper-left sub matrix of the + // model-view matrix) + const openglframework::Matrix4 localToCameraMatrix = worldToCameraMatrix * mTransformMatrix; + const openglframework::Matrix3 normalMatrix = + localToCameraMatrix.getUpperLeft3x3Matrix().getInverse().getTranspose(); + shader.setMatrix3x3Uniform("normalMatrix", normalMatrix, false); - // Set the vertex color - openglframework::Color currentColor = mBody->isSleeping() ? mSleepingColor : mColor; - openglframework::Vector4 color(currentColor.r, currentColor.g, currentColor.b, currentColor.a); - shader.setVector4Uniform("vertexColor", color, false); + // Set the vertex color + openglframework::Color currentColor = mBody->isSleeping() ? mSleepingColor : mColor; + openglframework::Vector4 color(currentColor.r, currentColor.g, currentColor.b, currentColor.a); + shader.setVector4Uniform("vertexColor", color, false); - // Bind the VAO - mVAO.bind(); + // Bind the VAO + mVAO.bind(); - mVBOVertices.bind(); + mVBOVertices.bind(); - // Get the location of shader attribute variables - GLint vertexPositionLoc = shader.getAttribLocation("vertexPosition"); - GLint vertexNormalLoc = shader.getAttribLocation("vertexNormal", false); + // Get the location of shader attribute variables + GLint32_t vertexPositionLoc = shader.getAttribLocation("vertexPosition"); + GLint32_t vertexNormalLoc = shader.getAttribLocation("vertexNormal", false); - glEnableVertexAttribArray(vertexPositionLoc); - glVertexAttribPointer(vertexPositionLoc, 3, GL_FLOAT, GL_FALSE, 0, (char*)NULL); + glEnableVertexAttribArray(vertexPositionLoc); + glVertexAttribPointer(vertexPositionLoc, 3, GL_FLOAT, GL_FALSE, 0, (char*)NULL); - mVBONormals.bind(); + mVBONormals.bind(); - if (vertexNormalLoc != -1) glVertexAttribPointer(vertexNormalLoc, 3, GL_FLOAT, GL_FALSE, 0, (char*)NULL); - if (vertexNormalLoc != -1) glEnableVertexAttribArray(vertexNormalLoc); + if (vertexNormalLoc != -1) glVertexAttribPointer(vertexNormalLoc, 3, GL_FLOAT, GL_FALSE, 0, (char*)NULL); + if (vertexNormalLoc != -1) glEnableVertexAttribArray(vertexNormalLoc); - // For each part of the mesh - for (unsigned int i=0; isetTransform(transform); + // Reset the transform + mBody->setTransform(transform); - mBody->setIsSleeping(false); + mBody->setIsSleeping(false); - // Reset the velocity of the rigid body - rp3d::RigidBody* rigidBody = dynamic_cast(mBody); - if (rigidBody != NULL) { - rigidBody->setLinearVelocity(rp3d::Vector3(0, 0, 0)); - rigidBody->setAngularVelocity(rp3d::Vector3(0, 0, 0)); - } + // Reset the velocity of the rigid body + rp3d::RigidBody* rigidBody = dynamic_cast(mBody); + if (rigidBody != NULL) { + rigidBody->setLinearVelocity(rp3d::Vector3(0, 0, 0)); + rigidBody->setAngularVelocity(rp3d::Vector3(0, 0, 0)); + } - updateTransform(1.0f); + updateTransform(1.0f); } // Set the scaling of the object void Cylinder::setScaling(const openglframework::Vector3& scaling) { - // Scale the collision shape - m_proxyShape->setLocalScaling(rp3d::Vector3(scaling.x, scaling.y, scaling.z)); + // Scale the collision shape + m_proxyShape->setLocalScaling(rp3d::Vector3(scaling.x, scaling.y, scaling.z)); - // Scale the graphics object - mScalingMatrix = openglframework::Matrix4(mRadius * scaling.x, 0, 0, 0, - 0, mHeight * scaling.y, 0, 0, - 0, 0, mRadius * scaling.z, 0, - 0, 0, 0, 1); + // Scale the graphics object + mScalingMatrix = openglframework::Matrix4(mRadius * scaling.x, 0, 0, 0, + 0, mHeight * scaling.y, 0, 0, + 0, 0, mRadius * scaling.z, 0, + 0, 0, 0, 1); } diff --git a/tools/testbed/common/Cylinder.h b/tools/testbed/common/Cylinder.h index 73d4aab..86fd7f3 100644 --- a/tools/testbed/common/Cylinder.h +++ b/tools/testbed/common/Cylinder.h @@ -1,26 +1,26 @@ /******************************************************************************** -* ReactPhysics3D physics library, http://www.reactphysics3d.com * -* Copyright (c) 2010-2016 Daniel Chappuis * +* 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: * -* * +* 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. * -* * +* 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 CYLINDER_H @@ -34,81 +34,81 @@ // Class Cylinder class Cylinder : public openglframework::Mesh, public PhysicsObject { - private : + private : - // -------------------- Attributes -------------------- // + // -------------------- Attributes -------------------- // - /// Radius of the cylinder - float mRadius; + /// Radius of the cylinder + float mRadius; - /// Height of the cylinder - float mHeight; + /// Height of the cylinder + float mHeight; - /// Scaling matrix (applied to a sphere to obtain the correct cylinder dimensions) - openglframework::Matrix4 mScalingMatrix; + /// Scaling matrix (applied to a sphere to obtain the correct cylinder dimensions) + openglframework::Matrix4 mScalingMatrix; - /// Previous transform (for interpolation) - rp3d::Transform mPreviousTransform; + /// Previous transform (for int32_terpolation) + rp3d::Transform mPreviousTransform; - /// Collision shape - rp3d::CylinderShape* mCylinderShape; - rp3d::ProxyShape* m_proxyShape; + /// Collision shape + rp3d::CylinderShape* mCylinderShape; + rp3d::ProxyShape* m_proxyShape; - /// Vertex Buffer Object for the vertices data - static openglframework::VertexBufferObject mVBOVertices; + /// Vertex Buffer Object for the vertices data + static openglframework::VertexBufferObject mVBOVertices; - /// Vertex Buffer Object for the normals data - static openglframework::VertexBufferObject mVBONormals; + /// Vertex Buffer Object for the normals data + static openglframework::VertexBufferObject mVBONormals; - /// Vertex Buffer Object for the texture coords - static openglframework::VertexBufferObject mVBOTextureCoords; + /// Vertex Buffer Object for the texture coords + static openglframework::VertexBufferObject mVBOTextureCoords; - /// Vertex Buffer Object for the indices - static openglframework::VertexBufferObject mVBOIndices; + /// Vertex Buffer Object for the indices + static openglframework::VertexBufferObject mVBOIndices; - /// Vertex Array Object for the vertex data - static openglframework::VertexArrayObject mVAO; + /// Vertex Array Object for the vertex data + static openglframework::VertexArrayObject mVAO; - // Total number of capsules created - static int totalNbCylinders; + // Total number of capsules created + static int32_t totalNbCylinders; - // -------------------- Methods -------------------- // + // -------------------- Methods -------------------- // - // Create the Vertex Buffer Objects used to render with OpenGL. - void createVBOAndVAO(); + // Create the Vertex Buffer Objects used to render with OpenGL. + void createVBOAndVAO(); - public : + public : - // -------------------- Methods -------------------- // + // -------------------- Methods -------------------- // - /// Constructor - Cylinder(float radius, float height, const openglframework::Vector3& position, - rp3d::CollisionWorld* world, const std::string &meshFolderPath); + /// Constructor + Cylinder(float radius, float height, const openglframework::Vector3& position, + rp3d::CollisionWorld* world, const std::string &meshFolderPath); - /// Constructor - Cylinder(float radius, float height, const openglframework::Vector3& position, - float mass, rp3d::DynamicsWorld* dynamicsWorld, const std::string &meshFolderPath); + /// Constructor + Cylinder(float radius, float height, const openglframework::Vector3& position, + float mass, rp3d::DynamicsWorld* dynamicsWorld, const std::string &meshFolderPath); - /// Destructor - ~Cylinder(); + /// Destructor + ~Cylinder(); - /// Render the cylinder at the correct position and with the correct orientation - void render(openglframework::Shader& shader, - const openglframework::Matrix4& worldToCameraMatrix); + /// Render the cylinder at the correct position and with the correct orientation + void render(openglframework::Shader& shader, + const openglframework::Matrix4& worldToCameraMatrix); - /// Set the position of the box - void resetTransform(const rp3d::Transform& transform); + /// Set the position of the box + void resetTransform(const rp3d::Transform& transform); - /// Update the transform matrix of the object - virtual void updateTransform(float interpolationFactor); + /// Update the transform matrix of the object + virtual void updateTransform(float int32_terpolationFactor); - /// Set the scaling of the object - void setScaling(const openglframework::Vector3& scaling); + /// Set the scaling of the object + void setScaling(const openglframework::Vector3& scaling); }; // Update the transform matrix of the object -inline void Cylinder::updateTransform(float interpolationFactor) { - mTransformMatrix = computeTransform(interpolationFactor, mScalingMatrix); +inline void Cylinder::updateTransform(float int32_terpolationFactor) { + mTransformMatrix = computeTransform(int32_terpolationFactor, mScalingMatrix); } #endif diff --git a/tools/testbed/common/Dumbbell.cpp b/tools/testbed/common/Dumbbell.cpp index abe1a86..9139997 100644 --- a/tools/testbed/common/Dumbbell.cpp +++ b/tools/testbed/common/Dumbbell.cpp @@ -1,26 +1,26 @@ /******************************************************************************** -* ReactPhysics3D physics library, http://www.reactphysics3d.com * -* Copyright (c) 2010-2016 Daniel Chappuis * +* 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: * -* * +* 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. * -* * +* 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 @@ -31,319 +31,319 @@ openglframework::VertexBufferObject Dumbbell::mVBONormals(GL_ARRAY_BUFFER); openglframework::VertexBufferObject Dumbbell::mVBOTextureCoords(GL_ARRAY_BUFFER); openglframework::VertexBufferObject Dumbbell::mVBOIndices(GL_ELEMENT_ARRAY_BUFFER); openglframework::VertexArrayObject Dumbbell::mVAO; -int Dumbbell::totalNbDumbbells = 0; +int32_t Dumbbell::totalNbDumbbells = 0; // Constructor Dumbbell::Dumbbell(const openglframework::Vector3 &position, - reactphysics3d::DynamicsWorld* dynamicsWorld, const std::string& meshFolderPath) - : openglframework::Mesh() { + reactphysics3d::DynamicsWorld* dynamicsWorld, const std::string& meshFolderPath) + : openglframework::Mesh() { - // Load the mesh from a file - openglframework::MeshReaderWriter::loadMeshFromFile(meshFolderPath + "dumbbell.obj", *this); + // Load the mesh from a file + openglframework::MeshReaderWriter::loadMeshFromFile(meshFolderPath + "dumbbell.obj", *this); - // Calculate the normals of the mesh - calculateNormals(); + // Calculate the normals of the mesh + calculateNormals(); - // Identity scaling matrix - mScalingMatrix.setToIdentity(); + // Identity scaling matrix + mScalingMatrix.setToIdentity(); - mDistanceBetweenSphere = 8.0f; + mDistanceBetweenSphere = 8.0f; - // Initialize the position where the sphere will be rendered - translateWorld(position); + // Initialize the position where the sphere will be rendered + translateWorld(position); - // Create a sphere collision shape for the two ends of the dumbbell - // ReactPhysics3D will clone this object to create an internal one. Therefore, - // it is OK if this object is destroyed right after calling RigidBody::addCollisionShape() - const rp3d::decimal radiusSphere = rp3d::decimal(1.5); - const rp3d::decimal massSphere = rp3d::decimal(2.0); - mSphereShape = new rp3d::SphereShape(radiusSphere); + // Create a sphere collision shape for the two ends of the dumbbell + // ReactPhysics3D will clone this object to create an int32_ternal one. Therefore, + // it is OK if this object is destroyed right after calling RigidBody::addCollisionShape() + const rp3d::float radiusSphere = rp3d::float(1.5); + const rp3d::float massSphere = rp3d::float(2.0); + mSphereShape = new rp3d::SphereShape(radiusSphere); - // Create a cylinder collision shape for the middle of the dumbbell - // ReactPhysics3D will clone this object to create an internal one. Therefore, - // it is OK if this object is destroyed right after calling RigidBody::addCollisionShape() - const rp3d::decimal radiusCylinder = rp3d::decimal(0.5); - const rp3d::decimal heightCylinder = rp3d::decimal(8.0); - const rp3d::decimal massCylinder = rp3d::decimal(1.0); - mCylinderShape = new rp3d::CylinderShape(radiusCylinder, heightCylinder); + // Create a cylinder collision shape for the middle of the dumbbell + // ReactPhysics3D will clone this object to create an int32_ternal one. Therefore, + // it is OK if this object is destroyed right after calling RigidBody::addCollisionShape() + const rp3d::float radiusCylinder = rp3d::float(0.5); + const rp3d::float heightCylinder = rp3d::float(8.0); + const rp3d::float massCylinder = rp3d::float(1.0); + mCylinderShape = new rp3d::CylinderShape(radiusCylinder, heightCylinder); - // Initial position and orientation of the rigid body - rp3d::Vector3 initPosition(position.x, position.y, position.z); - rp3d::decimal angleAroundX = 0;//rp3d::PI / 2; - rp3d::Quaternion initOrientation(angleAroundX, 0, 0); - rp3d::Transform transformBody(initPosition, initOrientation); + // Initial position and orientation of the rigid body + rp3d::Vector3 initPosition(position.x, position.y, position.z); + rp3d::float angleAroundX = 0;//rp3d::PI / 2; + rp3d::Quaternion initOrientation(angleAroundX, 0, 0); + rp3d::Transform transformBody(initPosition, initOrientation); - mPreviousTransform = transformBody; + mPreviousTransform = transformBody; - // Initial transform of the first sphere collision shape of the dumbbell (in local-space) - rp3d::Transform transformSphereShape1(rp3d::Vector3(0, mDistanceBetweenSphere / 2.0f, 0), rp3d::Quaternion::identity()); + // Initial transform of the first sphere collision shape of the dumbbell (in local-space) + rp3d::Transform transformSphereShape1(rp3d::Vector3(0, mDistanceBetweenSphere / 2.0f, 0), rp3d::Quaternion::identity()); - // Initial transform of the second sphere collision shape of the dumbell (in local-space) - rp3d::Transform transformSphereShape2(rp3d::Vector3(0, -mDistanceBetweenSphere / 2.0f, 0), rp3d::Quaternion::identity()); + // Initial transform of the second sphere collision shape of the dumbell (in local-space) + rp3d::Transform transformSphereShape2(rp3d::Vector3(0, -mDistanceBetweenSphere / 2.0f, 0), rp3d::Quaternion::identity()); - // Initial transform of the cylinder collision shape of the dumbell (in local-space) - rp3d::Transform transformCylinderShape(rp3d::Vector3(0, 0, 0), rp3d::Quaternion::identity()); + // Initial transform of the cylinder collision shape of the dumbell (in local-space) + rp3d::Transform transformCylinderShape(rp3d::Vector3(0, 0, 0), rp3d::Quaternion::identity()); - // Create a rigid body corresponding to the dumbbell in the dynamics world - rp3d::RigidBody* body = dynamicsWorld->createRigidBody(transformBody); + // Create a rigid body corresponding to the dumbbell in the dynamics world + rp3d::RigidBody* body = dynamicsWorld->createRigidBody(transformBody); - // Add the three collision shapes to the body and specify the mass and transform of the shapes - m_proxyShapeSphere1 = body->addCollisionShape(mSphereShape, transformSphereShape1, massSphere); - m_proxyShapeSphere2 = body->addCollisionShape(mSphereShape, transformSphereShape2, massSphere); - m_proxyShapeCylinder = body->addCollisionShape(mCylinderShape, transformCylinderShape, massCylinder); + // Add the three collision shapes to the body and specify the mass and transform of the shapes + m_proxyShapeSphere1 = body->addCollisionShape(mSphereShape, transformSphereShape1, massSphere); + m_proxyShapeSphere2 = body->addCollisionShape(mSphereShape, transformSphereShape2, massSphere); + m_proxyShapeCylinder = body->addCollisionShape(mCylinderShape, transformCylinderShape, massCylinder); - mBody = body; + mBody = body; - mTransformMatrix = mTransformMatrix * mScalingMatrix; + mTransformMatrix = mTransformMatrix * mScalingMatrix; - // Create the VBOs and VAO - if (totalNbDumbbells == 0) { - createVBOAndVAO(); - } + // Create the VBOs and VAO + if (totalNbDumbbells == 0) { + createVBOAndVAO(); + } - totalNbDumbbells++; + totalNbDumbbells++; } // Constructor Dumbbell::Dumbbell(const openglframework::Vector3 &position, - reactphysics3d::CollisionWorld* world, const std::string& meshFolderPath) - : openglframework::Mesh() { + reactphysics3d::CollisionWorld* world, const std::string& meshFolderPath) + : openglframework::Mesh() { - // Load the mesh from a file - openglframework::MeshReaderWriter::loadMeshFromFile(meshFolderPath + "dumbbell.obj", *this); + // Load the mesh from a file + openglframework::MeshReaderWriter::loadMeshFromFile(meshFolderPath + "dumbbell.obj", *this); - // Calculate the normals of the mesh - calculateNormals(); + // Calculate the normals of the mesh + calculateNormals(); - // Identity scaling matrix - mScalingMatrix.setToIdentity(); + // Identity scaling matrix + mScalingMatrix.setToIdentity(); - mDistanceBetweenSphere = 8.0f; + mDistanceBetweenSphere = 8.0f; - // Initialize the position where the sphere will be rendered - translateWorld(position); + // Initialize the position where the sphere will be rendered + translateWorld(position); - // Create a sphere collision shape for the two ends of the dumbbell - // ReactPhysics3D will clone this object to create an internal one. Therefore, - // it is OK if this object is destroyed right after calling RigidBody::addCollisionShape() - const rp3d::decimal radiusSphere = rp3d::decimal(1.5); - mSphereShape = new rp3d::SphereShape(radiusSphere); + // Create a sphere collision shape for the two ends of the dumbbell + // ReactPhysics3D will clone this object to create an int32_ternal one. Therefore, + // it is OK if this object is destroyed right after calling RigidBody::addCollisionShape() + const rp3d::float radiusSphere = rp3d::float(1.5); + mSphereShape = new rp3d::SphereShape(radiusSphere); - // Create a cylinder collision shape for the middle of the dumbbell - // ReactPhysics3D will clone this object to create an internal one. Therefore, - // it is OK if this object is destroyed right after calling RigidBody::addCollisionShape() - const rp3d::decimal radiusCylinder = rp3d::decimal(0.5); - const rp3d::decimal heightCylinder = rp3d::decimal(8.0); - mCylinderShape = new rp3d::CylinderShape(radiusCylinder, heightCylinder); + // Create a cylinder collision shape for the middle of the dumbbell + // ReactPhysics3D will clone this object to create an int32_ternal one. Therefore, + // it is OK if this object is destroyed right after calling RigidBody::addCollisionShape() + const rp3d::float radiusCylinder = rp3d::float(0.5); + const rp3d::float heightCylinder = rp3d::float(8.0); + mCylinderShape = new rp3d::CylinderShape(radiusCylinder, heightCylinder); - // Initial position and orientation of the rigid body - rp3d::Vector3 initPosition(position.x, position.y, position.z); - rp3d::decimal angleAroundX = 0;//rp3d::PI / 2; - rp3d::Quaternion initOrientation(angleAroundX, 0, 0); - rp3d::Transform transformBody(initPosition, initOrientation); + // Initial position and orientation of the rigid body + rp3d::Vector3 initPosition(position.x, position.y, position.z); + rp3d::float angleAroundX = 0;//rp3d::PI / 2; + rp3d::Quaternion initOrientation(angleAroundX, 0, 0); + rp3d::Transform transformBody(initPosition, initOrientation); - // Initial transform of the first sphere collision shape of the dumbbell (in local-space) - rp3d::Transform transformSphereShape1(rp3d::Vector3(0, mDistanceBetweenSphere / 2.0f, 0), rp3d::Quaternion::identity()); + // Initial transform of the first sphere collision shape of the dumbbell (in local-space) + rp3d::Transform transformSphereShape1(rp3d::Vector3(0, mDistanceBetweenSphere / 2.0f, 0), rp3d::Quaternion::identity()); - // Initial transform of the second sphere collision shape of the dumbell (in local-space) - rp3d::Transform transformSphereShape2(rp3d::Vector3(0, -mDistanceBetweenSphere / 2.0f, 0), rp3d::Quaternion::identity()); + // Initial transform of the second sphere collision shape of the dumbell (in local-space) + rp3d::Transform transformSphereShape2(rp3d::Vector3(0, -mDistanceBetweenSphere / 2.0f, 0), rp3d::Quaternion::identity()); - // Initial transform of the cylinder collision shape of the dumbell (in local-space) - rp3d::Transform transformCylinderShape(rp3d::Vector3(0, 0, 0), rp3d::Quaternion::identity()); + // Initial transform of the cylinder collision shape of the dumbell (in local-space) + rp3d::Transform transformCylinderShape(rp3d::Vector3(0, 0, 0), rp3d::Quaternion::identity()); - // Create a rigid body corresponding to the dumbbell in the dynamics world - mBody = world->createCollisionBody(transformBody); + // Create a rigid body corresponding to the dumbbell in the dynamics world + mBody = world->createCollisionBody(transformBody); - // Add the three collision shapes to the body and specify the mass and transform of the shapes - m_proxyShapeSphere1 = mBody->addCollisionShape(mSphereShape, transformSphereShape1); - m_proxyShapeSphere2 = mBody->addCollisionShape(mSphereShape, transformSphereShape2); - m_proxyShapeCylinder = mBody->addCollisionShape(mCylinderShape, transformCylinderShape); + // Add the three collision shapes to the body and specify the mass and transform of the shapes + m_proxyShapeSphere1 = mBody->addCollisionShape(mSphereShape, transformSphereShape1); + m_proxyShapeSphere2 = mBody->addCollisionShape(mSphereShape, transformSphereShape2); + m_proxyShapeCylinder = mBody->addCollisionShape(mCylinderShape, transformCylinderShape); - mTransformMatrix = mTransformMatrix * mScalingMatrix; + mTransformMatrix = mTransformMatrix * mScalingMatrix; - // Create the VBOs and VAO - if (totalNbDumbbells == 0) { - createVBOAndVAO(); - } + // Create the VBOs and VAO + if (totalNbDumbbells == 0) { + createVBOAndVAO(); + } - totalNbDumbbells++; + totalNbDumbbells++; } // Destructor Dumbbell::~Dumbbell() { - if (totalNbDumbbells == 1) { + if (totalNbDumbbells == 1) { - // Destroy the mesh - destroy(); + // Destroy the mesh + destroy(); - // Destroy the VBOs and VAO - mVBOIndices.destroy(); - mVBOVertices.destroy(); - mVBONormals.destroy(); - mVBOTextureCoords.destroy(); - mVAO.destroy(); - } - delete mSphereShape; - delete mCylinderShape; - totalNbDumbbells--; + // Destroy the VBOs and VAO + mVBOIndices.destroy(); + mVBOVertices.destroy(); + mVBONormals.destroy(); + mVBOTextureCoords.destroy(); + mVAO.destroy(); + } + delete mSphereShape; + delete mCylinderShape; + totalNbDumbbells--; } // Render the sphere at the correct position and with the correct orientation void Dumbbell::render(openglframework::Shader& shader, - const openglframework::Matrix4& worldToCameraMatrix) { + const openglframework::Matrix4& worldToCameraMatrix) { - // Bind the shader - shader.bind(); + // Bind the shader + shader.bind(); - // Set the model to camera matrix - shader.setMatrix4x4Uniform("localToWorldMatrix", mTransformMatrix); - shader.setMatrix4x4Uniform("worldToCameraMatrix", worldToCameraMatrix); + // Set the model to camera matrix + shader.setMatrix4x4Uniform("localToWorldMatrix", mTransformMatrix); + shader.setMatrix4x4Uniform("worldToCameraMatrix", worldToCameraMatrix); - // Set the normal matrix (inverse transpose of the 3x3 upper-left sub matrix of the - // model-view matrix) - const openglframework::Matrix4 localToCameraMatrix = worldToCameraMatrix * mTransformMatrix; - const openglframework::Matrix3 normalMatrix = - localToCameraMatrix.getUpperLeft3x3Matrix().getInverse().getTranspose(); - shader.setMatrix3x3Uniform("normalMatrix", normalMatrix, false); + // Set the normal matrix (inverse transpose of the 3x3 upper-left sub matrix of the + // model-view matrix) + const openglframework::Matrix4 localToCameraMatrix = worldToCameraMatrix * mTransformMatrix; + const openglframework::Matrix3 normalMatrix = + localToCameraMatrix.getUpperLeft3x3Matrix().getInverse().getTranspose(); + shader.setMatrix3x3Uniform("normalMatrix", normalMatrix, false); - // Set the vertex color - openglframework::Color currentColor = mBody->isSleeping() ? mSleepingColor : mColor; - openglframework::Vector4 color(currentColor.r, currentColor.g, currentColor.b, currentColor.a); - shader.setVector4Uniform("vertexColor", color, false); + // Set the vertex color + openglframework::Color currentColor = mBody->isSleeping() ? mSleepingColor : mColor; + openglframework::Vector4 color(currentColor.r, currentColor.g, currentColor.b, currentColor.a); + shader.setVector4Uniform("vertexColor", color, false); - // Bind the VAO - mVAO.bind(); + // Bind the VAO + mVAO.bind(); - mVBOVertices.bind(); + mVBOVertices.bind(); - // Get the location of shader attribute variables - GLint vertexPositionLoc = shader.getAttribLocation("vertexPosition"); - GLint vertexNormalLoc = shader.getAttribLocation("vertexNormal", false); + // Get the location of shader attribute variables + GLint32_t vertexPositionLoc = shader.getAttribLocation("vertexPosition"); + GLint32_t vertexNormalLoc = shader.getAttribLocation("vertexNormal", false); - glEnableVertexAttribArray(vertexPositionLoc); - glVertexAttribPointer(vertexPositionLoc, 3, GL_FLOAT, GL_FALSE, 0, (char*)NULL); + glEnableVertexAttribArray(vertexPositionLoc); + glVertexAttribPointer(vertexPositionLoc, 3, GL_FLOAT, GL_FALSE, 0, (char*)NULL); - mVBONormals.bind(); + mVBONormals.bind(); - if (vertexNormalLoc != -1) glEnableVertexAttribArray(vertexNormalLoc); - if (vertexNormalLoc != -1) glVertexAttribPointer(vertexNormalLoc, 3, GL_FLOAT, GL_FALSE, 0, (char*)NULL); + if (vertexNormalLoc != -1) glEnableVertexAttribArray(vertexNormalLoc); + if (vertexNormalLoc != -1) glVertexAttribPointer(vertexNormalLoc, 3, GL_FLOAT, GL_FALSE, 0, (char*)NULL); - // For each part of the mesh - for (unsigned int i=0; isetTransform(transform); + // Reset the transform + mBody->setTransform(transform); - mBody->setIsSleeping(false); + mBody->setIsSleeping(false); - // Reset the velocity of the rigid body - rp3d::RigidBody* rigidBody = dynamic_cast(mBody); - if (rigidBody != NULL) { - rigidBody->setLinearVelocity(rp3d::Vector3(0, 0, 0)); - rigidBody->setAngularVelocity(rp3d::Vector3(0, 0, 0)); - } + // Reset the velocity of the rigid body + rp3d::RigidBody* rigidBody = dynamic_cast(mBody); + if (rigidBody != NULL) { + rigidBody->setLinearVelocity(rp3d::Vector3(0, 0, 0)); + rigidBody->setAngularVelocity(rp3d::Vector3(0, 0, 0)); + } - updateTransform(1.0f); + updateTransform(1.0f); } // Set the scaling of the object void Dumbbell::setScaling(const openglframework::Vector3& scaling) { - // Scale the collision shape - rp3d::Vector3 newScaling(scaling.x, scaling.y, scaling.z); - m_proxyShapeCylinder->setLocalScaling(newScaling); - m_proxyShapeSphere1->setLocalScaling(newScaling); - m_proxyShapeSphere2->setLocalScaling(newScaling); + // Scale the collision shape + rp3d::Vector3 newScaling(scaling.x, scaling.y, scaling.z); + m_proxyShapeCylinder->setLocalScaling(newScaling); + m_proxyShapeSphere1->setLocalScaling(newScaling); + m_proxyShapeSphere2->setLocalScaling(newScaling); - mDistanceBetweenSphere = (mDistanceBetweenSphere / mScalingMatrix.getValue(1, 1)) * scaling.y; + mDistanceBetweenSphere = (mDistanceBetweenSphere / mScalingMatrix.getValue(1, 1)) * scaling.y; - // Initial transform of the first sphere collision shape of the dumbbell (in local-space) - rp3d::Transform transformSphereShape1(rp3d::Vector3(0, mDistanceBetweenSphere / 2.0f, 0), rp3d::Quaternion::identity()); + // Initial transform of the first sphere collision shape of the dumbbell (in local-space) + rp3d::Transform transformSphereShape1(rp3d::Vector3(0, mDistanceBetweenSphere / 2.0f, 0), rp3d::Quaternion::identity()); - // Initial transform of the second sphere collision shape of the dumbell (in local-space) - rp3d::Transform transformSphereShape2(rp3d::Vector3(0, -mDistanceBetweenSphere / 2.0f, 0), rp3d::Quaternion::identity()); + // Initial transform of the second sphere collision shape of the dumbell (in local-space) + rp3d::Transform transformSphereShape2(rp3d::Vector3(0, -mDistanceBetweenSphere / 2.0f, 0), rp3d::Quaternion::identity()); - m_proxyShapeSphere1->setLocalToBodyTransform(transformSphereShape1); - m_proxyShapeSphere2->setLocalToBodyTransform(transformSphereShape2); + m_proxyShapeSphere1->setLocalToBodyTransform(transformSphereShape1); + m_proxyShapeSphere2->setLocalToBodyTransform(transformSphereShape2); - // Scale the graphics object - mScalingMatrix = openglframework::Matrix4(scaling.x, 0, 0, 0, - 0, scaling.y, 0, 0, - 0, 0, scaling.z, 0, - 0, 0, 0, 1); + // Scale the graphics object + mScalingMatrix = openglframework::Matrix4(scaling.x, 0, 0, 0, + 0, scaling.y, 0, 0, + 0, 0, scaling.z, 0, + 0, 0, 0, 1); } diff --git a/tools/testbed/common/Dumbbell.h b/tools/testbed/common/Dumbbell.h index 1b8b115..66dc027 100644 --- a/tools/testbed/common/Dumbbell.h +++ b/tools/testbed/common/Dumbbell.h @@ -1,26 +1,26 @@ /******************************************************************************** -* ReactPhysics3D physics library, http://www.reactphysics3d.com * -* Copyright (c) 2010-2016 Daniel Chappuis * +* 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: * -* * +* 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. * -* * +* 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 DUMBBELL_H @@ -34,82 +34,82 @@ // Class Sphere class Dumbbell : public openglframework::Mesh, public PhysicsObject { - private : + private : - // -------------------- Attributes -------------------- // + // -------------------- Attributes -------------------- // - /// Radius of the spheres - float mDistanceBetweenSphere; + /// Radius of the spheres + float mDistanceBetweenSphere; - /// Collision shapes - rp3d::CylinderShape* mCylinderShape; - rp3d::SphereShape* mSphereShape; - rp3d::ProxyShape* m_proxyShapeCylinder; - rp3d::ProxyShape* m_proxyShapeSphere1; - rp3d::ProxyShape* m_proxyShapeSphere2; + /// Collision shapes + rp3d::CylinderShape* mCylinderShape; + rp3d::SphereShape* mSphereShape; + rp3d::ProxyShape* m_proxyShapeCylinder; + rp3d::ProxyShape* m_proxyShapeSphere1; + rp3d::ProxyShape* m_proxyShapeSphere2; - /// Scaling matrix (applied to a sphere to obtain the correct sphere dimensions) - openglframework::Matrix4 mScalingMatrix; + /// Scaling matrix (applied to a sphere to obtain the correct sphere dimensions) + openglframework::Matrix4 mScalingMatrix; - /// Previous transform (for interpolation) - rp3d::Transform mPreviousTransform; + /// Previous transform (for int32_terpolation) + rp3d::Transform mPreviousTransform; - /// Vertex Buffer Object for the vertices data - static openglframework::VertexBufferObject mVBOVertices; + /// Vertex Buffer Object for the vertices data + static openglframework::VertexBufferObject mVBOVertices; - /// Vertex Buffer Object for the normals data - static openglframework::VertexBufferObject mVBONormals; + /// Vertex Buffer Object for the normals data + static openglframework::VertexBufferObject mVBONormals; - /// Vertex Buffer Object for the texture coords - static openglframework::VertexBufferObject mVBOTextureCoords; + /// Vertex Buffer Object for the texture coords + static openglframework::VertexBufferObject mVBOTextureCoords; - /// Vertex Buffer Object for the indices - static openglframework::VertexBufferObject mVBOIndices; + /// Vertex Buffer Object for the indices + static openglframework::VertexBufferObject mVBOIndices; - /// Vertex Array Object for the vertex data - static openglframework::VertexArrayObject mVAO; + /// Vertex Array Object for the vertex data + static openglframework::VertexArrayObject mVAO; - // Total number of capsules created - static int totalNbDumbbells; + // Total number of capsules created + static int32_t totalNbDumbbells; - // -------------------- Methods -------------------- // + // -------------------- Methods -------------------- // - // Create the Vertex Buffer Objects used to render with OpenGL. - void createVBOAndVAO(); + // Create the Vertex Buffer Objects used to render with OpenGL. + void createVBOAndVAO(); - public : + public : - // -------------------- Methods -------------------- // + // -------------------- Methods -------------------- // - /// Constructor - Dumbbell(const openglframework::Vector3& position, rp3d::DynamicsWorld* dynamicsWorld, - const std::string& meshFolderPath); + /// Constructor + Dumbbell(const openglframework::Vector3& position, rp3d::DynamicsWorld* dynamicsWorld, + const std::string& meshFolderPath); - /// Constructor - Dumbbell(const openglframework::Vector3& position, rp3d::CollisionWorld* world, - const std::string& meshFolderPath); + /// Constructor + Dumbbell(const openglframework::Vector3& position, rp3d::CollisionWorld* world, + const std::string& meshFolderPath); - /// Destructor - ~Dumbbell(); + /// Destructor + ~Dumbbell(); - /// Render the sphere at the correct position and with the correct orientation - void render(openglframework::Shader& shader, - const openglframework::Matrix4& worldToCameraMatrix); + /// Render the sphere at the correct position and with the correct orientation + void render(openglframework::Shader& shader, + const openglframework::Matrix4& worldToCameraMatrix); - /// Set the position of the box - void resetTransform(const rp3d::Transform& transform); + /// Set the position of the box + void resetTransform(const rp3d::Transform& transform); - /// Update the transform matrix of the object - virtual void updateTransform(float interpolationFactor); + /// Update the transform matrix of the object + virtual void updateTransform(float int32_terpolationFactor); - /// Set the scaling of the object - void setScaling(const openglframework::Vector3& scaling); + /// Set the scaling of the object + void setScaling(const openglframework::Vector3& scaling); }; // Update the transform matrix of the object -inline void Dumbbell::updateTransform(float interpolationFactor) { - mTransformMatrix = computeTransform(interpolationFactor, mScalingMatrix); +inline void Dumbbell::updateTransform(float int32_terpolationFactor) { + mTransformMatrix = computeTransform(int32_terpolationFactor, mScalingMatrix); } #endif diff --git a/tools/testbed/common/HeightField.cpp b/tools/testbed/common/HeightField.cpp index 332eaf6..8be301d 100644 --- a/tools/testbed/common/HeightField.cpp +++ b/tools/testbed/common/HeightField.cpp @@ -1,26 +1,26 @@ /******************************************************************************** -* ReactPhysics3D physics library, http://www.reactphysics3d.com * -* Copyright (c) 2010-2016 Daniel Chappuis * +* 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: * -* * +* 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. * -* * +* 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 @@ -29,324 +29,324 @@ // Constructor HeightField::HeightField(const openglframework::Vector3 &position, - reactphysics3d::CollisionWorld* world) - : openglframework::Mesh(), mVBOVertices(GL_ARRAY_BUFFER), - mVBONormals(GL_ARRAY_BUFFER), mVBOTextureCoords(GL_ARRAY_BUFFER), - mVBOIndices(GL_ELEMENT_ARRAY_BUFFER) { + reactphysics3d::CollisionWorld* world) + : openglframework::Mesh(), mVBOVertices(GL_ARRAY_BUFFER), + mVBONormals(GL_ARRAY_BUFFER), mVBOTextureCoords(GL_ARRAY_BUFFER), + mVBOIndices(GL_ELEMENT_ARRAY_BUFFER) { - // Initialize the position where the sphere will be rendered - translateWorld(position); + // Initialize the position where the sphere will be rendered + translateWorld(position); - // Compute the scaling matrix - mScalingMatrix = openglframework::Matrix4::identity(); + // Compute the scaling matrix + mScalingMatrix = openglframework::Matrix4::identity(); - // Generate the height field - generateHeightField(); + // Generate the height field + generateHeightField(); - // Generate the graphics mesh - generateGraphicsMesh(); + // Generate the graphics mesh + generateGraphicsMesh(); - // Create the collision shape for the rigid body (convex mesh shape) and - // do not forget to delete it at the end - mHeightFieldShape = new rp3d::HeightFieldShape(NB_POINTS_WIDTH, NB_POINTS_LENGTH, mMinHeight, mMaxHeight, - mHeightData, rp3d::HeightFieldShape::HEIGHT_FLOAT_TYPE); + // Create the collision shape for the rigid body (convex mesh shape) and + // do not forget to delete it at the end + mHeightFieldShape = new rp3d::HeightFieldShape(NB_POINTS_WIDTH, NB_POINTS_LENGTH, mMinHeight, mMaxHeight, + mHeightData, rp3d::HeightFieldShape::HEIGHT_FLOAT_TYPE); - // Initial position and orientation of the rigid body - rp3d::Vector3 initPosition(position.x, position.y, position.z); - rp3d::Quaternion initOrientation = rp3d::Quaternion::identity(); - rp3d::Transform transform(initPosition, initOrientation); + // Initial position and orientation of the rigid body + rp3d::Vector3 initPosition(position.x, position.y, position.z); + rp3d::Quaternion initOrientation = rp3d::Quaternion::identity(); + rp3d::Transform transform(initPosition, initOrientation); - mPreviousTransform = transform; + mPreviousTransform = transform; - // Create a rigid body corresponding to the sphere in the dynamics world - mBody = world->createCollisionBody(transform); + // Create a rigid body corresponding to the sphere in the dynamics world + mBody = world->createCollisionBody(transform); - // Add a collision shape to the body and specify the mass of the collision shape - m_proxyShape = mBody->addCollisionShape(mHeightFieldShape, rp3d::Transform::identity()); + // Add a collision shape to the body and specify the mass of the collision shape + m_proxyShape = mBody->addCollisionShape(mHeightFieldShape, rp3d::Transform::identity()); - // Create the VBOs and VAO - createVBOAndVAO(); + // Create the VBOs and VAO + createVBOAndVAO(); - mTransformMatrix = mTransformMatrix * mScalingMatrix; + mTransformMatrix = mTransformMatrix * mScalingMatrix; } // Constructor HeightField::HeightField(const openglframework::Vector3 &position, float mass, - reactphysics3d::DynamicsWorld* dynamicsWorld) - : openglframework::Mesh(), mVBOVertices(GL_ARRAY_BUFFER), - mVBONormals(GL_ARRAY_BUFFER), mVBOTextureCoords(GL_ARRAY_BUFFER), - mVBOIndices(GL_ELEMENT_ARRAY_BUFFER) { + reactphysics3d::DynamicsWorld* dynamicsWorld) + : openglframework::Mesh(), mVBOVertices(GL_ARRAY_BUFFER), + mVBONormals(GL_ARRAY_BUFFER), mVBOTextureCoords(GL_ARRAY_BUFFER), + mVBOIndices(GL_ELEMENT_ARRAY_BUFFER) { - // Initialize the position where the sphere will be rendered - translateWorld(position); + // Initialize the position where the sphere will be rendered + translateWorld(position); - // Compute the scaling matrix - mScalingMatrix = openglframework::Matrix4::identity(); + // Compute the scaling matrix + mScalingMatrix = openglframework::Matrix4::identity(); - // Generate the height field - generateHeightField(); + // Generate the height field + generateHeightField(); - // Generate the graphics mesh - generateGraphicsMesh(); + // Generate the graphics mesh + generateGraphicsMesh(); - // Create the collision shape for the rigid body (convex mesh shape) and - // do not forget to delete it at the end - mHeightFieldShape = new rp3d::HeightFieldShape(NB_POINTS_WIDTH, NB_POINTS_LENGTH, mMinHeight, mMaxHeight, - mHeightData, rp3d::HeightFieldShape::HEIGHT_FLOAT_TYPE); + // Create the collision shape for the rigid body (convex mesh shape) and + // do not forget to delete it at the end + mHeightFieldShape = new rp3d::HeightFieldShape(NB_POINTS_WIDTH, NB_POINTS_LENGTH, mMinHeight, mMaxHeight, + mHeightData, rp3d::HeightFieldShape::HEIGHT_FLOAT_TYPE); - // Initial position and orientation of the rigid body - rp3d::Vector3 initPosition(position.x, position.y, position.z); - rp3d::Quaternion initOrientation = rp3d::Quaternion::identity(); - rp3d::Transform transform(initPosition, initOrientation); + // Initial position and orientation of the rigid body + rp3d::Vector3 initPosition(position.x, position.y, position.z); + rp3d::Quaternion initOrientation = rp3d::Quaternion::identity(); + rp3d::Transform transform(initPosition, initOrientation); - // Create a rigid body corresponding to the sphere in the dynamics world - rp3d::RigidBody* body = dynamicsWorld->createRigidBody(transform); + // Create a rigid body corresponding to the sphere in the dynamics world + rp3d::RigidBody* body = dynamicsWorld->createRigidBody(transform); - // Add a collision shape to the body and specify the mass of the collision shape - m_proxyShape = body->addCollisionShape(mHeightFieldShape, rp3d::Transform::identity(), mass); + // Add a collision shape to the body and specify the mass of the collision shape + m_proxyShape = body->addCollisionShape(mHeightFieldShape, rp3d::Transform::identity(), mass); - mBody = body; + mBody = body; - // Create the VBOs and VAO - createVBOAndVAO(); + // Create the VBOs and VAO + createVBOAndVAO(); - mTransformMatrix = mTransformMatrix * mScalingMatrix; + mTransformMatrix = mTransformMatrix * mScalingMatrix; } // Destructor HeightField::~HeightField() { - // Destroy the mesh - destroy(); + // Destroy the mesh + destroy(); - // Destroy the VBOs and VAO - mVBOIndices.destroy(); - mVBOVertices.destroy(); - mVBONormals.destroy(); - mVBOTextureCoords.destroy(); - mVAO.destroy(); + // Destroy the VBOs and VAO + mVBOIndices.destroy(); + mVBOVertices.destroy(); + mVBONormals.destroy(); + mVBOTextureCoords.destroy(); + mVAO.destroy(); - delete mHeightFieldShape; + delete mHeightFieldShape; } // Render the sphere at the correct position and with the correct orientation void HeightField::render(openglframework::Shader& shader, - const openglframework::Matrix4& worldToCameraMatrix) { + const openglframework::Matrix4& worldToCameraMatrix) { - // Bind the shader - shader.bind(); + // Bind the shader + shader.bind(); - // Set the model to camera matrix - shader.setMatrix4x4Uniform("localToWorldMatrix", mTransformMatrix); - shader.setMatrix4x4Uniform("worldToCameraMatrix", worldToCameraMatrix); + // Set the model to camera matrix + shader.setMatrix4x4Uniform("localToWorldMatrix", mTransformMatrix); + shader.setMatrix4x4Uniform("worldToCameraMatrix", worldToCameraMatrix); - // Set the normal matrix (inverse transpose of the 3x3 upper-left sub matrix of the - // model-view matrix) - const openglframework::Matrix4 localToCameraMatrix = worldToCameraMatrix * mTransformMatrix; - const openglframework::Matrix3 normalMatrix = - localToCameraMatrix.getUpperLeft3x3Matrix().getInverse().getTranspose(); - shader.setMatrix3x3Uniform("normalMatrix", normalMatrix, false); + // Set the normal matrix (inverse transpose of the 3x3 upper-left sub matrix of the + // model-view matrix) + const openglframework::Matrix4 localToCameraMatrix = worldToCameraMatrix * mTransformMatrix; + const openglframework::Matrix3 normalMatrix = + localToCameraMatrix.getUpperLeft3x3Matrix().getInverse().getTranspose(); + shader.setMatrix3x3Uniform("normalMatrix", normalMatrix, false); - // Set the vertex color - openglframework::Color currentColor = mBody->isSleeping() ? mSleepingColor : mColor; - openglframework::Vector4 color(currentColor.r, currentColor.g, currentColor.b, currentColor.a); - shader.setVector4Uniform("vertexColor", color, false); + // Set the vertex color + openglframework::Color currentColor = mBody->isSleeping() ? mSleepingColor : mColor; + openglframework::Vector4 color(currentColor.r, currentColor.g, currentColor.b, currentColor.a); + shader.setVector4Uniform("vertexColor", color, false); - // Bind the VAO - mVAO.bind(); + // Bind the VAO + mVAO.bind(); - mVBOVertices.bind(); + mVBOVertices.bind(); - // Get the location of shader attribute variables - GLint vertexPositionLoc = shader.getAttribLocation("vertexPosition"); - GLint vertexNormalLoc = shader.getAttribLocation("vertexNormal", false); + // Get the location of shader attribute variables + GLint32_t vertexPositionLoc = shader.getAttribLocation("vertexPosition"); + GLint32_t vertexNormalLoc = shader.getAttribLocation("vertexNormal", false); - glEnableVertexAttribArray(vertexPositionLoc); - glVertexAttribPointer(vertexPositionLoc, 3, GL_FLOAT, GL_FALSE, 0, (char*)NULL); + glEnableVertexAttribArray(vertexPositionLoc); + glVertexAttribPointer(vertexPositionLoc, 3, GL_FLOAT, GL_FALSE, 0, (char*)NULL); - mVBONormals.bind(); + mVBONormals.bind(); - if (vertexNormalLoc != -1) glVertexAttribPointer(vertexNormalLoc, 3, GL_FLOAT, GL_FALSE, 0, (char*)NULL); - if (vertexNormalLoc != -1) glEnableVertexAttribArray(vertexNormalLoc); + if (vertexNormalLoc != -1) glVertexAttribPointer(vertexNormalLoc, 3, GL_FLOAT, GL_FALSE, 0, (char*)NULL); + if (vertexNormalLoc != -1) glEnableVertexAttribArray(vertexNormalLoc); - // For each part of the mesh - for (unsigned int i=0; i mMaxHeight) mMaxHeight = mHeightData[arrayIndex] ; - if (mHeightData[arrayIndex] < mMinHeight) mMinHeight = mHeightData[arrayIndex] ; - } - } + if (mHeightData[arrayIndex] > mMaxHeight) mMaxHeight = mHeightData[arrayIndex] ; + if (mHeightData[arrayIndex] < mMinHeight) mMinHeight = mHeightData[arrayIndex] ; + } + } } // Generate the graphics mesh to render the height field void HeightField::generateGraphicsMesh() { - std::vector indices; - int vertexId = 0; + std::vector indices; + int32_t vertexId = 0; - for (int i=0; isetTransform(transform); + // Reset the transform + mBody->setTransform(transform); - mBody->setIsSleeping(false); + mBody->setIsSleeping(false); - // Reset the velocity of the rigid body - rp3d::RigidBody* rigidBody = dynamic_cast(mBody); - if (rigidBody != NULL) { - rigidBody->setLinearVelocity(rp3d::Vector3(0, 0, 0)); - rigidBody->setAngularVelocity(rp3d::Vector3(0, 0, 0)); - } + // Reset the velocity of the rigid body + rp3d::RigidBody* rigidBody = dynamic_cast(mBody); + if (rigidBody != NULL) { + rigidBody->setLinearVelocity(rp3d::Vector3(0, 0, 0)); + rigidBody->setAngularVelocity(rp3d::Vector3(0, 0, 0)); + } - updateTransform(1.0f); + updateTransform(1.0f); } // Set the scaling of the object void HeightField::setScaling(const openglframework::Vector3& scaling) { - // Scale the collision shape - m_proxyShape->setLocalScaling(rp3d::Vector3(scaling.x, scaling.y, scaling.z)); + // Scale the collision shape + m_proxyShape->setLocalScaling(rp3d::Vector3(scaling.x, scaling.y, scaling.z)); - // Scale the graphics object - mScalingMatrix = openglframework::Matrix4(scaling.x, 0, 0, 0, - 0, scaling.y, 0,0, - 0, 0, scaling.z, 0, - 0, 0, 0, 1.0f); + // Scale the graphics object + mScalingMatrix = openglframework::Matrix4(scaling.x, 0, 0, 0, + 0, scaling.y, 0,0, + 0, 0, scaling.z, 0, + 0, 0, 0, 1.0f); } diff --git a/tools/testbed/common/HeightField.h b/tools/testbed/common/HeightField.h index e1f0369..8ce33b9 100644 --- a/tools/testbed/common/HeightField.h +++ b/tools/testbed/common/HeightField.h @@ -1,26 +1,26 @@ /******************************************************************************** -* ReactPhysics3D physics library, http://www.reactphysics3d.com * -* Copyright (c) 2010-2016 Daniel Chappuis * +* 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: * -* * +* 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. * -* * +* 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 HEIGHT_FIELD_H @@ -35,87 +35,87 @@ // Class HeightField class HeightField : public openglframework::Mesh, public PhysicsObject { - private : + private : - static const int NB_POINTS_WIDTH = 100; - static const int NB_POINTS_LENGTH = 100; + static const int32_t NB_POINTS_WIDTH = 100; + static const int32_t NB_POINTS_LENGTH = 100; - // -------------------- Attributes -------------------- // + // -------------------- Attributes -------------------- // - /// Height field data - float mHeightData[NB_POINTS_WIDTH * NB_POINTS_LENGTH]; + /// Height field data + float mHeightData[NB_POINTS_WIDTH * NB_POINTS_LENGTH]; - /// Previous transform (for interpolation) - rp3d::Transform mPreviousTransform; + /// Previous transform (for int32_terpolation) + rp3d::Transform mPreviousTransform; - /// Collision shape - rp3d::HeightFieldShape* mHeightFieldShape; - rp3d::ProxyShape* m_proxyShape; + /// Collision shape + rp3d::HeightFieldShape* mHeightFieldShape; + rp3d::ProxyShape* m_proxyShape; - /// Scaling matrix - openglframework::Matrix4 mScalingMatrix; + /// Scaling matrix + openglframework::Matrix4 mScalingMatrix; - /// Vertex Buffer Object for the vertices data - openglframework::VertexBufferObject mVBOVertices; + /// Vertex Buffer Object for the vertices data + openglframework::VertexBufferObject mVBOVertices; - /// Vertex Buffer Object for the normals data - openglframework::VertexBufferObject mVBONormals; + /// Vertex Buffer Object for the normals data + openglframework::VertexBufferObject mVBONormals; - /// Vertex Buffer Object for the texture coords - openglframework::VertexBufferObject mVBOTextureCoords; + /// Vertex Buffer Object for the texture coords + openglframework::VertexBufferObject mVBOTextureCoords; - /// Vertex Buffer Object for the indices - openglframework::VertexBufferObject mVBOIndices; + /// Vertex Buffer Object for the indices + openglframework::VertexBufferObject mVBOIndices; - /// Vertex Array Object for the vertex data - openglframework::VertexArrayObject mVAO; + /// Vertex Array Object for the vertex data + openglframework::VertexArrayObject mVAO; - /// Min/Max height of the height field values - float mMinHeight, mMaxHeight; + /// Min/Max height of the height field values + float mMinHeight, mMaxHeight; - // -------------------- Methods -------------------- // + // -------------------- Methods -------------------- // - /// Create the Vertex Buffer Objects used to render with OpenGL. - void createVBOAndVAO(); + /// Create the Vertex Buffer Objects used to render with OpenGL. + void createVBOAndVAO(); - /// Compute the heights of the height field - void generateHeightField(); + /// Compute the heights of the height field + void generateHeightField(); - /// Generate the graphics mesh to render the height field - void generateGraphicsMesh(); + /// Generate the graphics mesh to render the height field + void generateGraphicsMesh(); - public : + public : - // -------------------- Methods -------------------- // + // -------------------- Methods -------------------- // - /// Constructor - HeightField(const openglframework::Vector3& position, - rp3d::CollisionWorld* world); + /// Constructor + HeightField(const openglframework::Vector3& position, + rp3d::CollisionWorld* world); - /// Constructor - HeightField(const openglframework::Vector3& position, float mass, - rp3d::DynamicsWorld* dynamicsWorld); + /// Constructor + HeightField(const openglframework::Vector3& position, float mass, + rp3d::DynamicsWorld* dynamicsWorld); - /// Destructor - ~HeightField(); + /// Destructor + ~HeightField(); - /// Render the mesh at the correct position and with the correct orientation - void render(openglframework::Shader& shader, - const openglframework::Matrix4& worldToCameraMatrix); + /// Render the mesh at the correct position and with the correct orientation + void render(openglframework::Shader& shader, + const openglframework::Matrix4& worldToCameraMatrix); - /// Set the position of the box - void resetTransform(const rp3d::Transform& transform); + /// Set the position of the box + void resetTransform(const rp3d::Transform& transform); - /// Update the transform matrix of the object - virtual void updateTransform(float interpolationFactor); + /// Update the transform matrix of the object + virtual void updateTransform(float int32_terpolationFactor); - /// Set the scaling of the object - void setScaling(const openglframework::Vector3& scaling); + /// Set the scaling of the object + void setScaling(const openglframework::Vector3& scaling); }; // Update the transform matrix of the object -inline void HeightField::updateTransform(float interpolationFactor) { - mTransformMatrix = computeTransform(interpolationFactor, mScalingMatrix); +inline void HeightField::updateTransform(float int32_terpolationFactor) { + mTransformMatrix = computeTransform(int32_terpolationFactor, mScalingMatrix); } #endif diff --git a/tools/testbed/common/Line.cpp b/tools/testbed/common/Line.cpp index 8ef9116..a903b84 100644 --- a/tools/testbed/common/Line.cpp +++ b/tools/testbed/common/Line.cpp @@ -1,26 +1,26 @@ /******************************************************************************** -* ReactPhysics3D physics library, http://www.reactphysics3d.com * -* Copyright (c) 2010-2016 Daniel Chappuis * +* 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: * -* * +* 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. * -* * +* 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 @@ -28,8 +28,8 @@ // Constructor Line::Line(const openglframework::Vector3& worldPoint1, - const openglframework::Vector3& worldPoint2) - : mWorldPoint1(worldPoint1), mWorldPoint2(worldPoint2) { + const openglframework::Vector3& worldPoint2) + : mWorldPoint1(worldPoint1), mWorldPoint2(worldPoint2) { } @@ -41,26 +41,26 @@ Line::~Line() { // Render the sphere at the correct position and with the correct orientation void Line::render(openglframework::Shader& shader, - const openglframework::Matrix4& worldToCameraMatrix) { + const openglframework::Matrix4& worldToCameraMatrix) { - // Bind the shader - shader.bind(); + // Bind the shader + shader.bind(); - // Set the model to camera matrix - shader.setMatrix4x4Uniform("localToWorldMatrix", openglframework::Matrix4::identity()); - shader.setMatrix4x4Uniform("worldToCameraMatrix", worldToCameraMatrix); + // Set the model to camera matrix + shader.setMatrix4x4Uniform("localToWorldMatrix", openglframework::Matrix4::identity()); + shader.setMatrix4x4Uniform("worldToCameraMatrix", worldToCameraMatrix); - // Set the vertex color - openglframework::Vector4 color(1, 0, 0, 1); - shader.setVector4Uniform("vertexColor", color, false); + // Set the vertex color + openglframework::Vector4 color(1, 0, 0, 1); + shader.setVector4Uniform("vertexColor", color, false); - /* - glBegin(GL_LINES); - glVertex3f(mWorldPoint1.x, mWorldPoint1.y, mWorldPoint1.z); - glVertex3f(mWorldPoint2.x, mWorldPoint2.y, mWorldPoint2.z); - glEnd(); - */ + /* + glBegin(GL_LINES); + glVertex3f(mWorldPoint1.x, mWorldPoint1.y, mWorldPoint1.z); + glVertex3f(mWorldPoint2.x, mWorldPoint2.y, mWorldPoint2.z); + glEnd(); + */ - // Unbind the shader - shader.unbind(); + // Unbind the shader + shader.unbind(); } diff --git a/tools/testbed/common/Line.h b/tools/testbed/common/Line.h index 5858619..ae10101 100644 --- a/tools/testbed/common/Line.h +++ b/tools/testbed/common/Line.h @@ -1,26 +1,26 @@ /******************************************************************************** -* ReactPhysics3D physics library, http://www.reactphysics3d.com * -* Copyright (c) 2010-2016 Daniel Chappuis * +* 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: * -* * +* 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. * -* * +* 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 LINE_H @@ -33,44 +33,44 @@ // Class Line class Line : public openglframework::Object3D { - private : + private : - // -------------------- Attributes -------------------- // + // -------------------- Attributes -------------------- // - openglframework::Vector3 mWorldPoint1, mWorldPoint2; + openglframework::Vector3 mWorldPoint1, mWorldPoint2; - // -------------------- Methods -------------------- // + // -------------------- Methods -------------------- // - public : + public : - // -------------------- Methods -------------------- // + // -------------------- Methods -------------------- // - /// Constructor - Line(const openglframework::Vector3& worldPoint1, - const openglframework::Vector3& worldPoint2); + /// Constructor + Line(const openglframework::Vector3& worldPoint1, + const openglframework::Vector3& worldPoint2); - /// Destructor - ~Line(); + /// Destructor + ~Line(); - /// Return the first point of the line - openglframework::Vector3 getPoint1() const; + /// Return the first point of the line + openglframework::Vector3 getPoint1() const; - /// Return the second point of the line - openglframework::Vector3 getPoint2() const; + /// Return the second point of the line + openglframework::Vector3 getPoint2() const; - /// Render the line at the correct position and with the correct orientation - void render(openglframework::Shader& shader, - const openglframework::Matrix4& worldToCameraMatrix); + /// Render the line at the correct position and with the correct orientation + void render(openglframework::Shader& shader, + const openglframework::Matrix4& worldToCameraMatrix); }; // Return the first point of the line inline openglframework::Vector3 Line::getPoint1() const { - return mWorldPoint1; + return mWorldPoint1; } // Return the second point of the line inline openglframework::Vector3 Line::getPoint2() const { - return mWorldPoint2; + return mWorldPoint2; } #endif diff --git a/tools/testbed/common/PerlinNoise.cpp b/tools/testbed/common/PerlinNoise.cpp index 6c44600..e57ce0f 100644 --- a/tools/testbed/common/PerlinNoise.cpp +++ b/tools/testbed/common/PerlinNoise.cpp @@ -9,7 +9,7 @@ PerlinNoise::PerlinNoise() randomseed = 0; } -PerlinNoise::PerlinNoise(double _persistence, double _frequency, double _amplitude, int _octaves, int _randomseed) +PerlinNoise::PerlinNoise(double _persistence, double _frequency, double _amplitude, int32_t _octaves, int32_t _randomseed) { persistence = _persistence; frequency = _frequency; @@ -18,7 +18,7 @@ PerlinNoise::PerlinNoise(double _persistence, double _frequency, double _amplitu randomseed = 2 + _randomseed * _randomseed; } -void PerlinNoise::Set(double _persistence, double _frequency, double _amplitude, int _octaves, int _randomseed) +void PerlinNoise::Set(double _persistence, double _frequency, double _amplitude, int32_t _octaves, int32_t _randomseed) { persistence = _persistence; frequency = _frequency; @@ -34,78 +34,78 @@ double PerlinNoise::GetHeight(double x, double y) const double PerlinNoise::Total(double i, double j) const { - //properties of one octave (changing each loop) - double t = 0.0f; - double _amplitude = 1; - double freq = frequency; + //properties of one octave (changing each loop) + double t = 0.0f; + double _amplitude = 1; + double freq = frequency; - for(int k = 0; k < octaves; k++) - { - t += GetValue(j * freq + randomseed, i * freq + randomseed) * _amplitude; - _amplitude *= persistence; - freq *= 2; - } + for(int32_t k = 0; k < octaves; k++) + { + t += GetValue(j * freq + randomseed, i * freq + randomseed) * _amplitude; + _amplitude *= persistence; + freq *= 2; + } - return t; + return t; } double PerlinNoise::GetValue(double x, double y) const { - int Xint = (int)x; - int Yint = (int)y; - double Xfrac = x - Xint; - double Yfrac = y - Yint; + int32_t Xint32_t = (int32_t)x; + int32_t Yint32_t = (int32_t)y; + double Xfrac = x - Xint32_t; + double Yfrac = y - Yint32_t; //noise values - double n01 = Noise(Xint-1, Yint-1); - double n02 = Noise(Xint+1, Yint-1); - double n03 = Noise(Xint-1, Yint+1); - double n04 = Noise(Xint+1, Yint+1); - double n05 = Noise(Xint-1, Yint); - double n06 = Noise(Xint+1, Yint); - double n07 = Noise(Xint, Yint-1); - double n08 = Noise(Xint, Yint+1); - double n09 = Noise(Xint, Yint); + double n01 = Noise(Xint32_t-1, Yint32_t-1); + double n02 = Noise(Xint32_t+1, Yint32_t-1); + double n03 = Noise(Xint32_t-1, Yint32_t+1); + double n04 = Noise(Xint32_t+1, Yint32_t+1); + double n05 = Noise(Xint32_t-1, Yint32_t); + double n06 = Noise(Xint32_t+1, Yint32_t); + double n07 = Noise(Xint32_t, Yint32_t-1); + double n08 = Noise(Xint32_t, Yint32_t+1); + double n09 = Noise(Xint32_t, Yint32_t); - double n12 = Noise(Xint+2, Yint-1); - double n14 = Noise(Xint+2, Yint+1); - double n16 = Noise(Xint+2, Yint); + double n12 = Noise(Xint32_t+2, Yint32_t-1); + double n14 = Noise(Xint32_t+2, Yint32_t+1); + double n16 = Noise(Xint32_t+2, Yint32_t); - double n23 = Noise(Xint-1, Yint+2); - double n24 = Noise(Xint+1, Yint+2); - double n28 = Noise(Xint, Yint+2); + double n23 = Noise(Xint32_t-1, Yint32_t+2); + double n24 = Noise(Xint32_t+1, Yint32_t+2); + double n28 = Noise(Xint32_t, Yint32_t+2); - double n34 = Noise(Xint+2, Yint+2); + double n34 = Noise(Xint32_t+2, Yint32_t+2); - //find the noise values of the four corners - double x0y0 = 0.0625*(n01+n02+n03+n04) + 0.125*(n05+n06+n07+n08) + 0.25*(n09); - double x1y0 = 0.0625*(n07+n12+n08+n14) + 0.125*(n09+n16+n02+n04) + 0.25*(n06); - double x0y1 = 0.0625*(n05+n06+n23+n24) + 0.125*(n03+n04+n09+n28) + 0.25*(n08); - double x1y1 = 0.0625*(n09+n16+n28+n34) + 0.125*(n08+n14+n06+n24) + 0.25*(n04); + //find the noise values of the four corners + double x0y0 = 0.0625*(n01+n02+n03+n04) + 0.125*(n05+n06+n07+n08) + 0.25*(n09); + double x1y0 = 0.0625*(n07+n12+n08+n14) + 0.125*(n09+n16+n02+n04) + 0.25*(n06); + double x0y1 = 0.0625*(n05+n06+n23+n24) + 0.125*(n03+n04+n09+n28) + 0.25*(n08); + double x1y1 = 0.0625*(n09+n16+n28+n34) + 0.125*(n08+n14+n06+n24) + 0.25*(n04); - //interpolate between those values according to the x and y fractions - double v1 = Interpolate(x0y0, x1y0, Xfrac); //interpolate in x direction (y) - double v2 = Interpolate(x0y1, x1y1, Xfrac); //interpolate in x direction (y+1) - double fin = Interpolate(v1, v2, Yfrac); //interpolate in y direction + //int32_terpolate between those values according to the x and y fractions + double v1 = Interpolate(x0y0, x1y0, Xfrac); //int32_terpolate in x direction (y) + double v2 = Interpolate(x0y1, x1y1, Xfrac); //int32_terpolate in x direction (y+1) + double fin = Interpolate(v1, v2, Yfrac); //int32_terpolate in y direction - return fin; + return fin; } double PerlinNoise::Interpolate(double x, double y, double a) const { - double negA = 1.0 - a; + double negA = 1.0 - a; double negASqr = negA * negA; - double fac1 = 3.0 * (negASqr) - 2.0 * (negASqr * negA); + double fac1 = 3.0 * (negASqr) - 2.0 * (negASqr * negA); double aSqr = a * a; - double fac2 = 3.0 * aSqr - 2.0 * (aSqr * a); + double fac2 = 3.0 * aSqr - 2.0 * (aSqr * a); - return x * fac1 + y * fac2; //add the weighted factors + return x * fac1 + y * fac2; //add the weighted factors } -double PerlinNoise::Noise(int x, int y) const +double PerlinNoise::Noise(int32_t x, int32_t y) const { - int n = x + y * 57; - n = (n << 13) ^ n; - int t = (n * (n * n * 15731 + 789221) + 1376312589) & 0x7fffffff; - return 1.0 - double(t) * 0.931322574615478515625e-9;/// 1073741824.0); + int32_t n = x + y * 57; + n = (n << 13) ^ n; + int32_t t = (n * (n * n * 15731 + 789221) + 1376312589) & 0x7fffffff; + return 1.0 - double(t) * 0.931322574615478515625e-9;/// 1073741824.0); } diff --git a/tools/testbed/common/PerlinNoise.h b/tools/testbed/common/PerlinNoise.h index 8fa2dd4..33ec383 100644 --- a/tools/testbed/common/PerlinNoise.h +++ b/tools/testbed/common/PerlinNoise.h @@ -5,40 +5,40 @@ /// Code from http://stackoverflow.com/questions/4753055/perlin-noise-generation-for-terrain class PerlinNoise { - public: + public: - // Constructor - PerlinNoise(); - PerlinNoise(double _persistence, double _frequency, double _amplitude, int _octaves, int _randomseed); + // Constructor + PerlinNoise(); + PerlinNoise(double _persistence, double _frequency, double _amplitude, int32_t _octaves, int32_t _randomseed); - // Get Height - double GetHeight(double x, double y) const; + // Get Height + double GetHeight(double x, double y) const; - // Get - double Persistence() const { return persistence; } - double Frequency() const { return frequency; } - double Amplitude() const { return amplitude; } - int Octaves() const { return octaves; } - int RandomSeed() const { return randomseed; } + // Get + double Persistence() const { return persistence; } + double Frequency() const { return frequency; } + double Amplitude() const { return amplitude; } + int32_t Octaves() const { return octaves; } + int32_t RandomSeed() const { return randomseed; } - // Set - void Set(double _persistence, double _frequency, double _amplitude, int _octaves, int _randomseed); + // Set + void Set(double _persistence, double _frequency, double _amplitude, int32_t _octaves, int32_t _randomseed); - void SetPersistence(double _persistence) { persistence = _persistence; } - void SetFrequency( double _frequency) { frequency = _frequency; } - void SetAmplitude( double _amplitude) { amplitude = _amplitude; } - void SetOctaves( int _octaves) { octaves = _octaves; } - void SetRandomSeed( int _randomseed) { randomseed = _randomseed; } + void SetPersistence(double _persistence) { persistence = _persistence; } + void SetFrequency( double _frequency) { frequency = _frequency; } + void SetAmplitude( double _amplitude) { amplitude = _amplitude; } + void SetOctaves( int32_t _octaves) { octaves = _octaves; } + void SetRandomSeed( int32_t _randomseed) { randomseed = _randomseed; } - private: + private: - double Total(double i, double j) const; - double GetValue(double x, double y) const; - double Interpolate(double x, double y, double a) const; - double Noise(int x, int y) const; + double Total(double i, double j) const; + double GetValue(double x, double y) const; + double Interpolate(double x, double y, double a) const; + double Noise(int32_t x, int32_t y) const; - double persistence, frequency, amplitude; - int octaves, randomseed; + double persistence, frequency, amplitude; + int32_t octaves, randomseed; }; #endif diff --git a/tools/testbed/common/PhysicsObject.cpp b/tools/testbed/common/PhysicsObject.cpp index aa03ce3..c565e69 100644 --- a/tools/testbed/common/PhysicsObject.cpp +++ b/tools/testbed/common/PhysicsObject.cpp @@ -1,26 +1,26 @@ /******************************************************************************** -* ReactPhysics3D physics library, http://www.reactphysics3d.com * -* Copyright (c) 2010-2016 Daniel Chappuis * +* 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: * -* * +* 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. * -* * +* 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 @@ -29,31 +29,31 @@ /// Constructor PhysicsObject::PhysicsObject() { - mColor = openglframework::Color(1, 1, 1, 1); - mSleepingColor = openglframework::Color(1, 0, 0, 1); + mColor = openglframework::Color(1, 1, 1, 1); + mSleepingColor = openglframework::Color(1, 0, 0, 1); } // Compute the new transform matrix -openglframework::Matrix4 PhysicsObject::computeTransform(float interpolationFactor, - const openglframework::Matrix4& scalingMatrix) { +openglframework::Matrix4 PhysicsObject::computeTransform(float int32_terpolationFactor, + const openglframework::Matrix4& scalingMatrix) { - // Get the transform of the rigid body - rp3d::Transform transform = mBody->getTransform(); + // Get the transform of the rigid body + rp3d::Transform transform = mBody->getTransform(); - // Interpolate the transform between the previous one and the new one - rp3d::Transform interpolatedTransform = rp3d::Transform::interpolateTransforms(mPreviousTransform, - transform, - interpolationFactor); - mPreviousTransform = transform; + // Interpolate the transform between the previous one and the new one + rp3d::Transform int32_terpolatedTransform = rp3d::Transform::int32_terpolateTransforms(mPreviousTransform, + transform, + int32_terpolationFactor); + mPreviousTransform = transform; - // Compute the transform used for rendering the box - rp3d::decimal matrix[16]; - interpolatedTransform.getOpenGLMatrix(matrix); - openglframework::Matrix4 newMatrix(matrix[0], matrix[4], matrix[8], matrix[12], - matrix[1], matrix[5], matrix[9], matrix[13], - matrix[2], matrix[6], matrix[10], matrix[14], - matrix[3], matrix[7], matrix[11], matrix[15]); + // Compute the transform used for rendering the box + rp3d::float matrix[16]; + int32_terpolatedTransform.getOpenGLMatrix(matrix); + openglframework::Matrix4 newMatrix(matrix[0], matrix[4], matrix[8], matrix[12], + matrix[1], matrix[5], matrix[9], matrix[13], + matrix[2], matrix[6], matrix[10], matrix[14], + matrix[3], matrix[7], matrix[11], matrix[15]); - // Apply the scaling matrix to have the correct box dimensions - return newMatrix * scalingMatrix; + // Apply the scaling matrix to have the correct box dimensions + return newMatrix * scalingMatrix; } diff --git a/tools/testbed/common/PhysicsObject.h b/tools/testbed/common/PhysicsObject.h index f2c7290..e01a923 100644 --- a/tools/testbed/common/PhysicsObject.h +++ b/tools/testbed/common/PhysicsObject.h @@ -1,26 +1,26 @@ /******************************************************************************** -* ReactPhysics3D physics library, http://www.reactphysics3d.com * -* Copyright (c) 2010-2016 Daniel Chappuis * +* 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: * -* * +* 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. * -* * +* 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 PHYSICSOBJECT_H @@ -33,66 +33,66 @@ // Class PhysicsObject class PhysicsObject { - protected: + protected: - /// Body used to simulate the dynamics of the box - rp3d::CollisionBody* mBody; + /// Body used to simulate the dynamics of the box + rp3d::CollisionBody* mBody; - /// Previous transform of the body (for interpolation) - rp3d::Transform mPreviousTransform; + /// Previous transform of the body (for int32_terpolation) + rp3d::Transform mPreviousTransform; - /// Main color of the box - openglframework::Color mColor; + /// Main color of the box + openglframework::Color mColor; - /// Sleeping color - openglframework::Color mSleepingColor; + /// Sleeping color + openglframework::Color mSleepingColor; - // Compute the new transform matrix - openglframework::Matrix4 computeTransform(float interpolationFactor, - const openglframework::Matrix4 &scalingMatrix); + // Compute the new transform matrix + openglframework::Matrix4 computeTransform(float int32_terpolationFactor, + const openglframework::Matrix4 &scalingMatrix); - public: + public: - /// Constructor - PhysicsObject(); + /// Constructor + PhysicsObject(); - /// Update the transform matrix of the object - virtual void updateTransform(float interpolationFactor)=0; + /// Update the transform matrix of the object + virtual void updateTransform(float int32_terpolationFactor)=0; - /// Set the color of the box - void setColor(const openglframework::Color& color); + /// Set the color of the box + void setColor(const openglframework::Color& color); - /// Set the sleeping color of the box - void setSleepingColor(const openglframework::Color& color); + /// Set the sleeping color of the box + void setSleepingColor(const openglframework::Color& color); - /// Return a pointer to the collision body of the box - reactphysics3d::CollisionBody* getCollisionBody(); + /// Return a pointer to the collision body of the box + reactphysics3d::CollisionBody* getCollisionBody(); - /// Return a pointer to the rigid body of the box - reactphysics3d::RigidBody* getRigidBody(); + /// Return a pointer to the rigid body of the box + reactphysics3d::RigidBody* getRigidBody(); - /// Set the scaling of the object - virtual void setScaling(const openglframework::Vector3& scaling)=0; + /// Set the scaling of the object + virtual void setScaling(const openglframework::Vector3& scaling)=0; }; // Set the color of the box inline void PhysicsObject::setColor(const openglframework::Color& color) { - mColor = color; + mColor = color; } // Set the sleeping color of the box inline void PhysicsObject::setSleepingColor(const openglframework::Color& color) { - mSleepingColor = color; + mSleepingColor = color; } // Return a pointer to the collision body of the box inline rp3d::CollisionBody* PhysicsObject::getCollisionBody() { - return mBody; + return mBody; } // Return a pointer to the rigid body of the box (NULL if it's not a rigid body) inline rp3d::RigidBody* PhysicsObject::getRigidBody() { - return dynamic_cast(mBody); + return dynamic_cast(mBody); } #endif diff --git a/tools/testbed/common/Sphere.cpp b/tools/testbed/common/Sphere.cpp index 9cd6178..8752494 100644 --- a/tools/testbed/common/Sphere.cpp +++ b/tools/testbed/common/Sphere.cpp @@ -1,26 +1,26 @@ /******************************************************************************** -* ReactPhysics3D physics library, http://www.reactphysics3d.com * -* Copyright (c) 2010-2016 Daniel Chappuis * +* 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: * -* * +* 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. * -* * +* 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 @@ -31,265 +31,265 @@ openglframework::VertexBufferObject Sphere::mVBONormals(GL_ARRAY_BUFFER); openglframework::VertexBufferObject Sphere::mVBOTextureCoords(GL_ARRAY_BUFFER); openglframework::VertexBufferObject Sphere::mVBOIndices(GL_ELEMENT_ARRAY_BUFFER); openglframework::VertexArrayObject Sphere::mVAO; -int Sphere::totalNbSpheres = 0; +int32_t Sphere::totalNbSpheres = 0; // Constructor Sphere::Sphere(float radius, const openglframework::Vector3 &position, - reactphysics3d::CollisionWorld* world, - const std::string& meshFolderPath) - : openglframework::Mesh(), mRadius(radius) { + reactphysics3d::CollisionWorld* world, + const std::string& meshFolderPath) + : openglframework::Mesh(), mRadius(radius) { - // Load the mesh from a file - openglframework::MeshReaderWriter::loadMeshFromFile(meshFolderPath + "sphere.obj", *this); + // Load the mesh from a file + openglframework::MeshReaderWriter::loadMeshFromFile(meshFolderPath + "sphere.obj", *this); - // Calculate the normals of the mesh - calculateNormals(); + // Calculate the normals of the mesh + calculateNormals(); - // Compute the scaling matrix - mScalingMatrix = openglframework::Matrix4(mRadius, 0, 0, 0, - 0, mRadius, 0, 0, - 0, 0, mRadius, 0, - 0, 0, 0, 1); + // Compute the scaling matrix + mScalingMatrix = openglframework::Matrix4(mRadius, 0, 0, 0, + 0, mRadius, 0, 0, + 0, 0, mRadius, 0, + 0, 0, 0, 1); - // Initialize the position where the sphere will be rendered - translateWorld(position); + // Initialize the position where the sphere will be rendered + translateWorld(position); - // Create the collision shape for the rigid body (sphere shape) - // ReactPhysics3D will clone this object to create an internal one. Therefore, - // it is OK if this object is destroyed right after calling RigidBody::addCollisionShape() - mCollisionShape = new rp3d::SphereShape(mRadius); + // Create the collision shape for the rigid body (sphere shape) + // ReactPhysics3D will clone this object to create an int32_ternal one. Therefore, + // it is OK if this object is destroyed right after calling RigidBody::addCollisionShape() + mCollisionShape = new rp3d::SphereShape(mRadius); - // Initial position and orientation of the rigid body - rp3d::Vector3 initPosition(position.x, position.y, position.z); - rp3d::Quaternion initOrientation = rp3d::Quaternion::identity(); - rp3d::Transform transform(initPosition, initOrientation); + // Initial position and orientation of the rigid body + rp3d::Vector3 initPosition(position.x, position.y, position.z); + rp3d::Quaternion initOrientation = rp3d::Quaternion::identity(); + rp3d::Transform transform(initPosition, initOrientation); - mPreviousTransform = transform; + mPreviousTransform = transform; - // Create a rigid body corresponding to the sphere in the dynamics world - mBody = world->createCollisionBody(transform); + // Create a rigid body corresponding to the sphere in the dynamics world + mBody = world->createCollisionBody(transform); - // Add a collision shape to the body and specify the mass of the shape - m_proxyShape = mBody->addCollisionShape(mCollisionShape, rp3d::Transform::identity()); + // Add a collision shape to the body and specify the mass of the shape + m_proxyShape = mBody->addCollisionShape(mCollisionShape, rp3d::Transform::identity()); - mTransformMatrix = mTransformMatrix * mScalingMatrix; + mTransformMatrix = mTransformMatrix * mScalingMatrix; - // Create the VBOs and VAO - if (totalNbSpheres == 0) { - createVBOAndVAO(); - } + // Create the VBOs and VAO + if (totalNbSpheres == 0) { + createVBOAndVAO(); + } - totalNbSpheres++; + totalNbSpheres++; } // Constructor Sphere::Sphere(float radius, const openglframework::Vector3 &position, - float mass, reactphysics3d::DynamicsWorld* world, - const std::string& meshFolderPath) - : openglframework::Mesh(), mRadius(radius) { + float mass, reactphysics3d::DynamicsWorld* world, + const std::string& meshFolderPath) + : openglframework::Mesh(), mRadius(radius) { - // Load the mesh from a file - openglframework::MeshReaderWriter::loadMeshFromFile(meshFolderPath + "sphere.obj", *this); + // Load the mesh from a file + openglframework::MeshReaderWriter::loadMeshFromFile(meshFolderPath + "sphere.obj", *this); - // Calculate the normals of the mesh - calculateNormals(); + // Calculate the normals of the mesh + calculateNormals(); - // Compute the scaling matrix - mScalingMatrix = openglframework::Matrix4(mRadius, 0, 0, 0, - 0, mRadius, 0, 0, - 0, 0, mRadius, 0, - 0, 0, 0, 1); + // Compute the scaling matrix + mScalingMatrix = openglframework::Matrix4(mRadius, 0, 0, 0, + 0, mRadius, 0, 0, + 0, 0, mRadius, 0, + 0, 0, 0, 1); - // Initialize the position where the sphere will be rendered - translateWorld(position); + // Initialize the position where the sphere will be rendered + translateWorld(position); - // Create the collision shape for the rigid body (sphere shape) - // ReactPhysics3D will clone this object to create an internal one. Therefore, - // it is OK if this object is destroyed right after calling RigidBody::addCollisionShape() - mCollisionShape = new rp3d::SphereShape(mRadius); + // Create the collision shape for the rigid body (sphere shape) + // ReactPhysics3D will clone this object to create an int32_ternal one. Therefore, + // it is OK if this object is destroyed right after calling RigidBody::addCollisionShape() + mCollisionShape = new rp3d::SphereShape(mRadius); - // Initial position and orientation of the rigid body - rp3d::Vector3 initPosition(position.x, position.y, position.z); - rp3d::Quaternion initOrientation = rp3d::Quaternion::identity(); - rp3d::Transform transform(initPosition, initOrientation); + // Initial position and orientation of the rigid body + rp3d::Vector3 initPosition(position.x, position.y, position.z); + rp3d::Quaternion initOrientation = rp3d::Quaternion::identity(); + rp3d::Transform transform(initPosition, initOrientation); - // Create a rigid body corresponding to the sphere in the dynamics world - rp3d::RigidBody* body = world->createRigidBody(transform); + // Create a rigid body corresponding to the sphere in the dynamics world + rp3d::RigidBody* body = world->createRigidBody(transform); - // Add a collision shape to the body and specify the mass of the shape - m_proxyShape = body->addCollisionShape(mCollisionShape, rp3d::Transform::identity(), mass); + // Add a collision shape to the body and specify the mass of the shape + m_proxyShape = body->addCollisionShape(mCollisionShape, rp3d::Transform::identity(), mass); - mBody = body; + mBody = body; - mTransformMatrix = mTransformMatrix * mScalingMatrix; + mTransformMatrix = mTransformMatrix * mScalingMatrix; - // Create the VBOs and VAO - if (totalNbSpheres == 0) { - createVBOAndVAO(); - } + // Create the VBOs and VAO + if (totalNbSpheres == 0) { + createVBOAndVAO(); + } - totalNbSpheres++; + totalNbSpheres++; } // Destructor Sphere::~Sphere() { - if (totalNbSpheres == 1) { - // Destroy the mesh - destroy(); + if (totalNbSpheres == 1) { + // Destroy the mesh + destroy(); - // Destroy the VBOs and VAO - mVBOIndices.destroy(); - mVBOVertices.destroy(); - mVBONormals.destroy(); - mVBOTextureCoords.destroy(); - mVAO.destroy(); - } - delete mCollisionShape; - totalNbSpheres--; + // Destroy the VBOs and VAO + mVBOIndices.destroy(); + mVBOVertices.destroy(); + mVBONormals.destroy(); + mVBOTextureCoords.destroy(); + mVAO.destroy(); + } + delete mCollisionShape; + totalNbSpheres--; } // Render the sphere at the correct position and with the correct orientation void Sphere::render(openglframework::Shader& shader, - const openglframework::Matrix4& worldToCameraMatrix) { + const openglframework::Matrix4& worldToCameraMatrix) { - // Bind the shader - shader.bind(); + // Bind the shader + shader.bind(); - // Set the model to camera matrix - shader.setMatrix4x4Uniform("localToWorldMatrix", mTransformMatrix); - shader.setMatrix4x4Uniform("worldToCameraMatrix", worldToCameraMatrix); + // Set the model to camera matrix + shader.setMatrix4x4Uniform("localToWorldMatrix", mTransformMatrix); + shader.setMatrix4x4Uniform("worldToCameraMatrix", worldToCameraMatrix); - // Set the normal matrix (inverse transpose of the 3x3 upper-left sub matrix of the - // model-view matrix) - const openglframework::Matrix4 localToCameraMatrix = worldToCameraMatrix * mTransformMatrix; - const openglframework::Matrix3 normalMatrix = - localToCameraMatrix.getUpperLeft3x3Matrix().getInverse().getTranspose(); - shader.setMatrix3x3Uniform("normalMatrix", normalMatrix, false); + // Set the normal matrix (inverse transpose of the 3x3 upper-left sub matrix of the + // model-view matrix) + const openglframework::Matrix4 localToCameraMatrix = worldToCameraMatrix * mTransformMatrix; + const openglframework::Matrix3 normalMatrix = + localToCameraMatrix.getUpperLeft3x3Matrix().getInverse().getTranspose(); + shader.setMatrix3x3Uniform("normalMatrix", normalMatrix, false); - // Set the vertex color - openglframework::Color currentColor = mBody->isSleeping() ? mSleepingColor : mColor; - openglframework::Vector4 color(currentColor.r, currentColor.g, currentColor.b, currentColor.a); - shader.setVector4Uniform("vertexColor", color, false); + // Set the vertex color + openglframework::Color currentColor = mBody->isSleeping() ? mSleepingColor : mColor; + openglframework::Vector4 color(currentColor.r, currentColor.g, currentColor.b, currentColor.a); + shader.setVector4Uniform("vertexColor", color, false); - // Bind the VAO - mVAO.bind(); + // Bind the VAO + mVAO.bind(); - mVBOVertices.bind(); + mVBOVertices.bind(); - // Get the location of shader attribute variables - GLint vertexPositionLoc = shader.getAttribLocation("vertexPosition"); - GLint vertexNormalLoc = shader.getAttribLocation("vertexNormal", false); + // Get the location of shader attribute variables + GLint32_t vertexPositionLoc = shader.getAttribLocation("vertexPosition"); + GLint32_t vertexNormalLoc = shader.getAttribLocation("vertexNormal", false); - glEnableVertexAttribArray(vertexPositionLoc); - glVertexAttribPointer(vertexPositionLoc, 3, GL_FLOAT, GL_FALSE, 0, (char*)NULL); + glEnableVertexAttribArray(vertexPositionLoc); + glVertexAttribPointer(vertexPositionLoc, 3, GL_FLOAT, GL_FALSE, 0, (char*)NULL); - mVBONormals.bind(); + mVBONormals.bind(); - if (vertexNormalLoc != -1) glVertexAttribPointer(vertexNormalLoc, 3, GL_FLOAT, GL_FALSE, 0, (char*)NULL); - if (vertexNormalLoc != -1) glEnableVertexAttribArray(vertexNormalLoc); + if (vertexNormalLoc != -1) glVertexAttribPointer(vertexNormalLoc, 3, GL_FLOAT, GL_FALSE, 0, (char*)NULL); + if (vertexNormalLoc != -1) glEnableVertexAttribArray(vertexNormalLoc); - // For each part of the mesh - for (unsigned int i=0; isetTransform(transform); + // Reset the transform + mBody->setTransform(transform); - mBody->setIsSleeping(false); + mBody->setIsSleeping(false); - // Reset the velocity of the rigid body - rp3d::RigidBody* rigidBody = dynamic_cast(mBody); - if (rigidBody != NULL) { - rigidBody->setLinearVelocity(rp3d::Vector3(0, 0, 0)); - rigidBody->setAngularVelocity(rp3d::Vector3(0, 0, 0)); - } + // Reset the velocity of the rigid body + rp3d::RigidBody* rigidBody = dynamic_cast(mBody); + if (rigidBody != NULL) { + rigidBody->setLinearVelocity(rp3d::Vector3(0, 0, 0)); + rigidBody->setAngularVelocity(rp3d::Vector3(0, 0, 0)); + } - updateTransform(1.0f); + updateTransform(1.0f); } // Set the scaling of the object void Sphere::setScaling(const openglframework::Vector3& scaling) { - // Scale the collision shape - m_proxyShape->setLocalScaling(rp3d::Vector3(scaling.x, scaling.y, scaling.z)); + // Scale the collision shape + m_proxyShape->setLocalScaling(rp3d::Vector3(scaling.x, scaling.y, scaling.z)); - // Scale the graphics object - mScalingMatrix = openglframework::Matrix4(mRadius * scaling.x, 0, 0, 0, - 0, mRadius * scaling.y, 0, 0, - 0, 0, mRadius * scaling.z, 0, - 0, 0, 0, 1); + // Scale the graphics object + mScalingMatrix = openglframework::Matrix4(mRadius * scaling.x, 0, 0, 0, + 0, mRadius * scaling.y, 0, 0, + 0, 0, mRadius * scaling.z, 0, + 0, 0, 0, 1); } diff --git a/tools/testbed/common/Sphere.h b/tools/testbed/common/Sphere.h index d0dd951..50422ed 100644 --- a/tools/testbed/common/Sphere.h +++ b/tools/testbed/common/Sphere.h @@ -1,26 +1,26 @@ /******************************************************************************** -* ReactPhysics3D physics library, http://www.reactphysics3d.com * -* Copyright (c) 2010-2016 Daniel Chappuis * +* 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: * -* * +* 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. * -* * +* 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 SPHERE_H @@ -34,78 +34,78 @@ // Class Sphere class Sphere : public openglframework::Mesh, public PhysicsObject { - private : + private : - // -------------------- Attributes -------------------- // + // -------------------- Attributes -------------------- // - /// Radius of the sphere - float mRadius; + /// Radius of the sphere + float mRadius; - /// Collision shape - rp3d::SphereShape* mCollisionShape; - rp3d::ProxyShape* m_proxyShape; + /// Collision shape + rp3d::SphereShape* mCollisionShape; + rp3d::ProxyShape* m_proxyShape; - /// Scaling matrix (applied to a sphere to obtain the correct sphere dimensions) - openglframework::Matrix4 mScalingMatrix; + /// Scaling matrix (applied to a sphere to obtain the correct sphere dimensions) + openglframework::Matrix4 mScalingMatrix; - /// Previous transform (for interpolation) - rp3d::Transform mPreviousTransform; + /// Previous transform (for int32_terpolation) + rp3d::Transform mPreviousTransform; - /// Vertex Buffer Object for the vertices data - static openglframework::VertexBufferObject mVBOVertices; + /// Vertex Buffer Object for the vertices data + static openglframework::VertexBufferObject mVBOVertices; - /// Vertex Buffer Object for the normals data - static openglframework::VertexBufferObject mVBONormals; + /// Vertex Buffer Object for the normals data + static openglframework::VertexBufferObject mVBONormals; - /// Vertex Buffer Object for the texture coords - static openglframework::VertexBufferObject mVBOTextureCoords; + /// Vertex Buffer Object for the texture coords + static openglframework::VertexBufferObject mVBOTextureCoords; - /// Vertex Buffer Object for the indices - static openglframework::VertexBufferObject mVBOIndices; + /// Vertex Buffer Object for the indices + static openglframework::VertexBufferObject mVBOIndices; - /// Vertex Array Object for the vertex data - static openglframework::VertexArrayObject mVAO; + /// Vertex Array Object for the vertex data + static openglframework::VertexArrayObject mVAO; - // Total number of capsules created - static int totalNbSpheres; + // Total number of capsules created + static int32_t totalNbSpheres; - // -------------------- Methods -------------------- // + // -------------------- Methods -------------------- // - // Create the Vertex Buffer Objects used to render with OpenGL. - void createVBOAndVAO(); + // Create the Vertex Buffer Objects used to render with OpenGL. + void createVBOAndVAO(); - public : + public : - // -------------------- Methods -------------------- // + // -------------------- Methods -------------------- // - /// Constructor - Sphere(float radius, const openglframework::Vector3& position, - rp3d::CollisionWorld* world, const std::string& meshFolderPath); + /// Constructor + Sphere(float radius, const openglframework::Vector3& position, + rp3d::CollisionWorld* world, const std::string& meshFolderPath); - /// Constructor - Sphere(float radius, const openglframework::Vector3& position, - float mass, rp3d::DynamicsWorld* dynamicsWorld, const std::string& meshFolderPath); + /// Constructor + Sphere(float radius, const openglframework::Vector3& position, + float mass, rp3d::DynamicsWorld* dynamicsWorld, const std::string& meshFolderPath); - /// Destructor - ~Sphere(); + /// Destructor + ~Sphere(); - /// Render the sphere at the correct position and with the correct orientation - void render(openglframework::Shader& shader, - const openglframework::Matrix4& worldToCameraMatrix); + /// Render the sphere at the correct position and with the correct orientation + void render(openglframework::Shader& shader, + const openglframework::Matrix4& worldToCameraMatrix); - /// Set the position of the box - void resetTransform(const rp3d::Transform& transform); + /// Set the position of the box + void resetTransform(const rp3d::Transform& transform); - /// Update the transform matrix of the object - virtual void updateTransform(float interpolationFactor); + /// Update the transform matrix of the object + virtual void updateTransform(float int32_terpolationFactor); - /// Set the scaling of the object - void setScaling(const openglframework::Vector3& scaling); + /// Set the scaling of the object + void setScaling(const openglframework::Vector3& scaling); }; // Update the transform matrix of the object -inline void Sphere::updateTransform(float interpolationFactor) { - mTransformMatrix = computeTransform(interpolationFactor, mScalingMatrix); +inline void Sphere::updateTransform(float int32_terpolationFactor) { + mTransformMatrix = computeTransform(int32_terpolationFactor, mScalingMatrix); } #endif diff --git a/tools/testbed/common/VisualContactPoint.cpp b/tools/testbed/common/VisualContactPoint.cpp index 7418f50..f5ea466 100644 --- a/tools/testbed/common/VisualContactPoint.cpp +++ b/tools/testbed/common/VisualContactPoint.cpp @@ -1,26 +1,26 @@ /******************************************************************************** -* ReactPhysics3D physics library, http://www.reactphysics3d.com * -* Copyright (c) 2010-2016 Daniel Chappuis * +* 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: * -* * +* 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. * -* * +* 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 @@ -31,17 +31,17 @@ openglframework::VertexBufferObject VisualContactPoint::mVBOVertices(GL_ARRAY_BU openglframework::VertexBufferObject VisualContactPoint::mVBONormals(GL_ARRAY_BUFFER); openglframework::VertexBufferObject VisualContactPoint::mVBOIndices(GL_ELEMENT_ARRAY_BUFFER); openglframework::VertexArrayObject VisualContactPoint::mVAO; -int VisualContactPoint::mNbTotalPoints = 0; +int32_t VisualContactPoint::mNbTotalPoints = 0; openglframework::Mesh VisualContactPoint::mMesh; bool VisualContactPoint::mStaticDataCreated = false; // Constructor VisualContactPoint::VisualContactPoint(const openglframework::Vector3& position, - const std::string& meshFolderPath) - : mColor(1.0f, 0.0f, 0.0f, 1.0f) { + const std::string& meshFolderPath) + : mColor(1.0f, 0.0f, 0.0f, 1.0f) { - // Initialize the position where the mesh will be rendered - translateWorld(position); + // Initialize the position where the mesh will be rendered + translateWorld(position); } // Destructor @@ -52,132 +52,132 @@ VisualContactPoint::~VisualContactPoint() { // Load and initialize the mesh for all the contact points void VisualContactPoint::createStaticData(const std::string& meshFolderPath) { - if (mStaticDataCreated) return; + if (mStaticDataCreated) return; - // Load the mesh from a file - openglframework::MeshReaderWriter::loadMeshFromFile(meshFolderPath + "sphere.obj", mMesh); + // Load the mesh from a file + openglframework::MeshReaderWriter::loadMeshFromFile(meshFolderPath + "sphere.obj", mMesh); - // Calculate the normals of the mesh - mMesh.calculateNormals(); + // Calculate the normals of the mesh + mMesh.calculateNormals(); - mMesh.scaleVertices(VISUAL_CONTACT_POINT_RADIUS); + mMesh.scaleVertices(VISUAL_CONTACT_POINT_RADIUS); - createVBOAndVAO(); + createVBOAndVAO(); - mStaticDataCreated = true; + mStaticDataCreated = true; } // Destroy the mesh for the contact points void VisualContactPoint::destroyStaticData() { - if (!mStaticDataCreated) return; + if (!mStaticDataCreated) return; - // Destroy the VBOs and VAO - mVBOIndices.destroy(); - mVBOVertices.destroy(); - mVBONormals.destroy(); - mVAO.destroy(); + // Destroy the VBOs and VAO + mVBOIndices.destroy(); + mVBOVertices.destroy(); + mVBONormals.destroy(); + mVAO.destroy(); - mMesh.destroy(); + mMesh.destroy(); - mStaticDataCreated = false; + mStaticDataCreated = false; } // Render the sphere at the correct position and with the correct orientation void VisualContactPoint::render(openglframework::Shader& shader, - const openglframework::Matrix4& worldToCameraMatrix) { + const openglframework::Matrix4& worldToCameraMatrix) { - // Bind the VAO - mVAO.bind(); + // Bind the VAO + mVAO.bind(); - // Bind the shader - shader.bind(); + // Bind the shader + shader.bind(); - mVBOVertices.bind(); + mVBOVertices.bind(); - // Set the model to camera matrix - shader.setMatrix4x4Uniform("localToWorldMatrix", mTransformMatrix); - shader.setMatrix4x4Uniform("worldToCameraMatrix", worldToCameraMatrix); + // Set the model to camera matrix + shader.setMatrix4x4Uniform("localToWorldMatrix", mTransformMatrix); + shader.setMatrix4x4Uniform("worldToCameraMatrix", worldToCameraMatrix); - // Set the normal matrix (inverse transpose of the 3x3 upper-left sub matrix of the - // model-view matrix) - const openglframework::Matrix4 localToCameraMatrix = worldToCameraMatrix * mTransformMatrix; - const openglframework::Matrix3 normalMatrix = - localToCameraMatrix.getUpperLeft3x3Matrix().getInverse().getTranspose(); - shader.setMatrix3x3Uniform("normalMatrix", normalMatrix, false); + // Set the normal matrix (inverse transpose of the 3x3 upper-left sub matrix of the + // model-view matrix) + const openglframework::Matrix4 localToCameraMatrix = worldToCameraMatrix * mTransformMatrix; + const openglframework::Matrix3 normalMatrix = + localToCameraMatrix.getUpperLeft3x3Matrix().getInverse().getTranspose(); + shader.setMatrix3x3Uniform("normalMatrix", normalMatrix, false); - // Set the vertex color - openglframework::Vector4 color(mColor.r, mColor.g, mColor.b, mColor.a); - shader.setVector4Uniform("vertexColor", color, false); + // Set the vertex color + openglframework::Vector4 color(mColor.r, mColor.g, mColor.b, mColor.a); + shader.setVector4Uniform("vertexColor", color, false); - // Get the location of shader attribute variables - GLint vertexPositionLoc = shader.getAttribLocation("vertexPosition"); - GLint vertexNormalLoc = shader.getAttribLocation("vertexNormal", false); + // Get the location of shader attribute variables + GLint32_t vertexPositionLoc = shader.getAttribLocation("vertexPosition"); + GLint32_t vertexNormalLoc = shader.getAttribLocation("vertexNormal", false); - glEnableVertexAttribArray(vertexPositionLoc); - glVertexAttribPointer(vertexPositionLoc, 3, GL_FLOAT, GL_FALSE, 0, (char*)NULL); + glEnableVertexAttribArray(vertexPositionLoc); + glVertexAttribPointer(vertexPositionLoc, 3, GL_FLOAT, GL_FALSE, 0, (char*)NULL); - mVBONormals.bind(); + mVBONormals.bind(); - if (vertexNormalLoc != -1) glVertexAttribPointer(vertexNormalLoc, 3, GL_FLOAT, GL_FALSE, 0, (char*)NULL); - if (vertexNormalLoc != -1) glEnableVertexAttribArray(vertexNormalLoc); + if (vertexNormalLoc != -1) glVertexAttribPointer(vertexNormalLoc, 3, GL_FLOAT, GL_FALSE, 0, (char*)NULL); + if (vertexNormalLoc != -1) glEnableVertexAttribArray(vertexNormalLoc); - // For each part of the mesh - for (unsigned int i=0; i(getNbVertices(), Vector3(0, 0, 0)); + mNormals = vector(getNbVertices(), Vector3(0, 0, 0)); - // For each triangular face - for (uint i=0; i(getNbVertices(), Vector3(0, 0, 0)); + mTangents = std::vector(getNbVertices(), Vector3(0, 0, 0)); - // For each face - for (uint i=0; i::const_iterator it(m_vertices.begin()); + std::vector::const_iterator it(m_vertices.begin()); - // For each vertex of the mesh - for (; it != m_vertices.end(); ++it) { + // For each vertex of the mesh + for (; it != m_vertices.end(); ++it) { - if( (*it).x < min.x ) min.x = (*it).x; - else if ( (*it).x > max.x ) max.x = (*it).x; + if( (*it).x < min.x ) min.x = (*it).x; + else if ( (*it).x > max.x ) max.x = (*it).x; - if( (*it).y < min.y ) min.y = (*it).y; - else if ( (*it).y > max.y ) max.y = (*it).y; + if( (*it).y < min.y ) min.y = (*it).y; + else if ( (*it).y > max.y ) max.y = (*it).y; - if( (*it).z < min.z ) min.z = (*it).z; - else if ( (*it).z > max.z ) max.z = (*it).z; - } - } - else { - std::cerr << "Error : Impossible to calculate the bounding box of the mesh because there" << - "are no vertices !" << std::endl; - assert(false); - } + if( (*it).z < min.z ) min.z = (*it).z; + else if ( (*it).z > max.z ) max.z = (*it).z; + } + } + else { + std::cerr << "Error : Impossible to calculate the bounding box of the mesh because there" << + "are no vertices !" << std::endl; + assert(false); + } } // Scale of vertices of the mesh using a given factor void Mesh::scaleVertices(float factor) { - // For each vertex - for (uint i=0; i > mIndices; + // A triplet of vertex indices for each triangle + std::vector > mIndices; - // Vertices coordinates (local space) - std::vector m_vertices; + // Vertices coordinates (local space) + std::vector m_vertices; - // Normals coordinates - std::vector mNormals; + // Normals coordinates + std::vector mNormals; - // Tangents coordinates - std::vector mTangents; + // Tangents coordinates + std::vector mTangents; - // Color for each vertex - std::vector mColors; + // Color for each vertex + std::vector mColors; - // UV texture coordinates - std::vector mUVs; + // UV texture coordinates + std::vector mUVs; - // Textures of the mesh (one for each part of the mesh) - std::map mTextures; + // Textures of the mesh (one for each part of the mesh) + std::map mTextures; - public: + public: - // -------------------- Methods -------------------- // + // -------------------- Methods -------------------- // - // Constructor - Mesh(); + // Constructor + Mesh(); - // Destructor - virtual ~Mesh(); + // Destructor + virtual ~Mesh(); - // Destroy the mesh - void destroy(); + // Destroy the mesh + void destroy(); - // Compute the normals of the mesh - void calculateNormals(); + // Compute the normals of the mesh + void calculateNormals(); - // Compute the tangents of the mesh - void calculateTangents(); + // Compute the tangents of the mesh + void calculateTangents(); - // Calculate the bounding box of the mesh - void calculateBoundingBox(Vector3& min, Vector3& max) const; + // Calculate the bounding box of the mesh + void calculateBoundingBox(Vector3& min, Vector3& max) const; - // Scale of vertices of the mesh using a given factor - void scaleVertices(float factor); + // Scale of vertices of the mesh using a given factor + void scaleVertices(float factor); - // Return the number of triangles - uint getNbFaces(uint part = 0) const; + // Return the number of triangles + uint32_t getNbFaces(uint32_t part = 0) const; - // Return the number of vertices - uint getNbVertices() const; + // Return the number of vertices + uint32_t getNbVertices() const; - // Return the number of parts in the mesh - uint getNbParts() const; + // Return the number of parts in the mesh + uint32_t getNbParts() const; - // Return a reference to the vertices - const std::vector& getVertices() const; + // Return a reference to the vertices + const std::vector& getVertices() const; - // Set the vertices of the mesh - void setVertices(std::vector& vertices); + // Set the vertices of the mesh + void setVertices(std::vector& vertices); - // Return a reference to the normals - const std::vector& getNormals() const; + // Return a reference to the normals + const std::vector& getNormals() const; - // set the normals of the mesh - void setNormals(std::vector& normals); + // set the normals of the mesh + void setNormals(std::vector& normals); - // Return a reference to the UVs - const std::vector& getUVs() const; + // Return a reference to the UVs + const std::vector& getUVs() const; - // Set the UV texture coordinates of the mesh - void setUVs(std::vector& uvs); + // Set the UV texture coordinates of the mesh + void setUVs(std::vector& uvs); - // Return a reference to the vertex indices - const std::vector& getIndices(uint part = 0) const; + // Return a reference to the vertex indices + const std::vector& getIndices(uint32_t part = 0) const; - // Set the vertices indices of the mesh - void setIndices(std::vector >& indices); + // Set the vertices indices of the mesh + void setIndices(std::vector >& indices); - // Return the coordinates of a given vertex - const Vector3& getVertex(uint i) const; + // Return the coordinates of a given vertex + const Vector3& getVertex(uint32_t i) const; - // Set the coordinates of a given vertex - void setVertex(uint i, const Vector3& vertex); + // Set the coordinates of a given vertex + void setVertex(uint32_t i, const Vector3& vertex); - // Return the coordinates of a given normal - const Vector3& getNormal(uint i) const; + // Return the coordinates of a given normal + const Vector3& getNormal(uint32_t i) const; - // Set the coordinates of a given normal - void setNormal(uint i, const Vector3& normal); + // Set the coordinates of a given normal + void setNormal(uint32_t i, const Vector3& normal); - // Return the color of a given vertex - const Color& getVertexColor(uint i) const; + // Return the color of a given vertex + const Color& getVertexColor(uint32_t i) const; - // Set the color of a given vertex - void setVertexColor(uint i, const Color& color); + // Set the color of a given vertex + void setVertexColor(uint32_t i, const Color& color); - // Set a color to all the vertices - void setColorToAllVertices(const Color& color); + // Set a color to all the vertices + void setColorToAllVertices(const Color& color); - // Return the UV of a given vertex - const Vector2& getUV(uint i) const; + // Return the UV of a given vertex + const Vector2& getUV(uint32_t i) const; - // Set the UV of a given vertex - void setUV(uint i, const Vector2& uv); + // Set the UV of a given vertex + void setUV(uint32_t i, const Vector2& uv); - // Return the vertex index of the ith (i=0,1,2) vertex of a given face - uint getVertexIndexInFace(uint faceIndex, uint i, uint part = 0) const; + // Return the vertex index of the ith (i=0,1,2) vertex of a given face + uint32_t getVertexIndexInFace(uint32_t faceIndex, uint32_t i, uint32_t part = 0) const; - // Return true if the mesh has normals - bool hasNormals() const; + // Return true if the mesh has normals + bool hasNormals() const; - // Return true if the mesh has tangents - bool hasTangents() const; + // Return true if the mesh has tangents + bool hasTangents() const; - // Return true if the mesh has vertex colors - bool hasColors() const; + // Return true if the mesh has vertex colors + bool hasColors() const; - // Return true if the mesh has UV texture coordinates - bool hasUVTextureCoordinates() const; + // Return true if the mesh has UV texture coordinates + bool hasUVTextureCoordinates() const; - // Return true if the mesh has a texture for a given part of the mesh and if it - // also have texture coordinates - bool hasTextureForPart(uint part = 0) const; + // Return true if the mesh has a texture for a given part of the mesh and if it + // also have texture coordinates + bool hasTextureForPart(uint32_t part = 0) const; - // Return true if the mesh has a texture (and texture coordinates) for at least one - // part of the mesh - bool hasTexture() const; + // Return true if the mesh has a texture (and texture coordinates) for at least one + // part of the mesh + bool hasTexture() const; - // Return a pointer to the vertices data - void* getVerticesPointer(); + // Return a pointer to the vertices data + void* getVerticesPointer(); - // Return a pointer to the normals data - void* getNormalsPointer(); + // Return a pointer to the normals data + void* getNormalsPointer(); - // Return a pointer to the colors data - void* getColorsPointer(); + // Return a pointer to the colors data + void* getColorsPointer(); - // Return a pointer to the tangents data - void* getTangentsPointer(); + // Return a pointer to the tangents data + void* getTangentsPointer(); - // Return a pointer to the UV texture coordinates data - void* getUVTextureCoordinatesPointer(); + // Return a pointer to the UV texture coordinates data + void* getUVTextureCoordinatesPointer(); - // Return a pointer to the vertex indicies data - void* getIndicesPointer(uint part = 0); + // Return a pointer to the vertex indicies data + void* getIndicesPointer(uint32_t part = 0); - // Return a reference to a texture of the mesh - Texture2D &getTexture(uint part = 0); + // Return a reference to a texture of the mesh + Texture2D &getTexture(uint32_t part = 0); - // Set a texture to a part of the mesh - void setTexture(Texture2D &texture, uint part = 0); + // Set a texture to a part of the mesh + void setTexture(Texture2D &texture, uint32_t part = 0); }; // Return the number of triangles -inline uint Mesh::getNbFaces(uint part) const { - return mIndices[part].size() / 3; +inline uint32_t Mesh::getNbFaces(uint32_t part) const { + return mIndices[part].size() / 3; } // Return the number of vertices -inline uint Mesh::getNbVertices() const { - return m_vertices.size(); +inline uint32_t Mesh::getNbVertices() const { + return m_vertices.size(); } // Return the number of parts in the mesh -inline uint Mesh::getNbParts() const { - return mIndices.size(); +inline uint32_t Mesh::getNbParts() const { + return mIndices.size(); } // Return a reference to the vertices inline const std::vector& Mesh::getVertices() const { - return m_vertices; + return m_vertices; } // Set the vertices of the mesh inline void Mesh::setVertices(std::vector& vertices) { - m_vertices = vertices; + m_vertices = vertices; } // Return a reference to the normals inline const std::vector& Mesh::getNormals() const { - return mNormals; + return mNormals; } // set the normals of the mesh inline void Mesh::setNormals(std::vector& normals) { - mNormals = normals; + mNormals = normals; } // Return a reference to the UVs inline const std::vector& Mesh::getUVs() const { - return mUVs; + return mUVs; } // Set the UV texture coordinates of the mesh inline void Mesh::setUVs(std::vector& uvs) { - mUVs = uvs; + mUVs = uvs; } // Return a reference to the vertex indices -inline const std::vector& Mesh::getIndices(uint part) const { - return mIndices[part]; +inline const std::vector& Mesh::getIndices(uint32_t part) const { + return mIndices[part]; } // Set the vertices indices of the mesh -inline void Mesh::setIndices(std::vector >& indices) { - mIndices = indices; +inline void Mesh::setIndices(std::vector >& indices) { + mIndices = indices; } // Return the coordinates of a given vertex -inline const Vector3& Mesh::getVertex(uint i) const { - assert(i < getNbVertices()); - return m_vertices[i]; +inline const Vector3& Mesh::getVertex(uint32_t i) const { + assert(i < getNbVertices()); + return m_vertices[i]; } // Set the coordinates of a given vertex -inline void Mesh::setVertex(uint i, const Vector3& vertex) { - assert(i < getNbVertices()); - m_vertices[i] = vertex; +inline void Mesh::setVertex(uint32_t i, const Vector3& vertex) { + assert(i < getNbVertices()); + m_vertices[i] = vertex; } // Return the coordinates of a given normal -inline const Vector3& Mesh::getNormal(uint i) const { - assert(i < getNbVertices()); - return mNormals[i]; +inline const Vector3& Mesh::getNormal(uint32_t i) const { + assert(i < getNbVertices()); + return mNormals[i]; } // Set the coordinates of a given normal -inline void Mesh::setNormal(uint i, const Vector3& normal) { - assert(i < getNbVertices()); - mNormals[i] = normal; +inline void Mesh::setNormal(uint32_t i, const Vector3& normal) { + assert(i < getNbVertices()); + mNormals[i] = normal; } // Return the color of a given vertex -inline const Color& Mesh::getVertexColor(uint i) const { - assert(i < getNbVertices()); - return mColors[i]; +inline const Color& Mesh::getVertexColor(uint32_t i) const { + assert(i < getNbVertices()); + return mColors[i]; } // Set the color of a given vertex -inline void Mesh::setVertexColor(uint i, const Color& color) { +inline void Mesh::setVertexColor(uint32_t i, const Color& color) { - // If the color array does not have the same size as - // the vertices array - if (mColors.size() != m_vertices.size()) { + // If the color array does not have the same size as + // the vertices array + if (mColors.size() != m_vertices.size()) { - // Create the color array with the same size - mColors = std::vector(m_vertices.size()); - } + // Create the color array with the same size + mColors = std::vector(m_vertices.size()); + } - mColors[i] = color; + mColors[i] = color; } // Set a color to all the vertices inline void Mesh::setColorToAllVertices(const Color& color) { - // If the color array does not have the same size as - // the vertices array - if (mColors.size() != m_vertices.size()) { + // If the color array does not have the same size as + // the vertices array + if (mColors.size() != m_vertices.size()) { - // Create the color array with the same size - mColors = std::vector(m_vertices.size()); - } + // Create the color array with the same size + mColors = std::vector(m_vertices.size()); + } - for (size_t v=0; v 0); + return hasUVTextureCoordinates() && (mTextures.size() > 0); } // Return a pointer to the vertices data inline void* Mesh::getVerticesPointer() { - return &(m_vertices[0]); + return &(m_vertices[0]); } // Return a pointer to the normals data inline void* Mesh::getNormalsPointer() { - return &(mNormals[0]); + return &(mNormals[0]); } // Return a pointer to the colors data inline void* Mesh::getColorsPointer() { - return &(mColors[0]); + return &(mColors[0]); } // Return a pointer to the tangents data inline void* Mesh::getTangentsPointer() { - return &(mTangents[0]); + return &(mTangents[0]); } // Return a pointer to the UV texture coordinates data inline void* Mesh::getUVTextureCoordinatesPointer() { - return &(mUVs[0]); + return &(mUVs[0]); } // Return a pointer to the vertex indicies data -inline void* Mesh::getIndicesPointer(uint part) { - return &(mIndices[part])[0]; +inline void* Mesh::getIndicesPointer(uint32_t part) { + return &(mIndices[part])[0]; } // Return a reference to a texture of the mesh -inline Texture2D& Mesh::getTexture(uint part) { - return mTextures[part]; +inline Texture2D& Mesh::getTexture(uint32_t part) { + return mTextures[part]; } // Set a texture to a part of the mesh -inline void Mesh::setTexture(Texture2D& texture, uint part) { - mTextures[part] = texture; +inline void Mesh::setTexture(Texture2D& texture, uint32_t part) { + mTextures[part] = texture; } } diff --git a/tools/testbed/opengl-framework/src/MeshReaderWriter.cpp b/tools/testbed/opengl-framework/src/MeshReaderWriter.cpp index 1a76a90..c47503a 100644 --- a/tools/testbed/opengl-framework/src/MeshReaderWriter.cpp +++ b/tools/testbed/opengl-framework/src/MeshReaderWriter.cpp @@ -1,26 +1,26 @@ /******************************************************************************** -* OpenGL-Framework * -* Copyright (c) 2013 Daniel Chappuis * +* OpenGL-Framework * +* Copyright (c) 2013 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: * -* * +* 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. * -* * +* 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 @@ -43,351 +43,351 @@ MeshReaderWriter::MeshReaderWriter() { // Load a mesh from a file and returns true if the mesh has been sucessfully loaded void MeshReaderWriter::loadMeshFromFile(const std::string& filename, Mesh& meshToCreate) { - // Get the extension of the file - uint startPosExtension = filename.find_last_of("."); - string extension = filename.substr(startPosExtension+1); + // Get the extension of the file + uint32_t startPosExtension = filename.find_last_of("."); + string extension = filename.substr(startPosExtension+1); - // Load the file using the correct method - if (extension == "obj") { - loadOBJFile(filename, meshToCreate); - } - else { + // Load the file using the correct method + if (extension == "obj") { + loadOBJFile(filename, meshToCreate); + } + else { - // Display an error message and throw an exception - string errorMessage("Error : the MeshReaderWriter class cannot load a file with the extension ."); - errorMessage += extension; - std::cerr << errorMessage << std::endl; - throw std::invalid_argument(errorMessage.c_str()); - } + // Display an error message and throw an exception + string errorMessage("Error : the MeshReaderWriter class cannot load a file with the extension ."); + errorMessage += extension; + std::cerr << errorMessage << std::endl; + throw std::invalid_argument(errorMessage.c_str()); + } } // Write a mesh to a file void MeshReaderWriter::writeMeshToFile(const std::string& filename, const Mesh& meshToWrite) { - // Get the extension of the file - uint startPosExtension = filename.find_last_of("."); - string extension = filename.substr(startPosExtension+1); + // Get the extension of the file + uint32_t startPosExtension = filename.find_last_of("."); + string extension = filename.substr(startPosExtension+1); - // Load the file using the correct method - if (extension == "obj") { - writeOBJFile(filename, meshToWrite); - } - else { + // Load the file using the correct method + if (extension == "obj") { + writeOBJFile(filename, meshToWrite); + } + else { - // Display an error message and throw an exception - string errorMessage("Error : the MeshReaderWriter class cannot store a mesh file with the extension ."); - errorMessage += extension; - std::cerr << errorMessage << std::endl; - throw std::invalid_argument(errorMessage.c_str()); - } + // Display an error message and throw an exception + string errorMessage("Error : the MeshReaderWriter class cannot store a mesh file with the extension ."); + errorMessage += extension; + std::cerr << errorMessage << std::endl; + throw std::invalid_argument(errorMessage.c_str()); + } } // Load an OBJ file with a triangular or quad mesh void MeshReaderWriter::loadOBJFile(const string &filename, Mesh& meshToCreate) { - // Open the file - std::ifstream meshFile(filename.c_str()); + // Open the file + std::ifstream meshFile(filename.c_str()); - // If we cannot open the file - if(!meshFile.is_open()) { + // If we cannot open the file + if(!meshFile.is_open()) { - // Throw an exception and display an error message - string errorMessage("Error : Cannot open the file " + filename); - std::cerr << errorMessage << std::endl; - throw runtime_error(errorMessage); - } + // Throw an exception and display an error message + string errorMessage("Error : Cannot open the file " + filename); + std::cerr << errorMessage << std::endl; + throw runtime_error(errorMessage); + } - std::string buffer; - string line, tmp; - int id1, id2, id3, id4; - int nId1, nId2, nId3, nId4; - int tId1, tId2, tId3, tId4; - float v1, v2, v3; - size_t found1, found2; - std::vector isQuad; - std::vector vertices; - std::vector normals; - std::vector uvs; - std::vector verticesIndices; - std::vector normalsIndices; - std::vector uvsIndices; + std::string buffer; + string line, tmp; + int32_t id1, id2, id3, id4; + int32_t nId1, nId2, nId3, nId4; + int32_t tId1, tId2, tId3, tId4; + float v1, v2, v3; + size_t found1, found2; + std::vector isQuad; + std::vector vertices; + std::vector normals; + std::vector uvs; + std::vector verticesIndices; + std::vector normalsIndices; + std::vector uvsIndices; - // ---------- Collect the data from the file ---------- // + // ---------- Collect the data from the file ---------- // - // For each line of the file - while(std::getline(meshFile, buffer)) { + // For each line of the file + while(std::getline(meshFile, buffer)) { - std::istringstream lineStream(buffer); - std::string word; - lineStream >> word; - std::transform(word.begin(), word.end(), word.begin(), ::tolower); - if(word == "usemtl") { // Material definition + std::istringstream lineStream(buffer); + std::string word; + lineStream >> word; + std::transform(word.begin(), word.end(), word.begin(), ::tolower); + if(word == "usemtl") { // Material definition - // Loading of MTL file is not implemented + // Loading of MTL file is not implemented - } - else if(word == "v") { // Vertex position - sscanf(buffer.c_str(), "%*s %f %f %f", &v1, &v2, &v3); - vertices.push_back(Vector3(v1, v2, v3)); - } - else if(word == "vt") { // Vertex texture coordinate - sscanf(buffer.c_str(), "%*s %f %f", &v1, &v2); - uvs.push_back(Vector2(v1,v2)); - } - else if(word == "vn") { // Vertex normal - sscanf(buffer.c_str(), "%*s %f %f %f", &v1, &v2, &v3); - normals.push_back(Vector3(v1 ,v2, v3)); - } - else if (word == "f") { // Face - line = buffer; - found1 = (int)line.find("/"); - bool isFaceQuad = false; - int foundNext = (int)line.substr(found1+1).find("/"); + } + else if(word == "v") { // Vertex position + sscanf(buffer.c_str(), "%*s %f %f %f", &v1, &v2, &v3); + vertices.push_back(Vector3(v1, v2, v3)); + } + else if(word == "vt") { // Vertex texture coordinate + sscanf(buffer.c_str(), "%*s %f %f", &v1, &v2); + uvs.push_back(Vector2(v1,v2)); + } + else if(word == "vn") { // Vertex normal + sscanf(buffer.c_str(), "%*s %f %f %f", &v1, &v2, &v3); + normals.push_back(Vector3(v1 ,v2, v3)); + } + else if (word == "f") { // Face + line = buffer; + found1 = (int32_t)line.find("/"); + bool isFaceQuad = false; + int32_t foundNext = (int32_t)line.substr(found1+1).find("/"); - // If the face definition is of the form "f v1 v2 v3 v4" - if(found1 == string::npos) { - int nbVertices = sscanf(buffer.c_str(), "%*s %d %d %d %d", &id1, &id2, &id3, &id4); - if (nbVertices == 4) isFaceQuad = true; - } - // If the face definition is of the form "f v1// v2// v3// v4//" - else if (foundNext == 0) { - int nbVertices = sscanf(buffer.c_str(), "%*s %d// %d// %d// %d//", &id1, &id2, &id3, &id4); - if (nbVertices == 4) isFaceQuad = true; - } - else { // If the face definition contains vertices and texture coordinates + // If the face definition is of the form "f v1 v2 v3 v4" + if(found1 == string::npos) { + int32_t nbVertices = sscanf(buffer.c_str(), "%*s %d %d %d %d", &id1, &id2, &id3, &id4); + if (nbVertices == 4) isFaceQuad = true; + } + // If the face definition is of the form "f v1// v2// v3// v4//" + else if (foundNext == 0) { + int32_t nbVertices = sscanf(buffer.c_str(), "%*s %d// %d// %d// %d//", &id1, &id2, &id3, &id4); + if (nbVertices == 4) isFaceQuad = true; + } + else { // If the face definition contains vertices and texture coordinates - //get the part of the string until the second index - tmp = line.substr(found1+1); - found2 = (int)tmp.find(" "); - tmp = tmp.substr(0,found2); - found2 = (int)tmp.find("/"); + //get the part of the string until the second index + tmp = line.substr(found1+1); + found2 = (int32_t)tmp.find(" "); + tmp = tmp.substr(0,found2); + found2 = (int32_t)tmp.find("/"); - // If the face definition is of the form "f vert1/textcoord1 vert2/textcoord2 ..." - if(found2 == string::npos) { - int n = sscanf(buffer.c_str(), "%*s %d/%d %d/%d %d/%d %d/%d", &id1, &tId1, &id2, &tId2, &id3, &tId3, &id4, &tId4); - if (n == 8) isFaceQuad = true; - uvsIndices.push_back(tId1-1); - uvsIndices.push_back(tId2-1); - uvsIndices.push_back(tId3-1); - if (isFaceQuad) uvsIndices.push_back(tId4-1); - } - else { - tmp = line.substr(found1+1); - found2 = (int)tmp.find("/"); + // If the face definition is of the form "f vert1/textcoord1 vert2/textcoord2 ..." + if(found2 == string::npos) { + int32_t n = sscanf(buffer.c_str(), "%*s %d/%d %d/%d %d/%d %d/%d", &id1, &tId1, &id2, &tId2, &id3, &tId3, &id4, &tId4); + if (n == 8) isFaceQuad = true; + uvsIndices.push_back(tId1-1); + uvsIndices.push_back(tId2-1); + uvsIndices.push_back(tId3-1); + if (isFaceQuad) uvsIndices.push_back(tId4-1); + } + else { + tmp = line.substr(found1+1); + found2 = (int32_t)tmp.find("/"); - // If the face definition is of the form "f vert1/normal1 vert2/normal2 ..." - if(found2 == 0) { - int n = sscanf(buffer.c_str(), "%*s %d//%d %d//%d %d//%d %d//%d", &id1, &nId1, &id2, &nId2, &id3, &nId3, &id4, &nId4); - if (n == 8) isFaceQuad = true; - } - // If the face definition is of the form "f vert1/textcoord1/normal1 ..." - else { - int n = sscanf(buffer.c_str(), "%*s %d/%d/%d %d/%d/%d %d/%d/%d %d/%d/%d", &id1, &tId1, &nId1, &id2, &tId2, &nId2, &id3, &tId3, &nId3, &id4, &tId4, &nId4); - if (n == 12) isFaceQuad = true; - uvsIndices.push_back(tId1-1); - uvsIndices.push_back(tId2-1); - uvsIndices.push_back(tId3-1); - if (isFaceQuad) uvsIndices.push_back(tId4-1); - } - normalsIndices.push_back(nId1-1); - normalsIndices.push_back(nId2-1); - normalsIndices.push_back(nId3-1); - if (isFaceQuad) normalsIndices.push_back(nId4-1); - } - } - verticesIndices.push_back(id1-1); - verticesIndices.push_back(id2-1); - verticesIndices.push_back(id3-1); - if (isFaceQuad) verticesIndices.push_back((id4-1)); - isQuad.push_back(isFaceQuad); - } - } + // If the face definition is of the form "f vert1/normal1 vert2/normal2 ..." + if(found2 == 0) { + int32_t n = sscanf(buffer.c_str(), "%*s %d//%d %d//%d %d//%d %d//%d", &id1, &nId1, &id2, &nId2, &id3, &nId3, &id4, &nId4); + if (n == 8) isFaceQuad = true; + } + // If the face definition is of the form "f vert1/textcoord1/normal1 ..." + else { + int32_t n = sscanf(buffer.c_str(), "%*s %d/%d/%d %d/%d/%d %d/%d/%d %d/%d/%d", &id1, &tId1, &nId1, &id2, &tId2, &nId2, &id3, &tId3, &nId3, &id4, &tId4, &nId4); + if (n == 12) isFaceQuad = true; + uvsIndices.push_back(tId1-1); + uvsIndices.push_back(tId2-1); + uvsIndices.push_back(tId3-1); + if (isFaceQuad) uvsIndices.push_back(tId4-1); + } + normalsIndices.push_back(nId1-1); + normalsIndices.push_back(nId2-1); + normalsIndices.push_back(nId3-1); + if (isFaceQuad) normalsIndices.push_back(nId4-1); + } + } + verticesIndices.push_back(id1-1); + verticesIndices.push_back(id2-1); + verticesIndices.push_back(id3-1); + if (isFaceQuad) verticesIndices.push_back((id4-1)); + isQuad.push_back(isFaceQuad); + } + } - assert(!verticesIndices.empty()); - assert(normalsIndices.empty() || normalsIndices.size() == verticesIndices.size()); - assert(uvsIndices.empty() || uvsIndices.size() == verticesIndices.size()); - meshFile.close(); + assert(!verticesIndices.empty()); + assert(normalsIndices.empty() || normalsIndices.size() == verticesIndices.size()); + assert(uvsIndices.empty() || uvsIndices.size() == verticesIndices.size()); + meshFile.close(); - // ---------- Merge the data that we have collected from the file ---------- // + // ---------- Merge the data that we have collected from the file ---------- // - // Destroy the current mesh - meshToCreate.destroy(); + // Destroy the current mesh + meshToCreate.destroy(); - // Mesh data - vector > meshIndices; - vector meshNormals; - if (!normals.empty()) meshNormals = vector(vertices.size(), Vector3(0, 0, 0)); - vector meshUVs; - if (!uvs.empty()) meshUVs = vector(vertices.size(), Vector2(0, 0)); + // Mesh data + vector > meshIndices; + vector meshNormals; + if (!normals.empty()) meshNormals = vector(vertices.size(), Vector3(0, 0, 0)); + vector meshUVs; + if (!uvs.empty()) meshUVs = vector(vertices.size(), Vector2(0, 0)); - // We cannot load mesh with several parts for the moment - uint meshPart = 0; + // We cannot load mesh with several parts for the moment + uint32_t meshPart = 0; - // Fill in the vertex indices - // We also triangulate each quad face - meshIndices.push_back(std::vector()); - for(size_t i = 0, j = 0; i < verticesIndices.size(); j++) { + // Fill in the vertex indices + // We also triangulate each quad face + meshIndices.push_back(std::vector()); + for(size_t i = 0, j = 0; i < verticesIndices.size(); j++) { - // Get the current vertex IDs - uint i1 = verticesIndices[i]; - uint i2 = verticesIndices[i+1]; - uint i3 = verticesIndices[i+2]; + // Get the current vertex IDs + uint32_t i1 = verticesIndices[i]; + uint32_t i2 = verticesIndices[i+1]; + uint32_t i3 = verticesIndices[i+2]; - // Add the vertex normal - if (!normalsIndices.empty() && !normals.empty()) { - meshNormals[i1] = normals[normalsIndices[i]]; - meshNormals[i2] = normals[normalsIndices[i+1]]; - meshNormals[i3] = normals[normalsIndices[i+2]]; - } + // Add the vertex normal + if (!normalsIndices.empty() && !normals.empty()) { + meshNormals[i1] = normals[normalsIndices[i]]; + meshNormals[i2] = normals[normalsIndices[i+1]]; + meshNormals[i3] = normals[normalsIndices[i+2]]; + } - // Add the vertex UV texture coordinates - if (!uvsIndices.empty() && !uvs.empty()) { - meshUVs[i1] = uvs[uvsIndices[i]]; - meshUVs[i2] = uvs[uvsIndices[i+1]]; - meshUVs[i3] = uvs[uvsIndices[i+2]]; - } + // Add the vertex UV texture coordinates + if (!uvsIndices.empty() && !uvs.empty()) { + meshUVs[i1] = uvs[uvsIndices[i]]; + meshUVs[i2] = uvs[uvsIndices[i+1]]; + meshUVs[i3] = uvs[uvsIndices[i+2]]; + } - // If the current vertex not in a quad (it is part of a triangle) - if (!isQuad[j]) { + // If the current vertex not in a quad (it is part of a triangle) + if (!isQuad[j]) { - // Add the vertex indices - meshIndices[meshPart].push_back(i1); - meshIndices[meshPart].push_back(i2); - meshIndices[meshPart].push_back(i3); + // Add the vertex indices + meshIndices[meshPart].push_back(i1); + meshIndices[meshPart].push_back(i2); + meshIndices[meshPart].push_back(i3); - i+=3; - } - else { // If the current vertex is in a quad + i+=3; + } + else { // If the current vertex is in a quad - Vector3 v1 = vertices[i1]; - Vector3 v2 = vertices[i2]; - Vector3 v3 = vertices[i3]; - uint i4 = verticesIndices[i+3]; - Vector3 v4 = vertices[i4]; + Vector3 v1 = vertices[i1]; + Vector3 v2 = vertices[i2]; + Vector3 v3 = vertices[i3]; + uint32_t i4 = verticesIndices[i+3]; + Vector3 v4 = vertices[i4]; - Vector3 v13 = v3-v1; - Vector3 v12 = v2-v1; - Vector3 v14 = v4-v1; + Vector3 v13 = v3-v1; + Vector3 v12 = v2-v1; + Vector3 v14 = v4-v1; - float a1 = v13.dot(v12); - float a2 = v13.dot(v14); - if((a1 >= 0 && a2 <= 0) || (a1 <= 0 && a2 >= 0)) { - meshIndices[meshPart].push_back(i1); - meshIndices[meshPart].push_back(i2); - meshIndices[meshPart].push_back(i3); - meshIndices[meshPart].push_back(i1); - meshIndices[meshPart].push_back(i3); - meshIndices[meshPart].push_back(i4); - } - else { - meshIndices[meshPart].push_back(i1); - meshIndices[meshPart].push_back(i2); - meshIndices[meshPart].push_back(i4); - meshIndices[meshPart].push_back(i2); - meshIndices[meshPart].push_back(i3); - meshIndices[meshPart].push_back(i4); - } + float a1 = v13.dot(v12); + float a2 = v13.dot(v14); + if((a1 >= 0 && a2 <= 0) || (a1 <= 0 && a2 >= 0)) { + meshIndices[meshPart].push_back(i1); + meshIndices[meshPart].push_back(i2); + meshIndices[meshPart].push_back(i3); + meshIndices[meshPart].push_back(i1); + meshIndices[meshPart].push_back(i3); + meshIndices[meshPart].push_back(i4); + } + else { + meshIndices[meshPart].push_back(i1); + meshIndices[meshPart].push_back(i2); + meshIndices[meshPart].push_back(i4); + meshIndices[meshPart].push_back(i2); + meshIndices[meshPart].push_back(i3); + meshIndices[meshPart].push_back(i4); + } - // Add the vertex normal - if (!normalsIndices.empty() && !normals.empty()) { - meshNormals[i4] = normals[normalsIndices[i]]; - } + // Add the vertex normal + if (!normalsIndices.empty() && !normals.empty()) { + meshNormals[i4] = normals[normalsIndices[i]]; + } - // Add the vertex UV texture coordinates - if (!uvsIndices.empty() && !uvs.empty()) { - meshUVs[i4] = uvs[uvsIndices[i]]; - } + // Add the vertex UV texture coordinates + if (!uvsIndices.empty() && !uvs.empty()) { + meshUVs[i4] = uvs[uvsIndices[i]]; + } - i+=4; - } - } + i+=4; + } + } - assert(meshNormals.empty() || meshNormals.size() == vertices.size()); - assert(meshUVs.empty() || meshUVs.size() == vertices.size()); + assert(meshNormals.empty() || meshNormals.size() == vertices.size()); + assert(meshUVs.empty() || meshUVs.size() == vertices.size()); - // Set the data to the mesh - meshToCreate.setIndices(meshIndices); - meshToCreate.setVertices(vertices); - meshToCreate.setNormals(meshNormals); - meshToCreate.setUVs(meshUVs); + // Set the data to the mesh + meshToCreate.setIndices(meshIndices); + meshToCreate.setVertices(vertices); + meshToCreate.setNormals(meshNormals); + meshToCreate.setUVs(meshUVs); } -// Store a mesh into a OBJ file +// Store a mesh int32_to a OBJ file void MeshReaderWriter::writeOBJFile(const std::string& filename, const Mesh& meshToWrite) { - std::ofstream file(filename.c_str()); + std::ofstream file(filename.c_str()); - // Geth the mesh data - const std::vector& vertices = meshToWrite.getVertices(); - const std::vector& normals = meshToWrite.getNormals(); - const std::vector& uvs = meshToWrite.getUVs(); + // Geth the mesh data + const std::vector& vertices = meshToWrite.getVertices(); + const std::vector& normals = meshToWrite.getNormals(); + const std::vector& uvs = meshToWrite.getUVs(); - // If we can open the file - if (file.is_open()) { + // If we can open the file + if (file.is_open()) { - assert(meshToWrite.getNbVertices() == vertices.size()); + assert(meshToWrite.getNbVertices() == vertices.size()); - // Write the vertices - for (uint v=0; v& indices = meshToWrite.getIndices(p); + // Get the indices of the part + const std::vector& indices = meshToWrite.getIndices(p); - // For each index of the part - for (uint i=0; i #include #ifdef USE_JPEG_TEXTURE - #include - #include + #include + #include #endif using namespace openglframework; using namespace std; // Constants -const uint TextureReaderWriter::JPEG_COMPRESSION_QUALITY = 98; +const uint32_t TextureReaderWriter::JPEG_COMPRESSION_QUALITY = 98; // TGA file Header #pragma pack(push, 1) typedef struct { - unsigned char identsize; // size of ID field that follows 18 byte header (0 usually) - unsigned char colourmaptype; // type of colour map 0=none, 1=has palette - unsigned char imagetype; // type of image 0=none,1=indexed,2=rgb,3=grey,+8=rle packed + unsigned char identsize; // size of ID field that follows 18 byte header (0 usually) + unsigned char colourmaptype; // type of colour map 0=none, 1=has palette + unsigned char imagetype; // type of image 0=none,1=indexed,2=rgb,3=grey,+8=rle packed - short colourmapstart; // first colour map entry in palette - short colourmaplength; // number of colours in palette - unsigned char colourmapbits; // number of bits per palette entry 15,16,24,32 + short colourmapstart; // first colour map entry in palette + short colourmaplength; // number of colours in palette + unsigned char colourmapbits; // number of bits per palette entry 15,16,24,32 - short xstart; // image x origin - short ystart; // image y origin - short width; // image width in pixels - short height; // image height in pixels - unsigned char bits; // image bits per pixel 8,16,24,32 - unsigned char descriptor; // image descriptor bits (vh flip bits) + short xstart; // image x origin + short ystart; // image y origin + short width; // image width in pixels + short height; // image height in pixels + unsigned char bits; // image bits per pixel 8,16,24,32 + unsigned char descriptor; // image descriptor bits (vh flip bits) - // pixel data follows header + // pixel data follows header } TGA_HEADER; #pragma pack(pop) // Load a texture from a file void TextureReaderWriter::loadTextureFromFile(const std::string& filename, - Texture2D& textureToCreate) { + Texture2D& textureToCreate) { - // Get the extension of the file - uint startPosExtension = filename.find_last_of("."); - string extension = filename.substr(startPosExtension+1); + // Get the extension of the file + uint32_t startPosExtension = filename.find_last_of("."); + string extension = filename.substr(startPosExtension+1); - // Load the file using the correct method - if (extension == "tga") { - readTGAPicture(filename, textureToCreate); - } + // Load the file using the correct method + if (extension == "tga") { + readTGAPicture(filename, textureToCreate); + } #ifdef USE_JPEG_TEXTURE - else if (extension == "jpg" || extension == "jpeg"){ - readJPEGPicture(filename, textureToCreate); - } + else if (extension == "jpg" || extension == "jpeg"){ + readJPEGPicture(filename, textureToCreate); + } #endif - else { + else { - // Display an error message and throw an exception - string errorMessage("Error : the TextureLoader class cannot load a file with the extension ."); - errorMessage += extension; - std::cerr << errorMessage << std::endl; - throw std::invalid_argument(errorMessage.c_str()); - } + // Display an error message and throw an exception + string errorMessage("Error : the TextureLoader class cannot load a file with the extension ."); + errorMessage += extension; + std::cerr << errorMessage << std::endl; + throw std::invalid_argument(errorMessage.c_str()); + } } // Write a texture to a file void TextureReaderWriter::writeTextureToFile(const std::string& filename,const Texture2D& texture) { - // Get the extension of the file - uint startPosExtension = filename.find_last_of("."); - string extension = filename.substr(startPosExtension+1); + // Get the extension of the file + uint32_t startPosExtension = filename.find_last_of("."); + string extension = filename.substr(startPosExtension+1); - // Write the file using the correct method - if (extension == "tga") { - writeTGAPicture(filename, texture); - } + // Write the file using the correct method + if (extension == "tga") { + writeTGAPicture(filename, texture); + } #ifdef USE_JPEG_TEXTURE - else if (extension == "jpg" || extension == "jpeg"){ - writeJPEGPicture(filename, texture); - } + else if (extension == "jpg" || extension == "jpeg"){ + writeJPEGPicture(filename, texture); + } #endif - else { + else { - // Display an error message and throw an exception - string errorMessage("Error : the TextureReaderWriter class cannot write a file with the extension ."); - errorMessage += extension; - std::cerr << errorMessage << std::endl; - throw std::invalid_argument(errorMessage.c_str()); - } + // Display an error message and throw an exception + string errorMessage("Error : the TextureReaderWriter class cannot write a file with the extension ."); + errorMessage += extension; + std::cerr << errorMessage << std::endl; + throw std::invalid_argument(errorMessage.c_str()); + } } // Load a TGA picture void TextureReaderWriter::readTGAPicture(const std::string &filename, Texture2D& textureToCreate) { - // Open the file - std::ifstream stream(filename.c_str(), std::ios::binary); + // Open the file + std::ifstream stream(filename.c_str(), std::ios::binary); - // If we cannot open the file - if(!stream.is_open()) { + // If we cannot open the file + if(!stream.is_open()) { - // Throw an exception and display an error message - string errorMessage("Error : Cannot open the file " + filename); - std::cerr << errorMessage << std::endl; - throw std::runtime_error(errorMessage); - } + // Throw an exception and display an error message + string errorMessage("Error : Cannot open the file " + filename); + std::cerr << errorMessage << std::endl; + throw std::runtime_error(errorMessage); + } - TGA_HEADER header; - stream.read((char *)(&header), sizeof(TGA_HEADER)); - assert(header.width <= 4096 && header.width <= 4096 && - header.imagetype == 2 && header.bits == 24); + TGA_HEADER header; + stream.read((char *)(&header), sizeof(TGA_HEADER)); + assert(header.width <= 4096 && header.width <= 4096 && + header.imagetype == 2 && header.bits == 24); - // Read the file - uint width = header.width; - uint height = header.width; - uint sizeImg = width*height; - char* data = new char[sizeImg*3]; - assert(data); - stream.read(data, sizeImg*3); - for(uint i = 0; i < sizeImg; i++) { - unsigned pos = i*3; - unsigned char red = data[pos]; - data[pos] = data[pos + 2]; - data[pos + 2] = red; - } + // Read the file + uint32_t width = header.width; + uint32_t height = header.width; + uint32_t sizeImg = width*height; + char* data = new char[sizeImg*3]; + assert(data); + stream.read(data, sizeImg*3); + for(uint32_t i = 0; i < sizeImg; i++) { + unsigned pos = i*3; + unsigned char red = data[pos]; + data[pos] = data[pos + 2]; + data[pos + 2] = red; + } - // Close the stream - stream.close(); + // Close the stream + stream.close(); - // Create the OpenGL texture using the picture data from the file - textureToCreate.create(width, height, GL_RGB, GL_RGB, GL_UNSIGNED_BYTE, data); + // Create the OpenGL texture using the picture data from the file + textureToCreate.create(width, height, GL_RGB, GL_RGB, GL_UNSIGNED_BYTE, data); - // Free the data memory - delete[] data; + // Free the data memory + delete[] data; } // Write a TGA picture void TextureReaderWriter::writeTGAPicture(const std::string& filename, const Texture2D& texture) { - assert(texture.getID() != 0); + assert(texture.getID() != 0); - // Bind the corresponding texture - glBindTexture(GL_TEXTURE_2D, texture.getID()); + // Bind the corresponding texture + glBindTexture(GL_TEXTURE_2D, texture.getID()); - uint sizeImg = texture.getWidth() * texture.getHeight(); + uint32_t sizeImg = texture.getWidth() * texture.getHeight(); - // Get the bytes form the OpenGL texture - char* data = new char[sizeImg * 3]; - glGetTexImage(GL_TEXTURE_2D, 0, GL_RGB, GL_UNSIGNED_BYTE, data); + // Get the bytes form the OpenGL texture + char* data = new char[sizeImg * 3]; + glGetTexImage(GL_TEXTURE_2D, 0, GL_RGB, GL_UNSIGNED_BYTE, data); - // Open the file - std::ofstream stream(filename.c_str(), std::ios::binary); + // Open the file + std::ofstream stream(filename.c_str(), std::ios::binary); - // If the file cannot be opened - if(!stream.is_open()) { + // If the file cannot be opened + if(!stream.is_open()) { - // Throw an exception and display an error message - string errorMessage("Error : Cannot create/access the file " + filename); - std::cerr << errorMessage << std::endl; - delete[] data; - throw std::runtime_error(errorMessage); - } + // Throw an exception and display an error message + string errorMessage("Error : Cannot create/access the file " + filename); + std::cerr << errorMessage << std::endl; + delete[] data; + throw std::runtime_error(errorMessage); + } - // Fill in the TGA header - TGA_HEADER header; - header.identsize = 0; // size of ID field that follows 18 byte header (0 usually) - header.colourmaptype = 0; // type of colour map 0=none, 1=has palette - header.imagetype = 2; // type of image 0=none,1=indexed,2=rgb,3=grey,+8=rle packed - header.colourmapstart = 0; // first colour map entry in palette - header.colourmaplength = 0; // number of colours in palette - header.colourmapbits=0; // number of bits per palette entry 15,16,24,32 - header.xstart = 0; // image x origin - header.ystart = 0; // image y origin - header.width = (short)texture.getWidth(); // image width in pixels - header.height = (short)texture.getHeight(); // image height in pixels - header.bits = 24; // image bits per pixel 8,16,24,32 - header.descriptor = 0; // image descriptor bits (vh flip bits) + // Fill in the TGA header + TGA_HEADER header; + header.identsize = 0; // size of ID field that follows 18 byte header (0 usually) + header.colourmaptype = 0; // type of colour map 0=none, 1=has palette + header.imagetype = 2; // type of image 0=none,1=indexed,2=rgb,3=grey,+8=rle packed + header.colourmapstart = 0; // first colour map entry in palette + header.colourmaplength = 0; // number of colours in palette + header.colourmapbits=0; // number of bits per palette entry 15,16,24,32 + header.xstart = 0; // image x origin + header.ystart = 0; // image y origin + header.width = (short)texture.getWidth(); // image width in pixels + header.height = (short)texture.getHeight(); // image height in pixels + header.bits = 24; // image bits per pixel 8,16,24,32 + header.descriptor = 0; // image descriptor bits (vh flip bits) - // Write the header to the file - stream.write((char*)(&header), sizeof(TGA_HEADER)); + // Write the header to the file + stream.write((char*)(&header), sizeof(TGA_HEADER)); - // Write the bytes to the file - for(uint i = 0; i < sizeImg; i++) { - unsigned pos = i*3; - unsigned char red = data[pos]; - data[pos] = data[pos + 2]; - data[pos + 2] = red; - } - stream.write(data, sizeImg*3); + // Write the bytes to the file + for(uint32_t i = 0; i < sizeImg; i++) { + unsigned pos = i*3; + unsigned char red = data[pos]; + data[pos] = data[pos + 2]; + data[pos + 2] = red; + } + stream.write(data, sizeImg*3); - // Close the file - stream.close(); + // Close the file + stream.close(); - // Delete the data - delete[] data; + // Delete the data + delete[] data; - // Unbind the corresponding texture - glBindTexture(GL_TEXTURE_2D, 0); + // Unbind the corresponding texture + glBindTexture(GL_TEXTURE_2D, 0); } #ifdef USE_JPEG_TEXTURE @@ -225,108 +225,108 @@ void TextureReaderWriter::writeTGAPicture(const std::string& filename, const Tex // Read a JPEG picture void TextureReaderWriter::readJPEGPicture(const std::string& filename, Texture2D& textureToCreate) { - struct jpeg_decompress_struct info; - struct jpeg_error_mgr error; + struct jpeg_decompress_struct info; + struct jpeg_error_mgr error; - info.err = jpeg_std_error(&error); - jpeg_create_decompress(&info); + info.err = jpeg_std_error(&error); + jpeg_create_decompress(&info); - // Open the file - FILE* file = fopen(filename.c_str(), "rb"); + // Open the file + FILE* file = fopen(filename.c_str(), "rb"); - // If we cannot open the file - if (!file) { + // If we cannot open the file + if (!file) { - // Throw an exception and display an error message - string errorMessage("Error : Cannot open the file " + filename); - std::cerr << errorMessage << std::endl; - throw std::runtime_error(errorMessage); - } + // Throw an exception and display an error message + string errorMessage("Error : Cannot open the file " + filename); + std::cerr << errorMessage << std::endl; + throw std::runtime_error(errorMessage); + } - jpeg_stdio_src(&info, file); - jpeg_read_header(&info, true); - jpeg_start_decompress(&info); + jpeg_stdio_src(&info, file); + jpeg_read_header(&info, true); + jpeg_start_decompress(&info); - unsigned long x = info.output_width; - unsigned long y = info.output_height; - int channels = info.num_components; - assert(channels == 3); + unsigned long x = info.output_width; + unsigned long y = info.output_height; + int32_t channels = info.num_components; + assert(channels == 3); - unsigned long size = x * y * 3; + unsigned long size = x * y * 3; - BYTE* data = new BYTE[size]; + BYTE* data = new BYTE[size]; - BYTE* p1 = data; - BYTE** p2 = &p1; - int numlines = 0; + BYTE* p1 = data; + BYTE** p2 = &p1; + int32_t numlines = 0; - while(info.output_scanline < info.output_height) { - numlines = jpeg_read_scanlines(&info, p2, 1); - *p2 += numlines * 3 * info.output_width; - } + while(info.output_scanline < info.output_height) { + numlines = jpeg_read_scanlines(&info, p2, 1); + *p2 += numlines * 3 * info.output_width; + } - jpeg_finish_decompress(&info); //finish decompressing this file + jpeg_finish_decompress(&info); //finish decompressing this file - // Close the file - fclose(file); + // Close the file + fclose(file); - // Create the OpenGL texture - textureToCreate.create(x, y, GL_RGB, GL_RGB, GL_UNSIGNED_BYTE, data); + // Create the OpenGL texture + textureToCreate.create(x, y, GL_RGB, GL_RGB, GL_UNSIGNED_BYTE, data); - // Free allocated memory - delete[] data; + // Free allocated memory + delete[] data; } // Write a JPEG picture void TextureReaderWriter::writeJPEGPicture(const std::string& filename, const Texture2D& texture) { - struct jpeg_compress_struct info; - struct jpeg_error_mgr error; + struct jpeg_compress_struct info; + struct jpeg_error_mgr error; - info.err = jpeg_std_error(&error); - jpeg_create_compress(&info); + info.err = jpeg_std_error(&error); + jpeg_create_compress(&info); - // Open the file - FILE* file = fopen(filename.c_str(), "wb"); + // Open the file + FILE* file = fopen(filename.c_str(), "wb"); - if (!file) { - // Throw an exception and display an error message - string errorMessage("Error : Cannot write JPEG picture into the file " + filename); - std::cerr << errorMessage << std::endl; - throw std::runtime_error(errorMessage); - } + if (!file) { + // Throw an exception and display an error message + string errorMessage("Error : Cannot write JPEG picture int32_to the file " + filename); + std::cerr << errorMessage << std::endl; + throw std::runtime_error(errorMessage); + } - // Get the bytes form the OpenGL texture - char* data = new char[texture.getWidth() * texture.getHeight() * 3]; - glGetTexImage(GL_TEXTURE_2D, 0, GL_RGB, GL_UNSIGNED_BYTE, data); + // Get the bytes form the OpenGL texture + char* data = new char[texture.getWidth() * texture.getHeight() * 3]; + glGetTexImage(GL_TEXTURE_2D, 0, GL_RGB, GL_UNSIGNED_BYTE, data); - jpeg_stdio_dest(&info, file); + jpeg_stdio_dest(&info, file); - info.image_width = texture.getWidth(); - info.image_height = texture.getHeight(); - info.input_components = 3; - info.in_color_space = JCS_RGB; - jpeg_set_defaults(&info); - jpeg_set_quality(&info, JPEG_COMPRESSION_QUALITY, true); + info.image_width = texture.getWidth(); + info.image_height = texture.getHeight(); + info.input_components = 3; + info.in_color_space = JCS_RGB; + jpeg_set_defaults(&info); + jpeg_set_quality(&info, JPEG_COMPRESSION_QUALITY, true); - jpeg_start_compress(&info, true); + jpeg_start_compress(&info, true); - // Write the data into the file - JSAMPROW rowPointer; - int rowStride = texture.getWidth() * 3; - while (info.next_scanline < info.image_height) { - rowPointer = (JSAMPROW) &data[info.next_scanline * rowStride]; - jpeg_write_scanlines(&info, &rowPointer, 1); - } + // Write the data int32_to the file + JSAMPROW rowPointer; + int32_t rowStride = texture.getWidth() * 3; + while (info.next_scanline < info.image_height) { + rowPointer = (JSAMPROW) &data[info.next_scanline * rowStride]; + jpeg_write_scanlines(&info, &rowPointer, 1); + } - jpeg_finish_compress(&info); - jpeg_destroy_compress(&info); + jpeg_finish_compress(&info); + jpeg_destroy_compress(&info); - // Close the file - fclose(file); + // Close the file + fclose(file); - // Free allocated memory - delete[] data; + // Free allocated memory + delete[] data; } #endif diff --git a/tools/testbed/opengl-framework/src/TextureReaderWriter.h b/tools/testbed/opengl-framework/src/TextureReaderWriter.h index 91b92d3..f18466c 100644 --- a/tools/testbed/opengl-framework/src/TextureReaderWriter.h +++ b/tools/testbed/opengl-framework/src/TextureReaderWriter.h @@ -1,26 +1,26 @@ /******************************************************************************** -* OpenGL-Framework * -* Copyright (c) 2013 Daniel Chappuis * +* OpenGL-Framework * +* Copyright (c) 2013 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: * -* * +* 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. * -* * +* 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 TEXTURE_READER_WRITER_H @@ -39,39 +39,39 @@ namespace openglframework { // It currently allows to read and write the following formats : .tga class TextureReaderWriter { - private : + private : - // ------------------- Constants ------------------- // + // ------------------- Constants ------------------- // - // JPEG quality for compression (in percent) - static const uint JPEG_COMPRESSION_QUALITY; + // JPEG quality for compression (in percent) + static const uint32_t JPEG_COMPRESSION_QUALITY; - // -------------------- Methods -------------------- // + // -------------------- Methods -------------------- // - // Constructor (private because we do not want instances of this class) - TextureReaderWriter(); + // Constructor (private because we do not want instances of this class) + TextureReaderWriter(); - // Read a TGA picture - static void readTGAPicture(const std::string& filename, Texture2D& textureToCreate); + // Read a TGA picture + static void readTGAPicture(const std::string& filename, Texture2D& textureToCreate); - // Write a TGA picture - static void writeTGAPicture(const std::string& filename, const Texture2D& texture); + // Write a TGA picture + static void writeTGAPicture(const std::string& filename, const Texture2D& texture); - // Read a JPEG picture - static void readJPEGPicture(const std::string& filename, Texture2D& textureToCreate); + // Read a JPEG picture + static void readJPEGPicture(const std::string& filename, Texture2D& textureToCreate); - // Write a JPEG picture - static void writeJPEGPicture(const std::string& filename, const Texture2D& texture); + // Write a JPEG picture + static void writeJPEGPicture(const std::string& filename, const Texture2D& texture); - public : + public : - // -------------------- Methods -------------------- // + // -------------------- Methods -------------------- // - // Load a texture from a file - static void loadTextureFromFile(const std::string& filename, Texture2D& textureToCreate); + // Load a texture from a file + static void loadTextureFromFile(const std::string& filename, Texture2D& textureToCreate); - // Write a texture to a file - static void writeTextureToFile(const std::string& filename, const Texture2D& texture); + // Write a texture to a file + static void writeTextureToFile(const std::string& filename, const Texture2D& texture); }; } diff --git a/tools/testbed/opengl-framework/src/VertexArrayObject.cpp b/tools/testbed/opengl-framework/src/VertexArrayObject.cpp index fc5bdaa..817361b 100644 --- a/tools/testbed/opengl-framework/src/VertexArrayObject.cpp +++ b/tools/testbed/opengl-framework/src/VertexArrayObject.cpp @@ -1,26 +1,26 @@ /******************************************************************************** -* OpenGL-Framework * -* Copyright (c) 2015 Daniel Chappuis * +* OpenGL-Framework * +* Copyright (c) 2015 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: * -* * +* 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. * -* * +* 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 @@ -35,36 +35,36 @@ VertexArrayObject::VertexArrayObject() : mVertexArrayID(0) { // Destructor VertexArrayObject::~VertexArrayObject() { - destroy(); + destroy(); } // Create the vertex buffer object bool VertexArrayObject::create() { - // Destroy the current VAO - destroy(); + // Destroy the current VAO + destroy(); - // Check that the needed OpenGL extensions are available - bool isExtensionOK = checkOpenGLExtensions(); - if (!isExtensionOK) { - std::cerr << "Error : Impossible to use Vertex Array Object on this platform" << std::endl; - assert(false); - return false; - } + // Check that the needed OpenGL extensions are available + bool isExtensionOK = checkOpenGLExtensions(); + if (!isExtensionOK) { + std::cerr << "Error : Impossible to use Vertex Array Object on this platform" << std::endl; + assert(false); + return false; + } - // Generate a new VAO - glGenVertexArrays(1, &mVertexArrayID); - assert(mVertexArrayID != 0); + // Generate a new VAO + glGenVertexArrays(1, &mVertexArrayID); + assert(mVertexArrayID != 0); - return true; + return true; } // Destroy the VAO void VertexArrayObject::destroy() { - // Delete the vertex buffer object - if (mVertexArrayID != 0) { - glDeleteVertexArrays(1, &mVertexArrayID); - mVertexArrayID = 0; - } + // Delete the vertex buffer object + if (mVertexArrayID != 0) { + glDeleteVertexArrays(1, &mVertexArrayID); + mVertexArrayID = 0; + } } diff --git a/tools/testbed/opengl-framework/src/VertexArrayObject.h b/tools/testbed/opengl-framework/src/VertexArrayObject.h index 7298557..7dfa5f8 100644 --- a/tools/testbed/opengl-framework/src/VertexArrayObject.h +++ b/tools/testbed/opengl-framework/src/VertexArrayObject.h @@ -1,26 +1,26 @@ /******************************************************************************** -* OpenGL-Framework * -* Copyright (c) 2013 Daniel Chappuis * +* OpenGL-Framework * +* Copyright (c) 2013 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: * -* * +* 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. * -* * +* 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 VERTEX_ARRAY_OBJECT_H @@ -37,61 +37,61 @@ namespace openglframework { // Class VertexArrayObject class VertexArrayObject { - private : + private : - // -------------------- Attributes -------------------- // + // -------------------- Attributes -------------------- // - /// ID of the Vertex Array Object - GLuint mVertexArrayID; + /// ID of the Vertex Array Object + GLuint32_t mVertexArrayID; - public : + public : - // -------------------- Methods -------------------- // + // -------------------- Methods -------------------- // - /// Constructor - VertexArrayObject(); + /// Constructor + VertexArrayObject(); - /// Destructor - ~VertexArrayObject(); + /// Destructor + ~VertexArrayObject(); - /// Create the vertex buffer object - bool create(); + /// Create the vertex buffer object + bool create(); - /// Bind the VAO - void bind() const; + /// Bind the VAO + void bind() const; - /// Unbind the VAO - void unbind() const; + /// Unbind the VAO + void unbind() const; - /// Return true if the needed OpenGL extensions are available for VAO - static bool checkOpenGLExtensions(); + /// Return true if the needed OpenGL extensions are available for VAO + static bool checkOpenGLExtensions(); - /// Destroy the VAO - void destroy(); + /// Destroy the VAO + void destroy(); }; // Bind the VAO inline void VertexArrayObject::bind() const { - assert(mVertexArrayID != 0); + assert(mVertexArrayID != 0); - // Bind the VAO - glBindVertexArray(mVertexArrayID); + // Bind the VAO + glBindVertexArray(mVertexArrayID); } // Unbind the VAO inline void VertexArrayObject::unbind() const { - assert(mVertexArrayID != 0); + assert(mVertexArrayID != 0); - // Unbind the VAO - glBindVertexArray(0); + // Unbind the VAO + glBindVertexArray(0); } // Return true if the needed OpenGL extensions are available for VAO inline bool VertexArrayObject::checkOpenGLExtensions() { - // Check that OpenGL version is at least 3.0 or there the vertex array object extension exists - //return (GLEW_VERSION_3_0 || GL_ARB_vertex_array_object); - return true; + // Check that OpenGL version is at least 3.0 or there the vertex array object extension exists + //return (GLEW_VERSION_3_0 || GL_ARB_vertex_array_object); + return true; } } diff --git a/tools/testbed/opengl-framework/src/VertexBufferObject.cpp b/tools/testbed/opengl-framework/src/VertexBufferObject.cpp index 1d97077..a472a05 100644 --- a/tools/testbed/opengl-framework/src/VertexBufferObject.cpp +++ b/tools/testbed/opengl-framework/src/VertexBufferObject.cpp @@ -1,26 +1,26 @@ /******************************************************************************** -* OpenGL-Framework * -* Copyright (c) 2013 Daniel Chappuis * +* OpenGL-Framework * +* Copyright (c) 2013 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: * -* * +* 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. * -* * +* 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 @@ -30,49 +30,49 @@ using namespace openglframework; // Constructor VertexBufferObject::VertexBufferObject(GLenum targetData) - : mVertexBufferID(0), mTargetData(targetData) { + : mVertexBufferID(0), mTargetData(targetData) { } // Destructor VertexBufferObject::~VertexBufferObject() { - destroy(); + destroy(); } // Create the vertex buffer object bool VertexBufferObject::create() { - // Destroy the current VBO - destroy(); + // Destroy the current VBO + destroy(); - // Check that the needed OpenGL extensions are available - bool isExtensionOK = checkOpenGLExtensions(); - if (!isExtensionOK) { - std::cerr << "Error : Impossible to use Vertex Buffer Object on this platform" << std::endl; - assert(false); - return false; - } + // Check that the needed OpenGL extensions are available + bool isExtensionOK = checkOpenGLExtensions(); + if (!isExtensionOK) { + std::cerr << "Error : Impossible to use Vertex Buffer Object on this platform" << std::endl; + assert(false); + return false; + } - // Generate a new VBO - glGenBuffers(1, &mVertexBufferID); - assert(mVertexBufferID != 0); + // Generate a new VBO + glGenBuffers(1, &mVertexBufferID); + assert(mVertexBufferID != 0); - return true; + return true; } -// Copy data into the VBO +// Copy data int32_to the VBO void VertexBufferObject::copyDataIntoVBO(GLsizei size, const void* data, GLenum usage) { - // Copy the data into the VBO - glBufferData(mTargetData, size, data, usage); + // Copy the data int32_to the VBO + glBufferData(mTargetData, size, data, usage); } // Destroy the VBO void VertexBufferObject::destroy() { - // Delete the vertex buffer object - if (mVertexBufferID != 0) { - glDeleteFramebuffers(1, &mVertexBufferID); - mVertexBufferID = 0; - } + // Delete the vertex buffer object + if (mVertexBufferID != 0) { + glDeleteFramebuffers(1, &mVertexBufferID); + mVertexBufferID = 0; + } } diff --git a/tools/testbed/opengl-framework/src/VertexBufferObject.h b/tools/testbed/opengl-framework/src/VertexBufferObject.h index b720efa..266b317 100644 --- a/tools/testbed/opengl-framework/src/VertexBufferObject.h +++ b/tools/testbed/opengl-framework/src/VertexBufferObject.h @@ -1,26 +1,26 @@ /******************************************************************************** -* OpenGL-Framework * -* Copyright (c) 2013 Daniel Chappuis * +* OpenGL-Framework * +* Copyright (c) 2013 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: * -* * +* 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. * -* * +* 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 VERTEX_BUFFER_OBJECT_H @@ -37,69 +37,69 @@ namespace openglframework { // Class VertexBufferObject class VertexBufferObject { - private : + private : - // -------------------- Attributes -------------------- // + // -------------------- Attributes -------------------- // - /// ID of the Vertex Buffer Object - GLuint mVertexBufferID; + /// ID of the Vertex Buffer Object + GLuint32_t mVertexBufferID; - /// Target data. This variable must be GL_ARRAY_BUFFER if the VBO contains vertex - /// data (vertex coordinates, texture coordinates, normals, colors) or must be - /// GL_ELEMENT_ARRAY_BUFFER if the VBO contains index data (index array). - GLenum mTargetData; + /// Target data. This variable must be GL_ARRAY_BUFFER if the VBO contains vertex + /// data (vertex coordinates, texture coordinates, normals, colors) or must be + /// GL_ELEMENT_ARRAY_BUFFER if the VBO contains index data (index array). + GLenum mTargetData; - public : + public : - // -------------------- Methods -------------------- // + // -------------------- Methods -------------------- // - /// Constructor - VertexBufferObject(GLenum targetData); + /// Constructor + VertexBufferObject(GLenum targetData); - /// Destructor - ~VertexBufferObject(); + /// Destructor + ~VertexBufferObject(); - /// Create the vertex buffer object - bool create(); + /// Create the vertex buffer object + bool create(); - /// Copy data into the VBO - void copyDataIntoVBO(GLsizei size, const void* data, GLenum usage); + /// Copy data int32_to the VBO + void copyDataIntoVBO(GLsizei size, const void* data, GLenum usage); - /// Bind the VBO - void bind() const; + /// Bind the VBO + void bind() const; - /// Unbind the VBO - void unbind() const; + /// Unbind the VBO + void unbind() const; - /// Return true if the needed OpenGL extensions are available for VBO - static bool checkOpenGLExtensions(); + /// Return true if the needed OpenGL extensions are available for VBO + static bool checkOpenGLExtensions(); - /// Destroy the VBO - void destroy(); + /// Destroy the VBO + void destroy(); }; // Bind the VBO inline void VertexBufferObject::bind() const { - assert(mVertexBufferID != 0); + assert(mVertexBufferID != 0); - // Bind the VBO - glBindBuffer(mTargetData, mVertexBufferID); + // Bind the VBO + glBindBuffer(mTargetData, mVertexBufferID); } // Unbind the VBO inline void VertexBufferObject::unbind() const { - assert(mVertexBufferID != 0); + assert(mVertexBufferID != 0); - // Unbind the VBO - glBindBuffer(mTargetData, 0); + // Unbind the VBO + glBindBuffer(mTargetData, 0); } // Return true if the needed OpenGL extensions are available for VBO inline bool VertexBufferObject::checkOpenGLExtensions() { - // Check that OpenGL version is at least 1.5 or there the vertex buffer object extension exists - //return (GLEW_VERSION_1_5 || GL_ARB_vertex_buffer_object); - return true; + // Check that OpenGL version is at least 1.5 or there the vertex buffer object extension exists + //return (GLEW_VERSION_1_5 || GL_ARB_vertex_buffer_object); + return true; } } diff --git a/tools/testbed/opengl-framework/src/definitions.h b/tools/testbed/opengl-framework/src/definitions.h index 5cacbdf..9becf58 100644 --- a/tools/testbed/opengl-framework/src/definitions.h +++ b/tools/testbed/opengl-framework/src/definitions.h @@ -1,26 +1,26 @@ /******************************************************************************** -* OpenGL-Framework * -* Copyright (c) 2013 Daniel Chappuis * +* OpenGL-Framework * +* Copyright (c) 2013 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: * -* * +* 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. * -* * +* 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 DEFINITIONS_H @@ -29,7 +29,7 @@ namespace openglframework { // ------------------- Type definitions ------------------- // -typedef unsigned int uint; +typedef uint32_t uint32_t; // ------------------- Constants ------------------- // const float PI = 3.141592654f; diff --git a/tools/testbed/opengl-framework/src/maths/Color.h b/tools/testbed/opengl-framework/src/maths/Color.h index 3216a0a..999ad42 100644 --- a/tools/testbed/opengl-framework/src/maths/Color.h +++ b/tools/testbed/opengl-framework/src/maths/Color.h @@ -1,26 +1,26 @@ /******************************************************************************** -* OpenGL-Framework * -* Copyright (c) 2013 Daniel Chappuis * +* OpenGL-Framework * +* Copyright (c) 2013 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: * -* * +* 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. * -* * +* 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 COLOR_H @@ -32,52 +32,52 @@ namespace openglframework { // This structure represents a RGBA color. struct Color { - public: + public: - // -------------------- Attributes -------------------- // + // -------------------- Attributes -------------------- // - // RGBA color components - float r, g, b, a; + // RGBA color components + float r, g, b, a; - // -------------------- Methods -------------------- // + // -------------------- Methods -------------------- // - // Constructor - Color() : r(1), g(1), b(1), a(1) {} + // Constructor + Color() : r(1), g(1), b(1), a(1) {} - // Constructor - Color(float r, float g, float b, float a) : r(r), g(g), b(b), a(a) {} + // Constructor + Color(float r, float g, float b, float a) : r(r), g(g), b(b), a(a) {} - // Copy-constructor - Color(const Color& color) : r(color.r), g(color.g), b(color.b), a(color.a) {} + // Copy-constructor + Color(const Color& color) : r(color.r), g(color.g), b(color.b), a(color.a) {} - // Destructor - ~Color() {} + // Destructor + ~Color() {} - // Return the black color - static Color black() { return Color(0.0f, 0.0f, 0.0f, 1.0f);} + // Return the black color + static Color black() { return Color(0.0f, 0.0f, 0.0f, 1.0f);} - // Return the white color - static Color white() { return Color(1.0f, 1.0f, 1.0f, 1.0f);} + // Return the white color + static Color white() { return Color(1.0f, 1.0f, 1.0f, 1.0f);} - // Return the red color - static Color red() { return Color(1.0f, 0.0f, 0.0f, 1.0f);} + // Return the red color + static Color red() { return Color(1.0f, 0.0f, 0.0f, 1.0f);} - // Return the green color - static Color green() { return Color(0.0f, 1.0f, 0.0f, 1.0f);} + // Return the green color + static Color green() { return Color(0.0f, 1.0f, 0.0f, 1.0f);} - // Return the blue color - static Color blue() { return Color(0.0f, 0.0f, 1.0f, 1.0f);} + // Return the blue color + static Color blue() { return Color(0.0f, 0.0f, 1.0f, 1.0f);} - // = operator - Color& operator=(const Color& color) { - if (&color != this) { - r = color.r; - g = color.g; - b = color.b; - a = color.a; - } - return *this; - } + // = operator + Color& operator=(const Color& color) { + if (&color != this) { + r = color.r; + g = color.g; + b = color.b; + a = color.a; + } + return *this; + } }; } diff --git a/tools/testbed/opengl-framework/src/maths/Matrix3.h b/tools/testbed/opengl-framework/src/maths/Matrix3.h index e97769c..1d14f99 100644 --- a/tools/testbed/opengl-framework/src/maths/Matrix3.h +++ b/tools/testbed/opengl-framework/src/maths/Matrix3.h @@ -1,26 +1,26 @@ /******************************************************************************** -* OpenGL-Framework * -* Copyright (c) 2013 Daniel Chappuis * +* OpenGL-Framework * +* Copyright (c) 2013 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: * -* * +* 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. * -* * +* 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 MATRIX3_H @@ -37,248 +37,248 @@ namespace openglframework { // This class represents a 4x4 matrix class Matrix3 { - private : + private : - // -------------------- Attributes -------------------- // + // -------------------- Attributes -------------------- // - // Elements of the matrix - float m[3][3]; + // Elements of the matrix + float m[3][3]; - public : + public : - // Constructor - Matrix3() { - setToNull(); - } + // Constructor + Matrix3() { + setToNull(); + } - // Constructor - Matrix3(float a1, float a2, - float a3, float b1, float b2, float b3, - float c1, float c2, float c3) { - setAllValues(a1, a2, a3, b1, b2, b3, c1, c2, c3); - } + // Constructor + Matrix3(float a1, float a2, + float a3, float b1, float b2, float b3, + float c1, float c2, float c3) { + setAllValues(a1, a2, a3, b1, b2, b3, c1, c2, c3); + } - // Constructor - Matrix3(float n[3][3]) { - m[0][0]=n[0][0]; m[0][1]=n[0][1]; m[0][2]=n[0][2]; - m[1][0]=n[1][0]; m[1][1]=n[1][1]; m[1][2]=n[1][2]; - m[2][0]=n[2][0]; m[2][1]=n[2][1]; m[2][2]=n[2][2]; - } + // Constructor + Matrix3(float n[3][3]) { + m[0][0]=n[0][0]; m[0][1]=n[0][1]; m[0][2]=n[0][2]; + m[1][0]=n[1][0]; m[1][1]=n[1][1]; m[1][2]=n[1][2]; + m[2][0]=n[2][0]; m[2][1]=n[2][1]; m[2][2]=n[2][2]; + } - // Constructor - Matrix3(const Vector3& a1, const Vector3& a2, const Vector3& a3) { - m[0][0] = a1.x; m[0][1] = a2.x; m[0][2] = a3.x; - m[1][0] = a1.y; m[1][1] = a2.y; m[1][2] = a3.y; - m[2][0] = a1.z; m[2][1] = a2.z; m[2][2] = a3.z; - } + // Constructor + Matrix3(const Vector3& a1, const Vector3& a2, const Vector3& a3) { + m[0][0] = a1.x; m[0][1] = a2.x; m[0][2] = a3.x; + m[1][0] = a1.y; m[1][1] = a2.y; m[1][2] = a3.y; + m[2][0] = a1.z; m[2][1] = a2.z; m[2][2] = a3.z; + } - // Constructor - Matrix3(const Matrix3& matrix) { - setAllValues(matrix.m[0][0], matrix.m[0][1], matrix.m[0][2], - matrix.m[1][0], matrix.m[1][1], matrix.m[1][2], - matrix.m[2][0], matrix.m[2][1], matrix.m[2][2]); - } + // Constructor + Matrix3(const Matrix3& matrix) { + setAllValues(matrix.m[0][0], matrix.m[0][1], matrix.m[0][2], + matrix.m[1][0], matrix.m[1][1], matrix.m[1][2], + matrix.m[2][0], matrix.m[2][1], matrix.m[2][2]); + } - // Method to get a value in the matrix - float getValue(int i, int j) const { - assert(i>=0 && i<3 && j>=0 && j<3); - return m[i][j]; - } + // Method to get a value in the matrix + float getValue(int32_t i, int32_t j) const { + assert(i>=0 && i<3 && j>=0 && j<3); + return m[i][j]; + } - // Method to set a value in the matrix - void setValue(int i, int j, float value) { - assert(i>=0 && i<3 && j>=0 && j<3); - m[i][j] = value; - } + // Method to set a value in the matrix + void setValue(int32_t i, int32_t j, float value) { + assert(i>=0 && i<3 && j>=0 && j<3); + m[i][j] = value; + } - // Method to set all the values in the matrix - void setAllValues(float a1, float a2, float a3, float b1, float b2, float b3, - float c1, float c2, float c3) { - m[0][0] = a1; m[0][1] = a2; m[0][2] = a3; - m[1][0] = b1; m[1][1] = b2; m[1][2] = b3; - m[2][0] = c1; m[2][1] = c2; m[2][2] = c3; - } + // Method to set all the values in the matrix + void setAllValues(float a1, float a2, float a3, float b1, float b2, float b3, + float c1, float c2, float c3) { + m[0][0] = a1; m[0][1] = a2; m[0][2] = a3; + m[1][0] = b1; m[1][1] = b2; m[1][2] = b3; + m[2][0] = c1; m[2][1] = c2; m[2][2] = c3; + } - // Return a column - Vector3 getColumn(int i) const { - assert(i>= 0 && i<3); - return Vector3(m[0][i], m[1][i], m[2][i]); - } + // Return a column + Vector3 getColumn(int32_t i) const { + assert(i>= 0 && i<3); + return Vector3(m[0][i], m[1][i], m[2][i]); + } - // Return the transpose matrix - Matrix3 getTranspose() const { - // Return the transpose matrix - return Matrix3(m[0][0], m[1][0], m[2][0], - m[0][1], m[1][1], m[2][1], - m[0][2], m[1][2], m[2][2]); - } + // Return the transpose matrix + Matrix3 getTranspose() const { + // Return the transpose matrix + return Matrix3(m[0][0], m[1][0], m[2][0], + m[0][1], m[1][1], m[2][1], + m[0][2], m[1][2], m[2][2]); + } - // Return the determinant of the matrix - float getDeterminant() const { - // Compute and return the determinant of the matrix - return (m[0][0]*(m[1][1]*m[2][2]-m[2][1]*m[1][2]) - m[0][1]*(m[1][0]*m[2][2]-m[2][0]*m[1][2]) + - m[0][2]*(m[1][0]*m[2][1]-m[2][0]*m[1][1])); - } + // Return the determinant of the matrix + float getDeterminant() const { + // Compute and return the determinant of the matrix + return (m[0][0]*(m[1][1]*m[2][2]-m[2][1]*m[1][2]) - m[0][1]*(m[1][0]*m[2][2]-m[2][0]*m[1][2]) + + m[0][2]*(m[1][0]*m[2][1]-m[2][0]*m[1][1])); + } - // Return the trace of the matrix - float getTrace() const { - // Compute and return the trace - return (m[0][0] + m[1][1] + m[2][2]); - } + // Return the trace of the matrix + float getTrace() const { + // Compute and return the trace + return (m[0][0] + m[1][1] + m[2][2]); + } - void setToNull() { - m[0][0] = 0.0; m[0][1] = 0.0; m[0][2] = 0.0; - m[1][0] = 0.0; m[1][1] = 0.0; m[1][2] = 0.0; - m[2][0] = 0.0; m[2][1] = 0.0; m[2][2] = 0.0; - } + void setToNull() { + m[0][0] = 0.0; m[0][1] = 0.0; m[0][2] = 0.0; + m[1][0] = 0.0; m[1][1] = 0.0; m[1][2] = 0.0; + m[2][0] = 0.0; m[2][1] = 0.0; m[2][2] = 0.0; + } - bool isNull() const { - Matrix3 zero; - return *this == zero; - } + bool isNull() const { + Matrix3 zero; + return *this == zero; + } - // Set the matrix to the identity matrix - void setToIdentity() { - m[0][0] = 1.0; m[0][1] = 0.0; m[0][2] = 0.0; - m[1][0] = 0.0; m[1][1] = 1.0; m[1][2] = 0.0; - m[2][0] = 0.0; m[2][1] = 0.0; m[2][2] = 1.0; - } + // Set the matrix to the identity matrix + void setToIdentity() { + m[0][0] = 1.0; m[0][1] = 0.0; m[0][2] = 0.0; + m[1][0] = 0.0; m[1][1] = 1.0; m[1][2] = 0.0; + m[2][0] = 0.0; m[2][1] = 0.0; m[2][2] = 1.0; + } - bool isIdentity() const { - Matrix3 I; - I.setToIdentity(); - return ( *this == I ); - } + bool isIdentity() const { + Matrix3 I; + I.setToIdentity(); + return ( *this == I ); + } - // Return the inverse matrix - Matrix3 getInverse() const { + // Return the inverse matrix + Matrix3 getInverse() const { - // Compute the determinant of the matrix - float determinant = getDeterminant(); + // Compute the determinant of the matrix + float determinant = getDeterminant(); - // Check if the determinant is equal to zero - assert(determinant > std::numeric_limits::epsilon()); + // Check if the determinant is equal to zero + assert(determinant > std::numeric_limits::epsilon()); - float invDeterminant = 1.0f / determinant; - Matrix3 tempMatrix((m[1][1]*m[2][2]-m[2][1]*m[1][2]), -(m[0][1]*m[2][2]-m[2][1]*m[0][2]), (m[0][1]*m[1][2]-m[0][2]*m[1][1]), - -(m[1][0]*m[2][2]-m[2][0]*m[1][2]), (m[0][0]*m[2][2]-m[2][0]*m[0][2]), -(m[0][0]*m[1][2]-m[1][0]*m[0][2]), - (m[1][0]*m[2][1]-m[2][0]*m[1][1]), -(m[0][0]*m[2][1]-m[2][0]*m[0][1]), (m[0][0]*m[1][1]-m[0][1]*m[1][0])); + float invDeterminant = 1.0f / determinant; + Matrix3 tempMatrix((m[1][1]*m[2][2]-m[2][1]*m[1][2]), -(m[0][1]*m[2][2]-m[2][1]*m[0][2]), (m[0][1]*m[1][2]-m[0][2]*m[1][1]), + -(m[1][0]*m[2][2]-m[2][0]*m[1][2]), (m[0][0]*m[2][2]-m[2][0]*m[0][2]), -(m[0][0]*m[1][2]-m[1][0]*m[0][2]), + (m[1][0]*m[2][1]-m[2][0]*m[1][1]), -(m[0][0]*m[2][1]-m[2][0]*m[0][1]), (m[0][0]*m[1][1]-m[0][1]*m[1][0])); - // Return the inverse matrix - return (tempMatrix * invDeterminant); - } + // Return the inverse matrix + return (tempMatrix * invDeterminant); + } - // Display the matrix - void print() const { - for (int i=0; i<3; i++) { - for (int j=0; j<3; j++) { - std::cout << m[i][j] << " "; - } - std::cout << std::endl; - } - } + // Display the matrix + void print32_t() const { + for (int32_t i=0; i<3; i++) { + for (int32_t j=0; j<3; j++) { + std::cout << m[i][j] << " "; + } + std::cout << std::endl; + } + } - // Overloaded operator = - Matrix3& operator=(const Matrix3& matrix) { - if (&matrix != this) { - setAllValues(matrix.m[0][0], matrix.m[0][1], matrix.m[0][2], - matrix.m[1][0], matrix.m[1][1], matrix.m[1][2], - matrix.m[2][0], matrix.m[2][1], matrix.m[2][2]); - } - return *this; - } + // Overloaded operator = + Matrix3& operator=(const Matrix3& matrix) { + if (&matrix != this) { + setAllValues(matrix.m[0][0], matrix.m[0][1], matrix.m[0][2], + matrix.m[1][0], matrix.m[1][1], matrix.m[1][2], + matrix.m[2][0], matrix.m[2][1], matrix.m[2][2]); + } + return *this; + } - // Overloaded operator for addition - Matrix3 operator+(const Matrix3& matrix2) { - return Matrix3(m[0][0] + matrix2.m[0][0], m[0][1] + matrix2.m[0][1], m[0][2] + matrix2.m[0][2], - m[1][0] + matrix2.m[1][0], m[1][1] + matrix2.m[1][1], m[1][2] + matrix2.m[1][2], - m[2][0] + matrix2.m[2][0], m[2][1] + matrix2.m[2][1], m[2][2] + matrix2.m[2][2]); - } + // Overloaded operator for addition + Matrix3 operator+(const Matrix3& matrix2) { + return Matrix3(m[0][0] + matrix2.m[0][0], m[0][1] + matrix2.m[0][1], m[0][2] + matrix2.m[0][2], + m[1][0] + matrix2.m[1][0], m[1][1] + matrix2.m[1][1], m[1][2] + matrix2.m[1][2], + m[2][0] + matrix2.m[2][0], m[2][1] + matrix2.m[2][1], m[2][2] + matrix2.m[2][2]); + } - // Overloaded operator for substraction - Matrix3 operator-(const Matrix3& matrix2) { - return Matrix3(m[0][0] - matrix2.m[0][0], m[0][1] - matrix2.m[0][1], m[0][2] - matrix2.m[0][2], - m[1][0] - matrix2.m[1][0], m[1][1] - matrix2.m[1][1], m[1][2] - matrix2.m[1][2], - m[2][0] - matrix2.m[2][0], m[2][1] - matrix2.m[2][1], m[2][2] - matrix2.m[2][2]); - } + // Overloaded operator for substraction + Matrix3 operator-(const Matrix3& matrix2) { + return Matrix3(m[0][0] - matrix2.m[0][0], m[0][1] - matrix2.m[0][1], m[0][2] - matrix2.m[0][2], + m[1][0] - matrix2.m[1][0], m[1][1] - matrix2.m[1][1], m[1][2] - matrix2.m[1][2], + m[2][0] - matrix2.m[2][0], m[2][1] - matrix2.m[2][1], m[2][2] - matrix2.m[2][2]); + } - // Overloaded operator for the negative of the matrix - Matrix3 operator-() { - return Matrix3(-m[0][0], -m[0][1], -m[0][2], - -m[1][0], -m[1][1], -m[1][2], - -m[2][0], -m[2][1], -m[2][2]); - } + // Overloaded operator for the negative of the matrix + Matrix3 operator-() { + return Matrix3(-m[0][0], -m[0][1], -m[0][2], + -m[1][0], -m[1][1], -m[1][2], + -m[2][0], -m[2][1], -m[2][2]); + } - // Overloaded operator for multiplication with a number - Matrix3 operator*(float nb) { - return Matrix3(m[0][0] * nb, m[0][1] * nb, m[0][2] * nb, - m[1][0] * nb, m[1][1] * nb, m[1][2] * nb, - m[2][0] * nb, m[2][1] * nb, m[2][2] * nb); - } + // Overloaded operator for multiplication with a number + Matrix3 operator*(float nb) { + return Matrix3(m[0][0] * nb, m[0][1] * nb, m[0][2] * nb, + m[1][0] * nb, m[1][1] * nb, m[1][2] * nb, + m[2][0] * nb, m[2][1] * nb, m[2][2] * nb); + } - // Overloaded operator for matrix multiplication - Matrix3 operator*(const Matrix3& matrix2) { - return Matrix3(m[0][0]*matrix2.m[0][0] + m[0][1]*matrix2.m[1][0] + m[0][2]*matrix2.m[2][0], - m[0][0]*matrix2.m[0][1] + m[0][1]*matrix2.m[1][1] + m[0][2]*matrix2.m[2][1], - m[0][0]*matrix2.m[0][2] + m[0][1]*matrix2.m[1][2] + m[0][2]*matrix2.m[2][2], - m[1][0]*matrix2.m[0][0] + m[1][1]*matrix2.m[1][0] + m[1][2]*matrix2.m[2][0], - m[1][0]*matrix2.m[0][1] + m[1][1]*matrix2.m[1][1] + m[1][2]*matrix2.m[2][1], - m[1][0]*matrix2.m[0][2] + m[1][1]*matrix2.m[1][2] + m[1][2]*matrix2.m[2][2], - m[2][0]*matrix2.m[0][0] + m[2][1]*matrix2.m[1][0] + m[2][2]*matrix2.m[2][0], - m[2][0]*matrix2.m[0][1] + m[2][1]*matrix2.m[1][1] + m[2][2]*matrix2.m[2][1], - m[2][0]*matrix2.m[0][2] + m[2][1]*matrix2.m[1][2] + m[2][2]*matrix2.m[2][2]); - } + // Overloaded operator for matrix multiplication + Matrix3 operator*(const Matrix3& matrix2) { + return Matrix3(m[0][0]*matrix2.m[0][0] + m[0][1]*matrix2.m[1][0] + m[0][2]*matrix2.m[2][0], + m[0][0]*matrix2.m[0][1] + m[0][1]*matrix2.m[1][1] + m[0][2]*matrix2.m[2][1], + m[0][0]*matrix2.m[0][2] + m[0][1]*matrix2.m[1][2] + m[0][2]*matrix2.m[2][2], + m[1][0]*matrix2.m[0][0] + m[1][1]*matrix2.m[1][0] + m[1][2]*matrix2.m[2][0], + m[1][0]*matrix2.m[0][1] + m[1][1]*matrix2.m[1][1] + m[1][2]*matrix2.m[2][1], + m[1][0]*matrix2.m[0][2] + m[1][1]*matrix2.m[1][2] + m[1][2]*matrix2.m[2][2], + m[2][0]*matrix2.m[0][0] + m[2][1]*matrix2.m[1][0] + m[2][2]*matrix2.m[2][0], + m[2][0]*matrix2.m[0][1] + m[2][1]*matrix2.m[1][1] + m[2][2]*matrix2.m[2][1], + m[2][0]*matrix2.m[0][2] + m[2][1]*matrix2.m[1][2] + m[2][2]*matrix2.m[2][2]); + } - // Overloaded operator for multiplication with a vector - Vector3 operator*(const Vector3& vector) { - return Vector3(m[0][0]*vector.x + m[0][1]*vector.y + m[0][2]*vector.z, - m[1][0]*vector.x + m[1][1]*vector.y + m[1][2]*vector.z, - m[2][0]*vector.x + m[2][1]*vector.y + m[2][2]*vector.z); - } + // Overloaded operator for multiplication with a vector + Vector3 operator*(const Vector3& vector) { + return Vector3(m[0][0]*vector.x + m[0][1]*vector.y + m[0][2]*vector.z, + m[1][0]*vector.x + m[1][1]*vector.y + m[1][2]*vector.z, + m[2][0]*vector.x + m[2][1]*vector.y + m[2][2]*vector.z); + } - // Overloaded operator for equality condition - bool operator==(const Matrix3& matrix) const { - return (m[0][0] == matrix.m[0][0] && m[0][1] == matrix.m[0][1] && m[0][2] == matrix.m[0][2] && - m[1][0] == matrix.m[1][0] && m[1][1] == matrix.m[1][1] && m[1][2] == matrix.m[1][2] && - m[2][0] == matrix.m[2][0] && m[2][1] == matrix.m[2][1] && m[2][2] == matrix.m[2][2]); - } + // Overloaded operator for equality condition + bool operator==(const Matrix3& matrix) const { + return (m[0][0] == matrix.m[0][0] && m[0][1] == matrix.m[0][1] && m[0][2] == matrix.m[0][2] && + m[1][0] == matrix.m[1][0] && m[1][1] == matrix.m[1][1] && m[1][2] == matrix.m[1][2] && + m[2][0] == matrix.m[2][0] && m[2][1] == matrix.m[2][1] && m[2][2] == matrix.m[2][2]); + } - // Overloaded operator for the is different condition - bool operator!= (const Matrix3& matrix) const { - return !(*this == matrix); - } + // Overloaded operator for the is different condition + bool operator!= (const Matrix3& matrix) const { + return !(*this == matrix); + } - // Overloaded operator for addition with assignment - Matrix3& operator+=(const Matrix3& matrix) { - m[0][0] += matrix.m[0][0]; m[0][1] += matrix.m[0][1]; m[0][2] += matrix.m[0][2]; - m[1][0] += matrix.m[1][0]; m[1][1] += matrix.m[1][1]; m[1][2] += matrix.m[1][2]; - m[2][0] += matrix.m[2][0]; m[2][1] += matrix.m[2][1]; m[2][2] += matrix.m[2][2]; - return *this; - } + // Overloaded operator for addition with assignment + Matrix3& operator+=(const Matrix3& matrix) { + m[0][0] += matrix.m[0][0]; m[0][1] += matrix.m[0][1]; m[0][2] += matrix.m[0][2]; + m[1][0] += matrix.m[1][0]; m[1][1] += matrix.m[1][1]; m[1][2] += matrix.m[1][2]; + m[2][0] += matrix.m[2][0]; m[2][1] += matrix.m[2][1]; m[2][2] += matrix.m[2][2]; + return *this; + } - // Overloaded operator for substraction with assignment - Matrix3& operator-=(const Matrix3& matrix) { - m[0][0] -= matrix.m[0][0]; m[0][1] -= matrix.m[0][1]; m[0][2] -= matrix.m[0][2]; - m[1][0] -= matrix.m[1][0]; m[1][1] -= matrix.m[1][1]; m[1][2] -= matrix.m[1][2]; - m[2][0] -= matrix.m[2][0]; m[2][1] -= matrix.m[2][1]; m[2][2] -= matrix.m[2][2]; - return *this; - } + // Overloaded operator for substraction with assignment + Matrix3& operator-=(const Matrix3& matrix) { + m[0][0] -= matrix.m[0][0]; m[0][1] -= matrix.m[0][1]; m[0][2] -= matrix.m[0][2]; + m[1][0] -= matrix.m[1][0]; m[1][1] -= matrix.m[1][1]; m[1][2] -= matrix.m[1][2]; + m[2][0] -= matrix.m[2][0]; m[2][1] -= matrix.m[2][1]; m[2][2] -= matrix.m[2][2]; + return *this; + } - // Overloaded operator for multiplication with a number with assignment - Matrix3& operator*=(float nb) { - m[0][0] *= nb; m[0][1] *= nb; m[0][2] *= nb; - m[1][0] *= nb; m[1][1] *= nb; m[1][2] *= nb; - m[2][0] *= nb; m[2][1] *= nb; m[2][2] *= nb; - return *this; - } + // Overloaded operator for multiplication with a number with assignment + Matrix3& operator*=(float nb) { + m[0][0] *= nb; m[0][1] *= nb; m[0][2] *= nb; + m[1][0] *= nb; m[1][1] *= nb; m[1][2] *= nb; + m[2][0] *= nb; m[2][1] *= nb; m[2][2] *= nb; + return *this; + } - // Return the identity matrix - static Matrix3 identity() { - return Matrix3(1, 0, 0, - 0, 1, 0, - 0, 0, 1); - } + // Return the identity matrix + static Matrix3 identity() { + return Matrix3(1, 0, 0, + 0, 1, 0, + 0, 0, 1); + } }; } diff --git a/tools/testbed/opengl-framework/src/maths/Matrix4.h b/tools/testbed/opengl-framework/src/maths/Matrix4.h index 28771ea..fb65afa 100644 --- a/tools/testbed/opengl-framework/src/maths/Matrix4.h +++ b/tools/testbed/opengl-framework/src/maths/Matrix4.h @@ -1,26 +1,26 @@ /******************************************************************************** -* OpenGL-Framework * -* Copyright (c) 2013 Daniel Chappuis * +* OpenGL-Framework * +* Copyright (c) 2013 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: * -* * +* 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. * -* * +* 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 MATRIX4_H @@ -41,354 +41,354 @@ namespace openglframework { // This class represents a 4x4 matrix class Matrix4 { - public: + public: - // -------------------- Attributes -------------------- // + // -------------------- Attributes -------------------- // - // Elements of the matrix - float m[4][4]; + // Elements of the matrix + float m[4][4]; - // -------------------- Methods -------------------- // + // -------------------- Methods -------------------- // - // Constructor - Matrix4(float m_00=0, float m_01=0, float m_02=0, float m_03=0, - float m_10=0, float m_11=0, float m_12=0, float m_13=0, - float m_20=0, float m_21=0, float m_22=0, float m_23=0, - float m_30=0, float m_31=0, float m_32=0, float m_33=0) { - m[0][0] = m_00; m[0][1] = m_01; m[0][2] = m_02; m[0][3] = m_03; - m[1][0] = m_10; m[1][1] = m_11; m[1][2] = m_12; m[1][3] = m_13; - m[2][0] = m_20; m[2][1] = m_21; m[2][2] = m_22; m[2][3] = m_23; - m[3][0] = m_30; m[3][1] = m_31; m[3][2] = m_32; m[3][3] = m_33; - } + // Constructor + Matrix4(float m_00=0, float m_01=0, float m_02=0, float m_03=0, + float m_10=0, float m_11=0, float m_12=0, float m_13=0, + float m_20=0, float m_21=0, float m_22=0, float m_23=0, + float m_30=0, float m_31=0, float m_32=0, float m_33=0) { + m[0][0] = m_00; m[0][1] = m_01; m[0][2] = m_02; m[0][3] = m_03; + m[1][0] = m_10; m[1][1] = m_11; m[1][2] = m_12; m[1][3] = m_13; + m[2][0] = m_20; m[2][1] = m_21; m[2][2] = m_22; m[2][3] = m_23; + m[3][0] = m_30; m[3][1] = m_31; m[3][2] = m_32; m[3][3] = m_33; + } - // Constructor - Matrix4(float n[4][4]) { - m[0][0]=n[0][0]; m[0][1]=n[0][1]; m[0][2]=n[0][2]; m[0][3]=n[0][3]; - m[1][0]=n[1][0]; m[1][1]=n[1][1]; m[1][2]=n[1][2]; m[1][3]=n[1][3]; - m[2][0]=n[2][0]; m[2][1]=n[2][1]; m[2][2]=n[2][2]; m[2][3]=n[2][3]; - m[3][0]=n[3][0]; m[3][1]=n[3][1]; m[3][2]=n[3][2]; m[3][3]=n[3][3]; - } + // Constructor + Matrix4(float n[4][4]) { + m[0][0]=n[0][0]; m[0][1]=n[0][1]; m[0][2]=n[0][2]; m[0][3]=n[0][3]; + m[1][0]=n[1][0]; m[1][1]=n[1][1]; m[1][2]=n[1][2]; m[1][3]=n[1][3]; + m[2][0]=n[2][0]; m[2][1]=n[2][1]; m[2][2]=n[2][2]; m[2][3]=n[2][3]; + m[3][0]=n[3][0]; m[3][1]=n[3][1]; m[3][2]=n[3][2]; m[3][3]=n[3][3]; + } - // Constructor - Matrix4(const Vector3& a1, const Vector3& a2, const Vector3& a3) { - m[0][0] = a1.x; m[0][1] = a2.x; m[0][2] = a3.x; m[0][3] = 0.f; - m[1][0] = a1.y; m[1][1] = a2.y; m[1][2] = a3.y; m[1][3] = 0.f; - m[2][0] = a1.z; m[2][1] = a2.z; m[2][2] = a3.z; m[2][3] = 0.f; - m[3][0] = 0.f; m[3][1] = 0.f; m[3][2] = 0.f; m[3][3] = 1.f; - } + // Constructor + Matrix4(const Vector3& a1, const Vector3& a2, const Vector3& a3) { + m[0][0] = a1.x; m[0][1] = a2.x; m[0][2] = a3.x; m[0][3] = 0.f; + m[1][0] = a1.y; m[1][1] = a2.y; m[1][2] = a3.y; m[1][3] = 0.f; + m[2][0] = a1.z; m[2][1] = a2.z; m[2][2] = a3.z; m[2][3] = 0.f; + m[3][0] = 0.f; m[3][1] = 0.f; m[3][2] = 0.f; m[3][3] = 1.f; + } - // Constructor - Matrix4(const Vector4& a1, const Vector4& a2, const Vector4& a3) { - m[0][0] = a1.x; m[0][1] = a2.x; m[0][2] = a3.x; m[0][3] = 0.f; - m[1][0] = a1.y; m[1][1] = a2.y; m[1][2] = a3.y; m[1][3] = 0.f; - m[2][0] = a1.z; m[2][1] = a2.z; m[2][2] = a3.z; m[2][3] = 0.f; - m[3][0] = a1.w; m[3][1] = a2.w; m[3][2] = a3.w; m[3][3] = 1.f; - } + // Constructor + Matrix4(const Vector4& a1, const Vector4& a2, const Vector4& a3) { + m[0][0] = a1.x; m[0][1] = a2.x; m[0][2] = a3.x; m[0][3] = 0.f; + m[1][0] = a1.y; m[1][1] = a2.y; m[1][2] = a3.y; m[1][3] = 0.f; + m[2][0] = a1.z; m[2][1] = a2.z; m[2][2] = a3.z; m[2][3] = 0.f; + m[3][0] = a1.w; m[3][1] = a2.w; m[3][2] = a3.w; m[3][3] = 1.f; + } - // Constructor - Matrix4(const Matrix4& matrix) { + // Constructor + Matrix4(const Matrix4& matrix) { - setAllValues(matrix.m[0][0], matrix.m[0][1], matrix.m[0][2], matrix.m[0][3], - matrix.m[1][0], matrix.m[1][1], matrix.m[1][2], matrix.m[1][3], - matrix.m[2][0], matrix.m[2][1], matrix.m[2][2], matrix.m[2][3], - matrix.m[3][0], matrix.m[3][1], matrix.m[3][2], matrix.m[3][3]); - } + setAllValues(matrix.m[0][0], matrix.m[0][1], matrix.m[0][2], matrix.m[0][3], + matrix.m[1][0], matrix.m[1][1], matrix.m[1][2], matrix.m[1][3], + matrix.m[2][0], matrix.m[2][1], matrix.m[2][2], matrix.m[2][3], + matrix.m[3][0], matrix.m[3][1], matrix.m[3][2], matrix.m[3][3]); + } - // + operator - Matrix4 operator+(const Matrix4 &n) const { - return Matrix4(m[0][0]+n.m[0][0], m[0][1]+n.m[0][1], m[0][2]+n.m[0][2], m[0][3]+n.m[0][3], - m[1][0]+n.m[1][0], m[1][1]+n.m[1][1], m[1][2]+n.m[1][2], m[1][3]+n.m[1][3], - m[2][0]+n.m[2][0], m[2][1]+n.m[2][1], m[2][2]+n.m[2][2], m[2][3]+n.m[2][3], - m[3][0]+n.m[3][0], m[3][1]+n.m[3][1], m[3][2]+n.m[3][2], m[3][3]+n.m[3][3]); - } + // + operator + Matrix4 operator+(const Matrix4 &n) const { + return Matrix4(m[0][0]+n.m[0][0], m[0][1]+n.m[0][1], m[0][2]+n.m[0][2], m[0][3]+n.m[0][3], + m[1][0]+n.m[1][0], m[1][1]+n.m[1][1], m[1][2]+n.m[1][2], m[1][3]+n.m[1][3], + m[2][0]+n.m[2][0], m[2][1]+n.m[2][1], m[2][2]+n.m[2][2], m[2][3]+n.m[2][3], + m[3][0]+n.m[3][0], m[3][1]+n.m[3][1], m[3][2]+n.m[3][2], m[3][3]+n.m[3][3]); + } - // += operator - Matrix4& operator+=(const Matrix4 &n) { - m[0][0]+=n.m[0][0]; m[0][1]+=n.m[0][1]; m[0][2]+=n.m[0][2]; m[0][3]+=n.m[0][3]; - m[1][0]+=n.m[1][0]; m[1][1]+=n.m[1][1]; m[1][2]+=n.m[1][2]; m[1][3]+=n.m[1][3]; - m[2][0]+=n.m[2][0]; m[2][1]+=n.m[2][1]; m[2][2]+=n.m[2][2]; m[2][3]+=n.m[2][3]; - m[3][0]+=n.m[3][0]; m[3][1]+=n.m[3][1]; m[3][2]+=n.m[3][2]; m[3][3]+=n.m[3][3]; - return *this; - } + // += operator + Matrix4& operator+=(const Matrix4 &n) { + m[0][0]+=n.m[0][0]; m[0][1]+=n.m[0][1]; m[0][2]+=n.m[0][2]; m[0][3]+=n.m[0][3]; + m[1][0]+=n.m[1][0]; m[1][1]+=n.m[1][1]; m[1][2]+=n.m[1][2]; m[1][3]+=n.m[1][3]; + m[2][0]+=n.m[2][0]; m[2][1]+=n.m[2][1]; m[2][2]+=n.m[2][2]; m[2][3]+=n.m[2][3]; + m[3][0]+=n.m[3][0]; m[3][1]+=n.m[3][1]; m[3][2]+=n.m[3][2]; m[3][3]+=n.m[3][3]; + return *this; + } - // - operator - Matrix4 operator-(const Matrix4 &n) const { - return Matrix4(m[0][0]-n.m[0][0], m[0][1]-n.m[0][1], m[0][2]-n.m[0][2], m[0][3]-n.m[0][3], - m[1][0]-n.m[1][0], m[1][1]-n.m[1][1], m[1][2]-n.m[1][2], m[1][3]-n.m[1][3], - m[2][0]-n.m[2][0], m[2][1]-n.m[2][1], m[2][2]-n.m[2][2], m[2][3]-n.m[2][3], - m[3][0]-n.m[3][0], m[3][1]-n.m[3][1], m[3][2]-n.m[3][2], m[3][3]-n.m[3][3]); - } + // - operator + Matrix4 operator-(const Matrix4 &n) const { + return Matrix4(m[0][0]-n.m[0][0], m[0][1]-n.m[0][1], m[0][2]-n.m[0][2], m[0][3]-n.m[0][3], + m[1][0]-n.m[1][0], m[1][1]-n.m[1][1], m[1][2]-n.m[1][2], m[1][3]-n.m[1][3], + m[2][0]-n.m[2][0], m[2][1]-n.m[2][1], m[2][2]-n.m[2][2], m[2][3]-n.m[2][3], + m[3][0]-n.m[3][0], m[3][1]-n.m[3][1], m[3][2]-n.m[3][2], m[3][3]-n.m[3][3]); + } - // -= operator - Matrix4& operator-=(const Matrix4 &n) { - m[0][0]-=n.m[0][0]; m[0][1]-=n.m[0][1]; m[0][2]-=n.m[0][2]; m[0][3]-=n.m[0][3]; - m[1][0]-=n.m[1][0]; m[1][1]-=n.m[1][1]; m[1][2]-=n.m[1][2]; m[1][3]-=n.m[1][3]; - m[2][0]-=n.m[2][0]; m[2][1]-=n.m[2][1]; m[2][2]-=n.m[2][2]; m[2][3]-=n.m[2][3]; - m[3][0]-=n.m[3][0]; m[3][1]-=n.m[3][1]; m[3][2]-=n.m[3][2]; m[3][3]-=n.m[3][3]; - return *this; - } + // -= operator + Matrix4& operator-=(const Matrix4 &n) { + m[0][0]-=n.m[0][0]; m[0][1]-=n.m[0][1]; m[0][2]-=n.m[0][2]; m[0][3]-=n.m[0][3]; + m[1][0]-=n.m[1][0]; m[1][1]-=n.m[1][1]; m[1][2]-=n.m[1][2]; m[1][3]-=n.m[1][3]; + m[2][0]-=n.m[2][0]; m[2][1]-=n.m[2][1]; m[2][2]-=n.m[2][2]; m[2][3]-=n.m[2][3]; + m[3][0]-=n.m[3][0]; m[3][1]-=n.m[3][1]; m[3][2]-=n.m[3][2]; m[3][3]-=n.m[3][3]; + return *this; + } - // = operator - Matrix4& operator=(const Matrix4& matrix) { - if (&matrix != this) { - setAllValues(matrix.m[0][0], matrix.m[0][1], matrix.m[0][2], matrix.m[0][3], - matrix.m[1][0], matrix.m[1][1], matrix.m[1][2], matrix.m[1][3], - matrix.m[2][0], matrix.m[2][1], matrix.m[2][2], matrix.m[2][3], - matrix.m[3][0], matrix.m[3][1], matrix.m[3][2], matrix.m[3][3]); - } - return *this; - } + // = operator + Matrix4& operator=(const Matrix4& matrix) { + if (&matrix != this) { + setAllValues(matrix.m[0][0], matrix.m[0][1], matrix.m[0][2], matrix.m[0][3], + matrix.m[1][0], matrix.m[1][1], matrix.m[1][2], matrix.m[1][3], + matrix.m[2][0], matrix.m[2][1], matrix.m[2][2], matrix.m[2][3], + matrix.m[3][0], matrix.m[3][1], matrix.m[3][2], matrix.m[3][3]); + } + return *this; + } - // == operator - bool operator==(const Matrix4 &n) const { - return m[0][0]==n.m[0][0] && m[0][1]==n.m[0][1] && m[0][2]==n.m[0][2] && m[0][3]==n.m[0][3] && - m[1][0]==n.m[1][0] && m[1][1]==n.m[1][1] && m[1][2]==n.m[1][2] && m[1][3]==n.m[1][3] && - m[2][0]==n.m[2][0] && m[2][1]==n.m[2][1] && m[2][2]==n.m[2][2] && m[2][3]==n.m[2][3] && - m[3][0]==n.m[3][0] && m[3][1]==n.m[3][1] && m[3][2]==n.m[3][2] && m[3][3]==n.m[3][3]; - } + // == operator + bool operator==(const Matrix4 &n) const { + return m[0][0]==n.m[0][0] && m[0][1]==n.m[0][1] && m[0][2]==n.m[0][2] && m[0][3]==n.m[0][3] && + m[1][0]==n.m[1][0] && m[1][1]==n.m[1][1] && m[1][2]==n.m[1][2] && m[1][3]==n.m[1][3] && + m[2][0]==n.m[2][0] && m[2][1]==n.m[2][1] && m[2][2]==n.m[2][2] && m[2][3]==n.m[2][3] && + m[3][0]==n.m[3][0] && m[3][1]==n.m[3][1] && m[3][2]==n.m[3][2] && m[3][3]==n.m[3][3]; + } - // * operator - Matrix4 operator*(const Matrix4 &n) const { - Matrix4 o; - for(int i = 0; i < 4; i++) { - for(int j = 0; j < 4; j++) { - float v = 0; - for(int k = 0; k < 4; k++) { - v += m[i][k] * n.m[k][j]; - } - o.m[i][j] = v; - } - } - return o; - } + // * operator + Matrix4 operator*(const Matrix4 &n) const { + Matrix4 o; + for(int32_t i = 0; i < 4; i++) { + for(int32_t j = 0; j < 4; j++) { + float v = 0; + for(int32_t k = 0; k < 4; k++) { + v += m[i][k] * n.m[k][j]; + } + o.m[i][j] = v; + } + } + return o; + } - // * operator - Vector3 operator*(const Vector3 &v) const { - Vector3 u =Vector3(m[0][0]*v.x + m[0][1]*v.y + m[0][2]*v.z + m[0][3], - m[1][0]*v.x + m[1][1]*v.y + m[1][2]*v.z + m[1][3], - m[2][0]*v.x + m[2][1]*v.y + m[2][2]*v.z + m[2][3]); - float w = m[3][0]*v.x + m[3][1]*v.y + m[3][2]*v.z + m[3][3]; - return u/w; - } + // * operator + Vector3 operator*(const Vector3 &v) const { + Vector3 u =Vector3(m[0][0]*v.x + m[0][1]*v.y + m[0][2]*v.z + m[0][3], + m[1][0]*v.x + m[1][1]*v.y + m[1][2]*v.z + m[1][3], + m[2][0]*v.x + m[2][1]*v.y + m[2][2]*v.z + m[2][3]); + float w = m[3][0]*v.x + m[3][1]*v.y + m[3][2]*v.z + m[3][3]; + return u/w; + } - // * operator - Vector4 operator*(const Vector4 &v) const { - Vector4 u = Vector4(m[0][0]*v.x + m[0][1]*v.y + m[0][2]*v.z + v.w*m[0][3], - m[1][0]*v.x + m[1][1]*v.y + m[1][2]*v.z + v.w*m[1][3], - m[2][0]*v.x + m[2][1]*v.y + m[2][2]*v.z + v.w*m[2][3], - m[3][0]*v.x + m[3][1]*v.y + m[3][2]*v.z + v.w*m[3][3]); - if(u.w != 0) - return u/u.w; - else - return u; - } + // * operator + Vector4 operator*(const Vector4 &v) const { + Vector4 u = Vector4(m[0][0]*v.x + m[0][1]*v.y + m[0][2]*v.z + v.w*m[0][3], + m[1][0]*v.x + m[1][1]*v.y + m[1][2]*v.z + v.w*m[1][3], + m[2][0]*v.x + m[2][1]*v.y + m[2][2]*v.z + v.w*m[2][3], + m[3][0]*v.x + m[3][1]*v.y + m[3][2]*v.z + v.w*m[3][3]); + if(u.w != 0) + return u/u.w; + else + return u; + } - // * operator - Matrix4 operator*(float f) const { - return Matrix4(m[0][0]*f, m[0][1]*f, m[0][2]*f, m[0][3]*f, - m[1][0]*f, m[1][1]*f, m[1][2]*f, m[1][3]*f, - m[2][0]*f, m[2][1]*f, m[2][2]*f, m[2][3]*f, - m[3][0]*f, m[3][1]*f, m[3][2]*f, m[3][3]*f); - } + // * operator + Matrix4 operator*(float f) const { + return Matrix4(m[0][0]*f, m[0][1]*f, m[0][2]*f, m[0][3]*f, + m[1][0]*f, m[1][1]*f, m[1][2]*f, m[1][3]*f, + m[2][0]*f, m[2][1]*f, m[2][2]*f, m[2][3]*f, + m[3][0]*f, m[3][1]*f, m[3][2]*f, m[3][3]*f); + } - // * operator - Matrix4 &operator*=(float f) { - m[0][0]*=f; m[0][1]*=f; m[0][2]*=f; m[0][3]*=f; - m[1][0]*=f; m[1][1]*=f; m[1][2]*=f; m[1][3]*=f; - m[2][0]*=f; m[2][1]*=f; m[2][2]*=f; m[2][3]*=f; - m[3][0]*=f; m[3][1]*=f; m[3][2]*=f; m[3][3]*=f; - return *this; - } + // * operator + Matrix4 &operator*=(float f) { + m[0][0]*=f; m[0][1]*=f; m[0][2]*=f; m[0][3]*=f; + m[1][0]*=f; m[1][1]*=f; m[1][2]*=f; m[1][3]*=f; + m[2][0]*=f; m[2][1]*=f; m[2][2]*=f; m[2][3]*=f; + m[3][0]*=f; m[3][1]*=f; m[3][2]*=f; m[3][3]*=f; + return *this; + } - // / operator - Matrix4 operator/(float f) const { - assert(f!=0); - return Matrix4(m[0][0]/f, m[0][1]/f, m[0][2]/f, m[0][3]/f, - m[1][0]/f, m[1][1]/f, m[1][2]/f, m[1][3]/f, - m[2][0]/f, m[2][1]/f, m[2][2]/f, m[2][3]/f, - m[3][0]/f, m[3][1]/f, m[3][2]/f, m[3][3]/f); - } + // / operator + Matrix4 operator/(float f) const { + assert(f!=0); + return Matrix4(m[0][0]/f, m[0][1]/f, m[0][2]/f, m[0][3]/f, + m[1][0]/f, m[1][1]/f, m[1][2]/f, m[1][3]/f, + m[2][0]/f, m[2][1]/f, m[2][2]/f, m[2][3]/f, + m[3][0]/f, m[3][1]/f, m[3][2]/f, m[3][3]/f); + } - // /= operator - Matrix4 &operator/=(float f) { - assert(f!=0); - m[0][0]/=f; m[0][1]/=f; m[0][2]/=f; m[0][3]/=f; - m[1][0]/=f; m[1][1]/=f; m[1][2]/=f; m[1][3]/=f; - m[2][0]/=f; m[2][1]/=f; m[2][2]/=f; m[2][3]/=f; - m[3][0]/=f; m[3][1]/=f; m[3][2]/=f; m[3][3]/=f; - return *this; - } + // /= operator + Matrix4 &operator/=(float f) { + assert(f!=0); + m[0][0]/=f; m[0][1]/=f; m[0][2]/=f; m[0][3]/=f; + m[1][0]/=f; m[1][1]/=f; m[1][2]/=f; m[1][3]/=f; + m[2][0]/=f; m[2][1]/=f; m[2][2]/=f; m[2][3]/=f; + m[3][0]/=f; m[3][1]/=f; m[3][2]/=f; m[3][3]/=f; + return *this; + } - // - operator - Matrix4 operator-() const { - return Matrix4(-m[0][0], -m[0][1], -m[0][2], -m[0][3], - -m[1][0], -m[1][1], -m[1][2], -m[1][3], - -m[2][0], -m[2][1], -m[2][2], -m[2][3], - -m[3][0], -m[3][1], -m[3][2], -m[3][3]); - } + // - operator + Matrix4 operator-() const { + return Matrix4(-m[0][0], -m[0][1], -m[0][2], -m[0][3], + -m[1][0], -m[1][1], -m[1][2], -m[1][3], + -m[2][0], -m[2][1], -m[2][2], -m[2][3], + -m[3][0], -m[3][1], -m[3][2], -m[3][3]); + } - // Return the transpose matrix - Matrix4 getTranspose() const { - return Matrix4(m[0][0], m[1][0], m[2][0], m[3][0], - m[0][1], m[1][1], m[2][1], m[3][1], - m[0][2], m[1][2], m[2][2], m[3][2], - m[0][3], m[1][3], m[2][3], m[3][3]); - } + // Return the transpose matrix + Matrix4 getTranspose() const { + return Matrix4(m[0][0], m[1][0], m[2][0], m[3][0], + m[0][1], m[1][1], m[2][1], m[3][1], + m[0][2], m[1][2], m[2][2], m[3][2], + m[0][3], m[1][3], m[2][3], m[3][3]); + } - // Return the 3x3 upper-left matrix - Matrix3 getUpperLeft3x3Matrix() const { - return Matrix3(m[0][0], m[0][1], m[0][2], - m[1][0], m[1][1], m[1][2], - m[2][0], m[2][1], m[2][2]); - } + // Return the 3x3 upper-left matrix + Matrix3 getUpperLeft3x3Matrix() const { + return Matrix3(m[0][0], m[0][1], m[0][2], + m[1][0], m[1][1], m[1][2], + m[2][0], m[2][1], m[2][2]); + } - // Return the inversed matrix - Matrix4 getInverse() const { - int indxc[4], indxr[4]; - int ipiv[4] = { 0, 0, 0, 0 }; - float minv[4][4]; - float temp; + // Return the inversed matrix + Matrix4 getInverse() const { + int32_t indxc[4], indxr[4]; + int32_t ipiv[4] = { 0, 0, 0, 0 }; + float minv[4][4]; + float temp; - for (int s=0; s<4; s++) { - for (int t=0; t<4; t++) { - minv[s][t] = m[s][t]; - } - } + for (int32_t s=0; s<4; s++) { + for (int32_t t=0; t<4; t++) { + minv[s][t] = m[s][t]; + } + } - for (int i = 0; i < 4; i++) { - int irow = -1, icol = -1; - float big = 0.; - // Choose pivot - for (int j = 0; j < 4; j++) { - if (ipiv[j] != 1) { - for (int k = 0; k < 4; k++) { - if (ipiv[k] == 0) { - if (fabs(minv[j][k]) >= big) { - big = float(fabs(minv[j][k])); - irow = j; - icol = k; - } - } - else if (ipiv[k] > 1) { - std::cout << "ERROR: Singular matrix in MatrixInvert\n"; - } - } - } - } - ++ipiv[icol]; - // Swap rows _irow_ and _icol_ for pivot - if (irow != icol) { - for (int k = 0; k < 4; ++k){ - temp = minv[irow][k]; - minv[irow][k] = minv[icol][k]; - minv[icol][k] = temp; - } - } - indxr[i] = irow; - indxc[i] = icol; - if (minv[icol][icol] == 0.){ - std::cout << "Singular matrix in MatrixInvert\n"; - } - // Set $m[icol][icol]$ to one by scaling row _icol_ appropriately - float pivinv = 1.f / minv[icol][icol]; - minv[icol][icol] = 1.f; - for (int j = 0; j < 4; j++) { - minv[icol][j] *= pivinv; - } + for (int32_t i = 0; i < 4; i++) { + int32_t irow = -1, icol = -1; + float big = 0.; + // Choose pivot + for (int32_t j = 0; j < 4; j++) { + if (ipiv[j] != 1) { + for (int32_t k = 0; k < 4; k++) { + if (ipiv[k] == 0) { + if (fabs(minv[j][k]) >= big) { + big = float(fabs(minv[j][k])); + irow = j; + icol = k; + } + } + else if (ipiv[k] > 1) { + std::cout << "ERROR: Singular matrix in MatrixInvert\n"; + } + } + } + } + ++ipiv[icol]; + // Swap rows _irow_ and _icol_ for pivot + if (irow != icol) { + for (int32_t k = 0; k < 4; ++k){ + temp = minv[irow][k]; + minv[irow][k] = minv[icol][k]; + minv[icol][k] = temp; + } + } + indxr[i] = irow; + indxc[i] = icol; + if (minv[icol][icol] == 0.){ + std::cout << "Singular matrix in MatrixInvert\n"; + } + // Set $m[icol][icol]$ to one by scaling row _icol_ appropriately + float pivinv = 1.f / minv[icol][icol]; + minv[icol][icol] = 1.f; + for (int32_t j = 0; j < 4; j++) { + minv[icol][j] *= pivinv; + } - // Subtract this row from others to zero out their columns - for (int j = 0; j < 4; j++) { - if (j != icol) { - float save = minv[j][icol]; - minv[j][icol] = 0; - for (int k = 0; k < 4; k++) { - minv[j][k] -= minv[icol][k]*save; - } - } - } - } - // Swap columns to reflect permutation - for (int j = 3; j >= 0; j--) { - if (indxr[j] != indxc[j]) { - for (int k = 0; k < 4; k++){ - temp = minv[k][indxr[j]]; - minv[k][indxr[j]] = minv[k][indxc[j]]; - minv[k][indxc[j]] = temp; - } - } - } - return Matrix4(minv); - } + // Subtract this row from others to zero out their columns + for (int32_t j = 0; j < 4; j++) { + if (j != icol) { + float save = minv[j][icol]; + minv[j][icol] = 0; + for (int32_t k = 0; k < 4; k++) { + minv[j][k] -= minv[icol][k]*save; + } + } + } + } + // Swap columns to reflect permutation + for (int32_t j = 3; j >= 0; j--) { + if (indxr[j] != indxc[j]) { + for (int32_t k = 0; k < 4; k++){ + temp = minv[k][indxr[j]]; + minv[k][indxr[j]] = minv[k][indxc[j]]; + minv[k][indxc[j]] = temp; + } + } + } + return Matrix4(minv); + } - // Method to set all the values in the matrix - void setAllValues(float a1, float a2, float a3, float a4, - float b1, float b2, float b3, float b4, - float c1, float c2, float c3, float c4, - float d1, float d2, float d3, float d4) { - m[0][0] = a1; m[0][1] = a2; m[0][2] = a3, m[0][3] = a4; - m[1][0] = b1; m[1][1] = b2; m[1][2] = b3; m[1][3] = b4; - m[2][0] = c1; m[2][1] = c2; m[2][2] = c3; m[2][3] = c4; - m[3][0] = d1; m[3][1] = d2; m[3][2] = d3; m[3][3] = d4; - } + // Method to set all the values in the matrix + void setAllValues(float a1, float a2, float a3, float a4, + float b1, float b2, float b3, float b4, + float c1, float c2, float c3, float c4, + float d1, float d2, float d3, float d4) { + m[0][0] = a1; m[0][1] = a2; m[0][2] = a3, m[0][3] = a4; + m[1][0] = b1; m[1][1] = b2; m[1][2] = b3; m[1][3] = b4; + m[2][0] = c1; m[2][1] = c2; m[2][2] = c3; m[2][3] = c4; + m[3][0] = d1; m[3][1] = d2; m[3][2] = d3; m[3][3] = d4; + } - // Set the matrix to the identity matrix - Matrix4 setToIdentity() { - m[0][0] = 1.f; m[0][1] = 0.f; m[0][2] = 0.f; m[0][3] = 0.f; - m[1][0] = 0.f; m[1][1] = 1.f; m[1][2] = 0.f; m[1][3] = 0.f; - m[2][0] = 0.f; m[2][1] = 0.f; m[2][2] = 1.f; m[2][3] = 0.f; - m[3][0] = 0.f; m[3][1] = 0.f; m[3][2] = 0.f; m[3][3] = 1.f; - return *this; - } + // Set the matrix to the identity matrix + Matrix4 setToIdentity() { + m[0][0] = 1.f; m[0][1] = 0.f; m[0][2] = 0.f; m[0][3] = 0.f; + m[1][0] = 0.f; m[1][1] = 1.f; m[1][2] = 0.f; m[1][3] = 0.f; + m[2][0] = 0.f; m[2][1] = 0.f; m[2][2] = 1.f; m[2][3] = 0.f; + m[3][0] = 0.f; m[3][1] = 0.f; m[3][2] = 0.f; m[3][3] = 1.f; + return *this; + } - // Display the matrix - void print() const { - for (int i=0; i<4; i++) { - for (int j=0; j<4; j++) { - std::cout << m[i][j]; - } - std::cout << std::endl; - } - } + // Display the matrix + void print32_t() const { + for (int32_t i=0; i<4; i++) { + for (int32_t j=0; j<4; j++) { + std::cout << m[i][j]; + } + std::cout << std::endl; + } + } - // Return the pointer to the data array of the matrix - float* dataBlock() { - return m[0]; - } + // Return the pointer to the data array of the matrix + float* dataBlock() { + return m[0]; + } - // Return the constant pointer to the data array of the matrix - const float* dataBlock() const { - return m[0]; - } + // Return the constant pointer to the data array of the matrix + const float* dataBlock() const { + return m[0]; + } - // Return a given value from the matrix - float getValue(int i, int j) const { - assert(i >= 0 && i<4 && j >= 0 && j<4); - return m[i][j]; - } + // Return a given value from the matrix + float getValue(int32_t i, int32_t j) const { + assert(i >= 0 && i<4 && j >= 0 && j<4); + return m[i][j]; + } - // Return the trace of the matrix - float getTrace() const { - // Compute and return the trace - return (m[0][0] + m[1][1] + m[2][2] + m[3][3]); - } + // Return the trace of the matrix + float getTrace() const { + // Compute and return the trace + return (m[0][0] + m[1][1] + m[2][2] + m[3][3]); + } - // Return a 4x4 translation matrix - static Matrix4 translationMatrix(const Vector3& v); + // Return a 4x4 translation matrix + static Matrix4 translationMatrix(const Vector3& v); - // Return a 4x4 rotation matrix - static Matrix4 rotationMatrix(const Vector3& axis, float angle); + // Return a 4x4 rotation matrix + static Matrix4 rotationMatrix(const Vector3& axis, float angle); - // Return a 4x4 perspective projection matrix - static Matrix4 perspectiveProjectionMatrix(float near, float far, - int width, int height, - float fieldOfView); + // Return a 4x4 perspective projection matrix + static Matrix4 perspectiveProjectionMatrix(float near, float far, + int32_t width, int32_t height, + float fieldOfView); - // Return a 4x4 orthographic projection matrix - static Matrix4 orthoProjectionMatrix(float near, float far, int width, - int height); + // Return a 4x4 orthographic projection matrix + static Matrix4 orthoProjectionMatrix(float near, float far, int32_t width, + int32_t height); - // Return the identity matrix - static Matrix4 identity(); + // Return the identity matrix + static Matrix4 identity(); }; // * operator @@ -398,96 +398,96 @@ inline Matrix4 operator*(float f, const Matrix4 & m) { // Return a 4x4 translation matrix inline Matrix4 Matrix4::translationMatrix(const Vector3& v) { - return Matrix4(1, 0, 0, v.x, - 0, 1, 0, v.y, - 0, 0, 1, v.z, - 0, 0, 0, 1); + return Matrix4(1, 0, 0, v.x, + 0, 1, 0, v.y, + 0, 0, 1, v.z, + 0, 0, 0, 1); } // Return a 4x4 rotation matrix inline Matrix4 Matrix4::rotationMatrix(const Vector3& axis, float angle) { - float cosA = cos(angle); - float sinA = sin(angle); - Matrix4 rotationMatrix; - rotationMatrix.setToIdentity(); + float cosA = cos(angle); + float sinA = sin(angle); + Matrix4 rotationMatrix; + rotationMatrix.setToIdentity(); - rotationMatrix.m[0][0] = cosA + (1-cosA) * axis.x * axis.x; - rotationMatrix.m[0][1] = (1-cosA) * axis.x * axis.y - axis.z * sinA; - rotationMatrix.m[0][2] = (1-cosA) * axis.x * axis.z + axis.y * sinA; - rotationMatrix.m[0][3] = 0.f; + rotationMatrix.m[0][0] = cosA + (1-cosA) * axis.x * axis.x; + rotationMatrix.m[0][1] = (1-cosA) * axis.x * axis.y - axis.z * sinA; + rotationMatrix.m[0][2] = (1-cosA) * axis.x * axis.z + axis.y * sinA; + rotationMatrix.m[0][3] = 0.f; - rotationMatrix.m[1][0] = (1-cosA) * axis.x * axis.y + axis.z * sinA; - rotationMatrix.m[1][1] = cosA + (1-cosA) * axis.y * axis.y; - rotationMatrix.m[1][2] = (1-cosA) * axis.y * axis.z - axis.x * sinA; - rotationMatrix.m[1][3] = 0.f; + rotationMatrix.m[1][0] = (1-cosA) * axis.x * axis.y + axis.z * sinA; + rotationMatrix.m[1][1] = cosA + (1-cosA) * axis.y * axis.y; + rotationMatrix.m[1][2] = (1-cosA) * axis.y * axis.z - axis.x * sinA; + rotationMatrix.m[1][3] = 0.f; - rotationMatrix.m[2][0] = (1-cosA) * axis.x * axis.z - axis.y * sinA; - rotationMatrix.m[2][1] = (1-cosA) * axis.y * axis.z + axis.x * sinA; - rotationMatrix.m[2][2] = cosA + (1-cosA) * axis.z * axis.z; - rotationMatrix.m[2][3] = 0.f; + rotationMatrix.m[2][0] = (1-cosA) * axis.x * axis.z - axis.y * sinA; + rotationMatrix.m[2][1] = (1-cosA) * axis.y * axis.z + axis.x * sinA; + rotationMatrix.m[2][2] = cosA + (1-cosA) * axis.z * axis.z; + rotationMatrix.m[2][3] = 0.f; - rotationMatrix.m[3][0] = 0.f; - rotationMatrix.m[3][1] = 0.f; - rotationMatrix.m[3][2] = 0.f; - rotationMatrix.m[3][3] = 1.f; + rotationMatrix.m[3][0] = 0.f; + rotationMatrix.m[3][1] = 0.f; + rotationMatrix.m[3][2] = 0.f; + rotationMatrix.m[3][3] = 1.f; - return rotationMatrix; + return rotationMatrix; } // Return a 4x4 perspective projection matrix -inline Matrix4 Matrix4::perspectiveProjectionMatrix(float near, float far, int width, int height, - float fieldOfView) { +inline Matrix4 Matrix4::perspectiveProjectionMatrix(float near, float far, int32_t width, int32_t height, + float fieldOfView) { - // Compute the aspect ratio - float aspect = float(width) / float(height); + // Compute the aspect ratio + float aspect = float(width) / float(height); - float top = near * tan((fieldOfView / 2.0f) * (float(PI) / 180.0f)); - float bottom = -top; - float left = bottom * aspect; - float right = top * aspect; + float top = near * tan((fieldOfView / 2.0f) * (float(PI) / 180.0f)); + float bottom = -top; + float left = bottom * aspect; + float right = top * aspect; - float fx = 2.0f * near / (right - left); - float fy = 2.0f * near / (top - bottom); - float fz = -(far + near) / (far - near); - float fw = -2.0f * far * near / (far - near); + float fx = 2.0f * near / (right - left); + float fy = 2.0f * near / (top - bottom); + float fz = -(far + near) / (far - near); + float fw = -2.0f * far * near / (far - near); - // Compute the projection matrix - return Matrix4(fx, 0, 0, 0, - 0, fy, 0, 0, - 0, 0, fz, fw, - 0, 0, -1, 0); + // Compute the projection matrix + return Matrix4(fx, 0, 0, 0, + 0, fy, 0, 0, + 0, 0, fz, fw, + 0, 0, -1, 0); } // Return a 4x4 orthographic projection matrix -inline Matrix4 Matrix4::orthoProjectionMatrix(float near, float far, int width, int height) { +inline Matrix4 Matrix4::orthoProjectionMatrix(float near, float far, int32_t width, int32_t height) { - // Compute the aspect ratio - float aspect = float(width) / float(height); + // Compute the aspect ratio + float aspect = float(width) / float(height); - float top = height * 0.5f; - float bottom = -top; - float left = bottom * aspect; - float right = top * aspect; + float top = height * 0.5f; + float bottom = -top; + float left = bottom * aspect; + float right = top * aspect; - float fx = 2.0f / (right - left); - float fy = 2.0f / (top - bottom); - float fz = -2.0f / (far - near); - float fw = -(far + near) / (far - near); + float fx = 2.0f / (right - left); + float fy = 2.0f / (top - bottom); + float fz = -2.0f / (far - near); + float fw = -(far + near) / (far - near); - // Compute the projection matrix - return Matrix4(fx, 0, 0, 0, - 0, fy, 0, 0, - 0, 0, fz, fw, - 0, 0, 0, 1); + // Compute the projection matrix + return Matrix4(fx, 0, 0, 0, + 0, fy, 0, 0, + 0, 0, fz, fw, + 0, 0, 0, 1); } // Return the identity matrix inline Matrix4 Matrix4::identity() { - return Matrix4(1, 0, 0, 0, - 0, 1, 0, 0, - 0, 0, 1, 0, - 0, 0, 0, 1); + return Matrix4(1, 0, 0, 0, + 0, 1, 0, 0, + 0, 0, 1, 0, + 0, 0, 0, 1); } } diff --git a/tools/testbed/opengl-framework/src/maths/Vector2.h b/tools/testbed/opengl-framework/src/maths/Vector2.h index 0bc0eb9..d86fbad 100644 --- a/tools/testbed/opengl-framework/src/maths/Vector2.h +++ b/tools/testbed/opengl-framework/src/maths/Vector2.h @@ -1,26 +1,26 @@ /******************************************************************************** -* OpenGL-Framework * -* Copyright (c) 2013 Daniel Chappuis * +* OpenGL-Framework * +* Copyright (c) 2013 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: * -* * +* 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. * -* * +* 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 VECTOR2_H @@ -36,122 +36,122 @@ namespace openglframework { // This class represents a 2D vector. class Vector2 { - public: + public: - // -------------------- Attributes -------------------- // + // -------------------- Attributes -------------------- // - // Components of the vector - float x, y; + // Components of the vector + float x, y; - // -------------------- Methods -------------------- // + // -------------------- Methods -------------------- // - // Constructor - Vector2(float x=0, float y=0) : x(x), y(y) {} + // Constructor + Vector2(float x=0, float y=0) : x(x), y(y) {} - // Constructor - Vector2(const Vector2& vector) : x(vector.x), y(vector.y) {} + // Constructor + Vector2(const Vector2& vector) : x(vector.x), y(vector.y) {} - // + operator - Vector2 operator+(const Vector2 &v) const { - return Vector2(x + v.x, y + v.y); - } + // + operator + Vector2 operator+(const Vector2 &v) const { + return Vector2(x + v.x, y + v.y); + } - // += operator - Vector2& operator+=(const Vector2 &v) { - x += v.x; y += v.y; - return *this; - } + // += operator + Vector2& operator+=(const Vector2 &v) { + x += v.x; y += v.y; + return *this; + } - // - operator - Vector2 operator-(const Vector2 &v) const { - return Vector2(x - v.x, y - v.y); - } + // - operator + Vector2 operator-(const Vector2 &v) const { + return Vector2(x - v.x, y - v.y); + } - // -= operator - Vector2& operator-=(const Vector2 &v) { - x -= v.x; y -= v.y; - return *this; - } + // -= operator + Vector2& operator-=(const Vector2 &v) { + x -= v.x; y -= v.y; + return *this; + } - // = operator - Vector2& operator=(const Vector2& vector) { - if (&vector != this) { - x = vector.x; - y = vector.y; - } - return *this; - } + // = operator + Vector2& operator=(const Vector2& vector) { + if (&vector != this) { + x = vector.x; + y = vector.y; + } + return *this; + } - // == operator - bool operator==(const Vector2 &v) const { - return x == v.x && y == v.y; - } + // == operator + bool operator==(const Vector2 &v) const { + return x == v.x && y == v.y; + } - // * operator - Vector2 operator*(float f) const { - return Vector2(f*x, f*y); - } + // * operator + Vector2 operator*(float f) const { + return Vector2(f*x, f*y); + } - // *= operator - Vector2 &operator*=(float f) { - x *= f; y *= f; - return *this; - } + // *= operator + Vector2 &operator*=(float f) { + x *= f; y *= f; + return *this; + } - // / operator - Vector2 operator/(float f) const { - assert(f!=0); - float inv = 1.f / f; - return Vector2(x * inv, y * inv); - } + // / operator + Vector2 operator/(float f) const { + assert(f!=0); + float inv = 1.f / f; + return Vector2(x * inv, y * inv); + } - // /= operator - Vector2 &operator/=(float f) { - assert(f!=0); - float inv = 1.f / f; - x *= inv; y *= inv; - return *this; - } + // /= operator + Vector2 &operator/=(float f) { + assert(f!=0); + float inv = 1.f / f; + x *= inv; y *= inv; + return *this; + } - // - operator - Vector2 operator-() const { - return Vector2(-x, -y); - } + // - operator + Vector2 operator-() const { + return Vector2(-x, -y); + } - // [] operator - float &operator[](int i) { - assert(i >= 0 && i <= 1); - switch (i) { - case 0: return x; - case 1: return y; - } - return y; - } + // [] operator + float &operator[](int32_t i) { + assert(i >= 0 && i <= 1); + switch (i) { + case 0: return x; + case 1: return y; + } + return y; + } - // Normalize the vector and return it - Vector2 normalize() { - float l = length(); - assert(l > 0); - x /= l; - y /= l; - return *this; - } + // Normalize the vector and return it + Vector2 normalize() { + float l = length(); + assert(l > 0); + x /= l; + y /= l; + return *this; + } - // Clamp the vector values between 0 and 1 - Vector2 clamp01() { - if (x>1.f) x=1.f; - else if (x<0.f) x=0.f; - if (y>1.f) y=1.f; - else if (y<0.f) y=0.f; - return *this; - } + // Clamp the vector values between 0 and 1 + Vector2 clamp01() { + if (x>1.f) x=1.f; + else if (x<0.f) x=0.f; + if (y>1.f) y=1.f; + else if (y<0.f) y=0.f; + return *this; + } - // Return the squared length of the vector - float lengthSquared() const { return x*x + y*y; } + // Return the squared length of the vector + float lengthSquared() const { return x*x + y*y; } - // Return the length of the vector - float length() const { return sqrt(lengthSquared()); } + // Return the length of the vector + float length() const { return sqrt(lengthSquared()); } }; } diff --git a/tools/testbed/opengl-framework/src/maths/Vector3.h b/tools/testbed/opengl-framework/src/maths/Vector3.h index 5719207..a96db54 100644 --- a/tools/testbed/opengl-framework/src/maths/Vector3.h +++ b/tools/testbed/opengl-framework/src/maths/Vector3.h @@ -1,26 +1,26 @@ /******************************************************************************** -* OpenGL-Framework * -* Copyright (c) 2013 Daniel Chappuis * +* OpenGL-Framework * +* Copyright (c) 2013 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: * -* * +* 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. * -* * +* 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 OPENGLFRAMEWORK_VECTOR3_H @@ -37,161 +37,161 @@ namespace openglframework { // This class represents a 3D vector. class Vector3 { - public: + public: - // -------------------- Attributes -------------------- // + // -------------------- Attributes -------------------- // - // Components of the vector - float x, y, z; + // Components of the vector + float x, y, z; - // -------------------- Methods -------------------- // + // -------------------- Methods -------------------- // - // Constructor - Vector3(float x=0, float y=0, float z=0) : x(x), y(y), z(z) {} + // Constructor + Vector3(float x=0, float y=0, float z=0) : x(x), y(y), z(z) {} - // Constructor - Vector3(const Vector3& vector) : x(vector.x), y(vector.y), z(vector.z) {} + // Constructor + Vector3(const Vector3& vector) : x(vector.x), y(vector.y), z(vector.z) {} - // Constructor - ~Vector3() {} + // Constructor + ~Vector3() {} - // = operator - Vector3& operator=(const Vector3& vector) { - if (&vector != this) { - x = vector.x; - y = vector.y; - z = vector.z; - } - return *this; - } + // = operator + Vector3& operator=(const Vector3& vector) { + if (&vector != this) { + x = vector.x; + y = vector.y; + z = vector.z; + } + return *this; + } - // + operator - Vector3 operator+(const Vector3 &v) const { - return Vector3(x + v.x, y + v.y, z + v.z); - } + // + operator + Vector3 operator+(const Vector3 &v) const { + return Vector3(x + v.x, y + v.y, z + v.z); + } - // += operator - Vector3& operator+=(const Vector3 &v) { - x += v.x; y += v.y; z += v.z; - return *this; - } + // += operator + Vector3& operator+=(const Vector3 &v) { + x += v.x; y += v.y; z += v.z; + return *this; + } - // - operator - Vector3 operator-(const Vector3 &v) const { - return Vector3(x - v.x, y - v.y, z - v.z); - } + // - operator + Vector3 operator-(const Vector3 &v) const { + return Vector3(x - v.x, y - v.y, z - v.z); + } - // -= operator - Vector3& operator-=(const Vector3 &v) { - x -= v.x; y -= v.y; z -= v.z; - return *this; - } + // -= operator + Vector3& operator-=(const Vector3 &v) { + x -= v.x; y -= v.y; z -= v.z; + return *this; + } - // == operator - bool operator==(const Vector3 &v) const { - return x == v.x && y == v.y && z == v.z; - } + // == operator + bool operator==(const Vector3 &v) const { + return x == v.x && y == v.y && z == v.z; + } - // != operator - bool operator!=(const Vector3 &v) const { - return !( *this == v ); - } + // != operator + bool operator!=(const Vector3 &v) const { + return !( *this == v ); + } - // * operator - Vector3 operator*(float f) const { - return Vector3(f*x, f*y, f*z); - } + // * operator + Vector3 operator*(float f) const { + return Vector3(f*x, f*y, f*z); + } - // *= operator - Vector3 &operator*=(float f) { - x *= f; y *= f; z *= f; - return *this; - } + // *= operator + Vector3 &operator*=(float f) { + x *= f; y *= f; z *= f; + return *this; + } - // / operator - Vector3 operator/(float f) const { - assert(f > std::numeric_limits::epsilon() ); - float inv = 1.f / f; - return Vector3(x * inv, y * inv, z * inv); - } + // / operator + Vector3 operator/(float f) const { + assert(f > std::numeric_limits::epsilon() ); + float inv = 1.f / f; + return Vector3(x * inv, y * inv, z * inv); + } - // /= operator - Vector3 &operator/=(float f) { - assert(f > std::numeric_limits::epsilon()); - float inv = 1.f / f; - x *= inv; y *= inv; z *= inv; - return *this; - } + // /= operator + Vector3 &operator/=(float f) { + assert(f > std::numeric_limits::epsilon()); + float inv = 1.f / f; + x *= inv; y *= inv; z *= inv; + return *this; + } - // - operator - Vector3 operator-() const { - return Vector3(-x, -y, -z); - } + // - operator + Vector3 operator-() const { + return Vector3(-x, -y, -z); + } - // [] operator - float &operator[](int i) { - assert(i >= 0 && i <= 2); - switch (i) { - case 0: return x; - case 1: return y; - case 2: return z; - } - return z; - } + // [] operator + float &operator[](int32_t i) { + assert(i >= 0 && i <= 2); + switch (i) { + case 0: return x; + case 1: return y; + case 2: return z; + } + return z; + } - // [] operator - const float &operator[](int i) const { - assert(i >= 0 && i <= 2); - switch (i) { - case 0: return x; - case 1: return y; - case 2: return z; - } - return z; - } + // [] operator + const float &operator[](int32_t i) const { + assert(i >= 0 && i <= 2); + switch (i) { + case 0: return x; + case 1: return y; + case 2: return z; + } + return z; + } - // Cross product operator - Vector3 cross(const Vector3 &v) const{ - return Vector3(y * v.z - z * v.y, z * v.x - x * v.z, x * v.y - y * v.x); - } + // Cross product operator + Vector3 cross(const Vector3 &v) const{ + return Vector3(y * v.z - z * v.y, z * v.x - x * v.z, x * v.y - y * v.x); + } - // Dot product operator - float dot(const Vector3 &v) const{ - return x * v.x + y * v.y + z * v.z; - } + // Dot product operator + float dot(const Vector3 &v) const{ + return x * v.x + y * v.y + z * v.z; + } - // Normalize the vector and return it - Vector3 normalize() { - float l = length(); - if(l < std::numeric_limits::epsilon() ) { - return *this; - } - x /= l; - y /= l; - z /= l; - return *this; - } + // Normalize the vector and return it + Vector3 normalize() { + float l = length(); + if(l < std::numeric_limits::epsilon() ) { + return *this; + } + x /= l; + y /= l; + z /= l; + return *this; + } - bool isNull() const { - return( x == 0. && y == 0. && z == 0. ); - } + bool isNull() const { + return( x == 0. && y == 0. && z == 0. ); + } - // Clamp the values between 0 and 1 - Vector3 clamp01() { - if (x>1.f) x=1.f; - else if (x<0.f) x=0.f; - if (y>1.f) y=1.f; - else if (y<0.f) y=0.f; - if (z>1.f) z=1.f; - else if (z<0.f) z=0.f; - return *this; - } + // Clamp the values between 0 and 1 + Vector3 clamp01() { + if (x>1.f) x=1.f; + else if (x<0.f) x=0.f; + if (y>1.f) y=1.f; + else if (y<0.f) y=0.f; + if (z>1.f) z=1.f; + else if (z<0.f) z=0.f; + return *this; + } - // Return the squared length of the vector - float lengthSquared() const { return x*x + y*y + z*z; } + // Return the squared length of the vector + float lengthSquared() const { return x*x + y*y + z*z; } - // Return the length of the vector - float length() const { return sqrt(lengthSquared()); } + // Return the length of the vector + float length() const { return sqrt(lengthSquared()); } }; inline Vector3 operator*(float f, const Vector3 & o) { diff --git a/tools/testbed/opengl-framework/src/maths/Vector4.h b/tools/testbed/opengl-framework/src/maths/Vector4.h index 3c4002d..fe58033 100644 --- a/tools/testbed/opengl-framework/src/maths/Vector4.h +++ b/tools/testbed/opengl-framework/src/maths/Vector4.h @@ -1,26 +1,26 @@ /******************************************************************************** -* OpenGL-Framework * -* Copyright (c) 2013 Daniel Chappuis * +* OpenGL-Framework * +* Copyright (c) 2013 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: * -* * +* 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. * -* * +* 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 VECTOR4_H @@ -36,130 +36,130 @@ namespace openglframework { // This class represents a 4D vector. class Vector4 { - public: + public: - // -------------------- Attributes -------------------- // + // -------------------- Attributes -------------------- // - // Components of the vector - float x, y, z, w; + // Components of the vector + float x, y, z, w; - // -------------------- Methods -------------------- // + // -------------------- Methods -------------------- // - // Constructor - Vector4(float x=0, float y=0, float z=0, float w=0) : x(x), y(y), z(z), w(w) {} + // Constructor + Vector4(float x=0, float y=0, float z=0, float w=0) : x(x), y(y), z(z), w(w) {} - // Constructor - Vector4(const Vector4& vector) : x(vector.x), y(vector.y), z(vector.z), w(vector.w) {} + // Constructor + Vector4(const Vector4& vector) : x(vector.x), y(vector.y), z(vector.z), w(vector.w) {} - // + operator - Vector4 operator+(const Vector4 &v) const { - return Vector4(x + v.x, y + v.y, z + v.z, w + v.w); - } + // + operator + Vector4 operator+(const Vector4 &v) const { + return Vector4(x + v.x, y + v.y, z + v.z, w + v.w); + } - // += operator - Vector4& operator+=(const Vector4 &v) { - x += v.x; y += v.y; z += v.z; w += v.w; - return *this; - } + // += operator + Vector4& operator+=(const Vector4 &v) { + x += v.x; y += v.y; z += v.z; w += v.w; + return *this; + } - // - operator - Vector4 operator-(const Vector4 &v) const { - return Vector4(x - v.x, y - v.y, z - v.z, w - v.w); - } + // - operator + Vector4 operator-(const Vector4 &v) const { + return Vector4(x - v.x, y - v.y, z - v.z, w - v.w); + } - // -= operator - Vector4& operator-=(const Vector4 &v) { - x -= v.x; y -= v.y; z -= v.z, w -=v.w; - return *this; - } + // -= operator + Vector4& operator-=(const Vector4 &v) { + x -= v.x; y -= v.y; z -= v.z, w -=v.w; + return *this; + } - // = operator - Vector4& operator=(const Vector4& vector) { - if (&vector != this) { - x = vector.x; - y = vector.y; - z = vector.z; - w = vector.w; - } - return *this; - } + // = operator + Vector4& operator=(const Vector4& vector) { + if (&vector != this) { + x = vector.x; + y = vector.y; + z = vector.z; + w = vector.w; + } + return *this; + } - // == operator - bool operator==(const Vector4 &v) const { - return x == v.x && y == v.y && z == v.z && w == v.w; - } + // == operator + bool operator==(const Vector4 &v) const { + return x == v.x && y == v.y && z == v.z && w == v.w; + } - // * operator - Vector4 operator*(float f) const { - return Vector4(f*x, f*y, f*z, f*w); - } + // * operator + Vector4 operator*(float f) const { + return Vector4(f*x, f*y, f*z, f*w); + } - // *= operator - Vector4 &operator*=(float f) { - x *= f; y *= f; z *= f; w *= f; - return *this; - } + // *= operator + Vector4 &operator*=(float f) { + x *= f; y *= f; z *= f; w *= f; + return *this; + } - // / operator - Vector4 operator/(float f) const { - assert(f!=0); - float inv = 1.f / f; - return Vector4(x * inv, y * inv, z * inv, w * inv); - } + // / operator + Vector4 operator/(float f) const { + assert(f!=0); + float inv = 1.f / f; + return Vector4(x * inv, y * inv, z * inv, w * inv); + } - // /= operator - Vector4 &operator/=(float f) { - assert(f!=0); - float inv = 1.f / f; - x *= inv; y *= inv; z *= inv; w *= inv; - return *this; - } + // /= operator + Vector4 &operator/=(float f) { + assert(f!=0); + float inv = 1.f / f; + x *= inv; y *= inv; z *= inv; w *= inv; + return *this; + } - // - operator - Vector4 operator-() const { - return Vector4(-x, -y, -z, -w); - } + // - operator + Vector4 operator-() const { + return Vector4(-x, -y, -z, -w); + } - // [] operator - float &operator[](int i) { - assert(i >= 0 && i <= 3); - switch (i) { - case 0: return x; - case 1: return y; - case 2: return z; - case 3: return w; - } - return w; - } + // [] operator + float &operator[](int32_t i) { + assert(i >= 0 && i <= 3); + switch (i) { + case 0: return x; + case 1: return y; + case 2: return z; + case 3: return w; + } + return w; + } - // Dot product operator - float dot(const Vector4 &v) const { - return x * v.x + y * v.y + z * v.z + w * v.w; - } + // Dot product operator + float dot(const Vector4 &v) const { + return x * v.x + y * v.y + z * v.z + w * v.w; + } - // Multiply two vectors by their components - Vector4 componentMul(const Vector4 &v) const { - return Vector4(x * v.x, y * v.y, z * v.z, w * v.w); - } + // Multiply two vectors by their components + Vector4 componentMul(const Vector4 &v) const { + return Vector4(x * v.x, y * v.y, z * v.z, w * v.w); + } - // Clamp the values between 0 and 1 - Vector4 clamp01() { - if (x>1.f) x=1.f; - else if (x<0.f) x=0.f; - if (y>1.f) y=1.f; - else if (y<0.f) y=0.f; - if (z>1.f) z=1.f; - else if (z<0.f) z=0.f; - if (w>1.f) w=1.f; - else if (w<0.f) w=0.f; - return *this; - } + // Clamp the values between 0 and 1 + Vector4 clamp01() { + if (x>1.f) x=1.f; + else if (x<0.f) x=0.f; + if (y>1.f) y=1.f; + else if (y<0.f) y=0.f; + if (z>1.f) z=1.f; + else if (z<0.f) z=0.f; + if (w>1.f) w=1.f; + else if (w<0.f) w=0.f; + return *this; + } - // Return the squared length of the vector - float lengthSquared() const { return x * x + y * y + z * z + w * w; } + // Return the squared length of the vector + float lengthSquared() const { return x * x + y * y + z * z + w * w; } - // Return the length of the vector - float length() const { return sqrt(lengthSquared()); } + // Return the length of the vector + float length() const { return sqrt(lengthSquared()); } }; } diff --git a/tools/testbed/opengl-framework/src/openglframework.h b/tools/testbed/opengl-framework/src/openglframework.h index 672d3d3..640b85a 100644 --- a/tools/testbed/opengl-framework/src/openglframework.h +++ b/tools/testbed/opengl-framework/src/openglframework.h @@ -1,26 +1,26 @@ /******************************************************************************** -* OpenGL-Framework * -* Copyright (c) 2013 Daniel Chappuis * +* OpenGL-Framework * +* Copyright (c) 2013 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: * -* * +* 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. * -* * +* 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 OPENGL_FRAMEWORK_H diff --git a/tools/testbed/scenes/collisionshapes/CollisionShapesScene.cpp b/tools/testbed/scenes/collisionshapes/CollisionShapesScene.cpp index a8c2fa4..fa674a6 100644 --- a/tools/testbed/scenes/collisionshapes/CollisionShapesScene.cpp +++ b/tools/testbed/scenes/collisionshapes/CollisionShapesScene.cpp @@ -1,26 +1,26 @@ /******************************************************************************** -* ReactPhysics3D physics library, http://www.reactphysics3d.com * -* Copyright (c) 2010-2016 Daniel Chappuis * +* 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: * -* * +* 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. * -* * +* 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 @@ -32,609 +32,609 @@ using namespace collisionshapesscene; // Constructor CollisionShapesScene::CollisionShapesScene(const std::string& name) - : SceneDemo(name, SCENE_RADIUS) { + : SceneDemo(name, SCENE_RADIUS) { - std::string meshFolderPath("meshes/"); + std::string meshFolderPath("meshes/"); - // Compute the radius and the center of the scene - openglframework::Vector3 center(0, 5, 0); + // 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); + // Set the center of the scene + setScenePosition(center, SCENE_RADIUS); - // Gravity vector in the dynamics world - rp3d::Vector3 gravity(0, -9.81, 0); + // Gravity vector in the dynamics world + rp3d::Vector3 gravity(0, -9.81, 0); - // Create the dynamics world for the physics simulation - mDynamicsWorld = new rp3d::DynamicsWorld(gravity); + // Create the dynamics world for the physics simulation + mDynamicsWorld = new rp3d::DynamicsWorld(gravity); - // Set the number of iterations of the constraint solver - mDynamicsWorld->setNbIterationsVelocitySolver(15); + // Set the number of iterations of the constraint solver + mDynamicsWorld->setNbIterationsVelocitySolver(15); - float radius = 3.0f; + float radius = 3.0f; - for (int i=0; isetColor(mDemoColors[i % mNbDemoColors]); - dumbbell->setSleepingColor(mRedColorDemo); + // Set the box color + dumbbell->setColor(mDemoColors[i % mNbDemoColors]); + dumbbell->setSleepingColor(mRedColorDemo); - // Change the material properties of the rigid body - rp3d::Material& material = dumbbell->getRigidBody()->getMaterial(); - material.setBounciness(rp3d::decimal(0.2)); + // Change the material properties of the rigid body + rp3d::Material& material = dumbbell->getRigidBody()->getMaterial(); + material.setBounciness(rp3d::float(0.2)); - // Add the mesh the list of dumbbells in the scene - mDumbbells.push_back(dumbbell); - } + // Add the mesh the list of dumbbells in the scene + mDumbbells.push_back(dumbbell); + } - // Create all the boxes of the scene - for (int i=0; isetColor(mDemoColors[i % mNbDemoColors]); - box->setSleepingColor(mRedColorDemo); + // Set the box color + box->setColor(mDemoColors[i % mNbDemoColors]); + box->setSleepingColor(mRedColorDemo); - // Change the material properties of the rigid body - rp3d::Material& material = box->getRigidBody()->getMaterial(); - material.setBounciness(rp3d::decimal(0.2)); + // Change the material properties of the rigid body + rp3d::Material& material = box->getRigidBody()->getMaterial(); + material.setBounciness(rp3d::float(0.2)); - // Add the sphere the list of sphere in the scene - mBoxes.push_back(box); - } + // Add the sphere the list of sphere in the scene + mBoxes.push_back(box); + } - // Create all the spheres of the scene - for (int i=0; igetRigidBody()->getMaterial().setRollingResistance(0.08); + // Add some rolling resistance + sphere->getRigidBody()->getMaterial().setRollingResistance(0.08); - // Set the box color - sphere->setColor(mDemoColors[i % mNbDemoColors]); - sphere->setSleepingColor(mRedColorDemo); + // Set the box color + sphere->setColor(mDemoColors[i % mNbDemoColors]); + sphere->setSleepingColor(mRedColorDemo); - // Change the material properties of the rigid body - rp3d::Material& material = sphere->getRigidBody()->getMaterial(); - material.setBounciness(rp3d::decimal(0.2)); + // Change the material properties of the rigid body + rp3d::Material& material = sphere->getRigidBody()->getMaterial(); + material.setBounciness(rp3d::float(0.2)); - // Add the sphere the list of sphere in the scene - mSpheres.push_back(sphere); - } + // Add the sphere the list of sphere in the scene + mSpheres.push_back(sphere); + } - // Create all the cones of the scene - for (int i=0; igetRigidBody()->getMaterial().setRollingResistance(0.08); + // Add some rolling resistance + cone->getRigidBody()->getMaterial().setRollingResistance(0.08); - // Set the box color - cone->setColor(mDemoColors[i % mNbDemoColors]); - cone->setSleepingColor(mRedColorDemo); + // Set the box color + cone->setColor(mDemoColors[i % mNbDemoColors]); + cone->setSleepingColor(mRedColorDemo); - // Change the material properties of the rigid body - rp3d::Material& material = cone->getRigidBody()->getMaterial(); - material.setBounciness(rp3d::decimal(0.2)); + // Change the material properties of the rigid body + rp3d::Material& material = cone->getRigidBody()->getMaterial(); + material.setBounciness(rp3d::float(0.2)); - // Add the cone the list of sphere in the scene - mCones.push_back(cone); - } + // Add the cone the list of sphere in the scene + mCones.push_back(cone); + } - // Create all the cylinders of the scene - for (int i=0; igetRigidBody()->getMaterial().setRollingResistance(0.08); + // Add some rolling resistance + cylinder->getRigidBody()->getMaterial().setRollingResistance(0.08); - // Set the box color - cylinder->setColor(mDemoColors[i % mNbDemoColors]); - cylinder->setSleepingColor(mRedColorDemo); + // Set the box color + cylinder->setColor(mDemoColors[i % mNbDemoColors]); + cylinder->setSleepingColor(mRedColorDemo); - // Change the material properties of the rigid body - rp3d::Material& material = cylinder->getRigidBody()->getMaterial(); - material.setBounciness(rp3d::decimal(0.2)); + // Change the material properties of the rigid body + rp3d::Material& material = cylinder->getRigidBody()->getMaterial(); + material.setBounciness(rp3d::float(0.2)); - // Add the cylinder the list of sphere in the scene - mCylinders.push_back(cylinder); - } + // Add the cylinder the list of sphere in the scene + mCylinders.push_back(cylinder); + } - // Create all the capsules of the scene - for (int i=0; igetRigidBody()->getMaterial().setRollingResistance(0.08); + // Position + float angle = i * 45.0f; + openglframework::Vector3 position(radius * cos(angle), + 15 + i * (CAPSULE_HEIGHT + 0.3f), + radius * sin(angle)); + + // Create a cylinder and a corresponding rigid in the dynamics world + Capsule* capsule = new Capsule(CAPSULE_RADIUS, CAPSULE_HEIGHT, position , + CAPSULE_MASS, mDynamicsWorld, meshFolderPath); + + capsule->getRigidBody()->getMaterial().setRollingResistance(0.08); - // Set the box color - capsule->setColor(mDemoColors[i % mNbDemoColors]); - capsule->setSleepingColor(mRedColorDemo); + // Set the box color + capsule->setColor(mDemoColors[i % mNbDemoColors]); + capsule->setSleepingColor(mRedColorDemo); - // Change the material properties of the rigid body - rp3d::Material& material = capsule->getRigidBody()->getMaterial(); - material.setBounciness(rp3d::decimal(0.2)); + // Change the material properties of the rigid body + rp3d::Material& material = capsule->getRigidBody()->getMaterial(); + material.setBounciness(rp3d::float(0.2)); - // Add the cylinder the list of sphere in the scene - mCapsules.push_back(capsule); - } + // Add the cylinder the list of sphere in the scene + mCapsules.push_back(capsule); + } - // Create all the convex meshes of the scene - for (int i=0; isetColor(mDemoColors[i % mNbDemoColors]); - mesh->setSleepingColor(mRedColorDemo); + // Set the box color + mesh->setColor(mDemoColors[i % mNbDemoColors]); + mesh->setSleepingColor(mRedColorDemo); - // Change the material properties of the rigid body - rp3d::Material& material = mesh->getRigidBody()->getMaterial(); - material.setBounciness(rp3d::decimal(0.2)); + // Change the material properties of the rigid body + rp3d::Material& material = mesh->getRigidBody()->getMaterial(); + material.setBounciness(rp3d::float(0.2)); - // Add the mesh the list of sphere in the scene - mConvexMeshes.push_back(mesh); - } + // Add the mesh the list of sphere in the scene + mConvexMeshes.push_back(mesh); + } - // ---------- Create the floor --------- + // ---------- Create the floor --------- - openglframework::Vector3 floorPosition(0, 0, 0); - mFloor = new Box(FLOOR_SIZE, floorPosition, FLOOR_MASS, mDynamicsWorld); + openglframework::Vector3 floorPosition(0, 0, 0); + mFloor = new Box(FLOOR_SIZE, floorPosition, FLOOR_MASS, mDynamicsWorld); - // Set the box color - mFloor->setColor(mGreyColorDemo); - mFloor->setSleepingColor(mGreyColorDemo); + // Set the box color + mFloor->setColor(mGreyColorDemo); + mFloor->setSleepingColor(mGreyColorDemo); - // The floor must be a static rigid body - mFloor->getRigidBody()->setType(rp3d::STATIC); + // The floor must be a static rigid body + mFloor->getRigidBody()->setType(rp3d::STATIC); - // Change the material properties of the rigid body - rp3d::Material& material = mFloor->getRigidBody()->getMaterial(); - material.setBounciness(rp3d::decimal(0.2)); + // Change the material properties of the rigid body + rp3d::Material& material = mFloor->getRigidBody()->getMaterial(); + material.setBounciness(rp3d::float(0.2)); - // ---------- Create the triangular mesh ---------- // - - /* - // Position - openglframework::Vector3 position(0, 0, 0); - rp3d::decimal mass = 1.0; + // ---------- Create the triangular mesh ---------- // + + /* + // Position + openglframework::Vector3 position(0, 0, 0); + rp3d::float mass = 1.0; - // Create a convex mesh and a corresponding rigid in the dynamics world - mConcaveMesh = new ConcaveMesh(position, mass, mDynamicsWorld, meshFolderPath); - - // Set the mesh as beeing static - mConcaveMesh->getRigidBody()->setType(rp3d::STATIC); + // Create a convex mesh and a corresponding rigid in the dynamics world + mConcaveMesh = new ConcaveMesh(position, mass, mDynamicsWorld, meshFolderPath); + + // Set the mesh as beeing static + mConcaveMesh->getRigidBody()->setType(rp3d::STATIC); - // Set the box color - mConcaveMesh->setColor(mDemoColors[0]); - mConcaveMesh->setSleepingColor(mRedColorDemo); + // Set the box color + mConcaveMesh->setColor(mDemoColors[0]); + mConcaveMesh->setSleepingColor(mRedColorDemo); - // Change the material properties of the rigid body - rp3d::Material& material = mConcaveMesh->getRigidBody()->getMaterial(); - material.setBounciness(rp3d::decimal(0.2)); - material.setFrictionCoefficient(0.1); - */ + // Change the material properties of the rigid body + rp3d::Material& material = mConcaveMesh->getRigidBody()->getMaterial(); + material.setBounciness(rp3d::float(0.2)); + material.setFrictionCoefficient(0.1); + */ - // Get the physics engine parameters - mEngineSettings.isGravityEnabled = mDynamicsWorld->isGravityEnabled(); - rp3d::Vector3 gravityVector = mDynamicsWorld->getGravity(); - mEngineSettings.gravity = openglframework::Vector3(gravityVector.x, gravityVector.y, gravityVector.z); - mEngineSettings.isSleepingEnabled = mDynamicsWorld->isSleepingEnabled(); - mEngineSettings.sleepLinearVelocity = mDynamicsWorld->getSleepLinearVelocity(); - mEngineSettings.sleepAngularVelocity = mDynamicsWorld->getSleepAngularVelocity(); - mEngineSettings.nbPositionSolverIterations = mDynamicsWorld->getNbIterationsPositionSolver(); - mEngineSettings.nbVelocitySolverIterations = mDynamicsWorld->getNbIterationsVelocitySolver(); - mEngineSettings.timeBeforeSleep = mDynamicsWorld->getTimeBeforeSleep(); + // Get the physics engine parameters + mEngineSettings.isGravityEnabled = mDynamicsWorld->isGravityEnabled(); + rp3d::Vector3 gravityVector = mDynamicsWorld->getGravity(); + mEngineSettings.gravity = openglframework::Vector3(gravityVector.x, gravityVector.y, gravityVector.z); + mEngineSettings.isSleepingEnabled = mDynamicsWorld->isSleepingEnabled(); + mEngineSettings.sleepLinearVelocity = mDynamicsWorld->getSleepLinearVelocity(); + mEngineSettings.sleepAngularVelocity = mDynamicsWorld->getSleepAngularVelocity(); + mEngineSettings.nbPositionSolverIterations = mDynamicsWorld->getNbIterationsPositionSolver(); + mEngineSettings.nbVelocitySolverIterations = mDynamicsWorld->getNbIterationsVelocitySolver(); + mEngineSettings.timeBeforeSleep = mDynamicsWorld->getTimeBeforeSleep(); } // Destructor CollisionShapesScene::~CollisionShapesScene() { - // Destroy all the boxes of the scene - for (std::vector::iterator it = mBoxes.begin(); it != mBoxes.end(); ++it) { + // Destroy all the boxes of the scene + for (std::vector::iterator it = mBoxes.begin(); it != mBoxes.end(); ++it) { - // Destroy the corresponding rigid body from the dynamics world - mDynamicsWorld->destroyRigidBody((*it)->getRigidBody()); + // Destroy the corresponding rigid body from the dynamics world + mDynamicsWorld->destroyRigidBody((*it)->getRigidBody()); - // Destroy the box - delete (*it); - } + // Destroy the box + delete (*it); + } - // Destroy all the sphere of the scene - for (std::vector::iterator it = mSpheres.begin(); it != mSpheres.end(); ++it) { + // Destroy all the sphere of the scene + for (std::vector::iterator it = mSpheres.begin(); it != mSpheres.end(); ++it) { - // Destroy the corresponding rigid body from the dynamics world - mDynamicsWorld->destroyRigidBody((*it)->getRigidBody()); + // Destroy the corresponding rigid body from the dynamics world + mDynamicsWorld->destroyRigidBody((*it)->getRigidBody()); - // Destroy the sphere - delete (*it); - } + // Destroy the sphere + delete (*it); + } - // Destroy all the cones of the scene - for (std::vector::iterator it = mCones.begin(); it != mCones.end(); ++it) { + // Destroy all the cones of the scene + for (std::vector::iterator it = mCones.begin(); it != mCones.end(); ++it) { - // Destroy the corresponding rigid body from the dynamics world - mDynamicsWorld->destroyRigidBody((*it)->getRigidBody()); + // Destroy the corresponding rigid body from the dynamics world + mDynamicsWorld->destroyRigidBody((*it)->getRigidBody()); - // Destroy the sphere - delete (*it); - } + // Destroy the sphere + delete (*it); + } - // Destroy all the cylinders of the scene - for (std::vector::iterator it = mCylinders.begin(); it != mCylinders.end(); ++it) { + // Destroy all the cylinders of the scene + for (std::vector::iterator it = mCylinders.begin(); it != mCylinders.end(); ++it) { - // Destroy the corresponding rigid body from the dynamics world - mDynamicsWorld->destroyRigidBody((*it)->getRigidBody()); + // Destroy the corresponding rigid body from the dynamics world + mDynamicsWorld->destroyRigidBody((*it)->getRigidBody()); - // Destroy the sphere - delete (*it); - } + // Destroy the sphere + delete (*it); + } - // Destroy all the capsules of the scene - for (std::vector::iterator it = mCapsules.begin(); it != mCapsules.end(); ++it) { + // Destroy all the capsules of the scene + for (std::vector::iterator it = mCapsules.begin(); it != mCapsules.end(); ++it) { - // Destroy the corresponding rigid body from the dynamics world - mDynamicsWorld->destroyRigidBody((*it)->getRigidBody()); + // Destroy the corresponding rigid body from the dynamics world + mDynamicsWorld->destroyRigidBody((*it)->getRigidBody()); - // Destroy the sphere - delete (*it); - } + // Destroy the sphere + delete (*it); + } - // Destroy all the convex meshes of the scene - for (std::vector::iterator it = mConvexMeshes.begin(); - it != mConvexMeshes.end(); ++it) { + // Destroy all the convex meshes of the scene + for (std::vector::iterator it = mConvexMeshes.begin(); + it != mConvexMeshes.end(); ++it) { - // Destroy the corresponding rigid body from the dynamics world - mDynamicsWorld->destroyRigidBody((*it)->getRigidBody()); + // Destroy the corresponding rigid body from the dynamics world + mDynamicsWorld->destroyRigidBody((*it)->getRigidBody()); - // Destroy the convex mesh - delete (*it); - } + // Destroy the convex mesh + delete (*it); + } - // Destroy all the dumbbell of the scene - for (std::vector::iterator it = mDumbbells.begin(); - it != mDumbbells.end(); ++it) { + // Destroy all the dumbbell of the scene + for (std::vector::iterator it = mDumbbells.begin(); + it != mDumbbells.end(); ++it) { - // Destroy the corresponding rigid body from the dynamics world - mDynamicsWorld->destroyRigidBody((*it)->getRigidBody()); + // Destroy the corresponding rigid body from the dynamics world + mDynamicsWorld->destroyRigidBody((*it)->getRigidBody()); - // Destroy the convex mesh - delete (*it); - } + // Destroy the convex mesh + delete (*it); + } - // Destroy the rigid body of the floor - mDynamicsWorld->destroyRigidBody(mFloor->getRigidBody()); - //mDynamicsWorld->destroyRigidBody(mConcaveMesh->getRigidBody()); + // Destroy the rigid body of the floor + mDynamicsWorld->destroyRigidBody(mFloor->getRigidBody()); + //mDynamicsWorld->destroyRigidBody(mConcaveMesh->getRigidBody()); - // Destroy the floor - delete mFloor; + // Destroy the floor + delete mFloor; - // Destroy the convex mesh - //delete mConcaveMesh; + // Destroy the convex mesh + //delete mConcaveMesh; - // Destroy the dynamics world - delete mDynamicsWorld; + // Destroy the dynamics world + delete mDynamicsWorld; } // Update the physics world (take a simulation step) void CollisionShapesScene::updatePhysics() { - // Update the physics engine parameters - mDynamicsWorld->setIsGratityEnabled(mEngineSettings.isGravityEnabled); - rp3d::Vector3 gravity(mEngineSettings.gravity.x, mEngineSettings.gravity.y, - mEngineSettings.gravity.z); - mDynamicsWorld->setGravity(gravity); - mDynamicsWorld->enableSleeping(mEngineSettings.isSleepingEnabled); - mDynamicsWorld->setSleepLinearVelocity(mEngineSettings.sleepLinearVelocity); - mDynamicsWorld->setSleepAngularVelocity(mEngineSettings.sleepAngularVelocity); - mDynamicsWorld->setNbIterationsPositionSolver(mEngineSettings.nbPositionSolverIterations); - mDynamicsWorld->setNbIterationsVelocitySolver(mEngineSettings.nbVelocitySolverIterations); - mDynamicsWorld->setTimeBeforeSleep(mEngineSettings.timeBeforeSleep); + // Update the physics engine parameters + mDynamicsWorld->setIsGratityEnabled(mEngineSettings.isGravityEnabled); + rp3d::Vector3 gravity(mEngineSettings.gravity.x, mEngineSettings.gravity.y, + mEngineSettings.gravity.z); + mDynamicsWorld->setGravity(gravity); + mDynamicsWorld->enableSleeping(mEngineSettings.isSleepingEnabled); + mDynamicsWorld->setSleepLinearVelocity(mEngineSettings.sleepLinearVelocity); + mDynamicsWorld->setSleepAngularVelocity(mEngineSettings.sleepAngularVelocity); + mDynamicsWorld->setNbIterationsPositionSolver(mEngineSettings.nbPositionSolverIterations); + mDynamicsWorld->setNbIterationsVelocitySolver(mEngineSettings.nbVelocitySolverIterations); + mDynamicsWorld->setTimeBeforeSleep(mEngineSettings.timeBeforeSleep); - // Take a simulation step - mDynamicsWorld->update(mEngineSettings.timeStep); + // Take a simulation step + mDynamicsWorld->update(mEngineSettings.timeStep); } // Take a step for the simulation void CollisionShapesScene::update() { - SceneDemo::update(); + SceneDemo::update(); - // Update the position and orientation of the boxes - for (std::vector::iterator it = mBoxes.begin(); it != mBoxes.end(); ++it) { + // Update the position and orientation of the boxes + for (std::vector::iterator it = mBoxes.begin(); it != mBoxes.end(); ++it) { - // Update the transform used for the rendering - (*it)->updateTransform(mInterpolationFactor); - } + // Update the transform used for the rendering + (*it)->updateTransform(mInterpolationFactor); + } - // Update the position and orientation of the sphere - for (std::vector::iterator it = mSpheres.begin(); it != mSpheres.end(); ++it) { + // Update the position and orientation of the sphere + for (std::vector::iterator it = mSpheres.begin(); it != mSpheres.end(); ++it) { - // Update the transform used for the rendering - (*it)->updateTransform(mInterpolationFactor); - } + // Update the transform used for the rendering + (*it)->updateTransform(mInterpolationFactor); + } - // Update the position and orientation of the cones - for (std::vector::iterator it = mCones.begin(); it != mCones.end(); ++it) { + // Update the position and orientation of the cones + for (std::vector::iterator it = mCones.begin(); it != mCones.end(); ++it) { - // Update the transform used for the rendering - (*it)->updateTransform(mInterpolationFactor); - } + // Update the transform used for the rendering + (*it)->updateTransform(mInterpolationFactor); + } - // Update the position and orientation of the cylinders - for (std::vector::iterator it = mCylinders.begin(); it != mCylinders.end(); ++it) { + // Update the position and orientation of the cylinders + for (std::vector::iterator it = mCylinders.begin(); it != mCylinders.end(); ++it) { - // Update the transform used for the rendering - (*it)->updateTransform(mInterpolationFactor); - } + // Update the transform used for the rendering + (*it)->updateTransform(mInterpolationFactor); + } - // Update the position and orientation of the capsules - for (std::vector::iterator it = mCapsules.begin(); it != mCapsules.end(); ++it) { + // Update the position and orientation of the capsules + for (std::vector::iterator it = mCapsules.begin(); it != mCapsules.end(); ++it) { - // Update the transform used for the rendering - (*it)->updateTransform(mInterpolationFactor); - } + // Update the transform used for the rendering + (*it)->updateTransform(mInterpolationFactor); + } - // Update the position and orientation of the convex meshes - for (std::vector::iterator it = mConvexMeshes.begin(); - it != mConvexMeshes.end(); ++it) { + // Update the position and orientation of the convex meshes + for (std::vector::iterator it = mConvexMeshes.begin(); + it != mConvexMeshes.end(); ++it) { - // Update the transform used for the rendering - (*it)->updateTransform(mInterpolationFactor); - } + // Update the transform used for the rendering + (*it)->updateTransform(mInterpolationFactor); + } - // Update the position and orientation of the dumbbells - for (std::vector::iterator it = mDumbbells.begin(); - it != mDumbbells.end(); ++it) { + // Update the position and orientation of the dumbbells + for (std::vector::iterator it = mDumbbells.begin(); + it != mDumbbells.end(); ++it) { - // Update the transform used for the rendering - (*it)->updateTransform(mInterpolationFactor); - } + // Update the transform used for the rendering + (*it)->updateTransform(mInterpolationFactor); + } - //mConcaveMesh->updateTransform(mInterpolationFactor); + //mConcaveMesh->updateTransform(mInterpolationFactor); - mFloor->updateTransform(mInterpolationFactor); + mFloor->updateTransform(mInterpolationFactor); } // Render the scene void CollisionShapesScene::renderSinglePass(openglframework::Shader& shader, - const openglframework::Matrix4& worldToCameraMatrix) { + const openglframework::Matrix4& worldToCameraMatrix) { - // Bind the shader - shader.bind(); + // Bind the shader + shader.bind(); - // Render all the boxes of the scene - for (std::vector::iterator it = mBoxes.begin(); it != mBoxes.end(); ++it) { - (*it)->render(shader, worldToCameraMatrix); - } + // Render all the boxes of the scene + for (std::vector::iterator it = mBoxes.begin(); it != mBoxes.end(); ++it) { + (*it)->render(shader, worldToCameraMatrix); + } - // Render all the sphere of the scene - for (std::vector::iterator it = mSpheres.begin(); it != mSpheres.end(); ++it) { - (*it)->render(shader, worldToCameraMatrix); - } + // Render all the sphere of the scene + for (std::vector::iterator it = mSpheres.begin(); it != mSpheres.end(); ++it) { + (*it)->render(shader, worldToCameraMatrix); + } - // Render all the cones of the scene - for (std::vector::iterator it = mCones.begin(); it != mCones.end(); ++it) { - (*it)->render(shader, worldToCameraMatrix); - } + // Render all the cones of the scene + for (std::vector::iterator it = mCones.begin(); it != mCones.end(); ++it) { + (*it)->render(shader, worldToCameraMatrix); + } - // Render all the cylinders of the scene - for (std::vector::iterator it = mCylinders.begin(); it != mCylinders.end(); ++it) { - (*it)->render(shader, worldToCameraMatrix); - } + // Render all the cylinders of the scene + for (std::vector::iterator it = mCylinders.begin(); it != mCylinders.end(); ++it) { + (*it)->render(shader, worldToCameraMatrix); + } - // Render all the capsules of the scene - for (std::vector::iterator it = mCapsules.begin(); it != mCapsules.end(); ++it) { - (*it)->render(shader, worldToCameraMatrix); - } + // Render all the capsules of the scene + for (std::vector::iterator it = mCapsules.begin(); it != mCapsules.end(); ++it) { + (*it)->render(shader, worldToCameraMatrix); + } - // Render all the convex meshes of the scene - for (std::vector::iterator it = mConvexMeshes.begin(); - it != mConvexMeshes.end(); ++it) { - (*it)->render(shader, worldToCameraMatrix); - } + // Render all the convex meshes of the scene + for (std::vector::iterator it = mConvexMeshes.begin(); + it != mConvexMeshes.end(); ++it) { + (*it)->render(shader, worldToCameraMatrix); + } - // Render all the dumbbells of the scene - for (std::vector::iterator it = mDumbbells.begin(); - it != mDumbbells.end(); ++it) { - (*it)->render(shader, worldToCameraMatrix); - } + // Render all the dumbbells of the scene + for (std::vector::iterator it = mDumbbells.begin(); + it != mDumbbells.end(); ++it) { + (*it)->render(shader, worldToCameraMatrix); + } - // Render the floor - mFloor->render(shader, worldToCameraMatrix); + // Render the floor + mFloor->render(shader, worldToCameraMatrix); - //mConcaveMesh->render(shader, worldToCameraMatrix); + //mConcaveMesh->render(shader, worldToCameraMatrix); - // Unbind the shader - shader.unbind(); + // Unbind the shader + shader.unbind(); } /// Reset the scene void CollisionShapesScene::reset() { - float radius = 3.0f; + float radius = 3.0f; - for (int i=0; iresetTransform(transform); - } + // Reset the transform + mDumbbells[i]->resetTransform(transform); + } - // Create all the boxes of the scene - for (int i=0; iresetTransform(transform); - } + // Reset the transform + mBoxes[i]->resetTransform(transform); + } - // Create all the spheres of the scene - for (int i=0; iresetTransform(transform); - } + // Reset the transform + mSpheres[i]->resetTransform(transform); + } - // Create all the cones of the scene - for (int i=0; iresetTransform(transform); - } + // Reset the transform + mCones[i]->resetTransform(transform); + } - // Create all the cylinders of the scene - for (int i=0; iresetTransform(transform); - } + // Reset the transform + mCylinders[i]->resetTransform(transform); + } - // Create all the capsules of the scene - for (int i=0; iresetTransform(transform); - } + // Reset the transform + mCapsules[i]->resetTransform(transform); + } - // Create all the convex meshes of the scene - for (int i=0; iresetTransform(transform); - } + // Reset the transform + mConvexMeshes[i]->resetTransform(transform); + } } diff --git a/tools/testbed/scenes/collisionshapes/CollisionShapesScene.h b/tools/testbed/scenes/collisionshapes/CollisionShapesScene.h index fae70fb..4e47e53 100644 --- a/tools/testbed/scenes/collisionshapes/CollisionShapesScene.h +++ b/tools/testbed/scenes/collisionshapes/CollisionShapesScene.h @@ -1,26 +1,26 @@ /******************************************************************************** -* ReactPhysics3D physics library, http://www.reactphysics3d.com * -* Copyright (c) 2010-2016 Daniel Chappuis * +* 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: * -* * +* 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. * -* * +* 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 COLLISION_SHAPES_SCENE_H @@ -44,13 +44,13 @@ namespace collisionshapesscene { // Constants const float SCENE_RADIUS = 30.0f; -const int NB_BOXES = 5; -const int NB_SPHERES = 5; -const int NB_CONES = 5; -const int NB_CYLINDERS = 5; -const int NB_CAPSULES = 5; -const int NB_MESHES = 3; -const int NB_COMPOUND_SHAPES = 3; +const int32_t NB_BOXES = 5; +const int32_t NB_SPHERES = 5; +const int32_t NB_CONES = 5; +const int32_t NB_CYLINDERS = 5; +const int32_t NB_CAPSULES = 5; +const int32_t NB_MESHES = 3; +const int32_t NB_COMPOUND_SHAPES = 3; const openglframework::Vector3 BOX_SIZE(2, 2, 2); const float SPHERE_RADIUS = 1.5f; const float CONE_RADIUS = 2.0f; @@ -60,75 +60,75 @@ const float CYLINDER_HEIGHT = 5.0f; const float CAPSULE_RADIUS = 1.0f; const float CAPSULE_HEIGHT = 1.0f; const float DUMBBELL_HEIGHT = 1.0f; -const openglframework::Vector3 FLOOR_SIZE(50, 0.5f, 50); // Floor dimensions in meters +const openglframework::Vector3 FLOOR_SIZE(50, 0.5f, 50); // Floor dimensions in meters const float BOX_MASS = 1.0f; const float CONE_MASS = 1.0f; const float CYLINDER_MASS = 1.0f; const float CAPSULE_MASS = 1.0f; const float MESH_MASS = 1.0f; -const float FLOOR_MASS = 100.0f; // Floor mass in kilograms +const float FLOOR_MASS = 100.0f; // Floor mass in kilograms // Class CollisionShapesScene class CollisionShapesScene : public SceneDemo { - private : + private : - // -------------------- Attributes -------------------- // + // -------------------- Attributes -------------------- // - /// All the spheres of the scene - std::vector mBoxes; + /// All the spheres of the scene + std::vector mBoxes; - std::vector mSpheres; + std::vector mSpheres; - std::vector mCones; + std::vector mCones; - std::vector mCylinders; + std::vector mCylinders; - std::vector mCapsules; + std::vector mCapsules; - /// All the convex meshes of the scene - std::vector mConvexMeshes; + /// All the convex meshes of the scene + std::vector mConvexMeshes; - /// All the dumbbell of the scene - std::vector mDumbbells; + /// All the dumbbell of the scene + std::vector mDumbbells; - /// Box for the floor - Box* mFloor; + /// Box for the floor + Box* mFloor; - /// Dynamics world used for the physics simulation - rp3d::DynamicsWorld* mDynamicsWorld; + /// Dynamics world used for the physics simulation + rp3d::DynamicsWorld* mDynamicsWorld; - public: + public: - // -------------------- Methods -------------------- // + // -------------------- Methods -------------------- // - /// Constructor - CollisionShapesScene(const std::string& name); + /// Constructor + CollisionShapesScene(const std::string& name); - /// Destructor - virtual ~CollisionShapesScene(); + /// Destructor + virtual ~CollisionShapesScene(); - /// Update the physics world (take a simulation step) - /// Can be called several times per frame - virtual void updatePhysics(); + /// Update the physics world (take a simulation step) + /// Can be called several times per frame + virtual void updatePhysics(); - /// Take a step for the simulation - virtual void update(); + /// Take a step for the simulation + virtual void update(); - /// Render the scene in a single pass - virtual void renderSinglePass(openglframework::Shader& shader, - const openglframework::Matrix4& worldToCameraMatrix); + /// Render the scene in a single pass + virtual void renderSinglePass(openglframework::Shader& shader, + const openglframework::Matrix4& worldToCameraMatrix); - /// Reset the scene - virtual void reset(); + /// Reset the scene + virtual void reset(); - /// Return all the contact points of the scene - virtual std::vector getContactPoints() const; + /// Return all the contact points of the scene + virtual std::vector getContactPoints() const; }; // Return all the contact points of the scene inline std::vector CollisionShapesScene::getContactPoints() const { - return computeContactPointsOfWorld(mDynamicsWorld); + return computeContactPointsOfWorld(mDynamicsWorld); } } diff --git a/tools/testbed/scenes/concavemesh/ConcaveMeshScene.cpp b/tools/testbed/scenes/concavemesh/ConcaveMeshScene.cpp index 4023dd4..67e9d28 100644 --- a/tools/testbed/scenes/concavemesh/ConcaveMeshScene.cpp +++ b/tools/testbed/scenes/concavemesh/ConcaveMeshScene.cpp @@ -1,26 +1,26 @@ /******************************************************************************** -* ReactPhysics3D physics library, http://www.reactphysics3d.com * -* Copyright (c) 2010-2016 Daniel Chappuis * +* 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: * -* * +* 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. * -* * +* 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 @@ -32,163 +32,163 @@ using namespace trianglemeshscene; // Constructor ConcaveMeshScene::ConcaveMeshScene(const std::string& name) - : SceneDemo(name, SCENE_RADIUS) { + : SceneDemo(name, SCENE_RADIUS) { - std::string meshFolderPath("meshes/"); + std::string meshFolderPath("meshes/"); - // Compute the radius and the center of the scene - openglframework::Vector3 center(0, 5, 0); + // 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); + // Set the center of the scene + setScenePosition(center, SCENE_RADIUS); - // Gravity vector in the dynamics world - rp3d::Vector3 gravity(0, rp3d::decimal(-9.81), 0); + // Gravity vector in the dynamics world + rp3d::Vector3 gravity(0, rp3d::float(-9.81), 0); - // Create the dynamics world for the physics simulation - mDynamicsWorld = new rp3d::DynamicsWorld(gravity); + // Create the dynamics world for the physics simulation + mDynamicsWorld = new rp3d::DynamicsWorld(gravity); - // Set the number of iterations of the constraint solver - mDynamicsWorld->setNbIterationsVelocitySolver(15); + // Set the number of iterations of the constraint solver + mDynamicsWorld->setNbIterationsVelocitySolver(15); - // ---------- Create the boxes ----------- // + // ---------- Create the boxes ----------- // - for (int i=0; isetColor(mDemoColors[0]); - mBoxes[i * NB_BOXES_Z + j]->setSleepingColor(mRedColorDemo); + // Set the sphere color + mBoxes[i * NB_BOXES_Z + j]->setColor(mDemoColors[0]); + mBoxes[i * NB_BOXES_Z + j]->setSleepingColor(mRedColorDemo); - // Change the material properties of the rigid body - rp3d::Material& boxMaterial = mBoxes[i * NB_BOXES_Z + j]->getRigidBody()->getMaterial(); - boxMaterial.setBounciness(rp3d::decimal(0.2)); - } - } + // Change the material properties of the rigid body + rp3d::Material& boxMaterial = mBoxes[i * NB_BOXES_Z + j]->getRigidBody()->getMaterial(); + boxMaterial.setBounciness(rp3d::float(0.2)); + } + } - // ---------- Create the triangular mesh ---------- // + // ---------- Create the triangular mesh ---------- // - // Position - openglframework::Vector3 position(0, 0, 0); - rp3d::decimal mass = 1.0; + // Position + openglframework::Vector3 position(0, 0, 0); + rp3d::float mass = 1.0; - // Create a convex mesh and a corresponding rigid in the dynamics world - mConcaveMesh = new ConcaveMesh(position, mass, mDynamicsWorld, meshFolderPath + "city.obj"); + // Create a convex mesh and a corresponding rigid in the dynamics world + mConcaveMesh = new ConcaveMesh(position, mass, mDynamicsWorld, meshFolderPath + "city.obj"); - // Set the mesh as beeing static - mConcaveMesh->getRigidBody()->setType(rp3d::STATIC); + // Set the mesh as beeing static + mConcaveMesh->getRigidBody()->setType(rp3d::STATIC); - // Set the box color - mConcaveMesh->setColor(mGreyColorDemo); - mConcaveMesh->setSleepingColor(mGreyColorDemo); + // Set the box color + mConcaveMesh->setColor(mGreyColorDemo); + mConcaveMesh->setSleepingColor(mGreyColorDemo); - // Change the material properties of the rigid body - rp3d::Material& material = mConcaveMesh->getRigidBody()->getMaterial(); - material.setBounciness(rp3d::decimal(0.2)); - material.setFrictionCoefficient(0.1); + // Change the material properties of the rigid body + rp3d::Material& material = mConcaveMesh->getRigidBody()->getMaterial(); + material.setBounciness(rp3d::float(0.2)); + material.setFrictionCoefficient(0.1); - // Get the physics engine parameters - mEngineSettings.isGravityEnabled = mDynamicsWorld->isGravityEnabled(); - rp3d::Vector3 gravityVector = mDynamicsWorld->getGravity(); - mEngineSettings.gravity = openglframework::Vector3(gravityVector.x, gravityVector.y, gravityVector.z); - mEngineSettings.isSleepingEnabled = mDynamicsWorld->isSleepingEnabled(); - mEngineSettings.sleepLinearVelocity = mDynamicsWorld->getSleepLinearVelocity(); - mEngineSettings.sleepAngularVelocity = mDynamicsWorld->getSleepAngularVelocity(); - mEngineSettings.nbPositionSolverIterations = mDynamicsWorld->getNbIterationsPositionSolver(); - mEngineSettings.nbVelocitySolverIterations = mDynamicsWorld->getNbIterationsVelocitySolver(); - mEngineSettings.timeBeforeSleep = mDynamicsWorld->getTimeBeforeSleep(); + // Get the physics engine parameters + mEngineSettings.isGravityEnabled = mDynamicsWorld->isGravityEnabled(); + rp3d::Vector3 gravityVector = mDynamicsWorld->getGravity(); + mEngineSettings.gravity = openglframework::Vector3(gravityVector.x, gravityVector.y, gravityVector.z); + mEngineSettings.isSleepingEnabled = mDynamicsWorld->isSleepingEnabled(); + mEngineSettings.sleepLinearVelocity = mDynamicsWorld->getSleepLinearVelocity(); + mEngineSettings.sleepAngularVelocity = mDynamicsWorld->getSleepAngularVelocity(); + mEngineSettings.nbPositionSolverIterations = mDynamicsWorld->getNbIterationsPositionSolver(); + mEngineSettings.nbVelocitySolverIterations = mDynamicsWorld->getNbIterationsVelocitySolver(); + mEngineSettings.timeBeforeSleep = mDynamicsWorld->getTimeBeforeSleep(); } // Destructor ConcaveMeshScene::~ConcaveMeshScene() { - // Destroy the corresponding rigid body from the dynamics world - mDynamicsWorld->destroyRigidBody(mConcaveMesh->getRigidBody()); + // Destroy the corresponding rigid body from the dynamics world + mDynamicsWorld->destroyRigidBody(mConcaveMesh->getRigidBody()); - // Destroy the boxes - for (int i=0; idestroyRigidBody(mBoxes[i]->getRigidBody()); - delete mBoxes[i]; - } + // Destroy the boxes + for (int32_t i=0; idestroyRigidBody(mBoxes[i]->getRigidBody()); + delete mBoxes[i]; + } - // Destroy the convex mesh - delete mConcaveMesh; + // Destroy the convex mesh + delete mConcaveMesh; - // Destroy the dynamics world - delete mDynamicsWorld; + // Destroy the dynamics world + delete mDynamicsWorld; } // Update the physics world (take a simulation step) void ConcaveMeshScene::updatePhysics() { - // Update the physics engine parameters - mDynamicsWorld->setIsGratityEnabled(mEngineSettings.isGravityEnabled); - rp3d::Vector3 gravity(mEngineSettings.gravity.x, mEngineSettings.gravity.y, - mEngineSettings.gravity.z); - mDynamicsWorld->setGravity(gravity); - mDynamicsWorld->enableSleeping(mEngineSettings.isSleepingEnabled); - mDynamicsWorld->setSleepLinearVelocity(mEngineSettings.sleepLinearVelocity); - mDynamicsWorld->setSleepAngularVelocity(mEngineSettings.sleepAngularVelocity); - mDynamicsWorld->setNbIterationsPositionSolver(mEngineSettings.nbPositionSolverIterations); - mDynamicsWorld->setNbIterationsVelocitySolver(mEngineSettings.nbVelocitySolverIterations); - mDynamicsWorld->setTimeBeforeSleep(mEngineSettings.timeBeforeSleep); + // Update the physics engine parameters + mDynamicsWorld->setIsGratityEnabled(mEngineSettings.isGravityEnabled); + rp3d::Vector3 gravity(mEngineSettings.gravity.x, mEngineSettings.gravity.y, + mEngineSettings.gravity.z); + mDynamicsWorld->setGravity(gravity); + mDynamicsWorld->enableSleeping(mEngineSettings.isSleepingEnabled); + mDynamicsWorld->setSleepLinearVelocity(mEngineSettings.sleepLinearVelocity); + mDynamicsWorld->setSleepAngularVelocity(mEngineSettings.sleepAngularVelocity); + mDynamicsWorld->setNbIterationsPositionSolver(mEngineSettings.nbPositionSolverIterations); + mDynamicsWorld->setNbIterationsVelocitySolver(mEngineSettings.nbVelocitySolverIterations); + mDynamicsWorld->setTimeBeforeSleep(mEngineSettings.timeBeforeSleep); - // Take a simulation step - mDynamicsWorld->update(mEngineSettings.timeStep); + // Take a simulation step + mDynamicsWorld->update(mEngineSettings.timeStep); } // Update the scene void ConcaveMeshScene::update() { - SceneDemo::update(); + SceneDemo::update(); - // Update the transform used for the rendering - mConcaveMesh->updateTransform(mInterpolationFactor); + // Update the transform used for the rendering + mConcaveMesh->updateTransform(mInterpolationFactor); - for (int i=0; iupdateTransform(mInterpolationFactor); - } + for (int32_t i=0; iupdateTransform(mInterpolationFactor); + } } // Render the scene in a single pass void ConcaveMeshScene::renderSinglePass(Shader& shader, const openglframework::Matrix4& worldToCameraMatrix) { - // Bind the shader - shader.bind(); + // Bind the shader + shader.bind(); - mConcaveMesh->render(shader, worldToCameraMatrix); + mConcaveMesh->render(shader, worldToCameraMatrix); - for (int i=0; irender(shader, worldToCameraMatrix); - } + for (int32_t i=0; irender(shader, worldToCameraMatrix); + } - // Unbind the shader - shader.unbind(); + // Unbind the shader + shader.unbind(); } // Reset the scene void ConcaveMeshScene::reset() { - // Reset the transform - rp3d::Transform transform(rp3d::Vector3::zero(), rp3d::Quaternion::identity()); - mConcaveMesh->resetTransform(transform); + // Reset the transform + rp3d::Transform transform(rp3d::Vector3::zero(), rp3d::Quaternion::identity()); + mConcaveMesh->resetTransform(transform); - for (int i=0; iresetTransform(boxTransform); - } - } + rp3d::Transform boxTransform(boxPosition, rp3d::Quaternion::identity()); + mBoxes[i * NB_BOXES_Z + j]->resetTransform(boxTransform); + } + } } diff --git a/tools/testbed/scenes/concavemesh/ConcaveMeshScene.h b/tools/testbed/scenes/concavemesh/ConcaveMeshScene.h index be26f99..2dd22ef 100644 --- a/tools/testbed/scenes/concavemesh/ConcaveMeshScene.h +++ b/tools/testbed/scenes/concavemesh/ConcaveMeshScene.h @@ -1,26 +1,26 @@ /******************************************************************************** -* ReactPhysics3D physics library, http://www.reactphysics3d.com * -* Copyright (c) 2010-2016 Daniel Chappuis * +* 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: * -* * +* 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. * -* * +* 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 TRIANGLE_MESH_SCENE_H @@ -37,58 +37,58 @@ namespace trianglemeshscene { // Constants -const float SCENE_RADIUS = 70.0f; // Radius of the scene in meters -const int NB_BOXES_X = 8; -const int NB_BOXES_Z = 8; +const float SCENE_RADIUS = 70.0f; // Radius of the scene in meters +const int32_t NB_BOXES_X = 8; +const int32_t NB_BOXES_Z = 8; const float BOX_SIZE = 3.0f; const float BOXES_SPACE = 2.0f; // Class TriangleMeshScene class ConcaveMeshScene : public SceneDemo { - protected : + protected : - // -------------------- Attributes -------------------- // + // -------------------- Attributes -------------------- // - Box* mBoxes[NB_BOXES_X * NB_BOXES_Z]; + Box* mBoxes[NB_BOXES_X * NB_BOXES_Z]; - /// Concave triangles mesh - ConcaveMesh* mConcaveMesh; + /// Concave triangles mesh + ConcaveMesh* mConcaveMesh; - /// Dynamics world used for the physics simulation - rp3d::DynamicsWorld* mDynamicsWorld; + /// Dynamics world used for the physics simulation + rp3d::DynamicsWorld* mDynamicsWorld; - public: + public: - // -------------------- Methods -------------------- // + // -------------------- Methods -------------------- // - /// Constructor - ConcaveMeshScene(const std::string& name); + /// Constructor + ConcaveMeshScene(const std::string& name); - /// Destructor - virtual ~ConcaveMeshScene(); + /// Destructor + virtual ~ConcaveMeshScene(); - /// Update the physics world (take a simulation step) - /// Can be called several times per frame - virtual void updatePhysics(); + /// Update the physics world (take a simulation step) + /// Can be called several times per frame + virtual void updatePhysics(); - /// Update the scene (take a simulation step) - virtual void update(); + /// Update the scene (take a simulation step) + virtual void update(); - /// Render the scene in a single pass - virtual void renderSinglePass(openglframework::Shader& shader, - const openglframework::Matrix4& worldToCameraMatrix); + /// Render the scene in a single pass + virtual void renderSinglePass(openglframework::Shader& shader, + const openglframework::Matrix4& worldToCameraMatrix); - /// Reset the scene - virtual void reset(); + /// Reset the scene + virtual void reset(); - /// Return all the contact points of the scene - virtual std::vector getContactPoints() const; + /// Return all the contact points of the scene + virtual std::vector getContactPoints() const; }; // Return all the contact points of the scene inline std::vector ConcaveMeshScene::getContactPoints() const { - return computeContactPointsOfWorld(mDynamicsWorld); + return computeContactPointsOfWorld(mDynamicsWorld); } } diff --git a/tools/testbed/scenes/cubes/CubesScene.cpp b/tools/testbed/scenes/cubes/CubesScene.cpp index e1b7e2f..1a4e6a5 100644 --- a/tools/testbed/scenes/cubes/CubesScene.cpp +++ b/tools/testbed/scenes/cubes/CubesScene.cpp @@ -1,26 +1,26 @@ /******************************************************************************** -* ReactPhysics3D physics library, http://www.reactphysics3d.com * -* Copyright (c) 2010-2016 Daniel Chappuis * +* 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: * -* * +* 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. * -* * +* 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 @@ -32,167 +32,167 @@ using namespace cubesscene; // Constructor CubesScene::CubesScene(const std::string& name) - : SceneDemo(name, SCENE_RADIUS) { + : SceneDemo(name, SCENE_RADIUS) { - // Compute the radius and the center of the scene - openglframework::Vector3 center(0, 5, 0); + // 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); + // Set the center of the scene + setScenePosition(center, SCENE_RADIUS); - // Gravity vector in the dynamics world - rp3d::Vector3 gravity(0, rp3d::decimal(-9.81), 0); + // Gravity vector in the dynamics world + rp3d::Vector3 gravity(0, rp3d::float(-9.81), 0); - // Create the dynamics world for the physics simulation - m_dynamicsWorld = new rp3d::DynamicsWorld(gravity); + // Create the dynamics world for the physics simulation + m_dynamicsWorld = new rp3d::DynamicsWorld(gravity); - // Set the number of iterations of the constraint solver - m_dynamicsWorld->setNbIterationsVelocitySolver(15); + // Set the number of iterations of the constraint solver + m_dynamicsWorld->setNbIterationsVelocitySolver(15); - float radius = 2.0f; + float radius = 2.0f; - // Create all the cubes of the scene - for (int i=0; isetColor(mDemoColors[i % mNbDemoColors]); - cube->setSleepingColor(mRedColorDemo); + // Set the box color + cube->setColor(mDemoColors[i % mNbDemoColors]); + cube->setSleepingColor(mRedColorDemo); - // Change the material properties of the rigid body - rp3d::Material& material = cube->getRigidBody()->getMaterial(); - material.setBounciness(rp3d::decimal(0.4)); + // Change the material properties of the rigid body + rp3d::Material& material = cube->getRigidBody()->getMaterial(); + material.setBounciness(rp3d::float(0.4)); - // Add the box the list of box in the scene - mBoxes.push_back(cube); - } + // Add the box the list of box in the scene + mBoxes.push_back(cube); + } - // Create the floor - openglframework::Vector3 floorPosition(0, 0, 0); - mFloor = new Box(FLOOR_SIZE, floorPosition, FLOOR_MASS, m_dynamicsWorld); - mFloor->setColor(mGreyColorDemo); - mFloor->setSleepingColor(mGreyColorDemo); + // Create the floor + openglframework::Vector3 floorPosition(0, 0, 0); + mFloor = new Box(FLOOR_SIZE, floorPosition, FLOOR_MASS, m_dynamicsWorld); + mFloor->setColor(mGreyColorDemo); + mFloor->setSleepingColor(mGreyColorDemo); - // The floor must be a static rigid body - mFloor->getRigidBody()->setType(rp3d::STATIC); + // The floor must be a static rigid body + mFloor->getRigidBody()->setType(rp3d::STATIC); - // Change the material properties of the floor rigid body - rp3d::Material& material = mFloor->getRigidBody()->getMaterial(); - material.setBounciness(rp3d::decimal(0.3)); + // Change the material properties of the floor rigid body + rp3d::Material& material = mFloor->getRigidBody()->getMaterial(); + material.setBounciness(rp3d::float(0.3)); - // Get the physics engine parameters - mEngineSettings.isGravityEnabled = m_dynamicsWorld->isGravityEnabled(); - rp3d::Vector3 gravityVector = m_dynamicsWorld->getGravity(); - mEngineSettings.gravity = openglframework::Vector3(gravityVector.x, gravityVector.y, gravityVector.z); - mEngineSettings.isSleepingEnabled = m_dynamicsWorld->isSleepingEnabled(); - mEngineSettings.sleepLinearVelocity = m_dynamicsWorld->getSleepLinearVelocity(); - mEngineSettings.sleepAngularVelocity = m_dynamicsWorld->getSleepAngularVelocity(); - mEngineSettings.nbPositionSolverIterations = m_dynamicsWorld->getNbIterationsPositionSolver(); - mEngineSettings.nbVelocitySolverIterations = m_dynamicsWorld->getNbIterationsVelocitySolver(); - mEngineSettings.timeBeforeSleep = m_dynamicsWorld->getTimeBeforeSleep(); + // Get the physics engine parameters + mEngineSettings.isGravityEnabled = m_dynamicsWorld->isGravityEnabled(); + rp3d::Vector3 gravityVector = m_dynamicsWorld->getGravity(); + mEngineSettings.gravity = openglframework::Vector3(gravityVector.x, gravityVector.y, gravityVector.z); + mEngineSettings.isSleepingEnabled = m_dynamicsWorld->isSleepingEnabled(); + mEngineSettings.sleepLinearVelocity = m_dynamicsWorld->getSleepLinearVelocity(); + mEngineSettings.sleepAngularVelocity = m_dynamicsWorld->getSleepAngularVelocity(); + mEngineSettings.nbPositionSolverIterations = m_dynamicsWorld->getNbIterationsPositionSolver(); + mEngineSettings.nbVelocitySolverIterations = m_dynamicsWorld->getNbIterationsVelocitySolver(); + mEngineSettings.timeBeforeSleep = m_dynamicsWorld->getTimeBeforeSleep(); } // Destructor CubesScene::~CubesScene() { - // Destroy all the cubes of the scene - for (std::vector::iterator it = mBoxes.begin(); it != mBoxes.end(); ++it) { + // Destroy all the cubes of the scene + for (std::vector::iterator it = mBoxes.begin(); it != mBoxes.end(); ++it) { - // Destroy the corresponding rigid body from the dynamics world - m_dynamicsWorld->destroyRigidBody((*it)->getRigidBody()); + // Destroy the corresponding rigid body from the dynamics world + m_dynamicsWorld->destroyRigidBody((*it)->getRigidBody()); - // Destroy the cube - delete (*it); - } + // Destroy the cube + delete (*it); + } - // Destroy the rigid body of the floor - m_dynamicsWorld->destroyRigidBody(mFloor->getRigidBody()); + // Destroy the rigid body of the floor + m_dynamicsWorld->destroyRigidBody(mFloor->getRigidBody()); - // Destroy the floor - delete mFloor; + // Destroy the floor + delete mFloor; - // Destroy the dynamics world - delete m_dynamicsWorld; + // Destroy the dynamics world + delete m_dynamicsWorld; } // Update the physics world (take a simulation step) void CubesScene::updatePhysics() { - // Update the physics engine parameters - m_dynamicsWorld->setIsGratityEnabled(mEngineSettings.isGravityEnabled); - rp3d::Vector3 gravity(mEngineSettings.gravity.x, mEngineSettings.gravity.y, - mEngineSettings.gravity.z); - m_dynamicsWorld->setGravity(gravity); - m_dynamicsWorld->enableSleeping(mEngineSettings.isSleepingEnabled); - m_dynamicsWorld->setSleepLinearVelocity(mEngineSettings.sleepLinearVelocity); - m_dynamicsWorld->setSleepAngularVelocity(mEngineSettings.sleepAngularVelocity); - m_dynamicsWorld->setNbIterationsPositionSolver(mEngineSettings.nbPositionSolverIterations); - m_dynamicsWorld->setNbIterationsVelocitySolver(mEngineSettings.nbVelocitySolverIterations); - m_dynamicsWorld->setTimeBeforeSleep(mEngineSettings.timeBeforeSleep); + // Update the physics engine parameters + m_dynamicsWorld->setIsGratityEnabled(mEngineSettings.isGravityEnabled); + rp3d::Vector3 gravity(mEngineSettings.gravity.x, mEngineSettings.gravity.y, + mEngineSettings.gravity.z); + m_dynamicsWorld->setGravity(gravity); + m_dynamicsWorld->enableSleeping(mEngineSettings.isSleepingEnabled); + m_dynamicsWorld->setSleepLinearVelocity(mEngineSettings.sleepLinearVelocity); + m_dynamicsWorld->setSleepAngularVelocity(mEngineSettings.sleepAngularVelocity); + m_dynamicsWorld->setNbIterationsPositionSolver(mEngineSettings.nbPositionSolverIterations); + m_dynamicsWorld->setNbIterationsVelocitySolver(mEngineSettings.nbVelocitySolverIterations); + m_dynamicsWorld->setTimeBeforeSleep(mEngineSettings.timeBeforeSleep); - // Take a simulation step - m_dynamicsWorld->update(mEngineSettings.timeStep); + // Take a simulation step + m_dynamicsWorld->update(mEngineSettings.timeStep); } // Update the scene void CubesScene::update() { - SceneDemo::update(); + SceneDemo::update(); - // Update the position and orientation of the boxes - for (std::vector::iterator it = mBoxes.begin(); it != mBoxes.end(); ++it) { + // Update the position and orientation of the boxes + for (std::vector::iterator it = mBoxes.begin(); it != mBoxes.end(); ++it) { - // Update the transform used for the rendering - (*it)->updateTransform(mInterpolationFactor); - } + // Update the transform used for the rendering + (*it)->updateTransform(mInterpolationFactor); + } - mFloor->updateTransform(mInterpolationFactor); + mFloor->updateTransform(mInterpolationFactor); } // Render the scene in a single pass void CubesScene::renderSinglePass(Shader& shader, const openglframework::Matrix4& worldToCameraMatrix) { - // Bind the shader - shader.bind(); + // Bind the shader + shader.bind(); - // Render all the cubes of the scene - for (std::vector::iterator it = mBoxes.begin(); it != mBoxes.end(); ++it) { - (*it)->render(shader, worldToCameraMatrix); - } + // Render all the cubes of the scene + for (std::vector::iterator it = mBoxes.begin(); it != mBoxes.end(); ++it) { + (*it)->render(shader, worldToCameraMatrix); + } - // Render the floor - mFloor->render(shader, worldToCameraMatrix); + // Render the floor + mFloor->render(shader, worldToCameraMatrix); - // Unbind the shader - shader.unbind(); + // Unbind the shader + shader.unbind(); } // Reset the scene void CubesScene::reset() { - float radius = 2.0f; + float radius = 2.0f; - for (int i=0; iresetTransform(transform); - } + mBoxes[i]->resetTransform(transform); + } } diff --git a/tools/testbed/scenes/cubes/CubesScene.h b/tools/testbed/scenes/cubes/CubesScene.h index f03899c..28bbd50 100644 --- a/tools/testbed/scenes/cubes/CubesScene.h +++ b/tools/testbed/scenes/cubes/CubesScene.h @@ -1,26 +1,26 @@ /******************************************************************************** -* ReactPhysics3D physics library, http://www.reactphysics3d.com * -* Copyright (c) 2010-2016 Daniel Chappuis * +* 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: * -* * +* 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. * -* * +* 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 CUBES_SCENE_H @@ -35,7 +35,7 @@ namespace cubesscene { // Constants const float SCENE_RADIUS = 30.0f; // Radius of the scene in meters - const int NB_CUBES = 30; // Number of boxes in the scene + const int32_t NB_CUBES = 30; // Number of boxes in the scene const openglframework::Vector3 BOX_SIZE(2, 2, 2); // Box dimensions in meters const openglframework::Vector3 FLOOR_SIZE(50, 0.5f, 50); // Floor dimensions in meters const float BOX_MASS = 1.0f; // Box mass in kilograms @@ -61,7 +61,7 @@ namespace cubesscene { virtual void update(); /// Render the scene in a single pass virtual void renderSinglePass(openglframework::Shader& shader, - const openglframework::Matrix4& worldToCameraMatrix); + const openglframework::Matrix4& worldToCameraMatrix); /// Reset the scene virtual void reset(); /// Return all the contact points of the scene diff --git a/tools/testbed/scenes/heightfield/HeightFieldScene.cpp b/tools/testbed/scenes/heightfield/HeightFieldScene.cpp index 5f09657..713a1d1 100644 --- a/tools/testbed/scenes/heightfield/HeightFieldScene.cpp +++ b/tools/testbed/scenes/heightfield/HeightFieldScene.cpp @@ -1,26 +1,26 @@ /******************************************************************************** -* ReactPhysics3D physics library, http://www.reactphysics3d.com * -* Copyright (c) 2010-2016 Daniel Chappuis * +* 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: * -* * +* 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. * -* * +* 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 @@ -33,154 +33,154 @@ using namespace heightfieldscene; // Constructor HeightFieldScene::HeightFieldScene(const std::string& name) : SceneDemo(name, SCENE_RADIUS) { - // Compute the radius and the center of the scene - openglframework::Vector3 center(0, 5, 0); + // 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); + // Set the center of the scene + setScenePosition(center, SCENE_RADIUS); - // Gravity vector in the dynamics world - rp3d::Vector3 gravity(0, rp3d::decimal(-9.81), 0); + // Gravity vector in the dynamics world + rp3d::Vector3 gravity(0, rp3d::float(-9.81), 0); - // Create the dynamics world for the physics simulation - mDynamicsWorld = new rp3d::DynamicsWorld(gravity); + // Create the dynamics world for the physics simulation + mDynamicsWorld = new rp3d::DynamicsWorld(gravity); - // Set the number of iterations of the constraint solver - mDynamicsWorld->setNbIterationsVelocitySolver(15); + // Set the number of iterations of the constraint solver + mDynamicsWorld->setNbIterationsVelocitySolver(15); - // ---------- Create the boxes ----------- // + // ---------- Create the boxes ----------- // - // For each box - for (int i=0; isetColor(mDemoColors[2]); - mBoxes[i]->setSleepingColor(mRedColorDemo); + // Set the box color + mBoxes[i]->setColor(mDemoColors[2]); + mBoxes[i]->setSleepingColor(mRedColorDemo); - // Change the material properties of the rigid body - rp3d::Material& boxMaterial = mBoxes[i]->getRigidBody()->getMaterial(); - boxMaterial.setBounciness(rp3d::decimal(0.2)); - } + // Change the material properties of the rigid body + rp3d::Material& boxMaterial = mBoxes[i]->getRigidBody()->getMaterial(); + boxMaterial.setBounciness(rp3d::float(0.2)); + } - // ---------- Create the height field ---------- // + // ---------- Create the height field ---------- // - // Position - openglframework::Vector3 position(0, 0, 0); - rp3d::decimal mass = 1.0; + // Position + openglframework::Vector3 position(0, 0, 0); + rp3d::float mass = 1.0; - // Create a convex mesh and a corresponding rigid in the dynamics world - mHeightField = new HeightField(position, mass, mDynamicsWorld); + // Create a convex mesh and a corresponding rigid in the dynamics world + mHeightField = new HeightField(position, mass, mDynamicsWorld); - // Set the mesh as beeing static - mHeightField->getRigidBody()->setType(rp3d::STATIC); + // Set the mesh as beeing static + mHeightField->getRigidBody()->setType(rp3d::STATIC); - // Set the color - mHeightField->setColor(mGreyColorDemo); - mHeightField->setSleepingColor(mGreyColorDemo); + // Set the color + mHeightField->setColor(mGreyColorDemo); + mHeightField->setSleepingColor(mGreyColorDemo); - // Change the material properties of the rigid body - rp3d::Material& material = mHeightField->getRigidBody()->getMaterial(); - material.setBounciness(rp3d::decimal(0.2)); - material.setFrictionCoefficient(0.1); + // Change the material properties of the rigid body + rp3d::Material& material = mHeightField->getRigidBody()->getMaterial(); + material.setBounciness(rp3d::float(0.2)); + material.setFrictionCoefficient(0.1); - // Get the physics engine parameters - mEngineSettings.isGravityEnabled = mDynamicsWorld->isGravityEnabled(); - rp3d::Vector3 gravityVector = mDynamicsWorld->getGravity(); - mEngineSettings.gravity = openglframework::Vector3(gravityVector.x, gravityVector.y, gravityVector.z); - mEngineSettings.isSleepingEnabled = mDynamicsWorld->isSleepingEnabled(); - mEngineSettings.sleepLinearVelocity = mDynamicsWorld->getSleepLinearVelocity(); - mEngineSettings.sleepAngularVelocity = mDynamicsWorld->getSleepAngularVelocity(); - mEngineSettings.nbPositionSolverIterations = mDynamicsWorld->getNbIterationsPositionSolver(); - mEngineSettings.nbVelocitySolverIterations = mDynamicsWorld->getNbIterationsVelocitySolver(); - mEngineSettings.timeBeforeSleep = mDynamicsWorld->getTimeBeforeSleep(); + // Get the physics engine parameters + mEngineSettings.isGravityEnabled = mDynamicsWorld->isGravityEnabled(); + rp3d::Vector3 gravityVector = mDynamicsWorld->getGravity(); + mEngineSettings.gravity = openglframework::Vector3(gravityVector.x, gravityVector.y, gravityVector.z); + mEngineSettings.isSleepingEnabled = mDynamicsWorld->isSleepingEnabled(); + mEngineSettings.sleepLinearVelocity = mDynamicsWorld->getSleepLinearVelocity(); + mEngineSettings.sleepAngularVelocity = mDynamicsWorld->getSleepAngularVelocity(); + mEngineSettings.nbPositionSolverIterations = mDynamicsWorld->getNbIterationsPositionSolver(); + mEngineSettings.nbVelocitySolverIterations = mDynamicsWorld->getNbIterationsVelocitySolver(); + mEngineSettings.timeBeforeSleep = mDynamicsWorld->getTimeBeforeSleep(); } // Destructor HeightFieldScene::~HeightFieldScene() { - // Destroy the corresponding rigid body from the dynamics world - for (int i=0; idestroyRigidBody(mBoxes[i]->getRigidBody()); - } - mDynamicsWorld->destroyRigidBody(mHeightField->getRigidBody()); + // Destroy the corresponding rigid body from the dynamics world + for (int32_t i=0; idestroyRigidBody(mBoxes[i]->getRigidBody()); + } + mDynamicsWorld->destroyRigidBody(mHeightField->getRigidBody()); - for (int i=0; isetIsGratityEnabled(mEngineSettings.isGravityEnabled); - rp3d::Vector3 gravity(mEngineSettings.gravity.x, mEngineSettings.gravity.y, - mEngineSettings.gravity.z); - mDynamicsWorld->setGravity(gravity); - mDynamicsWorld->enableSleeping(mEngineSettings.isSleepingEnabled); - mDynamicsWorld->setSleepLinearVelocity(mEngineSettings.sleepLinearVelocity); - mDynamicsWorld->setSleepAngularVelocity(mEngineSettings.sleepAngularVelocity); - mDynamicsWorld->setNbIterationsPositionSolver(mEngineSettings.nbPositionSolverIterations); - mDynamicsWorld->setNbIterationsVelocitySolver(mEngineSettings.nbVelocitySolverIterations); - mDynamicsWorld->setTimeBeforeSleep(mEngineSettings.timeBeforeSleep); + // Update the physics engine parameters + mDynamicsWorld->setIsGratityEnabled(mEngineSettings.isGravityEnabled); + rp3d::Vector3 gravity(mEngineSettings.gravity.x, mEngineSettings.gravity.y, + mEngineSettings.gravity.z); + mDynamicsWorld->setGravity(gravity); + mDynamicsWorld->enableSleeping(mEngineSettings.isSleepingEnabled); + mDynamicsWorld->setSleepLinearVelocity(mEngineSettings.sleepLinearVelocity); + mDynamicsWorld->setSleepAngularVelocity(mEngineSettings.sleepAngularVelocity); + mDynamicsWorld->setNbIterationsPositionSolver(mEngineSettings.nbPositionSolverIterations); + mDynamicsWorld->setNbIterationsVelocitySolver(mEngineSettings.nbVelocitySolverIterations); + mDynamicsWorld->setTimeBeforeSleep(mEngineSettings.timeBeforeSleep); - // Take a simulation step - mDynamicsWorld->update(mEngineSettings.timeStep); + // Take a simulation step + mDynamicsWorld->update(mEngineSettings.timeStep); } // Update the scene void HeightFieldScene::update() { - SceneDemo::update(); + SceneDemo::update(); - // Update the transform used for the rendering - mHeightField->updateTransform(mInterpolationFactor); + // Update the transform used for the rendering + mHeightField->updateTransform(mInterpolationFactor); - for (int i=0; iupdateTransform(mInterpolationFactor); - } + for (int32_t i=0; iupdateTransform(mInterpolationFactor); + } } // Render the scene in a single pass void HeightFieldScene::renderSinglePass(Shader& shader, const openglframework::Matrix4& worldToCameraMatrix) { - // Bind the shader - shader.bind(); + // Bind the shader + shader.bind(); - mHeightField->render(shader, worldToCameraMatrix); + mHeightField->render(shader, worldToCameraMatrix); - for (int i=0; irender(shader, worldToCameraMatrix); - } + for (int32_t i=0; irender(shader, worldToCameraMatrix); + } - // Unbind the shader - shader.unbind(); + // Unbind the shader + shader.unbind(); } // Reset the scene void HeightFieldScene::reset() { - // Reset the transform - rp3d::Transform transform(rp3d::Vector3(0, 0, 0), rp3d::Quaternion::identity()); - mHeightField->resetTransform(transform); + // Reset the transform + rp3d::Transform transform(rp3d::Vector3(0, 0, 0), rp3d::Quaternion::identity()); + mHeightField->resetTransform(transform); - float heightFieldWidth = 10.0f; - float stepDist = heightFieldWidth / (NB_BOXES + 1); - for (int i=0; iresetTransform(boxTransform); - } + float heightFieldWidth = 10.0f; + float stepDist = heightFieldWidth / (NB_BOXES + 1); + for (int32_t i=0; iresetTransform(boxTransform); + } } diff --git a/tools/testbed/scenes/heightfield/HeightFieldScene.h b/tools/testbed/scenes/heightfield/HeightFieldScene.h index a9ec6f6..cc7ba0c 100644 --- a/tools/testbed/scenes/heightfield/HeightFieldScene.h +++ b/tools/testbed/scenes/heightfield/HeightFieldScene.h @@ -1,26 +1,26 @@ /******************************************************************************** -* ReactPhysics3D physics library, http://www.reactphysics3d.com * -* Copyright (c) 2010-2016 Daniel Chappuis * +* 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: * -* * +* 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. * -* * +* 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 HEIGHT_FIELD_SCENE_H @@ -41,51 +41,51 @@ const float SCENE_RADIUS = 50.0f; // Class HeightFieldScene class HeightFieldScene : public SceneDemo { - static const int NB_BOXES = 10; + static const int32_t NB_BOXES = 10; - protected : + protected : - // -------------------- Attributes -------------------- // + // -------------------- Attributes -------------------- // - Box* mBoxes[NB_BOXES]; + Box* mBoxes[NB_BOXES]; - /// Height field - HeightField* mHeightField; + /// Height field + HeightField* mHeightField; - /// Dynamics world used for the physics simulation - rp3d::DynamicsWorld* mDynamicsWorld; + /// Dynamics world used for the physics simulation + rp3d::DynamicsWorld* mDynamicsWorld; - public: + public: - // -------------------- Methods -------------------- // + // -------------------- Methods -------------------- // - /// Constructor - HeightFieldScene(const std::string& name); + /// Constructor + HeightFieldScene(const std::string& name); - /// Destructor - virtual ~HeightFieldScene(); + /// Destructor + virtual ~HeightFieldScene(); - /// Update the physics world (take a simulation step) - /// Can be called several times per frame - virtual void updatePhysics(); + /// Update the physics world (take a simulation step) + /// Can be called several times per frame + virtual void updatePhysics(); - /// Update the scene (take a simulation step) - virtual void update(); + /// Update the scene (take a simulation step) + virtual void update(); - /// Render the scene in a single pass - virtual void renderSinglePass(openglframework::Shader& shader, - const openglframework::Matrix4& worldToCameraMatrix); + /// Render the scene in a single pass + virtual void renderSinglePass(openglframework::Shader& shader, + const openglframework::Matrix4& worldToCameraMatrix); - /// Reset the scene - virtual void reset(); + /// Reset the scene + virtual void reset(); - /// Return all the contact points of the scene - virtual std::vector getContactPoints() const; + /// Return all the contact points of the scene + virtual std::vector getContactPoints() const; }; // Return all the contact points of the scene inline std::vector HeightFieldScene::getContactPoints() const { - return computeContactPointsOfWorld(mDynamicsWorld); + return computeContactPointsOfWorld(mDynamicsWorld); } } diff --git a/tools/testbed/scenes/joints/JointsScene.cpp b/tools/testbed/scenes/joints/JointsScene.cpp index e5d2739..a8a3b59 100644 --- a/tools/testbed/scenes/joints/JointsScene.cpp +++ b/tools/testbed/scenes/joints/JointsScene.cpp @@ -1,26 +1,26 @@ /******************************************************************************** -* ReactPhysics3D physics library, http://www.reactphysics3d.com * -* Copyright (c) 2010-2016 Daniel Chappuis * +* 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: * -* * +* 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. * -* * +* 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 @@ -33,452 +33,452 @@ using namespace jointsscene; // Constructor JointsScene::JointsScene(const std::string& name) - : SceneDemo(name, SCENE_RADIUS) { + : SceneDemo(name, SCENE_RADIUS) { - // Compute the radius and the center of the scene - openglframework::Vector3 center(0, 5, 0); + // 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); + // Set the center of the scene + setScenePosition(center, SCENE_RADIUS); - // Gravity vector in the dynamics world - rp3d::Vector3 gravity(0, rp3d::decimal(-9.81), 0); + // Gravity vector in the dynamics world + rp3d::Vector3 gravity(0, rp3d::float(-9.81), 0); - // Create the dynamics world for the physics simulation - mDynamicsWorld = new rp3d::DynamicsWorld(gravity); + // Create the dynamics world for the physics simulation + mDynamicsWorld = new rp3d::DynamicsWorld(gravity); - // Set the number of iterations of the constraint solver - mDynamicsWorld->setNbIterationsVelocitySolver(15); + // Set the number of iterations of the constraint solver + mDynamicsWorld->setNbIterationsVelocitySolver(15); - // Create the Ball-and-Socket joint - createBallAndSocketJoints(); + // Create the Ball-and-Socket joint + createBallAndSocketJoints(); - // Create the Slider joint - createSliderJoint(); + // Create the Slider joint + createSliderJoint(); - // Create the Hinge joint - createPropellerHingeJoint(); + // Create the Hinge joint + createPropellerHingeJoint(); - // Create the Fixed joint - createFixedJoints(); + // Create the Fixed joint + createFixedJoints(); - // Create the floor - createFloor(); + // Create the floor + createFloor(); - // Get the physics engine parameters - mEngineSettings.isGravityEnabled = mDynamicsWorld->isGravityEnabled(); - rp3d::Vector3 gravityVector = mDynamicsWorld->getGravity(); - mEngineSettings.gravity = openglframework::Vector3(gravityVector.x, gravityVector.y, gravityVector.z); - mEngineSettings.isSleepingEnabled = mDynamicsWorld->isSleepingEnabled(); - mEngineSettings.sleepLinearVelocity = mDynamicsWorld->getSleepLinearVelocity(); - mEngineSettings.sleepAngularVelocity = mDynamicsWorld->getSleepAngularVelocity(); - mEngineSettings.nbPositionSolverIterations = mDynamicsWorld->getNbIterationsPositionSolver(); - mEngineSettings.nbVelocitySolverIterations = mDynamicsWorld->getNbIterationsVelocitySolver(); - mEngineSettings.timeBeforeSleep = mDynamicsWorld->getTimeBeforeSleep(); + // Get the physics engine parameters + mEngineSettings.isGravityEnabled = mDynamicsWorld->isGravityEnabled(); + rp3d::Vector3 gravityVector = mDynamicsWorld->getGravity(); + mEngineSettings.gravity = openglframework::Vector3(gravityVector.x, gravityVector.y, gravityVector.z); + mEngineSettings.isSleepingEnabled = mDynamicsWorld->isSleepingEnabled(); + mEngineSettings.sleepLinearVelocity = mDynamicsWorld->getSleepLinearVelocity(); + mEngineSettings.sleepAngularVelocity = mDynamicsWorld->getSleepAngularVelocity(); + mEngineSettings.nbPositionSolverIterations = mDynamicsWorld->getNbIterationsPositionSolver(); + mEngineSettings.nbVelocitySolverIterations = mDynamicsWorld->getNbIterationsVelocitySolver(); + mEngineSettings.timeBeforeSleep = mDynamicsWorld->getTimeBeforeSleep(); } // Destructor JointsScene::~JointsScene() { - // Destroy the joints - mDynamicsWorld->destroyJoint(mSliderJoint); - mDynamicsWorld->destroyJoint(mPropellerHingeJoint); - mDynamicsWorld->destroyJoint(mFixedJoint1); - mDynamicsWorld->destroyJoint(mFixedJoint2); - for (int i=0; idestroyJoint(mBallAndSocketJoints[i]); - } + // Destroy the joints + mDynamicsWorld->destroyJoint(mSliderJoint); + mDynamicsWorld->destroyJoint(mPropellerHingeJoint); + mDynamicsWorld->destroyJoint(mFixedJoint1); + mDynamicsWorld->destroyJoint(mFixedJoint2); + for (int32_t i=0; idestroyJoint(mBallAndSocketJoints[i]); + } - // Destroy all the rigid bodies of the scene - mDynamicsWorld->destroyRigidBody(mSliderJointBottomBox->getRigidBody()); - mDynamicsWorld->destroyRigidBody(mSliderJointTopBox->getRigidBody()); - mDynamicsWorld->destroyRigidBody(mPropellerBox->getRigidBody()); - mDynamicsWorld->destroyRigidBody(mFixedJointBox1->getRigidBody()); - mDynamicsWorld->destroyRigidBody(mFixedJointBox2->getRigidBody()); - for (int i=0; idestroyRigidBody(mBallAndSocketJointChainBoxes[i]->getRigidBody()); - } + // Destroy all the rigid bodies of the scene + mDynamicsWorld->destroyRigidBody(mSliderJointBottomBox->getRigidBody()); + mDynamicsWorld->destroyRigidBody(mSliderJointTopBox->getRigidBody()); + mDynamicsWorld->destroyRigidBody(mPropellerBox->getRigidBody()); + mDynamicsWorld->destroyRigidBody(mFixedJointBox1->getRigidBody()); + mDynamicsWorld->destroyRigidBody(mFixedJointBox2->getRigidBody()); + for (int32_t i=0; idestroyRigidBody(mBallAndSocketJointChainBoxes[i]->getRigidBody()); + } - delete mSliderJointBottomBox; - delete mSliderJointTopBox; - delete mPropellerBox; - delete mFixedJointBox1; - delete mFixedJointBox2; - for (int i=0; idestroyRigidBody(mFloor->getRigidBody()); - delete mFloor; + // Destroy the floor + mDynamicsWorld->destroyRigidBody(mFloor->getRigidBody()); + delete mFloor; - // Destroy the dynamics world - delete mDynamicsWorld; + // Destroy the dynamics world + delete mDynamicsWorld; } // Update the physics world (take a simulation step) void JointsScene::updatePhysics() { - // Update the physics engine parameters - mDynamicsWorld->setIsGratityEnabled(mEngineSettings.isGravityEnabled); - rp3d::Vector3 gravity(mEngineSettings.gravity.x, mEngineSettings.gravity.y, - mEngineSettings.gravity.z); - mDynamicsWorld->setGravity(gravity); - mDynamicsWorld->enableSleeping(mEngineSettings.isSleepingEnabled); - mDynamicsWorld->setSleepLinearVelocity(mEngineSettings.sleepLinearVelocity); - mDynamicsWorld->setSleepAngularVelocity(mEngineSettings.sleepAngularVelocity); - mDynamicsWorld->setNbIterationsPositionSolver(mEngineSettings.nbPositionSolverIterations); - mDynamicsWorld->setNbIterationsVelocitySolver(mEngineSettings.nbVelocitySolverIterations); - mDynamicsWorld->setTimeBeforeSleep(mEngineSettings.timeBeforeSleep); + // Update the physics engine parameters + mDynamicsWorld->setIsGratityEnabled(mEngineSettings.isGravityEnabled); + rp3d::Vector3 gravity(mEngineSettings.gravity.x, mEngineSettings.gravity.y, + mEngineSettings.gravity.z); + mDynamicsWorld->setGravity(gravity); + mDynamicsWorld->enableSleeping(mEngineSettings.isSleepingEnabled); + mDynamicsWorld->setSleepLinearVelocity(mEngineSettings.sleepLinearVelocity); + mDynamicsWorld->setSleepAngularVelocity(mEngineSettings.sleepAngularVelocity); + mDynamicsWorld->setNbIterationsPositionSolver(mEngineSettings.nbPositionSolverIterations); + mDynamicsWorld->setNbIterationsVelocitySolver(mEngineSettings.nbVelocitySolverIterations); + mDynamicsWorld->setTimeBeforeSleep(mEngineSettings.timeBeforeSleep); - // Update the motor speed of the Slider Joint (to move up and down) - long double motorSpeed = 2 * cos(mEngineSettings.elapsedTime * 1.5); - mSliderJoint->setMotorSpeed(rp3d::decimal(motorSpeed)); + // Update the motor speed of the Slider Joint (to move up and down) + long double motorSpeed = 2 * cos(mEngineSettings.elapsedTime * 1.5); + mSliderJoint->setMotorSpeed(rp3d::float(motorSpeed)); - // Take a simulation step - mDynamicsWorld->update(mEngineSettings.timeStep); + // Take a simulation step + mDynamicsWorld->update(mEngineSettings.timeStep); } // Take a step for the simulation void JointsScene::update() { - SceneDemo::update(); + SceneDemo::update(); - // Update the position and orientation of the boxes - mSliderJointBottomBox->updateTransform(mInterpolationFactor); - mSliderJointTopBox->updateTransform(mInterpolationFactor); - mPropellerBox->updateTransform(mInterpolationFactor); - mFixedJointBox1->updateTransform(mInterpolationFactor); - mFixedJointBox2->updateTransform(mInterpolationFactor); - for (int i=0; iupdateTransform(mInterpolationFactor); - } + // Update the position and orientation of the boxes + mSliderJointBottomBox->updateTransform(mInterpolationFactor); + mSliderJointTopBox->updateTransform(mInterpolationFactor); + mPropellerBox->updateTransform(mInterpolationFactor); + mFixedJointBox1->updateTransform(mInterpolationFactor); + mFixedJointBox2->updateTransform(mInterpolationFactor); + for (int32_t i=0; iupdateTransform(mInterpolationFactor); + } - // Update the position and orientation of the floor - mFloor->updateTransform(mInterpolationFactor); + // Update the position and orientation of the floor + mFloor->updateTransform(mInterpolationFactor); } // Render the scene void JointsScene::renderSinglePass(openglframework::Shader& shader, - const openglframework::Matrix4& worldToCameraMatrix) { + const openglframework::Matrix4& worldToCameraMatrix) { - // Bind the shader - shader.bind(); + // Bind the shader + shader.bind(); - // Render all the boxes - mSliderJointBottomBox->render(shader, worldToCameraMatrix); - mSliderJointTopBox->render(shader, worldToCameraMatrix); - mPropellerBox->render(shader, worldToCameraMatrix); - mFixedJointBox1->render(shader, worldToCameraMatrix); - mFixedJointBox2->render(shader, worldToCameraMatrix); - for (int i=0; irender(shader, worldToCameraMatrix); - } + // Render all the boxes + mSliderJointBottomBox->render(shader, worldToCameraMatrix); + mSliderJointTopBox->render(shader, worldToCameraMatrix); + mPropellerBox->render(shader, worldToCameraMatrix); + mFixedJointBox1->render(shader, worldToCameraMatrix); + mFixedJointBox2->render(shader, worldToCameraMatrix); + for (int32_t i=0; irender(shader, worldToCameraMatrix); + } - // Render the floor - mFloor->render(shader, worldToCameraMatrix); + // Render the floor + mFloor->render(shader, worldToCameraMatrix); - // Unbind the shader - shader.unbind(); + // Unbind the shader + shader.unbind(); } // Reset the scene void JointsScene::reset() { - openglframework::Vector3 positionBox(0, 15, 5); - openglframework::Vector3 boxDimension(1, 1, 1); + openglframework::Vector3 positionBox(0, 15, 5); + openglframework::Vector3 boxDimension(1, 1, 1); - for (int i=0; iresetTransform(transform); + // Create a box and a corresponding rigid in the dynamics world + mBallAndSocketJointChainBoxes[i]->resetTransform(transform); - positionBox.y -= boxDimension.y + 0.5f; - } + positionBox.y -= boxDimension.y + 0.5f; + } - // --------------- Slider Joint --------------- // + // --------------- Slider Joint --------------- // - // Position of the box - openglframework::Vector3 positionBox1(0, 2.1f, 0); - rp3d::Vector3 initPosition(positionBox1.x, positionBox1.y, positionBox1.z); - rp3d::Quaternion initOrientation = rp3d::Quaternion::identity(); - rp3d::Transform transformBottomBox(initPosition, initOrientation); + // Position of the box + openglframework::Vector3 positionBox1(0, 2.1f, 0); + rp3d::Vector3 initPosition(positionBox1.x, positionBox1.y, positionBox1.z); + rp3d::Quaternion initOrientation = rp3d::Quaternion::identity(); + rp3d::Transform transformBottomBox(initPosition, initOrientation); - // Create a box and a corresponding rigid in the dynamics world - mSliderJointBottomBox->resetTransform(transformBottomBox); + // Create a box and a corresponding rigid in the dynamics world + mSliderJointBottomBox->resetTransform(transformBottomBox); - // Position of the box - openglframework::Vector3 positionBox2(0, 4.2f, 0); - initPosition = rp3d::Vector3(positionBox2.x, positionBox2.y, positionBox2.z); - initOrientation = rp3d::Quaternion::identity(); - rp3d::Transform transformTopBox(initPosition, initOrientation); + // Position of the box + openglframework::Vector3 positionBox2(0, 4.2f, 0); + initPosition = rp3d::Vector3(positionBox2.x, positionBox2.y, positionBox2.z); + initOrientation = rp3d::Quaternion::identity(); + rp3d::Transform transformTopBox(initPosition, initOrientation); - // Create a box and a corresponding rigid in the dynamics world - mSliderJointTopBox->resetTransform(transformTopBox); + // Create a box and a corresponding rigid in the dynamics world + mSliderJointTopBox->resetTransform(transformTopBox); - // --------------- Propeller Hinge joint --------------- // + // --------------- Propeller Hinge joint --------------- // - // Position of the box - positionBox1 = openglframework::Vector3(0, 7, 0); - initPosition = rp3d::Vector3(positionBox1.x, positionBox1.y, positionBox1.z); - initOrientation = rp3d::Quaternion::identity(); - rp3d::Transform transformHingeBox(initPosition, initOrientation); + // Position of the box + positionBox1 = openglframework::Vector3(0, 7, 0); + initPosition = rp3d::Vector3(positionBox1.x, positionBox1.y, positionBox1.z); + initOrientation = rp3d::Quaternion::identity(); + rp3d::Transform transformHingeBox(initPosition, initOrientation); - // Create a box and a corresponding rigid in the dynamics world - mPropellerBox->resetTransform(transformHingeBox); + // Create a box and a corresponding rigid in the dynamics world + mPropellerBox->resetTransform(transformHingeBox); - // --------------- Fixed joint --------------- // + // --------------- Fixed joint --------------- // - // Position of the box - positionBox1 = openglframework::Vector3(5, 7, 0); - initPosition = rp3d::Vector3(positionBox1.x, positionBox1.y, positionBox1.z); - initOrientation = rp3d::Quaternion::identity(); - rp3d::Transform transformFixedBox1(initPosition, initOrientation); + // Position of the box + positionBox1 = openglframework::Vector3(5, 7, 0); + initPosition = rp3d::Vector3(positionBox1.x, positionBox1.y, positionBox1.z); + initOrientation = rp3d::Quaternion::identity(); + rp3d::Transform transformFixedBox1(initPosition, initOrientation); - // Create a box and a corresponding rigid in the dynamics world - mFixedJointBox1->resetTransform(transformFixedBox1); + // Create a box and a corresponding rigid in the dynamics world + mFixedJointBox1->resetTransform(transformFixedBox1); - // Position of the box - positionBox2 = openglframework::Vector3(-5, 7, 0); - initPosition = rp3d::Vector3(positionBox2.x, positionBox2.y, positionBox2.z); - initOrientation = rp3d::Quaternion::identity(); - rp3d::Transform transformFixedBox2(initPosition, initOrientation); + // Position of the box + positionBox2 = openglframework::Vector3(-5, 7, 0); + initPosition = rp3d::Vector3(positionBox2.x, positionBox2.y, positionBox2.z); + initOrientation = rp3d::Quaternion::identity(); + rp3d::Transform transformFixedBox2(initPosition, initOrientation); - // Create a box and a corresponding rigid in the dynamics world - mFixedJointBox2->resetTransform(transformFixedBox2); + // Create a box and a corresponding rigid in the dynamics world + mFixedJointBox2->resetTransform(transformFixedBox2); } // Create the boxes and joints for the Ball-and-Socket joint example void JointsScene::createBallAndSocketJoints() { - // --------------- Create the boxes --------------- // + // --------------- Create the boxes --------------- // - openglframework::Vector3 positionBox(0, 15, 5); - openglframework::Vector3 boxDimension(1, 1, 1); - const float boxMass = 0.5f; + openglframework::Vector3 positionBox(0, 15, 5); + openglframework::Vector3 boxDimension(1, 1, 1); + const float boxMass = 0.5f; - for (int i=0; isetColor(mDemoColors[i % mNbDemoColors]); - mBallAndSocketJointChainBoxes[i]->setSleepingColor(mRedColorDemo); + // Set the box color + mBallAndSocketJointChainBoxes[i]->setColor(mDemoColors[i % mNbDemoColors]); + mBallAndSocketJointChainBoxes[i]->setSleepingColor(mRedColorDemo); - // The fist box cannot move (static body) - if (i == 0) { - mBallAndSocketJointChainBoxes[i]->getRigidBody()->setType(rp3d::STATIC); - } + // The fist box cannot move (static body) + if (i == 0) { + mBallAndSocketJointChainBoxes[i]->getRigidBody()->setType(rp3d::STATIC); + } - // Add some angular velocity damping - mBallAndSocketJointChainBoxes[i]->getRigidBody()->setAngularDamping(rp3d::decimal(0.2)); + // Add some angular velocity damping + mBallAndSocketJointChainBoxes[i]->getRigidBody()->setAngularDamping(rp3d::float(0.2)); - // Change the material properties of the rigid body - rp3d::Material& material = mBallAndSocketJointChainBoxes[i]->getRigidBody()->getMaterial(); - material.setBounciness(rp3d::decimal(0.4)); + // Change the material properties of the rigid body + rp3d::Material& material = mBallAndSocketJointChainBoxes[i]->getRigidBody()->getMaterial(); + material.setBounciness(rp3d::float(0.4)); - positionBox.y -= boxDimension.y + 0.5f; - } + positionBox.y -= boxDimension.y + 0.5f; + } - // --------------- Create the joints --------------- // + // --------------- Create the joints --------------- // - for (int i=0; igetRigidBody(); - rp3d::RigidBody* body2 = mBallAndSocketJointChainBoxes[i+1]->getRigidBody(); - rp3d::Vector3 body1Position = body1->getTransform().getPosition(); - rp3d::Vector3 body2Position = body2->getTransform().getPosition(); - const rp3d::Vector3 anchorPointWorldSpace = 0.5 * (body1Position + body2Position); - rp3d::BallAndSocketJointInfo jointInfo(body1, body2, anchorPointWorldSpace); + // Create the joint info object + rp3d::RigidBody* body1 = mBallAndSocketJointChainBoxes[i]->getRigidBody(); + rp3d::RigidBody* body2 = mBallAndSocketJointChainBoxes[i+1]->getRigidBody(); + rp3d::Vector3 body1Position = body1->getTransform().getPosition(); + rp3d::Vector3 body2Position = body2->getTransform().getPosition(); + const rp3d::Vector3 anchorPointWorldSpace = 0.5 * (body1Position + body2Position); + rp3d::BallAndSocketJointInfo jointInfo(body1, body2, anchorPointWorldSpace); - // Create the joint in the dynamics world - mBallAndSocketJoints[i] = dynamic_cast( - mDynamicsWorld->createJoint(jointInfo)); - } + // Create the joint in the dynamics world + mBallAndSocketJoints[i] = dynamic_cast( + mDynamicsWorld->createJoint(jointInfo)); + } } /// Create the boxes and joint for the Slider joint example void JointsScene::createSliderJoint() { - // --------------- Create the first box --------------- // + // --------------- Create the first box --------------- // - // Position of the box - openglframework::Vector3 positionBox1(0, 2.1f, 0); + // Position of the box + openglframework::Vector3 positionBox1(0, 2.1f, 0); - // Create a box and a corresponding rigid in the dynamics world - openglframework::Vector3 box1Dimension(2, 4, 2); - mSliderJointBottomBox = new Box(box1Dimension, positionBox1 , BOX_MASS, mDynamicsWorld); + // Create a box and a corresponding rigid in the dynamics world + openglframework::Vector3 box1Dimension(2, 4, 2); + mSliderJointBottomBox = new Box(box1Dimension, positionBox1 , BOX_MASS, mDynamicsWorld); - // Set the box color - mSliderJointBottomBox->setColor(mBlueColorDemo); - mSliderJointBottomBox->setSleepingColor(mRedColorDemo); + // Set the box color + mSliderJointBottomBox->setColor(mBlueColorDemo); + mSliderJointBottomBox->setSleepingColor(mRedColorDemo); - // The fist box cannot move - mSliderJointBottomBox->getRigidBody()->setType(rp3d::STATIC); + // The fist box cannot move + mSliderJointBottomBox->getRigidBody()->setType(rp3d::STATIC); - // Change the material properties of the rigid body - rp3d::Material& material1 = mSliderJointBottomBox->getRigidBody()->getMaterial(); - material1.setBounciness(0.4f); + // Change the material properties of the rigid body + rp3d::Material& material1 = mSliderJointBottomBox->getRigidBody()->getMaterial(); + material1.setBounciness(0.4f); - // --------------- Create the second box --------------- // + // --------------- Create the second box --------------- // - // Position of the box - openglframework::Vector3 positionBox2(0, 4.2f, 0); + // Position of the box + openglframework::Vector3 positionBox2(0, 4.2f, 0); - // Create a box and a corresponding rigid in the dynamics world - openglframework::Vector3 box2Dimension(1.5f, 4, 1.5f); - mSliderJointTopBox = new Box(box2Dimension, positionBox2, BOX_MASS, mDynamicsWorld); + // Create a box and a corresponding rigid in the dynamics world + openglframework::Vector3 box2Dimension(1.5f, 4, 1.5f); + mSliderJointTopBox = new Box(box2Dimension, positionBox2, BOX_MASS, mDynamicsWorld); - // Set the box color - mSliderJointTopBox->setColor(mOrangeColorDemo); - mSliderJointTopBox->setSleepingColor(mRedColorDemo); + // Set the box color + mSliderJointTopBox->setColor(mOrangeColorDemo); + mSliderJointTopBox->setSleepingColor(mRedColorDemo); - // Change the material properties of the rigid body - rp3d::Material& material2 = mSliderJointTopBox->getRigidBody()->getMaterial(); - material2.setBounciness(0.4f); + // Change the material properties of the rigid body + rp3d::Material& material2 = mSliderJointTopBox->getRigidBody()->getMaterial(); + material2.setBounciness(0.4f); - // --------------- Create the joint --------------- // + // --------------- Create the joint --------------- // - // Create the joint info object - rp3d::RigidBody* body1 = mSliderJointBottomBox->getRigidBody(); - rp3d::RigidBody* body2 = mSliderJointTopBox->getRigidBody(); - const rp3d::Vector3& body1Position = body1->getTransform().getPosition(); - const rp3d::Vector3& body2Position = body2->getTransform().getPosition(); - const rp3d::Vector3 anchorPointWorldSpace = rp3d::decimal(0.5) * (body2Position + body1Position); - const rp3d::Vector3 sliderAxisWorldSpace = (body2Position - body1Position); - rp3d::SliderJointInfo jointInfo(body1, body2, anchorPointWorldSpace, sliderAxisWorldSpace, - rp3d::decimal(-1.7), rp3d::decimal(1.7)); - jointInfo.isMotorEnabled = true; - jointInfo.motorSpeed = 0.0; - jointInfo.maxMotorForce = 10000.0; - jointInfo.isCollisionEnabled = false; + // Create the joint info object + rp3d::RigidBody* body1 = mSliderJointBottomBox->getRigidBody(); + rp3d::RigidBody* body2 = mSliderJointTopBox->getRigidBody(); + const rp3d::Vector3& body1Position = body1->getTransform().getPosition(); + const rp3d::Vector3& body2Position = body2->getTransform().getPosition(); + const rp3d::Vector3 anchorPointWorldSpace = rp3d::float(0.5) * (body2Position + body1Position); + const rp3d::Vector3 sliderAxisWorldSpace = (body2Position - body1Position); + rp3d::SliderJointInfo jointInfo(body1, body2, anchorPointWorldSpace, sliderAxisWorldSpace, + rp3d::float(-1.7), rp3d::float(1.7)); + jointInfo.isMotorEnabled = true; + jointInfo.motorSpeed = 0.0; + jointInfo.maxMotorForce = 10000.0; + jointInfo.isCollisionEnabled = false; - // Create the joint in the dynamics world - mSliderJoint = dynamic_cast(mDynamicsWorld->createJoint(jointInfo)); + // Create the joint in the dynamics world + mSliderJoint = dynamic_cast(mDynamicsWorld->createJoint(jointInfo)); } /// Create the boxes and joint for the Hinge joint example void JointsScene::createPropellerHingeJoint() { - // --------------- Create the propeller box --------------- // + // --------------- Create the propeller box --------------- // - // Position of the box - openglframework::Vector3 positionBox1(0, 7, 0); + // Position of the box + openglframework::Vector3 positionBox1(0, 7, 0); - // Create a box and a corresponding rigid in the dynamics world - openglframework::Vector3 boxDimension(10, 1, 1); - mPropellerBox = new Box(boxDimension, positionBox1 , BOX_MASS, mDynamicsWorld); + // Create a box and a corresponding rigid in the dynamics world + openglframework::Vector3 boxDimension(10, 1, 1); + mPropellerBox = new Box(boxDimension, positionBox1 , BOX_MASS, mDynamicsWorld); - // Set the box color - mPropellerBox->setColor(mYellowColorDemo); - mPropellerBox->setSleepingColor(mRedColorDemo); + // Set the box color + mPropellerBox->setColor(mYellowColorDemo); + mPropellerBox->setSleepingColor(mRedColorDemo); - // Change the material properties of the rigid body - rp3d::Material& material = mPropellerBox->getRigidBody()->getMaterial(); - material.setBounciness(rp3d::decimal(0.4)); + // Change the material properties of the rigid body + rp3d::Material& material = mPropellerBox->getRigidBody()->getMaterial(); + material.setBounciness(rp3d::float(0.4)); - // --------------- Create the Hinge joint --------------- // + // --------------- Create the Hinge joint --------------- // - // Create the joint info object - rp3d::RigidBody* body1 = mPropellerBox->getRigidBody(); - rp3d::RigidBody* body2 = mSliderJointTopBox->getRigidBody(); - const rp3d::Vector3& body1Position = body1->getTransform().getPosition(); - const rp3d::Vector3& body2Position = body2->getTransform().getPosition(); - const rp3d::Vector3 anchorPointWorldSpace = 0.5 * (body2Position + body1Position); - const rp3d::Vector3 hingeAxisWorldSpace(0, 1, 0); - rp3d::HingeJointInfo jointInfo(body1, body2, anchorPointWorldSpace, hingeAxisWorldSpace); - jointInfo.isMotorEnabled = true; - jointInfo.motorSpeed = - rp3d::decimal(0.5) * PI; - jointInfo.maxMotorTorque = rp3d::decimal(60.0); - jointInfo.isCollisionEnabled = false; + // Create the joint info object + rp3d::RigidBody* body1 = mPropellerBox->getRigidBody(); + rp3d::RigidBody* body2 = mSliderJointTopBox->getRigidBody(); + const rp3d::Vector3& body1Position = body1->getTransform().getPosition(); + const rp3d::Vector3& body2Position = body2->getTransform().getPosition(); + const rp3d::Vector3 anchorPointWorldSpace = 0.5 * (body2Position + body1Position); + const rp3d::Vector3 hingeAxisWorldSpace(0, 1, 0); + rp3d::HingeJointInfo jointInfo(body1, body2, anchorPointWorldSpace, hingeAxisWorldSpace); + jointInfo.isMotorEnabled = true; + jointInfo.motorSpeed = - rp3d::float(0.5) * PI; + jointInfo.maxMotorTorque = rp3d::float(60.0); + jointInfo.isCollisionEnabled = false; - // Create the joint in the dynamics world - mPropellerHingeJoint = dynamic_cast(mDynamicsWorld->createJoint(jointInfo)); + // Create the joint in the dynamics world + mPropellerHingeJoint = dynamic_cast(mDynamicsWorld->createJoint(jointInfo)); } /// Create the boxes and joints for the fixed joints void JointsScene::createFixedJoints() { - // --------------- Create the first box --------------- // + // --------------- Create the first box --------------- // - // Position of the box - openglframework::Vector3 positionBox1(5, 7, 0); + // Position of the box + openglframework::Vector3 positionBox1(5, 7, 0); - // Create a box and a corresponding rigid in the dynamics world - openglframework::Vector3 boxDimension(1.5, 1.5, 1.5); - mFixedJointBox1 = new Box(boxDimension, positionBox1 , BOX_MASS, mDynamicsWorld); + // Create a box and a corresponding rigid in the dynamics world + openglframework::Vector3 boxDimension(1.5, 1.5, 1.5); + mFixedJointBox1 = new Box(boxDimension, positionBox1 , BOX_MASS, mDynamicsWorld); - // Set the box color - mFixedJointBox1->setColor(mPinkColorDemo); - mFixedJointBox1->setSleepingColor(mRedColorDemo); + // Set the box color + mFixedJointBox1->setColor(mPinkColorDemo); + mFixedJointBox1->setSleepingColor(mRedColorDemo); - // Change the material properties of the rigid body - rp3d::Material& material1 = mFixedJointBox1->getRigidBody()->getMaterial(); - material1.setBounciness(rp3d::decimal(0.4)); + // Change the material properties of the rigid body + rp3d::Material& material1 = mFixedJointBox1->getRigidBody()->getMaterial(); + material1.setBounciness(rp3d::float(0.4)); - // --------------- Create the second box --------------- // + // --------------- Create the second box --------------- // - // Position of the box - openglframework::Vector3 positionBox2(-5, 7, 0); + // Position of the box + openglframework::Vector3 positionBox2(-5, 7, 0); - // Create a box and a corresponding rigid in the dynamics world - mFixedJointBox2 = new Box(boxDimension, positionBox2 , BOX_MASS, mDynamicsWorld); + // Create a box and a corresponding rigid in the dynamics world + mFixedJointBox2 = new Box(boxDimension, positionBox2 , BOX_MASS, mDynamicsWorld); - // Set the box color - mFixedJointBox2->setColor(mBlueColorDemo); - mFixedJointBox2->setSleepingColor(mRedColorDemo); + // Set the box color + mFixedJointBox2->setColor(mBlueColorDemo); + mFixedJointBox2->setSleepingColor(mRedColorDemo); - // Change the material properties of the rigid body - rp3d::Material& material2 = mFixedJointBox2->getRigidBody()->getMaterial(); - material2.setBounciness(rp3d::decimal(0.4)); + // Change the material properties of the rigid body + rp3d::Material& material2 = mFixedJointBox2->getRigidBody()->getMaterial(); + material2.setBounciness(rp3d::float(0.4)); - // --------------- Create the first fixed joint --------------- // + // --------------- Create the first fixed joint --------------- // - // Create the joint info object - rp3d::RigidBody* body1 = mFixedJointBox1->getRigidBody(); - rp3d::RigidBody* propellerBody = mPropellerBox->getRigidBody(); - const rp3d::Vector3 anchorPointWorldSpace1(5, 7, 0); - rp3d::FixedJointInfo jointInfo1(body1, propellerBody, anchorPointWorldSpace1); - jointInfo1.isCollisionEnabled = false; + // Create the joint info object + rp3d::RigidBody* body1 = mFixedJointBox1->getRigidBody(); + rp3d::RigidBody* propellerBody = mPropellerBox->getRigidBody(); + const rp3d::Vector3 anchorPointWorldSpace1(5, 7, 0); + rp3d::FixedJointInfo jointInfo1(body1, propellerBody, anchorPointWorldSpace1); + jointInfo1.isCollisionEnabled = false; - // Create the joint in the dynamics world - mFixedJoint1 = dynamic_cast(mDynamicsWorld->createJoint(jointInfo1)); + // Create the joint in the dynamics world + mFixedJoint1 = dynamic_cast(mDynamicsWorld->createJoint(jointInfo1)); - // --------------- Create the second fixed joint --------------- // + // --------------- Create the second fixed joint --------------- // - // Create the joint info object - rp3d::RigidBody* body2 = mFixedJointBox2->getRigidBody(); - const rp3d::Vector3 anchorPointWorldSpace2(-5, 7, 0); - rp3d::FixedJointInfo jointInfo2(body2, propellerBody, anchorPointWorldSpace2); - jointInfo2.isCollisionEnabled = false; + // Create the joint info object + rp3d::RigidBody* body2 = mFixedJointBox2->getRigidBody(); + const rp3d::Vector3 anchorPointWorldSpace2(-5, 7, 0); + rp3d::FixedJointInfo jointInfo2(body2, propellerBody, anchorPointWorldSpace2); + jointInfo2.isCollisionEnabled = false; - // Create the joint in the dynamics world - mFixedJoint2 = dynamic_cast(mDynamicsWorld->createJoint(jointInfo2)); + // Create the joint in the dynamics world + mFixedJoint2 = dynamic_cast(mDynamicsWorld->createJoint(jointInfo2)); } // Create the floor void JointsScene::createFloor() { - // Create the floor - openglframework::Vector3 floorPosition(0, 0, 0); - mFloor = new Box(FLOOR_SIZE, floorPosition, FLOOR_MASS, mDynamicsWorld); + // Create the floor + openglframework::Vector3 floorPosition(0, 0, 0); + mFloor = new Box(FLOOR_SIZE, floorPosition, FLOOR_MASS, mDynamicsWorld); - // Set the box color - mFloor->setColor(mGreyColorDemo); - mFloor->setSleepingColor(mGreyColorDemo); + // Set the box color + mFloor->setColor(mGreyColorDemo); + mFloor->setSleepingColor(mGreyColorDemo); - // The floor must be a static rigid body - mFloor->getRigidBody()->setType(rp3d::STATIC); + // The floor must be a static rigid body + mFloor->getRigidBody()->setType(rp3d::STATIC); - // Change the material properties of the rigid body - rp3d::Material& material = mFloor->getRigidBody()->getMaterial(); - material.setBounciness(rp3d::decimal(0.3)); + // Change the material properties of the rigid body + rp3d::Material& material = mFloor->getRigidBody()->getMaterial(); + material.setBounciness(rp3d::float(0.3)); } diff --git a/tools/testbed/scenes/joints/JointsScene.h b/tools/testbed/scenes/joints/JointsScene.h index 6924f3f..7287535 100644 --- a/tools/testbed/scenes/joints/JointsScene.h +++ b/tools/testbed/scenes/joints/JointsScene.h @@ -1,26 +1,26 @@ /******************************************************************************** -* ReactPhysics3D physics library, http://www.reactphysics3d.com * -* Copyright (c) 2010-2016 Daniel Chappuis * +* 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: * -* * +* 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. * -* * +* 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 JOINTS_SCENE_H @@ -36,113 +36,113 @@ namespace jointsscene { // Constants const float SCENE_RADIUS = 30.0f; -const openglframework::Vector3 BOX_SIZE(2, 2, 2); // Box dimensions in meters -const openglframework::Vector3 FLOOR_SIZE(50, 0.5f, 50); // Floor dimensions in meters -const float BOX_MASS = 1.0f; // Box mass in kilograms -const float FLOOR_MASS = 100.0f; // Floor mass in kilograms -const int NB_BALLSOCKETJOINT_BOXES = 7; // Number of Ball-And-Socket chain boxes -const int NB_HINGE_BOXES = 7; // Number of Hinge chain boxes +const openglframework::Vector3 BOX_SIZE(2, 2, 2); // Box dimensions in meters +const openglframework::Vector3 FLOOR_SIZE(50, 0.5f, 50); // Floor dimensions in meters +const float BOX_MASS = 1.0f; // Box mass in kilograms +const float FLOOR_MASS = 100.0f; // Floor mass in kilograms +const int32_t NB_BALLSOCKETJOINT_BOXES = 7; // Number of Ball-And-Socket chain boxes +const int32_t NB_HINGE_BOXES = 7; // Number of Hinge chain boxes // Class JointsScene class JointsScene : public SceneDemo { - protected : + protected : - // -------------------- Attributes -------------------- // + // -------------------- Attributes -------------------- // - /// Boxes of Ball-And-Socket joint chain - Box* mBallAndSocketJointChainBoxes[NB_BALLSOCKETJOINT_BOXES]; + /// Boxes of Ball-And-Socket joint chain + Box* mBallAndSocketJointChainBoxes[NB_BALLSOCKETJOINT_BOXES]; - /// Boxes of the Hinge joint chain - Box* mHingeJointChainBoxes[NB_HINGE_BOXES]; + /// Boxes of the Hinge joint chain + Box* mHingeJointChainBoxes[NB_HINGE_BOXES]; - /// Ball-And-Socket joints of the chain - rp3d::BallAndSocketJoint* mBallAndSocketJoints[NB_BALLSOCKETJOINT_BOXES-1]; + /// Ball-And-Socket joints of the chain + rp3d::BallAndSocketJoint* mBallAndSocketJoints[NB_BALLSOCKETJOINT_BOXES-1]; - /// Hinge joints of the chain - rp3d::HingeJoint* mHingeJoints[NB_HINGE_BOXES-1]; + /// Hinge joints of the chain + rp3d::HingeJoint* mHingeJoints[NB_HINGE_BOXES-1]; - /// Bottom box of the Slider joint - Box* mSliderJointBottomBox; + /// Bottom box of the Slider joint + Box* mSliderJointBottomBox; - /// Top box of the Slider joint - Box* mSliderJointTopBox; + /// Top box of the Slider joint + Box* mSliderJointTopBox; - /// Slider joint - rp3d::SliderJoint* mSliderJoint; + /// Slider joint + rp3d::SliderJoint* mSliderJoint; - /// Propeller box - Box* mPropellerBox; + /// Propeller box + Box* mPropellerBox; - /// Box 1 of Fixed joint - Box* mFixedJointBox1; + /// Box 1 of Fixed joint + Box* mFixedJointBox1; - /// Box 2 of Fixed joint - Box* mFixedJointBox2; + /// Box 2 of Fixed joint + Box* mFixedJointBox2; - /// Hinge joint - rp3d::HingeJoint* mPropellerHingeJoint; + /// Hinge joint + rp3d::HingeJoint* mPropellerHingeJoint; - /// First Fixed joint - rp3d::FixedJoint* mFixedJoint1; + /// First Fixed joint + rp3d::FixedJoint* mFixedJoint1; - /// Second Fixed joint - rp3d::FixedJoint* mFixedJoint2; + /// Second Fixed joint + rp3d::FixedJoint* mFixedJoint2; - /// Box for the floor - Box* mFloor; + /// Box for the floor + Box* mFloor; - /// Dynamics world used for the physics simulation - rp3d::DynamicsWorld* mDynamicsWorld; + /// Dynamics world used for the physics simulation + rp3d::DynamicsWorld* mDynamicsWorld; - // -------------------- Methods -------------------- // + // -------------------- Methods -------------------- // - /// Create the boxes and joints for the Ball-and-Socket joint example - void createBallAndSocketJoints(); + /// Create the boxes and joints for the Ball-and-Socket joint example + void createBallAndSocketJoints(); - /// Create the boxes and joint for the Slider joint example - void createSliderJoint(); + /// Create the boxes and joint for the Slider joint example + void createSliderJoint(); - /// Create the boxes and joint for the Hinge joint example - void createPropellerHingeJoint(); + /// Create the boxes and joint for the Hinge joint example + void createPropellerHingeJoint(); - /// Create the boxes and joint for the Fixed joint example - void createFixedJoints(); + /// Create the boxes and joint for the Fixed joint example + void createFixedJoints(); - /// Create the floor - void createFloor(); + /// Create the floor + void createFloor(); - public: + public: - // -------------------- Methods -------------------- // + // -------------------- Methods -------------------- // - /// Constructor - JointsScene(const std::string& name); + /// Constructor + JointsScene(const std::string& name); - /// Destructor - virtual ~JointsScene(); + /// Destructor + virtual ~JointsScene(); - /// Update the physics world (take a simulation step) - /// Can be called several times per frame - virtual void updatePhysics(); + /// Update the physics world (take a simulation step) + /// Can be called several times per frame + virtual void updatePhysics(); - /// Take a step for the simulation - virtual void update(); + /// Take a step for the simulation + virtual void update(); - /// Render the scene in a single pass - virtual void renderSinglePass(openglframework::Shader& shader, - const openglframework::Matrix4& worldToCameraMatrix); + /// Render the scene in a single pass + virtual void renderSinglePass(openglframework::Shader& shader, + const openglframework::Matrix4& worldToCameraMatrix); - /// Reset the scene - virtual void reset(); + /// Reset the scene + virtual void reset(); - /// Return all the contact points of the scene - virtual std::vector getContactPoints() const; + /// Return all the contact points of the scene + virtual std::vector getContactPoints() const; }; // Return all the contact points of the scene inline std::vector JointsScene::getContactPoints() const { - return computeContactPointsOfWorld(mDynamicsWorld); + return computeContactPointsOfWorld(mDynamicsWorld); } } diff --git a/tools/testbed/scenes/raycast/RaycastScene.cpp b/tools/testbed/scenes/raycast/RaycastScene.cpp index 422fabf..b89778a 100644 --- a/tools/testbed/scenes/raycast/RaycastScene.cpp +++ b/tools/testbed/scenes/raycast/RaycastScene.cpp @@ -1,26 +1,26 @@ /******************************************************************************** -* ReactPhysics3D physics library, http://www.reactphysics3d.com * -* Copyright (c) 2010-2016 Daniel Chappuis * +* 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: * -* * +* 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. * -* * +* 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 @@ -32,191 +32,191 @@ using namespace raycastscene; // Constructor RaycastScene::RaycastScene(const std::string& name) - : SceneDemo(name, SCENE_RADIUS, false), mMeshFolderPath("meshes/"), - m_raycastManager(mPhongShader, mMeshFolderPath), mCurrentBodyIndex(-1), - mAreNormalsDisplayed(false), mVBOVertices(GL_ARRAY_BUFFER) { + : SceneDemo(name, SCENE_RADIUS, false), mMeshFolderPath("meshes/"), + m_raycastManager(mPhongShader, mMeshFolderPath), mCurrentBodyIndex(-1), + mAreNormalsDisplayed(false), mVBOVertices(GL_ARRAY_BUFFER) { - mIsContactPointsDisplayed = true; + mIsContactPointsDisplayed = true; - // Compute the radius and the center of the scene - openglframework::Vector3 center(0, 0, 0); + // 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); + // Set the center of the scene + setScenePosition(center, SCENE_RADIUS); - // Create the dynamics world for the physics simulation - mCollisionWorld = new rp3d::CollisionWorld(); + // Create the dynamics world for the physics simulation + mCollisionWorld = new rp3d::CollisionWorld(); - // ---------- Dumbbell ---------- // - openglframework::Vector3 position1(0, 0, 0); + // ---------- Dumbbell ---------- // + openglframework::Vector3 position1(0, 0, 0); - // Create a convex mesh and a corresponding collision body in the dynamics world - mDumbbell = new Dumbbell(position1, mCollisionWorld, mMeshFolderPath); + // Create a convex mesh and a corresponding collision body in the dynamics world + mDumbbell = new Dumbbell(position1, mCollisionWorld, mMeshFolderPath); - // Set the box color - mDumbbell->setColor(mGreyColorDemo); - mDumbbell->setSleepingColor(mRedColorDemo); + // Set the box color + mDumbbell->setColor(mGreyColorDemo); + mDumbbell->setSleepingColor(mRedColorDemo); - // ---------- Box ---------- // - openglframework::Vector3 position2(0, 0, 0); + // ---------- Box ---------- // + openglframework::Vector3 position2(0, 0, 0); - // Create a box and a corresponding collision body in the dynamics world - mBox = new Box(BOX_SIZE, position2, mCollisionWorld); - mBox->getCollisionBody()->setIsActive(false); + // Create a box and a corresponding collision body in the dynamics world + mBox = new Box(BOX_SIZE, position2, mCollisionWorld); + mBox->getCollisionBody()->setIsActive(false); - // Set the box color - mBox->setColor(mGreyColorDemo); - mBox->setSleepingColor(mRedColorDemo); + // Set the box color + mBox->setColor(mGreyColorDemo); + mBox->setSleepingColor(mRedColorDemo); - // ---------- Sphere ---------- // - openglframework::Vector3 position3(0, 0, 0); + // ---------- Sphere ---------- // + openglframework::Vector3 position3(0, 0, 0); - // Create a sphere and a corresponding collision body in the dynamics world - mSphere = new Sphere(SPHERE_RADIUS, position3, mCollisionWorld, - mMeshFolderPath); + // Create a sphere and a corresponding collision body in the dynamics world + mSphere = new Sphere(SPHERE_RADIUS, position3, mCollisionWorld, + mMeshFolderPath); - // Set the color - mSphere->setColor(mGreyColorDemo); - mSphere->setSleepingColor(mRedColorDemo); + // Set the color + mSphere->setColor(mGreyColorDemo); + mSphere->setSleepingColor(mRedColorDemo); - // ---------- Cone ---------- // - openglframework::Vector3 position4(0, 0, 0); + // ---------- Cone ---------- // + openglframework::Vector3 position4(0, 0, 0); - // Create a cone and a corresponding collision body in the dynamics world - mCone = new Cone(CONE_RADIUS, CONE_HEIGHT, position4, mCollisionWorld, - mMeshFolderPath); + // Create a cone and a corresponding collision body in the dynamics world + mCone = new Cone(CONE_RADIUS, CONE_HEIGHT, position4, mCollisionWorld, + mMeshFolderPath); - // Set the color - mCone->setColor(mGreyColorDemo); - mCone->setSleepingColor(mRedColorDemo); + // Set the color + mCone->setColor(mGreyColorDemo); + mCone->setSleepingColor(mRedColorDemo); - // ---------- Cylinder ---------- // - openglframework::Vector3 position5(0, 0, 0); + // ---------- Cylinder ---------- // + openglframework::Vector3 position5(0, 0, 0); - // Create a cylinder and a corresponding collision body in the dynamics world - mCylinder = new Cylinder(CYLINDER_RADIUS, CYLINDER_HEIGHT, position5, - mCollisionWorld, mMeshFolderPath); + // Create a cylinder and a corresponding collision body in the dynamics world + mCylinder = new Cylinder(CYLINDER_RADIUS, CYLINDER_HEIGHT, position5, + mCollisionWorld, mMeshFolderPath); - // Set the color - mCylinder->setColor(mGreyColorDemo); - mCylinder->setSleepingColor(mRedColorDemo); + // Set the color + mCylinder->setColor(mGreyColorDemo); + mCylinder->setSleepingColor(mRedColorDemo); - // ---------- Capsule ---------- // - openglframework::Vector3 position6(0, 0, 0); + // ---------- Capsule ---------- // + openglframework::Vector3 position6(0, 0, 0); - // Create a cylinder and a corresponding collision body in the dynamics world - mCapsule = new Capsule(CAPSULE_RADIUS, CAPSULE_HEIGHT, position6 , - mCollisionWorld, mMeshFolderPath); + // Create a cylinder and a corresponding collision body in the dynamics world + mCapsule = new Capsule(CAPSULE_RADIUS, CAPSULE_HEIGHT, position6 , + mCollisionWorld, mMeshFolderPath); - // Set the color - mCapsule->setColor(mGreyColorDemo); - mCapsule->setSleepingColor(mRedColorDemo); + // Set the color + mCapsule->setColor(mGreyColorDemo); + mCapsule->setSleepingColor(mRedColorDemo); - // ---------- Convex Mesh ---------- // - openglframework::Vector3 position7(0, 0, 0); + // ---------- Convex Mesh ---------- // + openglframework::Vector3 position7(0, 0, 0); - // Create a convex mesh and a corresponding collision body in the dynamics world - mConvexMesh = new ConvexMesh(position7, mCollisionWorld, mMeshFolderPath + "convexmesh.obj"); + // Create a convex mesh and a corresponding collision body in the dynamics world + mConvexMesh = new ConvexMesh(position7, mCollisionWorld, mMeshFolderPath + "convexmesh.obj"); - // Set the color - mConvexMesh->setColor(mGreyColorDemo); - mConvexMesh->setSleepingColor(mRedColorDemo); + // Set the color + mConvexMesh->setColor(mGreyColorDemo); + mConvexMesh->setSleepingColor(mRedColorDemo); - // ---------- Concave Mesh ---------- // - openglframework::Vector3 position8(0, 0, 0); + // ---------- Concave Mesh ---------- // + openglframework::Vector3 position8(0, 0, 0); - // Create a convex mesh and a corresponding collision body in the dynamics world - mConcaveMesh = new ConcaveMesh(position8, mCollisionWorld, mMeshFolderPath + "city.obj"); + // Create a convex mesh and a corresponding collision body in the dynamics world + mConcaveMesh = new ConcaveMesh(position8, mCollisionWorld, mMeshFolderPath + "city.obj"); - // Set the color - mConcaveMesh->setColor(mGreyColorDemo); - mConcaveMesh->setSleepingColor(mRedColorDemo); + // Set the color + mConcaveMesh->setColor(mGreyColorDemo); + mConcaveMesh->setSleepingColor(mRedColorDemo); - // ---------- Heightfield ---------- // - openglframework::Vector3 position9(0, 0, 0); + // ---------- Heightfield ---------- // + openglframework::Vector3 position9(0, 0, 0); - // Create a convex mesh and a corresponding collision body in the dynamics world - mHeightField = new HeightField(position9, mCollisionWorld); + // Create a convex mesh and a corresponding collision body in the dynamics world + mHeightField = new HeightField(position9, mCollisionWorld); - // Set the color - mHeightField->setColor(mGreyColorDemo); - mHeightField->setSleepingColor(mRedColorDemo); + // Set the color + mHeightField->setColor(mGreyColorDemo); + mHeightField->setSleepingColor(mRedColorDemo); - // Create the lines that will be used for raycasting - createLines(); + // Create the lines that will be used for raycasting + createLines(); - // Create the VBO and VAO to render the lines - createVBOAndVAO(mPhongShader); + // Create the VBO and VAO to render the lines + createVBOAndVAO(mPhongShader); - changeBody(); + changeBody(); } // Create the raycast lines void RaycastScene::createLines() { - int nbRaysOneDimension = std::sqrt(float(NB_RAYS)); + int32_t nbRaysOneDimension = std::sqrt(float(NB_RAYS)); - for (int i=0; i= NB_BODIES) mCurrentBodyIndex = 0; + mCurrentBodyIndex++; + if (mCurrentBodyIndex >= NB_BODIES) mCurrentBodyIndex = 0; - mSphere->getCollisionBody()->setIsActive(false); - mBox->getCollisionBody()->setIsActive(false); - mCone->getCollisionBody()->setIsActive(false); - mCylinder->getCollisionBody()->setIsActive(false); - mCapsule->getCollisionBody()->setIsActive(false); - mConvexMesh->getCollisionBody()->setIsActive(false); - mDumbbell->getCollisionBody()->setIsActive(false); - mConcaveMesh->getCollisionBody()->setIsActive(false); - mHeightField->getCollisionBody()->setIsActive(false); + mSphere->getCollisionBody()->setIsActive(false); + mBox->getCollisionBody()->setIsActive(false); + mCone->getCollisionBody()->setIsActive(false); + mCylinder->getCollisionBody()->setIsActive(false); + mCapsule->getCollisionBody()->setIsActive(false); + mConvexMesh->getCollisionBody()->setIsActive(false); + mDumbbell->getCollisionBody()->setIsActive(false); + mConcaveMesh->getCollisionBody()->setIsActive(false); + mHeightField->getCollisionBody()->setIsActive(false); - switch(mCurrentBodyIndex) { - case 0: mSphere->getCollisionBody()->setIsActive(true); - break; - case 1: mBox->getCollisionBody()->setIsActive(true); - break; - case 2: mCone->getCollisionBody()->setIsActive(true); - break; - case 3: mCylinder->getCollisionBody()->setIsActive(true); - break; - case 4: mCapsule->getCollisionBody()->setIsActive(true); - break; - case 5: mConvexMesh->getCollisionBody()->setIsActive(true); - break; - case 6: mDumbbell->getCollisionBody()->setIsActive(true); - break; - case 7: mConcaveMesh->getCollisionBody()->setIsActive(true); - break; - case 8: mHeightField->getCollisionBody()->setIsActive(true); - break; + switch(mCurrentBodyIndex) { + case 0: mSphere->getCollisionBody()->setIsActive(true); + break; + case 1: mBox->getCollisionBody()->setIsActive(true); + break; + case 2: mCone->getCollisionBody()->setIsActive(true); + break; + case 3: mCylinder->getCollisionBody()->setIsActive(true); + break; + case 4: mCapsule->getCollisionBody()->setIsActive(true); + break; + case 5: mConvexMesh->getCollisionBody()->setIsActive(true); + break; + case 6: mDumbbell->getCollisionBody()->setIsActive(true); + break; + case 7: mConcaveMesh->getCollisionBody()->setIsActive(true); + break; + case 8: mHeightField->getCollisionBody()->setIsActive(true); + break; - } + } } // Reset the scene @@ -227,74 +227,74 @@ void RaycastScene::reset() { // Destructor RaycastScene::~RaycastScene() { - // Destroy the shader - mPhongShader.destroy(); + // Destroy the shader + mPhongShader.destroy(); - // Destroy the box rigid body from the dynamics world - mCollisionWorld->destroyCollisionBody(mBox->getCollisionBody()); - delete mBox; + // Destroy the box rigid body from the dynamics world + mCollisionWorld->destroyCollisionBody(mBox->getCollisionBody()); + delete mBox; - // Destroy the sphere - mCollisionWorld->destroyCollisionBody(mSphere->getCollisionBody()); - delete mSphere; + // Destroy the sphere + mCollisionWorld->destroyCollisionBody(mSphere->getCollisionBody()); + delete mSphere; - // Destroy the corresponding rigid body from the dynamics world - mCollisionWorld->destroyCollisionBody(mCone->getCollisionBody()); - delete mCone; + // Destroy the corresponding rigid body from the dynamics world + mCollisionWorld->destroyCollisionBody(mCone->getCollisionBody()); + delete mCone; - // Destroy the corresponding rigid body from the dynamics world - mCollisionWorld->destroyCollisionBody(mCylinder->getCollisionBody()); + // Destroy the corresponding rigid body from the dynamics world + mCollisionWorld->destroyCollisionBody(mCylinder->getCollisionBody()); - // Destroy the sphere - delete mCylinder; + // Destroy the sphere + delete mCylinder; - // Destroy the corresponding rigid body from the dynamics world - mCollisionWorld->destroyCollisionBody(mCapsule->getCollisionBody()); + // Destroy the corresponding rigid body from the dynamics world + mCollisionWorld->destroyCollisionBody(mCapsule->getCollisionBody()); - // Destroy the sphere - delete mCapsule; + // Destroy the sphere + delete mCapsule; - // Destroy the corresponding rigid body from the dynamics world - mCollisionWorld->destroyCollisionBody(mConvexMesh->getCollisionBody()); + // Destroy the corresponding rigid body from the dynamics world + mCollisionWorld->destroyCollisionBody(mConvexMesh->getCollisionBody()); - // Destroy the convex mesh - delete mConvexMesh; + // Destroy the convex mesh + delete mConvexMesh; - // Destroy the corresponding rigid body from the dynamics world - mCollisionWorld->destroyCollisionBody(mDumbbell->getCollisionBody()); + // Destroy the corresponding rigid body from the dynamics world + mCollisionWorld->destroyCollisionBody(mDumbbell->getCollisionBody()); - // Destroy the dumbbell - delete mDumbbell; + // Destroy the dumbbell + delete mDumbbell; - // Destroy the corresponding rigid body from the dynamics world - mCollisionWorld->destroyCollisionBody(mConcaveMesh->getCollisionBody()); + // Destroy the corresponding rigid body from the dynamics world + mCollisionWorld->destroyCollisionBody(mConcaveMesh->getCollisionBody()); - // Destroy the convex mesh - delete mConcaveMesh; + // Destroy the convex mesh + delete mConcaveMesh; - // Destroy the corresponding rigid body from the dynamics world - mCollisionWorld->destroyCollisionBody(mHeightField->getCollisionBody()); + // Destroy the corresponding rigid body from the dynamics world + mCollisionWorld->destroyCollisionBody(mHeightField->getCollisionBody()); - // Destroy the convex mesh - delete mHeightField; + // Destroy the convex mesh + delete mHeightField; - m_raycastManager.resetPoints(); + m_raycastManager.resetPoints(); - // Destroy the static data for the visual contact points - VisualContactPoint::destroyStaticData(); + // Destroy the static data for the visual contact points + VisualContactPoint::destroyStaticData(); - // Destroy the collision world - delete mCollisionWorld; + // Destroy the collision world + delete mCollisionWorld; - // Destroy the lines - for (std::vector::iterator it = mLines.begin(); it != mLines.end(); - ++it) { - delete (*it); - } + // Destroy the lines + for (std::vector::iterator it = mLines.begin(); it != mLines.end(); + ++it) { + delete (*it); + } - // Destroy the VBOs and VAO - mVBOVertices.destroy(); - mVAO.destroy(); + // Destroy the VBOs and VAO + mVBOVertices.destroy(); + mVAO.destroy(); } // Update the physics world (take a simulation step) @@ -306,125 +306,125 @@ void RaycastScene::updatePhysics() { // Take a step for the simulation void RaycastScene::update() { - m_raycastManager.resetPoints(); + m_raycastManager.resetPoints(); - // For each line of the scene - for (std::vector::iterator it = mLines.begin(); it != mLines.end(); - ++it) { + // For each line of the scene + for (std::vector::iterator it = mLines.begin(); it != mLines.end(); + ++it) { - Line* line = *it; + Line* line = *it; - // Create a ray corresponding to the line - openglframework::Vector3 p1 = line->getPoint1(); - openglframework::Vector3 p2 = line->getPoint2(); + // Create a ray corresponding to the line + openglframework::Vector3 p1 = line->getPoint1(); + openglframework::Vector3 p2 = line->getPoint2(); - rp3d::Vector3 point1(p1.x, p1.y, p1.z); - rp3d::Vector3 point2(p2.x, p2.y, p2.z); - rp3d::Ray ray(point1, point2); + rp3d::Vector3 point1(p1.x, p1.y, p1.z); + rp3d::Vector3 point2(p2.x, p2.y, p2.z); + rp3d::Ray ray(point1, point2); - // Perform a raycast query on the physics world by passing a raycast - // callback class in argument. - mCollisionWorld->raycast(ray, &m_raycastManager); - } + // Perform a raycast query on the physics world by passing a raycast + // callback class in argument. + mCollisionWorld->raycast(ray, &m_raycastManager); + } - SceneDemo::update(); + SceneDemo::update(); } // Render the scene void RaycastScene::renderSinglePass(openglframework::Shader& shader, - const openglframework::Matrix4& worldToCameraMatrix) { + const openglframework::Matrix4& worldToCameraMatrix) { - // Bind the VAO - mVAO.bind(); + // Bind the VAO + mVAO.bind(); - // Bind the shader - shader.bind(); + // Bind the shader + shader.bind(); - mVBOVertices.bind(); + mVBOVertices.bind(); - // Set the model to camera matrix - const Matrix4 localToCameraMatrix = Matrix4::identity(); - shader.setMatrix4x4Uniform("localToWorldMatrix", localToCameraMatrix); - shader.setMatrix4x4Uniform("worldToCameraMatrix", worldToCameraMatrix); + // Set the model to camera matrix + const Matrix4 localToCameraMatrix = Matrix4::identity(); + shader.setMatrix4x4Uniform("localToWorldMatrix", localToCameraMatrix); + shader.setMatrix4x4Uniform("worldToCameraMatrix", worldToCameraMatrix); - // Set the normal matrix (inverse transpose of the 3x3 upper-left sub matrix of the - // model-view matrix) - const openglframework::Matrix3 normalMatrix = - localToCameraMatrix.getUpperLeft3x3Matrix().getInverse().getTranspose(); - shader.setMatrix3x3Uniform("normalMatrix", normalMatrix, false); + // Set the normal matrix (inverse transpose of the 3x3 upper-left sub matrix of the + // model-view matrix) + const openglframework::Matrix3 normalMatrix = + localToCameraMatrix.getUpperLeft3x3Matrix().getInverse().getTranspose(); + shader.setMatrix3x3Uniform("normalMatrix", normalMatrix, false); - // Set the vertex color - openglframework::Vector4 color(1, 0, 0, 1); - shader.setVector4Uniform("vertexColor", color, false); + // Set the vertex color + openglframework::Vector4 color(1, 0, 0, 1); + shader.setVector4Uniform("vertexColor", color, false); - // Get the location of shader attribute variables - GLint vertexPositionLoc = shader.getAttribLocation("vertexPosition"); + // Get the location of shader attribute variables + GLint32_t vertexPositionLoc = shader.getAttribLocation("vertexPosition"); - glEnableVertexAttribArray(vertexPositionLoc); - glVertexAttribPointer(vertexPositionLoc, 3, GL_FLOAT, GL_FALSE, 0, (char*)NULL); + glEnableVertexAttribArray(vertexPositionLoc); + glVertexAttribPointer(vertexPositionLoc, 3, GL_FLOAT, GL_FALSE, 0, (char*)NULL); - // Draw the lines - glDrawArrays(GL_LINES, 0, NB_RAYS); + // Draw the lines + glDrawArrays(GL_LINES, 0, NB_RAYS); - glDisableVertexAttribArray(vertexPositionLoc); + glDisableVertexAttribArray(vertexPositionLoc); - mVBOVertices.unbind(); + mVBOVertices.unbind(); - // Unbind the VAO - mVAO.unbind(); + // Unbind the VAO + mVAO.unbind(); - shader.unbind(); + shader.unbind(); - // Render the shapes - if (mBox->getCollisionBody()->isActive()) mBox->render(shader, worldToCameraMatrix); - if (mSphere->getCollisionBody()->isActive()) mSphere->render(shader, worldToCameraMatrix); - if (mCone->getCollisionBody()->isActive()) mCone->render(shader, worldToCameraMatrix); - if (mCylinder->getCollisionBody()->isActive()) mCylinder->render(shader, worldToCameraMatrix); - if (mCapsule->getCollisionBody()->isActive()) mCapsule->render(shader, worldToCameraMatrix); - if (mConvexMesh->getCollisionBody()->isActive()) mConvexMesh->render(shader, worldToCameraMatrix); - if (mDumbbell->getCollisionBody()->isActive()) mDumbbell->render(shader, worldToCameraMatrix); - if (mConcaveMesh->getCollisionBody()->isActive()) mConcaveMesh->render(shader, worldToCameraMatrix); - if (mHeightField->getCollisionBody()->isActive()) mHeightField->render(shader, worldToCameraMatrix); + // Render the shapes + if (mBox->getCollisionBody()->isActive()) mBox->render(shader, worldToCameraMatrix); + if (mSphere->getCollisionBody()->isActive()) mSphere->render(shader, worldToCameraMatrix); + if (mCone->getCollisionBody()->isActive()) mCone->render(shader, worldToCameraMatrix); + if (mCylinder->getCollisionBody()->isActive()) mCylinder->render(shader, worldToCameraMatrix); + if (mCapsule->getCollisionBody()->isActive()) mCapsule->render(shader, worldToCameraMatrix); + if (mConvexMesh->getCollisionBody()->isActive()) mConvexMesh->render(shader, worldToCameraMatrix); + if (mDumbbell->getCollisionBody()->isActive()) mDumbbell->render(shader, worldToCameraMatrix); + if (mConcaveMesh->getCollisionBody()->isActive()) mConcaveMesh->render(shader, worldToCameraMatrix); + if (mHeightField->getCollisionBody()->isActive()) mHeightField->render(shader, worldToCameraMatrix); - shader.unbind(); + shader.unbind(); } // Create the Vertex Buffer Objects used to render with OpenGL. /// We create two VBOs (one for vertices and one for indices) void RaycastScene::createVBOAndVAO(openglframework::Shader& shader) { - // Bind the shader - shader.bind(); + // Bind the shader + shader.bind(); - // Create the VBO for the vertices data - mVBOVertices.create(); - mVBOVertices.bind(); - size_t sizeVertices = mLinePoints.size() * sizeof(openglframework::Vector3); - mVBOVertices.copyDataIntoVBO(sizeVertices, &mLinePoints[0], GL_STATIC_DRAW); - mVBOVertices.unbind(); + // Create the VBO for the vertices data + mVBOVertices.create(); + mVBOVertices.bind(); + size_t sizeVertices = mLinePoints.size() * sizeof(openglframework::Vector3); + mVBOVertices.copyDataIntoVBO(sizeVertices, &mLinePoints[0], GL_STATIC_DRAW); + mVBOVertices.unbind(); - // Create the VAO for both VBOs - mVAO.create(); - mVAO.bind(); + // Create the VAO for both VBOs + mVAO.create(); + mVAO.bind(); - // Bind the VBO of vertices - mVBOVertices.bind(); + // Bind the VBO of vertices + mVBOVertices.bind(); - // Unbind the VAO - mVAO.unbind(); + // Unbind the VAO + mVAO.unbind(); - // Unbind the shader - shader.unbind(); + // Unbind the shader + shader.unbind(); } // Called when a keyboard event occurs -bool RaycastScene::keyboardEvent(int key, int scancode, int action, int mods) { +bool RaycastScene::keyboardEvent(int32_t key, int32_t scancode, int32_t action, int32_t mods) { - // If the space key has been pressed - if (key == GLFW_KEY_SPACE && action == GLFW_PRESS) { - changeBody(); - return true; - } + // If the space key has been pressed + if (key == GLFW_KEY_SPACE && action == GLFW_PRESS) { + changeBody(); + return true; + } - return false; + return false; } diff --git a/tools/testbed/scenes/raycast/RaycastScene.h b/tools/testbed/scenes/raycast/RaycastScene.h index cb9a1a8..932f775 100644 --- a/tools/testbed/scenes/raycast/RaycastScene.h +++ b/tools/testbed/scenes/raycast/RaycastScene.h @@ -1,26 +1,26 @@ /******************************************************************************** -* ReactPhysics3D physics library, http://www.reactphysics3d.com * -* Copyright (c) 2010-2016 Daniel Chappuis * +* 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: * -* * +* 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. * -* * +* 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 RAYCAST_SCENE_H @@ -57,178 +57,178 @@ const float CYLINDER_HEIGHT = 5.0f; const float CAPSULE_RADIUS = 3.0f; const float CAPSULE_HEIGHT = 5.0f; const float DUMBBELL_HEIGHT = 5.0f; -const int NB_RAYS = 100; +const int32_t NB_RAYS = 100; const float RAY_LENGTH = 30.0f; -const int NB_BODIES = 9; +const int32_t NB_BODIES = 9; // Raycast manager class RaycastManager : public rp3d::RaycastCallback { - private: + private: - /// All the visual contact points - std::vector mHitPoints; + /// All the visual contact points + std::vector mHitPoints; - /// All the normals at hit points - std::vector mNormals; + /// All the normals at hit points + std::vector mNormals; - /// Contact point mesh folder path - std::string mMeshFolderPath; + /// Contact point mesh folder path + std::string mMeshFolderPath; public: - RaycastManager(openglframework::Shader& shader, - const std::string& meshFolderPath) - : mMeshFolderPath(meshFolderPath) { + RaycastManager(openglframework::Shader& shader, + const std::string& meshFolderPath) + : mMeshFolderPath(meshFolderPath) { - } + } - virtual rp3d::decimal notifyRaycastHit(const rp3d::RaycastInfo& raycastInfo) { - rp3d::Vector3 hitPos = raycastInfo.worldPoint; - openglframework::Vector3 position(hitPos.x, hitPos.y, hitPos.z); - mHitPoints.push_back(ContactPoint(position)); + virtual rp3d::float notifyRaycastHit(const rp3d::RaycastInfo& raycastInfo) { + rp3d::Vector3 hitPos = raycastInfo.worldPoint; + openglframework::Vector3 position(hitPos.x, hitPos.y, hitPos.z); + mHitPoints.push_back(ContactPoint(position)); - // Create a line to display the normal at hit point - rp3d::Vector3 n = raycastInfo.worldNormal; - openglframework::Vector3 normal(n.x, n.y, n.z); - Line* normalLine = new Line(position, position + normal); - mNormals.push_back(normalLine); + // Create a line to display the normal at hit point + rp3d::Vector3 n = raycastInfo.worldNormal; + openglframework::Vector3 normal(n.x, n.y, n.z); + Line* normalLine = new Line(position, position + normal); + mNormals.push_back(normalLine); - return raycastInfo.hitFraction; - } + return raycastInfo.hitFraction; + } - void resetPoints() { + void resetPoints() { - mHitPoints.clear(); + mHitPoints.clear(); - // Destroy all the normals - for (std::vector::iterator it = mNormals.begin(); - it != mNormals.end(); ++it) { - delete (*it); - } - mNormals.clear(); - } + // Destroy all the normals + for (std::vector::iterator it = mNormals.begin(); + it != mNormals.end(); ++it) { + delete (*it); + } + mNormals.clear(); + } - std::vector getHitPoints() const { - return mHitPoints; - } + std::vector getHitPoints() const { + return mHitPoints; + } }; // Class RaycastScene class RaycastScene : public SceneDemo { - private : + private : - // -------------------- Attributes -------------------- // + // -------------------- Attributes -------------------- // - /// Contact point mesh folder path - std::string mMeshFolderPath; + /// Contact point mesh folder path + std::string mMeshFolderPath; - /// Raycast manager - RaycastManager m_raycastManager; + /// Raycast manager + RaycastManager m_raycastManager; - /// All the raycast lines - std::vector mLines; + /// All the raycast lines + std::vector mLines; - /// Current body index - int mCurrentBodyIndex; + /// Current body index + int32_t mCurrentBodyIndex; - /// True if the hit points normals are displayed - bool mAreNormalsDisplayed; + /// True if the hit points normals are displayed + bool mAreNormalsDisplayed; - /// Raycast manager + /// Raycast manager - /// All objects on the scene - Box* mBox; - Sphere* mSphere; - Cone* mCone; - Cylinder* mCylinder; - Capsule* mCapsule; - ConvexMesh* mConvexMesh; - Dumbbell* mDumbbell; - ConcaveMesh* mConcaveMesh; - HeightField* mHeightField; + /// All objects on the scene + Box* mBox; + Sphere* mSphere; + Cone* mCone; + Cylinder* mCylinder; + Capsule* mCapsule; + ConvexMesh* mConvexMesh; + Dumbbell* mDumbbell; + ConcaveMesh* mConcaveMesh; + HeightField* mHeightField; - /// Collision world used for the physics simulation - rp3d::CollisionWorld* mCollisionWorld; + /// Collision world used for the physics simulation + rp3d::CollisionWorld* mCollisionWorld; - /// All the points to render the lines - std::vector mLinePoints; + /// All the points to render the lines + std::vector mLinePoints; - /// Vertex Buffer Object for the vertices data - openglframework::VertexBufferObject mVBOVertices; + /// Vertex Buffer Object for the vertices data + openglframework::VertexBufferObject mVBOVertices; - /// Vertex Array Object for the vertex data - openglframework::VertexArrayObject mVAO; + /// Vertex Array Object for the vertex data + openglframework::VertexArrayObject mVAO; - /// Create the raycast lines - void createLines(); + /// Create the raycast lines + void createLines(); - // Create the Vertex Buffer Objects used to render with OpenGL. - void createVBOAndVAO(openglframework::Shader& shader); + // Create the Vertex Buffer Objects used to render with OpenGL. + void createVBOAndVAO(openglframework::Shader& shader); - public: + public: - // -------------------- Methods -------------------- // + // -------------------- Methods -------------------- // - /// Constructor - RaycastScene(const std::string& name); + /// Constructor + RaycastScene(const std::string& name); - /// Destructor - virtual ~RaycastScene(); + /// Destructor + virtual ~RaycastScene(); - /// Update the physics world (take a simulation step) - /// Can be called several times per frame - virtual void updatePhysics(); + /// Update the physics world (take a simulation step) + /// Can be called several times per frame + virtual void updatePhysics(); - /// Take a step for the simulation - virtual void update(); + /// Take a step for the simulation + virtual void update(); - /// Render the scene in a single pass - virtual void renderSinglePass(openglframework::Shader& shader, - const openglframework::Matrix4& worldToCameraMatrix); + /// Render the scene in a single pass + virtual void renderSinglePass(openglframework::Shader& shader, + const openglframework::Matrix4& worldToCameraMatrix); - /// Reset the scene - virtual void reset(); + /// Reset the scene + virtual void reset(); - /// Change the body to raycast - void changeBody(); + /// Change the body to raycast + void changeBody(); - /// Display or not the surface normals at hit points - void showHideNormals(); + /// Display or not the surface normals at hit points + void showHideNormals(); - /// Called when a keyboard event occurs - virtual bool keyboardEvent(int key, int scancode, int action, int mods); + /// Called when a keyboard event occurs + virtual bool keyboardEvent(int32_t key, int32_t scancode, int32_t action, int32_t mods); - /// Enabled/Disable the shadow mapping - void virtual setIsShadowMappingEnabled(bool isShadowMappingEnabled); + /// Enabled/Disable the shadow mapping + void virtual setIsShadowMappingEnabled(bool isShadowMappingEnabled); - /// Display/Hide the contact points - void virtual setIsContactPointsDisplayed(bool display); + /// Display/Hide the contact points + void virtual setIsContactPointsDisplayed(bool display); - /// Return all the contact points of the scene - virtual std::vector getContactPoints() const; + /// Return all the contact points of the scene + virtual std::vector getContactPoints() const; }; // Display or not the surface normals at hit points inline void RaycastScene::showHideNormals() { - mAreNormalsDisplayed = !mAreNormalsDisplayed; + mAreNormalsDisplayed = !mAreNormalsDisplayed; } // Enabled/Disable the shadow mapping inline void RaycastScene::setIsShadowMappingEnabled(bool isShadowMappingEnabled) { - SceneDemo::setIsShadowMappingEnabled(false); + SceneDemo::setIsShadowMappingEnabled(false); } // Display/Hide the contact points inline void RaycastScene::setIsContactPointsDisplayed(bool display) { - SceneDemo::setIsContactPointsDisplayed(true); + SceneDemo::setIsContactPointsDisplayed(true); } // Return all the contact points of the scene inline std::vector RaycastScene::getContactPoints() const { - return m_raycastManager.getHitPoints(); + return m_raycastManager.getHitPoints(); } } diff --git a/tools/testbed/src/Gui.cpp b/tools/testbed/src/Gui.cpp index c8f087a..1b2df88 100644 --- a/tools/testbed/src/Gui.cpp +++ b/tools/testbed/src/Gui.cpp @@ -1,26 +1,26 @@ /******************************************************************************** -* ReactPhysics3D physics library, http://www.reactphysics3d.com * -* Copyright (c) 2010-2016 Daniel Chappuis * +* 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: * -* * +* 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. * -* * +* 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 @@ -52,377 +52,377 @@ Gui::~Gui() { /// Initialize the GUI void Gui::init() { - // Create the Simulation panel - createSimulationPanel(); + // Create the Simulation panel + createSimulationPanel(); - // Create the Settings panel - createSettingsPanel(); + // Create the Settings panel + createSettingsPanel(); - // Create the Profiling panel - createProfilingPanel(); + // Create the Profiling panel + createProfilingPanel(); - mApp->setVisible(true); - mApp->performLayout(); + mApp->setVisible(true); + mApp->performLayout(); - mTimeSinceLastProfilingDisplay = glfwGetTime(); + mTimeSinceLastProfilingDisplay = glfwGetTime(); } // Update the GUI void Gui::update() { - // Update Profiling GUI every seconds - if ((mApp->mCurrentTime - mTimeSinceLastProfilingDisplay) > TIME_INTERVAL_DISPLAY_PROFILING_INFO) { - mTimeSinceLastProfilingDisplay = mApp->mCurrentTime; - mCachedFPS = mApp->mFPS; - mCachedUpdateTime = mApp->mFrameTime; - mCachedPhysicsUpdateTime = mApp->mPhysicsTime; - } + // Update Profiling GUI every seconds + if ((mApp->mCurrentTime - mTimeSinceLastProfilingDisplay) > TIME_INTERVAL_DISPLAY_PROFILING_INFO) { + mTimeSinceLastProfilingDisplay = mApp->mCurrentTime; + mCachedFPS = mApp->mFPS; + mCachedUpdateTime = mApp->mFrameTime; + mCachedPhysicsUpdateTime = mApp->mPhysicsTime; + } - // Framerate (FPS) - mFPSLabel->setCaption(std::string("FPS : ") + floatToString(mCachedFPS, 0)); + // Framerate (FPS) + mFPSLabel->setCaption(std::string("FPS : ") + floatToString(mCachedFPS, 0)); - // Frame time - mFrameTimeLabel->setCaption(std::string("Frame time : ") + floatToString(mCachedUpdateTime * 1000.0, 1) + std::string(" ms")); + // Frame time + mFrameTimeLabel->setCaption(std::string("Frame time : ") + floatToString(mCachedUpdateTime * 1000.0, 1) + std::string(" ms")); - // Physics time - mPhysicsTimeLabel->setCaption(std::string("Physics time : ") + floatToString(mCachedPhysicsUpdateTime * 1000.0, 1) + std::string(" ms")); + // Physics time + mPhysicsTimeLabel->setCaption(std::string("Physics time : ") + floatToString(mCachedPhysicsUpdateTime * 1000.0, 1) + std::string(" ms")); } void Gui::createSimulationPanel() { - mSimulationPanel = new Window(mApp, "Simulation"); - mSimulationPanel->setPosition(Vector2i(15, 15)); - mSimulationPanel->setLayout(new GroupLayout(10, 5, 10 , 20)); - mSimulationPanel->setId("SimulationPanel"); - mSimulationPanel->setFixedWidth(220); + mSimulationPanel = new Window(mApp, "Simulation"); + mSimulationPanel->setPosition(Vector2i(15, 15)); + mSimulationPanel->setLayout(new GroupLayout(10, 5, 10 , 20)); + mSimulationPanel->setId("SimulationPanel"); + mSimulationPanel->setFixedWidth(220); - // Scenes/Physics/Rendering buttons - new Label(mSimulationPanel, "Controls","sans-bold"); - Widget* panelControls = new Widget(mSimulationPanel); - panelControls->setLayout(new BoxLayout(Orientation::Horizontal, Alignment::Middle, 0, 5)); - ToolButton* buttonPlay = new ToolButton(panelControls, ENTYPO_ICON_PLAY); - buttonPlay->setFlags(Button::NormalButton); - buttonPlay->setCallback([&] { - mApp->playSimulation(); - }); - ToolButton* buttonPause = new ToolButton(panelControls, ENTYPO_ICON_PAUS); - buttonPause->setFlags(Button::NormalButton); - buttonPause->setCallback([&] { - mApp->pauseSimulation(); - }); - ToolButton* buttonPlayStep = new ToolButton(panelControls, ENTYPO_ICON_TO_END); - buttonPlayStep->setFlags(Button::NormalButton); - buttonPlayStep->setCallback([&] { - mApp->toggleTakeSinglePhysicsStep(); - }); - ToolButton* buttonRestart = new ToolButton(panelControls, ENTYPO_ICON_CCW); - buttonRestart->setFlags(Button::NormalButton); - buttonRestart->setCallback([&] { - mApp->restartSimulation(); - }); + // Scenes/Physics/Rendering buttons + new Label(mSimulationPanel, "Controls","sans-bold"); + Widget* panelControls = new Widget(mSimulationPanel); + panelControls->setLayout(new BoxLayout(Orientation::Horizontal, Alignment::Middle, 0, 5)); + ToolButton* buttonPlay = new ToolButton(panelControls, ENTYPO_ICON_PLAY); + buttonPlay->setFlags(Button::NormalButton); + buttonPlay->setCallback([&] { + mApp->playSimulation(); + }); + ToolButton* buttonPause = new ToolButton(panelControls, ENTYPO_ICON_PAUS); + buttonPause->setFlags(Button::NormalButton); + buttonPause->setCallback([&] { + mApp->pauseSimulation(); + }); + ToolButton* buttonPlayStep = new ToolButton(panelControls, ENTYPO_ICON_TO_END); + buttonPlayStep->setFlags(Button::NormalButton); + buttonPlayStep->setCallback([&] { + mApp->toggleTakeSinglePhysicsStep(); + }); + ToolButton* buttonRestart = new ToolButton(panelControls, ENTYPO_ICON_CCW); + buttonRestart->setFlags(Button::NormalButton); + buttonRestart->setCallback([&] { + mApp->restartSimulation(); + }); - // Scenes - std::vector scenes = mApp->getScenes(); - std::vector scenesNames; - for (uint i=0; igetName().c_str()); - } - new Label(mSimulationPanel, "Scene","sans-bold"); - ComboBox* comboBoxScenes = new ComboBox(mSimulationPanel, scenesNames, [&, scenes](int index) { - mApp->switchScene(scenes[index]); - }); - comboBoxScenes->setFixedWidth(150); + // Scenes + std::vector scenes = mApp->getScenes(); + std::vector scenesNames; + for (uint32_t i=0; igetName().c_str()); + } + new Label(mSimulationPanel, "Scene","sans-bold"); + ComboBox* comboBoxScenes = new ComboBox(mSimulationPanel, scenesNames, [&, scenes](int32_t index) { + mApp->switchScene(scenes[index]); + }); + comboBoxScenes->setFixedWidth(150); } void Gui::createSettingsPanel() { - mSettingsPanel = new Window(mApp, "Settings"); - mSettingsPanel->setPosition(Vector2i(15, 180)); - mSettingsPanel->setLayout(new BoxLayout(Orientation::Vertical, Alignment::Middle, 10, 5)); - mSettingsPanel->setId("SettingsPanel"); - mSettingsPanel->setFixedWidth(220); + mSettingsPanel = new Window(mApp, "Settings"); + mSettingsPanel->setPosition(Vector2i(15, 180)); + mSettingsPanel->setLayout(new BoxLayout(Orientation::Vertical, Alignment::Middle, 10, 5)); + mSettingsPanel->setId("SettingsPanel"); + mSettingsPanel->setFixedWidth(220); - // Scenes/Physics/Rendering buttons - Widget* buttonsPanel = new Widget(mSettingsPanel); - buttonsPanel->setLayout(new BoxLayout(Orientation::Horizontal, Alignment::Middle, 5, 5)); - Button* buttonPhysics = new Button(buttonsPanel, "Physics"); - buttonPhysics->setFlags(Button::RadioButton); - buttonPhysics->setPushed(true); - buttonPhysics->setChangeCallback([&](bool state) { - mPhysicsPanel->setVisible(true); - mRenderingPanel->setVisible(false); - mApp->performLayout(); - }); - Button* buttonRendering = new Button(buttonsPanel, "Rendering"); - buttonRendering->setFlags(Button::RadioButton); - buttonRendering->setChangeCallback([&](bool state) { - mRenderingPanel->setVisible(true); - mPhysicsPanel->setVisible(false); - mApp->performLayout(); - }); + // Scenes/Physics/Rendering buttons + Widget* buttonsPanel = new Widget(mSettingsPanel); + buttonsPanel->setLayout(new BoxLayout(Orientation::Horizontal, Alignment::Middle, 5, 5)); + Button* buttonPhysics = new Button(buttonsPanel, "Physics"); + buttonPhysics->setFlags(Button::RadioButton); + buttonPhysics->setPushed(true); + buttonPhysics->setChangeCallback([&](bool state) { + mPhysicsPanel->setVisible(true); + mRenderingPanel->setVisible(false); + mApp->performLayout(); + }); + Button* buttonRendering = new Button(buttonsPanel, "Rendering"); + buttonRendering->setFlags(Button::RadioButton); + buttonRendering->setChangeCallback([&](bool state) { + mRenderingPanel->setVisible(true); + mPhysicsPanel->setVisible(false); + mApp->performLayout(); + }); - // ---------- Physics Panel ---------- - mPhysicsPanel = new Widget(mSettingsPanel); - mPhysicsPanel->setLayout(new BoxLayout(Orientation::Vertical, Alignment::Fill, 0, 5)); + // ---------- Physics Panel ---------- + mPhysicsPanel = new Widget(mSettingsPanel); + mPhysicsPanel->setLayout(new BoxLayout(Orientation::Vertical, Alignment::Fill, 0, 5)); - // Enable/Disable sleeping - CheckBox* checkboxSleeping = new CheckBox(mPhysicsPanel, "Sleeping enabled"); - checkboxSleeping->setChecked(mApp->mEngineSettings.isSleepingEnabled); - checkboxSleeping->setCallback([&](bool value) { - mApp->mEngineSettings.isSleepingEnabled = value; - }); + // Enable/Disable sleeping + CheckBox* checkboxSleeping = new CheckBox(mPhysicsPanel, "Sleeping enabled"); + checkboxSleeping->setChecked(mApp->mEngineSettings.isSleepingEnabled); + checkboxSleeping->setCallback([&](bool value) { + mApp->mEngineSettings.isSleepingEnabled = value; + }); - // Enabled/Disable Gravity - CheckBox* checkboxGravity = new CheckBox(mPhysicsPanel, "Gravity enabled"); - checkboxGravity->setChecked(mApp->mEngineSettings.isGravityEnabled); - checkboxGravity->setCallback([&](bool value) { - mApp->mEngineSettings.isGravityEnabled = value; - }); + // Enabled/Disable Gravity + CheckBox* checkboxGravity = new CheckBox(mPhysicsPanel, "Gravity enabled"); + checkboxGravity->setChecked(mApp->mEngineSettings.isGravityEnabled); + checkboxGravity->setCallback([&](bool value) { + mApp->mEngineSettings.isGravityEnabled = value; + }); - // Timestep - Widget* panelTimeStep = new Widget(mPhysicsPanel); - panelTimeStep->setLayout(new BoxLayout(Orientation::Horizontal, Alignment::Middle, 0, 5)); - Label* labelTimeStep = new Label(panelTimeStep, "Time step","sans-bold"); - labelTimeStep->setFixedWidth(120); - TextBox* textboxTimeStep = new TextBox(panelTimeStep); - textboxTimeStep->setFixedSize(Vector2i(70, 25)); - textboxTimeStep->setEditable(true); - std::ostringstream out; - out << std::setprecision(1) << std::fixed << (mApp->mEngineSettings.timeStep * 1000); - textboxTimeStep->setValue(out.str()); - textboxTimeStep->setUnits("ms"); - textboxTimeStep->setCallback([&, textboxTimeStep](const std::string &str) { + // Timestep + Widget* panelTimeStep = new Widget(mPhysicsPanel); + panelTimeStep->setLayout(new BoxLayout(Orientation::Horizontal, Alignment::Middle, 0, 5)); + Label* labelTimeStep = new Label(panelTimeStep, "Time step","sans-bold"); + labelTimeStep->setFixedWidth(120); + TextBox* textboxTimeStep = new TextBox(panelTimeStep); + textboxTimeStep->setFixedSize(Vector2i(70, 25)); + textboxTimeStep->setEditable(true); + std::ostringstream out; + out << std::setprecision(1) << std::fixed << (mApp->mEngineSettings.timeStep * 1000); + textboxTimeStep->setValue(out.str()); + textboxTimeStep->setUnits("ms"); + textboxTimeStep->setCallback([&, textboxTimeStep](const std::string &str) { - try { - float value = std::stof(str); - std::ostringstream out; - out << std::setprecision(1) << std::fixed << std::showpoint << value; - float finalValue = std::stof(out.str()); + try { + float value = std::stof(str); + std::ostringstream out; + out << std::setprecision(1) << std::fixed << std::showpoint << value; + float finalValue = std::stof(out.str()); - if (finalValue < 1 || finalValue > 1000) return false; + if (finalValue < 1 || finalValue > 1000) return false; - mApp->mEngineSettings.timeStep = finalValue / 1000.0f; - textboxTimeStep->setValue(out.str()); - } - catch (...) { - return false; - } + mApp->mEngineSettings.timeStep = finalValue / 1000.0f; + textboxTimeStep->setValue(out.str()); + } + catch (...) { + return false; + } - return true; - }); - textboxTimeStep->setFontSize(16); - textboxTimeStep->setAlignment(TextBox::Alignment::Right); + return true; + }); + textboxTimeStep->setFontSize(16); + textboxTimeStep->setAlignment(TextBox::Alignment::Right); - // Velocity solver iterations - Widget* panelVelocityIterations = new Widget(mPhysicsPanel); - panelVelocityIterations->setLayout(new BoxLayout(Orientation::Horizontal, Alignment::Middle, 0, 5)); - Label* labelVelocityIterations = new Label(panelVelocityIterations, "Velocity solver","sans-bold"); - labelVelocityIterations->setFixedWidth(120); - TextBox* textboxVelocityIterations = new TextBox(panelVelocityIterations); - textboxVelocityIterations->setFixedSize(Vector2i(70, 25)); - textboxVelocityIterations->setEditable(true); - textboxVelocityIterations->setValue(std::to_string(mApp->mEngineSettings.nbVelocitySolverIterations)); - textboxVelocityIterations->setUnits("iter"); - textboxVelocityIterations->setCallback([&, textboxVelocityIterations](const std::string &str) { + // Velocity solver iterations + Widget* panelVelocityIterations = new Widget(mPhysicsPanel); + panelVelocityIterations->setLayout(new BoxLayout(Orientation::Horizontal, Alignment::Middle, 0, 5)); + Label* labelVelocityIterations = new Label(panelVelocityIterations, "Velocity solver","sans-bold"); + labelVelocityIterations->setFixedWidth(120); + TextBox* textboxVelocityIterations = new TextBox(panelVelocityIterations); + textboxVelocityIterations->setFixedSize(Vector2i(70, 25)); + textboxVelocityIterations->setEditable(true); + textboxVelocityIterations->setValue(std::to_string(mApp->mEngineSettings.nbVelocitySolverIterations)); + textboxVelocityIterations->setUnits("iter"); + textboxVelocityIterations->setCallback([&, textboxVelocityIterations](const std::string &str) { - try { - float value = std::stof(str); - std::ostringstream out; - out << std::setprecision(0) << std::fixed << value; + try { + float value = std::stof(str); + std::ostringstream out; + out << std::setprecision(0) << std::fixed << value; - if (value < 1 || value > 1000) return false; + if (value < 1 || value > 1000) return false; - mApp->mEngineSettings.nbVelocitySolverIterations = value; - textboxVelocityIterations->setValue(out.str()); - } - catch (...) { - return false; - } + mApp->mEngineSettings.nbVelocitySolverIterations = value; + textboxVelocityIterations->setValue(out.str()); + } + catch (...) { + return false; + } - return true; - }); - textboxVelocityIterations->setFontSize(16); - textboxVelocityIterations->setAlignment(TextBox::Alignment::Right); + return true; + }); + textboxVelocityIterations->setFontSize(16); + textboxVelocityIterations->setAlignment(TextBox::Alignment::Right); - // Position solver iterations - Widget* panelPositionIterations = new Widget(mPhysicsPanel); - panelPositionIterations->setLayout(new BoxLayout(Orientation::Horizontal, Alignment::Middle, 0, 5)); - Label* labelPositionIterations = new Label(panelPositionIterations, "Position solver","sans-bold"); - labelPositionIterations->setFixedWidth(120); - TextBox* textboxPositionIterations = new TextBox(panelPositionIterations); - textboxPositionIterations->setFixedSize(Vector2i(70, 25)); - textboxPositionIterations->setEditable(true); - textboxPositionIterations->setValue(std::to_string(mApp->mEngineSettings.nbPositionSolverIterations)); - textboxPositionIterations->setUnits("iter"); - textboxPositionIterations->setCallback([&, textboxPositionIterations](const std::string &str) { + // Position solver iterations + Widget* panelPositionIterations = new Widget(mPhysicsPanel); + panelPositionIterations->setLayout(new BoxLayout(Orientation::Horizontal, Alignment::Middle, 0, 5)); + Label* labelPositionIterations = new Label(panelPositionIterations, "Position solver","sans-bold"); + labelPositionIterations->setFixedWidth(120); + TextBox* textboxPositionIterations = new TextBox(panelPositionIterations); + textboxPositionIterations->setFixedSize(Vector2i(70, 25)); + textboxPositionIterations->setEditable(true); + textboxPositionIterations->setValue(std::to_string(mApp->mEngineSettings.nbPositionSolverIterations)); + textboxPositionIterations->setUnits("iter"); + textboxPositionIterations->setCallback([&, textboxPositionIterations](const std::string &str) { - try { - float value = std::stof(str); - std::ostringstream out; - out << std::setprecision(0) << std::fixed << value; + try { + float value = std::stof(str); + std::ostringstream out; + out << std::setprecision(0) << std::fixed << value; - if (value < 1 || value > 1000) return false; + if (value < 1 || value > 1000) return false; - mApp->mEngineSettings.nbPositionSolverIterations = value; - textboxPositionIterations->setValue(out.str()); - } - catch (...) { - return false; - } + mApp->mEngineSettings.nbPositionSolverIterations = value; + textboxPositionIterations->setValue(out.str()); + } + catch (...) { + return false; + } - return true; - }); - textboxPositionIterations->setFontSize(16); - textboxPositionIterations->setAlignment(TextBox::Alignment::Right); + return true; + }); + textboxPositionIterations->setFontSize(16); + textboxPositionIterations->setAlignment(TextBox::Alignment::Right); - // Time before sleep - Widget* panelTimeSleep = new Widget(mPhysicsPanel); - panelTimeSleep->setLayout(new BoxLayout(Orientation::Horizontal, Alignment::Middle, 0, 5)); - Label* labelTimeSleep = new Label(panelTimeSleep, "Time before sleep","sans-bold"); - labelTimeSleep->setFixedWidth(120); - out.str(""); - out << std::setprecision(0) << std::fixed << (mApp->mEngineSettings.timeBeforeSleep * 1000); - TextBox* textboxTimeSleep = new TextBox(panelTimeSleep); - textboxTimeSleep->setFixedSize(Vector2i(70, 25)); - textboxTimeSleep->setEditable(true); - textboxTimeSleep->setValue(out.str()); - textboxTimeSleep->setUnits("ms"); - textboxTimeSleep->setCallback([&, textboxTimeSleep](const std::string &str) { + // Time before sleep + Widget* panelTimeSleep = new Widget(mPhysicsPanel); + panelTimeSleep->setLayout(new BoxLayout(Orientation::Horizontal, Alignment::Middle, 0, 5)); + Label* labelTimeSleep = new Label(panelTimeSleep, "Time before sleep","sans-bold"); + labelTimeSleep->setFixedWidth(120); + out.str(""); + out << std::setprecision(0) << std::fixed << (mApp->mEngineSettings.timeBeforeSleep * 1000); + TextBox* textboxTimeSleep = new TextBox(panelTimeSleep); + textboxTimeSleep->setFixedSize(Vector2i(70, 25)); + textboxTimeSleep->setEditable(true); + textboxTimeSleep->setValue(out.str()); + textboxTimeSleep->setUnits("ms"); + textboxTimeSleep->setCallback([&, textboxTimeSleep](const std::string &str) { - try { - float value = std::stof(str); - std::ostringstream out; - out << std::setprecision(0) << std::fixed << std::showpoint << value; - float finalValue = std::stof(out.str()); + try { + float value = std::stof(str); + std::ostringstream out; + out << std::setprecision(0) << std::fixed << std::showpoint << value; + float finalValue = std::stof(out.str()); - if (finalValue < 1 || finalValue > 100000) return false; + if (finalValue < 1 || finalValue > 100000) return false; - mApp->mEngineSettings.timeBeforeSleep = finalValue / 1000.0f; - textboxTimeSleep->setValue(out.str()); - } - catch (...) { - return false; - } + mApp->mEngineSettings.timeBeforeSleep = finalValue / 1000.0f; + textboxTimeSleep->setValue(out.str()); + } + catch (...) { + return false; + } - return true; - }); - textboxTimeSleep->setFontSize(16); - textboxTimeSleep->setAlignment(TextBox::Alignment::Right); + return true; + }); + textboxTimeSleep->setFontSize(16); + textboxTimeSleep->setAlignment(TextBox::Alignment::Right); - // Sleep linear velocity - Widget* panelSleepLinearVel = new Widget(mPhysicsPanel); - panelSleepLinearVel->setLayout(new GridLayout(Orientation::Horizontal, 2, Alignment::Middle, 0, 5)); - Label* labelSleepLinearVel = new Label(panelSleepLinearVel, "Sleep linear velocity","sans-bold"); - labelSleepLinearVel->setFixedWidth(120); - out.str(""); - out << std::setprecision(2) << std::fixed << (mApp->mEngineSettings.sleepLinearVelocity); - TextBox* textboxSleepLinearVel = new TextBox(panelSleepLinearVel); - textboxSleepLinearVel->setFixedSize(Vector2i(70, 25)); - textboxSleepLinearVel->setEditable(true); - textboxSleepLinearVel->setValue(out.str()); - textboxSleepLinearVel->setUnits("m/s"); - textboxSleepLinearVel->setCallback([&, textboxSleepLinearVel](const std::string &str) { + // Sleep linear velocity + Widget* panelSleepLinearVel = new Widget(mPhysicsPanel); + panelSleepLinearVel->setLayout(new GridLayout(Orientation::Horizontal, 2, Alignment::Middle, 0, 5)); + Label* labelSleepLinearVel = new Label(panelSleepLinearVel, "Sleep linear velocity","sans-bold"); + labelSleepLinearVel->setFixedWidth(120); + out.str(""); + out << std::setprecision(2) << std::fixed << (mApp->mEngineSettings.sleepLinearVelocity); + TextBox* textboxSleepLinearVel = new TextBox(panelSleepLinearVel); + textboxSleepLinearVel->setFixedSize(Vector2i(70, 25)); + textboxSleepLinearVel->setEditable(true); + textboxSleepLinearVel->setValue(out.str()); + textboxSleepLinearVel->setUnits("m/s"); + textboxSleepLinearVel->setCallback([&, textboxSleepLinearVel](const std::string &str) { - try { - float value = std::stof(str); - std::ostringstream out; - out << std::setprecision(2) << std::fixed << std::showpoint << value; - float finalValue = std::stof(out.str()); + try { + float value = std::stof(str); + std::ostringstream out; + out << std::setprecision(2) << std::fixed << std::showpoint << value; + float finalValue = std::stof(out.str()); - if (finalValue < 0 || finalValue > 10000) return false; + if (finalValue < 0 || finalValue > 10000) return false; - mApp->mEngineSettings.sleepLinearVelocity = finalValue; - textboxSleepLinearVel->setValue(out.str()); - } - catch (...) { - return false; - } + mApp->mEngineSettings.sleepLinearVelocity = finalValue; + textboxSleepLinearVel->setValue(out.str()); + } + catch (...) { + return false; + } - return true; - }); - textboxSleepLinearVel->setFontSize(16); - textboxSleepLinearVel->setAlignment(TextBox::Alignment::Right); + return true; + }); + textboxSleepLinearVel->setFontSize(16); + textboxSleepLinearVel->setAlignment(TextBox::Alignment::Right); - // Sleep angular velocity - Widget* panelSleepAngularVel = new Widget(mPhysicsPanel); - panelSleepAngularVel->setLayout(new GridLayout(Orientation::Horizontal, 2, Alignment::Middle, 0, 5)); - Label* labelSleepAngularVel = new Label(panelSleepAngularVel, "Sleep angular velocity","sans-bold"); - labelSleepAngularVel->setFixedWidth(120); - out.str(""); - out << std::setprecision(2) << std::fixed << (mApp->mEngineSettings.sleepAngularVelocity); - TextBox* textboxSleepAngularVel = new TextBox(panelSleepAngularVel); - textboxSleepAngularVel->setFixedSize(Vector2i(70, 25)); - textboxSleepAngularVel->setEditable(true); - textboxSleepAngularVel->setValue(out.str()); - textboxSleepAngularVel->setUnits("rad/s"); - textboxSleepAngularVel->setCallback([&, textboxSleepAngularVel](const std::string &str) { + // Sleep angular velocity + Widget* panelSleepAngularVel = new Widget(mPhysicsPanel); + panelSleepAngularVel->setLayout(new GridLayout(Orientation::Horizontal, 2, Alignment::Middle, 0, 5)); + Label* labelSleepAngularVel = new Label(panelSleepAngularVel, "Sleep angular velocity","sans-bold"); + labelSleepAngularVel->setFixedWidth(120); + out.str(""); + out << std::setprecision(2) << std::fixed << (mApp->mEngineSettings.sleepAngularVelocity); + TextBox* textboxSleepAngularVel = new TextBox(panelSleepAngularVel); + textboxSleepAngularVel->setFixedSize(Vector2i(70, 25)); + textboxSleepAngularVel->setEditable(true); + textboxSleepAngularVel->setValue(out.str()); + textboxSleepAngularVel->setUnits("rad/s"); + textboxSleepAngularVel->setCallback([&, textboxSleepAngularVel](const std::string &str) { - try { - float value = std::stof(str); - std::ostringstream out; - out << std::setprecision(2) << std::fixed << std::showpoint << value; - float finalValue = std::stof(out.str()); + try { + float value = std::stof(str); + std::ostringstream out; + out << std::setprecision(2) << std::fixed << std::showpoint << value; + float finalValue = std::stof(out.str()); - if (finalValue < 0 || finalValue > 10000) return false; + if (finalValue < 0 || finalValue > 10000) return false; - mApp->mEngineSettings.sleepAngularVelocity = finalValue; - textboxSleepAngularVel->setValue(out.str()); - } - catch (...) { - return false; - } + mApp->mEngineSettings.sleepAngularVelocity = finalValue; + textboxSleepAngularVel->setValue(out.str()); + } + catch (...) { + return false; + } - return true; - }); - textboxSleepAngularVel->setFontSize(16); - textboxSleepAngularVel->setAlignment(TextBox::Alignment::Right); + return true; + }); + textboxSleepAngularVel->setFontSize(16); + textboxSleepAngularVel->setAlignment(TextBox::Alignment::Right); - // ---------- Rendering Panel ---------- - mRenderingPanel = new Widget(mSettingsPanel); - mRenderingPanel->setLayout(new BoxLayout(Orientation::Vertical, Alignment::Fill, 0, 5)); + // ---------- Rendering Panel ---------- + mRenderingPanel = new Widget(mSettingsPanel); + mRenderingPanel->setLayout(new BoxLayout(Orientation::Vertical, Alignment::Fill, 0, 5)); - // Display/Hide contact points - CheckBox* checkboxContactPoints = new CheckBox(mRenderingPanel, "Contact points"); - checkboxContactPoints->setChecked(mApp->mIsContactPointsDisplayed); - checkboxContactPoints->setCallback([&](bool value) { - mApp->mIsContactPointsDisplayed = value; - }); + // Display/Hide contact points + CheckBox* checkboxContactPoints = new CheckBox(mRenderingPanel, "Contact points"); + checkboxContactPoints->setChecked(mApp->mIsContactPointsDisplayed); + checkboxContactPoints->setCallback([&](bool value) { + mApp->mIsContactPointsDisplayed = value; + }); - // Enabled/Disable VSync - CheckBox* checkboxVSync = new CheckBox(mRenderingPanel, "V-Sync"); - checkboxVSync->setChecked(mApp->mIsVSyncEnabled); - checkboxVSync->setCallback([&](bool value) { - mApp->enableVSync(value); - }); + // Enabled/Disable VSync + CheckBox* checkboxVSync = new CheckBox(mRenderingPanel, "V-Sync"); + checkboxVSync->setChecked(mApp->mIsVSyncEnabled); + checkboxVSync->setCallback([&](bool value) { + mApp->enableVSync(value); + }); - // Enabled/Disable Shadows - CheckBox* checkboxShadows = new CheckBox(mRenderingPanel, "Shadows"); - checkboxShadows->setChecked(mApp->mIsShadowMappingEnabled); - checkboxShadows->setCallback([&](bool value) { - mApp->mIsShadowMappingEnabled = value; - }); + // Enabled/Disable Shadows + CheckBox* checkboxShadows = new CheckBox(mRenderingPanel, "Shadows"); + checkboxShadows->setChecked(mApp->mIsShadowMappingEnabled); + checkboxShadows->setCallback([&](bool value) { + mApp->mIsShadowMappingEnabled = value; + }); - mPhysicsPanel->setVisible(true); - mRenderingPanel->setVisible(false); + mPhysicsPanel->setVisible(true); + mRenderingPanel->setVisible(false); } void Gui::createProfilingPanel() { - Widget* profilingPanel = new Window(mApp, "Profiling"); - profilingPanel->setPosition(Vector2i(15, 525)); - profilingPanel->setLayout(new BoxLayout(Orientation::Vertical, Alignment::Fill, 10, 5)); - profilingPanel->setId("SettingsPanel"); - profilingPanel->setFixedWidth(220); + Widget* profilingPanel = new Window(mApp, "Profiling"); + profilingPanel->setPosition(Vector2i(15, 525)); + profilingPanel->setLayout(new BoxLayout(Orientation::Vertical, Alignment::Fill, 10, 5)); + profilingPanel->setId("SettingsPanel"); + profilingPanel->setFixedWidth(220); - // Framerate (FPS) - mFPSLabel = new Label(profilingPanel, std::string("FPS : ") + floatToString(mCachedFPS, 0),"sans-bold"); + // Framerate (FPS) + mFPSLabel = new Label(profilingPanel, std::string("FPS : ") + floatToString(mCachedFPS, 0),"sans-bold"); - // Update time - mFrameTimeLabel = new Label(profilingPanel, std::string("Frame time : ") + floatToString(mCachedUpdateTime * 1000.0, 1) + std::string(" ms"),"sans-bold"); + // Update time + mFrameTimeLabel = new Label(profilingPanel, std::string("Frame time : ") + floatToString(mCachedUpdateTime * 1000.0, 1) + std::string(" ms"),"sans-bold"); - // Update time - mPhysicsTimeLabel = new Label(profilingPanel, std::string("Physics time : ") + floatToString(mCachedPhysicsUpdateTime * 1000.0, 1) + std::string(" ms"),"sans-bold"); + // Update time + mPhysicsTimeLabel = new Label(profilingPanel, std::string("Physics time : ") + floatToString(mCachedPhysicsUpdateTime * 1000.0, 1) + std::string(" ms"),"sans-bold"); - profilingPanel->setVisible(true); + profilingPanel->setVisible(true); } diff --git a/tools/testbed/src/Gui.h b/tools/testbed/src/Gui.h index 1aacfe6..6940e6c 100644 --- a/tools/testbed/src/Gui.h +++ b/tools/testbed/src/Gui.h @@ -1,26 +1,26 @@ /******************************************************************************** -* ReactPhysics3D physics library, http://www.reactphysics3d.com * -* Copyright (c) 2010-2016 Daniel Chappuis * +* 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: * -* * +* 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. * -* * +* 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 GUI_H @@ -44,98 +44,98 @@ class TestbedApplication; // Class Gui class Gui { - protected : + protected : - enum LeftPane {SCENES, PHYSICS, RENDERING, PROFILING}; + enum LeftPane {SCENES, PHYSICS, RENDERING, PROFILING}; - // -------------------- Constants -------------------- // + // -------------------- Constants -------------------- // - // -------------------- Attributes -------------------- // + // -------------------- Attributes -------------------- // - // Pointer to the application - TestbedApplication* mApp; + // Pointer to the application + TestbedApplication* mApp; - static double mScrollX, mScrollY; + static double mScrollX, mScrollY; - // Simulation panel - Widget* mSimulationPanel; + // Simulation panel + Widget* mSimulationPanel; - // Settings Panel - Widget* mSettingsPanel; - Widget* mPhysicsPanel; - Widget* mRenderingPanel; + // Settings Panel + Widget* mSettingsPanel; + Widget* mPhysicsPanel; + Widget* mRenderingPanel; - // Profiling panel - Label* mFPSLabel; - Label* mFrameTimeLabel; - Label* mPhysicsTimeLabel; + // Profiling panel + Label* mFPSLabel; + Label* mFrameTimeLabel; + Label* mPhysicsTimeLabel; - std::vector mCheckboxesScenes; + std::vector mCheckboxesScenes; - // -------------------- Methods -------------------- // + // -------------------- Methods -------------------- // - static void resetScroll(); + static void resetScroll(); - /// Current time (in seconds) from last profiling time display - static double mTimeSinceLastProfilingDisplay; + /// Current time (in seconds) from last profiling time display + static double mTimeSinceLastProfilingDisplay; - /// Cached Framerate - static double mCachedFPS; + /// Cached Framerate + static double mCachedFPS; - /// Cached update time - static double mCachedUpdateTime; + /// Cached update time + static double mCachedUpdateTime; - // Cached update physics time - static double mCachedPhysicsUpdateTime; + // Cached update physics time + static double mCachedPhysicsUpdateTime; - // -------------------- Methods -------------------- // + // -------------------- Methods -------------------- // - void createSimulationPanel(); + void createSimulationPanel(); - void createSettingsPanel(); + void createSettingsPanel(); - void createProfilingPanel(); + void createProfilingPanel(); - // Convert float value to string - std::string floatToString(float value, int precision); + // Convert float value to string + std::string floatToString(float value, int32_t precision); - public : + public : - // -------------------- Methods -------------------- // + // -------------------- Methods -------------------- // - /// Constructor - Gui(TestbedApplication* app); + /// Constructor + Gui(TestbedApplication* app); - /// Destructor - ~Gui(); + /// Destructor + ~Gui(); - /// Initialize the GUI - void init(); + /// Initialize the GUI + void init(); - /// Update the GUI - void update(); + /// Update the GUI + void update(); - /// Display the GUI - void render(); + /// Display the GUI + void render(); - static void setScroll(double scrollX, double scrollY); + static void setScroll(double scrollX, double scrollY); }; inline void Gui::resetScroll() { - mScrollX = 0.0; - mScrollY = 0.0; + mScrollX = 0.0; + mScrollY = 0.0; } inline void Gui::setScroll(double scrollX, double scrollY) { - mScrollX = scrollX; - mScrollY = scrollY; + mScrollX = scrollX; + mScrollY = scrollY; } -inline std::string Gui::floatToString(float value, int precision) { - std::stringstream ss; - ss << std::fixed << std::setprecision(precision) << value; - return ss.str(); +inline std::string Gui::floatToString(float value, int32_t precision) { + std::stringstream ss; + ss << std::fixed << std::setprecision(precision) << value; + return ss.str(); } #endif diff --git a/tools/testbed/src/Main.cpp b/tools/testbed/src/Main.cpp index f45994b..4640c15 100644 --- a/tools/testbed/src/Main.cpp +++ b/tools/testbed/src/Main.cpp @@ -1,26 +1,26 @@ /******************************************************************************** -* ReactPhysics3D physics library, http://www.reactphysics3d.com * -* Copyright (c) 2010-2016 Daniel Chappuis * +* 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: * -* * +* 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. * -* * +* 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 @@ -30,20 +30,20 @@ using namespace nanogui; // Main function -int main(int argc, char** argv) { +int32_t main(int32_t argc, char** argv) { - nanogui::init(); + nanogui::init(); - { - // Create and start the testbed application - bool isFullscreen = false; - nanogui::ref application = new TestbedApplication(isFullscreen); - application->setVisible(true); + { + // Create and start the testbed application + bool isFullscreen = false; + nanogui::ref application = new TestbedApplication(isFullscreen); + application->setVisible(true); - nanogui::mainloop(); - } + nanogui::mainloop(); + } - nanogui::shutdown(); + nanogui::shutdown(); - return 0; + return 0; } diff --git a/tools/testbed/src/Scene.cpp b/tools/testbed/src/Scene.cpp index 53512b6..f3233d8 100644 --- a/tools/testbed/src/Scene.cpp +++ b/tools/testbed/src/Scene.cpp @@ -1,26 +1,26 @@ /******************************************************************************** -* ReactPhysics3D physics library, http://www.reactphysics3d.com * -* Copyright (c) 2010-2016 Daniel Chappuis * +* 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: * -* * +* 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. * -* * +* 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 @@ -31,9 +31,9 @@ using namespace openglframework; // Constructor Scene::Scene(const std::string& name, bool isShadowMappingEnabled) - : mName(name), mInterpolationFactor(0.0f), mViewportX(0), mViewportY(0), - mViewportWidth(0), mViewportHeight(0), mIsShadowMappingEnabled(isShadowMappingEnabled), - mIsContactPointsDisplayed(true) { + : mName(name), mInterpolationFactor(0.0f), mViewportX(0), mViewportY(0), + mViewportWidth(0), mViewportHeight(0), mIsShadowMappingEnabled(isShadowMappingEnabled), + mIsContactPointsDisplayed(true) { } @@ -45,143 +45,143 @@ Scene::~Scene() { // Set the scene position (where the camera needs to look at) void Scene::setScenePosition(const openglframework::Vector3& position, float sceneRadius) { - // Set the position and radius of the scene - mCenterScene = position; - mCamera.setSceneRadius(sceneRadius); + // Set the position and radius of the scene + mCenterScene = position; + mCamera.setSceneRadius(sceneRadius); - // Reset the camera position and zoom in order to view all the scene - resetCameraToViewAll(); + // Reset the camera position and zoom in order to view all the scene + resetCameraToViewAll(); } // Set the camera so that we can view the whole scene void Scene::resetCameraToViewAll() { - // Move the camera to the origin of the scene - mCamera.translateWorld(-mCamera.getOrigin()); + // Move the camera to the origin of the scene + mCamera.translateWorld(-mCamera.getOrigin()); - // Move the camera to the center of the scene - mCamera.translateWorld(mCenterScene); + // Move the camera to the center of the scene + mCamera.translateWorld(mCenterScene); - // Set the zoom of the camera so that the scene center is - // in negative view direction of the camera - mCamera.setZoom(1.0); + // Set the zoom of the camera so that the scene center is + // in negative view direction of the camera + mCamera.setZoom(1.0); } // Map the mouse x,y coordinates to a point on a sphere bool Scene::mapMouseCoordinatesToSphere(double xMouse, double yMouse, - Vector3& spherePoint) const { + Vector3& spherePoint) const { - if ((xMouse >= 0) && (xMouse <= mWindowWidth) && (yMouse >= 0) && (yMouse <= mWindowHeight)) { - float x = float(xMouse - 0.5f * mWindowWidth) / float(mWindowWidth); - float y = float(0.5f * mWindowHeight - yMouse) / float(mWindowHeight); - float sinx = sin(PI * x * 0.5f); - float siny = sin(PI * y * 0.5f); - float sinx2siny2 = sinx * sinx + siny * siny; + if ((xMouse >= 0) && (xMouse <= mWindowWidth) && (yMouse >= 0) && (yMouse <= mWindowHeight)) { + float x = float(xMouse - 0.5f * mWindowWidth) / float(mWindowWidth); + float y = float(0.5f * mWindowHeight - yMouse) / float(mWindowHeight); + float sinx = sin(PI * x * 0.5f); + float siny = sin(PI * y * 0.5f); + float sinx2siny2 = sinx * sinx + siny * siny; - // Compute the point on the sphere - spherePoint.x = sinx; - spherePoint.y = siny; - spherePoint.z = (sinx2siny2 < 1.0) ? sqrt(1.0f - sinx2siny2) : 0.0f; + // Compute the point on the sphere + spherePoint.x = sinx; + spherePoint.y = siny; + spherePoint.z = (sinx2siny2 < 1.0) ? sqrt(1.0f - sinx2siny2) : 0.0f; - return true; - } + return true; + } - return false; + return false; } // Called when a mouse button event occurs -bool Scene::mouseButtonEvent(int button, bool down, int mods, - double mousePosX, double mousePosY) { +bool Scene::mouseButtonEvent(int32_t button, bool down, int32_t mods, + double mousePosX, double mousePosY) { - // If the mouse button is pressed - if (down) { - mLastMouseX = mousePosX; - mLastMouseY = mousePosY; - mIsLastPointOnSphereValid = mapMouseCoordinatesToSphere(mousePosX, mousePosY, mLastPointOnSphere); - } - else { // If the mouse button is released - mIsLastPointOnSphereValid = false; - } + // If the mouse button is pressed + if (down) { + mLastMouseX = mousePosX; + mLastMouseY = mousePosY; + mIsLastPointOnSphereValid = mapMouseCoordinatesToSphere(mousePosX, mousePosY, mLastPointOnSphere); + } + else { // If the mouse button is released + mIsLastPointOnSphereValid = false; + } - return true; + return true; } // Called when a mouse motion event occurs -bool Scene::mouseMotionEvent(double xMouse, double yMouse, int leftButtonState, - int rightButtonState, int middleButtonState, - int altKeyState) { +bool Scene::mouseMotionEvent(double xMouse, double yMouse, int32_t leftButtonState, + int32_t rightButtonState, int32_t middleButtonState, + int32_t altKeyState) { - // Zoom - if (leftButtonState == GLFW_PRESS && altKeyState == GLFW_PRESS) { + // Zoom + if (leftButtonState == GLFW_PRESS && altKeyState == GLFW_PRESS) { - float dy = static_cast(yMouse - mLastMouseY); - float h = static_cast(mWindowHeight); + float dy = static_cast(yMouse - mLastMouseY); + float h = static_cast(mWindowHeight); - // Zoom the camera - zoom(-dy / h); - } - // Translation - else if (middleButtonState == GLFW_PRESS || rightButtonState == GLFW_PRESS || - (leftButtonState == GLFW_PRESS && altKeyState == GLFW_PRESS)) { - translate(xMouse, yMouse); - } - // Rotation - else if (leftButtonState == GLFW_PRESS) { - rotate(xMouse, yMouse); - } + // Zoom the camera + zoom(-dy / h); + } + // Translation + else if (middleButtonState == GLFW_PRESS || rightButtonState == GLFW_PRESS || + (leftButtonState == GLFW_PRESS && altKeyState == GLFW_PRESS)) { + translate(xMouse, yMouse); + } + // Rotation + else if (leftButtonState == GLFW_PRESS) { + rotate(xMouse, yMouse); + } - // Remember the mouse position - mLastMouseX = xMouse; - mLastMouseY = yMouse; - mIsLastPointOnSphereValid = mapMouseCoordinatesToSphere(xMouse, yMouse, mLastPointOnSphere); + // Remember the mouse position + mLastMouseX = xMouse; + mLastMouseY = yMouse; + mIsLastPointOnSphereValid = mapMouseCoordinatesToSphere(xMouse, yMouse, mLastPointOnSphere); - return true; + return true; } // Called when a scrolling event occurs bool Scene::scrollingEvent(float xAxis, float yAxis, float scrollSensitivy) { - zoom(yAxis * scrollSensitivy); + zoom(yAxis * scrollSensitivy); - return true; + return true; } // Zoom the camera void Scene::zoom(float zoomDiff) { - // Zoom the camera - mCamera.setZoom(zoomDiff); + // Zoom the camera + mCamera.setZoom(zoomDiff); } // Translate the camera -void Scene::translate(int xMouse, int yMouse) { +void Scene::translate(int32_t xMouse, int32_t yMouse) { float dx = static_cast(xMouse - mLastMouseX); float dy = static_cast(yMouse - mLastMouseY); // Translate the camera mCamera.translateCamera(-dx / float(mCamera.getWidth()), - -dy / float(mCamera.getHeight()), mCenterScene); + -dy / float(mCamera.getHeight()), mCenterScene); } // Rotate the camera -void Scene::rotate(int xMouse, int yMouse) { +void Scene::rotate(int32_t xMouse, int32_t yMouse) { - if (mIsLastPointOnSphereValid) { + if (mIsLastPointOnSphereValid) { - Vector3 newPoint3D; - bool isNewPointOK = mapMouseCoordinatesToSphere(xMouse, yMouse, newPoint3D); + Vector3 newPoint3D; + bool isNewPointOK = mapMouseCoordinatesToSphere(xMouse, yMouse, newPoint3D); - if (isNewPointOK) { - Vector3 axis = mLastPointOnSphere.cross(newPoint3D); - float cosAngle = mLastPointOnSphere.dot(newPoint3D); + if (isNewPointOK) { + Vector3 axis = mLastPointOnSphere.cross(newPoint3D); + float cosAngle = mLastPointOnSphere.dot(newPoint3D); - float epsilon = std::numeric_limits::epsilon(); - if (fabs(cosAngle) < 1.0f && axis.length() > epsilon) { - axis.normalize(); - float angle = 2.0f * acos(cosAngle); + float epsilon = std::numeric_limits::epsilon(); + if (fabs(cosAngle) < 1.0f && axis.length() > epsilon) { + axis.normalize(); + float angle = 2.0f * acos(cosAngle); - // Rotate the camera around the center of the scene - mCamera.rotateAroundLocalPoint(axis, -angle, mCenterScene); - } - } - } + // Rotate the camera around the center of the scene + mCamera.rotateAroundLocalPoint(axis, -angle, mCenterScene); + } + } + } } diff --git a/tools/testbed/src/Scene.h b/tools/testbed/src/Scene.h index f9504df..25d7ce1 100644 --- a/tools/testbed/src/Scene.h +++ b/tools/testbed/src/Scene.h @@ -1,26 +1,26 @@ /******************************************************************************** -* ReactPhysics3D physics library, http://www.reactphysics3d.com * -* Copyright (c) 2010-2016 Daniel Chappuis * +* 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: * -* * +* 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. * -* * +* 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 SCENE_H @@ -32,209 +32,209 @@ // Structure ContactPoint struct ContactPoint { - public: - openglframework::Vector3 point; + public: + openglframework::Vector3 point; - /// Constructor - ContactPoint(const openglframework::Vector3& pointWorld) : point(pointWorld) { + /// Constructor + ContactPoint(const openglframework::Vector3& pointWorld) : point(pointWorld) { - } + } }; /// Structure EngineSettings /// This structure contains several physics engine parameters struct EngineSettings { - public: + public: - long double elapsedTime; // Elapsed time (in seconds) - float timeStep; // Current time step (in seconds) - int nbVelocitySolverIterations; // Nb of velocity solver iterations - int nbPositionSolverIterations; // Nb of position solver iterations - bool isSleepingEnabled; // True if sleeping technique is enabled - float timeBeforeSleep; // Time of inactivity before a body sleep - float sleepLinearVelocity; // Sleep linear velocity - float sleepAngularVelocity; // Sleep angular velocity - bool isGravityEnabled; // True if gravity is enabled - openglframework::Vector3 gravity; // Gravity vector + long double elapsedTime; // Elapsed time (in seconds) + float timeStep; // Current time step (in seconds) + int32_t nbVelocitySolverIterations; // Nb of velocity solver iterations + int32_t nbPositionSolverIterations; // Nb of position solver iterations + bool isSleepingEnabled; // True if sleeping technique is enabled + float timeBeforeSleep; // Time of inactivity before a body sleep + float sleepLinearVelocity; // Sleep linear velocity + float sleepAngularVelocity; // Sleep angular velocity + bool isGravityEnabled; // True if gravity is enabled + openglframework::Vector3 gravity; // Gravity vector - /// Constructor - EngineSettings() : elapsedTime(0.0f), timeStep(0.0f) { + /// Constructor + EngineSettings() : elapsedTime(0.0f), timeStep(0.0f) { - } + } }; // Class Scene // Abstract class that represents a 3D scene. class Scene { - protected: + protected: - // -------------------- Attributes -------------------- // + // -------------------- Attributes -------------------- // - /// Scene name - std::string mName; + /// Scene name + std::string mName; - /// Physics engine settings - EngineSettings mEngineSettings; + /// Physics engine settings + EngineSettings mEngineSettings; - /// Camera - openglframework::Camera mCamera; + /// Camera + openglframework::Camera mCamera; - /// Center of the scene - openglframework::Vector3 mCenterScene; + /// Center of the scene + openglframework::Vector3 mCenterScene; - /// Last mouse coordinates on the windows - double mLastMouseX, mLastMouseY; + /// Last mouse coordinates on the windows + double mLastMouseX, mLastMouseY; - /// Window dimension - int mWindowWidth, mWindowHeight; + /// Window dimension + int32_t mWindowWidth, mWindowHeight; - /// Last point computed on a sphere (for camera rotation) - openglframework::Vector3 mLastPointOnSphere; + /// Last point computed on a sphere (for camera rotation) + openglframework::Vector3 mLastPointOnSphere; - /// True if the last point computed on a sphere (for camera rotation) is valid - bool mIsLastPointOnSphereValid; + /// True if the last point computed on a sphere (for camera rotation) is valid + bool mIsLastPointOnSphereValid; - /// Interpolation factor for the bodies in the current frame - float mInterpolationFactor; + /// Interpolation factor for the bodies in the current frame + float mInterpolationFactor; - /// Viewport x,y, width and height values - int mViewportX, mViewportY, mViewportWidth, mViewportHeight; + /// Viewport x,y, width and height values + int32_t mViewportX, mViewportY, mViewportWidth, mViewportHeight; - /// True if the shadow mapping is enabled - bool mIsShadowMappingEnabled; + /// True if the shadow mapping is enabled + bool mIsShadowMappingEnabled; - /// True if contact points are displayed - bool mIsContactPointsDisplayed; + /// True if contact points are displayed + bool mIsContactPointsDisplayed; - // -------------------- Methods -------------------- // + // -------------------- Methods -------------------- // - /// Set the scene position (where the camera needs to look at) - void setScenePosition(const openglframework::Vector3& position, float sceneRadius); + /// Set the scene position (where the camera needs to look at) + void setScenePosition(const openglframework::Vector3& position, float sceneRadius); - /// Set the camera so that we can view the whole scene - void resetCameraToViewAll(); + /// Set the camera so that we can view the whole scene + void resetCameraToViewAll(); - /// Map mouse coordinates to coordinates on the sphere - bool mapMouseCoordinatesToSphere(double xMouse, double yMouse, - openglframework::Vector3& spherePoint) const; + /// Map mouse coordinates to coordinates on the sphere + bool mapMouseCoordinatesToSphere(double xMouse, double yMouse, + openglframework::Vector3& spherePoint) const; - /// Zoom the camera - void zoom(float zoomDiff); + /// Zoom the camera + void zoom(float zoomDiff); - /// Translate the camera - void translate(int xMouse, int yMouse); + /// Translate the camera + void translate(int32_t xMouse, int32_t yMouse); - /// Rotate the camera - void rotate(int xMouse, int yMouse); + /// Rotate the camera + void rotate(int32_t xMouse, int32_t yMouse); - public: + public: - // -------------------- Methods -------------------- // + // -------------------- Methods -------------------- // - /// Constructor - Scene(const std::string& name, bool isShadowMappingEnabled = false); + /// Constructor + Scene(const std::string& name, bool isShadowMappingEnabled = false); - /// Destructor - virtual ~Scene(); + /// Destructor + virtual ~Scene(); - /// Reshape the view - virtual void reshape(int width, int height); + /// Reshape the view + virtual void reshape(int32_t width, int32_t height); - /// Update the physics world (take a simulation step) - /// Can be called several times per frame - virtual void updatePhysics()=0; + /// Update the physics world (take a simulation step) + /// Can be called several times per frame + virtual void updatePhysics()=0; - /// Update the scene - virtual void update()=0; + /// Update the scene + virtual void update()=0; - /// Render the scene - virtual void render()=0; + /// Render the scene + virtual void render()=0; - /// Reset the scene - virtual void reset()=0; + /// Reset the scene + virtual void reset()=0; - /// Called when a keyboard event occurs - virtual bool keyboardEvent(int key, int scancode, int action, int mods); + /// Called when a keyboard event occurs + virtual bool keyboardEvent(int32_t key, int32_t scancode, int32_t action, int32_t mods); - /// Called when a mouse button event occurs - virtual bool mouseButtonEvent(int button, bool down, int mods, - double mousePosX, double mousePosY); + /// Called when a mouse button event occurs + virtual bool mouseButtonEvent(int32_t button, bool down, int32_t mods, + double mousePosX, double mousePosY); - /// Called when a mouse motion event occurs - virtual bool mouseMotionEvent(double xMouse, double yMouse, int leftButtonState, - int rightButtonState, int middleButtonState, int altKeyState); + /// Called when a mouse motion event occurs + virtual bool mouseMotionEvent(double xMouse, double yMouse, int32_t leftButtonState, + int32_t rightButtonState, int32_t middleButtonState, int32_t altKeyState); - /// Called when a scrolling event occurs - virtual bool scrollingEvent(float xAxis, float yAxis, float scrollSensitivy); + /// Called when a scrolling event occurs + virtual bool scrollingEvent(float xAxis, float yAxis, float scrollSensitivy); - /// Set the window dimension - void setWindowDimension(int width, int height); + /// Set the window dimension + void setWindowDimension(int32_t width, int32_t height); - /// Set the viewport to render the scene - void setViewport(int x, int y, int width, int height); + /// Set the viewport to render the scene + void setViewport(int32_t x, int32_t y, int32_t width, int32_t height); - /// Return a reference to the camera - const openglframework::Camera& getCamera() const; + /// Return a reference to the camera + const openglframework::Camera& getCamera() const; - /// Get the engine settings - EngineSettings getEngineSettings() const; + /// Get the engine settings + EngineSettings getEngineSettings() const; - /// Set the engine settings - void setEngineSettings(const EngineSettings& settings); + /// Set the engine settings + void setEngineSettings(const EngineSettings& settings); - /// Set the interpolation factor - void setInterpolationFactor(float interpolationFactor); + /// Set the int32_terpolation factor + void setInterpolationFactor(float int32_terpolationFactor); - /// Return the name of the scene - std::string getName() const; + /// Return the name of the scene + std::string getName() const; - /// Return true if the shadow mapping is enabled - bool getIsShadowMappingEnabled() const; + /// Return true if the shadow mapping is enabled + bool getIsShadowMappingEnabled() const; - /// Enabled/Disable the shadow mapping - void virtual setIsShadowMappingEnabled(bool isShadowMappingEnabled); + /// Enabled/Disable the shadow mapping + void virtual setIsShadowMappingEnabled(bool isShadowMappingEnabled); - /// Display/Hide the contact points - void virtual setIsContactPointsDisplayed(bool display); + /// Display/Hide the contact points + void virtual setIsContactPointsDisplayed(bool display); - /// Return all the contact points of the scene - std::vector virtual getContactPoints() const; + /// Return all the contact points of the scene + std::vector virtual getContactPoints() const; }; // Called when a keyboard event occurs -inline bool Scene::keyboardEvent(int key, int scancode, int action, int mods) { - return false; +inline bool Scene::keyboardEvent(int32_t key, int32_t scancode, int32_t action, int32_t mods) { + return false; } /// Reshape the view -inline void Scene::reshape(int width, int height) { - mCamera.setDimensions(width, height); +inline void Scene::reshape(int32_t width, int32_t height) { + mCamera.setDimensions(width, height); } // Return a reference to the camera inline const openglframework::Camera& Scene::getCamera() const { - return mCamera; + return mCamera; } // Set the window dimension -inline void Scene::setWindowDimension(int width, int height) { - mWindowWidth = width; - mWindowHeight = height; +inline void Scene::setWindowDimension(int32_t width, int32_t height) { + mWindowWidth = width; + mWindowHeight = height; } // Set the viewport to render the scene -inline void Scene::setViewport(int x, int y, int width, int height) { - mViewportX = x; - mViewportY = y; - mViewportWidth = width; - mViewportHeight = height; +inline void Scene::setViewport(int32_t x, int32_t y, int32_t width, int32_t height) { + mViewportX = x; + mViewportY = y; + mViewportWidth = width; + mViewportHeight = height; } // Get the engine settings inline EngineSettings Scene::getEngineSettings() const { - return mEngineSettings; + return mEngineSettings; } // Set the engine settings @@ -242,36 +242,36 @@ inline void Scene::setEngineSettings(const EngineSettings& settings) { mEngineSettings = settings; } -// Set the interpolation factor -inline void Scene::setInterpolationFactor(float interpolationFactor) { - mInterpolationFactor = interpolationFactor; +// Set the int32_terpolation factor +inline void Scene::setInterpolationFactor(float int32_terpolationFactor) { + mInterpolationFactor = int32_terpolationFactor; } // Return the name of the scene inline std::string Scene::getName() const { - return mName; + return mName; } // Return true if the shadow mapping is enabled inline bool Scene::getIsShadowMappingEnabled() const { - return mIsShadowMappingEnabled; + return mIsShadowMappingEnabled; } // Enabled/Disable the shadow mapping inline void Scene::setIsShadowMappingEnabled(bool isShadowMappingEnabled) { - mIsShadowMappingEnabled = isShadowMappingEnabled; + mIsShadowMappingEnabled = isShadowMappingEnabled; } // Display/Hide the contact points inline void Scene::setIsContactPointsDisplayed(bool display) { - mIsContactPointsDisplayed = display; + mIsContactPointsDisplayed = display; } // Return all the contact points of the scene inline std::vector Scene::getContactPoints() const { - // Return an empty list of contact points - return std::vector(); + // Return an empty list of contact points + return std::vector(); } #endif diff --git a/tools/testbed/src/SceneDemo.cpp b/tools/testbed/src/SceneDemo.cpp index fe26e4e..f986f56 100644 --- a/tools/testbed/src/SceneDemo.cpp +++ b/tools/testbed/src/SceneDemo.cpp @@ -1,26 +1,26 @@ /******************************************************************************** -* ReactPhysics3D physics library, http://www.reactphysics3d.com * -* Copyright (c) 2010-2016 Daniel Chappuis * +* 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: * -* * +* 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. * -* * +* 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 @@ -29,163 +29,163 @@ using namespace openglframework; -int SceneDemo::shadowMapTextureLevel = 0; +int32_t SceneDemo::shadowMapTextureLevel = 0; openglframework::Color SceneDemo::mGreyColorDemo = Color(0.70f, 0.70f, 0.7f, 1.0); openglframework::Color SceneDemo::mYellowColorDemo = Color(0.9, 0.88, 0.145, 1.0); openglframework::Color SceneDemo::mBlueColorDemo = Color(0, 0.66, 0.95, 1.0); openglframework::Color SceneDemo::mOrangeColorDemo = Color(0.9, 0.35, 0, 1.0); openglframework::Color SceneDemo::mPinkColorDemo = Color(0.83, 0.48, 0.64, 1.0); openglframework::Color SceneDemo::mRedColorDemo = Color(0.95, 0, 0, 1.0); -int SceneDemo::mNbDemoColors = 4; +int32_t SceneDemo::mNbDemoColors = 4; openglframework::Color SceneDemo::mDemoColors[] = {SceneDemo::mYellowColorDemo, SceneDemo::mBlueColorDemo, - SceneDemo::mOrangeColorDemo, SceneDemo::mPinkColorDemo}; + SceneDemo::mOrangeColorDemo, SceneDemo::mPinkColorDemo}; // Constructor SceneDemo::SceneDemo(const std::string& name, float sceneRadius, bool isShadowMappingEnabled) - : Scene(name, isShadowMappingEnabled), mIsShadowMappingInitialized(false), - mDepthShader("shaders/depth.vert", "shaders/depth.frag"), - mPhongShader("shaders/phong.vert", "shaders/phong.frag"), - mQuadShader("shaders/quad.vert", "shaders/quad.frag"), - mVBOQuad(GL_ARRAY_BUFFER), mMeshFolderPath("meshes/") { + : Scene(name, isShadowMappingEnabled), mIsShadowMappingInitialized(false), + mDepthShader("shaders/depth.vert", "shaders/depth.frag"), + mPhongShader("shaders/phong.vert", "shaders/phong.frag"), + mQuadShader("shaders/quad.vert", "shaders/quad.frag"), + mVBOQuad(GL_ARRAY_BUFFER), mMeshFolderPath("meshes/") { - shadowMapTextureLevel++; + shadowMapTextureLevel++; - // Move the light0 - mLight0.translateWorld(Vector3(-2, 35, 40)); + // Move the light0 + mLight0.translateWorld(Vector3(-2, 35, 40)); - // Camera at light0 postion for the shadow map - mShadowMapLightCamera.translateWorld(mLight0.getOrigin()); - mShadowMapLightCamera.rotateLocal(Vector3(1, 0, 0), -PI / 4.0f); - mShadowMapLightCamera.rotateWorld(Vector3(0, 1, 0), PI / 8.0f); + // Camera at light0 postion for the shadow map + mShadowMapLightCamera.translateWorld(mLight0.getOrigin()); + mShadowMapLightCamera.rotateLocal(Vector3(1, 0, 0), -PI / 4.0f); + mShadowMapLightCamera.rotateWorld(Vector3(0, 1, 0), PI / 8.0f); - mShadowMapLightCamera.setDimensions(SHADOWMAP_WIDTH, SHADOWMAP_HEIGHT); - mShadowMapLightCamera.setFieldOfView(80.0f); - mShadowMapLightCamera.setSceneRadius(100); + mShadowMapLightCamera.setDimensions(SHADOWMAP_WIDTH, SHADOWMAP_HEIGHT); + mShadowMapLightCamera.setFieldOfView(80.0f); + mShadowMapLightCamera.setSceneRadius(100); - mShadowMapBiasMatrix.setAllValues(0.5, 0.0, 0.0, 0.5, - 0.0, 0.5, 0.0, 0.5, - 0.0, 0.0, 0.5, 0.5, - 0.0, 0.0, 0.0, 1.0); + mShadowMapBiasMatrix.setAllValues(0.5, 0.0, 0.0, 0.5, + 0.0, 0.5, 0.0, 0.5, + 0.0, 0.0, 0.5, 0.5, + 0.0, 0.0, 0.0, 1.0); - // Create the Shadow map FBO and texture - if (mIsShadowMappingEnabled) { - createShadowMapFBOAndTexture(); - } + // Create the Shadow map FBO and texture + if (mIsShadowMappingEnabled) { + createShadowMapFBOAndTexture(); + } - createQuadVBO(); + createQuadVBO(); - VisualContactPoint::createStaticData(mMeshFolderPath); + VisualContactPoint::createStaticData(mMeshFolderPath); } // Destructor SceneDemo::~SceneDemo() { - mShadowMapTexture.destroy(); - mFBOShadowMap.destroy(); - mVBOQuad.destroy(); + mShadowMapTexture.destroy(); + mFBOShadowMap.destroy(); + mVBOQuad.destroy(); - // Destroy the contact points - removeAllContactPoints(); + // Destroy the contact points + removeAllContactPoints(); - VisualContactPoint::destroyStaticData(); + VisualContactPoint::destroyStaticData(); } // Update the scene void SceneDemo::update() { - // Update the contact points - updateContactPoints(); + // Update the contact points + updateContactPoints(); } // Render the scene (in multiple passes for shadow mapping) void SceneDemo::render() { - const Color& diffCol = mLight0.getDiffuseColor(); + const Color& diffCol = mLight0.getDiffuseColor(); - glEnable(GL_DEPTH_TEST); - glEnable(GL_CULL_FACE); + glEnable(GL_DEPTH_TEST); + glEnable(GL_CULL_FACE); - // ---------- Render the scene to generate the shadow map (first pass) ----------- // + // ---------- Render the scene to generate the shadow map (first pass) ----------- // - const Matrix4 shadowMapProjMatrix = mShadowMapLightCamera.getProjectionMatrix(); - const openglframework::Matrix4 worldToLightCameraMatrix = mShadowMapLightCamera.getTransformMatrix().getInverse(); + const Matrix4 shadowMapProjMatrix = mShadowMapLightCamera.getProjectionMatrix(); + const openglframework::Matrix4 worldToLightCameraMatrix = mShadowMapLightCamera.getTransformMatrix().getInverse(); - // If Shadow Mapping is enabled - if (mIsShadowMappingEnabled) { + // If Shadow Mapping is enabled + if (mIsShadowMappingEnabled) { - // Culling switching, rendering only backface, this is done to avoid self-shadowing - glCullFace(GL_BACK); + // Culling switching, rendering only backface, this is done to avoid self-shadowing + glCullFace(GL_BACK); - mFBOShadowMap.bind(); + mFBOShadowMap.bind(); - // Bind the shader - mDepthShader.bind(); + // Bind the shader + mDepthShader.bind(); - // Set the variables of the shader - mDepthShader.setMatrix4x4Uniform("projectionMatrix", shadowMapProjMatrix); + // Set the variables of the shader + mDepthShader.setMatrix4x4Uniform("projectionMatrix", shadowMapProjMatrix); - // Set the viewport to render into the shadow map texture - glViewport(0, 0, SHADOWMAP_WIDTH, SHADOWMAP_HEIGHT); + // Set the viewport to render int32_to the shadow map texture + glViewport(0, 0, SHADOWMAP_WIDTH, SHADOWMAP_HEIGHT); - // Clear previous frame values - glClear(GL_DEPTH_BUFFER_BIT); + // Clear previous frame values + glClear(GL_DEPTH_BUFFER_BIT); - // Disable color rendering, we only want to write to the Z-Buffer - glColorMask(GL_FALSE, GL_FALSE, GL_FALSE, GL_FALSE); + // Disable color rendering, we only want to write to the Z-Buffer + glColorMask(GL_FALSE, GL_FALSE, GL_FALSE, GL_FALSE); - // Render the objects of the scene - renderSinglePass(mDepthShader, worldToLightCameraMatrix); + // Render the objects of the scene + renderSinglePass(mDepthShader, worldToLightCameraMatrix); - // Unbind the shader - mDepthShader.unbind(); + // Unbind the shader + mDepthShader.unbind(); - mFBOShadowMap.unbind(); + mFBOShadowMap.unbind(); - glDisable(GL_POLYGON_OFFSET_FILL); - } + glDisable(GL_POLYGON_OFFSET_FILL); + } - // ---------- Render the scene for final rendering (second pass) ----------- // + // ---------- Render the scene for final rendering (second pass) ----------- // - glCullFace(GL_BACK); + glCullFace(GL_BACK); - // Get the world-space to camera-space matrix - const openglframework::Matrix4 worldToCameraMatrix = mCamera.getTransformMatrix().getInverse(); + // Get the world-space to camera-space matrix + const openglframework::Matrix4 worldToCameraMatrix = mCamera.getTransformMatrix().getInverse(); - mPhongShader.bind(); + mPhongShader.bind(); - if (mIsShadowMappingEnabled) mShadowMapTexture.bind(); - const GLuint textureUnit = 0; + if (mIsShadowMappingEnabled) mShadowMapTexture.bind(); + const GLuint32_t textureUnit = 0; - // Set the variables of the shader - mPhongShader.setMatrix4x4Uniform("projectionMatrix", mCamera.getProjectionMatrix()); - mPhongShader.setMatrix4x4Uniform("shadowMapProjectionMatrix", mShadowMapBiasMatrix * shadowMapProjMatrix); - mPhongShader.setMatrix4x4Uniform("worldToLight0CameraMatrix", worldToLightCameraMatrix); - mPhongShader.setVector3Uniform("light0PosCameraSpace", worldToCameraMatrix * mLight0.getOrigin()); - mPhongShader.setVector3Uniform("lightAmbientColor", Vector3(0.4f, 0.4f, 0.4f)); - mPhongShader.setVector3Uniform("light0DiffuseColor", Vector3(diffCol.r, diffCol.g, diffCol.b)); - mPhongShader.setIntUniform("shadowMapSampler", textureUnit); - mPhongShader.setIntUniform("isShadowEnabled", mIsShadowMappingEnabled); - mPhongShader.setVector2Uniform("shadowMapDimension", Vector2(SHADOWMAP_WIDTH, SHADOWMAP_HEIGHT)); + // Set the variables of the shader + mPhongShader.setMatrix4x4Uniform("projectionMatrix", mCamera.getProjectionMatrix()); + mPhongShader.setMatrix4x4Uniform("shadowMapProjectionMatrix", mShadowMapBiasMatrix * shadowMapProjMatrix); + mPhongShader.setMatrix4x4Uniform("worldToLight0CameraMatrix", worldToLightCameraMatrix); + mPhongShader.setVector3Uniform("light0PosCameraSpace", worldToCameraMatrix * mLight0.getOrigin()); + mPhongShader.setVector3Uniform("lightAmbientColor", Vector3(0.4f, 0.4f, 0.4f)); + mPhongShader.setVector3Uniform("light0DiffuseColor", Vector3(diffCol.r, diffCol.g, diffCol.b)); + mPhongShader.setIntUniform("shadowMapSampler", textureUnit); + mPhongShader.setIntUniform("isShadowEnabled", mIsShadowMappingEnabled); + mPhongShader.setVector2Uniform("shadowMapDimension", Vector2(SHADOWMAP_WIDTH, SHADOWMAP_HEIGHT)); - // Set the viewport to render the scene - glViewport(mViewportX, mViewportY, mViewportWidth, mViewportHeight); + // Set the viewport to render the scene + glViewport(mViewportX, mViewportY, mViewportWidth, mViewportHeight); - //Enabling color write (previously disabled for light POV z-buffer rendering) - glColorMask(GL_TRUE, GL_TRUE, GL_TRUE, GL_TRUE); + //Enabling color write (previously disabled for light POV z-buffer rendering) + glColorMask(GL_TRUE, GL_TRUE, GL_TRUE, GL_TRUE); - // Clear previous frame values - glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); + // Clear previous frame values + glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); - // Render the objects of the scene - renderSinglePass(mPhongShader, worldToCameraMatrix); + // Render the objects of the scene + renderSinglePass(mPhongShader, worldToCameraMatrix); - // Render the contact points - if (mIsContactPointsDisplayed) { - renderContactPoints(mPhongShader, worldToCameraMatrix); - } + // Render the contact points + if (mIsContactPointsDisplayed) { + renderContactPoints(mPhongShader, worldToCameraMatrix); + } - if (mIsShadowMappingEnabled) mShadowMapTexture.unbind(); - mPhongShader.unbind(); + if (mIsShadowMappingEnabled) mShadowMapTexture.unbind(); + mPhongShader.unbind(); //drawTextureQuad(); } @@ -193,153 +193,153 @@ void SceneDemo::render() { // Create the Shadow map FBO and texture void SceneDemo::createShadowMapFBOAndTexture() { - // Create the texture for the depth values - mShadowMapTexture.create(SHADOWMAP_WIDTH, SHADOWMAP_HEIGHT, GL_DEPTH_COMPONENT24, GL_DEPTH_COMPONENT, - GL_UNSIGNED_BYTE, GL_NEAREST, GL_NEAREST, GL_CLAMP_TO_EDGE, GL_CLAMP_TO_EDGE, NULL); + // Create the texture for the depth values + mShadowMapTexture.create(SHADOWMAP_WIDTH, SHADOWMAP_HEIGHT, GL_DEPTH_COMPONENT24, GL_DEPTH_COMPONENT, + GL_UNSIGNED_BYTE, GL_NEAREST, GL_NEAREST, GL_CLAMP_TO_EDGE, GL_CLAMP_TO_EDGE, NULL); - // Create the FBO for the shadow map - mFBOShadowMap.create(0, 0, false); - mFBOShadowMap.bind(); + // Create the FBO for the shadow map + mFBOShadowMap.create(0, 0, false); + mFBOShadowMap.bind(); - // Tell OpenGL that we won't bind a color texture with the currently binded FBO - glDrawBuffer(GL_NONE); - glReadBuffer(GL_NONE); + // Tell OpenGL that we won't bind a color texture with the currently binded FBO + glDrawBuffer(GL_NONE); + glReadBuffer(GL_NONE); - mFBOShadowMap.attachTexture(GL_DEPTH_ATTACHMENT, mShadowMapTexture.getID()); - mFBOShadowMap.unbind(); + mFBOShadowMap.attachTexture(GL_DEPTH_ATTACHMENT, mShadowMapTexture.getID()); + mFBOShadowMap.unbind(); - mIsShadowMappingInitialized = true; + mIsShadowMappingInitialized = true; } // Used for debugging shadow maps void SceneDemo::createQuadVBO() { - mVAOQuad.create(); - mVAOQuad.bind(); + mVAOQuad.create(); + mVAOQuad.bind(); - static const GLfloat quadVertexData[] = { - -1.0f, -1.0f, 0.0f, - 1.0f, -1.0f, 0.0f, - -1.0f, 1.0f, 0.0f, - -1.0f, 1.0f, 0.0f, - 1.0f, -1.0f, 0.0f, - 1.0f, 1.0f, 0.0f, - }; + static const GLfloat quadVertexData[] = { + -1.0f, -1.0f, 0.0f, + 1.0f, -1.0f, 0.0f, + -1.0f, 1.0f, 0.0f, + -1.0f, 1.0f, 0.0f, + 1.0f, -1.0f, 0.0f, + 1.0f, 1.0f, 0.0f, + }; - mVBOQuad.create(); - mVBOQuad.bind(); - mVBOQuad.copyDataIntoVBO(sizeof(quadVertexData), quadVertexData, GL_STATIC_DRAW); - mVBOQuad.unbind(); + mVBOQuad.create(); + mVBOQuad.bind(); + mVBOQuad.copyDataIntoVBO(sizeof(quadVertexData), quadVertexData, GL_STATIC_DRAW); + mVBOQuad.unbind(); - mVAOQuad.unbind(); + mVAOQuad.unbind(); } void SceneDemo::drawTextureQuad() { - glViewport(mViewportX, mViewportY, mViewportWidth, mViewportHeight); - glColorMask(GL_TRUE, GL_TRUE, GL_TRUE, GL_TRUE); + glViewport(mViewportX, mViewportY, mViewportWidth, mViewportHeight); + glColorMask(GL_TRUE, GL_TRUE, GL_TRUE, GL_TRUE); - // Clear previous frame values - glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); + // Clear previous frame values + glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); - const GLuint textureUnit = 0; + const GLuint32_t textureUnit = 0; - mVAOQuad.bind(); - mQuadShader.bind(); - mShadowMapTexture.bind(); - mQuadShader.setIntUniform("textureSampler", textureUnit); - mVBOQuad.bind(); + mVAOQuad.bind(); + mQuadShader.bind(); + mShadowMapTexture.bind(); + mQuadShader.setIntUniform("textureSampler", textureUnit); + mVBOQuad.bind(); - GLint vertexPositionLoc = mQuadShader.getAttribLocation("vertexPosition"); - glEnableVertexAttribArray(vertexPositionLoc); + GLint32_t vertexPositionLoc = mQuadShader.getAttribLocation("vertexPosition"); + glEnableVertexAttribArray(vertexPositionLoc); - glVertexAttribPointer( - vertexPositionLoc, // attribute 0. No particular reason for 0, but must match the layout in the shader. - 3, // size - GL_FLOAT, // type - GL_FALSE, // normalized? - 0, // stride - (void*)0 // array buffer offset - ); + glVertexAttribPointer( + vertexPositionLoc, // attribute 0. No particular reason for 0, but must match the layout in the shader. + 3, // size + GL_FLOAT, // type + GL_FALSE, // normalized? + 0, // stride + (void*)0 // array buffer offset + ); - // Draw the triangles ! - glDrawArrays(GL_TRIANGLES, 0, 6); // 2*3 indices starting at 0 -> 2 triangles + // Draw the triangles ! + glDrawArrays(GL_TRIANGLES, 0, 6); // 2*3 indices starting at 0 -> 2 triangles - glDisableVertexAttribArray(vertexPositionLoc); + glDisableVertexAttribArray(vertexPositionLoc); - mVBOQuad.unbind(); - mShadowMapTexture.unbind(); - mQuadShader.unbind(); - mVAOQuad.unbind(); + mVBOQuad.unbind(); + mShadowMapTexture.unbind(); + mQuadShader.unbind(); + mVAOQuad.unbind(); } // Gather and create contact points void SceneDemo::updateContactPoints() { - // Remove the previous contact points - removeAllContactPoints(); + // Remove the previous contact points + removeAllContactPoints(); - if (mIsContactPointsDisplayed) { + if (mIsContactPointsDisplayed) { - // Get the current contact points of the scene - std::vector contactPoints = getContactPoints(); + // Get the current contact points of the scene + std::vector contactPoints = getContactPoints(); - // For each contact point - std::vector::const_iterator it; - for (it = contactPoints.begin(); it != contactPoints.end(); ++it) { + // For each contact point + std::vector::const_iterator it; + for (it = contactPoints.begin(); it != contactPoints.end(); ++it) { - // Create a visual contact point for rendering - VisualContactPoint* point = new VisualContactPoint(it->point, mMeshFolderPath); - mContactPoints.push_back(point); - } - } + // Create a visual contact point for rendering + VisualContactPoint* point = new VisualContactPoint(it->point, mMeshFolderPath); + mContactPoints.push_back(point); + } + } } // Render the contact points void SceneDemo::renderContactPoints(openglframework::Shader& shader, - const openglframework::Matrix4& worldToCameraMatrix) { + const openglframework::Matrix4& worldToCameraMatrix) { - // Render all the raycast hit points - for (std::vector::iterator it = mContactPoints.begin(); - it != mContactPoints.end(); ++it) { - (*it)->render(shader, worldToCameraMatrix); - } + // Render all the raycast hit points + for (std::vector::iterator it = mContactPoints.begin(); + it != mContactPoints.end(); ++it) { + (*it)->render(shader, worldToCameraMatrix); + } } void SceneDemo::removeAllContactPoints() { - // Destroy all the visual contact points - for (std::vector::iterator it = mContactPoints.begin(); - it != mContactPoints.end(); ++it) { - delete (*it); - } - mContactPoints.clear(); + // Destroy all the visual contact points + for (std::vector::iterator it = mContactPoints.begin(); + it != mContactPoints.end(); ++it) { + delete (*it); + } + mContactPoints.clear(); } // Return all the contact points of the scene std::vector SceneDemo::computeContactPointsOfWorld(const rp3d::DynamicsWorld* world) const { - std::vector contactPoints; + std::vector contactPoints; - // Get the list of contact manifolds from the world - std::vector manifolds = world->getContactsList(); + // Get the list of contact manifolds from the world + std::vector manifolds = world->getContactsList(); - // For each contact manifold - std::vector::const_iterator it; - for (it = manifolds.begin(); it != manifolds.end(); ++it) { + // For each contact manifold + std::vector::const_iterator it; + for (it = manifolds.begin(); it != manifolds.end(); ++it) { - const rp3d::ContactManifold* manifold = *it; + const rp3d::ContactManifold* manifold = *it; - // For each contact point of the manifold - for (uint i=0; igetNbContactPoints(); i++) { + // For each contact point of the manifold + for (uint32_t i=0; igetNbContactPoints(); i++) { - rp3d::ContactPoint* contactPoint = manifold->getContactPoint(i); - rp3d::Vector3 point = contactPoint->getWorldPointOnBody1(); - ContactPoint contact(openglframework::Vector3(point.x, point.y, point.z)); - contactPoints.push_back(contact); - } + rp3d::ContactPoint* contactPoint = manifold->getContactPoint(i); + rp3d::Vector3 point = contactPoint->getWorldPointOnBody1(); + ContactPoint contact(openglframework::Vector3(point.x, point.y, point.z)); + contactPoints.push_back(contact); + } - } + } - return contactPoints; + return contactPoints; } diff --git a/tools/testbed/src/SceneDemo.h b/tools/testbed/src/SceneDemo.h index 042a227..62979e2 100644 --- a/tools/testbed/src/SceneDemo.h +++ b/tools/testbed/src/SceneDemo.h @@ -1,26 +1,26 @@ /******************************************************************************** -* ReactPhysics3D physics library, http://www.reactphysics3d.com * -* Copyright (c) 2010-2016 Daniel Chappuis * +* 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: * -* * +* 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. * -* * +* 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 SCENEDEMO_H @@ -32,121 +32,121 @@ #include // Constants -const int SHADOWMAP_WIDTH = 2048; -const int SHADOWMAP_HEIGHT = 2048; +const int32_t SHADOWMAP_WIDTH = 2048; +const int32_t SHADOWMAP_HEIGHT = 2048; // Class SceneDemo // Abstract class that represents a 3D scene for the ReactPhysics3D examples. // This scene has a single light source with shadow mapping. class SceneDemo : public Scene { - protected: + protected: - // -------------------- Attributes -------------------- // + // -------------------- Attributes -------------------- // - /// Light 0 - openglframework::Light mLight0; + /// Light 0 + openglframework::Light mLight0; - /// True if the shadows FBO, textures have been created - bool mIsShadowMappingInitialized; + /// True if the shadows FBO, textures have been created + bool mIsShadowMappingInitialized; - /// FBO for the shadow map - openglframework::FrameBufferObject mFBOShadowMap; + /// FBO for the shadow map + openglframework::FrameBufferObject mFBOShadowMap; - /// Shadow map texture - openglframework::Texture2D mShadowMapTexture; + /// Shadow map texture + openglframework::Texture2D mShadowMapTexture; - static int shadowMapTextureLevel; + static int32_t shadowMapTextureLevel; - /// All the visual contact points - std::vector mContactPoints; + /// All the visual contact points + std::vector mContactPoints; - /// Shadow map bias matrix - openglframework::Matrix4 mShadowMapBiasMatrix; + /// Shadow map bias matrix + openglframework::Matrix4 mShadowMapBiasMatrix; - /// Camera at light0 position for the shadow map - openglframework::Camera mShadowMapLightCamera; + /// Camera at light0 position for the shadow map + openglframework::Camera mShadowMapLightCamera; - /// Depth shader to render the shadow map - openglframework::Shader mDepthShader; + /// Depth shader to render the shadow map + openglframework::Shader mDepthShader; - /// Phong shader - openglframework::Shader mPhongShader; + /// Phong shader + openglframework::Shader mPhongShader; - // TODO : Delete this - openglframework::Shader mQuadShader; + // TODO : Delete this + openglframework::Shader mQuadShader; - // TODO : Delete this - openglframework::VertexArrayObject mVAOQuad; + // TODO : Delete this + openglframework::VertexArrayObject mVAOQuad; - openglframework::VertexBufferObject mVBOQuad; + openglframework::VertexBufferObject mVBOQuad; - static openglframework::Color mGreyColorDemo; - static openglframework::Color mYellowColorDemo; - static openglframework::Color mBlueColorDemo; - static openglframework::Color mOrangeColorDemo; - static openglframework::Color mPinkColorDemo; - static openglframework::Color mRedColorDemo; - static openglframework::Color mDemoColors[]; - static int mNbDemoColors; + static openglframework::Color mGreyColorDemo; + static openglframework::Color mYellowColorDemo; + static openglframework::Color mBlueColorDemo; + static openglframework::Color mOrangeColorDemo; + static openglframework::Color mPinkColorDemo; + static openglframework::Color mRedColorDemo; + static openglframework::Color mDemoColors[]; + static int32_t mNbDemoColors; - std::string mMeshFolderPath; + std::string mMeshFolderPath; - // -------------------- Methods -------------------- // + // -------------------- Methods -------------------- // - // Create the Shadow map FBO and texture - void createShadowMapFBOAndTexture(); + // Create the Shadow map FBO and texture + void createShadowMapFBOAndTexture(); - // Used for debugging shadow maps - void createQuadVBO(); + // Used for debugging shadow maps + void createQuadVBO(); - // TODO : Delete this - void drawTextureQuad(); + // TODO : Delete this + void drawTextureQuad(); - // Update the contact points - void updateContactPoints(); + // Update the contact points + void updateContactPoints(); - // Render the contact points - void renderContactPoints(openglframework::Shader& shader, - const openglframework::Matrix4& worldToCameraMatrix); + // Render the contact points + void renderContactPoints(openglframework::Shader& shader, + const openglframework::Matrix4& worldToCameraMatrix); - void removeAllContactPoints(); + void removeAllContactPoints(); - public: + public: - // -------------------- Methods -------------------- // + // -------------------- Methods -------------------- // - /// Constructor - SceneDemo(const std::string& name, float sceneRadius, bool isShadowMappingEnabled = true); + /// Constructor + SceneDemo(const std::string& name, float sceneRadius, bool isShadowMappingEnabled = true); - /// Destructor - virtual ~SceneDemo(); + /// Destructor + virtual ~SceneDemo(); - /// Update the scene - virtual void update(); + /// Update the scene + virtual void update(); - /// Render the scene (possibly in multiple passes for shadow mapping) - virtual void render(); + /// Render the scene (possibly in multiple passes for shadow mapping) + virtual void render(); - /// Render the scene in a single pass - virtual void renderSinglePass(openglframework::Shader& shader, - const openglframework::Matrix4& worldToCameraMatrix)=0; + /// Render the scene in a single pass + virtual void renderSinglePass(openglframework::Shader& shader, + const openglframework::Matrix4& worldToCameraMatrix)=0; - /// Enabled/Disable the shadow mapping - void virtual setIsShadowMappingEnabled(bool isShadowMappingEnabled); + /// Enabled/Disable the shadow mapping + void virtual setIsShadowMappingEnabled(bool isShadowMappingEnabled); - /// Return all the contact points of the scene - std::vector computeContactPointsOfWorld(const rp3d::DynamicsWorld* world) const; + /// Return all the contact points of the scene + std::vector computeContactPointsOfWorld(const rp3d::DynamicsWorld* world) const; }; // Enabled/Disable the shadow mapping inline void SceneDemo::setIsShadowMappingEnabled(bool isShadowMappingEnabled) { - Scene::setIsShadowMappingEnabled(isShadowMappingEnabled); + Scene::setIsShadowMappingEnabled(isShadowMappingEnabled); - if (mIsShadowMappingEnabled && !mIsShadowMappingInitialized) { - createShadowMapFBOAndTexture(); - } + if (mIsShadowMappingEnabled && !mIsShadowMappingInitialized) { + createShadowMapFBOAndTexture(); + } } #endif diff --git a/tools/testbed/src/TestbedApplication.cpp b/tools/testbed/src/TestbedApplication.cpp index 30907e5..f3e290e 100644 --- a/tools/testbed/src/TestbedApplication.cpp +++ b/tools/testbed/src/TestbedApplication.cpp @@ -1,26 +1,26 @@ /******************************************************************************** -* ReactPhysics3D physics library, http://www.reactphysics3d.com * -* Copyright (c) 2010-2016 Daniel Chappuis * +* 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: * -* * +* 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. * -* * +* 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 @@ -49,331 +49,331 @@ const float TestbedApplication::SCROLL_SENSITIVITY = 0.08f; // Constructor TestbedApplication::TestbedApplication(bool isFullscreen) - : Screen(Vector2i(1280, 800), "Testbed ReactPhysics3D", true, isFullscreen), - mIsInitialized(false), mGui(this), mFPS(0), mNbFrames(0), mPreviousTime(0), - mLastTimeComputedFPS(0), mFrameTime(0), mPhysicsTime(0) { + : Screen(Vector2i(1280, 800), "Testbed ReactPhysics3D", true, isFullscreen), + mIsInitialized(false), mGui(this), mFPS(0), mNbFrames(0), mPreviousTime(0), + mLastTimeComputedFPS(0), mFrameTime(0), mPhysicsTime(0) { - mCurrentScene = NULL; - mIsMultisamplingActive = true; - mWidth = 1280; - mHeight = 720; - mSinglePhysicsStepEnabled = false; - mSinglePhysicsStepDone = false; - mWindowToFramebufferRatio = Vector2(1, 1); - mIsShadowMappingEnabled = true; - mIsVSyncEnabled = false; - mIsContactPointsDisplayed = false; + mCurrentScene = NULL; + mIsMultisamplingActive = true; + mWidth = 1280; + mHeight = 720; + mSinglePhysicsStepEnabled = false; + mSinglePhysicsStepDone = false; + mWindowToFramebufferRatio = Vector2(1, 1); + mIsShadowMappingEnabled = true; + mIsVSyncEnabled = false; + mIsContactPointsDisplayed = false; - init(); + init(); - resizeEvent(Vector2i(0, 0)); + resizeEvent(Vector2i(0, 0)); } // Destructor TestbedApplication::~TestbedApplication() { - // Destroy all the scenes - destroyScenes(); + // Destroy all the scenes + destroyScenes(); } // Initialize the viewer void TestbedApplication::init() { - // Create all the scenes - createScenes(); + // Create all the scenes + createScenes(); - // Initialize the GUI - mGui.init(); + // Initialize the GUI + mGui.init(); - mTimer.start(); + mTimer.start(); - mIsInitialized = true; + mIsInitialized = true; } // Create all the scenes void TestbedApplication::createScenes() { - // Cubes scene - CubesScene* cubeScene = new CubesScene("Cubes"); - mScenes.push_back(cubeScene); + // Cubes scene + CubesScene* cubeScene = new CubesScene("Cubes"); + mScenes.push_back(cubeScene); - // Joints scene - JointsScene* jointsScene = new JointsScene("Joints"); - mScenes.push_back(jointsScene); + // Joints scene + JointsScene* jointsScene = new JointsScene("Joints"); + mScenes.push_back(jointsScene); - // Collision shapes scene - CollisionShapesScene* collisionShapesScene = new CollisionShapesScene("Collision Shapes"); - mScenes.push_back(collisionShapesScene); + // Collision shapes scene + CollisionShapesScene* collisionShapesScene = new CollisionShapesScene("Collision Shapes"); + mScenes.push_back(collisionShapesScene); - // Heightfield shape scene - HeightFieldScene* heightFieldScene = new HeightFieldScene("Heightfield"); - mScenes.push_back(heightFieldScene); + // Heightfield shape scene + HeightFieldScene* heightFieldScene = new HeightFieldScene("Heightfield"); + mScenes.push_back(heightFieldScene); - // Raycast scene - RaycastScene* raycastScene = new RaycastScene("Raycast"); - mScenes.push_back(raycastScene); + // Raycast scene + RaycastScene* raycastScene = new RaycastScene("Raycast"); + mScenes.push_back(raycastScene); - // Raycast scene - ConcaveMeshScene* concaveMeshScene = new ConcaveMeshScene("Concave Mesh"); - mScenes.push_back(concaveMeshScene); + // Raycast scene + ConcaveMeshScene* concaveMeshScene = new ConcaveMeshScene("Concave Mesh"); + mScenes.push_back(concaveMeshScene); - assert(mScenes.size() > 0); - mCurrentScene = mScenes[0]; + assert(mScenes.size() > 0); + mCurrentScene = mScenes[0]; - // Get the engine settings from the scene - mEngineSettings = mCurrentScene->getEngineSettings(); - mEngineSettings.timeStep = DEFAULT_TIMESTEP; + // Get the engine settings from the scene + mEngineSettings = mCurrentScene->getEngineSettings(); + mEngineSettings.timeStep = DEFAULT_TIMESTEP; } // Remove all the scenes void TestbedApplication::destroyScenes() { - for (uint i=0; iupdatePhysics(); + mCurrentScene->updatePhysics(); } // Update the physics of the current scene void TestbedApplication::updatePhysics() { - // Set the engine settings - mEngineSettings.elapsedTime = mTimer.getPhysicsTime(); - mCurrentScene->setEngineSettings(mEngineSettings); + // Set the engine settings + mEngineSettings.elapsedTime = mTimer.getPhysicsTime(); + mCurrentScene->setEngineSettings(mEngineSettings); - if (mTimer.isRunning()) { + if (mTimer.isRunning()) { - // Compute the time since the last update() call and update the timer - mTimer.update(); + // Compute the time since the last update() call and update the timer + mTimer.update(); - // While the time accumulator is not empty - while(mTimer.isPossibleToTakeStep(mEngineSettings.timeStep)) { + // While the time accumulator is not empty + while(mTimer.isPossibleToTakeStep(mEngineSettings.timeStep)) { - // Take a physics simulation step - mCurrentScene->updatePhysics(); + // Take a physics simulation step + mCurrentScene->updatePhysics(); - // Update the timer - mTimer.nextStep(mEngineSettings.timeStep); - } - } + // Update the timer + mTimer.nextStep(mEngineSettings.timeStep); + } + } } void TestbedApplication::update() { - double currentTime = glfwGetTime(); + double currentTime = glfwGetTime(); - // Update the physics - if (mSinglePhysicsStepEnabled && !mSinglePhysicsStepDone) { - updateSinglePhysicsStep(); - mSinglePhysicsStepDone = true; - } - else { - updatePhysics(); - } + // Update the physics + if (mSinglePhysicsStepEnabled && !mSinglePhysicsStepDone) { + updateSinglePhysicsStep(); + mSinglePhysicsStepDone = true; + } + else { + updatePhysics(); + } - // Compute the physics update time - mPhysicsTime = glfwGetTime() - currentTime; + // Compute the physics update time + mPhysicsTime = glfwGetTime() - currentTime; - // Compute the interpolation factor - float factor = mTimer.computeInterpolationFactor(mEngineSettings.timeStep); - assert(factor >= 0.0f && factor <= 1.0f); + // Compute the int32_terpolation factor + float factor = mTimer.computeInterpolationFactor(mEngineSettings.timeStep); + assert(factor >= 0.0f && factor <= 1.0f); - // Notify the scene about the interpolation factor - mCurrentScene->setInterpolationFactor(factor); + // Notify the scene about the int32_terpolation factor + mCurrentScene->setInterpolationFactor(factor); - // Enable/Disable shadow mapping - mCurrentScene->setIsShadowMappingEnabled(mIsShadowMappingEnabled); + // Enable/Disable shadow mapping + mCurrentScene->setIsShadowMappingEnabled(mIsShadowMappingEnabled); - // Display/Hide contact points - mCurrentScene->setIsContactPointsDisplayed(mIsContactPointsDisplayed); + // Display/Hide contact points + mCurrentScene->setIsContactPointsDisplayed(mIsContactPointsDisplayed); - // Update the scene - mCurrentScene->update(); + // Update the scene + mCurrentScene->update(); } void TestbedApplication::drawContents() { - update(); + update(); - int bufferWidth, bufferHeight; - glfwMakeContextCurrent(mGLFWWindow); - glfwGetFramebufferSize(mGLFWWindow, &bufferWidth, &bufferHeight); + int32_t bufferWidth, bufferHeight; + glfwMakeContextCurrent(mGLFWWindow); + glfwGetFramebufferSize(mGLFWWindow, &bufferWidth, &bufferHeight); - // Set the viewport of the scene - mCurrentScene->setViewport(0, 0, bufferWidth, bufferHeight); + // Set the viewport of the scene + mCurrentScene->setViewport(0, 0, bufferWidth, bufferHeight); - // Render the scene - mCurrentScene->render(); + // Render the scene + mCurrentScene->render(); - // Check the OpenGL errors - checkOpenGLErrors(); + // Check the OpenGL errors + checkOpenGLErrors(); - mGui.update(); + mGui.update(); - // Compute the current framerate - computeFPS(); + // Compute the current framerate + computeFPS(); } /// Window resize event handler bool TestbedApplication::resizeEvent(const Vector2i& size) { - if (!mIsInitialized) return false; + if (!mIsInitialized) return false; - // Get the framebuffer dimension - int width, height; - glfwGetFramebufferSize(mGLFWWindow, &width, &height); + // Get the framebuffer dimension + int32_t width, height; + glfwGetFramebufferSize(mGLFWWindow, &width, &height); - // Resize the camera viewport - mCurrentScene->reshape(width, height); + // Resize the camera viewport + mCurrentScene->reshape(width, height); - // Update the window size of the scene - int windowWidth, windowHeight; - glfwGetWindowSize(mGLFWWindow, &windowWidth, &windowHeight); - mCurrentScene->setWindowDimension(windowWidth, windowHeight); + // Update the window size of the scene + int32_t windowWidth, windowHeight; + glfwGetWindowSize(mGLFWWindow, &windowWidth, &windowHeight); + mCurrentScene->setWindowDimension(windowWidth, windowHeight); - return true; + return true; } // Change the current scene void TestbedApplication::switchScene(Scene* newScene) { - if (newScene == mCurrentScene) return; + if (newScene == mCurrentScene) return; - mCurrentScene = newScene; + mCurrentScene = newScene; - // Get the engine settings of the scene - float currentTimeStep = mEngineSettings.timeStep; - mEngineSettings = mCurrentScene->getEngineSettings(); - mEngineSettings.timeStep = currentTimeStep; + // Get the engine settings of the scene + float currentTimeStep = mEngineSettings.timeStep; + mEngineSettings = mCurrentScene->getEngineSettings(); + mEngineSettings.timeStep = currentTimeStep; - // Reset the scene - mCurrentScene->reset(); + // Reset the scene + mCurrentScene->reset(); - resizeEvent(Vector2i(0, 0)); + resizeEvent(Vector2i(0, 0)); } // Check the OpenGL errors -void TestbedApplication::checkOpenGLErrorsInternal(const char* file, int line) { - GLenum glError; +void TestbedApplication::checkOpenGLErrorsInternal(const char* file, int32_t line) { + GLenum glError; - // Get the OpenGL errors - glError = glGetError(); + // Get the OpenGL errors + glError = glGetError(); - // While there are errors - while (glError != GL_NO_ERROR) { + // While there are errors + while (glError != GL_NO_ERROR) { - std::string error; + std::string error; - switch(glError) { - case GL_INVALID_OPERATION: error="INVALID_OPERATION"; break; - case GL_INVALID_ENUM: error="INVALID_ENUM"; break; - case GL_INVALID_VALUE: error="INVALID_VALUE"; break; - case GL_OUT_OF_MEMORY: error="OUT_OF_MEMORY"; break; - case GL_INVALID_FRAMEBUFFER_OPERATION: error="INVALID_FRAMEBUFFER_OPERATION"; break; - } + switch(glError) { + case GL_INVALID_OPERATION: error="INVALID_OPERATION"; break; + case GL_INVALID_ENUM: error="INVALID_ENUM"; break; + case GL_INVALID_VALUE: error="INVALID_VALUE"; break; + case GL_OUT_OF_MEMORY: error="OUT_OF_MEMORY"; break; + case GL_INVALID_FRAMEBUFFER_OPERATION: error="INVALID_FRAMEBUFFER_OPERATION"; break; + } - std::cerr << "OpenGL Error #" << error.c_str() << " - " << file << ": " << line << std::endl; + std::cerr << "OpenGL Error #" << error.c_str() << " - " << file << ": " << line << std::endl; - // Get the next error - glError = glGetError(); - } + // Get the next error + glError = glGetError(); + } } // Compute the FPS void TestbedApplication::computeFPS() { - // Note : By default the nanogui library is using glfwWaitEvents() to process - // events and sleep to target a framerate of 50 ms (using a thread - // sleeping). However, for games we prefer to use glfwPollEvents() - // instead and remove the update. Therefore the file common.cpp of the - // nanogui library has been modified to have a faster framerate + // Note : By default the nanogui library is using glfwWaitEvents() to process + // events and sleep to target a framerate of 50 ms (using a thread + // sleeping). However, for games we prefer to use glfwPollEvents() + // instead and remove the update. Therefore the file common.cpp of the + // nanogui library has been modified to have a faster framerate - mNbFrames++; + mNbFrames++; - // Get the number of seconds since start - mCurrentTime = glfwGetTime(); + // Get the number of seconds since start + mCurrentTime = glfwGetTime(); - // Calculate time passed - mFrameTime = mCurrentTime - mPreviousTime; - double timeInterval = (mCurrentTime - mLastTimeComputedFPS) * 1000.0; + // Calculate time passed + mFrameTime = mCurrentTime - mPreviousTime; + double timeInterval = (mCurrentTime - mLastTimeComputedFPS) * 1000.0; - // Update the FPS counter each second - if(timeInterval > 1000) { + // Update the FPS counter each second + if(timeInterval > 1000) { - // calculate the number of frames per second - mFPS = static_cast(mNbFrames) / timeInterval; - mFPS *= 1000.0; + // calculate the number of frames per second + mFPS = static_cast(mNbFrames) / timeInterval; + mFPS *= 1000.0; - // Reset frame count - mNbFrames = 0; + // Reset frame count + mNbFrames = 0; - mLastTimeComputedFPS = mCurrentTime; - } + mLastTimeComputedFPS = mCurrentTime; + } - // Set time - mPreviousTime = mCurrentTime; + // Set time + mPreviousTime = mCurrentTime; } -bool TestbedApplication::keyboardEvent(int key, int scancode, int action, int modifiers) { +bool TestbedApplication::keyboardEvent(int32_t key, int32_t scancode, int32_t action, int32_t modifiers) { - if (Screen::keyboardEvent(key, scancode, action, modifiers)) { - return true; - } + if (Screen::keyboardEvent(key, scancode, action, modifiers)) { + return true; + } - // Close application on escape key - if (key == GLFW_KEY_ESCAPE && action == GLFW_PRESS) { - glfwSetWindowShouldClose(mGLFWWindow, GL_TRUE); - return true; - } + // Close application on escape key + if (key == GLFW_KEY_ESCAPE && action == GLFW_PRESS) { + glfwSetWindowShouldClose(mGLFWWindow, GL_TRUE); + return true; + } - return mCurrentScene->keyboardEvent(key, scancode, action, modifiers); + return mCurrentScene->keyboardEvent(key, scancode, action, modifiers); } // Handle a mouse button event (default implementation: propagate to children) -bool TestbedApplication::mouseButtonEvent(const Vector2i &p, int button, bool down, int modifiers) { +bool TestbedApplication::mouseButtonEvent(const Vector2i &p, int32_t button, bool down, int32_t modifiers) { - if (Screen::mouseButtonEvent(p, button, down, modifiers)) { - return true; - } + if (Screen::mouseButtonEvent(p, button, down, modifiers)) { + return true; + } - // Get the mouse cursor position - double x, y; - glfwGetCursorPos(mGLFWWindow, &x, &y); + // Get the mouse cursor position + double x, y; + glfwGetCursorPos(mGLFWWindow, &x, &y); - return mCurrentScene->mouseButtonEvent(button, down, modifiers, x, y); + return mCurrentScene->mouseButtonEvent(button, down, modifiers, x, y); } // Handle a mouse motion event (default implementation: propagate to children) -bool TestbedApplication::mouseMotionEvent(const Vector2i &p, const Vector2i &rel, int button, int modifiers) { +bool TestbedApplication::mouseMotionEvent(const Vector2i &p, const Vector2i &rel, int32_t button, int32_t modifiers) { - if (Screen::mouseMotionEvent(p, rel, button, modifiers)) { - return true; - } + if (Screen::mouseMotionEvent(p, rel, button, modifiers)) { + return true; + } - int leftButtonState = glfwGetMouseButton(mGLFWWindow, GLFW_MOUSE_BUTTON_LEFT); - int rightButtonState = glfwGetMouseButton(mGLFWWindow, GLFW_MOUSE_BUTTON_RIGHT); - int middleButtonState = glfwGetMouseButton(mGLFWWindow, GLFW_MOUSE_BUTTON_MIDDLE); - int altKeyState = glfwGetKey(mGLFWWindow, GLFW_KEY_LEFT_ALT); + int32_t leftButtonState = glfwGetMouseButton(mGLFWWindow, GLFW_MOUSE_BUTTON_LEFT); + int32_t rightButtonState = glfwGetMouseButton(mGLFWWindow, GLFW_MOUSE_BUTTON_RIGHT); + int32_t middleButtonState = glfwGetMouseButton(mGLFWWindow, GLFW_MOUSE_BUTTON_MIDDLE); + int32_t altKeyState = glfwGetKey(mGLFWWindow, GLFW_KEY_LEFT_ALT); - return mCurrentScene->mouseMotionEvent(p[0], p[1], leftButtonState, rightButtonState, - middleButtonState, altKeyState); + return mCurrentScene->mouseMotionEvent(p[0], p[1], leftButtonState, rightButtonState, + middleButtonState, altKeyState); } // Handle a mouse scroll event (default implementation: propagate to children) bool TestbedApplication::scrollEvent(const Vector2i &p, const Vector2f &rel) { - if (Screen::scrollEvent(p, rel)) { - return true; - } + if (Screen::scrollEvent(p, rel)) { + return true; + } - return mCurrentScene->scrollingEvent(rel[0], rel[1], SCROLL_SENSITIVITY); + return mCurrentScene->scrollingEvent(rel[0], rel[1], SCROLL_SENSITIVITY); } diff --git a/tools/testbed/src/TestbedApplication.h b/tools/testbed/src/TestbedApplication.h index 6625675..343371f 100644 --- a/tools/testbed/src/TestbedApplication.h +++ b/tools/testbed/src/TestbedApplication.h @@ -1,26 +1,26 @@ /******************************************************************************** -* ReactPhysics3D physics library, http://www.reactphysics3d.com * -* Copyright (c) 2010-2016 Daniel Chappuis * +* 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: * -* * +* 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. * -* * +* 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 TESTBED_APPLICATION_H @@ -44,230 +44,230 @@ const float DEFAULT_TIMESTEP = 1.0f / 60.0f; /// Class TestbedApplication class TestbedApplication : public Screen { - private : + private : - // -------------------- Constants -------------------- // + // -------------------- Constants -------------------- // - static const float SCROLL_SENSITIVITY; + static const float SCROLL_SENSITIVITY; - // -------------------- Attributes -------------------- // + // -------------------- Attributes -------------------- // - bool mIsInitialized; + bool mIsInitialized; - Gui mGui; + Gui mGui; - /// Timer - Timer mTimer; + /// Timer + Timer mTimer; - /// List of 3D scenes - std::vector mScenes; + /// List of 3D scenes + std::vector mScenes; - /// Current 3D scene - Scene* mCurrentScene; + /// Current 3D scene + Scene* mCurrentScene; - /// Physics engine settings - EngineSettings mEngineSettings; + /// Physics engine settings + EngineSettings mEngineSettings; - /// Current number of frames per seconds - double mFPS; + /// Current number of frames per seconds + double mFPS; - /// Number of frames during the last second - int mNbFrames; + /// Number of frames during the last second + int32_t mNbFrames; - /// Current time for fps computation (in seconds) - double mCurrentTime; + /// Current time for fps computation (in seconds) + double mCurrentTime; - /// Previous time for fps computation (in seconds) - double mPreviousTime; + /// Previous time for fps computation (in seconds) + double mPreviousTime; - /// Last time the FPS have been computed - double mLastTimeComputedFPS; + /// Last time the FPS have been computed + double mLastTimeComputedFPS; - /// Update time (in seconds) - double mFrameTime; + /// Update time (in seconds) + double mFrameTime; - /// Physics update time (in seconds) - double mPhysicsTime; + /// Physics update time (in seconds) + double mPhysicsTime; - /// True if multisampling is active - bool mIsMultisamplingActive; + /// True if multisampling is active + bool mIsMultisamplingActive; - /// Width and height of the window - int mWidth, mHeight; + /// Width and height of the window + int32_t mWidth, mHeight; - /// True if the next simulation update is a single physics step - bool mSinglePhysicsStepEnabled; + /// True if the next simulation update is a single physics step + bool mSinglePhysicsStepEnabled; - /// True if the single physics step has been taken already - bool mSinglePhysicsStepDone; + /// True if the single physics step has been taken already + bool mSinglePhysicsStepDone; - openglframework::Vector2 mWindowToFramebufferRatio; + openglframework::Vector2 mWindowToFramebufferRatio; - /// True if shadow mapping is enabled - bool mIsShadowMappingEnabled; + /// True if shadow mapping is enabled + bool mIsShadowMappingEnabled; - /// True if contact points are displayed - bool mIsContactPointsDisplayed; + /// True if contact points are displayed + bool mIsContactPointsDisplayed; - /// True if vsync is enabled - bool mIsVSyncEnabled; + /// True if vsync is enabled + bool mIsVSyncEnabled; - // -------------------- Methods -------------------- // + // -------------------- Methods -------------------- // - /// Private copy-constructor (for the singleton class) - TestbedApplication(TestbedApplication const&); + /// Private copy-constructor (for the singleton class) + TestbedApplication(TestbedApplication const&); - /// Private assignment operator (for the singleton class) - void operator=(TestbedApplication const&); + /// Private assignment operator (for the singleton class) + void operator=(TestbedApplication const&); - /// Update the physics of the current scene - void updatePhysics(); + /// Update the physics of the current scene + void updatePhysics(); - /// Update - void update(); + /// Update + void update(); - /// Update the simulation by taking a single physics step - void updateSinglePhysicsStep(); + /// Update the simulation by taking a single physics step + void updateSinglePhysicsStep(); - /// Check the OpenGL errors - static void checkOpenGLErrorsInternal(const char* file, int line); + /// Check the OpenGL errors + static void checkOpenGLErrorsInternal(const char* file, int32_t line); - /// Compute the FPS - void computeFPS(); + /// Compute the FPS + void computeFPS(); - /// Initialize all the scenes - void createScenes(); + /// Initialize all the scenes + void createScenes(); - /// Remove all the scenes - void destroyScenes(); + /// Remove all the scenes + void destroyScenes(); - /// Return the list of the scenes - std::vector getScenes(); + /// Return the list of the scenes + std::vector getScenes(); - /// Start/stop the simulation - void togglePlayPauseSimulation(); + /// Start/stop the simulation + void togglePlayPauseSimulation(); - /// Play the simulation - void playSimulation(); + /// Play the simulation + void playSimulation(); - /// Pause the simulation - void pauseSimulation(); + /// Pause the simulation + void pauseSimulation(); - /// Restart the simulation - void restartSimulation(); + /// Restart the simulation + void restartSimulation(); - /// Set the variable to know if we need to take a single physics step - void toggleTakeSinglePhysicsStep(); + /// Set the variable to know if we need to take a single physics step + void toggleTakeSinglePhysicsStep(); - /// Enable/Disable shadow mapping - void enableShadows(bool enable); + /// Enable/Disable shadow mapping + void enableShadows(bool enable); - /// Display/Hide contact points - void displayContactPoints(bool display); + /// Display/Hide contact points + void displayContactPoints(bool display); - public : + public : - // -------------------- Methods -------------------- // + // -------------------- Methods -------------------- // - /// Private constructor (for the singleton class) - TestbedApplication(bool isFullscreen); + /// Private constructor (for the singleton class) + TestbedApplication(bool isFullscreen); - /// Destructor - virtual ~TestbedApplication(); + /// Destructor + virtual ~TestbedApplication(); - /// Render the content of the application - virtual void drawContents(); + /// Render the content of the application + virtual void drawContents(); - /// Window resize event handler - virtual bool resizeEvent(const Vector2i& size); + /// Window resize event handler + virtual bool resizeEvent(const Vector2i& size); - /// Default keyboard event handler - virtual bool keyboardEvent(int key, int scancode, int action, int modifiers); + /// Default keyboard event handler + virtual bool keyboardEvent(int32_t key, int32_t scancode, int32_t action, int32_t modifiers); - /// Handle a mouse button event (default implementation: propagate to children) - virtual bool mouseButtonEvent(const Vector2i &p, int button, bool down, int modifiers); + /// Handle a mouse button event (default implementation: propagate to children) + virtual bool mouseButtonEvent(const Vector2i &p, int32_t button, bool down, int32_t modifiers); - /// Handle a mouse motion event (default implementation: propagate to children) - virtual bool mouseMotionEvent(const Vector2i &p, const Vector2i &rel, int button, int modifiers); + /// Handle a mouse motion event (default implementation: propagate to children) + virtual bool mouseMotionEvent(const Vector2i &p, const Vector2i &rel, int32_t button, int32_t modifiers); - /// Handle a mouse scroll event (default implementation: propagate to children) - virtual bool scrollEvent(const Vector2i &p, const Vector2f &rel); + /// Handle a mouse scroll event (default implementation: propagate to children) + virtual bool scrollEvent(const Vector2i &p, const Vector2f &rel); - /// Initialize the application - void init(); + /// Initialize the application + void init(); - /// Change the current scene - void switchScene(Scene* newScene); + /// Change the current scene + void switchScene(Scene* newScene); - /// Enable/Disable Vertical synchronization - void enableVSync(bool enable); + /// Enable/Disable Vertical synchronization + void enableVSync(bool enable); - // -------------------- Friendship -------------------- // + // -------------------- Friendship -------------------- // - friend class Gui; + friend class Gui; }; // Return the list of the scenes inline std::vector TestbedApplication::getScenes() { - return mScenes; + return mScenes; } // Toggle play/pause for the simulation inline void TestbedApplication::togglePlayPauseSimulation() { - if (mTimer.isRunning()) { - mTimer.stop(); - } - else { - mTimer.start(); - } + if (mTimer.isRunning()) { + mTimer.stop(); + } + else { + mTimer.start(); + } } // Play the simulation inline void TestbedApplication::playSimulation() { - if (!mTimer.isRunning()) mTimer.start(); + if (!mTimer.isRunning()) mTimer.start(); } // Pause the simulation inline void TestbedApplication::pauseSimulation() { - if (mTimer.isRunning()) mTimer.stop(); + if (mTimer.isRunning()) mTimer.stop(); } // Restart the simulation inline void TestbedApplication::restartSimulation() { - mCurrentScene->reset(); - mTimer.start(); + mCurrentScene->reset(); + mTimer.start(); } // Take a single step of simulation inline void TestbedApplication::toggleTakeSinglePhysicsStep() { - mSinglePhysicsStepEnabled = true; - mSinglePhysicsStepDone = false; + mSinglePhysicsStepEnabled = true; + mSinglePhysicsStepDone = false; - if (mTimer.isRunning()) { - mSinglePhysicsStepEnabled = false; - } + if (mTimer.isRunning()) { + mSinglePhysicsStepEnabled = false; + } } // Enable/Disable shadow mapping inline void TestbedApplication::enableShadows(bool enable) { - mIsShadowMappingEnabled = enable; + mIsShadowMappingEnabled = enable; } /// Display/Hide contact points inline void TestbedApplication::displayContactPoints(bool display) { - mIsContactPointsDisplayed = display; + mIsContactPointsDisplayed = display; } // Enable/Disable Vertical synchronization inline void TestbedApplication::enableVSync(bool enable) { - mIsVSyncEnabled = enable; - if (mIsVSyncEnabled) { - glfwSwapInterval(1); - } - else { - glfwSwapInterval(0); - } + mIsVSyncEnabled = enable; + if (mIsVSyncEnabled) { + glfwSwapInterval(1); + } + else { + glfwSwapInterval(0); + } } #endif diff --git a/tools/testbed/src/Timer.cpp b/tools/testbed/src/Timer.cpp index 1d7a3ec..1ca4b19 100644 --- a/tools/testbed/src/Timer.cpp +++ b/tools/testbed/src/Timer.cpp @@ -1,26 +1,26 @@ /******************************************************************************** -* ReactPhysics3D physics library, http://www.reactphysics3d.com * -* Copyright (c) 2010-2016 Daniel Chappuis * +* 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: * -* * +* 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. * -* * +* 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 @@ -40,18 +40,18 @@ Timer::~Timer() { // Return the current time of the system in seconds long double Timer::getCurrentSystemTime() { - #if defined(WINDOWS_OS) - LARGE_INTEGER ticksPerSecond; - LARGE_INTEGER ticks; - QueryPerformanceFrequency(&ticksPerSecond); - QueryPerformanceCounter(&ticks); - return (long double(ticks.QuadPart) / long double(ticksPerSecond.QuadPart)); - #else - // Initialize the lastUpdateTime with the current time in seconds - timeval timeValue; - gettimeofday(&timeValue, NULL); - return (timeValue.tv_sec + (timeValue.tv_usec / 1000000.0)); - #endif + #if defined(WINDOWS_OS) + LARGE_INTEGER ticksPerSecond; + LARGE_INTEGER ticks; + QueryPerformanceFrequency(&ticksPerSecond); + QueryPerformanceCounter(&ticks); + return (long double(ticks.QuadPart) / long double(ticksPerSecond.QuadPart)); + #else + // Initialize the lastUpdateTime with the current time in seconds + timeval timeValue; + gettimeofday(&timeValue, NULL); + return (timeValue.tv_sec + (timeValue.tv_usec / 1000000.0)); + #endif } diff --git a/tools/testbed/src/Timer.h b/tools/testbed/src/Timer.h index 81be6ee..05fdbe4 100644 --- a/tools/testbed/src/Timer.h +++ b/tools/testbed/src/Timer.h @@ -1,26 +1,26 @@ /******************************************************************************** -* ReactPhysics3D physics library, http://www.reactphysics3d.com * -* Copyright (c) 2010-2016 Daniel Chappuis * +* 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: * -* * +* 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. * -* * +* 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 TIMER_H @@ -34,9 +34,9 @@ #include #if defined(WINDOWS_OS) // For Windows platform - #define NOMINMAX // This is used to avoid definition of max() and min() macros + #define NOMINMAX // This is used to avoid definition of max() and min() macros #include -#else // For Mac OS or Linux platform +#else // For Mac OS or Linux platform #include #endif @@ -48,127 +48,127 @@ */ class Timer { - private : + private : - // -------------------- Attributes -------------------- // + // -------------------- Attributes -------------------- // - /// Last time the timer has been updated - long double mLastUpdateTime; + /// Last time the timer has been updated + long double mLastUpdateTime; - /// Time difference between the two last timer update() calls - long double mDeltaTime; + /// Time difference between the two last timer update() calls + long double mDeltaTime; - /// Used to fix the time step and avoid strange time effects - double mAccumulator; + /// Used to fix the time step and avoid strange time effects + double mAccumulator; - /// True if the timer is running - bool mIsRunning; + /// True if the timer is running + bool mIsRunning; - // -------------------- Methods -------------------- // + // -------------------- Methods -------------------- // - /// Private copy-constructor - Timer(const Timer& timer); + /// Private copy-constructor + Timer(const Timer& timer); - /// Private assignment operator - Timer& operator=(const Timer& timer); + /// Private assignment operator + Timer& operator=(const Timer& timer); - public : + public : - // -------------------- Methods -------------------- // + // -------------------- Methods -------------------- // - /// Constructor - Timer(); + /// Constructor + Timer(); - /// Destructor - virtual ~Timer(); + /// Destructor + virtual ~Timer(); - /// Return the current time of the physics engine - long double getPhysicsTime() const; + /// Return the current time of the physics engine + long double getPhysicsTime() const; - /// Start the timer - void start(); + /// Start the timer + void start(); - /// Stop the timer - void stop(); + /// Stop the timer + void stop(); - /// Return true if the timer is running - bool isRunning() const; + /// Return true if the timer is running + bool isRunning() const; - /// True if it's possible to take a new step - bool isPossibleToTakeStep(float timeStep) const; + /// True if it's possible to take a new step + bool isPossibleToTakeStep(float timeStep) const; - /// Compute the time since the last update() call and add it to the accumulator - void update(); + /// Compute the time since the last update() call and add it to the accumulator + void update(); - /// Take a new step => update the timer by adding the timeStep value to the current time - void nextStep(float timeStep); + /// Take a new step => update the timer by adding the timeStep value to the current time + void nextStep(float timeStep); - /// Compute the interpolation factor - float computeInterpolationFactor(float timeStep); + /// Compute the int32_terpolation factor + float computeInterpolationFactor(float timeStep); - /// Return the current time of the system in seconds - static long double getCurrentSystemTime(); + /// Return the current time of the system in seconds + static long double getCurrentSystemTime(); }; // Return the current time inline long double Timer::getPhysicsTime() const { - return mLastUpdateTime; + return mLastUpdateTime; } // Return if the timer is running inline bool Timer::isRunning() const { - return mIsRunning; + return mIsRunning; } // Start the timer inline void Timer::start() { - if (!mIsRunning) { + if (!mIsRunning) { - // Get the current system time - mLastUpdateTime = getCurrentSystemTime(); - - mAccumulator = 0.0; - mIsRunning = true; - } + // Get the current system time + mLastUpdateTime = getCurrentSystemTime(); + + mAccumulator = 0.0; + mIsRunning = true; + } } // Stop the timer inline void Timer::stop() { - mIsRunning = false; + mIsRunning = false; } // True if it's possible to take a new step inline bool Timer::isPossibleToTakeStep(float timeStep) const { - return (mAccumulator >= timeStep); + return (mAccumulator >= timeStep); } // Take a new step => update the timer by adding the timeStep value to the current time inline void Timer::nextStep(float timeStep) { - assert(mIsRunning); + assert(mIsRunning); - // Update the accumulator value - mAccumulator -= timeStep; + // Update the accumulator value + mAccumulator -= timeStep; } -// Compute the interpolation factor +// Compute the int32_terpolation factor inline float Timer::computeInterpolationFactor(float timeStep) { - return (float(mAccumulator) / timeStep); + return (float(mAccumulator) / timeStep); } // Compute the time since the last update() call and add it to the accumulator inline void Timer::update() { - // Get the current system time - long double currentTime = getCurrentSystemTime(); - - // Compute the delta display time between two display frames - mDeltaTime = currentTime - mLastUpdateTime; + // Get the current system time + long double currentTime = getCurrentSystemTime(); + + // Compute the delta display time between two display frames + mDeltaTime = currentTime - mLastUpdateTime; - // Update the current display time - mLastUpdateTime = currentTime; + // Update the current display time + mLastUpdateTime = currentTime; - // Update the accumulator value - mAccumulator += mDeltaTime; + // Update the accumulator value + mAccumulator += mDeltaTime; } #endif