FGFS-4.1/simgear
4
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Fix a bug in the rotation_matrix function

Browse Source
This commit is contained in:
Erik Hofman 2020-09-09 15:50:25 +02:00
parent 1341d1e1f2
commit 01a190fea8
1 changed files with 23 additions and 23 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

46
simgear/math/simd4x4.hxx
View File

@ -51,29 +51,29 @@ inline void unit(simd4x4_t<T,N>& r) {
template<typename T> template<typename T>
inline simd4x4_t<T,4> rotation_matrix(T angle, const simd4_t<T,3>& axis) inline simd4x4_t<T,4> rotation_matrix(T angle, const simd4_t<T,3>& axis)
{ {
T s = std::sin(angle), c = std::cos(angle), t = T(1)-c; T s = std::sin(angle);
simd4_t<T,3> at = axis*t, as = axis*s; T c = std::cos(angle);
T t = T(1) - c;
simd4_t<T,3> at = axis*t;
simd4_t<T,3> as = axis*s;
simd4x4_t<T,4> m; simd4x4_t<T,4> m;
simd4x4::unit(m); simd4x4::unit(m);
for (int i=0; i<3; ++i) { simd4_t<T,3> aat = axis.ptr()[0]*at;
simd4_t<T,3> r = axis.ptr()[i]*at; m.ptr()[0][0] = aat.ptr()[0] + c;
for (int j=0; j<3; ++j) { m.ptr()[0][1] = aat.ptr()[1] + as.ptr()[2];
m.m4x4()[0][j] = r.v4()[j]; m.ptr()[0][2] = aat.ptr()[2] - as.ptr()[1];
}
}
m.ptr()[0][0] += c; aat = axis.ptr()[1]*at;
m.ptr()[0][1] += as.ptr()[2]; m.ptr()[1][0] = aat.ptr()[0] - as.ptr()[2];
m.ptr()[0][2] -= as.ptr()[1]; m.ptr()[1][1] = aat.ptr()[1] + c;
m.ptr()[1][2] = aat.ptr()[2] + as.ptr()[0];
m.ptr()[1][0] -= as.ptr()[2]; aat = axis.ptr()[2]*at;
m.ptr()[1][1] += c; m.ptr()[2][0] = aat.ptr()[0] + as.ptr()[1];
m.ptr()[1][2] += as.ptr()[0]; m.ptr()[2][1] = aat.ptr()[1] - as.ptr()[0];
m.ptr()[2][2] = aat.ptr()[2] + c;
m.ptr()[2][0] += as.ptr()[1];
m.ptr()[2][1] -= as.ptr()[0];
m.ptr()[2][2] += c;
return m; return m;
} }
@ -260,9 +260,9 @@ public:
template<typename T, int N> template<typename T, int N>
inline simd4x4_t<T,N> operator-(simd4x4_t<T,N> m) { inline simd4x4_t<T,N> operator-(simd4x4_t<T,N> m) {
simd4x4_t<T,N> r; simd4x4_t<T,N> r;
simd4x4::zeros(r); simd4x4::zeros(r);
r -= m; r -= m;
return r; return r;
} }
@ -341,7 +341,7 @@ public:
inline const __m128 (&m4x4(void) const)[4] { inline const __m128 (&m4x4(void) const)[4] {
return simd4x4; return simd4x4;
} }
inline const float (&ptr(void) const)[4][4] { inline const float (&ptr(void) const)[4][4] {
return mtx; return mtx;
} }
@ -734,7 +734,7 @@ public:
simd4x4_t(double m00, double m01, double m02, double m03, simd4x4_t(double m00, double m01, double m02, double m03,
double m10, double m11, double m12, double m13, double m10, double m11, double m12, double m13,
double m20, double m21, double m22, double m23, double m20, double m21, double m22, double m23,
double m30, double m31, double m32, double m33) double m30, double m31, double m32, double m33)
{ {
simd4x4[0][0] = _mm_set_pd(m10,m00); simd4x4[0][0] = _mm_set_pd(m10,m00);
simd4x4[0][1] = _mm_set_pd(m30,m20); simd4x4[0][1] = _mm_set_pd(m30,m20);
@ -1035,7 +1035,7 @@ public:
inline const __m128i (&m4x4(void) const)[4] { inline const __m128i (&m4x4(void) const)[4] {
return simd4x4; return simd4x4;
} }
inline const int (&ptr(void) const)[4][4] { inline const int (&ptr(void) const)[4][4] {
return mtx; return mtx;
} }