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@47:199042980678, 2014-04-17 (annotated)

Committer:
mbedalvaro
Date:
Thu Apr 17 08:04:14 2014 +0000
Revision:
47:199042980678
Parent:
46:e0dd2d1d07c1 
publishing for sharing with Ken Iwasaki

Who changed what in which revision?

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