set path [get3DEInstallPath]
source $path/user_data/tcl_archive/veclib.tcl

set tcl_precision 16

print3DEConsole "\nExport Backprojection Curve: Write 2D curve of the back-projected point into file.\n\n"

set fobj [getSelectedFobj]
if {$fobj==0}\
	{
	bell
	post3DEInfoRequester " \n    Error, there is no Sequence/Ref. Frame selected.    \n"
	return
	}

set pg [getSelectedPGroup]
if {$pg==0}\
	{
	bell
	post3DEInfoRequester " \n    Error, there is no Pointgroup selected.    \n"
	return
	}
	
set no_points [getNoPoints $pg]
set pg_type [getPGroupType $pg]
	
set filename [post3DEFileRequester "Export Tracking Curves..." "*"]

if {$filename!=""}\
	{
# Open specified file for writing
	set file [open $filename w+]
	
# Number of frames for the selected Sequence
	set no_frames [getFobjNoFrames $fobj]

# Camera of the selected Sequence
	set cam [getFobjCamera $fobj]

# Geomery of the Camera
	set focal [getCameraFocalLength $cam]
	set width [getCameraFBackWidth $cam]
	set height [getCameraFBackHeight $cam]
# Lens center offset
	set lcx [getCameraLensCenterX $cam]
	set lcy [getCameraLensCenterY $cam]

# Field of VIew size in pixel
	set fov_width [getFobjImageWidthFOV $fobj]
	set fov_height [getFobjImageHeightFOV $fobj]

# Check this, if you like
#	puts $file "Lens Center Offset X: $lcx"
#	puts $file "Lens Center Offset Y: $lcy"
#	puts $file "FOV Width: $fov_width"
#	puts $file "FOV Height: $fov_height"
	
	puts $file "// Backprojection of calculated points for frame object [getFobjName $fobj]"

# Do the export for all selected points
	for {set point [getFirstPoint $pg]} {$point!=0} {set point [getNextPoint $pg $point]}\
		{
		if {[getPointSelectionFlag $pg $point] == 1}\
			{
# Which point is considered
			puts $file "point [getPointName $pg $point]"

# Get position of the selected point for the selected point group
			set x [getPointCalcPosition3D $pg $point]

			for {set frame 1} {$frame <= $no_frames} {incr frame}\
				{
# Get camera position and rotation for this frame
				set pos [getPGroupPosition3D $pg $fobj $frame]
				set rot [getPGroupRotation3D $pg $fobj $frame]

# Case distiction: It's a camera point group
				if {$pg_type == "CAMERA"}\
					{
# Difference vector from the camera position to the point in 3d
					set d [vec3-vec3 $x $pos]
# Calculate the inverse of the camera matrix
					set rotinv [mat3trans $rot]
# Reformulate the Difference vector in the coordiante frame of the camera
					set dnew [mat3*vec3 $rotinv $d]
# Project this into the z=-1-Plane, that is: x' = x / -z, y' = y' -z (remember: the camera looks into -z direction)
					set d2 [vec2/s [list [vget $dnew 0] [vget $dnew 1]] [expr -[vget $dnew 2]]]
# Now we want to express this in pixel respective to the lower left corner of the field of view (dotted line)
# We scale it to metric units, remove the lens center offset, scale it down to dimensionless coordinates,
# so that (0,0) is the upper left corner of the field of view, and (1,1) is the lower right corner.
					set dfovx [expr ([vget $d2 0] * $focal - $lcx) / ($width) + .5]
					set dfovy [expr -([vget $d2 1] * $focal - $lcy) / ($height) + .5]
					}
# This doesn't work yet:
				if {$pg_type == "OBJECT"}\
					{
					set rotinv [mat3invert $rot]
					set d [mat3*vec3 $rotinv $x]
					set dnew [vec3+vec3 $d $pos]
					set d2 [vec2/s [list [vget $dnew 0] [vget $dnew 1]] [expr -[vget $dnew 2]]]
					set dfovx [expr ([vget $d2 0] * $focal - $lcx) / ($width) + .5]
					set dfovy [expr ([vget $d2 1] * $focal - $lcy) / ($height) + .5]
					}

# We want to have the result in Pixel
# Since 3DEs Tcl-Interface does not yet provide access to the Field of View offset, we cannot substract
# this offset here. All pixel positions are to be understood with respect to the field of view.
				set dx [expr $dfovx * $fov_width]
				set dy [expr $dfovy * $fov_height]

# Modify this according to your needs
				puts $file "$frame : $dx, $dy"
				}
			}
		}
	close $file
	}