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

move conversions Quaternion <-> Matrix3x3 from Matrix3x3.h into Quaternion.h

Browse Source 
git-svn-id: https://reactphysics3d.googlecode.com/svn/trunk@339 92aac97c-a6ce-11dd-a772-7fcde58d38e6
This commit is contained in:
chappuis.daniel 2010-06-19 20:15:44 +00:00
parent 0f38e643a3
commit 716081d982
4 changed files with 105 additions and 77 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

74
sources/reactphysics3d/mathematics/Matrix3x3.cpp
View File

@ -46,40 +46,6 @@ Matrix3x3::Matrix3x3(const Matrix3x3& matrix2) {
matrix2.array[2][0], matrix2.array[2][1], matrix2.array[2][2]);
}
// Create a Matrix3x3 from a quaternion (the quaternion can be non-unit)
Matrix3x3::Matrix3x3(const Quaternion& quaternion) {
double x = quaternion.getX();
double y = quaternion.getY();
double z = quaternion.getZ();
double w = quaternion.getW();
double nQ = x*x + y*y + z*z + w*w;
double s = 0.0;
if (nQ > 0.0) {
s = 2.0/nQ;
}
// Computations used for optimization (less multiplications)
double xs = x*s;
double ys = y*s;
double zs = z*s;
double wxs = w*xs;
double wys = w*ys;
double wzs = w*zs;
double xxs = x*xs;
double xys = x*ys;
double xzs = x*zs;
double yys = y*ys;
double yzs = y*zs;
double zzs = z*zs;
// Create the matrix corresponding to the quaternion
setAllValues(1.0-yys-zzs, xys-wzs, xzs + wys,
xys + wzs, 1.0-xxs-zzs, yzs-wxs,
xzs-wys, yzs + wxs, 1.0-xxs-yys);
}
// Destructor
Matrix3x3::~Matrix3x3() {
@ -109,46 +75,6 @@ Matrix3x3 Matrix3x3::getInverse() const throw(MathematicsException) {
}
}
// Return the quaternion corresponding to the matrix (it returns a unit quaternion)
Quaternion Matrix3x3::getQuaternion() const {
// Get the trace of the matrix
double trace = getTrace();
double r;
double s;
if (trace < 0.0) {
if (array[1][1] > array[0][0]) {
if(array[2][2] > array[1][1]) {
r = sqrt(array[2][2] - array[0][0] - array[1][1] + 1.0);
s = 0.5 / r;
return Quaternion((array[2][0] + array[0][2])*s, (array[1][2] + array[2][1])*s, 0.5*r, (array[1][0] - array[0][1])*s);
}
else {
r = sqrt(array[1][1] - array[2][2] - array[0][0] + 1.0);
s = 0.5 / r;
return Quaternion((array[0][1] + array[1][0])*s, 0.5 * r, (array[1][2] + array[2][1])*s, (array[0][2] - array[2][0])*s);
}
}
else if (array[2][2] > array[0][0]) {
r = sqrt(array[2][2] - array[0][0] - array[1][1] + 1.0);
s = 0.5 / r;
return Quaternion((array[2][0] + array[0][2])*s, (array[1][2] + array[2][1])*s, 0.5 * r, (array[1][0] - array[0][1])*s);
}
else {
r = sqrt(array[0][0] - array[1][1] - array[2][2] + 1.0);
s = 0.5 / r;
return Quaternion(0.5 * r, (array[0][1] + array[1][0])*s, (array[2][0] - array[0][2])*s, (array[2][1] - array[1][2])*s);
}
}
else {
r = sqrt(trace + 1.0);
s = 0.5/r;
return Quaternion((array[2][1]-array[1][2])*s, (array[0][2]-array[2][0])*s, (array[1][0]-array[0][1])*s, 0.5 * r);
}
}
// Return the 3x3 identity matrix
Matrix3x3 Matrix3x3::identity() {
// Return the isdentity matrix

3
sources/reactphysics3d/mathematics/Matrix3x3.h
View File

@ -23,7 +23,6 @@
// Libraries
#include "exceptions.h"
#include "Quaternion.h"
#include "Vector3D.h"
// ReactPhysics3D namespace
@ -44,7 +43,6 @@ class Matrix3x3 {
Matrix3x3(double a1, double a2, double a3, double b1, double b2, double b3,
double c1, double c2, double c3); // Constructor with arguments
Matrix3x3(const Matrix3x3& matrix); // Copy-constructor
Matrix3x3(const Quaternion& quaternion); // Create a Matrix3x3 from a Quaternion
virtual ~Matrix3x3(); // Destructor
double getValue(int i, int j) const throw(std::invalid_argument); // Get a value in the matrix
@ -55,7 +53,6 @@ class Matrix3x3 {
double getDeterminant() const; // Return the determinant of the matrix
double getTrace() const; // Return the trace of the matrix
Matrix3x3 getInverse() const throw(MathematicsException); // Return the inverse matrix
Quaternion getQuaternion() const; // Return the unit quaternion corresponding to the matrix
static Matrix3x3 identity(); // Return the 3x3 identity matrix
// --- Overloaded operators --- //

102
sources/reactphysics3d/mathematics/Quaternion.cpp
View File

@ -49,6 +49,78 @@ Quaternion::Quaternion(const Quaternion& quaternion)
}
// Create a unit quaternion from a rotation matrix
Quaternion::Quaternion(const Matrix3x3& matrix) {
// Get the trace of the matrix
double trace = matrix.getTrace();
double array[3][3];
for (int i=0; i<3; i++) {
for (int j=0; j<3; j++) {
array[i][j] = matrix.getValue(i, j);
}
}
double r;
double s;
if (trace < 0.0) {
if (array[1][1] > array[0][0]) {
if(array[2][2] > array[1][1]) {
r = sqrt(array[2][2] - array[0][0] - array[1][1] + 1.0);
s = 0.5 / r;
// Compute the quaternion
x = (array[2][0] + array[0][2])*s;
y = (array[1][2] + array[2][1])*s;
z = 0.5*r;
w = (array[1][0] - array[0][1])*s;
}
else {
r = sqrt(array[1][1] - array[2][2] - array[0][0] + 1.0);
s = 0.5 / r;
// Compute the quaternion
x = (array[0][1] + array[1][0])*s;
y = 0.5 * r;
z = (array[1][2] + array[2][1])*s;
w = (array[0][2] - array[2][0])*s;
}
}
else if (array[2][2] > array[0][0]) {
r = sqrt(array[2][2] - array[0][0] - array[1][1] + 1.0);
s = 0.5 / r;
// Compute the quaternion
x = (array[2][0] + array[0][2])*s;
y = (array[1][2] + array[2][1])*s;
z = 0.5 * r;
w = (array[1][0] - array[0][1])*s;
}
else {
r = sqrt(array[0][0] - array[1][1] - array[2][2] + 1.0);
s = 0.5 / r;
// Compute the quaternion
x = 0.5 * r;
y = (array[0][1] + array[1][0])*s;
z = (array[2][0] - array[0][2])*s;
w = (array[2][1] - array[1][2])*s;
}
}
else {
r = sqrt(trace + 1.0);
s = 0.5/r;
// Compute the quaternion
x = (array[2][1]-array[1][2])*s;
y = (array[0][2]-array[2][0])*s;
z = (array[1][0]-array[0][1])*s;
w = 0.5 * r;
}
}
// Destructor
Quaternion::~Quaternion() {
@ -82,6 +154,36 @@ void Quaternion::getRotationAngleAxis(double& angle, Vector3D& axis) const {
axis.setAllValues(rotationAxis.getX(), rotationAxis.getY(), rotationAxis.getZ());
}
// Return the orientation matrix corresponding to this quaternion
Matrix3x3 Quaternion::getMatrix() const {
double nQ = x*x + y*y + z*z + w*w;
double s = 0.0;
if (nQ > 0.0) {
s = 2.0/nQ;
}
// Computations used for optimization (less multiplications)
double xs = x*s;
double ys = y*s;
double zs = z*s;
double wxs = w*xs;
double wys = w*ys;
double wzs = w*zs;
double xxs = x*xs;
double xys = x*ys;
double xzs = x*zs;
double yys = y*ys;
double yzs = y*zs;
double zzs = z*zs;
// Create the matrix corresponding to the quaternion
return Matrix3x3(1.0-yys-zzs, xys-wzs, xzs + wys,
xys + wzs, 1.0-xxs-zzs, yzs-wxs,
xzs-wys, yzs + wxs, 1.0-xxs-yys);
}
// Compute the spherical linear interpolation between two quaternions.
// The t argument has to be such that 0 <= t <= 1. This method is static.
Quaternion Quaternion::slerp(const Quaternion& quaternion1, const Quaternion& quaternion2, double t) {

3
sources/reactphysics3d/mathematics/Quaternion.h
View File

@ -23,6 +23,7 @@
// Libraries
#include <cmath>
#include "Vector3D.h"
#include "Matrix3x3.h"
#include "exceptions.h"
// ReactPhysics3D namespace
@ -46,6 +47,7 @@ class Quaternion {
Quaternion(double x, double y, double z, double w); // Constructor with arguments
Quaternion(double w, const Vector3D& v); // Constructor with the component w and the vector v=(x y z)
Quaternion(const Quaternion& quaternion); // Copy-constructor
Quaternion(const Matrix3x3& matrix); // Create a unit quaternion from a rotation matrix
~Quaternion(); // Destructor
double getX() const; // Return the component x of the quaternion
double getY() const; // Return the component y of the quaternion
@ -60,6 +62,7 @@ class Quaternion {
Quaternion getUnit() const throw (MathematicsException); // 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
double scalarProduct(const Quaternion& quaternion) const; // Scalar 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,