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

Change the code structure

Browse Source 
git-svn-id: https://reactphysics3d.googlecode.com/svn/trunk@430 92aac97c-a6ce-11dd-a772-7fcde58d38e6
This commit is contained in:
chappuis.daniel 2011-07-09 16:58:50 +00:00
parent bd5d0fb230
commit 1d5a8e2491
28 changed files with 243 additions and 284 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
src/body/AABB.cpp
View File

@ -34,13 +34,12 @@ using namespace reactphysics3d;
using namespace std;
// Constructor
AABB::AABB(const Body* bodyPointer) : bodyPointer(bodyPointer) {
AABB::AABB() : bodyPointer(0) {
}
// Constructor
AABB::AABB(const Body* bodyPointer, const Transform& transform, const Vector3D& extents)
: bodyPointer(bodyPointer) {
AABB::AABB(const Transform& transform, const Vector3D& extents) : bodyPointer(0) {
update(transform, extents);
}

30
src/body/AABB.h
View File

@ -46,19 +46,20 @@ class AABB {
private :
Vector3D minCoordinates; // Minimum world coordinates of the AABB on the x,y and z axis
Vector3D maxCoordinates; // Maximum world coordinates of the AABB on the x,y and z axis
const Body* bodyPointer; // Pointer to the owner body
Body* bodyPointer; // Pointer to the owner body (not the abstract class Body but its derivative which is instanciable)
public :
AABB(const Body* body); // Constructor
AABB(const Body* bodyPointer, const Transform& transform, const Vector3D& extents); // Constructor
virtual ~AABB(); // Destructor
AABB(); // Constructor
AABB(const Transform& transform, const Vector3D& extents); // Constructor
virtual ~AABB(); // Destructor
Vector3D getCenter() const; // Return the center point
const Vector3D& getMinCoordinates() const; // Return the minimum coordinates of the AABB
const Vector3D& getMaxCoordinates() const; // Return the maximum coordinates of the AABB
const Body* getBodyPointer() const; // Return a pointer to the owner body
bool testCollision(const AABB& aabb) const; // Return true if the current AABB is overlapping is the AABB in argument
virtual void update(const Transform& newTransform, const Vector3D& extents); // Update the oriented bounding box orientation according to a new orientation of the rigid body
Vector3D getCenter() const; // Return the center point
const Vector3D& getMinCoordinates() const; // Return the minimum coordinates of the AABB
const Vector3D& getMaxCoordinates() const; // Return the maximum coordinates of the AABB
Body* getBodyPointer() const; // Return a pointer to the owner body
void setBodyPointer(Body* bodyPointer); // Set the body pointer
bool testCollision(const AABB& aabb) const; // Return true if the current AABB is overlapping is the AABB in argument
virtual void update(const Transform& newTransform, const Vector3D& extents); // Update the oriented bounding box orientation according to a new orientation of the rigid body
#ifdef VISUAL_DEBUG
virtual void draw() const; // Draw the AABB (only for testing purpose)
#endif
@ -80,15 +81,18 @@ inline const Vector3D& AABB::getMaxCoordinates() const {
}
// Return a pointer to the owner body
inline const Body* AABB::getBodyPointer() const {
inline Body* AABB::getBodyPointer() const {
return bodyPointer;
}
// Set the body pointer
inline void AABB::setBodyPointer(Body* bodyPointer) {
this->bodyPointer = bodyPointer;
}
// Return true if the current AABB is overlapping with the AABB in argument
// Two AABB overlap if they overlap in the three x, y and z axis at the same time
inline bool AABB::testCollision(const AABB& aabb) const {
Vector3D center2 = aabb.getCenter();
double margin = 2 * OBJECT_MARGIN;
if (maxCoordinates.getX() + OBJECT_MARGIN < aabb.minCoordinates.getX() - OBJECT_MARGIN ||
aabb.maxCoordinates.getX() + OBJECT_MARGIN < minCoordinates.getX() - OBJECT_MARGIN) return false;
if (maxCoordinates.getY() + OBJECT_MARGIN < aabb.minCoordinates.getY() - OBJECT_MARGIN ||

47
src/body/Body.cpp
View File

@ -24,56 +24,31 @@
// Libraries
#include "Body.h"
#include "BroadBoundingVolume.h"
#include "NarrowBoundingVolume.h"
#include "Shape.h"
// We want to use the ReactPhysics3D namespace
using namespace reactphysics3d;
// Constructor
Body::Body(const Transform& transform, double mass) throw(std::invalid_argument)
: transform(transform), mass(mass), narrowBoundingVolume(0), aabb(0) {
// Check if the mass is not larger than zero
if (mass <= 0.0) {
// We throw an exception
throw std::invalid_argument("Exception in Body constructor : the mass has to be different larger than zero");
}
Body::Body(const Transform& transform, Shape* shape, double mass)
: shape(shape), transform(transform), mass(mass) {
assert(mass > 0.0);
assert(shape);
isMotionEnabled = true;
isCollisionEnabled = true;
interpolationFactor = 0.0;
// Initialize the old transform
oldTransform = transform;
// Create the AABB for Broad-Phase collision detection
aabb = new AABB(this);
// Create the AABB for broad-phase collision detection
aabb = new AABB(transform, shape->getLocalExtents());
}
// Destructor
Body::~Body() {
/* TODO : DELETE THIS
if (broadBoundingVolume) {
delete broadBoundingVolume;
}
*/
if (narrowBoundingVolume) {
delete narrowBoundingVolume;
}
// Delete the AABB
delete aabb;
}
/* TODO : DELETE THIS
// Set the broad-phase bounding volume
void Body::setBroadBoundingVolume(BroadBoundingVolume* broadBoundingVolume) {
assert(broadBoundingVolume);
this->broadBoundingVolume = broadBoundingVolume;
broadBoundingVolume->setBodyPointer(this);
}
*/
// Set the narrow-phase bounding volume
void Body::setNarrowBoundingVolume(NarrowBoundingVolume* narrowBoundingVolume) {
assert(narrowBoundingVolume);
this->narrowBoundingVolume = narrowBoundingVolume;
narrowBoundingVolume->setBodyPointer(this);
}

59
src/body/Body.h
View File

@ -30,11 +30,11 @@
#include <cassert>
#include "../mathematics/Transform.h"
#include "../body/AABB.h"
#include "../body/Shape.h"
// Namespace reactphysics3d
namespace reactphysics3d {
class NarrowBoundingVolume;
/* -------------------------------------------------------------------
Class Body :
@ -44,22 +44,21 @@ class NarrowBoundingVolume;
*/
class Body {
protected :
double mass; // Mass of the body
Transform transform; // Position and orientation of the body
Transform oldTransform; // Last position and orientation of the body
double interpolationFactor; // Interpolation factor used for the state interpolation
// TODO : DELETE BroadBoundingVolume* broadBoundingVolume; // Bounding volume used for the broad-phase collision detection
NarrowBoundingVolume* narrowBoundingVolume; // Bounding volume used for the narrow-phase collision detection
bool isMotionEnabled; // True if the body is able to move
bool isCollisionEnabled; // True if the body can collide with others bodies
AABB* aabb; // Axis-Aligned Bounding Box for Broad-Phase collision detection
void setNarrowBoundingVolume(NarrowBoundingVolume* narrowBoundingVolume); // Set the narrow-phase bounding volume
Shape* shape; // Collision shape of the body
double mass; // Mass of the body
Transform transform; // Position and orientation of the body
Transform oldTransform; // Last position and orientation of the body
double interpolationFactor; // Interpolation factor used for the state interpolation
bool isMotionEnabled; // True if the body is able to move
bool isCollisionEnabled; // True if the body can collide with others bodies
AABB* aabb; // Axis-Aligned Bounding Box for Broad-Phase collision detection
public :
Body(const Transform& transform, double mass) throw(std::invalid_argument); // Constructor
virtual ~Body(); // Destructor
Body(const Transform& transform, Shape* shape, double mass); // Constructor
virtual ~Body(); // Destructor
Shape* getShape() const; // Return the collision shape
void setShape(Shape* shape); // Set the collision shape
double getMass() const; // Return the mass of the body
void setMass(double mass); // Set the mass of the body
const Transform& getTransform() const; // Return the current position and orientation
@ -71,10 +70,22 @@ class Body {
void setIsMotionEnabled(bool isMotionEnabled); // Set the value to true if the body can move
bool getIsCollisionEnabled() const; // Return true if the body can collide with others bodies
void setIsCollisionEnabled(bool isCollisionEnabled); // Set the isCollisionEnabled value
const NarrowBoundingVolume* getNarrowBoundingVolume() const; // Return the narrow-phase bounding volume of the body
void updateOldTransform(); // Update the old transform with the current one
void updateAABB(); // Update the Axis-Aligned Bounding Box coordinates
};
// Return the collision shape
inline Shape* Body::getShape() const {
assert(shape);
return shape;
}
// Set the collision shape
inline void Body::setShape(Shape* shape) {
assert(shape);
this->shape = shape;
}
// Method that return the mass of the body
inline double Body::getMass() const {
return mass;
@ -131,24 +142,18 @@ inline void Body::setIsCollisionEnabled(bool isCollisionEnabled) {
this->isCollisionEnabled = isCollisionEnabled;
}
/* TODO : DELETE
// Return the broad-phase bounding volume
inline const BroadBoundingVolume* Body::getBroadBoundingVolume() const {
return broadBoundingVolume;
}
*/
// Return the oriented bounding box of the rigid body
inline const NarrowBoundingVolume* Body::getNarrowBoundingVolume() const {
return narrowBoundingVolume;
}
// Update the old transform with the current one
// This is used to compute the interpolated position and orientation of the body
inline void Body::updateOldTransform() {
oldTransform = transform;
}
// Update the rigid body in order to reflect a change in the body state
inline void Body::updateAABB() {
// Update the AABB
aabb->update(transform, shape->getLocalExtents());
}
}

4
src/body/BoundingSphere.h
View File

@ -26,7 +26,7 @@
#define BOUNDING_SPHERE_H
// Libraries
#include "NarrowBoundingVolume.h"
#include "Shape.h"
#include "../mathematics/mathematics.h"
// ReactPhysics3D namespace
@ -37,7 +37,7 @@ namespace reactphysics3d {
This class represents a sphere bounding volume.
-------------------------------------------------------------------
*/
class BoundingSphere : public NarrowBoundingVolume {
class BoundingSphere : public Shape {
protected :
Vector3D center; // Center point of the sphere
double radius; // Radius of the sphere

4
src/body/ConeShape.h
View File

@ -26,7 +26,7 @@
#define BOUNDING_CONE_H
// Libraries
#include "NarrowBoundingVolume.h"
#include "Shape.h"
#include "../mathematics/mathematics.h"
// ReactPhysics3D namespace
@ -37,7 +37,7 @@ namespace reactphysics3d {
This class represents a cone bounding volume.
-------------------------------------------------------------------
*/
class ConeShape : public NarrowBoundingVolume {
class ConeShape : public Shape {
protected :
Vector3D center; // Center point of the sphere
double radius; // Radius of the sphere

4
src/body/OBB.h
View File

@ -27,7 +27,7 @@
// Libraries
#include <cfloat>
#include "NarrowBoundingVolume.h"
#include "Shape.h"
#include "../mathematics/mathematics.h"
// ReactPhysics3D namespace
@ -41,7 +41,7 @@ namespace reactphysics3d {
rigid body given an orientation and a position to the box
-------------------------------------------------------------------
*/
class OBB : public NarrowBoundingVolume {
class OBB : public Shape {
private :
Vector3D extent; // Extent sizes of the box

34
src/body/RigidBody.cpp
View File

@ -24,31 +24,24 @@
// Libraries
#include "RigidBody.h"
#include "BroadBoundingVolume.h"
#include "NarrowBoundingVolume.h"
#include "Shape.h"
// We want to use the ReactPhysics3D namespace
using namespace reactphysics3d;
// We want to use the ReactPhysics3D namespace
using namespace reactphysics3d;
// Constructor
// TODO : Use a Transform in the constructor instead of "position" and "orientation"
RigidBody::RigidBody(const Transform& transform, double mass, const Matrix3x3& inertiaTensorLocal,
NarrowBoundingVolume* narrowBoundingVolume)
: Body(transform, mass), inertiaTensorLocal(inertiaTensorLocal),
RigidBody::RigidBody(const Transform& transform, double mass, const Matrix3x3& inertiaTensorLocal, Shape* shape)
: Body(transform, shape, mass), inertiaTensorLocal(inertiaTensorLocal),
inertiaTensorLocalInverse(inertiaTensorLocal.getInverse()), massInverse(1.0/mass) {
restitution = 1.0;
isMotionEnabled = true;
isCollisionEnabled = true;
interpolationFactor = 0.0;
// Set the bounding volume for the narrow-phase collision detection
setNarrowBoundingVolume(narrowBoundingVolume);
// Set the body pointer of the AABB and the shape
aabb->setBodyPointer(this);
shape->setBodyPointer(this);
// Update the orientation of the OBB according to the orientation of the rigid body
update();
assert(narrowBoundingVolume);
assert(shape);
assert(aabb);
}
// Destructor
@ -56,10 +49,3 @@ RigidBody::~RigidBody() {
};
// Update the rigid body in order to reflect a change in the body state
void RigidBody::update() {
// TODO : Remove the following code when using a Transform
// Update the AABB
aabb->update(transform, narrowBoundingVolume->getLocalExtents());
}

11
src/body/RigidBody.h
View File

@ -28,6 +28,7 @@
// Libraries
#include <cassert>
#include "Body.h"
#include "Shape.h"
#include "../mathematics/mathematics.h"
// Namespace reactphysics3d
@ -42,11 +43,6 @@ namespace reactphysics3d {
*/
class RigidBody : public Body {
protected :
// TODO : Remove some of the following variables
//Vector3D position; // Position of the center of mass of the body
//Vector3D oldPosition; // Old position used to compute the interpolated position
//Quaternion orientation; // Orientation quaternion of the body
//Quaternion oldOrientation; // Old orientation used to compute the interpolated orientation
Vector3D linearVelocity; // Linear velocity of the body
Vector3D angularVelocity; // Angular velocity of the body
Vector3D externalForce; // Current external force on the body
@ -58,8 +54,8 @@ class RigidBody : public Body {
public :
RigidBody(const Transform& transform, double mass,
const Matrix3x3& inertiaTensorLocal, NarrowBoundingVolume* narrowBoundingVolume); // Constructor // Copy-constructor
virtual ~RigidBody(); // Destructor
const Matrix3x3& inertiaTensorLocal, Shape* shape); // Constructor // Copy-constructor
virtual ~RigidBody(); // Destructor
Vector3D getLinearVelocity() const; // Return the linear velocity
void setLinearVelocity(const Vector3D& linearVelocity); // Set the linear velocity of the body
@ -79,7 +75,6 @@ class RigidBody : public Body {
double getRestitution() const; // Get the restitution coefficient
void setRestitution(double restitution) throw(std::invalid_argument); // Set the restitution coefficient
void update(); // Update the rigid body in order to reflect a change in the body state
};
// Return the linear velocity

6
src/body/NarrowBoundingVolume.cpp → src/body/Shape.cpp
View File

@ -23,17 +23,17 @@
********************************************************************************/
// Libraries
#include "NarrowBoundingVolume.h"
#include "Shape.h"
// We want to use the ReactPhysics3D namespace
using namespace reactphysics3d;
// Constructor
NarrowBoundingVolume::NarrowBoundingVolume() : BoundingVolume() {
Shape::Shape() {
}
// Destructor
NarrowBoundingVolume::~NarrowBoundingVolume() {
Shape::~Shape() {
}

40
src/body/NarrowBoundingVolume.h → src/body/Shape.h
View File

@ -23,36 +23,50 @@
* THE SOFTWARE. *
********************************************************************************/
#ifndef NARROW_BOUNDING_VOLUME_H
#define NARROW_BOUNDING_VOLUME_H
#ifndef SHAPE_H
#define SHAPE_H
// Libraries
#include "BoundingVolume.h"
#include "AABB.h"
#include <cassert>
#include "../mathematics/Vector3D.h"
// ReactPhysics3D namespace
namespace reactphysics3d {
// Declarations
class Body;
/* -------------------------------------------------------------------
Class NarrowBoundingVolume :
This class represents the volume that contains a rigid body
This volume will be used to compute the narrow-phase collision
detection.
Class Shape :
This abstract class represents the shape of a body that is used during
the narrow-phase collision detection.
-------------------------------------------------------------------
*/
class NarrowBoundingVolume : public BoundingVolume {
class Shape {
protected :
Body* bodyPointer; // Pointer to the owner body (not the abstract class Body but its derivative which is instanciable)
public :
NarrowBoundingVolume(); // Constructor
virtual ~NarrowBoundingVolume(); // Destructor
Shape(); // Constructor
virtual ~Shape(); // Destructor
// TODO : DELETE virtual AABB* computeAABB() const=0; // Return the corresponding AABB
Body* getBodyPointer() const; // Return the body pointer
void setBodyPointer(Body* bodyPointer); // Set the body pointer
virtual Vector3D getSupportPoint(const Vector3D& direction, double margin=0.0) const=0; // Return a support point in a given direction
virtual Vector3D getLocalExtents() const=0; // Return the local extents in x,y and z direction
};
// Return the body pointer
inline Body* Shape::getBodyPointer() const {
assert(bodyPointer != NULL);
return bodyPointer;
}
// Set the body pointer
inline void Shape::setBodyPointer(Body* bodyPointer) {
this->bodyPointer = bodyPointer;
}
}
#endif

5
src/collision/BroadPhaseAlgorithm.h
View File

@ -26,7 +26,8 @@
#define BROAD_PHASE_ALGORITHM_H
// Libraries
#include "../body/BoundingVolume.h"
#include <vector>
#include "../body/Body.h"
// Namespace ReactPhysics3D
namespace reactphysics3d {
@ -52,7 +53,7 @@ class BroadPhaseAlgorithm {
virtual ~BroadPhaseAlgorithm(); // Destructor
virtual void computePossibleCollisionPairs(std::vector<Body*> addedBodies, std::vector<Body*> removedBodies,
std::vector<std::pair<const Body*, const Body* > >& possibleCollisionPairs)=0; // Compute the possible collision pairs of bodies
std::vector<std::pair<Body*, Body*> >& possibleCollisionPairs)=0; // Compute the possible collision pairs of bodies
};
} // End of reactphysics3d namespace

9
src/collision/CollisionDetection.cpp
View File

@ -90,13 +90,12 @@ void CollisionDetection::computeNarrowPhase() {
for (unsigned int i=0; i<possibleCollisionPairs.size(); i++) {
ContactInfo* contactInfo = NULL;
const RigidBody* rigidBody1 = dynamic_cast<const RigidBody*>(possibleCollisionPairs.at(i).first);
const RigidBody* rigidBody2 = dynamic_cast<const RigidBody*>(possibleCollisionPairs.at(i).second);
Body* const body1 = possibleCollisionPairs.at(i).first;
Body* const body2 = possibleCollisionPairs.at(i).second;
// Use the narrow-phase collision detection algorithm to check if there really are a contact
if (narrowPhaseAlgorithm->testCollision(rigidBody1->getNarrowBoundingVolume(), rigidBody1->getTransform(),
rigidBody2->getNarrowBoundingVolume(), rigidBody2->getTransform(),
contactInfo)) {
if (narrowPhaseAlgorithm->testCollision(body1->getShape(), body1->getTransform(),
body2->getShape(), body2->getTransform(), contactInfo)) {
assert(contactInfo);
// Add the contact info the current list of collision informations

2
src/collision/CollisionDetection.h
View File

@ -47,7 +47,7 @@ namespace reactphysics3d {
class CollisionDetection {
private :
PhysicsWorld* world; // Pointer to the physics world
std::vector<std::pair<const Body*, const Body* > > possibleCollisionPairs; // Possible collision pairs of bodies (computed by broadphase)
std::vector<std::pair<Body*, Body*> > possibleCollisionPairs; // Possible collision pairs of bodies (computed by broadphase)
std::vector<ContactInfo*> contactInfos; // Contact informations (computed by narrowphase)
BroadPhaseAlgorithm* broadPhaseAlgorithm; // Broad-phase algorithm
NarrowPhaseAlgorithm* narrowPhaseAlgorithm; // Narrow-phase algorithm

5
src/collision/ContactInfo.cpp
View File

@ -28,13 +28,14 @@
using namespace reactphysics3d;
// Constructor for SAT
ContactInfo::ContactInfo(Body* const body1, Body* const body2, const Vector3D& normal, double penetrationDepth)
// TODO : DELETE THIS
ContactInfo::ContactInfo(Body* body1, Body* body2, const Vector3D& normal, double penetrationDepth)
: body1(body1), body2(body2), normal(normal), penetrationDepth(penetrationDepth), point1(0.0, 0.0, 0.0), point2(0.0, 0.0, 0.0) {
}
// Constructor for GJK
ContactInfo::ContactInfo(Body* const body1, Body* const body2, const Vector3D& normal,
ContactInfo::ContactInfo(Body* body1, Body* body2, const Vector3D& normal,
double penetrationDepth, const Vector3D& point1, const Vector3D& point2)
: body1(body1), body2(body2), normal(normal), penetrationDepth(penetrationDepth), point1(point1), point2(point2) {

4
src/collision/ContactInfo.h
View File

@ -49,9 +49,9 @@ struct ContactInfo {
const Vector3D normal; // Normal vector the the collision contact
const double penetrationDepth; // Penetration depth of the contact
ContactInfo(Body* const body1, Body* const body2, const Vector3D& normal,
ContactInfo(Body* body1, Body* body2, const Vector3D& normal,
double penetrationDepth); // Constructor for SAT
ContactInfo(Body* const body1, Body* const body2, const Vector3D& normal,
ContactInfo(Body* body1, Body* body2, const Vector3D& normal,
double penetrationDepth, const Vector3D& point1, const Vector3D& point2); // Constructor for GJK
};

33
src/collision/EPA/EPAAlgorithm.cpp
View File

@ -79,10 +79,8 @@ int EPAAlgorithm::isOriginInTetrahedron(const Vector3D& p1, const Vector3D& p2,
// intersect. An initial simplex that contains origin has been computed with
// GJK algorithm. The EPA Algorithm will extend this simplex polytope to find
// the correct penetration depth
bool EPAAlgorithm::computePenetrationDepthAndContactPoints(Simplex simplex, const NarrowBoundingVolume* const boundingVolume1,
const Transform& transform1,
const NarrowBoundingVolume* const boundingVolume2,
const Transform& transform2,
bool EPAAlgorithm::computePenetrationDepthAndContactPoints(Simplex simplex, const Shape* shape1, const Transform& transform1,
const Shape* shape2, const Transform& transform2,
Vector3D& v, ContactInfo*& contactInfo) {
Vector3D suppPointsA[MAX_SUPPORT_POINTS]; // Support points of object A in local coordinates
Vector3D suppPointsB[MAX_SUPPORT_POINTS]; // Support points of object B in local coordinates
@ -148,18 +146,18 @@ bool EPAAlgorithm::computePenetrationDepthAndContactPoints(Simplex simplex, cons
Vector3D v3 = rotationMat * v2;
// Compute the support point in the direction of v1
suppPointsA[2] = boundingVolume1->getSupportPoint(v1, OBJECT_MARGIN);
suppPointsB[2] = shape2ToShape1 * boundingVolume2->getSupportPoint(rotateToShape2 * v1.getOpposite(), OBJECT_MARGIN);
suppPointsA[2] = shape1->getSupportPoint(v1, OBJECT_MARGIN);
suppPointsB[2] = shape2ToShape1 * shape2->getSupportPoint(rotateToShape2 * v1.getOpposite(), OBJECT_MARGIN);
points[2] = suppPointsA[2] - suppPointsB[2];
// Compute the support point in the direction of v2
suppPointsA[3] = boundingVolume1->getSupportPoint(v2, OBJECT_MARGIN);
suppPointsB[3] = shape2ToShape1 * boundingVolume2->getSupportPoint(rotateToShape2 * v2.getOpposite(), OBJECT_MARGIN);
suppPointsA[3] = shape1->getSupportPoint(v2, OBJECT_MARGIN);
suppPointsB[3] = shape2ToShape1 * shape2->getSupportPoint(rotateToShape2 * v2.getOpposite(), OBJECT_MARGIN);
points[3] = suppPointsA[3] - suppPointsB[3];
// Compute the support point in the direction of v3
suppPointsA[4] = boundingVolume1->getSupportPoint(v3, OBJECT_MARGIN);
suppPointsB[4] = shape2ToShape1 * boundingVolume2->getSupportPoint(rotateToShape2 * v3.getOpposite(), OBJECT_MARGIN);
suppPointsA[4] = shape1->getSupportPoint(v3, OBJECT_MARGIN);
suppPointsB[4] = shape2ToShape1 * shape2->getSupportPoint(rotateToShape2 * v3.getOpposite(), OBJECT_MARGIN);
points[4] = suppPointsA[4] - suppPointsB[4];
// Now we have an hexahedron (two tetrahedron glued together). We can simply keep the
@ -253,11 +251,11 @@ bool EPAAlgorithm::computePenetrationDepthAndContactPoints(Simplex simplex, cons
Vector3D n = v1.cross(v2);
// Compute the two new vertices to obtain a hexahedron
suppPointsA[3] = boundingVolume1->getSupportPoint(n, OBJECT_MARGIN);
suppPointsB[3] = shape2ToShape1 * boundingVolume2->getSupportPoint(rotateToShape2 * n.getOpposite(), OBJECT_MARGIN);
suppPointsA[3] = shape1->getSupportPoint(n, OBJECT_MARGIN);
suppPointsB[3] = shape2ToShape1 * shape2->getSupportPoint(rotateToShape2 * n.getOpposite(), OBJECT_MARGIN);
points[3] = suppPointsA[3] - suppPointsB[3];
suppPointsA[4] = boundingVolume1->getSupportPoint(n.getOpposite(), OBJECT_MARGIN);
suppPointsB[4] = shape2ToShape1 * boundingVolume2->getSupportPoint(rotateToShape2 * n, OBJECT_MARGIN);
suppPointsA[4] = shape1->getSupportPoint(n.getOpposite(), OBJECT_MARGIN);
suppPointsB[4] = shape2ToShape1 * shape2->getSupportPoint(rotateToShape2 * n, OBJECT_MARGIN);
points[4] = suppPointsA[4] - suppPointsB[4];
// Construct the triangle faces
@ -326,8 +324,8 @@ bool EPAAlgorithm::computePenetrationDepthAndContactPoints(Simplex simplex, cons
}
// Compute the support point of the Minkowski difference (A-B) in the closest point direction
suppPointsA[nbVertices] = boundingVolume1->getSupportPoint(triangle->getClosestPoint(), OBJECT_MARGIN);
suppPointsB[nbVertices] = shape2ToShape1 * boundingVolume2->getSupportPoint(rotateToShape2 * triangle->getClosestPoint().getOpposite(), OBJECT_MARGIN);
suppPointsA[nbVertices] = shape1->getSupportPoint(triangle->getClosestPoint(), OBJECT_MARGIN);
suppPointsB[nbVertices] = shape2ToShape1 *shape2->getSupportPoint(rotateToShape2 * triangle->getClosestPoint().getOpposite(), OBJECT_MARGIN);
points[nbVertices] = suppPointsA[nbVertices] - suppPointsB[nbVertices];
int indexNewVertex = nbVertices;
@ -376,8 +374,7 @@ bool EPAAlgorithm::computePenetrationDepthAndContactPoints(Simplex simplex, cons
Vector3D normal = v.getUnit();
double penetrationDepth = v.length();
assert(penetrationDepth > 0.0);
contactInfo = new ContactInfo(boundingVolume1->getBodyPointer(), boundingVolume2->getBodyPointer(),
normal, penetrationDepth, pA, pB);
contactInfo = new ContactInfo(shape1->getBodyPointer(), shape2->getBodyPointer(), normal, penetrationDepth, pA, pB);
return true;
}

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

@ -27,7 +27,7 @@
// Libraries
#include "../GJK/Simplex.h"
#include "../../body/NarrowBoundingVolume.h"
#include "../../body/Shape.h"
#include "../ContactInfo.h"
#include "../../mathematics/mathematics.h"
#include "TriangleEPA.h"
@ -82,10 +82,8 @@ class EPAAlgorithm {
EPAAlgorithm(); // Constructor
~EPAAlgorithm(); // Destructor
bool computePenetrationDepthAndContactPoints(Simplex simplex, const NarrowBoundingVolume* const boundingVolume1,
const Transform& transform1,
const NarrowBoundingVolume* const boundingVolume2,
const Transform& transform2,
bool computePenetrationDepthAndContactPoints(Simplex simplex, const Shape* shape1, const Transform& transform1,
const Shape* shape2, const Transform& transform2,
Vector3D& v, ContactInfo*& contactInfo); // Compute the penetration depth with EPA algorithm
};

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

@ -59,9 +59,11 @@ GJKAlgorithm::~GJKAlgorithm() {
// algorithm on the enlarged object to obtain a simplex polytope that contains the
// origin, they we give that simplex polytope to the EPA algorithm which will compute
// the correct penetration depth and contact points between the enlarged objects.
bool GJKAlgorithm::testCollision(const NarrowBoundingVolume* const boundingVolume1, const Transform& transform1,
const NarrowBoundingVolume* const boundingVolume2, const Transform& transform2,
ContactInfo*& contactInfo) {
bool GJKAlgorithm::testCollision(const Shape* shape1, const Transform& transform1,
const Shape* shape2, const Transform& transform2, ContactInfo*& contactInfo) {
assert(shape1 != shape2);
Vector3D suppA; // Support point of object A
Vector3D suppB; // Support point of object B
Vector3D w; // Support point of Minkowski difference A-B
@ -69,6 +71,8 @@ bool GJKAlgorithm::testCollision(const NarrowBoundingVolume* const boundingVolum
Vector3D pB; // Closest point of object B
double vDotw;
double prevDistSquare;
Body* const body1 = shape1->getBodyPointer();
Body* const body2 = shape2->getBodyPointer();
// Transform a point from body space of shape 2 to body space of shape 1 (the GJK algorithm is done in body space of shape 1)
Transform shape2ToShape1 = transform1.inverse() * transform2;
@ -76,8 +80,6 @@ bool GJKAlgorithm::testCollision(const NarrowBoundingVolume* const boundingVolum
// Matrix that transform a direction from body space of shape 1 into body space of shape 2
Matrix3x3 rotateToShape2 = transform2.getOrientation().getTranspose() * transform1.getOrientation();
assert(boundingVolume1 != boundingVolume2);
// Initialize the margin (sum of margins of both objects)
double margin = 2 * OBJECT_MARGIN;
double marginSquare = margin * margin;
@ -96,8 +98,8 @@ bool GJKAlgorithm::testCollision(const NarrowBoundingVolume* const boundingVolum
do {
// Compute the support points for original objects (without margins) A and B
suppA = boundingVolume1->getSupportPoint(v.getOpposite());
suppB = shape2ToShape1 * boundingVolume2->getSupportPoint(rotateToShape2 * v);
suppA = shape1->getSupportPoint(v.getOpposite());
suppB = shape2ToShape1 * shape2->getSupportPoint(rotateToShape2 * v);
// Compute the support point for the Minkowski difference A-B
w = suppA - suppB;
@ -125,8 +127,7 @@ bool GJKAlgorithm::testCollision(const NarrowBoundingVolume* const boundingVolum
// Compute the contact info
Vector3D normal = transform1.getOrientation() * v.getOpposite().getUnit();
double penetrationDepth = margin - dist;
contactInfo = new ContactInfo(boundingVolume1->getBodyPointer(), boundingVolume2->getBodyPointer(),
normal, penetrationDepth, pA, pB);
contactInfo = new ContactInfo(body1, body2, normal, penetrationDepth, pA, pB);
// There is an intersection, therefore we return true
return true;
@ -150,8 +151,7 @@ bool GJKAlgorithm::testCollision(const NarrowBoundingVolume* const boundingVolum
// Compute the contact info
Vector3D normal = transform1.getOrientation() * v.getOpposite().getUnit();
double penetrationDepth = margin - dist;
contactInfo = new ContactInfo(boundingVolume1->getBodyPointer(), boundingVolume2->getBodyPointer(),
normal, penetrationDepth, pA, pB);
contactInfo = new ContactInfo(body1, body2, normal, penetrationDepth, pA, pB);
// There is an intersection, therefore we return true
return true;
@ -173,8 +173,7 @@ bool GJKAlgorithm::testCollision(const NarrowBoundingVolume* const boundingVolum
// Compute the contact info
Vector3D normal = transform1.getOrientation() * v.getOpposite().getUnit();
double penetrationDepth = margin - dist;
contactInfo = new ContactInfo(boundingVolume1->getBodyPointer(), boundingVolume2->getBodyPointer(),
normal, penetrationDepth, pA, pB);
contactInfo = new ContactInfo(body1, body2, normal, penetrationDepth, pA, pB);
// There is an intersection, therefore we return true
return true;
@ -204,8 +203,7 @@ bool GJKAlgorithm::testCollision(const NarrowBoundingVolume* const boundingVolum
// Compute the contact info
Vector3D normal = transform1.getOrientation() * v.getOpposite().getUnit();
double penetrationDepth = margin - dist;
contactInfo = new ContactInfo(boundingVolume1->getBodyPointer(), boundingVolume2->getBodyPointer(),
normal, penetrationDepth, pA, pB);
contactInfo = new ContactInfo(body1, body2, normal, penetrationDepth, pA, pB);
// There is an intersection, therefore we return true
return true;
@ -220,7 +218,7 @@ bool GJKAlgorithm::testCollision(const NarrowBoundingVolume* const boundingVolum
// enlarged objects to compute a simplex polytope that contains the origin. Then, we give that simplex
// polytope to the EPA algorithm to compute the correct penetration depth and contact points between
// the enlarged objects.
return computePenetrationDepthForEnlargedObjects(boundingVolume1, transform1, boundingVolume2, transform2, contactInfo, v);
return computePenetrationDepthForEnlargedObjects(shape1, transform1, shape2, transform2, contactInfo, v);
}
// This method runs the GJK algorithm on the two enlarged objects (with margin)
@ -228,8 +226,8 @@ bool GJKAlgorithm::testCollision(const NarrowBoundingVolume* const boundingVolum
// assumed to intersect in the original objects (without margin). Therefore such
// a polytope must exist. Then, we give that polytope to the EPA algorithm to
// compute the correct penetration depth and contact points of the enlarged objects.
bool GJKAlgorithm::computePenetrationDepthForEnlargedObjects(const NarrowBoundingVolume* const boundingVolume1, const Transform& transform1,
const NarrowBoundingVolume* const boundingVolume2, const Transform& transform2,
bool GJKAlgorithm::computePenetrationDepthForEnlargedObjects(const Shape* const shape1, const Transform& transform1,
const Shape* const shape2, const Transform& transform2,
ContactInfo*& contactInfo, Vector3D& v) {
Simplex simplex;
Vector3D suppA;
@ -247,8 +245,8 @@ bool GJKAlgorithm::computePenetrationDepthForEnlargedObjects(const NarrowBoundin
do {
// Compute the support points for the enlarged object A and B
suppA = boundingVolume1->getSupportPoint(v.getOpposite(), OBJECT_MARGIN);
suppB = shape2ToShape1 * boundingVolume2->getSupportPoint(rotateToShape2 * v, OBJECT_MARGIN);
suppA = shape1->getSupportPoint(v.getOpposite(), OBJECT_MARGIN);
suppB = shape2ToShape1 * shape2->getSupportPoint(rotateToShape2 * v, OBJECT_MARGIN);
// Compute the support point for the Minkowski difference A-B
w = suppA - suppB;
@ -284,6 +282,5 @@ bool GJKAlgorithm::computePenetrationDepthForEnlargedObjects(const NarrowBoundin
// Give the simplex computed with GJK algorithm to the EPA algorithm which will compute the correct
// penetration depth and contact points between the two enlarged objects
return algoEPA.computePenetrationDepthAndContactPoints(simplex, boundingVolume1, transform1,
boundingVolume2, transform2, v, contactInfo);
return algoEPA.computePenetrationDepthAndContactPoints(simplex, shape1, transform1, shape2, transform2, v, contactInfo);
}

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

@ -28,7 +28,7 @@
// Libraries
#include "../NarrowPhaseAlgorithm.h"
#include "../ContactInfo.h"
#include "../../body/NarrowBoundingVolume.h"
#include "../../body/Shape.h"
#include "../EPA/EPAAlgorithm.h"
@ -61,23 +61,19 @@ class GJKAlgorithm : public NarrowPhaseAlgorithm {
private :
EPAAlgorithm algoEPA; // EPA Algorithm
bool computePenetrationDepthForEnlargedObjects(const NarrowBoundingVolume* const boundingVolume1,
const Transform& transform1,
const NarrowBoundingVolume* const boundingVolume2,
const Transform& transform2,
bool computePenetrationDepthForEnlargedObjects(const Shape* shape1, const Transform& transform1,
const Shape* shape2, const Transform& transform2,
ContactInfo*& contactInfo, Vector3D& v); // Compute the penetration depth for enlarged objects
public :
GJKAlgorithm(); // Constructor
~GJKAlgorithm(); // Destructor
virtual bool testCollision(const NarrowBoundingVolume* const boundingVolume1, const Transform& transform1,
const NarrowBoundingVolume* const boundingVolume2, const Transform& transform2,
ContactInfo*& contactInfo); // Return true and compute a contact info if the two bounding volume collide
virtual bool testCollision(const Shape* shape1, const Transform& transform1,
const Shape* shape2, const Transform& transform2,
ContactInfo*& contactInfo); // Return true and compute a contact info if the two bounding volume collide
};
} // End of the ReactPhysics3D namespace
// TODO : Check what would be a correct value for the OBJECT_MARGIN constant
#endif

10
src/collision/NarrowPhaseAlgorithm.h
View File

@ -26,7 +26,7 @@
#define NARROW_PHASE_ALGORITHM_H
// Libraries
#include "../body/BoundingVolume.h"
#include "../body/Body.h"
#include "ContactInfo.h"
// Namespace ReactPhysics3D
@ -47,11 +47,9 @@ class NarrowPhaseAlgorithm {
NarrowPhaseAlgorithm(); // Constructor
virtual ~NarrowPhaseAlgorithm(); // Destructor
virtual bool testCollision(const NarrowBoundingVolume* const boundingVolume1,
const Transform& transform1,
const NarrowBoundingVolume* const boundingVolume2,
const Transform& transform2,
ContactInfo*& contactInfo)=0; // Return true and compute a contact info if the two bounding volume collide
virtual bool testCollision(const Shape* shape1, const Transform& transform1,
const Shape* shape2, const Transform& transform2,
ContactInfo*& contactInfo)=0; // Return true and compute a contact info if the two bounding volume collide
};
} // End of reactphysics3d namespace

2
src/collision/SAPAlgorithm.cpp
View File

@ -78,7 +78,7 @@ void SAPAlgorithm::addBodiesAABB(vector<Body*> bodies) {
// computation. This methods computes the current possible collision pairs of
// bodies and update the "possibleCollisionPairs" argument.
void SAPAlgorithm::computePossibleCollisionPairs(vector<Body*> addedBodies, vector<Body*> removedBodies,
vector<pair<const Body*, const Body* > >& possibleCollisionPairs) {
vector<pair<Body*, Body*> >& possibleCollisionPairs) {
double variance[3]; // Variance of the distribution of the AABBs on the three x, y and z axis
double esperance[] = {0.0, 0.0, 0.0}; // Esperance of the distribution of the AABBs on the three x, y and z axis
double esperanceSquare[] = {0.0, 0.0, 0.0}; // Esperance of the square of the distribution values of the AABBs on the three x, y and z axis

2
src/collision/SAPAlgorithm.h
View File

@ -62,7 +62,7 @@ class SAPAlgorithm : public BroadPhaseAlgorithm {
virtual ~SAPAlgorithm(); // Destructor
virtual void computePossibleCollisionPairs(std::vector<Body*> addedBodies, std::vector<Body*> removedBodies,
std::vector<std::pair<const Body*, const Body* > >& possibleCollisionPairs); // Compute the possible collision pairs of bodies
std::vector<std::pair<Body*, Body*> >& possibleCollisionPairs); // Compute the possible collision pairs of bodies
};
// Static method that compare two AABBs. This method will be used to compare to AABBs

112
src/collision/SATAlgorithm.cpp
View File

@ -27,6 +27,7 @@
#include "../body/OBB.h"
#include "../body/RigidBody.h"
#include "../constraint/Contact.h"
#include "../mathematics/Transform.h"
#include <algorithm>
#include <cfloat>
#include <cmath>
@ -51,17 +52,16 @@ SATAlgorithm::~SATAlgorithm() {
// Return true and compute a contact info if the two bounding volume collide.
// The method returns false if there is no collision between the two bounding volumes.
bool SATAlgorithm::testCollision(const NarrowBoundingVolume* const boundingVolume1, const Transform& transform1,
const NarrowBoundingVolume* const boundingVolume2, const Transform& transform2,
ContactInfo*& contactInfo) {
bool SATAlgorithm::testCollision(const Body* body1, const Body* body2, ContactInfo*& contactInfo) {
assert(boundingVolume1 != boundingVolume2);
assert(body1 != body2);
// If the two bounding volumes are OBB
//const OBB* const obb1 = dynamic_cast<const OBB* const>(boundingVolume1);
//const OBB* const obb2 = dynamic_cast<const OBB* const>(boundingVolume2);
const OBB* const obb1 = dynamic_cast<const OBB* const>(boundingVolume1);
const OBB* const obb2 = dynamic_cast<const OBB* const>(boundingVolume2);
const Transform& transform1 = body1->getTransform();
const Transform& transform2 = body2->getTransform();
const RigidBody* rigidBody1 = dynamic_cast<const RigidBody*>(body1);
const RigidBody* rigidBody2 = dynamic_cast<const RigidBody*>(body2);
const OBB* obb1 = dynamic_cast<const OBB*>(rigidBody1->getShape());
const OBB* obb2 = dynamic_cast<const OBB*>(rigidBody2->getShape());
// If the two bounding volumes are OBB
if (obb1 && obb2) {
@ -81,8 +81,8 @@ bool SATAlgorithm::testCollision(const NarrowBoundingVolume* const boundingVolum
// OBB are the six face normals (3 for each OBB) and the nine vectors V = Ai x Bj where Ai is the ith face normal
// vector of OBB 1 and Bj is the jth face normal vector of OBB 2. We will use the notation Ai for the ith face
// normal of OBB 1 and Bj for the jth face normal of OBB 2.
bool SATAlgorithm::computeCollisionTest(const OBB* const obb1, const Transform& transform1,
const OBB* const obb2, const Transform& transform2, ContactInfo*& contactInfo) const {
bool SATAlgorithm::computeCollisionTest(const OBB* obb1, const Transform& transform1,
const OBB* obb2, const Transform& transform2, ContactInfo*& contactInfo) const {
double center; // Center of a projection interval
@ -121,8 +121,8 @@ bool SATAlgorithm::computeCollisionTest(const OBB* const obb1, const Transform&
}
udc1[0] = transform1.getOrientation().getColumn(0).dot(boxDistance);
center = udc1[0];
radius1 = obb1->getExtent(0);
radius2 = obb2->getExtent(0)*absC[0][0] + obb2->getExtent(1)*absC[0][1] + obb2->getExtent(2) * absC[0][2];
radius1 = obb1->getExtent().getX();
radius2 = obb2->getExtent().getX()*absC[0][0] + obb2->getExtent().getY()*absC[0][1] + obb2->getExtent().getZ() * absC[0][2];
min1 = -radius1;
max1 = radius1;
min2 = center - radius2;
@ -133,7 +133,7 @@ bool SATAlgorithm::computeCollisionTest(const OBB* const obb1, const Transform&
}
else if (penetrationDepth < minPenetrationDepth) { // Interval 1 and 2 overlap with a smaller penetration depth on this axis
minPenetrationDepth = penetrationDepth; // Update the minimum penetration depth
normal = computeContactNormal(obb1->getAxis(0), boxDistance); // Compute the contact normal with the correct sign
normal = computeContactNormal(transform1.getOrientation().getColumn(0), boxDistance); // Compute the contact normal with the correct sign
}
// Axis A1
@ -146,8 +146,8 @@ bool SATAlgorithm::computeCollisionTest(const OBB* const obb1, const Transform&
}
udc1[1] = transform1.getOrientation().getColumn(1).dot(boxDistance);
center = udc1[1];
radius1 = obb1->getExtent(1);
radius2 = obb2->getExtent(0)*absC[1][0] + obb2->getExtent(1)*absC[1][1] + obb2->getExtent(2) * absC[1][2];
radius1 = obb1->getExtent().getY();
radius2 = obb2->getExtent().getX()*absC[1][0] + obb2->getExtent().getY()*absC[1][1] + obb2->getExtent().getZ() * absC[1][2];
min1 = -radius1;
max1 = radius1;
min2 = center - radius2;
@ -158,7 +158,7 @@ bool SATAlgorithm::computeCollisionTest(const OBB* const obb1, const Transform&
}
else if (penetrationDepth < minPenetrationDepth) { // Interval 1 and 2 overlap with a smaller penetration depth on this axis
minPenetrationDepth = penetrationDepth; // Update the minimum penetration depth
normal = computeContactNormal(obb1->getAxis(1), boxDistance); // Compute the contact normal with the correct sign
normal = computeContactNormal(transform1.getOrientation().getColumn(1), boxDistance); // Compute the contact normal with the correct sign
}
// Axis A2
@ -171,8 +171,8 @@ bool SATAlgorithm::computeCollisionTest(const OBB* const obb1, const Transform&
}
udc1[2] = transform1.getOrientation().getColumn(2).dot(boxDistance);
center = udc1[2];
radius1 = obb1->getExtent(2);
radius2 = obb2->getExtent(0)*absC[2][0] + obb2->getExtent(1)*absC[2][1] + obb2->getExtent(2)*absC[2][2];
radius1 = obb1->getExtent().getZ();
radius2 = obb2->getExtent().getX()*absC[2][0] + obb2->getExtent().getY()*absC[2][1] + obb2->getExtent().getZ()*absC[2][2];
min1 = -radius1;
max1 = radius1;
min2 = center - radius2;
@ -183,14 +183,14 @@ bool SATAlgorithm::computeCollisionTest(const OBB* const obb1, const Transform&
}
else if (penetrationDepth < minPenetrationDepth) { // Interval 1 and 2 overlap with a smaller penetration depth on this axis
minPenetrationDepth = penetrationDepth; // Update the minimum penetration depth
normal = computeContactNormal(obb1->getAxis(2), boxDistance); // Compute the contact normal with the correct sign
normal = computeContactNormal(transform1.getOrientation().getColumn(2), boxDistance); // Compute the contact normal with the correct sign
}
// Axis B0
udc2[0] = transform2.getOrientation().getColumn(0).dot(boxDistance);
center = udc2[0];
radius1 = obb1->getExtent(0)*absC[0][0] + obb1->getExtent(1)*absC[1][0] + obb1->getExtent(2) * absC[2][0];
radius2 = obb2->getExtent(0);
radius1 = obb1->getExtent().getX()*absC[0][0] + obb1->getExtent().getY()*absC[1][0] + obb1->getExtent().getZ() * absC[2][0];
radius2 = obb2->getExtent().getX();
min1 = -radius1;
max1 = radius1;
min2 = center - radius2;
@ -201,14 +201,14 @@ bool SATAlgorithm::computeCollisionTest(const OBB* const obb1, const Transform&
}
else if (penetrationDepth < minPenetrationDepth) { // Interval 1 and 2 overlap with a smaller penetration depth on this axis
minPenetrationDepth = penetrationDepth; // Update the minimum penetration depth
normal = computeContactNormal(obb2->getAxis(0), boxDistance); // Compute the contact normal with the correct sign
normal = computeContactNormal(transform2.getOrientation().getColumn(0), boxDistance); // Compute the contact normal with the correct sign
}
// Axis B1
udc2[1] = transform2.getOrientation().getColumn(1).dot(boxDistance);
center = udc2[1];
radius1 = obb1->getExtent(0)*absC[0][1] + obb1->getExtent(1)*absC[1][1] + obb1->getExtent(2) * absC[2][1];
radius2 = obb2->getExtent(1);
radius1 = obb1->getExtent().getX()*absC[0][1] + obb1->getExtent().getY()*absC[1][1] + obb1->getExtent().getZ() * absC[2][1];
radius2 = obb2->getExtent().getY();
min1 = - radius1;
max1 = radius1;
min2 = center - radius2;
@ -219,14 +219,14 @@ bool SATAlgorithm::computeCollisionTest(const OBB* const obb1, const Transform&
}
else if (penetrationDepth < minPenetrationDepth) { // Interval 1 and 2 overlap with a smaller penetration depth on this axis
minPenetrationDepth = penetrationDepth; // Update the minimum penetration depth
normal = computeContactNormal(obb2->getAxis(1), boxDistance); // Compute the contact normal with the correct sign
normal = computeContactNormal(transform2.getOrientation().getColumn(1), boxDistance); // Compute the contact normal with the correct sign
}
// Axis B2
udc2[2] = transform2.getOrientation().getColumn(2).dot(boxDistance);
center = udc2[2];
radius1 = obb1->getExtent(0)*absC[0][2] + obb1->getExtent(1)*absC[1][2] + obb1->getExtent(2)*absC[2][2];
radius2 = obb2->getExtent(2);
radius1 = obb1->getExtent().getX()*absC[0][2] + obb1->getExtent().getY()*absC[1][2] + obb1->getExtent().getZ()*absC[2][2];
radius2 = obb2->getExtent().getZ();
min1 = - radius1;
max1 = radius1;
min2 = center - radius2;
@ -237,7 +237,7 @@ bool SATAlgorithm::computeCollisionTest(const OBB* const obb1, const Transform&
}
else if (penetrationDepth < minPenetrationDepth) { // Interval 1 and 2 overlap with a smaller penetration depth on this axis
minPenetrationDepth = penetrationDepth; // Update the minimum penetration depth
normal = computeContactNormal(obb2->getAxis(2), boxDistance); // Compute the contact normal with the correct sign
normal = computeContactNormal(transform2.getOrientation().getColumn(2), boxDistance); // Compute the contact normal with the correct sign
}
// If there exists a parallel pair of face normals
@ -253,8 +253,8 @@ bool SATAlgorithm::computeCollisionTest(const OBB* const obb1, const Transform&
// Axis A0 x B0
center = udc1[2] * c[1][0] - udc1[1] * c[2][0];
radius1 = obb1->getExtent(1) * absC[2][0] + obb1->getExtent(2) * absC[1][0];
radius2 = obb2->getExtent(1) * absC[0][2] + obb2->getExtent(2) * absC[0][1];
radius1 = obb1->getExtent().getY() * absC[2][0] + obb1->getExtent().getZ() * absC[1][0];
radius2 = obb2->getExtent().getY() * absC[0][2] + obb2->getExtent().getZ() * absC[0][1];
min1 = -radius1;
max1 = radius1;
min2 = center - radius2;
@ -265,13 +265,13 @@ bool SATAlgorithm::computeCollisionTest(const OBB* const obb1, const Transform&
}
else if (penetrationDepth < minPenetrationDepth) { // Interval 1 and 2 overlap with a smaller penetration depth on this axis
minPenetrationDepth = penetrationDepth; // Update the minimum penetration depth
normal = computeContactNormal(obb1->getAxis(0).cross(obb2->getAxis(0)), boxDistance); // Compute the contact normal with the correct sign
normal = computeContactNormal(transform1.getOrientation().getColumn(0).cross(transform2.getOrientation().getColumn(0)), boxDistance); // Compute the contact normal with the correct sign
}
// Axis A0 x B1
center = udc1[2] * c[1][1] - udc1[1] * c[2][1];
radius1 = obb1->getExtent(1) * absC[2][1] + obb1->getExtent(2) * absC[1][1];
radius2 = obb2->getExtent(0) * absC[0][2] + obb2->getExtent(2) * absC[0][0];
radius1 = obb1->getExtent().getY() * absC[2][1] + obb1->getExtent().getZ() * absC[1][1];
radius2 = obb2->getExtent().getX() * absC[0][2] + obb2->getExtent().getZ() * absC[0][0];
min1 = -radius1;
max1 = radius1;
min2 = center - radius2;
@ -282,13 +282,13 @@ bool SATAlgorithm::computeCollisionTest(const OBB* const obb1, const Transform&
}
else if (penetrationDepth < minPenetrationDepth) { // Interval 1 and 2 overlap with a smaller penetration depth on this axis
minPenetrationDepth = penetrationDepth; // Update the minimum penetration depth
normal = computeContactNormal(obb1->getAxis(0).cross(obb2->getAxis(1)), boxDistance); // Compute the contact normal with the correct sign
normal = computeContactNormal(transform1.getOrientation().getColumn(0).cross(transform2.getOrientation().getColumn(1)), boxDistance); // Compute the contact normal with the correct sign
}
// Axis A0 x B2
center = udc1[2] * c[1][2] - udc1[1] * c[2][2];
radius1 = obb1->getExtent(1) * absC[2][2] + obb1->getExtent(2) * absC[1][2];
radius2 = obb2->getExtent(0) * absC[0][1] + obb2->getExtent(1) * absC[0][0];
radius1 = obb1->getExtent().getY() * absC[2][2] + obb1->getExtent().getZ() * absC[1][2];
radius2 = obb2->getExtent().getX() * absC[0][1] + obb2->getExtent().getY() * absC[0][0];
min1 = -radius1;
max1 = radius1;
min2 = center - radius2;
@ -299,13 +299,13 @@ bool SATAlgorithm::computeCollisionTest(const OBB* const obb1, const Transform&
}
else if (penetrationDepth < minPenetrationDepth) { // Interval 1 and 2 overlap with a smaller penetration depth on this axis
minPenetrationDepth = penetrationDepth; // Update the minimum penetration depth
normal = computeContactNormal(obb1->getAxis(0).cross(obb2->getAxis(2)), boxDistance); // Compute the contact normal with the correct sign
normal = computeContactNormal(transform1.getOrientation().getColumn(0).cross(transform2.getOrientation().getColumn(2)), boxDistance); // Compute the contact normal with the correct sign
}
// Axis A1 x B0
center = udc1[0] * c[2][0] - udc1[2] * c[0][0];
radius1 = obb1->getExtent(0) * absC[2][0] + obb1->getExtent(2) * absC[0][0];
radius2 = obb2->getExtent(1) * absC[1][2] + obb2->getExtent(2) * absC[1][1];
radius1 = obb1->getExtent().getX() * absC[2][0] + obb1->getExtent().getZ() * absC[0][0];
radius2 = obb2->getExtent().getY() * absC[1][2] + obb2->getExtent().getZ() * absC[1][1];
min1 = -radius1;
max1 = radius1;
min2 = center - radius2;
@ -316,13 +316,13 @@ bool SATAlgorithm::computeCollisionTest(const OBB* const obb1, const Transform&
}
else if (penetrationDepth < minPenetrationDepth) { // Interval 1 and 2 overlap with a smaller penetration depth on this axis
minPenetrationDepth = penetrationDepth; // Update the minimum penetration depth
normal = computeContactNormal(obb1->getAxis(1).cross(obb2->getAxis(0)), boxDistance); // Compute the contact normal with the correct sign
normal = computeContactNormal(transform1.getOrientation().getColumn(1).cross(transform2.getOrientation().getColumn(0)), boxDistance); // Compute the contact normal with the correct sign
}
// Axis A1 x B1
center = udc1[0] * c[2][1] - udc1[2] * c[0][1];
radius1 = obb1->getExtent(0) * absC[2][1] + obb1->getExtent(2) * absC[0][1];
radius2 = obb2->getExtent(0) * absC[1][2] + obb2->getExtent(2) * absC[1][0];
radius1 = obb1->getExtent().getX() * absC[2][1] + obb1->getExtent().getZ() * absC[0][1];
radius2 = obb2->getExtent().getX() * absC[1][2] + obb2->getExtent().getZ() * absC[1][0];
min1 = -radius1;
max1 = radius1;
min2 = center - radius2;
@ -333,13 +333,13 @@ bool SATAlgorithm::computeCollisionTest(const OBB* const obb1, const Transform&
}
else if (penetrationDepth < minPenetrationDepth) { // Interval 1 and 2 overlap with a smaller penetration depth on this axis
minPenetrationDepth = penetrationDepth; // Update the minimum penetration depth
normal = computeContactNormal(obb1->getAxis(1).cross(obb2->getAxis(1)), boxDistance); // Compute the contact normal with the correct sign
normal = computeContactNormal(transform1.getOrientation().getColumn(1).cross(transform2.getOrientation().getColumn(1)), boxDistance); // Compute the contact normal with the correct sign
}
// Axis A1 x B2
center = udc1[0] * c[2][2] - udc1[2] * c[0][2];
radius1 = obb1->getExtent(0) * absC[2][2] + obb1->getExtent(2) * absC[0][2];
radius2 = obb2->getExtent(0) * absC[1][1] + obb2->getExtent(1) * absC[1][0];
radius1 = obb1->getExtent().getX() * absC[2][2] + obb1->getExtent().getZ() * absC[0][2];
radius2 = obb2->getExtent().getX() * absC[1][1] + obb2->getExtent().getY() * absC[1][0];
min1 = -radius1;
max1 = radius1;
min2 = center - radius2;
@ -350,13 +350,13 @@ bool SATAlgorithm::computeCollisionTest(const OBB* const obb1, const Transform&
}
else if (penetrationDepth < minPenetrationDepth) { // Interval 1 and 2 overlap with a smaller penetration depth on this axis
minPenetrationDepth = penetrationDepth; // Update the minimum penetration depth
normal = computeContactNormal(obb1->getAxis(1).cross(obb2->getAxis(2)), boxDistance); // Compute the contact normal with the correct sign
normal = computeContactNormal(transform1.getOrientation().getColumn(1).cross(transform2.getOrientation().getColumn(2)), boxDistance); // Compute the contact normal with the correct sign
}
// Axis A2 x B0
center = udc1[1] * c[0][0] - udc1[0] * c[1][0];
radius1 = obb1->getExtent(0) * absC[1][0] + obb1->getExtent(1) * absC[0][0];
radius2 = obb2->getExtent(1) * absC[2][2] + obb2->getExtent(2) * absC[2][1];
radius1 = obb1->getExtent().getX() * absC[1][0] + obb1->getExtent().getY() * absC[0][0];
radius2 = obb2->getExtent().getY() * absC[2][2] + obb2->getExtent().getZ() * absC[2][1];
min1 = -radius1;
max1 = radius1;
min2 = center - radius2;
@ -367,13 +367,13 @@ bool SATAlgorithm::computeCollisionTest(const OBB* const obb1, const Transform&
}
else if (penetrationDepth < minPenetrationDepth) { // Interval 1 and 2 overlap with a smaller penetration depth on this axis
minPenetrationDepth = penetrationDepth; // Update the minimum penetration depth
normal = computeContactNormal(obb1->getAxis(2).cross(obb2->getAxis(0)), boxDistance); // Compute the contact normal with the correct sign
normal = computeContactNormal(transform1.getOrientation().getColumn(2).cross(transform2.getOrientation().getColumn(0)), boxDistance); // Compute the contact normal with the correct sign
}
// Axis A2 x B1
center = udc1[1] * c[0][1] - udc1[0] * c[1][1];
radius1 = obb1->getExtent(0) * absC[1][1] + obb1->getExtent(1) * absC[0][1];
radius2 = obb2->getExtent(0) * absC[2][2] + obb2->getExtent(2) * absC[2][0];
radius1 = obb1->getExtent().getX() * absC[1][1] + obb1->getExtent().getY() * absC[0][1];
radius2 = obb2->getExtent().getX() * absC[2][2] + obb2->getExtent().getZ() * absC[2][0];
min1 = -radius1;
max1 = radius1;
min2 = center - radius2;
@ -384,13 +384,13 @@ bool SATAlgorithm::computeCollisionTest(const OBB* const obb1, const Transform&
}
else if (penetrationDepth < minPenetrationDepth) { // Interval 1 and 2 overlap with a smaller penetration depth on this axis
minPenetrationDepth = penetrationDepth; // Update the minimum penetration depth
normal = computeContactNormal(obb1->getAxis(2).cross(obb2->getAxis(1)), boxDistance); // Compute the contact normal with the correct sign
normal = computeContactNormal(transform1.getOrientation().getColumn(2).cross(transform2.getOrientation().getColumn(1)), boxDistance); // Compute the contact normal with the correct sign
}
// Axis A2 x B2
center = udc1[1] * c[0][2] - udc1[0] * c[1][2];
radius1 = obb1->getExtent(0) * absC[1][2] + obb1->getExtent(1) * absC[0][2];
radius2 = obb2->getExtent(0) * absC[2][1] + obb2->getExtent(1) * absC[2][0];
radius1 = obb1->getExtent().getX() * absC[1][2] + obb1->getExtent().getY() * absC[0][2];
radius2 = obb2->getExtent().getX() * absC[2][1] + obb2->getExtent().getY() * absC[2][0];
min1 = -radius1;
max1 = radius1;
min2 = center - radius2;
@ -401,7 +401,7 @@ bool SATAlgorithm::computeCollisionTest(const OBB* const obb1, const Transform&
}
else if (penetrationDepth < minPenetrationDepth) { // Interval 1 and 2 overlap with a smaller penetration depth on this axis
minPenetrationDepth = penetrationDepth; // Update the minimum penetration depth
normal = computeContactNormal(obb1->getAxis(2).cross(obb2->getAxis(2)), boxDistance); // Compute the contact normal with the correct sign
normal = computeContactNormal(transform1.getOrientation().getColumn(2).cross(transform2.getOrientation().getColumn(2)), boxDistance); // Compute the contact normal with the correct sign
}
// Compute the contact info

10
src/collision/SATAlgorithm.h
View File

@ -50,8 +50,8 @@ namespace reactphysics3d {
*/
class SATAlgorithm : public NarrowPhaseAlgorithm {
private :
bool computeCollisionTest(const OBB* const obb1, const Transform& transform1,
const OBB* const obb2, const Transform& transform2,
bool computeCollisionTest(const OBB* obb1, const Transform& transform1,
const OBB* obb2, const Transform& transform2,
ContactInfo*& contactInfo) const; // Return true and compute a contact info if the two OBB collide
double computePenetrationDepth(double min1, double max1, double min2, double max2) const; // Compute the penetration depth of two projection intervals
Vector3D computeContactNormal(const Vector3D& axis, const Vector3D& distanceOfOBBs) const; // Compute a contact normal
@ -60,11 +60,7 @@ class SATAlgorithm : public NarrowPhaseAlgorithm {
SATAlgorithm(); // Constructor
~SATAlgorithm(); // Destructor
virtual bool testCollision(const NarrowBoundingVolume* const boundingVolume1,
const Transform& transform1,
const NarrowBoundingVolume* const boundingVolume2,
const Transform& transform2,
ContactInfo*& contactInfo); // Return true and compute a contact info if the two bounding volume collide
virtual bool testCollision(const Body* body1, const Body* body2, ContactInfo*& contactInfo); // Return true and compute a contact info if the two bounding volume collide
};
// Return the contact normal with the correct sign (from obb1 toward obb2). "axis" is the axis vector direction where the

17
src/constraint/Contact.cpp
View File

@ -52,8 +52,9 @@ Contact::~Contact() {
// of the contact. The argument "noConstraint", is the row were the method have
// to start to fill in the J_sp matrix.
void Contact::computeJacobian(int noConstraint, Matrix1x6**& J_sp) const {
RigidBody* rigidBody1 = dynamic_cast<RigidBody*>(body1);
RigidBody* rigidBody2 = dynamic_cast<RigidBody*>(body2);
assert(body1);
assert(body2);
Vector3D r1;
Vector3D r2;
Vector3D r1CrossN;
@ -62,13 +63,10 @@ void Contact::computeJacobian(int noConstraint, Matrix1x6**& J_sp) const {
Vector3D r2CrossU1;
Vector3D r1CrossU2;
Vector3D r2CrossU2;
Vector3D body1Position = rigidBody1->getTransform().getPosition();
Vector3D body2Position = rigidBody2->getTransform().getPosition();
Vector3D body1Position = body1->getTransform().getPosition();
Vector3D body2Position = body2->getTransform().getPosition();
int currentIndex = noConstraint; // Current constraint index
assert(rigidBody1);
assert(rigidBody2);
// For each point in the contact
for (int i=0; i<nbPoints; i++) {
@ -181,12 +179,13 @@ void Contact::computeUpperBound(int noConstraint, Vector& upperBounds) const {
// "errorValues" is the error values vector of the constraint solver and this
// method has to fill in this vector starting from the row "noConstraint"
void Contact::computeErrorValue(int noConstraint, Vector& errorValues) const {
assert(body1);
assert(body2);
RigidBody* rigidBody1 = dynamic_cast<RigidBody*>(body1);
RigidBody* rigidBody2 = dynamic_cast<RigidBody*>(body2);
int index = noConstraint;
assert(rigidBody1);
assert(rigidBody2);
assert(noConstraint >= 0 && noConstraint + nbConstraints <= errorValues.getNbComponent());
// Compute the error value for the contact constraint

5
src/engine/PhysicsEngine.cpp
View File

@ -143,9 +143,8 @@ void PhysicsEngine::updateAllBodiesMotion() {
// Update the position and the orientation of the body according to the new velocity
updatePositionAndOrientationOfBody(*it, newLinearVelocity, newAngularVelocity);
// If the body state has changed, we have to update some informations in the rigid body
// TODO : DELETE THIS
rigidBody->update();
// Update the AABB of the rigid body
rigidBody->updateAABB();
}
}
}

2
src/reactphysics3d.h
View File

@ -38,7 +38,7 @@
#include "body/RigidBody.h"
#include "engine/PhysicsWorld.h"
#include "engine/PhysicsEngine.h"
#include "body/BoundingVolume.h"
#include "body/Shape.h"
#include "body/OBB.h"
#include "body/BoundingSphere.h"
#include "body/AABB.h"