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

Use a quaternion instead of a matrix in the Transform

Browse Source 
git-svn-id: https://reactphysics3d.googlecode.com/svn/trunk@435 92aac97c-a6ce-11dd-a772-7fcde58d38e6
This commit is contained in:
chappuis.daniel 2011-08-08 21:41:07 +00:00
parent 00b9fc45dc
commit d9452e727c
9 changed files with 74 additions and 71 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
src/body/RigidBody.h
View File

@ -142,7 +142,7 @@ inline void RigidBody::setInertiaTensorLocal(const Matrix3x3& inertiaTensorLocal
// where R is the rotation matrix (and R^T its transpose) of the current orientation quaternion of the body // where R is the rotation matrix (and R^T its transpose) of the current orientation quaternion of the body
inline Matrix3x3 RigidBody::getInertiaTensorWorld() const { inline Matrix3x3 RigidBody::getInertiaTensorWorld() const {
// Compute and return the inertia tensor in world coordinates // Compute and return the inertia tensor in world coordinates
return transform.getOrientation() * inertiaTensorLocal * transform.getOrientation().getTranspose(); return transform.getOrientation().getMatrix() * inertiaTensorLocal * transform.getOrientation().getMatrix().getTranspose();
} }
// Return the inverse of the inertia tensor in world coordinates // Return the inverse of the inertia tensor in world coordinates
@ -151,7 +151,7 @@ inline Matrix3x3 RigidBody::getInertiaTensorWorld() const {
// where R is the rotation matrix (and R^T its transpose) of the current orientation quaternion of the body // where R is the rotation matrix (and R^T its transpose) of the current orientation quaternion of the body
inline Matrix3x3 RigidBody::getInertiaTensorInverseWorld() const { inline Matrix3x3 RigidBody::getInertiaTensorInverseWorld() const {
// Compute and return the inertia tensor in world coordinates // Compute and return the inertia tensor in world coordinates
return transform.getOrientation() * inertiaTensorLocalInverse * transform.getOrientation().getTranspose(); return transform.getOrientation().getMatrix() * inertiaTensorLocalInverse * transform.getOrientation().getMatrix().getTranspose();
} }
// Set the linear velocity of the rigid body // Set the linear velocity of the rigid body

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

@ -92,7 +92,7 @@ bool EPAAlgorithm::computePenetrationDepthAndContactPoints(Simplex simplex, cons
Transform shape2ToShape1 = transform1.inverse() * transform2; Transform shape2ToShape1 = transform1.inverse() * transform2;
// Matrix that transform a direction from body space of shape 1 into body space of shape 2 // Matrix that transform a direction from body space of shape 1 into body space of shape 2
Matrix3x3 rotateToShape2 = transform2.getOrientation().getTranspose() * transform1.getOrientation(); Matrix3x3 rotateToShape2 = transform2.getOrientation().getMatrix().getTranspose() * transform1.getOrientation().getMatrix();
// 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);
@ -367,7 +367,7 @@ bool EPAAlgorithm::computePenetrationDepthAndContactPoints(Simplex simplex, cons
} while(nbTriangles > 0 && triangleHeap[0]->getDistSquare() <= upperBoundSquarePenDepth); } while(nbTriangles > 0 && triangleHeap[0]->getDistSquare() <= upperBoundSquarePenDepth);
// Compute the contact info (in world-space) // Compute the contact info (in world-space)
v = transform1.getOrientation() * triangle->getClosestPoint(); v = transform1.getOrientation().getMatrix() * triangle->getClosestPoint();
Vector3D pA = transform1 * triangle->computeClosestPointOfObject(suppPointsA); Vector3D pA = transform1 * triangle->computeClosestPointOfObject(suppPointsA);
Vector3D pB = transform1 * triangle->computeClosestPointOfObject(suppPointsB); Vector3D pB = transform1 * triangle->computeClosestPointOfObject(suppPointsB);
Vector3D normal = v.getUnit(); Vector3D normal = v.getUnit();

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

@ -76,7 +76,7 @@ bool GJKAlgorithm::testCollision(const Shape* shape1, const Transform& transform
Transform shape2ToShape1 = transform1.inverse() * transform2; Transform shape2ToShape1 = transform1.inverse() * transform2;
// Matrix that transform a direction from body space of shape 1 into body space of shape 2 // Matrix that transform a direction from body space of shape 1 into body space of shape 2
Matrix3x3 rotateToShape2 = transform2.getOrientation().getTranspose() * transform1.getOrientation(); Matrix3x3 rotateToShape2 = transform2.getOrientation().getMatrix().getTranspose() * transform1.getOrientation().getMatrix();
// Initialize the margin (sum of margins of both objects) // Initialize the margin (sum of margins of both objects)
double margin = 2 * OBJECT_MARGIN; double margin = 2 * OBJECT_MARGIN;
@ -123,7 +123,7 @@ bool GJKAlgorithm::testCollision(const Shape* shape1, const Transform& transform
pB = transform1 * (pB + (OBJECT_MARGIN / dist) * v); pB = transform1 * (pB + (OBJECT_MARGIN / dist) * v);
// Compute the contact info // Compute the contact info
Vector3D normal = transform1.getOrientation() * v.getOpposite().getUnit(); Vector3D normal = transform1.getOrientation().getMatrix() * v.getOpposite().getUnit();
double penetrationDepth = margin - dist; double penetrationDepth = margin - dist;
contactInfo = new ContactInfo(body1, body2, normal, penetrationDepth, pA, pB); contactInfo = new ContactInfo(body1, body2, normal, penetrationDepth, pA, pB);
@ -147,7 +147,7 @@ bool GJKAlgorithm::testCollision(const Shape* shape1, const Transform& transform
pB = transform1 * (pB + (OBJECT_MARGIN / dist) * v); pB = transform1 * (pB + (OBJECT_MARGIN / dist) * v);
// Compute the contact info // Compute the contact info
Vector3D normal = transform1.getOrientation() * v.getOpposite().getUnit(); Vector3D normal = transform1.getOrientation().getMatrix() * v.getOpposite().getUnit();
double penetrationDepth = margin - dist; double penetrationDepth = margin - dist;
contactInfo = new ContactInfo(body1, body2, normal, penetrationDepth, pA, pB); contactInfo = new ContactInfo(body1, body2, normal, penetrationDepth, pA, pB);
@ -169,7 +169,7 @@ bool GJKAlgorithm::testCollision(const Shape* shape1, const Transform& transform
pB = transform1 * (pB + (OBJECT_MARGIN / dist) * v); pB = transform1 * (pB + (OBJECT_MARGIN / dist) * v);
// Compute the contact info // Compute the contact info
Vector3D normal = transform1.getOrientation() * v.getOpposite().getUnit(); Vector3D normal = transform1.getOrientation().getMatrix() * v.getOpposite().getUnit();
double penetrationDepth = margin - dist; double penetrationDepth = margin - dist;
contactInfo = new ContactInfo(body1, body2, normal, penetrationDepth, pA, pB); contactInfo = new ContactInfo(body1, body2, normal, penetrationDepth, pA, pB);
@ -199,7 +199,7 @@ bool GJKAlgorithm::testCollision(const Shape* shape1, const Transform& transform
pB = transform1 * (pB + (OBJECT_MARGIN / dist) * v); pB = transform1 * (pB + (OBJECT_MARGIN / dist) * v);
// Compute the contact info // Compute the contact info
Vector3D normal = transform1.getOrientation() * v.getOpposite().getUnit(); Vector3D normal = transform1.getOrientation().getMatrix() * v.getOpposite().getUnit();
double penetrationDepth = margin - dist; double penetrationDepth = margin - dist;
contactInfo = new ContactInfo(body1, body2, normal, penetrationDepth, pA, pB); contactInfo = new ContactInfo(body1, body2, normal, penetrationDepth, pA, pB);
@ -235,7 +235,7 @@ bool GJKAlgorithm::computePenetrationDepthForEnlargedObjects(const Shape* const
Transform shape2ToShape1 = transform1.inverse() * transform2; Transform shape2ToShape1 = transform1.inverse() * transform2;
// Matrix that transform a direction from body space of shape 1 into body space of shape 2 // Matrix that transform a direction from body space of shape 1 into body space of shape 2
Matrix3x3 rotateToShape2 = transform2.getOrientation().getTranspose() * transform1.getOrientation(); Matrix3x3 rotateToShape2 = transform2.getOrientation().getMatrix().getTranspose() * transform1.getOrientation().getMatrix();
do { do {
// Compute the support points for the enlarged object A and B // Compute the support points for the enlarged object A and B

48
src/collision/SATAlgorithm.cpp
View File

@ -113,13 +113,13 @@ bool SATAlgorithm::computeCollisionTest(const BoxShape* obb1, const Transform& t
// Axis A0 // Axis A0
for (int i=0; i<3; ++i) { for (int i=0; i<3; ++i) {
c[0][i] = transform1.getOrientation().getColumn(0).dot(transform2.getOrientation().getColumn(i)); c[0][i] = transform1.getOrientation().getMatrix().getColumn(0).dot(transform2.getOrientation().getMatrix().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] = transform1.getOrientation().getColumn(0).dot(boxDistance); udc1[0] = transform1.getOrientation().getMatrix().getColumn(0).dot(boxDistance);
center = udc1[0]; center = udc1[0];
radius1 = obb1->getExtent().getX(); radius1 = obb1->getExtent().getX();
radius2 = obb2->getExtent().getX()*absC[0][0] + obb2->getExtent().getY()*absC[0][1] + obb2->getExtent().getZ() * absC[0][2]; radius2 = obb2->getExtent().getX()*absC[0][0] + obb2->getExtent().getY()*absC[0][1] + obb2->getExtent().getZ() * absC[0][2];
@ -133,18 +133,18 @@ bool SATAlgorithm::computeCollisionTest(const BoxShape* obb1, const Transform& t
} }
else if (penetrationDepth < minPenetrationDepth) { // Interval 1 and 2 overlap with a smaller penetration depth on this axis else if (penetrationDepth < minPenetrationDepth) { // Interval 1 and 2 overlap with a smaller penetration depth on this axis
minPenetrationDepth = penetrationDepth; // Update the minimum penetration depth minPenetrationDepth = penetrationDepth; // Update the minimum penetration depth
normal = computeContactNormal(transform1.getOrientation().getColumn(0), boxDistance); // Compute the contact normal with the correct sign normal = computeContactNormal(transform1.getOrientation().getMatrix().getColumn(0), boxDistance); // Compute the contact normal with the correct sign
} }
// Axis A1 // Axis A1
for (int i=0; i<3; ++i) { for (int i=0; i<3; ++i) {
c[1][i] = transform1.getOrientation().getColumn(1).dot(transform2.getOrientation().getColumn(i)); c[1][i] = transform1.getOrientation().getMatrix().getColumn(1).dot(transform2.getOrientation().getMatrix().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] = transform1.getOrientation().getColumn(1).dot(boxDistance); udc1[1] = transform1.getOrientation().getMatrix().getColumn(1).dot(boxDistance);
center = udc1[1]; center = udc1[1];
radius1 = obb1->getExtent().getY(); radius1 = obb1->getExtent().getY();
radius2 = obb2->getExtent().getX()*absC[1][0] + obb2->getExtent().getY()*absC[1][1] + obb2->getExtent().getZ() * absC[1][2]; radius2 = obb2->getExtent().getX()*absC[1][0] + obb2->getExtent().getY()*absC[1][1] + obb2->getExtent().getZ() * absC[1][2];
@ -158,18 +158,18 @@ bool SATAlgorithm::computeCollisionTest(const BoxShape* obb1, const Transform& t
} }
else if (penetrationDepth < minPenetrationDepth) { // Interval 1 and 2 overlap with a smaller penetration depth on this axis else if (penetrationDepth < minPenetrationDepth) { // Interval 1 and 2 overlap with a smaller penetration depth on this axis
minPenetrationDepth = penetrationDepth; // Update the minimum penetration depth minPenetrationDepth = penetrationDepth; // Update the minimum penetration depth
normal = computeContactNormal(transform1.getOrientation().getColumn(1), boxDistance); // Compute the contact normal with the correct sign normal = computeContactNormal(transform1.getOrientation().getMatrix().getColumn(1), boxDistance); // Compute the contact normal with the correct sign
} }
// Axis A2 // Axis A2
for (int i=0; i<3; ++i) { for (int i=0; i<3; ++i) {
c[2][i] = transform1.getOrientation().getColumn(2).dot(transform2.getOrientation().getColumn(i)); c[2][i] = transform1.getOrientation().getMatrix().getColumn(2).dot(transform2.getOrientation().getMatrix().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] = transform1.getOrientation().getColumn(2).dot(boxDistance); udc1[2] = transform1.getOrientation().getMatrix().getColumn(2).dot(boxDistance);
center = udc1[2]; center = udc1[2];
radius1 = obb1->getExtent().getZ(); radius1 = obb1->getExtent().getZ();
radius2 = obb2->getExtent().getX()*absC[2][0] + obb2->getExtent().getY()*absC[2][1] + obb2->getExtent().getZ()*absC[2][2]; radius2 = obb2->getExtent().getX()*absC[2][0] + obb2->getExtent().getY()*absC[2][1] + obb2->getExtent().getZ()*absC[2][2];
@ -183,11 +183,11 @@ bool SATAlgorithm::computeCollisionTest(const BoxShape* obb1, const Transform& t
} }
else if (penetrationDepth < minPenetrationDepth) { // Interval 1 and 2 overlap with a smaller penetration depth on this axis else if (penetrationDepth < minPenetrationDepth) { // Interval 1 and 2 overlap with a smaller penetration depth on this axis
minPenetrationDepth = penetrationDepth; // Update the minimum penetration depth minPenetrationDepth = penetrationDepth; // Update the minimum penetration depth
normal = computeContactNormal(transform1.getOrientation().getColumn(2), boxDistance); // Compute the contact normal with the correct sign normal = computeContactNormal(transform1.getOrientation().getMatrix().getColumn(2), boxDistance); // Compute the contact normal with the correct sign
} }
// Axis B0 // Axis B0
udc2[0] = transform2.getOrientation().getColumn(0).dot(boxDistance); udc2[0] = transform2.getOrientation().getMatrix().getColumn(0).dot(boxDistance);
center = udc2[0]; center = udc2[0];
radius1 = obb1->getExtent().getX()*absC[0][0] + obb1->getExtent().getY()*absC[1][0] + obb1->getExtent().getZ() * absC[2][0]; radius1 = obb1->getExtent().getX()*absC[0][0] + obb1->getExtent().getY()*absC[1][0] + obb1->getExtent().getZ() * absC[2][0];
radius2 = obb2->getExtent().getX(); radius2 = obb2->getExtent().getX();
@ -201,11 +201,11 @@ bool SATAlgorithm::computeCollisionTest(const BoxShape* obb1, const Transform& t
} }
else if (penetrationDepth < minPenetrationDepth) { // Interval 1 and 2 overlap with a smaller penetration depth on this axis else if (penetrationDepth < minPenetrationDepth) { // Interval 1 and 2 overlap with a smaller penetration depth on this axis
minPenetrationDepth = penetrationDepth; // Update the minimum penetration depth minPenetrationDepth = penetrationDepth; // Update the minimum penetration depth
normal = computeContactNormal(transform2.getOrientation().getColumn(0), boxDistance); // Compute the contact normal with the correct sign normal = computeContactNormal(transform2.getOrientation().getMatrix().getColumn(0), boxDistance); // Compute the contact normal with the correct sign
} }
// Axis B1 // Axis B1
udc2[1] = transform2.getOrientation().getColumn(1).dot(boxDistance); udc2[1] = transform2.getOrientation().getMatrix().getColumn(1).dot(boxDistance);
center = udc2[1]; center = udc2[1];
radius1 = obb1->getExtent().getX()*absC[0][1] + obb1->getExtent().getY()*absC[1][1] + obb1->getExtent().getZ() * absC[2][1]; radius1 = obb1->getExtent().getX()*absC[0][1] + obb1->getExtent().getY()*absC[1][1] + obb1->getExtent().getZ() * absC[2][1];
radius2 = obb2->getExtent().getY(); radius2 = obb2->getExtent().getY();
@ -219,11 +219,11 @@ bool SATAlgorithm::computeCollisionTest(const BoxShape* obb1, const Transform& t
} }
else if (penetrationDepth < minPenetrationDepth) { // Interval 1 and 2 overlap with a smaller penetration depth on this axis else if (penetrationDepth < minPenetrationDepth) { // Interval 1 and 2 overlap with a smaller penetration depth on this axis
minPenetrationDepth = penetrationDepth; // Update the minimum penetration depth minPenetrationDepth = penetrationDepth; // Update the minimum penetration depth
normal = computeContactNormal(transform2.getOrientation().getColumn(1), boxDistance); // Compute the contact normal with the correct sign normal = computeContactNormal(transform2.getOrientation().getMatrix().getColumn(1), boxDistance); // Compute the contact normal with the correct sign
} }
// Axis B2 // Axis B2
udc2[2] = transform2.getOrientation().getColumn(2).dot(boxDistance); udc2[2] = transform2.getOrientation().getMatrix().getColumn(2).dot(boxDistance);
center = udc2[2]; center = udc2[2];
radius1 = obb1->getExtent().getX()*absC[0][2] + obb1->getExtent().getY()*absC[1][2] + obb1->getExtent().getZ()*absC[2][2]; radius1 = obb1->getExtent().getX()*absC[0][2] + obb1->getExtent().getY()*absC[1][2] + obb1->getExtent().getZ()*absC[2][2];
radius2 = obb2->getExtent().getZ(); radius2 = obb2->getExtent().getZ();
@ -237,7 +237,7 @@ bool SATAlgorithm::computeCollisionTest(const BoxShape* obb1, const Transform& t
} }
else if (penetrationDepth < minPenetrationDepth) { // Interval 1 and 2 overlap with a smaller penetration depth on this axis else if (penetrationDepth < minPenetrationDepth) { // Interval 1 and 2 overlap with a smaller penetration depth on this axis
minPenetrationDepth = penetrationDepth; // Update the minimum penetration depth minPenetrationDepth = penetrationDepth; // Update the minimum penetration depth
normal = computeContactNormal(transform2.getOrientation().getColumn(2), boxDistance); // Compute the contact normal with the correct sign normal = computeContactNormal(transform2.getOrientation().getMatrix().getColumn(2), boxDistance); // Compute the contact normal with the correct sign
} }
// If there exists a parallel pair of face normals // If there exists a parallel pair of face normals
@ -265,7 +265,7 @@ bool SATAlgorithm::computeCollisionTest(const BoxShape* obb1, const Transform& t
} }
else if (penetrationDepth < minPenetrationDepth) { // Interval 1 and 2 overlap with a smaller penetration depth on this axis else if (penetrationDepth < minPenetrationDepth) { // Interval 1 and 2 overlap with a smaller penetration depth on this axis
minPenetrationDepth = penetrationDepth; // Update the minimum penetration depth minPenetrationDepth = penetrationDepth; // Update the minimum penetration depth
normal = computeContactNormal(transform1.getOrientation().getColumn(0).cross(transform2.getOrientation().getColumn(0)), boxDistance); // Compute the contact normal with the correct sign normal = computeContactNormal(transform1.getOrientation().getMatrix().getColumn(0).cross(transform2.getOrientation().getMatrix().getColumn(0)), boxDistance); // Compute the contact normal with the correct sign
} }
// Axis A0 x B1 // Axis A0 x B1
@ -282,7 +282,7 @@ bool SATAlgorithm::computeCollisionTest(const BoxShape* obb1, const Transform& t
} }
else if (penetrationDepth < minPenetrationDepth) { // Interval 1 and 2 overlap with a smaller penetration depth on this axis else if (penetrationDepth < minPenetrationDepth) { // Interval 1 and 2 overlap with a smaller penetration depth on this axis
minPenetrationDepth = penetrationDepth; // Update the minimum penetration depth minPenetrationDepth = penetrationDepth; // Update the minimum penetration depth
normal = computeContactNormal(transform1.getOrientation().getColumn(0).cross(transform2.getOrientation().getColumn(1)), boxDistance); // Compute the contact normal with the correct sign normal = computeContactNormal(transform1.getOrientation().getMatrix().getColumn(0).cross(transform2.getOrientation().getMatrix().getColumn(1)), boxDistance); // Compute the contact normal with the correct sign
} }
// Axis A0 x B2 // Axis A0 x B2
@ -299,7 +299,7 @@ bool SATAlgorithm::computeCollisionTest(const BoxShape* obb1, const Transform& t
} }
else if (penetrationDepth < minPenetrationDepth) { // Interval 1 and 2 overlap with a smaller penetration depth on this axis else if (penetrationDepth < minPenetrationDepth) { // Interval 1 and 2 overlap with a smaller penetration depth on this axis
minPenetrationDepth = penetrationDepth; // Update the minimum penetration depth minPenetrationDepth = penetrationDepth; // Update the minimum penetration depth
normal = computeContactNormal(transform1.getOrientation().getColumn(0).cross(transform2.getOrientation().getColumn(2)), boxDistance); // Compute the contact normal with the correct sign normal = computeContactNormal(transform1.getOrientation().getMatrix().getColumn(0).cross(transform2.getOrientation().getMatrix().getColumn(2)), boxDistance); // Compute the contact normal with the correct sign
} }
// Axis A1 x B0 // Axis A1 x B0
@ -316,7 +316,7 @@ bool SATAlgorithm::computeCollisionTest(const BoxShape* obb1, const Transform& t
} }
else if (penetrationDepth < minPenetrationDepth) { // Interval 1 and 2 overlap with a smaller penetration depth on this axis else if (penetrationDepth < minPenetrationDepth) { // Interval 1 and 2 overlap with a smaller penetration depth on this axis
minPenetrationDepth = penetrationDepth; // Update the minimum penetration depth minPenetrationDepth = penetrationDepth; // Update the minimum penetration depth
normal = computeContactNormal(transform1.getOrientation().getColumn(1).cross(transform2.getOrientation().getColumn(0)), boxDistance); // Compute the contact normal with the correct sign normal = computeContactNormal(transform1.getOrientation().getMatrix().getColumn(1).cross(transform2.getOrientation().getMatrix().getColumn(0)), boxDistance); // Compute the contact normal with the correct sign
} }
// Axis A1 x B1 // Axis A1 x B1
@ -333,7 +333,7 @@ bool SATAlgorithm::computeCollisionTest(const BoxShape* obb1, const Transform& t
} }
else if (penetrationDepth < minPenetrationDepth) { // Interval 1 and 2 overlap with a smaller penetration depth on this axis else if (penetrationDepth < minPenetrationDepth) { // Interval 1 and 2 overlap with a smaller penetration depth on this axis
minPenetrationDepth = penetrationDepth; // Update the minimum penetration depth minPenetrationDepth = penetrationDepth; // Update the minimum penetration depth
normal = computeContactNormal(transform1.getOrientation().getColumn(1).cross(transform2.getOrientation().getColumn(1)), boxDistance); // Compute the contact normal with the correct sign normal = computeContactNormal(transform1.getOrientation().getMatrix().getColumn(1).cross(transform2.getOrientation().getMatrix().getColumn(1)), boxDistance); // Compute the contact normal with the correct sign
} }
// Axis A1 x B2 // Axis A1 x B2
@ -350,7 +350,7 @@ bool SATAlgorithm::computeCollisionTest(const BoxShape* obb1, const Transform& t
} }
else if (penetrationDepth < minPenetrationDepth) { // Interval 1 and 2 overlap with a smaller penetration depth on this axis else if (penetrationDepth < minPenetrationDepth) { // Interval 1 and 2 overlap with a smaller penetration depth on this axis
minPenetrationDepth = penetrationDepth; // Update the minimum penetration depth minPenetrationDepth = penetrationDepth; // Update the minimum penetration depth
normal = computeContactNormal(transform1.getOrientation().getColumn(1).cross(transform2.getOrientation().getColumn(2)), boxDistance); // Compute the contact normal with the correct sign normal = computeContactNormal(transform1.getOrientation().getMatrix().getColumn(1).cross(transform2.getOrientation().getMatrix().getColumn(2)), boxDistance); // Compute the contact normal with the correct sign
} }
// Axis A2 x B0 // Axis A2 x B0
@ -367,7 +367,7 @@ bool SATAlgorithm::computeCollisionTest(const BoxShape* obb1, const Transform& t
} }
else if (penetrationDepth < minPenetrationDepth) { // Interval 1 and 2 overlap with a smaller penetration depth on this axis else if (penetrationDepth < minPenetrationDepth) { // Interval 1 and 2 overlap with a smaller penetration depth on this axis
minPenetrationDepth = penetrationDepth; // Update the minimum penetration depth minPenetrationDepth = penetrationDepth; // Update the minimum penetration depth
normal = computeContactNormal(transform1.getOrientation().getColumn(2).cross(transform2.getOrientation().getColumn(0)), boxDistance); // Compute the contact normal with the correct sign normal = computeContactNormal(transform1.getOrientation().getMatrix().getColumn(2).cross(transform2.getOrientation().getMatrix().getColumn(0)), boxDistance); // Compute the contact normal with the correct sign
} }
// Axis A2 x B1 // Axis A2 x B1
@ -384,7 +384,7 @@ bool SATAlgorithm::computeCollisionTest(const BoxShape* obb1, const Transform& t
} }
else if (penetrationDepth < minPenetrationDepth) { // Interval 1 and 2 overlap with a smaller penetration depth on this axis else if (penetrationDepth < minPenetrationDepth) { // Interval 1 and 2 overlap with a smaller penetration depth on this axis
minPenetrationDepth = penetrationDepth; // Update the minimum penetration depth minPenetrationDepth = penetrationDepth; // Update the minimum penetration depth
normal = computeContactNormal(transform1.getOrientation().getColumn(2).cross(transform2.getOrientation().getColumn(1)), boxDistance); // Compute the contact normal with the correct sign normal = computeContactNormal(transform1.getOrientation().getMatrix().getColumn(2).cross(transform2.getOrientation().getMatrix().getColumn(1)), boxDistance); // Compute the contact normal with the correct sign
} }
// Axis A2 x B2 // Axis A2 x B2
@ -401,7 +401,7 @@ bool SATAlgorithm::computeCollisionTest(const BoxShape* obb1, const Transform& t
} }
else if (penetrationDepth < minPenetrationDepth) { // Interval 1 and 2 overlap with a smaller penetration depth on this axis else if (penetrationDepth < minPenetrationDepth) { // Interval 1 and 2 overlap with a smaller penetration depth on this axis
minPenetrationDepth = penetrationDepth; // Update the minimum penetration depth minPenetrationDepth = penetrationDepth; // Update the minimum penetration depth
normal = computeContactNormal(transform1.getOrientation().getColumn(2).cross(transform2.getOrientation().getColumn(2)), boxDistance); // Compute the contact normal with the correct sign normal = computeContactNormal(transform1.getOrientation().getMatrix().getColumn(2).cross(transform2.getOrientation().getMatrix().getColumn(2)), boxDistance); // Compute the contact normal with the correct sign
} }
// Compute the contact info // Compute the contact info

4
src/engine/PhysicsEngine.cpp
View File

@ -166,8 +166,8 @@ void PhysicsEngine::updatePositionAndOrientationOfBody(Body* body, const Vector3
rigidBody->setAngularVelocity(newAngVelocity); rigidBody->setAngularVelocity(newAngVelocity);
// Get current position and orientation of the body // Get current position and orientation of the body
Vector3D currentPosition = rigidBody->getTransform().getPosition(); const Vector3D& currentPosition = rigidBody->getTransform().getPosition();
Quaternion currentOrientation = Quaternion(rigidBody->getTransform().getOrientation()); const Quaternion& currentOrientation = rigidBody->getTransform().getOrientation();
Vector3D newPosition = currentPosition + newLinVelocity * dt; Vector3D newPosition = currentPosition + newLinVelocity * dt;
Quaternion newOrientation = currentOrientation + Quaternion(newAngVelocity.getX(), newAngVelocity.getY(), newAngVelocity.getZ(), 0) * currentOrientation * 0.5 * dt; Quaternion newOrientation = currentOrientation + Quaternion(newAngVelocity.getX(), newAngVelocity.getY(), newAngVelocity.getZ(), 0) * currentOrientation * 0.5 * dt;

21
src/mathematics/Quaternion.h
View File

@ -64,10 +64,11 @@ class Quaternion {
void setW(double w); // Set the value w void setW(double w); // Set the value w
Vector3D vectorV() const; // Return the vector v=(x y z) of the quaternion Vector3D vectorV() const; // Return the vector v=(x y z) of the quaternion
double length() const; // Return the length of the quaternion double length() const; // Return the length of the quaternion
Quaternion getUnit() const throw (MathematicsException); // Return the unit quaternion Quaternion getUnit() const; // Return the unit quaternion
Quaternion getConjugate() const; // Return the conjugate quaternion Quaternion getConjugate() const; // Return the conjugate quaternion
Quaternion getInverse() const throw (MathematicsException); // Return the inverse of the quaternion Quaternion getInverse() const throw (MathematicsException); // Return the inverse of the quaternion
Matrix3x3 getMatrix() const; // Return the orientation matrix corresponding to this quaternion Matrix3x3 getMatrix() const; // Return the orientation matrix corresponding to this quaternion
static Quaternion identity(); // Return the identity quaternion
double dot(const Quaternion& quaternion) const; // Dot product between two quaternions double dot(const Quaternion& quaternion) const; // Dot product between two quaternions
void getRotationAngleAxis(double& angle, Vector3D& axis) const; // Compute the rotation angle (in radians) and the axis void getRotationAngleAxis(double& angle, Vector3D& axis) const; // Compute the rotation angle (in radians) and the axis
static Quaternion slerp(const Quaternion& quaternion1, static Quaternion slerp(const Quaternion& quaternion1,
@ -136,19 +137,19 @@ inline double Quaternion::length() const {
} }
// Return the unit quaternion // Return the unit quaternion
inline Quaternion Quaternion::getUnit() const throw(MathematicsException) { inline Quaternion Quaternion::getUnit() const {
double lengthQuaternion = length(); double lengthQuaternion = length();
// Check if the length is not equal to zero // Check if the length is not equal to zero
if (lengthQuaternion != 0.0) { assert (lengthQuaternion != 0.0);
// Compute and return the unit quaternion // Compute and return the unit quaternion
return Quaternion(x/lengthQuaternion, y/lengthQuaternion, z/lengthQuaternion, w/lengthQuaternion); return Quaternion(x/lengthQuaternion, y/lengthQuaternion, z/lengthQuaternion, w/lengthQuaternion);
} }
else {
// Throw an exception because it's impossible to compute a unit quaternion if its length is equal to zero // Return the identity quaternion
throw MathematicsException("MathematicsException : Impossible to compute the unit quaternion if the length of the quaternion is zero"); inline Quaternion Quaternion::identity() {
} return Quaternion(0.0, 0.0, 0.0, 1.0);
} }
// Return the conjugate of the quaternion (inline) // Return the conjugate of the quaternion (inline)

6
src/mathematics/Transform.cpp
View File

@ -31,19 +31,19 @@ using namespace reactphysics3d;
// Constructor // Constructor
Transform::Transform() { Transform::Transform() {
position = Vector3D(0.0, 0.0, 0.0); position = Vector3D(0.0, 0.0, 0.0);
orientation.setToIdentity(); orientation = Quaternion::identity();
} }
// Constructor // Constructor
Transform::Transform(const Vector3D& position, const Matrix3x3& orientation) { Transform::Transform(const Vector3D& position, const Matrix3x3& orientation) {
this->position = position; this->position = position;
this->orientation = orientation; this->orientation = Quaternion(orientation);
} }
// Constructor // Constructor
Transform::Transform(const Vector3D& position, const Quaternion& orientation) { Transform::Transform(const Vector3D& position, const Quaternion& orientation) {
this->position = position; this->position = position;
this->orientation = orientation.getMatrix(); this->orientation = orientation;
} }
// Destructor // Destructor

44
src/mathematics/Transform.h
View File

@ -41,8 +41,8 @@ namespace reactphysics3d {
*/ */
class Transform { class Transform {
private : private :
Vector3D position; // position Vector3D position; // Position
Matrix3x3 orientation; // orientation matrix Quaternion orientation; // Orientation
public : public :
Transform(); // Constructor Transform(); // Constructor
@ -52,8 +52,8 @@ class Transform {
const Vector3D& getPosition() const; // Return the origin of the transform const Vector3D& getPosition() const; // Return the origin of the transform
void setPosition(const Vector3D& position); // Set the origin of the transform void setPosition(const Vector3D& position); // Set the origin of the transform
const Matrix3x3& getOrientation() const; // Return the rotation matrix const Quaternion& getOrientation() const; // Return the orientation quaternion
void setOrientation(const Matrix3x3& orientation); // Set the rotation matrix of the transform void setOrientation(const Quaternion& orientation); // Set the rotation quaternion
void setToIdentity(); // Set the transform to the identity transform void setToIdentity(); // Set the transform to the identity transform
void setFromOpenGL(double* openglMatrix); // Set the transform from an OpenGL transform matrix void setFromOpenGL(double* openglMatrix); // Set the transform from an OpenGL transform matrix
void getOpenGLMatrix(double* openglMatrix) const; // Get the OpenGL matrix of the transform void getOpenGLMatrix(double* openglMatrix) const; // Get the OpenGL matrix of the transform
@ -62,7 +62,7 @@ class Transform {
const Transform& newTransform, const Transform& newTransform,
double interpolationFactor); // Return an interpolated transform double interpolationFactor); // Return an interpolated transform
Vector3D operator*(const Vector3D& vector) const; // Return the transformed vector 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 Transform operator*(const Transform& transform2) const; // Operator of multiplication of a transform with another one
}; };
@ -77,59 +77,61 @@ inline void Transform::setPosition(const Vector3D& position) {
} }
// Return the rotation matrix // Return the rotation matrix
inline const Matrix3x3& Transform::getOrientation() const { inline const Quaternion& Transform::getOrientation() const {
return orientation; return orientation;
} }
// Set the rotation matrix of the transform // Set the rotation matrix of the transform
inline void Transform::setOrientation(const Matrix3x3& orientation) { inline void Transform::setOrientation(const Quaternion& orientation) {
this->orientation = orientation; this->orientation = orientation;
} }
// Set the transform to the identity transform // Set the transform to the identity transform
inline void Transform::setToIdentity() { inline void Transform::setToIdentity() {
position = Vector3D(0.0, 0.0, 0.0); position = Vector3D(0.0, 0.0, 0.0);
orientation.setToIdentity(); orientation = Quaternion::identity();
} }
// Set the transform from an OpenGL transform matrix // Set the transform from an OpenGL transform matrix
inline void Transform::setFromOpenGL(double* openglMatrix) { inline void Transform::setFromOpenGL(double* openglMatrix) {
orientation.setAllValues(openglMatrix[0], openglMatrix[4], openglMatrix[8], Matrix3x3 matrix(openglMatrix[0], openglMatrix[4], openglMatrix[8],
openglMatrix[1], openglMatrix[5], openglMatrix[9], openglMatrix[1], openglMatrix[5], openglMatrix[9],
openglMatrix[2], openglMatrix[6], openglMatrix[10]); openglMatrix[2], openglMatrix[6], openglMatrix[10]);
orientation = Quaternion(matrix);
position.setAllValues(openglMatrix[12], openglMatrix[13], openglMatrix[14]); position.setAllValues(openglMatrix[12], openglMatrix[13], openglMatrix[14]);
} }
// Get the OpenGL matrix of the transform // Get the OpenGL matrix of the transform
inline void Transform::getOpenGLMatrix(double* openglMatrix) const { 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; const Matrix3x3& matrix = orientation.getMatrix();
openglMatrix[4] = orientation.getValue(0, 1); openglMatrix[5] = orientation.getValue(1, 1); openglMatrix[6] = orientation.getValue(2, 1); openglMatrix[7] = 0.0; openglMatrix[0] = matrix.getValue(0, 0); openglMatrix[1] = matrix.getValue(1, 0); openglMatrix[2] = matrix.getValue(2, 0); openglMatrix[3] = 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[4] = matrix.getValue(0, 1); openglMatrix[5] = matrix.getValue(1, 1); openglMatrix[6] = matrix.getValue(2, 1); openglMatrix[7] = 0.0;
openglMatrix[8] = matrix.getValue(0, 2); openglMatrix[9] = matrix.getValue(1, 2); openglMatrix[10] = matrix.getValue(2, 2); openglMatrix[11] = 0.0;
openglMatrix[12] = position.getX(); openglMatrix[13] = position.getY(); openglMatrix[14] = position.getZ(); openglMatrix[15] = 1.0; openglMatrix[12] = position.getX(); openglMatrix[13] = position.getY(); openglMatrix[14] = position.getZ(); openglMatrix[15] = 1.0;
} }
// Return the inverse of the transform // Return the inverse of the transform
inline Transform Transform::inverse() const { inline Transform Transform::inverse() const {
Matrix3x3 invMatrix = orientation.getTranspose(); const Quaternion& invQuaternion = orientation.getInverse();
return Transform(invMatrix * position.getOpposite(), invMatrix); Matrix3x3 invMatrix = invQuaternion.getMatrix();
return Transform(invMatrix * position.getOpposite(), invQuaternion);
} }
// Return an interpolated transform // Return an interpolated transform
inline Transform Transform::interpolateTransforms(const Transform& oldTransform, const Transform& newTransform, inline Transform Transform::interpolateTransforms(const Transform& oldTransform, const Transform& newTransform, double interpolationFactor) {
double interpolationFactor) { Vector3D interPosition = oldTransform.position * (1.0 - interpolationFactor) + newTransform.position * interpolationFactor;
Vector3D interPosition = oldTransform.position * (1.0-interpolationFactor) + newTransform.position * interpolationFactor;
Quaternion interOrientation = Quaternion::slerp(oldTransform.orientation, newTransform.orientation, interpolationFactor); Quaternion interOrientation = Quaternion::slerp(oldTransform.orientation, newTransform.orientation, interpolationFactor);
return Transform(interPosition, interOrientation); return Transform(interPosition, interOrientation);
} }
// Return the transformed vector // Return the transformed vector
inline Vector3D Transform::operator*(const Vector3D& vector) const { inline Vector3D Transform::operator*(const Vector3D& vector) const {
return (orientation * vector) + position; return (orientation.getMatrix() * vector) + position;
} }
// Operator of multiplication of a transform with another one // Operator of multiplication of a transform with another one
inline Transform Transform::operator*(const Transform& transform2) const { inline Transform Transform::operator*(const Transform& transform2) const {
return Transform(position + orientation * transform2.position, orientation * transform2.orientation); return Transform(position + orientation.getMatrix() * transform2.position, orientation * transform2.orientation);
} }
} // End of the ReactPhysics3D namespace } // End of the ReactPhysics3D namespace

2
src/shapes/AABB.h
View File

@ -101,7 +101,7 @@ inline bool AABB::testCollision(const AABB& aabb) const {
// Update the world minimum and maximum coordinates of the AABB on the three x,y and z axis // Update the world minimum and maximum coordinates of the AABB on the three x,y and z axis
inline void AABB::update(const Transform& newTransform, const Vector3D& extents) { inline void AABB::update(const Transform& newTransform, const Vector3D& extents) {
Matrix3x3 worldAxis = newTransform.getOrientation().getAbsoluteMatrix(); Matrix3x3 worldAxis = newTransform.getOrientation().getMatrix().getAbsoluteMatrix();
Vector3D worldExtents = Vector3D(worldAxis.getColumn(0).dot(extents), Vector3D worldExtents = Vector3D(worldAxis.getColumn(0).dot(extents),
worldAxis.getColumn(1).dot(extents), worldAxis.getColumn(1).dot(extents),
worldAxis.getColumn(2).dot(extents)); worldAxis.getColumn(2).dot(extents));