diff --git a/src/collision/PolyhedronMesh.h b/src/collision/PolyhedronMesh.h index b8cd1f52..8720a8db 100644 --- a/src/collision/PolyhedronMesh.h +++ b/src/collision/PolyhedronMesh.h @@ -85,6 +85,9 @@ class PolyhedronMesh { /// Return a vertex Vector3 getVertex(uint index) const; + /// Return the number of faces + uint getNbFaces() const; + /// Return a face normal Vector3 getFaceNormal(uint faceIndex) const; @@ -100,6 +103,11 @@ inline uint PolyhedronMesh::getNbVertices() const { return mHalfEdgeStructure.getNbVertices(); } +// Return the number of faces +inline uint PolyhedronMesh::getNbFaces() const { + return mHalfEdgeStructure.getNbFaces(); +} + // Return a face normal inline Vector3 PolyhedronMesh::getFaceNormal(uint faceIndex) const { assert(faceIndex < mHalfEdgeStructure.getNbFaces()); diff --git a/src/collision/ProxyShape.cpp b/src/collision/ProxyShape.cpp index 8847d094..e20ae485 100644 --- a/src/collision/ProxyShape.cpp +++ b/src/collision/ProxyShape.cpp @@ -62,7 +62,7 @@ bool ProxyShape::testPointInside(const Vector3& worldPoint) { * @param ray Ray to use for the raycasting * @param[out] raycastInfo Result of the raycasting that is valid only if the * methods returned true - * @return True if the ray hit the collision shape + * @return True if the ray hits the collision shape */ bool ProxyShape::raycast(const Ray& ray, RaycastInfo& raycastInfo) { diff --git a/src/collision/narrowphase/GJK/GJKAlgorithm.cpp b/src/collision/narrowphase/GJK/GJKAlgorithm.cpp index 62685867..a7bc8667 100644 --- a/src/collision/narrowphase/GJK/GJKAlgorithm.cpp +++ b/src/collision/narrowphase/GJK/GJKAlgorithm.cpp @@ -212,171 +212,3 @@ GJKAlgorithm::GJKResult GJKAlgorithm::testCollision(NarrowPhaseInfo* narrowPhase return GJKResult::INTERPENETRATE; } - - -// Use the GJK Algorithm to find if a point is inside a convex collision shape -bool GJKAlgorithm::testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) { - - Vector3 suppA; // Support point of object A - Vector3 w; // Support point of Minkowski difference A-B - decimal prevDistSquare; - - assert(proxyShape->getCollisionShape()->isConvex()); - - const ConvexShape* shape = static_cast(proxyShape->getCollisionShape()); - - // Support point of object B (object B is a single point) - const Vector3 suppB(localPoint); - - // Create a simplex set - VoronoiSimplex simplex; - - // Initial supporting direction - Vector3 v(1, 1, 1); - - // Initialize the upper bound for the square distance - decimal distSquare = DECIMAL_LARGEST; - - do { - - // Compute the support points for original objects (without margins) A and B - suppA = shape->getLocalSupportPointWithoutMargin(-v); - - // Compute the support point for the Minkowski difference A-B - w = suppA - suppB; - - // Add the new support point to the simplex - simplex.addPoint(w, suppA, suppB); - - // If the simplex is affinely dependent - if (simplex.isAffinelyDependent()) { - - return false; - } - - // Compute the point of the simplex closest to the origin - // If the computation of the closest point fail - if (!simplex.computeClosestPoint(v)) { - - return false; - } - - // Store and update the squared distance of the closest point - prevDistSquare = distSquare; - distSquare = v.lengthSquare(); - - // If the distance to the closest point doesn't improve a lot - if (prevDistSquare - distSquare <= MACHINE_EPSILON * prevDistSquare) { - - return false; - } - } while(!simplex.isFull() && distSquare > MACHINE_EPSILON * - simplex.getMaxLengthSquareOfAPoint()); - - // The point is inside the collision shape - return true; -} - -// Ray casting algorithm agains a convex collision shape using the GJK Algorithm -/// This method implements the GJK ray casting algorithm described by Gino Van Den Bergen in -/// "Ray Casting against General Convex Objects with Application to Continuous Collision Detection". -bool GJKAlgorithm::raycast(const Ray& ray, ProxyShape* proxyShape, RaycastInfo& raycastInfo) { - - assert(proxyShape->getCollisionShape()->isConvex()); - - const ConvexShape* shape = static_cast(proxyShape->getCollisionShape()); - - Vector3 suppA; // Current lower bound point on the ray (starting at ray's origin) - Vector3 suppB; // Support point on the collision shape - const decimal machineEpsilonSquare = MACHINE_EPSILON * MACHINE_EPSILON; - const decimal epsilon = decimal(0.0001); - - // Convert the ray origin and direction into the local-space of the collision shape - Vector3 rayDirection = ray.point2 - ray.point1; - - // If the points of the segment are two close, return no hit - if (rayDirection.lengthSquare() < machineEpsilonSquare) return false; - - Vector3 w; - - // Create a simplex set - VoronoiSimplex simplex; - - Vector3 n(decimal(0.0), decimal(0.0), decimal(0.0)); - decimal lambda = decimal(0.0); - suppA = ray.point1; // Current lower bound point on the ray (starting at ray's origin) - suppB = shape->getLocalSupportPointWithoutMargin(rayDirection); - Vector3 v = suppA - suppB; - decimal vDotW, vDotR; - decimal distSquare = v.lengthSquare(); - int nbIterations = 0; - - // GJK Algorithm loop - while (distSquare > epsilon && nbIterations < MAX_ITERATIONS_GJK_RAYCAST) { - - // Compute the support points - suppB = shape->getLocalSupportPointWithoutMargin(v); - w = suppA - suppB; - - vDotW = v.dot(w); - - if (vDotW > decimal(0)) { - - vDotR = v.dot(rayDirection); - - if (vDotR >= -machineEpsilonSquare) { - return false; - } - else { - - // We have found a better lower bound for the hit point along the ray - lambda = lambda - vDotW / vDotR; - suppA = ray.point1 + lambda * rayDirection; - w = suppA - suppB; - n = v; - } - } - - // Add the new support point to the simplex - if (!simplex.isPointInSimplex(w)) { - simplex.addPoint(w, suppA, suppB); - } - - // Compute the closest point - if (simplex.computeClosestPoint(v)) { - - distSquare = v.lengthSquare(); - } - else { - distSquare = decimal(0.0); - } - - // If the current lower bound distance is larger than the maximum raycasting distance - if (lambda > ray.maxFraction) return false; - - nbIterations++; - } - - // If the origin was inside the shape, we return no hit - if (lambda < MACHINE_EPSILON) return false; - - // Compute the closet points of both objects (without the margins) - Vector3 pointA; - Vector3 pointB; - simplex.computeClosestPointsOfAandB(pointA, pointB); - - // A raycast hit has been found, we fill in the raycast info - raycastInfo.hitFraction = lambda; - raycastInfo.worldPoint = pointB; - raycastInfo.body = proxyShape->getBody(); - raycastInfo.proxyShape = proxyShape; - - if (n.lengthSquare() >= machineEpsilonSquare) { // The normal vector is valid - raycastInfo.worldNormal = n; - } - else { // Degenerated normal vector, we return a zero normal vector - raycastInfo.worldNormal = Vector3(decimal(0), decimal(0), decimal(0)); - } - - return true; -} diff --git a/src/collision/narrowphase/GJK/GJKAlgorithm.h b/src/collision/narrowphase/GJK/GJKAlgorithm.h index a9daa897..ac9cd652 100644 --- a/src/collision/narrowphase/GJK/GJKAlgorithm.h +++ b/src/collision/narrowphase/GJK/GJKAlgorithm.h @@ -96,12 +96,6 @@ class GJKAlgorithm { /// Compute a contact info if the two bounding volumes collide. GJKResult testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts); - /// Use the GJK Algorithm to find if a point is inside a convex collision shape - bool testPointInside(const Vector3& localPoint, ProxyShape* proxyShape); - - /// Ray casting algorithm agains a convex collision shape using the GJK Algorithm - bool raycast(const Ray& ray, ProxyShape* proxyShape, RaycastInfo& raycastInfo); - #ifdef IS_PROFILING_ACTIVE /// Set the profiler diff --git a/src/collision/shapes/ConvexMeshShape.cpp b/src/collision/shapes/ConvexMeshShape.cpp index 5b6c70d6..d4037a8c 100644 --- a/src/collision/shapes/ConvexMeshShape.cpp +++ b/src/collision/shapes/ConvexMeshShape.cpp @@ -30,9 +30,6 @@ using namespace reactphysics3d; -// TODO : Check in every collision shape that localScalling is used correctly and even with SAT -// algorithm (not only in getLocalSupportPoint***() methods) - // Constructor to initialize with an array of 3D vertices. /// This method creates an internal copy of the input vertices. /** @@ -58,14 +55,14 @@ ConvexMeshShape::ConvexMeshShape(PolyhedronMesh* polyhedronMesh) /// runs in almost constant time. Vector3 ConvexMeshShape::getLocalSupportPointWithoutMargin(const Vector3& direction) const { - double maxDotProduct = DECIMAL_SMALLEST; + decimal maxDotProduct = DECIMAL_SMALLEST; uint indexMaxDotProduct = 0; // For each vertex of the mesh for (uint i=0; igetNbVertices(); i++) { // Compute the dot product of the current vertex - double dotProduct = direction.dot(mPolyhedronMesh->getVertex(i)); + decimal dotProduct = direction.dot(mPolyhedronMesh->getVertex(i)); // If the current dot product is larger than the maximum one if (dotProduct > maxDotProduct) { @@ -83,14 +80,11 @@ Vector3 ConvexMeshShape::getLocalSupportPointWithoutMargin(const Vector3& direct // Recompute the bounds of the mesh void ConvexMeshShape::recalculateBounds() { - // TODO : Only works if the local origin is inside the mesh - // => Make it more robust (init with first vertex of mesh instead) - - mMinBounds.setToZero(); - mMaxBounds.setToZero(); + mMinBounds = mPolyhedronMesh->getVertex(0); + mMaxBounds = mPolyhedronMesh->getVertex(0); // For each vertex of the mesh - for (uint i=0; igetNbVertices(); i++) { + for (uint i=1; igetNbVertices(); i++) { if (mPolyhedronMesh->getVertex(i).x > mMaxBounds.x) mMaxBounds.x = mPolyhedronMesh->getVertex(i).x; if (mPolyhedronMesh->getVertex(i).x < mMinBounds.x) mMinBounds.x = mPolyhedronMesh->getVertex(i).x; @@ -105,14 +99,105 @@ void ConvexMeshShape::recalculateBounds() { // Apply the local scaling factor mMaxBounds = mMaxBounds * mScaling; mMinBounds = mMinBounds * mScaling; - - // Add the object margin to the bounds - mMaxBounds += Vector3(mMargin, mMargin, mMargin); - mMinBounds -= Vector3(mMargin, mMargin, mMargin); } // Raycast method with feedback information +/// This method implements the technique in the book "Real-time Collision Detection" by +/// Christer Ericson. bool ConvexMeshShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape, MemoryAllocator& allocator) const { - return proxyShape->mBody->mWorld.mCollisionDetection.mNarrowPhaseGJKAlgorithm.raycast( - ray, proxyShape, raycastInfo); + + // Ray direction + Vector3 direction = ray.point2 - ray.point1; + + decimal tMin = decimal(0.0); + decimal tMax = ray.maxFraction; + Vector3 currentFaceNormal; + bool isIntersectionFound = false; + + const HalfEdgeStructure& halfEdgeStructure = mPolyhedronMesh->getHalfEdgeStructure(); + + // For each face of the convex mesh + for (uint f=0; f < mPolyhedronMesh->getNbFaces(); f++) { + + const HalfEdgeStructure::Face& face = halfEdgeStructure.getFace(f); + const Vector3 faceNormal = mPolyhedronMesh->getFaceNormal(f); + const HalfEdgeStructure::Vertex& faceVertex = halfEdgeStructure.getVertex(face.faceVertices[0]); + const Vector3 facePoint = mPolyhedronMesh->getVertex(faceVertex.vertexPointIndex); + decimal denom = faceNormal.dot(direction); + decimal planeD = faceNormal.dot(facePoint); + decimal dist = planeD - faceNormal.dot(ray.point1); + + // If ray is parallel to the face + if (denom == decimal(0.0)) { + + // If ray is outside the clipping face, we return no intersection + if (dist < decimal(0.0)) return false; + } + else { + + // Compute the intersection between the ray and the current face plane + decimal t = dist / denom; + + // Update the current ray intersection by clipping it with the current face plane + // If the place faces the ray + if (denom < decimal(0.0)) { + // Clip the current ray intersection as it enters the convex mesh + if (t > tMin) { + tMin = t; + currentFaceNormal = faceNormal; + isIntersectionFound = true; + } + } + else { + // Clip the current ray intersection as it exits the convex mesh + if (t < tMax) tMax = t; + } + + // If the ray intersection with the convex mesh becomes empty, report no intersection + if (tMin > tMax) return false; + } + } + + if (isIntersectionFound) { + + // The ray intersects with the convex mesh + assert(tMin >= decimal(0.0)); + assert(tMax <= ray.maxFraction); + assert(tMin <= tMax); + assert(currentFaceNormal.lengthSquare() > decimal(0.0)); + + // The ray intersects the three slabs, we compute the hit point + Vector3 localHitPoint = ray.point1 + tMin * direction; + + raycastInfo.hitFraction = tMin; + raycastInfo.body = proxyShape->getBody(); + raycastInfo.proxyShape = proxyShape; + raycastInfo.worldPoint = localHitPoint; + raycastInfo.worldNormal = currentFaceNormal; + + return true; + } + + return false; } + +// Return true if a point is inside the collision shape +bool ConvexMeshShape::testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const { + + const HalfEdgeStructure& halfEdgeStructure = mPolyhedronMesh->getHalfEdgeStructure(); + + // For each face plane of the convex mesh + for (uint f=0; f < mPolyhedronMesh->getNbFaces(); f++) { + + const HalfEdgeStructure::Face& face = halfEdgeStructure.getFace(f); + const Vector3 faceNormal = mPolyhedronMesh->getFaceNormal(f); + const HalfEdgeStructure::Vertex& faceVertex = halfEdgeStructure.getVertex(face.faceVertices[0]); + const Vector3 facePoint = mPolyhedronMesh->getVertex(faceVertex.vertexPointIndex); + + // If the point is out of the face plane, it is outside of the convex mesh + if (computePointToPlaneDistance(localPoint, faceNormal, facePoint) > decimal(0.0)) return false; + } + + return true; +} + diff --git a/src/collision/shapes/ConvexMeshShape.h b/src/collision/shapes/ConvexMeshShape.h index a24aa85b..49d3f773 100644 --- a/src/collision/shapes/ConvexMeshShape.h +++ b/src/collision/shapes/ConvexMeshShape.h @@ -173,15 +173,6 @@ inline void ConvexMeshShape::computeLocalInertiaTensor(Matrix3x3& tensor, decima 0.0, 0.0, factor * (xSquare + ySquare)); } -// Return true if a point is inside the collision shape -inline bool ConvexMeshShape::testPointInside(const Vector3& localPoint, - ProxyShape* proxyShape) const { - - // Use the GJK algorithm to test if the point is inside the convex mesh - return proxyShape->mBody->mWorld.mCollisionDetection. - mNarrowPhaseGJKAlgorithm.testPointInside(localPoint, proxyShape); -} - // Return the number of faces of the polyhedron inline uint ConvexMeshShape::getNbFaces() const { return mPolyhedronMesh->getHalfEdgeStructure().getNbFaces(); diff --git a/src/mathematics/Ray.h b/src/mathematics/Ray.h index 938e8960..8efc7416 100644 --- a/src/mathematics/Ray.h +++ b/src/mathematics/Ray.h @@ -44,10 +44,10 @@ struct Ray { // -------------------- Attributes -------------------- // - /// First point of the ray (origin) + /// First point of the ray (origin) in world-space Vector3 point1; - /// Second point of the ray + /// Second point of the ray in world-space Vector3 point2; /// Maximum fraction value diff --git a/src/mathematics/mathematics_functions.cpp b/src/mathematics/mathematics_functions.cpp index 99640c58..4494e1ec 100755 --- a/src/mathematics/mathematics_functions.cpp +++ b/src/mathematics/mathematics_functions.cpp @@ -375,6 +375,11 @@ Vector3 reactphysics3d::projectPointOntoPlane(const Vector3& point, const Vector return point - unitPlaneNormal.dot(point - planePoint) * unitPlaneNormal; } +// Return the distance between a point and a plane (the plane normal must be normalized) +decimal reactphysics3d::computePointToPlaneDistance(const Vector3& point, const Vector3& planeNormal, const Vector3& planePoint) { + return planeNormal.dot(point - planePoint); +} + // Return true if the given number is prime bool reactphysics3d::isPrimeNumber(int number) { diff --git a/src/mathematics/mathematics_functions.h b/src/mathematics/mathematics_functions.h index d0968904..62c10c74 100755 --- a/src/mathematics/mathematics_functions.h +++ b/src/mathematics/mathematics_functions.h @@ -123,6 +123,9 @@ List clipPolygonWithPlanes(const List& polygonVertices, const /// Project a point onto a plane that is given by a point and its unit length normal Vector3 projectPointOntoPlane(const Vector3& point, const Vector3& planeNormal, const Vector3& planePoint); +/// Return the distance between a point and a plane (the plane normal must be normalized) +decimal computePointToPlaneDistance(const Vector3& point, const Vector3& planeNormal, const Vector3& planePoint); + /// Return true if the given number is prime bool isPrimeNumber(int number); diff --git a/test/tests/collision/TestPointInside.h b/test/tests/collision/TestPointInside.h index afdb415c..540f9f03 100644 --- a/test/tests/collision/TestPointInside.h +++ b/test/tests/collision/TestPointInside.h @@ -59,12 +59,17 @@ class TestPointInside : public Test { CollisionBody* mCylinderBody; CollisionBody* mCompoundBody; + Vector3 mConvexMeshCubeVertices[8]; + int mConvexMeshCubeIndices[24]; + PolygonVertexArray* mConvexMeshPolygonVertexArray; + PolyhedronMesh* mConvexMeshPolyhedronMesh; + PolygonVertexArray::PolygonFace* mConvexMeshPolygonFaces; + // Collision shapes BoxShape* mBoxShape; SphereShape* mSphereShape; CapsuleShape* mCapsuleShape; ConvexMeshShape* mConvexMeshShape; - ConvexMeshShape* mConvexMeshShapeBodyEdgesInfo; // Transform Transform mBodyTransform; @@ -76,10 +81,7 @@ class TestPointInside : public Test { ProxyShape* mBoxProxyShape; ProxyShape* mSphereProxyShape; ProxyShape* mCapsuleProxyShape; - ProxyShape* mConeProxyShape; ProxyShape* mConvexMeshProxyShape; - ProxyShape* mConvexMeshProxyShapeEdgesInfo; - ProxyShape* mCylinderProxyShape; public : @@ -104,6 +106,7 @@ class TestPointInside : public Test { mConvexMeshBody = mWorld->createCollisionBody(mBodyTransform); mConvexMeshBodyEdgesInfo = mWorld->createCollisionBody(mBodyTransform); mCylinderBody = mWorld->createCollisionBody(mBodyTransform); + mConvexMeshBody = mWorld->createCollisionBody(mBodyTransform); mCompoundBody = mWorld->createCollisionBody(mBodyTransform); // Collision shape transform @@ -121,51 +124,44 @@ class TestPointInside : public Test { mSphereShape = new SphereShape(3); mSphereProxyShape = mSphereBody->addCollisionShape(mSphereShape, mShapeTransform); - mCapsuleShape = new CapsuleShape(2, 10); + mCapsuleShape = new CapsuleShape(3, 10); mCapsuleProxyShape = mCapsuleBody->addCollisionShape(mCapsuleShape, mShapeTransform); - // TODO : Create convex mesh shape with new way (polyhedron mesh) to add test again - /*mConvexMeshShape = new ConvexMeshShape(0.0); // Box of dimension (2, 3, 4) - mConvexMeshShape->addVertex(Vector3(-2, -3, -4)); - mConvexMeshShape->addVertex(Vector3(2, -3, -4)); - mConvexMeshShape->addVertex(Vector3(2, -3, 4)); - mConvexMeshShape->addVertex(Vector3(-2, -3, 4)); - mConvexMeshShape->addVertex(Vector3(-2, 3, -4)); - mConvexMeshShape->addVertex(Vector3(2, 3, -4)); - mConvexMeshShape->addVertex(Vector3(2, 3, 4)); - mConvexMeshShape->addVertex(Vector3(-2, 3, 4)); - mConvexMeshProxyShape = mConvexMeshBody->addCollisionShape(mConvexMeshShape, mShapeTransform); + mConvexMeshCubeVertices[0] = Vector3(-2, -3, 4); + mConvexMeshCubeVertices[1] = Vector3(2, -3, 4); + mConvexMeshCubeVertices[2] = Vector3(2, -3, -4); + mConvexMeshCubeVertices[3] = Vector3(-2, -3, -4); + mConvexMeshCubeVertices[4] = Vector3(-2, 3, 4); + mConvexMeshCubeVertices[5] = Vector3(2, 3, 4); + mConvexMeshCubeVertices[6] = Vector3(2, 3, -4); + mConvexMeshCubeVertices[7] = Vector3(-2, 3, -4); - mConvexMeshShapeBodyEdgesInfo = new ConvexMeshShape(0.0); - mConvexMeshShapeBodyEdgesInfo->addVertex(Vector3(-2, -3, -4)); - mConvexMeshShapeBodyEdgesInfo->addVertex(Vector3(2, -3, -4)); - mConvexMeshShapeBodyEdgesInfo->addVertex(Vector3(2, -3, 4)); - mConvexMeshShapeBodyEdgesInfo->addVertex(Vector3(-2, -3, 4)); - mConvexMeshShapeBodyEdgesInfo->addVertex(Vector3(-2, 3, -4)); - mConvexMeshShapeBodyEdgesInfo->addVertex(Vector3(2, 3, -4)); - mConvexMeshShapeBodyEdgesInfo->addVertex(Vector3(2, 3, 4)); - mConvexMeshShapeBodyEdgesInfo->addVertex(Vector3(-2, 3, 4)); - mConvexMeshShapeBodyEdgesInfo->addEdge(0, 1); - mConvexMeshShapeBodyEdgesInfo->addEdge(1, 2); - mConvexMeshShapeBodyEdgesInfo->addEdge(2, 3); - mConvexMeshShapeBodyEdgesInfo->addEdge(0, 3); - mConvexMeshShapeBodyEdgesInfo->addEdge(4, 5); - mConvexMeshShapeBodyEdgesInfo->addEdge(5, 6); - mConvexMeshShapeBodyEdgesInfo->addEdge(6, 7); - mConvexMeshShapeBodyEdgesInfo->addEdge(4, 7); - mConvexMeshShapeBodyEdgesInfo->addEdge(0, 4); - mConvexMeshShapeBodyEdgesInfo->addEdge(1, 5); - mConvexMeshShapeBodyEdgesInfo->addEdge(2, 6); - mConvexMeshShapeBodyEdgesInfo->addEdge(3, 7); - mConvexMeshShapeBodyEdgesInfo->setIsEdgesInformationUsed(true); - mConvexMeshProxyShapeEdgesInfo = mConvexMeshBodyEdgesInfo->addCollisionShape( - mConvexMeshShapeBodyEdgesInfo, - mShapeTransform); - */ + mConvexMeshCubeIndices[0] = 0; mConvexMeshCubeIndices[1] = 3; mConvexMeshCubeIndices[2] = 2; mConvexMeshCubeIndices[3] = 1; + mConvexMeshCubeIndices[4] = 4; mConvexMeshCubeIndices[5] = 5; mConvexMeshCubeIndices[6] = 6; mConvexMeshCubeIndices[7] = 7; + mConvexMeshCubeIndices[8] = 0; mConvexMeshCubeIndices[9] = 1; mConvexMeshCubeIndices[10] = 5; mConvexMeshCubeIndices[11] = 4; + mConvexMeshCubeIndices[12] = 1; mConvexMeshCubeIndices[13] = 2; mConvexMeshCubeIndices[14] = 6; mConvexMeshCubeIndices[15] = 5; + mConvexMeshCubeIndices[16] = 2; mConvexMeshCubeIndices[17] = 3; mConvexMeshCubeIndices[18] = 7; mConvexMeshCubeIndices[19] = 6; + mConvexMeshCubeIndices[20] = 0; mConvexMeshCubeIndices[21] = 4; mConvexMeshCubeIndices[22] = 7; mConvexMeshCubeIndices[23] = 3; + + mConvexMeshPolygonFaces = new PolygonVertexArray::PolygonFace[6]; + PolygonVertexArray::PolygonFace* face = mConvexMeshPolygonFaces; + for (int f = 0; f < 6; f++) { + face->indexBase = f * 4; + face->nbVertices = 4; + face++; + } + mConvexMeshPolygonVertexArray = new PolygonVertexArray(8, &(mConvexMeshCubeVertices[0]), sizeof(Vector3), + &(mConvexMeshCubeIndices[0]), sizeof(int), 6, mConvexMeshPolygonFaces, + PolygonVertexArray::VertexDataType::VERTEX_FLOAT_TYPE, + PolygonVertexArray::IndexDataType::INDEX_INTEGER_TYPE); + mConvexMeshPolyhedronMesh = new PolyhedronMesh(mConvexMeshPolygonVertexArray); + mConvexMeshShape = new ConvexMeshShape(mConvexMeshPolyhedronMesh); + Transform convexMeshTransform(Vector3(10, 0, 0), Quaternion::identity()); + mConvexMeshProxyShape = mConvexMeshBody->addCollisionShape(mConvexMeshShape, mShapeTransform); // Compound shape is a capsule and a sphere Vector3 positionShape2(Vector3(4, 2, -3)); - Quaternion orientationShape2 = Quaternion::fromEulerAngles(-3 *PI / 8, 1.5 * PI/ 3, PI / 13); + Quaternion orientationShape2 = Quaternion::fromEulerAngles(-3 * PI / 8, 1.5 * PI/ 3, PI / 13); Transform shapeTransform2(positionShape2, orientationShape2); mLocalShape2ToWorld = mBodyTransform * shapeTransform2; mCompoundBody->addCollisionShape(mCapsuleShape, mShapeTransform); @@ -177,8 +173,10 @@ class TestPointInside : public Test { delete mBoxShape; delete mSphereShape; delete mCapsuleShape; - //delete mConvexMeshShape; - //delete mConvexMeshShapeBodyEdgesInfo; + delete mConvexMeshShape; + delete mConvexMeshPolygonFaces; + delete mConvexMeshPolygonVertexArray; + delete mConvexMeshPolyhedronMesh; } /// Run the tests @@ -328,24 +326,24 @@ class TestPointInside : public Test { test(mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(-1, 2, 0.4))); test(mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(1.3, 1, 1.5))); - test(!mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(0, -7.1, 0))); - test(!mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(0, 7.1, 0))); - test(!mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 2.1))); - test(!mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(0, 0, -2.1))); - test(!mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(2.1, 0, 0))); - test(!mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(-2.1, 0, 0))); - test(!mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(0, 5, 2.1))); - test(!mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(0, 5, -2.1))); - test(!mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(2.1, 5, 0))); - test(!mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(-2.1, 5, 0))); - test(!mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(1.5, 5, 1.6))); - test(!mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(1.5, 5, -1.7))); - test(!mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(0, -5, 2.1))); - test(!mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(0, -5, -2.1))); - test(!mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(2.1, -5, 0))); - test(!mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(-2.1, -5, 0))); - test(!mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(1.5, -5, 1.6))); - test(!mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(1.5, -5, -1.7))); + test(!mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(0, -13.1, 0))); + test(!mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(0, 13.1, 0))); + test(!mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 3.1))); + test(!mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(0, 0, -3.1))); + test(!mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(3.1, 0, 0))); + test(!mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(-3.1, 0, 0))); + test(!mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(0, 5, 3.1))); + test(!mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(0, 5, -3.1))); + test(!mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(3.1, 5, 0))); + test(!mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(-3.1, 5, 0))); + test(!mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(2.5, 5, 2.6))); + test(!mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(2.5, 5, -2.7))); + test(!mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(0, -5, 3.1))); + test(!mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(0, -5, -3.1))); + test(!mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(3.1, -5, 0))); + test(!mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(-3.1, -5, 0))); + test(!mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(2.5, -5, 2.6))); + test(!mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(2.5, -5, -2.7))); // Tests with ProxyCapsuleShape test(mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 0))); @@ -375,33 +373,30 @@ class TestPointInside : public Test { test(mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-1, 2, 0.4))); test(mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(1.3, 1, 1.5))); - test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, -7.1, 0))); - test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 7.1, 0))); - test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 2.1))); - test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, -2.1))); - test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(2.1, 0, 0))); - test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-2.1, 0, 0))); - test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 5, 2.1))); - test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 5, -2.1))); - test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(2.1, 5, 0))); - test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-2.1, 5, 0))); - test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(1.5, 5, 1.6))); - test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(1.5, 5, -1.7))); - test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, -5, 2.1))); - test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, -5, -2.1))); - test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(2.1, -5, 0))); - test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-2.1, -5, 0))); - test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(1.5, -5, 1.6))); - test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(1.5, -5, -1.7))); + test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, -13.1, 0))); + test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 13.1, 0))); + test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 3.1))); + test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, -3.1))); + test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(3.1, 0, 0))); + test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-3.1, 0, 0))); + test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 5, 3.1))); + test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 5, -3.1))); + test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(3.1, 5, 0))); + test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-3.1, 5, 0))); + test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(2.5, 5, 2.6))); + test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(2.5, 5, -2.7))); + test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, -5, 3.1))); + test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, -5, -3.1))); + test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(3.1, -5, 0))); + test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-3.1, -5, 0))); + test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(2.5, -5, 2.6))); + test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(2.5, -5, -2.7))); } /// Test the ProxyConvexMeshShape::testPointInside() and /// CollisionBody::testPointInside() methods void testConvexMesh() { - // ----- Tests without using edges information ----- // - - /* // Tests with CollisionBody test(mConvexMeshBody->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 0))); test(mConvexMeshBody->testPointInside(mLocalShapeToWorld * Vector3(-1.9, 0, 0))); @@ -453,68 +448,12 @@ class TestPointInside : public Test { test(!mConvexMeshProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-10, -2, -1.5))); test(!mConvexMeshProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-1, 4, -2.5))); test(!mConvexMeshProxyShape->testPointInside(mLocalShapeToWorld * Vector3(1, -2, 4.5))); - - // ----- Tests using edges information ----- // - - // Tests with CollisionBody - test(mConvexMeshBodyEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 0))); - test(mConvexMeshBodyEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(-1.9, 0, 0))); - test(mConvexMeshBodyEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(1.9, 0, 0))); - test(mConvexMeshBodyEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(0, -2.9, 0))); - test(mConvexMeshBodyEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(0, 2.9, 0))); - test(mConvexMeshBodyEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(0, 0, -3.9))); - test(mConvexMeshBodyEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 3.9))); - test(mConvexMeshBodyEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(-1.9, -2.9, -3.9))); - test(mConvexMeshBodyEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(1.9, 2.9, 3.9))); - test(mConvexMeshBodyEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(-1, -2, -1.5))); - test(mConvexMeshBodyEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(-1, 2, -2.5))); - test(mConvexMeshBodyEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(1, -2, 3.5))); - - test(!mConvexMeshBodyEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(-2.1, 0, 0))); - test(!mConvexMeshBodyEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(2.1, 0, 0))); - test(!mConvexMeshBodyEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(0, -3.1, 0))); - test(!mConvexMeshBodyEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(0, 3.1, 0))); - test(!mConvexMeshBodyEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(0, 0, -4.1))); - test(!mConvexMeshBodyEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 4.1))); - test(!mConvexMeshBodyEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(-2.1, -3.1, -4.1))); - test(!mConvexMeshBodyEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(2.1, 3.1, 4.1))); - test(!mConvexMeshBodyEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(-10, -2, -1.5))); - test(!mConvexMeshBodyEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(-1, 4, -2.5))); - test(!mConvexMeshBodyEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(1, -2, 4.5))); - - // Tests with ProxyConvexMeshShape - test(mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 0))); - test(mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(-1.9, 0, 0))); - test(mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(1.9, 0, 0))); - test(mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(0, -2.9, 0))); - test(mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(0, 2.9, 0))); - test(mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(0, 0, -3.9))); - test(mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 3.9))); - test(mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(-1.9, -2.9, -3.9))); - test(mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(1.9, 2.9, 3.9))); - test(mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(-1, -2, -1.5))); - test(mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(-1, 2, -2.5))); - test(mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(1, -2, 3.5))); - - test(!mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(-2.1, 0, 0))); - test(!mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(2.1, 0, 0))); - test(!mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(0, -3.1, 0))); - test(!mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(0, 3.1, 0))); - test(!mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(0, 0, -4.1))); - test(!mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 4.1))); - test(!mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(-2.1, -3.1, -4.1))); - test(!mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(2.1, 3.1, 4.1))); - test(!mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(-10, -2, -1.5))); - test(!mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(-1, 4, -2.5))); - test(!mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(1, -2, 4.5))); - */ } /// Test the CollisionBody::testPointInside() method void testCompound() { // Points on the capsule - // TODO : Previous it was a cylinder (not a capsule). Maybe those tests are wrong now test(mCompoundBody->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 0))); test(mCompoundBody->testPointInside(mLocalShapeToWorld * Vector3(0, 3.9, 0))); test(mCompoundBody->testPointInside(mLocalShapeToWorld * Vector3(0, -3.9, 0))); diff --git a/test/tests/collision/TestRaycast.h b/test/tests/collision/TestRaycast.h index 3bae090c..60d3068b 100644 --- a/test/tests/collision/TestRaycast.h +++ b/test/tests/collision/TestRaycast.h @@ -213,25 +213,26 @@ class TestRaycast : public Test { mPolyhedronVertices[0] = Vector3(-2, -3, 4); mPolyhedronVertices[1] = Vector3(2, -3, 4); - mPolyhedronVertices[2] = Vector3(2, 3, 4); - mPolyhedronVertices[3] = Vector3(-2, 3, 4); - mPolyhedronVertices[4] = Vector3(2, -3, -4); - mPolyhedronVertices[5] = Vector3(2, 3, -4); - mPolyhedronVertices[6] = Vector3(-2, 3, -4); - mPolyhedronVertices[7] = Vector3(-2, -3, -4); + mPolyhedronVertices[2] = Vector3(2, -3, -4); + mPolyhedronVertices[3] = Vector3(-2, -3, -4); + mPolyhedronVertices[4] = Vector3(-2, 3, 4); + mPolyhedronVertices[5] = Vector3(2, 3, 4); + mPolyhedronVertices[6] = Vector3(2, 3, -4); + mPolyhedronVertices[7] = Vector3(-2, 3, -4); - mPolyhedronIndices[0] = 0; mPolyhedronIndices[1] = 1; mPolyhedronIndices[2] = 2; mPolyhedronIndices[3] = 3; - mPolyhedronIndices[4] = 1; mPolyhedronIndices[5] = 4; mPolyhedronIndices[6] = 5; mPolyhedronIndices[7] = 2; - mPolyhedronIndices[8] = 4; mPolyhedronIndices[9] = 7; mPolyhedronIndices[10] = 6; mPolyhedronIndices[11] = 5; - mPolyhedronIndices[12] = 0; mPolyhedronIndices[13] = 3; mPolyhedronIndices[14] = 6; mPolyhedronIndices[15] = 7; - mPolyhedronIndices[16] = 2; mPolyhedronIndices[17] = 5; mPolyhedronIndices[18] = 6; mPolyhedronIndices[19] = 3; - mPolyhedronIndices[20] = 1; mPolyhedronIndices[21] = 0; mPolyhedronIndices[22] = 7; mPolyhedronIndices[23] = 4; + mPolyhedronIndices[0] = 0; mPolyhedronIndices[1] = 3; mPolyhedronIndices[2] = 2; mPolyhedronIndices[3] = 1; + mPolyhedronIndices[4] = 4; mPolyhedronIndices[5] = 5; mPolyhedronIndices[6] = 6; mPolyhedronIndices[7] = 7; + mPolyhedronIndices[8] = 0; mPolyhedronIndices[9] = 1; mPolyhedronIndices[10] = 5; mPolyhedronIndices[11] = 4; + mPolyhedronIndices[12] = 1; mPolyhedronIndices[13] = 2; mPolyhedronIndices[14] = 6; mPolyhedronIndices[15] = 5; + mPolyhedronIndices[16] = 2; mPolyhedronIndices[17] = 3; mPolyhedronIndices[18] = 7; mPolyhedronIndices[19] = 6; + mPolyhedronIndices[20] = 0; mPolyhedronIndices[21] = 4; mPolyhedronIndices[22] = 7; mPolyhedronIndices[23] = 3; // Polygon faces descriptions for the polyhedron - for (int f=0; f < 8; f++) { - PolygonVertexArray::PolygonFace& face = mPolygonFaces[f]; - face.indexBase = f * 4; - face.nbVertices = 4; + PolygonVertexArray::PolygonFace* face = mPolygonFaces; + for (int f = 0; f < 6; f++) { + face->indexBase = f * 4; + face->nbVertices = 4; + face++; } // Create the polygon vertex array @@ -1302,12 +1303,14 @@ class TestRaycast : public Test { Vector3 point2 = mLocalShapeToWorld * Vector3(1, 2, -4); Ray ray(point1, point2); Vector3 hitPoint = mLocalShapeToWorld * Vector3(1, 2, 4); + Transform inverse = mLocalShapeToWorld.getInverse(); mCallback.shapeToTest = mConvexMeshProxyShape; // CollisionWorld::raycast() mCallback.reset(); mWorld->raycast(ray, &mCallback); + Vector3 localTest = inverse * mCallback.raycastInfo.worldPoint; test(mCallback.isHit); test(mCallback.raycastInfo.body == mConvexMeshBody); test(mCallback.raycastInfo.proxyShape == mConvexMeshProxyShape); diff --git a/test/tests/mathematics/TestMathematicsFunctions.h b/test/tests/mathematics/TestMathematicsFunctions.h index b76cb78c..5af12e60 100644 --- a/test/tests/mathematics/TestMathematicsFunctions.h +++ b/test/tests/mathematics/TestMathematicsFunctions.h @@ -103,6 +103,16 @@ class TestMathematicsFunctions : public Test { test(!sameSign(4, -7)); test(!sameSign(-4, 53)); + // Test computePointToPlaneDistance() + Vector3 p(8, 4, 0); + Vector3 n1(1, 0, 0); + Vector3 n2(-1, 0, 0); + Vector3 q1(1, 54, 0); + Vector3 q2(8, 17, 0); + test(approxEqual(computePointToPlaneDistance(q1, n1, p), decimal(-7))); + test(approxEqual(computePointToPlaneDistance(q1, n2, p), decimal(7))); + test(approxEqual(computePointToPlaneDistance(q2, n2, p), decimal(0.0))); + // Test computeBarycentricCoordinatesInTriangle() Vector3 a(0, 0, 0); Vector3 b(5, 0, 0);