Physics Sandbox

Physics Sandbox

--# Main
-- Physics Sandbox

-- Use this function to perform your initial setup
function setup()
    displayMode(FULLSCREEN)
    lineCapMode(PROJECT)
    debugDraw = PhysicsDebugDraw()
    
    --1 is max bouncyness, 0 is no bounce
    globalRestitution = 0.5
    --friction when objects are sliding against each other
    globalFriction = 0.1
    
    
    --myFPSReporter = FPSReporter(4)
    drawnObjects = {}
    physicsObjects = {}
    ropeLocations = {}
    paused = true
    drawingOnlyMode = true
    physics.pause()
    debug = false
end

function createJoints()
    local crossedLines = {}
    local tripleCross = {}
    --check for crossed lines
    for k,v in pairs(physicsObjects) do
        local intersects = {}
        local intersectCount = 0
        local positiveIntersects = 0
        for k2,v2 in pairs(physicsObjects) do
            if k ~= k2 then
                if v.shapeType == CHAIN and v2.shapeType == CHAIN and v:testOverlap(v2) then
                    table.insert(intersects, {body = v2, positiveIntersect = false})
                    intersectCount = intersectCount + 1
                    if k < k2 then
                        intersects[intersectCount].positiveIntersect = true
                        positiveIntersects = positiveIntersects + 1
                    end        
                end
            end    
        end
        if positiveIntersects > 0 then
            for k2,v2 in pairs(intersects) do
                if v2.positiveIntersect then
                    local p1 = linesIntersect(v.points[1]+v.position, v.points[2]+v.position, v2.body.points[1]+ v2.body.position, v2.body.points[2]+v2.body.position)
                    table.insert(crossedLines, {p = p1, body1 = v, body2 = v2.body, body1IsRope = false, body2IsRope = false})
                end
            end
        end
        if intersectCount == 2 then
            local p1 = linesIntersect(v.points[1]+v.position, v.points[2]+v.position, intersects[1].body.points[1]+ intersects[1].body.position, intersects[1].body.points[2]+intersects[1].body.position)
            local p2 = linesIntersect(v.points[1]+v.position, v.points[2]+v.position, intersects[2].body.points[1]+ intersects[2].body.position, intersects[2].body.points[2]+intersects[2].body.position)
            table.insert(tripleCross, {p1 = p1, p2 = p2, body1 = intersects[1].body, body2 = intersects[2].body, joinBody = v})
        end 
    end
    for k,v in pairs(crossedLines) do
        for k2,v2 in pairs(tripleCross) do
            if v.body1 == v2.joinBody then
                v.body1IsRope = true
            end
            if v2.joinBody == v.body2 then
                v.body2IsRope = true 
            end
        end
    end
    for k,v in pairs(physicsObjects) do
        if v.shapeType == CHAIN then
            --if it's a solo line then static objects
            local soloLine = true
            for k2,v2 in pairs(crossedLines) do
                if v == v2.body1 or v == v2.body2 then
                    soloLine = false
                end 
            end
            for k2,v2 in pairs(tripleCross) do
                if v == v2.body1 or v == v2.body2 or v == v2.joinBody then
                    soloLine = false
                end
            end
            if soloLine then
                local p1 = v.points[1] + vec2(v.x, v.y)
                local p2 = v.points[2] + vec2(v.x, v.y)
                
                for k2,v2 in pairs(physicsObjects) do
                    if v2:testOverlap(v) and v2.shapeType ~= CHAIN then
                        v2.type = STATIC
                    end
                end
            end
        end
    end
    --now process crossed lines as rotating joints        
    for k,v in pairs(crossedLines) do
        local countObjects = 0
        local joinedObjects = {}
        for k2,v2 in pairs(physicsObjects) do        
            if v2:testPoint(v.p) and v2.shapeType ~= CHAIN then
                countObjects = countObjects + 1
                if not v.body1IsRope then
                    local joint = physics.joint(REVOLUTE, v.body1, v2, v.p)
                    debugDraw:addJoint(joint)
                end
                if not v.body2IsRope then
                    local joint = physics.joint(REVOLUTE, v.body2, v2, v.p)
                    debugDraw:addJoint(joint)
                end
                --must be something in the multiple objects on joint that fails on multi start/stops
                if countObjects > 1 then
                    for k3,v3 in pairs (joinedObjects) do
                        local joint = physics.joint(REVOLUTE, v2, v3, v.p)
                        debugDraw:addJoint(joint)
                    end
                end
                table.insert(joinedObjects, v2)
            end
        end
        --if it's multiple objects let the crossed lines float
        if countObjects > 1 or ((v.body1IsRope or v.body2IsRope) and countObjects > 0) then
            v.body1.type = DYNAMIC
            v.body2.type = DYNAMIC
        end
    end
    --now process tripleCross as rope joints including destroying the joinBody
    for k,v in pairs(tripleCross) do
        table.insert(ropeLocations, {body1 = v.body1, body2 = v.body2, p1 = v.p1 - v.body1.position, p2 = v.p2 - v.body2.position })
        for k2,v2 in pairs(debugDraw.bodies) do
            if v2 == v.joinBody then
                table.remove(debugDraw.bodies, k2)
            end
        end
        for k2,v2 in pairs(physicsObjects) do
            if v2 == v.joinBody then
                table.remove(physicsObjects, k2) 
            end
        end 
        
        local joint = physics.joint(ROPE, v.body1, v.body2, v.p1, v.p2, v.p1:dist(v.p2))
        debugDraw:addJoint(joint)
        --I think this is better, but I'll ignore it as it causes a crash
        print(v.joinBody)
        --v.joinBody:destroy()
    end
end

function touched(touch)
    local draw
    if touch.x > WIDTH - 50 and touch.y < 50 and touch.state == BEGAN then
        drawingOnlyMode = not drawingOnlyMode
        return nil        
    end
    if touch.state == BEGAN and touch.x > WIDTH/2-40 and touch.x < WIDTH/2+40 and touch.y < 40 then
        if paused == true then 
            paused = false
            createJoints()
            physics.resume()
        else
            paused = true
            physics.pause()
            resetObjects()
        end   
        return nil
    end  
    if touch.state == BEGAN and touch.x < 100 and touch.y > HEIGHT - 50 then
        physicsObjects = {}
        debugDraw:clear()
        return nil
    end
          
    if not drawingOnlyMode then
        debugDraw:touched(touch)
    else
        if touch.state == BEGAN then
            drawnObjects[touch.id] = DrawnObject(vec2(touch.x, touch.y))
        elseif touch.state == MOVING then
            if drawnObjects[touch.id] ~= nil then
                drawnObjects[touch.id]:addPoint(vec2(touch.x, touch.y))
            end
        elseif touch.state == ENDED then
            if drawnObjects[touch.id] ~= nil then
                local object = drawnObjects[touch.id]:evaluateShape()
                if object ~= nil then
                    --general settings
                    object.restitution = globalRestitution
                    object.friction = globalFriction
                    table.insert(physicsObjects, object)
                    debugDraw:addBody(object)
                end
                drawnObjects[touch.id] = nil
            end
        end
    end
end

function resetObjects()
    for i,body in ipairs(physicsObjects) do
        if body.shapeType ~= CHAIN then
            body.type = DYNAMIC
        else
            body.type = STATIC
        end
    end
    --recreate chains for rope joints
    for k,v in pairs(ropeLocations) do
        local body = physics.body(CHAIN, false, v.p1 + v.body1.position, v.p2 + v.body2.position)
        body.type = STATIC
        table.insert(physicsObjects, body)
        debugDraw:addBody(body)
    end
    ropeLocations = {}
    debugDraw:removeJoints() 
    debug = true
end

-- This function gets called once every frame
function draw()
    -- This sets a dark background color 
    background(40, 40, 50)

    -- This sets the line thickness
    strokeWidth(5)
    color(255,255,255,255)
    for k,v in pairs(drawnObjects) do
        v:draw()
    end
    --draw the physics objects
    debugDraw:draw()
    
    for k,v in pairs(physicsObjects) do
        if v.shapeType == EDGE then
            local p1 = v.points[1] + vec2(v.x, v.y)
            local p2 = v.points[2] + vec2(v.x, v.y)
            ellipse(p1.x,p1.y,10,10)
            ellipse(p2.x,p2.y,10,10)    
        end
    end
    if paused then
        sprite("Cargo Bot:Play Button", WIDTH/2, 83/4, 165/2, 83/2)
    else
        sprite("Cargo Bot:Stop Button", WIDTH/2, 83/4, 165/2, 83/2)
    end
    if drawingOnlyMode then
        sprite("Cargo Bot:Command Left", WIDTH-25, 30)
    else
        sprite("Cargo Bot:Command Right", WIDTH-25, 30)
    end
    sprite("Cargo Bot:Clear Button",48,HEIGHT-24)
    -- Do your drawing here
    --myFPSReporter:draw(3)
    
end





--# DrawnObject
DrawnObject = class()

function DrawnObject:init(firstPoint)
    -- you can accept and set parameters here
    self.points = {}
    self.numPoints = 1
    self.points[1] = firstPoint
end

function DrawnObject:addPoint(newPoint)
    self.numPoints = self.numPoints + 1
    self.points[self.numPoints] = newPoint
end

function DrawnObject:draw()
    -- Codea does not automatically call this method
    for i=2,self.numPoints do
        line(self.points[i-1].x, self.points[i-1].y, self.points[i].x, self.points[i].y)
    end
end

function DrawnObject:evaluateShape()
    if self.numPoints == 1 then
        return nil
    end
    --angle threshold for saying it's near enough straight
    local threshold = 25
    local vOrigin = vec2(1,0)
    local angles = {}
    local foundAngle
    local currentAngle = 1
    table.insert(angles, {angle = math.deg( vOrigin:angleBetween( self.points[2] - self.points[1] )), firstPoint = 1, lastPoint = 2 })
    --loop through the line segements, and collect their angles
    for i=3, self.numPoints do
        local angle = math.deg( vOrigin:angleBetween( self.points[i] - self.points[i-1] ))
        --print(angle.."vs"..angles[currentAngle].angle.."-"..#angles)
        if angle > (angles[currentAngle].angle)-threshold and angle < (angles[currentAngle].angle)+threshold then
            angles[currentAngle].lastPoint = i
            angles[currentAngle].angle = math.deg(vOrigin:angleBetween( self.points[angles[currentAngle].lastPoint] - self.points[angles[currentAngle].firstPoint]))
        else
            table.insert(angles, {angle = angle, firstPoint = i-1, lastPoint = i})
            currentAngle = currentAngle + 1
        end
    end
    local avg = 0
    --loop through the collected angles to get an average count of segements per angle
    for i,v in ipairs(angles) do
        avg = avg + self.points[v.firstPoint]:dist(self.points[v.lastPoint])
        --avg = avg + v.count obsolete move to distance
    end
    
    --set threshold to discard angle segments which are too small (eg corners)
    print("avg:"..avg)
    --avg = avg * .05
    avg = (avg / currentAngle) / 2
    --avg = ((avg / #angles) / 2) - 1 obsolete move to distance
    print("avg factored:"..avg)
    local sides = {}
    --loop through our angle segments and count the number of sides that have enough points
    --***improve this, base on length of side rather than points?
    for i,v in ipairs(angles) do
        local dist = self.points[v.firstPoint]:dist(self.points[v.lastPoint])
        if dist > avg then 
            print(v.angle..":"..dist)
            table.insert(sides, v)
            --debug
            --local bdeb = physics.body(EDGE, self.points[v.firstPoint], self.points[v.lastPoint])
            --bdeb.type = DYNAMIC
            --debugDraw:addBody(bdeb)
        else 
            print("discrad:"..v.angle..":"..dist)
        end
    end
    
    --establish object parameters based on number of sides
    if #sides == 1 then
        print("line")
        local body = physics.body(CHAIN, false, self.points[sides[1].firstPoint], self.points[sides[1].lastPoint])
        body.type = STATIC
        return body
    end
    --check it's a closed object
    if self.points[1]:dist(self.points[#self.points]) < 50 then
        if #sides == 3 then
            print("triangle")
            local p1 = linesIntersect(self.points[sides[1].firstPoint], self.points[sides[1].lastPoint],self.points[sides[2].firstPoint], self.points[sides[2].lastPoint])
            local p2 = linesIntersect(self.points[sides[1].firstPoint], self.points[sides[1].lastPoint],self.points[sides[3].firstPoint], self.points[sides[3].lastPoint])
            local p3 = linesIntersect(self.points[sides[3].firstPoint], self.points[sides[3].lastPoint],self.points[sides[2].firstPoint], self.points[sides[2].lastPoint])
            local centreVec = (p1 + p2 + p3)/3
            local body = physics.body(POLYGON, p1-centreVec, p2-centreVec, p3-centreVec)
            body.x = centreVec.x
            body.y = centreVec.y
            return body
        end
        if #sides == 4 then
            print("rectangle")
            p1 = linesIntersect(self.points[sides[1].firstPoint], self.points[sides[1].lastPoint], self.points[sides[2].firstPoint], self.points[sides[2].lastPoint])
            p2 = linesIntersect(self.points[sides[3].firstPoint], self.points[sides[3].lastPoint], self.points[sides[2].firstPoint], self.points[sides[2].lastPoint])
            p3 = linesIntersect(self.points[sides[3].firstPoint], self.points[sides[3].lastPoint], self.points[sides[4].firstPoint], self.points[sides[4].lastPoint])
            p4 = linesIntersect(self.points[sides[1].firstPoint], self.points[sides[1].lastPoint], self.points[sides[4].firstPoint], self.points[sides[4].lastPoint])
            if self.points[sides[1].firstPoint]:dist(self.points[sides[1].lastPoint]) > self.points[sides[2].firstPoint]:dist(self.points[sides[2].lastPoint]) then
                angle = (sides[1].angle + sides[3].angle) / 2
            else
                angle = (sides[2].angle + sides[4].angle) / 2
                ph = p1
                p1 = p2
                p2 = p3
                p3 = p4
                p4 = ph
            end
            local centreVec = (p1+p2+p3+p4)/4
            p1 = (p1 - centreVec):rotate(math.rad(-angle))
            p2 = (p2 - centreVec):rotate(math.rad(-angle))
            p3 = (p3 - centreVec):rotate(math.rad(-angle))
            p4 = (p4 - centreVec):rotate(math.rad(-angle))
            x = (math.abs(p1.x) + math.abs(p2.x) + math.abs(p3.x) + math.abs(p4.x)) / 4
            y = (math.abs(p1.y) + math.abs(p2.y) + math.abs(p3.y) + math.abs(p4.y)) / 4
            local body = physics.body(POLYGON, vec2(-x,-y), vec2(x,-y), vec2(x,y), vec2(-x,y))
            body.angle = angle
            body.x = centreVec.x
            body.y = centreVec.y
            return body
        end
        if #sides > 5 then
            print("circle")
            local centreVec = vec2(0,0)
            for i,v in ipairs(self.points) do
                centreVec = centreVec + v
            end
            centreVec = centreVec / #self.points
            local radius = 0
            for i,v in ipairs(self.points) do
                radius = radius + centreVec:dist(v)
            end
            radius = radius / #self.points
            local body = physics.body(CIRCLE, radius)
            body.x = centreVec.x
            body.y = centreVec.y
            return body
        end
    else
        print("unclosed")
    end
    return nil
end
--# PhysicsDebugDraw
PhysicsDebugDraw = class()

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

function PhysicsDebugDraw:addBody(body)
    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:removeJoints()
    for i,joint in ipairs(self.joints) do
        joint:destroy()
    end      
    self.joints = {}
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
        if paused then
            v.body.x = v.tp.x
            v.body.y = v.tp.y
        else
            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
    end
    
    stroke(0,255,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,255,255,255)
    noFill()
    
    
    for i,body in ipairs(self.bodies) do
        pushMatrix()
        translate(body.x, body.y)
        rotate(body.angle)
    
        if 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.shapeType == POLYGON then
            strokeWidth(5.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(5.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(5.0)
            line(0,0,body.radius-3,0)
            strokeWidth(2.5)
            ellipse(0,0,body.radius*2)
        end
        
        popMatrix()
    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: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

--# Utils
function linesIntersect(line1V1, line1V2, line2V1, line2V2)
    --Line1
    local A1 = line1V2.y - line1V1.y
    local B1 = line1V1.x - line1V2.x
    local C1 = A1*line1V1.x + B1*line1V1.y

    --Line2
    local A2 = line2V2.y - line2V1.y
    local B2 = line2V1.x - line2V2.x
    local C2 = A2 * line2V1.x + B2 * line2V1.y

    local det = A1*B2 - A2*B1
    if det == 0 then
        return nil
        --parallel lines
    else
        local x = (B2*C1 - B1*C2)/det
        local y = (A1 * C2 - A2 * C1) / det
        return vec2(x,y)
    end
end
    
--# TODO
--[[

modify the movement mode to allow for drag movement of lines (some form of select if close)
 possibly do this by creating a notional circle for touch and then any object that intersects the circle gets touched

improve shape recognition, currently circles sometimes produce squares and square sometimes fail (2 sides) or produce triangles.  Needs a better side culling algorithm

ability to delete objects (drag to trash can?) and destroy objects that have fallen off the screen

consider the drawing for rope... so that it is a loose chain or something when it goes less than the rope length
consider retaining rope length across pauses

consider other joints, like sliding joints...

add icons for draw/movement mode

]]--