mrq/reactphysics3d
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Refactor the mathematics_functions.h file

Browse Source
This commit is contained in:
Daniel Chappuis 2020-09-08 19:03:22 +02:00
parent b55626df67
commit e7b951b8e4
13 changed files with 416 additions and 439 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
include/reactphysics3d/collision/narrowphase/GJK/VoronoiSimplex.h
View File

@ -28,6 +28,7 @@
// Libraries
#include <reactphysics3d/mathematics/Vector3.h>
#include <reactphysics3d/mathematics/Vector2.h>
/// ReactPhysics3D namespace
namespace reactphysics3d {

4
include/reactphysics3d/mathematics/Matrix2x2.h
View File

@ -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

6
include/reactphysics3d/mathematics/Matrix3x3.h
View File

@ -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

12
include/reactphysics3d/mathematics/Vector2.h
View File

@ -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

16
include/reactphysics3d/mathematics/Vector3.h
View File

@ -28,8 +28,9 @@
// Libraries
#include <cassert>
#include <reactphysics3d/mathematics/mathematics_functions.h>
#include <cmath>
#include <reactphysics3d/decimal.h>
#include <reactphysics3d/mathematics/mathematics_common.h>
#include <reactphysics3d/configuration.h>
/// 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

50
include/reactphysics3d/mathematics/mathematics_common.h Normal file
View File

@ -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 <reactphysics3d/configuration.h>
#include <reactphysics3d/decimal.h>
#include <cassert>
#include <cmath>
/// 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

366
include/reactphysics3d/mathematics/mathematics_functions.h
View File

@ -29,6 +29,7 @@
// Libraries
#include <reactphysics3d/configuration.h>
#include <reactphysics3d/decimal.h>
#include <reactphysics3d/mathematics/Vector3.h>
#include <algorithm>
#include <cassert>
#include <cmath>
@ -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<Vector3> 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<Vector3> clipSegmentWithPlanes(const Vector3& segA, const Vector3& segB,
const Array<Vector3>& planesPoints,
const Array<Vector3>& planesNormals,
MemoryAllocator& allocator);
MemoryAllocator& allocator) {
assert(planesPoints.size() == planesNormals.size());
/// Clip a polygon against multiple planes and return the clipped polygon vertices
Array<Vector3> clipPolygonWithPlanes(const Array<Vector3>& polygonVertices, const Array<Vector3>& planesPoints,
const Array<Vector3>& planesNormals, MemoryAllocator& allocator);
Array<Vector3> inputVertices(allocator, 2);
Array<Vector3> 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<Vector3>& polygonVertices, const Array<Vector3>& planesPoints,
const Array<Vector3>& planesNormals, Array<Vector3>& outClippedPolygonVertices,
MemoryAllocator& allocator) {
assert(planesPoints.size() == planesNormals.size());
const uint32 nbMaxElements = polygonVertices.size() + planesPoints.size();
Array<Vector3> 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<nbInputVertices; vEnd++) {
Vector3& v1 = inputVertices[vStart];
Vector3& v2 = inputVertices[vEnd];
const decimal v1DotN = (v1 - planesPoints[p]).dot(planesNormals[p]);
const 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)) {
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) {

1
include/reactphysics3d/systems/ContactSolverSystem.h
View File

@ -30,6 +30,7 @@
#include <reactphysics3d/configuration.h>
#include <reactphysics3d/mathematics/Vector3.h>
#include <reactphysics3d/mathematics/Matrix3x3.h>
#include <reactphysics3d/containers/Array.h>
#include <reactphysics3d/engine/Material.h>
/// ReactPhysics3D namespace

3
src/collision/narrowphase/SAT/SATAlgorithm.cpp
View File

@ -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<Vector3> clipPolygonVertices = clipPolygonWithPlanes(polygonVertices, planesPoints, planesNormals, mMemoryAllocator);
Array<Vector3> 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);

2
src/mathematics/Vector3.cpp
View File

@ -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) {

383
src/mathematics/mathematics_functions.cpp
View File

@ -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 <reactphysics3d/mathematics/mathematics_functions.h>
#include <reactphysics3d/mathematics/Vector3.h>
#include <reactphysics3d/mathematics/Vector2.h>
#include <cassert>
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<Vector3> reactphysics3d::clipSegmentWithPlanes(const Vector3& segA, const Vector3& segB,
const Array<Vector3>& planesPoints,
const Array<Vector3>& planesNormals,
MemoryAllocator& allocator) {
assert(planesPoints.size() == planesNormals.size());
Array<Vector3> inputVertices(allocator, 2);
Array<Vector3> 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<Vector3> reactphysics3d::clipPolygonWithPlanes(const Array<Vector3>& polygonVertices, const Array<Vector3>& planesPoints,
const Array<Vector3>& planesNormals, MemoryAllocator& allocator) {
assert(planesPoints.size() == planesNormals.size());
uint32 nbMaxElements = polygonVertices.size() + planesPoints.size();
Array<Vector3> inputVertices(allocator, nbMaxElements);
Array<Vector3> 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<nbInputVertices; vEnd++) {
Vector3& v1 = inputVertices[vStart];
Vector3& v2 = inputVertices[vEnd];
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);
}
}
// 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);
}

8
src/systems/SolveHingeJointSystem.cpp
View File

@ -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 {

3
test/tests/mathematics/TestMathematicsFunctions.h
View File

@ -253,7 +253,8 @@ class TestMathematicsFunctions : public Test {
polygonPlanesNormals.add(Vector3(0, -1, 0));
polygonPlanesPoints.add(Vector3(10, 5, 0));
Array<Vector3> clipPolygonVertices = clipPolygonWithPlanes(polygonVertices, polygonPlanesPoints, polygonPlanesNormals, mAllocator);
Array<Vector3> 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));