Skip to content

Instantly share code, notes, and snippets.

@niklas152
Created March 29, 2018 06:28
Show Gist options
  • Save niklas152/c67da42a3880193d02de411f8ef43720 to your computer and use it in GitHub Desktop.
Save niklas152/c67da42a3880193d02de411f8ef43720 to your computer and use it in GitHub Desktop.
this is the code used for generating the scan point cloud currently in use on our open bots
package com.niklashalle;
import edu.princeton.cs.introcs.Draw;
import java.awt.*;
public class GenerateScanCloud {
private static class Point {
double x;
double y;
Point(double x0, double y0) {
x = x0;
y = y0;
}
double distanceTo(Point oth) {
return Math.sqrt((this.x - oth.x) * (this.x - oth.x) + (this.y - oth.y) * (this.y - oth.y));
}
}
private static Draw drawTest;
private static void drawPoint(double x, double y) {
drawTest.point(x, y);
System.out.println("_points->push_back(Pixel(" + x/972.0 + " * CAMERA_FRAME_WIDTH, " + y/972.0 + " * CAMERA_FRAME_WIDTH));");
}
public static void main(String[] args) {
double frameSize = 972;
double netDensity = Math.ceil(frameSize / 100.0);
double frameMiddle = frameSize/2d;
drawTest = new Draw();
drawTest.setXscale(0, frameSize);
drawTest.setYscale(0, frameSize);
drawTest.filledRectangle(frameMiddle, frameMiddle, frameMiddle, frameMiddle);
drawTest.setPenColor(Color.BLUE);
drawTest.enableDoubleBuffering();
// not quite sure how large it really needs to be, so better overestimate
Point[] neighbors = new Point[(int) frameSize];
for (int i = 0; i < frameSize; ++i) {
neighbors[i] = new Point(0, 0);
}
// this account for most of the points very well
for (int angle = 0; angle < 360; ++angle) {
for (double radius = 10 * netDensity; radius <= frameSize/2d; radius += (1/3d) * (netDensity * frameSize / radius)) {
Point current = new Point(Math.sin(Math.toRadians(angle)) * radius + frameMiddle,
Math.cos(Math.toRadians(angle)) * radius + frameMiddle);
if (neighbors[(int) Math.round(radius)].distanceTo(current) >= netDensity * frameSize / (2*radius)) {
drawPoint(current.x, current.y);
neighbors[(int) Math.round(radius)] = current;
}
}
}
// adding some point very close to the bot manually
for (double angle = 0; angle < 2 * Math.PI; angle += Math.PI / 4.5) {
drawPoint(Math.sin(angle) * netDensity * 5 + frameMiddle,
Math.cos(angle) * netDensity * 5 + frameMiddle);
}
drawTest.show();
}
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment