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):

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
    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:

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

        # 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

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

    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:

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.



You must log in to answer this question.

Browse other questions tagged .