forked from bartvdbraak/blender
af9573e9ea
- Changed license from MIT to ZLib. - Added 3D Sweep and Prune contribution - More stable native constraint solver Sorry for any inconvenience caused by this checkin. All Blender buildsystems require update: added files and moved files.
396 lines
12 KiB
C++
396 lines
12 KiB
C++
/*
|
|
Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans http://continuousphysics.com/Bullet/
|
|
|
|
This software is provided 'as-is', without any express or implied warranty.
|
|
In no event will the authors be held liable for any damages arising from the use of this software.
|
|
Permission is granted to anyone to use this software for any purpose,
|
|
including commercial applications, and to alter it and redistribute it freely,
|
|
subject to the following restrictions:
|
|
|
|
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
|
|
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
|
3. This notice may not be removed or altered from any source distribution.
|
|
*/
|
|
|
|
|
|
#ifndef SimdMatrix3x3_H
|
|
#define SimdMatrix3x3_H
|
|
|
|
#include "SimdScalar.h"
|
|
|
|
#include "SimdVector3.h"
|
|
#include "SimdQuaternion.h"
|
|
|
|
|
|
class SimdMatrix3x3 {
|
|
public:
|
|
SimdMatrix3x3 () {}
|
|
|
|
// explicit SimdMatrix3x3(const SimdScalar *m) { setFromOpenGLSubMatrix(m); }
|
|
|
|
explicit SimdMatrix3x3(const SimdQuaternion& q) { setRotation(q); }
|
|
/*
|
|
template <typename SimdScalar>
|
|
Matrix3x3(const SimdScalar& yaw, const SimdScalar& pitch, const SimdScalar& roll)
|
|
{
|
|
setEulerYPR(yaw, pitch, roll);
|
|
}
|
|
*/
|
|
SimdMatrix3x3(const SimdScalar& xx, const SimdScalar& xy, const SimdScalar& xz,
|
|
const SimdScalar& yx, const SimdScalar& yy, const SimdScalar& yz,
|
|
const SimdScalar& zx, const SimdScalar& zy, const SimdScalar& zz)
|
|
{
|
|
setValue(xx, xy, xz,
|
|
yx, yy, yz,
|
|
zx, zy, zz);
|
|
}
|
|
|
|
SimdVector3 getColumn(int i) const
|
|
{
|
|
return SimdVector3(m_el[0][i],m_el[1][i],m_el[2][i]);
|
|
}
|
|
|
|
const SimdVector3& getRow(int i) const
|
|
{
|
|
return m_el[i];
|
|
}
|
|
|
|
|
|
SIMD_FORCE_INLINE SimdVector3& operator[](int i)
|
|
{
|
|
assert(0 <= i && i < 3);
|
|
return m_el[i];
|
|
}
|
|
|
|
const SimdVector3& operator[](int i) const
|
|
{
|
|
assert(0 <= i && i < 3);
|
|
return m_el[i];
|
|
}
|
|
|
|
SimdMatrix3x3& operator*=(const SimdMatrix3x3& m);
|
|
|
|
|
|
void setFromOpenGLSubMatrix(const SimdScalar *m)
|
|
{
|
|
m_el[0][0] = (m[0]);
|
|
m_el[1][0] = (m[1]);
|
|
m_el[2][0] = (m[2]);
|
|
m_el[0][1] = (m[4]);
|
|
m_el[1][1] = (m[5]);
|
|
m_el[2][1] = (m[6]);
|
|
m_el[0][2] = (m[8]);
|
|
m_el[1][2] = (m[9]);
|
|
m_el[2][2] = (m[10]);
|
|
}
|
|
|
|
void setValue(const SimdScalar& xx, const SimdScalar& xy, const SimdScalar& xz,
|
|
const SimdScalar& yx, const SimdScalar& yy, const SimdScalar& yz,
|
|
const SimdScalar& zx, const SimdScalar& zy, const SimdScalar& zz)
|
|
{
|
|
m_el[0][0] = SimdScalar(xx);
|
|
m_el[0][1] = SimdScalar(xy);
|
|
m_el[0][2] = SimdScalar(xz);
|
|
m_el[1][0] = SimdScalar(yx);
|
|
m_el[1][1] = SimdScalar(yy);
|
|
m_el[1][2] = SimdScalar(yz);
|
|
m_el[2][0] = SimdScalar(zx);
|
|
m_el[2][1] = SimdScalar(zy);
|
|
m_el[2][2] = SimdScalar(zz);
|
|
}
|
|
|
|
void setRotation(const SimdQuaternion& q)
|
|
{
|
|
SimdScalar d = q.length2();
|
|
assert(d != SimdScalar(0.0));
|
|
SimdScalar s = SimdScalar(2.0) / d;
|
|
SimdScalar xs = q[0] * s, ys = q[1] * s, zs = q[2] * s;
|
|
SimdScalar wx = q[3] * xs, wy = q[3] * ys, wz = q[3] * zs;
|
|
SimdScalar xx = q[0] * xs, xy = q[0] * ys, xz = q[0] * zs;
|
|
SimdScalar yy = q[1] * ys, yz = q[1] * zs, zz = q[2] * zs;
|
|
setValue(SimdScalar(1.0) - (yy + zz), xy - wz, xz + wy,
|
|
xy + wz, SimdScalar(1.0) - (xx + zz), yz - wx,
|
|
xz - wy, yz + wx, SimdScalar(1.0) - (xx + yy));
|
|
}
|
|
|
|
|
|
|
|
void setEulerYPR(const SimdScalar& yaw, const SimdScalar& pitch, const SimdScalar& roll)
|
|
{
|
|
|
|
SimdScalar cy(SimdCos(yaw));
|
|
SimdScalar sy(SimdSin(yaw));
|
|
SimdScalar cp(SimdCos(pitch));
|
|
SimdScalar sp(SimdSin(pitch));
|
|
SimdScalar cr(SimdCos(roll));
|
|
SimdScalar sr(SimdSin(roll));
|
|
SimdScalar cc = cy * cr;
|
|
SimdScalar cs = cy * sr;
|
|
SimdScalar sc = sy * cr;
|
|
SimdScalar ss = sy * sr;
|
|
setValue(cc - sp * ss, -cs - sp * sc, -sy * cp,
|
|
cp * sr, cp * cr, -sp,
|
|
sc + sp * cs, -ss + sp * cc, cy * cp);
|
|
|
|
}
|
|
|
|
/**
|
|
* setEulerZYX
|
|
* @param euler a const reference to a SimdVector3 of euler angles
|
|
* These angles are used to produce a rotation matrix. The euler
|
|
* angles are applied in ZYX order. I.e a vector is first rotated
|
|
* about X then Y and then Z
|
|
**/
|
|
|
|
void setEulerZYX(SimdScalar eulerX,SimdScalar eulerY,SimdScalar eulerZ) {
|
|
SimdScalar ci ( SimdCos(eulerX));
|
|
SimdScalar cj ( SimdCos(eulerY));
|
|
SimdScalar ch ( SimdCos(eulerZ));
|
|
SimdScalar si ( SimdSin(eulerX));
|
|
SimdScalar sj ( SimdSin(eulerY));
|
|
SimdScalar sh ( SimdSin(eulerZ));
|
|
SimdScalar cc = ci * ch;
|
|
SimdScalar cs = ci * sh;
|
|
SimdScalar sc = si * ch;
|
|
SimdScalar ss = si * sh;
|
|
|
|
setValue(cj * ch, sj * sc - cs, sj * cc + ss,
|
|
cj * sh, sj * ss + cc, sj * cs - sc,
|
|
-sj, cj * si, cj * ci);
|
|
}
|
|
|
|
void setIdentity()
|
|
{
|
|
setValue(SimdScalar(1.0), SimdScalar(0.0), SimdScalar(0.0),
|
|
SimdScalar(0.0), SimdScalar(1.0), SimdScalar(0.0),
|
|
SimdScalar(0.0), SimdScalar(0.0), SimdScalar(1.0));
|
|
}
|
|
|
|
void getOpenGLSubMatrix(SimdScalar *m) const
|
|
{
|
|
m[0] = SimdScalar(m_el[0][0]);
|
|
m[1] = SimdScalar(m_el[1][0]);
|
|
m[2] = SimdScalar(m_el[2][0]);
|
|
m[3] = SimdScalar(0.0);
|
|
m[4] = SimdScalar(m_el[0][1]);
|
|
m[5] = SimdScalar(m_el[1][1]);
|
|
m[6] = SimdScalar(m_el[2][1]);
|
|
m[7] = SimdScalar(0.0);
|
|
m[8] = SimdScalar(m_el[0][2]);
|
|
m[9] = SimdScalar(m_el[1][2]);
|
|
m[10] = SimdScalar(m_el[2][2]);
|
|
m[11] = SimdScalar(0.0);
|
|
}
|
|
|
|
void getRotation(SimdQuaternion& q) const
|
|
{
|
|
SimdScalar trace = m_el[0][0] + m_el[1][1] + m_el[2][2];
|
|
|
|
if (trace > SimdScalar(0.0))
|
|
{
|
|
SimdScalar s = SimdSqrt(trace + SimdScalar(1.0));
|
|
q[3] = s * SimdScalar(0.5);
|
|
s = SimdScalar(0.5) / s;
|
|
|
|
q[0] = (m_el[2][1] - m_el[1][2]) * s;
|
|
q[1] = (m_el[0][2] - m_el[2][0]) * s;
|
|
q[2] = (m_el[1][0] - m_el[0][1]) * s;
|
|
}
|
|
else
|
|
{
|
|
int i = m_el[0][0] < m_el[1][1] ?
|
|
(m_el[1][1] < m_el[2][2] ? 2 : 1) :
|
|
(m_el[0][0] < m_el[2][2] ? 2 : 0);
|
|
int j = (i + 1) % 3;
|
|
int k = (i + 2) % 3;
|
|
|
|
SimdScalar s = SimdSqrt(m_el[i][i] - m_el[j][j] - m_el[k][k] + SimdScalar(1.0));
|
|
q[i] = s * SimdScalar(0.5);
|
|
s = SimdScalar(0.5) / s;
|
|
|
|
q[3] = (m_el[k][j] - m_el[j][k]) * s;
|
|
q[j] = (m_el[j][i] + m_el[i][j]) * s;
|
|
q[k] = (m_el[k][i] + m_el[i][k]) * s;
|
|
}
|
|
}
|
|
|
|
|
|
|
|
void getEuler(SimdScalar& yaw, SimdScalar& pitch, SimdScalar& roll) const
|
|
{
|
|
pitch = SimdScalar(SimdAsin(-m_el[2][0]));
|
|
if (pitch < SIMD_2_PI)
|
|
{
|
|
if (pitch > SIMD_2_PI)
|
|
{
|
|
yaw = SimdScalar(SimdAtan2(m_el[1][0], m_el[0][0]));
|
|
roll = SimdScalar(SimdAtan2(m_el[2][1], m_el[2][2]));
|
|
}
|
|
else
|
|
{
|
|
yaw = SimdScalar(-SimdAtan2(-m_el[0][1], m_el[0][2]));
|
|
roll = SimdScalar(0.0);
|
|
}
|
|
}
|
|
else
|
|
{
|
|
yaw = SimdScalar(SimdAtan2(-m_el[0][1], m_el[0][2]));
|
|
roll = SimdScalar(0.0);
|
|
}
|
|
}
|
|
|
|
SimdVector3 getScaling() const
|
|
{
|
|
return SimdVector3(m_el[0][0] * m_el[0][0] + m_el[1][0] * m_el[1][0] + m_el[2][0] * m_el[2][0],
|
|
m_el[0][1] * m_el[0][1] + m_el[1][1] * m_el[1][1] + m_el[2][1] * m_el[2][1],
|
|
m_el[0][2] * m_el[0][2] + m_el[1][2] * m_el[1][2] + m_el[2][2] * m_el[2][2]);
|
|
}
|
|
|
|
|
|
SimdMatrix3x3 scaled(const SimdVector3& s) const
|
|
{
|
|
return SimdMatrix3x3(m_el[0][0] * s[0], m_el[0][1] * s[1], m_el[0][2] * s[2],
|
|
m_el[1][0] * s[0], m_el[1][1] * s[1], m_el[1][2] * s[2],
|
|
m_el[2][0] * s[0], m_el[2][1] * s[1], m_el[2][2] * s[2]);
|
|
}
|
|
|
|
SimdScalar determinant() const;
|
|
SimdMatrix3x3 adjoint() const;
|
|
SimdMatrix3x3 absolute() const;
|
|
SimdMatrix3x3 transpose() const;
|
|
SimdMatrix3x3 inverse() const;
|
|
|
|
SimdMatrix3x3 transposeTimes(const SimdMatrix3x3& m) const;
|
|
SimdMatrix3x3 timesTranspose(const SimdMatrix3x3& m) const;
|
|
|
|
SimdScalar tdot(int c, const SimdVector3& v) const
|
|
{
|
|
return m_el[0][c] * v[0] + m_el[1][c] * v[1] + m_el[2][c] * v[2];
|
|
}
|
|
|
|
protected:
|
|
SimdScalar cofac(int r1, int c1, int r2, int c2) const
|
|
{
|
|
return m_el[r1][c1] * m_el[r2][c2] - m_el[r1][c2] * m_el[r2][c1];
|
|
}
|
|
|
|
SimdVector3 m_el[3];
|
|
};
|
|
|
|
SIMD_FORCE_INLINE SimdMatrix3x3&
|
|
SimdMatrix3x3::operator*=(const SimdMatrix3x3& m)
|
|
{
|
|
setValue(m.tdot(0, m_el[0]), m.tdot(1, m_el[0]), m.tdot(2, m_el[0]),
|
|
m.tdot(0, m_el[1]), m.tdot(1, m_el[1]), m.tdot(2, m_el[1]),
|
|
m.tdot(0, m_el[2]), m.tdot(1, m_el[2]), m.tdot(2, m_el[2]));
|
|
return *this;
|
|
}
|
|
|
|
SIMD_FORCE_INLINE SimdScalar
|
|
SimdMatrix3x3::determinant() const
|
|
{
|
|
return triple((*this)[0], (*this)[1], (*this)[2]);
|
|
}
|
|
|
|
|
|
SIMD_FORCE_INLINE SimdMatrix3x3
|
|
SimdMatrix3x3::absolute() const
|
|
{
|
|
return SimdMatrix3x3(
|
|
SimdFabs(m_el[0][0]), SimdFabs(m_el[0][1]), SimdFabs(m_el[0][2]),
|
|
SimdFabs(m_el[1][0]), SimdFabs(m_el[1][1]), SimdFabs(m_el[1][2]),
|
|
SimdFabs(m_el[2][0]), SimdFabs(m_el[2][1]), SimdFabs(m_el[2][2]));
|
|
}
|
|
|
|
SIMD_FORCE_INLINE SimdMatrix3x3
|
|
SimdMatrix3x3::transpose() const
|
|
{
|
|
return SimdMatrix3x3(m_el[0][0], m_el[1][0], m_el[2][0],
|
|
m_el[0][1], m_el[1][1], m_el[2][1],
|
|
m_el[0][2], m_el[1][2], m_el[2][2]);
|
|
}
|
|
|
|
SIMD_FORCE_INLINE SimdMatrix3x3
|
|
SimdMatrix3x3::adjoint() const
|
|
{
|
|
return SimdMatrix3x3(cofac(1, 1, 2, 2), cofac(0, 2, 2, 1), cofac(0, 1, 1, 2),
|
|
cofac(1, 2, 2, 0), cofac(0, 0, 2, 2), cofac(0, 2, 1, 0),
|
|
cofac(1, 0, 2, 1), cofac(0, 1, 2, 0), cofac(0, 0, 1, 1));
|
|
}
|
|
|
|
SIMD_FORCE_INLINE SimdMatrix3x3
|
|
SimdMatrix3x3::inverse() const
|
|
{
|
|
SimdVector3 co(cofac(1, 1, 2, 2), cofac(1, 2, 2, 0), cofac(1, 0, 2, 1));
|
|
SimdScalar det = (*this)[0].dot(co);
|
|
assert(det != SimdScalar(0.0f));
|
|
SimdScalar s = SimdScalar(1.0f) / det;
|
|
return SimdMatrix3x3(co[0] * s, cofac(0, 2, 2, 1) * s, cofac(0, 1, 1, 2) * s,
|
|
co[1] * s, cofac(0, 0, 2, 2) * s, cofac(0, 2, 1, 0) * s,
|
|
co[2] * s, cofac(0, 1, 2, 0) * s, cofac(0, 0, 1, 1) * s);
|
|
}
|
|
|
|
SIMD_FORCE_INLINE SimdMatrix3x3
|
|
SimdMatrix3x3::transposeTimes(const SimdMatrix3x3& m) const
|
|
{
|
|
return SimdMatrix3x3(
|
|
m_el[0][0] * m[0][0] + m_el[1][0] * m[1][0] + m_el[2][0] * m[2][0],
|
|
m_el[0][0] * m[0][1] + m_el[1][0] * m[1][1] + m_el[2][0] * m[2][1],
|
|
m_el[0][0] * m[0][2] + m_el[1][0] * m[1][2] + m_el[2][0] * m[2][2],
|
|
m_el[0][1] * m[0][0] + m_el[1][1] * m[1][0] + m_el[2][1] * m[2][0],
|
|
m_el[0][1] * m[0][1] + m_el[1][1] * m[1][1] + m_el[2][1] * m[2][1],
|
|
m_el[0][1] * m[0][2] + m_el[1][1] * m[1][2] + m_el[2][1] * m[2][2],
|
|
m_el[0][2] * m[0][0] + m_el[1][2] * m[1][0] + m_el[2][2] * m[2][0],
|
|
m_el[0][2] * m[0][1] + m_el[1][2] * m[1][1] + m_el[2][2] * m[2][1],
|
|
m_el[0][2] * m[0][2] + m_el[1][2] * m[1][2] + m_el[2][2] * m[2][2]);
|
|
}
|
|
|
|
SIMD_FORCE_INLINE SimdMatrix3x3
|
|
SimdMatrix3x3::timesTranspose(const SimdMatrix3x3& m) const
|
|
{
|
|
return SimdMatrix3x3(
|
|
m_el[0].dot(m[0]), m_el[0].dot(m[1]), m_el[0].dot(m[2]),
|
|
m_el[1].dot(m[0]), m_el[1].dot(m[1]), m_el[1].dot(m[2]),
|
|
m_el[2].dot(m[0]), m_el[2].dot(m[1]), m_el[2].dot(m[2]));
|
|
|
|
}
|
|
|
|
SIMD_FORCE_INLINE SimdVector3
|
|
operator*(const SimdMatrix3x3& m, const SimdVector3& v)
|
|
{
|
|
return SimdVector3(m[0].dot(v), m[1].dot(v), m[2].dot(v));
|
|
}
|
|
|
|
|
|
SIMD_FORCE_INLINE SimdVector3
|
|
operator*(const SimdVector3& v, const SimdMatrix3x3& m)
|
|
{
|
|
return SimdVector3(m.tdot(0, v), m.tdot(1, v), m.tdot(2, v));
|
|
}
|
|
|
|
SIMD_FORCE_INLINE SimdMatrix3x3
|
|
operator*(const SimdMatrix3x3& m1, const SimdMatrix3x3& m2)
|
|
{
|
|
return SimdMatrix3x3(
|
|
m2.tdot(0, m1[0]), m2.tdot(1, m1[0]), m2.tdot(2, m1[0]),
|
|
m2.tdot(0, m1[1]), m2.tdot(1, m1[1]), m2.tdot(2, m1[1]),
|
|
m2.tdot(0, m1[2]), m2.tdot(1, m1[2]), m2.tdot(2, m1[2]));
|
|
}
|
|
|
|
|
|
SIMD_FORCE_INLINE SimdMatrix3x3 SimdMultTransposeLeft(const SimdMatrix3x3& m1, const SimdMatrix3x3& m2) {
|
|
return SimdMatrix3x3(
|
|
m1[0][0] * m2[0][0] + m1[1][0] * m2[1][0] + m1[2][0] * m2[2][0],
|
|
m1[0][0] * m2[0][1] + m1[1][0] * m2[1][1] + m1[2][0] * m2[2][1],
|
|
m1[0][0] * m2[0][2] + m1[1][0] * m2[1][2] + m1[2][0] * m2[2][2],
|
|
m1[0][1] * m2[0][0] + m1[1][1] * m2[1][0] + m1[2][1] * m2[2][0],
|
|
m1[0][1] * m2[0][1] + m1[1][1] * m2[1][1] + m1[2][1] * m2[2][1],
|
|
m1[0][1] * m2[0][2] + m1[1][1] * m2[1][2] + m1[2][1] * m2[2][2],
|
|
m1[0][2] * m2[0][0] + m1[1][2] * m2[1][0] + m1[2][2] * m2[2][0],
|
|
m1[0][2] * m2[0][1] + m1[1][2] * m2[1][1] + m1[2][2] * m2[2][1],
|
|
m1[0][2] * m2[0][2] + m1[1][2] * m2[1][2] + m1[2][2] * m2[2][2]);
|
|
}
|
|
|
|
|
|
#endif
|