#!/usr/bin/env python
#
# module: rotatable rectangle
#
# Copyright (C) 2002  Maciej Kalisiak

import sys
import getopt
from os.path import basename
import math
from math import cos, sin
from vector import *


def obb_collision_check(r1, r2):
    '''
    Quick Overlap Test for OBBs
    based on
    'OBBTree: A Hierarchical Structure for Rapid Interference Detection'
    by Gottschalk et al.
    '''
    # compute the rectangle centers
    c1 = vec_add(r1.origin, vec_mult(r1.axis1, 0.5), vec_mult(r1.axis2, 0.5))
    c2 = vec_add(r2.origin, vec_mult(r2.axis1, 0.5), vec_mult(r2.axis2, 0.5))

    # compute normalized axes
    A1 = vec_norm(r1.axis1)
    A2 = vec_norm(r1.axis2)
    B1 = vec_norm(r2.axis1)
    B2 = vec_norm(r2.axis2)

    # compute difference of box centers
    T = vec_sub(c1, c2)

    # try r1.axis1 (A1) as axis of separation
    rA = abs(vec_dot(r1.axis1, A1)) * 0.5
    rB = (abs(vec_dot(r2.axis1, A1)) + abs(vec_dot(r2.axis2, A1))) * 0.5
    if abs(vec_dot(T,A1)) > rA + rB:
	return 0

    # try r1.axis2 (A2) as axis of separation
    rA = abs(vec_dot(r1.axis2, A2)) * 0.5
    rB = (abs(vec_dot(r2.axis1, A2)) + abs(vec_dot(r2.axis2, A2))) * 0.5
    if abs(vec_dot(T,A2)) > rA + rB:
	return 0

    # try r2.axis1 (B1) as axis of separation
    rA = (abs(vec_dot(r1.axis1, B1))+abs(vec_dot(r1.axis2, B1))) * 0.5
    rB = abs(vec_dot(r2.axis1, B1)) * 0.5
    if abs(vec_dot(T,B1)) > rA + rB:
	return 0

    # try r2.axis2 (B2) as axis of separation
    rA = (abs(vec_dot(r1.axis1, B2))+abs(vec_dot(r1.axis2, B2))) * 0.5
    rB = abs(vec_dot(r2.axis2, B2)) * 0.5
    if abs(vec_dot(T,B2)) > rA + rB:
	return 0

    return 1


class OBB:
    '''
    Orientable Bounding Box (rotatable rectangle) class
    '''
    def __init__(self, origin, axis1, axis2):
	self.origin = origin
	self.axis1 = axis1
	self.axis2 = axis2

    def __str__(self):
        return '[%s %s %s]' % (self.origin, self.axis1, self.axis2)
        
    def copy(self):
	return OBB(self.origin, self.axis1, self.axis2)

    def is_equal_to(self, r):
	'''
	Returns 1 if the two rectangles are roughly equivalent.
	'''
	epsilon = 0.000001
	v1 = self.origin + self.axis1 + self.axis2
	v2 = r.origin + r.axis1 + r.axis2
	for i in range(len(v1)):
	    if abs(v1[i]-v2[i]) > epsilon:
		return 0
	return 1

    def get_points(self):
	'''
	Returns the four corner points of the rectangle.
	'''
	pnts = []
	pnts.append(self.origin)
	pnts.append(vec_add(self.origin, self.axis1))
	pnts.append(vec_add(self.origin, self.axis1, self.axis2))
	pnts.append(vec_add(self.origin, self.axis2))
	return pnts

    def moveto(self, x, y):
	'''
	Moves the rectangle to (x,y).
	'''
	self.origin = (x,y)

    def translate(self, dx, dy):
	'''
	Translates the rectangle by vector (dx,dy).
	'''
	self.origin = vec_add(self.origin, (dx,dy))

    def rotate(self, pnt, degs):
	'''
	Rotates the rectangle around the point `pnt' by `degs' degrees.
	'''
	rads = degs / 180.0 * math.pi

	# world coords of the 3 points defining the rectangle
	pnts = [self.origin, vec_add(self.origin, self.axis1),
			     vec_add(self.origin, self.axis2)]

	# translate coords so `pnt' is origin
	diffx = -pnt[0]
	diffy = -pnt[1]
	for i in range(len(pnts)):
	    pnts[i] = (pnts[i][0]+diffx, pnts[i][1]+diffy)

	# perform the rotation
	for i in range(len(pnts)):
	    pnts[i] = (pnts[i][0]*cos(rads) - pnts[i][1]*sin(rads),
		       pnts[i][0]*sin(rads) + pnts[i][1]*cos(rads))

	# translate coords back
	for i in range(len(pnts)):
	    pnts[i] = (pnts[i][0]-diffx, pnts[i][1]-diffy)

	# adjust rectangle representation
	self.origin = pnts[0]
	self.axis1 = vec_sub(pnts[1], pnts[0])
	self.axis2 = vec_sub(pnts[2], pnts[0])


def main():
    print 'Sample code not implemented yet.'

if __name__ == '__main__':
    try:
        opts, args = getopt.getopt(sys.argv[1:], "dp")
    except getopt.GetoptError, exc:
	print>> sys.stderr, exc.msg
        sys.exit(2)

    do_debug = 0
    do_prof = 0

    for o, a in opts:
	if o == '-d':
	    do_debug = 1
	elif o == '-p':
	    do_prof = 1

    cmd_str = 'main()'
    if not do_debug and not do_prof:
        eval(cmd_str)
    elif do_debug:
        import pdb
        pdb.run(cmd_str)
    elif do_prof:
	import profile
	profile.run(cmd_str, basename(sys.argv[0])+'.prof')