I am a first year computer programming student working on a project of Robots and Robot testing environment.
Although I'm new to Stackoverflow, I'm pretty aware that we are not supposed to help with assignments or homework.
I'm struggling to understand why such behavior is happening, so it would be really helpful just to understand why it's happening.
We are using Processing libraries to program in Java.
Our task is to create a robot testing environment and create three robots performing different movements.
The movement I'm trying to program is Patrol, which means just go around the edges of the screen.
The expected behavior is to go forward until it reaches a wall, turn left (90 degrees counter-clock wise), and go forward again.
This project is in conjunction with our Math class. That being said, we can't use methods like rotate(), translate(), pushMatrix() and popMatrix() to help rotating the forms (each robot is a triangle).
So, the steps I followed to rotate the triangles were:
1) translate its center point to the origin (0,0) and the vertices using the same translation;
2) rotate all points, where (x, y) become (y, -x);
3) translate back to correct position (inverse of the step 1 translation).
I set some boundaries if statements to put the triangle back to screen if, after rotation, anything went off screen.
My problem
After turning, the triangle is appearing in strange position, like teleporting.
I added a few lines so we can get each vertex's coordinates, the direction it's going and detect when direction is changed.
Code
There are two files: TestRobots, Robot.
Robot:
import processing.core.PApplet;
public class Robot{
int colour;
String name;
float width;
float height;
float x1;
float y1;
float x2;
float y2;
float x3;
float y3;
int direction;
int speed;
float centralPointX = width/2;
float centralPointY = height/2;
PApplet parent;
public Robot(PApplet parent, String name, int colour, float x1, float y1, float x2, float y2, float x3, float y3, int speed){
this.parent = parent;
this.name = name;
this.colour = colour;
this.x1 = x1;
this.y1 = y1;
this.x2 = x2;
this.y2 = y2;
this.x3 = x3;
this.y3 = y3;
this.speed = speed;
direction=4;
width = x2-x3;
if (width < 0){
width *= -1;
}
if (y2 > y3){
height = y2-y1;
}else{
height = y3-y1;
}
if (height < 0){
height *= -1;
}
if (y1<y2 && y1<y3){
direction=4;
}
}
public void drawRobot(){
parent.fill(colour);
parent.triangle(x1, y1, x2, y2, x3, y3);
parent.ellipseMode(parent.CENTER);
parent.ellipse(x1, y1, 3, 3);
}
public void moveForward(){
if(x1<parent.width || y1 <parent.height || x1 > 0 || y1 > 0){
switch (direction){
case 1:
x1 += speed;
x2 += speed;
x3 += speed;
break;
case 2:
y1 += speed;
y2 += speed;
y3 += speed;
break;
case 3:
x1 -= speed;
x2 -= speed;
x3 -= speed;
break;
case 4:
y1 -= speed;
y2 -= speed;
y3 -= speed;
break;
}
}
}
public void turnLeft(){
//Store original coordinates.
float tempX1 = x1;
float tempY1 = y1;
float tempX2 = x2;
float tempY2 = y2;
float tempX3 = x3;
float tempY3 = y3;
//Calculate translation of the central point of triangle to the origin.
float xTranslation = 0 - centralPointX;
float yTranslation = 0 - centralPointY;
//Translate all points by the translation calculated.
float translatedX1 = tempX1 + xTranslation;
float translatedY1 = tempY1 + yTranslation;
float translatedX2 = tempX2 + xTranslation;
float translatedY2 = tempY2 + yTranslation;
float translatedX3 = tempX3 + xTranslation;
float translatedY3 = tempY3 + yTranslation;
//Rotate all points 90 degrees counterclockwise, (x, y) --> (y, -x).
float rotatedX1 = translatedY1;
float rotatedY1 = -translatedX1;
float rotatedX2 = translatedY2;
float rotatedY2 = -translatedX2;
float rotatedX3 = translatedY3;
float rotatedY3 = -translatedX3;
//Translate all points back.
x1 = rotatedX1 - xTranslation;
y1 = rotatedY1 - yTranslation;
x2 = rotatedX2 - xTranslation;
y2 = rotatedY2 - yTranslation;
x3 = rotatedX3 - xTranslation;
y3 = rotatedY3 - yTranslation;
//Check which y and which x are the smallest, in order to correct any negative numbers.
float minX;
float minY;
if (y1<y2 && y1<y3){
minY = y1;
} else if (y2<y1 && y2<y3){
minY = y2;
} else {
minY = y3;
}
if (x1<x2 && x1<x3){
minX = x1;
} else if (x2<x1 && x2<x3){
minX = x2;
} else {
minX = x3;
}
//Check which y and which x are the biggest, in order to correct any out-of-screen draws.
float maxX;
float maxY;
if (y1>y2 && y1>y3){
maxY = y1;
} else if (y2>y1 && y2>y3){
maxY = y2;
} else {
maxY = y3;
}
if (x1>x2 && x1>x3){
maxX = x1;
} else if (x2>x1 && x2>x3){
maxX = x2;
} else {
maxX = x3;
}
//Correct position if any coordinate is negative.
if((minY-speed)<=minY){
float differenceY = -minY + 10;
y1 += differenceY;
y2 += differenceY;
y3 += differenceY;
}
if(x1<=(x1-speed)){
float differenceX = -minX + 10;
x1 += differenceX;
x2 += differenceX;
x3 += differenceX;
}
//Correct position if any coordinate is bigger than the screen size.
if((parent.height<=(maxY+speed))){
float differenceY = (-maxY+parent.height) + 10;
y1 -= differenceY;
y2 -= differenceY;
y3 -= differenceY;
}
if((x1+speed)>=parent.width){
float differenceX = (-maxX+parent.width) + 10;
x1 -= differenceX;
x2 -= differenceX;
x3 -= differenceX;
}
//Change direction variable and adjust it between 0 and 4.
direction -=1;
if (direction == 0){
direction = 4;
}
}
public void patrol(){
System.out.println("Direction is: "+ direction);
if(((y1-speed)<= 0)||((y1+speed)>= parent.height) || ((x1+speed)>=parent.width)||((x1-speed)<=0)){
turnLeft();
System.out.println("The NEW direction is: "+ direction);
}
moveForward();
}
}
TestRobots:
import processing.core.PApplet;
public class TestRobots extends PApplet{
Robot alice = new Robot(this, "Alice", 255, 257f, 389f, 309f, 450f, 209f, 450f, 3);
public static void main(String[] args){
PApplet.main("TestRobots");
}
public void settings(){
size(1000, 500);
}
public void setup(){
frameRate(30);
}
public void draw(){
background(255);
alice.patrol();
System.out.println("x1 = "+ alice.x1);
System.out.println("y1 = "+ alice.y1);
System.out.println("x2 = "+ alice.x2);
System.out.println("y2 = "+ alice.y2);
System.out.println("x3 = "+ alice.x3);
System.out.println("y3 = "+ alice.y3);
alice.drawRobot();
}
}
Here is a print of the generated output. I snipped the part where it changes direction:
Direction is: 2
x1 = 62.0
y1 = 497.0
x2 = 10.0
y2 = 436.0
x3 = 110.0
y3 = 436.0
The NEW direction is: 1
x1 = 500.0
y1 = 58.0
x2 = 439.0
y2 = 110.0
x3 = 439.0
y3 = 10.0
Same strange behavior here:
Direction is: 4
x1 = 257.0
y1 = 2.0
x2 = 309.0
y2 = 63.0
x3 = 209.0
y3 = 63.0
The NEW direction is: 3
x1 = -1.0
y1 = 62.0
x2 = 60.0
y2 = 10.0
x3 = 60.0
y3 = 110.0
The NEW direction is: 2
x1 = 62.0
y1 = 74.0
x2 = 10.0
y2 = 13.0
x3 = 110.0
y3 = 13.0
Thank you very much for reading until here and sorry in advance if I wasn't clear enough. It's my first question!
Gustavo
I think your problem relates to the variables centralPointX and centralPointY. Look at this code you wrote:
//Calculate translation of the central point of triangle to the origin.
float xTranslation = 0 - centralPointX;
float yTranslation = 0 - centralPointY;
//Translate all points by the translation calculated.
float translatedX1 = tempX1 + xTranslation;
float translatedY1 = tempY1 + yTranslation;
float translatedX2 = tempX2 + xTranslation;
float translatedY2 = tempY2 + yTranslation;
float translatedX3 = tempX3 + xTranslation;
float translatedY3 = tempY3 + yTranslation;
It seems from this that you think centralPointX and centralPointY indicate the center of the triangle... However, look at where they are defined:
float centralPointX = width/2;
float centralPointY = height/2;
So they don't actually represent the center xy co-ordinates. What you need to do is fix this part:
//Calculate translation of the central point of triangle to the origin.
float xTranslation = 0 - centralPointX;
float yTranslation = 0 - centralPointY;
So that it actually does what the comment says it should do, i.e., calculates the x and y co-ordinates of the center of your triangle.
I won't implement this for you, because as you said, it is homework. However, I think this should definitely point you in the right direction. Also, keep up the good work. I wish every first question was as well thought out as this.
Related
How can I make a algorithm that detects if a point (x, y) intercepts with a line. (x1, y1, x2, y2)?
I have already tried :
boolean onLine(float a, float b, float c, float d, float x, float y){
boolean answer = false;
float[] p1 = new float[] {a, b};
float[] p2 = new float[] {c, d};
float x_spacing = (p2[0] - p1[0]) / ((a+c)/2 + (b+d));
float y_spacing = (p2[1] - p1[1]) / ((a+c)/2 + (b+d));
List<float[]> line = new ArrayList();
float currentX = 0;
float currentY = 0;
while(currentX+a<c&¤tY+b<d){
currentX += x_spacing;
currentY += y_spacing;
line.add(new float[]{a+currentX, b+currentY});
}
for(int j = 0; j < line.size(); j++){
if(x > line.get(j)[0]-x_spacing && x < line.get(j)[0]+x_spacing && y > line.get(j)[1]-
y_spacing && y < line.get(j)[1]+y_spacing){
answer = true;
println("Hit line!");
break;
}
}
return answer;
}
This works sometimes, but is not always consistent.
I am putting this with a physics game, and I need this so the ball can roll down a line.
What are some ways I can improve it so that it works?.
EDIT: Thanks to Felix Castor I got it working. Here is the final Code:
boolean onLine(float x1, float y1, float x2, float y2, float xt, float yt,
float wid, float hit){
float Y = (y2 - y1)/(x2 - x1)* xt + y1 -(y2 - y1)/(x2 - x1) * x1;
boolean answer = false;
if(abs(Y - yt) < 5) answer = true;
if(abs(Y - yt-hit) < 5) answer = true;
if(abs(Y - yt-(hit/2)) < 5) answer = true;
if(abs(Y - yt+hit) < 5) answer = true;
if(abs(Y - yt+(hit/2)) < 5) answer = true;
return answer;
}
Using slope intercept form you can plug in your x and see if the y's are equal.
y = m*x + b
m = (y2 - y1)/(x2 - x1)
b = y1 - (y2 - y1)/(x2 - x1) * x1
So the equation becomes
Y = (y2 - y1)/(x2 - x1)* X + y1 -(y2 - y1)/(x2 - x1) * x1
given a point (xt, yt) you can plug in the xt into X and evaluate then compare the result to yt. If they are equal then the point is on the line.
if Y == yt given xt then the point is on the line.
You will need to handle the case where you have strictly horizontal lines as edge cases. Those will blow up the equation.
Edit: Conditions Changed
Since you are wanting to determine how far from the line a point is I would say the formula for the distance between a point and a line in cartesian space would be the way to go. See Distance from a point to a line section Line Defined By Two Points. The formula looks ugly but it's straight forward.
double numerator = Math.abs((y2 - y1) * xt - (x2 - x1) * yt + x2 * y1 - y2 * x1);
double denominator = Math.sqrt(Math.pow(y2 - y1, 2) + Math.pow(x2 - x1, 2));
double distance = numerator / denominator;
As before your test point is (xt, yt) and your line is defined by two points (x1, y1) and (x2, y2). Because distance is always >= 0 your test would be:
if( distance <= tolerance) return true
I think this is a better approach if you are interested in a tolerance.
Currently I have an ArrayList of vertices in a 3-dimensional cartesian coordinates system. The polygon is random. It can be a car, a cup or even a dragon.
Assuming the density does not change, how to calculate the centre of mass (x,y,z) of this 3D object?
I am storing the faces and vertices in ArrayList.
public ArrayList<stlFace> StlFaces = new ArrayList<stlFace>();
public ArrayList<VertexGeometric> VertexList = new ArrayList<VertexGeometric>();
I was using this for calculating surface which is proportional to mass of each face or triangle. And to calculate center off mass of each triangle and center of mass of whole object I was using this. I added helper methods getCenter() and getSurface() to Face class to encapsulate calculations specific to just one face/triangle.
public static class Vertex {
public float x = 0;
public float y = 0;
public float z = 0;
public Vertex(float x, float y, float z) {
this.x = x;
this.y = y;
this.z = z;
}
}
public static class Face {
public Vertex v1;
public Vertex v2;
public Vertex v3;
public Face(Vertex v1, Vertex v2, Vertex v3) {
this.v1 = v1;
this.v2 = v2;
this.v3 = v3;
}
public Vertex getCenter() {
Vertex triangleCenter = new Vertex(0, 0, 0);
triangleCenter.x += v1.x;
triangleCenter.x += v2.x;
triangleCenter.x += v3.x;
triangleCenter.y += v1.y;
triangleCenter.y += v2.y;
triangleCenter.y += v3.y;
triangleCenter.z += v1.z;
triangleCenter.z += v2.z;
triangleCenter.z += v3.z;
triangleCenter.x /= 3;
triangleCenter.y /= 3;
triangleCenter.z /= 3;
return triangleCenter;
}
public float getSurface() {
float x1 = v1.x - v2.x;
float x2 = v1.y - v2.y;
float x3 = v1.z - v2.z;
float y1 = v1.x - v3.x;
float y2 = v1.y - v3.y;
float y3 = v1.z - v3.z;
return (float) Math.sqrt(
Math.pow(x2 * y3 - x3 * y2, 2) +
Math.pow(x3 * y1 - x1 * y3, 2) +
Math.pow(x1 * y2 - x2 * y1, 2)
) / 2f;
}
}
public static Vertex calculateMassCenter(List<Face> faces) {
Vertex massCenter = new Vertex(0, 0, 0);
float mass = 0;
for (Face face : faces) {
Vertex triangleCenter = face.getCenter();
float faceMass = face.getSurface();
mass += faceMass;
massCenter.x += faceMass * triangleCenter.x;
massCenter.y += faceMass * triangleCenter.y;
massCenter.z += faceMass * triangleCenter.z;
}
massCenter.x /= mass;
massCenter.y /= mass;
massCenter.z /= mass;
return massCenter;
}
I need to cover some polygon with rectangles here's an example :
The black figure in a black square is the polygon that i need to cover with those green rectangles but i need to do it more efficiently that just make a net like i did. Because as you can see there can be place more green rectangles if i moved them.
Rectangles inside are fixed size(just not as big as polygon it self), one for all like in the picture, they can be places vertically and horizontally, i want to fill the polygon as much as it can fit it inside of it, this polygon is just for example, there can be different polygons with holes in them for example that black small square is a hole.
module = rectangle
private void coverWithModules(Graphics g, int[] xpoints, int[] ypoints) {
Polygon module;
int x1, x2, x3, x4, y1, y2, y3, y4;
int moduleRowNumber = 0;
int totalRows = (getMax(ypoints) / moduleHeight);
while (moduleRowNumber < totalRows) {
// first module
x1 = getMin(xpoints);
y1 = getMin(ypoints) + distance * moduleRowNumber + moduleHeight
* moduleRowNumber;
x2 = x1 + moduleWidth;
y2 = y1;
x3 = x1 + moduleWidth;
y3 = y1 + moduleHeight;
x4 = x1;
y4 = y1 + moduleHeight;
int[] x = { x1, x2, x3, x4 };
int[] y = { y1, y2, y3, y4 };
module = new Polygon();
// check if point are inside the polygon
checkModulePlacement(g, x, y, module);
// placing modules in a row
while (x1 < getMax(xpoints)) {
x1 = x2 + distance;
y1 = getMin(ypoints) + distance * moduleRowNumber
+ moduleHeight * moduleRowNumber;
x2 = x1 + moduleWidth;
y2 = y1;
x3 = x1 + moduleWidth;
y3 = y1 + moduleHeight;
x4 = x1;
y4 = y1 + moduleHeight;
int[] xx = { x1, x2, x3, x4 };
int[] yy = { y1, y2, y3, y4 };
module = new Polygon();
checkModulePlacement(g, xx, yy, module);
}
moduleRowNumber++;
}
}
private void checkModulePlacement(Graphics g, int[] x, int[] y, Polygon module) {
boolean pointInside = true;
boolean pointOnObstraction = true;
for (int i = 0; i < x.length; i++) {
if (pointInside) {
pointInside = roof.contains(x[i], y[i]);
}
module.addPoint(x[i], y[i]);
}
pointOnObstraction = checkForObstractions(module);
g.setColor(Color.GREEN);
if (pointInside == true && pointOnObstraction == false ) {
g.drawPolygon(module);
}
}
I was looking for something and i have found Something like this maybe there is more stuff like this ?
I don't know where to search for such info. What should i look up to get what i need ? Maybe there is some kind of library for this kind of things ?
My program contains main shapes i.e square, rect, circle they work good ( for x2,y2 in those shapes I used absolute values e.g square( x, y, 5, 5) ), but when working with triangle shape,
Triangles in my program glitches.
below is the code of my program modules,
if (vehicleStyle.getVehicleShape().equals(VehicleShape.TRIANGLE)) {
processingVisualizer
.fill(vehicleStyle.getColor().red,
vehicleStyle.getColor().green,
vehicleStyle.getColor().blue);
processingVisualizer.strokeWeight(1 * vehicleSize);
//System.out.println(x + "-" + y);
//## to place face toward movement direction
/*
* -----<|----
* | |
* \/ /\
* | |
* -----|>----
*/
float x2, y2, x3, y3;
if (x == 100) {
System.out.println( "x==100");
x2 = x - 5;
y2 = y + 5;
x3 = x + 5;
y3 = y + 5;
processingVisualizer.triangle(x, y, x2, y2, x3, y3);
} else if (x == 20) {
System.out.println( "x==20");
x2 = x - 2;
y2 = y - 2;
x3 = x + 2;
y3 = y - 2;
processingVisualizer.triangle(x, y, x2, y2, x3, y3);
} else if (y == 100) {
System.out.println( "y ==100");
x2 = x - 2;
y2 = y - 2;
x3 = x - 2;
y3 = y + 2;
processingVisualizer.triangle(x, y, x2, y2, x3, y3);
} else if (y == 20) {
System.out.println( "y ==20");
x2 = x+5;//x - 2;
y2 = y-5;
x3 = x+5 ;//- 2;
y3 = y+5;
processingVisualizer.triangle(x, y, x2, y2, x3, y3);
}
}
processingVisualizer.strokeWeight(1);
}
Can't test your code, but my assumption is the "glitch" happens because you are rendering a triangle only when your 4 conditions are met. You should update the triangle's corner positions based on your conditions if you like, but render the triangle all time, even when the positions are out of date:
if (vehicleStyle.getVehicleShape().equals(VehicleShape.TRIANGLE)) {
processingVisualizer
.fill(vehicleStyle.getColor().red,
vehicleStyle.getColor().green,
vehicleStyle.getColor().blue);
processingVisualizer.strokeWeight(1 * vehicleSize);
//System.out.println(x + "-" + y);
//## to place face toward movement direction
/*
* -----<|----
* | |
* \/ /\
* | |
* -----|>----
*/
float x2, y2, x3, y3;
if (x == 100) {
System.out.println( "x==100");
x2 = x - 5;
y2 = y + 5;
x3 = x + 5;
y3 = y + 5;
} else if (x == 20) {
System.out.println( "x==20");
x2 = x - 2;
y2 = y - 2;
x3 = x + 2;
y3 = y - 2;
} else if (y == 100) {
System.out.println( "y ==100");
x2 = x - 2;
y2 = y - 2;
x3 = x - 2;
y3 = y + 2;
} else if (y == 20) {
System.out.println( "y ==20");
x2 = x+5;//x - 2;
y2 = y-5;
x3 = x+5 ;//- 2;
y3 = y+5;
}
processingVisualizer.triangle(x, y, x2, y2, x3, y3);
}
processingVisualizer.strokeWeight(1);
}
Also, you can easily compute the direction of motion, if you store the previous position, using the arc tangent function (atan2()):
float angle = atan2(currentY-previousY,currentX-previousX);
Here's a quick example:
float cx,cy,px,py;//current x,y, previous x,y
float len = 15;
void setup(){
size(200,200);
background(255);
}
void draw(){
//update position - chase mouse with a bit of easing
cx -= (cx - mouseX) * .035;
cy -= (cy - mouseY) * .035;
//find direction of movement based on the current position
float angle = atan2(cy-py,cx-px);
//store previous position
px = cx;
py = cy;
//render
fill(255,10);noStroke();
rect(0,0,width,height);
fill(127,32);stroke(0);
pushMatrix();
translate(cx,cy);
pushMatrix();
rotate(angle);
triangle(len,0,-len,-len,-len,len);
line(0,0,len,0);
popMatrix();
popMatrix();
}
I need a function which takes a line (known by its coordinates)
and return a line with same angle, but limited to certain length.
My code gives correct values only when the line is turned 'right'
(proven only empirically, sorry).
Am I missing something?
public static double getAngleOfLine(int x1, int y1, int x2, int y2) {
double opposite = y2 - y1;
double adjacent = x2 - x1;
if (adjacent == Double.NaN) {
return 0;
}
return Math.atan(opposite / adjacent);
}
// returns newly calculated destX and destY values as int array
public static int[] getLengthLimitedLine(int startX, int startY,
int destX, int destY, int lengthLimit) {
double angle = getAngleOfLine(startX, startY, destX, destY);
return new int[]{
(int) (Math.cos(angle) * lengthLimit) + startX,
(int) (Math.sin(angle) * lengthLimit) + startY
};
}
BTW: I know that returning arrays in Java is stupid,
but it's just for the example.
It would be easier to just treat it as a vector. Normalize it by dividing my its magnitude then multiply by a factor of the desired length.
In your example, however, try Math.atan2.
In Python because I don't have a Java compiler handy:
import math
def getLengthLimitedLine(x1, y1, x2, y2, lengthLimit):
length = math.sqrt((x2-x1)**2 + (y2-y1)**2)
if length > lengthLimit:
shrink_factor = lengthLimit / length
x2 = x1 + (x2-x1) * shrink_factor
y2 = y1 + (y2-y1) * shrink_factor
return x2, y2
print getLengthLimitedLine(10, 20, 25, -5, 12)
# Prints (16.17, 9.71) which looks right to me 8-)
It's an easy problem if you understand something about vectors.
Given two points (x1, y1) and (x2, y2), you can calculate the vector from point 1 to 2:
v12 = (x2-x1)i + (y2-y2)j
where i and j are unit vectors in the x and y directions.
You can calculate the magnitude of v by taking the square root of the sum of squares of the components:
v = sqrt((x2-x2)^2 + (y2-y1)^2)
The unit vector from point 1 to point 2 equals v12 divided by its magnitude.
Given that, you can calculate the point along the unit vector that's the desired distance away by multiply the unit vector times the length and adding that to point 1.
Encapsulate Line in a class, add a unit method and a scale method.
public class Line {
private float x;
private float y;
public Line(float x1, float x2, float y1, float y2) {
this(x2 - x1, y2 - y1);
}
public Line(float x, float y) {
this.x = x;
this.y = y;
}
public float getLength() {
return (float) Math.sqrt((x * x) + (y * y));
}
public Line unit() {
return scale(1 / getLength());
}
public Line scale(float scale) {
return new Line(x * scale, y * scale);
}
}
Now you can get a line of arbitrary length l by calling
Line result = new Line(x1, x2, y1, y2).unit().scale(l);
No need to use trig, which can have some nasty edge cases. Just use similar triangles:
public static int[] getLengthLimitedLine(int startX, int startY,
int destX, int destY, int lengthLimit)
{
int deltaX = destX - startX;
int deltaY = destY - startY;
int lengthSquared = deltaX * deltaX + deltaY * deltaY;
// already short enough
if(lengthSquared <= lengthLimit * lengthLimit)
return new int[]{destX, destY};
double length = Math.sqrt(lengthSquared);
double newDeltaX = deltaX * lengthLimit / length;
double newDeltaY = deltaY * lengthLimit / length;
return new int[]{(int)(startX + newDeltaX), (int)(startY + newDeltaY)};
}
Just use the Pythagorean theorem, like so:
public static int[] getLengthLimitedLine(int start[], int dest[], int lengthLimit) {
int xlen = dest[0] - start[0]
int ylen = dest[1] - start[1]
double length = Math.sqrt(xlen * xlen + ylen * ylen)
if (length > lengthLimit) {
return new int[] {start[0], start[1],
start[0] + xlen / lengthLimit,
start[1] + ylen / lengthLimit}
} else {
return new int[] {start[0], start[1], dest[0], dest[1];}
}
}