Diff: main.cpp

Revision:

8:63eff653d0ba

Parent:

7:7f9abf427b06

Child:

9:ca52462f9ebc

--- a/main.cpp	Tue Apr 28 14:32:03 2015 +0000
+++ b/main.cpp	Wed Apr 29 15:12:57 2015 +0000
@@ -126,14 +126,16 @@
             debug.printf("%f, %f, %f, %f ", q1.w, q1.v.x, q1.v.y, q1.v.z);
             
             Vector3 vector_x(1,0,0),vector_y(0,1,0),vector_z(0,0,1);
-            Vector3 vector_x_local=q1.rotate(vector_x);
+            Vector3 pry=q1.getEulerAngles();
+            /*Vector3 vector_x_local=q1.rotate(vector_x);
             Vector3 vector_y_local=q1.rotate(vector_y);
             Vector3 vector_z_local=q1.rotate(vector_z);
             Vector3 direction_plane_normal=vector_z_local.cross_product(vector_z);
             Vector3 bar_projection=vector_z-vector_x_local*(vector_z.dot_product(vector_x_local)/vector_x_local.length_squared());
             Vector3 bar_projection_g=bar_projection-vector_z*(bar_projection.dot_product(vector_z)/vector_z.length_squared());
-            float angle=bar_projection_g.dot_product(bar_projection)/bar_projection.length()/bar_projection_g.length();
-            debug.printf("angle: %f \r\n ", angle/PI*180);
+            float angle=acos(vector_y_local.dot_product(vector_z)/vector_y_local.length()/vector_z.length());*/
+            //debug.printf("angle: %f  ", angle/PI*180);
+            debug.printf("p: %f r: %f y: %f \r\n ",pry.x/PI*180,pry.y/PI*180,pry.z/PI*180);
 
             //TeaPot Demo Packet for MotionFit SDK
             /*