From 9cabc1130c50bdf28a5fee4c986eb22ae06d06c3 Mon Sep 17 00:00:00 2001
From: Jens Ahrensfeld <jens@jayfield.org>
Date: Thu, 29 Jan 2015 19:59:16 +0000
Subject: [PATCH] - prepared for Fancy Camera

git-svn-id: http://moon:8086/svn/software/trunk/projects/FsTrack@169 b431acfa-c32f-4a4a-93f1-934dc6c82436
---
 Source/Kml.cpp | 90 ++++++++++++++++++++++++++++++++++++++++++++++++--
 Source/Kml.hpp |  1 +
 2 files changed, 88 insertions(+), 3 deletions(-)

diff --git a/Source/Kml.cpp b/Source/Kml.cpp
index dd2ec72..93c9526 100644
--- a/Source/Kml.cpp
+++ b/Source/Kml.cpp
@@ -4,11 +4,13 @@
 
 #include "Kml.hpp"
 
+//#define WITH_FANCY_CAMERA
 
 Kml::Kml(const char *pDocRoot)
 : kml(nullptr)
 , m_pPlanePlaceMark(nullptr)
 , m_pPathPlaceMark(nullptr)
+, m_pCameraPlaceMark(nullptr)
 , m_docRoot(pDocRoot)
 , m_altitudeModeString("absolute")
 , m_pathColorString("0xff00ffff")
@@ -42,7 +44,6 @@ void Kml::create()
 	kml = new XmlElement("kml");
 	XmlElement *pNetworkLinkControl = kml->createNewChildElement("NetworkLinkControl");
 	pNetworkLinkControl->createNewChildElement("LinkName")->addTextElement("FsTrack");
-	XmlElement *pCamera = pNetworkLinkControl->createNewChildElement("Camera");
 	XmlElement *pDoc = kml->createNewChildElement("Document");
 	XmlElement *pFile = kml->getChildByName("Document")->createNewChildElement("filename");
 	XmlElement *pSnippet = kml->getChildByName("Document")->createNewChildElement("Snippet");
@@ -62,6 +63,7 @@ void Kml::create()
 	pSnippet->addTextElement(time_str);
 
 	// Camera
+	XmlElement *pCamera = pNetworkLinkControl->createNewChildElement("Camera");
 	pCamera->createNewChildElement("longitude")->addTextElement("0");
 	pCamera->createNewChildElement("latitude")->addTextElement("0");
 	pCamera->createNewChildElement("altitude")->addTextElement("0");
@@ -77,6 +79,7 @@ void Kml::destroy()
 	kml = nullptr;
 	m_pPlanePlaceMark = nullptr;
 	m_pPathPlaceMark = nullptr;
+	m_pCameraPlaceMark = nullptr;
 }
 
 void Kml::setAltitudeMode(String const &modeString)
@@ -120,7 +123,6 @@ void Kml::setCameraTilt(float tilt_deg)
 
 void Kml::update(FlightData const &flightData)
 {
-	const ScopedLock sl (m_lock);
 	updatePlane(flightData);
 	updatePath(flightData);
 }
@@ -137,6 +139,8 @@ void Kml::setCameraAutoRoll(bool enable)
 
 void Kml::updatePlane(FlightData const &flightData)
 {
+	const ScopedLock sl (m_lock);
+
 	char temp_str[1024];
 
 	XmlElement *pDoc = kml->getChildByName("Document");
@@ -159,7 +163,7 @@ void Kml::updatePlane(FlightData const &flightData)
 
 	XmlElement *pCamera;
 
-	if(pPlacemark == NULL)
+	if(pPlacemark == nullptr)
 	{
 		// Placemark
 		pPlacemark = pDoc->createNewChildElement("Placemark");
@@ -244,20 +248,100 @@ void Kml::updatePlane(FlightData const &flightData)
 	
 		if (m_cameraAutoHeading)
 			pCamera->getChildByName("heading")->getFirstChildElement()->setText(String(flightData.hdgTrue_deg));
+		else
+			pCamera->getChildByName("heading")->getFirstChildElement()->setText("0");
 		
 		pCamera->getChildByName("tilt")->getFirstChildElement()->setText(String(m_cameraTilt));
 
 		if (m_cameraAutoRoll)
 			pCamera->getChildByName("roll")->getFirstChildElement()->setText(String(flightData.bank_deg));
+		else
+			pCamera->getChildByName("roll")->getFirstChildElement()->setText("0");
 
 		pCamera->getChildByName("altitudeMode")->getFirstChildElement()->setText(m_altitudeModeString);
 
 	}
 
+#ifdef WITH_FANCY_CAMERA
+	if (m_pCameraPlaceMark == nullptr)
+	{
+
+		// Placemark
+		m_pCameraPlaceMark = pDoc->createNewChildElement("Placemark");
+		m_pCameraPlaceMark->setAttribute("id", "Cam");
+		m_pCameraPlaceMark->createNewChildElement("name")->addTextElement("Camera");
+		pPoint = m_pCameraPlaceMark->createNewChildElement("Point");
+		pPoint->setAttribute("id", "CamPoint");
+		sprintf(temp_str, "%9.6f,%9.6f,%9.6f", flightData.lon,  flightData.lat,  flightData.alt_meter);
+		pPoint->createNewChildElement("coordinates")->addTextElement(temp_str);
+		pPoint->createNewChildElement("altitudeMode")->addTextElement(m_altitudeModeString);
+
+		// Name
+		pName = m_pCameraPlaceMark->createNewChildElement("name");
+		pName->addTextElement("Loxo");
+
+		// Style
+		pStyle = m_pCameraPlaceMark->createNewChildElement("Style");
+		
+		// LineStyle
+		XmlElement *pLineStyle = pStyle->createNewChildElement("LineStyle");
+		XmlElement *pLineColor = pLineStyle->createNewChildElement("color");
+		pLineColor->addTextElement("0xff0000ff");
+
+		XmlElement *pLineWidth = pLineStyle->createNewChildElement("width");
+		pLineWidth->addTextElement("1");
+
+		// LineString
+		XmlElement *pLineString = m_pCameraPlaceMark->createNewChildElement("LineString");
+		pExtrude = pLineString->createNewChildElement("extrude");
+		pExtrude->addTextElement("1");
+		pTesselate = pLineString->createNewChildElement("tesselate");
+		pTesselate->addTextElement("1");
+		pAltitudeMode = pLineString->createNewChildElement("altitudeMode");
+		pAltitudeMode->addTextElement("clampToGround");
+		pCoords = pLineString->createNewChildElement("coordinates");
+	}
+	else
+	{
+		const double pi = 3.14159265359;
+		double r = flightData.alt_meter;
+		double theta = flightData.lat/180*pi;		// 0 = Equator
+		double phi = flightData.lon/180*pi;		// 0 = Greenwich
+		double hdg = flightData.hdgTrue_deg/180*pi;
+
+
+
+		sprintf(temp_str, "%9.6f,%9.6f,%9.6f", phi/pi*180,  theta/pi*180,  r);
+		pCoords = m_pCameraPlaceMark->getChildByName("Point")->getChildByName("coordinates");
+		pCoords->deleteAllTextElements();
+		pCoords->addTextElement(temp_str);
+		m_pCameraPlaceMark->getChildByName("Point")->getChildByName("altitudeMode")->getFirstChildElement()->setText("clampToGround");
+
+		// Coordinates
+		pCoords = m_pCameraPlaceMark->getChildByName("LineString")->getChildByName("coordinates");
+		if (!pCoords)
+		{
+			pCoords = m_pCameraPlaceMark->getChildByName("LineString")->createNewChildElement("coordinates");
+		}
+		pCoords->deleteAllTextElements();
+
+		double k = 1.0/tan(hdg);
+		for (int i=0; i < 100; i++)
+		{
+
+			sprintf(temp_str, "%9.6f,%9.6f,%9.6f\n", phi/pi*180,  theta/pi*180,  r);
+			pCoords->addTextElement(temp_str);
+			theta = asin(tanh(k*phi)) + flightData.lat/180*pi;
+			phi += pi/10000;
+		}
+	}
+#endif
 }
 
 void Kml::updatePath(FlightData const &flightData)
 {
+	const ScopedLock sl (m_lock);
+
 	char temp_str[1024];
 
 	XmlElement *pDoc = kml->getChildByName("Document");
diff --git a/Source/Kml.hpp b/Source/Kml.hpp
index c7293be..634e921 100644
--- a/Source/Kml.hpp
+++ b/Source/Kml.hpp
@@ -23,6 +23,7 @@ private:
 	ScopedPointer<XmlElement> kml;
 	XmlElement *m_pPlanePlaceMark;
 	XmlElement *m_pPathPlaceMark;
+	XmlElement *m_pCameraPlaceMark;
 	char m_path[1024];
 	const char *m_docRoot;
 	String m_altitudeModeString;