#!BPY
# -*- coding: latin-1 -*-
"""
Name: 'Camera_Background_Matcher'
Blender: 248
Group: 'Animation'
Tooltip: 'Matches Camera and Meshes to a Background Object'
"""

__author__ = ["Mathias Weitz"]
__url__ = ("blender")
__version__ = "0.0.7 30/03/08"

__bpydoc__ = """\
Camera Matcher
"""
# --------------------------------------------------------------------------
# Camera to background matcher (c) Mathius Weitz
# --------------------------------------------------------------------------
# ***** BEGIN GPL LICENSE BLOCK *****
#
# 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.
#
# ***** END GPL LICENCE BLOCK *****
# --------------------------------------------------------------------------
import Blender
from Blender import *
from Blender import NMesh
from Blender.BGL import *
from Blender.Draw import *

import math
from math import *
from Blender.Mathutils import *

import inspect

# Events
EVENT_NOEVENT		   = 1
EVENT_START	   		   = 2
EVENT_EXIT	   		   = 3
EVENT_SAVEEMPTIES	   = 4
EVENT_RESETEMPTIES	   = 5
EVENT_STARTCAM		   = 6
EVENT_RESTRICT		   = 7
EVENT_CAMTRACK		   = 8
EVENT_CREATEEMPTIES    = 9
EVENT_MOVEXY	       = 10
EVENT_EMPTIESGROUND	   = 11

# Inits
V_RESTRICT = 1
V_MOVEXY = 0
origcam_cam = None

######################################################
# GUI drawing
######################################################
def draw():
	global EVENT_NOEVENT,EVENT_START,EVENT_STARTCAM,EVENT_CAMTRACK,EVENT_EXIT
	global EVENT_CREATEEMPTIES, EVENT_EMPTIESGROUND
	global EVENT_SAVEEMPTIES, EVENT_RESETEMPTIES
	global EVENT_RESTICT, V_RESTRICT, B_RESTRICT
	global EVENT_MOVEXY, V_MOVEXY, B_MOVEXY
	global empties, mesh_object
	
	#m10 = pi / 18
	#m30 = pi / 6
	#m60 = pi / 3
	#m90 = pi / 2
	#m120 = 2 * pi / 3

	#rx = m120
	#ry = 0
	#rz = m30
	
	#cameraDir = cam_getdir(rx,ry,rz)
	#cameraUpDir = cam_getupdir(rx,ry,rz)
	
	#print
	#print rx,ry,rz
	#print "camdir" , cameraDir
	#print "camupdir" , cameraUpDir
	
	#cam_getLocalDir(cameraDir, cameraUpDir)
	#cam_getLocalDir(cameraDir, cameraUpDir, [rx,ry,rz])
	
	calc_ok = True
	calcCam_ok = True
	calcTrack_ok = True
	
	########## Titles
	glClear(GL_COLOR_BUFFER_BIT)
	glColor3f(0,0,0)
	starty = 240
	glRasterPos2d(10,starty)
	Text ("Camera Matcher v0.0.8")
	
	starty -= 15
	glRasterPos2d(10,starty)
	
	minEmpties = 3
	if V_MOVEXY == 1:
		minEmpties = 2
	
	if minEmpties-1 < len(empties):
		va = -6
		if V_MOVEXY == 1:
			va = -3
		for ve in empties:
			va += 2
			if ve.moveX == 1:
				va -= 1
			if ve.moveY == 1:
				va -= 1
			if ve.moveZ == 1:
				va -= 1
		if va < 0:
			Text ("System is underestimated")
			calc_ok &= False
		else:
			Text ("Saved Empties: " +  str(len(empties)) + ", overestimated:" + str(va))
	elif 0 < len(empties):
		Text ("Saved Empties: " +  str(len(empties)) + ", you need at least " + str(minEmpties) + " empties")
		calc_ok &= False
		calcCam_ok &= False
	else:
		Text ("Create and save empties first")
		calc_ok &= False
		calcCam_ok &= False
				
	starty -= 15
	glRasterPos2d(10,starty)
	if mesh_object == None:
		Text ("Create and save mesh-object first")
		calc_ok &= False
		calcTrack_ok &= False
	else:
		if mesh_object.loc[0] == 0 and mesh_object.loc[1] == 0 and mesh_object.loc[2] == 0 and mesh_object.rot[0] == 0 and mesh_object.rot[1] == 0 and mesh_object.rot[2] == 0:
			calcTrack_ok &= False
			
	glColor3f(0,0.3,0.3)		
	starty -= 20
	glRasterPos2d(20,starty)
	Text ("Use the empties to place the reference points over the background image")		
	starty -= 15
	glRasterPos2d(20,starty)
	Text ("'Object Placing' translates the object, so that the reference points fits to the empties")			
	starty -= 15
	glRasterPos2d(20,starty)
	Text ("there may be a correct translating, but the object may be 'flipped'")			
	starty -= 15
	glRasterPos2d(40,starty)
	Text ("in this case, rotate the object and restart the calculation")	
	glColor3f(0,0.4,0.4)		
	starty -= 20
	glRasterPos2d(20,starty)
	Text ("the calculation is an numeric approach, which can result to local minima")	
	starty -= 15
	glRasterPos2d(20,starty)
	Text ("thus with different start points you can get different results")
		

	B_MOVEXY = Toggle("Move Restrict XY", EVENT_MOVEXY, 10, 70, 185, 18, V_MOVEXY, "leave out z-translation and xy-rotation, behaves like moving something over a flat ground")
			
	Button("Save Empties", EVENT_SAVEEMPTIES , 200, 50, 99, 18)
	Button("Level Zero", EVENT_EMPTIESGROUND , 301, 50, 99, 18, "Translate the Empties to the z-plane, this should help making additional Points, if the Camera is already matched")
	Button("Reset Empties", EVENT_RESETEMPTIES , 200, 30, 99, 18, "Set the Empties and the Meshobject back to the Position where they were saved")
	if mesh_object != None:
		Button("Create Empties", EVENT_CREATEEMPTIES , 301, 30, 99, 18, "Create Empties at every selected Vert of the Mesh")
	Button("Exit",EVENT_EXIT , 200, 10, 200, 18)
	if calc_ok:
		Button("Start Object Placing",EVENT_START , 10, 30, 185, 38)
	
	#if calcCam_ok:
	#	Button("Start Camera Placing",EVENT_STARTCAM , 10, 30, 185, 18)
		
	if calcTrack_ok:
		Button("Object Values to Camera",EVENT_CAMTRACK , 10, 10, 185, 18, "Set the Meshobject back to the orign and translate the Camera insteat")
	
	B_RESTRICT = Toggle("Restrict numeric calculation", EVENT_RESTRICT, 200, 70, 200, 18, V_RESTRICT, "Internal Parameter to mute the numerical approach")

	# Alle Empties
	glColor3f(0,0,0)
	#for i in range(len(empties)):
	#	ypos = 20 + 20 * i
	#	v = empties[i]
	#	glRasterPos2d(430,ypos + 5)
	#	Text (v.empty.getName())
	#	v.button_active = Toggle("active", v.event_active, 500, ypos, 50, 18, v.active)
	#	v.button_moveX = Toggle("x", v.event_moveX, 552, ypos, 18, 18, v.moveX)		
	#	v.button_moveY = Toggle("y", v.event_moveY, 572, ypos, 18, 18, v.moveY)		
	#	v.button_moveZ = Toggle("z", v.event_moveZ, 592, ypos, 18, 18, v.moveZ)		

def event(evt, val):
	if (evt == QKEY and not val): 
		Exit()

def bevent(evt):
	global EVENT_NOEVENT, EVENT_START, EVENT_STARTCAM, EVENT_CAMTRACK, EVENT_EXIT
	global EVENT_CREATEEMPTIES, EVENT_EMPTIESGROUND
	global EVENT_RESTICT, V_RESTRICT, B_RESTRICT
	global EVENT_MOVEXY, V_MOVEXY, B_MOVEXY
	global empties, mesh_object
	global EVENT_SAVEEMPTIES, EVENT_RESETEMPTIES

	if evt == EVENT_EXIT:
		Exit()	
	elif evt == EVENT_RESTRICT:
		V_RESTRICT = B_RESTRICT.val
	elif evt == EVENT_MOVEXY:
		V_MOVEXY = B_MOVEXY.val
		Blender.Redraw()
	elif evt == EVENT_CAMTRACK:  
		camtrack()
		Blender.Redraw()
	elif evt == EVENT_CREATEEMPTIES:  
		empties_create()
		Blender.Redraw()
	elif evt == EVENT_SAVEEMPTIES:  
		empties_save()
		Blender.Redraw()
	elif evt == EVENT_RESETEMPTIES:  
		empties_reset()
		Blender.Redraw()
	elif evt == EVENT_EMPTIESGROUND:  
		empties_ground()
		Blender.Redraw()
	elif evt == EVENT_START:
		cameraMetrik(0, V_RESTRICT, V_MOVEXY)
		Blender.Redraw()
	elif evt == EVENT_STARTCAM:
		cameraMetrik(1, V_RESTRICT, V_MOVEXY)
		Blender.Redraw()
	else:	
		#print "Event" , evt
		for i in range(len(empties)):
			v = empties[i]
			if evt == v.event_active:
				v.active = v.button_active.val
			if evt == v.event_moveX:
				v.moveX = v.button_moveX.val
			if evt == v.event_moveY:
				v.moveY = v.button_moveY.val
			if evt == v.event_moveZ:
				v.moveZ = v.button_moveZ.val
			# Es duerfen nicht alle drei Koordinaten frei sein, weil dann ist es uneindeutig.
			if v.moveX == 1 and v.moveY == 1 and v.moveZ == 1:
				v.moveX = 0
				v.moveY = 0
				v.moveZ = 0
		Blender.Redraw()
			
				
		
	
#################################
# Die Routinen fuer Points
#################################

def projPointA(p1,p2,p3,pt,dir,mode):
	"berechnet die senkrechten bzw durch dir(ection) bestimmte Projektion vom KoordinatenArray p \
	 auf der Ebene, die durch p2-p1 und p3-p1 aufgespannt wird \
	 Rueckgabe ist ein Array[xyz] der Koordinaten auf der Ebene \
	 und True, falls der Punkt in der Ebene liegt. \
	 mode = 1 wird auf die Kontrolle bei dreieckigen Flaechen verzichtet \
	 damit bekommt man viereckige Flaechen besser im Griff, aber es gibt auch mehr Fehler"   
	erg = [pt[0], pt[1], pt[2], True,0]
	abw = 0.001
	d1x = p2[0] - p1[0]
	d1y = p2[1] - p1[1]
	d1z = p2[2] - p1[2]
	d2x = p3[0] - p1[0]
	d2y = p3[1] - p1[1]
	d2z = p3[2] - p1[2]
	ddx = pt[0] - p1[0]
	ddy = pt[1] - p1[1]
	ddz = pt[2] - p1[2]
	if dir[0] == 0 and dir[1] == 0 and dir[2] == 0:
		ec = crossProd([d1x,d1y,d1z], [d2x,d2y,d2z])
	else:
		ec = dir
	e =  norm(ec)
	m = matrix3x3([d1x,d1y,d1z], [d2x,d2y,d2z],e)
	m.t()   
	detm = m.det()
	if 0.000001 < abs(detm):
		so =m.solve([ddx, ddy, ddz])
		b = True
		c1x = p2[0]*so[0] + p1[0]*(1-so[0])
		c1y = p2[1]*so[0] + p1[1]*(1-so[0])
		c1z = p2[2]*so[0] + p1[2]*(1-so[0])
		c2x = p3[0]*so[1] + p1[0]*(1-so[1])
		c2y = p3[1]*so[1] + p1[1]*(1-so[1])
		c2z = p3[2]*so[1] + p1[2]*(1-so[1])
		# Test, ob Punkt in der Ebene liegt
		if so[0] < -abw or (1+abw) < so[0] or so[1] < -abw or (1+abw) < so[1]:
			b = False
		if mode == 0 and (1+abw) < abs(so[0]) + abs(so[1]):
			b = False 
		erg = [c1x+c2x-p1[0], c1y+c2y-p1[1], c1z+c2z-p1[2], b, so[2]]
	
	return erg  


def projPoint(p1,p2,p3,pt,dir,mode):
	return projPointA(pointToArray(p1), pointToArray(p2), pointToArray(p3), \
		pointToArray(pt), dir, mode)
	 

def projPointEA(faceP, p, dir=[0,0,0]):
	"berechnet die senkrechten Projektion vom KoordinatenArray p \
	 auf der Ebene. Bei einer Ebene mit 4 Punkten wird \
	 der Punkt und der Abstand gemittelt."
	erg = [0,0,0,False,0]
	c = len(faceP.v)
	#print c,
	if (3 < c):
		# bei mehr als 3 Ecken wird das Ergebnis gemittelt   
		sumx = 0;
		sumy = 0;
		sumz = 0;
		sumdir = 0
		hit = False
		rc = 0
		for i in range(c):
			im = (i-1)%c
			ip = (i+1)%c
			pjt = projPointA( \
				pointToArray(faceP.v[i]), \
				pointToArray(faceP.v[im]), \
				pointToArray(faceP.v[ip]), \
				p, dir ,0)
			if pjt[3]:
				rc += 1
				sumx += pjt[0]
				sumy += pjt[1]
				sumz += pjt[2]
				sumdir += pjt[4]
			hit |= pjt[3]
			
		if 0 < rc:
			erg = [sumx / rc, sumy / rc, sumz / rc, hit, sumdir / rc]
	else:
		pjt = projPointA( \
			pointToArray(faceP.v[0]), \
			pointToArray(faceP.v[1]), \
			pointToArray(faceP.v[2]), \
			p, dir ,0)
		erg = [pjt[0], pjt[1], pjt[2], pjt[3], pjt[4]]
	return erg 

def projPointE(faceP,p, dir=[0,0,0]):
	return projPointEA(faceP, pointToArray(p), dir)

def edgeDistA (a1s,a1e,a2s,a2e,d = [0,0,0]):
	"wie edgeDistP, nur es werden 3-Array statt Verts uebergeben"
	erg = [0,0,0,False]
	# e ist das normierte Kreuzprodukt
	
	d1 = vecSub(a1e, a1s)
	d1neg = vecSub(a1s, a1e)
	d2 = vecSub(a2e, a2s)
	d2neg = vecSub(a2s, a2e)
	dd = vecSub(a1s, a2s)   
	ddneg = vecSub(a2s, a1s)
	de = d
	if d == [0,0,0]:
		de = crossProd(d1,d2)
	e = norm(de)
	m = matrix3x3(d1neg,d2,e)
	if abs(m.det()) == 0:
		if d == [0,0,0]:
			# beide Geraden laufen parallel
			# es gibt eventuell viele Loesungen
			# TODO
			pass
		else:
			# es Geraden laufen aus Sicht von d parallel
			pass
	else:
		m.t()
		so = m.solve(dd)
		erg[0] = so[0]
		erg[1] = so[1]
		erg[2] = so[2]
		if 0 <= so[0] and so[0] <= 1 and 0 <= so[1] and so[1] <= 1:
			erg[3] = True
	return erg  

def edgeDistP (p1s,p1e,p2s,p2e,d = [0,0,0]):
	"Kuerzeste Distanz zweier Geraden \
	  das Ergebniss ist ein Array mit \
		0. m1 = Verhaeltniss auf der ersten Geraden \
		1. m2 = Verhaeltniss auf der zweiten Geraden \
		2. dist = Distanz der beiden Geraden \
		3. b = True, wenn die kuerzeste Distanz innerhalb der Kanten liegt"
	return edgeDistA (pointToArray(p1s),pointToArray(p1e),pointToArray(p2s),pointToArray(p2e),d)

def edgeDistQ (p1s,p1e,p2s,p2e):
	"Suche naechsten Punkt zur Geraden \
	 p1e-p1s und p2e-p2s \
	 Ergebniss ist ein Array mit\
	   1. der Punkt auf der Geraden p2e-p2s \
	   2. True, falls sich der Punkt innerhalb von p1e-p1s befindet"
	erg = [0,False]
	z =  edgeDistP (p1s,p1e,p2s,p2e)
	mo = z[0]
	mb = False
	if 0 <= mo and mo <= 1:
		mb = True
	m = z[1]
	if m < 0:
		m = 0
	if 1 < m:
		m = 1
	erg[0] = propEdge(p2s, p2e, m)
	erg[1] = mb
	return erg 

def findClosestFace(me, co, dir):
	"Sucht das Face, das in der Richtung dir dem \
	Koordinaten in co am naechsten ist \
	Rueckgabe [face, coProj, dist]"
	T_Dir = 0
	coProj = [0,0,0]
	minAbw = 0.00001
	bestFace = 0
	besth = 10000   
	for ff in me.faces:
		pjt1 = projPointEA(ff,co, dir)
		if pjt1[3]:											 
			d = dist([co[0] - pjt1[0], co[1] - pjt1[1], co[2] - pjt1[2]])
			if minAbw < d and d < besth and (pjt1[4] <= 0 or (pjt1[4] != 0 and T_Dir == 1)):
				besth = d
				bestFace = ff
				coProj[0] = pjt1[0]
				coProj[1] = pjt1[1]
				coProj[2] = pjt1[2]
	if bestFace != 0:
		pass
		#pc1 = NMesh.Vert(coProj[0], coProj[1], coProj[2])
		#pc2 = NMesh.Vert(co[0], co[1], co[2])
		#me.verts.append(pc1)
		#me.verts.append(pc2)
		#e = me.addEdge(pc1,pc2)
		#po[i][1] = bestFace
		#po[i][2] = pc1
	else:
		print "found no bestFace: ", besth
	return [bestFace, coProj, besth]

def findAllEdges(me, v1, v2, dir):
	"Gibt eine Liste aller Edges zurueck \
	 die Listenelemente enthaelt folgende Infos \
	 0. ProjEdge \
	 1. Koordinaten auf der Ausgangsgeraden \
	 2. Distanz"
	crossEdges = []
	mminAbw = 0.0001
	for ee in me.edges:
		vv1 = pointToArray(ee.v1)
		vv2 = pointToArray(ee.v2)
		if not arrayEqual(v1, vv1) and not arrayEqual(v1, vv2) and not arrayEqual(v2, vv1) and not arrayEqual(v2, vv2): 
			er = edgeDistA(v1, v2, vv1, vv2,dir)
			if er[3] and 0 < er[2]:
				# Test, ob Kante verdeckt wird durch ein Face
				bestPoint1 = ratioPoint (v1, v2, er[0])
				erf = findClosestFace(me, bestPoint1, dir)
				if er[2] < erf[2] + mminAbw:
					crossEdges.append(ee)
	
	erg = [[0,v1,0],[0,v2,0]]
	b = True
	sec = 0
	while b:
		b = False
		newerg = []
		newerg.append(erg[0])
		for i in range(len(erg)-1):
			best = 100000
			targetEdge = 0
			bestRatio1 = 0
			bestPoint1 = [0,0,0]
			bestPoint2 = [0,0,0]
			for ee in crossEdges:
				vv1 = pointToArray(ee.v1)
				vv2 = pointToArray(ee.v2)
				er = edgeDistA(erg[i][1], erg[i+1][1], vv1, vv2,dir)
				if er[3]:	   
					if er[2] < best and mminAbw < er[0] and er[0] < 1 - mminAbw:
						best = er[2]
						targetEdge = ee
						bestRatio1 = er[0]
						bestPoint1 = ratioPoint (erg[i][1],erg[i+1][1],er[0])   
						bestPoint2 = ratioPoint (vv1,vv2,er[1])
		
			if best < 100000:   
				#print sec, bestRatio1, best	
				#pc1 = NMesh.Vert(bestPoint1[0], bestPoint1[1], bestPoint1[2])
				#pc2 = NMesh.Vert(bestPoint2[0], bestPoint2[1], bestPoint2[2])
				#me.verts.append(pc1)
				#me.verts.append(pc2)
				#e = me.addEdge(pc1,pc2)
				newerg.append([targetEdge, bestPoint1, best, bestPoint2])
				sec += 1
				if sec < 3500:
					b = True
			newerg.append(erg[i+1])
		erg = newerg
	return erg
		
				
def propEdge(p1, p2, m):
	"Gibt die Verhaeltnisskoordinaten \
	 zwischen den beiden Punkten zurueck"
	return [p2.co.x*m + p1.co.x*(1-m), p2.co.y*m + p1.co.y*(1-m), p2.co.z*m + p1.co.z*(1-m)]

def findFaces(me, ar):
	"die Faces, das die angegebenen Punkte enthaelt, \
	rueckgabe ist ein Array mit den faces \
	die Komplexitaet ist |f|*|ar|*|aveEdges| also etwa Anzahl der Faces * 16"
	global T_Ign
	erg = []
	for f in me.faces:
		if T_Ign != 1 or f.hide != 1:
			b = True
			for j in ar:
				found = False
				for k in f.v:
					if j == k:
						found = True
				b &= found
			if b:
				erg.append(f)
	return erg

def getConnected(me, v):
	"gibt eine Liste der verbundenen Punkte zurueck"
	erg = []
	for ee in me.edges:
		if ee.v1 == v:
			erg.append(ee.v2)
		if ee.v2 == v:
			erg.append(ee.v1)
	return erg

def crossProdP(p, p1, p2):
	"berechnet das Kreuzprodukt zwischen p1-p und p2-p"
	v1 = vecSub(pointToArray(p),pointToArray(p1))
	v2 = vecSub(pointToArray(p),pointToArray(p2))
	return crossProd(v1,v2)

def cosP(p, p1, p2):
	"berechnet den Cosinus p1-p und p2-p"
	v1 = vecSub(pointToArray(p),pointToArray(p1))
	v2 = vecSub(pointToArray(p),pointToArray(p2))
	return vecCos(v1,v2)

def queueIndexDist(i1,i2, plen):
	"die Distanz zweier Indezes in einer zyklischen Liste"
	min = abs(i1-i2)
	if i1 + plen - i2 < min:
		min = i1 + plen - i2
	if i2 + plen - i1 < min:
		min = i2 + plen - i1
	return min

def minA (li):
	erg = 0
	if 0 < len(li):
		erg = li[0]
	for w in li:
		if w < erg:
			erg = w
	return erg

def maxA (li):
	erg = 0
	if 0 < len(li):
		erg = li[0]
	for w in li:
		if erg < w:
			erg = w
	return erg

def arrayEqual(a1,a2):
	erg = False
	if len(a1) == len(a2):
		erg = True
		for i in range(len(a1)):
			if 0.0000001 < abs(a1[i] - a2[i]):
				erg = False
	return erg


def ratioPoint(a1,a2,r):
	return [a1[0]*(1-r)+a2[0]*r, a1[1]*(1-r)+a2[1]*r, a1[2]*(1-r)+a2[2]*r]


######################################
# mathematische Routinen
######################################

class matrix3x3:
	"Wird zum lösen linearer Systeme der Größe 3 benutzt. \
	Parameter sind 3 Array mit je 3 Elementen, \
	die wichtigste Routine ist solve(Point)"
	def __init__(self,p1,p2,p3):
		self.p1 = p1
		self.p2 = p2
		self.p3 = p3
	def det(self):
		"Determinante nach der sarrusschen Regel"
		return \
			self.p1[0] * (self.p2[1] * self.p3[2] - self.p2[2] * self.p3[1]) \
			- self.p2[0] * (self.p1[1] * self.p3[2] - self.p3[1] * self.p1[2]) \
			+ self.p3[0] * (self.p1[1] * self.p2[2] - self.p2[1] * self.p1[2])
	def t(self):
		h = self.p1[1]
		self.p1[1] = self.p2[0]
		self.p2[0] = h
		h = self.p1[2]
		self.p1[2] = self.p3[0]
		self.p3[0] = h
		h = self.p2[2]
		self.p2[2] = self.p3[1]
		self.p3[1] = h
		return self
	def write(self):
		print "[(" , self.p1[0] , ","  , self.p1[1] , "," , self.p1[2] , \
			") (" , self.p2[0] , ","  , self.p2[1] , "," , self.p2[2] , \
			") (" , self.p3[0] , ","  , self.p3[1] , "," , self.p3[2] ,  ")]"
		return self
	def solve(self,e):
		self.t()
		m = self.det()
		mx = matrix3x3(e, self.p2, self.p3)
		my = matrix3x3(self.p1, e, self.p3)
		mz = matrix3x3(self.p1, self.p2, e)
		erg = [0, 0, 0]
		if m != 0:
			erg = [mx.det()/m, my.det()/m, mz.det()/m]
		return erg

def printArray(v):
	print "[",
	for i in range(len(v)):
		print "%5.3f" % v[i],
	print "]",

def pointToArray(p1):
	return [p1.co.x, p1.co.y, p1.co.z]


def pointToArrayW(me, p1):
	w = getCrease(me, p1)
	erg = [p1.co.x, p1.co.y, p1.co.z, w]
	return erg

def pointPrint(p1):
	return "[" + "%5.2f"%p1.co.x + "," + "%5.2f"%p1.co.y + "," + "%5.2f"%p1.co.z + "]"

def vecAdd(v1,v2):
	"subtraktion"
	return [v2[0]+v1[0], v2[1]+v1[1], v2[2]+v1[2]]

def vecSub(v1,v2):
	"subtraktion"
	return [v2[0]-v1[0], v2[1]-v1[1], v2[2]-v1[2]]
	
def scalarMul(v,a):
	"multiplikation mit einem Skalar"
	return [a*v[0], a*v[1], a*v[2]]

def propEdgeV(v1, v2, m):
	"Gibt die Verhaeltnisskoordinaten \
	 zwischen den beiden Punkten zurueck"
	return [v2[0]*m + v1[0]*(1-m), v2[1]*m + v1[1]*(1-m), v2[2]*m + v1[2]*(1-m)]

def dist(v1):
	"quadratische Norm"
	return math.sqrt(v1[0]*v1[0]+v1[1]*v1[1]+v1[2]*v1[2])

def norm(v1):
	"normalisierter Norm"
	d = dist(v1)
	if d == 0: d = 1
	return [v1[0]/d,v1[1]/d,v1[2]/d]

def scalarProd(v1,v2):
	"zwei 3-Arrays, es wird das Scalarprodukt berechnet"
	return v1[0]*v2[0]+v1[1]*v2[1]+v1[2]*v2[2]

def crossProd(v1,v2):
	"zwei 3-Arrays, es wird das Kreuzprodukt berechnet"
	return [v1[1]*v2[2]-v1[2]*v2[1],v1[2]*v2[0]-v1[0]*v2[2],v1[0]*v2[1]-v1[1]*v2[0]]

def vecCos(v1,v2):
	"berechnet den Cosinus des Winkels zwischen zwei Vektoren"
	d1 = dist(v1)
	d2 = dist(v2)
	if d1 == 0:
		d1 = 1.0
	if d2 == 0:
		d2 = 1.0
	return scalarProd(v1,v2) / (d1 * d2)

def projPointV(k1,k2,pj):
	"Projektion von pj auf die durch k1,k2 aufgespannte Ebene \
	 Ergebniss ist der Punkt, die Verhaeltnisse, die Hoehe, \
	 und ein Flag, falls pj auf die Ebene projeziert wird"   
	erg = True
	
	ec = crossProd(k1, k2)
	e =  norm(ec)
	m = matrix3x3(k1, k2, e)
	m.t()
	so =m.solve(pj)
	
	cx = k1[0]*(1-so[0]) + k2[0]*(1-so[1])
	cy = k1[1]*(1-so[0]) + k2[1]*(1-so[1])
	cz = k1[2]*(1-so[0]) + k2[2]*(1-so[1])
	rp = [cx, cy, cz]
	# Test, ob Punkt in der Ebene liegt
	if so[0] < 0 or so[1] < 0:
		erg = False
		
	return [rp,so[0],so[1],so[2],erg] 



def edgeProj(startPunkt, m, endPunkt):
	"Gibt den Projektionspunkt von m auf die Gerade |startPunkt, endPunkt|"
	p = vecSub(startPunkt, endPunkt)
	v = vecSub(startPunkt, m)
	d = dist(p)
	if d == 0:
		d = 1.0
	#return vecAdd(startPunkt, norm(p))
	return vecAdd(startPunkt, scalarMul(norm(p), scalarProd(p,v) / d))
	#return vecAdd(startPunkt, scalarMul(p, scalarProd(p,v) / dp))
	
	
def lineDir(me, v1, d1):
	#p1 = NMesh.Vert(v1[0], v1[1], v1[2])
	v2 = vecAdd(v1, d1)
	#p2 = NMesh.Vert(v2[0], v2[1], v2[2])
			
	#me.verts.append(p1)
	#me.verts.append(p2)
	me.verts.extend(v1[0], v1[1], v1[2])
	me.verts.extend(v2[0], v2[1], v2[2])
	#me.addEdge(me.verts[-2], me.verts[-1])
	me.edges.extend(me.verts[-2], me.verts[-1])

def distAA(p1,p2):
	"Distanz zwischen zwei Punkten"
	return dist(vecSub(p1,p2))

def distPP(p1,p2):
	"Distanz zwischen zwei Punkten"
	return distAA(pointToArray(p1),pointToArray(p2))


######################################
# matrix
######################################

class matrix:
	def __init__(self):
		self.field = [[0]]
		
	def set(self,y,x,value):
		while len(self.field) <= y:
			self.field.append([0])
		while len(self.field[y]) <= x:
			self.field[y].append(0)
		self.field[y][x] = value
		
	def get(self,y,x):
		erg = 0
		if y < len(self.field):
			if x < len(self.field[y]):
				erg = self.field[y][x]
		return erg
	
	def dimX(self):
		erg = 0
		for i in range(len(self.field)):
			if erg < len(self.field[i]):
				erg = len(self.field[i])
		return erg
	
	
	def dimY(self):
		return len(self.field)
	
	def copy(self):
		erg = matrix()
		for i in range(len(self.field)):
			for j in range(len(self.field[i])):
				erg.set(i,j,self.field[i][j])
		return erg
	
	def t_copy(self):
		# transponierte Matrix
		erg = matrix()
		for i in range(len(self.field)):
			for j in range(len(self.field[i])):
				erg.set(j,i,self.field[i][j])
		return erg
	
	def write(self):
		for v1 in self.field:
			for v2 in v1:
				print "%7.2f" % v2,
			print
			
	def mul(self,y,a):
		# multipliziert Zeile y mit a
		if y < len(self.field):
			for i in range(len(self.field[y])):
				self.field[y][i] *= a
	
	def sub(self,y1,y2):
		# zieht von Zeile y1 die Zeile y2 ab
		if y2 < len(self.field):
			maxl = len(self.field[y2])
			if y1 < len(self.field):
				maxl = max(maxl,len(self.field[y2]))
			for i in range(maxl):
				self.set(y1,i, self.get(y1,i) - self.get(y2,i))
				
	def change(self,y1,y2):
		# tauscht Zeile y1 mit Zeile y2
		for i in range(self.dimX()):
			h = self.get(y1, i)
			self.set(y1, i, self.get(y2, i)) 
			self.set(y2, i, h)
			
	def mul_matrix(self, m):
		# multipliziert zwei Matrizen miteinander
		erg = matrix() 
		if self.dimX() == m.dimY():
			for y in range(self.dimY()):
				for x in range(m.dimX()):
					s = 0
					for k in range(self.dimX()):
						s += self.get(y,k) * m.get(k,x)
					erg.set(y,x,s) 
		return erg
	
	def mul_vektor(self, v):
		# multipliziert eine Matriz mit einem Array (= Vektor)
		erg = [] 
		#print "mul_vektor", self.dimX(), len(v) 
		if self.dimX() == len(v):
			for y in range(self.dimY()):
				s = 0
				for k in range(self.dimX()):
					s += self.get(y,k) * v[k]
				erg.append(s) 
		return erg
		
				 
	def gauss(self, e):
		# lineare Gleichung nach Gauss
		while len(e) < len(self.field):
			e.append(0)
		# forward-Schritt
		for i in range(len(self.field)-1):
			#self.write()
			#print e
			pivotE = self.get(i,i)
			if 1e-5 < abs(pivotE):
				for j in range(i+1, len(self.field)):
					#print "gauss, forward", i,j
					pivotJ = self.get(j,i)
					if 1e-5 < abs(pivotJ):
						faktor = pivotE/pivotJ
						self.mul(j, faktor)
						e[j] *= faktor
						e[j] -= e[i]
						self.sub(j,i)
			else:
				erg = 0
				for j in range(i+1, len(self.field)):
					if 0 < abs(self.field[j][i]):
						erg = j
				if 0 < erg:
					self.change(i,erg)
					h = e[i]
					e[i] = e[erg]
					e[erg] = h
				else:
					# Ausnahme, lineare Abhängigkeit
					pass 
		
		# backward-Schritt
		for i in range(len(self.field)-1,0,-1):
			#self.write()
			#print e
			pivotE = self.get(i,i)
			for j in range(0,i):
				#print "gauss, backward", i,j
				pivotJ = self.get(j,i)
				if 1e-5 < abs(pivotJ):
					faktor = pivotE/pivotJ
					self.mul(j, faktor)
					e[j] *= faktor
					e[j] -= e[i]
					self.sub(j,i)

			e[i] /= self.get(i,i)
		e[0] /= self.get(0,0)   
			
		return e
		

# Objekte & Routinen
	
class mLine:
	"Linie"
	def __init__(self,v1,v2):
		self.va = v1
		self.vb = v2
	
def addLine(col, v1, v2):
	b = 0
	for m in col:
		if (m.va == v1 and m.vb == v2) or (m.va == v2 and m.vb == v1):
			b = 1
	if b == 0:
		mm = mLine(v1,v2)
		col.append(mm)
		
def addEdgeE(p0, p1, dist, mode = 0):
	# zieht eine Linie von p0 nach p1
	# hat dist einen Wert, so werden in festen Abstaenden dist
	# Zwischenpunkte gesetzt
	# mode hat (noch) keine Funktion
	global me
	if dist < 0.001:
		me.addEdge(p0, p1)
	else:
		a0 = pointToArray(p0)
		a1 = pointToArray(p1)
		da = distAA(a0,a1)
		seg = int(da / dist + 0.999)
		if mode == 0:
			v0 = p0
			if 0 < seg:
				m = scalarMul(norm(vecSub(a0,a1)), dist)
				for i in range(1, seg):
					av = vecAdd(a0, scalarMul(m,i))
					pp = NMesh.Vert(av[0], av[1], av[2])
					me.verts.append(pp)
					me.addEdge(v0, pp)
					v0 = pp 
			me.addEdge(v0, p1)
		else:
			v0 = p1
			if 0 < seg:
				m = scalarMul(norm(vecSub(a1,a0)), dist)
				for i in range(1, seg):
					av = vecAdd(a1, scalarMul(m,i))
					pp = NMesh.Vert(av[0], av[1], av[2])
					me.verts.append(pp)
					me.addEdge(v0, pp)
					v0 = pp 
			me.addEdge(v0, p0)
			
class viewLine:
	"Sicht-Linie"
	def __init__(self,empty):
		self.empty = empty
		self.orig = empty.loc
		self.origrot = empty.rot.copy()
		self.loc = [0,0,0]
		self.dir = [0,0,0]
		self.v = None
		#
		self.active = 1
		self.moveX = 0
		self.moveY = 0
		self.moveZ = 0		
		# Trigger fuer Events
		self.event_active = 0
		self.event_moveX = 0
		self.event_moveY = 0
		self.event_moveZ = 0	
		# GUI-Elements
		self.button_active = None
		self.button_moveX = None
		self.button_moveY = None
		self.button_moveZ = None	
		
	def calcViewLine(self,camloc):
		xDif = self.empty.loc[0] - camloc[0]
		yDif = self.empty.loc[1] - camloc[1]
		zDif = self.empty.loc[2] - camloc[2]
			
		vecDif = [xDif,yDif,zDif]
		self.loc = self.empty.loc
		self.dir = norm(vecDif)
		
def create_EmptyAt(v):
	# create an Empty at the Location of the vertice
	pass
		
			
###########################################
# Start Test
###########################################

me = 0
empties = []
mesh_object = None

def empties_create():
	global empties, mesh_object, origcam, origcam_pos, origcam_rot
	print "*** start ***"
	time_start = sys.time()
	
	if mesh_object != None:
		scn = Scene.GetCurrent()
		d = mesh_object.getData()
		for v in d.verts:
			#print 
			#print v
			if v.sel == 1:
				addEmpty = True
				vco = pointToArray(v)
				p = None
				for ee in empties:
					eloc = ee.empty.loc
					d = distAA(eloc, vco)
					if d < 0.0001:
						addEmpty = False
						# das Vertice speichern
						p = v
				
				if addEmpty:
					n = scn.objects.new('Empty')
					n.loc = v.co
					e = viewLine(n)
					e.v = p
					empties.append(e)
				
	print "finished : %.2f"  % (sys.time() - time_start)
	
def empties_save():
	global empties, mesh_object, origcam, origcam_pos, origcam_rot
	print "*** start ***"
	time_start = sys.time()
	del empties[:]
	
	scn = Scene.GetCurrent()
	cam = scn.objects.camera
	origcam_pos = cam.loc
	origcam_rot = cam.rot.copy()
	origcam = cam
		
	for v in scn.objects.context:
		#print v, v.getType()
		if v.getType() == 'Empty':
			#print v.loc
			e = viewLine(v)
			empties.append(e)
		if v.getType() == 'Mesh':
			#print v.loc
			mesh_object = v
	
	# Trigger für die Empties setzen
	max_empty_event = 0
	for v in empties:
		max_empty_event = max(max_empty_event,v.event_active, v.event_moveX,v.event_moveY,v.event_moveZ)
		
	if max_empty_event == 0:
		max_empty_event = 1000
	max_empty_event += 1
		
	for iv in range(len(empties)):
		v = empties[iv]
		if v.event_active == 0:
			v.event_active = max_empty_event
			max_empty_event += 1
		if v.event_moveX == 0:
			v.event_moveX = max_empty_event
			max_empty_event += 1
		if v.event_moveY == 0:
			v.event_moveY = max_empty_event
			max_empty_event += 1
		if v.event_moveZ == 0:
			v.event_moveZ = max_empty_event
			max_empty_event += 1
			
	
	#self.event_moveX = 0
	#self.event_moveY = 0
	#self.event_moveZ = 0	
	# GUI-Elements
	#self.button_active = None
	#self.button_moveX = None
	#self.button_moveY = None
	#self.button_moveZ = None	
			
	print "finished : %.2f"  % (sys.time() - time_start)
		
def empties_ground():
	global empties, mesh_object, origcam, origcam_pos, origcam_rot
	print "*** start ***"
	time_start = sys.time()
	
	scn = Scene.GetCurrent()
	cam = scn.objects.camera
	
	for v in empties:
		if v.empty.sel:
			v.calcViewLine(cam.loc)
			if 0.00001 < abs(v.empty.loc[2]) and 0.01 < abs(v.dir[2]):
				#print v.empty.loc, v.dir
				x = v.empty.loc[2] / v.dir[2]
				v.empty.loc = v.empty.loc[0] - x*v.dir[0], v.empty.loc[1] - x*v.dir[1], 0
		#print v.empty.rot, v.origrot
		#v.empty.loc = v.orig
		#v.empty.rot = v.origrot
	print "finished : %.2f"  % (sys.time() - time_start)
		
def empties_reset():
	global empties, mesh_object, origcam, origcam_pos, origcam_rot
	print "*** start ***"
	time_start = sys.time()
	
	#print origcam_rot
	
	#if origcam != None:
	#	origcam.loc = origcam_pos
	#	origcam.rot = origcam_rot
		
	if mesh_object != None:
		mesh_object.loc = 0.0, 0.0 ,0.0
		mesh_object.rot = 0.0, 0.0 ,0.0
	
	for v in empties:
		#print v.empty.rot, v.origrot
		v.empty.loc = v.orig
		v.empty.rot = v.origrot
	print "finished : %.2f"  % (sys.time() - time_start)

def cam_getLocalDir(cameraDir, cameraUpDir, initValues=[0.0, 0.0 ,0.0]):
	
	print "******************* Start"
	
	erg = initValues
	t = [0.0] * 6

	bb = True
	j = 0
	while bb:
		j += 1
		if 100 < j:
			bb = False
		m = matrix()
		
		rx = erg[0]
		ry = erg[1]
		rz = erg[2]
		sx = sin(rx)
		cx = cos(rx)
		sy = sin(ry)
		cy = cos(ry)
		sz = sin(rz)
		cz = cos(rz)
		
		r11 = -sz*cx + cz*sy*sx
		r12 = -cz*cy*cx
		r13 = -cz*sx + sz*sy*cx
		m.set(0,0,r11)
		m.set(0,1,r12)
		m.set(0,2,r13)

		r21 = cz*cx + sz*sy*sx
		r22 = -sz*cy*cx
		r23 = -sz*sx - cz*sy*cx
		m.set(1,0,r21)
		m.set(1,1,r22)
		m.set(1,2,r23)

		r31 = cy*sx
		r32 = sy*cx
		r33 = 0
		m.set(2,0,r31)
		m.set(2,1,r32)
		m.set(2,2,r33)

		r41 = sz*sx + cz*sy*cx
		r42 = cz*cy*sx
		r43 = -cz*cx - sz*sy*sx
		m.set(3,0,r41)
		m.set(3,1,r42)
		m.set(3,2,r43)

		r51 = -cz*sx + sz*sy*cx
		r52 = sz*cy*sx
		r53 = -sz*cx + cz*sy*sx
		m.set(4,0,r51)
		m.set(4,1,r52)
		m.set(4,2,r53)

		r61 = cy*cx
		r62 = -sy*sx
		r63 = 0
		m.set(5,0,r61)
		m.set(5,1,r62)
		m.set(5,2,r63)
		
		# CameraDir
		cdx = -sz*sx - cz*sy*cx
		cdy = cz*sx - sz*sy*cx
		cdz = -cy*cx 
		# CameraUpDir
		cudx = -sz*cx + cz*sy*sx
		cudy = cz*cx + sz*sy*sx
		cudz = cy*sx
		
		t[0] =  cameraDir[0] - cdx
		t[1] =  cameraDir[1] - cdy
		t[2] =  cameraDir[2] - cdz
		
		t[3] =  cameraUpDir[0] - cudx
		t[4] =  cameraUpDir[1] - cudy
		t[5] =  cameraUpDir[2] - cudz
		
		
		#print "Differenz"
		#printArray(t)
		#print
		#print
	
		#print "Ableitung"
		#m.write()
		ma = m.t_copy().mul_matrix(m)
		#print
		#print "Ableitung zum Quadrat"
		#ma.write()
	
		#print
		#print "b Vektor"
		vb = m.t_copy().mul_vektor(t)
		#printArray(vb)
		#print
	
		#print "e Vektor"	
		ee = ma.gauss(vb)
		#printArray(ee)
		#print
		
		error = 0
	
		for i in range (len(erg)):
			#if ee[i] < -0.1:
			#	ee[i] = -0.1
			#if 0.1 < ee[i]:
			#	ee[i] = 0.1
			erg[i] += ee[i]
			error += abs(ee[i])
			
		
		for jj in range(3):
			while erg[jj] < -pi:
				erg[jj] += 2*pi
			while pi < erg[jj]:
				erg[jj] -= 2*pi

		#print "Ergebniss"
		#printArray(erg)
		#print
		
		#print j, "Fehler ", error
		if error < 1e-7:
			bb = False
	
		#print
		#print "******"
		
	#print "Iterationen:", j, ", Fehler:", error
	#print "*******************"
	return erg

	

def cam_getcalcdir(rx,ry,rz,mode = 0):
	cx = cos(rx)
	sx = -sin(rx)
	cy = cos(ry)
	sy = sin(ry)
	cz = cos(rz)
	sz = -sin(rz)
	erg = 0,0,0
	if mode == 0:
		erg = -sz*sx -cz*sy*cx, -cz*sx +sz*sy*cx, -cy*cx
	else:
		erg = sz*cx -cz*sy*sx, cz*cx +sz*sy*sx, - cy*sx
	return erg

def cam_getdir(rx,ry,rz):
	return cam_getcalcdir(rx,ry,rz,0)

def cam_getupdir(rx,ry,rz):
	return cam_getcalcdir(rx,ry,rz,1)
		
def camtrack():
	global me, empties, mesh_object, origcam, origcam_pos, origcam_rot
	print "*** start ***"
	time_start = sys.time()
	Blender.Window.EditMode(0)
	
		
	# Berechnen der Sichtlinien
	scn = Scene.GetCurrent()
	cam = scn.objects.camera
	camloc = cam.loc
	camrot = cam.rot
	
	#ob = scn.objects.active
	#me = mesh_object.getData()	
	#Mesh.Mode(1)
	
	#print mesh_object.loc
	#print mesh_object.rot
	
	cam.loc = camloc[0] - mesh_object.loc[0], camloc[1] - mesh_object.loc[1], camloc[2] - mesh_object.loc[2]
	
	rx = mesh_object.rot[0]
	ry = mesh_object.rot[1]
	rz = mesh_object.rot[2]
	
	s1 = sin(rz)
	c1 = cos(rz)
	s2 = -sin(ry)
	c2 = cos(ry)
	s3 = sin(rx)
	c3 = cos(rx)
	
	for i in range(len(empties)):
		#print "*** empty"
		#v.calcViewLine(camloc)
		#print erg[i]
		#printArray (empties[i].loc)
		#printArray (empties[i].empty.loc)
		# Rotation
			
		ex,ey,ez = empties[i].empty.loc[0] - mesh_object.loc[0],  empties[i].empty.loc[1] - mesh_object.loc[1], empties[i].empty.loc[2] - mesh_object.loc[2] 
		eex = c1*c2*ex + s1*c2*ey + s2*ez
		eey = (-c1*s2*s3 -s1*c3)*ex + (-s1*s2*s3 +c1*c3)*ey + c2*s3*ez
		eez = (-c1*s2*c3 +s1*s3)*ex + (-s1*s2*c3 -c1*s3)*ey + c2*c3*ez
		empties[i].empty.loc = eex,eey,eez
				
		
	ex = cam.loc[0]
	ey = cam.loc[1]
	ez = cam.loc[2]

	#zyx
	eex = c1*c2*ex + s1*c2*ey + s2*ez
	eey = (-c1*s2*s3 -s1*c3)*ex + (-s1*s2*s3 +c1*c3)*ey + c2*s3*ez
	eez = (-c1*s2*c3 +s1*s3)*ex + (-s1*s2*c3 -c1*s3)*ey + c2*c3*ez
	
	#xyz
	#eex = c1*c2*ex + (s1*c3-c1*s2*s3)*ey + (s1*s3 + c1*s2*c3)*ez
	#eey = -s1*c2*ex + (c1*c3 + s1*s2*s3)*ey + (c1*s3 - s1*s2*c3)*ez
	#eez = -s2*ex - c2*s3*ey + c2*c3*ez
	
	
	cam.loc = eex, eey, eez


	s1 = sin(rz)
	c1 = cos(rz)
	s2 = -sin(ry)
	c2 = cos(ry)
	s3 = sin(rx)
	c3 = cos(rx)
	
	ex = cam.rot[0]
	ey = cam.rot[1]
	ez = cam.rot[2]
	
	cdir = cam_getdir(ex,ey,ez)
	cudir = cam_getupdir(ex,ey,ez)
	
	cdirx = cdir[0]
	cdiry = cdir[1]
	cdirz = cdir[2]

	cudirx = cudir[0]
	cudiry = cudir[1]
	cudirz = cudir[2]
	
	#zyx
	ecdirx = c1*c2*cdirx + s1*c2*cdiry + s2*cdirz
	ecdiry = (-c1*s2*s3 -s1*c3)*cdirx + (-s1*s2*s3 +c1*c3)*cdiry + c2*s3*cdirz
	ecdirz = (-c1*s2*c3 +s1*s3)*cdirx + (-s1*s2*c3 -c1*s3)*cdiry + c2*c3*cdirz

	ecudirx = c1*c2*cudirx + s1*c2*cudiry + s2*cudirz
	ecudiry = (-c1*s2*s3 -s1*c3)*cudirx + (-s1*s2*s3 +c1*c3)*cudiry + c2*s3*cudirz
	ecudirz = (-c1*s2*c3 +s1*s3)*cudirx + (-s1*s2*c3 -c1*s3)*cudiry + c2*c3*cudirz

	ecdir = [ecdirx,ecdiry,ecdirz]
	ecudir = [ecudirx,ecudiry,ecudirz]
	
	#print "(", s1, c1, ") (", s2, c2, ") (", s3, c3, ")"
	#print "origDir:" , cdir
	#print "newDir:" , ecdir
	#print
	#print "origUDir:" , cudir
	#print "newUDir:" , ecudir
	#me = Mesh.New('dirMesh')
	#scn.objects.new(me,'dirObj')
	#lineDir(me, camloc, cdir)
	#lineDir(me, camloc, cudir)
	#lineDir(me, cam.loc, ecdir)
	#lineDir(me, cam.loc, ecudir)
		
	newCamRot = cam_getLocalDir(ecdir, ecudir, cam.rot.copy())

	#xyz
	#eex = c1*c2*ex + (s1*c3-c1*s2*s3)*ey + (s1*s3 + c1*s2*c3)*ez
	#eey = -s1*c2*ex + (c1*c3 + s1*s2*s3)*ey + (c1*s3 - s1*s2*c3)*ez
	#eez = -s2*ex - c2*s3*ey + c2*c3*ez



	#cam.rot = camrot[0] - eex, camrot[1] - eey, camrot[2] - eez
	cam.rot = newCamRot
		
	mesh_object.loc = 0,0,0
	mesh_object.rot = 0,0,0
	
	#me.update()
	Blender.Window.EditMode(0)
	#me = 0
	
	print "finished : %.2f"  % (sys.time() - time_start)
	
	
def cameraMetrik(trackcam = 0, restrict = 1, mode = 0):
	global me, empties, mesh_object, origcam, origcam_pos, origcam_rot
	print "*** start ***"
	time_start = sys.time()
	Blender.Window.EditMode(0)
	
	print "mode", mode
	
		
	# Berechnen der Sichtlinien
	scn = Scene.GetCurrent()
	cam = scn.objects.camera
	camloc = cam.loc
	for v in empties:
		#print "*** empty"
		v.calcViewLine(camloc)
		
	# feststellen der Empties, die sich wirklich geändert haben
	realEmpties = []
	for v in empties:
		print v.empty.getName()
		#print v.empty.rot, v.origrot
		if 0.001 < distAA(v.empty.loc,v.orig):
			realEmpties.append(v)
	if (len(realEmpties) < 3 and mode == 0) or (len(realEmpties) < 2 and mode == 1):
		realEmpties = empties
		
		
	# Erstellen der Ableitungsmatrix
	le = len(realEmpties)
		
	# Zielwerte
	# für jede Sichtlinie gibt es eine Variable
	# für Translation drei weitere (x,y,z)
	# für Rotation drei weitere (x,y,z)
	# also insgesamt Anzahl Empties + 6
	erg = None
	if mode == 1:
		erg = [0] * (len(realEmpties) + 3)
	else:
		erg = [0] * (len(realEmpties) + 6)
		
	if trackcam == 0:
		if mode == 1:
			erg[le] = mesh_object.loc[0]
			erg[le+1] = mesh_object.loc[1]
			erg[le+2] = -mesh_object.rot[2]
		else:
			erg[le] = mesh_object.loc[0]
			erg[le+1] = mesh_object.loc[1]
			erg[le+2] = mesh_object.loc[2]
			erg[le+3] = -mesh_object.rot[2]
			erg[le+4] = mesh_object.rot[1]
			erg[le+5] = -mesh_object.rot[0]
	else:
		erg[le] = 0
		erg[le+1] = 0
		erg[le+2] = 0
		erg[le+3] = 0
		erg[le+4] = 0
		erg[le+5] = 0
		
	t = [0] * (le * 3)
	
	bb = True
	j = 200
	while bb:
		j -= 1
		if j < 1:
			bb = False
		m = matrix()
		if mode == 1:
			for i in range(le):
				ii = i * 3
				e = realEmpties[i]
			
				# Ableitungen der Richtungsgeraden der Sichtlinie
				m.set(ii,i,e.dir[0])
				m.set(ii+1,i,e.dir[1])
				m.set(ii+2,i,e.dir[2])
			
				# Ableitungen der Translation
				m.set(ii,le,1)
				m.set(ii+1,le+1,1)
			
				# jetzt kommt der Hammer, Rotation
				rz = erg[le+2]
				s1 = sin(rz)
				c1 = cos(rz)
				ex = e.orig[0]
				ey = e.orig[1]
				ez = e.orig[2]
			
				r1 = -s1*ex + c1*ey
				r2 = -c1*ex - s1*ey
				r3 = 0
				m.set(ii,le+2,r1)
				m.set(ii+1,le+2,r2)
				m.set(ii+2,le+2,r3)
		
			
				# Zielkoordinaten
				s = erg[i]
				tx = erg[le]
				ty = erg[le+1]
				tz = mesh_object.loc[2]
				# Rotation
				eex = c1*ex + s1*ey
				eey = -s1*ex + c1*ey
				eez = ez
			
				t[ii] =    e.empty.loc[0] - s*e.dir[0] - eex - tx
				t[ii+1] =  e.empty.loc[1] - s*e.dir[1] - eey - ty
				t[ii+2] =  e.empty.loc[2] - s*e.dir[2] - eez - tz
			
			
			#print "Differenz"
			#printArray(t)
			#print
			#print
		
			#print "Ableitung"
			#m.write()
			ma = m.t_copy().mul_matrix(m)
			#print
			#print "Ableitung zum Quadrat"
			#ma.write()
		
			#print
			#print "b Vektor"
			vb = m.t_copy().mul_vektor(t)
			#printArray(vb)
			#print
		
			#print "e Vektor"	
			ee = ma.gauss(vb)
			#printArray(ee)
			#print
		
			if restrict == 1:
				rez = ee[le+2]
				if 0.7 < rez:
					ee[le+2] = 0.7
				elif rez < -0.7:
					ee[le+2] = -0.7
		
				mx = ee[le]
				my = ee[le+1]
				if 1.0 < mx:
					ee[le] = 1.0
				elif mx < -1.0:
					ee[le] = -1.0
				if 1.0 < my:
					ee[le+1] = 1.0
				elif my < -1.0:
					ee[le+1] = -1.0
	
			error = 0
		
			for i in range (len(erg)):
				erg[i] += ee[i]
				error += abs(ee[i])

			#print "Ergebniss"
			#printArray(erg)
			#print
		
			#print "Fehler ", error
			if error < 1e-8:
				bb = False
	
			#print
			#print "******"
		
		else:
			for i in range(le):
				ii = i * 3
				e = realEmpties[i]
			
				# Ableitungen der Richtungsgeraden der Sichtlinie
				m.set(ii,i,e.dir[0])
				m.set(ii+1,i,e.dir[1])
				m.set(ii+2,i,e.dir[2])
			
				# Ableitungen der Translation
				m.set(ii,le,1)
				m.set(ii+1,le+1,1)
				m.set(ii+2,le+2,1)
			
				# jetzt kommt der Hammer, Rotation
				rx = erg[le+3]
				ry = erg[le+4]
				rz = erg[le+5]
				s1 = sin(rx)
				c1 = cos(rx)
				s2 = sin(ry)
				c2 = cos(ry)
				s3 = sin(rz)
				c3 = cos(rz)
				ex = e.orig[0]
				ey = e.orig[1]
				ez = e.orig[2]
			
				r11 = -s1*c2*ex + (c1*c3 + s1*s2*s3)*ey + (c1*s3 - s1*s2*c3)*ez
				r12 = -c1*s2*ex - c1*c2*s3*ey + c1*c2*c3*ez
				r13 = (-s1*s3 - c1*s2*c3)*ey + (s1*c3 - c1*s2*s3)*ez
				m.set(ii,le+3,r11)
				m.set(ii,le+4,r12)
				m.set(ii,le+5,r13)
			
				r21 = -c1*c2*ex + (-s1*c3 + c1*s2*s3)*ey + (-s1*s3 - c1*s2*c3)*ez
				r22 = s1*s2*ex + s1*c2*s3*ey - s1*c2*c3*ez
				r23 = (-c1*s3 + s1*s2*c3)*ey + (c1*c3 + s1*s2*s3)*ez
				m.set(ii+1,le+3,r21)
				m.set(ii+1,le+4,r22)
				m.set(ii+1,le+5,r23)
			
				r31 = 0
				r32 = -c2*ex + s2*s3*ey - s2*c3*ez
				r33 = -c2*c3*ey - c2*s3*ez
				m.set(ii+2,le+3,r31)
				m.set(ii+2,le+4,r32)
				m.set(ii+2,le+5,r33)
			
				# Zielkoordinaten
				s = erg[i]
				tx = erg[le]
				ty = erg[le+1]
				tz = erg[le+2]
				# Rotation
				eex = c1*c2*ex + (s1*c3-c1*s2*s3)*ey + (s1*s3 + c1*s2*c3)*ez
				eey = -s1*c2*ex + (c1*c3 + s1*s2*s3)*ey + (c1*s3 - s1*s2*c3)*ez
				eez = -s2*ex - c2*s3*ey + c2*c3*ez
			
				t[ii] =    e.empty.loc[0] - s*e.dir[0] - eex - tx
				t[ii+1] =  e.empty.loc[1] - s*e.dir[1] - eey - ty
				t[ii+2] =  e.empty.loc[2] - s*e.dir[2] - eez - tz
			
			
			#print "Differenz"
			#printArray(t)
			#print
			#print
		
			#print "Ableitung"
			#m.write()
			ma = m.t_copy().mul_matrix(m)
			#print
			#print "Ableitung zum Quadrat"
			#ma.write()
		
			#print
			#print "b Vektor"
			vb = m.t_copy().mul_vektor(t)
			#printArray(vb)
			#print
		
			#print "e Vektor"	
			ee = ma.gauss(vb)
			#printArray(ee)
			#print
		
			if restrict == 1:
				rex = ee[le+3]
				rey = ee[le+4]
				rez = ee[le+5]
				if 0.7 < rex:
					ee[le+3] = 0.7
				elif rex < -0.7:
					ee[le+3] = -0.7
				if 0.7 < rey:
					ee[le+4] = 0.7
				elif rey < -0.7:
					ee[le+4] = -0.7
				if 0.7 < rez:
					ee[le+5] = 0.7
				elif rez < -0.7:
					ee[le+5] = -0.7
		
				mx = ee[le]
				my = ee[le+1]
				mz = ee[le+2]
				if 1.0 < mx:
					ee[le] = 1.0
				elif mx < -1.0:
					ee[le] = -1.0
				if 1.0 < my:
					ee[le+1] = 1.0
				elif my < -1.0:
					ee[le+1] = -1.0
				if 1.0 < mz:
					ee[le+2] = 1.0
				elif mz < -1.0:
					ee[le+2] = -1.0
	
			error = 0
		
			for i in range (len(erg)):
				erg[i] += ee[i]
				error += abs(ee[i])

			#print "Ergebniss"
			#printArray(erg)
			#print
		
			#print "Fehler ", error
			if error < 1e-8:
				bb = False
	
			#print
			#print "******"
		
		#print "*******************"

    # Die Empties setzen
	
	for i in range(len(realEmpties)):
		#print "*** empty"
		#v.calcViewLine(camloc)
		#print erg[i]
		#printArray (empties[i].loc)
		#printArray (empties[i].empty.loc)
		realEmpties[i].empty.loc = realEmpties[i].empty.loc[0] - erg[i] * realEmpties[i].dir[0],  realEmpties[i].empty.loc[1] - erg[i] * realEmpties[i].dir[1], realEmpties[i].empty.loc[2] - erg[i] * realEmpties[i].dir[2] 
		
	
	if mode == 1:
		tx = erg[le]
		ty = erg[le+1]
		tz = mesh_object.loc[2]	
		rx = erg[le+2]
		ry = mesh_object.rot[1]
		rz = -mesh_object.rot[0]
	else:
		tx = erg[le]
		ty = erg[le+1]
		tz = erg[le+2]	
		rx = erg[le+3]
		ry = erg[le+4]
		rz = erg[le+5]
	#printArray(erg)
	#print

	#print tx,ty,tz
	#print rx,ry,rz

	mesh_object.loc = [tx,ty,tz]
	mesh_object.rot = [-rz,ry,-rx]
	
		
	#Blender.Window.EditMode(1)
	#me = 0
	print "finished : %.2f"  % (sys.time() - time_start)
		
		

#Draw.Register 
Register(draw, event, bevent)