Alvaro Cassinelli
Laser Sensing Display for UI interfaces in the real world
Fork of
skinGames_forktest
by 
Alvaro Cassinelli
Embed:
(wiki syntax)
Show/hide line numbers
main.cpp
00001 /* 00002 00003 - NOTE: I could instantiate at least 960 points when NOT USING THE ETHERNET LIBRARY! (as 30 objects containing each 32 points). 00004 ... but then again I could not make only 36 1-point object on a grid? what's going on? an object takes MUCH MORE MEMORY than just 00005 the trajectory and 3d vector array... but where? 00006 Anyway, using the ETHERNET LIBRARY uses more than 20kB, and then I cannot instantiate more than a few letters... 00007 */ 00008 00009 //#define WITH_OSC // this consumes about 20kb or RAM!!! 00010 00011 #include "mbed.h" 00012 #include "WrapperFunctions.h" 00013 00014 #ifdef WITH_OSC 00015 #include "mbedOSC.h" 00016 #endif 00017 00018 // The following is because I still did not "wrap" ALL the LaserRenderer methods (so we have to use the object lsr directly, for instance to set a matrix: lsr.setIdentityPose()...) 00019 #include "LaserRenderer.h" 00020 extern LaserRenderer lsr; 00021 00022 // mbed IP address (server): 00023 #ifdef WITH_OSC 00024 #ifdef DHCP 00025 EthernetNetIf eth; 00026 #else 00027 EthernetNetIf eth( 00028 IpAddr(10,0,0,2), //IP Address of the mbed 00029 IpAddr(255,255,255,0), //Network Mask 00030 IpAddr(10,0,0,1), //Gateway 00031 IpAddr(10,0,0,1) //DNS 00032 ); 00033 #endif 00034 00035 //uint8_t serverMac[] = { 0xDE, 0xAD, 0xBE, 0xEF, 0xFE, 0xED }; 00036 uint8_t serverIp[] = { 10, 0, 0, 2 }; 00037 int serverPort = 10000; 00038 00039 //uint8_t destIp[] = {10, 0, 0, 3}; 00040 uint8_t destIp[] = {255, 255, 255, 255}; // broadcast, so we can use several computers for sound, etc 00041 int destPort = 12000; 00042 00043 char *topAddress="/mbed"; 00044 char *subAddress[3]= { "/test1" , "/test2" , "/test3" }; 00045 00046 OSCMessage recMes; 00047 OSCMessage sendMes; 00048 00049 OSCClass osc; 00050 //OSCClass osc(&recMes); // instantiate OSC communication object, and set the receiver container from the OSC packets 00051 #endif 00052 00053 // for tests: 00054 extern Scene scene; 00055 extern laserSensingDisplay lsd; 00056 00057 // ======================================================= 00058 00059 // Timers: 00060 Timer t; // we can read in ms or us 00061 00062 // Tickers: 00063 //Ticker timerForRendering; // now in the WrapperFunctions.cpp file 00064 //Ticker timerForSendingData; // better use a timer, so as not to interrupt the exact laser display ticker 00065 unsigned long lastTimeCreated; 00066 unsigned long lastTimeSensed; 00067 00068 // ====================================================================================================================== 00069 // DEFAULT CALIBRATION MATRICES (Extrinsics and Intrinsics). 00070 // -> would be better to load them from a system file!! (TO DO) 00071 // Note: last row is completed automatically in the loadExtrinsicMatrix and loadProjMatrix methods 00072 // Extrinsics of usb camera to projector (note: calibration was in cm, so the translation vector is in cm) 00073 float E1[12] = {9.8946330287356954e-01, -5.0979372852926171e-03, -1.4469410251272136e-01, 5.8356271792647112e+00, 00074 -4.7536907255693065e-03, 9.9769720674821438e-01, -6.7658599389111715e-02, -6.0821695799283804e+00, 00075 1.4470582120637832e-01, 6.7633532232509466e-02, 9.8716053943963022e-01, -8.2545447546057940e+00 00076 }; 00077 00078 // 9.9540692993883650e-01, -8.5578652572728450e-03, -9.5350966287597705e-02, 2.2256570914518332e+00, 00079 // 1.4872282274740087e-02, 9.9772789680840523e-01, 6.5710418886328711e-02, -6.9310510316834923e+00, 00080 // 9.4571978141945845e-02, -6.6826692814433777e-02, 9.9327253766416224e-01, 1.0699927478132011e+01 00081 //}; 00082 00083 00084 // INTRINSICS of laser projector: 00085 float K1[6] = { 5.9957148864529627e+03, 0.0000000e+000, 2.8869455045867635e+03, 00086 0.0000000e+000, 5.9647451141521778e+03, 2.9005238051336592e+03 00087 }; 00088 00089 // 7.0776317995422105e+03, 0., 2.4203185896583996e+03, 00090 // 0., 7.2889265345161484e+03, 1.7718110988625751e+03}; 00091 00092 // SCALE FACTOR for the SCAN: 00093 float scaleFactorProjector1 = 1.0; //scale factor for the projector image (the scan is, say, 600x600, but the resolution is 4096x4096) 00094 00095 // ====================================================================================================================== 00096 // LASER TERMINAL: 00097 // We assume that the "terminal" is operating on an a4 page. Units are in cm. 00098 V2 cursorPosition(0,0); 00099 00100 void createTextScene(); 00101 void addText(string, int, float, float); 00102 00103 float angle; 00104 string textToDisplay = "O"; 00105 float fontWidth = 2.5, fontHeight = 2.5; // if calibration is in cm, then this is in cm. 00106 00107 // ============================================================================================================== 00108 // Some global functions and variables (find a better way to integrate this!) 00109 extern "C" void mbed_reset(); 00110 00111 void createSceneTest(); 00112 void setOrthographicView(); 00113 00114 #ifdef WITH_OSC 00115 void processOSC(UDPSocketEvent e); 00116 #endif 00117 void interpretCommand(); 00118 void processSerial(); 00119 00120 int touchedTimes = 0; 00121 enum sensingModes {NO_SENSING, SENSING_WHOLE_SCENE, SENSING_PER_OBJECT}; 00122 sensingModes currentSensingMode=SENSING_PER_OBJECT;//NO_SENSING; 00123 00124 // ================= AUXILIARY VARIABLES FOR INTERPRETING COMMAND FROM SERIAL OR OSC ========================= 00125 // NOTE: the following arrays are GLOBAL (used in processOSC and processSerial, as well as in interpretCommand function): 00126 // max of two addresses (top and sub), of a max length of 24 characters: 00127 char address[2][24]; 00128 //long auxdata[2]; // to store a max of two arguments (note: we will only use LONGs) 00129 int data[2]; // this is to have -1 as NO DATA, to detect errors. 00130 int numArgs; 00131 00132 // An auxiliary buffer to store parameters data, or matrices, or points to load as vertices. 00133 // Note: for matrices (pose, projection), the maximum size we need is 12 floats, but as we will also receive vertices, let's give enough space (assuming 00134 // we can send 3d points, this means 3 floats per point; let's limit the number of points per object to 128 (a lot) 00135 float auxDataBuffer[128]; 00136 int auxDataBuffer_index=0; 00137 00138 // Commands: 00139 string command1, command2; 00140 00141 // this is a hack for the time being: 00142 int objectID; 00143 int xx, yy; // initial position of object or displacement 00144 bool firstTimeDisplay=true; 00145 00146 // ============================================================================================================== 00147 int main() 00148 { 00149 00150 t.start(); 00151 lastTimeCreated = t.read_ms(); 00152 lastTimeSensed = t.read_ms(); 00153 angle = 0; 00154 00155 // Setup: 00156 // (1) Hardware init (laser powers, positions...): 00157 IO.init(); 00158 00159 #ifdef WITH_OSC 00160 // OSC initialization: 00161 // Set the Ethernet port: 00162 printf("Setting up...\r\n"); 00163 EthernetErr ethErr = eth.setup(); 00164 if (ethErr) { 00165 printf("Error %d in setup.\r\n", ethErr); 00166 return -1; 00167 } 00168 printf("Setup OK\r\n"); 00169 00170 //(1) Sending message: 00171 // Set IP and Port: 00172 sendMes.setIp( destIp ); 00173 sendMes.setPort( destPort ); 00174 // Set data: 00175 sendMes.setTopAddress(topAddress); 00176 00177 //setting osc functionnality: 00178 //(2) Receiving: 00179 recMes.setIp( serverIp ); // not needed? 00180 osc.setReceiveMessage(&recMes); // this sets the receiver container for the OSC packets (we can avoid doing this if we use osc.getMessage() to get messages) 00181 osc.begin(serverPort, &processOSC); // binds the upd (osc) messages to an arbitrary listening port ("server" port), and callback function 00182 wait(1); 00183 #endif 00184 00185 //=================================================================== 00186 00187 /* 00188 //EXAMPLE 1: simple geometry ====================================================== 00189 // (1) Calibration matrices (Projection, and extrinsics - if needed): 00190 // Typically, the projection matrix is set from the PC side, or loaded by default (from file system). 00191 lsr.loadProjMatrix(K1, scaleFactorProjector1); // we could use a wrapper, but I won't for the time being. 00192 00193 // (2) Create the scene in "local" coordinates: 00194 createSceneTest(); // Create a default scene (for tests) 00195 00196 // (3) Set a default GLOBAL POSE and PROJECTION MATRIX (if we changed it) for rendering and start displaying: 00197 // - Using the current K (that correct for the center of the camera too!): 00198 //lsr.setIdentityPose(); 00199 //lsr.translate(0, 0, 2000); 00200 // - or doing orthoprojection: 00201 lsr.setOrthoProjection(); 00202 lsr.setIdentityPose(); 00203 lsr.translate(CENTER_AD_MIRROR_X, CENTER_AD_MIRROR_Y, 0); 00204 lsr.rotateZ(45); 00205 drawScene(); 00206 00207 // *** AND IMPORTANT *** the FIRST time, we need to set the display engine (ie, attach the interrupt). This is not needed afterwards, ONLY if we change the scene. 00208 lsr.startDisplay(); 00209 00210 // (4) Main loop: 00211 // Change the POSE when necessary (also called "viewing transformation" or "global modelview" in OpenGL jargon), render and display: 00212 while (1) { 00213 00214 if (pc.readable()) 00215 processSerial(); // here, the pose can be changed by a pc command 00216 hardwareKnobs(); 00217 00218 if (0) { //((t.read_ms()-lastTimeCreated)>10) { 00219 // for tests: 00220 myLed2 = !myLed2; 00221 00222 angle += 1; 00223 if (angle > 360) 00224 angle = 0; 00225 00226 // Set the current global pose: 00227 // lsr.setIdentityPose(); // we set the identity - then we don't need to do push pop... we start from "scratch" here. Not incremental rotation... 00228 // lsr.translate(0, 0, 1000); 00229 // lsr.rotateZ(angle); 00230 // lsr.rotateX(2.5 * angle); 00231 00232 lsr.setOrthoProjection(); 00233 lsr.setIdentityPose(); 00234 lsr.translate(CENTER_AD_MIRROR_X, CENTER_AD_MIRROR_Y, 0); 00235 lsr.rotateZ(angle); 00236 // lsr.rotateY( angle); 00237 00238 // Then, render (and display) the whole scene (note: we DEFINITELY don't want to call this all the time, only when the RT matrix changes) 00239 drawScene(); 00240 00241 lastTimeCreated = t.read_ms(); 00242 } // end auto-change pose loop 00243 00244 if ((t.read_ms() - lastTimeSensed) > 50) { // check data sensing every xx ms 00245 // Check objects and change colors or other properties as a function of the touch: 00246 // NOTE: the testing #define debugDelayMirrors should not be defined in the laserSensingDisplay.h 00247 // (note: we assume we are drawing the text). 00248 // There are two ways to query for an object input: using the ID, or from its pointer (scene->objectArray[i]) if we know it. This is faster. 00249 for (int i = 0; i < scene.totalObjects(); i++) { 00250 BaseObject* ptrObj = scene.objectArray[i]; 00251 if (ptrObj->sense()) { // this calls to ptrObj->displaySensingBuffer.processSensedData() 00252 ptrObj->setColor(0x02); 00253 00254 // char str[15]; 00255 // sprintf(str, "%d", touchedTimes); 00256 // textToDisplay=string(str);//string(itoa(touchedTimes)); 00257 // touchedTimes++; 00258 // createTextScene(); 00259 00260 } else 00261 ptrObj->setColor(0x01); 00262 } 00263 lastTimeSensed = t.read_ms(); 00264 } // end sensing loop 00265 00266 } // "end" infite loop 00267 // ================================= 00268 */ 00269 00270 // EXAMPLE 2: 3d text ====================================================== 00271 00272 // (1) Calibration matrices: 00273 // Typically, the projection and extrinsic matrix is set from the PC side, or loaded by default (from file system). 00274 lsr.loadProjMatrix(K1, scaleFactorProjector1); // we could use a wrapper, but I won't for the time being. 00275 lsr.loadExtrinsicsMatrix(E1); 00276 00277 // (2) Create the scene in "local" coordinates: 00278 createTextScene(); // Clear scene, and create a default scene (for tests), and update scene (but not yet rendered) 00279 00280 // (3) Set a default PERSPECTIVE and GLOBAL POSE for start displaying: 00281 lsr.loadProjMatrix(K1, scaleFactorProjector1); 00282 lsr.setIdentityPose(); 00283 lsr.multPoseMatrix(lsr.EXTRINSICS); // RT=E // we can simply do: lsr.setExtrinsicsPose() 00284 00285 // DEFAULT position in CAMERA coordinates (calibration was in cm): 00286 lsr.translate(0, 0, 130); 00287 00288 drawScene(); // this will be called when we change the pose matrix - but needs to be called at least once to start displaying 00289 updateScene(); // this is important: this will compute the number of objects, points, etc, to prepare the displaying engine (but rendering is NOT yet done, 00290 // rendering is done in the drawScene() function. 00291 // *** AND IMPORTANT *** the FIRST time, we need to set the display engine (ie, attach the interrupt). This is not needed afterwards, ONLY if we change the scene. 00292 00293 startDisplay(); 00294 00295 // Add more text (testing): 00296 // textToDisplay="1"; 00297 // addText(); 00298 00299 // (3) Set a default PERSPECTIVE and GLOBAL POSE for start displaying: 00300 // lsr.loadProjMatrix(K1, scaleFactorProjector1); 00301 // lsr.setIdentityPose(); 00302 // lsr.multPoseMatrix(lsr.EXTRINSICS); // RT=E // we can simply do: lsr.setExtrinsicsPose() 00303 // DEFAULT position in CAMERA coordinates (calibration was in cm): 00304 // lsr.translate(0, 0, 130); 00305 00306 // drawScene(); 00307 // updateScene(); 00308 00309 // (4) Main loop: 00310 while(true) { 00311 00312 // =========== Query hardware events (knobs, ethernet, serial port) ============ 00313 #ifdef WITH_OSC 00314 Net::poll(); // This polls the network connection for new activity, without keeping on calling this you won't receive any OSC! 00315 #endif 00316 00317 if (pc.readable()) processSerial(); // here, the pose can be changed by a pc command 00318 hardwareKnobs(); 00319 // ============================================================================= 00320 00321 // Automatic change of pose for tests 00322 if (0) {//(t.read_ms()-lastTimeCreated)>20) { 00323 angle+=.2; 00324 if (angle>360) angle=0; 00325 00326 lsr.setIdentityPose(); // we set the identity - then we don't need to do push pop... we start from "scratch" here. Not incremental rotation... 00327 lsr.flipY(); 00328 lsr.translate(0,0,1000); 00329 lsr.rotateZ(angle); 00330 //lsr.rotateX(angle); 00331 00332 drawScene(); 00333 00334 lastTimeCreated=t.read_ms(); 00335 } 00336 00337 if ((currentSensingMode!=NO_SENSING)&&((t.read_ms()-lastTimeSensed)>70)) { // check data sensing every xx ms 00338 // Check objects and change colors or other properties as a function of the touch: 00339 // NOTE: the testing #define debugDelayMirrors should not be defined in the laserSensingDisplay.h 00340 // (note: we assume we are drawing the text). 00341 // There are two ways to query for an object input: using the ID, or from its pointer (scene->objectArray[i]) if we know it. This is faster. 00342 switch(currentSensingMode) { 00343 case SENSING_WHOLE_SCENE: // Sensing whole scene ---------- 00344 if (scene.sense()) { 00345 00346 // COUNTING: 00347 // char str[15]; 00348 // sprintf(str, "%d", touchedTimes); 00349 // textToDisplay=string(str);//string(itoa(touchedTimes)); 00350 // touchedTimes++; 00351 // createTextScene(); 00352 // (3) Set a default PERSPECTIVE and GLOBAL POSE for start displaying: 00353 // lsr.loadProjMatrix(K1, scaleFactorProjector1); 00354 // lsr.setIdentityPose(); 00355 // lsr.multPoseMatrix(lsr.EXTRINSICS); // RT=E // we can simply do: lsr.setExtrinsicsPose() 00356 // This means that the current coordinate frame is the CAMERA, and calibration was in cm: 00357 // lsr.translate(0, 0, 100); 00358 ////lsr.flipY(); 00359 // drawScene(); // this will be called when we change the pose matrix - but needs to be called at least once to start displaying 00360 // } 00361 } 00362 break; 00363 case SENSING_PER_OBJECT: // Per-object sensing and action: 00364 for (int i=0; i<scene.totalObjects(); i++) { 00365 BaseObject* ptrObj=scene.objectArray[i]; 00366 if (ptrObj->sense()) { // this calls to ptrObj->displaySensingBuffer.processSensedData() 00367 ptrObj->setColor(0x02); // make it green 00368 00369 // for fun: let's rotate THIS letter: 00370 //lsr.pushPoseMatrix(); 00371 //lsr.setIdentityPose(); 00372 //lsr.rotateX(180); 00373 //transformObject(ptrObj); 00374 //lsr.popPoseMatrix(); 00375 // we need to redraw the scene (ie, reproject), or at least this object: 00376 //lsr.renderObject(ptrObj, lsr.RT); 00377 00378 // or "highlight" the object by putting "cartouche" around it object (note: do this only if it did NOT have 00379 //addCartoucheObject(ptrObj); 00380 00381 //pc.putc(ptrObj->ID()); 00382 //pc.printf("%d \n", ptrObj->ID()); 00383 pc.printf("%d", ptrObj->ID()); 00384 } else 00385 ptrObj->setColor(0x01); // make it blue 00386 } 00387 break; 00388 default: 00389 break; 00390 } 00391 lastTimeSensed=t.read_ms(); 00392 } // end sensing 00393 00394 } // infinite while loop 00395 // ================================= 00396 } 00397 00398 void setOrthographicView() 00399 { 00400 // clearScene(); 00401 // set color: 00402 lsr.setColor(0x02); // green 00403 //IO.setLaserLockinPower(0); // DISABLING SENSING LASER! (here the red). This is FOR CALIBRATION laser-camera 00404 00405 lsr.setOrthoProjection(); 00406 lsr.setIdentityPose(); // we could have done the translation here instead of in "local coordinates", but it's the same. 00407 } 00408 00409 void createSceneTest() 00410 { 00411 lsr.pushPoseMatrix(); 00412 00413 // (1) Clear the scene (this stops displaying, thus creating a "laser hot spot", but in principle it only happens in the setup) 00414 clearScene(); // NOTE: in the future (and if we have enough RAM, we can have several indexed "scenes", instead of a unique global) 00415 00416 // (2) Set displaying attributes (colors and local pose): 00417 // (a) Set the "local pose" (i.e., the MODELING TRANSFORMATION in OpenGL jargon): 00418 lsr.setIdentityPose(); // we could use a wrapper, but I won't for the time being. 00419 // (b) set color: 00420 lsr.setColor(0x02); 00421 00422 // Add a first object: using openGL-like syntaxis (here, a closed square): 00423 // begin(1); // start creating object with identifier = 1 00424 // vertex(10,10,1); 00425 // vertex(10,100,1); 00426 // vertex(100,100,1); 00427 // vertex(10,100,1); 00428 // vertex(10,10,1); // 00429 // end(); 00430 00431 // Add another objects using "object primitives": 00432 //begin(2); 00433 //lsr.translate(-100,-100,0); // recenter the square 00434 //square(200, 10); // this square has a corner in (0,0), and by default, z=0 00435 //end(); 00436 00437 // A "cube" (with one corner at the origin): 00438 //begin(3); 00439 //lsr.setIdentityPose(); // we set the identity - then we don't need to do push pop... we start from "scratch" here. Not incremental rotation... 00440 //lsr.translate(100,-100,-100); // recenter the cube (no need to push pop because we are seting the identity above) 00441 //lsr.rotateY(30); 00442 //cube(200,10); // cube(200, 20); 00443 //end(); 00444 00445 // A grid: 00446 // IMPORTANT: a grid is a multi-object built, and DOES NOT NEED to be between a begin and end 00447 lsr.setIdentityPose(); // we set the identity - then we don't need to do push pop... we start from "scratch" here. Not incremental rotation... 00448 lsr.translate(-500, -500, 0); 00449 grid(1000, 1000, 5, 5, 1); //grid(float sizeX, float sizeY, int nx, int ny, int repeatpoint); 00450 //gridCircles(300, 250, 6, 5, 10, 6); // gridCircles(float sizeX, float sizeY, int nx, int ny, float radius, int nbpointsCircle) 00451 00452 // A circle: 00453 //begin(4); 00454 //circle(200, 100); 00455 //end(); 00456 00457 updateScene(); // this is important: this will compute the number of objects, points, etc, to prepare the displaying engine (but rendering is NOT yet done, 00458 // rendering is done in the drawScene() function. 00459 00460 // pc.printf("Number of objects in the scene: %d\n", scene.totalObjects()); 00461 // pc.printf("Number of points in the scene: %d\n", scene.totalPoints()); 00462 00463 lsr.popPoseMatrix(); 00464 } 00465 00466 void createTextScene() 00467 { 00468 lsr.pushPoseMatrix(); 00469 //This function just creates text in local coordinates. We have two options: to create it as a unique object, or as separate objects (one per letter). 00470 // The first option is simpler now, we just use the wrapper function string3d(string _text, float totalWidth, float height): 00471 00472 // (1) Clear the scene (this stops displaying, thus creating a "laser hot spot", but in principle it only happens in the setup) 00473 clearScene(); // NOTE: in the future (and if we have enough RAM, we can have several indexed "scenes", instead of a unique global) 00474 00475 // (2) Set displaying attributes (colors and pose): 00476 // (a) Set the "local pose" (i.e., the MODELING TRANSFORMATION in OpenGL jargon): 00477 lsr.setIdentityPose(); // we could use a wrapper, but I won't for the time being. 00478 //lsr.flipY(); 00479 //lsr.translate(2,6,0); 00480 00481 lsr.flipY(); 00482 lsr.flipX(); 00483 lsr.translate(-15,-10,0); 00484 00485 // (b) set current color: 00486 lsr.setColor(0x04); // three LSB bits for RGB - so 0x02 is GREEN ON (note that the sensing laser, now red, is always on...). 00487 00488 // A string as a UNIQUE object: 00489 // begin(1); // start creating an object with index 1 00490 // no retouching of the modelview, meaning the text is in (0,0,0) in "local" coordinates 00491 // string3d(textToDisplay, textToDisplay.size()*fontWidth, fontHeight); 00492 //end(); 00493 00494 // A string as a *collection* of object-letters: 00495 //lsr.translate(-textToDisplay.length()*fontWidth/2,-fontHeight/2,0); 00496 for (unsigned short i = 0; i < textToDisplay.length(); i++) { 00497 char ch = textToDisplay.at(i); 00498 if (ch != ' ') { 00499 begin(i); 00500 letter3d(ch, fontWidth, fontHeight); 00501 end(); 00502 } 00503 lsr.translate(1.1*fontWidth, 0, 0); 00504 } 00505 00506 lsr.popPoseMatrix(); 00507 00508 } 00509 00510 void addText(string text, int id, float posx, float posy) 00511 { 00512 //clearScene(); 00513 lsr.pushPoseMatrix(); 00514 //This function just creates text in local coordinates. We have two options: to create it as a unique object, or as separate objects (one per letter). 00515 // The first option is simpler now, we just use the wrapper function string3d(string _text, float totalWidth, float height): 00516 00517 // (1) Clear the scene (this stops displaying, thus creating a "laser hot spot", but in principle it only happens in the setup) 00518 // clearScene(); // NOTE: in the future (and if we have enough RAM, we can have several indexed "scenes", instead of a unique global) 00519 00520 // (2) Set displaying attributes (colors and pose): 00521 // (a) Set the "local pose" (i.e., the MODELING TRANSFORMATION in OpenGL jargon): 00522 lsr.setIdentityPose(); // we could use a wrapper, but I won't for the time being. 00523 //lsr.flipY(); 00524 //lsr.translate(2,6,0); 00525 00526 lsr.flipY(); 00527 // lsr.flipX(); 00528 lsr.translate(posx,posy,0); 00529 00530 // (b) set current color: 00531 lsr.setColor(0x04); // three LSB bits for RGB - so 0x02 is GREEN ON (note that the sensing laser, now red, is always on...). 00532 00533 // A string as a UNIQUE object: 00534 // begin(1); // start creating an object with index 1 00535 // no retouching of the modelview, meaning the text is in (0,0,0) in "local" coordinates 00536 // string3d(textToDisplay, textToDisplay.size()*fontWidth, fontHeight); 00537 //end(); 00538 00539 // A string as a *collection* of object-letters: 00540 //lsr.translate(-textToDisplay.length()*fontWidth/2,-fontHeight/2,0); 00541 00542 for (unsigned short i = 0; i < text.length(); i++) { 00543 char ch = text.at(i); 00544 if (ch != ' ') { 00545 begin(id); 00546 letter3d(ch, fontWidth, fontHeight); 00547 end(); 00548 } 00549 lsr.translate(1.1*fontWidth, 0, 0); 00550 } 00551 lsr.popPoseMatrix(); 00552 00553 } 00554 00555 // ============ COMMUNICATION PROTOCOL ====================================== 00556 // String to store ALPHANUMERIC DATA (i.e., integers, floating point numbers, unsigned ints, etc represented as DEC): 00557 // TO DO: use string objects! 00558 char receivedStringData[24]; // note: an integer is two bytes long, represented with a maximum of 5 digits, but we may send floats or unsigned int... 00559 int indexStringData = 0; //position of the byte in the string 00560 00561 // String to store COMMAND WORDS (not used yet): 00562 char stringCommand[24]; 00563 int indexStringCommand = 0; 00564 bool commandReady = false; // will become true when receiving the byte 0 (i.e. the '/0' string terminator) 00565 00566 00567 #ifdef WITH_OSC 00568 //============= RECEIVE OSC COMMANDS ========================= 00569 // This is the callback function called when there are packets on the listening socket. It is not nice to have it 00570 // here, but for the time being having a "wrapping global" is the simplest solution (we cannot pass a member-function pointer 00571 // as handler to the upd object). 00572 void processOSC(UDPSocketEvent e) 00573 { 00574 osc.onUDPSocketEvent(e); 00575 00576 if (osc.newMessage) { 00577 00578 // pc.printf("RECEIVED NEW OSC MESSAGE ----\n"); 00579 00580 // Acquire the addresses and arguments and put them in the GLOBAL variables to be processed by the interpretCommand function (common to serial and OSC): 00581 command1=""; 00582 command2=""; 00583 for (int i=0; i<recMes.getAddressNum(); i++) { 00584 strcpy(address[i],recMes.getAddress(i)); // NOTE: up to the rest of the program to check if address[1] is really not null 00585 // pc.printf("Address %d = %s \n",i, address[i]); 00586 } 00587 // Acquire data: 00588 00589 numArgs=recMes.getArgNum(); 00590 data[0]=-1; 00591 data[1]=-1; 00592 for (int i=0; i<numArgs; i++) { 00593 data[i]=(int)recMes.getArgInt(i);//recMes.getArgFloat(i); 00594 //pc.printf("%4.2f %4.2f \n", data[i], recMes.getArgFloat(i)); 00595 // pc.printf("%i %i \n", data[i], recMes.getArgInt(i)); 00596 } 00597 00598 // Finally, interpret the command or data: 00599 interpretCommand(); 00600 00601 } 00602 } 00603 #endif 00604 00605 00606 //interpretCommand(const char& address[2][], const int& data[2]) { 00607 void interpretCommand() 00608 { 00609 // ========== DATA (all this is a hack because my OSC class does not accept BUNDLES...) ============= 00610 // Add data to auxiliary data buffer: 00611 if (command1 == "data") { 00612 // numArgs is the number of arguments (in my OSC version this is max 2): 00613 // pc.printf("--- Data pack: %d\n", numArgs); 00614 for (int i=0; i<numArgs; i++) { 00615 auxDataBuffer[auxDataBuffer_index+i] = 1.0*data[i]; 00616 // note: the index will be reset to 0 or whatever needed when a command USING data from the "d" message is received. 00617 // pc.printf("buffer index / data: %i %f \n", auxDataBuffer_index+i, auxDataBuffer[auxDataBuffer_index+i]);//data[i]); 00618 } 00619 auxDataBuffer_index+=numArgs; 00620 //pc.putc('/n'); 00621 } 00622 // ========== COMMANDS (using data on auxDataBuffer or directly as arguments ========================= 00623 00624 else if (command1 == "objectID") { // ID of the object 00625 receivedStringData[indexStringData] = 0; 00626 indexStringData = 0; 00627 objectID=atoi(receivedStringData); 00628 } 00629 00630 else if (command1 == "mbedReset" ) mbed_reset(); 00631 00632 else if (command1 == "testPower" ) { 00633 // TO DO 00634 } 00635 00636 // Enable/disable projection: 00637 else if ( command1 == "stopDisplay" ) stopDisplay(); 00638 else if (command1 =="resumeDisplay") resumeDisplay(); 00639 else if (command1 =="showLimits") showLimitsMirrors(50, 1); 00640 else if (command1 == "setSensingMode") { 00641 int value=data[0]; 00642 if (value!=-1) { // otherwise do nothing, this is a reception error (there was no data) 00643 switch(value) { 00644 case 0: 00645 currentSensingMode=NO_SENSING; 00646 break; 00647 case 1: 00648 currentSensingMode=SENSING_WHOLE_SCENE; 00649 break; 00650 case 2: 00651 currentSensingMode=SENSING_PER_OBJECT; 00652 break; 00653 default: 00654 break; 00655 } 00656 } 00657 } 00658 00659 // Change state machine color (objects created from this point on will have the state machine color) 00660 else if (command1 == "setStateColor") { // example: 0( (all OFF), 1( blue ON, 2( green ON, 4( red ON, 3( blue and green ON; etc. 00661 int value=data[0]; 00662 if (value!=-1) lsr.setColor(value); 00663 } 00664 00665 // Real time change of color of ALL the objects in the CURRENT scene (state machine color not changed) 00666 else if (command1 == "setAllObjectsColor") { // example: 0) (all OFF), 1) blue ON, 2) green ON, 4) red ON, 3) blue and green ON; etc. 00667 int value=data[0]; 00668 if (value!=-1) changeColorScene(value); 00669 } 00670 00671 00672 // =================== CREATION and DESTRUCTION of objects ===================================================== 00673 else if (command1 == "clearScene") { 00674 clearScene(); 00675 } 00676 00677 else if (command1 =="deleteObject") { // second address is object identifier (a string that will be converted 00678 deleteObject(objectID); 00679 } 00680 00681 else if (command1=="initialPosition") { // this is for setting the POSITION of the object to be created (or displacement): 00682 xx=auxDataBuffer[0]; yy=auxDataBuffer[1]; 00683 auxDataBuffer_index=0; 00684 } 00685 00686 // =================== TRANSFORMATION OF OBJECT POINTS ========================================================= 00687 // NOTE: this may in the end destroy the object rigidity because approximations... but without an RT per object, this is the only way to 00688 // modify object pose independently of the whole scene... 00689 else if (command1 =="moveObject") { 00690 translateObject(objectID, xx, yy, 0); 00691 } 00692 00693 00694 // ============================================================================================================== 00695 00696 00697 // Produce a grid of points. The projection is ORTHOGRAPHIC. Also, POSE is reset to identity (i.e., the sent points should have 00698 // been translated previously - this may be optional, we could first send the pose, then the points...) 00699 // Note: orthographic projection can be achieved by simply setting the projection matrix to ID, while 00700 // setting Z to 1, or by setting the projection matrix to a unit diagonal but the last term in the diagonal is set to 0. 00701 // Finally, if one wants to avoid an unnecessary matrix product, the laserRendering class provides a renderingMode that can be set to RAW. 00702 // NOTE: the following string of ASCII characters is assumed to have been input: 00703 // posX#posY#sizeX#sizeY#nx#ny#: 00704 // where the names (ex: posX) is in fact an ASCII string representing the number in decimal base. This means that when we arrive here, 00705 // the auxiliary array auxDataBuffer contains all that data (as floats), ex: auxDataBuffer[0] is just posX, and auxDataBuffer[5] is ny 00706 else if (command1 =="gridScene") { 00707 // Data is now ordered in the auxDataBuffer array: 00708 float posX = auxDataBuffer[0], posY = auxDataBuffer[1], sizeX = auxDataBuffer[2], sizeY = auxDataBuffer[3], nx = auxDataBuffer[4], ny = auxDataBuffer[5]; 00709 auxDataBuffer_index=0; // to restart loading the auxiliary buffer 'auxDataBuffer' 00710 clearScene(); 00711 // set color: 00712 lsr.setColor(0x02); // green 00713 //IO.setLaserLockinPower(0); // DISABLING SENSING LASER! (here the red). This is FOR CALIBRATION laser-camera 00714 00715 // set local pose matrix to identity: 00716 lsr.setIdentityPose(); 00717 // translate to the required position: 00718 lsr.translate(posX, posY, 0); // since the projection and pose are going to be set 00719 // to Identity (projection identity will automatically make the projection orthographic), this means that these 00720 // values are in "laser projector pixels" (in fact angles). 00721 // Create the collection of dot objects: 00722 // void grid(float sizeX, float sizeY, int nx, int ny, int repeatpoint); 00723 grid(sizeX, sizeY, nx, ny, 1); 00724 updateScene(); // this is important: this will compute the number of objects, points, etc, to prepare the displaying engine (but rendering is NOT yet done, 00725 // rendering is done in the drawScene() function. 00726 00727 // Now we can render the scene once: 00728 // Set a default GLOBAL POSE and orthographic projection for rendering and start displaying: 00729 lsr.setOrthoProjection(); 00730 lsr.setIdentityPose(); // we could have done the translation here instead of in "local coordinates", but it's the same. 00731 drawScene(); // this will be called when we change the pose matrix - but needs to be called at least one to start displaying 00732 } 00733 // NOTE: now that the grid is set, we don't need to do it again when changing the pose: JUST SEND THE POSE MATRIX and things will be re-rendered! 00734 00735 // This means we received "2D" vertex data (z assumed 0) to create A SINGLE SCENE out of these points (we will create a single object) 00736 // using orthographic projection. 00737 // This is in particular useful to create "structured light" grids - such the grid of planar points for calibration. 00738 // In the future, we can have functions to add objects, etc. Basically, every wrapped function should be call-able by the PC. 00739 // NOTE: the POSE is reset to ID, but we could use the current pose (i.e., send it first by the computer). But for the time being, and in order to modify the 00740 // cameraProjectorCalibration Xcode project as little as possible, I will perform the transformations on the computer. 00741 else if (command1 == "arbitraryGridScene") { 00742 // Data is now ordered in the auxDataBuffer array as PAIRS (X,Y). The argument to this message is the IDENTIFIER of the object: 00743 // The present value of auxDataBuffer_index contains the number of points x 2 00744 clearScene(); 00745 lsr.setIdentityPose(); 00746 //lsr.setColor(0x02); // fixed color, or use the current color (in which case, we need to call lsr.pushColor() and then pop it again). 00747 lsr.setColor(0x03); // blue+green 00748 IO.setLaserLockinPower(0); // DISABLING SENSING LASER! (here the red). This is FOR CALIBRATION laser-camera 00749 00750 // begin(1) // we can create a single object, or one object per position (with repetition of the point if needed) 00751 for (unsigned char i = 0; i < auxDataBuffer_index / 2; i++) { 00752 begin(i); 00753 for (unsigned char k=0; k<1; k++) // fixed repetition of points - we can send this as a parameter if we want. 00754 vertex(auxDataBuffer[2 * i], auxDataBuffer[2 * i + 1], 0); 00755 end(); 00756 } 00757 updateScene(); 00758 // Set a default GLOBAL POSE and ORTHO projection matrix for start displaying, or use the CURRENT ONE (for that, we should have 00759 // properly called lsr.pushPoseMatrix()) 00760 lsr.setOrthoProjection(); 00761 lsr.setIdentityPose(); 00762 drawScene(); 00763 auxDataBuffer_index = 0; // to restart loading the auxiliary buffer 'auxDataBuffer' through serial port 00764 } 00765 00766 // This means we received 2D vertex array to add an object assuming it's planar (we will create an object with all points having z=0 in current pose): 00767 else if (command1 == "createObject2D") { 00768 unsigned char numPoints= auxDataBuffer_index/2; 00769 if (objectID!=-1) { 00770 00771 // First, create the object: 00772 lsr.pushPoseMatrix(); 00773 lsr.setIdentityPose(); 00774 lsr.translate(xx,yy,0); 00775 00776 lsr.setColor(0x02); // we will render in current color 00777 00778 begin(objectID); 00779 for (unsigned char i = 0; i < numPoints; i++) { 00780 begin(i); 00781 vertex(auxDataBuffer[2 * i], auxDataBuffer[2 * i + 1],0); // z=0 for 2d objects (in current pose) 00782 //letter3d('A', fontWidth, fontHeight); 00783 //vertex(10*cos(2*PI/numPoints*i), 10*sin(2*PI/numPoints*i), 0); 00784 //vertex(5*cos(2*PI/numPoints*i+.1), 5*sin(2*PI/numPoints*i+.1), 0); 00785 //pc.printf("local (x,y): %f, %f \n", auxDataBuffer[2 * i], auxDataBuffer[2 * i+1]); 00786 // lsr.translate(1.1*fontWidth, 0, 0); 00787 } 00788 end(); 00789 lsr.popPoseMatrix(); 00790 00791 // Set the global projection and modelview: 00792 lsr.loadProjMatrix(K1, scaleFactorProjector1); 00793 lsr.setIdentityPose(); 00794 lsr.multPoseMatrix(lsr.EXTRINSICS); // RT=E // we can simply do: lsr.setExtrinsicsPose() 00795 // DEFAULT position in CAMERA coordinates (calibration was in cm): 00796 lsr.translate(0, 0, 130); 00797 00798 // RENDER the scene: 00799 // update the scene for display 00800 drawScene(); 00801 updateScene(); 00802 } 00803 } 00804 00805 // This means we received 3D vertex data to create an object. 00806 // It will be rendered in the CURRENT pose. 00807 else if (command1 == "createObject3D") { 00808 int objectID=data[1]; 00809 if (objectID!=-1) { 00810 // Data is now ordered in the auxDataBuffer array in triplets (X,Y,Z). 00811 // The present value of auxDataBuffer_index contains the number of points x 3 00812 //lsr.pushPoseMatrix(); // we will render on the current pose 00813 //clearScene(); 00814 //lsr.setIdentityPose(); 00815 //lsr.setColor(0x02); // we will render in current color 00816 begin(objectID); 00817 for (unsigned char i = 0; i < auxDataBuffer_index / 3; i++) { 00818 vertex(auxDataBuffer[3 * i], auxDataBuffer[3 * i + 1], 00819 auxDataBuffer[3 * i + 2]); 00820 } 00821 end(); 00822 updateScene(); 00823 // Set a default GLOBAL POSE and ORTHO projection matrix for start displaying, or use the CURRENT ONE (for that, we should have 00824 // properly called lsr.pushPoseMatrix()) 00825 //lsr.setOrthoProjection(); 00826 //lsr.setIdentityPose(); // we could have done the translation here instead of in "local coordinates", but it's the same. 00827 //lsr.popPoseMatrix(); 00828 drawScene(); 00829 auxDataBuffer_index = 0; // to restart loading the auxiliary buffer 'auxDataBuffer' through serial port 00830 } 00831 } 00832 00833 else if (command1=="createText") { 00834 receivedStringData[indexStringData] = 0; // termination for the string 00835 indexStringData = 0; 00836 // Serial.println(receivedStringData); 00837 textToDisplay = string(receivedStringData); 00838 00839 if (firstTimeDisplay) {clearScene(); firstTimeDisplay=false;} 00840 00841 addText(textToDisplay, objectID, xx, yy); 00842 drawScene(); 00843 updateScene(); 00844 } 00845 00846 // Create text: 00847 else if (command1 == "createTextObject") { 00848 00849 if (objectID!=-1) { 00850 00851 // CREATE OBJECT: 00852 lsr.pushPoseMatrix(); 00853 lsr.setIdentityPose(); // we could use a wrapper, but I won't for the time being. 00854 lsr.flipY(); 00855 //lsr.flipX(); 00856 lsr.translate(xx,yy,0); 00857 00858 begin(objectID); 00859 for (unsigned short i = 0; i < textToDisplay.length(); i++) { 00860 char ch = textToDisplay.at(i); 00861 if (ch != ' ') { 00862 // begin(i); 00863 letter3d(ch, fontWidth, fontHeight); 00864 // end(); 00865 } 00866 lsr.translate(1.1*fontWidth, 0, 0); 00867 } 00868 end(); 00869 00870 lsr.popPoseMatrix(); 00871 00872 // Set a default PERSPECTIVE and GLOBAL POSE for start displaying: 00873 lsr.loadProjMatrix(K1, scaleFactorProjector1); 00874 lsr.setIdentityPose(); 00875 lsr.multPoseMatrix(lsr.EXTRINSICS); // RT=E // we can simply do: lsr.setExtrinsicsPose() 00876 // DEFAULT position in CAMERA coordinates (calibration was in cm): 00877 lsr.translate(0, 0, 130); 00878 00879 drawScene(); // this will be called when we change the pose matrix - but needs to be called at least once to start displaying 00880 updateScene(); 00881 } 00882 } 00883 00884 00885 // (d) TERMINATOR indicating data was for the POSE MATRIX of the object (Mp) (with respect to the CAMERA): 00886 else if (command1 == "poseMatrix") { // when receiving this, it means that the WHOLE matrix data (4x4 values) have been sent in row/column format 00887 auxDataBuffer_index = 0; // to restart loading the auxiliary buffer 'auxDataBuffer' 00888 // Now, auxDataBuffer is a buffer with 12 values (4x3), corresponding to the pose of the object in CAMERA coordinartes (RT') 00889 lsr.setIdentityPose(); // RT=ID 00890 lsr.multPoseMatrix(lsr.EXTRINSICS); // RT=E // we can simply do: lsr.setExtrinsicsPose() 00891 lsr.multPoseMatrix(auxDataBuffer); // RT=ExRT' : this sets RT as the current object pose in PROJECTOR coordinates 00892 00893 drawScene(); // needed because we changed the POSE (but there is no need to re-create the geometry nor update the renderer (updateScene), just project) 00894 } 00895 00896 // (d) TERMINATOR indicating data was for the projection matrix (will probably be saved in "hard" in the microcontroller code): 00897 else if (command1 == "projectionMatrix") {// when receiving this character, it means that the WHOLE matrix data (3x3 values) have been sent (with '#' separator), in row/column format 00898 auxDataBuffer_index = 0; // to restart loading the auxiliary buffer 'auxDataBuffer' through serial port 00899 // store in projection matrix: 00900 lsr.loadProjMatrix(auxDataBuffer, 1.0); 00901 00902 drawScene(); // needed because we changed the PROJECTION matrix (but there is no need to re-create all the geometry, just project) 00903 } 00904 00905 //(e) TERMINATOR indicating the data was for the extrinsic matrix: 00906 else if (command1 == "extrinsicMatrix") { // when receiving this character, it means that the WHOLE matrix data (3x3 values) have been sent (with '#' separator), in row/column format 00907 auxDataBuffer_index = 0; // to restart loading the auxiliary buffer 'auxDataBuffer' through serial port 00908 // store in projection matrix: 00909 lsr.loadExtrinsicsMatrix(auxDataBuffer); 00910 00911 drawScene(); // needed because we changed the EXTRINSICS (hence the pose), but there is no need to re-create the geometry, just project. 00912 } 00913 } 00914 00915 void processSerial() 00916 { 00917 while (pc.readable() > 0) { 00918 char incomingByte = pc.getc(); 00919 00920 // For test: 00921 // pc.putc(incomingByte); 00922 00923 // (a) TEXT to display or NUMBERS (in ASCII) to convert to numerical value (rem: limited set of characters for the time being): 00924 // Note: numbers are float, so we need also the "." and "-" 00925 if ( (incomingByte >= '0' && incomingByte <= '9') // numbers 00926 || (incomingByte >= 'A' && incomingByte <= 'Z') // capital letters 00927 || (incomingByte >= 'a' && incomingByte <= 'z') // small letters (in fact, will be used for other symbols) 00928 || (incomingByte == ' ') // space 00929 || (incomingByte == '-') // minus sign 00930 || (incomingByte == '.') // decimal point 00931 ) { 00932 receivedStringData[indexStringData] = incomingByte; 00933 indexStringData++; 00934 } 00935 00936 // (b) a NUMBER to convert to float and to add to the auxiliary data buffer: 00937 else if (incomingByte == '#') { // this means that we received a whole number to convert to float (but we don't know yet if it's for 00938 // the modelview, the projection matrix or some other data): 00939 receivedStringData[indexStringData] = 0; 00940 indexStringData = 0; 00941 // Serial.println(receivedStringData); // for tests 00942 // convert to float and store in auxiliary "matrix" array: 00943 auxDataBuffer[auxDataBuffer_index] = atof(receivedStringData); 00944 //Serial.println( auxDataBuffer[auxDataBuffer_index]); // for tests 00945 auxDataBuffer_index++; 00946 } 00947 00948 // Enable/disable projection: 00949 else if (incomingByte == '}') { 00950 stopDisplay(); 00951 } else if (incomingByte == '{') { 00952 resumeDisplay(); 00953 } 00954 00955 // Show maximum excursion of mirrors: 00956 else if (incomingByte == '*') { 00957 showLimitsMirrors(50, 1); 00958 } 00959 00960 else if (incomingByte=='_') { 00961 clearScene(); 00962 } 00963 00964 else if (incomingByte == '+') { 00965 receivedStringData[indexStringData] = 0; 00966 indexStringData = 0; 00967 switch(atoi(receivedStringData)) { 00968 case 0: 00969 currentSensingMode=NO_SENSING; 00970 break; 00971 case 1: 00972 currentSensingMode=SENSING_WHOLE_SCENE; 00973 break; 00974 case 2: 00975 currentSensingMode=SENSING_PER_OBJECT; 00976 break; 00977 default: 00978 break; 00979 } 00980 } 00981 00982 // Performs a scan of the surface and send all the data serially (N>, with N the resolution of the scan) 00983 else if (incomingByte == '>') { 00984 receivedStringData[indexStringData] = 0; 00985 indexStringData = 0; 00986 //pc.printf("scan command issued¥n"); 00987 scanSerial(atoi(receivedStringData)); 00988 } 00989 00990 // Recompute the LUT table 00991 else if (incomingByte == '<') { 00992 recomputeLookUpTable(); 00993 } 00994 00995 // Change state machine color (objects created from this point on will have the state machine color) 00996 else if (incomingByte == '(') { // example: 0( (all OFF), 1( blue ON, 2( green ON, 4( red ON, 3( blue and green ON; etc. 00997 receivedStringData[indexStringData] = 0; 00998 indexStringData = 0; 00999 // convert to integer and set the state machine color: 01000 lsr.setColor(atoi(receivedStringData)); 01001 } 01002 01003 // Real time change of color of ALL the objects in the CURRENT scene (state machine color not changed) 01004 else if (incomingByte == ')') { // example: 0) (all OFF), 1) blue ON, 2) green ON, 4) red ON, 3) blue and green ON; etc. 01005 receivedStringData[indexStringData] = 0; 01006 indexStringData = 0; 01007 // convert to integer and set the color: 01008 changeColorScene(atoi(receivedStringData)); 01009 } 01010 01011 01012 // Produce a grid of points. The projection is ORTHOGRAPHIC. Also, POSE is reset to identity (i.e., the sent points should have 01013 // been translated previously - this may be optional, we could first send the pose, then the points...) 01014 // Note: orthographic projection can be achieved by simply setting the projection matrix to ID, while 01015 // setting Z to 1, or by setting the projection matrix to a unit diagonal but the last term in the diagonal is set to 0. 01016 // Finally, if one wants to avoid an unnecessary matrix product, the laserRendering class provides a renderingMode that can be set to RAW. 01017 // NOTE: the following string of ASCII characters is assumed to have been input: 01018 // posX#posY#sizeX#sizeY#nx#ny#: 01019 // where the names (ex: posX) is in fact an ASCII string representing the number in decimal base. This means that when we arrive here, 01020 // the auxiliary array auxDataBuffer contains all that data (as floats), ex: auxDataBuffer[0] is just posX, and auxDataBuffer[5] is ny 01021 else if (incomingByte == ':') { 01022 // Data is now ordered in the auxDataBuffer array: 01023 float posX = auxDataBuffer[0], posY = auxDataBuffer[1], sizeX = auxDataBuffer[2], sizeY = auxDataBuffer[3], nx = auxDataBuffer[4], ny = auxDataBuffer[5]; 01024 auxDataBuffer_index = 0; // to restart loading the auxiliary buffer 'auxDataBuffer' through serial port 01025 clearScene(); 01026 // set color: 01027 lsr.setColor(0x02); // green 01028 //IO.setLaserLockinPower(0); // DISABLING SENSING LASER! (here the red). This is FOR CALIBRATION laser-camera 01029 01030 // set local pose matrix to identity: 01031 lsr.setIdentityPose(); 01032 // translate to the required position: 01033 lsr.translate(posX, posY, 0); // since the projection and pose are going to be set 01034 // to Identity (projection identity will automatically make the projection orthographic), this means that these 01035 // values are in "laser projector pixels" (in fact angles). 01036 // Create the collection of dot objects: 01037 // void grid(float sizeX, float sizeY, int nx, int ny, int repeatpoint); 01038 grid(sizeX, sizeY, nx, ny, 1); 01039 updateScene(); // this is important: this will compute the number of objects, points, etc, to prepare the displaying engine (but rendering is NOT yet done, 01040 // rendering is done in the drawScene() function. 01041 01042 // Now we can render the scene once: 01043 // Set a default GLOBAL POSE and orthographic projection for rendering and start displaying: 01044 lsr.setOrthoProjection(); 01045 lsr.setIdentityPose(); // we could have done the translation here instead of in "local coordinates", but it's the same. 01046 drawScene(); // this will be called when we change the pose matrix - but needs to be called at least one to start displaying 01047 } 01048 // NOTE: now that the grid is set, we don't need to do it again when changing the pose: JUST SEND THE POSE MATRIX and things will be re-rendered! 01049 01050 // This means we received "2D" vertex data (z assumed 0) to create a single scene out of these points (we will create a single object) 01051 // using orthographic projection. 01052 // This is in particular useful to create "structured light" grids - such the grid of planar points for calibration. 01053 // In the future, we can have functions to add objects, etc. Basically, every wrapped function should be call-able by the PC. 01054 // NOTE: the POSE is reset to ID, but we could use the current pose (i.e., send it first by the computer). But for the time being, and in order to modify the 01055 // cameraProjectorCalibration Xcode project as little as possible, I will perform the transformations on the computer. 01056 else if (incomingByte == '@') { // ID of the object 01057 command1="objectID"; 01058 interpretCommand(); 01059 } 01060 01061 else if (incomingByte == '[') { 01062 command1 =="deleteObject"; 01063 interpretCommand(); 01064 } 01065 01066 else if (incomingByte == '}') { 01067 command1 =="moveObject"; 01068 interpretCommand();; // xx, yy contains already the necessary displacement 01069 } 01070 01071 01072 else if (incomingByte == '~') { // this is for setting the POSITION of the object to be created: 01073 command1="initialPosition"; 01074 interpretCommand(); 01075 } 01076 01077 01078 else if (incomingByte == '=') { // Create 2d object: 01079 command1 = "createObject2D"; 01080 interpretCommand(); 01081 } 01082 01083 // This means we received 3D vertex data to create a single scene out of these points (we will create a single object). 01084 // It will be rendered in the CURRENT pose. 01085 else if (incomingByte == ';') { 01086 // Data is now ordered in the auxDataBuffer array in triplets (X,Y,Z). 01087 // The present value of auxDataBuffer_index contains the number of points x 3 01088 lsr.pushPoseMatrix(); // we will render on the current pose... 01089 clearScene(); 01090 lsr.setIdentityPose(); 01091 //lsr.setColor(0x02); 01092 begin(1); 01093 for (unsigned char i = 0; i < auxDataBuffer_index / 3; i++) { 01094 vertex(auxDataBuffer[3 * i], auxDataBuffer[3 * i + 1], 01095 auxDataBuffer[3 * i + 2]); 01096 } 01097 end(); 01098 updateScene(); 01099 // Set a default GLOBAL POSE and ORTHO projection matrix for start displaying, or use the CURRENT ONE (for that, we should have 01100 // properly called lsr.pushPoseMatrix()) 01101 //lsr.setOrthoProjection(); 01102 //lsr.setIdentityPose(); // we could have done the translation here instead of in "local coordinates", but it's the same. 01103 lsr.popPoseMatrix(); 01104 drawScene(); 01105 auxDataBuffer_index = 0; // to restart loading the auxiliary buffer 'auxDataBuffer' through serial port 01106 } 01107 01108 01109 /* 01110 else if (incomingByte=='!') { // sets the width of the letter 01111 receivedStringData[indexStringData]=0; 01112 indexStringData=0; 01113 fontWidth=atof(receivedStringData); 01114 createTextScene(); // needed because we changed the geometry 01115 } 01116 01117 else if (incomingByte=='"') { // sets the height of the letter 01118 receivedStringData[indexStringData]=0; 01119 indexStringData=0; 01120 fontHeight=atof(receivedStringData); 01121 createTextScene(); // needed because we changed the geometry 01122 } 01123 */ 01124 01125 // (c) TERMINATOR indicating the data was text to display: 01126 else if (incomingByte == '"') { // this means that the previous data was TEXT to display 01127 command1="createText"; 01128 interpretCommand(); 01129 } 01130 01131 // (d) TERMINATOR indicating data was for the POSE MATRIX of the object (Mp) (with respect to the CAMERA): 01132 else if (incomingByte == '$') { // when receiving this character, it means that the WHOLE matrix data (4x4 values) have been sent (with '#' separator), in row/column format 01133 auxDataBuffer_index = 0; // to restart loading the auxiliary buffer 'auxDataBuffer' through serial port 01134 // Now, auxDataBuffer is a buffer with 12 values (4x3), corresponding to the pose of the object in CAMERA coordinartes (RT') 01135 lsr.setIdentityPose(); // RT=ID 01136 lsr.multPoseMatrix(lsr.EXTRINSICS); // RT=E // we can simply do: lsr.setExtrinsicsPose() 01137 lsr.multPoseMatrix(auxDataBuffer); // RT=ExRT' : this sets RT as the current object pose in PROJECTOR coordinates 01138 01139 drawScene(); // needed because we changed the POSE (but there is no need to re-create the geometry, just project) 01140 01141 // Handshake: 01142 pc.putc(13); 01143 } 01144 01145 // (d) TERMINATOR indicating data was for the projection matrix (will probably be saved in "hard" in the microcontroller code): 01146 else if (incomingByte == '%') { // when receiving this character, it means that the WHOLE matrix data (3x3 values) have been sent (with '#' separator), in row/column format 01147 auxDataBuffer_index = 0; // to restart loading the auxiliary buffer 'auxDataBuffer' through serial port 01148 // store in projection matrix: 01149 lsr.loadProjMatrix(auxDataBuffer, 1.0); 01150 01151 drawScene(); // needed because we changed the PROJECTION matrix (but there is no need to re-create all the geometry, just project) 01152 } 01153 01154 //(e) TERMINATOR indicating the data was for the extrinsic matrix: 01155 else if (incomingByte == '&') { // when receiving this character, it means that the WHOLE matrix data (3x3 values) have been sent (with '#' separator), in row/column format 01156 auxDataBuffer_index = 0; // to restart loading the auxiliary buffer 'auxDataBuffer' through serial port 01157 // store in projection matrix: 01158 lsr.loadExtrinsicsMatrix(auxDataBuffer); 01159 01160 drawScene(); // needed because we changed the EXTRINSICS (hence the pose), but there is no need to re-create the geometry, just project. 01161 } 01162 01163 } 01164 }
Generated on Tue Jul 12 2022 19:19:38 by
1.7.2