28 lines
728 B
Python
28 lines
728 B
Python
from math import floor, ceil
|
|
|
|
def padd(p1, p2):
|
|
return (p1[0] + p2[0], p1[1] + p2[1], p1[2] + p2[2])
|
|
|
|
def psub(p1, p2):
|
|
return (p1[0] - p2[0], p1[1] - p2[1], p1[2] - p2[2])
|
|
|
|
def pmul(p, s):
|
|
return (s*p[0], s*p[1], s*p[2])
|
|
|
|
def phyp(p1, p2):
|
|
return hypot(p1[0] - p2[0], p1[1] - p2[1], p1[2] - p2[2])
|
|
|
|
def phyp_bias(p1, p2, origin):
|
|
origin_distance = phyp(origin, p2)
|
|
height_diff = p2[1] - p1[1]
|
|
height_diff = height_diff*8 if height_diff < 0 else height_diff*0.5
|
|
return hypot(p1[0] - p2[0], height_diff, p1[2] - p2[2]) + origin_distance*1.5
|
|
|
|
def pint(p):
|
|
return (floor(p[0]), floor(p[1]), floor(p[2]))
|
|
|
|
def cap(x, amount):
|
|
sign = 1 if x >= 0 else -1
|
|
return sign * min(abs(x), amount)
|
|
|