diff --git a/CMakeLists.txt b/CMakeLists.txt index 9b6af8c9..37b8febe 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -78,6 +78,8 @@ SET (REACTPHYSICS3D_SOURCES "src/collision/narrowphase/SphereVsSphereAlgorithm.cpp" "src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.h" "src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.cpp" + "src/collision/narrowphase/SphereVsCapsuleAlgorithm.h" + "src/collision/narrowphase/SphereVsCapsuleAlgorithm.cpp" "src/collision/narrowphase/ConcaveVsConvexAlgorithm.h" "src/collision/narrowphase/ConcaveVsConvexAlgorithm.cpp" "src/collision/narrowphase/SphereVsConvexMeshAlgorithm.h" diff --git a/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.cpp b/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.cpp index 44badf67..2f9f8703 100644 --- a/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.cpp +++ b/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.cpp @@ -1,37 +1,151 @@ -/******************************************************************************** -* ReactPhysics3D physics library, http://www.reactphysics3d.com * -* Copyright (c) 2010-2016 Daniel Chappuis * -********************************************************************************* -* * -* This software is provided 'as-is', without any express or implied warranty. * -* In no event will the authors be held liable for any damages arising from the * -* use of this software. * -* * -* Permission is granted to anyone to use this software for any purpose, * -* including commercial applications, and to alter it and redistribute it * -* freely, subject to the following restrictions: * -* * -* 1. The origin of this software must not be misrepresented; you must not claim * -* that you wrote the original software. If you use this software in a * -* product, an acknowledgment in the product documentation would be * -* appreciated but is not required. * -* * -* 2. Altered source versions must be plainly marked as such, and must not be * -* misrepresented as being the original software. * -* * -* 3. This notice may not be removed or altered from any source distribution. * -* * -********************************************************************************/ - -// Libraries -#include "CapsuleVsCapsuleAlgorithm.h" -#include "collision/shapes/CapsuleShape.h" - -// We want to use the ReactPhysics3D namespace -using namespace reactphysics3d; - -bool CapsuleVsCapsuleAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseInfo, - ContactPointInfo& contactPointInfo) { - - -} +/******************************************************************************** +* ReactPhysics3D physics library, http://www.reactphysics3d.com * +* Copyright (c) 2010-2016 Daniel Chappuis * +********************************************************************************* +* * +* This software is provided 'as-is', without any express or implied warranty. * +* In no event will the authors be held liable for any damages arising from the * +* use of this software. * +* * +* Permission is granted to anyone to use this software for any purpose, * +* including commercial applications, and to alter it and redistribute it * +* freely, subject to the following restrictions: * +* * +* 1. The origin of this software must not be misrepresented; you must not claim * +* that you wrote the original software. If you use this software in a * +* product, an acknowledgment in the product documentation would be * +* appreciated but is not required. * +* * +* 2. Altered source versions must be plainly marked as such, and must not be * +* misrepresented as being the original software. * +* * +* 3. This notice may not be removed or altered from any source distribution. * +* * +********************************************************************************/ + +// Libraries +#include "CapsuleVsCapsuleAlgorithm.h" +#include "collision/shapes/CapsuleShape.h" + +// We want to use the ReactPhysics3D namespace +using namespace reactphysics3d; + +bool CapsuleVsCapsuleAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseInfo, ContactPointInfo& contactPointInfo) { + + const decimal parallelEpsilon = decimal(0.001); + + // Get the capsule collision shapes + const CapsuleShape* capsuleShape1 = static_cast(narrowPhaseInfo->collisionShape1); + const CapsuleShape* capsuleShape2 = static_cast(narrowPhaseInfo->collisionShape2); + + // Get the transform from capsule 1 local-space to capsule 2 local-space + const Transform capsule1ToCapsule2SpaceTransform = narrowPhaseInfo->shape1ToWorldTransform * narrowPhaseInfo->shape2ToWorldTransform.getInverse(); + + // Compute the end-points of the inner segment of the first capsule + Vector3 capsule1SegA(0, -capsuleShape1->getHeight() * decimal(0.5), 0); + Vector3 capsule1SegB(0, capsuleShape1->getHeight() * decimal(0.5), 0); + capsule1SegA = capsule1ToCapsule2SpaceTransform * capsule1SegA; + capsule1SegB = capsule1ToCapsule2SpaceTransform * capsule1SegB; + + // Compute the end-points of the inner segment of the second capsule + const Vector3 capsule2SegA(0, -capsuleShape2->getHeight() * decimal(0.5), 0); + const Vector3 capsule2SegB(0, capsuleShape2->getHeight() * decimal(0.5), 0); + + // The two inner capsule segments + const Vector3 seg1 = capsule1SegB - capsule1SegA; + const Vector3 seg2 = capsule2SegB - capsule2SegA; + + // Compute the sum of the radius of the two capsules (virtual spheres) + decimal sumRadius = capsuleShape2->getRadius() + capsuleShape1->getRadius(); + + // If the two capsules are parallel (we create two contact points) + if (seg1.cross(seg2).lengthSquare() < parallelEpsilon * parallelEpsilon) { + + // If the distance between the two segments is larger than the sum of the capsules radius (we do not have overlapping) + const decimal segmentsDistance = computeDistancePointToLineDistance(capsule1SegA, capsule1SegB, capsule2SegA); + if (segmentsDistance > sumRadius || segmentsDistance < MACHINE_EPSILON) { + + // The capsule are parallel but their inner segment distance is larger than the sum of the capsules radius. + // Therefore, we do not have overlap. If the inner segments overlap, we do not report any collision. + return false; + } + + // Compute the planes that goes through the extrem points of the inner segment of capsule 1 + decimal d1 = seg1.dot(capsule1SegA); + decimal d2 = -seg1.dot(capsule1SegB); + + // Clip the inner segment of capsule 2 with the two planes that go through extreme points of inner + // segment of capsule 1 + decimal t1 = computePlaneSegmentIntersection(capsule2SegB, capsule2SegA, d1, seg1); + decimal t2 = computePlaneSegmentIntersection(capsule2SegA, capsule2SegB, d2, -seg1); + + bool isClipValid = false; // True if the segments were overlapping (the clip segment is valid) + + // Clip the inner segment of capsule 2 + Vector3 clipPointA, clipPointB; + if (t1 >= decimal(0.0)) { + + if (t1 > decimal(1.0)) t1 = decimal(1.0); + clipPointA = capsule2SegB - t1 * seg2; + isClipValid = true; + } + if (t2 >= decimal(0.0) && t2 <= decimal(1.0)) { + + if (t2 > decimal(1.0)) t2 = decimal(1.0); + clipPointB = capsule2SegA + t2 * seg2; + isClipValid = true; + } + + // If we have a valid clip segment + if (isClipValid) { + + Vector3 segment1ToSegment2 = (capsule2SegA - capsule1SegA); + Vector3 segment1ToSegment2Normalized = segment1ToSegment2.getUnit(); + + const Vector3 contactPointACapsule1Local = capsule1ToCapsule2SpaceTransform.getInverse() * (clipPointA - segment1ToSegment2 + segment1ToSegment2Normalized * capsuleShape1->getRadius()); + const Vector3 contactPointBCapsule1Local = capsule1ToCapsule2SpaceTransform.getInverse() * (clipPointB - segment1ToSegment2 + segment1ToSegment2Normalized * capsuleShape1->getRadius()); + const Vector3 contactPointACapsule2Local = clipPointA - segment1ToSegment2Normalized * capsuleShape2->getRadius(); + const Vector3 contactPointBCapsule2Local = clipPointB - segment1ToSegment2Normalized * capsuleShape2->getRadius(); + + const Vector3 normalWorld = narrowPhaseInfo->shape2ToWorldTransform.getOrientation() * segment1ToSegment2Normalized; + + decimal penetrationDepth = sumRadius - segmentsDistance; + + // Create the contact info object + // TODO : Make sure we create two contact points at the same time (same method here) + contactPointInfo.init(normalWorld, penetrationDepth, contactPointACapsule1Local, contactPointBCapsule1Local); + contactPointInfo.init(normalWorld, penetrationDepth, contactPointACapsule2Local, contactPointBCapsule2Local); + } + } + + // Compute the closest points between the two inner capsule segments + Vector3 closestPointCapsule1Seg; + Vector3 closestPointCapsule2Seg; + computeClosestPointBetweenTwoSegments(capsule1SegA, capsule1SegB, capsule2SegA, capsule2SegB, + closestPointCapsule1Seg, closestPointCapsule2Seg); + + // Compute the distance between the sphere center and the closest point on the segment + Vector3 closestPointsSeg1ToSeg2 = (closestPointCapsule2Seg - closestPointCapsule1Seg); + const decimal closestPointsDistanceSquare = closestPointsSeg1ToSeg2.lengthSquare(); + + // If the collision shapes overlap + if (closestPointsDistanceSquare <= sumRadius * sumRadius && closestPointsDistanceSquare > MACHINE_EPSILON) { + + decimal closestPointsDistance = std::sqrt(closestPointsDistanceSquare); + closestPointsSeg1ToSeg2 /= closestPointsDistance; + + const Vector3 contactPointCapsule1Local = capsule1ToCapsule2SpaceTransform.getInverse() * (closestPointCapsule1Seg + closestPointsSeg1ToSeg2 * capsuleShape1->getRadius()); + const Vector3 contactPointCapsule2Local = closestPointCapsule2Seg - closestPointsSeg1ToSeg2 * capsuleShape2->getRadius(); + + const Vector3 normalWorld = narrowPhaseInfo->shape2ToWorldTransform.getOrientation() * closestPointsSeg1ToSeg2; + + decimal penetrationDepth = sumRadius - closestPointsDistance; + + // Create the contact info object + contactPointInfo.init(normalWorld, penetrationDepth, contactPointCapsule1Local, contactPointCapsule2Local); + + return true; + } + + return false; +} diff --git a/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.h b/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.h index 6a48495e..7a46f73a 100644 --- a/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.h +++ b/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.h @@ -1,71 +1,71 @@ -/******************************************************************************** -* 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_VS_CAPSULE_ALGORITHM_H -#define REACTPHYSICS3D_CAPSULE_VS_CAPSULE_ALGORITHM_H - -// Libraries -#include "body/Body.h" -#include "constraint/ContactPoint.h" -#include "NarrowPhaseAlgorithm.h" - - -/// Namespace ReactPhysics3D -namespace reactphysics3d { - -// Class CapsuleVsCapsuleAlgorithm -/** - * This class is used to compute the narrow-phase collision detection - * between two capsule collision shapes. - */ -class CapsuleVsCapsuleAlgorithm : public NarrowPhaseAlgorithm { - - protected : - - public : - - // -------------------- Methods -------------------- // - - /// Constructor - CapsuleVsCapsuleAlgorithm() = default; - - /// Destructor - virtual ~CapsuleVsCapsuleAlgorithm() override = default; - - /// Deleted copy-constructor - CapsuleVsCapsuleAlgorithm(const CapsuleVsCapsuleAlgorithm& algorithm) = delete; - - /// Deleted assignment operator - CapsuleVsCapsuleAlgorithm& operator=(const CapsuleVsCapsuleAlgorithm& algorithm) = delete; - - /// Compute a contact info if the two bounding volume collide - virtual bool testCollision(const NarrowPhaseInfo* narrowPhaseInfo, - ContactPointInfo& contactPointInfo) override; -}; - -} - -#endif - +/******************************************************************************** +* 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_VS_CAPSULE_ALGORITHM_H +#define REACTPHYSICS3D_CAPSULE_VS_CAPSULE_ALGORITHM_H + +// Libraries +#include "body/Body.h" +#include "constraint/ContactPoint.h" +#include "NarrowPhaseAlgorithm.h" + + +/// Namespace ReactPhysics3D +namespace reactphysics3d { + +// Class CapsuleVsCapsuleAlgorithm +/** + * This class is used to compute the narrow-phase collision detection + * between two capsules collision shapes. + */ +class CapsuleVsCapsuleAlgorithm : public NarrowPhaseAlgorithm { + + protected : + + public : + + // -------------------- Methods -------------------- // + + /// Constructor + CapsuleVsCapsuleAlgorithm() = default; + + /// Destructor + virtual ~CapsuleVsCapsuleAlgorithm() override = default; + + /// Deleted copy-constructor + CapsuleVsCapsuleAlgorithm(const CapsuleVsCapsuleAlgorithm& algorithm) = delete; + + /// Deleted assignment operator + CapsuleVsCapsuleAlgorithm& operator=(const CapsuleVsCapsuleAlgorithm& algorithm) = delete; + + /// Compute a contact info if the two bounding volume collide + virtual bool testCollision(const NarrowPhaseInfo* narrowPhaseInfo, + ContactPointInfo& contactPointInfo) override; +}; + +} + +#endif + diff --git a/src/collision/narrowphase/SphereVsSphereAlgorithm.cpp b/src/collision/narrowphase/SphereVsSphereAlgorithm.cpp index 6b28372d..e80688d5 100644 --- a/src/collision/narrowphase/SphereVsSphereAlgorithm.cpp +++ b/src/collision/narrowphase/SphereVsSphereAlgorithm.cpp @@ -1,69 +1,69 @@ -/******************************************************************************** -* ReactPhysics3D physics library, http://www.reactphysics3d.com * -* Copyright (c) 2010-2016 Daniel Chappuis * -********************************************************************************* -* * -* This software is provided 'as-is', without any express or implied warranty. * -* In no event will the authors be held liable for any damages arising from the * -* use of this software. * -* * -* Permission is granted to anyone to use this software for any purpose, * -* including commercial applications, and to alter it and redistribute it * -* freely, subject to the following restrictions: * -* * -* 1. The origin of this software must not be misrepresented; you must not claim * -* that you wrote the original software. If you use this software in a * -* product, an acknowledgment in the product documentation would be * -* appreciated but is not required. * -* * -* 2. Altered source versions must be plainly marked as such, and must not be * -* misrepresented as being the original software. * -* * -* 3. This notice may not be removed or altered from any source distribution. * -* * -********************************************************************************/ - -// Libraries -#include "SphereVsSphereAlgorithm.h" -#include "collision/shapes/SphereShape.h" - -// We want to use the ReactPhysics3D namespace -using namespace reactphysics3d; - -bool SphereVsSphereAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseInfo, - ContactPointInfo& contactPointInfo) { - - // Get the sphere collision shapes - const SphereShape* sphereShape1 = static_cast(narrowPhaseInfo->collisionShape1); - const SphereShape* sphereShape2 = static_cast(narrowPhaseInfo->collisionShape2); - - // Get the local-space to world-space transforms - const Transform& transform1 = narrowPhaseInfo->shape1ToWorldTransform; - const Transform& transform2 = narrowPhaseInfo->shape2ToWorldTransform; - - // 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.init(vectorBetweenCenters.getUnit(), penetrationDepth, - intersectionOnBody1, intersectionOnBody2); - - return true; - } - - return false; -} +/******************************************************************************** +* ReactPhysics3D physics library, http://www.reactphysics3d.com * +* Copyright (c) 2010-2016 Daniel Chappuis * +********************************************************************************* +* * +* This software is provided 'as-is', without any express or implied warranty. * +* In no event will the authors be held liable for any damages arising from the * +* use of this software. * +* * +* Permission is granted to anyone to use this software for any purpose, * +* including commercial applications, and to alter it and redistribute it * +* freely, subject to the following restrictions: * +* * +* 1. The origin of this software must not be misrepresented; you must not claim * +* that you wrote the original software. If you use this software in a * +* product, an acknowledgment in the product documentation would be * +* appreciated but is not required. * +* * +* 2. Altered source versions must be plainly marked as such, and must not be * +* misrepresented as being the original software. * +* * +* 3. This notice may not be removed or altered from any source distribution. * +* * +********************************************************************************/ + +// Libraries +#include "SphereVsSphereAlgorithm.h" +#include "collision/shapes/SphereShape.h" + +// We want to use the ReactPhysics3D namespace +using namespace reactphysics3d; + +bool SphereVsSphereAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseInfo, + ContactPointInfo& contactPointInfo) { + + // Get the sphere collision shapes + const SphereShape* sphereShape1 = static_cast(narrowPhaseInfo->collisionShape1); + const SphereShape* sphereShape2 = static_cast(narrowPhaseInfo->collisionShape2); + + // Get the local-space to world-space transforms + const Transform& transform1 = narrowPhaseInfo->shape1ToWorldTransform; + const Transform& transform2 = narrowPhaseInfo->shape2ToWorldTransform; + + // 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.init(vectorBetweenCenters.getUnit(), penetrationDepth, + intersectionOnBody1, intersectionOnBody2); + + return true; + } + + return false; +} diff --git a/src/collision/narrowphase/SphereVsSphereAlgorithm.h b/src/collision/narrowphase/SphereVsSphereAlgorithm.h index 24886f7f..be10d5a1 100644 --- a/src/collision/narrowphase/SphereVsSphereAlgorithm.h +++ b/src/collision/narrowphase/SphereVsSphereAlgorithm.h @@ -1,71 +1,71 @@ -/******************************************************************************** -* 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 - -// Libraries -#include "body/Body.h" -#include "constraint/ContactPoint.h" -#include "NarrowPhaseAlgorithm.h" - - -/// Namespace ReactPhysics3D -namespace reactphysics3d { - -// Class SphereVsSphereAlgorithm -/** - * This class is used to compute the narrow-phase collision detection - * between two sphere collision shapes. - */ -class SphereVsSphereAlgorithm : public NarrowPhaseAlgorithm { - - protected : - - public : - - // -------------------- Methods -------------------- // - - /// Constructor - SphereVsSphereAlgorithm() = default; - - /// Destructor - virtual ~SphereVsSphereAlgorithm() override = default; - - /// Deleted copy-constructor - SphereVsSphereAlgorithm(const SphereVsSphereAlgorithm& algorithm) = delete; - - /// Deleted assignment operator - SphereVsSphereAlgorithm& operator=(const SphereVsSphereAlgorithm& algorithm) = delete; - - /// Compute a contact info if the two bounding volume collide - virtual bool testCollision(const NarrowPhaseInfo* narrowPhaseInfo, - ContactPointInfo& contactPointInfo) override; -}; - -} - -#endif - +/******************************************************************************** +* 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 + +// Libraries +#include "body/Body.h" +#include "constraint/ContactPoint.h" +#include "NarrowPhaseAlgorithm.h" + + +/// Namespace ReactPhysics3D +namespace reactphysics3d { + +// Class SphereVsSphereAlgorithm +/** + * This class is used to compute the narrow-phase collision detection + * between two sphere collision shapes. + */ +class SphereVsSphereAlgorithm : public NarrowPhaseAlgorithm { + + protected : + + public : + + // -------------------- Methods -------------------- // + + /// Constructor + SphereVsSphereAlgorithm() = default; + + /// Destructor + virtual ~SphereVsSphereAlgorithm() override = default; + + /// Deleted copy-constructor + SphereVsSphereAlgorithm(const SphereVsSphereAlgorithm& algorithm) = delete; + + /// Deleted assignment operator + SphereVsSphereAlgorithm& operator=(const SphereVsSphereAlgorithm& algorithm) = delete; + + /// Compute a contact info if the two bounding volume collide + virtual bool testCollision(const NarrowPhaseInfo* narrowPhaseInfo, + ContactPointInfo& contactPointInfo) override; +}; + +} + +#endif + diff --git a/src/mathematics/mathematics_functions.cpp b/src/mathematics/mathematics_functions.cpp index 97bf6dcf..578cecdd 100644 --- a/src/mathematics/mathematics_functions.cpp +++ b/src/mathematics/mathematics_functions.cpp @@ -1,59 +1,202 @@ -/******************************************************************************** -* ReactPhysics3D physics library, http://www.reactphysics3d.com * -* Copyright (c) 2010-2016 Daniel Chappuis * -********************************************************************************* -* * -* This software is provided 'as-is', without any express or implied warranty. * -* In no event will the authors be held liable for any damages arising from the * -* use of this software. * -* * -* Permission is granted to anyone to use this software for any purpose, * -* including commercial applications, and to alter it and redistribute it * -* freely, subject to the following restrictions: * -* * -* 1. The origin of this software must not be misrepresented; you must not claim * -* that you wrote the original software. If you use this software in a * -* product, an acknowledgment in the product documentation would be * -* appreciated but is not required. * -* * -* 2. Altered source versions must be plainly marked as such, and must not be * -* misrepresented as being the original software. * -* * -* 3. This notice may not be removed or altered from any source distribution. * -* * -********************************************************************************/ - -// Libraries -#include "mathematics_functions.h" -#include "Vector3.h" - -using namespace reactphysics3d; - -/// Compute the barycentric coordinates u, v, w of a point p inside the triangle (a, b, c) -/// This method uses the technique described in the book Real-Time collision detection by -/// Christer Ericson. -void reactphysics3d::computeBarycentricCoordinatesInTriangle(const Vector3& a, const Vector3& b, const Vector3& c, - const Vector3& p, decimal& u, decimal& v, decimal& w) { - const Vector3 v0 = b - a; - const Vector3 v1 = c - a; - const Vector3 v2 = p - a; - - decimal d00 = v0.dot(v0); - decimal d01 = v0.dot(v1); - decimal d11 = v1.dot(v1); - decimal d20 = v2.dot(v0); - decimal d21 = v2.dot(v1); - - decimal denom = d00 * d11 - d01 * d01; - v = (d11 * d20 - d01 * d21) / denom; - w = (d00 * d21 - d01 * d20) / denom; - u = decimal(1.0) - v - w; -} - -// Clamp a vector such that it is no longer than a given maximum length -Vector3 reactphysics3d::clamp(const Vector3& vector, decimal maxLength) { - if (vector.lengthSquare() > maxLength * maxLength) { - return vector.getUnit() * maxLength; - } - return vector; -} +/******************************************************************************** +* ReactPhysics3D physics library, http://www.reactphysics3d.com * +* Copyright (c) 2010-2016 Daniel Chappuis * +********************************************************************************* +* * +* This software is provided 'as-is', without any express or implied warranty. * +* In no event will the authors be held liable for any damages arising from the * +* use of this software. * +* * +* Permission is granted to anyone to use this software for any purpose, * +* including commercial applications, and to alter it and redistribute it * +* freely, subject to the following restrictions: * +* * +* 1. The origin of this software must not be misrepresented; you must not claim * +* that you wrote the original software. If you use this software in a * +* product, an acknowledgment in the product documentation would be * +* appreciated but is not required. * +* * +* 2. Altered source versions must be plainly marked as such, and must not be * +* misrepresented as being the original software. * +* * +* 3. This notice may not be removed or altered from any source distribution. * +* * +********************************************************************************/ + +// Libraries +#include "mathematics_functions.h" +#include "Vector3.h" +#include + +using namespace reactphysics3d; + +/// Compute the barycentric coordinates u, v, w of a point p inside the triangle (a, b, c) +/// This method uses the technique described in the book Real-Time collision detection by +/// Christer Ericson. +void reactphysics3d::computeBarycentricCoordinatesInTriangle(const Vector3& a, const Vector3& b, const Vector3& c, + const Vector3& p, decimal& u, decimal& v, decimal& w) { + const Vector3 v0 = b - a; + const Vector3 v1 = c - a; + const Vector3 v2 = p - a; + + decimal d00 = v0.dot(v0); + decimal d01 = v0.dot(v1); + decimal d11 = v1.dot(v1); + decimal d20 = v2.dot(v0); + decimal d21 = v2.dot(v1); + + decimal denom = d00 * d11 - d01 * d01; + v = (d11 * d20 - d01 * d21) / denom; + w = (d00 * d21 - d01 * d20) / denom; + u = decimal(1.0) - v - w; +} + +/// Clamp a vector such that it is no longer than a given maximum length +Vector3 reactphysics3d::clamp(const Vector3& vector, decimal maxLength) { + if (vector.lengthSquare() > maxLength * maxLength) { + return vector.getUnit() * maxLength; + } + return vector; +} + +/// Compute and return a point on segment from "segPointA" and "segPointB" that is closest to point "pointC" +Vector3 reactphysics3d::computeClosestPointOnSegment(const Vector3& segPointA, const Vector3& segPointB, const Vector3& pointC) { + + const Vector3 ab = segPointB - segPointA; + + decimal abLengthSquare = ab.lengthSquare(); + + // If the segment has almost zero length + if (abLengthSquare < MACHINE_EPSILON) { + + // Return one end-point of the segment as the closest point + return segPointA; + } + + // Project point C onto "AB" line + decimal t = (pointC - segPointA).dot(ab) / abLengthSquare; + + // If projected point onto the line is outside the segment, clamp it to the segment + if (t < decimal(0.0)) t = decimal(0.0); + if (t > decimal(1.0)) t = decimal(1.0); + + // Return the closest point on the segment + return segPointA + t * ab; +} + +/// Compute the closest points between two segments +/// This method uses the technique described in the book Real-Time +/// collision detection by Christer Ericson. +void computeClosestPointBetweenTwoSegments(const Vector3& seg1PointA, const Vector3& seg1PointB, + const Vector3& seg2PointA, const Vector3& seg2PointB, + Vector3& closestPointSeg1, Vector3& closestPointSeg2) { + + const Vector3 d1 = seg1PointB - seg1PointA; + const Vector3 d2 = seg2PointB - seg2PointA; + const Vector3 r = seg1PointA - seg2PointA; + decimal a = d1.lengthSquare(); + decimal e = d2.lengthSquare(); + decimal f = d2.dot(r); + decimal s, t; + + // If both segments degenerate into points + if (a <= MACHINE_EPSILON && e <= MACHINE_EPSILON) { + + closestPointSeg1 = seg1PointA; + closestPointSeg2 = seg2PointA; + return; + } + if (a <= MACHINE_EPSILON) { // If first segment degenerates into a point + + s = decimal(0.0); + + // Compute the closest point on second segment + t = clamp(f / e, decimal(0.0), decimal(1.0)); + } + else { + + decimal c = d1.dot(r); + + // If the second segment degenerates into a point + if (e <= MACHINE_EPSILON) { + + t = decimal(0.0); + s = clamp(-c / a, decimal(0.0), decimal(1.0)); + } + else { + + decimal b = d1.dot(d2); + decimal denom = a * e - b * b; + + // If the segments are not parallel + if (denom != decimal(0.0)) { + + // Compute the closest point on line 1 to line 2 and + // clamp to first segment. + s = clamp((b * f - c * e) / denom, decimal(0.0), decimal(1.0)); + } + else { + + // Pick an arbitrary point on first segment + s = decimal(0.0); + } + + // Compute the point on line 2 closest to the closest point + // we have just found + t = (b * s + f) / e; + + // If this closest point is inside second segment (t in [0, 1]), we are done. + // Otherwise, we clamp the point to the second segment and compute again the + // closest point on segment 1 + if (t < decimal(0.0)) { + t = decimal(0.0); + s = clamp(-c / a, decimal(0.0), decimal(1.0)); + } + else if (t > decimal(1.0)) { + t = decimal(1.0); + s = clamp((b - c) / a, decimal(0.0), decimal(1.0)); + } + } + } + + // Compute the closest points on both segments + closestPointSeg1 = seg1PointA + d1 * s; + closestPointSeg2 = seg2PointA + d2 * t; +} + +/// Compute the intersection between a plane and a segment +// Let the plane define by the equation planeNormal.dot(X) = planeD with X a point on the plane and "planeNormal" the plane normal. This method +// computes the intersection P between the plane and the segment (segA, segB). The method returns the value "t" such +// that P = segA + t * (segB - segA). Note that it only returns a value in [0, 1] if there is an intersection. Otherwise, +// there is no intersection between the plane and the segment. +decimal computePlaneSegmentIntersection(const Vector3& segA, const Vector3& segB, const decimal planeD, const Vector3& planeNormal) { + + const decimal parallelEpsilon = decimal(0.0001); + decimal t = decimal(-1); + + // Segment AB + const Vector3 ab = segB - segA; + + decimal nDotAB = planeNormal.dot(ab); + + // If the segment is not parallel to the plane + if (nDotAB > parallelEpsilon) { + t = (planeD - planeNormal.dot(segA)) / nDotAB; + } + + return t; +} + +/// Compute the distance between a point "point" and a line given by the points "linePointA" and "linePointB" +decimal computeDistancePointToLineDistance(const Vector3& linePointA, const Vector3& linePointB, const Vector3& point) { + + decimal distAB = (linePointB - linePointA).length(); + + if (distAB < MACHINE_EPSILON) { + return (point - linePointA).length(); + } + + return ((point - linePointA).cross(point - linePointB)).length() / distAB; +} + + diff --git a/src/mathematics/mathematics_functions.h b/src/mathematics/mathematics_functions.h index 7efe6d44..315de65c 100644 --- a/src/mathematics/mathematics_functions.h +++ b/src/mathematics/mathematics_functions.h @@ -1,87 +1,103 @@ -/******************************************************************************** -* 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 - -// Libraries -#include "configuration.h" -#include "decimal.h" -#include -#include -#include - -/// ReactPhysics3D namespace -namespace reactphysics3d { - -struct Vector3; - -// ---------- Mathematics functions ---------- // - -/// Function to test if two real numbers are (almost) equal -/// We test if two numbers a and b are such that (a-b) are in [-EPSILON; EPSILON] -inline bool approxEqual(decimal a, decimal b, decimal epsilon = MACHINE_EPSILON) { - return (std::fabs(a - b) < epsilon); -} - -/// Function 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); -} - -/// 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); -} - -/// Return the minimum value among three values -inline decimal min3(decimal a, decimal b, decimal c) { - return std::min(std::min(a, b), c); -} - -/// Return the maximum value among three values -inline decimal max3(decimal a, decimal b, decimal c) { - 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); -} - -/// Clamp a vector such that it is no longer than a given maximum length -Vector3 clamp(const Vector3& vector, decimal 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); - -} - -#endif +/******************************************************************************** +* 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 + +// Libraries +#include "configuration.h" +#include "decimal.h" +#include +#include +#include + +/// ReactPhysics3D namespace +namespace reactphysics3d { + +struct Vector3; + +// ---------- Mathematics functions ---------- // + +/// Function to test if two real numbers are (almost) equal +/// We test if two numbers a and b are such that (a-b) are in [-EPSILON; EPSILON] +inline bool approxEqual(decimal a, decimal b, decimal epsilon = MACHINE_EPSILON) { + return (std::fabs(a - b) < epsilon); +} + +/// Function 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); +} + +/// 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); +} + +/// Return the minimum value among three values +inline decimal min3(decimal a, decimal b, decimal c) { + return std::min(std::min(a, b), c); +} + +/// Return the maximum value among three values +inline decimal max3(decimal a, decimal b, decimal c) { + 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); +} + +/// Clamp a vector such that it is no longer than a given maximum length +Vector3 clamp(const Vector3& vector, decimal maxLength); + +// Compute and return a point on segment from "segPointA" and "segPointB" that is closest to point "pointC" +Vector3 computeClosestPointOnSegment(const Vector3& segPointA, const Vector3& segPointB, const Vector3& pointC); + +// Compute the closest points between two segments +void computeClosestPointBetweenTwoSegments(const Vector3& seg1PointA, const Vector3& seg1PointB, + const Vector3& seg2PointA, const Vector3& seg2PointB, + Vector3& closestPointSeg1, Vector3& closestPointSeg2); + +/// 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); + +/// Compute the intersection between a plane and a segment +decimal computePlaneSegmentIntersection(const Vector3& segA, const Vector3& segB, const decimal planeD, const Vector3& planeNormal); + +/// Compute the distance between a point and a line +decimal computeDistancePointToLineDistance(const Vector3& linePointA, const Vector3& linePointB, const Vector3& point); + +} + + + +#endif diff --git a/test/tests/mathematics/TestMathematicsFunctions.h b/test/tests/mathematics/TestMathematicsFunctions.h index 5041fd48..32e19aad 100644 --- a/test/tests/mathematics/TestMathematicsFunctions.h +++ b/test/tests/mathematics/TestMathematicsFunctions.h @@ -1,133 +1,179 @@ -/******************************************************************************** -* ReactPhysics3D physics library, http://www.reactphysics3d.com * -* Copyright (c) 2010-2016 Daniel Chappuis * -********************************************************************************* -* * -* This software is provided 'as-is', without any express or implied warranty. * -* In no event will the authors be held liable for any damages arising from the * -* use of this software. * -* * -* Permission is granted to anyone to use this software for any purpose, * -* including commercial applications, and to alter it and redistribute it * -* freely, subject to the following restrictions: * -* * -* 1. The origin of this software must not be misrepresented; you must not claim * -* that you wrote the original software. If you use this software in a * -* product, an acknowledgment in the product documentation would be * -* appreciated but is not required. * -* * -* 2. Altered source versions must be plainly marked as such, and must not be * -* misrepresented as being the original software. * -* * -* 3. This notice may not be removed or altered from any source distribution. * -* * -********************************************************************************/ - -#ifndef TEST_MATHEMATICS_FUNCTIONS_H -#define TEST_MATHEMATICS_FUNCTIONS_H - -// Libraries -#include "Test.h" -#include "mathematics/mathematics_functions.h" - -/// Reactphysics3D namespace -namespace reactphysics3d { - -// Class TestMathematicsFunctions -/** - * Unit test for mathematics functions - */ -class TestMathematicsFunctions : public Test { - - private : - - // ---------- Atributes ---------- // - - - - public : - - // ---------- Methods ---------- // - - /// Constructor - TestMathematicsFunctions(const std::string& name): Test(name) {} - - /// 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 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 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 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)); - - computeBarycentricCoordinatesInTriangle(a, b, c, testPoint, u, v, w); - test(approxEqual(u + v + w, 1.0, 0.000001)); - } - - }; - -} - -#endif +/******************************************************************************** +* ReactPhysics3D physics library, http://www.reactphysics3d.com * +* Copyright (c) 2010-2016 Daniel Chappuis * +********************************************************************************* +* * +* This software is provided 'as-is', without any express or implied warranty. * +* In no event will the authors be held liable for any damages arising from the * +* use of this software. * +* * +* Permission is granted to anyone to use this software for any purpose, * +* including commercial applications, and to alter it and redistribute it * +* freely, subject to the following restrictions: * +* * +* 1. The origin of this software must not be misrepresented; you must not claim * +* that you wrote the original software. If you use this software in a * +* product, an acknowledgment in the product documentation would be * +* appreciated but is not required. * +* * +* 2. Altered source versions must be plainly marked as such, and must not be * +* misrepresented as being the original software. * +* * +* 3. This notice may not be removed or altered from any source distribution. * +* * +********************************************************************************/ + +#ifndef TEST_MATHEMATICS_FUNCTIONS_H +#define TEST_MATHEMATICS_FUNCTIONS_H + +// Libraries +#include "Test.h" +#include "mathematics/mathematics_functions.h" + +/// Reactphysics3D namespace +namespace reactphysics3d { + +// Class TestMathematicsFunctions +/** + * Unit test for mathematics functions + */ +class TestMathematicsFunctions : public Test { + + private : + + // ---------- Atributes ---------- // + + + + public : + + // ---------- Methods ---------- // + + /// Constructor + TestMathematicsFunctions(const std::string& name): Test(name) {} + + /// 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 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 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 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)); + + computeBarycentricCoordinatesInTriangle(a, b, c, testPoint, u, v, w); + test(approxEqual(u + v + w, 1.0, 0.000001)); + + // Test computeClosestPointBetweenTwoSegments() + Vector3 closestSeg1, closestSeg2; + computeClosestPointBetweenTwoSegments(Vector3(4, 0, 0), Vector3(6, 0, 0), Vector3(8, 0, 0), Vector3(8, 6, 0), closestSeg1, closestSeg2); + test(approxEqual(closestSeg1.x, 6.0, 0.000001)); + test(approxEqual(closestSeg1.y, 0.0, 0.000001)); + test(approxEqual(closestSeg1.z, 0.0, 0.000001)); + test(approxEqual(closestSeg2.x, 8.0, 0.000001)); + test(approxEqual(closestSeg2.y, 0.0, 0.000001)); + test(approxEqual(closestSeg2.z, 0.0, 0.000001)); + computeClosestPointBetweenTwoSegments(Vector3(4, 6, 5), Vector3(4, 6, 5), Vector3(8, 3, -9), Vector3(8, 3, -9), closestSeg1, closestSeg2); + test(approxEqual(closestSeg1.x, 4.0, 0.000001)); + test(approxEqual(closestSeg1.y, 6.0, 0.000001)); + test(approxEqual(closestSeg1.z, 5.0, 0.000001)); + test(approxEqual(closestSeg2.x, 8.0, 0.000001)); + test(approxEqual(closestSeg2.y, 3.0, 0.000001)); + test(approxEqual(closestSeg2.z, -9.0, 0.000001)); + computeClosestPointBetweenTwoSegments(Vector3(0, -5, 0), Vector3(0, 8, 0), Vector3(6, 3, 0), Vector3(10, -3, 0), closestSeg1, closestSeg2); + test(approxEqual(closestSeg1.x, 0.0, 0.000001)); + test(approxEqual(closestSeg1.y, 3.0, 0.000001)); + test(approxEqual(closestSeg1.z, 0.0, 0.000001)); + test(approxEqual(closestSeg2.x, 6.0, 0.000001)); + test(approxEqual(closestSeg2.y, 3.0, 0.000001)); + test(approxEqual(closestSeg2.z, 0.0, 0.000001)); + computeClosestPointBetweenTwoSegments(Vector3(1, -4, -5), Vector3(1, 4, -5), Vector3(-6, 5, -5), Vector3(6, 5, -5), closestSeg1, closestSeg2); + test(approxEqual(closestSeg1.x, 1.0, 0.000001)); + test(approxEqual(closestSeg1.y, 5.0, 0.000001)); + test(approxEqual(closestSeg1.z, -5.0, 0.000001)); + test(approxEqual(closestSeg2.x, 1.0, 0.000001)); + test(approxEqual(closestSeg2.y, 5.0, 0.000001)); + test(approxEqual(closestSeg2.z, -5.0, 0.000001)); + + // Test computePlaneSegmentIntersection(); + test(approxEqual(computePlaneSegmentIntersection(Vector3(-6, 3, 0), Vector3(6, 3, 0), 0.0, Vector3(-1, 0, 0)), 0.5, 0.000001)); + test(approxEqual(computePlaneSegmentIntersection(Vector3(-6, 3, 0), Vector3(6, 3, 0), 0.0, Vector3(1, 0, 0)), 0.5, 0.000001)); + test(approxEqual(computePlaneSegmentIntersection(Vector3(5, 12, 0), Vector3(5, 4, 0), 6, Vector3(0, 1, 0)), 0.75, 0.000001)); + test(approxEqual(computePlaneSegmentIntersection(Vector3(5, 4, 8), Vector3(9, 14, 8), 4, Vector3(0, 1, 0)), 0.0, 0.000001)); + decimal tIntersect = computePlaneSegmentIntersection(Vector3(5, 4, 0), Vector3(9, 4, 0), 4, Vector3(0, 1, 0)); + test(tIntersect < 0.0 && tIntersect > 1.0); + + // Test computeDistancePointToLineDistance() + test(approxEqual(computeDistancePointToLineDistance(Vector3(6, 0, 0), Vector3(14, 0, 0), Vector3(5, 3, 0)), 3.0, 0.000001)); + test(approxEqual(computeDistancePointToLineDistance(Vector3(6, -5, 0), Vector3(10, -5, 0), Vector3(4, 3, 0)), 8.0, 0.000001)); + test(approxEqual(computeDistancePointToLineDistance(Vector3(6, -5, 0), Vector3(10, -5, 0), Vector3(-43, 254, 0)), 259.0, 0.000001)); + test(approxEqual(computeDistancePointToLineDistance(Vector3(6, -5, 8), Vector3(10, -5, -5), Vector3(6, -5, 8)), 0.0, 0.000001)); + test(approxEqual(computeDistancePointToLineDistance(Vector3(6, -5, 8), Vector3(10, -5, -5), Vector3(10, -5, -5)), 0.0, 0.000001)); + } + + }; + +} + +#endif diff --git a/testbed/scenes/collisiondetection/CollisionDetectionScene.cpp b/testbed/scenes/collisiondetection/CollisionDetectionScene.cpp index 8aae0145..3d692501 100644 --- a/testbed/scenes/collisiondetection/CollisionDetectionScene.cpp +++ b/testbed/scenes/collisiondetection/CollisionDetectionScene.cpp @@ -1,415 +1,417 @@ -/******************************************************************************** -* ReactPhysics3D physics library, http://www.reactphysics3d.com * -* Copyright (c) 2010-2016 Daniel Chappuis * -********************************************************************************* -* * -* This software is provided 'as-is', without any express or implied warranty. * -* In no event will the authors be held liable for any damages arising from the * -* use of this software. * -* * -* Permission is granted to anyone to use this software for any purpose, * -* including commercial applications, and to alter it and redistribute it * -* freely, subject to the following restrictions: * -* * -* 1. The origin of this software must not be misrepresented; you must not claim * -* that you wrote the original software. If you use this software in a * -* product, an acknowledgment in the product documentation would be * -* appreciated but is not required. * -* * -* 2. Altered source versions must be plainly marked as such, and must not be * -* misrepresented as being the original software. * -* * -* 3. This notice may not be removed or altered from any source distribution. * -* * -********************************************************************************/ - -// Libraries -#include "CollisionDetectionScene.h" - -// Namespaces -using namespace openglframework; -using namespace collisiondetectionscene; - -// Constructor -CollisionDetectionScene::CollisionDetectionScene(const std::string& name) - : SceneDemo(name, SCENE_RADIUS, false), mMeshFolderPath("meshes/"), - mContactManager(mPhongShader, mMeshFolderPath), - mAreNormalsDisplayed(false), mVBOVertices(GL_ARRAY_BUFFER) { - - mSelectedShapeIndex = 0; - mIsContactPointsDisplayed = true; - mIsWireframeEnabled = true; - - // 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); - - // Create the dynamics world for the physics simulation - mCollisionWorld = new rp3d::CollisionWorld(); - - // ---------- Sphere 1 ---------- // - openglframework::Vector3 position1(0, 0, 0); - - // Create a sphere and a corresponding collision body in the dynamics world - mSphere1 = new Sphere(6, position1, mCollisionWorld, mMeshFolderPath); - mAllShapes.push_back(mSphere1); - - // Set the color - mSphere1->setColor(mGreyColorDemo); - mSphere1->setSleepingColor(mRedColorDemo); - - // ---------- Sphere 2 ---------- // - openglframework::Vector3 position2(4, 0, 0); - - // Create a sphere and a corresponding collision body in the dynamics world - mSphere2 = new Sphere(4, position2, mCollisionWorld, mMeshFolderPath); - mAllShapes.push_back(mSphere2); - - // Set the color - mSphere2->setColor(mGreyColorDemo); - mSphere2->setSleepingColor(mRedColorDemo); - - // ---------- 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); - - // Set the color - //mCone->setColor(mGreyColorDemo); - //mCone->setSleepingColor(mRedColorDemo); - - // ---------- 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); - - // Set the color - //mCylinder->setColor(mGreyColorDemo); - //mCylinder->setSleepingColor(mRedColorDemo); - - // ---------- 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); - - // Set the color - //mCapsule->setColor(mGreyColorDemo); - //mCapsule->setSleepingColor(mRedColorDemo); - - // ---------- 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"); - - // Set the color - //mConvexMesh->setColor(mGreyColorDemo); - //mConvexMesh->setSleepingColor(mRedColorDemo); - - // ---------- 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"); - - // Set the color - //mConcaveMesh->setColor(mGreyColorDemo); - //mConcaveMesh->setSleepingColor(mRedColorDemo); - - // ---------- 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); - - // Set the color - //mHeightField->setColor(mGreyColorDemo); - //mHeightField->setSleepingColor(mRedColorDemo); - - // Create the VBO and VAO to render the lines - createVBOAndVAO(mPhongShader); - - mAllShapes[mSelectedShapeIndex]->setColor(mBlueColorDemo); -} - -// Reset the scene -void CollisionDetectionScene::reset() { - -} - -// Destructor -CollisionDetectionScene::~CollisionDetectionScene() { - - // Destroy the shader - mPhongShader.destroy(); - - // Destroy the box rigid body from the dynamics world - //mCollisionWorld->destroyCollisionBody(mBox->getCollisionBody()); - //delete mBox; - - // Destroy the spheres - mCollisionWorld->destroyCollisionBody(mSphere1->getCollisionBody()); - delete mSphere1; - - mCollisionWorld->destroyCollisionBody(mSphere2->getCollisionBody()); - delete mSphere2; - - /* - // 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 sphere - delete mCylinder; - - // Destroy the corresponding rigid body from the dynamics world - mCollisionWorld->destroyCollisionBody(mCapsule->getCollisionBody()); - - // Destroy the sphere - delete mCapsule; - - // Destroy the corresponding rigid body from the dynamics world - mCollisionWorld->destroyCollisionBody(mConvexMesh->getCollisionBody()); - - // Destroy the convex mesh - delete mConvexMesh; - - // Destroy the corresponding rigid body from the dynamics world - mCollisionWorld->destroyCollisionBody(mDumbbell->getCollisionBody()); - - // Destroy the dumbbell - delete mDumbbell; - - // Destroy the corresponding rigid body from the dynamics world - mCollisionWorld->destroyCollisionBody(mConcaveMesh->getCollisionBody()); - - // Destroy the convex mesh - delete mConcaveMesh; - - // Destroy the corresponding rigid body from the dynamics world - mCollisionWorld->destroyCollisionBody(mHeightField->getCollisionBody()); - - // Destroy the convex mesh - delete mHeightField; - */ - - mContactManager.resetPoints(); - - // Destroy the static data for the visual contact points - VisualContactPoint::destroyStaticData(); - - // Destroy the collision world - delete mCollisionWorld; - - // Destroy the VBOs and VAO - mVBOVertices.destroy(); - mVAO.destroy(); -} - -// Update the physics world (take a simulation step) -void CollisionDetectionScene::updatePhysics() { - - -} - -// Take a step for the simulation -void CollisionDetectionScene::update() { - - mContactManager.resetPoints(); - - mCollisionWorld->testCollision(&mContactManager); - - SceneDemo::update(); -} - -// Render the scene -void CollisionDetectionScene::renderSinglePass(openglframework::Shader& shader, - const openglframework::Matrix4& worldToCameraMatrix) { - - /* - // Bind the VAO - mVAO.bind(); - - // Bind the shader - shader.bind(); - - mVBOVertices.bind(); - - // 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 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"); - - glEnableVertexAttribArray(vertexPositionLoc); - glVertexAttribPointer(vertexPositionLoc, 3, GL_FLOAT, GL_FALSE, 0, (char*)NULL); - - // Draw the lines - glDrawArrays(GL_LINES, 0, NB_RAYS); - - glDisableVertexAttribArray(vertexPositionLoc); - - mVBOVertices.unbind(); - - // Unbind the VAO - mVAO.unbind(); - - shader.unbind(); - */ - - // Render the shapes - if (mSphere1->getCollisionBody()->isActive()) mSphere1->render(shader, worldToCameraMatrix, mIsWireframeEnabled); - if (mSphere2->getCollisionBody()->isActive()) mSphere2->render(shader, worldToCameraMatrix, mIsWireframeEnabled); - - /* - if (mBox->getCollisionBody()->isActive()) mBox->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(); -} - -// Create the Vertex Buffer Objects used to render with OpenGL. -/// We create two VBOs (one for vertices and one for indices) -void CollisionDetectionScene::createVBOAndVAO(openglframework::Shader& shader) { - - // 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 VAO for both VBOs - mVAO.create(); - mVAO.bind(); - - // Bind the VBO of vertices - mVBOVertices.bind(); - - // Unbind the VAO - mVAO.unbind(); - - // Unbind the shader - shader.unbind(); -} - -void CollisionDetectionScene::selectNextShape() { - - int previousIndex = mSelectedShapeIndex; - mSelectedShapeIndex++; - if (mSelectedShapeIndex >= mAllShapes.size()) { - mSelectedShapeIndex = 0; - } - - mAllShapes[previousIndex]->setColor(mGreyColorDemo); - mAllShapes[mSelectedShapeIndex]->setColor(mBlueColorDemo); -} - -// Called when a keyboard event occurs -bool CollisionDetectionScene::keyboardEvent(int key, int scancode, int action, int mods) { - - // If the space key has been pressed - if (key == GLFW_KEY_SPACE && action == GLFW_PRESS) { - selectNextShape(); - return true; - } - - float stepDist = 0.5f; - float stepAngle = 20 * (3.14f / 180.0f); - - if (key == GLFW_KEY_RIGHT && action == GLFW_PRESS) { - rp3d::Transform transform = mAllShapes[mSelectedShapeIndex]->getTransform(); - transform.setPosition(transform.getPosition() + rp3d::Vector3(stepDist, 0, 0)); - mAllShapes[mSelectedShapeIndex]->setTransform(transform); - } - else if (key == GLFW_KEY_LEFT && action == GLFW_PRESS) { - rp3d::Transform transform = mAllShapes[mSelectedShapeIndex]->getTransform(); - transform.setPosition(transform.getPosition() + rp3d::Vector3(-stepDist, 0, 0)); - mAllShapes[mSelectedShapeIndex]->setTransform(transform); - } - else if (key == GLFW_KEY_UP && action == GLFW_PRESS) { - rp3d::Transform transform = mAllShapes[mSelectedShapeIndex]->getTransform(); - transform.setPosition(transform.getPosition() + rp3d::Vector3(0, stepDist, 0)); - mAllShapes[mSelectedShapeIndex]->setTransform(transform); - } - else if (key == GLFW_KEY_DOWN && action == GLFW_PRESS) { - rp3d::Transform transform = mAllShapes[mSelectedShapeIndex]->getTransform(); - transform.setPosition(transform.getPosition() + rp3d::Vector3(0, -stepDist, 0)); - mAllShapes[mSelectedShapeIndex]->setTransform(transform); - } - else if (key == GLFW_KEY_Z && action == GLFW_PRESS) { - rp3d::Transform transform = mAllShapes[mSelectedShapeIndex]->getTransform(); - transform.setPosition(transform.getPosition() + rp3d::Vector3(0, 0, stepDist)); - mAllShapes[mSelectedShapeIndex]->setTransform(transform); - } - else if (key == GLFW_KEY_H && action == GLFW_PRESS) { - rp3d::Transform transform = mAllShapes[mSelectedShapeIndex]->getTransform(); - transform.setPosition(transform.getPosition() + rp3d::Vector3(0, 0, -stepDist)); - mAllShapes[mSelectedShapeIndex]->setTransform(transform); - } - else if (key == GLFW_KEY_A && action == GLFW_PRESS) { - rp3d::Transform transform = mAllShapes[mSelectedShapeIndex]->getTransform(); - transform.setOrientation(rp3d::Quaternion(0, stepAngle, 0) * transform.getOrientation()); - mAllShapes[mSelectedShapeIndex]->setTransform(transform); - } - else if (key == GLFW_KEY_D && action == GLFW_PRESS) { - rp3d::Transform transform = mAllShapes[mSelectedShapeIndex]->getTransform(); - transform.setOrientation(rp3d::Quaternion(0, -stepAngle, 0) * transform.getOrientation()); - mAllShapes[mSelectedShapeIndex]->setTransform(transform); - } - else if (key == GLFW_KEY_W && action == GLFW_PRESS) { - rp3d::Transform transform = mAllShapes[mSelectedShapeIndex]->getTransform(); - transform.setOrientation(rp3d::Quaternion(stepAngle, 0, 0) * transform.getOrientation()); - mAllShapes[mSelectedShapeIndex]->setTransform(transform); - } - else if (key == GLFW_KEY_S && action == GLFW_PRESS) { - rp3d::Transform transform = mAllShapes[mSelectedShapeIndex]->getTransform(); - transform.setOrientation(rp3d::Quaternion(-stepAngle, 0, 0) * transform.getOrientation()); - mAllShapes[mSelectedShapeIndex]->setTransform(transform); - } - else if (key == GLFW_KEY_F && action == GLFW_PRESS) { - rp3d::Transform transform = mAllShapes[mSelectedShapeIndex]->getTransform(); - transform.setOrientation(rp3d::Quaternion(0, 0, stepAngle) * transform.getOrientation()); - mAllShapes[mSelectedShapeIndex]->setTransform(transform); - } - else if (key == GLFW_KEY_G && action == GLFW_PRESS) { - rp3d::Transform transform = mAllShapes[mSelectedShapeIndex]->getTransform(); - transform.setOrientation(rp3d::Quaternion(0, 0, -stepAngle) * transform.getOrientation()); - mAllShapes[mSelectedShapeIndex]->setTransform(transform); - } - - return false; -} +/******************************************************************************** +* ReactPhysics3D physics library, http://www.reactphysics3d.com * +* Copyright (c) 2010-2016 Daniel Chappuis * +********************************************************************************* +* * +* This software is provided 'as-is', without any express or implied warranty. * +* In no event will the authors be held liable for any damages arising from the * +* use of this software. * +* * +* Permission is granted to anyone to use this software for any purpose, * +* including commercial applications, and to alter it and redistribute it * +* freely, subject to the following restrictions: * +* * +* 1. The origin of this software must not be misrepresented; you must not claim * +* that you wrote the original software. If you use this software in a * +* product, an acknowledgment in the product documentation would be * +* appreciated but is not required. * +* * +* 2. Altered source versions must be plainly marked as such, and must not be * +* misrepresented as being the original software. * +* * +* 3. This notice may not be removed or altered from any source distribution. * +* * +********************************************************************************/ + +// Libraries +#include "CollisionDetectionScene.h" + +// Namespaces +using namespace openglframework; +using namespace collisiondetectionscene; + +// Constructor +CollisionDetectionScene::CollisionDetectionScene(const std::string& name) + : SceneDemo(name, SCENE_RADIUS, false), mMeshFolderPath("meshes/"), + mContactManager(mPhongShader, mMeshFolderPath), + mAreNormalsDisplayed(false), mVBOVertices(GL_ARRAY_BUFFER) { + + mSelectedShapeIndex = 0; + mIsContactPointsDisplayed = true; + mIsWireframeEnabled = true; + + // 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); + + // Create the dynamics world for the physics simulation + mCollisionWorld = new rp3d::CollisionWorld(); + + // ---------- Sphere 1 ---------- // + openglframework::Vector3 position1(0, 0, 0); + + // Create a sphere and a corresponding collision body in the dynamics world + mSphere1 = new Sphere(6, position1, mCollisionWorld, mMeshFolderPath); + mAllShapes.push_back(mSphere1); + + // Set the color + mSphere1->setColor(mGreyColorDemo); + mSphere1->setSleepingColor(mRedColorDemo); + + // ---------- Sphere 2 ---------- // + openglframework::Vector3 position2(4, 0, 0); + + // Create a sphere and a corresponding collision body in the dynamics world + mSphere2 = new Sphere(4, position2, mCollisionWorld, mMeshFolderPath); + mAllShapes.push_back(mSphere2); + + // Set the color + mSphere2->setColor(mGreyColorDemo); + mSphere2->setSleepingColor(mRedColorDemo); + + // ---------- Capsule 1 ---------- // + openglframework::Vector3 position3(4, 0, 0); + + // Create a cylinder and a corresponding collision body in the dynamics world + mCapsule1 = new Capsule(CAPSULE_RADIUS, CAPSULE_HEIGHT, position3, mCollisionWorld, mMeshFolderPath); + mAllShapes.push_back(mCapsule1); + + // Set the color + mCapsule1->setColor(mGreyColorDemo); + mCapsule1->setSleepingColor(mRedColorDemo); + + // ---------- 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); + + // Set the color + //mCone->setColor(mGreyColorDemo); + //mCone->setSleepingColor(mRedColorDemo); + + // ---------- 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); + + // Set the color + //mCylinder->setColor(mGreyColorDemo); + //mCylinder->setSleepingColor(mRedColorDemo); + + + // ---------- 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"); + + // Set the color + //mConvexMesh->setColor(mGreyColorDemo); + //mConvexMesh->setSleepingColor(mRedColorDemo); + + // ---------- 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"); + + // Set the color + //mConcaveMesh->setColor(mGreyColorDemo); + //mConcaveMesh->setSleepingColor(mRedColorDemo); + + // ---------- 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); + + // Set the color + //mHeightField->setColor(mGreyColorDemo); + //mHeightField->setSleepingColor(mRedColorDemo); + + // Create the VBO and VAO to render the lines + //createVBOAndVAO(mPhongShader); + + mAllShapes[mSelectedShapeIndex]->setColor(mBlueColorDemo); +} + +// Reset the scene +void CollisionDetectionScene::reset() { + +} + +// Destructor +CollisionDetectionScene::~CollisionDetectionScene() { + + // Destroy the shader + mPhongShader.destroy(); + + // Destroy the box rigid body from the dynamics world + //mCollisionWorld->destroyCollisionBody(mBox->getCollisionBody()); + //delete mBox; + + // Destroy the spheres + mCollisionWorld->destroyCollisionBody(mSphere1->getCollisionBody()); + delete mSphere1; + + mCollisionWorld->destroyCollisionBody(mSphere2->getCollisionBody()); + delete mSphere2; + + /* + // 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 sphere + delete mCylinder; + + // Destroy the corresponding rigid body from the dynamics world + mCollisionWorld->destroyCollisionBody(mCapsule->getCollisionBody()); + + // Destroy the sphere + delete mCapsule; + + // Destroy the corresponding rigid body from the dynamics world + mCollisionWorld->destroyCollisionBody(mConvexMesh->getCollisionBody()); + + // Destroy the convex mesh + delete mConvexMesh; + + // Destroy the corresponding rigid body from the dynamics world + mCollisionWorld->destroyCollisionBody(mDumbbell->getCollisionBody()); + + // Destroy the dumbbell + delete mDumbbell; + + // Destroy the corresponding rigid body from the dynamics world + mCollisionWorld->destroyCollisionBody(mConcaveMesh->getCollisionBody()); + + // Destroy the convex mesh + delete mConcaveMesh; + + // Destroy the corresponding rigid body from the dynamics world + mCollisionWorld->destroyCollisionBody(mHeightField->getCollisionBody()); + + // Destroy the convex mesh + delete mHeightField; + */ + + mContactManager.resetPoints(); + + // Destroy the static data for the visual contact points + VisualContactPoint::destroyStaticData(); + + // Destroy the collision world + delete mCollisionWorld; + + // Destroy the VBOs and VAO + mVBOVertices.destroy(); + mVAO.destroy(); +} + +// Update the physics world (take a simulation step) +void CollisionDetectionScene::updatePhysics() { + + +} + +// Take a step for the simulation +void CollisionDetectionScene::update() { + + mContactManager.resetPoints(); + + mCollisionWorld->testCollision(&mContactManager); + + SceneDemo::update(); +} + +// Render the scene +void CollisionDetectionScene::renderSinglePass(openglframework::Shader& shader, + const openglframework::Matrix4& worldToCameraMatrix) { + + /* + // Bind the VAO + mVAO.bind(); + + // Bind the shader + shader.bind(); + + mVBOVertices.bind(); + + // 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 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"); + + glEnableVertexAttribArray(vertexPositionLoc); + glVertexAttribPointer(vertexPositionLoc, 3, GL_FLOAT, GL_FALSE, 0, (char*)NULL); + + // Draw the lines + glDrawArrays(GL_LINES, 0, NB_RAYS); + + glDisableVertexAttribArray(vertexPositionLoc); + + mVBOVertices.unbind(); + + // Unbind the VAO + mVAO.unbind(); + + shader.unbind(); + */ + + // Render the shapes + if (mSphere1->getCollisionBody()->isActive()) mSphere1->render(shader, worldToCameraMatrix, mIsWireframeEnabled); + if (mSphere2->getCollisionBody()->isActive()) mSphere2->render(shader, worldToCameraMatrix, mIsWireframeEnabled); + if (mCapsule1->getCollisionBody()->isActive()) mCapsule1->render(shader, worldToCameraMatrix, mIsWireframeEnabled); + + /* + if (mBox->getCollisionBody()->isActive()) mBox->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(); +} + +// Create the Vertex Buffer Objects used to render with OpenGL. +/// We create two VBOs (one for vertices and one for indices) +void CollisionDetectionScene::createVBOAndVAO(openglframework::Shader& shader) { + + // 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 VAO for both VBOs + mVAO.create(); + mVAO.bind(); + + // Bind the VBO of vertices + mVBOVertices.bind(); + + // Unbind the VAO + mVAO.unbind(); + + // Unbind the shader + shader.unbind(); +} + +void CollisionDetectionScene::selectNextShape() { + + int previousIndex = mSelectedShapeIndex; + mSelectedShapeIndex++; + if (mSelectedShapeIndex >= mAllShapes.size()) { + mSelectedShapeIndex = 0; + } + + mAllShapes[previousIndex]->setColor(mGreyColorDemo); + mAllShapes[mSelectedShapeIndex]->setColor(mBlueColorDemo); +} + +// Called when a keyboard event occurs +bool CollisionDetectionScene::keyboardEvent(int key, int scancode, int action, int mods) { + + // If the space key has been pressed + if (key == GLFW_KEY_SPACE && action == GLFW_PRESS) { + selectNextShape(); + return true; + } + + float stepDist = 0.5f; + float stepAngle = 20 * (3.14f / 180.0f); + + if (key == GLFW_KEY_RIGHT && action == GLFW_PRESS) { + rp3d::Transform transform = mAllShapes[mSelectedShapeIndex]->getTransform(); + transform.setPosition(transform.getPosition() + rp3d::Vector3(stepDist, 0, 0)); + mAllShapes[mSelectedShapeIndex]->setTransform(transform); + } + else if (key == GLFW_KEY_LEFT && action == GLFW_PRESS) { + rp3d::Transform transform = mAllShapes[mSelectedShapeIndex]->getTransform(); + transform.setPosition(transform.getPosition() + rp3d::Vector3(-stepDist, 0, 0)); + mAllShapes[mSelectedShapeIndex]->setTransform(transform); + } + else if (key == GLFW_KEY_UP && action == GLFW_PRESS) { + rp3d::Transform transform = mAllShapes[mSelectedShapeIndex]->getTransform(); + transform.setPosition(transform.getPosition() + rp3d::Vector3(0, stepDist, 0)); + mAllShapes[mSelectedShapeIndex]->setTransform(transform); + } + else if (key == GLFW_KEY_DOWN && action == GLFW_PRESS) { + rp3d::Transform transform = mAllShapes[mSelectedShapeIndex]->getTransform(); + transform.setPosition(transform.getPosition() + rp3d::Vector3(0, -stepDist, 0)); + mAllShapes[mSelectedShapeIndex]->setTransform(transform); + } + else if (key == GLFW_KEY_Z && action == GLFW_PRESS) { + rp3d::Transform transform = mAllShapes[mSelectedShapeIndex]->getTransform(); + transform.setPosition(transform.getPosition() + rp3d::Vector3(0, 0, stepDist)); + mAllShapes[mSelectedShapeIndex]->setTransform(transform); + } + else if (key == GLFW_KEY_H && action == GLFW_PRESS) { + rp3d::Transform transform = mAllShapes[mSelectedShapeIndex]->getTransform(); + transform.setPosition(transform.getPosition() + rp3d::Vector3(0, 0, -stepDist)); + mAllShapes[mSelectedShapeIndex]->setTransform(transform); + } + else if (key == GLFW_KEY_A && action == GLFW_PRESS) { + rp3d::Transform transform = mAllShapes[mSelectedShapeIndex]->getTransform(); + transform.setOrientation(rp3d::Quaternion(0, stepAngle, 0) * transform.getOrientation()); + mAllShapes[mSelectedShapeIndex]->setTransform(transform); + } + else if (key == GLFW_KEY_D && action == GLFW_PRESS) { + rp3d::Transform transform = mAllShapes[mSelectedShapeIndex]->getTransform(); + transform.setOrientation(rp3d::Quaternion(0, -stepAngle, 0) * transform.getOrientation()); + mAllShapes[mSelectedShapeIndex]->setTransform(transform); + } + else if (key == GLFW_KEY_W && action == GLFW_PRESS) { + rp3d::Transform transform = mAllShapes[mSelectedShapeIndex]->getTransform(); + transform.setOrientation(rp3d::Quaternion(stepAngle, 0, 0) * transform.getOrientation()); + mAllShapes[mSelectedShapeIndex]->setTransform(transform); + } + else if (key == GLFW_KEY_S && action == GLFW_PRESS) { + rp3d::Transform transform = mAllShapes[mSelectedShapeIndex]->getTransform(); + transform.setOrientation(rp3d::Quaternion(-stepAngle, 0, 0) * transform.getOrientation()); + mAllShapes[mSelectedShapeIndex]->setTransform(transform); + } + else if (key == GLFW_KEY_F && action == GLFW_PRESS) { + rp3d::Transform transform = mAllShapes[mSelectedShapeIndex]->getTransform(); + transform.setOrientation(rp3d::Quaternion(0, 0, stepAngle) * transform.getOrientation()); + mAllShapes[mSelectedShapeIndex]->setTransform(transform); + } + else if (key == GLFW_KEY_G && action == GLFW_PRESS) { + rp3d::Transform transform = mAllShapes[mSelectedShapeIndex]->getTransform(); + transform.setOrientation(rp3d::Quaternion(0, 0, -stepAngle) * transform.getOrientation()); + mAllShapes[mSelectedShapeIndex]->setTransform(transform); + } + + return false; +} diff --git a/testbed/scenes/collisiondetection/CollisionDetectionScene.h b/testbed/scenes/collisiondetection/CollisionDetectionScene.h index d3a7de00..323b9c34 100644 --- a/testbed/scenes/collisiondetection/CollisionDetectionScene.h +++ b/testbed/scenes/collisiondetection/CollisionDetectionScene.h @@ -1,231 +1,235 @@ -/******************************************************************************** -* 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 COLLISION_DETECTION_SCENE_H -#define COLLISION_DETECTION_SCENE_H - -// Libraries -#include -#include "openglframework.h" -#include "reactphysics3d.h" -#include "SceneDemo.h" -#include "Sphere.h" -#include "Box.h" -#include "Capsule.h" -#include "Line.h" -#include "ConvexMesh.h" -#include "ConcaveMesh.h" -#include "HeightField.h" -#include "Dumbbell.h" -#include "VisualContactPoint.h" - -namespace collisiondetectionscene { - -// Constants -const float SCENE_RADIUS = 30.0f; -const openglframework::Vector3 BOX_SIZE(4, 2, 1); -const float SPHERE_RADIUS = 3.0f; -const float CONE_RADIUS = 3.0f; -const float CONE_HEIGHT = 5.0f; -const float CYLINDER_RADIUS = 3.0f; -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 float RAY_LENGTH = 30.0f; -const int NB_BODIES = 9; - -// Raycast manager -class ContactManager : public rp3d::CollisionCallback { - - private: - - /// All the visual contact points - std::vector mContactPoints; - - /// All the normals at contact points - std::vector mNormals; - - /// Contact point mesh folder path - std::string mMeshFolderPath; - - public: - - ContactManager(openglframework::Shader& shader, - const std::string& meshFolderPath) - : mMeshFolderPath(meshFolderPath) { - - } - - /// This method will be called for each reported contact point - virtual void notifyContact(const CollisionCallbackInfo& collisionCallbackInfo) override { - - rp3d::Vector3 point1 = collisionCallbackInfo.contactPoint.localPoint1; - point1 = collisionCallbackInfo.proxyShape1->getLocalToWorldTransform() * point1; - openglframework::Vector3 position1(point1.x, point1.y, point1.z); - mContactPoints.push_back(ContactPoint(position1)); - - rp3d::Vector3 point2 = collisionCallbackInfo.contactPoint.localPoint2; - point2 = collisionCallbackInfo.proxyShape2->getLocalToWorldTransform() * point2; - openglframework::Vector3 position2(point2.x, point2.y, point2.z); - mContactPoints.push_back(ContactPoint(position2)); - - // Create a line to display the normal at hit point - rp3d::Vector3 n = collisionCallbackInfo.contactPoint.normal; - openglframework::Vector3 normal(n.x, n.y, n.z); - Line* normalLine = new Line(position1, position1 + normal); - mNormals.push_back(normalLine); - } - - void resetPoints() { - - mContactPoints.clear(); - - // Destroy all the normals - for (std::vector::iterator it = mNormals.begin(); - it != mNormals.end(); ++it) { - delete (*it); - } - mNormals.clear(); - } - - std::vector getContactPoints() const { - return mContactPoints; - } -}; - -// Class CollisionDetectionScene -class CollisionDetectionScene : public SceneDemo { - - private : - - // -------------------- Attributes -------------------- // - - /// Contact point mesh folder path - std::string mMeshFolderPath; - - /// Contact manager - ContactManager mContactManager; - - bool mAreNormalsDisplayed; - - /// All objects on the scene - //Box* mBox; - Sphere* mSphere1; - Sphere* mSphere2; - //Cone* mCone; - //Cylinder* mCylinder; - //Capsule* mCapsule; - //ConvexMesh* mConvexMesh; - //Dumbbell* mDumbbell; - //ConcaveMesh* mConcaveMesh; - //HeightField* mHeightField; - - std::vector mAllShapes; - - int mSelectedShapeIndex; - - /// Collision world used for the physics simulation - rp3d::CollisionWorld* mCollisionWorld; - - /// All the points to render the lines - std::vector mLinePoints; - - /// Vertex Buffer Object for the vertices data - openglframework::VertexBufferObject mVBOVertices; - - /// Vertex Array Object for the vertex data - openglframework::VertexArrayObject mVAO; - - /// Create the Vertex Buffer Objects used to render with OpenGL. - void createVBOAndVAO(openglframework::Shader& shader); - - /// Select the next shape - void selectNextShape(); - - public: - - // -------------------- Methods -------------------- // - - /// Constructor - CollisionDetectionScene(const std::string& name); - - /// Destructor - virtual ~CollisionDetectionScene() override; - - /// Update the physics world (take a simulation step) - /// Can be called several times per frame - virtual void updatePhysics() override; - - /// Take a step for the simulation - virtual void update() override; - - /// Render the scene in a single pass - virtual void renderSinglePass(openglframework::Shader& shader, - const openglframework::Matrix4& worldToCameraMatrix) override; - - /// Reset the scene - virtual void reset() override; - - /// 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) override; - - /// Enabled/Disable the shadow mapping - virtual void setIsShadowMappingEnabled(bool isShadowMappingEnabled) override; - - /// Display/Hide the contact points - virtual void setIsContactPointsDisplayed(bool display) override; - - /// Return all the contact points of the scene - virtual std::vector getContactPoints() const override; -}; - -// Display or not the surface normals at hit points -inline void CollisionDetectionScene::showHideNormals() { - mAreNormalsDisplayed = !mAreNormalsDisplayed; -} - -// Enabled/Disable the shadow mapping -inline void CollisionDetectionScene::setIsShadowMappingEnabled(bool isShadowMappingEnabled) { - SceneDemo::setIsShadowMappingEnabled(false); -} - -// Display/Hide the contact points -inline void CollisionDetectionScene::setIsContactPointsDisplayed(bool display) { - SceneDemo::setIsContactPointsDisplayed(true); -} - -// Return all the contact points of the scene -inline std::vector CollisionDetectionScene::getContactPoints() const { - return mContactManager.getContactPoints(); -} - -} - -#endif +/******************************************************************************** +* 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 COLLISION_DETECTION_SCENE_H +#define COLLISION_DETECTION_SCENE_H + +// Libraries +#include +#include "openglframework.h" +#include "reactphysics3d.h" +#include "SceneDemo.h" +#include "Sphere.h" +#include "Box.h" +#include "Cone.h" +#include "Cylinder.h" +#include "Capsule.h" +#include "Line.h" +#include "ConvexMesh.h" +#include "ConcaveMesh.h" +#include "HeightField.h" +#include "Dumbbell.h" +#include "VisualContactPoint.h" + +namespace collisiondetectionscene { + +// Constants +const float SCENE_RADIUS = 30.0f; +const openglframework::Vector3 BOX_SIZE(4, 2, 1); +const float SPHERE_RADIUS = 3.0f; +const float CONE_RADIUS = 3.0f; +const float CONE_HEIGHT = 5.0f; +const float CYLINDER_RADIUS = 3.0f; +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 float RAY_LENGTH = 30.0f; +const int NB_BODIES = 9; + +// Raycast manager +class ContactManager : public rp3d::CollisionCallback { + + private: + + /// All the visual contact points + std::vector mContactPoints; + + /// All the normals at contact points + std::vector mNormals; + + /// Contact point mesh folder path + std::string mMeshFolderPath; + + public: + + ContactManager(openglframework::Shader& shader, + const std::string& meshFolderPath) + : mMeshFolderPath(meshFolderPath) { + + } + + /// This method will be called for each reported contact point + virtual void notifyContact(const CollisionCallbackInfo& collisionCallbackInfo) override { + + rp3d::Vector3 point1 = collisionCallbackInfo.contactPoint.localPoint1; + point1 = collisionCallbackInfo.proxyShape1->getLocalToWorldTransform() * point1; + openglframework::Vector3 position1(point1.x, point1.y, point1.z); + mContactPoints.push_back(ContactPoint(position1)); + + rp3d::Vector3 point2 = collisionCallbackInfo.contactPoint.localPoint2; + point2 = collisionCallbackInfo.proxyShape2->getLocalToWorldTransform() * point2; + openglframework::Vector3 position2(point2.x, point2.y, point2.z); + mContactPoints.push_back(ContactPoint(position2)); + + // Create a line to display the normal at hit point + rp3d::Vector3 n = collisionCallbackInfo.contactPoint.normal; + openglframework::Vector3 normal(n.x, n.y, n.z); + Line* normalLine = new Line(position1, position1 + normal); + mNormals.push_back(normalLine); + } + + void resetPoints() { + + mContactPoints.clear(); + + // Destroy all the normals + for (std::vector::iterator it = mNormals.begin(); + it != mNormals.end(); ++it) { + delete (*it); + } + mNormals.clear(); + } + + std::vector getContactPoints() const { + return mContactPoints; + } +}; + +// Class CollisionDetectionScene +class CollisionDetectionScene : public SceneDemo { + + private : + + // -------------------- Attributes -------------------- // + + /// Contact point mesh folder path + std::string mMeshFolderPath; + + /// Contact manager + ContactManager mContactManager; + + bool mAreNormalsDisplayed; + + /// All objects on the scene + //Box* mBox; + Sphere* mSphere1; + Sphere* mSphere2; + Capsule* mCapsule1; + Capsule* mCapsule2; + //Cone* mCone; + //Cylinder* mCylinder; + //Capsule* mCapsule; + //ConvexMesh* mConvexMesh; + //Dumbbell* mDumbbell; + //ConcaveMesh* mConcaveMesh; + //HeightField* mHeightField; + + std::vector mAllShapes; + + int mSelectedShapeIndex; + + /// Collision world used for the physics simulation + rp3d::CollisionWorld* mCollisionWorld; + + /// All the points to render the lines + std::vector mLinePoints; + + /// Vertex Buffer Object for the vertices data + openglframework::VertexBufferObject mVBOVertices; + + /// Vertex Array Object for the vertex data + openglframework::VertexArrayObject mVAO; + + /// Create the Vertex Buffer Objects used to render with OpenGL. + void createVBOAndVAO(openglframework::Shader& shader); + + /// Select the next shape + void selectNextShape(); + + public: + + // -------------------- Methods -------------------- // + + /// Constructor + CollisionDetectionScene(const std::string& name); + + /// Destructor + virtual ~CollisionDetectionScene() override; + + /// Update the physics world (take a simulation step) + /// Can be called several times per frame + virtual void updatePhysics() override; + + /// Take a step for the simulation + virtual void update() override; + + /// Render the scene in a single pass + virtual void renderSinglePass(openglframework::Shader& shader, + const openglframework::Matrix4& worldToCameraMatrix) override; + + /// Reset the scene + virtual void reset() override; + + /// 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) override; + + /// Enabled/Disable the shadow mapping + virtual void setIsShadowMappingEnabled(bool isShadowMappingEnabled) override; + + /// Display/Hide the contact points + virtual void setIsContactPointsDisplayed(bool display) override; + + /// Return all the contact points of the scene + virtual std::vector getContactPoints() const override; +}; + +// Display or not the surface normals at hit points +inline void CollisionDetectionScene::showHideNormals() { + mAreNormalsDisplayed = !mAreNormalsDisplayed; +} + +// Enabled/Disable the shadow mapping +inline void CollisionDetectionScene::setIsShadowMappingEnabled(bool isShadowMappingEnabled) { + SceneDemo::setIsShadowMappingEnabled(false); +} + +// Display/Hide the contact points +inline void CollisionDetectionScene::setIsContactPointsDisplayed(bool display) { + SceneDemo::setIsContactPointsDisplayed(true); +} + +// Return all the contact points of the scene +inline std::vector CollisionDetectionScene::getContactPoints() const { + return mContactManager.getContactPoints(); +} + +} + +#endif