From 7f7275b7dc8308709e97151e24175e23087b1d35 Mon Sep 17 00:00:00 2001 From: SantaSpeen Date: Sun, 25 Sep 2022 14:01:17 +0300 Subject: [PATCH] Steady! Ready? Lua! --- .gitattributes | 2 + .gitignore | 50 + README.md | 2 + Resources/Server/AdminTools/AdminTools.lua | 50 + Resources/Server/AdminTools/config.json | 10 + lua/graphpath.lua | 1012 ++++++++++++++++++++ lua/json.lua | 219 +++++ lua/mathlib.lua | 995 +++++++++++++++++++ 8 files changed, 2340 insertions(+) create mode 100644 .gitattributes create mode 100644 .gitignore create mode 100644 README.md create mode 100644 Resources/Server/AdminTools/AdminTools.lua create mode 100644 Resources/Server/AdminTools/config.json create mode 100644 lua/graphpath.lua create mode 100644 lua/json.lua create mode 100644 lua/mathlib.lua diff --git a/.gitattributes b/.gitattributes new file mode 100644 index 0000000..dfe0770 --- /dev/null +++ b/.gitattributes @@ -0,0 +1,2 @@ +# Auto detect text files and perform LF normalization +* text=auto diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..61ddf62 --- /dev/null +++ b/.gitignore @@ -0,0 +1,50 @@ +# Compiled Lua sources +luac.out + +# luarocks build files +*.src.rock +*.zip +*.tar.gz + +# Object files +*.o +*.os +*.ko +*.obj +*.elf + +# Precompiled Headers +*.gch +*.pch + +# Libraries +*.lib +*.a +*.la +*.lo +*.def +*.exp + +# Shared objects (inc. Windows DLLs) +*.dll +*.so +*.so.* +*.dylib + +# Executables +*.exe +*.out +*.app +*.i*86 +*.x86_64 +*.hex + +# Idea +.idea/ + +# BeamMP +.sentry-native/ +BeamMP-Server.exe +ServerConfig.toml +*.log +trafficSync/ diff --git a/README.md b/README.md new file mode 100644 index 0000000..7c06f94 --- /dev/null +++ b/README.md @@ -0,0 +1,2 @@ +# TrafficSync-BeamMP for +Mod to sync traffic signals like those found on East Coast USA on BeamMP servers diff --git a/Resources/Server/AdminTools/AdminTools.lua b/Resources/Server/AdminTools/AdminTools.lua new file mode 100644 index 0000000..87fc73f --- /dev/null +++ b/Resources/Server/AdminTools/AdminTools.lua @@ -0,0 +1,50 @@ +--- +--- Created by SantaSpeen. +--- DateTime: 25.09.2022 13:38 +--- +jsonLib = require('json') + +pluginConfigFile = "config.json" +pluginName = "AdminTools" + +pluginPath = debug.getinfo(1).source:gsub("\\","/") +pluginPath = string.gsub(pluginPath, pluginName + ".lua", "") + +-- -- -- -- -- Init functions -- -- -- -- -- + +function log(...) + print("[AdminTools] " .. tostring(...)) +end + +function stringToTable(str, sep) + if sep == nil then + sep = "%s" + end + local t = {} + local i = 1 + for s in string.gmatch(str, "([^"..sep.."]+)") do + t[i] = s + i = i + 1 + end + return t +end + +function jsonReadFile(path) + local jsonFile, error = io.open(path,"r") + if error then return nil, error end + local jsonText = jsonFile:read("*a") + jsonFile:close() + return jsonLib.parse(jsonText), false +end +-- -- -- -- -- -- -- -- -- -- -- -- -- -- -- + + +function onInit() + log("Start init") + log("Plugin path: " .. pluginPath) + log("Reading config..") + if jsonReadFile(pluginConfigFile) then + + end + log("Init complete.") +end diff --git a/Resources/Server/AdminTools/config.json b/Resources/Server/AdminTools/config.json new file mode 100644 index 0000000..372969e --- /dev/null +++ b/Resources/Server/AdminTools/config.json @@ -0,0 +1,10 @@ +{ + "admins": [], + "prefix": "/", + "database": { + "mode": "SQLite", + "host": "", + "login": "", + "password": "" + } +} \ No newline at end of file diff --git a/lua/graphpath.lua b/lua/graphpath.lua new file mode 100644 index 0000000..390c559 --- /dev/null +++ b/lua/graphpath.lua @@ -0,0 +1,1012 @@ +--[[ +Copyright (c) 2012 Hello!Game, 2015 BeamNG GmbH + +Permission is hereby granted, free of charge, to any person obtaining a copy +of newinst software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is furnished +to do so, subject to the following conditions: + +The above copyright notice and newinst permission notice shall be included in all +copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, +INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A +PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT +HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION +OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE +OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. +]] + +---------------------------------------------------------------- +-- example : +--[[ +gp = newGraphpath() +gp:edge("a", "b", 7) +gp:edge("a", "c", 9) +gp:edge("a", "f", 14) +gp:edge("b", "d", 15) +gp:edge("b", "c", 10) +gp:edge("c", "d", 11) +gp:edge("c", "f", 2) +gp:edge("d", "e", 6) +gp:edge("e", "f", 9) + +print( table.concat( gp:getPath("a","e"), "->") ) +]] + +require('mathlib') +--local bit = require "bit" + +local tableInsert, min, max, random = table.insert, math.min, math.max, math.random + +local M = {} + +local minheap = {} +minheap.__index = minheap + +local function newMinheap() + return setmetatable({length = 0, vals = {}}, minheap) +end + +function minheap:peekKey() + return self[1] +end + +function minheap:empty() + return self.length == 0 +end + +function minheap:clear() + table.clear(self.vals) + self.length = 0 +end + +function minheap:insert(k, v) + local vals = self.vals + -- float the new key up from the bottom of the heap + local child_index = self.length + 1 -- array index of the new child node to be added to heap + self.length = child_index -- update the central heap length record + + while child_index > 1 do + local parent_index = child_index/2--rshift(child_index, 1) + local parent_key = self[parent_index] + if k >= parent_key then + break + else + self[child_index], vals[child_index] = parent_key, vals[parent_index] + child_index = parent_index + end + end + + self[child_index], vals[child_index] = k, v +end + +function minheap:pop() + if self.length <= 0 then return end + local vals = self.vals + local result_key, result_val = self[1], vals[1] -- get top value + local heapLength = self.length + local last_key, last_val = self[heapLength], vals[heapLength] + heapLength = heapLength - 1 + local child_index = 2 + local parent_index = 1 + + while child_index <= heapLength do + local next_child = child_index + 1 + if next_child <= heapLength and self[next_child] < self[child_index] then + child_index = next_child + end + local child_key = self[child_index] + if last_key < child_key then + break + else + self[parent_index], vals[parent_index] = child_key, vals[child_index] + parent_index = child_index + child_index = child_index + child_index + end + end + + self.length = heapLength + self[parent_index], vals[parent_index] = last_key, last_val + return result_key, result_val +end + +----------------------------------------------------------------- + +local Graphpath = {} +Graphpath.__index = Graphpath + +local function newGraphpath() + return setmetatable({graph = {}, positions = {}, radius = {}}, Graphpath) +end + +function Graphpath:export() + return {graph = self.graph, positions = self.positions, radius = self.radius} +end + +function Graphpath:import(graphData) + self.graph = graphData.graph + self.positions = graphData.positions + self.radius = graphData.radius +end + +function Graphpath:clear() + self.graph = {} +end + +function Graphpath:edge(sp, ep, dist) + if self.graph[sp] == nil then + self.graph[sp] = {} + end + + self.graph[sp][ep] = {dist or 1} + + if self.graph[ep] == nil then + self.graph[ep] = {} + end +end + +function Graphpath:uniEdge(sp, ep, dist, drivability, speedLimit) + dist = dist or 1 + if self.graph[sp] == nil then + self.graph[sp] = {} + end + + local data = {dist, drivability, sp, speedLimit} -- sp is the inNode of the edge + + self.graph[sp][ep] = data + + if self.graph[ep] == nil then + self.graph[ep] = {} + end + + self.graph[ep][sp] = data +end + +function Graphpath:bidiEdge(sp, ep, dist, drivability, speedLimit) + dist = dist or 1 + if self.graph[sp] == nil then + self.graph[sp] = {} + end + + local data = {dist, drivability, nil, speedLimit} -- no inNode means edge is bidirectional + + self.graph[sp][ep] = data + + if self.graph[ep] == nil then + self.graph[ep] = {} + end + + self.graph[ep][sp] = data +end + +function Graphpath:setPointPosition(p, pos) + self.positions[p] = pos +end + +function Graphpath:setPointPositionRadius(p, pos, radius) + self.positions[p] = pos + self.radius[p] = radius +end + +function Graphpath:setNodeRadius(node, radius) + self.radius[node] = radius +end + +local function invertPath(goal, road) + local path = table.new(20, 0) + local e = 0 + while goal do -- unroll path from goal to source + e = e + 1 + path[e] = goal + goal = road[goal] + end + + for s = 1, e * 0.5 do -- reverse order to get source to goal + path[s], path[e] = path[e], path[s] + e = e - 1 + end + + return path +end + +do + local graph, index, S, nodeData, allSCC + + local function strongConnect(node) + -- Set the depth index for node to the smallest unused index + index = index + 1 + nodeData[node] = {index = index, lowlink = index, onStack = true} + tableInsert(S, node) + + -- Consider succesors of node + for adjNode, value in pairs(graph[node]) do + if value[2] == 1 then + if nodeData[adjNode] == nil then -- adjNode is a descendant of 'node' in the search tree + strongConnect(adjNode) + nodeData[node].lowlink = min(nodeData[node].lowlink, nodeData[adjNode].lowlink) + elseif nodeData[adjNode].onStack then -- adjNode is not a descendant of 'node' in the search tree + nodeData[node].lowlink = min(nodeData[node].lowlink, nodeData[adjNode].index) + end + end + end + + -- generate an scc (smallest possible scc is one node), i.e. in a directed accyclic graph each node constitutes an scc + if nodeData[node].lowlink == nodeData[node].index then + local currentSCC = {} + local currentSCCLen = 0 + repeat + local w = table.remove(S) + nodeData[w].onStack = false + currentSCC[w] = true + currentSCCLen = currentSCCLen + 1 + until node == w + currentSCC[0] = currentSCCLen + tableInsert(allSCC, currentSCC) + end + end + + function Graphpath:scc(v) + --[[ https://en.wikipedia.org/wiki/Tarjan%27s_strongly_connected_components_algorithm + calculates the strongly connected components (scc) of the map graph. + If v is provided, it only calculates the scc containing / is reachable from v. + Returns an array of dicts ('allSCC') --]] + + graph = self.graph + if v and graph[v] == nil then return {} end + + index, S, nodeData, allSCC = 0, {}, {}, {} + + if v then -- get only the scc containing/reachable from v + strongConnect(v) + else -- get all scc of the map graph + for node, _ in pairs(graph) do + if nodeData[node] == nil then + strongConnect(node) + end + end + end + return allSCC + end +end + +function Graphpath:getPath(start, goal, dirMult) + local graph = self.graph + if graph[start] == nil or graph[goal] == nil then return {} end + + local dirCoeff = {[true] = dirMult or 1, [false] = 1} + + local cost, node = 0, start + local minParent = {[node] = false} + local queued = {} + local road = {} -- predecessor subgraph + + local q = newMinheap() + repeat + if road[node] == nil then + road[node] = minParent[node] + if node == goal then break end + for child, data in pairs(graph[node]) do + if road[child] == nil then -- if the shortest path to child has not already been found + local currentChildCost = queued[child] -- lowest value with which child has entered the que + local newChildCost = cost + data[1] * dirCoeff[data[3] == child] + if not currentChildCost or newChildCost < currentChildCost then + q:insert(newChildCost, child) + minParent[child] = node + queued[child] = newChildCost + end + end + end + end + cost, node = q:pop() + until not cost + + return invertPath(goal, road) +end + +function Graphpath:getPointNodePath(start, target, cutOffDrivability, dirMult, penaltyAboveCutoff, penaltyBelowCutoff, wZ) + -- Shortest path between a point and a node or vice versa. + -- start/target: either start or target should be a node name, the other a vec3 point + -- cutOffDrivability: penalize roads with drivability <= cutOffDrivability + -- dirMult: amount of penalty for traversing edges in the 'illegal direction' (reasonable penalty values: 1e3-1e4). 1 = no penalty + -- If equal to nil or 1 then it means no penalty. + -- penaltyAboveCutoff: penalty multiplier for roads above the drivability cutoff + -- penaltyBelowCutoff: penalty multiplier for roads below the drivability cutoff + -- wZ: number. When higher than 1 distance minimization is biased to minimizing z diamension more so than x, y. + + local graph = self.graph + local invert + if start.x then + start, target = target, start + invert = true + end + if graph[start] == nil or target == nil then return {} end + + wZ = wZ or 4 + cutOffDrivability = cutOffDrivability or 0 + penaltyAboveCutoff = penaltyAboveCutoff or 1 + penaltyBelowCutoff = penaltyBelowCutoff or 10000 + + local dirCoeff = {[true] = dirMult or 1, [false] = 1} + local drivCoeff = {[true] = penaltyAboveCutoff, [false] = penaltyBelowCutoff} + + local positions = self.positions + local cost, node = 0, start + local minParent = {[node] = false} + local minCost = {[node] = cost} + local road = {} + + local targetMinCost = math.huge + local targetMinCostLink + local tmpVec = vec3() + local nodeToTargetVec = vec3() + + local q = newMinheap() + repeat + if road[node] == nil then + road[node] = minParent[node] -- t[2] is the predecessor of node in the shortest path to node + if node == target then break end + + local p1 = positions[node] + nodeToTargetVec:set(target) + nodeToTargetVec:setSub(p1) + local pathCost = cost + square(square(nodeToTargetVec.x) + square(nodeToTargetVec.y) + square(wZ * nodeToTargetVec.z)) + if pathCost < targetMinCost then + q:insert(pathCost, target) + targetMinCost = pathCost + minParent[target] = node + targetMinCostLink = nil + end + + local parent = road[node] + for child, data in pairs(graph[node]) do + local edgeCost + local outNode = invert and node or child + if road[child] == nil then -- if the shortest path to child has not already been found + edgeCost = data[1] * dirCoeff[data[3] == outNode] * drivCoeff[data[2] > cutOffDrivability] + pathCost = cost + edgeCost + local childMinCost = minCost[child] + if not childMinCost or pathCost < childMinCost then + q:insert(pathCost, child) + minCost[child] = pathCost + minParent[child] = node + end + end + if cost < targetMinCost and child ~= parent then + tmpVec:set(positions[child]) + tmpVec:setSub(p1) -- edgeVec + local xnorm = min(1, max(0, tmpVec:dot(nodeToTargetVec) / (tmpVec:squaredLength() + 1e-30))) + if xnorm > 0 and xnorm < 1 then + tmpVec:setScaled(-xnorm) + tmpVec:setAdd(nodeToTargetVec) -- distToEdgeVec + pathCost = cost + (edgeCost or data[1] * dirCoeff[data[3] == outNode] * drivCoeff[data[2] > cutOffDrivability]) * xnorm + + square(square(tmpVec.x) + square(tmpVec.y) + square(wZ * tmpVec.z)) + if pathCost < targetMinCost then + q:insert(pathCost, target) + targetMinCost = pathCost + minParent[target] = node + targetMinCostLink = child + end + end + end + end + end + + cost, node = q:pop() + until not cost + + local path = {targetMinCostLink} -- last path node has to be added ad hoc + local e = #path + target = road[node] -- if all is well, node here should be the targetPos + while target do + e = e + 1 + path[e] = target + target = road[target] + end + + if not invert then + for i = 1, e * 0.5 do -- reverse order to get source to target + path[i], path[e] = path[e], path[i] + e = e - 1 + end + end + + return path +end + +function Graphpath:getPointToPointPath(startPos, iter, targetPos, cutOffDrivability, dirMult, penaltyAboveCutoff, penaltyBelowCutoff, wZ) + -- startPos: path source position + -- startPosLinks: graph nodes closest (by some measure) to startPos to be used as links to it + -- targetPos: target position (vec3) + -- cutOffDrivability: penalize roads with drivability <= cutOffDrivability + -- dirMult: amount of penalty to impose to path if it does not respect road legal directions (should be larger than 1 typically >= 10e4). + -- If equal to nil or 1 then it means no penalty. + -- penaltyAboveCutoff: penalty multiplier for roads above the drivability cutoff + -- penaltyBelowCutoff: penalty multiplier for roads below the drivability cutoff + -- wZ: number (typically >= 1). When higher than 1 the destination node of optimum path will be biased towards minimizing height difference to targetPos. + + local graph = self.graph + if startPos == nil or targetPos == nil or startPos == targetPos then return {} end + + wZ = wZ or 1 + cutOffDrivability = cutOffDrivability or 0 + penaltyAboveCutoff = penaltyAboveCutoff or 1 + penaltyBelowCutoff = penaltyBelowCutoff or 10000 + + local dirCoeff = {[true] = dirMult or 1, [false] = 1} + local drivCoeff = {[true] = penaltyAboveCutoff, [false] = penaltyBelowCutoff} + + local positions = self.positions + local minCost = table.new(0, 32) + local minParent = table.new(0, 32) + local xnorms = table.new(0, 32) + local road = table.new(0, 32) -- initialize shortest paths linked list + local tmpGraph = {} + + local nextNode, nextCost, nextXnorm = iter() -- get the closest neighboor + minCost[nextNode] = nextCost + xnorms[nextNode] = nextXnorm + minParent[nextNode] = false + local node, cost = nextNode, nextCost + nextNode, nextCost, nextXnorm = iter() + + local targetMinCost = math.huge + local targetMinCostLink + local p1 = vec3() + local tmpVec = vec3() + local nodeToTargetVec = vec3() + + local q = newMinheap() -- initialize que + while cost do + if road[node] == nil then + road[node] = minParent[node] + if node == targetPos then break end + + if not graph[node] then + local n1id, n2id = node[1], node[2] + local xnorm = xnorms[node] + local data = graph[n1id][n2id] + local dist, driv, inNode = data[1], data[2], data[3] + table.clear(tmpGraph) + tmpGraph[node] = {[n1id] = {dist * xnorm, driv, (inNode == n2id and node) or inNode}, + [n2id] = {dist * (1 - xnorm), driv, (inNode == n1id and node) or inNode}} + p1:set(positions[n2id]) + p1:setSub(positions[n1id]) + p1:setScaled(xnorm) + p1:setAdd(positions[n1id]) + else + p1:set(positions[node]) + end + + nodeToTargetVec:set(targetPos) + nodeToTargetVec:setSub(p1) + local pathCost = cost + square(square(nodeToTargetVec.x) + square(nodeToTargetVec.y) + square(wZ * nodeToTargetVec.z)) + if pathCost < targetMinCost then + q:insert(pathCost, targetPos) + targetMinCost = pathCost + minParent[targetPos] = node + targetMinCostLink = nil + end + + local parent = road[node] + for child, data in pairs(graph[node] or tmpGraph[node]) do + local edgeCost + if road[child] == nil then -- if the shortest path to child has not already been found + edgeCost = data[1] * dirCoeff[data[3] == child] * drivCoeff[data[2] > cutOffDrivability] + local pathCost = cost + edgeCost + local childMinCost = minCost[child] + if not childMinCost or pathCost < childMinCost then + q:insert(pathCost, child) + minCost[child] = pathCost + minParent[child] = node + end + end + if cost < targetMinCost and child ~= parent then + tmpVec:set(positions[child]) + tmpVec:setSub(p1) -- edgeVec + local xnorm = min(1, max(0, tmpVec:dot(nodeToTargetVec) / (tmpVec:squaredLength() + 1e-30))) + if xnorm > 0 and xnorm < 1 then + tmpVec:setScaled(-xnorm) + tmpVec:setAdd(nodeToTargetVec) -- distToEdgeVec + pathCost = cost + (edgeCost or data[1] * dirCoeff[data[3] == child] * drivCoeff[data[2] > cutOffDrivability]) * xnorm + + square(square(tmpVec.x) + square(tmpVec.y) + square(wZ * tmpVec.z)) + if pathCost < targetMinCost then + q:insert(pathCost, targetPos) + targetMinCost = pathCost + minParent[targetPos] = node + targetMinCostLink = child + end + end + end + end + end + + if (q:peekKey() or math.huge) <= (nextCost or math.huge) then + cost, node = q:pop() + else + minCost[nextNode] = nextCost + xnorms[nextNode] = nextXnorm + minParent[nextNode] = false + node, cost = nextNode, nextCost + nextNode, nextCost, nextXnorm = iter() + end + end + + local path = {targetMinCostLink} -- last path node has to be added ad hoc + local e = #path + local target = road[node] -- if all is well, node here should be the targetPos + while target do + e = e + 1 + path[e] = target + target = road[target] + end + + -- add the starNode link to the path if it is not there + if graph[path[e]] == nil then + local tmp1 = path[e][1] + local tmp2 = path[e][2] + path[e] = nil + e = e - 1 + if path[e] == tmp1 and path[e-1] ~= tmp2 then + e = e + 1 + path[e] = tmp2 + elseif path[e] == tmp2 and path[e-1] ~= tmp1 then + e = e + 1 + path[e] = tmp1 + end + end + + for i = 1, e * 0.5 do -- reverse order to get source to target + path[i], path[e] = path[e], path[i] + e = e - 1 + end + + return path +end + +function Graphpath:getPathT(start, mePos, pathLenLim, illegalDirPenalty, initDir) + -- Produces an optimum path away from node 'start' to a distance of ~ 'pathLenLim' from 'mePos', with a moderate bias to edge coliniarity + -- Some randomness is achieved through a small augmentation of the pathLenLim value. + local graph = self.graph + if graph[start] == nil then return {} end + + local dirCoeff = {[true] = illegalDirPenalty or 1, [false] = 1} + + pathLenLim = square(pathLenLim * (1 + math.random() * 0.15)) -- augment pathLenLim by a random amount up to 15% the initial value + + local positions = self.positions + local cost, node = 0, start + local minParent = {[node] = false} + local queued = {} + local road = {} -- predessesor of node in the shortest path to node + local curSegDir = vec3(initDir) + local nextSegDir = vec3() + + local q = newMinheap() + repeat + if road[node] == nil then + local parent = minParent[node] + road[node] = parent + local nodePos = positions[node] + if parent then + if mePos:squaredDistance(nodePos) >= pathLenLim then break end + curSegDir:set(nodePos) + curSegDir:setSub(positions[parent]) + curSegDir:normalize() + end + for child, edgeData in pairs(graph[node]) do + if road[child] == nil then + nextSegDir:set(positions[child]) + nextSegDir:setSub(nodePos) + nextSegDir:normalize() + local t = 0.5 * (1 + nextSegDir:dot(curSegDir)) + local newChildCost = cost + edgeData[1] * dirCoeff[edgeData[3] == child] + (10 / (t + 0.001) - 9.995) + local currentChildCost = queued[child] + if currentChildCost == nil or currentChildCost > newChildCost then + q:insert(newChildCost, child) + minParent[child] = node + queued[child] = newChildCost + end + end + end + end + cost, node = q:pop() + until not cost -- que is empty + + return invertPath(node, road) +end + +function Graphpath:getFilteredPath(start, goal, cutOffDrivability, dirMult, penaltyAboveCutoff, penaltyBelowCutoff) + local graph = self.graph + if graph[start] == nil or graph[goal] == nil then return {} end + + cutOffDrivability = cutOffDrivability or 0 + penaltyAboveCutoff = penaltyAboveCutoff or 1 + penaltyBelowCutoff = penaltyBelowCutoff or 10000 + + local drivCoeff = {[true] = penaltyAboveCutoff, [false] = penaltyBelowCutoff} + local dirCoeff = {[true] = dirMult or 1, [false] = 1} + + local cost, node = 0, start + local minParent = {[node] = false} + local road = {} -- predecessor subgraph + local queued = {} + + local q = newMinheap() + repeat + if road[node] == nil then + road[node] = minParent[node] + if node == goal then break end + for child, data in pairs(graph[node]) do + if road[child] == nil then + local currentChildCost = queued[child] + local newChildCost = cost + data[1] * dirCoeff[data[3] == child] * drivCoeff[data[2] > cutOffDrivability] + if currentChildCost == nil or currentChildCost > newChildCost then + q:insert(newChildCost, child) + minParent[child] = node + queued[child] = newChildCost + end + end + end + end + cost, node = q:pop() + until not cost + + return invertPath(goal, road) +end + +function Graphpath:spanMap(source, nodeBehind, target, edgeDict, dirMult) + local graph = self.graph + if graph[source] == nil or graph[target] == nil then return {} end + + dirMult = dirMult or 1 + local dirCoeff = {[true] = dirMult, [false] = 1} + + local q = newMinheap() + local cost, t = 0, {source, false} + local road = {} -- predecessor subgraph + local queued = {} + + repeat + local node = t[1] + if road[node] == nil then + road[node] = t[2] + if node == target then break end + for child, data in pairs(graph[node]) do + if road[child] == nil then + local currentChildCost = queued[child] + local newChildCost = cost + data[1] * dirCoeff[data[3] == child] * (edgeDict[node..'\0'..child] or 1e20) * ((node == source and child == nodeBehind and 300) or 1) + if currentChildCost == nil or currentChildCost > newChildCost then + q:insert(newChildCost, {child, node}) + queued[child] = newChildCost + end + end + end + end + cost, t = q:pop() + until not cost + + return invertPath(target, road) +end + +function Graphpath:getPathAwayFrom(start, goal, mePos, stayAwayPos, dirMult) + local graph = self.graph + if graph[start] == nil or graph[goal] == nil then return {} end + + dirMult = dirMult or 1 + local dirCoeff = {[true] = dirMult, [false] = 1} + + local positions = self.positions + local q = newMinheap() + local cost, t = 0, {start, false} + local road = {} -- predecessor subgraph + local queued = {} + + repeat + local node = t[1] + if road[node] == nil then + road[node] = t[2] + if node == goal then break end + for child, data in pairs(graph[node]) do + if road[child] == nil then + local currentChildCost = queued[child] + local childPos = positions[child] + local newChildCost = cost + data[1] * dirCoeff[data[3] == child] * mePos:squaredDistance(childPos) / (stayAwayPos:squaredDistance(childPos) + 1e-30) + if currentChildCost == nil or currentChildCost > newChildCost then + q:insert(newChildCost, {child, node}) + queued[child] = newChildCost + end + end + end + end + cost, t = q:pop() + until not cost + + return invertPath(goal, road) +end + +function Graphpath:getMaxNodeAround(start, radius, dir) + local graph = self.graph + if graph[start] == nil then return nil end + + local graphpos = self.positions + local startpos = graphpos[start] + local stackP = 1 + local stack = {start} + local visited = {} + local maxFoundNode = start + local maxFoundScore = 0 + + repeat + local node = stack[stackP] + stack[stackP] = nil + stackP = stackP - 1 + + local nodeStartVec = graphpos[node] - startpos + local posNodeDist = nodeStartVec:squaredLength() + local posNodeScore = dir and nodeStartVec:dot(dir) or posNodeDist + + if posNodeScore > maxFoundScore then + maxFoundScore = posNodeScore + maxFoundNode = node + end + + if posNodeDist < radius * radius then + for child, _ in pairs(graph[node]) do + if visited[child] == nil then + visited[child] = 1 + stackP = stackP + 1 + stack[stackP] = child + end + end + end + until stackP <= 0 + + return maxFoundNode +end + +function Graphpath:getBranchNodesAround(start, radius) + local graph = self.graph + if graph[start] == nil then return nil end + + local graphpos = self.positions + local startpos = graphpos[start] + local stackP = 1 + local stack = {start} + local visited = {} + local branches = {} + + repeat + local node = stack[stackP] + stack[stackP] = nil + stackP = stackP - 1 + + local posNodeDist = (graphpos[node] - startpos):squaredLength() + + if posNodeDist < radius * radius then + local childCount = 0 + for child, _ in pairs(graph[node]) do + if visited[child] == nil then + visited[child] = 1 + stackP = stackP + 1 + stack[stackP] = child + childCount = childCount + 1 + end + end + + if childCount >= 2 then + table.insert(branches, node) + end + end + until stackP <= 0 + + return branches +end + +local fleeDirScoreCoeff = {[false] = 1, [true] = 0.8} +function Graphpath:getFleePath(startNode, initialDir, chasePos, pathLenLimit, rndDirCoef, rndDistCoef) + local graph = self.graph + if graph[startNode] == nil then return nil end + + pathLenLimit = pathLenLimit or 100 + rndDirCoef = rndDirCoef or 0 + rndDistCoef = min(rndDistCoef or 0.05, 1) + local graphpos = self.positions + local visited = {startNode = 0.2} + local path = {startNode} + local pathLen = 0 + + local prevNode = startNode + local prevDir = vec3(initialDir) + local rnd2 = rndDirCoef * 2 + local chaseAIdist = graphpos[prevNode]:squaredDistance(chasePos) * 0.1 + + repeat + local maxScore = -math.huge + local maxNode = -1 + local maxVec + local maxLen + + local rDistCoef = min(1, pathLen * rndDistCoef) + + -- randomize dir + prevDir:set( + prevDir.x + (random() * rnd2 - rndDirCoef) * rDistCoef, + prevDir.y + (random() * rnd2 - rndDirCoef) * rDistCoef, + 0) + + local prevPos = graphpos[prevNode] + local chaseCoef = min(0.5, rndDistCoef * chaseAIdist) + + for child, link in pairs(graph[prevNode]) do + local childPos = graphpos[child] + local pathVec = childPos - prevPos + local pathVecLen = pathVec:length() + local driveability = link[2] + local vis = visited[child] or 1 + local posNodeScore = vis * fleeDirScoreCoeff[link[3] == child] * driveability * (3 + pathVec:dot(prevDir) / max(pathVecLen, 1)) * max(0, 3 + (chasePos - childPos):normalized():dot(prevDir) * chaseCoef) + visited[child] = vis * 0.2 + if posNodeScore >= maxScore then + maxNode = child + maxScore = posNodeScore + maxVec = pathVec + maxLen = pathVecLen + end + end + + if maxNode == -1 then + break + end + + prevNode = maxNode + prevDir = maxVec / (maxLen + 1e-30) + pathLen = pathLen + maxLen + table.insert(path, maxNode) + + until pathLen > pathLenLimit + + return path +end + +local dirScoreCoeff = {[true] = 0.1, [false] = 1} +function Graphpath:getRandomPathG(startNode, initialDir, pathLenLimit, rndDirCoef, rndDistCoef, oneway) + local graph = self.graph + if graph[startNode] == nil then return nil end + + pathLenLimit = pathLenLimit or 100 + rndDirCoef = rndDirCoef or 0 + rndDistCoef = min(rndDistCoef or 0.05, 1e30) + local graphpos = self.positions + local visited = {startNode = 0.2} + local path = {startNode} + local pathLen = 0 + + if oneway == nil then oneway = true end + + local prevNode = startNode + local ropePos = graphpos[prevNode] - initialDir * 15 + + dirScoreCoeff[true] = oneway == false and 1 or 0.1 + + repeat + local maxScore = -math.huge + local maxNode = -1 + local maxVec + local maxLen + + local curPos = graphpos[prevNode] + local prevDir = curPos - ropePos + local prevDirLen = prevDir:length() + prevDir = prevDir / (prevDirLen + 1e-30) + ropePos = curPos - prevDir * min(prevDirLen, 15) + + -- randomize dir + local rDistDirCoef = min(1, pathLen * rndDistCoef) * rndDirCoef + prevDir:set( + prevDir.x + (random() * 2 - 1) * rDistDirCoef, + prevDir.y + (random() * 2 - 1) * rDistDirCoef, + 0) + + prevDir:normalize() + + for child, link in pairs(graph[prevNode]) do + local pathVec = graphpos[child] - curPos + local pathVecLen = pathVec:length() + local vis = visited[child] or 1 + local posNodeScore = vis * dirScoreCoeff[link[3] == child] * link[2] * (2 + pathVec:dot(prevDir) / max(pathVecLen, 1)) + visited[child] = vis * 0.2 + if posNodeScore >= maxScore then + maxNode = child + maxScore = posNodeScore + maxLen = pathVecLen + maxVec = pathVec + end + end + + if maxNode == -1 then + break + end + + if maxVec:dot(prevDir) <= 0 then + ropePos = curPos + end + prevNode = maxNode + pathLen = pathLen + maxLen + table.insert(path, maxNode) + + until pathLen > pathLenLimit + + return path +end + +-- produces a random path with a bias towards edge coliniarity +function Graphpath:getRandomPath(nodeAhead, nodeBehind, dirMult) + local graph = self.graph + if graph[nodeAhead] == nil or graph[nodeBehind] == nil then return {} end + + dirMult = dirMult or 1 + local dirCoeff = {[true] = dirMult, [false] = 1} + + local positions = self.positions + + local q = newMinheap() + local cost, t = 0, {nodeAhead, false} + local road = {} -- predecessor subgraph + local queued = {} + local node + local choiceSet = {} + local costSum = 0 + local pathLength = {[nodeBehind] = 0} + + repeat + if road[t[1]] == nil then + node = t[1] + local parent = t[2] or nodeBehind + road[node] = t[2] + pathLength[node] = pathLength[parent] + (positions[node] - positions[parent]):length() + if pathLength[node] <= 300 or not t[2] then + local nodePos = positions[node] + local edgeDirVec = (positions[parent] - nodePos):normalized() + for child, data in pairs(graph[node]) do + if road[child] == nil then + local childCurrCost = queued[child] + local penalty = 1 + 10 * square(max(0, edgeDirVec:dot((positions[child] - nodePos):normalized()) - 0.2)) + local childNewCost = cost + penalty * data[1] * dirCoeff[data[3] == child] * ((node == nodeAhead and child == nodeBehind) and 1e4 or 1) + if childCurrCost == nil or childCurrCost > childNewCost then + queued[child] = childNewCost + q:insert(childNewCost, {child, node}) + end + end + end + else + tableInsert(choiceSet, {node, square(1/cost)}) + costSum = costSum + square(1/cost) + if #choiceSet == 5 then + break + end + end + end + cost, t = q:pop() + until not cost + + local randNum = costSum * math.random() + local runningSum = 0 + + for i = 1, #choiceSet do + local newRunningSum = choiceSet[i][2] + runningSum + if runningSum <= randNum and randNum <= newRunningSum then + node = choiceSet[i][1] + break + end + runningSum = newRunningSum + end + + return invertPath(node, road) +end + +-- public interface +M.newMinheap = newMinheap +M.newGraphpath = newGraphpath +return M diff --git a/lua/json.lua b/lua/json.lua new file mode 100644 index 0000000..6587122 --- /dev/null +++ b/lua/json.lua @@ -0,0 +1,219 @@ +--[[ json.lua +A compact pure-Lua JSON library. +The main functions are: json.stringify, json.parse. +## json.stringify: +This expects the following to be true of any tables being encoded: + * They only have string or number keys. Number keys must be represented as + strings in json; this is part of the json spec. + * They are not recursive. Such a structure cannot be specified in json. +A Lua table is considered to be an array if and only if its set of keys is a +consecutive sequence of positive integers starting at 1. Arrays are encoded like +so: `[2, 3, false, "hi"]`. Any other type of Lua table is encoded as a json +object, encoded like so: `{"key1": 2, "key2": false}`. +Because the Lua nil value cannot be a key, and as a table value is considerd +equivalent to a missing key, there is no way to express the json "null" value in +a Lua table. The only way this will output "null" is if your entire input obj is +nil itself. +An empty Lua table, {}, could be considered either a json object or array - +it's an ambiguous edge case. We choose to treat this as an object as it is the +more general type. +To be clear, none of the above considerations is a limitation of this code. +Rather, it is what we get when we completely observe the json specification for +as arbitrary a Lua object as json is capable of expressing. +## json.parse: +This function parses json, with the exception that it does not pay attention to +\u-escaped unicode code points in strings. +It is difficult for Lua to return null as a value. In order to prevent the loss +of keys with a null value in a json string, this function uses the one-off +table value json.null (which is just an empty table) to indicate null values. +This way you can check if a value is null with the conditional +`val == json.null`. +If you have control over the data and are using Lua, I would recommend just +avoiding null values in your data to begin with. +--]] + + +local json = {} + + +-- Internal functions. + +local function kind_of(obj) + if type(obj) ~= 'table' then return type(obj) end + local i = 1 + for _ in pairs(obj) do + if obj[i] ~= nil then i = i + 1 else return 'table' end + end + if i == 1 then return 'table' else return 'array' end +end + +local function escape_str(s) + local in_char = {'\\', '"', '/', '\b', '\f', '\n', '\r', '\t'} + local out_char = {'\\', '"', '/', 'b', 'f', 'n', 'r', 't'} + for i, c in ipairs(in_char) do + s = s:gsub(c, '\\' .. out_char[i]) + end + return s +end + +-- Returns pos, did_find; there are two cases: +-- 1. Delimiter found: pos = pos after leading space + delim; did_find = true. +-- 2. Delimiter not found: pos = pos after leading space; did_find = false. +-- This throws an error if err_if_missing is true and the delim is not found. +local function skip_delim(str, pos, delim, err_if_missing) + pos = pos + #str:match('^%s*', pos) + if str:sub(pos, pos) ~= delim then + if err_if_missing then + error('Expected ' .. delim .. ' near position ' .. pos) + end + return pos, false + end + return pos + 1, true +end + +-- Expects the given pos to be the first character after the opening quote. +-- Returns val, pos; the returned pos is after the closing quote character. +local function parse_str_val(str, pos, val) + val = val or '' + local early_end_error = 'End of input found while parsing string.' + if pos > #str then error(early_end_error) end + local c = str:sub(pos, pos) + if c == '"' then return val, pos + 1 end + if c ~= '\\' then return parse_str_val(str, pos + 1, val .. c) end + -- We must have a \ character. + local esc_map = {b = '\b', f = '\f', n = '\n', r = '\r', t = '\t'} + local nextc = str:sub(pos + 1, pos + 1) + if not nextc then error(early_end_error) end + return parse_str_val(str, pos + 2, val .. (esc_map[nextc] or nextc)) +end + +-- Returns val, pos; the returned pos is after the number's final character. +local function parse_num_val(str, pos) + local num_str = str:match('^-?%d+%.?%d*[eE]?[+-]?%d*', pos) + local val = tonumber(num_str) + if not val then error('Error parsing number at position ' .. pos .. '.') end + return val, pos + #num_str +end + + +-- Public values and functions. + +function json.rawStringify(obj, as_key) + local s = {} -- We'll build the string as an array of strings to be concatenated. + local kind = kind_of(obj) -- This is 'array' if it's an array or type(obj) otherwise. + if kind == 'array' then + if as_key then error('Can\'t encode array as key.') end + s[#s + 1] = '[' + for i, val in ipairs(obj) do + if i > 1 then s[#s + 1] = ', ' end + s[#s + 1] = json.rawStringify(val) + end + s[#s + 1] = ']' + elseif kind == 'table' then + if as_key then error('Can\'t encode table as key.') end + s[#s + 1] = '{' + for k, v in pairs(obj) do + if #s > 1 then s[#s + 1] = ', ' end + s[#s + 1] = json.rawStringify(k, true) + s[#s + 1] = ':' + s[#s + 1] = json.rawStringify(v) + end + s[#s + 1] = '}' + elseif kind == 'string' then + return '"' .. escape_str(obj) .. '"' + elseif kind == 'number' then + if as_key then return '"' .. tostring(obj) .. '"' end + return tostring(obj) + elseif kind == 'boolean' then + return tostring(obj) + elseif kind == 'nil' then + return 'null' + else + error('Unjsonifiable type: ' .. kind .. '.') + end + return table.concat(s) +end + +function json.stringify(obj, as_key) + local s = {} -- We'll build the string as an array of strings to be concatenated. + local kind = kind_of(obj) -- This is 'array' if it's an array or type(obj) otherwise. + if kind == 'array' then + if as_key then error('Can\'t encode array as key.') end + s[#s + 1] = '[' + for i, val in ipairs(obj) do + if i > 1 then s[#s + 1] = ', ' end + s[#s + 1] = json.stringify(val) + end + s[#s + 1] = ']' + elseif kind == 'table' then + if as_key then error('Can\'t encode table as key.') end + s[#s + 1] = '{\t' + for k, v in pairs(obj) do + if #s > 1 then s[#s + 1] = ',' end + if kind_of(v) == 'table' then s[#s + 1] = "\n" else s[#s + 1] = "\n\t" end + s[#s + 1] = json.stringify(k, true) + s[#s + 1] = ':' + s[#s + 1] = json.stringify(v) + end + s[#s + 1] = '\n}' + elseif kind == 'string' then + return '"' .. escape_str(obj) .. '"' + elseif kind == 'number' then + if as_key then return '"' .. tostring(obj) .. '"' end + return tostring(obj) + elseif kind == 'boolean' then + return tostring(obj) + elseif kind == 'nil' then + return 'null' + else + error('Unjsonifiable type: ' .. kind .. '.') + end + return table.concat(s) +end + +json.null = {} -- This is a one-off table to represent the null value. + +function json.parse(str, pos, end_delim) + pos = pos or 1 + if pos > #str then error('Reached unexpected end of input.') end + local pos = pos + #str:match('^%s*', pos) -- Skip whitespace. + local first = str:sub(pos, pos) + if first == '{' then -- Parse an object. + local obj, key, delim_found = {}, true, true + pos = pos + 1 + while true do + key, pos = json.parse(str, pos, '}') + if key == nil then return obj, pos end + if not delim_found then error('Comma missing between object items.') end + pos = skip_delim(str, pos, ':', true) -- true -> error if missing. + obj[key], pos = json.parse(str, pos) + pos, delim_found = skip_delim(str, pos, ',') + end + elseif first == '[' then -- Parse an array. + local arr, val, delim_found = {}, true, true + pos = pos + 1 + while true do + val, pos = json.parse(str, pos, ']') + if val == nil then return arr, pos end + if not delim_found then error('Comma missing between array items.') end + arr[#arr + 1] = val + pos, delim_found = skip_delim(str, pos, ',') + end + elseif first == '"' then -- Parse a string. + return parse_str_val(str, pos + 1) + elseif first == '-' or first:match('%d') then -- Parse a number. + return parse_num_val(str, pos) + elseif first == end_delim then -- End of an object or array. + return nil, pos + 1 + else -- Parse true, false, or null. + local literals = {['true'] = true, ['false'] = false, ['null'] = json.null} + for lit_str, lit_val in pairs(literals) do + local lit_end = pos + #lit_str - 1 + if str:sub(pos, lit_end) == lit_str then return lit_val, lit_end + 1 end + end + local pos_info_str = 'position ' .. pos .. ': ' .. str:sub(pos, pos + 10) + error('Invalid json syntax starting at ' .. pos_info_str) + end +end + +return json \ No newline at end of file diff --git a/lua/mathlib.lua b/lua/mathlib.lua new file mode 100644 index 0000000..dae555d --- /dev/null +++ b/lua/mathlib.lua @@ -0,0 +1,995 @@ +-- This Source Code Form is subject to the terms of the bCDDL, v. 1.1. +-- If a copy of the bCDDL was not distributed with this +-- file, You can obtain one at http://beamng.com/bCDDL-1.1.txt + +--[[ +Usage: + +a = vec3(1,2,3) +b = vec3({1,2,3}) +c = vec3({x = 1, y = 2, z = 3}) +print(a == b) +print( (a-b) == vec3(0, 0, 0) ) +print( (c*1) ) +print( vec3(10,0,0):dot(vec3(10,0,0)) ) +]] + +local min, max, sqrt, abs, random = math.min, math.max, math.sqrt, math.abs, math.random + +local newLuaVec3xyz +local LuaVec3 = {} +LuaVec3.__index = LuaVec3 + +local ffifound, ffi = pcall(require, 'ffi') +if ffifound then + -- FFI available, so use it + ffi.cdef [[struct __luaVec3_t {double x,y,z;}; + struct __luaQuat_t {double x,y,z,w;};]] + newLuaVec3xyz = ffi.typeof("struct __luaVec3_t") + ffi.metatype("struct __luaVec3_t", LuaVec3) +else + ffi = nil + -- no FFI available, compatibility mode + newLuaVec3xyz = function (x, y, z) + return (setmetatable({x = x, y = y, z = z}, LuaVec3)) -- parenthesis needed to workaround extreme slowdown from: NYI return to lower frame + end +end + +function vec3toString(v) + return string.format('vec3(%.9g,%.9g,%.9g)', v.x, v.y, v.z) +end + +function stringToVec3(s) + local args = split(s, ' ') + if #args ~= 3 then return nil end + return newLuaVec3xyz(tonumber(args[1]), tonumber(args[2]), tonumber(args[3])) +end + +function vec3(x, y, z) + if y == nil then + if type(x) == 'table' and x[3] ~= nil then + return newLuaVec3xyz(x[1], x[2], x[3]) + else + if x ~= nil then + return newLuaVec3xyz(x.x, x.y, x.z) + else + return newLuaVec3xyz(0, 0, 0) + end + end + else + return newLuaVec3xyz(x, y, z or 0) + end +end + +function LuaVec3:set(x, y, z) + if y == nil then + self.x, self.y, self.z = x.x, x.y, x.z + else + self.x, self.y, self.z = x, y, z + end +end + +function LuaVec3:xyz() + return self.x, self.y, self.z +end + +function LuaVec3:__tostring() + return string.format('vec3(%.9g,%.9g,%.9g)', self.x, self.y, self.z) +end + +function LuaVec3:toTable() + return {self.x, self.y, self.z} +end + +function LuaVec3:toDict() + return {x = self.x, y = self.y, z = self.z} +end + +function LuaVec3:length() + return sqrt(self.x * self.x + self.y * self.y + self.z * self.z) +end + +function LuaVec3:lengthGuarded() + return sqrt(self.x * self.x + self.y * self.y + self.z * self.z) + 1e-30 +end + +function LuaVec3:squaredLength() + return self.x * self.x + self.y * self.y + self.z * self.z +end + +function LuaVec3.__add(a, b) + return newLuaVec3xyz(a.x + b.x, a.y + b.y, a.z + b.z) +end + +function LuaVec3.__sub(a, b) + return newLuaVec3xyz(a.x - b.x, a.y - b.y, a.z - b.z) +end + +function LuaVec3.__unm(a) + return newLuaVec3xyz(-a.x, -a.y, -a.z) +end + +function LuaVec3.__mul(a, b) + if type(b) == 'number' then + return newLuaVec3xyz(b * a.x, b * a.y, b * a.z) + else + return newLuaVec3xyz(a * b.x, a * b.y, a * b.z) + end +end + +function LuaVec3.__div(a,b) + if type(b) == 'number' then + b = 1 / b + return newLuaVec3xyz(b * a.x, b * a.y, b * a.z) + else + a = 1 / a + return newLuaVec3xyz(a * b.x, a * b.y, a * b.z) + end +end + +function LuaVec3.__eq(a, b) + return b ~= nil and a.x == b.x and a.y == b.y and a.z == b.z +end + +function LuaVec3:dot(a) + return self.x * a.x + self.y * a.y + self.z * a.z +end + +function LuaVec3:cross(a) + return newLuaVec3xyz(self.y * a.z - self.z * a.y, self.z * a.x - self.x * a.z, self.x * a.y - self.y * a.x) +end + +function LuaVec3:z0() + return newLuaVec3xyz(self.x, self.y, 0) +end + +function LuaVec3:perpendicular() + local k = abs(self.x) + 0.5 + k = k - math.floor(k) + return newLuaVec3xyz(-self.y, self.x - k * self.z, k * self.y) +end + +function LuaVec3:perpendicularN() + local p = self:perpendicular() + local r = 1 / (p:length() + 1e-30) + p.x, p.y, p.z = p.x * r, p.y * r, p.z * r + return p +end + +function LuaVec3:cosAngle(a) + return max(min(self:dot(a) / (sqrt(self:squaredLength() * a:squaredLength()) + 1e-30), 1), -1) +end + +function LuaVec3:normalize() + local r = 1 / (self:length() + 1e-30) + self.x, self.y, self.z = self.x * r, self.y * r, self.z * r +end + +function LuaVec3:resize(a) + local r = a / (self:length() + 1e-30) + self.x, self.y, self.z = self.x * r, self.y * r, self.z * r +end + +function LuaVec3:normalized() + local r = 1 / (self:length() + 1e-30) + return newLuaVec3xyz(self.x * r, self.y * r, self.z * r) +end + +function LuaVec3:resized(a) + local r = a / (self:length() + 1e-30) + return newLuaVec3xyz(self.x * r, self.y * r, self.z * r) +end + +function LuaVec3:distance(a) + local tmp = self.x - a.x + local d = tmp * tmp + tmp = self.y - a.y + d = d + tmp * tmp + tmp = self.z - a.z + return sqrt(d + tmp * tmp) +end + +function LuaVec3:squaredDistance(a) + local tmp = self.x - a.x + local d = tmp * tmp + tmp = self.y - a.y + d = d + tmp * tmp + tmp = self.z - a.z + return d + tmp * tmp +end + +-- a, b are two line points, self is the point +function LuaVec3:distanceToLine(a, b) + local ab = a - b + local an = a - self + return an:distance(ab * (ab:dot(an) / (ab:squaredLength() + 1e-30))) +end + +function LuaVec3:squaredDistanceToLine(a, b) + local ab = a - b + local an = a - self + return an:squaredDistance(ab * (ab:dot(an) / (ab:squaredLength() + 1e-30))) +end + +function LuaVec3:distanceToLineSegment(a, b) + local ab, an = a - b, a - self + return an:distance(ab * min(max(ab:dot(an) / (ab:squaredLength() + 1e-30), 0), 1)) +end + +function LuaVec3:xnormDistanceToLineSegment(a, b) + local ab, an = a - b, a - self + local xnorm = ab:dot(an) / (ab:squaredLength() + 1e-30) + return xnorm, an:distance(ab * min(max(xnorm, 0), 1)) +end + +function LuaVec3:squaredDistanceToLineSegment(a, b) + local ab, an = a - b, a - self + return an:squaredDistance(ab * min(max(ab:dot(an) / (ab:squaredLength() + 1e-30), 0), 1)) +end + +function LuaVec3:xnormSquaredDistanceToLineSegment(a, b) + local ab, an = a - b, a - self + local xnorm = ab:dot(an) / (ab:squaredLength() + 1e-30) + return xnorm, an:squaredDistance(ab * min(max(xnorm, 0), 1)) +end + +function LuaVec3:xnormOnLine(a, b) + local ba = b - a + return ba:dot(self - a) / (ba:squaredLength() + 1e-30) +end + +function LuaVec3:triangleBarycentricNorm(a, b, c) + local ca, bc = c - a, b - c + local norm = ca:cross(bc) + local normsqlen = norm:squaredLength() + 1e-30 + local pacnorm = (self - c):cross(norm) + return bc:dot(pacnorm) / normsqlen, ca:dot(pacnorm) / normsqlen, norm / sqrt(normsqlen) +end + +function LuaVec3:toPoint3F() + return Point3F(self.x, self.y, self.z) +end + +function LuaVec3:toFloat3() + return float3(self.x, self.y, self.z) +end + +function LuaVec3:projectToOriginPlane(pnorm) + return self - (pnorm * (self:dot(pnorm))) +end + +-- self is a point in plane +function LuaVec3:xnormPlaneWithLine(pnorm, a, b) + return (self - a):dot(pnorm) / ((b - a):dot(pnorm) + 1e-30) +end + +-- self is center of sphere, returns two xnorms (low, high). It returns pair 1,0 if no hit found +function LuaVec3:xnormsSphereWithLine(radius, a, b) + local lDif = b - a + local invDif2len = 1 / max(lDif:squaredLength(), 1e-30) + local ac = a - self + local dotab = -ac:dot(lDif) * invDif2len + local D = dotab * dotab + (radius * radius - ac:squaredLength()) * invDif2len + if D >= 0 then + D = sqrt(D) + return dotab - D, dotab + D + else + return 1, 0 + end +end + +function LuaVec3:basisCoordinates(c1, c2, c3) + local c2xc3 = c2:cross(c3) + local invDet = 1 / c1:dot(c2xc3) + return newLuaVec3xyz(c2xc3:dot(self)*invDet, c3:cross(c1):dot(self)*invDet, c1:cross(c2):dot(self)*invDet) +end + +function LuaVec3:componentMul(b) + return newLuaVec3xyz(self.x * b.x, self.y * b.y, self.z * b.z) +end + +function LuaVec3:setMin(b) + self.x, self.y, self.z = min(self.x, b.x), min(self.y, b.y), min(self.z, b.z) +end + +function LuaVec3:setMax(b) + self.x, self.y, self.z = max(self.x, b.x), max(self.y, b.y), max(self.z, b.z) +end + +function LuaVec3:setAdd(b) + self.x, self.y, self.z = self.x + b.x, self.y + b.y, self.z + b.z +end + +function LuaVec3:setSub(b) + self.x, self.y, self.z = self.x - b.x, self.y - b.y, self.z - b.z +end + +function LuaVec3:setScaled(b) + self.x, self.y, self.z = self.x * b, self.y * b, self.z * b +end + +local function fractPos(x) + return x - math.floor(x) +end + +function LuaVec3:getBlueNoise3d() + self.x, self.y, self.z = fractPos(self.x + 0.81917251339616443), fractPos(self.y + 0.6710436067037892084), fractPos(self.z + 0.54970047790197026) + return self +end + +function LuaVec3:getBlueNoise2d() + self.x, self.y, self.z = fractPos(self.x + 0.75487766624669276), fractPos(self.y + 0.56984029099805327), 0 + return self +end + +function LuaVec3:getRandomPointInSphere(radius) + radius = radius or 1 + local sx, sy, sz = 0, 0, 0; + for i = 1, 4 do + local x, y, z = random(), random(), random() + sx, sy, sz = sx + x, sy + y, sz + z + local xc, yc, zc = x - 0.5, y - 0.5, z - 0.5 + if xc * xc + yc * yc + zc * zc <= 0.25 then + local r2 = radius * 2 + self.x, self.y, self.z = xc * r2, yc * r2, zc * r2 + return self + end + end + + sx, sy, sz = sx - 2, sy - 2, sz - 2 + local u = random() + local norm = sqrt(0.5 * (sqrt(u) + u)/(sx*sx + sy*sy + sz*sz + 1e-25)) * radius + self.x, self.y, self.z = sx*norm, sy*norm, sz*norm + return self +end + +function LuaVec3:getRandomPointInCircle(radius) + radius = radius or 1 + local sx, sy = 0, 0 + for i = 1, 4 do + local x, y = random(), random() + sx, sy = sx + x, sy + y + local xc, yc = x - 0.5, y - 0.5 + if xc * xc + yc * yc <= 0.25 then + local r2 = radius * 2 + self.x, self.y, self.z = xc * r2, yc * r2, 0 + return self + end + end + + sx, sy = sx - 2, sy - 2 + local norm = sqrt(random()/(sx*sx+sy*sy + 1e-25)) * radius + self.x, self.y, self.z = sx*norm, sy*norm, 0 + return self +end + +function LuaVec3:getBluePointInSphere(radius) + radius = radius or 1 + local bx, by, bz = self.x, self.y, self.z + for i = 1, 8 do -- seen up to 6 + local x, y, z = fractPos(bx + 0.81917251339616443 * i), fractPos(by + 0.6710436067037892084 * i), fractPos(bz + 0.54970047790197026 * i) + local xc, yc, zc = x - 0.5, y - 0.5, z - 0.5 + if xc * xc + yc * yc + zc * zc <= 0.25 then + self.x, self.y, self.z = x, y, z + local r2 = radius * 2 + return newLuaVec3xyz(xc * r2, yc * r2, zc * r2) + end + end + return newLuaVec3xyz(0, 0, 0) +end + +function LuaVec3:getBluePointInCircle(radius) + radius = radius or 1 + local bx, by = self.x, self.y + for i = 1, 5 do -- seen up to 3 + local x, y = fractPos(bx + 0.75487766624669276 * i), fractPos(by + 0.56984029099805327 * i) + local xc, yc = x - 0.5, y - 0.5 + if xc * xc + yc * yc <= 0.25 then + self.x, self.y = x, y + local r2 = radius * 2 + return newLuaVec3xyz(xc * r2, yc * r2, 0) + end + end + return newLuaVec3xyz(0, 0, 0) +end + +-- returns random gauss number in [0..3] +function randomGauss3() + return random() + random() + random() +end + +-- returns xnormals for the two lines: http://geomalgorithms.com/a07-_distance.html +function closestLinePoints(l1p0, l1p1, l2p0, l2p1) + local a, b, c, d, e + do + -- limit the number of live vars to help out luajit + local u = l1p1 - l1p0 + local v = l2p1 - l2p0 + local w = l1p0 - l2p0 + a = u:squaredLength() + b = u:dot(v) + c = v:squaredLength() + d = u:dot(w) + e = v:dot(w) + end + local D = a * c - b * b + + if D < 1e-8 then + local tc + if b > c then + tc = d / b + else + tc = e / (c + 1e-30) + end + return 0, tc + else + return (b * e - c * d) / D, (a * e - b * d) / D + end +end + +function linePointFromXnorm(p0, p1, xnorm) + return newLuaVec3xyz(p0.x + (p1.x-p0.x) * xnorm, p0.y + (p1.y-p0.y) * xnorm, p0.z + (p1.z-p0.z) * xnorm) +end + +-------------------------------------------------------------------------------- +-- Quaternion + +local LuaQuat = {} +LuaQuat.__index = LuaQuat +local newLuaQuatxyzw + +if ffi then + newLuaQuatxyzw = ffi.typeof("struct __luaQuat_t") + ffi.metatype("struct __luaQuat_t", LuaQuat) +else + newLuaQuatxyzw = function (_x, _y, _z, _w) + return (setmetatable({ x = _x, y = _y, z = _z, w = _w }, LuaQuat)) -- parenthesis needed to workaround extreme slowdown from: NYI return to lower frame + end +end + +-- Returns quat. Both inputs should be normalized +function LuaVec3:getRotationTo(v) + local w = 1 + self:dot(v) + local qv + + if (w < 1e-6) then + w = 0 + qv = v:perpendicular() + else + qv = self:cross(v) + end + local q = newLuaQuatxyzw(qv.x, qv.y, qv.z, -w) + q:normalize() + return q +end + +-- Rotates by quaternion q +function LuaVec3:rotated(q) + local qv = newLuaVec3xyz(q.x, q.y, q.z) + local t = 2 * qv:cross(self) + return self - q.w * t + qv:cross(t) +end + +-- we follow t3d's quat convention which has uses a negative w :/ +function quat(x, y, z, w) + if y == nil then + if type(x) == 'table' and x[4] ~= nil then + return newLuaQuatxyzw(x[1], x[2], x[3], x[4]) + elseif x == nil then + return newLuaQuatxyzw(1, 0, 0, 0) + else + return newLuaQuatxyzw(x.x, x.y, x.z, x.w) + end + else + return newLuaQuatxyzw(x, y, z, w) + end +end + +function LuaQuat:__tostring() + return string.format('quat(%.9g,%.9g,%.9g,%.9g)', self.x, self.y, self.z, self.w) +end + +function LuaQuat:toTable() + return {self.x, self.y, self.z, self.w} +end + +function LuaQuat:toDict() + return {x = self.x, y = self.y, z = self.z, w = self.w} +end + +function LuaQuat:set(a) + self.x, self.y, self.z, self.w = a.x, a.y, a.z, a.w +end + +function LuaQuat:norm() + return sqrt(self.x * self.x + self.y * self.y + self.z * self.z + self.w * self.w) +end + +function LuaQuat:squaredNorm() + return self.x * self.x + self.y * self.y + self.z * self.z + self.w * self.w +end + +function LuaQuat:normalize() + local r = 1/(self:norm() + 1e-30) + self.x, self.y, self.z, self.w = self.x * r, self.y * r, self.z * r, self.w * r +end + +function LuaQuat:normalized() + local r = 1/(self:norm() + 1e-30) + return newLuaQuatxyzw(self.x * r, self.y * r, self.z * r, self.w * r) +end + +function LuaQuat:inversed() + local InvSqNorm = -1 / (self:squaredNorm() + 1e-30) + return newLuaQuatxyzw(self.x * InvSqNorm, self.y * InvSqNorm, self.z * InvSqNorm, -self.w * InvSqNorm) +end + +function LuaQuat.__unm(a) + return newLuaQuatxyzw(-a.x, -a.y, -a.z, -a.w) +end + +function LuaQuat.__mul(a, b) + if type(a) == 'number' then + return newLuaQuatxyzw(b.x * a, b.y * a, b.z * a, b.w * a) + elseif type(b) == 'number' then + return newLuaQuatxyzw(a.x * b, a.y * b, a.z * b, a.w * b) + elseif (ffi and ffi.istype('struct __luaVec3_t', b)) or b.w == nil then + local qv = newLuaVec3xyz(a.x, a.y, a.z) + local t = 2 * qv:cross(b) + return b - a.w * t + qv:cross(t) + else + return newLuaQuatxyzw(a.w * b.x + a.x * b.w + a.y * b.z - a.z * b.y, + a.w * b.y + a.y * b.w + a.z * b.x - a.x * b.z, + a.w * b.z + a.z * b.w + a.x * b.y - a.y * b.x, + a.w * b.w - a.x * b.x - a.y * b.y - a.z * b.z) + end +end + +function LuaQuat.__sub(a, b) + return newLuaQuatxyzw(a.x - b.x, a.y - b.y, a.z - b.z, a.w - b.w) +end + +function LuaQuat.__div(a, b) + if type(a) == 'number' then + return newLuaQuatxyzw(b.x / a, b.y / a, b.z / a, b.w / a) + elseif type(b) == 'number' then + return newLuaQuatxyzw(a.x / b, a.y / b, a.z / b, a.w / b) + end + return a * b:inversed() +end + +function LuaQuat.__add(a, b) + return newLuaQuatxyzw(a.x + b.x, a.y + b.y, a.z + b.z, a.w + b.w) +end + +function LuaQuat:dot(a) + return self.x * a.x + self.y * a.y + self.z * a.z + self.w * a.w +end + +function LuaQuat:distance(a) + return 0.5 * (self - a):squaredNorm() +end + +function LuaQuat:nlerp(a, t) + local tmp = (1 - t) * self + (self:dot(a) < 0 and -t or t) * a + tmp:normalize() + return tmp +end + +function LuaQuat:slerp(a, t) + local dot = clamp(self:dot(a), -1, 1) + + if dot > 0.9995 then + return self:nlerp(a, t) + end + + local theta = math.acos(dot)*t + return (self*math.cos(theta) + (a - self*dot):normalized()*math.sin(theta)):normalized(); +end + +-- returns reverse rotation +function LuaQuat:conjugated() + return newLuaQuatxyzw(-self.x, -self.y, -self.z, self.w) +end + +function LuaQuat:scale(a) + self.x, self.y, self.z, self.w = self.x * a, self.y * a, self.z * a, self.w * a + return self +end + +--http://bediyap.com/programming/convert-quaternion-to-euler-rotations/ +function LuaQuat.toEulerYXZ(q) + local wxsq = q.w*q.w-q.x*q.x + local yzsq = q.z*q.z-q.y*q.y + return newLuaVec3xyz( + math.atan2(2*(q.x*q.y + q.w*q.z), wxsq-yzsq), + math.asin(max(min(-2*(q.y*q.z - q.w*q.x), 1), -1)), + math.atan2(2*(q.x*q.z + q.w*q.y), wxsq+yzsq)) +end + +function LuaQuat:toTorqueQuat() + local sinhalf = math.sqrt(self.x * self.x + self.y * self.y + self.z * self.z) + local tw = math.acos(self.w) * 360 / math.pi + if sinhalf ~= 0 then + return {x = self.x / sinhalf, y = self.y / sinhalf, z = self.z / sinhalf, w = tw} + else + return {x = 1, y = 0, z = 0, w = tw} + end +end + +-- function LuaQuat:pow(a) +-- self:scale(a) +-- local vlen = sqrt( self.x*self.x + self.y*self.y + self.z*self.z ) +-- local ret = math.exp(self.w) +-- local coef = ret * math.sin(vlen) / (vlen + 1e-60) + +-- return newLuaQuatxyzw( coef*self.x, coef*self.y, coef*self.z, -ret* math.cos(vlen) ) +-- end + +local function quatFromAxesMatrix(m) + local q = {[0] = 0, 0, 0, 0} + local trace = m[0][0] + m[1][1] + m[2][2]; + if trace > 0 then + local s = sqrt(trace + 1) + q[3] = s * 0.5 + s = 0.5 / s + q[0] = (m[1][2] - m[2][1]) * s + q[1] = (m[2][0] - m[0][2]) * s + q[2] = (m[0][1] - m[1][0]) * s + else + local i = 0 + if m[1][1] > m[0][0] then i = 1 end + if m[2][2] > m[i][i] then i = 2 end + local j = (i + 1) % 3 + local k = (j + 1) % 3 + + local s = sqrt((m[i][i] - (m[j][j] + m[k][k])) + 1) + q[i] = s * 0.5 + s = 0.5 / s + q[j] = (m[i][j] + m[j][i]) * s + q[k] = (m[i][k] + m[k][i]) * s + q[3] = (m[j][k] - m[k][j]) * s + end + + local tmp = newLuaQuatxyzw(q[0], q[1], q[2], q[3]) + tmp:normalize() + return tmp +end + +function quatFromDir(dir, up) + local k = up or vec3(0, 0, 1) + local dirNorm = dir:normalized() + local i = dirNorm:cross(k) + i:normalize() + k = i:cross(dirNorm) + k:normalize() + + return quatFromAxesMatrix({[0]={[0]=i.x, dirNorm.x, k.x}, {[0]=i.y, dirNorm.y, k.y}, {[0]=i.z, dirNorm.z, k.z}}) +end + +function lookAt(lookAt, up) + up = up or vec3(0, 0, 1) + local forward = lookAt:normalized() + local right = forward:cross(up) + right:normalize() + up = right:cross(forward) + up:normalize() + + local w = sqrt(1 + right.x + up.y + forward.z) * 0.5 + local w4_recip = 1 / (4 * w) + local x = (forward.y - up.z) * w4_recip + local y = (right.z - forward.x) * w4_recip + local z = (up.x - right.y) * w4_recip + return newLuaQuatxyzw(x,y,z,w) +end + +function quatFromAxisAngle(axle, angleRad) + angleRad = angleRad * 0.5 + local fsin = math.sin(angleRad) + return newLuaQuatxyzw(fsin * axle.x, fsin * axle.y, fsin * axle.z, math.cos(angleRad)) +end + +function quatFromEuler(x, y, z) + x, y, z = x * 0.5, y * 0.5, z * 0.5 + local sx = math.sin(x) + local cx = math.cos(x) + local sy = math.sin(y) + local cy = math.cos(y) + local sz = math.sin(z) + local cz = math.cos(z) + + local cycz, sysz, sycz, cysz = cy*cz, sy*sz, sy*cz, cy*sz + return newLuaQuatxyzw(cycz*sx + sysz*cx, sycz*cx + cysz*sx, cysz*cx - sycz*sx, cycz*cx - sysz*sx) +end + +-- returns -1, 1 +function sign2(x) + return max(min(x * math.huge, 1), -1) +end + +-- returns -1, 0, 1 +function sign(x) + return max(min((x * 1e200) * 1e200, 1), -1) +end + +fsign = sign + +-- returns sign(s) * abs(v) +function signApply(s, v) + local absv = abs(v) + return max(min((s * 1e200) * 1e200, absv), -absv) +end + +function guardZero(x) --branchless + return 1 / max(min(1/x, 1e300), -1e300) +end + +function clamp(x, minValue, maxValue ) + return min(max(x, minValue), maxValue) +end + +function square(a) + return a * a +end + +function round(a) + return math.floor(a+.5) +end + +function isnan(a) + return not(a == a) +end + +function isinf(a) + return abs(a) == math.huge +end + +function isnaninf(a) + return a * 0 ~= 0 +end + +function linearScale(v, minValue, maxValue, minOutput, maxOutput) + return minOutput + min(max((v - minValue) / (maxValue - minValue), 0), 1) * (maxOutput - minOutput) +end + +function lerp(from, to, t) + return from + (to - from) * t -- monotonic +end + +function smoothstep(x) + x = min(max(x, 0), 1) -- monotonic guard + return x*x*(3 - 2*x) +end + +function smootherstep(x) + return min(max(x*x*x*(x*(x*6 - 15) + 10), 0), 1) +end + +function smootheststep(x) + x = min(max(x, 0), 1) + return square(x*x)*(35-x*(x*(x*20-70)+84)) +end + +function smoothmin(a, b, k) + k = k or 0.1 + local h = min(max(0.5 + 0.5*(b-a)/k, 0), 1) + return a*h - (b - k*h)*(1-h) +end + +function biasFun(x, k) + local xk = x * k + return (x + xk)/(1 + xk) +end + +function nanError(x) + if x ~= x then + error('NaN found') + end + return x +end + +function axisSystemCreate(nx, ny, nz) + local rx, ry, rz = vec3(), vec3(), vec3() + + local row = ny:cross(nz) + local invdet = 1 / nx:dot(row) + row = row * invdet + rx.x, ry.x, rz.x = row.x, row.y, row.z + + row = nz:cross(nx) * invdet + rx.y, ry.y, rz.y = row.x, row.y, row.z + + row = nx:cross(ny) * invdet + rx.z, ry.z, rz.z = row.x, row.y, row.z + return rx, ry, rz +end + +function axisSystemApply(nx, ny, nz, v) + return nx * v.x + ny * v.y + nz * v.z +end + +function cardinalSpline(p0, p1, p2, p3, t, s, d1, d2, d3) + d1, d2, d3 = max(d1 or 1, 1e-30), d2 or 1, max(d3 or 1, 1e-30) + s = (s or 0.5) * 2 + local sd2 = s * d2 + local tt, t_1 = t * t, t-1 + local t_1sq = t_1 * t_1 + + local m1 = (p1 - p0) / d1 + (p0 - p2) / (d1 + d2) + local m2 = (p1 - p3) / (d2 + d3) + (p3 - p2) / d3 + + return t*t_1sq*sd2*m1 + tt*t_1*sd2*m2 + t_1sq * (2 * t + 1) * p1 - tt * (2*t-3) * p2 + s*t_1*(t*t_1 + tt) * (p2 - p1) +end + +function catmullRom(p0, p1, p2, p3, t, s) + return cardinalSpline(p0, p1, p2, p3, t, s or 0.5, 1, 1, 1) +end + +function catmullRomChordal(p0, p1, p2, p3, t, s) + return cardinalSpline(p0, p1, p2, p3, t, s or 0.5, p0:distance(p1), p1:distance(p2), p2:distance(p3)) +end + +function catmullRomCentripetal(p0, p1, p2, p3, t, s) + return cardinalSpline(p0, p1, p2, p3, t, s or 0.5, sqrt(p0:distance(p1)), sqrt(p1:distance(p2)), sqrt(p2:distance(p3))) +end + +function monotonicSteffen(y0, y1, y2, y3, x0, x1, x2, x3, x) + local x1x0, x2x1, x3x2 = x1-x0, x2-x1, x3-x2 + local delta0, delta1, delta2 = (y1-y0) / (x1x0 + 1e-30), (y2-y1) / (x2x1 + 1e-30), (y3-y2) / (x3x2 + 1e-30) + local m1 = (sign(delta0)+sign(delta1)) * min(abs(delta0),abs(delta1), 0.5*abs((x2x1*delta0 + x1x0*delta1) / (x2-x0 + 1e-30))) + local m2 = (sign(delta1)+sign(delta2)) * min(abs(delta1),abs(delta2), 0.5*abs((x3x2*delta1 + x2x1*delta2) / (x3-x1 + 1e-30))) + local xx1 = x - x1 + local xrel = xx1 / max(x2x1, 1e-30) + return y1 + xx1*(m1 + xrel*(delta1 - m1 + (xrel - 1)*(m1 + m2 - 2*delta1))) +end + +function biQuadratic(p0, p1, p2, p3, t) + local p12 = p1 + (p2 - p1) * (t * 0.5 + 0.25) + if t <= 0.5 then + local p01 = p0 + (p1 - p0) * (t * 0.5 + 0.75) + return p01 + (p12 - p01) * (t + 0.5) + else + return p12 + (p2 + (p3 - p2) * (t * 0.5 - 0.25) - p12) * (t - 0.5) + end +end + +function overlapsOBB_OBB(c1, x1, y1, z1, c2, x2, y2, z2) + local cc = c1 - c2 + local d11, d12, d13 = abs(x1:dot(x2)), abs(x1:dot(y2)), abs(x1:dot(z2)) + local d21, d22, d23 = abs(y1:dot(x2)), abs(y1:dot(y2)), abs(y1:dot(z2)) + local d31, d32, d33 = abs(z1:dot(x2)), abs(z1:dot(y2)), abs(z1:dot(z2)) + + return abs(cc:dot(x1))-d11-d12-d13<=x1:squaredLength() and abs(cc:dot(y1))-d21-d22-d23<=y1:squaredLength() + and abs(cc:dot(z1))-d31-d32-d33<=z1:squaredLength() and abs(cc:dot(x2))-d11-d21-d31<=x2:squaredLength() + and abs(cc:dot(y2))-d12-d22-d32<=y2:squaredLength() and abs(cc:dot(z2))-d13-d23-d33<=z2:squaredLength() +end + +-- untested +function containsOBB_OBB(c1, x1, y1, z1, c2, x2, y2, z2) + local cc = c1 - c2 + return abs(cc:dot(x1))+abs(x1:dot(x2))+abs(x1:dot(y2))+abs(x1:dot(z2))<=x1:squaredLength() + and abs(cc:dot(y1))+abs(y1:dot(x2))+abs(y1:dot(y2))+abs(y1:dot(z2))<=y1:squaredLength() + and abs(cc:dot(z1))+abs(z1:dot(x2))+abs(z1:dot(y2))+abs(z1:dot(z2))<=z1:squaredLength() +end + +function overlapsOBB_Sphere(c1, x1, y1, z1, c2, r2) + local cc = c1 - c2 + local x1len, y1len, z1len = x1:length(), y1:length(), z1:length() + local ccx, ccy, ccz = abs(cc:dot(x1)), abs(cc:dot(y1)), abs(cc:dot(z1)) + + return ccx<=x1len*(x1len+r2) and ccy<=y1len*(y1len+r2) and ccz<=z1len*(z1len+r2) + and (ccx<=x1len*x1len or ccy<=y1len*y1len or ccz<=z1len*z1len or + square(ccx/(x1len+1e-30)-x1len)+square(ccy/(y1len+1e-30)-y1len)+square(ccz/(z1len+1e-30)-z1len)<=r2*r2) +end + +function overlapsOBB_Plane(c1, x1, y1, z1, plpos, pln) + return abs((c1 - plpos):dot(pln))<=abs(x1:dot(pln))+abs(y1:dot(pln))+abs(z1:dot(pln)) +end + +function containsOBB_Sphere(c1, x1, y1, z1, c2, r2) + local cc = c1 - c2 + local x1len, y1len, z1len = x1:length(), y1:length(), z1:length() + return abs(cc:dot(x1))<=x1len*(x1len-r2) and abs(cc:dot(y1))<=y1len*(y1len-r2) and abs(cc:dot(z1))<=z1len*(z1len-r2) +end + +function containsSphere_OBB(c1, r1, c2, x2, y2, z2) + local cc = c1 - c2 + local ccx1, cc_x2, y2z2, y2_z2 = cc+x2, cc-x2, y2+z2, y2-z2 + return max((ccx1+y2z2):squaredLength(), (ccx1+y2_z2):squaredLength(), (ccx1-y2_z2):squaredLength(), (ccx1-y2z2):squaredLength(), + (cc_x2+y2z2):squaredLength(), (cc_x2+y2_z2):squaredLength(), (cc_x2-y2_z2):squaredLength(), (cc_x2-y2z2):squaredLength())<=r1*r1 +end + +function containsOBB_point(c1, x1, y1, z1, p) + local cc = c1 - p + return abs(cc:dot(x1))<=x1:squaredLength() and abs(cc:dot(y1))<=y1:squaredLength() and abs(cc:dot(z1))<=z1:squaredLength() +end + +function containsEllipsoid_Point(c1, x1, y1, z1, p) + local cc = p - c1 + local x, y, z = cc:dot(x1), cc:dot(y1), cc:dot(z1) + local a2, b2, c2 = x1:squaredLength(), y1:squaredLength(), z1:squaredLength() + a2, b2, c2 = a2*a2, b2*b2, c2*c2 + local b2c2 = b2*c2 + return x*x*b2c2 + a2*(y*y*c2 + z*z*b2) <= a2*b2c2 +end + +function constainsCylinder_Point(cposa, cposb, cR, p) + local xnorm, r2 = p:xnormSquaredDistanceToLineSegment(cposa, cposb) + return xnorm >=0 and xnorm <= 1 and r2 <= cR*cR +end + +function altitudeOBB_Plane(c1, x1, y1, z1, plpos, pln) + return (c1 - plpos):dot(pln)+abs(x1:dot(pln))+abs(y1:dot(pln))+abs(z1:dot(pln)) +end + +-- returns signed distance of plane on the ray +function intersectsRay_Plane(rpos, rdir, plpos, pln) + return min((plpos - rpos):dot(pln) / rdir:dot(pln), math.huge) +end + +-- hit: minhit < maxhit, inside: minhit < 0 +function intersectsRay_OBB(rpos, rdir, c1, x1, y1, z1) + local rposc1 = c1 - rpos + local rposc1x1, x1sq, invrdirx1 = rposc1:dot(x1), x1:squaredLength(), 1 / rdir:dot(x1) + local dx1, dx2 = (rposc1x1 - x1sq) * invrdirx1, min((rposc1x1 + x1sq) * invrdirx1, math.huge) + local rposc1y1, y1sq, invrdiry1 = rposc1:dot(y1), y1:squaredLength(), 1 / rdir:dot(y1) + local dy1, dy2 = (rposc1y1 - y1sq) * invrdiry1, min((rposc1y1 + y1sq) * invrdiry1, math.huge) + local rposc1z1, z1sq, invrdirz1 = rposc1:dot(z1), z1:squaredLength(), 1 / rdir:dot(z1) + local dz1, dz2 = (rposc1z1 - z1sq) * invrdirz1, min((rposc1z1 + z1sq) * invrdirz1, math.huge) + + local minhit, maxhit = max(min(dx1, dx2), min(dy1, dy2), min(dz1, dz2)), min(max(dx1, dx2), max(dy1, dy2), max(dz1, dz2)) + return (minhit <= maxhit and minhit or math.huge), maxhit +end + +function intersectsRay_Sphere(rpos, rdir, cpos, cr) + local rcpos = cpos - rpos + local dcr = rdir:dot(rcpos) + local s = dcr*dcr - rcpos:squaredLength() + cr*cr + if s < 0 then return math.huge, math.huge end + s = sqrt(s) + return dcr - s, dcr + s +end + +function intersectsRay_Ellipsoid(rpos, rdir, c1, x1, y1, z1) + local invx1, invy1, invz1 = 1 / (x1:squaredLength() + 1e-30), 1 / (y1:squaredLength() + 1e-30), 1 / (z1:squaredLength() + 1e-30) + local cc = rpos - c1 + local pM = vec3(cc:dot(x1)*invx1, cc:dot(y1)*invy1, cc:dot(z1)*invz1) + local dirM = vec3(rdir:dot(x1)*invx1, rdir:dot(y1)*invy1, rdir:dot(z1)*invz1) + + local a, b, c = dirM:squaredLength(), 2*pM:dot(dirM), pM:squaredLength() - 1 + local d = b*b - 4*a*c + if d < 0 then return math.huge, math.huge end + d = -b -sign(b)*sqrt(d) + local r1, r2 = 0.5*d / a, 2*c / d + return min(r1, r2), max(r1,r2) +end + +function intersectsRay_Cylinder(rpos, rdir, cposa, cposb, cR) + local rca, cba = cposa - rpos, cposb - cposa + local cpnorm = cba:normalized() + local cp = rca:projectToOriginPlane(cpnorm) + local rdp = rdir:projectToOriginPlane(cpnorm) + local minhit, maxhit = intersectsRay_Sphere(vec3(0,0,0), rdp:normalized(), cp, cR) + local invrdplen = 1 / (rdp:length() + 1e-30) + minhit, maxhit = minhit * invrdplen, maxhit * invrdplen + local plhita, plhitb = intersectsRay_Plane(rpos, rdir, cposa, cpnorm), intersectsRay_Plane(rpos, rdir, cposb, cpnorm) + minhit, maxhit = max(minhit, min(plhita, plhitb)), min(maxhit, max(plhita, plhitb)) + return (minhit <= maxhit and minhit or math.huge), maxhit +end + +-- returns hit distance, barycentric x, y +function intersectsRay_Triangle(rpos, rdir, a, b, c) + local ca, bc = c - a, b - c + local norm = ca:cross(bc) + local rposc = rpos - c + local pOnTri = rposc:dot(norm) / rdir:dot(norm) + if pOnTri <= 0 then + local pacnorm = (rposc - rdir * pOnTri):cross(norm) + local bx, by = bc:dot(pacnorm), ca:dot(pacnorm) + local normSq = norm:squaredLength() + 1e-30 + if min(bx, by) >= 0 and bx + by <= normSq then + return -pOnTri, bx / normSq, by / normSq + end + end + return math.huge, -1, -1 +end \ No newline at end of file