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();
// Create the narrow-phase algorithm that will be used (Separating axis algorithm)
narrowPhaseAlgorithm = new GJKAlgorithm();
narrowPhaseAlgorithm = new GJKAlgorithm(); // TODO : Use GJK algo here
}
// Destructor
@ -90,8 +90,13 @@ void CollisionDetection::computeNarrowPhase() {
for (unsigned int i=0; i<possibleCollisionPairs.size(); i++) {
ContactInfo* contactInfo = NULL;
// Use the narrow-phase collision detection algorithm to check if the really are a contact
if (narrowPhaseAlgorithm->testCollision(possibleCollisionPairs.at(i).first->getNarrowBoundingVolume(), possibleCollisionPairs.at(i).second->getNarrowBoundingVolume(), contactInfo)) {
const RigidBody* rigidBody1 = dynamic_cast<const RigidBody*>(possibleCollisionPairs.at(i).first);
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);
// Add the contact info the current list of collision informations
@ -108,7 +113,7 @@ void CollisionDetection::computeAllContacts() {
assert(contactInfo);
// 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);
}
/* TODO : DELETE THIS
// 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
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));
}
}
*/

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
// the correct penetration depth
bool EPAAlgorithm::computePenetrationDepthAndContactPoints(Simplex simplex, const NarrowBoundingVolume* const boundingVolume1,
const Transform& transform1,
const NarrowBoundingVolume* const boundingVolume2,
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
@ -88,6 +90,12 @@ bool EPAAlgorithm::computePenetrationDepthAndContactPoints(Simplex simplex, cons
TrianglesStore triangleStore; // Store the triangles
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
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
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];
// Compute the support point in the direction of v2
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];
// Compute the support point in the direction of v3
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];
// 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
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];
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];
// 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
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];
int indexNewVertex = nbVertices;
@ -361,10 +369,10 @@ bool EPAAlgorithm::computePenetrationDepthAndContactPoints(Simplex simplex, cons
} while(nbTriangles > 0 && triangleHeap[0]->getDistSquare() <= upperBoundSquarePenDepth);
// Compute the contact info
v = triangle->getClosestPoint();
Vector3D pA = triangle->computeClosestPointOfObject(suppPointsA);
Vector3D pB = triangle->computeClosestPointOfObject(suppPointsB);
// Compute the contact info (in world-space)
v = transform1.getOrientation() * triangle->getClosestPoint();
Vector3D pA = transform1 * triangle->computeClosestPointOfObject(suppPointsA);
Vector3D pB = transform1 * triangle->computeClosestPointOfObject(suppPointsB);
Vector3D normal = v.getUnit();
double penetrationDepth = v.length();
assert(penetrationDepth > 0.0);

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

@ -83,7 +83,9 @@ class EPAAlgorithm {
~EPAAlgorithm(); // Destructor
bool computePenetrationDepthAndContactPoints(Simplex simplex, const NarrowBoundingVolume* const boundingVolume1,
const Transform& transform1,
const NarrowBoundingVolume* const boundingVolume2,
const Transform& transform2,
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
// 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 NarrowBoundingVolume* const boundingVolume2,
bool GJKAlgorithm::testCollision(const NarrowBoundingVolume* const boundingVolume1, const Transform& transform1,
const NarrowBoundingVolume* const boundingVolume2, const Transform& transform2,
ContactInfo*& contactInfo) {
Vector3D suppA; // Support point of object A
Vector3D suppB; // Support point of object B
@ -70,6 +70,12 @@ bool GJKAlgorithm::testCollision(const NarrowBoundingVolume* const boundingVolum
double vDotw;
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);
// 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)
// TODO : Implement frame coherence. For each pair of body, store
// 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
double distSquare = DBL_MAX;
@ -91,7 +97,7 @@ 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 = boundingVolume2->getSupportPoint(v);
suppB = shape2ToShape1 * boundingVolume2->getSupportPoint(rotateToShape2 * v);
// Compute the support point for the Minkowski difference A-B
w = suppA - suppB;
@ -113,11 +119,11 @@ bool GJKAlgorithm::testCollision(const NarrowBoundingVolume* const boundingVolum
// object with the margins
double dist = sqrt(distSquare);
assert(dist > 0.0);
pA = pA - (OBJECT_MARGIN / dist) * v;
pB = pB + (OBJECT_MARGIN / dist) * v;
pA = transform1 * (pA - (OBJECT_MARGIN / dist) * v);
pB = transform1 * (pB + (OBJECT_MARGIN / dist) * v);
// Compute the contact info
Vector3D normal = v.getOpposite().getUnit();
Vector3D normal = transform1.getOrientation() * v.getOpposite().getUnit();
double penetrationDepth = margin - dist;
contactInfo = new ContactInfo(boundingVolume1->getBodyPointer(), boundingVolume2->getBodyPointer(),
normal, penetrationDepth, pA, pB);
@ -138,11 +144,11 @@ bool GJKAlgorithm::testCollision(const NarrowBoundingVolume* const boundingVolum
// object with the margins
double dist = sqrt(distSquare);
assert(dist > 0.0);
pA = pA - (OBJECT_MARGIN / dist) * v;
pB = pB + (OBJECT_MARGIN / dist) * v;
pA = transform1 * (pA - (OBJECT_MARGIN / dist) * v);
pB = transform1 * (pB + (OBJECT_MARGIN / dist) * v);
// Compute the contact info
Vector3D normal = v.getOpposite().getUnit();
Vector3D normal = transform1.getOrientation() * v.getOpposite().getUnit();
double penetrationDepth = margin - dist;
contactInfo = new ContactInfo(boundingVolume1->getBodyPointer(), boundingVolume2->getBodyPointer(),
normal, penetrationDepth, pA, pB);
@ -161,11 +167,11 @@ bool GJKAlgorithm::testCollision(const NarrowBoundingVolume* const boundingVolum
// object with the margins
double dist = sqrt(distSquare);
assert(dist > 0.0);
pA = pA - (OBJECT_MARGIN / dist) * v;
pB = pB + (OBJECT_MARGIN / dist) * v;
pA = transform1 * (pA - (OBJECT_MARGIN / dist) * v);
pB = transform1 * (pB + (OBJECT_MARGIN / dist) * v);
// Compute the contact info
Vector3D normal = v.getOpposite().getUnit();
Vector3D normal = transform1.getOrientation() * v.getOpposite().getUnit();
double penetrationDepth = margin - dist;
contactInfo = new ContactInfo(boundingVolume1->getBodyPointer(), boundingVolume2->getBodyPointer(),
normal, penetrationDepth, pA, pB);
@ -192,11 +198,11 @@ bool GJKAlgorithm::testCollision(const NarrowBoundingVolume* const boundingVolum
// object with the margins
double dist = sqrt(distSquare);
assert(dist > 0.0);
pA = pA - (OBJECT_MARGIN / dist) * v;
pB = pB + (OBJECT_MARGIN / dist) * v;
pA = transform1 * (pA - (OBJECT_MARGIN / dist) * v);
pB = transform1 * (pB + (OBJECT_MARGIN / dist) * v);
// Compute the contact info
Vector3D normal = v.getOpposite().getUnit();
Vector3D normal = transform1.getOrientation() * v.getOpposite().getUnit();
double penetrationDepth = margin - dist;
contactInfo = new ContactInfo(boundingVolume1->getBodyPointer(), boundingVolume2->getBodyPointer(),
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
// polytope to the EPA algorithm to compute the correct penetration depth and contact points between
// 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)
@ -222,8 +228,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 NarrowBoundingVolume* const boundingVolume2,
bool GJKAlgorithm::computePenetrationDepthForEnlargedObjects(const NarrowBoundingVolume* const boundingVolume1, const Transform& transform1,
const NarrowBoundingVolume* const boundingVolume2, const Transform& transform2,
ContactInfo*& contactInfo, Vector3D& v) {
Simplex simplex;
Vector3D suppA;
@ -232,11 +238,17 @@ bool GJKAlgorithm::computePenetrationDepthForEnlargedObjects(const NarrowBoundin
double vDotw;
double distSquare = DBL_MAX;
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 {
// Compute the support points for the enlarged object A and B
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
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
// 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
bool computePenetrationDepthForEnlargedObjects(const NarrowBoundingVolume* const boundingVolume1,
const Transform& transform1,
const NarrowBoundingVolume* const boundingVolume2,
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 NarrowBoundingVolume* const boundingVolume2,
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
};

2
src/collision/NarrowPhaseAlgorithm.h
View File

@ -48,7 +48,9 @@ class NarrowPhaseAlgorithm {
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
};

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
};
// 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
#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
for (vector<Body*>::iterator it = bodies.begin(); it != bodies.end(); ++it) {
aabb = dynamic_cast<const AABB*>((*it)->getBroadBoundingVolume());
aabb = (*it)->getAABB();
assert(aabb);
elemToRemove = find(sortedAABBs.begin(), sortedAABBs.end(), 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) {
aabb = 0;
aabb = dynamic_cast<const AABB*>((*it)->getBroadBoundingVolume());
aabb = (*it)->getAABB();
assert(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
for (it2 = it + 1; it2 != sortedAABBs.end(); ++it2) {
// 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;
}

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
// axis.
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

33
src/collision/SATAlgorithm.cpp
View File

@ -32,6 +32,9 @@
#include <cmath>
#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
using namespace reactphysics3d;
@ -48,8 +51,8 @@ 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 NarrowBoundingVolume* const boundingVolume2,
bool SATAlgorithm::testCollision(const NarrowBoundingVolume* const boundingVolume1, const Transform& transform1,
const NarrowBoundingVolume* const boundingVolume2, const Transform& transform2,
ContactInfo*& contactInfo) {
assert(boundingVolume1 != boundingVolume2);
@ -63,7 +66,7 @@ bool SATAlgorithm::testCollision(const NarrowBoundingVolume* const boundingVolum
// If the two bounding volumes are OBB
if (obb1 && obb2) {
// Compute the collision test between two OBB
return computeCollisionTest(obb1, obb2, contactInfo);
return computeCollisionTest(obb1, transform1, obb2, transform2, contactInfo);
}
else {
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
// 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 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
@ -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 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
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]);
if (absC[0][i] > cutoff) {
existsParallelPair = true;
}
}
udc1[0] = obb1->getAxis(0).dot(boxDistance);
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];
@ -133,13 +138,13 @@ bool SATAlgorithm::computeCollisionTest(const OBB* const obb1, const OBB* const
// Axis A1
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]);
if (absC[1][i] > cutoff) {
existsParallelPair = true;
}
}
udc1[1] = obb1->getAxis(1).dot(boxDistance);
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];
@ -158,13 +163,13 @@ bool SATAlgorithm::computeCollisionTest(const OBB* const obb1, const OBB* const
// Axis A2
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]);
if (absC[2][i] > cutoff) {
existsParallelPair = true;
}
}
udc1[2] = obb1->getAxis(2).dot(boxDistance);
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];
@ -182,7 +187,7 @@ bool SATAlgorithm::computeCollisionTest(const OBB* const obb1, const OBB* const
}
// Axis B0
udc2[0] = obb2->getAxis(0).dot(boxDistance);
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);
@ -200,7 +205,7 @@ bool SATAlgorithm::computeCollisionTest(const OBB* const obb1, const OBB* const
}
// Axis B1
udc2[1] = obb2->getAxis(1).dot(boxDistance);
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);
@ -218,7 +223,7 @@ bool SATAlgorithm::computeCollisionTest(const OBB* const obb1, const OBB* const
}
// Axis B2
udc2[2] = obb2->getAxis(2).dot(boxDistance);
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);

7
src/collision/SATAlgorithm.h
View File

@ -29,6 +29,7 @@
#include "NarrowPhaseAlgorithm.h"
#include "../constraint/Contact.h"
#include "../body/OBB.h"
#include "../mathematics/Transform.h"
// ReactPhysics3D namespace
namespace reactphysics3d {
@ -49,7 +50,9 @@ namespace reactphysics3d {
*/
class SATAlgorithm : public NarrowPhaseAlgorithm {
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
Vector3D computeContactNormal(const Vector3D& axis, const Vector3D& distanceOfOBBs) const; // Compute a contact normal
@ -58,7 +61,9 @@ class SATAlgorithm : public NarrowPhaseAlgorithm {
~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
};

4
src/constraint/Contact.cpp
View File

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

17
src/engine/PhysicsEngine.cpp
View File

@ -144,6 +144,7 @@ void PhysicsEngine::updateAllBodiesMotion() {
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();
}
}
@ -159,18 +160,20 @@ void PhysicsEngine::updatePositionAndOrientationOfBody(Body* body, const Vector3
assert(rigidBody);
// Update the old position and orientation of the body
rigidBody->updateOldPositionAndOrientation();
// Normalize the orientation quaternion
rigidBody->setOrientation(rigidBody->getOrientation().getUnit());
rigidBody->updateOldTransform();
// Update the linear and angular velocity of the body
rigidBody->setLinearVelocity(newLinVelocity);
rigidBody->setAngularVelocity(newAngVelocity);
// Update the position and the orientation of the body
rigidBody->setPosition(rigidBody->getPosition() + newLinVelocity * dt);
rigidBody->setOrientation(rigidBody->getOrientation() + Quaternion(newAngVelocity.getX(), newAngVelocity.getY(), newAngVelocity.getZ(), 0) * rigidBody->getOrientation() * 0.5 * dt);
// Get current position and orientation of the body
Vector3D currentPosition = rigidBody->getTransform().getPosition();
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

14
src/mathematics/Matrix3x3.cpp
View File

@ -70,20 +70,6 @@ Matrix3x3 Matrix3x3::getInverse() const {
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
Matrix3x3& Matrix3x3::operator=(const Matrix3x3& matrix2) {
// 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 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
Vector3D getColumn(int i) const; // Return a column
Matrix3x3 getTranspose() const; // Return the transpose matrix
double getDeterminant() const; // Return the determinant of the matrix
double getTrace() const; // Return the trace of the 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
// --- 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;
}
// 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
inline Matrix3x3 Matrix3x3::getTranspose() const {
// 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]);
}
// 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
inline Matrix3x3 Matrix3x3::identity() {
// 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 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
inline Matrix3x3 Matrix3x3::operator+(const Matrix3x3& matrix2) const {
// 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 "Vector.h"
#include "Vector3D.h"
#include "Transform.h"
#include "../constants.h"
#include "exceptions.h"
#include "mathematics_functions.h"