diff --git a/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.cpp b/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.cpp index a6c7a020..17008622 100644 --- a/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.cpp +++ b/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.cpp @@ -66,7 +66,7 @@ bool CapsuleVsCapsuleAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhase if (areParallelVectors(seg1, seg2)) { // 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); + const decimal segmentsDistance = computePointToLineDistance(capsule1SegA, capsule1SegB, capsule2SegA); if (segmentsDistance >= sumRadius) { // The capsule are parallel but their inner segment distance is larger than the sum of the capsules radius. diff --git a/src/collision/narrowphase/SAT/SATAlgorithm.cpp b/src/collision/narrowphase/SAT/SATAlgorithm.cpp index 3637b3dc..fd09db27 100644 --- a/src/collision/narrowphase/SAT/SATAlgorithm.cpp +++ b/src/collision/narrowphase/SAT/SATAlgorithm.cpp @@ -169,8 +169,7 @@ bool SATAlgorithm::testCollisionCapsuleVsConvexPolyhedron(const NarrowPhaseInfo* const Vector3 capsuleSegB(0, capsuleShape->getHeight() * decimal(0.5), 0); const Vector3 capsuleSegmentAxis = capsuleSegB - capsuleSegA; - // For each direction that is the cross product of the capsule inner segment and - // an edge of the polyhedron + // For each direction that is the cross product of the capsule inner segment and an edge of the polyhedron for (uint e = 0; e < polyhedron->getNbHalfEdges(); e += 2) { // Get an edge from the polyhedron (convert it into the capsule local-space) @@ -326,18 +325,327 @@ bool SATAlgorithm::isMinkowskiFaceCapsuleVsEdge(const Vector3& capsuleSegment, c return capsuleSegment.dot(edgeAdjacentFace1Normal) * capsuleSegment.dot(edgeAdjacentFace2Normal) < decimal(0.0); } -// Test collision between a triangle and a convex mesh -bool SATAlgorithm::testCollisionTriangleVsConvexMesh(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo) const { - - assert(narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::TRIANGLE); - assert(narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::CONVEX_POLYHEDRON); -} - -// Test collision between two convex meshes -bool SATAlgorithm::testCollisionConvexMeshVsConvexMesh(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo) const { +// Test collision between two convex polyhedrons +bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo) const { assert(narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CONVEX_POLYHEDRON); assert(narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::CONVEX_POLYHEDRON); + + const ConvexPolyhedronShape* polyhedron1 = static_cast(narrowPhaseInfo->collisionShape1); + const ConvexPolyhedronShape* polyhedron2 = static_cast(narrowPhaseInfo->collisionShape2); + + const Transform polyhedron1ToPolyhedron2 = narrowPhaseInfo->shape2ToWorldTransform.getInverse() * narrowPhaseInfo->shape1ToWorldTransform; + const Transform polyhedron2ToPolyhedron1 = polyhedron1ToPolyhedron2.getInverse(); + + decimal minPenetrationDepth = DECIMAL_LARGEST; + uint minFaceIndex = 0; + bool isMinPenetrationFaceNormal = false; + bool isMinPenetrationFaceNormalPolyhedron1 = false; + Vector3 separatingEdge1A, separatingEdge1B; + Vector3 separatingEdge2A, separatingEdge2B; + Vector3 minEdgeVsEdgeSeparatingAxisPolyhedron2Space; + + // Test all the face normals of the polyhedron 1 for separating axis + uint faceIndex; + decimal penetrationDepth = testFaceDirectionPolyhedronVsPolyhedron(polyhedron1, polyhedron2, polyhedron1ToPolyhedron2, faceIndex); + if (penetrationDepth <= decimal(0.0)) { + + // We have found a separating axis + return false; + } + if (penetrationDepth < minPenetrationDepth) { + isMinPenetrationFaceNormal = true; + minPenetrationDepth = penetrationDepth; + minFaceIndex = faceIndex; + isMinPenetrationFaceNormalPolyhedron1 = true; + } + + // Test all the face normals of the polyhedron 2 for separating axis + penetrationDepth = testFaceDirectionPolyhedronVsPolyhedron(polyhedron2, polyhedron1, polyhedron2ToPolyhedron1, faceIndex); + if (penetrationDepth <= decimal(0.0)) { + + // We have found a separating axis + return false; + } + if (penetrationDepth < minPenetrationDepth) { + isMinPenetrationFaceNormal = true; + minPenetrationDepth = penetrationDepth; + minFaceIndex = faceIndex; + isMinPenetrationFaceNormalPolyhedron1 = false; + } + + // Test the cross products of edges of polyhedron 1 with edges of polyhedron 2 for separating axis + for (uint i=0; i < polyhedron1->getNbHalfEdges(); i += 2) { + + // Get an edge of polyhedron 1 + HalfEdgeStructure::Edge edge1 = polyhedron1->getHalfEdge(i); + + const Vector3 edge1A = polyhedron1ToPolyhedron2 * polyhedron1->getVertexPosition(edge1.vertexIndex); + const Vector3 edge1B = polyhedron1ToPolyhedron2 * polyhedron1->getVertexPosition(polyhedron1->getHalfEdge(edge1.nextEdgeIndex).vertexIndex); + const Vector3 edge1Direction = edge1B - edge1A; + + for (uint j=0; j < polyhedron2->getNbHalfEdges(); j += 2) { + + // Get an edge of polyhedron 2 + HalfEdgeStructure::Edge edge2 = polyhedron2->getHalfEdge(j); + + const Vector3 edge2A = polyhedron2->getVertexPosition(edge2.vertexIndex); + const Vector3 edge2B = polyhedron2->getVertexPosition(polyhedron2->getHalfEdge(edge2.nextEdgeIndex).vertexIndex); + const Vector3 edge2Direction = edge2B - edge2A; + + // If the two edges build a minkowski face (and the cross product is + // therefore a candidate for separating axis + if (testEdgesBuildMinkowskiFace(polyhedron1, edge1, polyhedron2, edge2, polyhedron1ToPolyhedron2)) { + + Vector3 separatingAxisPolyhedron2Space; + + // Compute the penetration depth + decimal penetrationDepth = computeDistanceBetweenEdges(edge1A, edge2A, polyhedron2->getCentroid(), + edge1Direction, edge2Direction, separatingAxisPolyhedron2Space); + + if (penetrationDepth <= decimal(0.0)) { + + // We have found a separating axis + return false; + } + + if (penetrationDepth < minPenetrationDepth) { + minPenetrationDepth = penetrationDepth; + isMinPenetrationFaceNormalPolyhedron1 = false; + isMinPenetrationFaceNormal = false; + separatingEdge1A = edge1A; + separatingEdge1B = edge1B; + separatingEdge2A = edge2A; + separatingEdge2B = edge2B; + minEdgeVsEdgeSeparatingAxisPolyhedron2Space = separatingAxisPolyhedron2Space; + } + + } + } + } + + assert(minPenetrationDepth > decimal(0.0)); + assert((isMinPenetrationFaceNormal && minFaceIndex >= 0) || !isMinPenetrationFaceNormal); + + // If the separation axis is a face normal + if (isMinPenetrationFaceNormal) { + + const ConvexPolyhedronShape* referencePolyhedron = isMinPenetrationFaceNormalPolyhedron1 ? polyhedron1 : polyhedron2; + const ConvexPolyhedronShape* incidentPolyhedron = isMinPenetrationFaceNormalPolyhedron1 ? polyhedron2 : polyhedron1; + const Transform& referenceToIncidentTransform = isMinPenetrationFaceNormalPolyhedron1 ? polyhedron1ToPolyhedron2 : polyhedron2ToPolyhedron1; + const Transform& incidentToReferenceTransform = isMinPenetrationFaceNormalPolyhedron1 ? polyhedron2ToPolyhedron1 : polyhedron1ToPolyhedron2; + + const Vector3 axisReferenceSpace = referencePolyhedron->getFaceNormal(minFaceIndex); + const Vector3 axisIncidentSpace = referenceToIncidentTransform.getOrientation() * axisReferenceSpace; + + // Compute the world normal + const Vector3 normalWorld = isMinPenetrationFaceNormalPolyhedron1 ? narrowPhaseInfo->shape1ToWorldTransform.getOrientation() * axisReferenceSpace : + -(narrowPhaseInfo->shape2ToWorldTransform.getOrientation() * axisReferenceSpace); + + // Get the reference face + HalfEdgeStructure::Face referenceFace = referencePolyhedron->getFace(minFaceIndex); + + // Find the incident face on the other polyhedron (most anti-parallel face) + uint incidentFaceIndex = findMostAntiParallelFaceOnPolyhedron(incidentPolyhedron, axisIncidentSpace); + + // Get the incident face + HalfEdgeStructure::Face incidentFace = incidentPolyhedron->getFace(incidentFaceIndex); + + std::vector polygonVertices; // Vertices to clip of the incident face + std::vector planesNormals; // Normals of the clipping planes + std::vector planesPoints; // Points on the clipping planes + + // Get all the vertices of the incident face (in the reference local-space) + std::vector::const_iterator it; + for (it = incidentFace.faceVertices.begin(); it != incidentFace.faceVertices.end(); ++it) { + const Vector3 faceVertexIncidentSpace = incidentPolyhedron->getVertexPosition(*it); + polygonVertices.push_back(incidentToReferenceTransform * faceVertexIncidentSpace); + } + + // Get the reference face clipping planes + uint currentEdgeIndex = referenceFace.edgeIndex; + uint firstEdgeIndex = currentEdgeIndex; + do { + + // Get the adjacent edge + HalfEdgeStructure::Edge edge = referencePolyhedron->getHalfEdge(currentEdgeIndex); + + // Get the twin edge + HalfEdgeStructure::Edge twinEdge = referencePolyhedron->getHalfEdge(edge.twinEdgeIndex); + + // Get the adjacent face normal (and negate it to have a clipping plane) + Vector3 faceNormal = -referencePolyhedron->getFaceNormal(twinEdge.faceIndex); + + // Get a vertex of the clipping plane (vertex of the adjacent edge) + Vector3 faceVertex = referencePolyhedron->getVertexPosition(edge.vertexIndex); + + planesNormals.push_back(faceNormal); + planesPoints.push_back(faceVertex); + + // Go to the next adjacent edge of the reference face + currentEdgeIndex = edge.nextEdgeIndex; + + } while (currentEdgeIndex != firstEdgeIndex); + + // Clip the reference faces with the adjacent planes of the reference face + std::vector clipPolygonVertices = clipPolygonWithPlanes(polygonVertices, planesPoints, planesNormals); + + // We only keep the clipped points that are below the reference face + const Vector3 referenceFaceVertex = referencePolyhedron->getVertexPosition(firstEdgeIndex); + std::vector::const_iterator itPoints; + for (itPoints = clipPolygonVertices.begin(); itPoints != clipPolygonVertices.end(); ++itPoints) { + + // If the clip point is bellow the reference face + if (((*itPoints) - referenceFaceVertex).dot(axisReferenceSpace) < decimal(0.0)) { + + // Convert the clip incident polyhedron vertex into the incident polyhedron local-space + const Vector3 contactPointIncidentPolyhedron = referenceToIncidentTransform * (*itPoints); + + // Project the contact point onto the reference face + Vector3 contactPointReferencePolyhedron = (*itPoints) + axisReferenceSpace * minPenetrationDepth; + + // Create a new contact point + contactManifoldInfo.addContactPoint(normalWorld, minPenetrationDepth, + isMinPenetrationFaceNormalPolyhedron1 ? contactPointReferencePolyhedron : contactPointIncidentPolyhedron, + isMinPenetrationFaceNormalPolyhedron1 ? contactPointIncidentPolyhedron : contactPointReferencePolyhedron); + } + } + } + else { // If we have an edge vs edge contact + + // Compute the closest points between the two edges (in the local-space of poylhedron 2) + Vector3 closestPointPolyhedron1Edge, closestPointPolyhedron2Edge; + computeClosestPointBetweenTwoSegments(separatingEdge1A, separatingEdge1B, separatingEdge2A, separatingEdge2B, + closestPointPolyhedron1Edge, closestPointPolyhedron2Edge); + + // Compute the contact point on polyhedron 1 edge in the local-space of polyhedron 1 + const Vector3 closestPointPolyhedron1EdgeLocalSpace = polyhedron2ToPolyhedron1 * closestPointPolyhedron1Edge; + + // Compute the world normal + const Vector3 normalWorld = narrowPhaseInfo->shape2ToWorldTransform.getOrientation() * minEdgeVsEdgeSeparatingAxisPolyhedron2Space; + + // Create the contact point + contactManifoldInfo.addContactPoint(normalWorld, minPenetrationDepth, + closestPointPolyhedron1EdgeLocalSpace, closestPointPolyhedron2Edge); + } + + return true; +} + +// Find and return the index of the polyhedron face with the most anti-parallel face normal given a direction vector +// This is used to find the incident face on a polyhedron of a given reference face of another polyhedron +uint SATAlgorithm::findMostAntiParallelFaceOnPolyhedron(const ConvexPolyhedronShape* polyhedron, const Vector3& direction) const { + + decimal minDotProduct = DECIMAL_LARGEST; + uint mostAntiParallelFace = 0; + + // For each face of the polyhedron + for (uint i=0; i < polyhedron->getNbFaces(); i++) { + + // Get the face normal + decimal dotProduct = polyhedron->getFaceNormal(i).dot(direction); + if (dotProduct < minDotProduct) { + minDotProduct = dotProduct; + mostAntiParallelFace = i; + } + } + + return mostAntiParallelFace; +} + +// Compute and return the distance between the two edges in the direction of the candidate separating axis +decimal SATAlgorithm::computeDistanceBetweenEdges(const Vector3& edge1A, const Vector3& edge2A, const Vector3& polyhedron2Centroid, + const Vector3& edge1Direction, const Vector3& edge2Direction, + Vector3& outSeparatingAxisPolyhedron2Space) const { + + // If the two edges are parallel + if (areParallelVectors(edge1Direction, edge2Direction)) { + + // Return a large penetration depth to skip those edges + return DECIMAL_LARGEST; + } + + // Compute the candidate separating axis (cross product between two polyhedrons edges) + Vector3 axis = edge1Direction.cross(edge2Direction).getUnit(); + + // Make sure the axis direction is going from first to second polyhedron + if (axis.dot(edge2A - polyhedron2Centroid) > decimal(0.0)) { + axis = -axis; + } + + outSeparatingAxisPolyhedron2Space = axis; + + // Compute and return the distance between the edges + return -axis.dot(edge2A - edge1A); +} + +// Test all the normals of a polyhedron for separating axis in the polyhedron vs polyhedron case +decimal SATAlgorithm::testFaceDirectionPolyhedronVsPolyhedron(const ConvexPolyhedronShape* polyhedron1, + const ConvexPolyhedronShape* polyhedron2, + const Transform& polyhedron1ToPolyhedron2, + uint& minFaceIndex) const { + + decimal minPenetrationDepth = DECIMAL_LARGEST; + + // For each face of the first polyhedron + for (uint f = 0; f < polyhedron1->getNbFaces(); f++) { + + HalfEdgeStructure::Face face = polyhedron1->getFace(f); + + // Get the face normal + const Vector3 faceNormal = polyhedron1->getFaceNormal(f); + + // Convert the face normal into the local-space of polyhedron 2 + const Vector3 faceNormalPolyhedron2Space = polyhedron1ToPolyhedron2.getOrientation() * faceNormal; + + // Get the support point of polyhedron 2 in the inverse direction of face normal + const Vector3 supportPoint = polyhedron2->getLocalSupportPointWithoutMargin(-faceNormalPolyhedron2Space, nullptr); + + // Compute the penetration depth + const Vector3 faceVertex = polyhedron1ToPolyhedron2 * polyhedron1->getVertexPosition(face.faceVertices[0]); + decimal penetrationDepth = (faceVertex - supportPoint).dot(faceNormalPolyhedron2Space); + + // If the penetration depth is negative, we have found a separating axis + if (penetrationDepth <= decimal(0.0)) { + return penetrationDepth; + } + + // Check if we have found a new minimum penetration axis + if (penetrationDepth < minPenetrationDepth) { + minPenetrationDepth = penetrationDepth; + minFaceIndex = f; + } + } + + return minPenetrationDepth; +} + + +// Return true if two edges of two polyhedrons build a minkowski face (and can therefore be a separating axis) +bool SATAlgorithm::testEdgesBuildMinkowskiFace(const ConvexPolyhedronShape* polyhedron1, const HalfEdgeStructure::Edge& edge1, + const ConvexPolyhedronShape* polyhedron2, const HalfEdgeStructure::Edge& edge2, + const Transform& polyhedron1ToPolyhedron2) const { + + const Vector3 a = polyhedron1ToPolyhedron2 * polyhedron1->getFaceNormal(edge1.faceIndex); + const Vector3 b = polyhedron1ToPolyhedron2 * polyhedron1->getFaceNormal(polyhedron1->getHalfEdge(edge1.twinEdgeIndex).faceIndex); + + const Vector3 c = polyhedron2->getFaceNormal(edge2.faceIndex); + const Vector3 d = polyhedron2->getFaceNormal(polyhedron2->getHalfEdge(edge2.twinEdgeIndex).faceIndex); + + // Compute b.cross(a) using the edge direction + const Vector3 edge1Vertex1 = polyhedron1->getVertexPosition(edge1.vertexIndex); + const Vector3 edge1Vertex2 = polyhedron1->getVertexPosition(polyhedron1->getHalfEdge(edge1.twinEdgeIndex).vertexIndex); + const Vector3 bCrossA = polyhedron1ToPolyhedron2.getOrientation() * (edge1Vertex2 - edge1Vertex1); + + // Compute d.cross(c) using the edge direction + const Vector3 edge2Vertex1 = polyhedron2->getVertexPosition(edge2.vertexIndex); + const Vector3 edge2Vertex2 = polyhedron2->getVertexPosition(polyhedron2->getHalfEdge(edge2.twinEdgeIndex).vertexIndex); + const Vector3 dCrossC = edge2Vertex2 - edge2Vertex1; + + // Test if the two arcs of the Gauss Map intersect (therefore forming a minkowski face) + // Note that we negate the normals of the second polyhedron because we are looking at the + // Gauss map of the minkowski difference of the polyhedrons + return testGaussMapArcsIntersect(a, b, -c, -d, bCrossA, dCrossC); } @@ -346,10 +654,8 @@ bool SATAlgorithm::testCollisionConvexMeshVsConvexMesh(const NarrowPhaseInfo* na /// and edge between faces with normal C and D on second polygon create a face on the Minkowski /// sum of both polygons. If this is the case, it means that the cross product of both edges /// might be a separating axis. -bool SATAlgorithm::testGaussMapArcsIntersect(const Vector3& a, const Vector3& b, - const Vector3& c, const Vector3& d) const { - const Vector3 bCrossA = b.cross(a); - const Vector3 dCrossC = d.cross(c); +bool SATAlgorithm::testGaussMapArcsIntersect(const Vector3& a, const Vector3& b, const Vector3& c, const Vector3& d, + const Vector3& bCrossA, const Vector3& dCrossC) const { const decimal cba = c.dot(bCrossA); const decimal dba = d.dot(bCrossA); diff --git a/src/collision/narrowphase/SAT/SATAlgorithm.h b/src/collision/narrowphase/SAT/SATAlgorithm.h index 42e380f2..ae5da16e 100644 --- a/src/collision/narrowphase/SAT/SATAlgorithm.h +++ b/src/collision/narrowphase/SAT/SATAlgorithm.h @@ -43,9 +43,23 @@ class SATAlgorithm { // -------------------- Methods -------------------- // + /// Return true if two edges of two polyhedrons build a minkowski face (and can therefore be a separating axis) + bool testEdgesBuildMinkowskiFace(const ConvexPolyhedronShape* polyhedron1, const HalfEdgeStructure::Edge& edge1, + const ConvexPolyhedronShape* polyhedron2, const HalfEdgeStructure::Edge& edge2, + const Transform& polyhedron1ToPolyhedron2) const; + /// Return true if the arcs AB and CD on the Gauss Map intersect bool testGaussMapArcsIntersect(const Vector3& a, const Vector3& b, - const Vector3& c, const Vector3& d) const; + const Vector3& c, const Vector3& d, + const Vector3& bCrossA, const Vector3& dCrossC) const; + + // Find and return the index of the polyhedron face with the most anti-parallel face normal given a direction vector + uint findMostAntiParallelFaceOnPolyhedron(const ConvexPolyhedronShape* polyhedron, const Vector3& direction) const; + + /// Compute and return the distance between the two edges in the direction of the candidate separating axis + decimal computeDistanceBetweenEdges(const Vector3& edge1A, const Vector3& edge2A, const Vector3& polyhedron2Centroid, + const Vector3& edge1Direction, const Vector3& edge2Direction, + Vector3& outSeparatingAxis) const; public : @@ -80,12 +94,12 @@ class SATAlgorithm { bool isMinkowskiFaceCapsuleVsEdge(const Vector3& capsuleSegment, const Vector3& edgeAdjacentFace1Normal, const Vector3& edgeAdjacentFace2Normal) const; - /// Test collision between a triangle and a convex mesh - bool testCollisionTriangleVsConvexMesh(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo) const; - /// Test collision between two convex meshes - bool testCollisionConvexMeshVsConvexMesh(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo) const; + bool testCollisionConvexPolyhedronVsConvexPolyhedron(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo) const; + /// Test all the normals of a polyhedron for separating axis in the polyhedron vs polyhedron case + decimal testFaceDirectionPolyhedronVsPolyhedron(const ConvexPolyhedronShape* polyhedron1, const ConvexPolyhedronShape* polyhedron2, + const Transform& polyhedron1ToPolyhedron2, uint& minFaceIndex) const; }; } diff --git a/src/mathematics/mathematics_functions.cpp b/src/mathematics/mathematics_functions.cpp index 36ba113a..e01c749a 100644 --- a/src/mathematics/mathematics_functions.cpp +++ b/src/mathematics/mathematics_functions.cpp @@ -194,7 +194,7 @@ decimal reactphysics3d::computePlaneSegmentIntersection(const Vector3& segA, con } // Compute the distance between a point "point" and a line given by the points "linePointA" and "linePointB" -decimal reactphysics3d::computeDistancePointToLineDistance(const Vector3& linePointA, const Vector3& linePointB, const Vector3& point) { +decimal reactphysics3d::computePointToLineDistance(const Vector3& linePointA, const Vector3& linePointB, const Vector3& point) { decimal distAB = (linePointB - linePointA).length(); diff --git a/src/mathematics/mathematics_functions.h b/src/mathematics/mathematics_functions.h index 44ce9cf4..c695f342 100644 --- a/src/mathematics/mathematics_functions.h +++ b/src/mathematics/mathematics_functions.h @@ -98,7 +98,7 @@ void computeBarycentricCoordinatesInTriangle(const Vector3& a, const Vector3& b, 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); +decimal computePointToLineDistance(const Vector3& linePointA, const Vector3& linePointB, const Vector3& point); /// Clip a segment against multiple planes and return the clipped segment vertices std::vector clipSegmentWithPlanes(const Vector3& segA, const Vector3& segB, diff --git a/test/tests/mathematics/TestMathematicsFunctions.h b/test/tests/mathematics/TestMathematicsFunctions.h index 04a81a69..b45415ee 100644 --- a/test/tests/mathematics/TestMathematicsFunctions.h +++ b/test/tests/mathematics/TestMathematicsFunctions.h @@ -162,13 +162,12 @@ class TestMathematicsFunctions : public Test { 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)); - + // Test computePointToLineDistance() + test(approxEqual(computePointToLineDistance(Vector3(6, 0, 0), Vector3(14, 0, 0), Vector3(5, 3, 0)), 3.0, 0.000001)); + test(approxEqual(computePointToLineDistance(Vector3(6, -5, 0), Vector3(10, -5, 0), Vector3(4, 3, 0)), 8.0, 0.000001)); + test(approxEqual(computePointToLineDistance(Vector3(6, -5, 0), Vector3(10, -5, 0), Vector3(-43, 254, 0)), 259.0, 0.000001)); + test(approxEqual(computePointToLineDistance(Vector3(6, -5, 8), Vector3(10, -5, -5), Vector3(6, -5, 8)), 0.0, 0.000001)); + test(approxEqual(computePointToLineDistance(Vector3(6, -5, 8), Vector3(10, -5, -5), Vector3(10, -5, -5)), 0.0, 0.000001)); // Test clipSegmentWithPlanes() std::vector segmentVertices;