Ragdoll physics in Codea

Ragdoll

--# Main
-- Ragdoll Physics

function setup()
    parameter.watch("1/DeltaTime")
    physics.continuous = true
    
    floor = physics.body(EDGE, vec2(0,0), vec2(WIDTH,0))
        
    DebugDraw = PhysicsDebugDraw()
    
    Ragdoll(WIDTH / 2, HEIGHT / 4)
    Ragdoll(WIDTH / 2, HEIGHT * 3/4)
    
    Bridge(WIDTH / 2, HEIGHT / 2, WIDTH)
end

function draw()
    background(40, 40, 50)

    noSmooth()
    
    DebugDraw:draw()
end

function touched(t)
    DebugDraw:touched(t)
end

--# Ragdoll
Ragdoll = class()


local bodyBox = function(width, height, x, y, tex)
    local box = physics.body(POLYGON, vec2(-width / 2, -height / 2), vec2(width / 2, -height / 2), vec2(width / 2, height / 2), vec2(-width / 2, height / 2))
    box.restitution = 0.1
    box.friction = 0.4
    box.bullet = true
    box.position = vec2(x, y)
    box.texture = tex

    return box
end

local bodyJoint = function(lower, upper, b1, b2, x, y)
    local joint = physics.joint(REVOLUTE, b1, b2, vec2(x, y))
    joint.enableLimit = true
    joint.lowerLimit = lower
    joint.upperLimit = upper
    
    return joint
end

function Ragdoll:init(x, y)
    self.bones, self.joints = {}, {}
    
    -- Sizes
    local headSize = 50
    local torsoWidth, torsoHeight = 30, 20
    local upperArmWidth, upperArmHeight = 13, 36
    local lowerArmWidth, lowerArmHeight = 12, 34
    local upperLegWidth, upperLegHeight = 15, 44
    local lowerLegWidth, lowerLegHeight = 12, 40
    
    --[[ Textures
    local headTexture = readImage("Space Art:UFO")
    local torsoTexture = readImage("Cargo Bot:Crate Blue 1")
    local upperArmTexture = readImage("Cargo Bot:Claw Arm")
    local lowerArmTexture = readImage("Cargo Bot:Claw Arm")
    local upperLegTexture = readImage("Cargo Bot:Claw Arm")
    local lowerLegTexture = readImage("Cargo Bot:Claw Arm")
    ]]
    
    -- Bones
    -- Head
    local head = physics.body(CIRCLE, headSize / 2)
    head.restitution = 0.300
    head.friction = 0.4
    head.bullet = true
    head.position = vec2(x, y)
    
    head.texture = headTexture
    
    self.bones[1] = head
    
    -- Torso
    self.bones[2] = bodyBox(torsoWidth, torsoHeight, x, y - headSize / 2 - torsoHeight / 2, torsoTexture)

    self.bones[3] = bodyBox(torsoWidth, torsoHeight, x, y - headSize / 2 - torsoHeight * 1.5, torsoTexture)

    self.bones[4] = bodyBox(torsoWidth, torsoHeight, x, y - headSize / 2 - torsoHeight * 2.5, torsoTexture)
    
    -- Arms
    self.bones[5] = bodyBox(upperArmWidth, upperArmHeight, x - torsoWidth / 2 - upperArmWidth / 2, y - headSize / 2 - upperArmHeight / 2, upperArmTexture)
    
    self.bones[6] = bodyBox(upperArmWidth, upperArmHeight, x + torsoWidth / 2 + upperArmWidth / 2, y - headSize / 2 - upperArmHeight / 2, upperArmTexture)
    
    self.bones[7] = bodyBox(lowerArmWidth, lowerArmHeight, x - torsoWidth / 2 - upperArmWidth / 2, y - headSize / 2 - upperArmHeight - lowerArmHeight / 2, lowerArmTexture)
    
    self.bones[8] = bodyBox(lowerArmWidth, lowerArmHeight, x + torsoWidth / 2 + upperArmWidth / 2, y - headSize / 2 - upperArmHeight - lowerArmHeight / 2, lowerArmTexture)
    
    -- Legs
    self.bones[9] = bodyBox(upperLegWidth, upperLegHeight, x - upperLegWidth / 2, y - headSize / 2 - torsoHeight * 3 - upperLegHeight / 2, upperLegTexture)
    
    self.bones[10] = bodyBox(upperLegWidth, upperLegHeight, x + upperLegWidth / 2, y - headSize / 2 - torsoHeight * 3 - upperLegHeight / 2, upperLegTexture)
    
    self.bones[11] = bodyBox(lowerLegWidth, lowerLegHeight, x - upperLegWidth / 2, y - headSize / 2 - torsoHeight * 3 - upperLegHeight - lowerLegHeight / 2, lowerLegTexture)
    
    self.bones[12] = bodyBox(lowerLegWidth, lowerLegHeight, x + upperLegWidth / 2, y - headSize / 2 - torsoHeight * 3 - upperLegHeight - lowerLegHeight / 2, lowerLegTexture)
    
    -- Joints
    
    -- Head to torso
    self.joints[1] = bodyJoint(-40, 40, self.bones[2], self.bones[1], x, y - headSize / 2)
    
    -- Upper arms to torso
    self.joints[2] = bodyJoint(-85, 130, self.bones[2], self.bones[5], x - torsoWidth / 2, y - headSize / 2)
    self.joints[3] = bodyJoint(-130, 85, self.bones[2], self.bones[6], x + torsoWidth / 2, y - headSize / 2)
    
    -- Upper arms to lower arms
    self.joints[4] = bodyJoint(-130, 10, self.bones[5], self.bones[7], x - torsoWidth / 2 - upperArmWidth / 2, y - headSize / 2 - upperArmHeight)
    self.joints[5] = bodyJoint(-10, 130, self.bones[6], self.bones[8], x + torsoWidth / 2 + upperArmWidth / 2, y - headSize / 2 - upperArmHeight)
    
    -- Torsos (Shoulders -> Stomach -> Hips)
    self.joints[6] = bodyJoint(-15, 15, self.bones[2], self.bones[3], x, y - headSize / 2 - torsoHeight)
    self.joints[7] = bodyJoint(-15, 15, self.bones[3], self.bones[4], x, y - headSize / 2 - torsoHeight * 2)
    
    -- Hips to upper legs
    self.joints[8] = bodyJoint(-25, 45, self.bones[4], self.bones[9], x - upperLegWidth / 2, y - headSize / 2 - torsoHeight * 3)
    self.joints[9] = bodyJoint(-45, 25, self.bones[4], self.bones[10], x + upperLegWidth / 2, y - headSize / 2 - torsoHeight * 3)
    
    -- Upper legs to lower legs
    self.joints[10] = bodyJoint(-25, 115, self.bones[9], self.bones[11], x - upperLegWidth / 2, y - headSize / 2 - torsoHeight * 3 - upperLegHeight)
    self.joints[11] = bodyJoint(-115, 25, self.bones[10], self.bones[12], x + upperLegWidth / 2, y - headSize / 2 - torsoHeight * 3 - upperLegHeight)
    
    
    
    for i, bone in pairs(self.bones) do
        DebugDraw:addBody(bone)
    end
    
    for i, joint in pairs(self.joints) do
        DebugDraw:addJoint(joint)
    end
end
--# Bridge
Bridge = class()

local plankWidth, plankHeight = 48, 10
local plankSpace = plankWidth / 4

local bridgeBox = function(x, y)
    local box = physics.body(POLYGON, vec2(-plankWidth / 2, -plankHeight / 2), vec2(plankWidth / 2, -plankHeight / 2), vec2(plankWidth / 2, plankHeight / 2), vec2(-plankWidth / 2, plankHeight / 2))
    box.restitution = 0.1
    box.density = 7.5
    box.friction = 0.2
    box.position = vec2(x, y)
    box.colour = color(0, 188, 255, 255)
    -- box.texture = readImage("Cargo Bot:Crate Blue 1")

    return box
end

local bridgeJoint = function(b1, b2, x, y)
    local joint = physics.joint(REVOLUTE, b1, b2, vec2(x, y)) 
    joint.enableLimit = true
    joint.lowerLimit = -15
    joint.upperLimit = 15
    
    return joint
end


function Bridge:init(x, y, width)
    self.planks, self.joints = {}, {}
    
    local numPlanks = math.floor(width / (plankWidth + (plankSpace / 2)))
        
    local numPlanks = 1
    local fullWidth = plankWidth
    while fullWidth + plankWidth + plankSpace <= width do
        numPlanks = numPlanks + 1
        fullWidth = fullWidth + plankWidth + plankSpace
    end
    
    local startX = x - fullWidth / 2 + plankWidth / 2
    
    -- Build planks and joints
    local anchorBody = physics.body(EDGE, vec2(0,0), vec2(0,0))
    anchorBody.position = vec2(startX, y)
    local prevBody = anchorBody
    for i = 0, numPlanks - 1 do
        local px = startX + plankWidth * i + plankSpace * i
        self.planks[i + 1] = bridgeBox(px, y)
        
        local anchor = px - plankWidth / 2 - plankSpace / 2
        self.joints[i + 1] = bridgeJoint(prevBody, self.planks[i + 1], anchor, y)
        
        prevBody = self.planks[i + 1]
        
        if i == numPlanks - 1 then
            self.joints[numPlanks + 1] = bridgeJoint(prevBody, anchorBody, px + plankWidth / 2 + plankSpace / 2, y)
        end
    end
    
    for i, plank in pairs(self.planks) do
        DebugDraw:addBody(plank)
    end
    
    for i, joint in pairs(self.joints) do
        DebugDraw:addJoint(joint)
    end
end
--# PhysicsDebugDraw
PhysicsDebugDraw = class()

function PhysicsDebugDraw:init()
    self.bodies = {}
    self.joints = {}
    self.touchMap = {}
    self.contacts = {}
end

function PhysicsDebugDraw:addBody(body, tex)
    tex = tex or body.texture
    
    if tex then
        local m = mesh()
        if body.shapeType == POLYGON then
            local mi = body.points[1]
            local ma = body.points[1]
            for k,v in ipairs(body.points) do
                mi.x = math.min(mi.x,v.x)
                mi.y = math.min(mi.y,v.y)
                ma.x = math.max(ma.x,v.x)
                ma.y = math.max(ma.y,v.y)
            end
            local vertices = triangulate(body.points)
            local texCoords = {}
            for k,v in ipairs(vertices) do
                u = v - mi
                u.x = u.x/(ma.x - mi.x)
                u.y = u.y/(ma.y - mi.y)
                table.insert(texCoords,u)
            end
            m.vertices = vertices
            m.texCoords = texCoords
            m:setColors(color(255))
            m.texture = tex 
        elseif body.shapeType == CIRCLE then
            m.texture = tex
            m:addRect(0, 0, body.radius * 2, body.radius * 2)
        end
        
        body.mesh = m
    end
    
    table.insert(self.bodies,body)
end

function PhysicsDebugDraw:addJoint(joint)
    table.insert(self.joints,joint)
end

function PhysicsDebugDraw:clear()
    -- deactivate all bodies
    
    for i,body in ipairs(self.bodies) do
        body:destroy()
    end
  
    for i,joint in ipairs(self.joints) do
        joint:destroy()
    end      
    
    self.bodies = {}
    self.joints = {}
    self.contacts = {}
    self.touchMap = {}
end

function PhysicsDebugDraw:draw()
    
    pushStyle()
    --smooth()
    strokeWidth(5)
    stroke(128,0,128)
    
    local gain = 2.0
    local damp = 0.5
    for k,v in pairs(self.touchMap) do
        local worldAnchor = v.body:getWorldPoint(v.anchor)
        local touchPoint = v.tp
        local diff = touchPoint - worldAnchor
        local vel = v.body:getLinearVelocityFromWorldPoint(worldAnchor)
        v.body:applyForce( (1/1) * diff * gain - vel * damp, worldAnchor)
        
        line(touchPoint.x, touchPoint.y, worldAnchor.x, worldAnchor.y)
    end
    
    
    stroke(255,255,255,255)
    noFill()
    
    
    for i,body in ipairs(self.bodies) do
        pushMatrix()
        translate(body.x, body.y)
        rotate(body.angle)
    
        if body.colour then
            stroke(body.colour)
        elseif body.type == STATIC then
            stroke(255,255,255,255)
        elseif body.type == DYNAMIC then
            stroke(150,255,150,255)
        elseif body.type == KINEMATIC then
            stroke(150,150,255,255)
        end
    
        if body.mesh then
            body.mesh:draw()
        elseif body.shapeType == POLYGON then
            strokeWidth(3.0)
            local points = body.points
            for j = 1,#points do
                a = points[j]
                b = points[(j % #points)+1]
                line(a.x, a.y, b.x, b.y)
            end
        elseif body.shapeType == CHAIN or body.shapeType == EDGE then
            strokeWidth(3.0)
            local points = body.points
            for j = 1,#points-1 do
                a = points[j]
                b = points[j+1]
                line(a.x, a.y, b.x, b.y)
            end      
        elseif body.shapeType == CIRCLE then
            strokeWidth(3.0)
            line(0,0,body.radius-3,0)            
            ellipse(0,0,body.radius*2)
        end
        
        popMatrix()
    end 
    
    stroke(255, 75, 0, 255)
    strokeWidth(5)
    for k,joint in pairs(self.joints) do
        local a = joint.anchorA
        local b = joint.anchorB
        line(a.x,a.y,b.x,b.y)
    end
    
    stroke(255, 0, 0, 255)
    fill(255, 0, 0, 255)

    for k,v in pairs(self.contacts) do
        for m,n in ipairs(v.points) do
            ellipse(n.x, n.y, 10, 10)
        end
    end
    
    popStyle()
end

function PhysicsDebugDraw:touched(touch)
    local touchPoint = vec2(touch.x, touch.y)
    if touch.state == BEGAN then
        for i,body in ipairs(self.bodies) do
            if body.type == DYNAMIC and body:testPoint(touchPoint) then
                self.touchMap[touch.id] = {tp = touchPoint, body = body, anchor = body:getLocalPoint(touchPoint)} 
                return true
            end
        end
    elseif touch.state == MOVING and self.touchMap[touch.id] then
        self.touchMap[touch.id].tp = touchPoint
        return true
    elseif touch.state == ENDED and self.touchMap[touch.id] then
        self.touchMap[touch.id] = nil
        return true;
    end
    return false
end

function PhysicsDebugDraw:collide(contact)
    if contact.state == BEGAN then
        self.contacts[contact.id] = contact
        sound(SOUND_HIT, 2643)
    elseif contact.state == MOVING then
        self.contacts[contact.id] = contact
    elseif contact.state == ENDED then
        self.contacts[contact.id] = nil
    end
end