|
4
|
1 #include <math.h>
|
|
|
2 #include <stdlib.h>
|
|
|
3 #include <stdio.h>
|
|
|
4 #include "SceneGraphRoot.h"
|
|
|
5 #include "MainLoop.h"
|
|
|
6
|
|
|
7 #include <XnOpenNI.h>
|
|
|
8 #include <XnCppWrapper.h>
|
|
|
9 #include <XnVNite.h>
|
|
|
10
|
|
|
11 #include "kinect.h"
|
|
|
12
|
|
|
13 #define INIT_XML_PATH "./kinect.xml"
|
|
|
14
|
|
|
15 void checkRC(const XnStatus &rc, const char *what) {
|
|
|
16 if (rc != XN_STATUS_OK) {
|
|
|
17 printf("%s faild: %s\n", what, xnGetStatusString(rc));
|
|
|
18 }
|
|
|
19 }
|
|
|
20
|
|
|
21 void checkErrors(XnStatus &rc, xn::EnumerationErrors &errors, const char *what) {
|
|
|
22 if (rc == XN_STATUS_NO_NODE_PRESENT) {
|
|
|
23 XnChar strError[1024];
|
|
|
24 errors.ToString(strError, 1024);
|
|
|
25 printf("%s\n", strError);
|
|
|
26 }
|
|
|
27 }
|
|
|
28
|
|
|
29 SessionState NIState::gSessionState = NOT_IN_SESSION;
|
|
|
30 XnBool NIState::gBDrawDepthMap = true;
|
|
|
31 xn::Context NIState::gContext;
|
|
|
32 XnVSessionManager *NIState::gPSessionManager;
|
|
|
33 XnVPointDrawer *NIState::gPDrawer;
|
|
|
34
|
|
|
35 void NIState::XN_CALLBACK_TYPE sessionStarting(const XnPoint3D &ptPosition, void *userCxt) {
|
|
|
36 printf("Session start: (%f, %f, %f)\n", ptPosition.X, ptPosition.Y, ptPosition.Z);
|
|
|
37 gSessionState = IN_SESSION;
|
|
|
38 }
|
|
|
39 void NIState::XN_CALLBACK_TYPE sessionEnding(void *userCxt) {
|
|
|
40 printf("Session end\n");
|
|
|
41 gSessionState = NOT_IN_SESSION;
|
|
|
42 }
|
|
|
43 void NIState::XN_CALLBACK_TYPE focusProgress(const XnChar *strFocus, const XnPoint3D &ptPosition,
|
|
|
44 XnFloat fProgress, void *userCxt) {
|
|
|
45 printf("Focus progress: %s @(%f, %f, %f): %f\n", strFocus, ptPosition.X, ptPosition.Y, ptPosition.Z, fProgress);
|
|
|
46 }
|
|
|
47 void NIState::XN_CALLBACK_TYPE noHands(void *UserCxt) {
|
|
|
48 if (gSessionState != NOT_IN_SESSION) {
|
|
|
49 printf("Quick refocus\n");
|
|
|
50 gSessionState = QUICK_REFOCUS;
|
|
|
51 }
|
|
|
52 }
|
|
|
53
|
|
|
54 // prototype
|
|
|
55 static void ball_move(SceneGraphPtr node, void *sgroot_, int screen_w, int screen_h);
|
|
|
56 static void null_collision(SceneGraphPtr node, void *sgroot_, int screen_w, int screen_h, SceneGraphPtr tree);
|
|
|
57
|
|
|
58 static void
|
|
|
59 ball_move(SceneGraphPtr node, void *sgroot_, int screen_w, int screen_h)
|
|
|
60 {
|
|
|
61 NIState::gContext.WaitNoneUpdateAll();
|
|
|
62 NIState::gPSessionManager->Update(&NIState::gContext);
|
|
|
63 float x, y, z;
|
|
|
64 int error = NIState::gPDrawer->getPosition(x, y, z);
|
|
|
65 if (error > 0) {
|
|
|
66 node->xyz[0] = x/640 * screen_w;
|
|
|
67 node->xyz[1] = y/480 * screen_h;
|
|
|
68 node->xyz[2] = z - 500;
|
|
|
69 }
|
|
|
70 }
|
|
|
71
|
|
|
72 static void
|
|
|
73 null_collision(SceneGraphPtr node, void *sgroot_, int screen_w, int screen_h,
|
|
|
74 SceneGraphPtr tree)
|
|
|
75 {
|
|
|
76 }
|
|
|
77
|
|
|
78 MainLoopPtr
|
|
|
79 Kinect::init(Viewer *sgroot, int screen_w, int screen_h)
|
|
|
80 {
|
|
|
81 SceneGraphPtr ball;
|
|
|
82
|
|
|
83 srandom(100);
|
|
|
84
|
|
|
85 sgroot->createFromXMLfile("xml_file/Ball.xml");
|
|
|
86
|
|
|
87 ball = sgroot->createSceneGraph("Ball");
|
|
|
88 ball->set_move_collision(ball_move, null_collision);
|
|
|
89
|
|
|
90 ball->xyz[0] = screen_w/2;
|
|
|
91 ball->xyz[1] = screen_h/2;
|
|
|
92 ball->xyz[2] = 550.0f;
|
|
|
93
|
|
|
94 sgroot->setSceneData(ball);
|
|
|
95
|
|
|
96 xn::EnumerationErrors errors;
|
|
|
97 XnStatus rc = NIState::gContext.InitFromXmlFile(INIT_XML_PATH, &errors);
|
|
|
98 checkErrors(rc, errors, "InitFromXMLFile");
|
|
|
99 checkRC(rc, "InitFromXMLFile");
|
|
|
100
|
|
|
101 xn::DepthGenerator gDepthGenerator;
|
|
|
102 rc = NIState::gContext.FindExistingNode(XN_NODE_TYPE_DEPTH, gDepthGenerator);
|
|
|
103 checkRC(rc, "Find depth generator");
|
|
|
104
|
|
|
105 xn::HandsGenerator gHandsGenerator;
|
|
|
106 rc = NIState::gContext.FindExistingNode(XN_NODE_TYPE_HANDS, gHandsGenerator);
|
|
|
107 checkRC(rc, "Find hands generator");
|
|
|
108 NIState::gPSessionManager = new XnVSessionManager();
|
|
|
109 rc = NIState::gPSessionManager->Initialize(&NIState::gContext, "Click,Wave", "RaiseHand");
|
|
|
110 checkRC(rc, "SessionManager::Initialize");
|
|
|
111
|
|
|
112 NIState::gPSessionManager->RegisterSession(NULL, NIState::sessionStarting, NIState::sessionEnding, NIState::focusProgress);
|
|
|
113
|
|
|
114 NIState::gPDrawer = new XnVPointDrawer(20, gDepthGenerator);
|
|
|
115 XnVFlowRouter *gPFlowRouter = new XnVFlowRouter;
|
|
|
116 gPFlowRouter->SetActive(NIState::gPDrawer);
|
|
|
117
|
|
|
118 NIState::gPSessionManager->AddListener(gPFlowRouter);
|
|
|
119
|
|
|
120 NIState::gPDrawer->RegisterNoPoints(NULL, NIState::noHands);
|
|
|
121 NIState::gPDrawer->setDepthMap(NIState::gBDrawDepthMap);
|
|
|
122
|
|
|
123 rc = NIState::gContext.StartGeneratingAll();
|
|
|
124 checkRC(rc, "StartGenerating");
|
|
|
125
|
|
|
126 return sgroot;
|
|
|
127 }
|
|
|
128
|
|
|
129 extern Application *
|
|
|
130 application() {
|
|
|
131 return new Kinect();
|
|
|
132 }
|
|
|
133
|
|
|
134 const char *usr_help_str = "Usage: ./kinect [OPTION]\n";
|
|
|
135
|
|
|
136 extern int init(TaskManager *manager, int argc, char *argv[]);
|
|
|
137 extern void task_initialize();
|
|
|
138 static void TMend(TaskManager *manager);
|
|
|
139
|
|
|
140 int
|
|
|
141 TMmain(TaskManager *manager, int argc, char *argv[])
|
|
|
142 {
|
|
|
143 task_initialize();
|
|
|
144 manager->set_TMend(TMend);
|
|
|
145 return init(manager, argc, argv);
|
|
|
146 }
|
|
|
147
|
|
|
148 void
|
|
|
149 TMend(TaskManager *manager)
|
|
|
150 {
|
|
|
151 printf("kinect end\n");
|
|
|
152 }
|
|
|
153
|
|
|
154 /* end */
|
