diff --git a/src/body/BoundingSphere.cpp b/src/body/BoundingSphere.cpp index 36d935a4..4acdf8e4 100644 --- a/src/body/BoundingSphere.cpp +++ b/src/body/BoundingSphere.cpp @@ -1,7 +1,7 @@ /******************************************************************************** * ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ * -* Copyright (c) 2010 Daniel Chappuis * +* Copyright (c) 2011 Daniel Chappuis * ********************************************************************************* * * * Permission is hereby granted, free of charge, to any person obtaining a copy * diff --git a/src/body/BoundingSphere.h b/src/body/BoundingSphere.h index 54346cf5..0f36a4f3 100644 --- a/src/body/BoundingSphere.h +++ b/src/body/BoundingSphere.h @@ -1,6 +1,6 @@ /******************************************************************************** * ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ * -* Copyright (c) 2010 Daniel Chappuis * +* Copyright (c) 2011 Daniel Chappuis * ********************************************************************************* * * * Permission is hereby granted, free of charge, to any person obtaining a copy * @@ -46,14 +46,14 @@ class BoundingSphere : public NarrowBoundingVolume { BoundingSphere(const Vector3D& center, double radius); // Constructor virtual ~BoundingSphere(); // Destructor - Vector3D getCenter() const; // Return the center point of the sphere - void setCenter(const Vector3D& center); // Set the center point of the sphere - double getRadius() const; // Return the radius of the sphere - void setRadius(double radius); // Set the radius of the sphere + Vector3D getCenter() const; // Return the center point of the sphere + void setCenter(const Vector3D& center); // Set the center point of the sphere + double getRadius() const; // Return the radius of the sphere + void setRadius(double radius); // Set the radius of the sphere virtual void update(const Vector3D& newCenter, - const Quaternion& rotationQuaternion); // Update the sphere orientation according to a new orientation of the rigid body - virtual AABB* computeAABB() const; // Return the corresponding AABB - virtual Vector3D getSupportPoint(const Vector3D& direction) const; // Return a support point in a given direction + const Quaternion& rotationQuaternion); // Update the sphere orientation according to a new orientation of the rigid body + virtual AABB* computeAABB() const; // Return the corresponding AABB + virtual Vector3D getSupportPoint(const Vector3D& direction, double margin=0.0) const; // Return a support point in a given direction #ifdef VISUAL_DEBUG virtual void draw() const; // Draw the sphere (only for testing purpose) @@ -87,11 +87,12 @@ inline void BoundingSphere::update(const Vector3D& newCenter, const Quaternion& } // Return a support point in a given direction -inline Vector3D BoundingSphere::getSupportPoint(const Vector3D& direction) const { +inline Vector3D BoundingSphere::getSupportPoint(const Vector3D& direction, double margin) const { assert(direction.length() > 0.0); + assert(margin >= 0.0); // Return the support point of the sphere in the given direction - return center + radius * direction.getUnit(); + return center + (radius + margin) * direction.getUnit(); } }; // End of the ReactPhysics3D namespace diff --git a/src/body/NarrowBoundingVolume.h b/src/body/NarrowBoundingVolume.h index 8b0a9c55..d0a19e97 100644 --- a/src/body/NarrowBoundingVolume.h +++ b/src/body/NarrowBoundingVolume.h @@ -48,8 +48,8 @@ class NarrowBoundingVolume : public BoundingVolume { NarrowBoundingVolume(); // Constructor virtual ~NarrowBoundingVolume(); // Destructor - virtual AABB* computeAABB() const=0; // Return the corresponding AABB - virtual Vector3D getSupportPoint(const Vector3D& direction) const=0; // Return a support point in a given direction + virtual AABB* computeAABB() const=0; // Return the corresponding AABB + virtual Vector3D getSupportPoint(const Vector3D& direction, double margin=0.0) const=0; // Return a support point in a given direction }; } diff --git a/src/body/OBB.cpp b/src/body/OBB.cpp index 9f84eb2d..023e09ec 100644 --- a/src/body/OBB.cpp +++ b/src/body/OBB.cpp @@ -131,7 +131,7 @@ vector OBB::getExtremeVertices(const Vector3D& directionAxis) const { // Check if the given axis is parallel to an axis on the OBB if (axis[0].isParallelWith(directionAxis)) { - if (axis[0].scalarProduct(directionAxis) >= 0) { // If both axis are in the same direction + if (axis[0].dot(directionAxis) >= 0) { // If both axis are in the same direction extremeVertices = getFace(0); // The extreme is the face 0 } else { @@ -139,7 +139,7 @@ vector OBB::getExtremeVertices(const Vector3D& directionAxis) const { } } else if(axis[1].isParallelWith(directionAxis)) { - if (axis[1].scalarProduct(directionAxis) >= 0) { // If both axis are in the same direction + if (axis[1].dot(directionAxis) >= 0) { // If both axis are in the same direction extremeVertices = getFace(2); // The extreme is the face 2 } else { @@ -148,7 +148,7 @@ vector OBB::getExtremeVertices(const Vector3D& directionAxis) const { } else if(axis[2].isParallelWith(directionAxis)) { - if (axis[2].scalarProduct(directionAxis) >= 0) { // If both axis are in the same direction + if (axis[2].dot(directionAxis) >= 0) { // If both axis are in the same direction extremeVertices = getFace(4); // The extreme is the face 4 } else { @@ -163,7 +163,7 @@ vector OBB::getExtremeVertices(const Vector3D& directionAxis) const { Vector3D vertex = getVertex(i); // Compute the projection length of the current vertex onto the projection axis - double projectionLength = directionAxis.scalarProduct(vertex-center) / directionAxis.length(); + double projectionLength = directionAxis.dot(vertex-center) / directionAxis.length(); // If we found a bigger projection length if (projectionLength > maxProjectionLength + EPSILON) { diff --git a/src/body/OBB.h b/src/body/OBB.h index 1d8d0c21..413daaad 100644 --- a/src/body/OBB.h +++ b/src/body/OBB.h @@ -66,7 +66,7 @@ class OBB : public NarrowBoundingVolume { virtual std::vector getExtremeVertices(const Vector3D& axis) const; // Return all the vertices that are projected at the extreme of the projection of the bouding volume on the axis virtual void update(const Vector3D& newCenter, const Quaternion& rotationQuaternion); // Update the oriented bounding box orientation according to a new orientation of the rigid body virtual AABB* computeAABB() const; // Return the corresponding AABB - virtual Vector3D getSupportPoint(const Vector3D& direction) const; // Return a support point in a given direction + virtual Vector3D getSupportPoint(const Vector3D& direction, double margin=0.0) const; // Return a support point in a given direction #ifdef VISUAL_DEBUG virtual void draw() const; // Draw the OBB (only for testing purpose) @@ -228,7 +228,10 @@ inline void OBB::update(const Vector3D& newCenter, const Quaternion& rotationQua } // Return a support point in a given direction -inline Vector3D OBB::getSupportPoint(const Vector3D& direction) const { +inline Vector3D OBB::getSupportPoint(const Vector3D& direction, double margin) const { + assert(direction.length() > 0.0); + assert(margin >= 0.0); + // TODO : Implement this method assert(false); return Vector3D(); diff --git a/src/collision/EPA/EPAAlgorithm.h b/src/collision/EPA/EPAAlgorithm.h index e9707b4b..7d1afd72 100644 --- a/src/collision/EPA/EPAAlgorithm.h +++ b/src/collision/EPA/EPAAlgorithm.h @@ -27,9 +27,9 @@ // Libraries #include "../GJK/Simplex.h" -#include "../body/NarrowBoundingVolume.h" -#include "ContactInfo.h" -#include "../mathematics/mathematics.h" +#include "../../body/NarrowBoundingVolume.h" +#include "../ContactInfo.h" +#include "../../mathematics/mathematics.h" // ReactPhysics3D namespace namespace reactphysics3d { diff --git a/src/collision/EPA/TriangleEPA.cpp b/src/collision/EPA/TriangleEPA.cpp index 7d395ebc..0291b3ba 100644 --- a/src/collision/EPA/TriangleEPA.cpp +++ b/src/collision/EPA/TriangleEPA.cpp @@ -71,7 +71,7 @@ bool TriangleEPA::computeClosestPoint(const Vector3D* vertices) { // If the determinant is positive if (det > 0.0) { // Compute the closest point v - closestPoint = p0 + (lambda1 * v1 * lambda2 * v2) / det; + closestPoint = p0 + 1.0 / det * (lambda1 * v1 + lambda2 * v2); // Compute the square distance of closest point to the origin distSquare = closestPoint.dot(closestPoint); diff --git a/src/collision/GJK/GJKAlgorithm.cpp b/src/collision/GJK/GJKAlgorithm.cpp index d86806f4..9c723cec 100644 --- a/src/collision/GJK/GJKAlgorithm.cpp +++ b/src/collision/GJK/GJKAlgorithm.cpp @@ -26,8 +26,8 @@ // Libraries #include "GJKAlgorithm.h" #include "Simplex.h" -#include "../constraint/Contact.h" -#include "../constants.h" +#include "../../constraint/Contact.h" +#include "../../constants.h" #include #include #include diff --git a/src/collision/GJK/GJKAlgorithm.h b/src/collision/GJK/GJKAlgorithm.h index 32e0bde1..92f9ec06 100644 --- a/src/collision/GJK/GJKAlgorithm.h +++ b/src/collision/GJK/GJKAlgorithm.h @@ -26,10 +26,10 @@ #define GJKALGORITHM_H // Libraries -#include "NarrowPhaseAlgorithm.h" -#include "ContactInfo.h" -#include "../body/NarrowBoundingVolume.h" -#include "EPAAlgorithm.h" +#include "../NarrowPhaseAlgorithm.h" +#include "../ContactInfo.h" +#include "../../body/NarrowBoundingVolume.h" +#include "../EPA/EPAAlgorithm.h" // ReactPhysics3D namespace diff --git a/src/collision/GJK/Simplex.h b/src/collision/GJK/Simplex.h index ae93340c..eb23b23a 100644 --- a/src/collision/GJK/Simplex.h +++ b/src/collision/GJK/Simplex.h @@ -26,7 +26,7 @@ #define SIMPLEX_H // Libraries -#include "../mathematics/mathematics.h" +#include "../../mathematics/mathematics.h" #include // ReactPhysics3D namespace diff --git a/src/constraint/Contact.cpp b/src/constraint/Contact.cpp index ff957332..815b85b2 100644 --- a/src/constraint/Contact.cpp +++ b/src/constraint/Contact.cpp @@ -74,8 +74,8 @@ void Contact::computeJacobian(int noConstraint, Matrix1x6**& J_sp) const { r1 = points[i] - body1Position; r2 = points[i] - body2Position; - r1CrossN = r1.crossProduct(normal); - r2CrossN = r2.crossProduct(normal); + r1CrossN = r1.cross(normal); + r2CrossN = r2.cross(normal); // Compute the jacobian matrix for the body 1 for the contact constraint //J_sp[currentIndex][0].changeSize(1, 6); @@ -98,10 +98,10 @@ void Contact::computeJacobian(int noConstraint, Matrix1x6**& J_sp) const { currentIndex++; // Compute the jacobian matrix for the body 1 for the first friction constraint - r1CrossU1 = r1.crossProduct(frictionVectors[0]); - r2CrossU1 = r2.crossProduct(frictionVectors[0]); - r1CrossU2 = r1.crossProduct(frictionVectors[1]); - r2CrossU2 = r2.crossProduct(frictionVectors[1]); + r1CrossU1 = r1.cross(frictionVectors[0]); + r2CrossU1 = r2.cross(frictionVectors[0]); + r1CrossU2 = r1.cross(frictionVectors[1]); + r2CrossU2 = r2.cross(frictionVectors[1]); //J_sp[currentIndex][0].changeSize(1, 6); J_sp[currentIndex][0].setValue(0, -frictionVectors[0].getX()); J_sp[currentIndex][0].setValue(1, -frictionVectors[0].getY()); @@ -193,7 +193,7 @@ void Contact::computeErrorValue(int noConstraint, Vector& errorValues) const { Vector3D velocity1 = rigidBody1->getLinearVelocity(); Vector3D velocity2 = rigidBody2->getLinearVelocity(); double restitutionCoeff = rigidBody1->getRestitution() * rigidBody2->getRestitution(); - double errorValue = restitutionCoeff * (normal.scalarProduct(velocity1) - normal.scalarProduct(velocity2)) + PENETRATION_FACTOR * penetrationDepth; + double errorValue = restitutionCoeff * (normal.dot(velocity1) - normal.dot(velocity2)) + PENETRATION_FACTOR * penetrationDepth; // Assign the error value to the vector of error values for (int i=0; iscalarProduct(vector); + double scalarProd = this->dot(vector); return approxEqual(std::abs(scalarProd), length() * vector.length()); } diff --git a/src/mathematics/mathematics.h b/src/mathematics/mathematics.h index 85631fd1..e6a40f02 100644 --- a/src/mathematics/mathematics.h +++ b/src/mathematics/mathematics.h @@ -65,11 +65,11 @@ inline void closestPointsBetweenTwoLines(const reactphysics3d::Vector3D& point1, const reactphysics3d::Vector3D& d2, double* alpha, double* beta) { reactphysics3d::Vector3D r = point1 - point2; - double a = d1.scalarProduct(d1); - double b = d1.scalarProduct(d2); - double c = d1.scalarProduct(r); - double e = d2.scalarProduct(d2); - double f = d2.scalarProduct(r); + double a = d1.dot(d1); + double b = d1.dot(d2); + double c = d1.dot(r); + double e = d2.dot(d2); + double f = d2.dot(r); double d = a*e-b*b; // The two lines must not be parallel @@ -183,7 +183,7 @@ inline std::vector projectPointsOntoPlane(const std::v // For each point of the set for (unsigned int i=0; i projectPointsOntoPlane(const std::v // Compute the distance between a point "P" and a line (given by a point "A" and a vector "v") inline double computeDistanceBetweenPointAndLine(const reactphysics3d::Vector3D& P, const reactphysics3d::Vector3D& A, const reactphysics3d::Vector3D& v) { assert(v.length() != 0); - return ((P-A).crossProduct(v).length() / (v.length())); + return ((P-A).cross(v).length() / (v.length())); } // Compute the orthogonal projection of a point "P" on a line (given by a point "A" and a vector "v") inline reactphysics3d::Vector3D computeOrthogonalProjectionOfPointOntoALine(const reactphysics3d::Vector3D& P, const reactphysics3d::Vector3D& A, const reactphysics3d::Vector3D& v) { - return (A + ((P-A).scalarProduct(v) / (v.scalarProduct(v))) * v); + return (A + ((P-A).dot(v) / (v.dot(v))) * v); } // Given a point P and 4 points that form a rectangle (point P and the 4 points have to be on the same plane) this method computes @@ -281,8 +281,8 @@ inline void computeParallelSegmentsIntersection(const reactphysics3d::Vector3D& assert(d1.isParallelWith(d2)); // Compute the projection of the two points of the second segment onto the vector of segment 1 - double projSeg2PointA = d1.getUnit().scalarProduct(seg2PointA - seg1PointA); - double projSeg2PointB = d1.getUnit().scalarProduct(seg2PointB - seg1PointA); + double projSeg2PointA = d1.getUnit().dot(seg2PointA - seg1PointA); + double projSeg2PointB = d1.getUnit().dot(seg2PointB - seg1PointA); // The projections intervals should intersect assert(!(projSeg2PointA < 0.0 && projSeg2PointB < 0.0)); @@ -343,9 +343,9 @@ inline std::vector clipSegmentWithRectangleInPlane(con reactphysics3d::Vector3D planeNormal = clipRectangle[(i+2) % 4] - clipRectangle[(i+1) % 4]; // If the point P is inside the clip plane - if (planeNormal.scalarProduct(P-A) >= 0.0 - epsilon) { + if (planeNormal.dot(P-A) >= 0.0 - epsilon) { // If the point S is inside the clip plane - if (planeNormal.scalarProduct(S-A) >= 0.0 - epsilon) { + if (planeNormal.dot(S-A) >= 0.0 - epsilon) { outputSegment.push_back(P); outputSegment.push_back(S); } @@ -357,7 +357,7 @@ inline std::vector clipSegmentWithRectangleInPlane(con outputSegment.push_back(intersectPoint); } } - else if (planeNormal.scalarProduct(S-A) > 0.0 - epsilon) { // P is outside and S is inside the clip plane + else if (planeNormal.dot(S-A) > 0.0 - epsilon) { // P is outside and S is inside the clip plane // Compute the intersection point between the segment SP and the clip plane reactphysics3d::Vector3D intersectPoint = computeLinesIntersection(S, P-S, A, B-A); @@ -398,10 +398,10 @@ inline std::vector clipPolygonWithRectangleInPlane(con reactphysics3d::Vector3D P = inputPolygon[(j+1) % inputPolygon.size()]; // If the point P is inside the clip plane - double test = planeNormal.scalarProduct(P-A); - if (planeNormal.scalarProduct(P-A) >= 0.0 - epsilon) { + double test = planeNormal.dot(P-A); + if (planeNormal.dot(P-A) >= 0.0 - epsilon) { // If the point S is also inside the clip plane - if (planeNormal.scalarProduct(S-A) >= 0.0 - epsilon) { + if (planeNormal.dot(S-A) >= 0.0 - epsilon) { outputPolygon.push_back(P); } else { // If the point S is outside the clip plane @@ -412,7 +412,7 @@ inline std::vector clipPolygonWithRectangleInPlane(con outputPolygon.push_back(P); } } - else if (planeNormal.scalarProduct(S-A) > 0.0) { + else if (planeNormal.dot(S-A) > 0.0) { // Compute the intersection point between the segment SP and the clip plane reactphysics3d::Vector3D intersectPoint = computeLinesIntersection(S, P-S, A, B-A); @@ -431,13 +431,13 @@ inline std::vector clipPolygonWithRectangleInPlane(con // the lineVector must not be orthogonal to the planeNormal. inline reactphysics3d::Vector3D intersectLineWithPlane(const reactphysics3d::Vector3D& linePoint, const reactphysics3d::Vector3D& lineVector, const reactphysics3d::Vector3D& planePoint, const reactphysics3d::Vector3D& planeNormal) { - assert(!approxEqual(lineVector.scalarProduct(planeNormal), 0.0)); + assert(!approxEqual(lineVector.dot(planeNormal), 0.0)); // The plane is represented by the equation planeNormal dot X = d where X is a point of the plane - double d = planeNormal.scalarProduct(planePoint); + double d = planeNormal.dot(planePoint); // Compute the parameter t - double t = (d - planeNormal.scalarProduct(linePoint)) / planeNormal.scalarProduct(lineVector); + double t = (d - planeNormal.dot(linePoint)) / planeNormal.dot(lineVector); // Compute the intersection point return linePoint + lineVector * t;