diff --git a/include/reactphysics3d/collision/narrowphase/GJK/VoronoiSimplex.h b/include/reactphysics3d/collision/narrowphase/GJK/VoronoiSimplex.h index fb9ddc8d..785d4edf 100644 --- a/include/reactphysics3d/collision/narrowphase/GJK/VoronoiSimplex.h +++ b/include/reactphysics3d/collision/narrowphase/GJK/VoronoiSimplex.h @@ -28,6 +28,7 @@ // Libraries #include +#include /// ReactPhysics3D namespace namespace reactphysics3d { diff --git a/include/reactphysics3d/mathematics/Matrix2x2.h b/include/reactphysics3d/mathematics/Matrix2x2.h index 5f69e982..66c651f6 100644 --- a/include/reactphysics3d/mathematics/Matrix2x2.h +++ b/include/reactphysics3d/mathematics/Matrix2x2.h @@ -242,8 +242,8 @@ RP3D_FORCE_INLINE Matrix2x2 Matrix2x2::zero() { // Return the matrix with absolute values RP3D_FORCE_INLINE Matrix2x2 Matrix2x2::getAbsoluteMatrix() const { - return Matrix2x2(fabs(mRows[0][0]), fabs(mRows[0][1]), - fabs(mRows[1][0]), fabs(mRows[1][1])); + return Matrix2x2(std::abs(mRows[0][0]), std::abs(mRows[0][1]), + std::abs(mRows[1][0]), std::abs(mRows[1][1])); } // Overloaded operator for addition diff --git a/include/reactphysics3d/mathematics/Matrix3x3.h b/include/reactphysics3d/mathematics/Matrix3x3.h index 575adfc4..0a2661a0 100644 --- a/include/reactphysics3d/mathematics/Matrix3x3.h +++ b/include/reactphysics3d/mathematics/Matrix3x3.h @@ -261,9 +261,9 @@ RP3D_FORCE_INLINE Matrix3x3 Matrix3x3::computeSkewSymmetricMatrixForCrossProduct // Return the matrix with absolute values RP3D_FORCE_INLINE Matrix3x3 Matrix3x3::getAbsoluteMatrix() const { - return Matrix3x3(std::fabs(mRows[0][0]), std::fabs(mRows[0][1]), std::fabs(mRows[0][2]), - std::fabs(mRows[1][0]), std::fabs(mRows[1][1]), std::fabs(mRows[1][2]), - std::fabs(mRows[2][0]), std::fabs(mRows[2][1]), std::fabs(mRows[2][2])); + return Matrix3x3(std::abs(mRows[0][0]), std::abs(mRows[0][1]), std::abs(mRows[0][2]), + std::abs(mRows[1][0]), std::abs(mRows[1][1]), std::abs(mRows[1][2]), + std::abs(mRows[2][0]), std::abs(mRows[2][1]), std::abs(mRows[2][2])); } // Overloaded operator for addition diff --git a/include/reactphysics3d/mathematics/Vector2.h b/include/reactphysics3d/mathematics/Vector2.h index aee3214d..a19598a6 100644 --- a/include/reactphysics3d/mathematics/Vector2.h +++ b/include/reactphysics3d/mathematics/Vector2.h @@ -148,6 +148,9 @@ struct Vector2 { /// Return the zero vector static Vector2 zero(); + /// Function to test if two vectors are (almost) equal + static bool approxEqual(const Vector2& vec1, const Vector2& vec2, decimal epsilon = MACHINE_EPSILON); + // -------------------- Friends -------------------- // friend Vector2 operator+(const Vector2& vector1, const Vector2& vector2); @@ -230,7 +233,7 @@ RP3D_FORCE_INLINE int Vector2::getMaxAxis() const { // Return true if the vector is unit and false otherwise RP3D_FORCE_INLINE bool Vector2::isUnit() const { - return approxEqual(lengthSquare(), 1.0); + return reactphysics3d::approxEqual(lengthSquare(), decimal(1.0)); } // Return true if the values are not NAN OR INF @@ -240,7 +243,7 @@ RP3D_FORCE_INLINE bool Vector2::isFinite() const { // Return true if the vector is the zero vector RP3D_FORCE_INLINE bool Vector2::isZero() const { - return approxEqual(lengthSquare(), 0.0); + return reactphysics3d::approxEqual(lengthSquare(), decimal(0.0)); } // Overloaded operator for the equality condition @@ -371,6 +374,11 @@ RP3D_FORCE_INLINE Vector2 Vector2::zero() { return Vector2(0, 0); } +// Function to test if two vectors are (almost) equal +RP3D_FORCE_INLINE bool approxEqual(const Vector2& vec1, const Vector2& vec2, decimal epsilon) { + return approxEqual(vec1.x, vec2.x, epsilon) && approxEqual(vec1.y, vec2.y, epsilon); +} + } #endif diff --git a/include/reactphysics3d/mathematics/Vector3.h b/include/reactphysics3d/mathematics/Vector3.h index b1adf487..a6dae046 100644 --- a/include/reactphysics3d/mathematics/Vector3.h +++ b/include/reactphysics3d/mathematics/Vector3.h @@ -28,8 +28,9 @@ // Libraries #include -#include +#include #include +#include #include /// ReactPhysics3D namespace @@ -161,6 +162,9 @@ struct Vector3 { /// Return the zero vector static Vector3 zero(); + /// Function to test if two vectors are (almost) equal + static bool approxEqual(const Vector3& vec1, const Vector3& vec2, decimal epsilon = MACHINE_EPSILON); + // -------------------- Friends -------------------- // friend Vector3 operator+(const Vector3& vector1, const Vector3& vector2); @@ -252,7 +256,7 @@ RP3D_FORCE_INLINE int Vector3::getMaxAxis() const { // Return true if the vector is unit and false otherwise RP3D_FORCE_INLINE bool Vector3::isUnit() const { - return approxEqual(lengthSquare(), 1.0); + return reactphysics3d::approxEqual(lengthSquare(), decimal(1.0)); } // Return true if the values are not NAN OR INF @@ -262,7 +266,7 @@ RP3D_FORCE_INLINE bool Vector3::isFinite() const { // Return true if the vector is the zero vector RP3D_FORCE_INLINE bool Vector3::isZero() const { - return approxEqual(lengthSquare(), 0.0); + return reactphysics3d::approxEqual(lengthSquare(), decimal(0.0)); } // Overloaded operator for the equality condition @@ -411,6 +415,12 @@ RP3D_FORCE_INLINE Vector3 Vector3::zero() { return Vector3(0, 0, 0); } +// Function to test if two vectors are (almost) equal +RP3D_FORCE_INLINE bool approxEqual(const Vector3& vec1, const Vector3& vec2, decimal epsilon) { + return approxEqual(vec1.x, vec2.x, epsilon) && approxEqual(vec1.y, vec2.y, epsilon) && + approxEqual(vec1.z, vec2.z, epsilon); +} + } #endif diff --git a/include/reactphysics3d/mathematics/mathematics_common.h b/include/reactphysics3d/mathematics/mathematics_common.h new file mode 100644 index 00000000..8c1ef6f5 --- /dev/null +++ b/include/reactphysics3d/mathematics/mathematics_common.h @@ -0,0 +1,50 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://www.reactphysics3d.com * +* Copyright (c) 2010-2020 Daniel Chappuis * +********************************************************************************* +* * +* This software is provided 'as-is', without any express or implied warranty. * +* In no event will the authors be held liable for any damages arising from the * +* use of this software. * +* * +* Permission is granted to anyone to use this software for any purpose, * +* including commercial applications, and to alter it and redistribute it * +* freely, subject to the following restrictions: * +* * +* 1. The origin of this software must not be misrepresented; you must not claim * +* that you wrote the original software. If you use this software in a * +* product, an acknowledgment in the product documentation would be * +* appreciated but is not required. * +* * +* 2. Altered source versions must be plainly marked as such, and must not be * +* misrepresented as being the original software. * +* * +* 3. This notice may not be removed or altered from any source distribution. * +* * +********************************************************************************/ + +#ifndef REACTPHYSICS3D_MATHEMATICS_COMMON_H +#define REACTPHYSICS3D_MATHEMATICS_COMMON_H + +// Libraries +#include +#include +#include +#include + +/// ReactPhysics3D namespace +namespace reactphysics3d { + +// ---------- 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] +RP3D_FORCE_INLINE bool approxEqual(decimal a, decimal b, decimal epsilon = MACHINE_EPSILON) { + return (std::abs(a - b) < epsilon); +} + + +} + + +#endif diff --git a/include/reactphysics3d/mathematics/mathematics_functions.h b/include/reactphysics3d/mathematics/mathematics_functions.h index 2f1aa0d5..fb333658 100755 --- a/include/reactphysics3d/mathematics/mathematics_functions.h +++ b/include/reactphysics3d/mathematics/mathematics_functions.h @@ -29,6 +29,7 @@ // Libraries #include #include +#include #include #include #include @@ -42,18 +43,6 @@ struct Vector2; // ---------- Mathematics functions ---------- // -/// Function to test if two real numbers are (almost) equal -/// We test if two numbers a and b are such that (a-b) are in [-EPSILON; EPSILON] -RP3D_FORCE_INLINE bool approxEqual(decimal a, decimal b, decimal epsilon = MACHINE_EPSILON) { - return (std::fabs(a - b) < epsilon); -} - -/// Function to test if two vectors are (almost) equal -bool approxEqual(const Vector3& vec1, const Vector3& vec2, decimal epsilon = MACHINE_EPSILON); - -/// Function to test if two vectors are (almost) equal -bool approxEqual(const Vector2& vec1, const Vector2& vec2, decimal epsilon = MACHINE_EPSILON); - /// Function that returns the result of the "value" clamped by /// two others values "lowerLimit" and "upperLimit" RP3D_FORCE_INLINE int clamp(int value, int lowerLimit, int upperLimit) { @@ -83,48 +72,347 @@ RP3D_FORCE_INLINE bool sameSign(decimal a, decimal b) { return a * b >= decimal(0.0); } -/// Return true if two vectors are parallel -bool areParallelVectors(const Vector3& vector1, const Vector3& vector2); +// Return true if two vectors are parallel +RP3D_FORCE_INLINE bool areParallelVectors(const Vector3& vector1, const Vector3& vector2) { + return vector1.cross(vector2).lengthSquare() < decimal(0.00001); +} -/// Return true if two vectors are orthogonal -bool areOrthogonalVectors(const Vector3& vector1, const Vector3& vector2); -/// Clamp a vector such that it is no longer than a given maximum length -Vector3 clamp(const Vector3& vector, decimal maxLength); +// Return true if two vectors are orthogonal +RP3D_FORCE_INLINE bool areOrthogonalVectors(const Vector3& vector1, const Vector3& vector2) { + return std::abs(vector1.dot(vector2)) < decimal(0.001); +} + + +// Clamp a vector such that it is no longer than a given maximum length +RP3D_FORCE_INLINE Vector3 clamp(const Vector3& vector, decimal maxLength) { + if (vector.lengthSquare() > maxLength * maxLength) { + return vector.getUnit() * maxLength; + } + return vector; +} // Compute and return a point on segment from "segPointA" and "segPointB" that is closest to point "pointC" -Vector3 computeClosestPointOnSegment(const Vector3& segPointA, const Vector3& segPointB, const Vector3& pointC); +RP3D_FORCE_INLINE Vector3 computeClosestPointOnSegment(const Vector3& segPointA, const Vector3& segPointB, const Vector3& pointC) { + + const Vector3 ab = segPointB - segPointA; + + decimal abLengthSquare = ab.lengthSquare(); + + // If the segment has almost zero length + if (abLengthSquare < MACHINE_EPSILON) { + + // Return one end-point of the segment as the closest point + return segPointA; + } + + // Project point C onto "AB" line + decimal t = (pointC - segPointA).dot(ab) / abLengthSquare; + + // If projected point onto the line is outside the segment, clamp it to the segment + if (t < decimal(0.0)) t = decimal(0.0); + if (t > decimal(1.0)) t = decimal(1.0); + + // Return the closest point on the segment + return segPointA + t * ab; +} // Compute the closest points between two segments -void computeClosestPointBetweenTwoSegments(const Vector3& seg1PointA, const Vector3& seg1PointB, - const Vector3& seg2PointA, const Vector3& seg2PointB, - Vector3& closestPointSeg1, Vector3& closestPointSeg2); +// This method uses the technique described in the book Real-Time +// collision detection by Christer Ericson. +RP3D_FORCE_INLINE void computeClosestPointBetweenTwoSegments(const Vector3& seg1PointA, const Vector3& seg1PointB, + const Vector3& seg2PointA, const Vector3& seg2PointB, + Vector3& closestPointSeg1, Vector3& closestPointSeg2) { -/// Compute the barycentric coordinates u, v, w of a point p inside the triangle (a, b, c) -void computeBarycentricCoordinatesInTriangle(const Vector3& a, const Vector3& b, const Vector3& c, - const Vector3& p, decimal& u, decimal& v, decimal& w); + const Vector3 d1 = seg1PointB - seg1PointA; + const Vector3 d2 = seg2PointB - seg2PointA; + const Vector3 r = seg1PointA - seg2PointA; + decimal a = d1.lengthSquare(); + decimal e = d2.lengthSquare(); + decimal f = d2.dot(r); + decimal s, t; -/// Compute the intersection between a plane and a segment -decimal computePlaneSegmentIntersection(const Vector3& segA, const Vector3& segB, const decimal planeD, const Vector3& planeNormal); + // If both segments degenerate into points + if (a <= MACHINE_EPSILON && e <= MACHINE_EPSILON) { -/// Compute the distance between a point and a line -decimal computePointToLineDistance(const Vector3& linePointA, const Vector3& linePointB, const Vector3& point); + closestPointSeg1 = seg1PointA; + closestPointSeg2 = seg2PointA; + return; + } + if (a <= MACHINE_EPSILON) { // If first segment degenerates into a point -/// Clip a segment against multiple planes and return the clipped segment vertices -Array clipSegmentWithPlanes(const Vector3& segA, const Vector3& segB, + 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 barycentric coordinates u, v, w of a point p inside the triangle (a, b, c) +// This method uses the technique described in the book Real-Time collision detection by +// Christer Ericson. +RP3D_FORCE_INLINE void computeBarycentricCoordinatesInTriangle(const Vector3& a, const Vector3& b, const Vector3& c, + const Vector3& p, decimal& u, decimal& v, decimal& w) { + const Vector3 v0 = b - a; + const Vector3 v1 = c - a; + const Vector3 v2 = p - a; + + const decimal d00 = v0.dot(v0); + const decimal d01 = v0.dot(v1); + const decimal d11 = v1.dot(v1); + const decimal d20 = v2.dot(v0); + const decimal d21 = v2.dot(v1); + + const decimal denom = d00 * d11 - d01 * d01; + v = (d11 * d20 - d01 * d21) / denom; + w = (d00 * d21 - d01 * d20) / denom; + u = decimal(1.0) - v - w; +} + +// Compute the intersection between a plane and a segment +// Let the plane define by the equation planeNormal.dot(X) = planeD with X a point on the plane and "planeNormal" the plane normal. This method +// computes the intersection P between the plane and the segment (segA, segB). The method returns the value "t" such +// that P = segA + t * (segB - segA). Note that it only returns a value in [0, 1] if there is an intersection. Otherwise, +// there is no intersection between the plane and the segment. +RP3D_FORCE_INLINE decimal computePlaneSegmentIntersection(const Vector3& segA, const Vector3& segB, const decimal planeD, const Vector3& planeNormal) { + + const decimal parallelEpsilon = decimal(0.0001); + decimal t = decimal(-1); + + const decimal nDotAB = planeNormal.dot(segB - segA); + + // If the segment is not parallel to the plane + if (std::abs(nDotAB) > parallelEpsilon) { + t = (planeD - planeNormal.dot(segA)) / nDotAB; + } + + return t; +} + +// Compute the distance between a point "point" and a line given by the points "linePointA" and "linePointB" +RP3D_FORCE_INLINE decimal computePointToLineDistance(const Vector3& linePointA, const Vector3& linePointB, const Vector3& point) { + + decimal distAB = (linePointB - linePointA).length(); + + if (distAB < MACHINE_EPSILON) { + return (point - linePointA).length(); + } + + return ((point - linePointA).cross(point - linePointB)).length() / distAB; +} + + +// Clip a segment against multiple planes and return the clipped segment vertices +// This method implements the Sutherland–Hodgman clipping algorithm +RP3D_FORCE_INLINE Array clipSegmentWithPlanes(const Vector3& segA, const Vector3& segB, const Array& planesPoints, const Array& planesNormals, - MemoryAllocator& allocator); + MemoryAllocator& allocator) { + assert(planesPoints.size() == planesNormals.size()); -/// Clip a polygon against multiple planes and return the clipped polygon vertices -Array clipPolygonWithPlanes(const Array& polygonVertices, const Array& planesPoints, - const Array& planesNormals, MemoryAllocator& allocator); + Array inputVertices(allocator, 2); + Array outputVertices(allocator, 2); -/// 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); + inputVertices.add(segA); + inputVertices.add(segB); -/// 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); + // For each clipping plane + const uint32 nbPlanesPoints = planesPoints.size(); + for (uint32 p=0; p < nbPlanesPoints; p++) { + + // If there is no more vertices, stop + if (inputVertices.size() == 0) return inputVertices; + + assert(inputVertices.size() == 2); + + outputVertices.clear(); + + Vector3& v1 = inputVertices[0]; + Vector3& v2 = inputVertices[1]; + + decimal v1DotN = (v1 - planesPoints[p]).dot(planesNormals[p]); + decimal v2DotN = (v2 - planesPoints[p]).dot(planesNormals[p]); + + // If the second vertex is in front of the clippling plane + if (v2DotN >= decimal(0.0)) { + + // If the first vertex is not in front of the clippling plane + if (v1DotN < decimal(0.0)) { + + // The second point we keep is the intersection between the segment v1, v2 and the clipping plane + decimal t = computePlaneSegmentIntersection(v1, v2, planesNormals[p].dot(planesPoints[p]), planesNormals[p]); + + if (t >= decimal(0) && t <= decimal(1.0)) { + outputVertices.add(v1 + t * (v2 - v1)); + } + else { + outputVertices.add(v2); + } + } + else { + outputVertices.add(v1); + } + + // Add the second vertex + outputVertices.add(v2); + } + else { // If the second vertex is behind the clipping plane + + // If the first vertex is in front of the clippling plane + if (v1DotN >= decimal(0.0)) { + + outputVertices.add(v1); + + // The first point we keep is the intersection between the segment v1, v2 and the clipping plane + decimal t = computePlaneSegmentIntersection(v1, v2, -planesNormals[p].dot(planesPoints[p]), -planesNormals[p]); + + if (t >= decimal(0.0) && t <= decimal(1.0)) { + outputVertices.add(v1 + t * (v2 - v1)); + } + } + } + + inputVertices = outputVertices; + } + + return outputVertices; +} + +// Clip a polygon against multiple planes and return the clipped polygon vertices +// This method implements the Sutherland–Hodgman clipping algorithm +RP3D_FORCE_INLINE void clipPolygonWithPlanes(const Array& polygonVertices, const Array& planesPoints, + const Array& planesNormals, Array& outClippedPolygonVertices, + MemoryAllocator& allocator) { + + assert(planesPoints.size() == planesNormals.size()); + + const uint32 nbMaxElements = polygonVertices.size() + planesPoints.size(); + Array inputVertices(allocator, nbMaxElements); + + outClippedPolygonVertices.reserve(nbMaxElements); + + inputVertices.addRange(polygonVertices); + + // For each clipping plane + const uint32 nbPlanesPoints = planesPoints.size(); + for (uint32 p=0; p < nbPlanesPoints; p++) { + + outClippedPolygonVertices.clear(); + + const uint32 nbInputVertices = inputVertices.size(); + uint32 vStart = nbInputVertices - 1; + + // For each edge of the polygon + for (uint vEnd = 0; vEnd= decimal(0.0)) { + + // If the first vertex is not in front of the clippling plane + if (v1DotN < decimal(0.0)) { + + // The second point we keep is the intersection between the segment v1, v2 and the clipping plane + decimal t = computePlaneSegmentIntersection(v1, v2, planesNormals[p].dot(planesPoints[p]), planesNormals[p]); + + if (t >= decimal(0) && t <= decimal(1.0)) { + outClippedPolygonVertices.add(v1 + t * (v2 - v1)); + } + else { + outClippedPolygonVertices.add(v2); + } + } + + // Add the second vertex + outClippedPolygonVertices.add(v2); + } + else { // If the second vertex is behind the clipping plane + + // If the first vertex is in front of the clippling plane + if (v1DotN >= decimal(0.0)) { + + // The first point we keep is the intersection between the segment v1, v2 and the clipping plane + decimal t = computePlaneSegmentIntersection(v1, v2, -planesNormals[p].dot(planesPoints[p]), -planesNormals[p]); + + if (t >= decimal(0.0) && t <= decimal(1.0)) { + outClippedPolygonVertices.add(v1 + t * (v2 - v1)); + } + else { + outClippedPolygonVertices.add(v1); + } + } + } + + vStart = vEnd; + } + + inputVertices = outClippedPolygonVertices; + } +} + +// Project a point onto a plane that is given by a point and its unit length normal +RP3D_FORCE_INLINE Vector3 projectPointOntoPlane(const Vector3& point, const Vector3& unitPlaneNormal, const Vector3& planePoint) { + return point - unitPlaneNormal.dot(point - planePoint) * unitPlaneNormal; +} + +// Return the distance between a point and a plane (the plane normal must be normalized) +RP3D_FORCE_INLINE decimal computePointToPlaneDistance(const Vector3& point, const Vector3& planeNormal, const Vector3& planePoint) { + return planeNormal.dot(point - planePoint); +} /// Return true if a number is a power of two RP3D_FORCE_INLINE bool isPowerOfTwo(uint32 number) { diff --git a/include/reactphysics3d/systems/ContactSolverSystem.h b/include/reactphysics3d/systems/ContactSolverSystem.h index 9c5a87db..213e9baa 100644 --- a/include/reactphysics3d/systems/ContactSolverSystem.h +++ b/include/reactphysics3d/systems/ContactSolverSystem.h @@ -30,6 +30,7 @@ #include #include #include +#include #include /// ReactPhysics3D namespace diff --git a/src/collision/narrowphase/SAT/SATAlgorithm.cpp b/src/collision/narrowphase/SAT/SATAlgorithm.cpp index bf175b01..3e1daa4c 100644 --- a/src/collision/narrowphase/SAT/SATAlgorithm.cpp +++ b/src/collision/narrowphase/SAT/SATAlgorithm.cpp @@ -959,7 +959,8 @@ bool SATAlgorithm::computePolyhedronVsPolyhedronFaceContactPoints(bool isMinPene assert(planesNormals.size() == planesPoints.size()); // Clip the reference faces with the adjacent planes of the reference face - Array clipPolygonVertices = clipPolygonWithPlanes(polygonVertices, planesPoints, planesNormals, mMemoryAllocator); + Array clipPolygonVertices(mMemoryAllocator); + clipPolygonWithPlanes(polygonVertices, planesPoints, planesNormals, clipPolygonVertices, mMemoryAllocator); // We only keep the clipped points that are below the reference face const Vector3 referenceFaceVertex = referencePolyhedron->getVertexPosition(referencePolyhedron->getHalfEdge(firstEdgeIndex).vertexIndex); diff --git a/src/mathematics/Vector3.cpp b/src/mathematics/Vector3.cpp index 5509e853..b6bfca54 100644 --- a/src/mathematics/Vector3.cpp +++ b/src/mathematics/Vector3.cpp @@ -48,7 +48,7 @@ Vector3 Vector3::getOneUnitOrthogonalVector() const { assert(length() > MACHINE_EPSILON); // Get the minimum element of the vector - Vector3 vectorAbs(std::fabs(x), std::fabs(y), std::fabs(z)); + Vector3 vectorAbs(std::abs(x), std::abs(y), std::abs(z)); int minElement = vectorAbs.getMinAxis(); if (minElement == 0) { diff --git a/src/mathematics/mathematics_functions.cpp b/src/mathematics/mathematics_functions.cpp deleted file mode 100755 index 57995192..00000000 --- a/src/mathematics/mathematics_functions.cpp +++ /dev/null @@ -1,383 +0,0 @@ -/******************************************************************************** -* ReactPhysics3D physics library, http://www.reactphysics3d.com * -* Copyright (c) 2010-2020 Daniel Chappuis * -********************************************************************************* -* * -* This software is provided 'as-is', without any express or implied warranty. * -* In no event will the authors be held liable for any damages arising from the * -* use of this software. * -* * -* Permission is granted to anyone to use this software for any purpose, * -* including commercial applications, and to alter it and redistribute it * -* freely, subject to the following restrictions: * -* * -* 1. The origin of this software must not be misrepresented; you must not claim * -* that you wrote the original software. If you use this software in a * -* product, an acknowledgment in the product documentation would be * -* appreciated but is not required. * -* * -* 2. Altered source versions must be plainly marked as such, and must not be * -* misrepresented as being the original software. * -* * -* 3. This notice may not be removed or altered from any source distribution. * -* * -********************************************************************************/ - -// Libraries -#include -#include -#include -#include - -using namespace reactphysics3d; - - -// Function to test if two vectors are (almost) equal -bool reactphysics3d::approxEqual(const Vector3& vec1, const Vector3& vec2, decimal epsilon) { - return approxEqual(vec1.x, vec2.x, epsilon) && approxEqual(vec1.y, vec2.y, epsilon) && - approxEqual(vec1.z, vec2.z, epsilon); -} - -// Function to test if two vectors are (almost) equal -bool reactphysics3d::approxEqual(const Vector2& vec1, const Vector2& vec2, decimal epsilon) { - return approxEqual(vec1.x, vec2.x, epsilon) && approxEqual(vec1.y, vec2.y, epsilon); -} - -// Compute the barycentric coordinates u, v, w of a point p inside the triangle (a, b, c) -// This method uses the technique described in the book Real-Time collision detection by -// Christer Ericson. -void reactphysics3d::computeBarycentricCoordinatesInTriangle(const Vector3& a, const Vector3& b, const Vector3& c, - const Vector3& p, decimal& u, decimal& v, decimal& w) { - const Vector3 v0 = b - a; - const Vector3 v1 = c - a; - const Vector3 v2 = p - a; - - decimal d00 = v0.dot(v0); - decimal d01 = v0.dot(v1); - decimal d11 = v1.dot(v1); - decimal d20 = v2.dot(v0); - decimal d21 = v2.dot(v1); - - decimal denom = d00 * d11 - d01 * d01; - v = (d11 * d20 - d01 * d21) / denom; - w = (d00 * d21 - d01 * d20) / denom; - u = decimal(1.0) - v - w; -} - -// Clamp a vector such that it is no longer than a given maximum length -Vector3 reactphysics3d::clamp(const Vector3& vector, decimal maxLength) { - if (vector.lengthSquare() > maxLength * maxLength) { - return vector.getUnit() * maxLength; - } - return vector; -} - -// Return true if two vectors are parallel -bool reactphysics3d::areParallelVectors(const Vector3& vector1, const Vector3& vector2) { - return vector1.cross(vector2).lengthSquare() < decimal(0.00001); -} - -// Return true if two vectors are orthogonal -bool reactphysics3d::areOrthogonalVectors(const Vector3& vector1, const Vector3& vector2) { - return std::abs(vector1.dot(vector2)) < decimal(0.001); -} - -// Compute and return a point on segment from "segPointA" and "segPointB" that is closest to point "pointC" -Vector3 reactphysics3d::computeClosestPointOnSegment(const Vector3& segPointA, const Vector3& segPointB, const Vector3& pointC) { - - const Vector3 ab = segPointB - segPointA; - - decimal abLengthSquare = ab.lengthSquare(); - - // If the segment has almost zero length - if (abLengthSquare < MACHINE_EPSILON) { - - // Return one end-point of the segment as the closest point - return segPointA; - } - - // Project point C onto "AB" line - decimal t = (pointC - segPointA).dot(ab) / abLengthSquare; - - // If projected point onto the line is outside the segment, clamp it to the segment - if (t < decimal(0.0)) t = decimal(0.0); - if (t > decimal(1.0)) t = decimal(1.0); - - // Return the closest point on the segment - return segPointA + t * ab; -} - -// Compute the closest points between two segments -// This method uses the technique described in the book Real-Time -// collision detection by Christer Ericson. -void reactphysics3d::computeClosestPointBetweenTwoSegments(const Vector3& seg1PointA, const Vector3& seg1PointB, - const Vector3& seg2PointA, const Vector3& seg2PointB, - Vector3& closestPointSeg1, Vector3& closestPointSeg2) { - - const Vector3 d1 = seg1PointB - seg1PointA; - const Vector3 d2 = seg2PointB - seg2PointA; - const Vector3 r = seg1PointA - seg2PointA; - decimal a = d1.lengthSquare(); - decimal e = d2.lengthSquare(); - decimal f = d2.dot(r); - decimal s, t; - - // If both segments degenerate into points - if (a <= MACHINE_EPSILON && e <= MACHINE_EPSILON) { - - closestPointSeg1 = seg1PointA; - closestPointSeg2 = seg2PointA; - return; - } - if (a <= MACHINE_EPSILON) { // If first segment degenerates into a point - - s = decimal(0.0); - - // Compute the closest point on second segment - t = clamp(f / e, decimal(0.0), decimal(1.0)); - } - else { - - decimal c = d1.dot(r); - - // If the second segment degenerates into a point - if (e <= MACHINE_EPSILON) { - - t = decimal(0.0); - s = clamp(-c / a, decimal(0.0), decimal(1.0)); - } - else { - - decimal b = d1.dot(d2); - decimal denom = a * e - b * b; - - // If the segments are not parallel - if (denom != decimal(0.0)) { - - // Compute the closest point on line 1 to line 2 and - // clamp to first segment. - s = clamp((b * f - c * e) / denom, decimal(0.0), decimal(1.0)); - } - else { - - // Pick an arbitrary point on first segment - s = decimal(0.0); - } - - // Compute the point on line 2 closest to the closest point - // we have just found - t = (b * s + f) / e; - - // If this closest point is inside second segment (t in [0, 1]), we are done. - // Otherwise, we clamp the point to the second segment and compute again the - // closest point on segment 1 - if (t < decimal(0.0)) { - t = decimal(0.0); - s = clamp(-c / a, decimal(0.0), decimal(1.0)); - } - else if (t > decimal(1.0)) { - t = decimal(1.0); - s = clamp((b - c) / a, decimal(0.0), decimal(1.0)); - } - } - } - - // Compute the closest points on both segments - closestPointSeg1 = seg1PointA + d1 * s; - closestPointSeg2 = seg2PointA + d2 * t; -} - -// Compute the intersection between a plane and a segment -// Let the plane define by the equation planeNormal.dot(X) = planeD with X a point on the plane and "planeNormal" the plane normal. This method -// computes the intersection P between the plane and the segment (segA, segB). The method returns the value "t" such -// that P = segA + t * (segB - segA). Note that it only returns a value in [0, 1] if there is an intersection. Otherwise, -// there is no intersection between the plane and the segment. -decimal reactphysics3d::computePlaneSegmentIntersection(const Vector3& segA, const Vector3& segB, const decimal planeD, const Vector3& planeNormal) { - - const decimal parallelEpsilon = decimal(0.0001); - decimal t = decimal(-1); - - decimal nDotAB = planeNormal.dot(segB - segA); - - // If the segment is not parallel to the plane - if (std::abs(nDotAB) > parallelEpsilon) { - t = (planeD - planeNormal.dot(segA)) / nDotAB; - } - - return t; -} - -// Compute the distance between a point "point" and a line given by the points "linePointA" and "linePointB" -decimal reactphysics3d::computePointToLineDistance(const Vector3& linePointA, const Vector3& linePointB, const Vector3& point) { - - decimal distAB = (linePointB - linePointA).length(); - - if (distAB < MACHINE_EPSILON) { - return (point - linePointA).length(); - } - - return ((point - linePointA).cross(point - linePointB)).length() / distAB; -} - -// Clip a segment against multiple planes and return the clipped segment vertices -// This method implements the Sutherland–Hodgman clipping algorithm -Array reactphysics3d::clipSegmentWithPlanes(const Vector3& segA, const Vector3& segB, - const Array& planesPoints, - const Array& planesNormals, - MemoryAllocator& allocator) { - assert(planesPoints.size() == planesNormals.size()); - - Array inputVertices(allocator, 2); - Array outputVertices(allocator, 2); - - inputVertices.add(segA); - inputVertices.add(segB); - - // For each clipping plane - const uint32 nbPlanesPoints = planesPoints.size(); - for (uint32 p=0; p < nbPlanesPoints; p++) { - - // If there is no more vertices, stop - if (inputVertices.size() == 0) return inputVertices; - - assert(inputVertices.size() == 2); - - outputVertices.clear(); - - Vector3& v1 = inputVertices[0]; - Vector3& v2 = inputVertices[1]; - - decimal v1DotN = (v1 - planesPoints[p]).dot(planesNormals[p]); - decimal v2DotN = (v2 - planesPoints[p]).dot(planesNormals[p]); - - // If the second vertex is in front of the clippling plane - if (v2DotN >= decimal(0.0)) { - - // If the first vertex is not in front of the clippling plane - if (v1DotN < decimal(0.0)) { - - // The second point we keep is the intersection between the segment v1, v2 and the clipping plane - decimal t = computePlaneSegmentIntersection(v1, v2, planesNormals[p].dot(planesPoints[p]), planesNormals[p]); - - if (t >= decimal(0) && t <= decimal(1.0)) { - outputVertices.add(v1 + t * (v2 - v1)); - } - else { - outputVertices.add(v2); - } - } - else { - outputVertices.add(v1); - } - - // Add the second vertex - outputVertices.add(v2); - } - else { // If the second vertex is behind the clipping plane - - // If the first vertex is in front of the clippling plane - if (v1DotN >= decimal(0.0)) { - - outputVertices.add(v1); - - // The first point we keep is the intersection between the segment v1, v2 and the clipping plane - decimal t = computePlaneSegmentIntersection(v1, v2, -planesNormals[p].dot(planesPoints[p]), -planesNormals[p]); - - if (t >= decimal(0.0) && t <= decimal(1.0)) { - outputVertices.add(v1 + t * (v2 - v1)); - } - } - } - - inputVertices = outputVertices; - } - - return outputVertices; -} - -// Clip a polygon against multiple planes and return the clipped polygon vertices -// This method implements the Sutherland–Hodgman clipping algorithm -Array reactphysics3d::clipPolygonWithPlanes(const Array& polygonVertices, const Array& planesPoints, - const Array& planesNormals, MemoryAllocator& allocator) { - - assert(planesPoints.size() == planesNormals.size()); - - uint32 nbMaxElements = polygonVertices.size() + planesPoints.size(); - Array inputVertices(allocator, nbMaxElements); - Array outputVertices(allocator, nbMaxElements); - - inputVertices.addRange(polygonVertices); - - // For each clipping plane - const uint32 nbPlanesPoints = planesPoints.size(); - for (uint32 p=0; p < nbPlanesPoints; p++) { - - outputVertices.clear(); - - uint32 nbInputVertices = inputVertices.size(); - uint32 vStart = nbInputVertices - 1; - - // For each edge of the polygon - for (uint vEnd = 0; vEnd= decimal(0.0)) { - - // If the first vertex is not in front of the clippling plane - if (v1DotN < decimal(0.0)) { - - // The second point we keep is the intersection between the segment v1, v2 and the clipping plane - decimal t = computePlaneSegmentIntersection(v1, v2, planesNormals[p].dot(planesPoints[p]), planesNormals[p]); - - if (t >= decimal(0) && t <= decimal(1.0)) { - outputVertices.add(v1 + t * (v2 - v1)); - } - else { - outputVertices.add(v2); - } - } - - // Add the second vertex - outputVertices.add(v2); - } - else { // If the second vertex is behind the clipping plane - - // If the first vertex is in front of the clippling plane - if (v1DotN >= decimal(0.0)) { - - // The first point we keep is the intersection between the segment v1, v2 and the clipping plane - decimal t = computePlaneSegmentIntersection(v1, v2, -planesNormals[p].dot(planesPoints[p]), -planesNormals[p]); - - if (t >= decimal(0.0) && t <= decimal(1.0)) { - outputVertices.add(v1 + t * (v2 - v1)); - } - else { - outputVertices.add(v1); - } - } - } - - vStart = vEnd; - } - - inputVertices = outputVertices; - } - - return outputVertices; -} - -// Project a point onto a plane that is given by a point and its unit length normal -Vector3 reactphysics3d::projectPointOntoPlane(const Vector3& point, const Vector3& unitPlaneNormal, const Vector3& planePoint) { - return point - unitPlaneNormal.dot(point - planePoint) * unitPlaneNormal; -} - -// Return the distance between a point and a plane (the plane normal must be normalized) -decimal reactphysics3d::computePointToPlaneDistance(const Vector3& point, const Vector3& planeNormal, const Vector3& planePoint) { - return planeNormal.dot(point - planePoint); -} diff --git a/src/systems/SolveHingeJointSystem.cpp b/src/systems/SolveHingeJointSystem.cpp index a4f9946e..ee8864ea 100644 --- a/src/systems/SolveHingeJointSystem.cpp +++ b/src/systems/SolveHingeJointSystem.cpp @@ -852,13 +852,13 @@ decimal SolveHingeJointSystem::computeCorrespondingAngleNearLimits(decimal input return inputAngle; } else if (inputAngle > upperLimitAngle) { - decimal diffToUpperLimit = std::fabs(computeNormalizedAngle(inputAngle - upperLimitAngle)); - decimal diffToLowerLimit = std::fabs(computeNormalizedAngle(inputAngle - lowerLimitAngle)); + decimal diffToUpperLimit = std::abs(computeNormalizedAngle(inputAngle - upperLimitAngle)); + decimal diffToLowerLimit = std::abs(computeNormalizedAngle(inputAngle - lowerLimitAngle)); return (diffToUpperLimit > diffToLowerLimit) ? (inputAngle - PI_TIMES_2) : inputAngle; } else if (inputAngle < lowerLimitAngle) { - decimal diffToUpperLimit = std::fabs(computeNormalizedAngle(upperLimitAngle - inputAngle)); - decimal diffToLowerLimit = std::fabs(computeNormalizedAngle(lowerLimitAngle - inputAngle)); + decimal diffToUpperLimit = std::abs(computeNormalizedAngle(upperLimitAngle - inputAngle)); + decimal diffToLowerLimit = std::abs(computeNormalizedAngle(lowerLimitAngle - inputAngle)); return (diffToUpperLimit > diffToLowerLimit) ? inputAngle : (inputAngle + PI_TIMES_2); } else { diff --git a/test/tests/mathematics/TestMathematicsFunctions.h b/test/tests/mathematics/TestMathematicsFunctions.h index b5c77fe2..77c4ee52 100644 --- a/test/tests/mathematics/TestMathematicsFunctions.h +++ b/test/tests/mathematics/TestMathematicsFunctions.h @@ -253,7 +253,8 @@ class TestMathematicsFunctions : public Test { polygonPlanesNormals.add(Vector3(0, -1, 0)); polygonPlanesPoints.add(Vector3(10, 5, 0)); - Array clipPolygonVertices = clipPolygonWithPlanes(polygonVertices, polygonPlanesPoints, polygonPlanesNormals, mAllocator); + Array clipPolygonVertices(mAllocator); + clipPolygonWithPlanes(polygonVertices, polygonPlanesPoints, polygonPlanesNormals, clipPolygonVertices, mAllocator); rp3d_test(clipPolygonVertices.size() == 4); rp3d_test(approxEqual(clipPolygonVertices[0].x, 0, 0.000001)); rp3d_test(approxEqual(clipPolygonVertices[0].y, 2, 0.000001));