import poser
import Numeric
from _tdmt import *

scene = poser.Scene()

act = scene.CurrentActor()

thegeom = act.Geometry()

#FLT_EPSILON = 0.000001#1.1920929000000001e-010 * 0.01
FLT_EPSILON = 1.1920929000000001e-010 * 0.01

def getVerts(geom):
	"""
	Pre-fetch vertex coordinates for faster access.
	Returns a Numeric array of vert coords by vert indices
	Send worldspace=0 keyword when calling function, to use localspace
	"""
	numVerts = geom.NumVertices()

	verts = [[0.0,0.0,0.0] for i in range(numVerts)]
	verts = Numeric.array(verts,Numeric.Float)
	for i in range(numVerts):
		v = geom.WorldVertex(i)
		verts[i] = [v.X(),v.Y(),v.Z()]

	return verts

def getPolys(geom):
	"""
	Convert Poser Sets() into vertex-indexed polygon lists
	"""
	polylist = []
	gset = geom.Sets()
	polys = geom.Polygons()

	for p in polys:
		l = p.NumVertices()
		s = p.Start()
		e = s + l
		vl = Numeric.array([v for v in gset[s:e]],Numeric.Int)
		polylist.append(vl)

	return polylist

def polycenters(polys,verts):
	pcenters = [[0.0,0.0,0.0] for i in range(len(polys))]
	for p_i in range(len(polys)):
		poly = polys[p_i]
		for vi in poly:
			for i in range(3):
				pcenters[p_i][i] += verts[vi][i]
		for i in range(3):
			pcenters[p_i][i] /= len(poly)
	return pcenters

def sub(v1,v2):
	dest = []
	dest.append(v1[0]-v2[0])
	dest.append(v1[1]-v2[1])
	dest.append(v1[2]-v2[2])
	return dest

def plane_dir(direx,v0,v1,v2):
	edge1 = [v1[0]-v0[0], v1[1]-v0[1], v1[2]-v0[2]] #sub(v1,v0)
	edge2 = [v2[0]-v0[0], v2[1]-v0[1], v2[2]-v0[2]] # sub(v2,v0)

	pvec =  [direx[1]*edge2[2] - direx[2]*edge2[1],
		 direx[2]*edge2[0] - direx[0]*edge2[2],
		 direx[0]*edge2[1] - direx[1]*edge2[0]] #cross(direx, edge2)

	det =  edge1[0]*pvec[0] + edge1[1]*pvec[1] + edge1[2]*pvec[2] #dot(edge1, pvec)
	return det

def ob_center(verts):
	center = [0.0,0.0,0.0]
	for vi in range(len(verts)):
		for i in range(3):
			center[i] += verts[vi][i]
	for i in range(3):
		center[i] /= len(verts)
	return center

def testing_boxes(point,sz,name,disp,merge):
	"""
	build the testing boxes
	"""
	b = [point[0]-sz, point[0]+sz, point[1]-sz, point[1]+sz, point[2]-sz, point[2]+sz]
	build_box(b,name,disp,merge)

def build_box(b1,name,display,merge,vertarray=None):
	"""
	Create a box using the Poser geometry API
	b1 format is [minX,maxX,minY,maxY,minZ,maxZ]
	if merge == 1, prop will be merged with any pre-existing
	prop of the same name.
	"""
	addmesh = 0
	if merge:
		props = [a for a in scene.Actors() if a.Name().find(name) != -1 and a.IsProp()]
		if len(props) >= 1: addmesh = 1
	if addmesh:
		newbox = props[0]
		bbox = newbox.Geometry()
	else: bbox = poser.NewGeometry()
	if vertarray == None:
		Verts = Numeric.array([ [b1[0],b1[2],b1[4]], [b1[0],b1[3],b1[4]], [b1[0],b1[3],b1[5]],
								[b1[0],b1[2],b1[5]],[b1[1],b1[2],b1[4]],[b1[1],b1[3],b1[4]],
								[b1[1],b1[3],b1[5]],[b1[1],b1[2],b1[5]] ], Numeric.Float)
	else:	# Use vertarray
		Verts = vertarray
	Polys = Numeric.array([[0,4],[4,4],[8,4],[12,4],[16,4],[20,4]],Numeric.Int)
	Sets = Numeric.array([3,2,1,0,4,5,6,7,5,4,0,1,6,5,1,2,7,6,2,3,4,7,3,0],Numeric.Int)
	bbox.AddGeneralMesh(Polys,Sets,Verts)
	if addmesh:
		newbox.SetGeometry(bbox)
		newbox.MarkGeomChanged()
	else: newbox = scene.CreatePropFromGeom(bbox,name)
	if display == 1: newbox.SetDisplayStyle(poser.kDisplayCodeEDGESONLY)
	if display == 2: newbox.SetDisplayStyle(poser.kDisplayCodeWIREFRAME)
	scene.DrawAll()

def test(testing=0):
	verts = getVerts(thegeom)
	polys = getPolys(thegeom)
	centers = polycenters(polys,verts)
	obcenter = ob_center(verts)
	copypolys = [[] for i in polys]
	for p_i in range(thegeom.NumPolygons()):
		center = centers[p_i]
		direx = sub(obcenter,center)
		newpoly = [i for i in polys[p_i]]
		det = plane_dir(direx,verts[polys[p_i][0]],verts[polys[p_i][1]],verts[polys[p_i][2]])
		if det < FLT_EPSILON:
			if testing:
				testing_boxes(center,0.005,"poly%s"%(p_i),0,0)
			newpoly.reverse()
		copypolys[p_i] = newpoly
	addgeom = poser.NewGeometry()
	for p_i in range(thegeom.NumPolygons()):
		line = [verts[i] for i in copypolys[p_i]]
		addgeom.AddPolygon(Numeric.array(line,Numeric.Float))
	addgeom.Weld()
	addobj = scene.CreatePropFromGeom(addgeom,"%s_fixed" %(act.Name()))
	scene.ProcessSomeEvents()
	scene.DrawAll()

test(testing=1)
#test()