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

implementation of GJK and EPA collision detection algorithm continued

Browse Source 
git-svn-id: https://reactphysics3d.googlecode.com/svn/trunk@418 92aac97c-a6ce-11dd-a772-7fcde58d38e6
This commit is contained in:
chappuis.daniel 2011-02-07 15:09:45 +00:00
parent 844df20be0
commit 4ed45d43ed
16 changed files with 72 additions and 68 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
src/body/BoundingSphere.cpp
View File

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

21
src/body/BoundingSphere.h
View File

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

4
src/body/NarrowBoundingVolume.h
View File

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

8
src/body/OBB.cpp
View File

@ -131,7 +131,7 @@ vector<Vector3D> 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<Vector3D> 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<Vector3D> 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<Vector3D> 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) {

7
src/body/OBB.h
View File

@ -66,7 +66,7 @@ class OBB : public NarrowBoundingVolume {
virtual std::vector<Vector3D> 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();

6
src/collision/EPA/EPAAlgorithm.h
View File

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

2
src/collision/EPA/TriangleEPA.cpp
View File

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

4
src/collision/GJK/GJKAlgorithm.cpp
View File

@ -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 <algorithm>
#include <cmath>
#include <cfloat>

8
src/collision/GJK/GJKAlgorithm.h
View File

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

2
src/collision/GJK/Simplex.h
View File

@ -26,7 +26,7 @@
#define SIMPLEX_H
// Libraries
#include "../mathematics/mathematics.h"
#include "../../mathematics/mathematics.h"
#include <vector>
// ReactPhysics3D namespace

14
src/constraint/Contact.cpp
View File

@ -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; i<nbPoints; i++) {

2
src/constraint/Contact.h
View File

@ -87,7 +87,7 @@ inline void Contact::computeFrictionVectors() {
frictionVectors.push_back(vector1);
// Compute the second orthogonal vector using the cross product
frictionVectors.push_back(normal.crossProduct(vector1));
frictionVectors.push_back(normal.cross(vector1));
}
// Return the normal vector of the contact

2
src/mathematics/Quaternion.cpp
View File

@ -197,7 +197,7 @@ Quaternion Quaternion::slerp(const Quaternion& quaternion1, const Quaternion& qu
double invert = 1.0;
// Compute cos(theta) using the quaternion scalar product
double cosineTheta = quaternion1.scalarProduct(quaternion2);
double cosineTheta = quaternion1.dot(quaternion2);
// Take care of the sign of cosineTheta
if (cosineTheta < 0.0) {

6
src/mathematics/Quaternion.h
View File

@ -68,7 +68,7 @@ class Quaternion {
Quaternion getConjugate() const; // Return the conjugate quaternion
Quaternion getInverse() const throw (MathematicsException); // Return the inverse of the quaternion
Matrix3x3 getMatrix() const; // Return the orientation matrix corresponding to this quaternion
double scalarProduct(const Quaternion& quaternion) const; // Scalar product between two quaternions
double dot(const Quaternion& quaternion) const; // Dot product between two quaternions
void getRotationAngleAxis(double& angle, Vector3D& axis) const; // Compute the rotation angle (in radians) and the axis
static Quaternion slerp(const Quaternion& quaternion1,
const Quaternion& quaternion2, double t); // Compute the spherical linear interpolation between two quaternions
@ -174,7 +174,7 @@ inline Quaternion Quaternion::getInverse() const throw(MathematicsException) {
}
// Scalar product between two quaternions
inline double Quaternion::scalarProduct(const Quaternion& quaternion) const {
inline double Quaternion::dot(const Quaternion& quaternion) const {
return (x*quaternion.x + y*quaternion.y + z*quaternion.z + w*quaternion.w);
}
@ -199,7 +199,7 @@ inline Quaternion Quaternion::operator*(double nb) const {
// Overloaded operator for the multiplication of two quaternions
inline Quaternion Quaternion::operator*(const Quaternion& quaternion) const {
// Return the result of the multiplication
return Quaternion(w*quaternion.w - vectorV().scalarProduct(quaternion.vectorV()), w*quaternion.vectorV()+quaternion.w*vectorV() + vectorV().crossProduct(quaternion.vectorV()));
return Quaternion(w*quaternion.w - vectorV().dot(quaternion.vectorV()), w*quaternion.vectorV()+quaternion.w*vectorV() + vectorV().cross(quaternion.vectorV()));
}
// Overloaded operator for the assignment

12
src/mathematics/Vector3D.h
View File

@ -67,9 +67,9 @@ class Vector3D {
bool isUnit() const; // Return true if the vector is unit and false otherwise
bool isZero() const; // Return true if the current vector is the zero vector
Vector3D getOpposite() const; // Return the vector in the opposite direction
Vector3D getOneOrthogonalVector() const; // Return one unit orthogonal vectors of the current vector
double scalarProduct(const Vector3D& vector) const; // Scalar product of two vectors
Vector3D crossProduct(const Vector3D& vector) const; // Cross product of two vectors
Vector3D getOneOrthogonalVector() const; // Return one unit orthogonal vectors of the current vector
double dot(const Vector3D& vector) const; // Dot product of two vectors
Vector3D cross(const Vector3D& vector) const; // Cross product of two vectors
bool isParallelWith(const Vector3D& vector) const; // Return true if two vectors are parallel
// --- Overloaded operators --- //
@ -151,20 +151,20 @@ inline Vector3D Vector3D::getOpposite() const {
}
// Scalar product of two vectors (inline)
inline double Vector3D::scalarProduct(const Vector3D& vector) const {
inline double Vector3D::dot(const Vector3D& vector) const {
// Compute and return the result of the scalar product
return (x * vector.x + y * vector.y + z * vector.z);
}
// Cross product of two vectors (inline)
inline Vector3D Vector3D::crossProduct(const Vector3D& vector) const {
inline Vector3D Vector3D::cross(const Vector3D& vector) const {
// Compute and return the cross product
return Vector3D(y * vector.z - z * vector.y, z * vector.x - x * vector.z , x * vector.y - y * vector.x);
}
// Return true if two vectors are parallel
inline bool Vector3D::isParallelWith(const Vector3D& vector) const {
double scalarProd = this->scalarProduct(vector);
double scalarProd = this->dot(vector);
return approxEqual(std::abs(scalarProd), length() * vector.length());
}

40
src/mathematics/mathematics.h
View File

@ -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<reactphysics3d::Vector3D> projectPointsOntoPlane(const std::v
// For each point of the set
for (unsigned int i=0; i<points.size(); ++i) {
// Compute the projection of the point onto the plane
projectedPoints.push_back(points[i] - (n * (points[i] - A).scalarProduct(n)));
projectedPoints.push_back(points[i] - (n * (points[i] - A).dot(n)));
}
@ -195,13 +195,13 @@ inline std::vector<reactphysics3d::Vector3D> 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<reactphysics3d::Vector3D> 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<reactphysics3d::Vector3D> 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<reactphysics3d::Vector3D> 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<reactphysics3d::Vector3D> 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<reactphysics3d::Vector3D> 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;