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@43:1dd4cfc30788, 2013-10-21 (annotated)

Committer:
mbedalvaro
Date:
Mon Oct 21 11:26:48 2013 +0000
Revision:
43:1dd4cfc30788
Parent:
42:5f21a710ebc5
Child:
44:2432c218f191
This is working very simply, objects don't have behaviours. IT would be nice to add "behaviours" that may be common to all the objects. These could be a collection of methods acting on the 3d coordintates of the objects, and belong to a friend class

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