shuvit/scripts/ragdoll.py

import bge
#from mathutils import Vector
from math import radians
from mathutils import Quaternion, Vector, Matrix

armature = "Char4"

def get_bone_pose_matrix_cleaned(bone):
    offset_m4 = (Matrix.Translation(bone.location) * Quaternion(bone.rotation_quaternion).to_matrix().to_4x4())
    return bone.pose_matrix * offset_m4.inverted()

def zero_constraints(own, scene):
    armature = scene.objects['Char4']
    for const in armature.constraints:
        
        if 'rd_cl' in str(const):
            const.enforce = 0
            print(const)
        if 'rd_cr' in str(const):
            const.enforce = 0   
    for x in own['rd_rb'].groupMembers:
        x.suspendDynamics(True)

    
def update_rb(physBody, scene):
    #print('updating rigid body positions')
    armature = scene.objects['Char4']
    master = armature.parent
    master = scene.objects['control_cube.002']
    
    for pB in physBody.groupMembers: 
        if 'bone' in pB:   
            bone_name = pB["bone"]
            bone = armature.channels[bone_name]   
            pos = (bone.pose_head)+master.worldPosition
            rot = Vector(master.worldOrientation.to_euler()) * Vector(bone.channel_matrix.to_euler())    
            pose_bone = bone
            obj = armature
            matrix_final = obj.worldTransform * get_bone_pose_matrix_cleaned(bone)
            pB.worldTransform = matrix_final            
             
def max_constraints(own, scene, physBody):            
    armature = scene.objects['Char4']
    if armature.parent != None:
        master = armature.parent
        if not armature.parent.isSuspendDynamics:
            #armature.parent.suspendDynamics(True)        
            pass
    armature.removeParent()
    #print(armature.parent)
    #print(physBody.groupMembers)
    physBody.restoreDynamics()
    for pB in physBody.groupMembers:
        pB.restoreDynamics()
        if 'bone' in pB:
            bone_name = pB["bone"]
            bone = armature.channels[bone_name]
            if 'init' not in pB:
                pB["origin_pos"] = pB.worldPosition
                pB["origin_rot"] = Vector(pB.worldOrientation.to_euler())  
            pos = (bone.pose_head)+master.worldPosition
            rot = Vector(bone.channel_matrix.to_euler())   
                    
            for const in armature.constraints:
                if pB['bone'] in str(const) and ('rd_cl' in str(const) or 'rd_cr' in str(const)):
                    const.target = pB
                    const.enforce = 1
                    #print('enforcing', pB, const)
                else:
                    pass
        else:
            #print('no bone in', pB)            
            pass
    
    
def main():
    #print('doing main')
    cont = bge.logic.getCurrentController()
    own = cont.owner
    scene = bge.logic.getCurrentScene()
    sens = cont.sensors['rd_key']
    dict = bge.logic.globalDict
    
    if 'inited' not in own:
        own['inited'] = True
        own['ragdoll_active'] = False
        pB = scene.objectsInactive["npc_ed_arm_physBody"]  
        own['rd_rb'] = scene.addObject(pB)
        own['rd_rb'].suspendDynamics()
        own['rd_set_vel'] = False
        own['rd_vel'] = None
        own['rd_incer'] = 0
        zero_constraints(own, scene)    

    #if (sens.positive) or (dict['aBut'] == 1 and dict['bBut'] == 1) or own['fall'] == True:
    if (sens.positive) or own['fall'] == True:
        #print('option1')
        #own['fall'] = True
        if own['ragdoll_active'] == False:
            #own['fall'] = 1
            own['fall'] = True
            print('do ragdoll')
            incer = -1
            update_rb(own['rd_rb'], scene)   
            while incer < 20:
                incer += 1
                scene.objects[armature].stopAction(incer)
            pB = scene.objectsInactive["npc_ed_arm_physBody"]        
            physBody = own['rd_rb']
            physBody.worldPosition = own.worldPosition
            physBody.worldOrientation = own.worldOrientation
            own['rd_vel'] = own.worldLinearVelocity.copy()
            print('setting linvel', own['rd_vel'])
            own['rd_set_vel']  = True
            physBody.name = 'ragdollll'
            physBody['to_delete'] = True            
            own['rd_to_delete'] = physBody
            own['ragdoll_active'] = True
            max_constraints(own, scene, physBody)
            own.visible = False
            own['throw_deck'] = False
            cont.actuators['Camera'].height = 2.0
            #cont.actuators['Camera'].min = 14.0
            #cont.actuators['Camera'].max = 18.0
            cont.actuators['Camera'].damping = .000001
            cont.activate(cont.actuators['Camera']) 
                        
            #own.suspendDynamics()
            
    else:

        if own['ragdoll_active'] == True:
            #pass
            scene.objects[armature].stopAction(2)
            scene.objects[armature].stopAction(7)
            scene.objects[armature].update()
            cont.actuators['Camera'].object = scene.objects['ragdoll_parent']
            wp = scene.objects['camCube'].worldPosition - scene.objects['ragdoll_parent'].worldPosition
            wp = wp * .9
            cont.actuators['Camera'].height = 2.0
            cont.actuators['Camera'].min = 4.0
            cont.actuators['Camera'].max = 8.0
            cont.actuators['Camera'].damping = .000001
            cont.activate(cont.actuators['Camera']) 
            #print(own['rd_set_vel'], 'vel setter')
            if own['rd_set_vel'] == True:
                print(own['rd_incer'], 'rdincer', own['rd_vel'])
                
                scene.objects['npc_ed_arm_phys_master'].setLinearVelocity(own['rd_vel'] * 20, False)
                own['rd_set_vel'] = False
                
                if own['rd_incer'] > 20:
                    own['rd_incer'] += 1
                    #print(
                    #scene.objects['npc_ed_arm_phys_master'].setLinearVelocity(own['rd_vel'] * 4000, False)
                    #scene.objects['npc_ed_arm_phys_master'].worldLinearVelocity.x = 100
                    #scene.objects['npc_ed_arm_phys_master'].applyForce([5000,0,0], False)
                    for pB in own['rd_rb'].groupMembers:
                        print('setting', pB, own['rd_vel'])
                        
                        try:
                            #pass
                            #pB.applyForce([5000,0,500], False)
                            #pB.worldLinearVelocity.x = 100
                            #pB.worldLinearVelocity = own['rd_vel']
                            pB.setLinearVelocity(own['rd_vel'], False)
                            print('adding force to', pB, pB.worldLinearVelocity)
                        except:
                            print('cant add force to ', pB)
                else:
                    own['rd_incer'] = 0
                    own['rd_set_vel'] = False
            
            if dict['yBut'] == 1:                
                print('turn off ragdoll')
                zero_constraints(own, scene)
                scene.objects[armature].setParent(own, False, True)
                own.restoreDynamics()
                for x in own['rd_rb'].groupMembers:
                    x.suspendDynamics(True)
                own['ragdoll_active'] = False    
                print(scene.objects[armature].parent, 'parent')
                cont.actuators['Camera'].object = scene.objects['camCube']
                cont.activate(cont.actuators['Camera']) 
                own.worldPosition = scene.objects['npc_ed_arm_phys_master'].worldPosition
                own.worldPosition.z += .4
                cont.activate(cont.actuators['walk']) 


            if dict['lUD'] < -.08:
                physBody = own['rd_rb']
                excludes = ['npc_ed_arm_phys_hand.R', 'npc_ed_arm_phys_hand.L', 'npc_ed_arm_phys_forearm.fk.L', 'npc_ed_arm_phys_forearm.fk.R', 'npc_ed_arm_phys_upper_arm.fk.L', 'npc_ed_arm_phys_upper_arm.fk.R']
                #excludes = []
                for x in physBody.groupMembers:
                    print(x.name)
                                    
                    if x.name not in excludes:
                        pass
                        #x.applyForce([0,0,15], False)
                    else:
                        #x.applyForce([0,0,100], False)   
                        pass 

                scene.objects['npc_ed_arm_phys_master'].applyForce([0,0,500], False)
                scene.objects['npc_ed_arm_phys_head'].applyForce([0,0,400], False)
                scene.objects['npc_ed_arm_phys_chest'].applyForce([0,0,200], False)
                scene.objects['npc_ed_arm_phys_chest-1'].applyForce([0,0,200], False)
                scene.objects['npc_ed_arm_phys_spine'].applyForce([0,0,200], False)
                scene.objects['npc_ed_arm_phys_neck'].applyForce([0,0,200], False)
                scene.objects['npc_ed_arm_phys_deltoid.L'].applyForce([0,0,200], False)
                scene.objects['npc_ed_arm_phys_deltoid.R'].applyForce([0,0,200], False)
                scene.objects['npc_ed_arm_phys_clavicle.L'].applyForce([0,0,200], False)
                scene.objects['npc_ed_arm_phys_clavicle.R'].applyForce([0,0,200], False)

            if dict['rUD'] > .08:
                scene.objects['npc_ed_arm_phys_master'].applyForce([0,600,0], True)
        else:
            update_rb(own['rd_rb'], scene)   
            #pass