Commits

Anonymous committed 54ea1f6 Merge

Merge

Comments (0)

Files changed (1)

QuadTrack/OrientationFinder.cpp

 #include "OrientationFinder.h"
 #include <math.h>
 #include <vector>
-
+#include "Coordinate.h"
+#include <complex>
 OrientationFinder::OrientationFinder(){
-	this->numMarkers = 5;
+	this->numMarkers = 4;
 }
 
 //Finds the orientation of the quad-rotor from four frames
 	}
 	vector<double> values;
 	if(markerX[0] > markerX[1] && markerX[0] > markerX[2] && markerX[0] > markerX[3]){
-		values = yawCalculationPartial(3, markerX, 90); //NTS: Change value to come from calibration data
+		values = yawCalculationPartial(1, markerX, 90); //NTS: Change value to come from calibration data
 	}
 	else if(markerX[1] > markerX[0] && markerX[1] > markerX[2] && markerX[1] > markerX[3]){
-		values = yawCalculationPartial(3, markerX, 0);
+		values = yawCalculationPartial(2, markerX, 0);
 	}
-	else if(markerX[2] > markerX[1] && markerX[2] > markerX[2] && markerX[0] > markerX[3]){
+	else if(markerX[2] > markerX[1] && markerX[2] > markerX[0] && markerX[2] > markerX[3]){
 		values = yawCalculationPartial(3, markerX, 180);
 	}
 	else if(markerX[3] > markerX[1] && markerX[3] > markerX[2] && markerX[3] > markerX[0]){
-		values = yawCalculationPartial(3, markerX, 270);
+		values = yawCalculationPartial(0, markerX, 270);
 	}
 
 
 vector<double> OrientationFinder::yawCalculationPartial(int middleFront, int* xPositions, double cameraOffset){
 	int left = 0;
 	int right = 0;
+	int xPosT [4] = {xPositions[0],xPositions[1],xPositions[2],xPositions[3]};
 	if(middleFront == 3){
-		right = 0;
-		left = 2;
+		right = 2;
+		left = 0;
 	}
-	else if(middleFront = 0){
-		right = 1;
-		left = 3;
+	else if(middleFront == 0){
+		right = 3;
+		left = 1;
 	}
 	else{
 		right = middleFront + 1;
 	
 	double dist = xPositions[right] - xPositions[left];
 	double middlePoint = xPositions[middleFront] - xPositions[left];
-	double normMiddle = (middlePoint - dist/2)/dist;
-	double angle = cameraOffset + normMiddle*45;
+	double adjacent = -1+middlePoint/(dist/2);
+	double angle= (180/3.141)*asin(sqrt((double)2)/2*adjacent);
 
 	retVector.push_back(angle);
 	retVector.push_back(dist);
 	
 	return returnPitchRoll;
 }
+
+
+ int main(){
+
+	 Coordinate2D coord1 (152,229);
+	 Coordinate2D coord2 (4,208);
+	 Coordinate2D coord3 (219,242);
+	 Coordinate2D coord4 (196,274);
+	/* Coordinate2D coord1 (100,229);
+	 Coordinate2D coord2 (101,208);
+	 Coordinate2D coord3 (201,242);
+	 Coordinate2D coord4 (200,274);*/
+	 Marker mark1 (coord1, Marker::Type::Empty);
+	 Marker mark2 (coord2, Marker::Type::Empty);
+	 Marker mark3 (coord3, Marker::Type::Empty);
+	 Marker mark4 (coord4, Marker::Type::Empty);
+	 vector<Marker> points;
+	 points.push_back(mark1);
+	 points.push_back(mark2);
+	 points.push_back(mark3);
+	 points.push_back(mark4);
+
+	 OrientationFinder* orient = new OrientationFinder();
+	 orient->yawCalculationTotal(points,0);
+ }