Alvaro Cassinelli
Laser Sensing Display for UI interfaces in the real world
Fork of
skinGames_forktest
by 
Alvaro Cassinelli
Diff: main.cpp
- Revision:
- 42:5f21a710ebc5
- Parent:
- 41:74e24a0e6e50
- Child:
- 43:1dd4cfc30788
--- a/main.cpp Wed Oct 16 17:26:13 2013 +0000
+++ b/main.cpp Fri Oct 18 07:25:03 2013 +0000
@@ -7,6 +7,12 @@
#include "mbed.h"
#include "mbedOSC.h"
+#include "WrapperFunctions.h"
+
+// 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()...)
+#include "LaserRenderer.h"
+extern LaserRenderer lsr;
+
// mbed IP address (server):
#ifdef DHCP
@@ -21,30 +27,23 @@
#endif
//uint8_t serverMac[] = { 0xDE, 0xAD, 0xBE, 0xEF, 0xFE, 0xED };
-uint8_t serverIp[] = { 10, 0, 0, 2 };
+uint8_t serverIp[] = { 10, 0, 0, 2 };
int serverPort = 10000;
-//uint8_t destIp[] = {10, 0, 0, 3};
+//uint8_t destIp[] = {10, 0, 0, 3};
uint8_t destIp[] = {255, 255, 255, 255}; // broadcast, so we can use several computers for sound, etc
int destPort = 12000;
char *topAddress="/mbed";
-char *subAddress[3]={ "/test1" , "/test2" , "/test3" };
+char *subAddress[3]= { "/test1" , "/test2" , "/test3" };
OSCMessage recMes;
OSCMessage sendMes;
OSCClass osc;
//OSCClass osc(&recMes); // instantiate OSC communication object, and set the receiver container from the OSC packets
-void processOSC(UDPSocketEvent e);
-#include "WrapperFunctions.h"
-
-// The following is because I still did not "wrap" the LaserRenderer methods (so we have to use the object lsr directly, for instance to set a matrix: lsr.setIdentityPose()...)
-#include "LaserRenderer.h"
-extern LaserRenderer lsr;
-
// for tests:
extern Scene scene;
extern laserSensingDisplay lsd;
@@ -61,18 +60,18 @@
unsigned long lastTimeSensed;
// ======================================================================================================================
-// DEFAULT CALIBRATION MATRICES (Extrinsics and Intrinsics).
+// DEFAULT CALIBRATION MATRICES (Extrinsics and Intrinsics).
// -> would be better to load them from a system file!! (TO DO)
// Note: last row is completed automatically in the loadExtrinsicMatrix and loadProjMatrix methods
// Extrinsics of usb camera to projector (note: calibration was in cm, so the translation vector is in cm)
float E1[12] = {9.8946330287356954e-01, -5.0979372852926171e-03, -1.4469410251272136e-01, 5.8356271792647112e+00,
- -4.7536907255693065e-03, 9.9769720674821438e-01, -6.7658599389111715e-02, -6.0821695799283804e+00,
- 1.4470582120637832e-01, 6.7633532232509466e-02, 9.8716053943963022e-01, -8.2545447546057940e+00
- };
+ -4.7536907255693065e-03, 9.9769720674821438e-01, -6.7658599389111715e-02, -6.0821695799283804e+00,
+ 1.4470582120637832e-01, 6.7633532232509466e-02, 9.8716053943963022e-01, -8.2545447546057940e+00
+ };
// 9.9540692993883650e-01, -8.5578652572728450e-03, -9.5350966287597705e-02, 2.2256570914518332e+00,
- // 1.4872282274740087e-02, 9.9772789680840523e-01, 6.5710418886328711e-02, -6.9310510316834923e+00,
- // 9.4571978141945845e-02, -6.6826692814433777e-02, 9.9327253766416224e-01, 1.0699927478132011e+01
+// 1.4872282274740087e-02, 9.9772789680840523e-01, 6.5710418886328711e-02, -6.9310510316834923e+00,
+// 9.4571978141945845e-02, -6.6826692814433777e-02, 9.9327253766416224e-01, 1.0699927478132011e+01
//};
@@ -81,15 +80,15 @@
0.0000000e+000, 5.9647451141521778e+03, 2.9005238051336592e+03
};
-// 7.0776317995422105e+03, 0., 2.4203185896583996e+03,
+// 7.0776317995422105e+03, 0., 2.4203185896583996e+03,
// 0., 7.2889265345161484e+03, 1.7718110988625751e+03};
// SCALE FACTOR for the SCAN:
float scaleFactorProjector1 = 1.0; //scale factor for the projector image (the scan is, say, 600x600, but the resolution is 4096x4096)
// ======================================================================================================================
-// LASER TERMINAL:
-// We assume that the "terminal" is operating on an a4 page. Units are in cm.
+// LASER TERMINAL:
+// We assume that the "terminal" is operating on an a4 page. Units are in cm.
V2 cursorPosition(0,0);
void createTextScene();
@@ -97,7 +96,6 @@
string textToDisplay = "HELLO";
float fontWidth = 2.5, fontHeight = 2.5; // if calibration is in cm, then this is in cm.
-
// ==============================================================================================================
// Some global functions and variables (find a better way to integrate this!)
extern "C" void mbed_reset();
@@ -105,13 +103,28 @@
void createSceneTest();
void setOrthographicView();
+void processOSC(UDPSocketEvent e);
void interpretData();
-void processOSC();
void processSerial();
int touchedTimes = 0;
enum sensingModes {NO_SENSING, SENSING_WHOLE_SCENE, SENSING_PER_OBJECT};
-sensingModes currentSensingMode=SENSING_PER_OBJECT;//NO_SENSING;
+sensingModes currentSensingMode=SENSING_PER_OBJECT;//NO_SENSING;
+
+// ================= AUXILIARY VARIABLES FOR INTERPRETING COMMAND FROM SERIAL OR OSC =========================
+// NOTE: the following arrays are GLOBAL (used in processOSC and processSerial, as well as in interpretData function):
+// max of two addresses (top and sub), of a max length of 24 characters:
+char address[2][24];
+//long auxdata[2]; // to store a max of two arguments (note: we will only use LONGs)
+int data[2]; // this is to have -1 as NO DATA, to detect errors.
+int numArgs;
+
+// An auxiliary buffer to store parameters data, or matrices, or points to load as vertices.
+// 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
+// we can send 3d points, this means 3 floats per point; let's limit the number of points per object to 128 (a lot)
+float auxDataBuffer[128];
+int auxDataBuffer_index=0;
+
// ==============================================================================================================
int main()
@@ -125,7 +138,7 @@
// Setup:
// (1) Hardware init (laser powers, positions...):
IO.init();
-
+
// OSC initialization:
// Set the Ethernet port:
printf("Setting up...\r\n");
@@ -145,10 +158,23 @@
//setting osc functionnality:
//(2) Receiving:
- // recMes.setIp( serverIp ); // not needed?
+ recMes.setIp( serverIp ); // not needed?
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)
osc.begin(serverPort, &processOSC); // binds the upd (osc) messages to an arbitrary listening port ("server" port), and callback function
-
+ wait(1);
+
+ //while(true) {
+// // COMMUNICATION:
+// // (a) Reading commands:
+// // Ethernet:
+// Net::poll(); // this will take care of calling processOSC(UDPSocketEvent e) when a new packet arrives.
+//
+// wait(.5);
+// sendMes.setTopAddress("osc");
+// sendMes.setSubAddress("working");
+// osc.sendOsc( &sendMes );
+// }
+
//===================================================================
/*
@@ -242,8 +268,11 @@
lsr.loadExtrinsicsMatrix(E1);
// (2) Create the scene in "local" coordinates:
- //createTextScene(); // Create a default scene (for tests)
- setOrthographicView();
+ createTextScene(); // Create a default scene (for tests)
+
+ // or start with nothing:
+ //clearScene();
+ //setOrthographicView();
// (3) Set a default PERSPECTIVE and GLOBAL POSE for start displaying:
lsr.loadProjMatrix(K1, scaleFactorProjector1);
@@ -260,7 +289,7 @@
// (4) Main loop:
while(true) {
-
+
// =========== Query hardware events (knobs, ethernet, serial port) ============
Net::poll(); // This polls the network connection for new activity, without keeping on calling this you won't receive any OSC!
if (pc.readable()) processSerial(); // here, the pose can be changed by a pc command
@@ -324,7 +353,7 @@
// we need to redraw the scene (ie, reproject), or at least this object:
//lsr.renderObject(ptrObj, lsr.RT);
- // or "highlight" the object by putting "cartouche" around it object (note: do this only if it did NOT have
+ // or "highlight" the object by putting "cartouche" around it object (note: do this only if it did NOT have
//addCartoucheObject(ptrObj);
//pc.putc(i+48);
@@ -343,14 +372,15 @@
// =================================
}
-void setOrthographicView() {
- clearScene();
- // set color:
- lsr.setColor(0x02); // green
- //IO.setLaserLockinPower(0); // DISABLING SENSING LASER! (here the red). This is FOR CALIBRATION laser-camera
-
- lsr.setOrthoProjection();
- lsr.setIdentityPose(); // we could have done the translation here instead of in "local coordinates", but it's the same.
+void setOrthographicView()
+{
+ // clearScene();
+ // set color:
+ lsr.setColor(0x02); // green
+ //IO.setLaserLockinPower(0); // DISABLING SENSING LASER! (here the red). This is FOR CALIBRATION laser-camera
+
+ lsr.setOrthoProjection();
+ lsr.setIdentityPose(); // we could have done the translation here instead of in "local coordinates", but it's the same.
}
void createSceneTest()
@@ -457,80 +487,126 @@
}
-// ================= INTERPRET COMMAND =========================
-// NOTE: the following arrays are GLOBAL (used in processOSC and processSerial, as well as in interpretData function):
-// max of two addresses (top and sub), of a max length of 24 characters:
-char address[2][24];
-//long auxdata[2]; // to store a max of two arguments (note: we will only use LONGs)
-int data[2]; // this is to have -1 as NO DATA, to detect errors.
-int numArgs;
+void addText()
+{
+ lsr.pushPoseMatrix();
+ //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).
+ // The first option is simpler now, we just use the wrapper function string3d(string _text, float totalWidth, float height):
+
+ // (1) Clear the scene (this stops displaying, thus creating a "laser hot spot", but in principle it only happens in the setup)
+ // clearScene(); // NOTE: in the future (and if we have enough RAM, we can have several indexed "scenes", instead of a unique global)
+
+ // (2) Set displaying attributes (colors and pose):
+ // (a) Set the "local pose" (i.e., the MODELING TRANSFORMATION in OpenGL jargon):
+ lsr.setIdentityPose(); // we could use a wrapper, but I won't for the time being.
+ //lsr.flipY();
+ //lsr.translate(2,6,0);
+
+ lsr.flipY();
+ lsr.flipX();
+ lsr.translate(-15,-10,0);
+
+ // (b) set current color:
+ lsr.setColor(0x04); // three LSB bits for RGB - so 0x02 is GREEN ON (note that the sensing laser, now red, is always on...).
-// An auxiliary buffer to store parameters data, or matrices, or points to load as vertices.
-// 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
-// we can send 3d points, this means 3 floats per point; let's limit the number of points per object to 128 (a lot)
-float auxDataBuffer[128];
-int auxDataBuffer_index=0;
+ // A string as a UNIQUE object:
+ // begin(1); // start creating an object with index 1
+ // no retouching of the modelview, meaning the text is in (0,0,0) in "local" coordinates
+ // string3d(textToDisplay, textToDisplay.size()*fontWidth, fontHeight);
+ //end();
+
+ // A string as a *collection* of object-letters:
+ //lsr.translate(-textToDisplay.length()*fontWidth/2,-fontHeight/2,0);
+ for (unsigned short i = 0; i < textToDisplay.length(); i++) {
+ char ch = textToDisplay.at(i);
+ if (ch != ' ') {
+ begin(i);
+ letter3d(ch, fontWidth, fontHeight);
+ end();
+ }
+ lsr.translate(1.1*fontWidth, 0, 0);
+ }
+
+ updateScene(); // this is important: this will compute the number of objects, points, etc, to prepare the displaying engine (but rendering is NOT yet done,
+ // rendering is done in the drawScene() function.
+
+ lsr.popPoseMatrix();
+
+}
//================== OSC messageReceivedCallback function =====================
//============= RECEIVE OSC COMMANDS =========================
// This is the callback function called when there are packets on the listening socket. It is not nice to have it
// here, but for the time being having a "wrapping global" is the simplest solution (we cannot pass a member-function pointer
// as handler to the upd object).
-void processOSC(UDPSocketEvent e) {
+void processOSC(UDPSocketEvent e)
+{
osc.onUDPSocketEvent(e);
if (osc.newMessage) {
-
- pc.putc('A');
-
+
+ // pc.printf("RECEIVED NEW OSC MESSAGE ----\n");
+
// Acquire the addresses and arguments and put them in the GLOBAL variables to be processed by the interpretData function (common to serial and OSC):
strcpy(address[0],"");
strcpy(address[1],"");
- for (int i=0; i<recMes.getAddressNum(); i++) strcpy(address[i],recMes.getAddress(i)); // NOTE: up to the rest of the program to check if address[1] is really not null
+ for (int i=0; i<recMes.getAddressNum(); i++) {
+ strcpy(address[i],recMes.getAddress(i)); // NOTE: up to the rest of the program to check if address[1] is really not null
+ // pc.printf("Address %d = %s \n",i, address[i]);
+ }
// Acquire data:
+
numArgs=recMes.getArgNum();
data[0]=-1;
data[1]=-1;
- for (int i=0; i<numArgs; i++) data[i]=(int)recMes.getArgInt(i);
+ for (int i=0; i<numArgs; i++) {
+ data[i]=(int)recMes.getArgInt(i);//recMes.getArgFloat(i);
+ //pc.printf("%4.2f %4.2f \n", data[i], recMes.getArgFloat(i));
+ // pc.printf("%i %i \n", data[i], recMes.getArgInt(i));
+ }
// Finally, interpret the command or data:
- interpretData();
-
- }
+ interpretData();
+
+ }
}
//interpretData(const char& address[2][], const int& data[2]) {
-void interpretData() {
-
+void interpretData()
+{
+
// ========== DATA (all this is a hack because my OSC class does not accept BUNDLES...) =============
// Add data to auxiliary data buffer:
if (!strcmp(address[0], "data")) {
// numArgs is the number of arguments (in my OSC version this is max 2):
- for (int i=0; i<numArgs; i++) auxDataBuffer[auxDataBuffer_index] = data[i];
- auxDataBuffer_index+=numArgs;
- // note: the index will be reset to 0 or whatever needed when a command USING data from the "d" message is received.
+ // pc.printf("--- Data pack: %d\n", numArgs);
+ for (int i=0; i<numArgs; i++) {
+ auxDataBuffer[auxDataBuffer_index+i] = 1.0*data[i];
+ // note: the index will be reset to 0 or whatever needed when a command USING data from the "d" message is received.
+ // pc.printf("buffer index / data: %i %f \n", auxDataBuffer_index+i, auxDataBuffer[auxDataBuffer_index+i]);//data[i]);
+ }
+ auxDataBuffer_index+=numArgs;
+ //pc.putc('/n');
}
-
// ========== COMMANDS (using data on auxDataBuffer or directly as arguments =========================
-
+
else if ( !strcmp(address[0], "mbedReset" ) ) mbed_reset();
-
- else if ( !strcmp(address[0], "testPower" ) ) {
+
+ else if ( !strcmp(address[0], "testPower" ) ) {
// TO DO
}
-
- // Enable/disable projection:
- else if ( !strcmp(address[0], "stopDisplay" )) stopDisplay();
- else if (!strcmp(address[0], "resumeDisplay")) resumeDisplay();
- else if (!strcmp(address[0], "showLimits")) {
+
+ // Enable/disable projection:
+ else if ( !strcmp(address[0], "stopDisplay" )) stopDisplay();
+ else if (!strcmp(address[0], "resumeDisplay")) resumeDisplay();
+ else if (!strcmp(address[0], "showLimits")) {
int value=data[0]; // argument is the number of seconds for display
if (value!=-1) {
showLimitsMirrors(50, value);
}
- }
- else if (!strcmp(address[0], "setSensingMode")) {
+ } else if (!strcmp(address[0], "setSensingMode")) {
int value=data[0];
if (value!=-1) { // otherwise do nothing, this is a reception error (there was no data)
switch(value) {
@@ -547,131 +623,169 @@
break;
}
}
- }
+ }
- // Change state machine color (objects created from this point on will have the state machine color)
- else if (!strcmp(address[0], "setStateColor")) { // example: 0( (all OFF), 1( blue ON, 2( green ON, 4( red ON, 3( blue and green ON; etc.
- int value=data[0];
+ // Change state machine color (objects created from this point on will have the state machine color)
+ else if (!strcmp(address[0], "setStateColor")) { // example: 0( (all OFF), 1( blue ON, 2( green ON, 4( red ON, 3( blue and green ON; etc.
+ int value=data[0];
if (value!=-1) lsr.setColor(value);
- }
+ }
- // Real time change of color of ALL the objects in the CURRENT scene (state machine color not changed)
- else if (!strcmp(address[0], "setAllObjectsColor")) { // example: 0) (all OFF), 1) blue ON, 2) green ON, 4) red ON, 3) blue and green ON; etc.
- int value=data[0];
- if (value!=-1) changeColorScene(value);
- }
+ // Real time change of color of ALL the objects in the CURRENT scene (state machine color not changed)
+ else if (!strcmp(address[0], "setAllObjectsColor")) { // example: 0) (all OFF), 1) blue ON, 2) green ON, 4) red ON, 3) blue and green ON; etc.
+ int value=data[0];
+ if (value!=-1) changeColorScene(value);
+ }
// =================== CREATION and DESTRUCTION of objects =========================================================
- else if (!strcmp(address[0], "clearScene")) {
- clearScene();
- }
-
- else if (!strcmp(address[0], "deleteObject")) { // second address is object identifier (a string that will be converted
- int idobject=atoi(address[1]);
- deleteObject(idobject);
- }
+ else if (!strcmp(address[0], "clearScene")) {
+ clearScene();
+ }
+
+ else if (!strcmp(address[0], "deleteObject")) { // second address is object identifier (a string that will be converted
+ int idobject=atoi(address[1]);
+ deleteObject(idobject);
+ }
+
+ // Produce a grid of points. The projection is ORTHOGRAPHIC. Also, POSE is reset to identity (i.e., the sent points should have
+ // been translated previously - this may be optional, we could first send the pose, then the points...)
+ // Note: orthographic projection can be achieved by simply setting the projection matrix to ID, while
+ // 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.
+ // Finally, if one wants to avoid an unnecessary matrix product, the laserRendering class provides a renderingMode that can be set to RAW.
+ // NOTE: the following string of ASCII characters is assumed to have been input:
+ // posX#posY#sizeX#sizeY#nx#ny#:
+ // where the names (ex: posX) is in fact an ASCII string representing the number in decimal base. This means that when we arrive here,
+ // the auxiliary array auxDataBuffer contains all that data (as floats), ex: auxDataBuffer[0] is just posX, and auxDataBuffer[5] is ny
+ else if (!strcmp(address[0], "gridScene")) {
+ // Data is now ordered in the auxDataBuffer array:
+ float posX = auxDataBuffer[0], posY = auxDataBuffer[1], sizeX = auxDataBuffer[2], sizeY = auxDataBuffer[3], nx = auxDataBuffer[4], ny = auxDataBuffer[5];
+ auxDataBuffer_index=0; // to restart loading the auxiliary buffer 'auxDataBuffer'
+ clearScene();
+ // set color:
+ lsr.setColor(0x02); // green
+ //IO.setLaserLockinPower(0); // DISABLING SENSING LASER! (here the red). This is FOR CALIBRATION laser-camera
- // Produce a grid of points. The projection is ORTHOGRAPHIC. Also, POSE is reset to identity (i.e., the sent points should have
- // been translated previously - this may be optional, we could first send the pose, then the points...)
- // Note: orthographic projection can be achieved by simply setting the projection matrix to ID, while
- // 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.
- // Finally, if one wants to avoid an unnecessary matrix product, the laserRendering class provides a renderingMode that can be set to RAW.
- // NOTE: the following string of ASCII characters is assumed to have been input:
- // posX#posY#sizeX#sizeY#nx#ny#:
- // where the names (ex: posX) is in fact an ASCII string representing the number in decimal base. This means that when we arrive here,
- // the auxiliary array auxDataBuffer contains all that data (as floats), ex: auxDataBuffer[0] is just posX, and auxDataBuffer[5] is ny
- else if (!strcmp(address[0], "gridScene")) {
- // Data is now ordered in the auxDataBuffer array:
- float posX = auxDataBuffer[0], posY = auxDataBuffer[1], sizeX = auxDataBuffer[2], sizeY = auxDataBuffer[3], nx = auxDataBuffer[4], ny = auxDataBuffer[5];
- auxDataBuffer_index=0; // to restart loading the auxiliary buffer 'auxDataBuffer'
- clearScene();
- // set color:
- lsr.setColor(0x02); // green
- //IO.setLaserLockinPower(0); // DISABLING SENSING LASER! (here the red). This is FOR CALIBRATION laser-camera
-
- // set local pose matrix to identity:
- lsr.setIdentityPose();
- // translate to the required position:
- lsr.translate(posX, posY, 0); // since the projection and pose are going to be set
- // to Identity (projection identity will automatically make the projection orthographic), this means that these
- // values are in "laser projector pixels" (in fact angles).
- // Create the collection of dot objects:
- // void grid(float sizeX, float sizeY, int nx, int ny, int repeatpoint);
- grid(sizeX, sizeY, nx, ny, 1);
- updateScene(); // this is important: this will compute the number of objects, points, etc, to prepare the displaying engine (but rendering is NOT yet done,
- // rendering is done in the drawScene() function.
+ // set local pose matrix to identity:
+ lsr.setIdentityPose();
+ // translate to the required position:
+ lsr.translate(posX, posY, 0); // since the projection and pose are going to be set
+ // to Identity (projection identity will automatically make the projection orthographic), this means that these
+ // values are in "laser projector pixels" (in fact angles).
+ // Create the collection of dot objects:
+ // void grid(float sizeX, float sizeY, int nx, int ny, int repeatpoint);
+ grid(sizeX, sizeY, nx, ny, 1);
+ updateScene(); // this is important: this will compute the number of objects, points, etc, to prepare the displaying engine (but rendering is NOT yet done,
+ // rendering is done in the drawScene() function.
- // Now we can render the scene once:
- // Set a default GLOBAL POSE and orthographic projection for rendering and start displaying:
- lsr.setOrthoProjection();
- lsr.setIdentityPose(); // we could have done the translation here instead of in "local coordinates", but it's the same.
- drawScene(); // this will be called when we change the pose matrix - but needs to be called at least one to start displaying
- }
- // 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!
+ // Now we can render the scene once:
+ // Set a default GLOBAL POSE and orthographic projection for rendering and start displaying:
+ lsr.setOrthoProjection();
+ lsr.setIdentityPose(); // we could have done the translation here instead of in "local coordinates", but it's the same.
+ drawScene(); // this will be called when we change the pose matrix - but needs to be called at least one to start displaying
+ }
+ // 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!
+
+ // 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)
+ // using orthographic projection.
+ // This is in particular useful to create "structured light" grids - such the grid of planar points for calibration.
+ // In the future, we can have functions to add objects, etc. Basically, every wrapped function should be call-able by the PC.
+ // 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
+ // cameraProjectorCalibration Xcode project as little as possible, I will perform the transformations on the computer.
+ else if (!strcmp(address[0], "arbitraryGridScene")) {
+ // Data is now ordered in the auxDataBuffer array as PAIRS (X,Y). The argument to this message is the IDENTIFIER of the object:
+ // The present value of auxDataBuffer_index contains the number of points x 2
+ clearScene();
+ lsr.setIdentityPose();
+ //lsr.setColor(0x02); // fixed color, or use the current color (in which case, we need to call lsr.pushColor() and then pop it again).
+ lsr.setColor(0x03); // blue+green
+ IO.setLaserLockinPower(0); // DISABLING SENSING LASER! (here the red). This is FOR CALIBRATION laser-camera
- // 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)
- // using orthographic projection.
- // This is in particular useful to create "structured light" grids - such the grid of planar points for calibration.
- // In the future, we can have functions to add objects, etc. Basically, every wrapped function should be call-able by the PC.
- // 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
- // cameraProjectorCalibration Xcode project as little as possible, I will perform the transformations on the computer.
- else if (!strcmp(address[0], "arbitraryGridScene")) {
- // Data is now ordered in the auxDataBuffer array as PAIRS (X,Y). The argument to this message is the IDENTIFIER of the object:
- // The present value of auxDataBuffer_index contains the number of points x 2
- clearScene();
- lsr.setIdentityPose();
- //lsr.setColor(0x02); // fixed color, or use the current color (in which case, we need to call lsr.pushColor() and then pop it again).
- lsr.setColor(0x03); // blue+green
- IO.setLaserLockinPower(0); // DISABLING SENSING LASER! (here the red). This is FOR CALIBRATION laser-camera
-
- // begin(1) // we can create a single object, or one object per position (with repetition of the point if needed)
- for (unsigned char i = 0; i < auxDataBuffer_index / 2; i++) {
- begin(i);
- for (unsigned char k=0; k<1; k++) // fixed repetition of points - we can send this as a parameter if we want.
- vertex(auxDataBuffer[2 * i], auxDataBuffer[2 * i + 1], 0);
- end();
- }
- updateScene();
- // Set a default GLOBAL POSE and ORTHO projection matrix for start displaying, or use the CURRENT ONE (for that, we should have
- // properly called lsr.pushPoseMatrix())
- lsr.setOrthoProjection();
- lsr.setIdentityPose();
- drawScene();
- auxDataBuffer_index = 0; // to restart loading the auxiliary buffer 'auxDataBuffer' through serial port
+ // begin(1) // we can create a single object, or one object per position (with repetition of the point if needed)
+ for (unsigned char i = 0; i < auxDataBuffer_index / 2; i++) {
+ begin(i);
+ for (unsigned char k=0; k<1; k++) // fixed repetition of points - we can send this as a parameter if we want.
+ vertex(auxDataBuffer[2 * i], auxDataBuffer[2 * i + 1], 0);
+ end();
}
+ updateScene();
+ // Set a default GLOBAL POSE and ORTHO projection matrix for start displaying, or use the CURRENT ONE (for that, we should have
+ // properly called lsr.pushPoseMatrix())
+ lsr.setOrthoProjection();
+ lsr.setIdentityPose();
+ drawScene();
+ auxDataBuffer_index = 0; // to restart loading the auxiliary buffer 'auxDataBuffer' through serial port
+ }
+
+ // 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):
+ else if (!strcmp(address[0], "createObject2D")) {
+ unsigned char numPoints= 11;//auxDataBuffer_index/2;
+ //auxDataBuffer_index = 0; // to restart loading the auxiliary buffer 'auxDataBuffer' through serial port
+
+ // pc.printf("creating 2d object with %d points\n", numPoints);
+
+ int IDObject=atoi(address[1]); // the object identifier is on the second address
+ // the initial position is on data[0-1]
+ // pc.printf("Object ID is %d \n", IDObject);
+ //pc.printf("Position at: %4.2f, %4.2f \n", (float)data[0], (float)data[1]);
+ // pc.printf("Position at: %d, %d \n", (int)data[0], (int)data[1]);
+
+ if (IDObject!=-1) {
+
+ if(true) {
+
+ clearScene();
+
+ // First, create the object:
+ lsr.pushPoseMatrix();
+ lsr.setIdentityPose();
+
+ // lsr.translate(-15,-10,0);
+
+ lsr.translate(20,20,0);
- // 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):
- else if (!strcmp(address[0], "createObject2d")) {
- int IDObject=data[1];
- if (IDObject!=-1) {
- // Data is now ordered in the auxDataBuffer array in triplets (X,Y,Z).
- // The present value of auxDataBuffer_index contains the number of points x 3
- //lsr.pushPoseMatrix(); // we will render on the current pose
- //clearScene();
- //lsr.setIdentityPose();
- //lsr.setColor(0x02); // we will render in current color
- begin(IDObject);
- for (unsigned char i = 0; i < auxDataBuffer_index / 3; i++) {
- vertex(auxDataBuffer[3 * i], auxDataBuffer[3 * i + 1],0); // z=0 for 2d objects (in current pose)
+ // lsr.translate(5,10,0);
+ // lsr.translate((int)data[0], (int)data[1], 0);
+
+ lsr.setColor(0x02); // we will render in current color
+
+ //begin(IDObject);
+ for (unsigned char i = 0; i < numPoints; i++) {
+ begin(i);
+ //vertex(auxDataBuffer[2 * i], auxDataBuffer[2 * i + 1],0); // z=0 for 2d objects (in current pose)
+ //letter3d('A', fontWidth, fontHeight);
+ vertex(10*cos(2*PI/numPoints*i), 10*sin(2*PI/numPoints*i), 0);
+ vertex(5*cos(2*PI/numPoints*i+.1), 5*sin(2*PI/numPoints*i+.1), 0);
+ end();
+ //pc.printf("local (x,y): %f, %f \n", auxDataBuffer[2 * i], auxDataBuffer[2 * i+1]);
+ // lsr.translate(1.1*fontWidth, 0, 0);
+ }
+ lsr.popPoseMatrix();
+
+ // update the scene for display
+ updateScene();
+
+ //createTextScene();
+
+ // Set the global projection and modelview:
+ lsr.loadProjMatrix(K1, scaleFactorProjector1);
+ lsr.setIdentityPose();
+ lsr.multPoseMatrix(lsr.EXTRINSICS); // RT=E // we can simply do: lsr.setExtrinsicsPose()
+ // DEFAULT position in CAMERA coordinates (calibration was in cm):
+ lsr.translate(0, 0, 130);
+
+ // RENDER the scene:
+ drawScene(); // this will be called when we change the pose matrix - but needs to be called at least once to start displaying
}
- end();
- updateScene();
- // Set a default GLOBAL POSE and ORTHO projection matrix for start displaying, or use the CURRENT ONE (for that, we should have
- // properly called lsr.pushPoseMatrix())
- //lsr.setOrthoProjection();
- //lsr.setIdentityPose(); // we could have done the translation here instead of in "local coordinates", but it's the same.
- //lsr.popPoseMatrix();
- drawScene();
- auxDataBuffer_index = 0; // to restart loading the auxiliary buffer 'auxDataBuffer' through serial port
- }
+
}
+ }
- // This means we received 3D vertex data to create an object.
- // It will be rendered in the CURRENT pose.
- else if (!strcmp(address[0], "createObject3d")) {
- int IDObject=data[1];
- if (IDObject!=-1) {
+ // This means we received 3D vertex data to create an object.
+ // It will be rendered in the CURRENT pose.
+ else if (!strcmp(address[0], "createObject3D")) {
+ int IDObject=data[1];
+ if (IDObject!=-1) {
// Data is now ordered in the auxDataBuffer array in triplets (X,Y,Z).
// The present value of auxDataBuffer_index contains the number of points x 3
//lsr.pushPoseMatrix(); // we will render on the current pose
@@ -692,52 +806,101 @@
//lsr.popPoseMatrix();
drawScene();
auxDataBuffer_index = 0; // to restart loading the auxiliary buffer 'auxDataBuffer' through serial port
- }
}
+ }
+
+ // Create text:
+ else if (!strcmp(address[0], "createTextObject")) {
+
+
+ // //NOTE: the actual text will be in the second address, and the argument will be the ID of this new text object:
+ textToDisplay = address[1];
+ int IDObject=data[0];
+ if (IDObject!=-1) {
+
- // Create text:
- else if (!strcmp(address[0], "createTextObject")) {
- //NOTE: the actual text will be in the second address, and the argument will be the ID of this new text object:
- textToDisplay = address[1];
- int IDObject=data[1];
- if (IDObject!=-1) {
- begin(IDObject);
- string3d(textToDisplay, fontWidth*textToDisplay.length(), fontHeight);
- end();
- updateScene();
- drawScene();
- }
- }
-
+ //createTextScene();
+// // Set a default PERSPECTIVE and GLOBAL POSE for start displaying:
+// lsr.loadProjMatrix(K1, scaleFactorProjector1);
+// lsr.setIdentityPose();
+// lsr.multPoseMatrix(lsr.EXTRINSICS); // RT=E // we can simply do: lsr.setExtrinsicsPose()
+// //This means that the current coordinate frame is the CAMERA, and calibration was in cm:
+// lsr.translate(0, 0, 100);
+// //lsr.flipY();
+// drawScene(); // this will be called when we change the pose matrix - but needs to be called at least once to start displaying
+
+
+ // clearScene();
+ stopDisplay();
+
+ // CREATE OBJECT:
+ lsr.pushPoseMatrix();
+
+ lsr.setIdentityPose(); // we could use a wrapper, but I won't for the time being.
+ lsr.flipY();
+ //lsr.flipX();
+ lsr.translate(-15,-10,0);
+
+ //begin(IDObject);
+// string3d(textToDisplay, fontWidth*textToDisplay.length(), fontHeight);
+// end();
- // (d) TERMINATOR indicating data was for the POSE MATRIX of the object (Mp) (with respect to the CAMERA):
- else if (!strcmp(address[0], "poseMatrix")) { // when receiving this, it means that the WHOLE matrix data (4x4 values) have been sent in row/column format
- auxDataBuffer_index = 0; // to restart loading the auxiliary buffer 'auxDataBuffer'
- // Now, auxDataBuffer is a buffer with 12 values (4x3), corresponding to the pose of the object in CAMERA coordinartes (RT')
- lsr.setIdentityPose(); // RT=ID
+ //begin(IDObject);
+ for (unsigned short i = 0; i < textToDisplay.length(); i++) {
+ char ch = textToDisplay.at(i);
+ if (ch != ' ') {
+ begin(i);
+ letter3d(ch, fontWidth, fontHeight);
+ end();
+ }
+ lsr.translate(1.1*fontWidth, 0, 0);
+ }
+ //end();
+
+ lsr.popPoseMatrix();
+
+ updateScene();
+
+ // Set a default PERSPECTIVE and GLOBAL POSE for start displaying:
+ lsr.loadProjMatrix(K1, scaleFactorProjector1);
+ lsr.setIdentityPose();
lsr.multPoseMatrix(lsr.EXTRINSICS); // RT=E // we can simply do: lsr.setExtrinsicsPose()
- lsr.multPoseMatrix(auxDataBuffer); // RT=ExRT' : this sets RT as the current object pose in PROJECTOR coordinates
+ // DEFAULT position in CAMERA coordinates (calibration was in cm):
+ lsr.translate(0, 0, 130);
- drawScene(); // needed because we changed the POSE (but there is no need to re-create the geometry nor update the renderer (updateScene), just project)
+ drawScene(); // this will be called when we change the pose matrix - but needs to be called at least once to start displaying
}
+ }
+
- // (d) TERMINATOR indicating data was for the projection matrix (will probably be saved in "hard" in the microcontroller code):
- else if (!strcmp(address[0], "projectionMatrix")) {// when receiving this character, it means that the WHOLE matrix data (3x3 values) have been sent (with '#' separator), in row/column format
- auxDataBuffer_index = 0; // to restart loading the auxiliary buffer 'auxDataBuffer' through serial port
- // store in projection matrix:
- lsr.loadProjMatrix(auxDataBuffer, 1.0);
+ // (d) TERMINATOR indicating data was for the POSE MATRIX of the object (Mp) (with respect to the CAMERA):
+ else if (!strcmp(address[0], "poseMatrix")) { // when receiving this, it means that the WHOLE matrix data (4x4 values) have been sent in row/column format
+ auxDataBuffer_index = 0; // to restart loading the auxiliary buffer 'auxDataBuffer'
+ // Now, auxDataBuffer is a buffer with 12 values (4x3), corresponding to the pose of the object in CAMERA coordinartes (RT')
+ lsr.setIdentityPose(); // RT=ID
+ lsr.multPoseMatrix(lsr.EXTRINSICS); // RT=E // we can simply do: lsr.setExtrinsicsPose()
+ lsr.multPoseMatrix(auxDataBuffer); // RT=ExRT' : this sets RT as the current object pose in PROJECTOR coordinates
- drawScene(); // needed because we changed the PROJECTION matrix (but there is no need to re-create all the geometry, just project)
- }
+ drawScene(); // needed because we changed the POSE (but there is no need to re-create the geometry nor update the renderer (updateScene), just project)
+ }
- //(e) TERMINATOR indicating the data was for the extrinsic matrix:
- else if (!strcmp(address[0], "extrinsicMatrix")) { // when receiving this character, it means that the WHOLE matrix data (3x3 values) have been sent (with '#' separator), in row/column format
- auxDataBuffer_index = 0; // to restart loading the auxiliary buffer 'auxDataBuffer' through serial port
- // store in projection matrix:
- lsr.loadExtrinsicsMatrix(auxDataBuffer);
+ // (d) TERMINATOR indicating data was for the projection matrix (will probably be saved in "hard" in the microcontroller code):
+ else if (!strcmp(address[0], "projectionMatrix")) {// when receiving this character, it means that the WHOLE matrix data (3x3 values) have been sent (with '#' separator), in row/column format
+ auxDataBuffer_index = 0; // to restart loading the auxiliary buffer 'auxDataBuffer' through serial port
+ // store in projection matrix:
+ lsr.loadProjMatrix(auxDataBuffer, 1.0);
+
+ drawScene(); // needed because we changed the PROJECTION matrix (but there is no need to re-create all the geometry, just project)
+ }
- drawScene(); // needed because we changed the EXTRINSICS (hence the pose), but there is no need to re-create the geometry, just project.
- }
+ //(e) TERMINATOR indicating the data was for the extrinsic matrix:
+ else if (!strcmp(address[0], "extrinsicMatrix")) { // when receiving this character, it means that the WHOLE matrix data (3x3 values) have been sent (with '#' separator), in row/column format
+ auxDataBuffer_index = 0; // to restart loading the auxiliary buffer 'auxDataBuffer' through serial port
+ // store in projection matrix:
+ lsr.loadExtrinsicsMatrix(auxDataBuffer);
+
+ drawScene(); // needed because we changed the EXTRINSICS (hence the pose), but there is no need to re-create the geometry, just project.
+ }
}
//============= RECEIVE SERIAL COMMANDS =========================
@@ -762,12 +925,12 @@
// (a) TEXT to display or NUMBERS (in ASCII) to convert to numerical value (rem: limited set of characters for the time being):
// Note: numbers are float, so we need also the "." and "-"
if ( (incomingByte >= '0' && incomingByte <= '9') // numbers
- || (incomingByte >= 'A' && incomingByte <= 'Z') // capital letters
- || (incomingByte >= 'a' && incomingByte <= 'z') // small letters (in fact, will be used for other symbols)
- || (incomingByte == ' ') // space
- || (incomingByte == '-') // minus sign
- || (incomingByte == '.') // decimal point
- ) {
+ || (incomingByte >= 'A' && incomingByte <= 'Z') // capital letters
+ || (incomingByte >= 'a' && incomingByte <= 'z') // small letters (in fact, will be used for other symbols)
+ || (incomingByte == ' ') // space
+ || (incomingByte == '-') // minus sign
+ || (incomingByte == '.') // decimal point
+ ) {
receivedStringData[indexStringData] = incomingByte;
indexStringData++;
}
@@ -783,7 +946,7 @@
else if (incomingByte == '*') {
showLimitsMirrors(50, 120);
}
-
+
else if (incomingByte == '+') {
receivedStringData[indexStringData] = 0;
@@ -850,7 +1013,7 @@
// set color:
lsr.setColor(0x02); // green
//IO.setLaserLockinPower(0); // DISABLING SENSING LASER! (here the red). This is FOR CALIBRATION laser-camera
-
+
// set local pose matrix to identity:
lsr.setIdentityPose();
// translate to the required position:
@@ -885,7 +1048,7 @@
//lsr.setColor(0x02); // fixed color, or use the current color (in which case, we need to call lsr.pushColor() and then pop it again).
lsr.setColor(0x03); // blue+green
IO.setLaserLockinPower(0); // DISABLING SENSING LASER! (here the red). This is FOR CALIBRATION laser-camera
-
+
// begin(1) // we can create a single object, or one object per position (with repetition of the point if needed)
for (unsigned char i = 0; i < auxDataBuffer_index / 2; i++) {
begin(i);
@@ -961,10 +1124,12 @@
indexStringData = 0;
// Serial.println(receivedStringData);
textToDisplay = string(receivedStringData);
- createTextScene();
+ // stopDisplay();
+ // addText();
+ createTextScene();
drawScene(); // needed because we changed the geometry, and we need to compute the projection once for display (of course, this will
// happens anyway when the computer send the pose matrix).
-
+ // resumeDisplay();
}
// (d) TERMINATOR indicating data was for the POSE MATRIX of the object (Mp) (with respect to the CAMERA):