diff --git a/src/body/RigidBody.h b/src/body/RigidBody.h index 02b264bb..ec87faf5 100644 --- a/src/body/RigidBody.h +++ b/src/body/RigidBody.h @@ -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 inline Matrix3x3 RigidBody::getInertiaTensorWorld() const { // 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 @@ -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 inline Matrix3x3 RigidBody::getInertiaTensorInverseWorld() const { // 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 diff --git a/src/collision/EPA/EPAAlgorithm.cpp b/src/collision/EPA/EPAAlgorithm.cpp index db244a08..fd4ebac3 100644 --- a/src/collision/EPA/EPAAlgorithm.cpp +++ b/src/collision/EPA/EPAAlgorithm.cpp @@ -92,7 +92,7 @@ bool EPAAlgorithm::computePenetrationDepthAndContactPoints(Simplex simplex, cons 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(); + Matrix3x3 rotateToShape2 = transform2.getOrientation().getMatrix().getTranspose() * transform1.getOrientation().getMatrix(); // Get the simplex computed previously by the GJK algorithm 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); // 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 pB = transform1 * triangle->computeClosestPointOfObject(suppPointsB); Vector3D normal = v.getUnit(); diff --git a/src/collision/GJK/GJKAlgorithm.cpp b/src/collision/GJK/GJKAlgorithm.cpp index f9e2118f..ec455042 100644 --- a/src/collision/GJK/GJKAlgorithm.cpp +++ b/src/collision/GJK/GJKAlgorithm.cpp @@ -76,7 +76,7 @@ bool GJKAlgorithm::testCollision(const Shape* shape1, const Transform& transform 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(); + Matrix3x3 rotateToShape2 = transform2.getOrientation().getMatrix().getTranspose() * transform1.getOrientation().getMatrix(); // Initialize the margin (sum of margins of both objects) 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); // Compute the contact info - Vector3D normal = transform1.getOrientation() * v.getOpposite().getUnit(); + Vector3D normal = transform1.getOrientation().getMatrix() * v.getOpposite().getUnit(); double penetrationDepth = margin - dist; 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); // Compute the contact info - Vector3D normal = transform1.getOrientation() * v.getOpposite().getUnit(); + Vector3D normal = transform1.getOrientation().getMatrix() * v.getOpposite().getUnit(); double penetrationDepth = margin - dist; 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); // Compute the contact info - Vector3D normal = transform1.getOrientation() * v.getOpposite().getUnit(); + Vector3D normal = transform1.getOrientation().getMatrix() * v.getOpposite().getUnit(); double penetrationDepth = margin - dist; 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); // Compute the contact info - Vector3D normal = transform1.getOrientation() * v.getOpposite().getUnit(); + Vector3D normal = transform1.getOrientation().getMatrix() * v.getOpposite().getUnit(); double penetrationDepth = margin - dist; contactInfo = new ContactInfo(body1, body2, normal, penetrationDepth, pA, pB); @@ -235,7 +235,7 @@ bool GJKAlgorithm::computePenetrationDepthForEnlargedObjects(const Shape* const 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(); + Matrix3x3 rotateToShape2 = transform2.getOrientation().getMatrix().getTranspose() * transform1.getOrientation().getMatrix(); do { // Compute the support points for the enlarged object A and B diff --git a/src/collision/SATAlgorithm.cpp b/src/collision/SATAlgorithm.cpp index 27bb4dd9..136cd230 100644 --- a/src/collision/SATAlgorithm.cpp +++ b/src/collision/SATAlgorithm.cpp @@ -113,13 +113,13 @@ bool SATAlgorithm::computeCollisionTest(const BoxShape* obb1, const Transform& t // Axis A0 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]); if (absC[0][i] > cutoff) { existsParallelPair = true; } } - udc1[0] = transform1.getOrientation().getColumn(0).dot(boxDistance); + udc1[0] = transform1.getOrientation().getMatrix().getColumn(0).dot(boxDistance); center = udc1[0]; radius1 = obb1->getExtent().getX(); 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 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 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]); if (absC[1][i] > cutoff) { existsParallelPair = true; } } - udc1[1] = transform1.getOrientation().getColumn(1).dot(boxDistance); + udc1[1] = transform1.getOrientation().getMatrix().getColumn(1).dot(boxDistance); center = udc1[1]; radius1 = obb1->getExtent().getY(); 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 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 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]); if (absC[2][i] > cutoff) { existsParallelPair = true; } } - udc1[2] = transform1.getOrientation().getColumn(2).dot(boxDistance); + udc1[2] = transform1.getOrientation().getMatrix().getColumn(2).dot(boxDistance); center = udc1[2]; radius1 = obb1->getExtent().getZ(); 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 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 - udc2[0] = transform2.getOrientation().getColumn(0).dot(boxDistance); + udc2[0] = transform2.getOrientation().getMatrix().getColumn(0).dot(boxDistance); center = udc2[0]; radius1 = obb1->getExtent().getX()*absC[0][0] + obb1->getExtent().getY()*absC[1][0] + obb1->getExtent().getZ() * absC[2][0]; 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 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 - udc2[1] = transform2.getOrientation().getColumn(1).dot(boxDistance); + udc2[1] = transform2.getOrientation().getMatrix().getColumn(1).dot(boxDistance); center = udc2[1]; radius1 = obb1->getExtent().getX()*absC[0][1] + obb1->getExtent().getY()*absC[1][1] + obb1->getExtent().getZ() * absC[2][1]; 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 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 - udc2[2] = transform2.getOrientation().getColumn(2).dot(boxDistance); + udc2[2] = transform2.getOrientation().getMatrix().getColumn(2).dot(boxDistance); center = udc2[2]; radius1 = obb1->getExtent().getX()*absC[0][2] + obb1->getExtent().getY()*absC[1][2] + obb1->getExtent().getZ()*absC[2][2]; 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 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 @@ -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 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 @@ -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 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 @@ -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 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 @@ -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 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 @@ -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 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 @@ -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 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 @@ -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 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 @@ -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 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 @@ -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 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 diff --git a/src/engine/PhysicsEngine.cpp b/src/engine/PhysicsEngine.cpp index 60fac5b3..79952a8e 100644 --- a/src/engine/PhysicsEngine.cpp +++ b/src/engine/PhysicsEngine.cpp @@ -166,8 +166,8 @@ void PhysicsEngine::updatePositionAndOrientationOfBody(Body* body, const Vector3 rigidBody->setAngularVelocity(newAngVelocity); // Get current position and orientation of the body - Vector3D currentPosition = rigidBody->getTransform().getPosition(); - Quaternion currentOrientation = Quaternion(rigidBody->getTransform().getOrientation()); + const Vector3D& currentPosition = rigidBody->getTransform().getPosition(); + const Quaternion& currentOrientation = rigidBody->getTransform().getOrientation(); Vector3D newPosition = currentPosition + newLinVelocity * dt; Quaternion newOrientation = currentOrientation + Quaternion(newAngVelocity.getX(), newAngVelocity.getY(), newAngVelocity.getZ(), 0) * currentOrientation * 0.5 * dt; diff --git a/src/mathematics/Quaternion.h b/src/mathematics/Quaternion.h index c112f52e..6e92be05 100644 --- a/src/mathematics/Quaternion.h +++ b/src/mathematics/Quaternion.h @@ -64,10 +64,11 @@ class Quaternion { void setW(double w); // Set the value w Vector3D vectorV() const; // Return the vector v=(x y z) 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 getInverse() const throw (MathematicsException); // Return the inverse of the 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 void getRotationAngleAxis(double& angle, Vector3D& axis) const; // Compute the rotation angle (in radians) and the axis static Quaternion slerp(const Quaternion& quaternion1, @@ -136,19 +137,19 @@ inline double Quaternion::length() const { } // Return the unit quaternion -inline Quaternion Quaternion::getUnit() const throw(MathematicsException) { +inline Quaternion Quaternion::getUnit() const { double lengthQuaternion = length(); // Check if the length is not equal to zero - if (lengthQuaternion != 0.0) { + assert (lengthQuaternion != 0.0); - // Compute and return the unit quaternion - 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 - throw MathematicsException("MathematicsException : Impossible to compute the unit quaternion if the length of the quaternion is zero"); - } + // Compute and return the unit quaternion + return Quaternion(x/lengthQuaternion, y/lengthQuaternion, z/lengthQuaternion, w/lengthQuaternion); +} + +// Return the identity quaternion +inline Quaternion Quaternion::identity() { + return Quaternion(0.0, 0.0, 0.0, 1.0); } // Return the conjugate of the quaternion (inline) diff --git a/src/mathematics/Transform.cpp b/src/mathematics/Transform.cpp index 110e76d9..3805461c 100644 --- a/src/mathematics/Transform.cpp +++ b/src/mathematics/Transform.cpp @@ -31,19 +31,19 @@ using namespace reactphysics3d; // Constructor Transform::Transform() { position = Vector3D(0.0, 0.0, 0.0); - orientation.setToIdentity(); + orientation = Quaternion::identity(); } // Constructor Transform::Transform(const Vector3D& position, const Matrix3x3& orientation) { this->position = position; - this->orientation = orientation; + this->orientation = Quaternion(orientation); } // Constructor Transform::Transform(const Vector3D& position, const Quaternion& orientation) { this->position = position; - this->orientation = orientation.getMatrix(); + this->orientation = orientation; } // Destructor diff --git a/src/mathematics/Transform.h b/src/mathematics/Transform.h index 70e3607b..cc92f190 100644 --- a/src/mathematics/Transform.h +++ b/src/mathematics/Transform.h @@ -41,8 +41,8 @@ namespace reactphysics3d { */ class Transform { private : - Vector3D position; // position - Matrix3x3 orientation; // orientation matrix + Vector3D position; // Position + Quaternion orientation; // Orientation public : Transform(); // Constructor @@ -52,8 +52,8 @@ class Transform { 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 + const Quaternion& getOrientation() const; // Return the orientation quaternion + void setOrientation(const Quaternion& orientation); // Set the rotation quaternion 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 @@ -62,7 +62,7 @@ class Transform { const Transform& newTransform, 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 }; @@ -77,59 +77,61 @@ inline void Transform::setPosition(const Vector3D& position) { } // Return the rotation matrix -inline const Matrix3x3& Transform::getOrientation() const { +inline const Quaternion& Transform::getOrientation() const { return orientation; } // Set the rotation matrix of the transform -inline void Transform::setOrientation(const Matrix3x3& orientation) { +inline void Transform::setOrientation(const Quaternion& 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(); + orientation = Quaternion::identity(); } // 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]); + Matrix3x3 matrix(openglMatrix[0], openglMatrix[4], openglMatrix[8], + openglMatrix[1], openglMatrix[5], openglMatrix[9], + openglMatrix[2], openglMatrix[6], openglMatrix[10]); + orientation = Quaternion(matrix); 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; + const Matrix3x3& matrix = orientation.getMatrix(); + openglMatrix[0] = matrix.getValue(0, 0); openglMatrix[1] = matrix.getValue(1, 0); openglMatrix[2] = matrix.getValue(2, 0); openglMatrix[3] = 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; } // Return the inverse of the transform inline Transform Transform::inverse() const { - Matrix3x3 invMatrix = orientation.getTranspose(); - return Transform(invMatrix * position.getOpposite(), invMatrix); + const Quaternion& invQuaternion = orientation.getInverse(); + Matrix3x3 invMatrix = invQuaternion.getMatrix(); + return Transform(invMatrix * position.getOpposite(), invQuaternion); } // 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; +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; + return (orientation.getMatrix() * 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); + return Transform(position + orientation.getMatrix() * transform2.position, orientation * transform2.orientation); } } // End of the ReactPhysics3D namespace diff --git a/src/shapes/AABB.h b/src/shapes/AABB.h index 30342fbc..df4f9b89 100644 --- a/src/shapes/AABB.h +++ b/src/shapes/AABB.h @@ -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 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), worldAxis.getColumn(1).dot(extents), worldAxis.getColumn(2).dot(extents));