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

Add Transform and changes in code design

Browse Source 
git-svn-id: https://reactphysics3d.googlecode.com/svn/trunk@429 92aac97c-a6ce-11dd-a772-7fcde58d38e6
This commit is contained in:
chappuis.daniel 2011-06-30 20:25:34 +00:00
parent bf8f9cf482
commit bd5d0fb230
19 changed files with 373 additions and 115 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
src/collision/CollisionDetection.cpp
View File

@ -45,7 +45,7 @@ CollisionDetection::CollisionDetection(PhysicsWorld* world) {
broadPhaseAlgorithm = new SAPAlgorithm(); broadPhaseAlgorithm = new SAPAlgorithm();
// Create the narrow-phase algorithm that will be used (Separating axis algorithm) // Create the narrow-phase algorithm that will be used (Separating axis algorithm)
narrowPhaseAlgorithm = new GJKAlgorithm(); narrowPhaseAlgorithm = new GJKAlgorithm(); // TODO : Use GJK algo here
} }
// Destructor // Destructor
@ -90,8 +90,13 @@ void CollisionDetection::computeNarrowPhase() {
for (unsigned int i=0; i<possibleCollisionPairs.size(); i++) { for (unsigned int i=0; i<possibleCollisionPairs.size(); i++) {
ContactInfo* contactInfo = NULL; ContactInfo* contactInfo = NULL;
// Use the narrow-phase collision detection algorithm to check if the really are a contact const RigidBody* rigidBody1 = dynamic_cast<const RigidBody*>(possibleCollisionPairs.at(i).first);
if (narrowPhaseAlgorithm->testCollision(possibleCollisionPairs.at(i).first->getNarrowBoundingVolume(), possibleCollisionPairs.at(i).second->getNarrowBoundingVolume(), contactInfo)) { const RigidBody* rigidBody2 = dynamic_cast<const RigidBody*>(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)) {
assert(contactInfo); assert(contactInfo);
// Add the contact info the current list of collision informations // Add the contact info the current list of collision informations
@ -108,7 +113,7 @@ void CollisionDetection::computeAllContacts() {
assert(contactInfo); assert(contactInfo);
// Compute one or several new contacts and add them into the physics world // Compute one or several new contacts and add them into the physics world
computeContactGJK(contactInfo); computeContactGJK(contactInfo); // TODO : Call computeContactGJK() here
} }
} }
@ -128,6 +133,7 @@ void CollisionDetection::computeContactGJK(const ContactInfo* const contactInfo)
world->addConstraint(contact); world->addConstraint(contact);
} }
/* TODO : DELETE THIS
// Compute a contact for the SAT algorithm (and add it to the physics world) for two colliding bodies // Compute a contact for the SAT algorithm (and add it to the physics world) for two colliding bodies
// This method only works for collision between two OBB bounding volumes // This method only works for collision between two OBB bounding volumes
void CollisionDetection::computeContactSAT(const ContactInfo* const contactInfo) { void CollisionDetection::computeContactSAT(const ContactInfo* const contactInfo) {
@ -257,4 +263,5 @@ void CollisionDetection::computeContactSAT(const ContactInfo* const contactInfo)
world->addConstraint(new Contact(obb1->getBodyPointer(), obb2->getBodyPointer(), normal, penetrationDepth, clippedFace)); world->addConstraint(new Contact(obb1->getBodyPointer(), obb2->getBodyPointer(), normal, penetrationDepth, clippedFace));
} }
} }
*/

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

@ -80,7 +80,9 @@ int EPAAlgorithm::isOriginInTetrahedron(const Vector3D& p1, const Vector3D& p2,
// GJK algorithm. The EPA Algorithm will extend this simplex polytope to find // GJK algorithm. The EPA Algorithm will extend this simplex polytope to find
// the correct penetration depth // the correct penetration depth
bool EPAAlgorithm::computePenetrationDepthAndContactPoints(Simplex simplex, const NarrowBoundingVolume* const boundingVolume1, bool EPAAlgorithm::computePenetrationDepthAndContactPoints(Simplex simplex, const NarrowBoundingVolume* const boundingVolume1,
const Transform& transform1,
const NarrowBoundingVolume* const boundingVolume2, const NarrowBoundingVolume* const boundingVolume2,
const Transform& transform2,
Vector3D& v, ContactInfo*& contactInfo) { Vector3D& v, ContactInfo*& contactInfo) {
Vector3D suppPointsA[MAX_SUPPORT_POINTS]; // Support points of object A in local coordinates 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 Vector3D suppPointsB[MAX_SUPPORT_POINTS]; // Support points of object B in local coordinates
@ -88,6 +90,12 @@ bool EPAAlgorithm::computePenetrationDepthAndContactPoints(Simplex simplex, cons
TrianglesStore triangleStore; // Store the triangles TrianglesStore triangleStore; // Store the triangles
TriangleEPA* triangleHeap[MAX_FACETS]; // Heap that contains the face candidate of the EPA algorithm TriangleEPA* triangleHeap[MAX_FACETS]; // Heap that contains the face candidate of the EPA algorithm
// 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;
// Matrix that transform a direction from body space of shape 1 into body space of shape 2
Matrix3x3 rotateToShape2 = transform2.getOrientation().getTranspose() * transform1.getOrientation();
// Get the simplex computed previously by the GJK algorithm // Get the simplex computed previously by the GJK algorithm
unsigned int nbVertices = simplex.getSimplex(suppPointsA, suppPointsB, points); unsigned int nbVertices = simplex.getSimplex(suppPointsA, suppPointsB, points);
@ -141,17 +149,17 @@ bool EPAAlgorithm::computePenetrationDepthAndContactPoints(Simplex simplex, cons
// Compute the support point in the direction of v1 // Compute the support point in the direction of v1
suppPointsA[2] = boundingVolume1->getSupportPoint(v1, OBJECT_MARGIN); suppPointsA[2] = boundingVolume1->getSupportPoint(v1, OBJECT_MARGIN);
suppPointsB[2] = boundingVolume2->getSupportPoint(v1.getOpposite(), OBJECT_MARGIN); suppPointsB[2] = shape2ToShape1 * boundingVolume2->getSupportPoint(rotateToShape2 * v1.getOpposite(), OBJECT_MARGIN);
points[2] = suppPointsA[2] - suppPointsB[2]; points[2] = suppPointsA[2] - suppPointsB[2];
// Compute the support point in the direction of v2 // Compute the support point in the direction of v2
suppPointsA[3] = boundingVolume1->getSupportPoint(v2, OBJECT_MARGIN); suppPointsA[3] = boundingVolume1->getSupportPoint(v2, OBJECT_MARGIN);
suppPointsB[3] = boundingVolume2->getSupportPoint(v2.getOpposite(), OBJECT_MARGIN); suppPointsB[3] = shape2ToShape1 * boundingVolume2->getSupportPoint(rotateToShape2 * v2.getOpposite(), OBJECT_MARGIN);
points[3] = suppPointsA[3] - suppPointsB[3]; points[3] = suppPointsA[3] - suppPointsB[3];
// Compute the support point in the direction of v3 // Compute the support point in the direction of v3
suppPointsA[4] = boundingVolume1->getSupportPoint(v3, OBJECT_MARGIN); suppPointsA[4] = boundingVolume1->getSupportPoint(v3, OBJECT_MARGIN);
suppPointsB[4] = boundingVolume2->getSupportPoint(v3.getOpposite(), OBJECT_MARGIN); suppPointsB[4] = shape2ToShape1 * boundingVolume2->getSupportPoint(rotateToShape2 * v3.getOpposite(), OBJECT_MARGIN);
points[4] = suppPointsA[4] - suppPointsB[4]; points[4] = suppPointsA[4] - suppPointsB[4];
// Now we have an hexahedron (two tetrahedron glued together). We can simply keep the // Now we have an hexahedron (two tetrahedron glued together). We can simply keep the
@ -246,10 +254,10 @@ bool EPAAlgorithm::computePenetrationDepthAndContactPoints(Simplex simplex, cons
// Compute the two new vertices to obtain a hexahedron // Compute the two new vertices to obtain a hexahedron
suppPointsA[3] = boundingVolume1->getSupportPoint(n, OBJECT_MARGIN); suppPointsA[3] = boundingVolume1->getSupportPoint(n, OBJECT_MARGIN);
suppPointsB[3] = boundingVolume2->getSupportPoint(n.getOpposite(), OBJECT_MARGIN); suppPointsB[3] = shape2ToShape1 * boundingVolume2->getSupportPoint(rotateToShape2 * n.getOpposite(), OBJECT_MARGIN);
points[3] = suppPointsA[3] - suppPointsB[3]; points[3] = suppPointsA[3] - suppPointsB[3];
suppPointsA[4] = boundingVolume1->getSupportPoint(n.getOpposite(), OBJECT_MARGIN); suppPointsA[4] = boundingVolume1->getSupportPoint(n.getOpposite(), OBJECT_MARGIN);
suppPointsB[4] = boundingVolume2->getSupportPoint(n, OBJECT_MARGIN); suppPointsB[4] = shape2ToShape1 * boundingVolume2->getSupportPoint(rotateToShape2 * n, OBJECT_MARGIN);
points[4] = suppPointsA[4] - suppPointsB[4]; points[4] = suppPointsA[4] - suppPointsB[4];
// Construct the triangle faces // Construct the triangle faces
@ -319,7 +327,7 @@ bool EPAAlgorithm::computePenetrationDepthAndContactPoints(Simplex simplex, cons
// Compute the support point of the Minkowski difference (A-B) in the closest point direction // Compute the support point of the Minkowski difference (A-B) in the closest point direction
suppPointsA[nbVertices] = boundingVolume1->getSupportPoint(triangle->getClosestPoint(), OBJECT_MARGIN); suppPointsA[nbVertices] = boundingVolume1->getSupportPoint(triangle->getClosestPoint(), OBJECT_MARGIN);
suppPointsB[nbVertices] = boundingVolume2->getSupportPoint(triangle->getClosestPoint().getOpposite(), OBJECT_MARGIN); suppPointsB[nbVertices] = shape2ToShape1 * boundingVolume2->getSupportPoint(rotateToShape2 * triangle->getClosestPoint().getOpposite(), OBJECT_MARGIN);
points[nbVertices] = suppPointsA[nbVertices] - suppPointsB[nbVertices]; points[nbVertices] = suppPointsA[nbVertices] - suppPointsB[nbVertices];
int indexNewVertex = nbVertices; int indexNewVertex = nbVertices;
@ -361,10 +369,10 @@ bool EPAAlgorithm::computePenetrationDepthAndContactPoints(Simplex simplex, cons
} while(nbTriangles > 0 && triangleHeap[0]->getDistSquare() <= upperBoundSquarePenDepth); } while(nbTriangles > 0 && triangleHeap[0]->getDistSquare() <= upperBoundSquarePenDepth);
// Compute the contact info // Compute the contact info (in world-space)
v = triangle->getClosestPoint(); v = transform1.getOrientation() * triangle->getClosestPoint();
Vector3D pA = triangle->computeClosestPointOfObject(suppPointsA); Vector3D pA = transform1 * triangle->computeClosestPointOfObject(suppPointsA);
Vector3D pB = triangle->computeClosestPointOfObject(suppPointsB); Vector3D pB = transform1 * triangle->computeClosestPointOfObject(suppPointsB);
Vector3D normal = v.getUnit(); Vector3D normal = v.getUnit();
double penetrationDepth = v.length(); double penetrationDepth = v.length();
assert(penetrationDepth > 0.0); assert(penetrationDepth > 0.0);

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

@ -83,7 +83,9 @@ class EPAAlgorithm {
~EPAAlgorithm(); // Destructor ~EPAAlgorithm(); // Destructor
bool computePenetrationDepthAndContactPoints(Simplex simplex, const NarrowBoundingVolume* const boundingVolume1, bool computePenetrationDepthAndContactPoints(Simplex simplex, const NarrowBoundingVolume* const boundingVolume1,
const Transform& transform1,
const NarrowBoundingVolume* const boundingVolume2, const NarrowBoundingVolume* const boundingVolume2,
const Transform& transform2,
Vector3D& v, ContactInfo*& contactInfo); // Compute the penetration depth with EPA algorithm Vector3D& v, ContactInfo*& contactInfo); // Compute the penetration depth with EPA algorithm
}; };

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

@ -59,8 +59,8 @@ GJKAlgorithm::~GJKAlgorithm() {
// algorithm on the enlarged object to obtain a simplex polytope that contains the // 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 // 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. // the correct penetration depth and contact points between the enlarged objects.
bool GJKAlgorithm::testCollision(const NarrowBoundingVolume* const boundingVolume1, bool GJKAlgorithm::testCollision(const NarrowBoundingVolume* const boundingVolume1, const Transform& transform1,
const NarrowBoundingVolume* const boundingVolume2, const NarrowBoundingVolume* const boundingVolume2, const Transform& transform2,
ContactInfo*& contactInfo) { ContactInfo*& contactInfo) {
Vector3D suppA; // Support point of object A Vector3D suppA; // Support point of object A
Vector3D suppB; // Support point of object B Vector3D suppB; // Support point of object B
@ -70,6 +70,12 @@ bool GJKAlgorithm::testCollision(const NarrowBoundingVolume* const boundingVolum
double vDotw; double vDotw;
double prevDistSquare; double prevDistSquare;
// 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;
// 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); assert(boundingVolume1 != boundingVolume2);
// Initialize the margin (sum of margins of both objects) // Initialize the margin (sum of margins of both objects)
@ -83,7 +89,7 @@ bool GJKAlgorithm::testCollision(const NarrowBoundingVolume* const boundingVolum
// Get the last point V (last separating axis) // Get the last point V (last separating axis)
// TODO : Implement frame coherence. For each pair of body, store // TODO : Implement frame coherence. For each pair of body, store
// the last separating axis and use it to initialize the v vector // the last separating axis and use it to initialize the v vector
Vector3D v(1.0, 1.0, 1.0); Vector3D v(0.0, 1.0, 0.0);
// Initialize the upper bound for the square distance // Initialize the upper bound for the square distance
double distSquare = DBL_MAX; double distSquare = DBL_MAX;
@ -91,7 +97,7 @@ bool GJKAlgorithm::testCollision(const NarrowBoundingVolume* const boundingVolum
do { do {
// Compute the support points for original objects (without margins) A and B // Compute the support points for original objects (without margins) A and B
suppA = boundingVolume1->getSupportPoint(v.getOpposite()); suppA = boundingVolume1->getSupportPoint(v.getOpposite());
suppB = boundingVolume2->getSupportPoint(v); suppB = shape2ToShape1 * boundingVolume2->getSupportPoint(rotateToShape2 * v);
// Compute the support point for the Minkowski difference A-B // Compute the support point for the Minkowski difference A-B
w = suppA - suppB; w = suppA - suppB;
@ -113,11 +119,11 @@ bool GJKAlgorithm::testCollision(const NarrowBoundingVolume* const boundingVolum
// object with the margins // object with the margins
double dist = sqrt(distSquare); double dist = sqrt(distSquare);
assert(dist > 0.0); assert(dist > 0.0);
pA = pA - (OBJECT_MARGIN / dist) * v; pA = transform1 * (pA - (OBJECT_MARGIN / dist) * v);
pB = pB + (OBJECT_MARGIN / dist) * v; pB = transform1 * (pB + (OBJECT_MARGIN / dist) * v);
// Compute the contact info // Compute the contact info
Vector3D normal = v.getOpposite().getUnit(); Vector3D normal = transform1.getOrientation() * v.getOpposite().getUnit();
double penetrationDepth = margin - dist; double penetrationDepth = margin - dist;
contactInfo = new ContactInfo(boundingVolume1->getBodyPointer(), boundingVolume2->getBodyPointer(), contactInfo = new ContactInfo(boundingVolume1->getBodyPointer(), boundingVolume2->getBodyPointer(),
normal, penetrationDepth, pA, pB); normal, penetrationDepth, pA, pB);
@ -138,11 +144,11 @@ bool GJKAlgorithm::testCollision(const NarrowBoundingVolume* const boundingVolum
// object with the margins // object with the margins
double dist = sqrt(distSquare); double dist = sqrt(distSquare);
assert(dist > 0.0); assert(dist > 0.0);
pA = pA - (OBJECT_MARGIN / dist) * v; pA = transform1 * (pA - (OBJECT_MARGIN / dist) * v);
pB = pB + (OBJECT_MARGIN / dist) * v; pB = transform1 * (pB + (OBJECT_MARGIN / dist) * v);
// Compute the contact info // Compute the contact info
Vector3D normal = v.getOpposite().getUnit(); Vector3D normal = transform1.getOrientation() * v.getOpposite().getUnit();
double penetrationDepth = margin - dist; double penetrationDepth = margin - dist;
contactInfo = new ContactInfo(boundingVolume1->getBodyPointer(), boundingVolume2->getBodyPointer(), contactInfo = new ContactInfo(boundingVolume1->getBodyPointer(), boundingVolume2->getBodyPointer(),
normal, penetrationDepth, pA, pB); normal, penetrationDepth, pA, pB);
@ -161,11 +167,11 @@ bool GJKAlgorithm::testCollision(const NarrowBoundingVolume* const boundingVolum
// object with the margins // object with the margins
double dist = sqrt(distSquare); double dist = sqrt(distSquare);
assert(dist > 0.0); assert(dist > 0.0);
pA = pA - (OBJECT_MARGIN / dist) * v; pA = transform1 * (pA - (OBJECT_MARGIN / dist) * v);
pB = pB + (OBJECT_MARGIN / dist) * v; pB = transform1 * (pB + (OBJECT_MARGIN / dist) * v);
// Compute the contact info // Compute the contact info
Vector3D normal = v.getOpposite().getUnit(); Vector3D normal = transform1.getOrientation() * v.getOpposite().getUnit();
double penetrationDepth = margin - dist; double penetrationDepth = margin - dist;
contactInfo = new ContactInfo(boundingVolume1->getBodyPointer(), boundingVolume2->getBodyPointer(), contactInfo = new ContactInfo(boundingVolume1->getBodyPointer(), boundingVolume2->getBodyPointer(),
normal, penetrationDepth, pA, pB); normal, penetrationDepth, pA, pB);
@ -192,11 +198,11 @@ bool GJKAlgorithm::testCollision(const NarrowBoundingVolume* const boundingVolum
// object with the margins // object with the margins
double dist = sqrt(distSquare); double dist = sqrt(distSquare);
assert(dist > 0.0); assert(dist > 0.0);
pA = pA - (OBJECT_MARGIN / dist) * v; pA = transform1 * (pA - (OBJECT_MARGIN / dist) * v);
pB = pB + (OBJECT_MARGIN / dist) * v; pB = transform1 * (pB + (OBJECT_MARGIN / dist) * v);
// Compute the contact info // Compute the contact info
Vector3D normal = v.getOpposite().getUnit(); Vector3D normal = transform1.getOrientation() * v.getOpposite().getUnit();
double penetrationDepth = margin - dist; double penetrationDepth = margin - dist;
contactInfo = new ContactInfo(boundingVolume1->getBodyPointer(), boundingVolume2->getBodyPointer(), contactInfo = new ContactInfo(boundingVolume1->getBodyPointer(), boundingVolume2->getBodyPointer(),
normal, penetrationDepth, pA, pB); normal, penetrationDepth, pA, pB);
@ -214,7 +220,7 @@ bool GJKAlgorithm::testCollision(const NarrowBoundingVolume* const boundingVolum
// enlarged objects to compute a simplex polytope that contains the origin. Then, we give that simplex // 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 // polytope to the EPA algorithm to compute the correct penetration depth and contact points between
// the enlarged objects. // the enlarged objects.
return computePenetrationDepthForEnlargedObjects(boundingVolume1, boundingVolume2, contactInfo, v); return computePenetrationDepthForEnlargedObjects(boundingVolume1, transform1, boundingVolume2, transform2, contactInfo, v);
} }
// This method runs the GJK algorithm on the two enlarged objects (with margin) // This method runs the GJK algorithm on the two enlarged objects (with margin)
@ -222,8 +228,8 @@ bool GJKAlgorithm::testCollision(const NarrowBoundingVolume* const boundingVolum
// assumed to intersect in the original objects (without margin). Therefore such // 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 // 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. // compute the correct penetration depth and contact points of the enlarged objects.
bool GJKAlgorithm::computePenetrationDepthForEnlargedObjects(const NarrowBoundingVolume* const boundingVolume1, bool GJKAlgorithm::computePenetrationDepthForEnlargedObjects(const NarrowBoundingVolume* const boundingVolume1, const Transform& transform1,
const NarrowBoundingVolume* const boundingVolume2, const NarrowBoundingVolume* const boundingVolume2, const Transform& transform2,
ContactInfo*& contactInfo, Vector3D& v) { ContactInfo*& contactInfo, Vector3D& v) {
Simplex simplex; Simplex simplex;
Vector3D suppA; Vector3D suppA;
@ -232,11 +238,17 @@ bool GJKAlgorithm::computePenetrationDepthForEnlargedObjects(const NarrowBoundin
double vDotw; double vDotw;
double distSquare = DBL_MAX; double distSquare = DBL_MAX;
double prevDistSquare; double prevDistSquare;
// 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;
// Matrix that transform a direction from body space of shape 1 into body space of shape 2
Matrix3x3 rotateToShape2 = transform2.getOrientation().getTranspose() * transform1.getOrientation();
do { do {
// Compute the support points for the enlarged object A and B // Compute the support points for the enlarged object A and B
suppA = boundingVolume1->getSupportPoint(v.getOpposite(), OBJECT_MARGIN); suppA = boundingVolume1->getSupportPoint(v.getOpposite(), OBJECT_MARGIN);
suppB = boundingVolume2->getSupportPoint(v, OBJECT_MARGIN); suppB = shape2ToShape1 * boundingVolume2->getSupportPoint(rotateToShape2 * v, OBJECT_MARGIN);
// Compute the support point for the Minkowski difference A-B // Compute the support point for the Minkowski difference A-B
w = suppA - suppB; w = suppA - suppB;
@ -272,5 +284,6 @@ bool GJKAlgorithm::computePenetrationDepthForEnlargedObjects(const NarrowBoundin
// Give the simplex computed with GJK algorithm to the EPA algorithm which will compute the correct // 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 // penetration depth and contact points between the two enlarged objects
return algoEPA.computePenetrationDepthAndContactPoints(simplex, boundingVolume1, boundingVolume2, v, contactInfo); return algoEPA.computePenetrationDepthAndContactPoints(simplex, boundingVolume1, transform1,
boundingVolume2, transform2, v, contactInfo);
} }

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

@ -62,15 +62,17 @@ class GJKAlgorithm : public NarrowPhaseAlgorithm {
EPAAlgorithm algoEPA; // EPA Algorithm EPAAlgorithm algoEPA; // EPA Algorithm
bool computePenetrationDepthForEnlargedObjects(const NarrowBoundingVolume* const boundingVolume1, bool computePenetrationDepthForEnlargedObjects(const NarrowBoundingVolume* const boundingVolume1,
const Transform& transform1,
const NarrowBoundingVolume* const boundingVolume2, const NarrowBoundingVolume* const boundingVolume2,
const Transform& transform2,
ContactInfo*& contactInfo, Vector3D& v); // Compute the penetration depth for enlarged objects ContactInfo*& contactInfo, Vector3D& v); // Compute the penetration depth for enlarged objects
public : public :
GJKAlgorithm(); // Constructor GJKAlgorithm(); // Constructor
~GJKAlgorithm(); // Destructor ~GJKAlgorithm(); // Destructor
virtual bool testCollision(const NarrowBoundingVolume* const boundingVolume1, virtual bool testCollision(const NarrowBoundingVolume* const boundingVolume1, const Transform& transform1,
const NarrowBoundingVolume* const boundingVolume2, const NarrowBoundingVolume* const boundingVolume2, const Transform& transform2,
ContactInfo*& contactInfo); // Return true and compute a contact info if the two bounding volume collide ContactInfo*& contactInfo); // Return true and compute a contact info if the two bounding volume collide
}; };

2
src/collision/NarrowPhaseAlgorithm.h
View File

@ -48,7 +48,9 @@ class NarrowPhaseAlgorithm {
virtual ~NarrowPhaseAlgorithm(); // Destructor virtual ~NarrowPhaseAlgorithm(); // Destructor
virtual bool testCollision(const NarrowBoundingVolume* const boundingVolume1, virtual bool testCollision(const NarrowBoundingVolume* const boundingVolume1,
const Transform& transform1,
const NarrowBoundingVolume* const boundingVolume2, const NarrowBoundingVolume* const boundingVolume2,
const Transform& transform2,
ContactInfo*& contactInfo)=0; // Return true and compute a contact info if the two bounding volume collide ContactInfo*& contactInfo)=0; // Return true and compute a contact info if the two bounding volume collide
}; };

33
src/collision/NoBroadPhaseAlgorithm.cpp
View File

@ -38,4 +38,37 @@ NoBroadPhaseAlgorithm::~NoBroadPhaseAlgorithm() {
} }
// Compute the possible collision pairs of bodies
// The arguments "addedBodies" and "removedBodies" are respectively the set
// of bodies that have been added and removed since the last broad-phase
// computation. Before the call, the argument "possibleCollisionPairs"
// correspond to the possible colliding pairs of bodies from the last broad-phase
// computation. This methods computes the current possible collision pairs of
// bodies and update the "possibleCollisionPairs" argument. This broad-phase
// algorithm doesn't do anything and therefore the "possibleCollisionPairs" set
// must contains all the possible pairs of bodies
void NoBroadPhaseAlgorithm::computePossibleCollisionPairs(std::vector<Body*> addedBodies, std::vector<Body*> removedBodies,
std::vector<std::pair<const Body*, const Body* > >& possibleCollisionPairs) {
// Add the new bodies
for (std::vector<Body*>::iterator it = addedBodies.begin(); it < addedBodies.end(); it++) {
bodies.push_back(*it);
}
// Remove the bodies to be removed
for (std::vector<Body*>::iterator it = removedBodies.begin(); it < removedBodies.end(); it++) {
bodies.erase(std::find(bodies.begin(), bodies.end(), *it));
}
// If the set of bodies have been changed
if (addedBodies.size() + removedBodies.size() > 0) {
// Recompute all the possible pairs of bodies
possibleCollisionPairs.clear();
for (std::vector<Body*>::iterator it1 = addedBodies.begin(); it1 < addedBodies.end(); it1++) {
for (std::vector<Body*>::iterator it2 = addedBodies.begin(); it2 < addedBodies.end(); it2++) {
if (*it1 != *it2) {
possibleCollisionPairs.push_back(std::make_pair(*it1, *it2));
}
}
}
}
}

36
src/collision/NoBroadPhaseAlgorithm.h
View File

@ -51,42 +51,6 @@ class NoBroadPhaseAlgorithm : public BroadPhaseAlgorithm {
std::vector<std::pair<const Body*, const Body* > >& possibleCollisionPairs); // Compute the possible collision pairs of bodies std::vector<std::pair<const Body*, const Body* > >& possibleCollisionPairs); // Compute the possible collision pairs of bodies
}; };
// Compute the possible collision pairs of bodies
// The arguments "addedBodies" and "removedBodies" are respectively the set
// of bodies that have been added and removed since the last broad-phase
// computation. Before the call, the argument "possibleCollisionPairs"
// correspond to the possible colliding pairs of bodies from the last broad-phase
// computation. This methods computes the current possible collision pairs of
// bodies and update the "possibleCollisionPairs" argument. This broad-phase
// algorithm doesn't do anything and therefore the "possibleCollisionPairs" set
// must contains all the possible pairs of bodies
inline void NoBroadPhaseAlgorithm::computePossibleCollisionPairs(std::vector<Body*> addedBodies, std::vector<Body*> removedBodies,
std::vector<std::pair<const Body*, const Body* > >& possibleCollisionPairs) {
// Add the new bodies
for (std::vector<Body*>::iterator it = addedBodies.begin(); it < addedBodies.end(); it++) {
bodies.push_back(*it);
}
// Remove the bodies to be removed
for (std::vector<Body*>::iterator it = removedBodies.begin(); it < removedBodies.end(); it++) {
bodies.erase(std::find(bodies.begin(), bodies.end(), *it));
}
// If the set of bodies have been changed
if (addedBodies.size() + removedBodies.size() > 0) {
// Recompute all the possible pairs of bodies
possibleCollisionPairs.clear();
for (std::vector<Body*>::iterator it1 = addedBodies.begin(); it1 < addedBodies.end(); it1++) {
for (std::vector<Body*>::iterator it2 = addedBodies.begin(); it2 < addedBodies.end(); it2++) {
if (*it1 != *it2) {
possibleCollisionPairs.push_back(std::make_pair(*it1, *it2));
}
}
}
}
}
} // End of reactphysics3d namespace } // End of reactphysics3d namespace
#endif #endif

6
src/collision/SAPAlgorithm.cpp
View File

@ -50,7 +50,7 @@ void SAPAlgorithm::removeBodiesAABB(vector<Body*> bodies) {
// Removed the AABB of the bodies that have been removed // Removed the AABB of the bodies that have been removed
for (vector<Body*>::iterator it = bodies.begin(); it != bodies.end(); ++it) { for (vector<Body*>::iterator it = bodies.begin(); it != bodies.end(); ++it) {
aabb = dynamic_cast<const AABB*>((*it)->getBroadBoundingVolume()); aabb = (*it)->getAABB();
assert(aabb); assert(aabb);
elemToRemove = find(sortedAABBs.begin(), sortedAABBs.end(), aabb); elemToRemove = find(sortedAABBs.begin(), sortedAABBs.end(), aabb);
assert((*elemToRemove) == aabb); assert((*elemToRemove) == aabb);
@ -64,7 +64,7 @@ void SAPAlgorithm::addBodiesAABB(vector<Body*> bodies) {
for (vector<Body*>::iterator it = bodies.begin(); it != bodies.end(); ++it) { for (vector<Body*>::iterator it = bodies.begin(); it != bodies.end(); ++it) {
aabb = 0; aabb = 0;
aabb = dynamic_cast<const AABB*>((*it)->getBroadBoundingVolume()); aabb = (*it)->getAABB();
assert(aabb); assert(aabb);
sortedAABBs.push_back(aabb); sortedAABBs.push_back(aabb);
} }
@ -123,7 +123,7 @@ void SAPAlgorithm::computePossibleCollisionPairs(vector<Body*> addedBodies, vect
// Test collision against all possible overlapping AABBs following the current one // Test collision against all possible overlapping AABBs following the current one
for (it2 = it + 1; it2 != sortedAABBs.end(); ++it2) { for (it2 = it + 1; it2 != sortedAABBs.end(); ++it2) {
// Stop when the tested AABBs are beyond the end of the current AABB // Stop when the tested AABBs are beyond the end of the current AABB
if ((*it2)->getMinValueOnAxis(sortAxis) > (*it)->getMaxValueOnAxis(sortAxis)) { if ((*it2)->getMinCoordinates().getValue(sortAxis) > (*it)->getMaxCoordinates().getValue(sortAxis)) {
break; break;
} }

2
src/collision/SAPAlgorithm.h
View File

@ -72,7 +72,7 @@ class SAPAlgorithm : public BroadPhaseAlgorithm {
// sorting axis (sortAxis) is smaller than the minimum value of "b" on this same // sorting axis (sortAxis) is smaller than the minimum value of "b" on this same
// axis. // axis.
inline bool SAPAlgorithm::compareAABBs(const AABB* a, const AABB* b) { inline bool SAPAlgorithm::compareAABBs(const AABB* a, const AABB* b) {
return (a->getMinValueOnAxis(sortAxis) < b->getMinValueOnAxis(sortAxis)); return (a->getMinCoordinates().getValue(sortAxis) < b->getMinCoordinates().getValue(sortAxis));
} }
} // End of reactphysics3d namespace } // End of reactphysics3d namespace

33
src/collision/SATAlgorithm.cpp
View File

@ -32,6 +32,9 @@
#include <cmath> #include <cmath>
#include <cassert> #include <cassert>
// TODO : SAT Algorithm for box-box collision does not work anymore since the use of
// transform. Maybe a problem with the computation of the normal vector
// Anyway, the GJK algorithm should be used instead
// We want to use the ReactPhysics3D namespace // We want to use the ReactPhysics3D namespace
using namespace reactphysics3d; using namespace reactphysics3d;
@ -48,8 +51,8 @@ SATAlgorithm::~SATAlgorithm() {
// Return true and compute a contact info if the two bounding volume collide. // 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. // The method returns false if there is no collision between the two bounding volumes.
bool SATAlgorithm::testCollision(const NarrowBoundingVolume* const boundingVolume1, bool SATAlgorithm::testCollision(const NarrowBoundingVolume* const boundingVolume1, const Transform& transform1,
const NarrowBoundingVolume* const boundingVolume2, const NarrowBoundingVolume* const boundingVolume2, const Transform& transform2,
ContactInfo*& contactInfo) { ContactInfo*& contactInfo) {
assert(boundingVolume1 != boundingVolume2); assert(boundingVolume1 != boundingVolume2);
@ -63,7 +66,7 @@ bool SATAlgorithm::testCollision(const NarrowBoundingVolume* const boundingVolum
// If the two bounding volumes are OBB // If the two bounding volumes are OBB
if (obb1 && obb2) { if (obb1 && obb2) {
// Compute the collision test between two OBB // Compute the collision test between two OBB
return computeCollisionTest(obb1, obb2, contactInfo); return computeCollisionTest(obb1, transform1, obb2, transform2, contactInfo);
} }
else { else {
return false; return false;
@ -78,7 +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 // 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 // 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. // normal of OBB 1 and Bj for the jth face normal of OBB 2.
bool SATAlgorithm::computeCollisionTest(const OBB* const obb1, const OBB* const obb2, ContactInfo*& contactInfo) const { bool SATAlgorithm::computeCollisionTest(const OBB* const obb1, const Transform& transform1,
const OBB* const obb2, const Transform& transform2, ContactInfo*& contactInfo) const {
double center; // Center of a projection interval double center; // Center of a projection interval
@ -104,17 +108,18 @@ bool SATAlgorithm::computeCollisionTest(const OBB* const obb1, const OBB* const
double udc1[3]; // DotProduct(obb1.Ai, obb2.center - obb1.center) double udc1[3]; // DotProduct(obb1.Ai, obb2.center - obb1.center)
double udc2[3]; // DotProduct(obb2.Ai, obb2.center - obb1.center) double udc2[3]; // DotProduct(obb2.Ai, obb2.center - obb1.center)
Vector3D boxDistance = obb2->getCenter() - obb1->getCenter(); // Vector between the centers of the OBBs //Vector3D boxDistance = obb2->getCenter() - obb1->getCenter(); // Vector between the centers of the OBBs
Vector3D boxDistance = transform2.getPosition() - transform1.getPosition(); // Vector between the centers of the OBBs
// Axis A0 // Axis A0
for (int i=0; i<3; ++i) { for (int i=0; i<3; ++i) {
c[0][i] = obb1->getAxis(0).dot(obb2->getAxis(i)); c[0][i] = transform1.getOrientation().getColumn(0).dot(transform2.getOrientation().getColumn(i));
absC[0][i] = fabs(c[0][i]); absC[0][i] = fabs(c[0][i]);
if (absC[0][i] > cutoff) { if (absC[0][i] > cutoff) {
existsParallelPair = true; existsParallelPair = true;
} }
} }
udc1[0] = obb1->getAxis(0).dot(boxDistance); udc1[0] = transform1.getOrientation().getColumn(0).dot(boxDistance);
center = udc1[0]; center = udc1[0];
radius1 = obb1->getExtent(0); radius1 = obb1->getExtent(0);
radius2 = obb2->getExtent(0)*absC[0][0] + obb2->getExtent(1)*absC[0][1] + obb2->getExtent(2) * absC[0][2]; radius2 = obb2->getExtent(0)*absC[0][0] + obb2->getExtent(1)*absC[0][1] + obb2->getExtent(2) * absC[0][2];
@ -133,13 +138,13 @@ bool SATAlgorithm::computeCollisionTest(const OBB* const obb1, const OBB* const
// Axis A1 // Axis A1
for (int i=0; i<3; ++i) { for (int i=0; i<3; ++i) {
c[1][i] = obb1->getAxis(1).dot(obb2->getAxis(i)); c[1][i] = transform1.getOrientation().getColumn(1).dot(transform2.getOrientation().getColumn(i));
absC[1][i] = fabs(c[1][i]); absC[1][i] = fabs(c[1][i]);
if (absC[1][i] > cutoff) { if (absC[1][i] > cutoff) {
existsParallelPair = true; existsParallelPair = true;
} }
} }
udc1[1] = obb1->getAxis(1).dot(boxDistance); udc1[1] = transform1.getOrientation().getColumn(1).dot(boxDistance);
center = udc1[1]; center = udc1[1];
radius1 = obb1->getExtent(1); radius1 = obb1->getExtent(1);
radius2 = obb2->getExtent(0)*absC[1][0] + obb2->getExtent(1)*absC[1][1] + obb2->getExtent(2) * absC[1][2]; radius2 = obb2->getExtent(0)*absC[1][0] + obb2->getExtent(1)*absC[1][1] + obb2->getExtent(2) * absC[1][2];
@ -158,13 +163,13 @@ bool SATAlgorithm::computeCollisionTest(const OBB* const obb1, const OBB* const
// Axis A2 // Axis A2
for (int i=0; i<3; ++i) { for (int i=0; i<3; ++i) {
c[2][i] = obb1->getAxis(2).dot(obb2->getAxis(i)); c[2][i] = transform1.getOrientation().getColumn(2).dot(transform2.getOrientation().getColumn(i));
absC[2][i] = fabs(c[2][i]); absC[2][i] = fabs(c[2][i]);
if (absC[2][i] > cutoff) { if (absC[2][i] > cutoff) {
existsParallelPair = true; existsParallelPair = true;
} }
} }
udc1[2] = obb1->getAxis(2).dot(boxDistance); udc1[2] = transform1.getOrientation().getColumn(2).dot(boxDistance);
center = udc1[2]; center = udc1[2];
radius1 = obb1->getExtent(2); radius1 = obb1->getExtent(2);
radius2 = obb2->getExtent(0)*absC[2][0] + obb2->getExtent(1)*absC[2][1] + obb2->getExtent(2)*absC[2][2]; radius2 = obb2->getExtent(0)*absC[2][0] + obb2->getExtent(1)*absC[2][1] + obb2->getExtent(2)*absC[2][2];
@ -182,7 +187,7 @@ bool SATAlgorithm::computeCollisionTest(const OBB* const obb1, const OBB* const
} }
// Axis B0 // Axis B0
udc2[0] = obb2->getAxis(0).dot(boxDistance); udc2[0] = transform2.getOrientation().getColumn(0).dot(boxDistance);
center = udc2[0]; center = udc2[0];
radius1 = obb1->getExtent(0)*absC[0][0] + obb1->getExtent(1)*absC[1][0] + obb1->getExtent(2) * absC[2][0]; radius1 = obb1->getExtent(0)*absC[0][0] + obb1->getExtent(1)*absC[1][0] + obb1->getExtent(2) * absC[2][0];
radius2 = obb2->getExtent(0); radius2 = obb2->getExtent(0);
@ -200,7 +205,7 @@ bool SATAlgorithm::computeCollisionTest(const OBB* const obb1, const OBB* const
} }
// Axis B1 // Axis B1
udc2[1] = obb2->getAxis(1).dot(boxDistance); udc2[1] = transform2.getOrientation().getColumn(1).dot(boxDistance);
center = udc2[1]; center = udc2[1];
radius1 = obb1->getExtent(0)*absC[0][1] + obb1->getExtent(1)*absC[1][1] + obb1->getExtent(2) * absC[2][1]; radius1 = obb1->getExtent(0)*absC[0][1] + obb1->getExtent(1)*absC[1][1] + obb1->getExtent(2) * absC[2][1];
radius2 = obb2->getExtent(1); radius2 = obb2->getExtent(1);
@ -218,7 +223,7 @@ bool SATAlgorithm::computeCollisionTest(const OBB* const obb1, const OBB* const
} }
// Axis B2 // Axis B2
udc2[2] = obb2->getAxis(2).dot(boxDistance); udc2[2] = transform2.getOrientation().getColumn(2).dot(boxDistance);
center = udc2[2]; center = udc2[2];
radius1 = obb1->getExtent(0)*absC[0][2] + obb1->getExtent(1)*absC[1][2] + obb1->getExtent(2)*absC[2][2]; radius1 = obb1->getExtent(0)*absC[0][2] + obb1->getExtent(1)*absC[1][2] + obb1->getExtent(2)*absC[2][2];
radius2 = obb2->getExtent(2); radius2 = obb2->getExtent(2);

7
src/collision/SATAlgorithm.h
View File

@ -29,6 +29,7 @@
#include "NarrowPhaseAlgorithm.h" #include "NarrowPhaseAlgorithm.h"
#include "../constraint/Contact.h" #include "../constraint/Contact.h"
#include "../body/OBB.h" #include "../body/OBB.h"
#include "../mathematics/Transform.h"
// ReactPhysics3D namespace // ReactPhysics3D namespace
namespace reactphysics3d { namespace reactphysics3d {
@ -49,7 +50,9 @@ namespace reactphysics3d {
*/ */
class SATAlgorithm : public NarrowPhaseAlgorithm { class SATAlgorithm : public NarrowPhaseAlgorithm {
private : private :
bool computeCollisionTest(const OBB* const obb1, const OBB* const obb2, ContactInfo*& contactInfo) const; // Return true and compute a contact info if the two OBB collide bool computeCollisionTest(const OBB* const obb1, const Transform& transform1,
const OBB* const 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 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 Vector3D computeContactNormal(const Vector3D& axis, const Vector3D& distanceOfOBBs) const; // Compute a contact normal
@ -58,7 +61,9 @@ class SATAlgorithm : public NarrowPhaseAlgorithm {
~SATAlgorithm(); // Destructor ~SATAlgorithm(); // Destructor
virtual bool testCollision(const NarrowBoundingVolume* const boundingVolume1, virtual bool testCollision(const NarrowBoundingVolume* const boundingVolume1,
const Transform& transform1,
const NarrowBoundingVolume* const boundingVolume2, const NarrowBoundingVolume* const boundingVolume2,
const Transform& transform2,
ContactInfo*& contactInfo); // Return true and compute a contact info if the two bounding volume collide ContactInfo*& contactInfo); // Return true and compute a contact info if the two bounding volume collide
}; };

4
src/constraint/Contact.cpp
View File

@ -62,8 +62,8 @@ void Contact::computeJacobian(int noConstraint, Matrix1x6**& J_sp) const {
Vector3D r2CrossU1; Vector3D r2CrossU1;
Vector3D r1CrossU2; Vector3D r1CrossU2;
Vector3D r2CrossU2; Vector3D r2CrossU2;
Vector3D body1Position = rigidBody1->getPosition(); Vector3D body1Position = rigidBody1->getTransform().getPosition();
Vector3D body2Position = rigidBody2->getPosition(); Vector3D body2Position = rigidBody2->getTransform().getPosition();
int currentIndex = noConstraint; // Current constraint index int currentIndex = noConstraint; // Current constraint index
assert(rigidBody1); assert(rigidBody1);

17
src/engine/PhysicsEngine.cpp
View File

@ -144,6 +144,7 @@ void PhysicsEngine::updateAllBodiesMotion() {
updatePositionAndOrientationOfBody(*it, newLinearVelocity, newAngularVelocity); updatePositionAndOrientationOfBody(*it, newLinearVelocity, newAngularVelocity);
// If the body state has changed, we have to update some informations in the rigid body // If the body state has changed, we have to update some informations in the rigid body
// TODO : DELETE THIS
rigidBody->update(); rigidBody->update();
} }
} }
@ -159,18 +160,20 @@ void PhysicsEngine::updatePositionAndOrientationOfBody(Body* body, const Vector3
assert(rigidBody); assert(rigidBody);
// Update the old position and orientation of the body // Update the old position and orientation of the body
rigidBody->updateOldPositionAndOrientation(); rigidBody->updateOldTransform();
// Normalize the orientation quaternion
rigidBody->setOrientation(rigidBody->getOrientation().getUnit());
// Update the linear and angular velocity of the body // Update the linear and angular velocity of the body
rigidBody->setLinearVelocity(newLinVelocity); rigidBody->setLinearVelocity(newLinVelocity);
rigidBody->setAngularVelocity(newAngVelocity); rigidBody->setAngularVelocity(newAngVelocity);
// Update the position and the orientation of the body // Get current position and orientation of the body
rigidBody->setPosition(rigidBody->getPosition() + newLinVelocity * dt); Vector3D currentPosition = rigidBody->getTransform().getPosition();
rigidBody->setOrientation(rigidBody->getOrientation() + Quaternion(newAngVelocity.getX(), newAngVelocity.getY(), newAngVelocity.getZ(), 0) * rigidBody->getOrientation() * 0.5 * dt); Quaternion currentOrientation = Quaternion(rigidBody->getTransform().getOrientation());
Vector3D newPosition = currentPosition + newLinVelocity * dt;
Quaternion newOrientation = currentOrientation + Quaternion(newAngVelocity.getX(), newAngVelocity.getY(), newAngVelocity.getZ(), 0) * currentOrientation * 0.5 * dt;
Transform newTransform(newPosition, newOrientation.getUnit());
rigidBody->setTransform(newTransform);
} }
// Compute and set the interpolation factor to all bodies // Compute and set the interpolation factor to all bodies

14
src/mathematics/Matrix3x3.cpp
View File

@ -70,20 +70,6 @@ Matrix3x3 Matrix3x3::getInverse() const {
return (invDeterminant * tempMatrix.getTranspose()); return (invDeterminant * tempMatrix.getTranspose());
} }
// Overloaded operator for multiplication with a matrix
Matrix3x3 Matrix3x3::operator*(const Matrix3x3& matrix2) const {
// Compute and return the multiplication of the matrices
return Matrix3x3(array[0][0]*matrix2.array[0][0] + array[0][1]*matrix2.array[1][0] + array[0][2]*matrix2.array[2][0],
array[0][0]*matrix2.array[0][1] + array[0][1]*matrix2.array[1][1] + array[0][2]*matrix2.array[2][1],
array[0][0]*matrix2.array[0][2] + array[0][1]*matrix2.array[1][2] + array[0][2]*matrix2.array[2][2],
array[1][0]*matrix2.array[0][0] + array[1][1]*matrix2.array[1][0] + array[1][2]*matrix2.array[2][0],
array[1][0]*matrix2.array[0][1] + array[1][1]*matrix2.array[1][1] + array[1][2]*matrix2.array[2][1],
array[1][0]*matrix2.array[0][2] + array[1][1]*matrix2.array[1][2] + array[1][2]*matrix2.array[2][2],
array[2][0]*matrix2.array[0][0] + array[2][1]*matrix2.array[1][0] + array[2][2]*matrix2.array[2][0],
array[2][0]*matrix2.array[0][1] + array[2][1]*matrix2.array[1][1] + array[2][2]*matrix2.array[2][1],
array[2][0]*matrix2.array[0][2] + array[2][1]*matrix2.array[1][2] + array[2][2]*matrix2.array[2][2]);
}
// Overloaded operator for assignment // Overloaded operator for assignment
Matrix3x3& Matrix3x3::operator=(const Matrix3x3& matrix2) { Matrix3x3& Matrix3x3::operator=(const Matrix3x3& matrix2) {
// Check for self-assignment // Check for self-assignment

37
src/mathematics/Matrix3x3.h
View File

@ -54,10 +54,13 @@ class Matrix3x3 {
void setValue(int i, int j, double value); // Set a value in the matrix void setValue(int i, int j, double value); // Set a value in the matrix
void setAllValues(double a1, double a2, double a3, double b1, double b2, double b3, void setAllValues(double a1, double a2, double a3, double b1, double b2, double b3,
double c1, double c2, double c3); // Set all the values in the matrix double c1, double c2, double c3); // Set all the values in the matrix
Vector3D getColumn(int i) const; // Return a column
Matrix3x3 getTranspose() const; // Return the transpose matrix Matrix3x3 getTranspose() const; // Return the transpose matrix
double getDeterminant() const; // Return the determinant of the matrix double getDeterminant() const; // Return the determinant of the matrix
double getTrace() const; // Return the trace of the matrix double getTrace() const; // Return the trace of the matrix
Matrix3x3 getInverse() const; // Return the inverse matrix Matrix3x3 getInverse() const; // Return the inverse matrix
Matrix3x3 getAbsoluteMatrix() const; // Return the matrix with absolute values
void setToIdentity(); // Set the matrix to the identity matrix
static Matrix3x3 identity(); // Return the 3x3 identity matrix static Matrix3x3 identity(); // Return the 3x3 identity matrix
// --- Overloaded operators --- // // --- Overloaded operators --- //
@ -91,6 +94,12 @@ inline void Matrix3x3::setAllValues(double a1, double a2, double a3, double b1,
array[2][0] = c1; array[2][1] = c2; array[2][2] = c3; array[2][0] = c1; array[2][1] = c2; array[2][2] = c3;
} }
// Return a column
inline Vector3D Matrix3x3::getColumn(int i) const {
assert(i>= 0 && i<3);
return Vector3D(array[0][i], array[1][i], array[2][i]);
}
// Return the transpose matrix // Return the transpose matrix
inline Matrix3x3 Matrix3x3::getTranspose() const { inline Matrix3x3 Matrix3x3::getTranspose() const {
// Return the transpose matrix // Return the transpose matrix
@ -133,12 +142,40 @@ inline bool Matrix3x3::operator==(const Matrix3x3& matrix2) const {
array[2][0] == matrix2.array[2][0] && array[2][1] == matrix2.array[2][1] && array[2][2] == matrix2.array[2][2]); array[2][0] == matrix2.array[2][0] && array[2][1] == matrix2.array[2][1] && array[2][2] == matrix2.array[2][2]);
} }
// Set the matrix to the identity matrix
inline void Matrix3x3::setToIdentity() {
array[0][0] = 1.0; array[0][1] = 0.0; array[0][2] = 0.0;
array[1][0] = 0.0; array[1][1] = 1.0; array[1][2] = 0.0;
array[2][0] = 0.0; array[2][1] = 0.0; array[2][2] = 1.0;
}
// Return the 3x3 identity matrix // Return the 3x3 identity matrix
inline Matrix3x3 Matrix3x3::identity() { inline Matrix3x3 Matrix3x3::identity() {
// Return the isdentity matrix // Return the isdentity matrix
return Matrix3x3(1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0); return Matrix3x3(1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0);
} }
// Return the matrix with absolute values
inline Matrix3x3 Matrix3x3::getAbsoluteMatrix() const {
return Matrix3x3(fabs(array[0][0]), fabs(array[0][1]), fabs(array[0][2]),
fabs(array[1][0]), fabs(array[1][1]), fabs(array[1][2]),
fabs(array[2][0]), fabs(array[2][1]), fabs(array[2][2]));
}
// Overloaded operator for multiplication with a matrix
inline Matrix3x3 Matrix3x3::operator*(const Matrix3x3& matrix2) const {
// Compute and return the multiplication of the matrices
return Matrix3x3(array[0][0]*matrix2.array[0][0] + array[0][1]*matrix2.array[1][0] + array[0][2]*matrix2.array[2][0],
array[0][0]*matrix2.array[0][1] + array[0][1]*matrix2.array[1][1] + array[0][2]*matrix2.array[2][1],
array[0][0]*matrix2.array[0][2] + array[0][1]*matrix2.array[1][2] + array[0][2]*matrix2.array[2][2],
array[1][0]*matrix2.array[0][0] + array[1][1]*matrix2.array[1][0] + array[1][2]*matrix2.array[2][0],
array[1][0]*matrix2.array[0][1] + array[1][1]*matrix2.array[1][1] + array[1][2]*matrix2.array[2][1],
array[1][0]*matrix2.array[0][2] + array[1][1]*matrix2.array[1][2] + array[1][2]*matrix2.array[2][2],
array[2][0]*matrix2.array[0][0] + array[2][1]*matrix2.array[1][0] + array[2][2]*matrix2.array[2][0],
array[2][0]*matrix2.array[0][1] + array[2][1]*matrix2.array[1][1] + array[2][2]*matrix2.array[2][1],
array[2][0]*matrix2.array[0][2] + array[2][1]*matrix2.array[1][2] + array[2][2]*matrix2.array[2][2]);
}
// Overloaded operator for addition // Overloaded operator for addition
inline Matrix3x3 Matrix3x3::operator+(const Matrix3x3& matrix2) const { inline Matrix3x3 Matrix3x3::operator+(const Matrix3x3& matrix2) const {
// Return the sum matrix // Return the sum matrix

52
src/mathematics/Transform.cpp Normal file
View File

@ -0,0 +1,52 @@
/********************************************************************************
* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ *
* Copyright (c) 2010 Daniel Chappuis *
*********************************************************************************
* *
* Permission is hereby granted, free of charge, to any person obtaining a copy *
* of this software and associated documentation files (the "Software"), to deal *
* in the Software without restriction, including without limitation the rights *
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell *
* copies of the Software, and to permit persons to whom the Software is *
* furnished to do so, subject to the following conditions: *
* *
* The above copyright notice and this permission notice shall be included in *
* all copies or substantial portions of the Software. *
* *
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR *
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, *
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE *
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER *
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, *
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN *
* THE SOFTWARE. *
********************************************************************************/
// Libraries
#include "Transform.h"
// Namespaces
using namespace reactphysics3d;
// Constructor
Transform::Transform() {
position = Vector3D(0.0, 0.0, 0.0);
orientation.setToIdentity();
}
// Constructor
Transform::Transform(const Vector3D& position, const Matrix3x3& orientation) {
this->position = position;
this->orientation = orientation;
}
// Constructor
Transform::Transform(const Vector3D& position, const Quaternion& orientation) {
this->position = position;
this->orientation = orientation.getMatrix();
}
// Destructor
Transform::~Transform() {
}

138
src/mathematics/Transform.h Normal file
View File

@ -0,0 +1,138 @@
/********************************************************************************
* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ *
* Copyright (c) 2010 Daniel Chappuis *
*********************************************************************************
* *
* Permission is hereby granted, free of charge, to any person obtaining a copy *
* of this software and associated documentation files (the "Software"), to deal *
* in the Software without restriction, including without limitation the rights *
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell *
* copies of the Software, and to permit persons to whom the Software is *
* furnished to do so, subject to the following conditions: *
* *
* The above copyright notice and this permission notice shall be included in *
* all copies or substantial portions of the Software. *
* *
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR *
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, *
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE *
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER *
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, *
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN *
* THE SOFTWARE. *
********************************************************************************/
#ifndef TRANSFORM_H
#define TRANSFORM_H
// Libraries
#include "Matrix3x3.h"
#include "Vector3D.h"
#include "Quaternion.h"
// ReactPhysiscs3D namespace
namespace reactphysics3d {
/* -------------------------------------------------------------------
Class Transform :
This class represents a position and an orientation in 3D. It can
also be seen as representing a translation and a rotation
-------------------------------------------------------------------
*/
class Transform {
private :
Vector3D position; // position
Matrix3x3 orientation; // orientation matrix
public :
Transform(); // Constructor
Transform(const Vector3D& position, const Matrix3x3& orientation); // Constructor
Transform(const Vector3D& position, const Quaternion& orientation); // Constructor
~Transform(); // Destructor
const Vector3D& getPosition() const; // Return the origin of the transform
void setPosition(const Vector3D& position); // Set the origin of the transform
const Matrix3x3& getOrientation() const; // Return the rotation matrix
void setOrientation(const Matrix3x3& orientation); // Set the rotation matrix of the transform
void setToIdentity(); // Set the transform to the identity transform
void setFromOpenGL(double* openglMatrix); // Set the transform from an OpenGL transform matrix
void getOpenGLMatrix(double* openglMatrix) const; // Get the OpenGL matrix of the transform
Transform inverse() const; // Return the inverse of the transform
static Transform interpolateTransforms(const Transform& oldTransform,
const Transform& newTransform,
double interpolationFactor); // Return an interpolated transform
Vector3D operator*(const Vector3D& vector) const; // Return the transformed vector
Transform operator*(const Transform& transform2) const; // Operator of multiplication of a transform with another one
};
// Return the position of the transform
inline const Vector3D& Transform::getPosition() const {
return position;
}
// Set the origin of the transform
inline void Transform::setPosition(const Vector3D& position) {
this->position = position;
}
// Return the rotation matrix
inline const Matrix3x3& Transform::getOrientation() const {
return orientation;
}
// Set the rotation matrix of the transform
inline void Transform::setOrientation(const Matrix3x3& orientation) {
this->orientation = orientation;
}
// Set the transform to the identity transform
inline void Transform::setToIdentity() {
position = Vector3D(0.0, 0.0, 0.0);
orientation.setToIdentity();
}
// Set the transform from an OpenGL transform matrix
inline void Transform::setFromOpenGL(double* openglMatrix) {
orientation.setAllValues(openglMatrix[0], openglMatrix[4], openglMatrix[8],
openglMatrix[1], openglMatrix[5], openglMatrix[9],
openglMatrix[2], openglMatrix[6], openglMatrix[10]);
position.setAllValues(openglMatrix[12], openglMatrix[13], openglMatrix[14]);
}
// Get the OpenGL matrix of the transform
inline void Transform::getOpenGLMatrix(double* openglMatrix) const {
openglMatrix[0] = orientation.getValue(0, 0); openglMatrix[1] = orientation.getValue(1, 0); openglMatrix[2] = orientation.getValue(2, 0); openglMatrix[3] = 0.0;
openglMatrix[4] = orientation.getValue(0, 1); openglMatrix[5] = orientation.getValue(1, 1); openglMatrix[6] = orientation.getValue(2, 1); openglMatrix[7] = 0.0;
openglMatrix[8] = orientation.getValue(0, 2); openglMatrix[9] = orientation.getValue(1, 2); openglMatrix[10] = orientation.getValue(2, 2); openglMatrix[11] = 0.0;
openglMatrix[12] = position.getX(); openglMatrix[13] = position.getY(); openglMatrix[14] = position.getZ(); openglMatrix[15] = 1.0;
}
// Return the inverse of the transform
inline Transform Transform::inverse() const {
Matrix3x3 invMatrix = orientation.getTranspose();
return Transform(invMatrix * position.getOpposite(), invMatrix);
}
// Return an interpolated transform
inline Transform Transform::interpolateTransforms(const Transform& oldTransform, const Transform& newTransform,
double interpolationFactor) {
Vector3D interPosition = oldTransform.position * (1.0-interpolationFactor) + newTransform.position * interpolationFactor;
Quaternion interOrientation = Quaternion::slerp(oldTransform.orientation, newTransform.orientation, interpolationFactor);
return Transform(interPosition, interOrientation);
}
// Return the transformed vector
inline Vector3D Transform::operator*(const Vector3D& vector) const {
return (orientation * vector) + position;
}
// Operator of multiplication of a transform with another one
inline Transform Transform::operator*(const Transform& transform2) const {
return Transform(position + orientation * transform2.position, orientation * transform2.orientation);
}
} // End of the ReactPhysics3D namespace
#endif

1
src/mathematics/mathematics.h
View File

@ -31,6 +31,7 @@
#include "Quaternion.h" #include "Quaternion.h"
#include "Vector.h" #include "Vector.h"
#include "Vector3D.h" #include "Vector3D.h"
#include "Transform.h"
#include "../constants.h" #include "../constants.h"
#include "exceptions.h" #include "exceptions.h"
#include "mathematics_functions.h" #include "mathematics_functions.h"