summary |
shortlog |
log |
commit | commitdiff |
review |
tree
raw |
patch |
inline | side by side (from parent 1:
9f84331)
gitlab #91
Change-Id: I458db0787a0b2be4282df4619bcfdaab674e3ef1
else if (d < -0.9999)
return tr*2.0; // point following turn is directly behind the nozzle, in theory, we should return Double.Inf...
else if (d < -0.9999)
return tr*2.0; // point following turn is directly behind the nozzle, in theory, we should return Double.Inf...
int iter = 10;
Vector3d tp0 = tcp.getPosition();
try {
int iter = 10;
Vector3d tp0 = tcp.getPosition();
try {
}
double a = dir.angle(dir2);
}
double a = dir.angle(dir2);
- double t = Math.tan(a * 0.5);
- double R = 0.0;
- if (t > MathTools.NEAR_ZERO)
- R = tr * t;
+
+ // other is directly between dcp and tcp, a zero angle turn should do
+ if (Math.PI - a <= MathTools.NEAR_ZERO)
+ return 0.0;
+
+ double R = tr * Math.tan(a * 0.5);
if (R <= curr)
break;
curr = R*1.001;
if (R <= curr)
break;
curr = R*1.001;
return tcp.getTurnAngle();
return Math.PI; // FIXME : argh
}
return tcp.getTurnAngle();
return Math.PI; // FIXME : argh
}
- double turnAngle = prev.angle(next);
+
+ final boolean isDegenerate = prev.lengthSquared() < MathTools.NEAR_ZERO || next.lengthSquared() < MathTools.NEAR_ZERO;
+ double turnAngle = isDegenerate ? 0.0 : prev.angle(next);
Vector3d turnAxis = new Vector3d();
turnAxis.cross(prev, next);
Vector3d turnAxis = new Vector3d();
turnAxis.cross(prev, next);