local mathRad, mathCos, mathSin = math.rad, math.cos, math.sin

function findRotation ( x1, y1, x2, y2 )
    local t = -math.deg( math.atan2( x2 - x1, y2 - y1 ) )
    return t < 0 and t + 360 or t
end

function findPitch (startX,startY,startZ,endX,endY,endZ)

	local distanceX = startX - endX
	local distanceY = startZ - endZ -- Y = Z
	local distanceZ = startY - endY -- Z = Y 

	local pitch = math.atan2(distanceY, math.sqrt(math.pow(distanceZ,2) + math.pow(distanceX,2)))
	
	return pitch
end



function getNotStreamedInElementMatrix (element)
    local rx, ry, rz = getElementRotation(element, "ZXY")
    rx, ry, rz = mathRad(rx), mathRad(ry), mathRad(rz)
    local matrix = {}
    matrix[1] = {}
    matrix[1][1] = mathCos(rz)*mathCos(ry) - mathSin(rz)*mathSin(rx)*mathSin(ry)
    matrix[1][2] = mathCos(ry)*mathSin(rz) + mathCos(rz)*mathSin(rx)*mathSin(ry)
    matrix[1][3] = -mathCos(rx)*mathSin(ry)
    matrix[1][4] = 1

    matrix[2] = {}
    matrix[2][1] = -mathCos(rx)*mathSin(rz)
    matrix[2][2] = mathCos(rz)*mathCos(rx)
    matrix[2][3] = mathSin(rx)
    matrix[2][4] = 1

    matrix[3] = {}
    matrix[3][1] = mathCos(rz)*mathSin(ry) + mathCos(ry)*mathSin(rz)*mathSin(rx)
    matrix[3][2] = mathSin(rz)*mathSin(ry) - mathCos(rz)*mathCos(ry)*mathSin(rx)
    matrix[3][3] = mathCos(rx)*mathCos(ry)
    matrix[3][4] = 1

    matrix[4] = {}
    matrix[4][1], matrix[4][2], matrix[4][3] = getElementPosition(element)
    matrix[4][4] = 1

    return matrix
end

--[[
function createMatrix (x, y, z, rx, ry, rz)
    rx, ry, rz = mathRad(rx), mathRad(ry), mathRad(rz)
    local matrix = {}
    matrix[1] = {}
    matrix[1][1] = mathCos(rz)*mathCos(ry) - mathSin(rz)*mathSin(rx)*mathSin(ry)
    matrix[1][2] = mathCos(ry)*mathSin(rz) + mathCos(rz)*mathSin(rx)*mathSin(ry)
    matrix[1][3] = -mathCos(rx)*mathSin(ry)
    matrix[1][4] = 1

    matrix[2] = {}
    matrix[2][1] = -mathCos(rx)*mathSin(rz)
    matrix[2][2] = mathCos(rz)*mathCos(rx)
    matrix[2][3] = mathSin(rx)
    matrix[2][4] = 1

    matrix[3] = {}
    matrix[3][1] = mathCos(rz)*mathSin(ry) + mathCos(ry)*mathSin(rz)*mathSin(rx)
    matrix[3][2] = mathSin(rz)*mathSin(ry) - mathCos(rz)*mathCos(ry)*mathSin(rx)
    matrix[3][3] = mathCos(rx)*mathCos(ry)
    matrix[3][4] = 1

    matrix[4] = {}
    matrix[4][1], matrix[4][2], matrix[4][3] = x, y, z
    matrix[4][4] = 1

    return matrix
end]]

function createMatrix (x, y, z, rx, ry, rz)
    return Matrix ( Vector3(x, y, z), Vector3(rx, ry, rz) )
end



function getPositionFromElementOffset (element, offX, offY, offZ)
    local m = getElementMatrix ( element ) or getNotStreamedInElementMatrix(element)  -- Get the matrix
    local x = offX * m[1][1] + offY * m[2][1] + offZ * m[3][1] + m[4][1]  -- Apply transform
    local y = offX * m[1][2] + offY * m[2][2] + offZ * m[3][2] + m[4][2]
    local z = offX * m[1][3] + offY * m[2][3] + offZ * m[3][3] + m[4][3]
    return x, y, z                               -- Return the transformed point
end

--[[function getPositionFromMatrixOffset (m, offX, offY, offZ)
    local x = offX * m[1][1] + offY * m[2][1] + offZ * m[3][1] + m[4][1]  -- Apply transform
    local y = offX * m[1][2] + offY * m[2][2] + offZ * m[3][2] + m[4][2]
    local z = offX * m[1][3] + offY * m[2][3] + offZ * m[3][3] + m[4][3]
    return x, y, z                               -- Return the transformed point
end]]

function getPositionFromMatrixOffset (m, offX, offY, offZ)
    return unpackVector(m:transformPosition(Vector3(offX, offY, offZ)))
end


function unpackVector (vector)
    return vector.x, vector.y, vector.z
end








