RPCS3/rpcs3
1
0
Fork 0
mirror of https://github.com/RPCS3/rpcs3.git synced 2026-03-09 23:14:46 +01:00
Code Issues Actions 2 Packages Projects Releases Wiki Activity

cellGem: improve quaternion rotation

Browse source 
rotate_vector caused wrong results for default_orientation and 0,0,x
This commit is contained in:
Megamouse 2026-03-08 13:56:49 +01:00
parent c38bf70464
commit eeda9c7738
2 changed files with 46 additions and 23 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

46
rpcs3/Emu/Io/ps_move_data.cpp
View file

@ -24,34 +24,34 @@ void ps_move_data::reset_sensors()
angaccel_world = {};
}
ps_move_data::vect<3> ps_move_data::rotate_vector(const vect<4>& q, const vect<3>& v)
{
const auto cross = [](const vect<3>& a, const vect<3>& b)
{
return vect<3>({
a.y() * b.z() - a.z() * b.y(),
a.z() * b.x() - a.x() * b.z(),
a.x() * b.y() - a.y() * b.x()
});
};
// q = (x, y, z, w)
const vect<3> q_vec({q.x(), q.y(), q.z()});
// t = 2 * cross(q_vec, v)
const vect<3> t = cross(q_vec, v) * 2.0f;
// v' = v + w * t + cross(q_vec, t)
const vect<3> v_prime = v + t * q.w() + cross(q_vec, t);
return v_prime;
}
void ps_move_data::update_orientation(f32 delta_time)
{
if (!delta_time)
return;
// Rotate vector v by quaternion q
const auto rotate_vector = [](const vect<4>& q, const vect<3>& v)
{
const vect<4> qv({0.0f, v.x(), v.y(), v.z()});
const vect<4> q_inv({q.w(), -q.x(), -q.y(), -q.z()});
// t = q * v
vect<4> t;
t.w() = -q.x() * qv.x() - q.y() * qv.y() - q.z() * qv.z();
t.x() = q.w() * qv.x() + q.y() * qv.z() - q.z() * qv.y();
t.y() = q.w() * qv.y() - q.x() * qv.z() + q.z() * qv.x();
t.z() = q.w() * qv.z() + q.x() * qv.y() - q.y() * qv.x();
// r = t * q_inv
vect<4> r;
r.w() = -t.x() * q_inv.x() - t.y() * q_inv.y() - t.z() * q_inv.z();
r.x() = t.w() * q_inv.x() + t.y() * q_inv.z() - t.z() * q_inv.y();
r.y() = t.w() * q_inv.y() - t.x() * q_inv.z() + t.z() * q_inv.x();
r.z() = t.w() * q_inv.z() + t.x() * q_inv.y() - t.y() * q_inv.x();
return vect<3>({r.x(), r.y(), r.z()});
};
if constexpr (use_imu_for_velocity)
{
// Gravity in world frame

23
rpcs3/Emu/Io/ps_move_data.h
View file

@ -15,6 +15,26 @@ struct ps_move_data
template <typename I>
const T& operator[](I i) const { return data[i]; }
vect<Size, T> operator*(f32 s) const
{
vect<Size, T> result = *this;
for (int i = 0; i < Size; ++i)
{
result[i] *= s;
}
return result;
}
vect<Size, T> operator+(const vect<Size, T>& other) const
{
vect<Size, T> result = *this;
for (int i = 0; i < Size; ++i)
{
result[i] += other[i];
}
return result;
}
T x() const requires (Size >= 1) { return data[0]; }
T y() const requires (Size >= 2) { return data[1]; }
T z() const requires (Size >= 3) { return data[2]; }
@ -72,4 +92,7 @@ struct ps_move_data
void reset_sensors();
void update_orientation(f32 delta_time);
void update_velocity(u64 timestamp, be_t<f32> pos_world[4]);
// Rotate vector v by quaternion q
static vect<3> rotate_vector(const vect<4>& q, const vect<3>& v);
};