// 3D Environment Laser Scanner (Processing Part2) // By Callan Mackay // Published on 22/02/2016 void setup(){ size(1000, 1000, P3D); // make the render window 1000 x 1000 pixels and 3D capable } float rotmot; // current angle of the motor float rotcam = 30*PI/180; // CHANGE THIS ACCORDINGLY the angle of the webcam where 0deg is straight forward and 90deg is looking at the laser (converted to radians) float rotlaser = 0; // same as above but for the linear laser float rotpixX; // angle of the current white pixel from the camera's centre view in X-direction float rotpixY; // angle of the current white pixel from the camera's centre view in Y-direction float pixangleX = 50.0/640*PI/180; // angle between each pixel in webcam in X-direction float pixangleY = 50.0/640*PI/180; // angle between each pixel in webcam in Y-direction float[] A,B,AB; // constants needed for line equation float[] C,D,CD; // constants needed for plane equation float n,t; // constants needed for line/plane intersection equation float pointX, pointY, pointZ; // temporarily holds the X,Y,Z coordinates of a new point in 3D space float[] drawpointX; // holds all X coordinates of points in 3D space float[] drawpointY; // holds all Y coordinates of points in 3D space float[] drawpointZ; // holds all Z coordinates of points in 3D space int k = 0; // counts the number of white pixels in entire scan for placing them in the "drawpoint" vectors void draw(){ background(0); // make the background black perspective(PI/3.0,(float)width/height,1,500); // adjust the near and far clipping planes to see the whole scene (last two number) stroke(255, 0, 0); // make the line red line(0,0,0,50,0,0); // make a line in the x-direction of 50 stroke(0, 255, 0); // make the line green line(0,0,0,0,50,0); // make a line in the y-direction of 50 stroke(0, 0, 255); // make the line blue line(0,0,0,0,0,50); // make a line in the z-direction of 50 camera(50*cos(mouseX/100.0)+10, 20, -50*sin(mouseX/100.0)-30, // X,Y,Z location of the camera (not the webcam) 10, 0, -30, // X,Y,Z location of what the camera is looking at (not the webcam) 0.0, -1.0, 0.0); // where is up for the camera (not the webcam) if(keyPressed){ // only renders point cloud if a key is pressed on the keyboard String[] myString = loadStrings("test.txt"); // save the text file to the first element of a string vector drawpointX = new float[splitTokens(myString[0], ",").length/2]; // make the drawpointX vector the length of how many white pixels there are drawpointY = new float[splitTokens(myString[0], ",").length/2]; // make the drawpointY vector the length of how many white pixels there are drawpointZ = new float[splitTokens(myString[0], ",").length/2]; // make the drawpointZ vector the length of how many white pixels there are myString = splitTokens(myString[0], ";"); // split up the large string into multiple string each containing a webcam frame k = 0; // reset "k" to zero for(int i = 0; i < myString.length; i++){ // repeat loop for every element in "myString" (number of webcam frames) String[] stringPart = splitTokens(myString[i], ","); // split "myString" up into each pixel coordinate rotmot = float(stringPart[0]); // save the motor angle from the first string element to "rotmot" pushMatrix(); // create set of local coordinates rotateY(-rotmot*PI/180); // rotate everything in Y by the motor angle pushMatrix(); // create another set of local coordinates translate(-19, 2, 0); // move everything along the scanner arm to the laser position in 3D space rotateY(rotlaser); // rotate everything in Y by the laser angle //Plane Equation C = new float[]{modelX(0,0,0), modelY(0,0,0), modelZ(0,0,0)}; // create a point on laser plane with global coordinates translate(10, 0, 0); // move along by 10 in X-direction D = new float[]{modelX(0,0,0), modelY(0,0,0), modelZ(0,0,0)}; // create another point, 10 in X-direction away from laser plane CD = new float[]{D[0]-C[0], D[1]-C[1], D[2]-C[2]}; // create a line perpindicular to plane using points "C" and "D" (NORMAL VECTOR) // CD[0]x + CD[1]y + CD[2]z = n popMatrix(); // close off local coordinates to return current position to the motor shaft with the motor's angle for(int j = 1; j < stringPart.length; j = j+2){ // make the loop repeat for every second element in the "stringPart" vector (repeats for every set of X-Y white pixel coordinate in a single webcam frame) rotpixX = (float(stringPart[j])-320)*pixangleX; // measuring the white pixels angle in X-direction from the cameras centre rotpixY = (240-float(stringPart[j+1]))*pixangleY; // measuring the white pixels angle in Y-direction from the cameras centre pushMatrix(); // move into another set of local coordinates translate(18, 4.5, 0); // move along the arm from the motor shaft to the webcam's centre rotateY(rotcam); // rotate to the webcam's angle rotateX(rotpixY); // rotate the imaginary line by "rotpixY" in the X-axis to align with current white pixel rotateY(rotpixX); // rotate the imaginary line by "rotpixX" in the Y-axis to align with current white pixel //Line Equation A = new float[]{modelX(0,0,0), modelY(0,0,0), modelZ(0,0,0)}; // create a point on webcam's centre with global coordinates translate(0,0,10); // move along by 10 in Z-direction B = new float[]{modelX(0,0,0), modelY(0,0,0), modelZ(0,0,0)}; // create another point, 10 in Z-direction out in front of the webcam, along the imaginary line AB = new float[]{B[0]-A[0], B[1]-A[1], B[2]-A[2]}; // create the line from the webcam using points "A" and "B" (PROJECTION LINE) popMatrix(); // close off local coordinates to return current position to the motor shaft with the motor's angle // Intersection of Line and Plane t = (n - CD[0]*A[0] - CD[1]*A[1] - CD[2]*A[2]) / (CD[0]*AB[0] + CD[1]*AB[1] + CD[2]*AB[2]); // Equation for the laser plane rearranged with equations for projection line substituted in to find constant "t" pointX = A[0] + AB[0]*t; // Using "t" in line equation to find X-coordinate of intersection pointY = A[1] + AB[1]*t; // Using "t" in line equation to find X-coordinate of intersection pointZ = A[2] + AB[2]*t; // Using "t" in line equation to find X-coordinate of intersection drawpointX[k] = (pointX); // inserting temporary X-coordinate into the X-coordinate vector to hold all data points drawpointY[k] = (pointY); // inserting temporary Y-coordinate into the Y-coordinate vector to hold all data points drawpointZ[k] = (pointZ); // inserting temporary Z-coordinate into the Z-coordinate vector to hold all data points k++; // increment "k" for moving onto the next white pixel i.e. data point } popMatrix(); // close off local coordinates to return current position to (0,0,0) in global space } } if(k > 0){ // check to see if any data points were created for (int i = 0; i < drawpointX.length; i++){ // repeat loop for every data point stroke(255); // make the data point white point(drawpointX[i], drawpointY[i], drawpointZ[i]); // draw the point cloud using the "drawpointX,Y,Z" vectors } } }