Merge branch 'mathematics_optimization' into develop
This commit is contained in:
Daniel Chappuis
commit
c63284a432
|
|
@ -62,7 +62,7 @@ Vector3 CylinderShape::getLocalSupportPoint(const Vector3& direction, decimal ma
|
|||
Vector3 w(direction.x, 0.0, direction.z);
|
||||
decimal lengthW = sqrt(direction.x * direction.x + direction.z * direction.z);
|
||||
|
||||
if (lengthW != 0.0) {
|
||||
if (lengthW > MACHINE_EPSILON) {
|
||||
if (uDotv < 0.0) supportPoint.y = -mHalfHeight;
|
||||
else supportPoint.y = mHalfHeight;
|
||||
supportPoint += (mRadius / lengthW) * w;
|
||||
|
|
|
|||
|
|
@ -73,21 +73,27 @@ Matrix3x3& Matrix3x3::operator=(const Matrix3x3& matrix) {
|
|||
|
||||
// Return the inverse matrix
|
||||
Matrix3x3 Matrix3x3::getInverse() const {
|
||||
|
||||
// Compute the determinant of the matrix
|
||||
decimal determinant = getDeterminant();
|
||||
|
||||
// Check if the determinant is equal to zero
|
||||
assert(determinant != 0.0);
|
||||
decimal invDeterminant = 1.0 / determinant;
|
||||
Matrix3x3 tempMatrix;
|
||||
assert(std::abs(determinant) > MACHINE_EPSILON);
|
||||
|
||||
// Compute the inverse of the matrix
|
||||
tempMatrix.setAllValues((mArray[1][1]*mArray[2][2]-mArray[2][1]*mArray[1][2]), -(mArray[1][0]*mArray[2][2]-mArray[2][0]*mArray[1][2]), (mArray[1][0]*mArray[2][1]-mArray[2][0]*mArray[1][1]),
|
||||
-(mArray[0][1]*mArray[2][2]-mArray[2][1]*mArray[0][2]), (mArray[0][0]*mArray[2][2]-mArray[2][0]*mArray[0][2]), -(mArray[0][0]*mArray[2][1]-mArray[2][0]*mArray[0][1]),
|
||||
(mArray[0][1]*mArray[1][2]-mArray[0][2]*mArray[1][1]), -(mArray[0][0]*mArray[1][2]-mArray[1][0]*mArray[0][2]), (mArray[0][0]*mArray[1][1]-mArray[0][1]*mArray[1][0]));
|
||||
decimal invDeterminant = decimal(1.0) / determinant;
|
||||
|
||||
Matrix3x3 tempMatrix((mArray[1][1]*mArray[2][2]-mArray[2][1]*mArray[1][2]),
|
||||
-(mArray[0][1]*mArray[2][2]-mArray[2][1]*mArray[0][2]),
|
||||
(mArray[0][1]*mArray[1][2]-mArray[0][2]*mArray[1][1]),
|
||||
-(mArray[1][0]*mArray[2][2]-mArray[2][0]*mArray[1][2]),
|
||||
(mArray[0][0]*mArray[2][2]-mArray[2][0]*mArray[0][2]),
|
||||
-(mArray[0][0]*mArray[1][2]-mArray[1][0]*mArray[0][2]),
|
||||
(mArray[1][0]*mArray[2][1]-mArray[2][0]*mArray[1][1]),
|
||||
-(mArray[0][0]*mArray[2][1]-mArray[2][0]*mArray[0][1]),
|
||||
(mArray[0][0]*mArray[1][1]-mArray[0][1]*mArray[1][0]));
|
||||
|
||||
// Return the inverse matrix
|
||||
return (invDeterminant * tempMatrix.getTranspose());
|
||||
return (invDeterminant * tempMatrix);
|
||||
}
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -32,26 +32,24 @@
|
|||
using namespace reactphysics3d;
|
||||
|
||||
// Constructor of the class
|
||||
Quaternion::Quaternion()
|
||||
:mX(0.0), mY(0.0), mZ(0.0), mW(0.0) {
|
||||
Quaternion::Quaternion() : x(0.0), y(0.0), z(0.0), w(0.0) {
|
||||
|
||||
}
|
||||
|
||||
// Constructor with arguments
|
||||
Quaternion::Quaternion(decimal x, decimal y, decimal z, decimal w)
|
||||
:mX(x), mY(y), mZ(z), mW(w) {
|
||||
Quaternion::Quaternion(decimal newX, decimal newY, decimal newZ, decimal newW)
|
||||
:x(newX), y(newY), z(newZ), w(newW) {
|
||||
|
||||
}
|
||||
|
||||
// Constructor with the component w and the vector v=(x y z)
|
||||
Quaternion::Quaternion(decimal w, const Vector3& v)
|
||||
:mX(v.x), mY(v.y), mZ(v.z), mW(w) {
|
||||
Quaternion::Quaternion(decimal newW, const Vector3& v) : x(v.x), y(v.y), z(v.z), w(newW) {
|
||||
|
||||
}
|
||||
|
||||
// Copy-constructor
|
||||
Quaternion::Quaternion(const Quaternion& quaternion)
|
||||
:mX(quaternion.mX), mY(quaternion.mY), mZ(quaternion.mZ), mW(quaternion.mW) {
|
||||
:x(quaternion.x), y(quaternion.y), z(quaternion.z), w(quaternion.w) {
|
||||
|
||||
}
|
||||
|
||||
|
|
@ -78,20 +76,20 @@ Quaternion::Quaternion(const Matrix3x3& matrix) {
|
|||
s = 0.5 / r;
|
||||
|
||||
// Compute the quaternion
|
||||
mX = (array[2][0] + array[0][2])*s;
|
||||
mY = (array[1][2] + array[2][1])*s;
|
||||
mZ = 0.5*r;
|
||||
mW = (array[1][0] - array[0][1])*s;
|
||||
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
|
||||
mX = (array[0][1] + array[1][0])*s;
|
||||
mY = 0.5 * r;
|
||||
mZ = (array[1][2] + array[2][1])*s;
|
||||
mW = (array[0][2] - array[2][0])*s;
|
||||
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]) {
|
||||
|
|
@ -99,20 +97,20 @@ Quaternion::Quaternion(const Matrix3x3& matrix) {
|
|||
s = 0.5 / r;
|
||||
|
||||
// Compute the quaternion
|
||||
mX = (array[2][0] + array[0][2])*s;
|
||||
mY = (array[1][2] + array[2][1])*s;
|
||||
mZ = 0.5 * r;
|
||||
mW = (array[1][0] - array[0][1])*s;
|
||||
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
|
||||
mX = 0.5 * r;
|
||||
mY = (array[0][1] + array[1][0])*s;
|
||||
mZ = (array[2][0] - array[0][2])*s;
|
||||
mW = (array[2][1] - array[1][2])*s;
|
||||
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 {
|
||||
|
|
@ -120,10 +118,10 @@ Quaternion::Quaternion(const Matrix3x3& matrix) {
|
|||
s = 0.5/r;
|
||||
|
||||
// Compute the quaternion
|
||||
mX = (array[2][1]-array[1][2])*s;
|
||||
mY = (array[0][2]-array[2][0])*s;
|
||||
mZ = (array[1][0]-array[0][1])*s;
|
||||
mW = 0.5 * r;
|
||||
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;
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -148,10 +146,10 @@ void Quaternion::getRotationAngleAxis(decimal& angle, Vector3& axis) const {
|
|||
}
|
||||
|
||||
// Compute the roation angle
|
||||
angle = acos(quaternion.mW) * 2.0;
|
||||
angle = acos(quaternion.w) * 2.0;
|
||||
|
||||
// Compute the 3D rotation axis
|
||||
Vector3 rotationAxis(quaternion.mX, quaternion.mY, quaternion.mZ);
|
||||
Vector3 rotationAxis(quaternion.x, quaternion.y, quaternion.z);
|
||||
|
||||
// Normalize the rotation axis
|
||||
rotationAxis = rotationAxis.getUnit();
|
||||
|
|
@ -163,7 +161,7 @@ void Quaternion::getRotationAngleAxis(decimal& angle, Vector3& axis) const {
|
|||
// Return the orientation matrix corresponding to this quaternion
|
||||
Matrix3x3 Quaternion::getMatrix() const {
|
||||
|
||||
decimal nQ = mX*mX + mY*mY + mZ*mZ + mW*mW;
|
||||
decimal nQ = x*x + y*y + z*z + w*w;
|
||||
decimal s = 0.0;
|
||||
|
||||
if (nQ > 0.0) {
|
||||
|
|
@ -171,18 +169,18 @@ Matrix3x3 Quaternion::getMatrix() const {
|
|||
}
|
||||
|
||||
// Computations used for optimization (less multiplications)
|
||||
decimal xs = mX*s;
|
||||
decimal ys = mY*s;
|
||||
decimal zs = mZ*s;
|
||||
decimal wxs = mW*xs;
|
||||
decimal wys = mW*ys;
|
||||
decimal wzs = mW*zs;
|
||||
decimal xxs = mX*xs;
|
||||
decimal xys = mX*ys;
|
||||
decimal xzs = mX*zs;
|
||||
decimal yys = mY*ys;
|
||||
decimal yzs = mY*zs;
|
||||
decimal zzs = mZ*zs;
|
||||
decimal xs = x*s;
|
||||
decimal ys = y*s;
|
||||
decimal zs = z*s;
|
||||
decimal wxs = w*xs;
|
||||
decimal wys = w*ys;
|
||||
decimal wzs = w*zs;
|
||||
decimal xxs = x*xs;
|
||||
decimal xys = x*ys;
|
||||
decimal xzs = x*zs;
|
||||
decimal yys = y*ys;
|
||||
decimal yzs = y*zs;
|
||||
decimal zzs = z*zs;
|
||||
|
||||
// Create the matrix corresponding to the quaternion
|
||||
return Matrix3x3(1.0-yys-zzs, xys-wzs, xzs + wys,
|
||||
|
|
|
|||
|
|
@ -41,16 +41,14 @@ namespace reactphysics3d {
|
|||
q = (x*i, y*j, z*k, w) to represent a quaternion.
|
||||
-------------------------------------------------------------------
|
||||
*/
|
||||
class Quaternion {
|
||||
struct Quaternion {
|
||||
|
||||
private :
|
||||
public :
|
||||
|
||||
// -------------------- Attributes -------------------- //
|
||||
|
||||
// Components of the quaternion
|
||||
decimal mX, mY, mZ, mW;
|
||||
|
||||
public :
|
||||
decimal x, y, z, w;
|
||||
|
||||
// -------------------- Methods -------------------- //
|
||||
|
||||
|
|
@ -58,10 +56,10 @@ class Quaternion {
|
|||
Quaternion();
|
||||
|
||||
// Constructor with arguments
|
||||
Quaternion(decimal x, decimal y, decimal z, decimal w);
|
||||
Quaternion(decimal newX, decimal newY, decimal newZ, decimal newW);
|
||||
|
||||
// Constructor with the component w and the vector v=(x y z)
|
||||
Quaternion(decimal w, const Vector3& v);
|
||||
Quaternion(decimal newW, const Vector3& v);
|
||||
|
||||
// Copy-constructor
|
||||
Quaternion(const Quaternion& quaternion);
|
||||
|
|
@ -72,30 +70,6 @@ class Quaternion {
|
|||
// Destructor
|
||||
~Quaternion();
|
||||
|
||||
// Return the component x of the quaternion
|
||||
decimal getX() const;
|
||||
|
||||
// Return the component y of the quaternion
|
||||
decimal getY() const;
|
||||
|
||||
// Return the component z of the quaternion
|
||||
decimal getZ() const;
|
||||
|
||||
// Return the component w of the quaternion
|
||||
decimal getW() const;
|
||||
|
||||
// Set the value x
|
||||
void setX(decimal x);
|
||||
|
||||
// Set the value y
|
||||
void setY(decimal y);
|
||||
|
||||
// Set the value z
|
||||
void setZ(decimal z);
|
||||
|
||||
// Set the value w
|
||||
void setW(decimal w);
|
||||
|
||||
// Return the vector v=(x y z) of the quaternion
|
||||
Vector3 vectorV() const;
|
||||
|
||||
|
|
@ -146,55 +120,16 @@ class Quaternion {
|
|||
bool operator==(const Quaternion& quaternion) const;
|
||||
};
|
||||
|
||||
// Get the value x (inline)
|
||||
inline decimal Quaternion::getX() const {
|
||||
return mX;
|
||||
}
|
||||
|
||||
// Get the value y (inline)
|
||||
inline decimal Quaternion::getY() const {
|
||||
return mY;
|
||||
}
|
||||
|
||||
// Get the value z (inline)
|
||||
inline decimal Quaternion::getZ() const {
|
||||
return mZ;
|
||||
}
|
||||
|
||||
// Get the value w (inline)
|
||||
inline decimal Quaternion::getW() const {
|
||||
return mW;
|
||||
}
|
||||
|
||||
// Set the value x (inline)
|
||||
inline void Quaternion::setX(decimal x) {
|
||||
mX = x;
|
||||
}
|
||||
|
||||
// Set the value y (inline)
|
||||
inline void Quaternion::setY(decimal y) {
|
||||
mY = y;
|
||||
}
|
||||
|
||||
// Set the value z (inline)
|
||||
inline void Quaternion::setZ(decimal z) {
|
||||
mZ = z;
|
||||
}
|
||||
|
||||
// Set the value w (inline)
|
||||
inline void Quaternion::setW(decimal w) {
|
||||
mW = w;
|
||||
}
|
||||
|
||||
// Return the vector v=(x y z) of the quaternion
|
||||
inline Vector3 Quaternion::vectorV() const {
|
||||
|
||||
// Return the vector v
|
||||
return Vector3(mX, mY, mZ);
|
||||
return Vector3(x, y, z);
|
||||
}
|
||||
|
||||
// Return the length of the quaternion (inline)
|
||||
inline decimal Quaternion::length() const {
|
||||
return sqrt(mX*mX + mY*mY + mZ*mZ + mW*mW);
|
||||
return sqrt(x*x + y*y + z*z + w*w);
|
||||
}
|
||||
|
||||
// Return the unit quaternion
|
||||
|
|
@ -202,11 +137,11 @@ inline Quaternion Quaternion::getUnit() const {
|
|||
decimal lengthQuaternion = length();
|
||||
|
||||
// Check if the length is not equal to zero
|
||||
assert (lengthQuaternion != 0.0);
|
||||
assert (lengthQuaternion > MACHINE_EPSILON);
|
||||
|
||||
// Compute and return the unit quaternion
|
||||
return Quaternion(mX/lengthQuaternion, mY/lengthQuaternion,
|
||||
mZ/lengthQuaternion, mW/lengthQuaternion);
|
||||
return Quaternion(x / lengthQuaternion, y / lengthQuaternion,
|
||||
z / lengthQuaternion, w / lengthQuaternion);
|
||||
}
|
||||
|
||||
// Return the identity quaternion
|
||||
|
|
@ -216,62 +151,62 @@ inline Quaternion Quaternion::identity() {
|
|||
|
||||
// Return the conjugate of the quaternion (inline)
|
||||
inline Quaternion Quaternion::getConjugate() const {
|
||||
return Quaternion(-mX, -mY, -mZ, mW);
|
||||
return Quaternion(-x, -y, -z, w);
|
||||
}
|
||||
|
||||
// Return the inverse of the quaternion (inline)
|
||||
inline Quaternion Quaternion::getInverse() const {
|
||||
|
||||
decimal lengthQuaternion = length();
|
||||
lengthQuaternion = lengthQuaternion * lengthQuaternion;
|
||||
|
||||
assert (lengthQuaternion != 0.0);
|
||||
assert (lengthQuaternion > MACHINE_EPSILON);
|
||||
|
||||
// Compute and return the inverse quaternion
|
||||
return Quaternion(-mX/lengthQuaternion, -mY/lengthQuaternion,
|
||||
-mZ/lengthQuaternion, mW/lengthQuaternion);
|
||||
return Quaternion(-x / lengthQuaternion, -y / lengthQuaternion,
|
||||
-z / lengthQuaternion, w / lengthQuaternion);
|
||||
}
|
||||
|
||||
// Scalar product between two quaternions
|
||||
inline decimal Quaternion::dot(const Quaternion& quaternion) const {
|
||||
return (mX*quaternion.mX + mY*quaternion.mY + mZ*quaternion.mZ + mW*quaternion.mW);
|
||||
return (x*quaternion.x + y*quaternion.y + z*quaternion.z + w*quaternion.w);
|
||||
}
|
||||
|
||||
// Overloaded operator for the addition of two quaternions
|
||||
inline Quaternion Quaternion::operator+(const Quaternion& quaternion) const {
|
||||
|
||||
// Return the result quaternion
|
||||
return Quaternion(mX + quaternion.mX, mY + quaternion.mY,
|
||||
mZ + quaternion.mZ, mW + quaternion.mW);
|
||||
return Quaternion(x + quaternion.x, y + quaternion.y, z + quaternion.z, w + quaternion.w);
|
||||
}
|
||||
|
||||
// Overloaded operator for the substraction of two quaternions
|
||||
inline Quaternion Quaternion::operator-(const Quaternion& quaternion) const {
|
||||
|
||||
// Return the result of the substraction
|
||||
return Quaternion(mX-quaternion.mX, mY - quaternion.mY,
|
||||
mZ - quaternion.mZ, mW - quaternion.mW);
|
||||
return Quaternion(x - quaternion.x, y - quaternion.y, z - quaternion.z, w - quaternion.w);
|
||||
}
|
||||
|
||||
// Overloaded operator for the multiplication with a constant
|
||||
inline Quaternion Quaternion::operator*(decimal nb) const {
|
||||
// Return the result
|
||||
return Quaternion(nb*mX, nb*mY, nb*mZ, nb*mW);
|
||||
return Quaternion(nb * x, nb * y, nb * z, nb * w);
|
||||
}
|
||||
|
||||
// Overloaded operator for the multiplication of two quaternions
|
||||
inline Quaternion Quaternion::operator*(const Quaternion& quaternion) const {
|
||||
// Return the result of the multiplication
|
||||
return Quaternion(mW*quaternion.mW - vectorV().dot(quaternion.vectorV()),
|
||||
mW*quaternion.vectorV()+quaternion.mW*vectorV() +
|
||||
return Quaternion(w * quaternion.w - vectorV().dot(quaternion.vectorV()),
|
||||
w * quaternion.vectorV() + quaternion.w * vectorV() +
|
||||
vectorV().cross(quaternion.vectorV()));
|
||||
}
|
||||
|
||||
// Overloaded operator for the assignment
|
||||
inline Quaternion& Quaternion::operator=(const Quaternion& quaternion) {
|
||||
|
||||
// Check for self-assignment
|
||||
if (this != &quaternion) {
|
||||
mX = quaternion.mX;
|
||||
mY = quaternion.mY;
|
||||
mZ = quaternion.mZ;
|
||||
mW = quaternion.mW;
|
||||
x = quaternion.x;
|
||||
y = quaternion.y;
|
||||
z = quaternion.z;
|
||||
w = quaternion.w;
|
||||
}
|
||||
|
||||
// Return this quaternion
|
||||
|
|
@ -280,10 +215,10 @@ inline Quaternion& Quaternion::operator=(const Quaternion& quaternion) {
|
|||
|
||||
// Overloaded operator for equality condition
|
||||
inline bool Quaternion::operator==(const Quaternion& quaternion) const {
|
||||
return (mX == quaternion.mX && mY == quaternion.mY &&
|
||||
mZ == quaternion.mZ && mW == quaternion.mW);
|
||||
return (x == quaternion.x && y == quaternion.y &&
|
||||
z == quaternion.z && w == quaternion.w);
|
||||
}
|
||||
|
||||
} // End of the ReactPhysics3D namespace
|
||||
}
|
||||
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -30,27 +30,26 @@
|
|||
using namespace reactphysics3d;
|
||||
|
||||
// Constructor
|
||||
Transform::Transform() {
|
||||
mPosition = Vector3(0.0, 0.0, 0.0);
|
||||
mOrientation = Quaternion::identity();
|
||||
Transform::Transform() : mPosition(Vector3(0.0, 0.0, 0.0)), mOrientation(Quaternion::identity()) {
|
||||
|
||||
}
|
||||
|
||||
// Constructor
|
||||
Transform::Transform(const Vector3& position, const Matrix3x3& orientation) {
|
||||
mPosition = position;
|
||||
mOrientation = Quaternion(orientation);
|
||||
Transform::Transform(const Vector3& position, const Matrix3x3& orientation)
|
||||
: mPosition(position), mOrientation(Quaternion(orientation)) {
|
||||
|
||||
}
|
||||
|
||||
// Constructor
|
||||
Transform::Transform(const Vector3& position, const Quaternion& orientation) {
|
||||
mPosition = position;
|
||||
mOrientation = orientation;
|
||||
Transform::Transform(const Vector3& position, const Quaternion& orientation)
|
||||
: mPosition(position), mOrientation(orientation) {
|
||||
|
||||
}
|
||||
|
||||
// Copy-constructor
|
||||
Transform::Transform(const Transform& transform) {
|
||||
mPosition = transform.mPosition;
|
||||
mOrientation = transform.mOrientation;
|
||||
Transform::Transform(const Transform& transform)
|
||||
: mPosition(transform.mPosition), mOrientation(transform.mOrientation) {
|
||||
|
||||
}
|
||||
|
||||
// Destructor
|
||||
|
|
|
|||
Loading…
Reference in New Issue
Block a user