def intersects(self, r):
''' ???
:param Ray r:
:returns: ?, point, normal
:rtype: float, Vector3D, Vector3D
'''
y = r.start - self._center
c = vector.dot(y, y) - self._radius * self._radius
b = vector.dot(r.direction, y)
if b * b - c < 0:
return float('inf'), None, None
t1 = -b + math.sqrt(b * b - c)
t2 = -b - math.sqrt(b * b - c)
if t1 < 0:
return float('inf'), None, None
else:
if t2 < 0:
point = r.start + r.direction * t1
normal = (point - self._center).normalized
if t1 < r.t_max:
return t1, point, normal
else:
return float('inf'), None, None
else:
point = r.start + r.direction * t2
normal = (point - self._center).normalized
if t2 < r.t_max:
return t2, point, normal
else:
return float('inf'), None, None
def calculate_normal(self, b1, b2):
"""
Calculates the normal of b1 with respect to b2
"""
normal_x = 0.0
normal_y = 0.0
# Difference between body centers
center_dir = vector.normalize(vector.sub(b1.rect.center(), b2.rect.center()))
cos_threshold = b2.rect.w / math.sqrt(math.pow(b2.rect.h, 2) + math.pow(b2.rect.w, 2))
if vector.dot(center_dir, vector.Vector2(1, 0)) >= cos_threshold:
# b1 is at the right side of b2
normal_x = 1.0
elif vector.dot(center_dir, vector.Vector2(-1, 0)) >= cos_threshold:
# b1 is at the left side of b2
normal_x = -1.0
elif vector.dot(center_dir, vector.Vector2(0, -1)) >= math.cos(math.pi * 0.5 - math.acos(cos_threshold)):
# b1 is above b2
normal_y = -1.0
else:
# b1 is below b2
normal_y = 1.0
return vector.Vector2(normal_x, normal_y)
def lighting(self, lights, camera, transform):
temp_color = Vector3(0, 0, 0)
supern = (self.normal * transform).normalized()
for l in range(len(lights)):
center = self.center * transform
amb = self.scene.pairwise_mult(self.md[self.material]["amb"])
temp_color += amb
n = (lights[l].pos - center).normalized()
o = vector.dot(n, supern)
if o < 0:
pass
else:
r = 2 * o * supern - n
i = VectorN(camera.pos.x, camera.pos.y, camera.pos.z, 1)
v = (i - center).normalized()
m = vector.dot(v, r)
diff_c = o * lights[l].diff.pairwise_mult(
self.md[self.material]["diff"])
if m < 0:
spec_c = vector.Vector3(0, 0, 0)
else:
spec_c = m ** self.md[self.material]["hard"] * \
(lights[l].spec.pairwise_mult(self.md[self.material]["spec"]))
temp_color += spec_c + diff_c
return temp_color.clamp()
def G_GGX_Isotropic(parameters: Tensor, normals: Tensor,
directions: Tensor, halfways: Tensor) -> Tensor:
'''
Evaluates the isotropic GGX shadow-masking G1 function for the provided halfway and (incident or outbound) directions
using the given parameters. See https://www.cs.cornell.edu/~srm/publications/EGSR07-btdf.pdf for more details.
Args:
parameters: Tensor [B, R, C, 4] of α and normal parameters.
normals: Tensor [B, R, C, D, 3] of surface normals.
directions: Tensor [B, R, C, D, 3] of (incident or outbound) directions.
halfways: Tensor [B, R, C, D, 3] of halfway directions.
Returns:
Tensor [B, R, C, D, 3] of G1 visibility values for each (incident or outbound) direction.
'''
assert parameters.size(
3
) == 1, 'Isotropic GGX shadow-masking G1 function must have 1 value for each spatial parameter.'
alphas = torch.unsqueeze(parameters, dim=3)
visible = torch.sign(
vector.dot(directions, halfways) *
vector.dot(directions, normals)) == 1
zeniths = vector.dot(directions, normals)
return visible / (zeniths + torch.sqrt(alphas**2 +
(1 - alphas**2) * zeniths**2))
def rr_int(p1, v1, p2, v2):
"""Intersect ray though p1 direction v1 with ray through p2 direction v2.
Returns a list of zero or one solutions
"""
diag_print("rr_int "+str(p1)+str(v1)+str(p2)+str(v2),"intersections")
s = ll_int(p1,v1,p2,v2)
if len(s) > 0 and tol_gte(vector.dot(s[0]-p2,v2), 0) and tol_gte(vector.dot(s[0]-p1,v1),0):
return s
else:
return []
def test_perp_3d():
for i in range(10):
u = normalised(vector.randvec(3))
v, w = perp_3d(u)
print u, vector.norm(u)
print v, vector.norm(v)
print w, vector.norm(w)
print tol_eq(vector.dot(u, v), 0.0)
print tol_eq(vector.dot(v, w), 0.0)
print tol_eq(vector.dot(w, u), 0.0)
def test_perp_3d():
for i in range(10):
u = normalised(vector.randvec(3))
v,w = perp_3d(u)
print u, vector.norm(u)
print v, vector.norm(v)
print w, vector.norm(w)
print tol_eq(vector.dot(u,v),0.0)
print tol_eq(vector.dot(v,w),0.0)
print tol_eq(vector.dot(w,u),0.0)
def dir_from_segment(point, a, b):
if dot(b - a, point - a) <= 0:
return normalize(point - a)
if dot(a - b, point - b) <= 0:
return normalize(point - b)
normal = normalize(b - a)
normal = Vector(-normal.y, normal.x)
if dot(normal, point - a) < 0:
normal = -normal
assert(distance_to_segment(point, a, b) < distance_to_segment(point + normal, a, b))
return normal
def rr_int(p1, v1, p2, v2):
"""Intersect ray though p1 direction v1 with ray through p2 direction v2.
Returns a list of zero or one solutions
"""
diag_print("rr_int " + str(p1) + str(v1) + str(p2) + str(v2),
"intersections")
s = ll_int(p1, v1, p2, v2)
if len(s) > 0 and tol_gte(vector.dot(s[0] - p2, v2), 0) and tol_gte(
vector.dot(s[0] - p1, v1), 0):
return s
else:
return []
def dir_from_segment(point, a, b):
if dot(b - a, point - a) <= 0:
return normalize(point - a)
if dot(a - b, point - b) <= 0:
return normalize(point - b)
normal = normalize(b - a)
normal = Vector(-normal.y, normal.x)
if dot(normal, point - a) < 0:
normal = -normal
assert (distance_to_segment(point, a, b) < distance_to_segment(
point + normal, a, b))
return normal
def intersect(self, ray):
v1 = ray.orig - self.a
v2 = self.b - self.a
v3 = Vector(-ray.dir.y, ray.dir.x)
if dot(v2, v3) == 0:
return None
ts = dot(v1, v3) / dot(v2, v3)
tr = cross(v2, v1) / dot(v2, v3)
if tr >= 0.0 and 0.0 <= ts <= 1.0:
return self.a + v2 * ts
else:
return None
def intersect(self, ray):
v1 = ray.orig - self.a
v2 = self.b - self.a
v3 = Vector(-ray.dir.y, ray.dir.x)
if dot(v2, v3) == 0:
return None
ts = dot(v1, v3) / dot(v2, v3)
tr = cross(v2, v1) / dot(v2, v3)
if tr >= 0.0 and 0.0 <= ts <= 1.0:
return self.a + v2 * ts
else:
return None
def rayTest(self, R):
"""Returns a list of sorted Raycollisions or none if its empty."""
Q = vector.dot(R.Dhat, self.norm)
if Q != 0:
t = (self.distance - vector.dot(R.origin, self.norm)) / Q
if t < 0:
return []
else:
p = R.getPoint(t)
return [Raycollision(R, self, t, p, self.norm)]
else:
return []
def f_matrix2fit(beta, x, efit):
b = numpy.matrix(lmap(lambda a, b: a - b, x[:3], beta[:3]))
r = numpy.matrix([[1, efit[0], efit[1]], [efit[2], beta[4], efit[3]],
[efit[4], efit[5], beta[5]]])
m = r * b
m = list(numpy.array(m.transpose()))
r0 = lmap(lambda y: beta[3]**2 - vector.dot(y, y), m)
#return r0
g = list(numpy.array(numpy.matrix(x[3:]).transpose()))
r1 = lmap(lambda y, z: beta[6] - vector.dot(y, z), m, g)
return r0 + r1
def biCGsolve(self, x0, b, tol=1.0e-10, nmax=1000):
"""
Solve self*x = b and return x using the bi-conjugate gradient method
"""
try:
if not vector.isVector(b):
raise TypeError, self.__class__, " in solve "
else:
if self.size()[0] != len(b) or self.size()[1] != len(b):
print("**Incompatible sizes in solve")
print("**size()=", self.size()[0], self.size()[1])
print("**len=", len(b))
else:
kvec = diag(self) # preconditionner
n = len(b)
x = x0 # initial guess
r = b - dot(self, x)
rbar = r
w = r / kvec
wbar = rbar / kvec
p = vector.zeros(n)
pbar = vector.zeros(n)
beta = 0.0
rho = vector.dot(rbar, w)
err = vector.norm(dot(self, x) - b)
k = 0
print(" bi-conjugate gradient convergence (log error)")
while abs(err) > tol and k < nmax:
p = w + beta * p
pbar = wbar + beta * pbar
z = dot(self, p)
alpha = rho / vector.dot(pbar, z)
r = r - alpha * z
rbar = rbar - alpha * dot(pbar, self)
w = r / kvec
wbar = rbar / kvec
rhoold = rho
rho = vector.dot(rbar, w)
x = x + alpha * p
beta = rho / rhoold
err = vector.norm(dot(self, x) - b)
print(k, " %5.1f " % math.log10(err))
k = k + 1
return x
except:
print("ERROR ", self.__class__, "::biCGsolve")
def biCGsolve(self, x0, b, tol=1.0e-10, nmax=1000):
"""
Solve self*x = b and return x using the bi-conjugate gradient method
"""
try:
if not vector.isVector(b):
raise TypeError, self.__class__, ' in solve '
else:
if self.size()[0] != len(b) or self.size()[1] != len(b):
print '**Incompatible sizes in solve'
print '**size()=', self.size()[0], self.size()[1]
print '**len=', len(b)
else:
kvec = diag(self) # preconditionner
n = len(b)
x = x0 # initial guess
r = b - dot(self, x)
rbar = r
w = r / kvec
wbar = rbar / kvec
p = vector.zeros(n)
pbar = vector.zeros(n)
beta = 0.0
rho = vector.dot(rbar, w)
err = vector.norm(dot(self, x) - b)
k = 0
print " bi-conjugate gradient convergence (log error)"
while abs(err) > tol and k < nmax:
p = w + beta * p
pbar = wbar + beta * pbar
z = dot(self, p)
alpha = rho / vector.dot(pbar, z)
r = r - alpha * z
rbar = rbar - alpha * dot(pbar, self)
w = r / kvec
wbar = rbar / kvec
rhoold = rho
rho = vector.dot(rbar, w)
x = x + alpha * p
beta = rho / rhoold
err = vector.norm(dot(self, x) - b)
print k, ' %5.1f ' % math.log10(err)
k = k + 1
return x
except:
print 'ERROR ', self.__class__, '::biCGsolve'
def biCGsolve(self,x0, b, tol=1.0e-10, nmax = 1000): """ Solve self*x = b and return x using the bi-conjugate gradient method """ try: if not vector.isVector(b): raise TypeError, self.__class__,' in solve ' else: if self.size()[0] != len(b) or self.size()[1] != len(b): print '**Incompatible sizes in solve' print '**size()=', self.size()[0], self.size()[1] print '**len=', len(b) else: kvec = diag(self) # preconditionner n = len(b) x = x0 # initial guess r = b - dot(self, x) rbar = r w = r/kvec; wbar = rbar/kvec; p = vector.zeros(n); pbar = vector.zeros(n); beta = 0.0; rho = vector.dot(rbar, w); err = vector.norm(dot(self,x) - b); k = 0 print " bi-conjugate gradient convergence (log error)" while abs(err) > tol and k < nmax: p = w + beta*p; pbar = wbar + beta*pbar; z = dot(self, p); alpha = rho/vector.dot(pbar, z); r = r - alpha*z; rbar = rbar - alpha* dot(pbar, self); w = r/kvec; wbar = rbar/kvec; rhoold = rho; rho = vector.dot(rbar, w); x = x + alpha*p; beta = rho/rhoold; err = vector.norm(dot(self, x) - b); print k,' %5.1f ' % math.log10(err) k = k+1 return x except: print 'ERROR ',self.__class__,'::biCGsolve'
def ray_collision(self, ray_origin, ray_direction, max_dist=-1):
""" returns a list of pairs (object, point) where the ray hits this object, or an empty list if no hits"""
offset = self.mPosition - ray_origin
offset_mag_sq = vector.dot(offset, offset)
# If this ray originates outside the circle (first clause) and is facing away from the sphere (second clause),
# there can't be a hit -- no need to go farther
if offset_mag_sq > self.mRadiusSquared and vector.dot(
offset, ray_direction) < 0:
return []
para_dist = vector.dot(offset, ray_direction)
para_dist_sq = para_dist**2
perp_dist_sq = offset_mag_sq - para_dist_sq
if perp_dist_sq <= self.mRadiusSquared:
# The ray goes through the sphere. Start to calculate the intersection points
closest = ray_origin + para_dist * ray_direction
if perp_dist_sq == self.mRadiusSquared:
# Incredibly rare, but oh well. Just one intersection at the tip of the sphere
dist = (closest - ray_origin).magnitude()
if max_dist < 0 or dist < max_dist:
return [closest]
else:
return []
else:
closest_offset = (self.mRadiusSquared - perp_dist_sq)**0.5
if offset_mag_sq < self.mRadiusSquared:
# The ray originates inside the sphere. There will just be one collision
hit_pt = closest + closest_offset * ray_direction
dist = (hit_pt - ray_origin).magnitude()
if max_dist < 0 or dist < max_dist:
return [hit_pt]
else:
return []
else:
# More common case: the ray originates outside, there're two collisions
hit_points = [
closest - closest_offset * ray_direction,
closest + closest_offset * ray_direction
]
for i in range(len(hit_points) - 1, -1, -1):
dist = (hit_points[i] - ray_origin).magnitude()
if max_dist >= 0 and dist > max_dist:
del hit_points[i]
return hit_points
# If we get here, no hits!
return []
def angle_3p(p1, p2, p3):
"""Returns the angle, in radians, rotating vector p2p1 to vector p2p3.
arg keywords:
p1 - a vector
p2 - a vector
p3 - a vector
returns: a number
In 2D, the angle is a signed angle, range [-pi,pi], corresponding
to a clockwise rotation. If p1-p2-p3 is clockwise, then angle > 0.
In 3D, the angle is unsigned, range [0,pi]
"""
d21 = vector.norm(p2-p1)
d23 = vector.norm(p3-p2)
if tol_eq(d21,0) or tol_eq(d23,0):
# degenerate, indeterminate angle
return None
v21 = (p1-p2) / d21
v23 = (p3-p2) / d23
t = vector.dot(v21,v23) # / (d21 * d23)
if t > 1.0: # check for floating point error
t = 1.0
elif t < -1.0:
t = -1.0
angle = math.acos(t)
if len(p1) == 2: # 2D case
if is_counterclockwise(p1,p2,p3):
angle = -angle
return angle
def ComputeDeviation(points, fit):
m, d = 0, 0
for p in points:
v = vector.sub(p[:3], fit[:3])
m += (1 - vector.dot(v, v) / fit[3]**2)**2
if len(fit) > 4:
n = vector.dot(v, p[3:]) / vector.norm(v)
if abs(n) <= 1:
ang = math.degrees(math.asin(n))
d += (fit[4] - ang)**2
else:
d += 1e111
m /= len(points)
d /= len(points)
return [m**.5, d**.5]
def findAxisLeastPenetration(self, faceIndex, poly1, poly2):
bestDistance = -1000000.0
bestIndex = 0
for i in range(poly1.vertexCount):
#transform A's face normal int B's model space
nw = poly1.u * poly1.normals[i]
buT = poly2.u.transpose()
n = buT * nw
#retrieve support point from B along -n
s = poly2.getSupport( -n )
#retrieve vertex on face from A , transform into B's model space
v = poly1.vertices[i]
v = poly1.u * v + poly1.body.pos
v -= poly2.body.pos
v = buT * v
#compute penetration ( in B's model)
d = dot(n, s - v)
if d > bestDistance:
bestDistance = d
bestIndex = i
faceIndex[0] = bestIndex
return bestDistance
def sss_int(p1, r1, p2, r2, p3, r3):
"""Intersect three spheres, centered in p1, p2, p3 with radius r1,r2,r3 respectively.
Returns a list of zero, one or two solution points.
"""
solutions = []
# plane though p1, p2, p3
n = vector.cross(p2 - p1, p3 - p1)
n = n / vector.norm(n)
# intersect circles in plane
cp1 = vector.vector([0.0, 0.0])
cp2 = vector.vector([vector.norm(p2 - p1), 0.0])
cpxs = cc_int(cp1, r1, cp2, r2)
if len(cpxs) == 0:
return []
# px, rx, nx is circle
px = p1 + (p2 - p1) * cpxs[0][0] / vector.norm(p2 - p1)
rx = abs(cpxs[0][1])
# plane of intersection cicle
nx = p2 - p1
nx = nx / vector.norm(nx)
# print "px,rx,nx:",px,rx,nx
# py = project p3 on px,nx
dy3 = vector.dot(p3 - px, nx)
py = p3 - (nx * dy3)
if tol_gt(dy3, r3):
return []
ry = math.sin(math.acos(abs(dy3 / r3))) * r3
# print "py,ry:",py,ry
cpx = vector.vector([0.0, 0.0])
cpy = vector.vector([vector.norm(py - px), 0.0])
cp4s = cc_int(cpx, rx, cpy, ry)
for cp4 in cp4s:
p4 = px + (py - px) * cp4[0] / vector.norm(py - px) + n * cp4[1]
solutions.append(p4)
return solutions
def hit(self, ray: vector.Ray, t_min: float, t_max: float):
oc = ray.origin - self.center
a = ray.direction.length_squared()
half_b = vector.dot(oc, ray.direction)
c = oc.length_squared() - self.radius * self.radius
discriminant = half_b * half_b - a * c
if discriminant > 0:
root = math.sqrt(discriminant)
temp = (-half_b - root) / a
if t_min < temp < t_max:
hit_rec = HitRecord(None, None, None, None)
hit_rec.t = temp
hit_rec.position = ray.at(hit_rec.t)
hit_rec.material = self.m
outward_normal = (hit_rec.position - self.center).divide(
self.radius)
hit_rec.set_face_normal(ray, outward_normal)
return True, hit_rec
temp = (-half_b + root) / a
if t_min < temp < t_max:
hit_rec = HitRecord(None, None, None, None)
hit_rec.t = temp
hit_rec.position = ray.at(hit_rec.t)
hit_rec.material = self.m
outward_normal = (hit_rec.position - self.center).divide(
self.radius)
hit_rec.set_face_normal(ray, outward_normal)
return True, hit_rec
return False, None
def matrix_multiply(A, B):
if cols(A) != rows(B):
raise RuntimeError(
"Can't multiply matrices with dimensions {} and {}".format(
shape(A), shape(B)))
return [[dot(row, col) for col in zip(*B)] for row in A]
def set_face_normal(self, ray: vector.Ray, outward_normal: vector.Vec3):
self.front_face = vector.dot(ray.direction, outward_normal) < 0
if self.front_face:
self.normal = outward_normal
else:
self.normal = -outward_normal
def is_in_disk(q, Disk):
p1 = Disk[0]
p2 = Disk[1]
if len(Disk) == 2:
c2 = p1 + p2
rad_vec = p1 * 2 - c2
dist_vec = q * 2 - c2
return vec.dot(dist_vec, dist_vec) <= vec.dot(rad_vec, rad_vec)
if len(Disk) == 3:
p3 = Disk[2]
D, DX, DY = getD_DX_DY(p1, p2, p3)
return ((D * q.x - DX) ** 2 + (D * q.y + DY) ** 2) <= ((D * p1.x - DX) ** 2 + (D * p1.y + DY) ** 2)
return False
def angle_3p(p1, p2, p3):
"""Returns the angle, in radians, rotating vector p2p1 to vector p2p3.
arg keywords:
p1 - a vector
p2 - a vector
p3 - a vector
returns: a number
In 2D, the angle is a signed angle, range [-pi,pi], corresponding
to a clockwise rotation. If p1-p2-p3 is clockwise, then angle > 0.
In 3D, the angle is unsigned, range [0,pi]
"""
d21 = vector.norm(p2 - p1)
d23 = vector.norm(p3 - p2)
if tol_eq(d21, 0) or tol_eq(d23, 0):
# degenerate, indeterminate angle
return None
v21 = (p1 - p2) / d21
v23 = (p3 - p2) / d23
t = vector.dot(v21, v23) # / (d21 * d23)
if t > 1.0: # check for floating point error
t = 1.0
elif t < -1.0:
t = -1.0
angle = math.acos(t)
if len(p1) == 2: # 2D case
if is_counterclockwise(p1, p2, p3):
angle = -angle
return angle
def stoch_descent(X, y, alpha, w):
"""
Stochastic gradient descent
:param X:
:param y:
:param alpha:
:param w:
:return:
"""
global logs, logs_stoch
logs = []
logs_stoch = []
random.seed(0)
idx = list(range(len(X)))
for epoch in range(1000):
random.shuffle(idx)
w_old = w
for i in idx:
loss = y[i] - vector.dot(X[i], w)
gradient = vector.mul(loss, X[i])
w = vector.add(w, vector.mul(alpha, gradient))
logs_stoch += (w, alpha, sse(X, y, w))
if vector.norm(vector.sub(w, w_old)) / vector.norm(w) < 1.0e-5:
break
logs += (w, alpha, sse(X, y, w))
print("Epoch", epoch)
return w
def point_inside(self, pt):
offset = pt - self.mPosition
for i in range(2):
proj = abs(vector.dot(offset, self.mAxes[i]))
if proj > self.mExtents[i]:
return False
return True
def scatter(self, ray_in: vector.Ray, hit_record: hittable.HitRecord):
attenuation = vector.Vec3(1.0, 1.0, 1.0)
etai_over_etat = self.ref_idx
if hit_record.front_face:
etai_over_etat = 1.0 / self.ref_idx
unit_direction = vector.unit_vector(ray_in.direction)
cos_theta = min(vector.dot(-unit_direction, hit_record.normal), 1.0)
sin_theta = math.sqrt(1.0 - cos_theta*cos_theta)
if etai_over_etat * sin_theta > 1.0:
reflected = vector.reflect(unit_direction, hit_record.normal)
scattered = vector.Ray(hit_record.position, reflected)
return True, attenuation, scattered
reflect_prob = vector.schlick(cos_theta, etai_over_etat)
if util.random_double() < reflect_prob:
reflected = vector.reflect(unit_direction, hit_record.normal)
scattered = vector.Ray(hit_record.position, reflected)
return True, attenuation, scattered
refracted = vector.refract(unit_direction, hit_record.normal, etai_over_etat)
scattered = vector.Ray(hit_record.position, refracted)
return True, attenuation, scattered
def is_right_handed(p1, p2, p3, p4):
"""return True if tetrahedron p1 p2 p3 p4 is right handed"""
u = p2 - p1
v = p3 - p1
uv = vector.cross(u, v)
w = p4 - p1
return vector.dot(uv, w) > 0
def __init__(self, material, origin, norm, width, height):
"""Derives the base attributes from baseobject, but also takes a normal direction and point on the plane."""
super().__init__(material, origin)
self.norm = norm.normalized()
self.distance = vector.dot(self.origin, self.norm)
self.height = height
self.width = width
def stoch_descent(X, y, alpha, w):
"""
Stochastic gradient descent
:param X:
:param y:
:param alpha:
:param w:
:return:
"""
global logs, logs_stoch
logs = []
logs_stoch = []
random.seed(0)
idx = list(range(len(X)))
for epoch in range(1000):
random.shuffle(idx)
w_old = w
for i in idx:
loss = y[i] - vector.dot(X[i], w)
gradient = vector.mul(loss, X[i])
w = vector.add(w, vector.mul(alpha, gradient))
logs_stoch += (w, alpha, sse(X, y, w))
if vector.norm(vector.sub(w, w_old)) / vector.norm(w) < 1.0e-5:
break
logs += (w, alpha, sse(X, y, w))
print("Epoch", epoch)
return w
def is_coplanar(p1,p2,p3,p4):
"""return True if tetrahedron p1 p2 p3 p4 is co-planar (not left- or right-handed)"""
u = p2-p1
v = p3-p1
uv = vector.cross(u,v)
w = p4-p1
#return vector.dot(uv,w) == 0
return tol_eq(vector.dot(uv,w), 0)
def is_coplanar(p1, p2, p3, p4):
"""return True if tetrahedron p1 p2 p3 p4 is co-planar (not left- or right-handed)"""
u = p2 - p1
v = p3 - p1
uv = vector.cross(u, v)
w = p4 - p1
#return vector.dot(uv,w) == 0
return tol_eq(vector.dot(uv, w), 0)
def is_left_handed(p1, p2, p3, p4):
"""return True if tetrahedron p1 p2 p3 p4 is left handed"""
u = p2 - p1
v = p3 - p1
uv = vector.cross(u, v)
w = p4 - p1
#return vector.dot(uv,w) < 0
return tol_lt(vector.dot(uv, w), 0)
def vec2vec2quat(a, b):
n = vector.cross(a, b)
fac = vector.dot(a, b) / vector.norm(a) / vector.norm(b)
fac = min(max(fac, -1),
1) # protect against possible slight numerical errors
ang = math.acos(fac)
return angvec2quat(ang, n)
def is_right_handed(p1,p2,p3,p4):
"""return True if tetrahedron p1 p2 p3 p4 is right handed"""
u = p2-p1
v = p3-p1
uv = vector.cross(u,v)
w = p4-p1
#return vector.dot(uv,w) > 0
return tol_gt(vector.dot(uv,w), 0)
def right_angle(v1, v2):
x = vector.Vector(v1[0], v1[1])
y = vector.Vector(v2[0], v2[1])
dot = vector.dot(x, y)
_angle = math.acos(dot / x.length() / y.length())
if x[0] * y[1] < y[0] * x[1]:
_angle = 2 * math.pi - _angle
return _angle
def right_angle(v1, v2):
x = vector.Vector(v1[0], v1[1])
y = vector.Vector(v2[0], v2[1])
dot = vector.dot(x, y)
_angle = math.acos(dot / x.length() / y.length())
if x[0] * y[1] < y[0] * x[1]:
_angle = 2 * math.pi - _angle
return _angle
def _perceptron(feature_vect, w, positive): r = v.dot(feature_vect, w) if positive and r<=0: return v.vec_add(w, feature_vect), False elif not positive and r>=0: return v.vec_sub(w, feature_vect), False else: return w, True
def on_ball_paddle_collision(self, ball_body, paddle_body, normal):
# Adjusts the ball direction if the paddle is moving when the ball collides with it
angle = math.acos(dot(normal, ball_body.direction)) # Angle between the reflected direction and the normal
delta_angle = abs(((math.pi * 0.5) - angle) * 0.5) # Half the angle that remains if were to perform a 90 degree reflection
if paddle_body.direction.x > 0: # Clockwise rotation because the paddle is moving to the right
ball_body.direction = normalize(rotate(ball_body.direction, delta_angle))
elif paddle_body.direction.x < 0: # Counter-clockwise rotation because the paddle is moving to the left
ball_body.direction = normalize(rotate(ball_body.direction, -delta_angle))
def rayTest(self, R):
"""Returns a list of sorted Raycollisions or none if its empty."""
hits = []
hat_rack = [
self.Xhat, -self.Xhat, self.Yhat, -self.Yhat, self.Zhat, -self.Zhat
]
hext_test = [
self.hext.x, self.hext.x, self.hext.y, self.hext.y, self.hext.z,
self.hext.z
]
for i in range(6):
norm = hat_rack[i]
d = vector.dot((self.origin + (hext_test[i] * norm)), norm)
if vector.dot(R.Dhat, norm) != 0:
t = (d - vector.dot(R.origin, norm)) / (vector.dot(
R.Dhat, norm))
if t < 0:
continue
Pw = R.getPoint(t)
Q = Pw - self.origin
Pl = vector.Vector3(vector.dot(Q, self.Xhat),
vector.dot(Q, self.Yhat),
vector.dot(Q, self.Zhat))
if -self.hext.x - 0.0001 <= Pl.x and Pl.x <= self.hext.x + 0.0001:
if -self.hext.y - 0.0001 <= Pl.y and Pl.y <= self.hext.y + 0.0001:
if -self.hext.z - 0.0001 <= Pl.z and Pl.z <= self.hext.z + 0.0001:
hits += [
Raycollision(R, self, t, R.getPoint(t), norm)
]
return hits
def align_squares(self, guess, cam_matrix, distortion_coefficients,
object_points):
"""
Needed because ABCD has different camera location from BCDA. Takes a
square's corners, camera intrisics, object information, and a camera
rotation to align to. Finds the camera position that gives a rotation
vector (unit vector in cam direction using grid axes) closest to guess.
:param guess:
:param cam_matrix:
:param distortion_coefficients:
:param object_points:
:return: nothing
"""
alignment_scores = [dot(self.cam_rot, guess), 0, 0, 0]
# Loop through possible orders: ABCD, BCDA, CDAB, DABC
for rot in range(1, 4):
# Shift to the rot permutation
temp_corners = np.roll(self.corners, rot, axis=0)
# Get vector from camera to center of square
temp_corners = temp_corners.reshape(4, 2, 1).astype(float)
inliers, self.rvec, self.tvec = cv2.solvePnP(
object_points, temp_corners, cam_matrix,
distortion_coefficients)
# Get rotation information for change to square center at origin
rot_matrix = cv2.Rodrigues(self.rvec)
cam_pos = np.multiply(cv2.transpose(rot_matrix[0]),
-1).dot(self.tvec)
cam_to_grid_transform = np.concatenate(
(cv2.transpose(rot_matrix[0]), cam_pos), axis=1)
# Compare camera unit vector to guess
alignment_scores[rot] = dot(
list(cam_to_grid_transform.dot(np.array([0, 0, 1, 0]))), guess)
# Pick the orientation that was best and recompute camera location
self.corners = np.roll(self.corners,
alignment_scores.index(max(alignment_scores)),
axis=0)
self.update_pos_stats(cam_matrix, distortion_coefficients,
object_points)
def update(self, mouse, keys, dt): #GRAVITY VELOCITY / ACCELERATION
self.camera.update(mouse)
self.rotation = self.camera.rotation
wish_vector = vector.vec3()
wish_vector.x = ((SDLK_d in keys) - (SDLK_a in keys))
wish_vector.y = ((SDLK_w in keys) - (SDLK_s in keys))
true_wish = wish_vector.rotate(0, 0, -self.rotation.z)
self.front = vector.vec3(0, 1, 0).rotate(0, 0, -self.rotation.z)
#GET CURRENT FRICTION (FROM SURFACE CONTACT)
true_speed = self.speed * (1 if self.onGround else 1.75)
self.position += true_wish * true_speed * dt #NEED FRICTION
if not self.onGround:
self.velocity += 0.5 * vector.vec3(0, 0, -9.81) * dt #GRAVITY
self.onGround = False
self.old_position = self.position
self.position += self.velocity
#min maxing old & new positions
min_x = min(self.old_position.x, self.position.x)
max_x = max(self.old_position.x, self.position.x)
min_y = min(self.old_position.y, self.position.y)
max_y = max(self.old_position.y, self.position.y)
min_z = min(self.old_position.z, self.position.z)
max_z = max(self.old_position.z, self.position.z)
self.swept_aabb = aabb(
vector.vec3(min_x, min_y, min_z) + self.aabb.min,
vector.vec3(max_x, max_y, max_z) + self.aabb.max)
global planes
for plane in planes: #filtered with position & bsp nodes
#also should combine results rather than applying in order
if self.swept_aabb.intersects(plane.aabb):
p = vector.dot(self.position, plane.normal)
max_p = self.swept_aabb.depth_along_axis(plane.normal)
if p <= max_p and p <= abs(plane.distance): # simplify
# push out of the plane, without changing velocity
self.position += math.fsum([plane.distance, -p
]) * plane.normal
# reset jump? (45 degree check)
if vector.dot(plane.normal,
vector.vec3(z=1)) <= math.sqrt(2):
self.onGround = True
self.velocity = vector.vec3()
#friction, surf & bounce
## self.velocity -= self.velocity * plane.normal
if SDLK_SPACE in keys: #JUMP
self.velocity.z += .6
def paint_half_plane(xr, yr, v, sign, color):
xl = []
yl = []
for x in xr:
for y in yr:
if sign * (vector.dot([1, -x], v) - y) > 0:
xl.append(x)
yl.append(y)
plt.plot(xl, yl, color)
def is_counterclockwise(p1,p2,p3):
""" returns True iff triangle p1,p2,p3 is counterclockwise oriented"""
assert len(p1)==2
assert len(p1)==len(p2)
assert len(p2)==len(p3)
u = p2 - p1
v = p3 - p2;
perp_u = vector.vector([-u[1], u[0]])
return tol_gt(vector.dot(perp_u,v), 0)
def get_color(self, p):
'''
:param Vector3D p: ??
'''
if self._angle < math.acos(vector.dot(self._direction,
p - self._position) /
(p - self._position).length):
return color.RaytracerColor()
return self._color
def paint_half_plane(xr, yr, v, sign, color):
xl = []
yl = []
for x in xr:
for y in yr:
if sign * (vector.dot([1, -x], v) - y) > 0:
xl.append(x)
yl.append(y)
plt.plot(xl, yl, color)
def intersect_circle(self, c):
d = self.b - self.a
f = self.a - c.center
a = dot(d, d)
b = 2 * dot(f, d)
c = dot(f, f) - c.radius * c.radius
discriminant = b * b - 4 * a * c
if discriminant < 0:
# no intersection
return None
else:
discriminant = sqrt(discriminant)
t1 = (-b - discriminant) / (2.0 * a)
t2 = (-b + discriminant) / (2.0 * a)
if t1 >= 0 and t1 <= 1:
return True
if t2 >= 0 and t2 <= 1:
return True
return None
def sse(X, y, w):
"""
Sum of squared errors
:param X:
:param y:
:param w:
:return:
"""
error = vector.sub(y, vector.mul_mat_vec(X, w))
return vector.dot(error, error)
def on_ball_left_right_collision(self, ball_body, wall_body, normal):
angle = math.acos(dot(normal, ball_body.direction)) # Angle between the reflected direction and the normal
# If the angle is too flat, add a small rotation to the reflected direction
if angle < 0.1:
delta_angle = 0.2
if ball_body.direction.y > 0: # Counter-clockwise rotation because the ball is moving downwards
ball_body.direction = normalize(rotate(ball_body.direction, -delta_angle))
elif ball_body.direction.y <= 0: # Clockwise rotation because the ball is moving upwards
ball_body.direction = normalize(rotate(ball_body.direction, delta_angle))
def CGsolve(self, x0, b, tol=1.0e-10, nmax=1000, verbose=1):
"""
Solve self*x = b and return x using the conjugate gradient method
"""
if not vector.isVector(b):
raise TypeError, self.__class__, " in solve "
else:
if self.size()[0] != len(b) or self.size()[1] != len(b):
print("**Incompatible sizes in solve")
print("**size()=", self.size()[0], self.size()[1])
print("**len=", len(b))
else:
kvec = diag(self) # preconditionner
n = len(b)
x = x0 # initial guess
r = b - dot(self, x)
try:
w = r / kvec
except:
print("***singular kvec")
p = vector.zeros(n)
beta = 0.0
rho = vector.dot(r, w)
err = vector.norm(dot(self, x) - b)
k = 0
if verbose:
print(" conjugate gradient convergence (log error)")
while abs(err) > tol and k < nmax:
p = w + beta * p
z = dot(self, p)
alpha = rho / vector.dot(p, z)
r = r - alpha * z
w = r / kvec
rhoold = rho
rho = vector.dot(r, w)
x = x + alpha * p
beta = rho / rhoold
err = vector.norm(dot(self, x) - b)
if verbose:
print(k, " %5.1f " % math.log10(err))
k = k + 1
return x
def cr_int(p1,r,p2,v):
"""
Intersect a circle (p1,r) with ray (p2,v) (a half-line)
where p1, p2 and v are 2-vectors, r is a scalar
Returns a list of zero, one or two solutions.
"""
sols = []
all = cl_int(p1,r,p2,v)
for s in all:
if tol_gte(vector.dot(s-p2,v), 0):
sols.append(s)
return sols
def sss_int(p1, r1, p2, r2, p3, r3):
"""Intersect three spheres, centered in p1, p2, p3 with radius r1,r2,r3 respectively.
Returns a list of zero, one or two solution points.
"""
solutions = []
# intersect circles in plane
cp1 = vector.vector([0.0,0.0])
cp2 = vector.vector([vector.norm(p2-p1), 0.0])
cpxs = cc_int(cp1, r1, cp2, r2)
if len(cpxs) == 0:
return []
# determine normal of plane though p1, p2, p3
n = vector.cross(p2-p1, p3-p1)
if not tol_eq(vector.norm(n),0.0):
n = n / vector.norm(n)
else:
# traingle p1, p2, p3 is degenerate
# check for 2d solutions
if len(cpxs) == 0:
return []
# project cpxs back to 3d and check radius r3
cp4 = cpxs[0]
u = normalised(p2-p1)
v,w = perp_3d(u)
p4 = p1 + cp4[0] * u + cp4[1] * v
if tol_eq(vector.norm(p4-p3), r3):
return [p4]
else:
return []
# px, rx, nx is circle
px = p1 + (p2-p1) * cpxs[0][0] / vector.norm(p2-p1)
rx = abs(cpxs[0][1])
nx = p2-p1
nx = nx / vector.norm(nx)
# py is projection of p3 on px,nx
dy3 = vector.dot(p3-px, nx)
py = p3 - (nx * dy3)
if tol_gt(dy3, r3):
return []
# ry is radius of circle in py
if tol_eq(r3,0.0):
ry = 0.0
else:
ry = math.sin(math.acos(min(1.0,abs(dy3/r3))))*r3
# determine intersection of circle px, rx and circle py, ry, projected relative to line py-px
cpx = vector.vector([0.0,0.0])
cpy = vector.vector([vector.norm(py-px), 0.0])
cp4s = cc_int(cpx, rx, cpy, ry)
for cp4 in cp4s:
p4 = px + (py-px) * cp4[0] / vector.norm(py-px) + n * cp4[1]
solutions.append(p4)
return solutions
def distance_point_line(p,l1,l2):
"""distance from point p to line l1-l2"""
# v,w is p, l2 relative to l1
v = p-l1
w = l2-l1
# x = projection v on w
lw = vector.norm(w)
if tol_eq(lw,0):
x = 0*w
else:
x = w * vector.dot(v,w) / lw
# result is distance x,v
return vector.norm(x-v)
def align_squares(self, guess, cam_matrix, distortion_coefficients, object_points):
"""
Needed because ABCD has different camera location from BCDA. Takes a
square's corners, camera intrisics, object information, and a camera
rotation to align to. Finds the camera position that gives a rotation
vector (unit vector in cam direction using grid axes) closest to guess.
:param guess:
:param cam_matrix:
:param distortion_coefficients:
:param object_points:
:return: nothing
"""
alignment_scores = [dot(self.cam_rot, guess), 0, 0, 0]
# Loop through possible orders: ABCD, BCDA, CDAB, DABC
for rot in range(1, 4):
# Shift to the rot permutation
temp_corners = np.roll(self.corners, rot, axis=0)
# Get vector from camera to center of square
temp_corners = temp_corners.reshape(4,2,1).astype(float)
inliers, self.rvec, self.tvec = cv2.solvePnP(object_points, temp_corners,
cam_matrix, distortion_coefficients)
# Get rotation information for change to square center at origin
rot_matrix = cv2.Rodrigues(self.rvec)
cam_pos = np.multiply(cv2.transpose(rot_matrix[0]), -1).dot(self.tvec)
cam_to_grid_transform = np.concatenate((cv2.transpose(rot_matrix[0]), cam_pos), axis=1)
# Compare camera unit vector to guess
alignment_scores[rot] = dot(list(cam_to_grid_transform.dot(np.array([0, 0, 1, 0]))), guess)
# Pick the orientation that was best and recompute camera location
self.corners = np.roll(self.corners, alignment_scores.index(max(alignment_scores)), axis=0)
self.update_pos_stats(cam_matrix, distortion_coefficients, object_points)
def can_go_home(self, bag):
"""
Checks if the stick can go home.
:param bag: The parameter bag.
:return:
"""
# no detected puck movement: go home
if 'direction' not in bag.puck:
return True
# nothing to do if the puck is not here
if bag.puck.position[0] < 0.2:
return True
# check if the puck is flying away from home
return vector.dot(self.AWAY_FROM_HOME, bag.puck.direction) > 0.25
def intersect(self, ray):
oc = self.center - ray.orig
# let P be projection of C on the line
# then l = |OP|, h = |CP|
l = dot(oc, ray.dir)
h = cross(oc, ray.dir)
if self.radius < abs(h):
return None
q = sqrt(self.radius ** 2 - h ** 2)
# find t: o + d * t == intersection
t = l - q
if t < 0:
t = l + q
if t < 0:
return None
else:
return ray.orig + t * ray.dir
def line_segment_point_distance(point, seg):
seg_start = vector.Vector(seg[0][0], seg[0][1])
seg_end = vector.Vector(seg[1][0], seg[1][1])
x, y = point
point = vector.Vector(x, y)
seg_dir = seg_end - seg_start
seg_length = seg_dir.length()**2
if seg_length == 0:
return (point - seg_start).length()
t = vector.dot(point - seg_start, seg_dir) / seg_length
if t < 0:
return (point - seg_start).length()
elif t > 1:
return (point - seg_end).length()
projection = seg_start + t * seg_dir
return (point - projection).length()
