#[MIT license:]
#
# Copyright (c) 2004  Dave Pape
#
# Permission is hereby granted, free of charge, to any person obtaining a copy
# of this 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 this 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.


from operator import *
from math import *
from dmsgl import *

class Sphere:
	def __init__(self, center=[0,0,0], radius=1):
		self.setCenter(center)
		self.radius = radius
	def setCenter(self, center):
		if isinstance(center, Vector): self.center = center[:]
		elif isSequenceType(center): self.center = Vector(center)
		else: raise TypeError, 'center must be a Vector or list'
	def draw(self):
		glPushMatrix()
		glTranslatef(self.center[0], self.center[1], self.center[2])
		glutSolidSphere(self.radius, 32, 16)
		glPopMatrix()
	def drawWireframe(self):
		glPushMatrix()
		glTranslatef(self.center[0], self.center[1], self.center[2])
		glutWireSphere(self.radius, 20, 10)
		glPopMatrix()

class AABB:
	def __init__(self, min=[-1,-1,-1], max=[1,1,1]):
		self.min = min[:]
		self.max = max[:]
	def draw(self):
		glBegin(GL_QUADS)
		glNormal3f(0, 0, -1)
		glVertex3f(self.min[0], self.min[1], self.min[2])
		glVertex3f(self.min[0], self.max[1], self.min[2])
		glVertex3f(self.max[0], self.max[1], self.min[2])
		glVertex3f(self.max[0], self.min[1], self.min[2])
		glNormal3f(0, 0, 1)
		glVertex3f(self.min[0], self.min[1], self.max[2])
		glVertex3f(self.max[0], self.min[1], self.max[2])
		glVertex3f(self.max[0], self.max[1], self.max[2])
		glVertex3f(self.min[0], self.max[1], self.max[2])
		glNormal3f(-1, 0, 0)
		glVertex3f(self.min[0], self.min[1], self.min[2])
		glVertex3f(self.min[0], self.min[1], self.max[2])
		glVertex3f(self.min[0], self.max[1], self.max[2])
		glVertex3f(self.min[0], self.max[1], self.min[2])
		glNormal3f(1, 0, 0)
		glVertex3f(self.max[0], self.min[1], self.min[2])
		glVertex3f(self.max[0], self.max[1], self.min[2])
		glVertex3f(self.max[0], self.max[1], self.max[2])
		glVertex3f(self.max[0], self.min[1], self.max[2])
		glNormal3f(0, -1, 0)
		glVertex3f(self.min[0], self.min[1], self.min[2])
		glVertex3f(self.max[0], self.min[1], self.min[2])
		glVertex3f(self.max[0], self.min[1], self.max[2])
		glVertex3f(self.min[0], self.min[1], self.max[2])
		glNormal3f(0, 1, 0)
		glVertex3f(self.min[0], self.max[1], self.min[2])
		glVertex3f(self.min[0], self.max[1], self.max[2])
		glVertex3f(self.max[0], self.max[1], self.max[2])
		glVertex3f(self.max[0], self.max[1], self.min[2])
		glEnd()
	def drawWireframe(self):
		glBegin(GL_LINE_STRIP)
		glVertex3f(self.min[0], self.min[1], self.min[2])
		glVertex3f(self.max[0], self.min[1], self.min[2])
		glVertex3f(self.max[0], self.max[1], self.min[2])
		glVertex3f(self.min[0], self.max[1], self.min[2])
		glVertex3f(self.min[0], self.min[1], self.min[2])
		glVertex3f(self.min[0], self.min[1], self.max[2])
		glVertex3f(self.max[0], self.min[1], self.max[2])
		glVertex3f(self.max[0], self.max[1], self.max[2])
		glVertex3f(self.min[0], self.max[1], self.max[2])
		glVertex3f(self.min[0], self.min[1], self.max[2])
		glEnd()
		glBegin(GL_LINES)
		glVertex3f(self.min[0], self.max[1], self.min[2])
		glVertex3f(self.min[0], self.max[1], self.max[2])
		glVertex3f(self.max[0], self.max[1], self.min[2])
		glVertex3f(self.max[0], self.max[1], self.max[2])
		glVertex3f(self.max[0], self.min[1], self.min[2])
		glVertex3f(self.max[0], self.min[1], self.max[2])
		glEnd()

class OBB:
	def __init__(self, center=[0,0,0], u=[1,0,0], v=[0,1,0], w=[0,0,1]):
		self.setCenter(center)
		self.setAxes(u, v, w)
	def setCenter(self, center):
		if isinstance(center, Vector): self.center = center[:]
		elif isSequenceType(center): self.center = Vector(center)
		else: raise TypeError, 'center must be a Vector or list'
	def setAxes(self, u, v, w):
		if isinstance(u, Vector): self.u = u[:]
		elif isSequenceType(u): self.u = Vector(u)
		else: raise TypeError, 'u must be a Vector or list'
		if isinstance(v, Vector): self.v = v[:]
		elif isSequenceType(v): self.v = Vector(v)
		else: raise TypeError, 'v must be a Vector or list'
		if isinstance(w, Vector): self.w = w[:]
		elif isSequenceType(w): self.w = Vector(w)
		else: raise TypeError, 'w must be a Vector or list'
	def draw(self):
		u = self.u[:]
		u.normalize()
		v = self.v[:]
		v.normalize()
		w = self.w[:]
		w.normalize()
		glBegin(GL_QUADS)
		glNormal3fv(-w)
		glVertex3fv(self.center - self.u - self.v - self.w)
		glVertex3fv(self.center - self.u + self.v - self.w)
		glVertex3fv(self.center + self.u + self.v - self.w)
		glVertex3fv(self.center + self.u - self.v - self.w)
		glNormal3fv(w)
		glVertex3fv(self.center - self.u - self.v + self.w)
		glVertex3fv(self.center + self.u - self.v + self.w)
		glVertex3fv(self.center + self.u + self.v + self.w)
		glVertex3fv(self.center - self.u + self.v + self.w)
		glNormal3fv(-u)
		glVertex3fv(self.center - self.u - self.v - self.w)
		glVertex3fv(self.center - self.u - self.v + self.w)
		glVertex3fv(self.center - self.u + self.v + self.w)
		glVertex3fv(self.center - self.u + self.v - self.w)
		glNormal3fv(u)
		glVertex3fv(self.center + self.u - self.v - self.w)
		glVertex3fv(self.center + self.u + self.v - self.w)
		glVertex3fv(self.center + self.u + self.v + self.w)
		glVertex3fv(self.center + self.u - self.v + self.w)
		glNormal3fv(-v)
		glVertex3fv(self.center + self.u - self.v - self.w)
		glVertex3fv(self.center + self.u - self.v + self.w)
		glVertex3fv(self.center - self.u - self.v + self.w)
		glVertex3fv(self.center - self.u - self.v - self.w)
		glNormal3fv(v)
		glVertex3fv(self.center - self.u + self.v - self.w)
		glVertex3fv(self.center - self.u + self.v + self.w)
		glVertex3fv(self.center + self.u + self.v + self.w)
		glVertex3fv(self.center + self.u + self.v - self.w)
		glEnd()
	def drawWireframe(self):
		glBegin(GL_LINE_STRIP)
		glVertex3fv(self.center - self.u - self.v - self.w)
		glVertex3fv(self.center + self.u - self.v - self.w)
		glVertex3fv(self.center + self.u + self.v - self.w)
		glVertex3fv(self.center - self.u + self.v - self.w)
		glVertex3fv(self.center - self.u - self.v - self.w)
		glVertex3fv(self.center - self.u - self.v + self.w)
		glVertex3fv(self.center + self.u - self.v + self.w)
		glVertex3fv(self.center + self.u + self.v + self.w)
		glVertex3fv(self.center - self.u + self.v + self.w)
		glVertex3fv(self.center - self.u - self.v + self.w)
		glEnd()
		glBegin(GL_LINES)
		glVertex3fv(self.center - self.u + self.v - self.w)
		glVertex3fv(self.center - self.u + self.v + self.w)
		glVertex3fv(self.center + self.u + self.v - self.w)
		glVertex3fv(self.center + self.u + self.v + self.w)
		glVertex3fv(self.center + self.u - self.v - self.w)
		glVertex3fv(self.center + self.u - self.v + self.w)
		glEnd()

class Line:
	def __init__(self, origin=[0,0,0], direction=[0,0,1]):
		self.setOrigin(origin)
		self.setDirection(direction)
	def setOrigin(self, origin):
		if isinstance(origin, Vector): self.origin = origin[:]
		elif isSequenceType(origin): self.origin = Vector(origin)
		else: raise TypeError, 'origin must be a Vector or list'
	def setDirection(self, direction):
		if isinstance(direction, Vector): self.direction = direction[:]
		elif isSequenceType(direction): self.direction = Vector(direction)
		else: raise TypeError, 'direction must be a Vector or list'
		self.direction.normalize()
	def draw(self, t0=0, t1=1):
		self.drawWireframe(t0, t1)
	def drawWireframe(self, t0=0, t1=1):
		glBegin(GL_LINES)
		glVertex3fv(self.origin + self.direction * t0)
		glVertex3fv(self.origin + self.direction * t1)
		glEnd()

class Segment(Line):
	def __init__(self, origin=[0,0,0], point2=None, direction=[0,0,1], length=1):
		self.setOrigin(origin)
		if point2:
			d = -(self.origin - point2)
			self.length = d.length()
			self.setDirection(d)
		else:
			self.setDirection(direction)
			self.length = length
	def draw(self):
		self.drawWireframe()
	def drawWireframe(self):
		glBegin(GL_LINES)
		glVertex3fv(self.origin)
		glVertex3fv(self.origin + self.direction * self.length)
		glEnd()

class Plane:
	def __init__(self, coeffs=[0, 0, 1, 0]):
		self.coeffs = coeffs[:]
	def value(self, p):
		return self.coeffs[0]*p[0] + self.coeffs[1]*p[1] + self.coeffs[2]*p[2] + self.coeffs[3]
	def draw(self, width=1, height=1):
		norm = Vector([self.coeffs[0], self.coeffs[1], self.coeffs[2]])
		center = norm * (-self.coeffs[3] / norm.lengthSquared())
		norm.normalize()
		up = Vector([0, 1, 0])
		if abs(norm.dot(up)) > 0.999:
			up = Vector([0, 0, 1])
		up -= norm * norm.dot(up)
		up.normalize()
		right = up.cross(norm)
		glBegin(GL_QUADS)
		glNormal3fv(norm)
		glVertex3fv(center - right*width/2.0 - up*height/2.0)
		glVertex3fv(center + right*width/2.0 - up*height/2.0)
		glVertex3fv(center + right*width/2.0 + up*height/2.0)
		glVertex3fv(center - right*width/2.0 + up*height/2.0)
		glEnd()
	def drawWireframe(self, width=1, height=1):
		norm = Vector([self.coeffs[0], self.coeffs[1], self.coeffs[2]])
		center = norm * (-self.coeffs[3] / norm.lengthSquared())
		norm.normalize()
		up = Vector([0, 1, 0])
		if abs(norm.dot(up)) > 0.999:
			up = Vector([0, 0, 1])
		up -= norm * norm.dot(up)
		up.normalize()
		right = up.cross(norm)
		glBegin(GL_LINE_LOOP)
		glVertex3fv(center - right*width/2.0 - up*height/2.0)
		glVertex3fv(center + right*width/2.0 - up*height/2.0)
		glVertex3fv(center + right*width/2.0 + up*height/2.0)
		glVertex3fv(center - right*width/2.0 + up*height/2.0)
		glEnd()


def PointSphereDistance(p, sphere):
	d = sphere.center.distance(p) - sphere.radius
	if d < 0: return 0
	else: return d

def PointLineDistance(p, line):
	if isinstance(p, Vector): diff = p - line.origin
	else: diff = Vector(p) - line.origin
	proj = diff.dot(line.direction)
	return sqrt(diff.dot(diff) - proj*proj)

def PointSegmentDistance(p, segment):
	if isinstance(p, Vector): diff = p - segment.origin
	else: diff = Vector(p) - segment.origin
	proj = diff.dot(segment.direction)
	if proj < 0:
		return segment.origin.distance(p)
	elif proj > segment.length:
		p2 = segment.origin + segment.direction * segment.length
		return p2.distance(p)
	else:
		return sqrt(diff.dot(diff) - proj*proj)

def PointPlaneDistance(p, plane):
	d = plane.value(p)
	magSq = plane.coeffs[0]*plane.coeffs[0] + plane.coeffs[1]*plane.coeffs[1] + plane.coeffs[2]*plane.coeffs[2]
	if abs(maqSq-1) > 0.000001:
		d /= sqrt(magSq)
	return d

def PointAABBDistance(p, box):
	if p[0] < box.min[0]: dx = box.min[0] - p[0]
	elif p[0] > box.max[0]: dx = p[0] - box.min[0]
	else: dx = 0
	if p[1] < box.min[1]: dy = box.min[1] - p[1]
	elif p[1] > box.max[1]: dy = p[1] - box.min[1]
	else: dy = 0
	if p[2] < box.min[2]: dz = box.min[2] - p[2]
	elif p[2] > box.max[2]: dz = p[2] - box.min[2]
	else: dz = 0
	return sqrt(dx*dx + dy*dy + dz*dz)

def PointOBBDistance(p, box):
	if isinstance(p, Vector): pRel = p - box.center
	else: pRel = Vector(p) - box.center
	ulen = box.u.length()
	x = pRel.dot(box.u) / ulen
	if x < -ulen: dx = -ulen - x
	elif x > ulen: dx = x - ulen
	else: dx = 0
	vlen = box.v.length()
	y = pRel.dot(box.v) / vlen
	if y < -vlen: dy = -vlen - y
	elif y > vlen: dy = y - vlen
	else: dy = 0
	wlen = box.w.length()
	z = pRel.dot(box.w) / wlen
	if z < -wlen: dz = -wlen - z
	elif z > wlen: dz = z - wlen
	else: dz = 0
	return sqrt(dx*dx + dy*dy + dz*dz)


def SphereSphereDistance(s1, s2):
	d = PointSphereDistance(s1.center, s2) - s1.radius
	if d < 0: return 0
	else: return d

def SphereLineDistance(s, line):
	d = PointLineDistance(s.center, line) - s.radius
	if d < 0: return 0
	else: return d

def SphereSegmentDistance(s, segment):
	d = PointSegmentDistance(s.center, segment) - s.radius
	if d < 0: return 0
	else: return d

def SpherePlaneDistance(s, plane):
	d = PointPlaneDistance(s.center, plane) - s.radius
	if d < 0: return 0
	else: return d

def SphereAABBDistance(s, box):
	d = PointAABBDistance(s.center, box) - s.radius
	if d < 0: return 0
	else: return d

def SphereOBBDistance(s, box):
	d = PointOBBDistance(s.center, box) - s.radius
	if d < 0: return 0
	else: return d


def SphereContainsPoint(sphere, p):
	return (sphere.center.distanceSquared(p) <= sphere.radius*sphere.radius)

def AABBContainsPoint(box, p):
	return (p[0] >= box.min[0]) and (p[0] <= box.max[0]) \
		   and (p[1] >= box.min[1]) and (p[1] <= box.max[1]) \
		   and (p[2] >= box.min[2]) and (p[2] <= box.max[2])

def OBBContainsPoint(box, p):
	if isinstance(p, Vector): pRel = p - box.center
	else: pRel = Vector(p) - box.center
	return (abs(pRel.dot(box.u)) <= box.u.lengthSquared()) \
		   and (abs(pRel.dot(box.v)) <= box.v.lengthSquared()) \
		   and (abs(pRel.dot(box.w)) <= box.w.lengthSquared())


def SphereCollidesSphere(s1, s2):
	d = s1.center.distanceSquared(s2.center)
	rsum = s1.radius + s2.radius
	return (d <= rsum*rsum)

def SphereCollidesAABB(sphere, box):
	return (SphereAABBDistance(sphere, box) <= 0)

def SphereCollidesOBB(sphere, box):
	return (SphereOBBDistance(sphere, box) <= 0)

def AABBCollidesAABB(box1, box2):
	xmin = max(box1.min[0], box2.min[0])
	xmax = min(box1.max[0], box2.max[0])
	ymin = max(box1.min[1], box2.min[1])
	ymax = min(box1.max[1], box2.max[1])
	zmin = max(box1.min[2], box2.min[2])
	zmax = min(box1.max[2], box2.max[2])
	return (xmin <= xmax) and (ymin <= ymax) and (zmin <= zmax)


def SphereContainsSphere(s1,s2):
	rdiff = s1.radius - s2.radius
	if rdiff < 0: return False
	d = s1.center.distanceSquared(s2.center)
	return (d <= rdiff*rdiff)

def AABBContainsAABB(box1, box2):
	return (box1.min[0] <= box2.min[0]) \
		   and (box1.max[0] >= box2.max[0]) \
		   and (box1.min[1] <= box2.min[1]) \
		   and (box1.max[1] >= box2.max[1]) \
		   and (box1.min[2] <= box2.min[2]) \
		   and (box1.max[2] >= box2.max[2])


def LineIntersectsSphere(line, sphere):
	diff = line.origin - sphere.center
	b = line.direction.dot(diff)
	c = diff.dot(diff) - sphere.radius * sphere.radius
	return (b*b >= c)

def LineIntersectsPlane(line, plane):
	denom = line.direction[0]*plane.coeffs[0] + line.direction[1]*plane.coeffs[1] + line.direction[2]*plane.coeffs[2]
	return (denom != 0)


def LineSphereIntersection(line, sphere):
	diff = line.origin - sphere.center
	b = line.direction.dot(diff)
	c = diff.dot(diff) - sphere.radius * sphere.radius
	if b*b < c: return None
	if b*b == c:
		return line.origin + line.direction * (-b)
	else:
		q = sqrt(b*b - c)
		t1 = -q - b
		t2 = q - b
		return [line.origin + line.direction * t1, line.origin + line.direction * t2]

def LinePlaneIntersection(line, plane):
	denom = line.direction[0]*plane.coeffs[0] + line.direction[1]*plane.coeffs[1] + line.direction[2]*plane.coeffs[2]
	if denom == 0: return None
	t = -plane.value(line.origin) / denom
	return line.origin + line.direction * t


def SegmentSphereIntersection(segment, sphere):
	diff = segment.origin - sphere.center
	b = -segment.direction.dot(diff)
	c = diff.dot(diff) - sphere.radius * sphere.radius
	if b*b < c: return None
	if b*b == c:
		if (b < 0) or (b > segment.length): return None
		return segment.origin + segment.direction * b
	else:
		q = sqrt(b*b - c)
		t1 = b - q
		t2 = b + q
		if ((t1 < 0) or (t1 > segment.length)) and ((t2 < 0) or (t2 > segment.length)):
			return None
		elif (t1 < 0) or (t1 > segment.length):
			return segment.origin + segment.direction * t2
		elif (t2 < 0) or (t2 > segment.length):
			return segment.origin + segment.direction * t1
		return [segment.origin + segment.direction * t1, segment.origin + segment.direction * t2]

def SegmentPlaneIntersection(segment, plane):
	denom = segment.direction[0]*plane.coeffs[0] + segment.direction[1]*plane.coeffs[1] + segment.direction[2]*plane.coeffs[2]
	if denom == 0: return None
	t = -plane.value(segment.origin) / denom
	if (t < 0) or (t > segment.length): return None
	return segment.origin + segment.direction * t


def WFObjectBoundingSphere(obj):
	if len(obj.v) == 0:
		return Sphere([0,0,0],0)
	sum = Vector([0,0,0])
	for v in obj.v:
		sum += v
	center = sum / len(obj.v)
	rSquared = 0
	for v in obj.v:
		dSquared = center.distanceSquared(v)
		if dSquared > rSquared:
			rSquared = dSquared
	return Sphere(center,sqrt(rSquared))

def WFObjectBoundingAABB(obj):
	if len(obj.v) == 0:
		return AABB([0,0,0], [0,0,0])
	xmin = obj.v[0][0]
	xmax = xmin
	ymin = obj.v[0][1]
	ymax = ymin
	zmin = obj.v[0][2]
	zmax = zmin
	for v in obj.v:
		if v[0] < xmin: xmin = v[0]
		elif v[0] > xmax: xmax = v[0]
		if v[1] < ymin: ymin = v[1]
		elif v[1] > ymax: ymax = v[1]
		if v[2] < zmin: zmin = v[2]
		elif v[2] > zmax: zmax = v[2]
	return AABB([xmin,ymin,zmin], [xmax,ymax,zmax])

def WFObjectBoundingOBB(obj):
	aaBox = WFObjectBoundingAABB(obj)
	center = (Vector(aaBox.max) + aaBox.min) / 2.0
	x = aaBox.max[0] - center[0]
	y = aaBox.max[1] - center[1]
	z = aaBox.max[2] - center[2]
	return OBB(center, [x,0,0], [0,y,0], [0,0,z])