merged most of the changes from the branch into trunk, except for COLLADA, libxml and glut glitches.
Still need to verify to make sure no unwanted renaming is introduced.
This commit is contained in:
ejcoumans
@@ -1,395 +0,0 @@
|
||||
/*
|
||||
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 "LinearMath/SimdScalar.h"
|
||||
|
||||
#include "LinearMath/SimdVector3.h"
|
||||
#include "LinearMath/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
|
||||
@@ -1,290 +0,0 @@
|
||||
/*
|
||||
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 SIMD__QUATERNION_H_
|
||||
#define SIMD__QUATERNION_H_
|
||||
|
||||
#include "LinearMath/SimdVector3.h"
|
||||
|
||||
class SimdQuaternion : public SimdQuadWord {
|
||||
public:
|
||||
SimdQuaternion() {}
|
||||
|
||||
// template <typename SimdScalar>
|
||||
// explicit Quaternion(const SimdScalar *v) : Tuple4<SimdScalar>(v) {}
|
||||
|
||||
SimdQuaternion(const SimdScalar& x, const SimdScalar& y, const SimdScalar& z, const SimdScalar& w)
|
||||
: SimdQuadWord(x, y, z, w)
|
||||
{}
|
||||
|
||||
SimdQuaternion(const SimdVector3& axis, const SimdScalar& angle)
|
||||
{
|
||||
setRotation(axis, angle);
|
||||
}
|
||||
|
||||
SimdQuaternion(const SimdScalar& yaw, const SimdScalar& pitch, const SimdScalar& roll)
|
||||
{
|
||||
setEuler(yaw, pitch, roll);
|
||||
}
|
||||
|
||||
void setRotation(const SimdVector3& axis, const SimdScalar& angle)
|
||||
{
|
||||
SimdScalar d = axis.length();
|
||||
assert(d != SimdScalar(0.0));
|
||||
SimdScalar s = SimdSin(angle * SimdScalar(0.5)) / d;
|
||||
setValue(axis.x() * s, axis.y() * s, axis.z() * s,
|
||||
SimdCos(angle * SimdScalar(0.5)));
|
||||
}
|
||||
|
||||
void setEuler(const SimdScalar& yaw, const SimdScalar& pitch, const SimdScalar& roll)
|
||||
{
|
||||
SimdScalar halfYaw = SimdScalar(yaw) * SimdScalar(0.5);
|
||||
SimdScalar halfPitch = SimdScalar(pitch) * SimdScalar(0.5);
|
||||
SimdScalar halfRoll = SimdScalar(roll) * SimdScalar(0.5);
|
||||
SimdScalar cosYaw = SimdCos(halfYaw);
|
||||
SimdScalar sinYaw = SimdSin(halfYaw);
|
||||
SimdScalar cosPitch = SimdCos(halfPitch);
|
||||
SimdScalar sinPitch = SimdSin(halfPitch);
|
||||
SimdScalar cosRoll = SimdCos(halfRoll);
|
||||
SimdScalar sinRoll = SimdSin(halfRoll);
|
||||
setValue(cosRoll * sinPitch * cosYaw + sinRoll * cosPitch * sinYaw,
|
||||
cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw,
|
||||
sinRoll * cosPitch * cosYaw - cosRoll * sinPitch * sinYaw,
|
||||
cosRoll * cosPitch * cosYaw + sinRoll * sinPitch * sinYaw);
|
||||
}
|
||||
|
||||
SimdQuaternion& operator+=(const SimdQuaternion& q)
|
||||
{
|
||||
m_x += q.x(); m_y += q.y(); m_z += q.z(); m_unusedW += q[3];
|
||||
return *this;
|
||||
}
|
||||
|
||||
SimdQuaternion& operator-=(const SimdQuaternion& q)
|
||||
{
|
||||
m_x -= q.x(); m_y -= q.y(); m_z -= q.z(); m_unusedW -= q[3];
|
||||
return *this;
|
||||
}
|
||||
|
||||
SimdQuaternion& operator*=(const SimdScalar& s)
|
||||
{
|
||||
m_x *= s; m_y *= s; m_z *= s; m_unusedW *= s;
|
||||
return *this;
|
||||
}
|
||||
|
||||
|
||||
SimdQuaternion& operator*=(const SimdQuaternion& q)
|
||||
{
|
||||
setValue(m_unusedW * q.x() + m_x * q[3] + m_y * q.z() - m_z * q.y(),
|
||||
m_unusedW * q.y() + m_y * q[3] + m_z * q.x() - m_x * q.z(),
|
||||
m_unusedW * q.z() + m_z * q[3] + m_x * q.y() - m_y * q.x(),
|
||||
m_unusedW * q[3] - m_x * q.x() - m_y * q.y() - m_z * q.z());
|
||||
return *this;
|
||||
}
|
||||
|
||||
SimdScalar dot(const SimdQuaternion& q) const
|
||||
{
|
||||
return m_x * q.x() + m_y * q.y() + m_z * q.z() + m_unusedW * q[3];
|
||||
}
|
||||
|
||||
SimdScalar length2() const
|
||||
{
|
||||
return dot(*this);
|
||||
}
|
||||
|
||||
SimdScalar length() const
|
||||
{
|
||||
return SimdSqrt(length2());
|
||||
}
|
||||
|
||||
SimdQuaternion& normalize()
|
||||
{
|
||||
return *this /= length();
|
||||
}
|
||||
|
||||
SIMD_FORCE_INLINE SimdQuaternion
|
||||
operator*(const SimdScalar& s) const
|
||||
{
|
||||
return SimdQuaternion(x() * s, y() * s, z() * s, m_unusedW * s);
|
||||
}
|
||||
|
||||
|
||||
|
||||
SimdQuaternion operator/(const SimdScalar& s) const
|
||||
{
|
||||
assert(s != SimdScalar(0.0));
|
||||
return *this * (SimdScalar(1.0) / s);
|
||||
}
|
||||
|
||||
|
||||
SimdQuaternion& operator/=(const SimdScalar& s)
|
||||
{
|
||||
assert(s != SimdScalar(0.0));
|
||||
return *this *= SimdScalar(1.0) / s;
|
||||
}
|
||||
|
||||
|
||||
SimdQuaternion normalized() const
|
||||
{
|
||||
return *this / length();
|
||||
}
|
||||
|
||||
SimdScalar angle(const SimdQuaternion& q) const
|
||||
{
|
||||
SimdScalar s = SimdSqrt(length2() * q.length2());
|
||||
assert(s != SimdScalar(0.0));
|
||||
return SimdAcos(dot(q) / s);
|
||||
}
|
||||
|
||||
SimdScalar getAngle() const
|
||||
{
|
||||
SimdScalar s = 2.f * SimdAcos(m_unusedW);
|
||||
return s;
|
||||
}
|
||||
|
||||
|
||||
|
||||
SimdQuaternion inverse() const
|
||||
{
|
||||
return SimdQuaternion(m_x, m_y, m_z, -m_unusedW);
|
||||
}
|
||||
|
||||
SIMD_FORCE_INLINE SimdQuaternion
|
||||
operator+(const SimdQuaternion& q2) const
|
||||
{
|
||||
const SimdQuaternion& q1 = *this;
|
||||
return SimdQuaternion(q1.x() + q2.x(), q1.y() + q2.y(), q1.z() + q2.z(), q1[3] + q2[3]);
|
||||
}
|
||||
|
||||
SIMD_FORCE_INLINE SimdQuaternion
|
||||
operator-(const SimdQuaternion& q2) const
|
||||
{
|
||||
const SimdQuaternion& q1 = *this;
|
||||
return SimdQuaternion(q1.x() - q2.x(), q1.y() - q2.y(), q1.z() - q2.z(), q1[3] - q2[3]);
|
||||
}
|
||||
|
||||
SIMD_FORCE_INLINE SimdQuaternion operator-() const
|
||||
{
|
||||
const SimdQuaternion& q2 = *this;
|
||||
return SimdQuaternion( - q2.x(), - q2.y(), - q2.z(), - q2[3]);
|
||||
}
|
||||
|
||||
SIMD_FORCE_INLINE SimdQuaternion farthest( const SimdQuaternion& qd) const
|
||||
{
|
||||
SimdQuaternion diff,sum;
|
||||
diff = *this - qd;
|
||||
sum = *this + qd;
|
||||
if( diff.dot(diff) > sum.dot(sum) )
|
||||
return qd;
|
||||
return (-qd);
|
||||
}
|
||||
|
||||
SimdQuaternion slerp(const SimdQuaternion& q, const SimdScalar& t) const
|
||||
{
|
||||
SimdScalar theta = angle(q);
|
||||
if (theta != SimdScalar(0.0))
|
||||
{
|
||||
SimdScalar d = SimdScalar(1.0) / SimdSin(theta);
|
||||
SimdScalar s0 = SimdSin((SimdScalar(1.0) - t) * theta);
|
||||
SimdScalar s1 = SimdSin(t * theta);
|
||||
return SimdQuaternion((m_x * s0 + q.x() * s1) * d,
|
||||
(m_y * s0 + q.y() * s1) * d,
|
||||
(m_z * s0 + q.z() * s1) * d,
|
||||
(m_unusedW * s0 + q[3] * s1) * d);
|
||||
}
|
||||
else
|
||||
{
|
||||
return *this;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
};
|
||||
|
||||
|
||||
|
||||
SIMD_FORCE_INLINE SimdQuaternion
|
||||
operator-(const SimdQuaternion& q)
|
||||
{
|
||||
return SimdQuaternion(-q.x(), -q.y(), -q.z(), -q[3]);
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
SIMD_FORCE_INLINE SimdQuaternion
|
||||
operator*(const SimdQuaternion& q1, const SimdQuaternion& q2) {
|
||||
return SimdQuaternion(q1[3] * q2.x() + q1.x() * q2[3] + q1.y() * q2.z() - q1.z() * q2.y(),
|
||||
q1[3] * q2.y() + q1.y() * q2[3] + q1.z() * q2.x() - q1.x() * q2.z(),
|
||||
q1[3] * q2.z() + q1.z() * q2[3] + q1.x() * q2.y() - q1.y() * q2.x(),
|
||||
q1[3] * q2[3] - q1.x() * q2.x() - q1.y() * q2.y() - q1.z() * q2.z());
|
||||
}
|
||||
|
||||
SIMD_FORCE_INLINE SimdQuaternion
|
||||
operator*(const SimdQuaternion& q, const SimdVector3& w)
|
||||
{
|
||||
return SimdQuaternion( q[3] * w.x() + q.y() * w.z() - q.z() * w.y(),
|
||||
q[3] * w.y() + q.z() * w.x() - q.x() * w.z(),
|
||||
q[3] * w.z() + q.x() * w.y() - q.y() * w.x(),
|
||||
-q.x() * w.x() - q.y() * w.y() - q.z() * w.z());
|
||||
}
|
||||
|
||||
SIMD_FORCE_INLINE SimdQuaternion
|
||||
operator*(const SimdVector3& w, const SimdQuaternion& q)
|
||||
{
|
||||
return SimdQuaternion( w.x() * q[3] + w.y() * q.z() - w.z() * q.y(),
|
||||
w.y() * q[3] + w.z() * q.x() - w.x() * q.z(),
|
||||
w.z() * q[3] + w.x() * q.y() - w.y() * q.x(),
|
||||
-w.x() * q.x() - w.y() * q.y() - w.z() * q.z());
|
||||
}
|
||||
|
||||
SIMD_FORCE_INLINE SimdScalar
|
||||
dot(const SimdQuaternion& q1, const SimdQuaternion& q2)
|
||||
{
|
||||
return q1.dot(q2);
|
||||
}
|
||||
|
||||
|
||||
SIMD_FORCE_INLINE SimdScalar
|
||||
length(const SimdQuaternion& q)
|
||||
{
|
||||
return q.length();
|
||||
}
|
||||
|
||||
SIMD_FORCE_INLINE SimdScalar
|
||||
angle(const SimdQuaternion& q1, const SimdQuaternion& q2)
|
||||
{
|
||||
return q1.angle(q2);
|
||||
}
|
||||
|
||||
|
||||
SIMD_FORCE_INLINE SimdQuaternion
|
||||
inverse(const SimdQuaternion& q)
|
||||
{
|
||||
return q.inverse();
|
||||
}
|
||||
|
||||
SIMD_FORCE_INLINE SimdQuaternion
|
||||
slerp(const SimdQuaternion& q1, const SimdQuaternion& q2, const SimdScalar& t)
|
||||
{
|
||||
return q1.slerp(q2, t);
|
||||
}
|
||||
|
||||
|
||||
#endif
|
||||
|
||||
|
||||
|
||||
@@ -1,128 +0,0 @@
|
||||
/*
|
||||
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 SIMD___SCALAR_H
|
||||
#define SIMD___SCALAR_H
|
||||
|
||||
#include <math.h>
|
||||
#undef max
|
||||
|
||||
|
||||
|
||||
#include <cstdlib>
|
||||
#include <cfloat>
|
||||
#include <float.h>
|
||||
|
||||
#ifdef WIN32
|
||||
|
||||
#if defined(__MINGW32__) || defined(__CYGWIN__)
|
||||
#define SIMD_FORCE_INLINE inline
|
||||
#else
|
||||
#pragma warning(disable:4530)
|
||||
#pragma warning(disable:4996)
|
||||
#define SIMD_FORCE_INLINE __forceinline
|
||||
#endif //__MINGW32__
|
||||
|
||||
//#define ATTRIBUTE_ALIGNED16(a) __declspec(align(16)) a
|
||||
#define ATTRIBUTE_ALIGNED16(a) a
|
||||
#include <assert.h>
|
||||
#define ASSERT assert
|
||||
#else
|
||||
|
||||
//non-windows systems
|
||||
|
||||
#define SIMD_FORCE_INLINE inline
|
||||
#define ATTRIBUTE_ALIGNED16(a) a
|
||||
#ifndef assert
|
||||
#include <assert.h>
|
||||
#endif
|
||||
#define ASSERT assert
|
||||
#endif
|
||||
|
||||
|
||||
|
||||
typedef float SimdScalar;
|
||||
|
||||
#if defined (__sun) || defined (__sun__) || defined (__sparc) || defined (__APPLE__)
|
||||
//use double float precision operation on those platforms for Blender
|
||||
|
||||
SIMD_FORCE_INLINE SimdScalar SimdSqrt(SimdScalar x) { return sqrt(x); }
|
||||
SIMD_FORCE_INLINE SimdScalar SimdFabs(SimdScalar x) { return fabs(x); }
|
||||
SIMD_FORCE_INLINE SimdScalar SimdCos(SimdScalar x) { return cos(x); }
|
||||
SIMD_FORCE_INLINE SimdScalar SimdSin(SimdScalar x) { return sin(x); }
|
||||
SIMD_FORCE_INLINE SimdScalar SimdTan(SimdScalar x) { return tan(x); }
|
||||
SIMD_FORCE_INLINE SimdScalar SimdAcos(SimdScalar x) { return acos(x); }
|
||||
SIMD_FORCE_INLINE SimdScalar SimdAsin(SimdScalar x) { return asin(x); }
|
||||
SIMD_FORCE_INLINE SimdScalar SimdAtan(SimdScalar x) { return atan(x); }
|
||||
SIMD_FORCE_INLINE SimdScalar SimdAtan2(SimdScalar x, SimdScalar y) { return atan2(x, y); }
|
||||
SIMD_FORCE_INLINE SimdScalar SimdExp(SimdScalar x) { return exp(x); }
|
||||
SIMD_FORCE_INLINE SimdScalar SimdLog(SimdScalar x) { return log(x); }
|
||||
SIMD_FORCE_INLINE SimdScalar SimdPow(SimdScalar x,SimdScalar y) { return pow(x,y); }
|
||||
|
||||
#else
|
||||
|
||||
SIMD_FORCE_INLINE SimdScalar SimdSqrt(SimdScalar x) { return sqrtf(x); }
|
||||
SIMD_FORCE_INLINE SimdScalar SimdFabs(SimdScalar x) { return fabsf(x); }
|
||||
SIMD_FORCE_INLINE SimdScalar SimdCos(SimdScalar x) { return cosf(x); }
|
||||
SIMD_FORCE_INLINE SimdScalar SimdSin(SimdScalar x) { return sinf(x); }
|
||||
SIMD_FORCE_INLINE SimdScalar SimdTan(SimdScalar x) { return tanf(x); }
|
||||
SIMD_FORCE_INLINE SimdScalar SimdAcos(SimdScalar x) { return acosf(x); }
|
||||
SIMD_FORCE_INLINE SimdScalar SimdAsin(SimdScalar x) { return asinf(x); }
|
||||
SIMD_FORCE_INLINE SimdScalar SimdAtan(SimdScalar x) { return atanf(x); }
|
||||
SIMD_FORCE_INLINE SimdScalar SimdAtan2(SimdScalar x, SimdScalar y) { return atan2f(x, y); }
|
||||
SIMD_FORCE_INLINE SimdScalar SimdExp(SimdScalar x) { return expf(x); }
|
||||
SIMD_FORCE_INLINE SimdScalar SimdLog(SimdScalar x) { return logf(x); }
|
||||
SIMD_FORCE_INLINE SimdScalar SimdPow(SimdScalar x,SimdScalar y) { return powf(x,y); }
|
||||
|
||||
#endif
|
||||
|
||||
|
||||
const SimdScalar SIMD_2_PI = 6.283185307179586232f;
|
||||
const SimdScalar SIMD_PI = SIMD_2_PI * SimdScalar(0.5f);
|
||||
const SimdScalar SIMD_HALF_PI = SIMD_2_PI * SimdScalar(0.25f);
|
||||
const SimdScalar SIMD_RADS_PER_DEG = SIMD_2_PI / SimdScalar(360.0f);
|
||||
const SimdScalar SIMD_DEGS_PER_RAD = SimdScalar(360.0f) / SIMD_2_PI;
|
||||
const SimdScalar SIMD_EPSILON = FLT_EPSILON;
|
||||
const SimdScalar SIMD_INFINITY = FLT_MAX;
|
||||
|
||||
SIMD_FORCE_INLINE bool SimdFuzzyZero(SimdScalar x) { return SimdFabs(x) < SIMD_EPSILON; }
|
||||
|
||||
SIMD_FORCE_INLINE bool SimdEqual(SimdScalar a, SimdScalar eps) {
|
||||
return (((a) <= eps) && !((a) < -eps));
|
||||
}
|
||||
SIMD_FORCE_INLINE bool SimdGreaterEqual (SimdScalar a, SimdScalar eps) {
|
||||
return (!((a) <= eps));
|
||||
}
|
||||
|
||||
/*SIMD_FORCE_INLINE SimdScalar SimdCos(SimdScalar x) { return cosf(x); }
|
||||
SIMD_FORCE_INLINE SimdScalar SimdSin(SimdScalar x) { return sinf(x); }
|
||||
SIMD_FORCE_INLINE SimdScalar SimdTan(SimdScalar x) { return tanf(x); }
|
||||
SIMD_FORCE_INLINE SimdScalar SimdAcos(SimdScalar x) { return acosf(x); }
|
||||
SIMD_FORCE_INLINE SimdScalar SimdAsin(SimdScalar x) { return asinf(x); }
|
||||
SIMD_FORCE_INLINE SimdScalar SimdAtan(SimdScalar x) { return atanf(x); }
|
||||
SIMD_FORCE_INLINE SimdScalar SimdAtan2(SimdScalar x, SimdScalar y) { return atan2f(x, y); }
|
||||
*/
|
||||
|
||||
SIMD_FORCE_INLINE int SimdSign(SimdScalar x) {
|
||||
return x < 0.0f ? -1 : x > 0.0f ? 1 : 0;
|
||||
}
|
||||
|
||||
SIMD_FORCE_INLINE SimdScalar SimdRadians(SimdScalar x) { return x * SIMD_RADS_PER_DEG; }
|
||||
SIMD_FORCE_INLINE SimdScalar SimdDegrees(SimdScalar x) { return x * SIMD_DEGS_PER_RAD; }
|
||||
|
||||
|
||||
|
||||
#endif //SIMD___SCALAR_H
|
||||
@@ -1,283 +0,0 @@
|
||||
/*
|
||||
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 SimdTransform_H
|
||||
#define SimdTransform_H
|
||||
|
||||
#include "LinearMath/SimdVector3.h"
|
||||
#include "LinearMath/SimdMatrix3x3.h"
|
||||
|
||||
|
||||
#define IGNORE_TYPE 1
|
||||
|
||||
class SimdTransform {
|
||||
|
||||
|
||||
public:
|
||||
|
||||
enum {
|
||||
TRANSLATION = 0x01,
|
||||
ROTATION = 0x02,
|
||||
RIGID = TRANSLATION | ROTATION,
|
||||
SCALING = 0x04,
|
||||
LINEAR = ROTATION | SCALING,
|
||||
AFFINE = TRANSLATION | LINEAR
|
||||
};
|
||||
|
||||
SimdTransform() {}
|
||||
|
||||
explicit SIMD_FORCE_INLINE SimdTransform(const SimdQuaternion& q,
|
||||
const SimdVector3& c = SimdVector3(SimdScalar(0), SimdScalar(0), SimdScalar(0)))
|
||||
: m_basis(q),
|
||||
m_origin(c)
|
||||
#ifndef IGNORE_TYPE
|
||||
, m_type(RIGID)
|
||||
#endif //IGNORE_TYPE
|
||||
{}
|
||||
|
||||
explicit SIMD_FORCE_INLINE SimdTransform(const SimdMatrix3x3& b,
|
||||
const SimdVector3& c = SimdVector3(SimdScalar(0), SimdScalar(0), SimdScalar(0)),
|
||||
unsigned int type = AFFINE)
|
||||
: m_basis(b),
|
||||
m_origin(c)
|
||||
#ifndef IGNORE_TYPE
|
||||
, m_type(type)
|
||||
#endif //IGNORE_TYPE
|
||||
{}
|
||||
|
||||
|
||||
SIMD_FORCE_INLINE void mult(const SimdTransform& t1, const SimdTransform& t2) {
|
||||
m_basis = t1.m_basis * t2.m_basis;
|
||||
m_origin = t1(t2.m_origin);
|
||||
#ifndef IGNORE_TYPE
|
||||
m_type = t1.m_type | t2.m_type;
|
||||
#endif //IGNORE_TYPE
|
||||
}
|
||||
|
||||
void multInverseLeft(const SimdTransform& t1, const SimdTransform& t2) {
|
||||
SimdVector3 v = t2.m_origin - t1.m_origin;
|
||||
#ifndef IGNORE_TYPE
|
||||
if (t1.m_type & SCALING) {
|
||||
SimdMatrix3x3 inv = t1.m_basis.inverse();
|
||||
m_basis = inv * t2.m_basis;
|
||||
m_origin = inv * v;
|
||||
}
|
||||
else
|
||||
#else
|
||||
{
|
||||
m_basis = SimdMultTransposeLeft(t1.m_basis, t2.m_basis);
|
||||
m_origin = v * t1.m_basis;
|
||||
#endif //IGNORE_TYPE
|
||||
}
|
||||
#ifndef IGNORE_TYPE
|
||||
m_type = t1.m_type | t2.m_type;
|
||||
#endif //IGNORE_TYPE
|
||||
}
|
||||
|
||||
SIMD_FORCE_INLINE SimdVector3 operator()(const SimdVector3& x) const
|
||||
{
|
||||
return SimdVector3(m_basis[0].dot(x) + m_origin[0],
|
||||
m_basis[1].dot(x) + m_origin[1],
|
||||
m_basis[2].dot(x) + m_origin[2]);
|
||||
}
|
||||
|
||||
SIMD_FORCE_INLINE SimdVector3 operator*(const SimdVector3& x) const
|
||||
{
|
||||
return (*this)(x);
|
||||
}
|
||||
|
||||
SIMD_FORCE_INLINE SimdMatrix3x3& getBasis() { return m_basis; }
|
||||
SIMD_FORCE_INLINE const SimdMatrix3x3& getBasis() const { return m_basis; }
|
||||
|
||||
SIMD_FORCE_INLINE SimdVector3& getOrigin() { return m_origin; }
|
||||
SIMD_FORCE_INLINE const SimdVector3& getOrigin() const { return m_origin; }
|
||||
|
||||
SimdQuaternion getRotation() const {
|
||||
SimdQuaternion q;
|
||||
m_basis.getRotation(q);
|
||||
return q;
|
||||
}
|
||||
template <typename Scalar2>
|
||||
void setValue(const Scalar2 *m)
|
||||
{
|
||||
m_basis.setValue(m);
|
||||
m_origin.setValue(&m[12]);
|
||||
#ifndef IGNORE_TYPE
|
||||
m_type = AFFINE;
|
||||
#endif //IGNORE_TYPE
|
||||
}
|
||||
|
||||
|
||||
void setFromOpenGLMatrix(const SimdScalar *m)
|
||||
{
|
||||
m_basis.setFromOpenGLSubMatrix(m);
|
||||
m_origin[0] = m[12];
|
||||
m_origin[1] = m[13];
|
||||
m_origin[2] = m[14];
|
||||
}
|
||||
|
||||
void getOpenGLMatrix(SimdScalar *m) const
|
||||
{
|
||||
m_basis.getOpenGLSubMatrix(m);
|
||||
m[12] = m_origin[0];
|
||||
m[13] = m_origin[1];
|
||||
m[14] = m_origin[2];
|
||||
m[15] = SimdScalar(1.0f);
|
||||
}
|
||||
|
||||
SIMD_FORCE_INLINE void setOrigin(const SimdVector3& origin)
|
||||
{
|
||||
m_origin = origin;
|
||||
#ifndef IGNORE_TYPE
|
||||
m_type |= TRANSLATION;
|
||||
#endif //IGNORE_TYPE
|
||||
}
|
||||
|
||||
SIMD_FORCE_INLINE SimdVector3 invXform(const SimdVector3& inVec) const;
|
||||
|
||||
|
||||
|
||||
SIMD_FORCE_INLINE void setBasis(const SimdMatrix3x3& basis)
|
||||
{
|
||||
m_basis = basis;
|
||||
#ifndef IGNORE_TYPE
|
||||
m_type |= LINEAR;
|
||||
#endif //IGNORE_TYPE
|
||||
}
|
||||
|
||||
SIMD_FORCE_INLINE void setRotation(const SimdQuaternion& q)
|
||||
{
|
||||
m_basis.setRotation(q);
|
||||
#ifndef IGNORE_TYPE
|
||||
m_type = (m_type & ~LINEAR) | ROTATION;
|
||||
#endif //IGNORE_TYPE
|
||||
}
|
||||
|
||||
SIMD_FORCE_INLINE void scale(const SimdVector3& scaling)
|
||||
{
|
||||
m_basis = m_basis.scaled(scaling);
|
||||
#ifndef IGNORE_TYPE
|
||||
m_type |= SCALING;
|
||||
#endif //IGNORE_TYPE
|
||||
}
|
||||
|
||||
void setIdentity()
|
||||
{
|
||||
m_basis.setIdentity();
|
||||
m_origin.setValue(SimdScalar(0.0), SimdScalar(0.0), SimdScalar(0.0));
|
||||
#ifndef IGNORE_TYPE
|
||||
m_type = 0x0;
|
||||
#endif //IGNORE_TYPE
|
||||
}
|
||||
|
||||
SIMD_FORCE_INLINE bool isIdentity() const {
|
||||
#ifdef IGNORE_TYPE
|
||||
return false;
|
||||
#else
|
||||
return m_type == 0x0;
|
||||
#endif
|
||||
}
|
||||
|
||||
SimdTransform& operator*=(const SimdTransform& t)
|
||||
{
|
||||
m_origin += m_basis * t.m_origin;
|
||||
m_basis *= t.m_basis;
|
||||
#ifndef IGNORE_TYPE
|
||||
m_type |= t.m_type;
|
||||
#endif //IGNORE_TYPE
|
||||
return *this;
|
||||
}
|
||||
|
||||
SimdTransform inverse() const
|
||||
{
|
||||
#ifdef IGNORE_TYPE
|
||||
SimdMatrix3x3 inv = m_basis.transpose();
|
||||
return SimdTransform(inv, inv * -m_origin, 0);
|
||||
#else
|
||||
if (m_type)
|
||||
{
|
||||
SimdMatrix3x3 inv = (m_type & SCALING) ?
|
||||
m_basis.inverse() :
|
||||
m_basis.transpose();
|
||||
|
||||
return SimdTransform(inv, inv * -m_origin, m_type);
|
||||
}
|
||||
|
||||
return *this;
|
||||
#endif //IGNORE_TYPE
|
||||
}
|
||||
|
||||
SimdTransform inverseTimes(const SimdTransform& t) const;
|
||||
|
||||
SimdTransform operator*(const SimdTransform& t) const;
|
||||
|
||||
private:
|
||||
|
||||
SimdMatrix3x3 m_basis;
|
||||
SimdVector3 m_origin;
|
||||
#ifndef IGNORE_TYPE
|
||||
unsigned int m_type;
|
||||
#endif //IGNORE_TYPE
|
||||
};
|
||||
|
||||
|
||||
SIMD_FORCE_INLINE SimdVector3
|
||||
SimdTransform::invXform(const SimdVector3& inVec) const
|
||||
{
|
||||
SimdVector3 v = inVec - m_origin;
|
||||
return (m_basis.transpose() * v);
|
||||
}
|
||||
|
||||
SIMD_FORCE_INLINE SimdTransform
|
||||
SimdTransform::inverseTimes(const SimdTransform& t) const
|
||||
{
|
||||
SimdVector3 v = t.getOrigin() - m_origin;
|
||||
#ifndef IGNORE_TYPE
|
||||
if (m_type & SCALING)
|
||||
{
|
||||
SimdMatrix3x3 inv = m_basis.inverse();
|
||||
return SimdTransform(inv * t.getBasis(), inv * v,
|
||||
m_type | t.m_type);
|
||||
}
|
||||
else
|
||||
#else
|
||||
{
|
||||
return SimdTransform(m_basis.transposeTimes(t.m_basis),
|
||||
v * m_basis, 0);
|
||||
}
|
||||
#endif //IGNORE_TYPE
|
||||
}
|
||||
|
||||
SIMD_FORCE_INLINE SimdTransform
|
||||
SimdTransform::operator*(const SimdTransform& t) const
|
||||
{
|
||||
return SimdTransform(m_basis * t.m_basis,
|
||||
(*this)(t.m_origin),
|
||||
#ifndef IGNORE_TYPE
|
||||
m_type | t.m_type);
|
||||
#else
|
||||
0);
|
||||
#endif
|
||||
}
|
||||
|
||||
|
||||
|
||||
#endif
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
@@ -1,403 +0,0 @@
|
||||
/*
|
||||
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 SIMD__VECTOR3_H
|
||||
#define SIMD__VECTOR3_H
|
||||
|
||||
#include "SimdQuadWord.h"
|
||||
|
||||
|
||||
///SimdVector3 is 16byte aligned, and has an extra unused component m_w
|
||||
///this extra component can be used by derived classes (Quaternion?) or by user
|
||||
class SimdVector3 : public SimdQuadWord {
|
||||
|
||||
|
||||
public:
|
||||
SIMD_FORCE_INLINE SimdVector3() {}
|
||||
|
||||
|
||||
|
||||
SIMD_FORCE_INLINE SimdVector3(const SimdScalar& x, const SimdScalar& y, const SimdScalar& z)
|
||||
:SimdQuadWord(x,y,z,0.f)
|
||||
{
|
||||
}
|
||||
|
||||
// SIMD_FORCE_INLINE SimdVector3(const SimdScalar& x, const SimdScalar& y, const SimdScalar& z,const SimdScalar& w)
|
||||
// : SimdQuadWord(x,y,z,w)
|
||||
// {
|
||||
// }
|
||||
|
||||
|
||||
|
||||
SIMD_FORCE_INLINE SimdVector3& operator+=(const SimdVector3& v)
|
||||
{
|
||||
m_x += v.x(); m_y += v.y(); m_z += v.z();
|
||||
return *this;
|
||||
}
|
||||
|
||||
|
||||
|
||||
SIMD_FORCE_INLINE SimdVector3& operator-=(const SimdVector3& v)
|
||||
{
|
||||
m_x -= v.x(); m_y -= v.y(); m_z -= v.z();
|
||||
return *this;
|
||||
}
|
||||
|
||||
SIMD_FORCE_INLINE SimdVector3& operator*=(const SimdScalar& s)
|
||||
{
|
||||
m_x *= s; m_y *= s; m_z *= s;
|
||||
return *this;
|
||||
}
|
||||
|
||||
SIMD_FORCE_INLINE SimdVector3& operator/=(const SimdScalar& s)
|
||||
{
|
||||
assert(s != SimdScalar(0.0));
|
||||
return *this *= SimdScalar(1.0) / s;
|
||||
}
|
||||
|
||||
SIMD_FORCE_INLINE SimdScalar dot(const SimdVector3& v) const
|
||||
{
|
||||
return m_x * v.x() + m_y * v.y() + m_z * v.z();
|
||||
}
|
||||
|
||||
SIMD_FORCE_INLINE SimdScalar length2() const
|
||||
{
|
||||
return dot(*this);
|
||||
}
|
||||
|
||||
SIMD_FORCE_INLINE SimdScalar length() const
|
||||
{
|
||||
return SimdSqrt(length2());
|
||||
}
|
||||
|
||||
SIMD_FORCE_INLINE SimdScalar distance2(const SimdVector3& v) const;
|
||||
|
||||
SIMD_FORCE_INLINE SimdScalar distance(const SimdVector3& v) const;
|
||||
|
||||
SIMD_FORCE_INLINE SimdVector3& normalize()
|
||||
{
|
||||
return *this /= length();
|
||||
}
|
||||
|
||||
SIMD_FORCE_INLINE SimdVector3 normalized() const;
|
||||
|
||||
SIMD_FORCE_INLINE SimdVector3 rotate( const SimdVector3& wAxis, const SimdScalar angle );
|
||||
|
||||
SIMD_FORCE_INLINE SimdScalar angle(const SimdVector3& v) const
|
||||
{
|
||||
SimdScalar s = SimdSqrt(length2() * v.length2());
|
||||
assert(s != SimdScalar(0.0));
|
||||
return SimdAcos(dot(v) / s);
|
||||
}
|
||||
|
||||
SIMD_FORCE_INLINE SimdVector3 absolute() const
|
||||
{
|
||||
return SimdVector3(
|
||||
SimdFabs(m_x),
|
||||
SimdFabs(m_y),
|
||||
SimdFabs(m_z));
|
||||
}
|
||||
|
||||
SIMD_FORCE_INLINE SimdVector3 cross(const SimdVector3& v) const
|
||||
{
|
||||
return SimdVector3(
|
||||
m_y * v.z() - m_z * v.y(),
|
||||
m_z * v.x() - m_x * v.z(),
|
||||
m_x * v.y() - m_y * v.x());
|
||||
}
|
||||
|
||||
SIMD_FORCE_INLINE SimdScalar triple(const SimdVector3& v1, const SimdVector3& v2) const
|
||||
{
|
||||
return m_x * (v1.y() * v2.z() - v1.z() * v2.y()) +
|
||||
m_y * (v1.z() * v2.x() - v1.x() * v2.z()) +
|
||||
m_z * (v1.x() * v2.y() - v1.y() * v2.x());
|
||||
}
|
||||
|
||||
SIMD_FORCE_INLINE int minAxis() const
|
||||
{
|
||||
return m_x < m_y ? (m_x < m_z ? 0 : 2) : (m_y < m_z ? 1 : 2);
|
||||
}
|
||||
|
||||
SIMD_FORCE_INLINE int maxAxis() const
|
||||
{
|
||||
return m_x < m_y ? (m_y < m_z ? 2 : 1) : (m_x < m_z ? 2 : 0);
|
||||
}
|
||||
|
||||
SIMD_FORCE_INLINE int furthestAxis() const
|
||||
{
|
||||
return absolute().minAxis();
|
||||
}
|
||||
|
||||
SIMD_FORCE_INLINE int closestAxis() const
|
||||
{
|
||||
return absolute().maxAxis();
|
||||
}
|
||||
|
||||
SIMD_FORCE_INLINE void setInterpolate3(const SimdVector3& v0, const SimdVector3& v1, SimdScalar rt)
|
||||
{
|
||||
SimdScalar s = 1.0f - rt;
|
||||
m_x = s * v0[0] + rt * v1.x();
|
||||
m_y = s * v0[1] + rt * v1.y();
|
||||
m_z = s * v0[2] + rt * v1.z();
|
||||
//don't do the unused w component
|
||||
// m_co[3] = s * v0[3] + rt * v1[3];
|
||||
}
|
||||
|
||||
SIMD_FORCE_INLINE SimdVector3 lerp(const SimdVector3& v, const SimdScalar& t) const
|
||||
{
|
||||
return SimdVector3(m_x + (v.x() - m_x) * t,
|
||||
m_y + (v.y() - m_y) * t,
|
||||
m_z + (v.z() - m_z) * t);
|
||||
}
|
||||
|
||||
|
||||
SIMD_FORCE_INLINE SimdVector3& operator*=(const SimdVector3& v)
|
||||
{
|
||||
m_x *= v.x(); m_y *= v.y(); m_z *= v.z();
|
||||
return *this;
|
||||
}
|
||||
|
||||
|
||||
|
||||
};
|
||||
|
||||
SIMD_FORCE_INLINE SimdVector3
|
||||
operator+(const SimdVector3& v1, const SimdVector3& v2)
|
||||
{
|
||||
return SimdVector3(v1.x() + v2.x(), v1.y() + v2.y(), v1.z() + v2.z());
|
||||
}
|
||||
|
||||
SIMD_FORCE_INLINE SimdVector3
|
||||
operator*(const SimdVector3& v1, const SimdVector3& v2)
|
||||
{
|
||||
return SimdVector3(v1.x() * v2.x(), v1.y() * v2.y(), v1.z() * v2.z());
|
||||
}
|
||||
|
||||
SIMD_FORCE_INLINE SimdVector3
|
||||
operator-(const SimdVector3& v1, const SimdVector3& v2)
|
||||
{
|
||||
return SimdVector3(v1.x() - v2.x(), v1.y() - v2.y(), v1.z() - v2.z());
|
||||
}
|
||||
|
||||
SIMD_FORCE_INLINE SimdVector3
|
||||
operator-(const SimdVector3& v)
|
||||
{
|
||||
return SimdVector3(-v.x(), -v.y(), -v.z());
|
||||
}
|
||||
|
||||
SIMD_FORCE_INLINE SimdVector3
|
||||
operator*(const SimdVector3& v, const SimdScalar& s)
|
||||
{
|
||||
return SimdVector3(v.x() * s, v.y() * s, v.z() * s);
|
||||
}
|
||||
|
||||
SIMD_FORCE_INLINE SimdVector3
|
||||
operator*(const SimdScalar& s, const SimdVector3& v)
|
||||
{
|
||||
return v * s;
|
||||
}
|
||||
|
||||
SIMD_FORCE_INLINE SimdVector3
|
||||
operator/(const SimdVector3& v, const SimdScalar& s)
|
||||
{
|
||||
assert(s != SimdScalar(0.0));
|
||||
return v * (SimdScalar(1.0) / s);
|
||||
}
|
||||
|
||||
SIMD_FORCE_INLINE SimdVector3
|
||||
operator/(const SimdVector3& v1, const SimdVector3& v2)
|
||||
{
|
||||
return SimdVector3(v1.x() / v2.x(),v1.y() / v2.y(),v1.z() / v2.z());
|
||||
}
|
||||
|
||||
SIMD_FORCE_INLINE SimdScalar
|
||||
dot(const SimdVector3& v1, const SimdVector3& v2)
|
||||
{
|
||||
return v1.dot(v2);
|
||||
}
|
||||
|
||||
|
||||
|
||||
SIMD_FORCE_INLINE SimdScalar
|
||||
distance2(const SimdVector3& v1, const SimdVector3& v2)
|
||||
{
|
||||
return v1.distance2(v2);
|
||||
}
|
||||
|
||||
|
||||
SIMD_FORCE_INLINE SimdScalar
|
||||
distance(const SimdVector3& v1, const SimdVector3& v2)
|
||||
{
|
||||
return v1.distance(v2);
|
||||
}
|
||||
|
||||
SIMD_FORCE_INLINE SimdScalar
|
||||
angle(const SimdVector3& v1, const SimdVector3& v2)
|
||||
{
|
||||
return v1.angle(v2);
|
||||
}
|
||||
|
||||
SIMD_FORCE_INLINE SimdVector3
|
||||
cross(const SimdVector3& v1, const SimdVector3& v2)
|
||||
{
|
||||
return v1.cross(v2);
|
||||
}
|
||||
|
||||
SIMD_FORCE_INLINE SimdScalar
|
||||
triple(const SimdVector3& v1, const SimdVector3& v2, const SimdVector3& v3)
|
||||
{
|
||||
return v1.triple(v2, v3);
|
||||
}
|
||||
|
||||
SIMD_FORCE_INLINE SimdVector3
|
||||
lerp(const SimdVector3& v1, const SimdVector3& v2, const SimdScalar& t)
|
||||
{
|
||||
return v1.lerp(v2, t);
|
||||
}
|
||||
|
||||
|
||||
SIMD_FORCE_INLINE bool operator==(const SimdVector3& p1, const SimdVector3& p2)
|
||||
{
|
||||
return p1[0] == p2[0] && p1[1] == p2[1] && p1[2] == p2[2];
|
||||
}
|
||||
|
||||
SIMD_FORCE_INLINE SimdScalar SimdVector3::distance2(const SimdVector3& v) const
|
||||
{
|
||||
return (v - *this).length2();
|
||||
}
|
||||
|
||||
SIMD_FORCE_INLINE SimdScalar SimdVector3::distance(const SimdVector3& v) const
|
||||
{
|
||||
return (v - *this).length();
|
||||
}
|
||||
|
||||
SIMD_FORCE_INLINE SimdVector3 SimdVector3::normalized() const
|
||||
{
|
||||
return *this / length();
|
||||
}
|
||||
|
||||
SIMD_FORCE_INLINE SimdVector3 SimdVector3::rotate( const SimdVector3& wAxis, const SimdScalar angle )
|
||||
{
|
||||
// wAxis must be a unit lenght vector
|
||||
|
||||
SimdVector3 o = wAxis * wAxis.dot( *this );
|
||||
SimdVector3 x = *this - o;
|
||||
SimdVector3 y;
|
||||
|
||||
y = wAxis.cross( *this );
|
||||
|
||||
return ( o + x * SimdCos( angle ) + y * SimdSin( angle ) );
|
||||
}
|
||||
|
||||
class SimdVector4 : public SimdVector3
|
||||
{
|
||||
public:
|
||||
|
||||
SIMD_FORCE_INLINE SimdVector4() {}
|
||||
|
||||
|
||||
SIMD_FORCE_INLINE SimdVector4(const SimdScalar& x, const SimdScalar& y, const SimdScalar& z,const SimdScalar& w)
|
||||
: SimdVector3(x,y,z)
|
||||
{
|
||||
m_unusedW = w;
|
||||
}
|
||||
|
||||
|
||||
SIMD_FORCE_INLINE SimdVector4 absolute4() const
|
||||
{
|
||||
return SimdVector4(
|
||||
SimdFabs(m_x),
|
||||
SimdFabs(m_y),
|
||||
SimdFabs(m_z),
|
||||
SimdFabs(m_unusedW));
|
||||
}
|
||||
|
||||
|
||||
|
||||
float getW() const { return m_unusedW;}
|
||||
|
||||
|
||||
SIMD_FORCE_INLINE int maxAxis4() const
|
||||
{
|
||||
int maxIndex = -1;
|
||||
float maxVal = -1e30f;
|
||||
if (m_x > maxVal)
|
||||
{
|
||||
maxIndex = 0;
|
||||
maxVal = m_x;
|
||||
}
|
||||
if (m_y > maxVal)
|
||||
{
|
||||
maxIndex = 1;
|
||||
maxVal = m_y;
|
||||
}
|
||||
if (m_z > maxVal)
|
||||
{
|
||||
maxIndex = 2;
|
||||
maxVal = m_z;
|
||||
}
|
||||
if (m_unusedW > maxVal)
|
||||
{
|
||||
maxIndex = 3;
|
||||
maxVal = m_unusedW;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
return maxIndex;
|
||||
|
||||
}
|
||||
|
||||
|
||||
SIMD_FORCE_INLINE int minAxis4() const
|
||||
{
|
||||
int minIndex = -1;
|
||||
float minVal = 1e30f;
|
||||
if (m_x < minVal)
|
||||
{
|
||||
minIndex = 0;
|
||||
minVal = m_x;
|
||||
}
|
||||
if (m_y < minVal)
|
||||
{
|
||||
minIndex = 1;
|
||||
minVal = m_y;
|
||||
}
|
||||
if (m_z < minVal)
|
||||
{
|
||||
minIndex = 2;
|
||||
minVal = m_z;
|
||||
}
|
||||
if (m_unusedW < minVal)
|
||||
{
|
||||
minIndex = 3;
|
||||
minVal = m_unusedW;
|
||||
}
|
||||
|
||||
return minIndex;
|
||||
|
||||
}
|
||||
|
||||
|
||||
SIMD_FORCE_INLINE int closestAxis4() const
|
||||
{
|
||||
return absolute4().maxAxis4();
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
#endif //SIMD__VECTOR3_H
|
||||
@@ -17,15 +17,15 @@ subject to the following restrictions:
|
||||
#ifndef AABB_UTIL2
|
||||
#define AABB_UTIL2
|
||||
|
||||
#include "LinearMath/SimdVector3.h"
|
||||
#include "LinearMath/btVector3.h"
|
||||
|
||||
#define SimdMin(a,b) ((a < b ? a : b))
|
||||
#define SimdMax(a,b) ((a > b ? a : b))
|
||||
#define btMin(a,b) ((a < b ? a : b))
|
||||
#define btMax(a,b) ((a > b ? a : b))
|
||||
|
||||
|
||||
/// conservative test for overlap between two aabbs
|
||||
SIMD_FORCE_INLINE bool TestAabbAgainstAabb2(const SimdVector3 &aabbMin1, const SimdVector3 &aabbMax1,
|
||||
const SimdVector3 &aabbMin2, const SimdVector3 &aabbMax2)
|
||||
SIMD_FORCE_INLINE bool TestAabbAgainstAabb2(const btVector3 &aabbMin1, const btVector3 &aabbMax1,
|
||||
const btVector3 &aabbMin2, const btVector3 &aabbMax2)
|
||||
{
|
||||
bool overlap = true;
|
||||
overlap = (aabbMin1[0] > aabbMax2[0] || aabbMax1[0] < aabbMin2[0]) ? false : overlap;
|
||||
@@ -35,21 +35,21 @@ SIMD_FORCE_INLINE bool TestAabbAgainstAabb2(const SimdVector3 &aabbMin1, const S
|
||||
}
|
||||
|
||||
/// conservative test for overlap between triangle and aabb
|
||||
SIMD_FORCE_INLINE bool TestTriangleAgainstAabb2(const SimdVector3 *vertices,
|
||||
const SimdVector3 &aabbMin, const SimdVector3 &aabbMax)
|
||||
SIMD_FORCE_INLINE bool TestTriangleAgainstAabb2(const btVector3 *vertices,
|
||||
const btVector3 &aabbMin, const btVector3 &aabbMax)
|
||||
{
|
||||
const SimdVector3 &p1 = vertices[0];
|
||||
const SimdVector3 &p2 = vertices[1];
|
||||
const SimdVector3 &p3 = vertices[2];
|
||||
const btVector3 &p1 = vertices[0];
|
||||
const btVector3 &p2 = vertices[1];
|
||||
const btVector3 &p3 = vertices[2];
|
||||
|
||||
if (SimdMin(SimdMin(p1[0], p2[0]), p3[0]) > aabbMax[0]) return false;
|
||||
if (SimdMax(SimdMax(p1[0], p2[0]), p3[0]) < aabbMin[0]) return false;
|
||||
if (btMin(btMin(p1[0], p2[0]), p3[0]) > aabbMax[0]) return false;
|
||||
if (btMax(btMax(p1[0], p2[0]), p3[0]) < aabbMin[0]) return false;
|
||||
|
||||
if (SimdMin(SimdMin(p1[2], p2[2]), p3[2]) > aabbMax[2]) return false;
|
||||
if (SimdMax(SimdMax(p1[2], p2[2]), p3[2]) < aabbMin[2]) return false;
|
||||
if (btMin(btMin(p1[2], p2[2]), p3[2]) > aabbMax[2]) return false;
|
||||
if (btMax(btMax(p1[2], p2[2]), p3[2]) < aabbMin[2]) return false;
|
||||
|
||||
if (SimdMin(SimdMin(p1[1], p2[1]), p3[1]) > aabbMax[1]) return false;
|
||||
if (SimdMax(SimdMax(p1[1], p2[1]), p3[1]) < aabbMin[1]) return false;
|
||||
if (btMin(btMin(p1[1], p2[1]), p3[1]) > aabbMax[1]) return false;
|
||||
if (btMax(btMax(p1[1], p2[1]), p3[1]) < aabbMin[1]) return false;
|
||||
return true;
|
||||
}
|
||||
|
||||
@@ -28,10 +28,10 @@ DEALINGS IN THE SOFTWARE.
|
||||
#ifndef IDEBUG_DRAW__H
|
||||
#define IDEBUG_DRAW__H
|
||||
|
||||
#include "LinearMath/SimdVector3.h"
|
||||
#include "LinearMath/btVector3.h"
|
||||
|
||||
|
||||
class IDebugDraw
|
||||
class btIDebugDraw
|
||||
{
|
||||
public:
|
||||
|
||||
@@ -52,11 +52,11 @@ class IDebugDraw
|
||||
DBG_MAX_DEBUG_DRAW_MODE
|
||||
};
|
||||
|
||||
virtual ~IDebugDraw() {};
|
||||
virtual ~btIDebugDraw() {};
|
||||
|
||||
virtual void DrawLine(const SimdVector3& from,const SimdVector3& to,const SimdVector3& color)=0;
|
||||
virtual void DrawLine(const btVector3& from,const btVector3& to,const btVector3& color)=0;
|
||||
|
||||
virtual void DrawContactPoint(const SimdVector3& PointOnB,const SimdVector3& normalOnB,float distance,int lifeTime,const SimdVector3& color)=0;
|
||||
virtual void DrawContactPoint(const btVector3& PointOnB,const btVector3& normalOnB,float distance,int lifeTime,const btVector3& color)=0;
|
||||
|
||||
virtual void SetDebugMode(int debugMode) =0;
|
||||
|
||||
@@ -17,25 +17,25 @@ subject to the following restrictions:
|
||||
#ifndef GEN_LIST_H
|
||||
#define GEN_LIST_H
|
||||
|
||||
class GEN_Link {
|
||||
class btGEN_Link {
|
||||
public:
|
||||
GEN_Link() : m_next(0), m_prev(0) {}
|
||||
GEN_Link(GEN_Link *next, GEN_Link *prev) : m_next(next), m_prev(prev) {}
|
||||
btGEN_Link() : m_next(0), m_prev(0) {}
|
||||
btGEN_Link(btGEN_Link *next, btGEN_Link *prev) : m_next(next), m_prev(prev) {}
|
||||
|
||||
GEN_Link *getNext() const { return m_next; }
|
||||
GEN_Link *getPrev() const { return m_prev; }
|
||||
btGEN_Link *getNext() const { return m_next; }
|
||||
btGEN_Link *getPrev() const { return m_prev; }
|
||||
|
||||
bool isHead() const { return m_prev == 0; }
|
||||
bool isTail() const { return m_next == 0; }
|
||||
|
||||
void insertBefore(GEN_Link *link) {
|
||||
void insertBefore(btGEN_Link *link) {
|
||||
m_next = link;
|
||||
m_prev = link->m_prev;
|
||||
m_next->m_prev = this;
|
||||
m_prev->m_next = this;
|
||||
}
|
||||
|
||||
void insertAfter(GEN_Link *link) {
|
||||
void insertAfter(btGEN_Link *link) {
|
||||
m_next = link->m_next;
|
||||
m_prev = link;
|
||||
m_next->m_prev = this;
|
||||
@@ -48,23 +48,23 @@ public:
|
||||
}
|
||||
|
||||
private:
|
||||
GEN_Link *m_next;
|
||||
GEN_Link *m_prev;
|
||||
btGEN_Link *m_next;
|
||||
btGEN_Link *m_prev;
|
||||
};
|
||||
|
||||
class GEN_List {
|
||||
class btGEN_List {
|
||||
public:
|
||||
GEN_List() : m_head(&m_tail, 0), m_tail(0, &m_head) {}
|
||||
btGEN_List() : m_head(&m_tail, 0), m_tail(0, &m_head) {}
|
||||
|
||||
GEN_Link *getHead() const { return m_head.getNext(); }
|
||||
GEN_Link *getTail() const { return m_tail.getPrev(); }
|
||||
btGEN_Link *getHead() const { return m_head.getNext(); }
|
||||
btGEN_Link *getTail() const { return m_tail.getPrev(); }
|
||||
|
||||
void addHead(GEN_Link *link) { link->insertAfter(&m_head); }
|
||||
void addTail(GEN_Link *link) { link->insertBefore(&m_tail); }
|
||||
void addHead(btGEN_Link *link) { link->insertAfter(&m_head); }
|
||||
void addTail(btGEN_Link *link) { link->insertBefore(&m_tail); }
|
||||
|
||||
private:
|
||||
GEN_Link m_head;
|
||||
GEN_Link m_tail;
|
||||
btGEN_Link m_head;
|
||||
btGEN_Link m_tail;
|
||||
};
|
||||
|
||||
#endif
|
||||
395
src/LinearMath/btMatrix3x3.h
Normal file
395
src/LinearMath/btMatrix3x3.h
Normal file
@@ -0,0 +1,395 @@
|
||||
/*
|
||||
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 btMatrix3x3_H
|
||||
#define btMatrix3x3_H
|
||||
|
||||
#include "LinearMath/btScalar.h"
|
||||
|
||||
#include "LinearMath/btVector3.h"
|
||||
#include "LinearMath/btQuaternion.h"
|
||||
|
||||
|
||||
class btMatrix3x3 {
|
||||
public:
|
||||
btMatrix3x3 () {}
|
||||
|
||||
// explicit btMatrix3x3(const btScalar *m) { setFromOpenGLSubMatrix(m); }
|
||||
|
||||
explicit btMatrix3x3(const btQuaternion& q) { setRotation(q); }
|
||||
/*
|
||||
template <typename btScalar>
|
||||
Matrix3x3(const btScalar& yaw, const btScalar& pitch, const btScalar& roll)
|
||||
{
|
||||
setEulerYPR(yaw, pitch, roll);
|
||||
}
|
||||
*/
|
||||
btMatrix3x3(const btScalar& xx, const btScalar& xy, const btScalar& xz,
|
||||
const btScalar& yx, const btScalar& yy, const btScalar& yz,
|
||||
const btScalar& zx, const btScalar& zy, const btScalar& zz)
|
||||
{
|
||||
setValue(xx, xy, xz,
|
||||
yx, yy, yz,
|
||||
zx, zy, zz);
|
||||
}
|
||||
|
||||
btVector3 getColumn(int i) const
|
||||
{
|
||||
return btVector3(m_el[0][i],m_el[1][i],m_el[2][i]);
|
||||
}
|
||||
|
||||
const btVector3& getRow(int i) const
|
||||
{
|
||||
return m_el[i];
|
||||
}
|
||||
|
||||
|
||||
SIMD_FORCE_INLINE btVector3& operator[](int i)
|
||||
{
|
||||
assert(0 <= i && i < 3);
|
||||
return m_el[i];
|
||||
}
|
||||
|
||||
const btVector3& operator[](int i) const
|
||||
{
|
||||
assert(0 <= i && i < 3);
|
||||
return m_el[i];
|
||||
}
|
||||
|
||||
btMatrix3x3& operator*=(const btMatrix3x3& m);
|
||||
|
||||
|
||||
void setFromOpenGLSubMatrix(const btScalar *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 btScalar& xx, const btScalar& xy, const btScalar& xz,
|
||||
const btScalar& yx, const btScalar& yy, const btScalar& yz,
|
||||
const btScalar& zx, const btScalar& zy, const btScalar& zz)
|
||||
{
|
||||
m_el[0][0] = btScalar(xx);
|
||||
m_el[0][1] = btScalar(xy);
|
||||
m_el[0][2] = btScalar(xz);
|
||||
m_el[1][0] = btScalar(yx);
|
||||
m_el[1][1] = btScalar(yy);
|
||||
m_el[1][2] = btScalar(yz);
|
||||
m_el[2][0] = btScalar(zx);
|
||||
m_el[2][1] = btScalar(zy);
|
||||
m_el[2][2] = btScalar(zz);
|
||||
}
|
||||
|
||||
void setRotation(const btQuaternion& q)
|
||||
{
|
||||
btScalar d = q.length2();
|
||||
assert(d != btScalar(0.0));
|
||||
btScalar s = btScalar(2.0) / d;
|
||||
btScalar xs = q[0] * s, ys = q[1] * s, zs = q[2] * s;
|
||||
btScalar wx = q[3] * xs, wy = q[3] * ys, wz = q[3] * zs;
|
||||
btScalar xx = q[0] * xs, xy = q[0] * ys, xz = q[0] * zs;
|
||||
btScalar yy = q[1] * ys, yz = q[1] * zs, zz = q[2] * zs;
|
||||
setValue(btScalar(1.0) - (yy + zz), xy - wz, xz + wy,
|
||||
xy + wz, btScalar(1.0) - (xx + zz), yz - wx,
|
||||
xz - wy, yz + wx, btScalar(1.0) - (xx + yy));
|
||||
}
|
||||
|
||||
|
||||
|
||||
void setEulerYPR(const btScalar& yaw, const btScalar& pitch, const btScalar& roll)
|
||||
{
|
||||
|
||||
btScalar cy(btCos(yaw));
|
||||
btScalar sy(btSin(yaw));
|
||||
btScalar cp(btCos(pitch));
|
||||
btScalar sp(btSin(pitch));
|
||||
btScalar cr(btCos(roll));
|
||||
btScalar sr(btSin(roll));
|
||||
btScalar cc = cy * cr;
|
||||
btScalar cs = cy * sr;
|
||||
btScalar sc = sy * cr;
|
||||
btScalar 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 btVector3 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(btScalar eulerX,btScalar eulerY,btScalar eulerZ) {
|
||||
btScalar ci ( btCos(eulerX));
|
||||
btScalar cj ( btCos(eulerY));
|
||||
btScalar ch ( btCos(eulerZ));
|
||||
btScalar si ( btSin(eulerX));
|
||||
btScalar sj ( btSin(eulerY));
|
||||
btScalar sh ( btSin(eulerZ));
|
||||
btScalar cc = ci * ch;
|
||||
btScalar cs = ci * sh;
|
||||
btScalar sc = si * ch;
|
||||
btScalar 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(btScalar(1.0), btScalar(0.0), btScalar(0.0),
|
||||
btScalar(0.0), btScalar(1.0), btScalar(0.0),
|
||||
btScalar(0.0), btScalar(0.0), btScalar(1.0));
|
||||
}
|
||||
|
||||
void getOpenGLSubMatrix(btScalar *m) const
|
||||
{
|
||||
m[0] = btScalar(m_el[0][0]);
|
||||
m[1] = btScalar(m_el[1][0]);
|
||||
m[2] = btScalar(m_el[2][0]);
|
||||
m[3] = btScalar(0.0);
|
||||
m[4] = btScalar(m_el[0][1]);
|
||||
m[5] = btScalar(m_el[1][1]);
|
||||
m[6] = btScalar(m_el[2][1]);
|
||||
m[7] = btScalar(0.0);
|
||||
m[8] = btScalar(m_el[0][2]);
|
||||
m[9] = btScalar(m_el[1][2]);
|
||||
m[10] = btScalar(m_el[2][2]);
|
||||
m[11] = btScalar(0.0);
|
||||
}
|
||||
|
||||
void getRotation(btQuaternion& q) const
|
||||
{
|
||||
btScalar trace = m_el[0][0] + m_el[1][1] + m_el[2][2];
|
||||
|
||||
if (trace > btScalar(0.0))
|
||||
{
|
||||
btScalar s = btSqrt(trace + btScalar(1.0));
|
||||
q[3] = s * btScalar(0.5);
|
||||
s = btScalar(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;
|
||||
|
||||
btScalar s = btSqrt(m_el[i][i] - m_el[j][j] - m_el[k][k] + btScalar(1.0));
|
||||
q[i] = s * btScalar(0.5);
|
||||
s = btScalar(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(btScalar& yaw, btScalar& pitch, btScalar& roll) const
|
||||
{
|
||||
pitch = btScalar(btAsin(-m_el[2][0]));
|
||||
if (pitch < SIMD_2_PI)
|
||||
{
|
||||
if (pitch > SIMD_2_PI)
|
||||
{
|
||||
yaw = btScalar(btAtan2(m_el[1][0], m_el[0][0]));
|
||||
roll = btScalar(btAtan2(m_el[2][1], m_el[2][2]));
|
||||
}
|
||||
else
|
||||
{
|
||||
yaw = btScalar(-btAtan2(-m_el[0][1], m_el[0][2]));
|
||||
roll = btScalar(0.0);
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
yaw = btScalar(btAtan2(-m_el[0][1], m_el[0][2]));
|
||||
roll = btScalar(0.0);
|
||||
}
|
||||
}
|
||||
|
||||
btVector3 getScaling() const
|
||||
{
|
||||
return btVector3(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]);
|
||||
}
|
||||
|
||||
|
||||
btMatrix3x3 scaled(const btVector3& s) const
|
||||
{
|
||||
return btMatrix3x3(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]);
|
||||
}
|
||||
|
||||
btScalar determinant() const;
|
||||
btMatrix3x3 adjoint() const;
|
||||
btMatrix3x3 absolute() const;
|
||||
btMatrix3x3 transpose() const;
|
||||
btMatrix3x3 inverse() const;
|
||||
|
||||
btMatrix3x3 transposeTimes(const btMatrix3x3& m) const;
|
||||
btMatrix3x3 timesTranspose(const btMatrix3x3& m) const;
|
||||
|
||||
btScalar tdot(int c, const btVector3& v) const
|
||||
{
|
||||
return m_el[0][c] * v[0] + m_el[1][c] * v[1] + m_el[2][c] * v[2];
|
||||
}
|
||||
|
||||
protected:
|
||||
btScalar 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];
|
||||
}
|
||||
|
||||
btVector3 m_el[3];
|
||||
};
|
||||
|
||||
SIMD_FORCE_INLINE btMatrix3x3&
|
||||
btMatrix3x3::operator*=(const btMatrix3x3& 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 btScalar
|
||||
btMatrix3x3::determinant() const
|
||||
{
|
||||
return triple((*this)[0], (*this)[1], (*this)[2]);
|
||||
}
|
||||
|
||||
|
||||
SIMD_FORCE_INLINE btMatrix3x3
|
||||
btMatrix3x3::absolute() const
|
||||
{
|
||||
return btMatrix3x3(
|
||||
btFabs(m_el[0][0]), btFabs(m_el[0][1]), btFabs(m_el[0][2]),
|
||||
btFabs(m_el[1][0]), btFabs(m_el[1][1]), btFabs(m_el[1][2]),
|
||||
btFabs(m_el[2][0]), btFabs(m_el[2][1]), btFabs(m_el[2][2]));
|
||||
}
|
||||
|
||||
SIMD_FORCE_INLINE btMatrix3x3
|
||||
btMatrix3x3::transpose() const
|
||||
{
|
||||
return btMatrix3x3(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 btMatrix3x3
|
||||
btMatrix3x3::adjoint() const
|
||||
{
|
||||
return btMatrix3x3(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 btMatrix3x3
|
||||
btMatrix3x3::inverse() const
|
||||
{
|
||||
btVector3 co(cofac(1, 1, 2, 2), cofac(1, 2, 2, 0), cofac(1, 0, 2, 1));
|
||||
btScalar det = (*this)[0].dot(co);
|
||||
assert(det != btScalar(0.0f));
|
||||
btScalar s = btScalar(1.0f) / det;
|
||||
return btMatrix3x3(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 btMatrix3x3
|
||||
btMatrix3x3::transposeTimes(const btMatrix3x3& m) const
|
||||
{
|
||||
return btMatrix3x3(
|
||||
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 btMatrix3x3
|
||||
btMatrix3x3::timesTranspose(const btMatrix3x3& m) const
|
||||
{
|
||||
return btMatrix3x3(
|
||||
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 btVector3
|
||||
operator*(const btMatrix3x3& m, const btVector3& v)
|
||||
{
|
||||
return btVector3(m[0].dot(v), m[1].dot(v), m[2].dot(v));
|
||||
}
|
||||
|
||||
|
||||
SIMD_FORCE_INLINE btVector3
|
||||
operator*(const btVector3& v, const btMatrix3x3& m)
|
||||
{
|
||||
return btVector3(m.tdot(0, v), m.tdot(1, v), m.tdot(2, v));
|
||||
}
|
||||
|
||||
SIMD_FORCE_INLINE btMatrix3x3
|
||||
operator*(const btMatrix3x3& m1, const btMatrix3x3& m2)
|
||||
{
|
||||
return btMatrix3x3(
|
||||
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 btMatrix3x3 btMultTransposeLeft(const btMatrix3x3& m1, const btMatrix3x3& m2) {
|
||||
return btMatrix3x3(
|
||||
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
|
||||
@@ -14,11 +14,11 @@ subject to the following restrictions:
|
||||
|
||||
|
||||
|
||||
#ifndef SimdPoint3_H
|
||||
#define SimdPoint3_H
|
||||
#ifndef btPoint3_H
|
||||
#define btPoint3_H
|
||||
|
||||
#include "LinearMath/SimdVector3.h"
|
||||
#include "LinearMath/btVector3.h"
|
||||
|
||||
typedef SimdVector3 SimdPoint3;
|
||||
typedef btVector3 btPoint3;
|
||||
|
||||
#endif
|
||||
@@ -16,30 +16,30 @@ subject to the following restrictions:
|
||||
#ifndef SIMD_QUADWORD_H
|
||||
#define SIMD_QUADWORD_H
|
||||
|
||||
#include "LinearMath/SimdScalar.h"
|
||||
#include "LinearMath/btScalar.h"
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
class SimdQuadWord
|
||||
class btQuadWord
|
||||
{
|
||||
protected:
|
||||
ATTRIBUTE_ALIGNED16 (SimdScalar m_x);
|
||||
SimdScalar m_y;
|
||||
SimdScalar m_z;
|
||||
SimdScalar m_unusedW;
|
||||
ATTRIBUTE_ALIGNED16 (btScalar m_x);
|
||||
btScalar m_y;
|
||||
btScalar m_z;
|
||||
btScalar m_unusedW;
|
||||
|
||||
public:
|
||||
|
||||
SIMD_FORCE_INLINE SimdScalar& operator[](int i) { return (&m_x)[i]; }
|
||||
SIMD_FORCE_INLINE const SimdScalar& operator[](int i) const { return (&m_x)[i]; }
|
||||
SIMD_FORCE_INLINE btScalar& operator[](int i) { return (&m_x)[i]; }
|
||||
SIMD_FORCE_INLINE const btScalar& operator[](int i) const { return (&m_x)[i]; }
|
||||
|
||||
SIMD_FORCE_INLINE const SimdScalar& getX() const { return m_x; }
|
||||
SIMD_FORCE_INLINE const btScalar& getX() const { return m_x; }
|
||||
|
||||
SIMD_FORCE_INLINE const SimdScalar& getY() const { return m_y; }
|
||||
SIMD_FORCE_INLINE const btScalar& getY() const { return m_y; }
|
||||
|
||||
SIMD_FORCE_INLINE const SimdScalar& getZ() const { return m_z; }
|
||||
SIMD_FORCE_INLINE const btScalar& getZ() const { return m_z; }
|
||||
|
||||
SIMD_FORCE_INLINE void setX(float x) { m_x = x;};
|
||||
|
||||
@@ -47,31 +47,31 @@ class SimdQuadWord
|
||||
|
||||
SIMD_FORCE_INLINE void setZ(float z) { m_z = z;};
|
||||
|
||||
SIMD_FORCE_INLINE const SimdScalar& x() const { return m_x; }
|
||||
SIMD_FORCE_INLINE const btScalar& x() const { return m_x; }
|
||||
|
||||
SIMD_FORCE_INLINE const SimdScalar& y() const { return m_y; }
|
||||
SIMD_FORCE_INLINE const btScalar& y() const { return m_y; }
|
||||
|
||||
SIMD_FORCE_INLINE const SimdScalar& z() const { return m_z; }
|
||||
SIMD_FORCE_INLINE const btScalar& z() const { return m_z; }
|
||||
|
||||
|
||||
operator SimdScalar *() { return &m_x; }
|
||||
operator const SimdScalar *() const { return &m_x; }
|
||||
operator btScalar *() { return &m_x; }
|
||||
operator const btScalar *() const { return &m_x; }
|
||||
|
||||
SIMD_FORCE_INLINE void setValue(const SimdScalar& x, const SimdScalar& y, const SimdScalar& z)
|
||||
SIMD_FORCE_INLINE void setValue(const btScalar& x, const btScalar& y, const btScalar& z)
|
||||
{
|
||||
m_x=x;
|
||||
m_y=y;
|
||||
m_z=z;
|
||||
}
|
||||
|
||||
/* void getValue(SimdScalar *m) const
|
||||
/* void getValue(btScalar *m) const
|
||||
{
|
||||
m[0] = m_x;
|
||||
m[1] = m_y;
|
||||
m[2] = m_z;
|
||||
}
|
||||
*/
|
||||
SIMD_FORCE_INLINE void setValue(const SimdScalar& x, const SimdScalar& y, const SimdScalar& z,const SimdScalar& w)
|
||||
SIMD_FORCE_INLINE void setValue(const btScalar& x, const btScalar& y, const btScalar& z,const btScalar& w)
|
||||
{
|
||||
m_x=x;
|
||||
m_y=y;
|
||||
@@ -79,25 +79,25 @@ class SimdQuadWord
|
||||
m_unusedW=w;
|
||||
}
|
||||
|
||||
SIMD_FORCE_INLINE SimdQuadWord() :
|
||||
SIMD_FORCE_INLINE btQuadWord() :
|
||||
m_x(0.f),m_y(0.f),m_z(0.f),m_unusedW(0.f)
|
||||
{
|
||||
}
|
||||
|
||||
SIMD_FORCE_INLINE SimdQuadWord(const SimdScalar& x, const SimdScalar& y, const SimdScalar& z)
|
||||
SIMD_FORCE_INLINE btQuadWord(const btScalar& x, const btScalar& y, const btScalar& z)
|
||||
:m_x(x),m_y(y),m_z(z)
|
||||
//todo, remove this in release/simd ?
|
||||
,m_unusedW(0.f)
|
||||
{
|
||||
}
|
||||
|
||||
SIMD_FORCE_INLINE SimdQuadWord(const SimdScalar& x, const SimdScalar& y, const SimdScalar& z,const SimdScalar& w)
|
||||
SIMD_FORCE_INLINE btQuadWord(const btScalar& x, const btScalar& y, const btScalar& z,const btScalar& w)
|
||||
:m_x(x),m_y(y),m_z(z),m_unusedW(w)
|
||||
{
|
||||
}
|
||||
|
||||
|
||||
SIMD_FORCE_INLINE void setMax(const SimdQuadWord& other)
|
||||
SIMD_FORCE_INLINE void setMax(const btQuadWord& other)
|
||||
{
|
||||
if (other.m_x > m_x)
|
||||
m_x = other.m_x;
|
||||
@@ -112,7 +112,7 @@ class SimdQuadWord
|
||||
m_unusedW = other.m_unusedW;
|
||||
}
|
||||
|
||||
SIMD_FORCE_INLINE void setMin(const SimdQuadWord& other)
|
||||
SIMD_FORCE_INLINE void setMin(const btQuadWord& other)
|
||||
{
|
||||
if (other.m_x < m_x)
|
||||
m_x = other.m_x;
|
||||
290
src/LinearMath/btQuaternion.h
Normal file
290
src/LinearMath/btQuaternion.h
Normal file
@@ -0,0 +1,290 @@
|
||||
/*
|
||||
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 SIMD__QUATERNION_H_
|
||||
#define SIMD__QUATERNION_H_
|
||||
|
||||
#include "LinearMath/btVector3.h"
|
||||
|
||||
class btQuaternion : public btQuadWord {
|
||||
public:
|
||||
btQuaternion() {}
|
||||
|
||||
// template <typename btScalar>
|
||||
// explicit Quaternion(const btScalar *v) : Tuple4<btScalar>(v) {}
|
||||
|
||||
btQuaternion(const btScalar& x, const btScalar& y, const btScalar& z, const btScalar& w)
|
||||
: btQuadWord(x, y, z, w)
|
||||
{}
|
||||
|
||||
btQuaternion(const btVector3& axis, const btScalar& angle)
|
||||
{
|
||||
setRotation(axis, angle);
|
||||
}
|
||||
|
||||
btQuaternion(const btScalar& yaw, const btScalar& pitch, const btScalar& roll)
|
||||
{
|
||||
setEuler(yaw, pitch, roll);
|
||||
}
|
||||
|
||||
void setRotation(const btVector3& axis, const btScalar& angle)
|
||||
{
|
||||
btScalar d = axis.length();
|
||||
assert(d != btScalar(0.0));
|
||||
btScalar s = btSin(angle * btScalar(0.5)) / d;
|
||||
setValue(axis.x() * s, axis.y() * s, axis.z() * s,
|
||||
btCos(angle * btScalar(0.5)));
|
||||
}
|
||||
|
||||
void setEuler(const btScalar& yaw, const btScalar& pitch, const btScalar& roll)
|
||||
{
|
||||
btScalar halfYaw = btScalar(yaw) * btScalar(0.5);
|
||||
btScalar halfPitch = btScalar(pitch) * btScalar(0.5);
|
||||
btScalar halfRoll = btScalar(roll) * btScalar(0.5);
|
||||
btScalar cosYaw = btCos(halfYaw);
|
||||
btScalar sinYaw = btSin(halfYaw);
|
||||
btScalar cosPitch = btCos(halfPitch);
|
||||
btScalar sinPitch = btSin(halfPitch);
|
||||
btScalar cosRoll = btCos(halfRoll);
|
||||
btScalar sinRoll = btSin(halfRoll);
|
||||
setValue(cosRoll * sinPitch * cosYaw + sinRoll * cosPitch * sinYaw,
|
||||
cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw,
|
||||
sinRoll * cosPitch * cosYaw - cosRoll * sinPitch * sinYaw,
|
||||
cosRoll * cosPitch * cosYaw + sinRoll * sinPitch * sinYaw);
|
||||
}
|
||||
|
||||
btQuaternion& operator+=(const btQuaternion& q)
|
||||
{
|
||||
m_x += q.x(); m_y += q.y(); m_z += q.z(); m_unusedW += q[3];
|
||||
return *this;
|
||||
}
|
||||
|
||||
btQuaternion& operator-=(const btQuaternion& q)
|
||||
{
|
||||
m_x -= q.x(); m_y -= q.y(); m_z -= q.z(); m_unusedW -= q[3];
|
||||
return *this;
|
||||
}
|
||||
|
||||
btQuaternion& operator*=(const btScalar& s)
|
||||
{
|
||||
m_x *= s; m_y *= s; m_z *= s; m_unusedW *= s;
|
||||
return *this;
|
||||
}
|
||||
|
||||
|
||||
btQuaternion& operator*=(const btQuaternion& q)
|
||||
{
|
||||
setValue(m_unusedW * q.x() + m_x * q[3] + m_y * q.z() - m_z * q.y(),
|
||||
m_unusedW * q.y() + m_y * q[3] + m_z * q.x() - m_x * q.z(),
|
||||
m_unusedW * q.z() + m_z * q[3] + m_x * q.y() - m_y * q.x(),
|
||||
m_unusedW * q[3] - m_x * q.x() - m_y * q.y() - m_z * q.z());
|
||||
return *this;
|
||||
}
|
||||
|
||||
btScalar dot(const btQuaternion& q) const
|
||||
{
|
||||
return m_x * q.x() + m_y * q.y() + m_z * q.z() + m_unusedW * q[3];
|
||||
}
|
||||
|
||||
btScalar length2() const
|
||||
{
|
||||
return dot(*this);
|
||||
}
|
||||
|
||||
btScalar length() const
|
||||
{
|
||||
return btSqrt(length2());
|
||||
}
|
||||
|
||||
btQuaternion& normalize()
|
||||
{
|
||||
return *this /= length();
|
||||
}
|
||||
|
||||
SIMD_FORCE_INLINE btQuaternion
|
||||
operator*(const btScalar& s) const
|
||||
{
|
||||
return btQuaternion(x() * s, y() * s, z() * s, m_unusedW * s);
|
||||
}
|
||||
|
||||
|
||||
|
||||
btQuaternion operator/(const btScalar& s) const
|
||||
{
|
||||
assert(s != btScalar(0.0));
|
||||
return *this * (btScalar(1.0) / s);
|
||||
}
|
||||
|
||||
|
||||
btQuaternion& operator/=(const btScalar& s)
|
||||
{
|
||||
assert(s != btScalar(0.0));
|
||||
return *this *= btScalar(1.0) / s;
|
||||
}
|
||||
|
||||
|
||||
btQuaternion normalized() const
|
||||
{
|
||||
return *this / length();
|
||||
}
|
||||
|
||||
btScalar angle(const btQuaternion& q) const
|
||||
{
|
||||
btScalar s = btSqrt(length2() * q.length2());
|
||||
assert(s != btScalar(0.0));
|
||||
return btAcos(dot(q) / s);
|
||||
}
|
||||
|
||||
btScalar getAngle() const
|
||||
{
|
||||
btScalar s = 2.f * btAcos(m_unusedW);
|
||||
return s;
|
||||
}
|
||||
|
||||
|
||||
|
||||
btQuaternion inverse() const
|
||||
{
|
||||
return btQuaternion(m_x, m_y, m_z, -m_unusedW);
|
||||
}
|
||||
|
||||
SIMD_FORCE_INLINE btQuaternion
|
||||
operator+(const btQuaternion& q2) const
|
||||
{
|
||||
const btQuaternion& q1 = *this;
|
||||
return btQuaternion(q1.x() + q2.x(), q1.y() + q2.y(), q1.z() + q2.z(), q1[3] + q2[3]);
|
||||
}
|
||||
|
||||
SIMD_FORCE_INLINE btQuaternion
|
||||
operator-(const btQuaternion& q2) const
|
||||
{
|
||||
const btQuaternion& q1 = *this;
|
||||
return btQuaternion(q1.x() - q2.x(), q1.y() - q2.y(), q1.z() - q2.z(), q1[3] - q2[3]);
|
||||
}
|
||||
|
||||
SIMD_FORCE_INLINE btQuaternion operator-() const
|
||||
{
|
||||
const btQuaternion& q2 = *this;
|
||||
return btQuaternion( - q2.x(), - q2.y(), - q2.z(), - q2[3]);
|
||||
}
|
||||
|
||||
SIMD_FORCE_INLINE btQuaternion farthest( const btQuaternion& qd) const
|
||||
{
|
||||
btQuaternion diff,sum;
|
||||
diff = *this - qd;
|
||||
sum = *this + qd;
|
||||
if( diff.dot(diff) > sum.dot(sum) )
|
||||
return qd;
|
||||
return (-qd);
|
||||
}
|
||||
|
||||
btQuaternion slerp(const btQuaternion& q, const btScalar& t) const
|
||||
{
|
||||
btScalar theta = angle(q);
|
||||
if (theta != btScalar(0.0))
|
||||
{
|
||||
btScalar d = btScalar(1.0) / btSin(theta);
|
||||
btScalar s0 = btSin((btScalar(1.0) - t) * theta);
|
||||
btScalar s1 = btSin(t * theta);
|
||||
return btQuaternion((m_x * s0 + q.x() * s1) * d,
|
||||
(m_y * s0 + q.y() * s1) * d,
|
||||
(m_z * s0 + q.z() * s1) * d,
|
||||
(m_unusedW * s0 + q[3] * s1) * d);
|
||||
}
|
||||
else
|
||||
{
|
||||
return *this;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
};
|
||||
|
||||
|
||||
|
||||
SIMD_FORCE_INLINE btQuaternion
|
||||
operator-(const btQuaternion& q)
|
||||
{
|
||||
return btQuaternion(-q.x(), -q.y(), -q.z(), -q[3]);
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
SIMD_FORCE_INLINE btQuaternion
|
||||
operator*(const btQuaternion& q1, const btQuaternion& q2) {
|
||||
return btQuaternion(q1[3] * q2.x() + q1.x() * q2[3] + q1.y() * q2.z() - q1.z() * q2.y(),
|
||||
q1[3] * q2.y() + q1.y() * q2[3] + q1.z() * q2.x() - q1.x() * q2.z(),
|
||||
q1[3] * q2.z() + q1.z() * q2[3] + q1.x() * q2.y() - q1.y() * q2.x(),
|
||||
q1[3] * q2[3] - q1.x() * q2.x() - q1.y() * q2.y() - q1.z() * q2.z());
|
||||
}
|
||||
|
||||
SIMD_FORCE_INLINE btQuaternion
|
||||
operator*(const btQuaternion& q, const btVector3& w)
|
||||
{
|
||||
return btQuaternion( q[3] * w.x() + q.y() * w.z() - q.z() * w.y(),
|
||||
q[3] * w.y() + q.z() * w.x() - q.x() * w.z(),
|
||||
q[3] * w.z() + q.x() * w.y() - q.y() * w.x(),
|
||||
-q.x() * w.x() - q.y() * w.y() - q.z() * w.z());
|
||||
}
|
||||
|
||||
SIMD_FORCE_INLINE btQuaternion
|
||||
operator*(const btVector3& w, const btQuaternion& q)
|
||||
{
|
||||
return btQuaternion( w.x() * q[3] + w.y() * q.z() - w.z() * q.y(),
|
||||
w.y() * q[3] + w.z() * q.x() - w.x() * q.z(),
|
||||
w.z() * q[3] + w.x() * q.y() - w.y() * q.x(),
|
||||
-w.x() * q.x() - w.y() * q.y() - w.z() * q.z());
|
||||
}
|
||||
|
||||
SIMD_FORCE_INLINE btScalar
|
||||
dot(const btQuaternion& q1, const btQuaternion& q2)
|
||||
{
|
||||
return q1.dot(q2);
|
||||
}
|
||||
|
||||
|
||||
SIMD_FORCE_INLINE btScalar
|
||||
length(const btQuaternion& q)
|
||||
{
|
||||
return q.length();
|
||||
}
|
||||
|
||||
SIMD_FORCE_INLINE btScalar
|
||||
angle(const btQuaternion& q1, const btQuaternion& q2)
|
||||
{
|
||||
return q1.angle(q2);
|
||||
}
|
||||
|
||||
|
||||
SIMD_FORCE_INLINE btQuaternion
|
||||
inverse(const btQuaternion& q)
|
||||
{
|
||||
return q.inverse();
|
||||
}
|
||||
|
||||
SIMD_FORCE_INLINE btQuaternion
|
||||
slerp(const btQuaternion& q1, const btQuaternion& q2, const btScalar& t)
|
||||
{
|
||||
return q1.slerp(q2, t);
|
||||
}
|
||||
|
||||
|
||||
#endif
|
||||
|
||||
|
||||
|
||||
@@ -7,10 +7,10 @@
|
||||
* *
|
||||
* This library is free software; you can redistribute it and/or *
|
||||
* modify it under the terms of EITHER: *
|
||||
* (1) The GNU Lesser General Public License as published by the Free *
|
||||
* (1) The GNU Lesser bteral Public License as published by the Free *
|
||||
* Software Foundation; either version 2.1 of the License, or (at *
|
||||
* your option) any later version. The text of the GNU Lesser *
|
||||
* General Public License is included with this library in the *
|
||||
* bteral Public License is included with this library in the *
|
||||
* file license-LGPL.txt. *
|
||||
* (2) The BSD-style license that is included with this library in *
|
||||
* the file license-BSD.txt. *
|
||||
@@ -27,19 +27,19 @@
|
||||
// Credits: The Clock class was inspired by the Timer classes in
|
||||
// Ogre (www.ogre3d.org).
|
||||
|
||||
#include "LinearMath/GenQuickprof.h"
|
||||
#include "LinearMath/btQuickprof.h"
|
||||
|
||||
#ifdef USE_QUICKPROF
|
||||
|
||||
// Note: We must declare these private static variables again here to
|
||||
// avoid link errors.
|
||||
bool Profiler::mEnabled = false;
|
||||
hidden::Clock Profiler::mClock;
|
||||
unsigned long int Profiler::mCurrentCycleStartMicroseconds = 0;
|
||||
unsigned long int Profiler::mLastCycleDurationMicroseconds = 0;
|
||||
std::map<std::string, hidden::ProfileBlock*> Profiler::mProfileBlocks;
|
||||
std::ofstream Profiler::mOutputFile;
|
||||
bool Profiler::mFirstFileOutput = true;
|
||||
Profiler::BlockTimingMethod Profiler::mFileOutputMethod;
|
||||
unsigned long int Profiler::mCycleNumber = 0;
|
||||
bool btProfiler::mEnabled = false;
|
||||
hidden::Clock btProfiler::mClock;
|
||||
unsigned long int btProfiler::mCurrentCycleStartMicroseconds = 0;
|
||||
unsigned long int btProfiler::mLastCycleDurationMicroseconds = 0;
|
||||
std::map<std::string, hidden::ProfileBlock*> btProfiler::mProfileBlocks;
|
||||
std::ofstream btProfiler::mOutputFile;
|
||||
bool btProfiler::mFirstFileOutput = true;
|
||||
btProfiler::BlockTimingMethod btProfiler::mFileOutputMethod;
|
||||
unsigned long int btProfiler::mCycleNumber = 0;
|
||||
#endif //USE_QUICKPROF
|
||||
@@ -7,10 +7,10 @@
|
||||
* *
|
||||
* This library is free software; you can redistribute it and/or *
|
||||
* modify it under the terms of EITHER: *
|
||||
* (1) The GNU Lesser General Public License as published by the Free *
|
||||
* (1) The GNU Lesser bteral Public License as published by the Free *
|
||||
* Software Foundation; either version 2.1 of the License, or (at *
|
||||
* your option) any later version. The text of the GNU Lesser *
|
||||
* General Public License is included with this library in the *
|
||||
* bteral Public License is included with this library in the *
|
||||
* file license-LGPL.txt. *
|
||||
* (2) The BSD-style license that is included with this library in *
|
||||
* the file license-BSD.txt. *
|
||||
@@ -255,7 +255,7 @@ namespace hidden
|
||||
};
|
||||
|
||||
/// A static class that manages timing for a set of profiling blocks.
|
||||
class Profiler
|
||||
class btProfiler
|
||||
{
|
||||
public:
|
||||
/// A set of ways to retrieve block timing data.
|
||||
@@ -333,9 +333,9 @@ public:
|
||||
BlockTimingMethod method=BLOCK_TOTAL_PERCENT);
|
||||
|
||||
//private:
|
||||
inline Profiler();
|
||||
inline btProfiler();
|
||||
|
||||
inline ~Profiler();
|
||||
inline ~btProfiler();
|
||||
|
||||
/// Prints an error message to standard output.
|
||||
inline static void printError(const std::string& msg)
|
||||
@@ -372,19 +372,19 @@ public:
|
||||
};
|
||||
|
||||
|
||||
Profiler::Profiler()
|
||||
btProfiler::btProfiler()
|
||||
{
|
||||
// This never gets called because a Profiler instance is never
|
||||
// This never gets called because a btProfiler instance is never
|
||||
// created.
|
||||
}
|
||||
|
||||
Profiler::~Profiler()
|
||||
btProfiler::~btProfiler()
|
||||
{
|
||||
// This never gets called because a Profiler instance is never
|
||||
// This never gets called because a btProfiler instance is never
|
||||
// created.
|
||||
}
|
||||
|
||||
void Profiler::init(const std::string outputFilename,
|
||||
void btProfiler::init(const std::string outputFilename,
|
||||
BlockTimingMethod outputMethod)
|
||||
{
|
||||
mEnabled = true;
|
||||
@@ -402,7 +402,7 @@ void Profiler::init(const std::string outputFilename,
|
||||
mCurrentCycleStartMicroseconds = mClock.getTimeMicroseconds();
|
||||
}
|
||||
|
||||
void Profiler::destroy()
|
||||
void btProfiler::destroy()
|
||||
{
|
||||
if (!mEnabled)
|
||||
{
|
||||
@@ -422,7 +422,7 @@ void Profiler::destroy()
|
||||
}
|
||||
}
|
||||
|
||||
void Profiler::beginBlock(const std::string& name)
|
||||
void btProfiler::beginBlock(const std::string& name)
|
||||
{
|
||||
if (!mEnabled)
|
||||
{
|
||||
@@ -448,7 +448,7 @@ void Profiler::beginBlock(const std::string& name)
|
||||
block->currentBlockStartMicroseconds = mClock.getTimeMicroseconds();
|
||||
}
|
||||
|
||||
void Profiler::endBlock(const std::string& name)
|
||||
void btProfiler::endBlock(const std::string& name)
|
||||
{
|
||||
if (!mEnabled)
|
||||
{
|
||||
@@ -474,7 +474,7 @@ void Profiler::endBlock(const std::string& name)
|
||||
block->totalMicroseconds += blockDuration;
|
||||
}
|
||||
|
||||
double Profiler::getBlockTime(const std::string& name,
|
||||
double btProfiler::getBlockTime(const std::string& name,
|
||||
BlockTimingMethod method)
|
||||
{
|
||||
if (!mEnabled)
|
||||
@@ -552,7 +552,7 @@ double Profiler::getBlockTime(const std::string& name,
|
||||
return result;
|
||||
}
|
||||
|
||||
void Profiler::endProfilingCycle()
|
||||
void btProfiler::endProfilingCycle()
|
||||
{
|
||||
if (!mEnabled)
|
||||
{
|
||||
@@ -608,7 +608,7 @@ void Profiler::endProfilingCycle()
|
||||
mCurrentCycleStartMicroseconds = mClock.getTimeMicroseconds();
|
||||
}
|
||||
|
||||
std::string Profiler::createStatsString(BlockTimingMethod method)
|
||||
std::string btProfiler::createStatsString(BlockTimingMethod method)
|
||||
{
|
||||
if (!mEnabled)
|
||||
{
|
||||
128
src/LinearMath/btScalar.h
Normal file
128
src/LinearMath/btScalar.h
Normal file
@@ -0,0 +1,128 @@
|
||||
/*
|
||||
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 SIMD___SCALAR_H
|
||||
#define SIMD___SCALAR_H
|
||||
|
||||
#include <math.h>
|
||||
#undef max
|
||||
|
||||
|
||||
|
||||
#include <cstdlib>
|
||||
#include <cfloat>
|
||||
#include <float.h>
|
||||
|
||||
#ifdef WIN32
|
||||
|
||||
#if defined(__MINGW32__) || defined(__CYGWIN__)
|
||||
#define SIMD_FORCE_INLINE inline
|
||||
#else
|
||||
#pragma warning(disable:4530)
|
||||
#pragma warning(disable:4996)
|
||||
#define SIMD_FORCE_INLINE __forceinline
|
||||
#endif //__MINGW32__
|
||||
|
||||
//#define ATTRIBUTE_ALIGNED16(a) __declspec(align(16)) a
|
||||
#define ATTRIBUTE_ALIGNED16(a) a
|
||||
#include <assert.h>
|
||||
#define ASSERT assert
|
||||
#else
|
||||
|
||||
//non-windows systems
|
||||
|
||||
#define SIMD_FORCE_INLINE inline
|
||||
#define ATTRIBUTE_ALIGNED16(a) a
|
||||
#ifndef assert
|
||||
#include <assert.h>
|
||||
#endif
|
||||
#define ASSERT assert
|
||||
#endif
|
||||
|
||||
|
||||
|
||||
typedef float btScalar;
|
||||
|
||||
#if defined (__sun) || defined (__sun__) || defined (__sparc) || defined (__APPLE__)
|
||||
//use double float precision operation on those platforms for Blender
|
||||
|
||||
SIMD_FORCE_INLINE btScalar btSqrt(btScalar x) { return sqrt(x); }
|
||||
SIMD_FORCE_INLINE btScalar btFabs(btScalar x) { return fabs(x); }
|
||||
SIMD_FORCE_INLINE btScalar btCos(btScalar x) { return cos(x); }
|
||||
SIMD_FORCE_INLINE btScalar btSin(btScalar x) { return sin(x); }
|
||||
SIMD_FORCE_INLINE btScalar btTan(btScalar x) { return tan(x); }
|
||||
SIMD_FORCE_INLINE btScalar btAcos(btScalar x) { return acos(x); }
|
||||
SIMD_FORCE_INLINE btScalar btAsin(btScalar x) { return asin(x); }
|
||||
SIMD_FORCE_INLINE btScalar btAtan(btScalar x) { return atan(x); }
|
||||
SIMD_FORCE_INLINE btScalar btAtan2(btScalar x, btScalar y) { return atan2(x, y); }
|
||||
SIMD_FORCE_INLINE btScalar btExp(btScalar x) { return exp(x); }
|
||||
SIMD_FORCE_INLINE btScalar btLog(btScalar x) { return log(x); }
|
||||
SIMD_FORCE_INLINE btScalar btPow(btScalar x,btScalar y) { return pow(x,y); }
|
||||
|
||||
#else
|
||||
|
||||
SIMD_FORCE_INLINE btScalar btSqrt(btScalar x) { return sqrtf(x); }
|
||||
SIMD_FORCE_INLINE btScalar btFabs(btScalar x) { return fabsf(x); }
|
||||
SIMD_FORCE_INLINE btScalar btCos(btScalar x) { return cosf(x); }
|
||||
SIMD_FORCE_INLINE btScalar btSin(btScalar x) { return sinf(x); }
|
||||
SIMD_FORCE_INLINE btScalar btTan(btScalar x) { return tanf(x); }
|
||||
SIMD_FORCE_INLINE btScalar btAcos(btScalar x) { return acosf(x); }
|
||||
SIMD_FORCE_INLINE btScalar btAsin(btScalar x) { return asinf(x); }
|
||||
SIMD_FORCE_INLINE btScalar btAtan(btScalar x) { return atanf(x); }
|
||||
SIMD_FORCE_INLINE btScalar btAtan2(btScalar x, btScalar y) { return atan2f(x, y); }
|
||||
SIMD_FORCE_INLINE btScalar btExp(btScalar x) { return expf(x); }
|
||||
SIMD_FORCE_INLINE btScalar btLog(btScalar x) { return logf(x); }
|
||||
SIMD_FORCE_INLINE btScalar btPow(btScalar x,btScalar y) { return powf(x,y); }
|
||||
|
||||
#endif
|
||||
|
||||
|
||||
const btScalar SIMD_2_PI = 6.283185307179586232f;
|
||||
const btScalar SIMD_PI = SIMD_2_PI * btScalar(0.5f);
|
||||
const btScalar SIMD_HALF_PI = SIMD_2_PI * btScalar(0.25f);
|
||||
const btScalar SIMD_RADS_PER_DEG = SIMD_2_PI / btScalar(360.0f);
|
||||
const btScalar SIMD_DEGS_PER_RAD = btScalar(360.0f) / SIMD_2_PI;
|
||||
const btScalar SIMD_EPSILON = FLT_EPSILON;
|
||||
const btScalar SIMD_INFINITY = FLT_MAX;
|
||||
|
||||
SIMD_FORCE_INLINE bool btFuzzyZero(btScalar x) { return btFabs(x) < SIMD_EPSILON; }
|
||||
|
||||
SIMD_FORCE_INLINE bool btEqual(btScalar a, btScalar eps) {
|
||||
return (((a) <= eps) && !((a) < -eps));
|
||||
}
|
||||
SIMD_FORCE_INLINE bool btGreaterEqual (btScalar a, btScalar eps) {
|
||||
return (!((a) <= eps));
|
||||
}
|
||||
|
||||
/*SIMD_FORCE_INLINE btScalar btCos(btScalar x) { return cosf(x); }
|
||||
SIMD_FORCE_INLINE btScalar btSin(btScalar x) { return sinf(x); }
|
||||
SIMD_FORCE_INLINE btScalar btTan(btScalar x) { return tanf(x); }
|
||||
SIMD_FORCE_INLINE btScalar btAcos(btScalar x) { return acosf(x); }
|
||||
SIMD_FORCE_INLINE btScalar btAsin(btScalar x) { return asinf(x); }
|
||||
SIMD_FORCE_INLINE btScalar btAtan(btScalar x) { return atanf(x); }
|
||||
SIMD_FORCE_INLINE btScalar btAtan2(btScalar x, btScalar y) { return atan2f(x, y); }
|
||||
*/
|
||||
|
||||
SIMD_FORCE_INLINE int btSign(btScalar x) {
|
||||
return x < 0.0f ? -1 : x > 0.0f ? 1 : 0;
|
||||
}
|
||||
|
||||
SIMD_FORCE_INLINE btScalar btRadians(btScalar x) { return x * SIMD_RADS_PER_DEG; }
|
||||
SIMD_FORCE_INLINE btScalar btDegrees(btScalar x) { return x * SIMD_DEGS_PER_RAD; }
|
||||
|
||||
|
||||
|
||||
#endif //SIMD___SCALAR_H
|
||||
@@ -18,22 +18,22 @@ subject to the following restrictions:
|
||||
#define SIMD_MINMAX_H
|
||||
|
||||
template <class T>
|
||||
SIMD_FORCE_INLINE const T& SimdMin(const T& a, const T& b) {
|
||||
SIMD_FORCE_INLINE const T& btMin(const T& a, const T& b) {
|
||||
return b < a ? b : a;
|
||||
}
|
||||
|
||||
template <class T>
|
||||
SIMD_FORCE_INLINE const T& SimdMax(const T& a, const T& b) {
|
||||
SIMD_FORCE_INLINE const T& btMax(const T& a, const T& b) {
|
||||
return a < b ? b : a;
|
||||
}
|
||||
|
||||
template <class T>
|
||||
SIMD_FORCE_INLINE void SimdSetMin(T& a, const T& b) {
|
||||
SIMD_FORCE_INLINE void btSetMin(T& a, const T& b) {
|
||||
if (a > b) a = b;
|
||||
}
|
||||
|
||||
template <class T>
|
||||
SIMD_FORCE_INLINE void SimdSetMax(T& a, const T& b) {
|
||||
SIMD_FORCE_INLINE void btSetMax(T& a, const T& b) {
|
||||
if (a < b) a = b;
|
||||
}
|
||||
|
||||
236
src/LinearMath/btTransform.h
Normal file
236
src/LinearMath/btTransform.h
Normal file
@@ -0,0 +1,236 @@
|
||||
/*
|
||||
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 btTransform_H
|
||||
#define btTransform_H
|
||||
|
||||
#include "LinearMath/btVector3.h"
|
||||
#include "LinearMath/btMatrix3x3.h"
|
||||
|
||||
|
||||
|
||||
class btTransform {
|
||||
|
||||
|
||||
public:
|
||||
|
||||
enum {
|
||||
TRANSLATION = 0x01,
|
||||
ROTATION = 0x02,
|
||||
RIGID = TRANSLATION | ROTATION,
|
||||
SCALING = 0x04,
|
||||
LINEAR = ROTATION | SCALING,
|
||||
AFFINE = TRANSLATION | LINEAR
|
||||
};
|
||||
|
||||
btTransform() {}
|
||||
|
||||
explicit SIMD_FORCE_INLINE btTransform(const btQuaternion& q,
|
||||
const btVector3& c = btVector3(btScalar(0), btScalar(0), btScalar(0)))
|
||||
: m_basis(q),
|
||||
m_origin(c),
|
||||
m_type(RIGID)
|
||||
{}
|
||||
|
||||
explicit SIMD_FORCE_INLINE btTransform(const btMatrix3x3& b,
|
||||
const btVector3& c = btVector3(btScalar(0), btScalar(0), btScalar(0)),
|
||||
unsigned int type = AFFINE)
|
||||
: m_basis(b),
|
||||
m_origin(c),
|
||||
m_type(type)
|
||||
{}
|
||||
|
||||
|
||||
SIMD_FORCE_INLINE void mult(const btTransform& t1, const btTransform& t2) {
|
||||
m_basis = t1.m_basis * t2.m_basis;
|
||||
m_origin = t1(t2.m_origin);
|
||||
m_type = t1.m_type | t2.m_type;
|
||||
}
|
||||
|
||||
void multInverseLeft(const btTransform& t1, const btTransform& t2) {
|
||||
btVector3 v = t2.m_origin - t1.m_origin;
|
||||
if (t1.m_type & SCALING) {
|
||||
btMatrix3x3 inv = t1.m_basis.inverse();
|
||||
m_basis = inv * t2.m_basis;
|
||||
m_origin = inv * v;
|
||||
}
|
||||
else {
|
||||
m_basis = btMultTransposeLeft(t1.m_basis, t2.m_basis);
|
||||
m_origin = v * t1.m_basis;
|
||||
}
|
||||
m_type = t1.m_type | t2.m_type;
|
||||
}
|
||||
|
||||
SIMD_FORCE_INLINE btVector3 operator()(const btVector3& x) const
|
||||
{
|
||||
return btVector3(m_basis[0].dot(x) + m_origin[0],
|
||||
m_basis[1].dot(x) + m_origin[1],
|
||||
m_basis[2].dot(x) + m_origin[2]);
|
||||
}
|
||||
|
||||
SIMD_FORCE_INLINE btVector3 operator*(const btVector3& x) const
|
||||
{
|
||||
return (*this)(x);
|
||||
}
|
||||
|
||||
SIMD_FORCE_INLINE btMatrix3x3& getBasis() { return m_basis; }
|
||||
SIMD_FORCE_INLINE const btMatrix3x3& getBasis() const { return m_basis; }
|
||||
|
||||
SIMD_FORCE_INLINE btVector3& getOrigin() { return m_origin; }
|
||||
SIMD_FORCE_INLINE const btVector3& getOrigin() const { return m_origin; }
|
||||
|
||||
btQuaternion getRotation() const {
|
||||
btQuaternion q;
|
||||
m_basis.getRotation(q);
|
||||
return q;
|
||||
}
|
||||
template <typename Scalar2>
|
||||
void setValue(const Scalar2 *m)
|
||||
{
|
||||
m_basis.setValue(m);
|
||||
m_origin.setValue(&m[12]);
|
||||
m_type = AFFINE;
|
||||
}
|
||||
|
||||
|
||||
void setFromOpenGLMatrix(const btScalar *m)
|
||||
{
|
||||
m_basis.setFromOpenGLSubMatrix(m);
|
||||
m_origin[0] = m[12];
|
||||
m_origin[1] = m[13];
|
||||
m_origin[2] = m[14];
|
||||
}
|
||||
|
||||
void getOpenGLMatrix(btScalar *m) const
|
||||
{
|
||||
m_basis.getOpenGLSubMatrix(m);
|
||||
m[12] = m_origin[0];
|
||||
m[13] = m_origin[1];
|
||||
m[14] = m_origin[2];
|
||||
m[15] = btScalar(1.0f);
|
||||
}
|
||||
|
||||
SIMD_FORCE_INLINE void setOrigin(const btVector3& origin)
|
||||
{
|
||||
m_origin = origin;
|
||||
m_type |= TRANSLATION;
|
||||
}
|
||||
|
||||
SIMD_FORCE_INLINE btVector3 invXform(const btVector3& inVec) const;
|
||||
|
||||
|
||||
|
||||
SIMD_FORCE_INLINE void setBasis(const btMatrix3x3& basis)
|
||||
{
|
||||
m_basis = basis;
|
||||
m_type |= LINEAR;
|
||||
}
|
||||
|
||||
SIMD_FORCE_INLINE void setRotation(const btQuaternion& q)
|
||||
{
|
||||
m_basis.setRotation(q);
|
||||
m_type = (m_type & ~LINEAR) | ROTATION;
|
||||
}
|
||||
|
||||
SIMD_FORCE_INLINE void scale(const btVector3& scaling)
|
||||
{
|
||||
m_basis = m_basis.scaled(scaling);
|
||||
m_type |= SCALING;
|
||||
}
|
||||
|
||||
void setIdentity()
|
||||
{
|
||||
m_basis.setIdentity();
|
||||
m_origin.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0));
|
||||
m_type = 0x0;
|
||||
}
|
||||
|
||||
SIMD_FORCE_INLINE bool isIdentity() const { return m_type == 0x0; }
|
||||
|
||||
btTransform& operator*=(const btTransform& t)
|
||||
{
|
||||
m_origin += m_basis * t.m_origin;
|
||||
m_basis *= t.m_basis;
|
||||
m_type |= t.m_type;
|
||||
return *this;
|
||||
}
|
||||
|
||||
btTransform inverse() const
|
||||
{
|
||||
if (m_type)
|
||||
{
|
||||
btMatrix3x3 inv = (m_type & SCALING) ?
|
||||
m_basis.inverse() :
|
||||
m_basis.transpose();
|
||||
|
||||
return btTransform(inv, inv * -m_origin, m_type);
|
||||
}
|
||||
|
||||
return *this;
|
||||
}
|
||||
|
||||
btTransform inverseTimes(const btTransform& t) const;
|
||||
|
||||
btTransform operator*(const btTransform& t) const;
|
||||
|
||||
private:
|
||||
|
||||
btMatrix3x3 m_basis;
|
||||
btVector3 m_origin;
|
||||
unsigned int m_type;
|
||||
};
|
||||
|
||||
|
||||
SIMD_FORCE_INLINE btVector3
|
||||
btTransform::invXform(const btVector3& inVec) const
|
||||
{
|
||||
btVector3 v = inVec - m_origin;
|
||||
return (m_basis.transpose() * v);
|
||||
}
|
||||
|
||||
SIMD_FORCE_INLINE btTransform
|
||||
btTransform::inverseTimes(const btTransform& t) const
|
||||
{
|
||||
btVector3 v = t.getOrigin() - m_origin;
|
||||
if (m_type & SCALING)
|
||||
{
|
||||
btMatrix3x3 inv = m_basis.inverse();
|
||||
return btTransform(inv * t.getBasis(), inv * v,
|
||||
m_type | t.m_type);
|
||||
}
|
||||
else
|
||||
{
|
||||
return btTransform(m_basis.transposeTimes(t.m_basis),
|
||||
v * m_basis, m_type | t.m_type);
|
||||
}
|
||||
}
|
||||
|
||||
SIMD_FORCE_INLINE btTransform
|
||||
btTransform::operator*(const btTransform& t) const
|
||||
{
|
||||
return btTransform(m_basis * t.m_basis,
|
||||
(*this)(t.m_origin),
|
||||
m_type | t.m_type);
|
||||
}
|
||||
|
||||
|
||||
|
||||
#endif
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
@@ -16,29 +16,29 @@ subject to the following restrictions:
|
||||
#ifndef SIMD_TRANSFORM_UTIL_H
|
||||
#define SIMD_TRANSFORM_UTIL_H
|
||||
|
||||
#include "LinearMath/SimdTransform.h"
|
||||
#include "LinearMath/btTransform.h"
|
||||
#define ANGULAR_MOTION_TRESHOLD 0.5f*SIMD_HALF_PI
|
||||
|
||||
|
||||
|
||||
#define SIMDSQRT12 SimdScalar(0.7071067811865475244008443621048490)
|
||||
#define SIMDSQRT12 btScalar(0.7071067811865475244008443621048490)
|
||||
|
||||
#define SimdRecipSqrt(x) ((float)(1.0f/SimdSqrt(float(x)))) /* reciprocal square root */
|
||||
#define btRecipSqrt(x) ((float)(1.0f/btSqrt(float(x)))) /* reciprocal square root */
|
||||
|
||||
inline SimdVector3 SimdAabbSupport(const SimdVector3& halfExtents,const SimdVector3& supportDir)
|
||||
inline btVector3 btAabbSupport(const btVector3& halfExtents,const btVector3& supportDir)
|
||||
{
|
||||
return SimdVector3(supportDir.x() < SimdScalar(0.0f) ? -halfExtents.x() : halfExtents.x(),
|
||||
supportDir.y() < SimdScalar(0.0f) ? -halfExtents.y() : halfExtents.y(),
|
||||
supportDir.z() < SimdScalar(0.0f) ? -halfExtents.z() : halfExtents.z());
|
||||
return btVector3(supportDir.x() < btScalar(0.0f) ? -halfExtents.x() : halfExtents.x(),
|
||||
supportDir.y() < btScalar(0.0f) ? -halfExtents.y() : halfExtents.y(),
|
||||
supportDir.z() < btScalar(0.0f) ? -halfExtents.z() : halfExtents.z());
|
||||
}
|
||||
|
||||
|
||||
inline void SimdPlaneSpace1 (const SimdVector3& n, SimdVector3& p, SimdVector3& q)
|
||||
inline void btPlaneSpace1 (const btVector3& n, btVector3& p, btVector3& q)
|
||||
{
|
||||
if (SimdFabs(n[2]) > SIMDSQRT12) {
|
||||
if (btFabs(n[2]) > SIMDSQRT12) {
|
||||
// choose p in y-z plane
|
||||
SimdScalar a = n[1]*n[1] + n[2]*n[2];
|
||||
SimdScalar k = SimdRecipSqrt (a);
|
||||
btScalar a = n[1]*n[1] + n[2]*n[2];
|
||||
btScalar k = btRecipSqrt (a);
|
||||
p[0] = 0;
|
||||
p[1] = -n[2]*k;
|
||||
p[2] = n[1]*k;
|
||||
@@ -49,8 +49,8 @@ inline void SimdPlaneSpace1 (const SimdVector3& n, SimdVector3& p, SimdVector3&
|
||||
}
|
||||
else {
|
||||
// choose p in x-y plane
|
||||
SimdScalar a = n[0]*n[0] + n[1]*n[1];
|
||||
SimdScalar k = SimdRecipSqrt (a);
|
||||
btScalar a = n[0]*n[0] + n[1]*n[1];
|
||||
btScalar k = btRecipSqrt (a);
|
||||
p[0] = -n[1]*k;
|
||||
p[1] = n[0]*k;
|
||||
p[2] = 0;
|
||||
@@ -64,23 +64,23 @@ inline void SimdPlaneSpace1 (const SimdVector3& n, SimdVector3& p, SimdVector3&
|
||||
|
||||
|
||||
/// Utils related to temporal transforms
|
||||
class SimdTransformUtil
|
||||
class btTransformUtil
|
||||
{
|
||||
|
||||
public:
|
||||
|
||||
static void IntegrateTransform(const SimdTransform& curTrans,const SimdVector3& linvel,const SimdVector3& angvel,SimdScalar timeStep,SimdTransform& predictedTransform)
|
||||
static void IntegrateTransform(const btTransform& curTrans,const btVector3& linvel,const btVector3& angvel,btScalar timeStep,btTransform& predictedTransform)
|
||||
{
|
||||
predictedTransform.setOrigin(curTrans.getOrigin() + linvel * timeStep);
|
||||
// #define QUATERNION_DERIVATIVE
|
||||
#ifdef QUATERNION_DERIVATIVE
|
||||
SimdQuaternion orn = curTrans.getRotation();
|
||||
btQuaternion orn = curTrans.getRotation();
|
||||
orn += (angvel * orn) * (timeStep * 0.5f);
|
||||
orn.normalize();
|
||||
#else
|
||||
//exponential map
|
||||
SimdVector3 axis;
|
||||
SimdScalar fAngle = angvel.length();
|
||||
btVector3 axis;
|
||||
btScalar fAngle = angvel.length();
|
||||
//limit the angular motion
|
||||
if (fAngle*timeStep > ANGULAR_MOTION_TRESHOLD)
|
||||
{
|
||||
@@ -95,41 +95,41 @@ public:
|
||||
else
|
||||
{
|
||||
// sync(fAngle) = sin(c*fAngle)/t
|
||||
axis = angvel*( SimdSin(0.5f*fAngle*timeStep)/fAngle );
|
||||
axis = angvel*( btSin(0.5f*fAngle*timeStep)/fAngle );
|
||||
}
|
||||
SimdQuaternion dorn (axis.x(),axis.y(),axis.z(),SimdCos( fAngle*timeStep*0.5f ));
|
||||
SimdQuaternion orn0 = curTrans.getRotation();
|
||||
btQuaternion dorn (axis.x(),axis.y(),axis.z(),btCos( fAngle*timeStep*0.5f ));
|
||||
btQuaternion orn0 = curTrans.getRotation();
|
||||
|
||||
SimdQuaternion predictedOrn = dorn * orn0;
|
||||
btQuaternion predictedOrn = dorn * orn0;
|
||||
#endif
|
||||
predictedTransform.setRotation(predictedOrn);
|
||||
}
|
||||
|
||||
static void CalculateVelocity(const SimdTransform& transform0,const SimdTransform& transform1,SimdScalar timeStep,SimdVector3& linVel,SimdVector3& angVel)
|
||||
static void CalculateVelocity(const btTransform& transform0,const btTransform& transform1,btScalar timeStep,btVector3& linVel,btVector3& angVel)
|
||||
{
|
||||
linVel = (transform1.getOrigin() - transform0.getOrigin()) / timeStep;
|
||||
#ifdef USE_QUATERNION_DIFF
|
||||
SimdQuaternion orn0 = transform0.getRotation();
|
||||
SimdQuaternion orn1a = transform1.getRotation();
|
||||
SimdQuaternion orn1 = orn0.farthest(orn1a);
|
||||
SimdQuaternion dorn = orn1 * orn0.inverse();
|
||||
btQuaternion orn0 = transform0.getRotation();
|
||||
btQuaternion orn1a = transform1.getRotation();
|
||||
btQuaternion orn1 = orn0.farthest(orn1a);
|
||||
btQuaternion dorn = orn1 * orn0.inverse();
|
||||
#else
|
||||
SimdMatrix3x3 dmat = transform1.getBasis() * transform0.getBasis().inverse();
|
||||
SimdQuaternion dorn;
|
||||
btMatrix3x3 dmat = transform1.getBasis() * transform0.getBasis().inverse();
|
||||
btQuaternion dorn;
|
||||
dmat.getRotation(dorn);
|
||||
#endif//USE_QUATERNION_DIFF
|
||||
|
||||
SimdVector3 axis;
|
||||
SimdScalar angle;
|
||||
btVector3 axis;
|
||||
btScalar angle;
|
||||
angle = dorn.getAngle();
|
||||
axis = SimdVector3(dorn.x(),dorn.y(),dorn.z());
|
||||
axis = btVector3(dorn.x(),dorn.y(),dorn.z());
|
||||
axis[3] = 0.f;
|
||||
//check for axis length
|
||||
SimdScalar len = axis.length2();
|
||||
btScalar len = axis.length2();
|
||||
if (len < SIMD_EPSILON*SIMD_EPSILON)
|
||||
axis = SimdVector3(1.f,0.f,0.f);
|
||||
axis = btVector3(1.f,0.f,0.f);
|
||||
else
|
||||
axis /= SimdSqrt(len);
|
||||
axis /= btSqrt(len);
|
||||
|
||||
|
||||
angVel = axis * angle / timeStep;
|
||||
403
src/LinearMath/btVector3.h
Normal file
403
src/LinearMath/btVector3.h
Normal file
@@ -0,0 +1,403 @@
|
||||
/*
|
||||
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 SIMD__VECTOR3_H
|
||||
#define SIMD__VECTOR3_H
|
||||
|
||||
#include "btQuadWord.h"
|
||||
|
||||
|
||||
///btVector3 is 16byte aligned, and has an extra unused component m_w
|
||||
///this extra component can be used by derived classes (Quaternion?) or by user
|
||||
class btVector3 : public btQuadWord {
|
||||
|
||||
|
||||
public:
|
||||
SIMD_FORCE_INLINE btVector3() {}
|
||||
|
||||
|
||||
|
||||
SIMD_FORCE_INLINE btVector3(const btScalar& x, const btScalar& y, const btScalar& z)
|
||||
:btQuadWord(x,y,z,0.f)
|
||||
{
|
||||
}
|
||||
|
||||
// SIMD_FORCE_INLINE btVector3(const btScalar& x, const btScalar& y, const btScalar& z,const btScalar& w)
|
||||
// : btQuadWord(x,y,z,w)
|
||||
// {
|
||||
// }
|
||||
|
||||
|
||||
|
||||
SIMD_FORCE_INLINE btVector3& operator+=(const btVector3& v)
|
||||
{
|
||||
m_x += v.x(); m_y += v.y(); m_z += v.z();
|
||||
return *this;
|
||||
}
|
||||
|
||||
|
||||
|
||||
SIMD_FORCE_INLINE btVector3& operator-=(const btVector3& v)
|
||||
{
|
||||
m_x -= v.x(); m_y -= v.y(); m_z -= v.z();
|
||||
return *this;
|
||||
}
|
||||
|
||||
SIMD_FORCE_INLINE btVector3& operator*=(const btScalar& s)
|
||||
{
|
||||
m_x *= s; m_y *= s; m_z *= s;
|
||||
return *this;
|
||||
}
|
||||
|
||||
SIMD_FORCE_INLINE btVector3& operator/=(const btScalar& s)
|
||||
{
|
||||
assert(s != btScalar(0.0));
|
||||
return *this *= btScalar(1.0) / s;
|
||||
}
|
||||
|
||||
SIMD_FORCE_INLINE btScalar dot(const btVector3& v) const
|
||||
{
|
||||
return m_x * v.x() + m_y * v.y() + m_z * v.z();
|
||||
}
|
||||
|
||||
SIMD_FORCE_INLINE btScalar length2() const
|
||||
{
|
||||
return dot(*this);
|
||||
}
|
||||
|
||||
SIMD_FORCE_INLINE btScalar length() const
|
||||
{
|
||||
return btSqrt(length2());
|
||||
}
|
||||
|
||||
SIMD_FORCE_INLINE btScalar distance2(const btVector3& v) const;
|
||||
|
||||
SIMD_FORCE_INLINE btScalar distance(const btVector3& v) const;
|
||||
|
||||
SIMD_FORCE_INLINE btVector3& normalize()
|
||||
{
|
||||
return *this /= length();
|
||||
}
|
||||
|
||||
SIMD_FORCE_INLINE btVector3 normalized() const;
|
||||
|
||||
SIMD_FORCE_INLINE btVector3 rotate( const btVector3& wAxis, const btScalar angle );
|
||||
|
||||
SIMD_FORCE_INLINE btScalar angle(const btVector3& v) const
|
||||
{
|
||||
btScalar s = btSqrt(length2() * v.length2());
|
||||
assert(s != btScalar(0.0));
|
||||
return btAcos(dot(v) / s);
|
||||
}
|
||||
|
||||
SIMD_FORCE_INLINE btVector3 absolute() const
|
||||
{
|
||||
return btVector3(
|
||||
btFabs(m_x),
|
||||
btFabs(m_y),
|
||||
btFabs(m_z));
|
||||
}
|
||||
|
||||
SIMD_FORCE_INLINE btVector3 cross(const btVector3& v) const
|
||||
{
|
||||
return btVector3(
|
||||
m_y * v.z() - m_z * v.y(),
|
||||
m_z * v.x() - m_x * v.z(),
|
||||
m_x * v.y() - m_y * v.x());
|
||||
}
|
||||
|
||||
SIMD_FORCE_INLINE btScalar triple(const btVector3& v1, const btVector3& v2) const
|
||||
{
|
||||
return m_x * (v1.y() * v2.z() - v1.z() * v2.y()) +
|
||||
m_y * (v1.z() * v2.x() - v1.x() * v2.z()) +
|
||||
m_z * (v1.x() * v2.y() - v1.y() * v2.x());
|
||||
}
|
||||
|
||||
SIMD_FORCE_INLINE int minAxis() const
|
||||
{
|
||||
return m_x < m_y ? (m_x < m_z ? 0 : 2) : (m_y < m_z ? 1 : 2);
|
||||
}
|
||||
|
||||
SIMD_FORCE_INLINE int maxAxis() const
|
||||
{
|
||||
return m_x < m_y ? (m_y < m_z ? 2 : 1) : (m_x < m_z ? 2 : 0);
|
||||
}
|
||||
|
||||
SIMD_FORCE_INLINE int furthestAxis() const
|
||||
{
|
||||
return absolute().minAxis();
|
||||
}
|
||||
|
||||
SIMD_FORCE_INLINE int closestAxis() const
|
||||
{
|
||||
return absolute().maxAxis();
|
||||
}
|
||||
|
||||
SIMD_FORCE_INLINE void setInterpolate3(const btVector3& v0, const btVector3& v1, btScalar rt)
|
||||
{
|
||||
btScalar s = 1.0f - rt;
|
||||
m_x = s * v0[0] + rt * v1.x();
|
||||
m_y = s * v0[1] + rt * v1.y();
|
||||
m_z = s * v0[2] + rt * v1.z();
|
||||
//don't do the unused w component
|
||||
// m_co[3] = s * v0[3] + rt * v1[3];
|
||||
}
|
||||
|
||||
SIMD_FORCE_INLINE btVector3 lerp(const btVector3& v, const btScalar& t) const
|
||||
{
|
||||
return btVector3(m_x + (v.x() - m_x) * t,
|
||||
m_y + (v.y() - m_y) * t,
|
||||
m_z + (v.z() - m_z) * t);
|
||||
}
|
||||
|
||||
|
||||
SIMD_FORCE_INLINE btVector3& operator*=(const btVector3& v)
|
||||
{
|
||||
m_x *= v.x(); m_y *= v.y(); m_z *= v.z();
|
||||
return *this;
|
||||
}
|
||||
|
||||
|
||||
|
||||
};
|
||||
|
||||
SIMD_FORCE_INLINE btVector3
|
||||
operator+(const btVector3& v1, const btVector3& v2)
|
||||
{
|
||||
return btVector3(v1.x() + v2.x(), v1.y() + v2.y(), v1.z() + v2.z());
|
||||
}
|
||||
|
||||
SIMD_FORCE_INLINE btVector3
|
||||
operator*(const btVector3& v1, const btVector3& v2)
|
||||
{
|
||||
return btVector3(v1.x() * v2.x(), v1.y() * v2.y(), v1.z() * v2.z());
|
||||
}
|
||||
|
||||
SIMD_FORCE_INLINE btVector3
|
||||
operator-(const btVector3& v1, const btVector3& v2)
|
||||
{
|
||||
return btVector3(v1.x() - v2.x(), v1.y() - v2.y(), v1.z() - v2.z());
|
||||
}
|
||||
|
||||
SIMD_FORCE_INLINE btVector3
|
||||
operator-(const btVector3& v)
|
||||
{
|
||||
return btVector3(-v.x(), -v.y(), -v.z());
|
||||
}
|
||||
|
||||
SIMD_FORCE_INLINE btVector3
|
||||
operator*(const btVector3& v, const btScalar& s)
|
||||
{
|
||||
return btVector3(v.x() * s, v.y() * s, v.z() * s);
|
||||
}
|
||||
|
||||
SIMD_FORCE_INLINE btVector3
|
||||
operator*(const btScalar& s, const btVector3& v)
|
||||
{
|
||||
return v * s;
|
||||
}
|
||||
|
||||
SIMD_FORCE_INLINE btVector3
|
||||
operator/(const btVector3& v, const btScalar& s)
|
||||
{
|
||||
assert(s != btScalar(0.0));
|
||||
return v * (btScalar(1.0) / s);
|
||||
}
|
||||
|
||||
SIMD_FORCE_INLINE btVector3
|
||||
operator/(const btVector3& v1, const btVector3& v2)
|
||||
{
|
||||
return btVector3(v1.x() / v2.x(),v1.y() / v2.y(),v1.z() / v2.z());
|
||||
}
|
||||
|
||||
SIMD_FORCE_INLINE btScalar
|
||||
dot(const btVector3& v1, const btVector3& v2)
|
||||
{
|
||||
return v1.dot(v2);
|
||||
}
|
||||
|
||||
|
||||
|
||||
SIMD_FORCE_INLINE btScalar
|
||||
distance2(const btVector3& v1, const btVector3& v2)
|
||||
{
|
||||
return v1.distance2(v2);
|
||||
}
|
||||
|
||||
|
||||
SIMD_FORCE_INLINE btScalar
|
||||
distance(const btVector3& v1, const btVector3& v2)
|
||||
{
|
||||
return v1.distance(v2);
|
||||
}
|
||||
|
||||
SIMD_FORCE_INLINE btScalar
|
||||
angle(const btVector3& v1, const btVector3& v2)
|
||||
{
|
||||
return v1.angle(v2);
|
||||
}
|
||||
|
||||
SIMD_FORCE_INLINE btVector3
|
||||
cross(const btVector3& v1, const btVector3& v2)
|
||||
{
|
||||
return v1.cross(v2);
|
||||
}
|
||||
|
||||
SIMD_FORCE_INLINE btScalar
|
||||
triple(const btVector3& v1, const btVector3& v2, const btVector3& v3)
|
||||
{
|
||||
return v1.triple(v2, v3);
|
||||
}
|
||||
|
||||
SIMD_FORCE_INLINE btVector3
|
||||
lerp(const btVector3& v1, const btVector3& v2, const btScalar& t)
|
||||
{
|
||||
return v1.lerp(v2, t);
|
||||
}
|
||||
|
||||
|
||||
SIMD_FORCE_INLINE bool operator==(const btVector3& p1, const btVector3& p2)
|
||||
{
|
||||
return p1[0] == p2[0] && p1[1] == p2[1] && p1[2] == p2[2];
|
||||
}
|
||||
|
||||
SIMD_FORCE_INLINE btScalar btVector3::distance2(const btVector3& v) const
|
||||
{
|
||||
return (v - *this).length2();
|
||||
}
|
||||
|
||||
SIMD_FORCE_INLINE btScalar btVector3::distance(const btVector3& v) const
|
||||
{
|
||||
return (v - *this).length();
|
||||
}
|
||||
|
||||
SIMD_FORCE_INLINE btVector3 btVector3::normalized() const
|
||||
{
|
||||
return *this / length();
|
||||
}
|
||||
|
||||
SIMD_FORCE_INLINE btVector3 btVector3::rotate( const btVector3& wAxis, const btScalar angle )
|
||||
{
|
||||
// wAxis must be a unit lenght vector
|
||||
|
||||
btVector3 o = wAxis * wAxis.dot( *this );
|
||||
btVector3 x = *this - o;
|
||||
btVector3 y;
|
||||
|
||||
y = wAxis.cross( *this );
|
||||
|
||||
return ( o + x * btCos( angle ) + y * btSin( angle ) );
|
||||
}
|
||||
|
||||
class btVector4 : public btVector3
|
||||
{
|
||||
public:
|
||||
|
||||
SIMD_FORCE_INLINE btVector4() {}
|
||||
|
||||
|
||||
SIMD_FORCE_INLINE btVector4(const btScalar& x, const btScalar& y, const btScalar& z,const btScalar& w)
|
||||
: btVector3(x,y,z)
|
||||
{
|
||||
m_unusedW = w;
|
||||
}
|
||||
|
||||
|
||||
SIMD_FORCE_INLINE btVector4 absolute4() const
|
||||
{
|
||||
return btVector4(
|
||||
btFabs(m_x),
|
||||
btFabs(m_y),
|
||||
btFabs(m_z),
|
||||
btFabs(m_unusedW));
|
||||
}
|
||||
|
||||
|
||||
|
||||
float getW() const { return m_unusedW;}
|
||||
|
||||
|
||||
SIMD_FORCE_INLINE int maxAxis4() const
|
||||
{
|
||||
int maxIndex = -1;
|
||||
float maxVal = -1e30f;
|
||||
if (m_x > maxVal)
|
||||
{
|
||||
maxIndex = 0;
|
||||
maxVal = m_x;
|
||||
}
|
||||
if (m_y > maxVal)
|
||||
{
|
||||
maxIndex = 1;
|
||||
maxVal = m_y;
|
||||
}
|
||||
if (m_z > maxVal)
|
||||
{
|
||||
maxIndex = 2;
|
||||
maxVal = m_z;
|
||||
}
|
||||
if (m_unusedW > maxVal)
|
||||
{
|
||||
maxIndex = 3;
|
||||
maxVal = m_unusedW;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
return maxIndex;
|
||||
|
||||
}
|
||||
|
||||
|
||||
SIMD_FORCE_INLINE int minAxis4() const
|
||||
{
|
||||
int minIndex = -1;
|
||||
float minVal = 1e30f;
|
||||
if (m_x < minVal)
|
||||
{
|
||||
minIndex = 0;
|
||||
minVal = m_x;
|
||||
}
|
||||
if (m_y < minVal)
|
||||
{
|
||||
minIndex = 1;
|
||||
minVal = m_y;
|
||||
}
|
||||
if (m_z < minVal)
|
||||
{
|
||||
minIndex = 2;
|
||||
minVal = m_z;
|
||||
}
|
||||
if (m_unusedW < minVal)
|
||||
{
|
||||
minIndex = 3;
|
||||
minVal = m_unusedW;
|
||||
}
|
||||
|
||||
return minIndex;
|
||||
|
||||
}
|
||||
|
||||
|
||||
SIMD_FORCE_INLINE int closestAxis4() const
|
||||
{
|
||||
return absolute4().maxAxis4();
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
#endif //SIMD__VECTOR3_H
|
||||
Reference in New Issue
Block a user