java - 高斯求积以近似 Java 中的贝塞尔曲线长度
问题描述
我试图弄清楚如何近似我的第 n 次贝塞尔曲线的长度,我发现我可以使用高斯正交积分来近似它。在我的控制点 P1(220, 40)、P2(220, 260) P3(35, 200) 和 P4(120, 160) 多次尝试后,我的程序应该给我一个 272.87 的曲线长度(如图所示这里);但是,我只得到了 229.18 左右的结果。我已经检查了GaussLegendre 类的常量,它们是正确的。
有人可以告诉我我做错了什么。
这是完整的课程
import java.util.Arrays;
import java.util.List;
import org.waltonrobotics.controller.PathData;
import org.waltonrobotics.controller.Pose;
/**
* Everything about Bezier Curves https://pomax.github.io/bezierinfo/
*/
public class DynamicBezierCurve extends Path {
private final double startVelocity;
private final double endVelocity;
private final int degree;
private double[] coefficients;
/**
* This constructor is used with the splines, but feel free to use it when creating your own motions
*
* @param vCruise - the cruise velocity of the robot
* @param aMax - the maximum acceleration of the robot
* @param startVelocity - the start velocity
* @param endVelocity - the end velocity
* @param isBackwards - whether or not to move the robot backwards
* @param controlPoints - the control points that define the curve
*/
public DynamicBezierCurve(double vCruise, double aMax, double startVelocity, double endVelocity,
boolean isBackwards, List<Pose> controlPoints) {
super(vCruise, aMax, isBackwards, controlPoints);
this.startVelocity = startVelocity;
this.endVelocity = endVelocity;
// The starting average encoder distance should always be 0
degree = getKeyPoints().size() - 1;
coefficients = calculateCoefficients(degree);
}
public DynamicBezierCurve(double vCruise, double aMax, double startVelocity, double endVelocity,
boolean isBackwards,
Pose... controlPoints) {
this(vCruise, aMax, startVelocity, endVelocity, isBackwards, Arrays.asList(controlPoints));
}
/**
* Uses the formula to find the value of nCr
*
* @return nCr
*/
private static double findNumberOfCombination(int n, int r) {
int nFactorial = factorial(n);
int rFactorial = factorial(r);
int nMinusRFactorial = factorial(n - r);
return nFactorial / (rFactorial * nMinusRFactorial);
}
/**
* Finds the factorial of any integer or double, d
*
* @return the factorial of d
*/
private static int factorial(int d) {
int result = 1;
for (int i = 1; i <= d; i++) {
result = result * i;
}
return result;
}
public double computeArcLength() {
int n = 10;
GaussLegendre gl = new GaussLegendre(n, -1, 1);
double[] t = gl.getNodes();
double[] C = gl.getWeights();
double z = 1;
double sum = 0;
double zDivision = z / 2;
for (int i = 0; i < t.length; i++) {
double ti = t[i];
ti = zDivision * ti + zDivision;
Pose point = getDerivative(ti);
double Ci = C[i];
sum += Ci * Math.hypot(point.getX(), point.getY());
}
sum = zDivision * sum;
return sum;
}
@Override
public PathData createPathData(double startAverageEncoderLength, PathData previousPathData, double percentage) {
Pose centerPoint = getPoint(percentage);
PathData pathData;
// pathData= calculateData(startAverageEncoderLength, previousPathData, centerPoint);
pathData = new PathData(centerPoint);
return pathData;
}
private Pose getDerivative(double percentage) {
double dx = 0;
double dy = 0;
if (percentage == 1.0) {
int last = getKeyPoints().size() - 1;
dx = getKeyPoints().get(last).getX()
- getKeyPoints().get(last - 1).getX();
dy = getKeyPoints().get(last).getY()
- getKeyPoints().get(last - 1).getY();
} else {
for (int i = 0; i < degree; i++) {
Pose pointI = getKeyPoints().get(i);
double multiplier =
coefficients[i] * StrictMath.pow(1 - percentage, (degree - i)) * StrictMath
.pow(percentage, (double) i);
Pose nextPointI = getKeyPoints().get(i + 1);
dx += (multiplier = multiplier * (degree)) * (nextPointI.getX() - pointI.getX());
dy += multiplier * (nextPointI.getY() - pointI.getY());
}
}
double angle = StrictMath.atan2(dy, dx);
if (isBackwards()) {
angle += Math.PI;
}
angle %= (2 * Math.PI);
return new Pose(dx, dy, angle);
}
private Pose getPoint(double percentage) {
return getPoint(degree, percentage);
}
/**
* @param percentage - t
* @return the Pose that is at percentage t along the curve
*/
private Pose getPoint(double degree, double percentage) {
double xCoordinateAtPercentage = 0;
double yCoordinateAtPercentage = 0;
double dx = 0;
double dy = 0;
for (int i = 0; i <= degree; i++) {
Pose pointI = getKeyPoints().get(i);
double multiplier =
coefficients[i] * StrictMath.pow(1 - percentage, (degree - i)) * StrictMath.pow(percentage, (double) i);
xCoordinateAtPercentage += (multiplier * pointI.getX());
yCoordinateAtPercentage += (multiplier * pointI.getY());
if (percentage != 1 && i < degree) {
Pose nextPointI = getKeyPoints().get(i + 1);
dx += (multiplier = multiplier * (degree)) * (nextPointI.getX() - pointI.getX());
dy += multiplier * (nextPointI.getY() - pointI.getY());
}
}
if (percentage == 1.0) {
int last = getKeyPoints().size() - 1;
dx = getKeyPoints().get(last).getX()
- getKeyPoints().get(last - 1).getX();
dy = getKeyPoints().get(last).getY()
- getKeyPoints().get(last - 1).getY();
}
double angle = StrictMath.atan2(dy, dx);
if (isBackwards()) {
angle += Math.PI;
}
angle %= (2 * Math.PI);
return new Pose(xCoordinateAtPercentage, yCoordinateAtPercentage, angle);
// return new Pose(dx, dy, angle);
}
/**
* Updates the coefficients used for calculations
*/
private double[] calculateCoefficients(int degree) {
double[] coefficients = new double[degree + 1];
for (int i = 0; i < coefficients.length; i++) {
coefficients[i] = findNumberOfCombination(degree, i);
}
return coefficients;
}
}
解决方案
为了减少累积误差,您可以使用
sum += Ci * Math.hypot(point.getX(), point.getY());
代替
sum += Ci * Math.sqrt(Math.pow(point.getX(), 2) + Math.pow(point.getY(), 2));
(但是,由于您发布的代码不完整,我无法使用它来验证此更改将产生多大的影响)
推荐阅读
- c++ - 密集方阵的大 RAM 要求
- python-2.7 - 使用 pyenv 和 python 2.7.12 导入 cElementTree 失败
- python - 从带有可点击点的地图中抓取数据
- javascript - 在 Angular 2+ 模板中调用匿名函数
- dojo - Aikau Select 没有填满选项
- angular - 如何在 ngx-chart 中将 yOrient 属性设置为正确?
- python - 熊猫类扩展不起作用
- javascript - 在 ExpressJS 中创建一个简单的待办事项列表
- python - ATBS:逗号代码
- javascript - 如何在没有按钮的情况下访问浏览器?(离子 3)