I'm working on fixing Godot's physics joints. Currently, it uses Euler angles and it doesn't help me in building active ragdolls for my game. I heard that using quaternions is the way to go. So I decided to write my own code to make it use quaternions instead. This is what I have working so far in my prototype in GDScript (c++ pending):
func _ready():
baseBTOrig = body_a.global_transform.basis.inverse() * body_b.global_transform.basis
func _physics_process(delta):
apply_rot_spring_quat(delta)
func calc_target_orientation(Abasis:Basis):
#node B's actual initial Transform that follows node A around in current global space
var baseBTOActual = Abasis*baseBTOrig
var qx = Quat(Abasis.x,rest_angle_x*PI/180.0)
var qy = Quat(Abasis.y,rest_angle_y*PI/180.0)
var qz = Quat(Abasis.z,rest_angle_z*PI/180.0)
var qBTargRo = qz*qy*qx# Quaternion node B Target Rotation
var BTargetBasis = Basis()
BTargetBasis.x = qBTargRo*baseBTOActual.x
BTargetBasis.y = qBTargRo*baseBTOActual.y
BTargetBasis.z = qBTargRo*baseBTOActual.z
return Quat(BTargetBasis)
"""
Thanks to:
DMGregory: https://gamedev.stackexchange.com/questions/182850/rotate-rigidbody-to-face-away-from-camera-with-addtorque/182873#182873
and
The Step Event: https://youtu.be/vewwP8Od_7s
For the calculations
"""
func apply_rot_spring_quat(delta):# apply spring rotation using quaternion
if Engine.editor_hint:
return
var bAV = Vector3()# Node B Angular Velocity
var aAV = Vector3()# Node A Angular Velocity
var bI = Basis() # Node B inverse inertia tensor
var aI = Basis() # Node A inverse inertia tensor
if body_b.is_class("RigidBody"):
bAV = body_b.angular_velocity
bI = body_b.get_inverse_inertia_tensor()
else:
bAV = Vector3(0.0,0.0,0.0)
if body_a.is_class("RigidBody"):
aAV = body_a.angular_velocity
aI = body_a.get_inverse_inertia_tensor()
else:
aAV = Vector3(0.0,0.0,0.0)
#Quaternion Node B Transform Basis
var qBT = Quat(body_b.global_transform.basis)
#Quaternion Target Orientation
var qTargetO = calc_target_orientation(body_a.global_transform.basis)
var rotChange = qTargetO * qBT.inverse() #rotation change quaternion
var angle = 2.0 * acos(rotChange.w)
#if node B's quat is already facing the same way as qTargetO the axis shoots to infinity
#this is my sorry ass attempt to protect the code from it
if(is_nan(angle)):
if body_b.is_class("RigidBody"):
body_b.add_torque(-bAV)
if body_a.is_class("RigidBody"):
body_a.add_torque(-aAV)
return
# rotation change quaternion's "V" component
var v = Vector3(rotChange.x,rotChange.y,rotChange.z)
var axis = v / sin(angle*0.5)# the quats axis
if(angle>PI):
angle -= 2.0*PI
#as node B's quat faces the same way as qTargetO the angle nears 0
#this slows it down to stop the axis from reaching infinity
if(is_equal_approx(angle,0.0)):
if body_b.is_class("RigidBody"):
body_b.add_torque(-bAV)
if body_a.is_class("RigidBody"):
body_a.add_torque(-aAV)
return
var targetAngVel = axis*angle/delta
var tb_consolidated = (stiffnessB)*(bI*targetAngVel) - dampingB*(bAV)
var ta_consolidated = -(stiffnessA)*(aI*targetAngVel) - dampingA*(aAV)
if body_b.is_class("RigidBody") and body_b != null:
body_b.add_torque(tb_consolidated)
if body_a.is_class("RigidBody") and body_a != null:
body_a.add_torque(ta_consolidated)
In short my computation is:
vec3 target_ang_vel = q_rotation_axis * q_angle / delta
vec3 angular_v_b = stiffness_b* inverse_inertia_tensor_b * target_ang_vel - damping_b * body_b.current_angular_velocity
vec3 angular_v_a = -stiffness_a* inverse_inertia_tensor_a * target_ang_vel - damping_a * body_a.current_angular_velocity
body_b.add_torque(angular_v_b)
body_a.add_torque(angular_v_a)
The problem is it spazzes out when the dampening and stiffness parameters are too high and the mass of either rigid body are too small.
Moreover, I tried attaching a long square bar with a mass of 50 on the other end of the joint (like an arm). It vibrated into the 4th dimension when I tried to make it twist and flex the arm upward:
rest_angle_x = -45
rest_angle_y = 0
rest_angle_z = 45
stiffness_b = Vec3(5000)
stiffness_a = Vec3(5000)
dampening_b = Vec3(5000)
dampening_a = Vec3(5000)
I tried doing the same thing using Godot's default joint settings. Sure it wasn't rotating the way I wanted it to but it didn't go crazy like how mine does:
Generic6DOFJoint:
angular_spring_(xyz)/damping = 5000
angular_spring_(xyz)/stiffness = 5000
Am I missing something? am I doing something wrong? I don't know where to start looking for a solution for this. I'd appreciate all the help that I can get and it would be great if someone could please point me to the right direction.
