Блог

Я пытаюсь совершить колебательное движение, используя кватернионы (физика покачивания — точки P, перемещающейся по сфере вокруг начала координат O).

Из моих исследований я знаю, что в 3D я могу рассчитать следующий шаг вращения q[n 1] по предыдущему вращению кватерниона и угловой скорости:

 q[n 1] = q[n]   0.5 * wq * q[n]   #  q[n] - quat rotation at n-th time step.
wq = Quaternion(0, w.x, w.y, w.z) # quaternion from angular velocity w
w = w   acc * dt # angular velocity from euler integration
acc = (-stiff*x   damp*w)/m # damped spring motion  m a =  - stiffness * x   damp * vel
 

x — различия между положением покоя и текущим положением. Мой вопрос в том, что мне подключить к x для 3D-вращения? Я попытался поместить различия во вращении между направлением покоя и текущим направлением как угол оси*(vec3) и как поворот Эйлера (vec3):

 x = rot_diff = rest_dir - current_dir  as  vec3_axis * angle
 

Даже при низкой жесткости и высоком демпфировании система взрывается, когда я поворачиваю rest_dir более чем на одну ось (например, поворот на 90 градусов X — ок,
20 градусов в y — sim становится нестабильным). Это заставляет меня задуматься, возможно, мне придется разделить уравнение ускорения на три компонента — поскольку вращение вокруг одной оси работает нормально… — может быть, относительно вектора rest_dir?

Код

 quat_diff = self.rest_dir.rotation_difference(self.current_dir)
axis, angle = quat_diff.to_axis_angle()        
diff_ang_vel = angle*axis


spring_force = - self.stiffness*diff_ang_vel - self.damping * self.ang_vel # F = -kx - bv - g*m
acc = spring_force / self.mass
self.ang_vel  = acc * dt
        
omega_quat = Quaternion((0, self.ang_vel.x, self.ang_vel.y, self.ang_vel.z))
self.current_dir  = 0.5 * omega_quat @ self.current_dir
self.current_dir.normalize()