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

36
simgear/math/simd4x4.hxx
View File

@ -51,29 +51,29 @@ inline void unit(simd4x4_t<T,N>& r) {
template<typename T>
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;
simd4_t<T,3> at = axis*t, as = axis*s;
T s = std::sin(angle);
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::unit(m);
for (int i=0; i<3; ++i) {
simd4_t<T,3> r = axis.ptr()[i]*at;
for (int j=0; j<3; ++j) {
m.m4x4()[0][j] = r.v4()[j];
}
}
simd4_t<T,3> aat = axis.ptr()[0]*at;
m.ptr()[0][0] = aat.ptr()[0] + c;
m.ptr()[0][1] = aat.ptr()[1] + as.ptr()[2];
m.ptr()[0][2] = aat.ptr()[2] - as.ptr()[1];
m.ptr()[0][0] += c;
m.ptr()[0][1] += as.ptr()[2];
m.ptr()[0][2] -= as.ptr()[1];
aat = axis.ptr()[1]*at;
m.ptr()[1][0] = aat.ptr()[0] - as.ptr()[2];
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];
m.ptr()[1][1] += c;
m.ptr()[1][2] += as.ptr()[0];
m.ptr()[2][0] += as.ptr()[1];
m.ptr()[2][1] -= as.ptr()[0];
m.ptr()[2][2] += c;
aat = axis.ptr()[2]*at;
m.ptr()[2][0] = aat.ptr()[0] + as.ptr()[1];
m.ptr()[2][1] = aat.ptr()[1] - as.ptr()[0];
m.ptr()[2][2] = aat.ptr()[2] + c;
return m;
}