Alvaro Cassinelli / Mbed 2 deprecated laserUI

Laser Sensing Display for UI interfaces in the real world

Dependencies:   mbed

Fork of skinGames_forktest by Alvaro Cassinelli

main.cpp@42:5f21a710ebc5, 2013-10-18 (annotated)

Committer:
mbedalvaro
Date:
Fri Oct 18 07:25:03 2013 +0000
Revision:
42:5f21a710ebc5
Parent:
41:74e24a0e6e50
Child:
43:1dd4cfc30788
lots of problems, in particular it seems I cannot add new objects without clearing the whole scene first!!

Who changed what in which revision?

UserRevisionLine numberNew contents of line
mbedalvaro 40:3ba2b0ea9f33 1 /*
mbedalvaro 40:3ba2b0ea9f33 2
mbedalvaro 40:3ba2b0ea9f33 3 - NOTE: I could instantiate at least 960 points! (as 30 objects containing each 32 points)
mbedalvaro 40:3ba2b0ea9f33 4 ... 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
mbedalvaro 40:3ba2b0ea9f33 5 the trajectory and 3d vector array... but where?
mbedalvaro 40:3ba2b0ea9f33 6 */
mbedalvaro 40:3ba2b0ea9f33 7
mbedalvaro 40:3ba2b0ea9f33 8 #include "mbed.h"
mbedalvaro 40:3ba2b0ea9f33 9 #include "mbedOSC.h"
mbedalvaro 42:5f21a710ebc5 10 #include "WrapperFunctions.h"
mbedalvaro 42:5f21a710ebc5 11
mbedalvaro 42:5f21a710ebc5 12 // 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()...)
mbedalvaro 42:5f21a710ebc5 13 #include "LaserRenderer.h"
mbedalvaro 42:5f21a710ebc5 14 extern LaserRenderer lsr;
mbedalvaro 42:5f21a710ebc5 15
mbedalvaro 40:3ba2b0ea9f33 16
mbedalvaro 41:74e24a0e6e50 17 // mbed IP address (server):
mbedalvaro 40:3ba2b0ea9f33 18 #ifdef DHCP
mbedalvaro 40:3ba2b0ea9f33 19 EthernetNetIf eth;
mbedalvaro 40:3ba2b0ea9f33 20 #else
mbedalvaro 40:3ba2b0ea9f33 21 EthernetNetIf eth(
mbedalvaro 41:74e24a0e6e50 22 IpAddr(10,0,0,2), //IP Address of the mbed
mbedalvaro 40:3ba2b0ea9f33 23 IpAddr(255,255,255,0), //Network Mask
mbedalvaro 40:3ba2b0ea9f33 24 IpAddr(10,0,0,1), //Gateway
mbedalvaro 40:3ba2b0ea9f33 25 IpAddr(10,0,0,1) //DNS
mbedalvaro 40:3ba2b0ea9f33 26 );
mbedalvaro 40:3ba2b0ea9f33 27 #endif
mbedalvaro 40:3ba2b0ea9f33 28
mbedalvaro 41:74e24a0e6e50 29 //uint8_t serverMac[] = { 0xDE, 0xAD, 0xBE, 0xEF, 0xFE, 0xED };
mbedalvaro 42:5f21a710ebc5 30 uint8_t serverIp[] = { 10, 0, 0, 2 };
mbedalvaro 41:74e24a0e6e50 31 int serverPort = 10000;
mbedalvaro 41:74e24a0e6e50 32
mbedalvaro 42:5f21a710ebc5 33 //uint8_t destIp[] = {10, 0, 0, 3};
mbedalvaro 41:74e24a0e6e50 34 uint8_t destIp[] = {255, 255, 255, 255}; // broadcast, so we can use several computers for sound, etc
mbedalvaro 41:74e24a0e6e50 35 int destPort = 12000;
mbedalvaro 41:74e24a0e6e50 36
mbedalvaro 41:74e24a0e6e50 37 char *topAddress="/mbed";
mbedalvaro 42:5f21a710ebc5 38 char *subAddress[3]= { "/test1" , "/test2" , "/test3" };
mbedalvaro 41:74e24a0e6e50 39
mbedalvaro 40:3ba2b0ea9f33 40 OSCMessage recMes;
mbedalvaro 40:3ba2b0ea9f33 41 OSCMessage sendMes;
mbedalvaro 40:3ba2b0ea9f33 42
mbedalvaro 40:3ba2b0ea9f33 43 OSCClass osc;
mbedalvaro 41:74e24a0e6e50 44 //OSCClass osc(&recMes); // instantiate OSC communication object, and set the receiver container from the OSC packets
mbedalvaro 40:3ba2b0ea9f33 45
mbedalvaro 40:3ba2b0ea9f33 46
mbedalvaro 40:3ba2b0ea9f33 47 // for tests:
mbedalvaro 40:3ba2b0ea9f33 48 extern Scene scene;
mbedalvaro 40:3ba2b0ea9f33 49 extern laserSensingDisplay lsd;
mbedalvaro 40:3ba2b0ea9f33 50
mbedalvaro 40:3ba2b0ea9f33 51 // =======================================================
mbedalvaro 40:3ba2b0ea9f33 52
mbedalvaro 40:3ba2b0ea9f33 53 // Timers:
mbedalvaro 40:3ba2b0ea9f33 54 Timer t; // we can read in ms or us
mbedalvaro 40:3ba2b0ea9f33 55
mbedalvaro 40:3ba2b0ea9f33 56 // Tickers:
mbedalvaro 40:3ba2b0ea9f33 57 //Ticker timerForRendering; // now in the WrapperFunctions.cpp file
mbedalvaro 40:3ba2b0ea9f33 58 //Ticker timerForSendingData; // better use a timer, so as not to interrupt the exact laser display ticker
mbedalvaro 40:3ba2b0ea9f33 59 unsigned long lastTimeCreated;
mbedalvaro 40:3ba2b0ea9f33 60 unsigned long lastTimeSensed;
mbedalvaro 40:3ba2b0ea9f33 61
mbedalvaro 40:3ba2b0ea9f33 62 // ======================================================================================================================
mbedalvaro 42:5f21a710ebc5 63 // DEFAULT CALIBRATION MATRICES (Extrinsics and Intrinsics).
mbedalvaro 40:3ba2b0ea9f33 64 // -> would be better to load them from a system file!! (TO DO)
mbedalvaro 40:3ba2b0ea9f33 65 // Note: last row is completed automatically in the loadExtrinsicMatrix and loadProjMatrix methods
mbedalvaro 40:3ba2b0ea9f33 66 // Extrinsics of usb camera to projector (note: calibration was in cm, so the translation vector is in cm)
mbedalvaro 40:3ba2b0ea9f33 67 float E1[12] = {9.8946330287356954e-01, -5.0979372852926171e-03, -1.4469410251272136e-01, 5.8356271792647112e+00,
mbedalvaro 42:5f21a710ebc5 68 -4.7536907255693065e-03, 9.9769720674821438e-01, -6.7658599389111715e-02, -6.0821695799283804e+00,
mbedalvaro 42:5f21a710ebc5 69 1.4470582120637832e-01, 6.7633532232509466e-02, 9.8716053943963022e-01, -8.2545447546057940e+00
mbedalvaro 42:5f21a710ebc5 70 };
mbedalvaro 40:3ba2b0ea9f33 71
mbedalvaro 40:3ba2b0ea9f33 72 // 9.9540692993883650e-01, -8.5578652572728450e-03, -9.5350966287597705e-02, 2.2256570914518332e+00,
mbedalvaro 42:5f21a710ebc5 73 // 1.4872282274740087e-02, 9.9772789680840523e-01, 6.5710418886328711e-02, -6.9310510316834923e+00,
mbedalvaro 42:5f21a710ebc5 74 // 9.4571978141945845e-02, -6.6826692814433777e-02, 9.9327253766416224e-01, 1.0699927478132011e+01
mbedalvaro 40:3ba2b0ea9f33 75 //};
mbedalvaro 40:3ba2b0ea9f33 76
mbedalvaro 40:3ba2b0ea9f33 77
mbedalvaro 40:3ba2b0ea9f33 78 // INTRINSICS of laser projector:
mbedalvaro 40:3ba2b0ea9f33 79 float K1[6] = { 5.9957148864529627e+03, 0.0000000e+000, 2.8869455045867635e+03,
mbedalvaro 40:3ba2b0ea9f33 80 0.0000000e+000, 5.9647451141521778e+03, 2.9005238051336592e+03
mbedalvaro 40:3ba2b0ea9f33 81 };
mbedalvaro 40:3ba2b0ea9f33 82
mbedalvaro 42:5f21a710ebc5 83 // 7.0776317995422105e+03, 0., 2.4203185896583996e+03,
mbedalvaro 40:3ba2b0ea9f33 84 // 0., 7.2889265345161484e+03, 1.7718110988625751e+03};
mbedalvaro 40:3ba2b0ea9f33 85
mbedalvaro 40:3ba2b0ea9f33 86 // SCALE FACTOR for the SCAN:
mbedalvaro 40:3ba2b0ea9f33 87 float scaleFactorProjector1 = 1.0; //scale factor for the projector image (the scan is, say, 600x600, but the resolution is 4096x4096)
mbedalvaro 40:3ba2b0ea9f33 88
mbedalvaro 40:3ba2b0ea9f33 89 // ======================================================================================================================
mbedalvaro 42:5f21a710ebc5 90 // LASER TERMINAL:
mbedalvaro 42:5f21a710ebc5 91 // We assume that the "terminal" is operating on an a4 page. Units are in cm.
mbedalvaro 40:3ba2b0ea9f33 92 V2 cursorPosition(0,0);
mbedalvaro 40:3ba2b0ea9f33 93
mbedalvaro 40:3ba2b0ea9f33 94 void createTextScene();
mbedalvaro 40:3ba2b0ea9f33 95 float angle;
mbedalvaro 40:3ba2b0ea9f33 96 string textToDisplay = "HELLO";
mbedalvaro 40:3ba2b0ea9f33 97 float fontWidth = 2.5, fontHeight = 2.5; // if calibration is in cm, then this is in cm.
mbedalvaro 40:3ba2b0ea9f33 98
mbedalvaro 40:3ba2b0ea9f33 99 // ==============================================================================================================
mbedalvaro 40:3ba2b0ea9f33 100 // Some global functions and variables (find a better way to integrate this!)
mbedalvaro 40:3ba2b0ea9f33 101 extern "C" void mbed_reset();
mbedalvaro 40:3ba2b0ea9f33 102
mbedalvaro 40:3ba2b0ea9f33 103 void createSceneTest();
mbedalvaro 41:74e24a0e6e50 104 void setOrthographicView();
mbedalvaro 40:3ba2b0ea9f33 105
mbedalvaro 42:5f21a710ebc5 106 void processOSC(UDPSocketEvent e);
mbedalvaro 40:3ba2b0ea9f33 107 void interpretData();
mbedalvaro 40:3ba2b0ea9f33 108 void processSerial();
mbedalvaro 40:3ba2b0ea9f33 109
mbedalvaro 40:3ba2b0ea9f33 110 int touchedTimes = 0;
mbedalvaro 40:3ba2b0ea9f33 111 enum sensingModes {NO_SENSING, SENSING_WHOLE_SCENE, SENSING_PER_OBJECT};
mbedalvaro 42:5f21a710ebc5 112 sensingModes currentSensingMode=SENSING_PER_OBJECT;//NO_SENSING;
mbedalvaro 42:5f21a710ebc5 113
mbedalvaro 42:5f21a710ebc5 114 // ================= AUXILIARY VARIABLES FOR INTERPRETING COMMAND FROM SERIAL OR OSC =========================
mbedalvaro 42:5f21a710ebc5 115 // NOTE: the following arrays are GLOBAL (used in processOSC and processSerial, as well as in interpretData function):
mbedalvaro 42:5f21a710ebc5 116 // max of two addresses (top and sub), of a max length of 24 characters:
mbedalvaro 42:5f21a710ebc5 117 char address[2][24];
mbedalvaro 42:5f21a710ebc5 118 //long auxdata[2]; // to store a max of two arguments (note: we will only use LONGs)
mbedalvaro 42:5f21a710ebc5 119 int data[2]; // this is to have -1 as NO DATA, to detect errors.
mbedalvaro 42:5f21a710ebc5 120 int numArgs;
mbedalvaro 42:5f21a710ebc5 121
mbedalvaro 42:5f21a710ebc5 122 // An auxiliary buffer to store parameters data, or matrices, or points to load as vertices.
mbedalvaro 42:5f21a710ebc5 123 // 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
mbedalvaro 42:5f21a710ebc5 124 // we can send 3d points, this means 3 floats per point; let's limit the number of points per object to 128 (a lot)
mbedalvaro 42:5f21a710ebc5 125 float auxDataBuffer[128];
mbedalvaro 42:5f21a710ebc5 126 int auxDataBuffer_index=0;
mbedalvaro 42:5f21a710ebc5 127
mbedalvaro 40:3ba2b0ea9f33 128
mbedalvaro 40:3ba2b0ea9f33 129 // ==============================================================================================================
mbedalvaro 40:3ba2b0ea9f33 130 int main()
mbedalvaro 40:3ba2b0ea9f33 131 {
mbedalvaro 40:3ba2b0ea9f33 132
mbedalvaro 40:3ba2b0ea9f33 133 t.start();
mbedalvaro 40:3ba2b0ea9f33 134 lastTimeCreated = t.read_ms();
mbedalvaro 40:3ba2b0ea9f33 135 lastTimeSensed = t.read_ms();
mbedalvaro 40:3ba2b0ea9f33 136 angle = 0;
mbedalvaro 40:3ba2b0ea9f33 137
mbedalvaro 40:3ba2b0ea9f33 138 // Setup:
mbedalvaro 40:3ba2b0ea9f33 139 // (1) Hardware init (laser powers, positions...):
mbedalvaro 40:3ba2b0ea9f33 140 IO.init();
mbedalvaro 42:5f21a710ebc5 141
mbedalvaro 40:3ba2b0ea9f33 142 // OSC initialization:
mbedalvaro 41:74e24a0e6e50 143 // Set the Ethernet port:
mbedalvaro 41:74e24a0e6e50 144 printf("Setting up...\r\n");
mbedalvaro 40:3ba2b0ea9f33 145 EthernetErr ethErr = eth.setup();
mbedalvaro 40:3ba2b0ea9f33 146 if (ethErr) {
mbedalvaro 41:74e24a0e6e50 147 printf("Error %d in setup.\r\n", ethErr);
mbedalvaro 40:3ba2b0ea9f33 148 return -1;
mbedalvaro 40:3ba2b0ea9f33 149 }
mbedalvaro 41:74e24a0e6e50 150 printf("Setup OK\r\n");
mbedalvaro 40:3ba2b0ea9f33 151
mbedalvaro 41:74e24a0e6e50 152 //(1) Sending message:
mbedalvaro 40:3ba2b0ea9f33 153 // Set IP and Port:
mbedalvaro 40:3ba2b0ea9f33 154 sendMes.setIp( destIp );
mbedalvaro 40:3ba2b0ea9f33 155 sendMes.setPort( destPort );
mbedalvaro 40:3ba2b0ea9f33 156 // Set data:
mbedalvaro 41:74e24a0e6e50 157 sendMes.setTopAddress(topAddress);
mbedalvaro 40:3ba2b0ea9f33 158
mbedalvaro 40:3ba2b0ea9f33 159 //setting osc functionnality:
mbedalvaro 40:3ba2b0ea9f33 160 //(2) Receiving:
mbedalvaro 42:5f21a710ebc5 161 recMes.setIp( serverIp ); // not needed?
mbedalvaro 40:3ba2b0ea9f33 162 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)
mbedalvaro 40:3ba2b0ea9f33 163 osc.begin(serverPort, &processOSC); // binds the upd (osc) messages to an arbitrary listening port ("server" port), and callback function
mbedalvaro 42:5f21a710ebc5 164 wait(1);
mbedalvaro 42:5f21a710ebc5 165
mbedalvaro 42:5f21a710ebc5 166 //while(true) {
mbedalvaro 42:5f21a710ebc5 167 // // COMMUNICATION:
mbedalvaro 42:5f21a710ebc5 168 // // (a) Reading commands:
mbedalvaro 42:5f21a710ebc5 169 // // Ethernet:
mbedalvaro 42:5f21a710ebc5 170 // Net::poll(); // this will take care of calling processOSC(UDPSocketEvent e) when a new packet arrives.
mbedalvaro 42:5f21a710ebc5 171 //
mbedalvaro 42:5f21a710ebc5 172 // wait(.5);
mbedalvaro 42:5f21a710ebc5 173 // sendMes.setTopAddress("osc");
mbedalvaro 42:5f21a710ebc5 174 // sendMes.setSubAddress("working");
mbedalvaro 42:5f21a710ebc5 175 // osc.sendOsc( &sendMes );
mbedalvaro 42:5f21a710ebc5 176 // }
mbedalvaro 42:5f21a710ebc5 177
mbedalvaro 40:3ba2b0ea9f33 178 //===================================================================
mbedalvaro 40:3ba2b0ea9f33 179
mbedalvaro 40:3ba2b0ea9f33 180 /*
mbedalvaro 40:3ba2b0ea9f33 181 //EXAMPLE 1: simple geometry ======================================================
mbedalvaro 40:3ba2b0ea9f33 182 // (1) Calibration matrices (Projection, and extrinsics - if needed):
mbedalvaro 40:3ba2b0ea9f33 183 // Typically, the projection matrix is set from the PC side, or loaded by default (from file system).
mbedalvaro 40:3ba2b0ea9f33 184 lsr.loadProjMatrix(K1, scaleFactorProjector1); // we could use a wrapper, but I won't for the time being.
mbedalvaro 40:3ba2b0ea9f33 185
mbedalvaro 40:3ba2b0ea9f33 186 // (2) Create the scene in "local" coordinates:
mbedalvaro 40:3ba2b0ea9f33 187 createSceneTest(); // Create a default scene (for tests)
mbedalvaro 40:3ba2b0ea9f33 188
mbedalvaro 40:3ba2b0ea9f33 189 // (3) Set a default GLOBAL POSE and PROJECTION MATRIX (if we changed it) for rendering and start displaying:
mbedalvaro 40:3ba2b0ea9f33 190 // - Using the current K (that correct for the center of the camera too!):
mbedalvaro 40:3ba2b0ea9f33 191 //lsr.setIdentityPose();
mbedalvaro 40:3ba2b0ea9f33 192 //lsr.translate(0, 0, 2000);
mbedalvaro 40:3ba2b0ea9f33 193 // - or doing orthoprojection:
mbedalvaro 40:3ba2b0ea9f33 194 lsr.setOrthoProjection();
mbedalvaro 40:3ba2b0ea9f33 195 lsr.setIdentityPose();
mbedalvaro 40:3ba2b0ea9f33 196 lsr.translate(CENTER_AD_MIRROR_X, CENTER_AD_MIRROR_Y, 0);
mbedalvaro 40:3ba2b0ea9f33 197 lsr.rotateZ(45);
mbedalvaro 40:3ba2b0ea9f33 198 drawScene();
mbedalvaro 40:3ba2b0ea9f33 199
mbedalvaro 40:3ba2b0ea9f33 200 // *** 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.
mbedalvaro 40:3ba2b0ea9f33 201 lsr.startDisplay();
mbedalvaro 40:3ba2b0ea9f33 202
mbedalvaro 40:3ba2b0ea9f33 203 // (4) Main loop:
mbedalvaro 40:3ba2b0ea9f33 204 // Change the POSE when necessary (also called "viewing transformation" or "global modelview" in OpenGL jargon), render and display:
mbedalvaro 40:3ba2b0ea9f33 205 while (1) {
mbedalvaro 40:3ba2b0ea9f33 206
mbedalvaro 40:3ba2b0ea9f33 207 if (pc.readable())
mbedalvaro 40:3ba2b0ea9f33 208 processSerial(); // here, the pose can be changed by a pc command
mbedalvaro 40:3ba2b0ea9f33 209 hardwareKnobs();
mbedalvaro 40:3ba2b0ea9f33 210
mbedalvaro 40:3ba2b0ea9f33 211 if (0) { //((t.read_ms()-lastTimeCreated)>10) {
mbedalvaro 40:3ba2b0ea9f33 212 // for tests:
mbedalvaro 40:3ba2b0ea9f33 213 myLed2 = !myLed2;
mbedalvaro 40:3ba2b0ea9f33 214
mbedalvaro 40:3ba2b0ea9f33 215 angle += 1;
mbedalvaro 40:3ba2b0ea9f33 216 if (angle > 360)
mbedalvaro 40:3ba2b0ea9f33 217 angle = 0;
mbedalvaro 40:3ba2b0ea9f33 218
mbedalvaro 40:3ba2b0ea9f33 219 // Set the current global pose:
mbedalvaro 40:3ba2b0ea9f33 220 // lsr.setIdentityPose(); // we set the identity - then we don't need to do push pop... we start from "scratch" here. Not incremental rotation...
mbedalvaro 40:3ba2b0ea9f33 221 // lsr.translate(0, 0, 1000);
mbedalvaro 40:3ba2b0ea9f33 222 // lsr.rotateZ(angle);
mbedalvaro 40:3ba2b0ea9f33 223 // lsr.rotateX(2.5 * angle);
mbedalvaro 40:3ba2b0ea9f33 224
mbedalvaro 40:3ba2b0ea9f33 225 lsr.setOrthoProjection();
mbedalvaro 40:3ba2b0ea9f33 226 lsr.setIdentityPose();
mbedalvaro 40:3ba2b0ea9f33 227 lsr.translate(CENTER_AD_MIRROR_X, CENTER_AD_MIRROR_Y, 0);
mbedalvaro 40:3ba2b0ea9f33 228 lsr.rotateZ(angle);
mbedalvaro 40:3ba2b0ea9f33 229 // lsr.rotateY( angle);
mbedalvaro 40:3ba2b0ea9f33 230
mbedalvaro 40:3ba2b0ea9f33 231 // 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)
mbedalvaro 40:3ba2b0ea9f33 232 drawScene();
mbedalvaro 40:3ba2b0ea9f33 233
mbedalvaro 40:3ba2b0ea9f33 234 lastTimeCreated = t.read_ms();
mbedalvaro 40:3ba2b0ea9f33 235 } // end auto-change pose loop
mbedalvaro 40:3ba2b0ea9f33 236
mbedalvaro 40:3ba2b0ea9f33 237 if ((t.read_ms() - lastTimeSensed) > 50) { // check data sensing every xx ms
mbedalvaro 40:3ba2b0ea9f33 238 // Check objects and change colors or other properties as a function of the touch:
mbedalvaro 40:3ba2b0ea9f33 239 // NOTE: the testing #define debugDelayMirrors should not be defined in the laserSensingDisplay.h
mbedalvaro 40:3ba2b0ea9f33 240 // (note: we assume we are drawing the text).
mbedalvaro 40:3ba2b0ea9f33 241 // 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.
mbedalvaro 40:3ba2b0ea9f33 242 for (int i = 0; i < scene.totalObjects(); i++) {
mbedalvaro 40:3ba2b0ea9f33 243 BaseObject* ptrObj = scene.objectArray[i];
mbedalvaro 40:3ba2b0ea9f33 244 if (ptrObj->sense()) { // this calls to ptrObj->displaySensingBuffer.processSensedData()
mbedalvaro 40:3ba2b0ea9f33 245 ptrObj->setColor(0x02);
mbedalvaro 40:3ba2b0ea9f33 246
mbedalvaro 40:3ba2b0ea9f33 247 // char str[15];
mbedalvaro 40:3ba2b0ea9f33 248 // sprintf(str, "%d", touchedTimes);
mbedalvaro 40:3ba2b0ea9f33 249 // textToDisplay=string(str);//string(itoa(touchedTimes));
mbedalvaro 40:3ba2b0ea9f33 250 // touchedTimes++;
mbedalvaro 40:3ba2b0ea9f33 251 // createTextScene();
mbedalvaro 40:3ba2b0ea9f33 252
mbedalvaro 40:3ba2b0ea9f33 253 } else
mbedalvaro 40:3ba2b0ea9f33 254 ptrObj->setColor(0x01);
mbedalvaro 40:3ba2b0ea9f33 255 }
mbedalvaro 40:3ba2b0ea9f33 256 lastTimeSensed = t.read_ms();
mbedalvaro 40:3ba2b0ea9f33 257 } // end sensing loop
mbedalvaro 40:3ba2b0ea9f33 258
mbedalvaro 40:3ba2b0ea9f33 259 } // "end" infite loop
mbedalvaro 40:3ba2b0ea9f33 260 // =================================
mbedalvaro 40:3ba2b0ea9f33 261 */
mbedalvaro 40:3ba2b0ea9f33 262
mbedalvaro 40:3ba2b0ea9f33 263 // EXAMPLE 2: 3d text ======================================================
mbedalvaro 40:3ba2b0ea9f33 264
mbedalvaro 40:3ba2b0ea9f33 265 // (1) Calibration matrices:
mbedalvaro 40:3ba2b0ea9f33 266 // Typically, the projection and extrinsic matrix is set from the PC side, or loaded by default (from file system).
mbedalvaro 40:3ba2b0ea9f33 267 lsr.loadProjMatrix(K1, scaleFactorProjector1); // we could use a wrapper, but I won't for the time being.
mbedalvaro 40:3ba2b0ea9f33 268 lsr.loadExtrinsicsMatrix(E1);
mbedalvaro 40:3ba2b0ea9f33 269
mbedalvaro 40:3ba2b0ea9f33 270 // (2) Create the scene in "local" coordinates:
mbedalvaro 42:5f21a710ebc5 271 createTextScene(); // Create a default scene (for tests)
mbedalvaro 42:5f21a710ebc5 272
mbedalvaro 42:5f21a710ebc5 273 // or start with nothing:
mbedalvaro 42:5f21a710ebc5 274 //clearScene();
mbedalvaro 42:5f21a710ebc5 275 //setOrthographicView();
mbedalvaro 40:3ba2b0ea9f33 276
mbedalvaro 40:3ba2b0ea9f33 277 // (3) Set a default PERSPECTIVE and GLOBAL POSE for start displaying:
mbedalvaro 40:3ba2b0ea9f33 278 lsr.loadProjMatrix(K1, scaleFactorProjector1);
mbedalvaro 40:3ba2b0ea9f33 279 lsr.setIdentityPose();
mbedalvaro 40:3ba2b0ea9f33 280 lsr.multPoseMatrix(lsr.EXTRINSICS); // RT=E // we can simply do: lsr.setExtrinsicsPose()
mbedalvaro 40:3ba2b0ea9f33 281
mbedalvaro 40:3ba2b0ea9f33 282 // DEFAULT position in CAMERA coordinates (calibration was in cm):
mbedalvaro 40:3ba2b0ea9f33 283 lsr.translate(0, 0, 130);
mbedalvaro 40:3ba2b0ea9f33 284
mbedalvaro 40:3ba2b0ea9f33 285 drawScene(); // this will be called when we change the pose matrix - but needs to be called at least once to start displaying
mbedalvaro 40:3ba2b0ea9f33 286
mbedalvaro 40:3ba2b0ea9f33 287 // *** 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.
mbedalvaro 40:3ba2b0ea9f33 288 startDisplay();
mbedalvaro 40:3ba2b0ea9f33 289
mbedalvaro 40:3ba2b0ea9f33 290 // (4) Main loop:
mbedalvaro 40:3ba2b0ea9f33 291 while(true) {
mbedalvaro 42:5f21a710ebc5 292
mbedalvaro 40:3ba2b0ea9f33 293 // =========== Query hardware events (knobs, ethernet, serial port) ============
mbedalvaro 40:3ba2b0ea9f33 294 Net::poll(); // This polls the network connection for new activity, without keeping on calling this you won't receive any OSC!
mbedalvaro 40:3ba2b0ea9f33 295 if (pc.readable()) processSerial(); // here, the pose can be changed by a pc command
mbedalvaro 40:3ba2b0ea9f33 296 hardwareKnobs();
mbedalvaro 40:3ba2b0ea9f33 297 // =============================================================================
mbedalvaro 40:3ba2b0ea9f33 298
mbedalvaro 40:3ba2b0ea9f33 299 // Automatic change of pose for tests
mbedalvaro 40:3ba2b0ea9f33 300 if (0) {//(t.read_ms()-lastTimeCreated)>20) {
mbedalvaro 40:3ba2b0ea9f33 301 angle+=.2;
mbedalvaro 40:3ba2b0ea9f33 302 if (angle>360) angle=0;
mbedalvaro 40:3ba2b0ea9f33 303
mbedalvaro 40:3ba2b0ea9f33 304 lsr.setIdentityPose(); // we set the identity - then we don't need to do push pop... we start from "scratch" here. Not incremental rotation...
mbedalvaro 40:3ba2b0ea9f33 305 lsr.flipY();
mbedalvaro 40:3ba2b0ea9f33 306 lsr.translate(0,0,1000);
mbedalvaro 40:3ba2b0ea9f33 307 lsr.rotateZ(angle);
mbedalvaro 40:3ba2b0ea9f33 308 //lsr.rotateX(angle);
mbedalvaro 40:3ba2b0ea9f33 309
mbedalvaro 40:3ba2b0ea9f33 310 drawScene();
mbedalvaro 40:3ba2b0ea9f33 311
mbedalvaro 40:3ba2b0ea9f33 312 lastTimeCreated=t.read_ms();
mbedalvaro 40:3ba2b0ea9f33 313 }
mbedalvaro 40:3ba2b0ea9f33 314
mbedalvaro 40:3ba2b0ea9f33 315 if ((currentSensingMode!=NO_SENSING)&&((t.read_ms()-lastTimeSensed)>80)) { // check data sensing every xx ms
mbedalvaro 40:3ba2b0ea9f33 316 // Check objects and change colors or other properties as a function of the touch:
mbedalvaro 40:3ba2b0ea9f33 317 // NOTE: the testing #define debugDelayMirrors should not be defined in the laserSensingDisplay.h
mbedalvaro 40:3ba2b0ea9f33 318 // (note: we assume we are drawing the text).
mbedalvaro 40:3ba2b0ea9f33 319 // 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.
mbedalvaro 40:3ba2b0ea9f33 320 switch(currentSensingMode) {
mbedalvaro 40:3ba2b0ea9f33 321 case SENSING_WHOLE_SCENE: // Sensing whole scene ----------
mbedalvaro 40:3ba2b0ea9f33 322 if (scene.sense()) {
mbedalvaro 40:3ba2b0ea9f33 323
mbedalvaro 40:3ba2b0ea9f33 324 // COUNTING:
mbedalvaro 40:3ba2b0ea9f33 325 // char str[15];
mbedalvaro 40:3ba2b0ea9f33 326 // sprintf(str, "%d", touchedTimes);
mbedalvaro 40:3ba2b0ea9f33 327 // textToDisplay=string(str);//string(itoa(touchedTimes));
mbedalvaro 40:3ba2b0ea9f33 328 // touchedTimes++;
mbedalvaro 40:3ba2b0ea9f33 329 // createTextScene();
mbedalvaro 40:3ba2b0ea9f33 330 // (3) Set a default PERSPECTIVE and GLOBAL POSE for start displaying:
mbedalvaro 40:3ba2b0ea9f33 331 // lsr.loadProjMatrix(K1, scaleFactorProjector1);
mbedalvaro 40:3ba2b0ea9f33 332 // lsr.setIdentityPose();
mbedalvaro 40:3ba2b0ea9f33 333 // lsr.multPoseMatrix(lsr.EXTRINSICS); // RT=E // we can simply do: lsr.setExtrinsicsPose()
mbedalvaro 40:3ba2b0ea9f33 334 // This means that the current coordinate frame is the CAMERA, and calibration was in cm:
mbedalvaro 40:3ba2b0ea9f33 335 // lsr.translate(0, 0, 100);
mbedalvaro 40:3ba2b0ea9f33 336 ////lsr.flipY();
mbedalvaro 40:3ba2b0ea9f33 337 // drawScene(); // this will be called when we change the pose matrix - but needs to be called at least once to start displaying
mbedalvaro 40:3ba2b0ea9f33 338 // }
mbedalvaro 40:3ba2b0ea9f33 339 }
mbedalvaro 40:3ba2b0ea9f33 340 break;
mbedalvaro 40:3ba2b0ea9f33 341 case SENSING_PER_OBJECT: // Per-object sensing and action:
mbedalvaro 40:3ba2b0ea9f33 342 for (int i=0; i<textToDisplay.length(); i++) {
mbedalvaro 40:3ba2b0ea9f33 343 BaseObject* ptrObj=scene.objectArray[i];
mbedalvaro 40:3ba2b0ea9f33 344 if (ptrObj->sense()) { // this calls to ptrObj->displaySensingBuffer.processSensedData()
mbedalvaro 40:3ba2b0ea9f33 345 ptrObj->setColor(0x02); // make it green
mbedalvaro 40:3ba2b0ea9f33 346
mbedalvaro 40:3ba2b0ea9f33 347 // for fun: let's rotate THIS letter:
mbedalvaro 40:3ba2b0ea9f33 348 //lsr.pushPoseMatrix();
mbedalvaro 40:3ba2b0ea9f33 349 //lsr.setIdentityPose();
mbedalvaro 40:3ba2b0ea9f33 350 //lsr.rotateX(180);
mbedalvaro 40:3ba2b0ea9f33 351 //transformObject(ptrObj);
mbedalvaro 40:3ba2b0ea9f33 352 //lsr.popPoseMatrix();
mbedalvaro 40:3ba2b0ea9f33 353 // we need to redraw the scene (ie, reproject), or at least this object:
mbedalvaro 40:3ba2b0ea9f33 354 //lsr.renderObject(ptrObj, lsr.RT);
mbedalvaro 40:3ba2b0ea9f33 355
mbedalvaro 42:5f21a710ebc5 356 // or "highlight" the object by putting "cartouche" around it object (note: do this only if it did NOT have
mbedalvaro 40:3ba2b0ea9f33 357 //addCartoucheObject(ptrObj);
mbedalvaro 40:3ba2b0ea9f33 358
mbedalvaro 40:3ba2b0ea9f33 359 //pc.putc(i+48);
mbedalvaro 40:3ba2b0ea9f33 360
mbedalvaro 40:3ba2b0ea9f33 361 } else
mbedalvaro 40:3ba2b0ea9f33 362 ptrObj->setColor(0x01); // make it blue
mbedalvaro 40:3ba2b0ea9f33 363 }
mbedalvaro 40:3ba2b0ea9f33 364 break;
mbedalvaro 40:3ba2b0ea9f33 365 default:
mbedalvaro 40:3ba2b0ea9f33 366 break;
mbedalvaro 40:3ba2b0ea9f33 367 }
mbedalvaro 40:3ba2b0ea9f33 368 lastTimeSensed=t.read_ms();
mbedalvaro 40:3ba2b0ea9f33 369 } // end sensing
mbedalvaro 40:3ba2b0ea9f33 370
mbedalvaro 40:3ba2b0ea9f33 371 } // infinite while loop
mbedalvaro 40:3ba2b0ea9f33 372 // =================================
mbedalvaro 40:3ba2b0ea9f33 373 }
mbedalvaro 40:3ba2b0ea9f33 374
mbedalvaro 42:5f21a710ebc5 375 void setOrthographicView()
mbedalvaro 42:5f21a710ebc5 376 {
mbedalvaro 42:5f21a710ebc5 377 // clearScene();
mbedalvaro 42:5f21a710ebc5 378 // set color:
mbedalvaro 42:5f21a710ebc5 379 lsr.setColor(0x02); // green
mbedalvaro 42:5f21a710ebc5 380 //IO.setLaserLockinPower(0); // DISABLING SENSING LASER! (here the red). This is FOR CALIBRATION laser-camera
mbedalvaro 42:5f21a710ebc5 381
mbedalvaro 42:5f21a710ebc5 382 lsr.setOrthoProjection();
mbedalvaro 42:5f21a710ebc5 383 lsr.setIdentityPose(); // we could have done the translation here instead of in "local coordinates", but it's the same.
mbedalvaro 41:74e24a0e6e50 384 }
mbedalvaro 41:74e24a0e6e50 385
mbedalvaro 40:3ba2b0ea9f33 386 void createSceneTest()
mbedalvaro 40:3ba2b0ea9f33 387 {
mbedalvaro 40:3ba2b0ea9f33 388 lsr.pushPoseMatrix();
mbedalvaro 40:3ba2b0ea9f33 389
mbedalvaro 40:3ba2b0ea9f33 390 // (1) Clear the scene (this stops displaying, thus creating a "laser hot spot", but in principle it only happens in the setup)
mbedalvaro 40:3ba2b0ea9f33 391 clearScene(); // NOTE: in the future (and if we have enough RAM, we can have several indexed "scenes", instead of a unique global)
mbedalvaro 40:3ba2b0ea9f33 392
mbedalvaro 40:3ba2b0ea9f33 393 // (2) Set displaying attributes (colors and local pose):
mbedalvaro 40:3ba2b0ea9f33 394 // (a) Set the "local pose" (i.e., the MODELING TRANSFORMATION in OpenGL jargon):
mbedalvaro 40:3ba2b0ea9f33 395 lsr.setIdentityPose(); // we could use a wrapper, but I won't for the time being.
mbedalvaro 40:3ba2b0ea9f33 396 // (b) set color:
mbedalvaro 40:3ba2b0ea9f33 397 lsr.setColor(0x02);
mbedalvaro 40:3ba2b0ea9f33 398
mbedalvaro 40:3ba2b0ea9f33 399 // Add a first object: using openGL-like syntaxis (here, a closed square):
mbedalvaro 40:3ba2b0ea9f33 400 // begin(1); // start creating object with identifier = 1
mbedalvaro 40:3ba2b0ea9f33 401 // vertex(10,10,1);
mbedalvaro 40:3ba2b0ea9f33 402 // vertex(10,100,1);
mbedalvaro 40:3ba2b0ea9f33 403 // vertex(100,100,1);
mbedalvaro 40:3ba2b0ea9f33 404 // vertex(10,100,1);
mbedalvaro 40:3ba2b0ea9f33 405 // vertex(10,10,1); //
mbedalvaro 40:3ba2b0ea9f33 406 // end();
mbedalvaro 40:3ba2b0ea9f33 407
mbedalvaro 40:3ba2b0ea9f33 408 // Add another objects using "object primitives":
mbedalvaro 40:3ba2b0ea9f33 409 //begin(2);
mbedalvaro 40:3ba2b0ea9f33 410 //lsr.translate(-100,-100,0); // recenter the square
mbedalvaro 40:3ba2b0ea9f33 411 //square(200, 10); // this square has a corner in (0,0), and by default, z=0
mbedalvaro 40:3ba2b0ea9f33 412 //end();
mbedalvaro 40:3ba2b0ea9f33 413
mbedalvaro 40:3ba2b0ea9f33 414 // A "cube" (with one corner at the origin):
mbedalvaro 40:3ba2b0ea9f33 415 //begin(3);
mbedalvaro 40:3ba2b0ea9f33 416 //lsr.setIdentityPose(); // we set the identity - then we don't need to do push pop... we start from "scratch" here. Not incremental rotation...
mbedalvaro 40:3ba2b0ea9f33 417 //lsr.translate(100,-100,-100); // recenter the cube (no need to push pop because we are seting the identity above)
mbedalvaro 40:3ba2b0ea9f33 418 //lsr.rotateY(30);
mbedalvaro 40:3ba2b0ea9f33 419 //cube(200,10); // cube(200, 20);
mbedalvaro 40:3ba2b0ea9f33 420 //end();
mbedalvaro 40:3ba2b0ea9f33 421
mbedalvaro 40:3ba2b0ea9f33 422 // A grid:
mbedalvaro 40:3ba2b0ea9f33 423 // IMPORTANT: a grid is a multi-object built, and DOES NOT NEED to be between a begin and end
mbedalvaro 40:3ba2b0ea9f33 424 lsr.setIdentityPose(); // we set the identity - then we don't need to do push pop... we start from "scratch" here. Not incremental rotation...
mbedalvaro 40:3ba2b0ea9f33 425 lsr.translate(-500, -500, 0);
mbedalvaro 40:3ba2b0ea9f33 426 grid(1000, 1000, 5, 5, 1); //grid(float sizeX, float sizeY, int nx, int ny, int repeatpoint);
mbedalvaro 40:3ba2b0ea9f33 427 //gridCircles(300, 250, 6, 5, 10, 6); // gridCircles(float sizeX, float sizeY, int nx, int ny, float radius, int nbpointsCircle)
mbedalvaro 40:3ba2b0ea9f33 428
mbedalvaro 40:3ba2b0ea9f33 429 // A circle:
mbedalvaro 40:3ba2b0ea9f33 430 //begin(4);
mbedalvaro 40:3ba2b0ea9f33 431 //circle(200, 100);
mbedalvaro 40:3ba2b0ea9f33 432 //end();
mbedalvaro 40:3ba2b0ea9f33 433
mbedalvaro 40:3ba2b0ea9f33 434 updateScene(); // this is important: this will compute the number of objects, points, etc, to prepare the displaying engine (but rendering is NOT yet done,
mbedalvaro 40:3ba2b0ea9f33 435 // rendering is done in the drawScene() function.
mbedalvaro 40:3ba2b0ea9f33 436
mbedalvaro 40:3ba2b0ea9f33 437 // pc.printf("Number of objects in the scene: %d\n", scene.totalObjects());
mbedalvaro 40:3ba2b0ea9f33 438 // pc.printf("Number of points in the scene: %d\n", scene.totalPoints());
mbedalvaro 40:3ba2b0ea9f33 439
mbedalvaro 40:3ba2b0ea9f33 440 lsr.popPoseMatrix();
mbedalvaro 40:3ba2b0ea9f33 441 }
mbedalvaro 40:3ba2b0ea9f33 442
mbedalvaro 40:3ba2b0ea9f33 443 void createTextScene()
mbedalvaro 40:3ba2b0ea9f33 444 {
mbedalvaro 40:3ba2b0ea9f33 445 lsr.pushPoseMatrix();
mbedalvaro 40:3ba2b0ea9f33 446 //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).
mbedalvaro 40:3ba2b0ea9f33 447 // The first option is simpler now, we just use the wrapper function string3d(string _text, float totalWidth, float height):
mbedalvaro 40:3ba2b0ea9f33 448
mbedalvaro 40:3ba2b0ea9f33 449 // (1) Clear the scene (this stops displaying, thus creating a "laser hot spot", but in principle it only happens in the setup)
mbedalvaro 40:3ba2b0ea9f33 450 clearScene(); // NOTE: in the future (and if we have enough RAM, we can have several indexed "scenes", instead of a unique global)
mbedalvaro 40:3ba2b0ea9f33 451
mbedalvaro 40:3ba2b0ea9f33 452 // (2) Set displaying attributes (colors and pose):
mbedalvaro 40:3ba2b0ea9f33 453 // (a) Set the "local pose" (i.e., the MODELING TRANSFORMATION in OpenGL jargon):
mbedalvaro 40:3ba2b0ea9f33 454 lsr.setIdentityPose(); // we could use a wrapper, but I won't for the time being.
mbedalvaro 40:3ba2b0ea9f33 455 //lsr.flipY();
mbedalvaro 40:3ba2b0ea9f33 456 //lsr.translate(2,6,0);
mbedalvaro 40:3ba2b0ea9f33 457
mbedalvaro 40:3ba2b0ea9f33 458 lsr.flipY();
mbedalvaro 40:3ba2b0ea9f33 459 lsr.flipX();
mbedalvaro 40:3ba2b0ea9f33 460 lsr.translate(-15,-10,0);
mbedalvaro 40:3ba2b0ea9f33 461
mbedalvaro 40:3ba2b0ea9f33 462 // (b) set current color:
mbedalvaro 40:3ba2b0ea9f33 463 lsr.setColor(0x04); // three LSB bits for RGB - so 0x02 is GREEN ON (note that the sensing laser, now red, is always on...).
mbedalvaro 40:3ba2b0ea9f33 464
mbedalvaro 40:3ba2b0ea9f33 465 // A string as a UNIQUE object:
mbedalvaro 40:3ba2b0ea9f33 466 // begin(1); // start creating an object with index 1
mbedalvaro 40:3ba2b0ea9f33 467 // no retouching of the modelview, meaning the text is in (0,0,0) in "local" coordinates
mbedalvaro 40:3ba2b0ea9f33 468 // string3d(textToDisplay, textToDisplay.size()*fontWidth, fontHeight);
mbedalvaro 40:3ba2b0ea9f33 469 //end();
mbedalvaro 40:3ba2b0ea9f33 470
mbedalvaro 40:3ba2b0ea9f33 471 // A string as a *collection* of object-letters:
mbedalvaro 40:3ba2b0ea9f33 472 //lsr.translate(-textToDisplay.length()*fontWidth/2,-fontHeight/2,0);
mbedalvaro 40:3ba2b0ea9f33 473 for (unsigned short i = 0; i < textToDisplay.length(); i++) {
mbedalvaro 40:3ba2b0ea9f33 474 char ch = textToDisplay.at(i);
mbedalvaro 40:3ba2b0ea9f33 475 if (ch != ' ') {
mbedalvaro 40:3ba2b0ea9f33 476 begin(i);
mbedalvaro 40:3ba2b0ea9f33 477 letter3d(ch, fontWidth, fontHeight);
mbedalvaro 40:3ba2b0ea9f33 478 end();
mbedalvaro 40:3ba2b0ea9f33 479 }
mbedalvaro 40:3ba2b0ea9f33 480 lsr.translate(1.1*fontWidth, 0, 0);
mbedalvaro 40:3ba2b0ea9f33 481 }
mbedalvaro 40:3ba2b0ea9f33 482
mbedalvaro 40:3ba2b0ea9f33 483 updateScene(); // this is important: this will compute the number of objects, points, etc, to prepare the displaying engine (but rendering is NOT yet done,
mbedalvaro 40:3ba2b0ea9f33 484 // rendering is done in the drawScene() function.
mbedalvaro 40:3ba2b0ea9f33 485
mbedalvaro 40:3ba2b0ea9f33 486 lsr.popPoseMatrix();
mbedalvaro 40:3ba2b0ea9f33 487
mbedalvaro 40:3ba2b0ea9f33 488 }
mbedalvaro 40:3ba2b0ea9f33 489
mbedalvaro 42:5f21a710ebc5 490 void addText()
mbedalvaro 42:5f21a710ebc5 491 {
mbedalvaro 42:5f21a710ebc5 492 lsr.pushPoseMatrix();
mbedalvaro 42:5f21a710ebc5 493 //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).
mbedalvaro 42:5f21a710ebc5 494 // The first option is simpler now, we just use the wrapper function string3d(string _text, float totalWidth, float height):
mbedalvaro 42:5f21a710ebc5 495
mbedalvaro 42:5f21a710ebc5 496 // (1) Clear the scene (this stops displaying, thus creating a "laser hot spot", but in principle it only happens in the setup)
mbedalvaro 42:5f21a710ebc5 497 // clearScene(); // NOTE: in the future (and if we have enough RAM, we can have several indexed "scenes", instead of a unique global)
mbedalvaro 42:5f21a710ebc5 498
mbedalvaro 42:5f21a710ebc5 499 // (2) Set displaying attributes (colors and pose):
mbedalvaro 42:5f21a710ebc5 500 // (a) Set the "local pose" (i.e., the MODELING TRANSFORMATION in OpenGL jargon):
mbedalvaro 42:5f21a710ebc5 501 lsr.setIdentityPose(); // we could use a wrapper, but I won't for the time being.
mbedalvaro 42:5f21a710ebc5 502 //lsr.flipY();
mbedalvaro 42:5f21a710ebc5 503 //lsr.translate(2,6,0);
mbedalvaro 42:5f21a710ebc5 504
mbedalvaro 42:5f21a710ebc5 505 lsr.flipY();
mbedalvaro 42:5f21a710ebc5 506 lsr.flipX();
mbedalvaro 42:5f21a710ebc5 507 lsr.translate(-15,-10,0);
mbedalvaro 42:5f21a710ebc5 508
mbedalvaro 42:5f21a710ebc5 509 // (b) set current color:
mbedalvaro 42:5f21a710ebc5 510 lsr.setColor(0x04); // three LSB bits for RGB - so 0x02 is GREEN ON (note that the sensing laser, now red, is always on...).
mbedalvaro 40:3ba2b0ea9f33 511
mbedalvaro 42:5f21a710ebc5 512 // A string as a UNIQUE object:
mbedalvaro 42:5f21a710ebc5 513 // begin(1); // start creating an object with index 1
mbedalvaro 42:5f21a710ebc5 514 // no retouching of the modelview, meaning the text is in (0,0,0) in "local" coordinates
mbedalvaro 42:5f21a710ebc5 515 // string3d(textToDisplay, textToDisplay.size()*fontWidth, fontHeight);
mbedalvaro 42:5f21a710ebc5 516 //end();
mbedalvaro 42:5f21a710ebc5 517
mbedalvaro 42:5f21a710ebc5 518 // A string as a *collection* of object-letters:
mbedalvaro 42:5f21a710ebc5 519 //lsr.translate(-textToDisplay.length()*fontWidth/2,-fontHeight/2,0);
mbedalvaro 42:5f21a710ebc5 520 for (unsigned short i = 0; i < textToDisplay.length(); i++) {
mbedalvaro 42:5f21a710ebc5 521 char ch = textToDisplay.at(i);
mbedalvaro 42:5f21a710ebc5 522 if (ch != ' ') {
mbedalvaro 42:5f21a710ebc5 523 begin(i);
mbedalvaro 42:5f21a710ebc5 524 letter3d(ch, fontWidth, fontHeight);
mbedalvaro 42:5f21a710ebc5 525 end();
mbedalvaro 42:5f21a710ebc5 526 }
mbedalvaro 42:5f21a710ebc5 527 lsr.translate(1.1*fontWidth, 0, 0);
mbedalvaro 42:5f21a710ebc5 528 }
mbedalvaro 42:5f21a710ebc5 529
mbedalvaro 42:5f21a710ebc5 530 updateScene(); // this is important: this will compute the number of objects, points, etc, to prepare the displaying engine (but rendering is NOT yet done,
mbedalvaro 42:5f21a710ebc5 531 // rendering is done in the drawScene() function.
mbedalvaro 42:5f21a710ebc5 532
mbedalvaro 42:5f21a710ebc5 533 lsr.popPoseMatrix();
mbedalvaro 42:5f21a710ebc5 534
mbedalvaro 42:5f21a710ebc5 535 }
mbedalvaro 40:3ba2b0ea9f33 536
mbedalvaro 40:3ba2b0ea9f33 537 //================== OSC messageReceivedCallback function =====================
mbedalvaro 40:3ba2b0ea9f33 538 //============= RECEIVE OSC COMMANDS =========================
mbedalvaro 40:3ba2b0ea9f33 539 // This is the callback function called when there are packets on the listening socket. It is not nice to have it
mbedalvaro 40:3ba2b0ea9f33 540 // here, but for the time being having a "wrapping global" is the simplest solution (we cannot pass a member-function pointer
mbedalvaro 40:3ba2b0ea9f33 541 // as handler to the upd object).
mbedalvaro 42:5f21a710ebc5 542 void processOSC(UDPSocketEvent e)
mbedalvaro 42:5f21a710ebc5 543 {
mbedalvaro 40:3ba2b0ea9f33 544 osc.onUDPSocketEvent(e);
mbedalvaro 40:3ba2b0ea9f33 545
mbedalvaro 40:3ba2b0ea9f33 546 if (osc.newMessage) {
mbedalvaro 42:5f21a710ebc5 547
mbedalvaro 42:5f21a710ebc5 548 // pc.printf("RECEIVED NEW OSC MESSAGE ----\n");
mbedalvaro 42:5f21a710ebc5 549
mbedalvaro 40:3ba2b0ea9f33 550 // Acquire the addresses and arguments and put them in the GLOBAL variables to be processed by the interpretData function (common to serial and OSC):
mbedalvaro 40:3ba2b0ea9f33 551 strcpy(address[0],"");
mbedalvaro 40:3ba2b0ea9f33 552 strcpy(address[1],"");
mbedalvaro 42:5f21a710ebc5 553 for (int i=0; i<recMes.getAddressNum(); i++) {
mbedalvaro 42:5f21a710ebc5 554 strcpy(address[i],recMes.getAddress(i)); // NOTE: up to the rest of the program to check if address[1] is really not null
mbedalvaro 42:5f21a710ebc5 555 // pc.printf("Address %d = %s \n",i, address[i]);
mbedalvaro 42:5f21a710ebc5 556 }
mbedalvaro 40:3ba2b0ea9f33 557 // Acquire data:
mbedalvaro 42:5f21a710ebc5 558
mbedalvaro 40:3ba2b0ea9f33 559 numArgs=recMes.getArgNum();
mbedalvaro 40:3ba2b0ea9f33 560 data[0]=-1;
mbedalvaro 40:3ba2b0ea9f33 561 data[1]=-1;
mbedalvaro 42:5f21a710ebc5 562 for (int i=0; i<numArgs; i++) {
mbedalvaro 42:5f21a710ebc5 563 data[i]=(int)recMes.getArgInt(i);//recMes.getArgFloat(i);
mbedalvaro 42:5f21a710ebc5 564 //pc.printf("%4.2f %4.2f \n", data[i], recMes.getArgFloat(i));
mbedalvaro 42:5f21a710ebc5 565 // pc.printf("%i %i \n", data[i], recMes.getArgInt(i));
mbedalvaro 42:5f21a710ebc5 566 }
mbedalvaro 40:3ba2b0ea9f33 567
mbedalvaro 40:3ba2b0ea9f33 568 // Finally, interpret the command or data:
mbedalvaro 42:5f21a710ebc5 569 interpretData();
mbedalvaro 42:5f21a710ebc5 570
mbedalvaro 42:5f21a710ebc5 571 }
mbedalvaro 40:3ba2b0ea9f33 572 }
mbedalvaro 40:3ba2b0ea9f33 573
mbedalvaro 40:3ba2b0ea9f33 574
mbedalvaro 40:3ba2b0ea9f33 575
mbedalvaro 40:3ba2b0ea9f33 576 //interpretData(const char& address[2][], const int& data[2]) {
mbedalvaro 42:5f21a710ebc5 577 void interpretData()
mbedalvaro 42:5f21a710ebc5 578 {
mbedalvaro 42:5f21a710ebc5 579
mbedalvaro 40:3ba2b0ea9f33 580 // ========== DATA (all this is a hack because my OSC class does not accept BUNDLES...) =============
mbedalvaro 40:3ba2b0ea9f33 581 // Add data to auxiliary data buffer:
mbedalvaro 40:3ba2b0ea9f33 582 if (!strcmp(address[0], "data")) {
mbedalvaro 40:3ba2b0ea9f33 583 // numArgs is the number of arguments (in my OSC version this is max 2):
mbedalvaro 42:5f21a710ebc5 584 // pc.printf("--- Data pack: %d\n", numArgs);
mbedalvaro 42:5f21a710ebc5 585 for (int i=0; i<numArgs; i++) {
mbedalvaro 42:5f21a710ebc5 586 auxDataBuffer[auxDataBuffer_index+i] = 1.0*data[i];
mbedalvaro 42:5f21a710ebc5 587 // note: the index will be reset to 0 or whatever needed when a command USING data from the "d" message is received.
mbedalvaro 42:5f21a710ebc5 588 // pc.printf("buffer index / data: %i %f \n", auxDataBuffer_index+i, auxDataBuffer[auxDataBuffer_index+i]);//data[i]);
mbedalvaro 42:5f21a710ebc5 589 }
mbedalvaro 42:5f21a710ebc5 590 auxDataBuffer_index+=numArgs;
mbedalvaro 42:5f21a710ebc5 591 //pc.putc('/n');
mbedalvaro 40:3ba2b0ea9f33 592 }
mbedalvaro 40:3ba2b0ea9f33 593 // ========== COMMANDS (using data on auxDataBuffer or directly as arguments =========================
mbedalvaro 42:5f21a710ebc5 594
mbedalvaro 40:3ba2b0ea9f33 595 else if ( !strcmp(address[0], "mbedReset" ) ) mbed_reset();
mbedalvaro 42:5f21a710ebc5 596
mbedalvaro 42:5f21a710ebc5 597 else if ( !strcmp(address[0], "testPower" ) ) {
mbedalvaro 41:74e24a0e6e50 598 // TO DO
mbedalvaro 41:74e24a0e6e50 599 }
mbedalvaro 42:5f21a710ebc5 600
mbedalvaro 42:5f21a710ebc5 601 // Enable/disable projection:
mbedalvaro 42:5f21a710ebc5 602 else if ( !strcmp(address[0], "stopDisplay" )) stopDisplay();
mbedalvaro 42:5f21a710ebc5 603 else if (!strcmp(address[0], "resumeDisplay")) resumeDisplay();
mbedalvaro 42:5f21a710ebc5 604 else if (!strcmp(address[0], "showLimits")) {
mbedalvaro 40:3ba2b0ea9f33 605 int value=data[0]; // argument is the number of seconds for display
mbedalvaro 40:3ba2b0ea9f33 606 if (value!=-1) {
mbedalvaro 40:3ba2b0ea9f33 607 showLimitsMirrors(50, value);
mbedalvaro 40:3ba2b0ea9f33 608 }
mbedalvaro 42:5f21a710ebc5 609 } else if (!strcmp(address[0], "setSensingMode")) {
mbedalvaro 40:3ba2b0ea9f33 610 int value=data[0];
mbedalvaro 40:3ba2b0ea9f33 611 if (value!=-1) { // otherwise do nothing, this is a reception error (there was no data)
mbedalvaro 40:3ba2b0ea9f33 612 switch(value) {
mbedalvaro 40:3ba2b0ea9f33 613 case 0:
mbedalvaro 40:3ba2b0ea9f33 614 currentSensingMode=NO_SENSING;
mbedalvaro 40:3ba2b0ea9f33 615 break;
mbedalvaro 40:3ba2b0ea9f33 616 case 1:
mbedalvaro 40:3ba2b0ea9f33 617 currentSensingMode=SENSING_WHOLE_SCENE;
mbedalvaro 40:3ba2b0ea9f33 618 break;
mbedalvaro 40:3ba2b0ea9f33 619 case 2:
mbedalvaro 40:3ba2b0ea9f33 620 currentSensingMode=SENSING_PER_OBJECT;
mbedalvaro 40:3ba2b0ea9f33 621 break;
mbedalvaro 40:3ba2b0ea9f33 622 default:
mbedalvaro 40:3ba2b0ea9f33 623 break;
mbedalvaro 40:3ba2b0ea9f33 624 }
mbedalvaro 40:3ba2b0ea9f33 625 }
mbedalvaro 42:5f21a710ebc5 626 }
mbedalvaro 40:3ba2b0ea9f33 627
mbedalvaro 42:5f21a710ebc5 628 // Change state machine color (objects created from this point on will have the state machine color)
mbedalvaro 42:5f21a710ebc5 629 else if (!strcmp(address[0], "setStateColor")) { // example: 0( (all OFF), 1( blue ON, 2( green ON, 4( red ON, 3( blue and green ON; etc.
mbedalvaro 42:5f21a710ebc5 630 int value=data[0];
mbedalvaro 40:3ba2b0ea9f33 631 if (value!=-1) lsr.setColor(value);
mbedalvaro 42:5f21a710ebc5 632 }
mbedalvaro 40:3ba2b0ea9f33 633
mbedalvaro 42:5f21a710ebc5 634 // Real time change of color of ALL the objects in the CURRENT scene (state machine color not changed)
mbedalvaro 42:5f21a710ebc5 635 else if (!strcmp(address[0], "setAllObjectsColor")) { // example: 0) (all OFF), 1) blue ON, 2) green ON, 4) red ON, 3) blue and green ON; etc.
mbedalvaro 42:5f21a710ebc5 636 int value=data[0];
mbedalvaro 42:5f21a710ebc5 637 if (value!=-1) changeColorScene(value);
mbedalvaro 42:5f21a710ebc5 638 }
mbedalvaro 40:3ba2b0ea9f33 639
mbedalvaro 40:3ba2b0ea9f33 640
mbedalvaro 40:3ba2b0ea9f33 641 // =================== CREATION and DESTRUCTION of objects =========================================================
mbedalvaro 42:5f21a710ebc5 642 else if (!strcmp(address[0], "clearScene")) {
mbedalvaro 42:5f21a710ebc5 643 clearScene();
mbedalvaro 42:5f21a710ebc5 644 }
mbedalvaro 42:5f21a710ebc5 645
mbedalvaro 42:5f21a710ebc5 646 else if (!strcmp(address[0], "deleteObject")) { // second address is object identifier (a string that will be converted
mbedalvaro 42:5f21a710ebc5 647 int idobject=atoi(address[1]);
mbedalvaro 42:5f21a710ebc5 648 deleteObject(idobject);
mbedalvaro 42:5f21a710ebc5 649 }
mbedalvaro 42:5f21a710ebc5 650
mbedalvaro 42:5f21a710ebc5 651 // Produce a grid of points. The projection is ORTHOGRAPHIC. Also, POSE is reset to identity (i.e., the sent points should have
mbedalvaro 42:5f21a710ebc5 652 // been translated previously - this may be optional, we could first send the pose, then the points...)
mbedalvaro 42:5f21a710ebc5 653 // Note: orthographic projection can be achieved by simply setting the projection matrix to ID, while
mbedalvaro 42:5f21a710ebc5 654 // 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.
mbedalvaro 42:5f21a710ebc5 655 // Finally, if one wants to avoid an unnecessary matrix product, the laserRendering class provides a renderingMode that can be set to RAW.
mbedalvaro 42:5f21a710ebc5 656 // NOTE: the following string of ASCII characters is assumed to have been input:
mbedalvaro 42:5f21a710ebc5 657 // posX#posY#sizeX#sizeY#nx#ny#:
mbedalvaro 42:5f21a710ebc5 658 // where the names (ex: posX) is in fact an ASCII string representing the number in decimal base. This means that when we arrive here,
mbedalvaro 42:5f21a710ebc5 659 // the auxiliary array auxDataBuffer contains all that data (as floats), ex: auxDataBuffer[0] is just posX, and auxDataBuffer[5] is ny
mbedalvaro 42:5f21a710ebc5 660 else if (!strcmp(address[0], "gridScene")) {
mbedalvaro 42:5f21a710ebc5 661 // Data is now ordered in the auxDataBuffer array:
mbedalvaro 42:5f21a710ebc5 662 float posX = auxDataBuffer[0], posY = auxDataBuffer[1], sizeX = auxDataBuffer[2], sizeY = auxDataBuffer[3], nx = auxDataBuffer[4], ny = auxDataBuffer[5];
mbedalvaro 42:5f21a710ebc5 663 auxDataBuffer_index=0; // to restart loading the auxiliary buffer 'auxDataBuffer'
mbedalvaro 42:5f21a710ebc5 664 clearScene();
mbedalvaro 42:5f21a710ebc5 665 // set color:
mbedalvaro 42:5f21a710ebc5 666 lsr.setColor(0x02); // green
mbedalvaro 42:5f21a710ebc5 667 //IO.setLaserLockinPower(0); // DISABLING SENSING LASER! (here the red). This is FOR CALIBRATION laser-camera
mbedalvaro 40:3ba2b0ea9f33 668
mbedalvaro 42:5f21a710ebc5 669 // set local pose matrix to identity:
mbedalvaro 42:5f21a710ebc5 670 lsr.setIdentityPose();
mbedalvaro 42:5f21a710ebc5 671 // translate to the required position:
mbedalvaro 42:5f21a710ebc5 672 lsr.translate(posX, posY, 0); // since the projection and pose are going to be set
mbedalvaro 42:5f21a710ebc5 673 // to Identity (projection identity will automatically make the projection orthographic), this means that these
mbedalvaro 42:5f21a710ebc5 674 // values are in "laser projector pixels" (in fact angles).
mbedalvaro 42:5f21a710ebc5 675 // Create the collection of dot objects:
mbedalvaro 42:5f21a710ebc5 676 // void grid(float sizeX, float sizeY, int nx, int ny, int repeatpoint);
mbedalvaro 42:5f21a710ebc5 677 grid(sizeX, sizeY, nx, ny, 1);
mbedalvaro 42:5f21a710ebc5 678 updateScene(); // this is important: this will compute the number of objects, points, etc, to prepare the displaying engine (but rendering is NOT yet done,
mbedalvaro 42:5f21a710ebc5 679 // rendering is done in the drawScene() function.
mbedalvaro 40:3ba2b0ea9f33 680
mbedalvaro 42:5f21a710ebc5 681 // Now we can render the scene once:
mbedalvaro 42:5f21a710ebc5 682 // Set a default GLOBAL POSE and orthographic projection for rendering and start displaying:
mbedalvaro 42:5f21a710ebc5 683 lsr.setOrthoProjection();
mbedalvaro 42:5f21a710ebc5 684 lsr.setIdentityPose(); // we could have done the translation here instead of in "local coordinates", but it's the same.
mbedalvaro 42:5f21a710ebc5 685 drawScene(); // this will be called when we change the pose matrix - but needs to be called at least one to start displaying
mbedalvaro 42:5f21a710ebc5 686 }
mbedalvaro 42:5f21a710ebc5 687 // 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!
mbedalvaro 42:5f21a710ebc5 688
mbedalvaro 42:5f21a710ebc5 689 // 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)
mbedalvaro 42:5f21a710ebc5 690 // using orthographic projection.
mbedalvaro 42:5f21a710ebc5 691 // This is in particular useful to create "structured light" grids - such the grid of planar points for calibration.
mbedalvaro 42:5f21a710ebc5 692 // In the future, we can have functions to add objects, etc. Basically, every wrapped function should be call-able by the PC.
mbedalvaro 42:5f21a710ebc5 693 // 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
mbedalvaro 42:5f21a710ebc5 694 // cameraProjectorCalibration Xcode project as little as possible, I will perform the transformations on the computer.
mbedalvaro 42:5f21a710ebc5 695 else if (!strcmp(address[0], "arbitraryGridScene")) {
mbedalvaro 42:5f21a710ebc5 696 // Data is now ordered in the auxDataBuffer array as PAIRS (X,Y). The argument to this message is the IDENTIFIER of the object:
mbedalvaro 42:5f21a710ebc5 697 // The present value of auxDataBuffer_index contains the number of points x 2
mbedalvaro 42:5f21a710ebc5 698 clearScene();
mbedalvaro 42:5f21a710ebc5 699 lsr.setIdentityPose();
mbedalvaro 42:5f21a710ebc5 700 //lsr.setColor(0x02); // fixed color, or use the current color (in which case, we need to call lsr.pushColor() and then pop it again).
mbedalvaro 42:5f21a710ebc5 701 lsr.setColor(0x03); // blue+green
mbedalvaro 42:5f21a710ebc5 702 IO.setLaserLockinPower(0); // DISABLING SENSING LASER! (here the red). This is FOR CALIBRATION laser-camera
mbedalvaro 40:3ba2b0ea9f33 703
mbedalvaro 42:5f21a710ebc5 704 // begin(1) // we can create a single object, or one object per position (with repetition of the point if needed)
mbedalvaro 42:5f21a710ebc5 705 for (unsigned char i = 0; i < auxDataBuffer_index / 2; i++) {
mbedalvaro 42:5f21a710ebc5 706 begin(i);
mbedalvaro 42:5f21a710ebc5 707 for (unsigned char k=0; k<1; k++) // fixed repetition of points - we can send this as a parameter if we want.
mbedalvaro 42:5f21a710ebc5 708 vertex(auxDataBuffer[2 * i], auxDataBuffer[2 * i + 1], 0);
mbedalvaro 42:5f21a710ebc5 709 end();
mbedalvaro 40:3ba2b0ea9f33 710 }
mbedalvaro 42:5f21a710ebc5 711 updateScene();
mbedalvaro 42:5f21a710ebc5 712 // Set a default GLOBAL POSE and ORTHO projection matrix for start displaying, or use the CURRENT ONE (for that, we should have
mbedalvaro 42:5f21a710ebc5 713 // properly called lsr.pushPoseMatrix())
mbedalvaro 42:5f21a710ebc5 714 lsr.setOrthoProjection();
mbedalvaro 42:5f21a710ebc5 715 lsr.setIdentityPose();
mbedalvaro 42:5f21a710ebc5 716 drawScene();
mbedalvaro 42:5f21a710ebc5 717 auxDataBuffer_index = 0; // to restart loading the auxiliary buffer 'auxDataBuffer' through serial port
mbedalvaro 42:5f21a710ebc5 718 }
mbedalvaro 42:5f21a710ebc5 719
mbedalvaro 42:5f21a710ebc5 720 // 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):
mbedalvaro 42:5f21a710ebc5 721 else if (!strcmp(address[0], "createObject2D")) {
mbedalvaro 42:5f21a710ebc5 722 unsigned char numPoints= 11;//auxDataBuffer_index/2;
mbedalvaro 42:5f21a710ebc5 723 //auxDataBuffer_index = 0; // to restart loading the auxiliary buffer 'auxDataBuffer' through serial port
mbedalvaro 42:5f21a710ebc5 724
mbedalvaro 42:5f21a710ebc5 725 // pc.printf("creating 2d object with %d points\n", numPoints);
mbedalvaro 42:5f21a710ebc5 726
mbedalvaro 42:5f21a710ebc5 727 int IDObject=atoi(address[1]); // the object identifier is on the second address
mbedalvaro 42:5f21a710ebc5 728 // the initial position is on data[0-1]
mbedalvaro 42:5f21a710ebc5 729 // pc.printf("Object ID is %d \n", IDObject);
mbedalvaro 42:5f21a710ebc5 730 //pc.printf("Position at: %4.2f, %4.2f \n", (float)data[0], (float)data[1]);
mbedalvaro 42:5f21a710ebc5 731 // pc.printf("Position at: %d, %d \n", (int)data[0], (int)data[1]);
mbedalvaro 42:5f21a710ebc5 732
mbedalvaro 42:5f21a710ebc5 733 if (IDObject!=-1) {
mbedalvaro 42:5f21a710ebc5 734
mbedalvaro 42:5f21a710ebc5 735 if(true) {
mbedalvaro 42:5f21a710ebc5 736
mbedalvaro 42:5f21a710ebc5 737 clearScene();
mbedalvaro 42:5f21a710ebc5 738
mbedalvaro 42:5f21a710ebc5 739 // First, create the object:
mbedalvaro 42:5f21a710ebc5 740 lsr.pushPoseMatrix();
mbedalvaro 42:5f21a710ebc5 741 lsr.setIdentityPose();
mbedalvaro 42:5f21a710ebc5 742
mbedalvaro 42:5f21a710ebc5 743 // lsr.translate(-15,-10,0);
mbedalvaro 42:5f21a710ebc5 744
mbedalvaro 42:5f21a710ebc5 745 lsr.translate(20,20,0);
mbedalvaro 40:3ba2b0ea9f33 746
mbedalvaro 42:5f21a710ebc5 747 // lsr.translate(5,10,0);
mbedalvaro 42:5f21a710ebc5 748 // lsr.translate((int)data[0], (int)data[1], 0);
mbedalvaro 42:5f21a710ebc5 749
mbedalvaro 42:5f21a710ebc5 750 lsr.setColor(0x02); // we will render in current color
mbedalvaro 42:5f21a710ebc5 751
mbedalvaro 42:5f21a710ebc5 752 //begin(IDObject);
mbedalvaro 42:5f21a710ebc5 753 for (unsigned char i = 0; i < numPoints; i++) {
mbedalvaro 42:5f21a710ebc5 754 begin(i);
mbedalvaro 42:5f21a710ebc5 755 //vertex(auxDataBuffer[2 * i], auxDataBuffer[2 * i + 1],0); // z=0 for 2d objects (in current pose)
mbedalvaro 42:5f21a710ebc5 756 //letter3d('A', fontWidth, fontHeight);
mbedalvaro 42:5f21a710ebc5 757 vertex(10*cos(2*PI/numPoints*i), 10*sin(2*PI/numPoints*i), 0);
mbedalvaro 42:5f21a710ebc5 758 vertex(5*cos(2*PI/numPoints*i+.1), 5*sin(2*PI/numPoints*i+.1), 0);
mbedalvaro 42:5f21a710ebc5 759 end();
mbedalvaro 42:5f21a710ebc5 760 //pc.printf("local (x,y): %f, %f \n", auxDataBuffer[2 * i], auxDataBuffer[2 * i+1]);
mbedalvaro 42:5f21a710ebc5 761 // lsr.translate(1.1*fontWidth, 0, 0);
mbedalvaro 42:5f21a710ebc5 762 }
mbedalvaro 42:5f21a710ebc5 763 lsr.popPoseMatrix();
mbedalvaro 42:5f21a710ebc5 764
mbedalvaro 42:5f21a710ebc5 765 // update the scene for display
mbedalvaro 42:5f21a710ebc5 766 updateScene();
mbedalvaro 42:5f21a710ebc5 767
mbedalvaro 42:5f21a710ebc5 768 //createTextScene();
mbedalvaro 42:5f21a710ebc5 769
mbedalvaro 42:5f21a710ebc5 770 // Set the global projection and modelview:
mbedalvaro 42:5f21a710ebc5 771 lsr.loadProjMatrix(K1, scaleFactorProjector1);
mbedalvaro 42:5f21a710ebc5 772 lsr.setIdentityPose();
mbedalvaro 42:5f21a710ebc5 773 lsr.multPoseMatrix(lsr.EXTRINSICS); // RT=E // we can simply do: lsr.setExtrinsicsPose()
mbedalvaro 42:5f21a710ebc5 774 // DEFAULT position in CAMERA coordinates (calibration was in cm):
mbedalvaro 42:5f21a710ebc5 775 lsr.translate(0, 0, 130);
mbedalvaro 42:5f21a710ebc5 776
mbedalvaro 42:5f21a710ebc5 777 // RENDER the scene:
mbedalvaro 42:5f21a710ebc5 778 drawScene(); // this will be called when we change the pose matrix - but needs to be called at least once to start displaying
mbedalvaro 40:3ba2b0ea9f33 779 }
mbedalvaro 42:5f21a710ebc5 780
mbedalvaro 40:3ba2b0ea9f33 781 }
mbedalvaro 42:5f21a710ebc5 782 }
mbedalvaro 40:3ba2b0ea9f33 783
mbedalvaro 42:5f21a710ebc5 784 // This means we received 3D vertex data to create an object.
mbedalvaro 42:5f21a710ebc5 785 // It will be rendered in the CURRENT pose.
mbedalvaro 42:5f21a710ebc5 786 else if (!strcmp(address[0], "createObject3D")) {
mbedalvaro 42:5f21a710ebc5 787 int IDObject=data[1];
mbedalvaro 42:5f21a710ebc5 788 if (IDObject!=-1) {
mbedalvaro 40:3ba2b0ea9f33 789 // Data is now ordered in the auxDataBuffer array in triplets (X,Y,Z).
mbedalvaro 40:3ba2b0ea9f33 790 // The present value of auxDataBuffer_index contains the number of points x 3
mbedalvaro 40:3ba2b0ea9f33 791 //lsr.pushPoseMatrix(); // we will render on the current pose
mbedalvaro 40:3ba2b0ea9f33 792 //clearScene();
mbedalvaro 40:3ba2b0ea9f33 793 //lsr.setIdentityPose();
mbedalvaro 40:3ba2b0ea9f33 794 //lsr.setColor(0x02); // we will render in current color
mbedalvaro 40:3ba2b0ea9f33 795 begin(IDObject);
mbedalvaro 40:3ba2b0ea9f33 796 for (unsigned char i = 0; i < auxDataBuffer_index / 3; i++) {
mbedalvaro 40:3ba2b0ea9f33 797 vertex(auxDataBuffer[3 * i], auxDataBuffer[3 * i + 1],
mbedalvaro 40:3ba2b0ea9f33 798 auxDataBuffer[3 * i + 2]);
mbedalvaro 40:3ba2b0ea9f33 799 }
mbedalvaro 40:3ba2b0ea9f33 800 end();
mbedalvaro 40:3ba2b0ea9f33 801 updateScene();
mbedalvaro 40:3ba2b0ea9f33 802 // Set a default GLOBAL POSE and ORTHO projection matrix for start displaying, or use the CURRENT ONE (for that, we should have
mbedalvaro 40:3ba2b0ea9f33 803 // properly called lsr.pushPoseMatrix())
mbedalvaro 40:3ba2b0ea9f33 804 //lsr.setOrthoProjection();
mbedalvaro 40:3ba2b0ea9f33 805 //lsr.setIdentityPose(); // we could have done the translation here instead of in "local coordinates", but it's the same.
mbedalvaro 40:3ba2b0ea9f33 806 //lsr.popPoseMatrix();
mbedalvaro 40:3ba2b0ea9f33 807 drawScene();
mbedalvaro 40:3ba2b0ea9f33 808 auxDataBuffer_index = 0; // to restart loading the auxiliary buffer 'auxDataBuffer' through serial port
mbedalvaro 40:3ba2b0ea9f33 809 }
mbedalvaro 42:5f21a710ebc5 810 }
mbedalvaro 42:5f21a710ebc5 811
mbedalvaro 42:5f21a710ebc5 812 // Create text:
mbedalvaro 42:5f21a710ebc5 813 else if (!strcmp(address[0], "createTextObject")) {
mbedalvaro 42:5f21a710ebc5 814
mbedalvaro 42:5f21a710ebc5 815
mbedalvaro 42:5f21a710ebc5 816 // //NOTE: the actual text will be in the second address, and the argument will be the ID of this new text object:
mbedalvaro 42:5f21a710ebc5 817 textToDisplay = address[1];
mbedalvaro 42:5f21a710ebc5 818 int IDObject=data[0];
mbedalvaro 42:5f21a710ebc5 819 if (IDObject!=-1) {
mbedalvaro 42:5f21a710ebc5 820
mbedalvaro 40:3ba2b0ea9f33 821
mbedalvaro 42:5f21a710ebc5 822 //createTextScene();
mbedalvaro 42:5f21a710ebc5 823 // // Set a default PERSPECTIVE and GLOBAL POSE for start displaying:
mbedalvaro 42:5f21a710ebc5 824 // lsr.loadProjMatrix(K1, scaleFactorProjector1);
mbedalvaro 42:5f21a710ebc5 825 // lsr.setIdentityPose();
mbedalvaro 42:5f21a710ebc5 826 // lsr.multPoseMatrix(lsr.EXTRINSICS); // RT=E // we can simply do: lsr.setExtrinsicsPose()
mbedalvaro 42:5f21a710ebc5 827 // //This means that the current coordinate frame is the CAMERA, and calibration was in cm:
mbedalvaro 42:5f21a710ebc5 828 // lsr.translate(0, 0, 100);
mbedalvaro 42:5f21a710ebc5 829 // //lsr.flipY();
mbedalvaro 42:5f21a710ebc5 830 // drawScene(); // this will be called when we change the pose matrix - but needs to be called at least once to start displaying
mbedalvaro 42:5f21a710ebc5 831
mbedalvaro 42:5f21a710ebc5 832
mbedalvaro 42:5f21a710ebc5 833 // clearScene();
mbedalvaro 42:5f21a710ebc5 834 stopDisplay();
mbedalvaro 42:5f21a710ebc5 835
mbedalvaro 42:5f21a710ebc5 836 // CREATE OBJECT:
mbedalvaro 42:5f21a710ebc5 837 lsr.pushPoseMatrix();
mbedalvaro 42:5f21a710ebc5 838
mbedalvaro 42:5f21a710ebc5 839 lsr.setIdentityPose(); // we could use a wrapper, but I won't for the time being.
mbedalvaro 42:5f21a710ebc5 840 lsr.flipY();
mbedalvaro 42:5f21a710ebc5 841 //lsr.flipX();
mbedalvaro 42:5f21a710ebc5 842 lsr.translate(-15,-10,0);
mbedalvaro 42:5f21a710ebc5 843
mbedalvaro 42:5f21a710ebc5 844 //begin(IDObject);
mbedalvaro 42:5f21a710ebc5 845 // string3d(textToDisplay, fontWidth*textToDisplay.length(), fontHeight);
mbedalvaro 42:5f21a710ebc5 846 // end();
mbedalvaro 40:3ba2b0ea9f33 847
mbedalvaro 42:5f21a710ebc5 848 //begin(IDObject);
mbedalvaro 42:5f21a710ebc5 849 for (unsigned short i = 0; i < textToDisplay.length(); i++) {
mbedalvaro 42:5f21a710ebc5 850 char ch = textToDisplay.at(i);
mbedalvaro 42:5f21a710ebc5 851 if (ch != ' ') {
mbedalvaro 42:5f21a710ebc5 852 begin(i);
mbedalvaro 42:5f21a710ebc5 853 letter3d(ch, fontWidth, fontHeight);
mbedalvaro 42:5f21a710ebc5 854 end();
mbedalvaro 42:5f21a710ebc5 855 }
mbedalvaro 42:5f21a710ebc5 856 lsr.translate(1.1*fontWidth, 0, 0);
mbedalvaro 42:5f21a710ebc5 857 }
mbedalvaro 42:5f21a710ebc5 858 //end();
mbedalvaro 42:5f21a710ebc5 859
mbedalvaro 42:5f21a710ebc5 860 lsr.popPoseMatrix();
mbedalvaro 42:5f21a710ebc5 861
mbedalvaro 42:5f21a710ebc5 862 updateScene();
mbedalvaro 42:5f21a710ebc5 863
mbedalvaro 42:5f21a710ebc5 864 // Set a default PERSPECTIVE and GLOBAL POSE for start displaying:
mbedalvaro 42:5f21a710ebc5 865 lsr.loadProjMatrix(K1, scaleFactorProjector1);
mbedalvaro 42:5f21a710ebc5 866 lsr.setIdentityPose();
mbedalvaro 40:3ba2b0ea9f33 867 lsr.multPoseMatrix(lsr.EXTRINSICS); // RT=E // we can simply do: lsr.setExtrinsicsPose()
mbedalvaro 42:5f21a710ebc5 868 // DEFAULT position in CAMERA coordinates (calibration was in cm):
mbedalvaro 42:5f21a710ebc5 869 lsr.translate(0, 0, 130);
mbedalvaro 40:3ba2b0ea9f33 870
mbedalvaro 42:5f21a710ebc5 871 drawScene(); // this will be called when we change the pose matrix - but needs to be called at least once to start displaying
mbedalvaro 40:3ba2b0ea9f33 872 }
mbedalvaro 42:5f21a710ebc5 873 }
mbedalvaro 42:5f21a710ebc5 874
mbedalvaro 40:3ba2b0ea9f33 875
mbedalvaro 42:5f21a710ebc5 876 // (d) TERMINATOR indicating data was for the POSE MATRIX of the object (Mp) (with respect to the CAMERA):
mbedalvaro 42:5f21a710ebc5 877 else if (!strcmp(address[0], "poseMatrix")) { // when receiving this, it means that the WHOLE matrix data (4x4 values) have been sent in row/column format
mbedalvaro 42:5f21a710ebc5 878 auxDataBuffer_index = 0; // to restart loading the auxiliary buffer 'auxDataBuffer'
mbedalvaro 42:5f21a710ebc5 879 // Now, auxDataBuffer is a buffer with 12 values (4x3), corresponding to the pose of the object in CAMERA coordinartes (RT')
mbedalvaro 42:5f21a710ebc5 880 lsr.setIdentityPose(); // RT=ID
mbedalvaro 42:5f21a710ebc5 881 lsr.multPoseMatrix(lsr.EXTRINSICS); // RT=E // we can simply do: lsr.setExtrinsicsPose()
mbedalvaro 42:5f21a710ebc5 882 lsr.multPoseMatrix(auxDataBuffer); // RT=ExRT' : this sets RT as the current object pose in PROJECTOR coordinates
mbedalvaro 40:3ba2b0ea9f33 883
mbedalvaro 42:5f21a710ebc5 884 drawScene(); // needed because we changed the POSE (but there is no need to re-create the geometry nor update the renderer (updateScene), just project)
mbedalvaro 42:5f21a710ebc5 885 }
mbedalvaro 40:3ba2b0ea9f33 886
mbedalvaro 42:5f21a710ebc5 887 // (d) TERMINATOR indicating data was for the projection matrix (will probably be saved in "hard" in the microcontroller code):
mbedalvaro 42:5f21a710ebc5 888 else if (!strcmp(address[0], "projectionMatrix")) {// when receiving this character, it means that the WHOLE matrix data (3x3 values) have been sent (with '#' separator), in row/column format
mbedalvaro 42:5f21a710ebc5 889 auxDataBuffer_index = 0; // to restart loading the auxiliary buffer 'auxDataBuffer' through serial port
mbedalvaro 42:5f21a710ebc5 890 // store in projection matrix:
mbedalvaro 42:5f21a710ebc5 891 lsr.loadProjMatrix(auxDataBuffer, 1.0);
mbedalvaro 42:5f21a710ebc5 892
mbedalvaro 42:5f21a710ebc5 893 drawScene(); // needed because we changed the PROJECTION matrix (but there is no need to re-create all the geometry, just project)
mbedalvaro 42:5f21a710ebc5 894 }
mbedalvaro 40:3ba2b0ea9f33 895
mbedalvaro 42:5f21a710ebc5 896 //(e) TERMINATOR indicating the data was for the extrinsic matrix:
mbedalvaro 42:5f21a710ebc5 897 else if (!strcmp(address[0], "extrinsicMatrix")) { // when receiving this character, it means that the WHOLE matrix data (3x3 values) have been sent (with '#' separator), in row/column format
mbedalvaro 42:5f21a710ebc5 898 auxDataBuffer_index = 0; // to restart loading the auxiliary buffer 'auxDataBuffer' through serial port
mbedalvaro 42:5f21a710ebc5 899 // store in projection matrix:
mbedalvaro 42:5f21a710ebc5 900 lsr.loadExtrinsicsMatrix(auxDataBuffer);
mbedalvaro 42:5f21a710ebc5 901
mbedalvaro 42:5f21a710ebc5 902 drawScene(); // needed because we changed the EXTRINSICS (hence the pose), but there is no need to re-create the geometry, just project.
mbedalvaro 42:5f21a710ebc5 903 }
mbedalvaro 40:3ba2b0ea9f33 904 }
mbedalvaro 40:3ba2b0ea9f33 905
mbedalvaro 40:3ba2b0ea9f33 906 //============= RECEIVE SERIAL COMMANDS =========================
mbedalvaro 40:3ba2b0ea9f33 907 // String to store ALPHANUMERIC DATA (i.e., integers, floating point numbers, unsigned ints, etc represented as DEC):
mbedalvaro 40:3ba2b0ea9f33 908 // TO DO: use string objects!
mbedalvaro 40:3ba2b0ea9f33 909 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...
mbedalvaro 40:3ba2b0ea9f33 910 int indexStringData = 0; //position of the byte in the string
mbedalvaro 40:3ba2b0ea9f33 911
mbedalvaro 40:3ba2b0ea9f33 912 // String to store COMMAND WORDS (not used yet):
mbedalvaro 40:3ba2b0ea9f33 913 char stringCommand[24];
mbedalvaro 40:3ba2b0ea9f33 914 int indexStringCommand = 0;
mbedalvaro 40:3ba2b0ea9f33 915 bool commandReady = false; // will become true when receiving the byte 0 (i.e. the '/0' string terminator)
mbedalvaro 40:3ba2b0ea9f33 916
mbedalvaro 40:3ba2b0ea9f33 917 void processSerial()
mbedalvaro 40:3ba2b0ea9f33 918 {
mbedalvaro 40:3ba2b0ea9f33 919 while (pc.readable() > 0) {
mbedalvaro 40:3ba2b0ea9f33 920 char incomingByte = pc.getc();
mbedalvaro 40:3ba2b0ea9f33 921
mbedalvaro 40:3ba2b0ea9f33 922 // For test:
mbedalvaro 40:3ba2b0ea9f33 923 // pc.putc(incomingByte);
mbedalvaro 40:3ba2b0ea9f33 924
mbedalvaro 40:3ba2b0ea9f33 925 // (a) TEXT to display or NUMBERS (in ASCII) to convert to numerical value (rem: limited set of characters for the time being):
mbedalvaro 40:3ba2b0ea9f33 926 // Note: numbers are float, so we need also the "." and "-"
mbedalvaro 40:3ba2b0ea9f33 927 if ( (incomingByte >= '0' && incomingByte <= '9') // numbers
mbedalvaro 42:5f21a710ebc5 928 || (incomingByte >= 'A' && incomingByte <= 'Z') // capital letters
mbedalvaro 42:5f21a710ebc5 929 || (incomingByte >= 'a' && incomingByte <= 'z') // small letters (in fact, will be used for other symbols)
mbedalvaro 42:5f21a710ebc5 930 || (incomingByte == ' ') // space
mbedalvaro 42:5f21a710ebc5 931 || (incomingByte == '-') // minus sign
mbedalvaro 42:5f21a710ebc5 932 || (incomingByte == '.') // decimal point
mbedalvaro 42:5f21a710ebc5 933 ) {
mbedalvaro 40:3ba2b0ea9f33 934 receivedStringData[indexStringData] = incomingByte;
mbedalvaro 40:3ba2b0ea9f33 935 indexStringData++;
mbedalvaro 40:3ba2b0ea9f33 936 }
mbedalvaro 40:3ba2b0ea9f33 937
mbedalvaro 40:3ba2b0ea9f33 938 // Enable/disable projection:
mbedalvaro 40:3ba2b0ea9f33 939 else if (incomingByte == '}') {
mbedalvaro 40:3ba2b0ea9f33 940 stopDisplay();
mbedalvaro 40:3ba2b0ea9f33 941 } else if (incomingByte == '{') {
mbedalvaro 40:3ba2b0ea9f33 942 resumeDisplay();
mbedalvaro 40:3ba2b0ea9f33 943 }
mbedalvaro 40:3ba2b0ea9f33 944
mbedalvaro 40:3ba2b0ea9f33 945 // Show maximum excursion of mirrors:
mbedalvaro 40:3ba2b0ea9f33 946 else if (incomingByte == '*') {
mbedalvaro 40:3ba2b0ea9f33 947 showLimitsMirrors(50, 120);
mbedalvaro 40:3ba2b0ea9f33 948 }
mbedalvaro 42:5f21a710ebc5 949
mbedalvaro 40:3ba2b0ea9f33 950
mbedalvaro 40:3ba2b0ea9f33 951 else if (incomingByte == '+') {
mbedalvaro 40:3ba2b0ea9f33 952 receivedStringData[indexStringData] = 0;
mbedalvaro 40:3ba2b0ea9f33 953 indexStringData = 0;
mbedalvaro 40:3ba2b0ea9f33 954 switch(atoi(receivedStringData)) {
mbedalvaro 40:3ba2b0ea9f33 955 case 0:
mbedalvaro 40:3ba2b0ea9f33 956 currentSensingMode=NO_SENSING;
mbedalvaro 40:3ba2b0ea9f33 957 break;
mbedalvaro 40:3ba2b0ea9f33 958 case 1:
mbedalvaro 40:3ba2b0ea9f33 959 currentSensingMode=SENSING_WHOLE_SCENE;
mbedalvaro 40:3ba2b0ea9f33 960 break;
mbedalvaro 40:3ba2b0ea9f33 961 case 2:
mbedalvaro 40:3ba2b0ea9f33 962 currentSensingMode=SENSING_PER_OBJECT;
mbedalvaro 40:3ba2b0ea9f33 963 break;
mbedalvaro 40:3ba2b0ea9f33 964 default:
mbedalvaro 40:3ba2b0ea9f33 965 break;
mbedalvaro 40:3ba2b0ea9f33 966 }
mbedalvaro 40:3ba2b0ea9f33 967 }
mbedalvaro 40:3ba2b0ea9f33 968
mbedalvaro 40:3ba2b0ea9f33 969 // Performs a scan of the surface and send all the data serially (N>, with N the resolution of the scan)
mbedalvaro 40:3ba2b0ea9f33 970 else if (incomingByte == '>') {
mbedalvaro 40:3ba2b0ea9f33 971 receivedStringData[indexStringData] = 0;
mbedalvaro 40:3ba2b0ea9f33 972 indexStringData = 0;
mbedalvaro 40:3ba2b0ea9f33 973 //pc.printf("scan command issued¥n");
mbedalvaro 40:3ba2b0ea9f33 974 scanSerial(atoi(receivedStringData));
mbedalvaro 40:3ba2b0ea9f33 975 }
mbedalvaro 40:3ba2b0ea9f33 976
mbedalvaro 40:3ba2b0ea9f33 977 // Recompute the LUT table
mbedalvaro 40:3ba2b0ea9f33 978 else if (incomingByte == '<') {
mbedalvaro 40:3ba2b0ea9f33 979 recomputeLookUpTable();
mbedalvaro 40:3ba2b0ea9f33 980 }
mbedalvaro 40:3ba2b0ea9f33 981
mbedalvaro 40:3ba2b0ea9f33 982 // Change state machine color (objects created from this point on will have the state machine color)
mbedalvaro 40:3ba2b0ea9f33 983 else if (incomingByte == '(') { // example: 0( (all OFF), 1( blue ON, 2( green ON, 4( red ON, 3( blue and green ON; etc.
mbedalvaro 40:3ba2b0ea9f33 984 receivedStringData[indexStringData] = 0;
mbedalvaro 40:3ba2b0ea9f33 985 indexStringData = 0;
mbedalvaro 40:3ba2b0ea9f33 986 // convert to integer and set the state machine color:
mbedalvaro 40:3ba2b0ea9f33 987 lsr.setColor(atoi(receivedStringData));
mbedalvaro 40:3ba2b0ea9f33 988 }
mbedalvaro 40:3ba2b0ea9f33 989
mbedalvaro 40:3ba2b0ea9f33 990 // Real time change of color of ALL the objects in the CURRENT scene (state machine color not changed)
mbedalvaro 40:3ba2b0ea9f33 991 else if (incomingByte == ')') { // example: 0) (all OFF), 1) blue ON, 2) green ON, 4) red ON, 3) blue and green ON; etc.
mbedalvaro 40:3ba2b0ea9f33 992 receivedStringData[indexStringData] = 0;
mbedalvaro 40:3ba2b0ea9f33 993 indexStringData = 0;
mbedalvaro 40:3ba2b0ea9f33 994 // convert to integer and set the color:
mbedalvaro 40:3ba2b0ea9f33 995 changeColorScene(atoi(receivedStringData));
mbedalvaro 40:3ba2b0ea9f33 996 }
mbedalvaro 40:3ba2b0ea9f33 997
mbedalvaro 40:3ba2b0ea9f33 998
mbedalvaro 40:3ba2b0ea9f33 999 // Produce a grid of points. The projection is ORTHOGRAPHIC. Also, POSE is reset to identity (i.e., the sent points should have
mbedalvaro 40:3ba2b0ea9f33 1000 // been translated previously - this may be optional, we could first send the pose, then the points...)
mbedalvaro 40:3ba2b0ea9f33 1001 // Note: orthographic projection can be achieved by simply setting the projection matrix to ID, while
mbedalvaro 40:3ba2b0ea9f33 1002 // 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.
mbedalvaro 40:3ba2b0ea9f33 1003 // Finally, if one wants to avoid an unnecessary matrix product, the laserRendering class provides a renderingMode that can be set to RAW.
mbedalvaro 40:3ba2b0ea9f33 1004 // NOTE: the following string of ASCII characters is assumed to have been input:
mbedalvaro 40:3ba2b0ea9f33 1005 // posX#posY#sizeX#sizeY#nx#ny#:
mbedalvaro 40:3ba2b0ea9f33 1006 // where the names (ex: posX) is in fact an ASCII string representing the number in decimal base. This means that when we arrive here,
mbedalvaro 40:3ba2b0ea9f33 1007 // the auxiliary array auxDataBuffer contains all that data (as floats), ex: auxDataBuffer[0] is just posX, and auxDataBuffer[5] is ny
mbedalvaro 40:3ba2b0ea9f33 1008 else if (incomingByte == ':') {
mbedalvaro 40:3ba2b0ea9f33 1009 // Data is now ordered in the auxDataBuffer array:
mbedalvaro 40:3ba2b0ea9f33 1010 float posX = auxDataBuffer[0], posY = auxDataBuffer[1], sizeX = auxDataBuffer[2], sizeY = auxDataBuffer[3], nx = auxDataBuffer[4], ny = auxDataBuffer[5];
mbedalvaro 40:3ba2b0ea9f33 1011 auxDataBuffer_index = 0; // to restart loading the auxiliary buffer 'auxDataBuffer' through serial port
mbedalvaro 40:3ba2b0ea9f33 1012 clearScene();
mbedalvaro 40:3ba2b0ea9f33 1013 // set color:
mbedalvaro 40:3ba2b0ea9f33 1014 lsr.setColor(0x02); // green
mbedalvaro 40:3ba2b0ea9f33 1015 //IO.setLaserLockinPower(0); // DISABLING SENSING LASER! (here the red). This is FOR CALIBRATION laser-camera
mbedalvaro 42:5f21a710ebc5 1016
mbedalvaro 40:3ba2b0ea9f33 1017 // set local pose matrix to identity:
mbedalvaro 40:3ba2b0ea9f33 1018 lsr.setIdentityPose();
mbedalvaro 40:3ba2b0ea9f33 1019 // translate to the required position:
mbedalvaro 40:3ba2b0ea9f33 1020 lsr.translate(posX, posY, 0); // since the projection and pose are going to be set
mbedalvaro 40:3ba2b0ea9f33 1021 // to Identity (projection identity will automatically make the projection orthographic), this means that these
mbedalvaro 40:3ba2b0ea9f33 1022 // values are in "laser projector pixels" (in fact angles).
mbedalvaro 40:3ba2b0ea9f33 1023 // Create the collection of dot objects:
mbedalvaro 40:3ba2b0ea9f33 1024 // void grid(float sizeX, float sizeY, int nx, int ny, int repeatpoint);
mbedalvaro 40:3ba2b0ea9f33 1025 grid(sizeX, sizeY, nx, ny, 1);
mbedalvaro 40:3ba2b0ea9f33 1026 updateScene(); // this is important: this will compute the number of objects, points, etc, to prepare the displaying engine (but rendering is NOT yet done,
mbedalvaro 40:3ba2b0ea9f33 1027 // rendering is done in the drawScene() function.
mbedalvaro 40:3ba2b0ea9f33 1028
mbedalvaro 40:3ba2b0ea9f33 1029 // Now we can render the scene once:
mbedalvaro 40:3ba2b0ea9f33 1030 // Set a default GLOBAL POSE and orthographic projection for rendering and start displaying:
mbedalvaro 40:3ba2b0ea9f33 1031 lsr.setOrthoProjection();
mbedalvaro 40:3ba2b0ea9f33 1032 lsr.setIdentityPose(); // we could have done the translation here instead of in "local coordinates", but it's the same.
mbedalvaro 40:3ba2b0ea9f33 1033 drawScene(); // this will be called when we change the pose matrix - but needs to be called at least one to start displaying
mbedalvaro 40:3ba2b0ea9f33 1034 }
mbedalvaro 40:3ba2b0ea9f33 1035 // 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!
mbedalvaro 40:3ba2b0ea9f33 1036
mbedalvaro 40:3ba2b0ea9f33 1037 // 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)
mbedalvaro 40:3ba2b0ea9f33 1038 // using orthographic projection.
mbedalvaro 40:3ba2b0ea9f33 1039 // This is in particular useful to create "structured light" grids - such the grid of planar points for calibration.
mbedalvaro 40:3ba2b0ea9f33 1040 // In the future, we can have functions to add objects, etc. Basically, every wrapped function should be call-able by the PC.
mbedalvaro 40:3ba2b0ea9f33 1041 // 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
mbedalvaro 40:3ba2b0ea9f33 1042 // cameraProjectorCalibration Xcode project as little as possible, I will perform the transformations on the computer.
mbedalvaro 40:3ba2b0ea9f33 1043 else if (incomingByte == '=') {
mbedalvaro 40:3ba2b0ea9f33 1044 // Data is now ordered in the auxDataBuffer array as PAIRS (X,Y)
mbedalvaro 40:3ba2b0ea9f33 1045 // The present value of auxDataBuffer_index contains the number of points x 2
mbedalvaro 40:3ba2b0ea9f33 1046 clearScene();
mbedalvaro 40:3ba2b0ea9f33 1047 lsr.setIdentityPose();
mbedalvaro 40:3ba2b0ea9f33 1048 //lsr.setColor(0x02); // fixed color, or use the current color (in which case, we need to call lsr.pushColor() and then pop it again).
mbedalvaro 40:3ba2b0ea9f33 1049 lsr.setColor(0x03); // blue+green
mbedalvaro 40:3ba2b0ea9f33 1050 IO.setLaserLockinPower(0); // DISABLING SENSING LASER! (here the red). This is FOR CALIBRATION laser-camera
mbedalvaro 42:5f21a710ebc5 1051
mbedalvaro 40:3ba2b0ea9f33 1052 // begin(1) // we can create a single object, or one object per position (with repetition of the point if needed)
mbedalvaro 40:3ba2b0ea9f33 1053 for (unsigned char i = 0; i < auxDataBuffer_index / 2; i++) {
mbedalvaro 40:3ba2b0ea9f33 1054 begin(i);
mbedalvaro 40:3ba2b0ea9f33 1055 for (unsigned char k=0; k<1; k++) // fixed repetition of points - we can send this as a parameter if we want.
mbedalvaro 40:3ba2b0ea9f33 1056 vertex(auxDataBuffer[2 * i], auxDataBuffer[2 * i + 1], 0);
mbedalvaro 40:3ba2b0ea9f33 1057 end();
mbedalvaro 40:3ba2b0ea9f33 1058 }
mbedalvaro 40:3ba2b0ea9f33 1059 updateScene();
mbedalvaro 40:3ba2b0ea9f33 1060 // Set a default GLOBAL POSE and ORTHO projection matrix for start displaying, or use the CURRENT ONE (for that, we should have
mbedalvaro 40:3ba2b0ea9f33 1061 // properly called lsr.pushPoseMatrix())
mbedalvaro 40:3ba2b0ea9f33 1062 lsr.setOrthoProjection();
mbedalvaro 40:3ba2b0ea9f33 1063 lsr.setIdentityPose();
mbedalvaro 40:3ba2b0ea9f33 1064 drawScene();
mbedalvaro 40:3ba2b0ea9f33 1065 auxDataBuffer_index = 0; // to restart loading the auxiliary buffer 'auxDataBuffer' through serial port
mbedalvaro 40:3ba2b0ea9f33 1066 }
mbedalvaro 40:3ba2b0ea9f33 1067
mbedalvaro 40:3ba2b0ea9f33 1068 // This means we received 3D vertex data to create a single scene out of these points (we will create a single object).
mbedalvaro 40:3ba2b0ea9f33 1069 // It will be rendered in the CURRENT pose.
mbedalvaro 40:3ba2b0ea9f33 1070 else if (incomingByte == ';') {
mbedalvaro 40:3ba2b0ea9f33 1071 // Data is now ordered in the auxDataBuffer array in triplets (X,Y,Z).
mbedalvaro 40:3ba2b0ea9f33 1072 // The present value of auxDataBuffer_index contains the number of points x 3
mbedalvaro 40:3ba2b0ea9f33 1073 lsr.pushPoseMatrix(); // we will render on the current pose...
mbedalvaro 40:3ba2b0ea9f33 1074 clearScene();
mbedalvaro 40:3ba2b0ea9f33 1075 lsr.setIdentityPose();
mbedalvaro 40:3ba2b0ea9f33 1076 //lsr.setColor(0x02);
mbedalvaro 40:3ba2b0ea9f33 1077 begin(1);
mbedalvaro 40:3ba2b0ea9f33 1078 for (unsigned char i = 0; i < auxDataBuffer_index / 3; i++) {
mbedalvaro 40:3ba2b0ea9f33 1079 vertex(auxDataBuffer[3 * i], auxDataBuffer[3 * i + 1],
mbedalvaro 40:3ba2b0ea9f33 1080 auxDataBuffer[3 * i + 2]);
mbedalvaro 40:3ba2b0ea9f33 1081 }
mbedalvaro 40:3ba2b0ea9f33 1082 end();
mbedalvaro 40:3ba2b0ea9f33 1083 updateScene();
mbedalvaro 40:3ba2b0ea9f33 1084 // Set a default GLOBAL POSE and ORTHO projection matrix for start displaying, or use the CURRENT ONE (for that, we should have
mbedalvaro 40:3ba2b0ea9f33 1085 // properly called lsr.pushPoseMatrix())
mbedalvaro 40:3ba2b0ea9f33 1086 //lsr.setOrthoProjection();
mbedalvaro 40:3ba2b0ea9f33 1087 //lsr.setIdentityPose(); // we could have done the translation here instead of in "local coordinates", but it's the same.
mbedalvaro 40:3ba2b0ea9f33 1088 lsr.popPoseMatrix();
mbedalvaro 40:3ba2b0ea9f33 1089 drawScene();
mbedalvaro 40:3ba2b0ea9f33 1090 auxDataBuffer_index = 0; // to restart loading the auxiliary buffer 'auxDataBuffer' through serial port
mbedalvaro 40:3ba2b0ea9f33 1091 }
mbedalvaro 40:3ba2b0ea9f33 1092
mbedalvaro 40:3ba2b0ea9f33 1093 // (b) a NUMBER to convert to float and to add to the auxiliary data buffer:
mbedalvaro 40:3ba2b0ea9f33 1094 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
mbedalvaro 40:3ba2b0ea9f33 1095 // the modelview, the projection matrix or some other data):
mbedalvaro 40:3ba2b0ea9f33 1096 receivedStringData[indexStringData] = 0;
mbedalvaro 40:3ba2b0ea9f33 1097 indexStringData = 0;
mbedalvaro 40:3ba2b0ea9f33 1098 // Serial.println(receivedStringData); // for tests
mbedalvaro 40:3ba2b0ea9f33 1099 // convert to float and store in auxiliary "matrix" array:
mbedalvaro 40:3ba2b0ea9f33 1100 auxDataBuffer[auxDataBuffer_index] = atof(receivedStringData);
mbedalvaro 40:3ba2b0ea9f33 1101 //Serial.println( auxDataBuffer[auxDataBuffer_index]); // for tests
mbedalvaro 40:3ba2b0ea9f33 1102 auxDataBuffer_index++;
mbedalvaro 40:3ba2b0ea9f33 1103 }
mbedalvaro 40:3ba2b0ea9f33 1104
mbedalvaro 40:3ba2b0ea9f33 1105 /*
mbedalvaro 40:3ba2b0ea9f33 1106 else if (incomingByte=='!') { // sets the width of the letter
mbedalvaro 40:3ba2b0ea9f33 1107 receivedStringData[indexStringData]=0;
mbedalvaro 40:3ba2b0ea9f33 1108 indexStringData=0;
mbedalvaro 40:3ba2b0ea9f33 1109 fontWidth=atof(receivedStringData);
mbedalvaro 40:3ba2b0ea9f33 1110 createTextScene(); // needed because we changed the geometry
mbedalvaro 40:3ba2b0ea9f33 1111 }
mbedalvaro 40:3ba2b0ea9f33 1112
mbedalvaro 40:3ba2b0ea9f33 1113 else if (incomingByte=='"') { // sets the height of the letter
mbedalvaro 40:3ba2b0ea9f33 1114 receivedStringData[indexStringData]=0;
mbedalvaro 40:3ba2b0ea9f33 1115 indexStringData=0;
mbedalvaro 40:3ba2b0ea9f33 1116 fontHeight=atof(receivedStringData);
mbedalvaro 40:3ba2b0ea9f33 1117 createTextScene(); // needed because we changed the geometry
mbedalvaro 40:3ba2b0ea9f33 1118 }
mbedalvaro 40:3ba2b0ea9f33 1119 */
mbedalvaro 40:3ba2b0ea9f33 1120
mbedalvaro 40:3ba2b0ea9f33 1121 // (c) TERMINATOR indicating the data was text to display:
mbedalvaro 40:3ba2b0ea9f33 1122 else if (incomingByte == '"') { // this means that the previous data was TEXT to display
mbedalvaro 40:3ba2b0ea9f33 1123 receivedStringData[indexStringData] = 0; // termination for the string
mbedalvaro 40:3ba2b0ea9f33 1124 indexStringData = 0;
mbedalvaro 40:3ba2b0ea9f33 1125 // Serial.println(receivedStringData);
mbedalvaro 40:3ba2b0ea9f33 1126 textToDisplay = string(receivedStringData);
mbedalvaro 42:5f21a710ebc5 1127 // stopDisplay();
mbedalvaro 42:5f21a710ebc5 1128 // addText();
mbedalvaro 42:5f21a710ebc5 1129 createTextScene();
mbedalvaro 40:3ba2b0ea9f33 1130 drawScene(); // needed because we changed the geometry, and we need to compute the projection once for display (of course, this will
mbedalvaro 40:3ba2b0ea9f33 1131 // happens anyway when the computer send the pose matrix).
mbedalvaro 42:5f21a710ebc5 1132 // resumeDisplay();
mbedalvaro 40:3ba2b0ea9f33 1133 }
mbedalvaro 40:3ba2b0ea9f33 1134
mbedalvaro 40:3ba2b0ea9f33 1135 // (d) TERMINATOR indicating data was for the POSE MATRIX of the object (Mp) (with respect to the CAMERA):
mbedalvaro 40:3ba2b0ea9f33 1136 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
mbedalvaro 40:3ba2b0ea9f33 1137 auxDataBuffer_index = 0; // to restart loading the auxiliary buffer 'auxDataBuffer' through serial port
mbedalvaro 40:3ba2b0ea9f33 1138 // Now, auxDataBuffer is a buffer with 12 values (4x3), corresponding to the pose of the object in CAMERA coordinartes (RT')
mbedalvaro 40:3ba2b0ea9f33 1139 lsr.setIdentityPose(); // RT=ID
mbedalvaro 40:3ba2b0ea9f33 1140 lsr.multPoseMatrix(lsr.EXTRINSICS); // RT=E // we can simply do: lsr.setExtrinsicsPose()
mbedalvaro 40:3ba2b0ea9f33 1141 lsr.multPoseMatrix(auxDataBuffer); // RT=ExRT' : this sets RT as the current object pose in PROJECTOR coordinates
mbedalvaro 40:3ba2b0ea9f33 1142
mbedalvaro 40:3ba2b0ea9f33 1143 drawScene(); // needed because we changed the POSE (but there is no need to re-create the geometry, just project)
mbedalvaro 40:3ba2b0ea9f33 1144
mbedalvaro 40:3ba2b0ea9f33 1145 // Handshake:
mbedalvaro 40:3ba2b0ea9f33 1146 pc.putc(13);
mbedalvaro 40:3ba2b0ea9f33 1147 }
mbedalvaro 40:3ba2b0ea9f33 1148
mbedalvaro 40:3ba2b0ea9f33 1149 // (d) TERMINATOR indicating data was for the projection matrix (will probably be saved in "hard" in the microcontroller code):
mbedalvaro 40:3ba2b0ea9f33 1150 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
mbedalvaro 40:3ba2b0ea9f33 1151 auxDataBuffer_index = 0; // to restart loading the auxiliary buffer 'auxDataBuffer' through serial port
mbedalvaro 40:3ba2b0ea9f33 1152 // store in projection matrix:
mbedalvaro 40:3ba2b0ea9f33 1153 lsr.loadProjMatrix(auxDataBuffer, 1.0);
mbedalvaro 40:3ba2b0ea9f33 1154
mbedalvaro 40:3ba2b0ea9f33 1155 drawScene(); // needed because we changed the PROJECTION matrix (but there is no need to re-create all the geometry, just project)
mbedalvaro 40:3ba2b0ea9f33 1156 }
mbedalvaro 40:3ba2b0ea9f33 1157
mbedalvaro 40:3ba2b0ea9f33 1158 //(e) TERMINATOR indicating the data was for the extrinsic matrix:
mbedalvaro 40:3ba2b0ea9f33 1159 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
mbedalvaro 40:3ba2b0ea9f33 1160 auxDataBuffer_index = 0; // to restart loading the auxiliary buffer 'auxDataBuffer' through serial port
mbedalvaro 40:3ba2b0ea9f33 1161 // store in projection matrix:
mbedalvaro 40:3ba2b0ea9f33 1162 lsr.loadExtrinsicsMatrix(auxDataBuffer);
mbedalvaro 40:3ba2b0ea9f33 1163
mbedalvaro 40:3ba2b0ea9f33 1164 drawScene(); // needed because we changed the EXTRINSICS (hence the pose), but there is no need to re-create the geometry, just project.
mbedalvaro 40:3ba2b0ea9f33 1165 }
mbedalvaro 40:3ba2b0ea9f33 1166
mbedalvaro 40:3ba2b0ea9f33 1167 }
mbedalvaro 40:3ba2b0ea9f33 1168 }