# How do you compute for angular spring physics for physics joints in game engines?

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:
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"):
if body_a.is_class("RigidBody"):
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"):
if body_a.is_class("RigidBody"):
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:

if body_a.is_class("RigidBody") and body_a != null:


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



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.