Use a quaternion instead of a matrix in the Transform
git-svn-id: https://reactphysics3d.googlecode.com/svn/trunk@435 92aac97c-a6ce-11dd-a772-7fcde58d38e6
This commit is contained in:
chappuis.daniel
parent
00b9fc45dc
commit
d9452e727c
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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();
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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)
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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));
|
||||
|
|
|
|||
Loading…
Reference in New Issue
Block a user