python - 3D 弹簧 - 如何将适当的角度差输入弹簧拉伸 (F = -kx)

Question

我正在尝试使用四元数进行振荡运动（微动物理 - 点 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 - 好的，在 y 方向 +20 度 - 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()  

标签： pythonangularspringquaternionsmotion