# -*- indent-tabs-mode: t -*-

# Soya 3D
# Copyright (C) 2004 Jean-Baptiste LAMY -- jiba@tuxfamily.org
#
# This program is free software; you can redistribute it and/or modify
# it under the terms of the GNU General Public License as published by
# the Free Software Foundation; either version 2 of the License, or
# (at your option) any later version.
#
# This program is distributed in the hope that it will be useful,
# but WITHOUT ANY WARRANTY; without even the implied warranty of
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
# GNU General Public License for more details.
#
# You should have received a copy of the GNU General Public License
# along with this program; if not, write to the Free Software
# Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA

# A collider for a Land

cdef class _Land(GeomObject):
		"""Landscape/terrain collider for Soya. This is based on Benoit Chaperot's
		contributed terrain collider for ODE, with some changes to take advantage
		of precomputed normals in Soya's landscape engine.

		How it works: 

		We loop through each "cell," or square of vertices in the heightfield,
		that is underneath the axis-aligned bounding box (AABB) of the geom we're
		testing for collisions with. For rendering, each cell is divided into
		two triangles. We use ray collision to check each edge of one of the 
		triangles, because the other triangle shares edges with adjacent cells
		and we don't want to check edges twice. We make a ray for each direction
		along the edge to get two collision points so we can take the midpoint
		of the two collision points if there is a collision. Right now the normal
		is halfway between the normals computed by Soya for the vertices making
		the edge being tested. This could be weighted, but we don't bother right
		now and it seems to work fine.

		Next, we test the plane of each triangle. We use ODE's own plane collision
		routines to give us a set of collision points on the plane. We then test
		to see if any of the collision points are within the triangle we're testing
		by first seeing if the point is within the cell (a simple range check),
		then testing if it's in the correct isoceles right triangle, which means
		just looking at the sum of the x and z deltas from the upper left vertex
		of the cell. If it's less than the length of one leg, it's in the upper
		triangle. If it's greater, it's in the lower. We keep the normal from any
		plane contact points we keep.

		Ray collision is not currently implemented, but could easily be handled
		using Soya's own raypicking routines. Likewise plane collision, which 
		might be able to use the view frustum culling routines. I don't currently
		plan to implement these because I have ambitions about doing all collision
		detection within Soya rather than using ODE's collision detection.
		"""

		cdef _soya._Land _land

		cdef void _get_aabb(self, dReal aabb[6]):
				"""Get the axis-aligned bounding box.
				This is slow and it's fortunate that lands aren't currently considered
				placeable so it only gets called once. If we want lands to be
				placeable, we need to cache the information and invalidate it
				if the land is moved. This may require going back to subclassing
				_soya._Land, or coming up with a hook mechanism for getting notified
				when a coordsyst is updated."""

				cdef _soya._Land land
				cdef float min_x, max_x, min_y, max_y, min_z, max_z
				cdef int i
				cdef GLfloat m[19], P[3]

				land = self._land

				# Calculate the true AABB
				# Make sure it's not getting called a lot
				print "Calculating AABB for land (slow)"

				# Get the matrix for transforming points to world coordinates
				_soya.multiply_matrix(m, land._root_matrix(), self.world._inverted_root_matrix())

				# Set up the minima and maxima
				_soya.point_by_matrix_copy(P, land._vertices[0].coord, m)
				min_x = P[0]
				max_x = P[0]
				min_y = P[1]
				max_y = P[1]
				min_z = P[2]
				max_z = P[2]

				for i from 1 <= i < land._nb_vertex_width * land._nb_vertex_depth:
						# Transform each vertex to world coordinates
						_soya.point_by_matrix_copy(P, land._vertices[i].coord, m)
						if P[0] < min_x:
								min_x = P[0]

						if P[0] > max_x:
								max_x = P[0]

						if P[1] < min_y:
								min_y = P[1]
						
						if P[1] > max_y:
								max_y = P[1]

						if P[2] < min_z:
								min_z = P[2]

						if P[2] > max_z:
								max_z = P[2]
				
				aabb[0] = min_x
				aabb[1] = max_x
				aabb[2] = min_y
				aabb[3] = max_y
				aabb[4] = min_z
				aabb[5] = max_z

		cdef int _collide_cell(self, int x, int z, dGeomID o1, 
													 dGeomID o2, int max_contacts, int flags, 
													 dContactGeom *contact, int skip):
				"""Test for any collisions within a single cell of the heightfield
				grid."""

				cdef _soya.LandVertex *vA, *vB, *vC, *vD
				cdef GLfloat A[3], B[3], C[3], D[3], NB[3], NC[3], ND[3]
				cdef GLfloat AB[3], AC[3], BC[3], BD[3], CD[3]
				cdef GLfloat plane[4]
				cdef int num_contacts, numPlaneContacts
				cdef dContactGeom ContactA[3], ContactB[3], *pContact
				cdef dContactGeom planeContact[10]
				cdef GLfloat m[19] # Matrix to convert to world coordinates
				cdef _soya._Land land

				num_contacts = 0

				land = self._land

				_soya.multiply_matrix(m, land._root_matrix(), self.world._inverted_root_matrix())
					
				# Get the four vertices for this cell
				# Pointer arithmetic is evil.
				vA = land._vertices + (x + z * land._nb_vertex_width)
				vB = vA + 1
				vC = vA + land._nb_vertex_width
				vD = vB + land._nb_vertex_width

				# Hmmm, already have normals calculated for both
				# vertices *and* triangles. This could be good.

				# Transform each vertex to the world's coordsys
				_soya.point_by_matrix_copy(A, vA.coord, m)
				_soya.point_by_matrix_copy(B, vB.coord, m)
				_soya.point_by_matrix_copy(C, vC.coord, m)
				_soya.point_by_matrix_copy(D, vD.coord, m)

				# Transform the normals to the world's coordsys
				_soya.point_by_matrix_copy(NB, vB.normal, m)
				_soya.point_by_matrix_copy(NC, vC.normal, m)
				_soya.point_by_matrix_copy(ND, vD.normal, m)
				
				# Renormalize the normals. This is only needed if we allow the land
				# to be scaled by Land.scale(fx, fy, fz)
				_soya.vector_normalize(NB)
				_soya.vector_normalize(NC)
				_soya.vector_normalize(ND)


				# Make all the vectors we need
				_soya.vector_from_points(AB, A, B)
				_soya.vector_normalize(AB)
				_soya.vector_from_points(AC, A, C)
				_soya.vector_normalize(AC)
				_soya.vector_from_points(BC, B, C)
				_soya.vector_normalize(BC)
				_soya.vector_from_points(BD, B, D)
				_soya.vector_normalize(BD)
				_soya.vector_from_points(CD, C, D)
				_soya.vector_normalize(CD)

				# Ray collision to test edges of one triangle
				# Don't need to test the other because adjacent cells will
				num_contacts = num_contacts + collide_edge(B, C, BC, NB, 
						NC, o1, o2, max_contacts - num_contacts, flags, 
						<dContactGeom*>(<char*>contact + (num_contacts * skip)))

				if num_contacts == max_contacts:
						return num_contacts

				num_contacts = num_contacts + collide_edge(B, D, BD, NB,
						ND, o1, o2, max_contacts - num_contacts, flags, 
						<dContactGeom*>(<char*>contact + (num_contacts * skip)))

				if num_contacts == max_contacts:
						return num_contacts

				num_contacts = num_contacts + collide_edge(C, D, CD, NC,
						ND, o1, o2, max_contacts - num_contacts, flags, 
						<dContactGeom*>(<char*>contact + (num_contacts * skip)))

				if num_contacts == max_contacts:
						return num_contacts

				# Now do planes for each of the two triangle planes
				# XXX If the plane test fails, we could skip the ray tests.
				_soya.vector_cross_product(plane, AC, AB)
				#_soya.vector_normalize(plane)
				plane[3] = plane[0] * A[0] + plane[1] * A[1] + plane[2] * A[2]
				dGeomPlaneSetParams(_land_plane, plane[0], plane[1], plane[2], plane[3])

				numPlaneContacts = dCollide(o2, _land_plane, flags, planeContact, sizeof(dContactGeom))

				#print "a", numPlaneContacts

				for i from 0 <= i < numPlaneContacts:
						# Figure out if the point is in the triangle.
						# Only need to test x and z coord because we already know it's
						# in the plane and the plane is not vertical

						# First, check if it's in the cell
						if planeContact[i].pos[0] < A[0] or planeContact[i].pos[0] > D[0]:
								continue

						if planeContact[i].pos[2] < A[2] or planeContact[i].pos[2] > D[2]:
								continue

						# Now, check if it's in the correct triangle in the cell.
						# This is made easier since we only support isoceles right triangles
						if (planeContact[i].pos[0] - A[0] + planeContact[i].pos[2] - A[2]) > (D[0] - A[0]):
								continue
						
						# It's in the triangle, so add it to our list of contacts
						pContact = <dContactGeom*>((<char*>contact) + (num_contacts * skip))
						#print <long>contact, <long>pContact, num_contacts, skip
						pContact.pos[0] = planeContact[i].pos[0]
						pContact.pos[1] = planeContact[i].pos[1]
						pContact.pos[2] = planeContact[i].pos[2]
						pContact.normal[0] = -planeContact[i].normal[0]
						pContact.normal[1] = -planeContact[i].normal[1]
						pContact.normal[2] = -planeContact[i].normal[2]
						pContact.depth = planeContact[i].depth
						pContact.g1 = o1
						pContact.g2 = o2

						##print "a", pContact.pos[0], pContact.pos[1], pContact.pos[2], pContact.normal[0], pContact.normal[1], pContact.normal[2], pContact.depth
						num_contacts = num_contacts + 1

						if num_contacts == max_contacts:
								return num_contacts

				# Try the plane for the other triangle
				_soya.vector_cross_product(plane, BD, CD)
				_soya.vector_normalize(plane)
				plane[3] = plane[0] * D[0] + plane[1] * D[1] + plane[2] * D[2]
				dGeomPlaneSetParams(_land_plane, plane[0], plane[1], plane[2], plane[3])

				numPlaneContacts = dCollide(o2, _land_plane, flags, planeContact, sizeof(dContactGeom))

				#print "b", numPlaneContacts

				for i from 0 <= i < numPlaneContacts:
						# Figure out if the point is in the triangle.
						# Only need to test x and z coord because we already know it's
						# in the plane and the plane is not vertical

						# First, check if it's in the cell
						if planeContact[i].pos[0] < A[0] or planeContact[i].pos[0] > D[0]:
								continue

						if planeContact[i].pos[2] < A[2] or planeContact[i].pos[2] > D[2]:
								continue

						# Now, check if it's in the correct triangle in the cell.
						# This is made easier since we only support isoceles right triangles
						if (planeContact[i].pos[0] - A[0] + planeContact[i].pos[2] - A[2]) < (D[0] - A[0]):
								continue
						
						# It's in the triangle, so add it to our list of contacts
						pContact = <dContactGeom*>((<char*>contact) + (num_contacts * skip))
						#print <long>contact, <long>pContact, num_contacts, skip
						pContact.pos[0] = planeContact[i].pos[0]
						pContact.pos[1] = planeContact[i].pos[1]
						pContact.pos[2] = planeContact[i].pos[2]
						pContact.normal[0] = -planeContact[i].normal[0]
						pContact.normal[1] = -planeContact[i].normal[1]
						pContact.normal[2] = -planeContact[i].normal[2]
						pContact.depth = planeContact[i].depth
						pContact.g1 = o1
						pContact.g2 = o2
						
						#print "b", pContact.pos[0], pContact.pos[1], pContact.pos[2], pContact.normal[0], pContact.normal[1], pContact.normal[2], pContact.depth

						num_contacts = num_contacts + 1

						if num_contacts == max_contacts:
								return num_contacts

				return num_contacts


		cdef int _collide(self, dGeomID o1, dGeomID o2, int flags,
											dContactGeom *contact, int skip):
				cdef int num_contacts, max_contacts, x, z, i, j, k
				cdef GLfloat min_x, max_x, min_z, max_z
				cdef dReal aabb[6], depth
				cdef dContactGeom *pContact
				cdef dVector3 lengths
				#cdef dReal *R
				cdef _soya.LandVertex *vA, *vB, *vC, *vD
				cdef GLfloat m[19] # Matrix to convert to land's coordinate system
				cdef GLfloat BC, BD, CD
				cdef GLfloat plane[4]
				cdef _soya._Land land
				cdef GLfloat P[3] # Point for converting AABB corners

				land = self._land

				# Get the maximum number of contacts from the flags
				max_contacts = (flags & 0xffff) or 1

				# Only 10 contacts allowed for called collision functions
				flags = (flags & 0xffff0000) | 10

				#R = dGeomGetRotation(o2)

				# Get the AAAB of the geom
				dGeomGetAABB(o2, aabb)

				# Convert the AABB to the land's coordinate system. XXX this is slower
				# than if we could assume the lands' axes were aligned with the
				# world's axes, but we can't.

				# Make the matrix
				_soya.multiply_matrix(m, self.world._root_matrix(), land._inverted_root_matrix())

				P[0] = aabb[0]
				P[1] = aabb[2]
				P[2] = aabb[4]
				
				_soya.point_by_matrix(P, m)

				# Only care about x and z axes
				min_x = P[0]
				max_x = P[0]
				min_z = P[2]
				max_z = P[2]

				# Do the other seven corners
				for i, j, k in ((1, 2, 4), (0, 3, 4), (1, 3, 4), (0, 2, 5), (1, 2, 5), (0, 3, 5), (1, 3, 5)):
						P[0] = aabb[i]
						P[1] = aabb[j]
						P[2] = aabb[k]

						_soya.point_by_matrix(P, m)
		
						if P[0] < min_x:
								min_x = P[0]
						if P[0] > max_x:
								max_x = P[0]
						if P[2] < min_z:
								min_z = P[2]
						if P[2] > max_z:
								max_z = P[2]

				num_contacts = 0
				# Test all cells that are under the AABB
				for z from <int>floor(max(0, min_z)) <= z < <int>ceil(min(land._nb_vertex_depth, max_z)):
						for x from <int>floor(max(0, min_x)) <= x < <int>ceil(min(land._nb_vertex_width, max_x)):
								num_contacts = num_contacts + self._collide_cell(x, z, o1, 
										o2, max_contacts - num_contacts, flags, 
										<dContactGeom*>((<char*>contact) + (num_contacts*skip)), 
										skip)

				return num_contacts

		def __new__(self, _soya._Land land, SpaceBase space=None, *args, **kw):
				self._land = land 
				self.gid = dCreateGeom(dLandClass)

				if space is not None:
						dSpaceAdd(space.sid, self.gid)

		def __init__(self, _soya._Land land, SpaceBase space=None):
				GeomObject.__init__(self, land._parent, space)

		def __dealloc__(self):
				print "dealloc", self

		def placeable(self):
				return False

		property body:
				def __get__(self):
						"""Land can't have a body, so return environment"""
						return environment


cdef dGeomID _land_plane # Reusable plane geom
cdef dGeomID _land_ray   # Reusable ray geom

_land_plane = dCreatePlane(NULL, 0.0, 1.0, 0.0, 0.0)
_land_ray = dCreateRay(NULL, 1.0)

cdef void LandGetAABB(dGeomID geom, dReal aabb[6]):
		cdef _Land land
		land = <_Land>dGeomGetData(geom)
		land._get_aabb(aabb)

cdef int LandCollide(dGeomID o1, dGeomID o2, int flags,
										 dContactGeom *contact, int skip):
		cdef _Land land
		land = <_Land>dGeomGetData(o1)
		return land._collide(o1, o2, flags, contact, skip)


cdef dColliderFn * LandGetColliderFn(int gclass):

		if gclass in (dSphereClass, dBoxClass, dCCylinderClass):
				return LandCollide

		return NULL


cdef int LandAABBTest(dGeomID o1, dGeomID o2, dReal aabb2[6]):
		pass


cdef dGeomClass dLandGeomClass

dLandGeomClass.bytes = 0
dLandGeomClass.collider = LandGetColliderFn
dLandGeomClass.aabb = LandGetAABB
dLandGeomClass.aabb_test = NULL # Need to write this function
dLandGeomClass.dtor = NULL
		
cdef int dLandClass

dLandClass = dCreateGeomClass(&dLandGeomClass)