|
@@ -270,10 +270,10 @@ public:
|
|
|
m_el[1] = V2;
|
|
m_el[1] = V2;
|
|
|
m_el[2] = V3;
|
|
m_el[2] = V3;
|
|
|
#else
|
|
#else
|
|
|
- btScalar xs = q.x() * s, ys = q.y() * s, zs = q.z() * s;
|
|
|
|
|
- btScalar wx = q.w() * xs, wy = q.w() * ys, wz = q.w() * zs;
|
|
|
|
|
- btScalar xx = q.x() * xs, xy = q.x() * ys, xz = q.x() * zs;
|
|
|
|
|
- btScalar yy = q.y() * ys, yz = q.y() * zs, zz = q.z() * zs;
|
|
|
|
|
|
|
+ btScalar xs = q.m_floats[0] * s, ys = q.m_floats[1] * s, zs = q.m_floats[2] * s;
|
|
|
|
|
+ btScalar wx = q.m_floats[3] * xs, wy = q.m_floats[3] * ys, wz = q.m_floats[3] * zs;
|
|
|
|
|
+ btScalar xx = q.m_floats[0] * xs, xy = q.m_floats[0] * ys, xz = q.m_floats[0] * zs;
|
|
|
|
|
+ btScalar yy = q.m_floats[1] * ys, yz = q.m_floats[1] * zs, zz = q.m_floats[2] * zs;
|
|
|
setValue(
|
|
setValue(
|
|
|
btScalar(1.0) - (yy + zz), xy - wz, xz + wy,
|
|
btScalar(1.0) - (yy + zz), xy - wz, xz + wy,
|
|
|
xy + wz, btScalar(1.0) - (xx + zz), yz - wx,
|
|
xy + wz, btScalar(1.0) - (xx + zz), yz - wx,
|
|
@@ -400,17 +400,17 @@ public:
|
|
|
vm[1] = v1;
|
|
vm[1] = v1;
|
|
|
vm[2] = v2;
|
|
vm[2] = v2;
|
|
|
#else
|
|
#else
|
|
|
- m[0] = btScalar(m_el[0].x());
|
|
|
|
|
- m[1] = btScalar(m_el[1].x());
|
|
|
|
|
- m[2] = btScalar(m_el[2].x());
|
|
|
|
|
|
|
+ m[0] = btScalar(m_el[0].m_floats[0]);
|
|
|
|
|
+ m[1] = btScalar(m_el[1].m_floats[0]);
|
|
|
|
|
+ m[2] = btScalar(m_el[2].m_floats[0]);
|
|
|
m[3] = btScalar(0.0);
|
|
m[3] = btScalar(0.0);
|
|
|
- m[4] = btScalar(m_el[0].y());
|
|
|
|
|
- m[5] = btScalar(m_el[1].y());
|
|
|
|
|
- m[6] = btScalar(m_el[2].y());
|
|
|
|
|
|
|
+ m[4] = btScalar(m_el[0].m_floats[1]);
|
|
|
|
|
+ m[5] = btScalar(m_el[1].m_floats[1]);
|
|
|
|
|
+ m[6] = btScalar(m_el[2].m_floats[1]);
|
|
|
m[7] = btScalar(0.0);
|
|
m[7] = btScalar(0.0);
|
|
|
- m[8] = btScalar(m_el[0].z());
|
|
|
|
|
- m[9] = btScalar(m_el[1].z());
|
|
|
|
|
- m[10] = btScalar(m_el[2].z());
|
|
|
|
|
|
|
+ m[8] = btScalar(m_el[0].m_floats[2]);
|
|
|
|
|
+ m[9] = btScalar(m_el[1].m_floats[2]);
|
|
|
|
|
+ m[10] = btScalar(m_el[2].m_floats[2]);
|
|
|
m[11] = btScalar(0.0);
|
|
m[11] = btScalar(0.0);
|
|
|
#endif
|
|
#endif
|
|
|
}
|
|
}
|
|
@@ -420,7 +420,7 @@ public:
|
|
|
void getRotation(btQuaternion & q) const
|
|
void getRotation(btQuaternion & q) const
|
|
|
{
|
|
{
|
|
|
#if (defined(BT_USE_SSE_IN_API) && defined(BT_USE_SSE)) || defined(BT_USE_NEON)
|
|
#if (defined(BT_USE_SSE_IN_API) && defined(BT_USE_SSE)) || defined(BT_USE_NEON)
|
|
|
- btScalar trace = m_el[0].x() + m_el[1].y() + m_el[2].z();
|
|
|
|
|
|
|
+ btScalar trace = m_el[0].m_floats[0] + m_el[1].m_floats[1] + m_el[2].m_floats[2];
|
|
|
btScalar s, x;
|
|
btScalar s, x;
|
|
|
|
|
|
|
|
union {
|
|
union {
|
|
@@ -432,18 +432,18 @@ public:
|
|
|
{
|
|
{
|
|
|
x = trace + btScalar(1.0);
|
|
x = trace + btScalar(1.0);
|
|
|
|
|
|
|
|
- temp.f[0] = m_el[2].y() - m_el[1].z();
|
|
|
|
|
- temp.f[1] = m_el[0].z() - m_el[2].x();
|
|
|
|
|
- temp.f[2] = m_el[1].x() - m_el[0].y();
|
|
|
|
|
|
|
+ temp.f[0] = m_el[2].m_floats[1] - m_el[1].m_floats[2];
|
|
|
|
|
+ temp.f[1] = m_el[0].m_floats[2] - m_el[2].m_floats[0];
|
|
|
|
|
+ temp.f[2] = m_el[1].m_floats[0] - m_el[0].m_floats[1];
|
|
|
temp.f[3] = x;
|
|
temp.f[3] = x;
|
|
|
//temp.f[3]= s * btScalar(0.5);
|
|
//temp.f[3]= s * btScalar(0.5);
|
|
|
}
|
|
}
|
|
|
else
|
|
else
|
|
|
{
|
|
{
|
|
|
int i, j, k;
|
|
int i, j, k;
|
|
|
- if (m_el[0].x() < m_el[1].y())
|
|
|
|
|
|
|
+ if (m_el[0].m_floats[0] < m_el[1].m_floats[1])
|
|
|
{
|
|
{
|
|
|
- if (m_el[1].y() < m_el[2].z())
|
|
|
|
|
|
|
+ if (m_el[1].m_floats[1] < m_el[2].m_floats[2])
|
|
|
{
|
|
{
|
|
|
i = 2;
|
|
i = 2;
|
|
|
j = 0;
|
|
j = 0;
|
|
@@ -458,7 +458,7 @@ public:
|
|
|
}
|
|
}
|
|
|
else
|
|
else
|
|
|
{
|
|
{
|
|
|
- if (m_el[0].x() < m_el[2].z())
|
|
|
|
|
|
|
+ if (m_el[0].m_floats[0] < m_el[2].m_floats[2])
|
|
|
{
|
|
{
|
|
|
i = 2;
|
|
i = 2;
|
|
|
j = 0;
|
|
j = 0;
|
|
@@ -487,7 +487,7 @@ public:
|
|
|
|
|
|
|
|
q *= s;
|
|
q *= s;
|
|
|
#else
|
|
#else
|
|
|
- btScalar trace = m_el[0].x() + m_el[1].y() + m_el[2].z();
|
|
|
|
|
|
|
+ btScalar trace = m_el[0].m_floats[0] + m_el[1].m_floats[1] + m_el[2].m_floats[2];
|
|
|
|
|
|
|
|
btScalar temp[4];
|
|
btScalar temp[4];
|
|
|
|
|
|
|
@@ -497,13 +497,13 @@ public:
|
|
|
temp[3] = (s * btScalar(0.5));
|
|
temp[3] = (s * btScalar(0.5));
|
|
|
s = btScalar(0.5) / s;
|
|
s = btScalar(0.5) / s;
|
|
|
|
|
|
|
|
- temp[0] = ((m_el[2].y() - m_el[1].z()) * s);
|
|
|
|
|
- temp[1] = ((m_el[0].z() - m_el[2].x()) * s);
|
|
|
|
|
- temp[2] = ((m_el[1].x() - m_el[0].y()) * s);
|
|
|
|
|
|
|
+ temp[0] = ((m_el[2].m_floats[1] - m_el[1].m_floats[2]) * s);
|
|
|
|
|
+ temp[1] = ((m_el[0].m_floats[2] - m_el[2].m_floats[0]) * s);
|
|
|
|
|
+ temp[2] = ((m_el[1].m_floats[0] - m_el[0].m_floats[1]) * s);
|
|
|
}
|
|
}
|
|
|
else
|
|
else
|
|
|
{
|
|
{
|
|
|
- int i = m_el[0].x() < m_el[1].y() ? (m_el[1].y() < m_el[2].z() ? 2 : 1) : (m_el[0].x() < m_el[2].z() ? 2 : 0);
|
|
|
|
|
|
|
+ int i = m_el[0].m_floats[0] < m_el[1].m_floats[1] ? (m_el[1].m_floats[1] < m_el[2].m_floats[2] ? 2 : 1) : (m_el[0].m_floats[0] < m_el[2].m_floats[2] ? 2 : 0);
|
|
|
int j = (i + 1) % 3;
|
|
int j = (i + 1) % 3;
|
|
|
int k = (i + 2) % 3;
|
|
int k = (i + 2) % 3;
|
|
|
|
|
|
|
@@ -526,9 +526,9 @@ public:
|
|
|
void getEulerYPR(btScalar & yaw, btScalar & pitch, btScalar & roll) const
|
|
void getEulerYPR(btScalar & yaw, btScalar & pitch, btScalar & roll) const
|
|
|
{
|
|
{
|
|
|
// first use the normal calculus
|
|
// first use the normal calculus
|
|
|
- yaw = btScalar(btAtan2(m_el[1].x(), m_el[0].x()));
|
|
|
|
|
- pitch = btScalar(btAsin(-m_el[2].x()));
|
|
|
|
|
- roll = btScalar(btAtan2(m_el[2].y(), m_el[2].z()));
|
|
|
|
|
|
|
+ yaw = btScalar(btAtan2(m_el[1].m_floats[0], m_el[0].m_floats[0]));
|
|
|
|
|
+ pitch = btScalar(btAsin(-m_el[2].m_floats[0]));
|
|
|
|
|
+ roll = btScalar(btAtan2(m_el[2].m_floats[1], m_el[2].m_floats[2]));
|
|
|
|
|
|
|
|
// on pitch = +/-HalfPI
|
|
// on pitch = +/-HalfPI
|
|
|
if (btFabs(pitch) == SIMD_HALF_PI)
|
|
if (btFabs(pitch) == SIMD_HALF_PI)
|
|
@@ -564,14 +564,14 @@ public:
|
|
|
//get the pointer to the raw data
|
|
//get the pointer to the raw data
|
|
|
|
|
|
|
|
// Check that pitch is not at a singularity
|
|
// Check that pitch is not at a singularity
|
|
|
- if (btFabs(m_el[2].x()) >= 1)
|
|
|
|
|
|
|
+ if (btFabs(m_el[2].m_floats[0]) >= 1)
|
|
|
{
|
|
{
|
|
|
euler_out.yaw = 0;
|
|
euler_out.yaw = 0;
|
|
|
euler_out2.yaw = 0;
|
|
euler_out2.yaw = 0;
|
|
|
|
|
|
|
|
// From difference of angles formula
|
|
// From difference of angles formula
|
|
|
- btScalar delta = btAtan2(m_el[0].x(), m_el[0].z());
|
|
|
|
|
- if (m_el[2].x() > 0) //gimbal locked up
|
|
|
|
|
|
|
+ btScalar delta = btAtan2(m_el[0].m_floats[0], m_el[0].m_floats[2]);
|
|
|
|
|
+ if (m_el[2].m_floats[0] > 0) //gimbal locked up
|
|
|
{
|
|
{
|
|
|
euler_out.pitch = SIMD_PI / btScalar(2.0);
|
|
euler_out.pitch = SIMD_PI / btScalar(2.0);
|
|
|
euler_out2.pitch = SIMD_PI / btScalar(2.0);
|
|
euler_out2.pitch = SIMD_PI / btScalar(2.0);
|
|
@@ -588,18 +588,18 @@ public:
|
|
|
}
|
|
}
|
|
|
else
|
|
else
|
|
|
{
|
|
{
|
|
|
- euler_out.pitch = -btAsin(m_el[2].x());
|
|
|
|
|
|
|
+ euler_out.pitch = -btAsin(m_el[2].m_floats[0]);
|
|
|
euler_out2.pitch = SIMD_PI - euler_out.pitch;
|
|
euler_out2.pitch = SIMD_PI - euler_out.pitch;
|
|
|
|
|
|
|
|
- euler_out.roll = btAtan2(m_el[2].y() / btCos(euler_out.pitch),
|
|
|
|
|
- m_el[2].z() / btCos(euler_out.pitch));
|
|
|
|
|
- euler_out2.roll = btAtan2(m_el[2].y() / btCos(euler_out2.pitch),
|
|
|
|
|
- m_el[2].z() / btCos(euler_out2.pitch));
|
|
|
|
|
|
|
+ euler_out.roll = btAtan2(m_el[2].m_floats[1] / btCos(euler_out.pitch),
|
|
|
|
|
+ m_el[2].m_floats[2] / btCos(euler_out.pitch));
|
|
|
|
|
+ euler_out2.roll = btAtan2(m_el[2].m_floats[1] / btCos(euler_out2.pitch),
|
|
|
|
|
+ m_el[2].m_floats[2] / btCos(euler_out2.pitch));
|
|
|
|
|
|
|
|
- euler_out.yaw = btAtan2(m_el[1].x() / btCos(euler_out.pitch),
|
|
|
|
|
- m_el[0].x() / btCos(euler_out.pitch));
|
|
|
|
|
- euler_out2.yaw = btAtan2(m_el[1].x() / btCos(euler_out2.pitch),
|
|
|
|
|
- m_el[0].x() / btCos(euler_out2.pitch));
|
|
|
|
|
|
|
+ euler_out.yaw = btAtan2(m_el[1].m_floats[0] / btCos(euler_out.pitch),
|
|
|
|
|
+ m_el[0].m_floats[0] / btCos(euler_out.pitch));
|
|
|
|
|
+ euler_out2.yaw = btAtan2(m_el[1].m_floats[0] / btCos(euler_out2.pitch),
|
|
|
|
|
+ m_el[0].m_floats[0] / btCos(euler_out2.pitch));
|
|
|
}
|
|
}
|
|
|
|
|
|
|
|
if (solution_number == 1)
|
|
if (solution_number == 1)
|
|
@@ -625,9 +625,9 @@ public:
|
|
|
return btMatrix3x3(m_el[0] * s, m_el[1] * s, m_el[2] * s);
|
|
return btMatrix3x3(m_el[0] * s, m_el[1] * s, m_el[2] * s);
|
|
|
#else
|
|
#else
|
|
|
return btMatrix3x3(
|
|
return btMatrix3x3(
|
|
|
- m_el[0].x() * s.x(), m_el[0].y() * s.y(), m_el[0].z() * s.z(),
|
|
|
|
|
- m_el[1].x() * s.x(), m_el[1].y() * s.y(), m_el[1].z() * s.z(),
|
|
|
|
|
- m_el[2].x() * s.x(), m_el[2].y() * s.y(), m_el[2].z() * s.z());
|
|
|
|
|
|
|
+ m_el[0].m_floats[0] * s.m_floats[0], m_el[0].m_floats[1] * s.m_floats[1], m_el[0].m_floats[2] * s.m_floats[2],
|
|
|
|
|
+ m_el[1].m_floats[0] * s.m_floats[0], m_el[1].m_floats[1] * s.m_floats[1], m_el[1].m_floats[2] * s.m_floats[2],
|
|
|
|
|
+ m_el[2].m_floats[0] * s.m_floats[0], m_el[2].m_floats[1] * s.m_floats[1], m_el[2].m_floats[2] * s.m_floats[2]);
|
|
|
#endif
|
|
#endif
|
|
|
}
|
|
}
|
|
|
|
|
|
|
@@ -668,15 +668,15 @@ public:
|
|
|
|
|
|
|
|
SIMD_FORCE_INLINE btScalar tdotx(const btVector3& v) const
|
|
SIMD_FORCE_INLINE btScalar tdotx(const btVector3& v) const
|
|
|
{
|
|
{
|
|
|
- return m_el[0].x() * v.x() + m_el[1].x() * v.y() + m_el[2].x() * v.z();
|
|
|
|
|
|
|
+ return m_el[0].m_floats[0] * v.m_floats[0] + m_el[1].m_floats[0] * v.m_floats[1] + m_el[2].m_floats[0] * v.m_floats[2];
|
|
|
}
|
|
}
|
|
|
SIMD_FORCE_INLINE btScalar tdoty(const btVector3& v) const
|
|
SIMD_FORCE_INLINE btScalar tdoty(const btVector3& v) const
|
|
|
{
|
|
{
|
|
|
- return m_el[0].y() * v.x() + m_el[1].y() * v.y() + m_el[2].y() * v.z();
|
|
|
|
|
|
|
+ return m_el[0].m_floats[1] * v.m_floats[0] + m_el[1].m_floats[1] * v.m_floats[1] + m_el[2].m_floats[1] * v.m_floats[2];
|
|
|
}
|
|
}
|
|
|
SIMD_FORCE_INLINE btScalar tdotz(const btVector3& v) const
|
|
SIMD_FORCE_INLINE btScalar tdotz(const btVector3& v) const
|
|
|
{
|
|
{
|
|
|
- return m_el[0].z() * v.x() + m_el[1].z() * v.y() + m_el[2].z() * v.z();
|
|
|
|
|
|
|
+ return m_el[0].m_floats[2] * v.m_floats[0] + m_el[1].m_floats[2] * v.m_floats[1] + m_el[2].m_floats[2] * v.m_floats[2];
|
|
|
}
|
|
}
|
|
|
|
|
|
|
|
///extractRotation is from "A robust method to extract the rotational part of deformations"
|
|
///extractRotation is from "A robust method to extract the rotational part of deformations"
|
|
@@ -942,9 +942,9 @@ operator*(const btMatrix3x3& m, const btScalar& k)
|
|
|
vmulq_n_f32(m[2].mVec128, k));
|
|
vmulq_n_f32(m[2].mVec128, k));
|
|
|
#else
|
|
#else
|
|
|
return btMatrix3x3(
|
|
return btMatrix3x3(
|
|
|
- m[0].x() * k, m[0].y() * k, m[0].z() * k,
|
|
|
|
|
- m[1].x() * k, m[1].y() * k, m[1].z() * k,
|
|
|
|
|
- m[2].x() * k, m[2].y() * k, m[2].z() * k);
|
|
|
|
|
|
|
+ m[0].m_floats[0] * k, m[0].m_floats[1] * k, m[0].m_floats[2] * k,
|
|
|
|
|
+ m[1].m_floats[0] * k, m[1].m_floats[1] * k, m[1].m_floats[2] * k,
|
|
|
|
|
+ m[2].m_floats[0] * k, m[2].m_floats[1] * k, m[2].m_floats[2] * k);
|
|
|
#endif
|
|
#endif
|
|
|
}
|
|
}
|
|
|
|
|
|
|
@@ -1039,9 +1039,9 @@ btMatrix3x3::absolute() const
|
|
|
(float32x4_t)vandq_s32((int32x4_t)m_el[2].mVec128, btv3AbsMask));
|
|
(float32x4_t)vandq_s32((int32x4_t)m_el[2].mVec128, btv3AbsMask));
|
|
|
#else
|
|
#else
|
|
|
return btMatrix3x3(
|
|
return btMatrix3x3(
|
|
|
- btFabs(m_el[0].x()), btFabs(m_el[0].y()), btFabs(m_el[0].z()),
|
|
|
|
|
- btFabs(m_el[1].x()), btFabs(m_el[1].y()), btFabs(m_el[1].z()),
|
|
|
|
|
- btFabs(m_el[2].x()), btFabs(m_el[2].y()), btFabs(m_el[2].z()));
|
|
|
|
|
|
|
+ btFabs(m_el[0].m_floats[0]), btFabs(m_el[0].m_floats[1]), btFabs(m_el[0].m_floats[2]),
|
|
|
|
|
+ btFabs(m_el[1].m_floats[0]), btFabs(m_el[1].m_floats[1]), btFabs(m_el[1].m_floats[2]),
|
|
|
|
|
+ btFabs(m_el[2].m_floats[0]), btFabs(m_el[2].m_floats[1]), btFabs(m_el[2].m_floats[2]));
|
|
|
#endif
|
|
#endif
|
|
|
}
|
|
}
|
|
|
|
|
|
|
@@ -1075,9 +1075,9 @@ btMatrix3x3::transpose() const
|
|
|
float32x4_t v2 = vcombine_f32(vget_high_f32(top.val[0]), q); // z0 z1 z2 0
|
|
float32x4_t v2 = vcombine_f32(vget_high_f32(top.val[0]), q); // z0 z1 z2 0
|
|
|
return btMatrix3x3(v0, v1, v2);
|
|
return btMatrix3x3(v0, v1, v2);
|
|
|
#else
|
|
#else
|
|
|
- return btMatrix3x3(m_el[0].x(), m_el[1].x(), m_el[2].x(),
|
|
|
|
|
- m_el[0].y(), m_el[1].y(), m_el[2].y(),
|
|
|
|
|
- m_el[0].z(), m_el[1].z(), m_el[2].z());
|
|
|
|
|
|
|
+ return btMatrix3x3(m_el[0].m_floats[0], m_el[1].m_floats[0], m_el[2].m_floats[0],
|
|
|
|
|
+ m_el[0].m_floats[1], m_el[1].m_floats[1], m_el[2].m_floats[1],
|
|
|
|
|
+ m_el[0].m_floats[2], m_el[1].m_floats[2], m_el[2].m_floats[2]);
|
|
|
#endif
|
|
#endif
|
|
|
}
|
|
}
|
|
|
|
|
|
|
@@ -1097,9 +1097,9 @@ btMatrix3x3::inverse() const
|
|
|
//btFullAssert(det != btScalar(0.0));
|
|
//btFullAssert(det != btScalar(0.0));
|
|
|
btAssert(det != btScalar(0.0));
|
|
btAssert(det != btScalar(0.0));
|
|
|
btScalar s = btScalar(1.0) / det;
|
|
btScalar s = btScalar(1.0) / det;
|
|
|
- return btMatrix3x3(co.x() * s, cofac(0, 2, 2, 1) * s, cofac(0, 1, 1, 2) * s,
|
|
|
|
|
- co.y() * s, cofac(0, 0, 2, 2) * s, cofac(0, 2, 1, 0) * s,
|
|
|
|
|
- co.z() * s, cofac(0, 1, 2, 0) * s, cofac(0, 0, 1, 1) * s);
|
|
|
|
|
|
|
+ return btMatrix3x3(co.m_floats[0] * s, cofac(0, 2, 2, 1) * s, cofac(0, 1, 1, 2) * s,
|
|
|
|
|
+ co.m_floats[1] * s, cofac(0, 0, 2, 2) * s, cofac(0, 2, 1, 0) * s,
|
|
|
|
|
+ co.m_floats[2] * s, cofac(0, 1, 2, 0) * s, cofac(0, 0, 1, 1) * s);
|
|
|
}
|
|
}
|
|
|
|
|
|
|
|
SIMD_FORCE_INLINE btMatrix3x3
|
|
SIMD_FORCE_INLINE btMatrix3x3
|
|
@@ -1146,15 +1146,15 @@ btMatrix3x3::transposeTimes(const btMatrix3x3& m) const
|
|
|
return btMatrix3x3(r0, r1, r2);
|
|
return btMatrix3x3(r0, r1, r2);
|
|
|
#else
|
|
#else
|
|
|
return btMatrix3x3(
|
|
return btMatrix3x3(
|
|
|
- m_el[0].x() * m[0].x() + m_el[1].x() * m[1].x() + m_el[2].x() * m[2].x(),
|
|
|
|
|
- m_el[0].x() * m[0].y() + m_el[1].x() * m[1].y() + m_el[2].x() * m[2].y(),
|
|
|
|
|
- m_el[0].x() * m[0].z() + m_el[1].x() * m[1].z() + m_el[2].x() * m[2].z(),
|
|
|
|
|
- m_el[0].y() * m[0].x() + m_el[1].y() * m[1].x() + m_el[2].y() * m[2].x(),
|
|
|
|
|
- m_el[0].y() * m[0].y() + m_el[1].y() * m[1].y() + m_el[2].y() * m[2].y(),
|
|
|
|
|
- m_el[0].y() * m[0].z() + m_el[1].y() * m[1].z() + m_el[2].y() * m[2].z(),
|
|
|
|
|
- m_el[0].z() * m[0].x() + m_el[1].z() * m[1].x() + m_el[2].z() * m[2].x(),
|
|
|
|
|
- m_el[0].z() * m[0].y() + m_el[1].z() * m[1].y() + m_el[2].z() * m[2].y(),
|
|
|
|
|
- m_el[0].z() * m[0].z() + m_el[1].z() * m[1].z() + m_el[2].z() * m[2].z());
|
|
|
|
|
|
|
+ m_el[0].m_floats[0] * m[0].m_floats[0] + m_el[1].m_floats[0] * m[1].m_floats[0] + m_el[2].m_floats[0] * m[2].m_floats[0],
|
|
|
|
|
+ m_el[0].m_floats[0] * m[0].m_floats[1] + m_el[1].m_floats[0] * m[1].m_floats[1] + m_el[2].m_floats[0] * m[2].m_floats[1],
|
|
|
|
|
+ m_el[0].m_floats[0] * m[0].m_floats[2] + m_el[1].m_floats[0] * m[1].m_floats[2] + m_el[2].m_floats[0] * m[2].m_floats[2],
|
|
|
|
|
+ m_el[0].m_floats[1] * m[0].m_floats[0] + m_el[1].m_floats[1] * m[1].m_floats[0] + m_el[2].m_floats[1] * m[2].m_floats[0],
|
|
|
|
|
+ m_el[0].m_floats[1] * m[0].m_floats[1] + m_el[1].m_floats[1] * m[1].m_floats[1] + m_el[2].m_floats[1] * m[2].m_floats[1],
|
|
|
|
|
+ m_el[0].m_floats[1] * m[0].m_floats[2] + m_el[1].m_floats[1] * m[1].m_floats[2] + m_el[2].m_floats[1] * m[2].m_floats[2],
|
|
|
|
|
+ m_el[0].m_floats[2] * m[0].m_floats[0] + m_el[1].m_floats[2] * m[1].m_floats[0] + m_el[2].m_floats[2] * m[2].m_floats[0],
|
|
|
|
|
+ m_el[0].m_floats[2] * m[0].m_floats[1] + m_el[1].m_floats[2] * m[1].m_floats[1] + m_el[2].m_floats[2] * m[2].m_floats[1],
|
|
|
|
|
+ m_el[0].m_floats[2] * m[0].m_floats[2] + m_el[1].m_floats[2] * m[1].m_floats[2] + m_el[2].m_floats[2] * m[2].m_floats[2]);
|
|
|
#endif
|
|
#endif
|
|
|
}
|
|
}
|
|
|
|
|
|