import math class Vector3D: def __init__(self, x, y, z): self.x = x self.y = y self.z = z def cross(self, v): return Vector3D(self.y * v.z - self.z * v.y, self.z * v.x - self.x * v.z, self.x * v.y - self.y * v.x) def dot(self, v): return self.x * v.x + self.y * v.y + self.z * v.z def length2(self): return self.dot(self) def normalize(self): l = math.sqrt(self.length2()) self.x /= l self.y /= l self.z /= l def __eq__(self, v): return self.x == v.x and\ self.y == v.y and\ self.z == v.z def __neg__(self): return Vector3D(-self.x, -self.y, -self.z) def __add__(self, v): return Vector3D(self.x + v.x, self.y + v.y, self.z + v.z) def __sub__(self, v): return Vector3D(self.x - v.x, self.y - v.y, self.z - v.z) def __mul__(self, t): return Vector3D(t * self.x, t * self.y, t * self.z) class Ray: def __init__(self, src, dir): self.src = src self.dir = dir class Matrix3x3: def __init__(self, v1, v2, v3): """v1, v2, v3 should be Vector3Ds which make up rows of matrix. """ self.v1 = v1 self.v2 = v2 self.v3 = v3 def __mul__(self, v): return Vector3D(self.v1.dot(v), self.v2.dot(v), self.v3.dot(v)) def same_line_side(p, q, a, b): """Determine if two points on same side of line. p, q, a, b assumed coplanar return True iff p, q on same side of line through a, b """ n1 = (a - p).cross(b - p) n2 = (a - q).cross(b - q) return n1.dot(n2) >= 0.0 def strictly_inside_line_segment(p, a, b): """Determine if point lies on line segment (and is not end point). a, b assumed distinct; integer arithmetic assumed for full accuracy Return True iff p on line through a, b and between a, b and not a or b """ return ((p - a).cross(b - a).length2() == 0 and\ (p - a).dot(b - a) > 0 and\ (p - b).dot(a - b) > 0) class Triangle: def __init__(self, v1, v2, v3): self.v1 = v1 self.v2 = v2 self.v3 = v3 def normal(self): n = (self.v1 - self.v2).cross(self.v1 - self.v3) n.normalize() return n def cog(self): return Vector3D((self.v1.x + self.v2.x + self.v3.x) / 3.0, (self.v1.y + self.v2.y + self.v3.y) / 3.0, (self.v1.z + self.v2.z + self.v3.z) / 3.0) def is_inside(self, p): if not same_line_side(p, self.v1, self.v2, self.v3) or\ not same_line_side(p, self.v2, self.v3, self.v1) or\ not same_line_side(p, self.v3, self.v1, self.v2): return False return True def get_intersection(self, ray): n = self.normal() t = ray.dir.dot(n) if t == 0.0: return None t = (self.v1 - ray.src).dot(n) / t # v2 or v3 would also work as plane eqn is v.n = const I = ray.src + ray.dir * t if not self.is_inside(I) or t < 0.0: return None return I def area(self): return 0.5 * math.sqrt((self.v2 - self.v1).cross(self.v3 - self.v1).length2())